int lowpan_recon_start(struct ieee154_frame_addr *frame_addr, struct lowpan_reconstruct *recon, uint8_t *pkt, size_t len) { uint8_t *unpack_point, *unpack_end; struct packed_lowmsg msg; msg.data = pkt; msg.len = len; msg.headers = getHeaderBitmap(&msg); if (msg.headers == LOWMSG_NALP) return -1; /* remove the 6lowpan headers from the payload */ unpack_point = getLowpanPayload(&msg); len -= (unpack_point - pkt); /* set up the reconstruction, or just fill in the packet length */ if (hasFrag1Header(&msg)) { getFragDgramTag(&msg, &recon->r_tag); getFragDgramSize(&msg, &recon->r_size); } else { recon->r_size = LIB6LOWPAN_MAX_LEN + LOWPAN_LINK_MTU; } recon->r_buf = malloc(recon->r_size); if (!recon->r_buf) return -2; memset(recon->r_buf, 0, recon->r_size); recon->r_app_len = NULL; if (*unpack_point == LOWPAN_IPV6_PATTERN) { /* uncompressed header... no need to un-hc */ unpack_point++; len --; memcpy(recon->r_buf, unpack_point, len); unpack_end = recon->r_buf + len; } else { /* unpack the first fragment */ unpack_end = lowpan_unpack_headers(recon, frame_addr, unpack_point, len); } if (!unpack_end) { free(recon->r_buf); return -3; } if (!hasFrag1Header(&msg)) { recon->r_size = (unpack_end - recon->r_buf); } recon->r_bytes_rcvd = unpack_end - recon->r_buf; ((struct ip6_hdr *)(recon->r_buf))->ip6_plen = htons(recon->r_size - sizeof(struct ip6_hdr)); /* fill in any elided app data length fields */ if (recon->r_app_len) { *recon->r_app_len = htons(recon->r_size - (recon->r_transport_header - recon->r_buf)); } /* done, updated all the fields */ /* reconstruction is complete if r_bytes_rcvd == r_size */ return 0; }
void *pan_read(void * arg) { while (run_state == S_RUNNING) { struct packed_lowmsg lowmsg; struct ieee154_frame_addr frame_address; int length; uint8_t *frame = read_pan_packet(&length); uint8_t *buf = frame + 1; /* skip the dispatch byte */ if (!frame) continue; if (frame[0] != TOS_SERIAL_802_15_4_ID) { warn("invalid frame received!\n"); goto done; } info("serial packet arrived! (len: %i)\n", length); print_buffer(buf, length); buf = unpack_ieee154_hdr(buf, &frame_address); length -= buf - frame; if (!buf) { warn("unpacking IEEE154 header failed!\n"); goto done; } lowmsg.data = buf; lowmsg.len = length; lowmsg.headers = getHeaderBitmap(&lowmsg); if (lowmsg.headers == LOWMSG_NALP) { warn("lowmsg NALP!\n"); goto done; } if (hasFrag1Header(&lowmsg) || hasFragNHeader(&lowmsg)) { struct lowpan_reconstruct *recon; uint16_t tag, source_key; int rv; pthread_mutex_lock(&reconstruct_lock); source_key = ieee154_hashaddr(&frame_address.ieee_src); getFragDgramTag(&lowmsg, &tag); recon = get_reconstruct(source_key, tag); if (!recon) { pthread_mutex_unlock(&reconstruct_lock); goto done; } if (hasFrag1Header(&lowmsg)) rv = lowpan_recon_start(&frame_address, recon, buf, length); else rv = lowpan_recon_add(recon, buf, length); if (rv < 0) { recon->r_timeout = T_FAILED1; pthread_mutex_unlock(&reconstruct_lock); goto done; } else { recon->r_timeout = T_ACTIVE; recon->r_source_key = source_key; recon->r_tag = tag; } if (recon->r_size == recon->r_bytes_rcvd) { deliver_to_kernel(recon); } pthread_mutex_unlock(&reconstruct_lock); } else { int rv; struct lowpan_reconstruct recon; buf = getLowpanPayload(&lowmsg); if ((rv = lowpan_recon_start(&frame_address, &recon, buf, length)) < 0) { warn("reconstruction failed!\n"); goto done; } if (recon.r_size == recon.r_bytes_rcvd) { deliver_to_kernel(&recon); } else { free(recon.r_buf); } } done: free(frame); } }
int randTest() { int place = 0; packed_lowmsg_t pkt; pkt.data = buf; pkt.len = 20; uint16_t mesh_orig, mesh_final, frag_size, frag_tag; uint8_t mesh_hops, bcast_seq, frag_offset; uint8_t mesh, bcast, frag1, fragN; uint16_t val16; uint8_t val8; mesh_orig = rand() % 0xffff; mesh_final = rand() % 0xffff; frag_size = rand() % 0x07ff; frag_tag = rand() % 0xffff; mesh_hops = rand() % 0x0f; bcast_seq = rand() % 0xff; frag_offset = rand() & 0xff; mesh = rand() % 2; bcast = rand() % 2; frag1 = (rand() % 3); if (frag1 == 0) fragN = 1; else fragN = 0; setupHeaders(&pkt, mesh, bcast, frag1, fragN); if (mesh) { setMeshHopsLeft(&pkt, mesh_hops); setMeshOriginAddr(&pkt, mesh_orig); setMeshFinalAddr(&pkt, mesh_final); } if (bcast) { setBcastSeqno(&pkt, bcast_seq); } if (frag1) { setFragDgramSize(&pkt, frag_size); setFragDgramTag(&pkt, frag_tag); } if (fragN) { setFragDgramSize(&pkt, frag_size); setFragDgramTag(&pkt, frag_tag); setFragDgramOffset(&pkt, frag_offset); } // test it out. if (mesh) { if (getMeshHopsLeft(&pkt, &val8)) { place = 1; goto done; } if (val8 != mesh_hops) { place = 2; goto done; } if (getMeshOriginAddr(&pkt, &val16)) { place = 3; goto done; } if (val16 != mesh_orig) { place = 4; goto done; } if (getMeshFinalAddr(&pkt, &val16)) { place = 5; goto done; } if (val16 != mesh_final) { place = 6; goto done; } } if (bcast) { if (getBcastSeqno(&pkt, &val8)) { place = 7; goto done; } if (val8 != bcast_seq) { place = 8; goto done; } } if (frag1) { if (getFragDgramSize(&pkt, &val16)) { place = 9; goto done; } if (val16 != frag_size) { place = 10; goto done; } if (getFragDgramTag(&pkt, &val16)) { place = 11; goto done; } if (val16 != frag_tag) { place = 12; goto done; } } if (fragN) { if (getFragDgramSize(&pkt, &val16)) { place = 13; goto done; } if (val16 != frag_size) { place = 14; goto done; } if (getFragDgramTag(&pkt, &val16)) { place = 15; goto done; } if (val16 != frag_tag) { place = 16; goto done; } if (getFragDgramOffset(&pkt, &val8)) { place = 17; goto done; } if (val8 != frag_offset) { place = 18; goto done; } } return 0; done: printf("\nThere was an error: place %i\n", place); printf(" Test headers: mesh: %i bcast: %i frag1: %i fragN: %i\n", mesh, bcast, frag1, fragN); if (mesh) printf(" mesh hops: 0x%x origin: 0x%x final: 0x%x\n", mesh_hops, mesh_orig, mesh_final); if (bcast) printf(" bcast seqno: 0x%x\n", bcast_seq); if (frag1) printf(" frag1 size: 0x%x tag: 0x%x\n", frag_size, frag_tag); if (fragN) printf(" fragN size: 0x%x tag: 0x%x offset: 0x%x\n", frag_size, frag_tag, frag_offset); printf(" Packed packet contents:\n"); printPacket(buf, 100); printf("\n"); return 1; }