/*----------------------------------------------------------------------------*/ 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 handle_beacon(packet_t* p) { add_neighbor(&(p->header.src),p->info.rssi); #if !SINK // TODO what if the network changes? uint8_t new_hops = get_payload_at(p, BEACON_HOPS_INDEX); if (new_hops <= conf.hops_from_sink-1 && p->info.rssi > conf.rssi_from_sink) { conf.nxh_vs_sink = p->header.src; conf.hops_from_sink = new_hops+1; conf.rssi_from_sink = p->info.rssi; conf.sink_address = p->header.nxh; } #endif packet_deallocate(p); }
/*----------------------------------------------------------------------------*/ void handle_packet(packet_t* p) { if (p->info.rssi >= conf.rssi_min && p->header.net == conf.my_net){ if (p->header.typ == BEACON){ PRINTF("[PHD]: Beacon\n"); handle_beacon(p); } else { if (is_my_address(&(p->header.nxh))){ switch (p->header.typ){ case DATA: PRINTF("[PHD]: Data\n"); handle_data(p); break; case RESPONSE: PRINTF("[PHD]: Response\n"); handle_response(p); break; case OPEN_PATH: PRINTF("[PHD]: Open Path\n"); handle_open_path(p); break; case CONFIG: PRINTF("[PHD]: Config\n"); handle_config(p); break; default: PRINTF("[PHD]: Request/Report\n"); handle_report(p); break; } } } } else { packet_deallocate(p); } }
/*----------------------------------------------------------------------------*/ 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); }
/*----------------------------------------------------------------------------*/ void handle_beacon(packet_t* p) { add_neighbor(&(p->header.src),p->info.rssi); #if !SINK uint8_t new_hops = get_payload_at(p, BEACON_HOPS_INDEX); uint8_t new_distance = p->info.rssi; if (address_cmp(&(conf.nxh_vs_sink), &(p->header.src)) || #if MOBILE (new_distance < conf.distance_from_sink) #else (new_hops <= conf.hops_from_sink-1 && new_distance < conf.distance_from_sink) #endif ) { conf.nxh_vs_sink = p->header.src; conf.distance_from_sink = new_distance; conf.sink_address = p->header.nxh; conf.hops_from_sink = new_hops+1; } #endif packet_deallocate(p); }
/*----------------------------------------------------------------------------*/ 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; uint8_t my_index = 0; 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_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); PRINTF("[PHD]: "); print_entry(e); PRINTF("\n"); 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); PRINTF("[PHD]: "); print_entry(e); PRINTF("\n"); 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); } }