void network_handler(ev, data){
char buf[UIP_BUFSIZE]; // packet data buffer
unsigned short cmd; //
DataPayload *dp;
ChannelState *state = NULL;
uint16_t len = uip_datalen();
PRINTF("ipaddr=%d.%d.%d.%d\n", uip_ipaddr_to_quad(&(UDP_HDR->srcipaddr)));
PRINTF("Packet is %d bytes long\n",len);
memcpy(buf, uip_appdata, len);
buf[len] = '\0';
dp = (DataPayload *)buf;
PRINTF("Data is %d bytes long\n",uip_ntohs(dp->dhdr.tlen));
cmd = dp->hdr.cmd; // only a byte so no reordering :)
PRINTF("Received a %s command.\n", cmdnames[cmd]);
PRINTF("Message for channel %d\n",dp->hdr.dst_chan_num);
if (dp->hdr.dst_chan_num == HOMECHANNEL || cmd == DISCONNECT){
if (cmd == QACK){
state = &home_channel_state;
copy_link_address(state);
}
else if (cmd == DISCONNECT){
state = get_channel_state(dp->hdr.dst_chan_num);
if (state){
remove_channel(state->chan_num);
}
state = &home_channel_state;
copy_link_address(state);
}
}
else{
state = get_channel_state(dp->hdr.dst_chan_num);
if (state == NULL){
PRINTF("Channel %d doesn't exist\n", dp->hdr.dst_chan_num);
return;
}
if (check_seqno(state, dp) == 0) {
printf("OH NOES\n");
return;
}else { //CHECK IF RIGHT CONNECTION
//copy_link_address(state);
}
}
// Received a message so reset pingOUT
state->pingOUT = 0;
if (cmd == QUERY) PRINTF("I'm a controller, Ignoring QUERY\n");
else if (cmd == CONNECT) PRINTF("I'm a controller, Ignoring CONNECT\n");
else if (cmd == QACK) qack_handler(state, dp);
else if (cmd == CACK) cack_handler(state, dp);
else if (cmd == RESPONSE) response_handler(state, dp);
else if (cmd == CMDACK) command_ack_handler(state,dp);
else if (cmd == PING) ping_handler(state, dp);
else if (cmd == PACK) pack_handler(state, dp);
else if (cmd == DISCONNECT) close_handler(state,dp);
}
static void
handle_incoming_data()
{
PRINTF("Incoming packet size: %u \n", (u16_t)uip_datalen());
if (init_buffer(COAP_DATA_BUFF_SIZE)) {
if (uip_newdata()) {
coap_packet_t* response = (coap_packet_t*)allocate_buffer(sizeof(coap_packet_t));
parse_message(response, uip_appdata, uip_datalen());
if (response) {
response_handler(response);
}
}
delete_buffer();
}
}
static ENGINE_ERROR_CODE execute_command(const void *cmd_cookie, const void *cookie,
int argc, mc_extension_token_t *argv,
ENGINE_ERROR_CODE (*response_handler)(const void *cookie,
int nbytes,
const char *dta))
{
if (cmd_cookie == &noop_descriptor) {
return response_handler(cookie, 4, "OK\r\n");
} else {
if (response_handler(cookie, argv[0].length, argv[0].value) != ENGINE_SUCCESS) {
return ENGINE_DISCONNECT;
}
for (int ii = 1; ii < argc; ++ii) {
if (response_handler(cookie, 2, " [") != ENGINE_SUCCESS ||
response_handler(cookie, argv[ii].length, argv[ii].value) != ENGINE_SUCCESS ||
response_handler(cookie, 1, "]") != ENGINE_SUCCESS) {
return ENGINE_DISCONNECT;
}
}
return response_handler(cookie, 2, "\r\n");
}
}
static int parse_fifo(struct inv_icm30xxx * s, const uint8_t * buffer, uint32_t len)
{
int rc = 0;
struct FifoProtocolPacket packet;
uint32_t index = 0;
int32_t remaining = (int32_t)len;
inv_fifo_protocol_parse_packet_reset(&packet);
packet.type = FIFOPROTOCOL_PACKET_TYPE_DATA;
while(remaining >= FIFOPROTOCOL_PACKET_SIZE) {
/* check header / footer */
if(buffer[index] != FIFOPROTOCOL_PROTECTED_HEADER
|| buffer[index+FIFOPROTOCOL_PACKET_SIZE-1] != FIFOPROTOCOL_PROTECTED_FOOTER) {
INV_MSG(INV_MSG_LEVEL_VERBOSE, "Bad header/footer in fifo packet, dropping one byte...");
index += 1;
remaining -= 1;
continue;
}
/* header/footer ok - parse packet */
rc = inv_fifo_protocol_parse_packet_r(&packet, &buffer[index],
FIFOPROTOCOL_PACKET_SIZE, s->fifop_exp_pkt_cnt);
/* retry if parsing error with last packet */
if(rc == FIFOPROTOCOL_ERROR_PARSE) {
INV_MSG(INV_MSG_LEVEL_VERBOSE, "Error parsing fifo packet. One packet lost."
" Retrying with current packet...");
rc = inv_fifo_protocol_parse_packet_r(&packet, &buffer[index],
FIFOPROTOCOL_PACKET_SIZE, s->fifop_exp_pkt_cnt);
}
/* invalid packet, skip it */
if(rc == FIFOPROTOCOL_ERROR_SIZE || rc == FIFOPROTOCOL_ERROR_PARSE) {
INV_MSG(INV_MSG_LEVEL_VERBOSE, "Error parsing fifo packet. Dropping full packet...");
}
/* valid packet, call handlers to notify upper layer */
else if(rc == FIFOPROTOCOL_ERROR_NO) {
if(packet.type == FIFOPROTOCOL_PACKET_TYPE_ANSWER) {
response_handler(s, packet.sensorid,
(FifoProtocolCmd)packet.u.command.cmd,
packet.arg, packet.size);
} else {
data_handler(s, packet.sensorid,
packet.u.data.timestamp, packet.u.data.status,
packet.u.data.accuracy, packet.arg, packet.size);
}
}
else {
/* incomplete packet */
}
/* parse next packet */
index += FIFOPROTOCOL_PACKET_SIZE;
remaining -= FIFOPROTOCOL_PACKET_SIZE;
}
/* If we did not receive all packets needed for current sensor id to be fully parsed, then make
sure the already analyzed packet(s) will be part of next call to this function */
if (rc == FIFOPROTOCOL_ERROR_INCOMPLETE) {
// INV_MSG(INV_MSG_LEVEL_DEBUG, "Incomplete fifo packet, rewind fifo index.");
remaining += packet.states.packet_cnt * FIFOPROTOCOL_PACKET_SIZE;
}
/* return number of remaining bytes from input buffer */
return remaining;
}