static bool
sctp_mt(const struct sk_buff *skb, const struct xt_match_param *par)
{
const struct xt_sctp_info *info = par->matchinfo;
const sctp_sctphdr_t *sh;
sctp_sctphdr_t _sh;
if (par->fragoff != 0) {
duprintf("Dropping non-first fragment.. FIXME\n");
return false;
}
sh = skb_header_pointer(skb, par->thoff, sizeof(_sh), &_sh);
if (sh == NULL) {
duprintf("Dropping evil TCP offset=0 tinygram.\n");
*par->hotdrop = true;
return false;
}
duprintf("spt: %d\tdpt: %d\n", ntohs(sh->source), ntohs(sh->dest));
return SCCHECK(ntohs(sh->source) >= info->spts[0]
&& ntohs(sh->source) <= info->spts[1],
XT_SCTP_SRC_PORTS, info->flags, info->invflags)
&& SCCHECK(ntohs(sh->dest) >= info->dpts[0]
&& ntohs(sh->dest) <= info->dpts[1],
XT_SCTP_DEST_PORTS, info->flags, info->invflags)
&& SCCHECK(match_packet(skb, par->thoff + sizeof(sctp_sctphdr_t),
info, par->hotdrop),
XT_SCTP_CHUNK_TYPES, info->flags, info->invflags);
}
bool XBeeUtil::wait_for_packet_type(XBee* xbee, int timeout, int api_id, bool (*match_packet)(), void (*queue_packet)())
{
unsigned long end = millis() + timeout;
while(millis() < end)
{
if(xbee->readPacket(end - millis()) && xbee->getResponse().getApiId() == api_id)
{
if(match_packet != NULL)
{
if(match_packet())
{
return true;
}else
{
if (queue_packet != NULL) {
queue_packet();
}
}
}else
{
return true;
}
} else {
if (queue_packet != NULL) {
queue_packet();
}
}
}
return false;
}
int handle_packet( qqclient* qq, qqpacket* p, uchar* data, int len )
{
qqpacketmgr* mgr = &qq->packetmgr;
mgr->recv_packets ++;
bytebuffer* buf;
NEW( buf, sizeof( bytebuffer ) );
if( !buf ){
DBG("error: no enough memory to allocate buf.");
return -99;
}
buf->pos = 0;
buf->len = buf->size = len;
memcpy( buf->data, data, buf->len );
//get packet info
if( qq->network == TCP || qq->network == PROXY_HTTP )
get_word( buf ); //packet len
p->head = get_byte( buf );
p->tail = buf->data[buf->len-1];
if( p->head != 0x02 || p->tail!=0x03 || buf->len > 2000 ){
DBG("[%u] wrong packet. len=%d head: %d tail: %d", qq->number, buf->len, p->head, p->tail );
DEL( buf );
return -1;
}
p->version = get_word( buf );
p->command = get_word( buf );
p->seqno = get_word( buf );
uint chk_repeat = (p->seqno<<16)|p->command;
//check repeat
if( loop_search( &mgr->recv_loop, (void*)chk_repeat, repeat_searcher ) == NULL ){
loop_push_to_tail( &mgr->recv_loop, (void*)chk_repeat );
p->match = match_packet( mgr, p );
p->time_alive = time(NULL);
//do a test
if ( !p->buf ){
DBG("%u: Error impossible. p->buf: %x p->command: %x", qq->number, p->buf, p->command );
}
//deal with the packet
process_packet( qq, p, buf );
qqpacket* t;
while( (t = loop_pop_from_tail( &mgr->temp_loop )) ){
loop_push_to_head( &mgr->ready_loop, t );
}
if( p->match ){
loop_remove( &mgr->sent_loop, p->match );
delete_func( p->match );
}
p->match = NULL;
mgr->failed_packets = 0;
}else{
// DBG("repeated packet: cmd: %x seqno:%x", p->command, p->seqno );
}
DEL( buf );
check_ready_packets( qq );
return 0;
}
/*----------------------------------------------------------------------------*/
void
handle_data(packet_t* p)
{
if (is_my_address(&(p->header.dst)))
{
PRINTF("[PHD]: Consuming Packet\n");
packet_deallocate(p);
} else {
match_packet(p);
}
}
/*----------------------------------------------------------------------------*/
void
handle_response(packet_t* p)
{
if (is_my_address(&(p->header.dst)))
{
entry_t* e = get_entry_from_array(p->payload, p->header.len - PLD_INDEX);
if (e != NULL)
{
add_entry(e);
}
packet_deallocate(p);
} else {
match_packet(p);
}
}
/*----------------------------------------------------------------------------*/
int
run_action(packet_t* p, uint8_t* s, action_t* a){
int len = list_length(a->bytes);
int i = 0;
uint8_t action_array[len];
byte_t* b;
if (len>0){
for(b = list_head(a->bytes); b != NULL; b = b->next) {
action_array[i] = b->value;
++i;
}
}
switch(a->type){
case FORWARD_U:
p->header.nxh = get_address_from_array(action_array);
rf_unicast_send(p);
break;
case FORWARD_B:
p->header.nxh = get_address_from_array(action_array);
rf_broadcast_send(p);
break;
case ASK:
create_and_send_request(p);
break;
case MATCH:
// TODO there may be problems if match is in the middle of a list of actions
match_packet(p);
break;
case FUNCTION:
// TODO function
break;
case SET:
set_action(p, s, action_array);
break;
default:
packet_deallocate(p);
break;
}
return -1;
}
/*----------------------------------------------------------------------------*/
void
test_flowtable(void)
{
uint8_t array[25] = {
20, 18, 0, 6, 0, 2, 82, 0, 15, 0, 1, 114,
0, 16, 0, 2, 50, 0, 17, 0, 5, 1, 4, 254,
0};
if (!list_length(flowtable)){
entry_t* e = get_entry_from_array(array,25);
if (e != NULL){
add_entry(e);
}
}else{
entry_free(list_pop(flowtable));
}
uint8_t array1[116] = {
1, 116, 0, 0, 0, 2, 2, 100, 0, 0,
11, 12, 13, 14, 15, 16, 17, 18, 19, 20,
21, 90, 10, 10, 10, 11, 22, 10, 12, 10,
35, 11, 14, 18, 16, 12, 10, 10, 10, 40,
40, 10, 18, 15, 11, 10, 10, 10, 10, 10,
50, 11, 13, 13, 12, 25, 25, 31, 11, 01,
61, 11, 17, 18, 16, 13, 10, 11, 10, 12,
71, 11, 17, 18, 16, 13, 10, 11, 10, 12,
81, 11, 17, 18, 16, 13, 10, 11, 10, 12,
91, 11, 17, 18, 16, 13, 10, 11, 10, 12,
11, 11, 17, 18, 16, 13, 10, 11, 10, 12,
10, 13, 254,11, 12, 13
};
packet_t* p = get_packet_from_array(array1);
match_packet(p);
packet_deallocate(p);
}
static bool
match(const struct sk_buff *skb,
const struct net_device *in,
const struct net_device *out,
const struct xt_match *match,
const void *matchinfo,
int offset,
unsigned int protoff,
bool *hotdrop)
{
const struct xt_sctp_info *info = matchinfo;
sctp_sctphdr_t _sh, *sh;
if (offset) {
duprintf("Dropping non-first fragment.. FIXME\n");
return false;
}
sh = skb_header_pointer(skb, protoff, sizeof(_sh), &_sh);
if (sh == NULL) {
duprintf("Dropping evil TCP offset=0 tinygram.\n");
*hotdrop = true;
return false;
}
duprintf("spt: %d\tdpt: %d\n", ntohs(sh->source), ntohs(sh->dest));
return SCCHECK(ntohs(sh->source) >= info->spts[0]
&& ntohs(sh->source) <= info->spts[1],
XT_SCTP_SRC_PORTS, info->flags, info->invflags)
&& SCCHECK(ntohs(sh->dest) >= info->dpts[0]
&& ntohs(sh->dest) <= info->dpts[1],
XT_SCTP_DEST_PORTS, info->flags, info->invflags)
&& SCCHECK(match_packet(skb, protoff + sizeof (sctp_sctphdr_t),
info, hotdrop),
XT_SCTP_CHUNK_TYPES, info->flags, info->invflags);
}
/*----------------------------------------------------------------------------*/
void
handle_config(packet_t* p)
{
if (is_my_address(&(p->header.dst)))
{
#if SINK
if (!is_my_address(&(p->header.src))){
print_packet_uart(p);
} else {
#endif
uint8_t i = 0;
uint8_t id = p->payload[i] & 127;
if ((p->payload[i] & 128) == CNF_READ)
{
//READ
switch (id)
{
// TODO
case RESET:
case GET_ALIAS:
case GET_RULE:
case GET_FUNCTION:
break;
case MY_NET:
case MY_ADDRESS:
case PACKET_TTL:
case RSSI_MIN:
case BEACON_PERIOD:
case REPORT_PERIOD:
case RULE_TTL:
// TODO check payload size
if (conf_size[id] == 1){
memcpy(&(p->payload[i+1]), conf_ptr[id], conf_size[id]);
} else if (conf_size[id] == 2) {
uint16_t value = *((uint16_t*)conf_ptr[id]);
p->payload[i+1] = value >> 8;
p->payload[i+2] = value & 0xFF;
}
break;
default:
break;
}
swap_addresses(&(p->header.src),&(p->header.dst));
p->header.len += conf_size[id];
match_packet(p);
} else {
//WRITE
switch (id)
{
// TODO
case ADD_ALIAS:
case REM_ALIAS:
case ADD_RULE:
case REM_RULE:
case ADD_FUNCTION:
case REM_FUNCTION:
break;
case RESET:
watchdog_reboot();
break;
case MY_NET:
case MY_ADDRESS:
case PACKET_TTL:
case RSSI_MIN:
case BEACON_PERIOD:
case REPORT_PERIOD:
case RULE_TTL:
if (conf_size[id] == 1){
memcpy((uint8_t*)conf_ptr[id], &(p->payload[i+1]), conf_size[id]);
} else if (conf_size[id] == 2) {
uint16_t h = p->payload[i+1] << 8;
uint16_t l = p->payload[i+2];
*((uint16_t*)conf_ptr[id]) = h + l;
}
break;
default:
break;
}
packet_deallocate(p);
}
#if SINK
}
/*----------------------------------------------------------------------------*/
void
handle_open_path(packet_t* p)
{
int i;
uint8_t n_windows = get_payload_at(p,OPEN_PATH_WINDOWS_INDEX);
uint8_t start = n_windows*WINDOW_SIZE + 1;
uint8_t path_len = (p->header.len - (start + PLD_INDEX))/ADDRESS_LENGTH;
int my_index = -1;
uint8_t my_position = 0;
uint8_t end = p->header.len - PLD_INDEX;
for (i = start; i < end; i += ADDRESS_LENGTH)
{
address_t tmp = get_address_from_array(&(p->payload[i]));
if (is_my_address(&tmp))
{
my_index = i;
break;
}
my_position++;
}
if (my_index == -1){
printf("[PHD]: Nothing to learn, matching...\n");
match_packet(p);
} else {
if (my_position > 0)
{
uint8_t prev = my_index - ADDRESS_LENGTH;
uint8_t first = start;
entry_t* e = create_entry();
window_t* w = create_window();
w->operation = EQUAL;
w->size = SIZE_2;
w->lhs = DST_INDEX;
w->lhs_location = PACKET;
w->rhs = MERGE_BYTES(p->payload[first], p->payload[first+1]);
w->rhs_location = CONST;
add_window(e,w);
for (i = 0; i<n_windows; ++i)
{
add_window(e, get_window_from_array(&(p->payload[i*WINDOW_SIZE + 1])));
}
action_t* a = create_action(FORWARD_U, &(p->payload[prev]), ADDRESS_LENGTH);
add_action(e,a);
add_entry(e);
}
if (my_position < path_len-1)
{
uint8_t next = my_index + ADDRESS_LENGTH;
uint8_t last = end - ADDRESS_LENGTH;
entry_t* e = create_entry();
window_t* w = create_window();
w->operation = EQUAL;
w->size = SIZE_2;
w->lhs = DST_INDEX;
w->lhs_location = PACKET;
w->rhs = MERGE_BYTES(p->payload[last], p->payload[last+1]);
w->rhs_location = CONST;
add_window(e,w);
for (i = 0; i<n_windows; ++i)
{
add_window(e, get_window_from_array(&(p->payload[i*WINDOW_SIZE + 1])));
}
action_t* a = create_action(FORWARD_U, &(p->payload[next]), ADDRESS_LENGTH);
add_action(e,a);
add_entry(e);
address_t next_address = get_address_from_array(&(p->payload[next]));
p->header.nxh = next_address;
p->header.dst = next_address;
rf_unicast_send(p);
}
if (my_position == path_len-1){
packet_deallocate(p);
}
}
}