diff --git a/include/nuttx/net/sixlowpan.h b/include/nuttx/net/sixlowpan.h index 0211d17ce6f..04b69da6048 100644 --- a/include/nuttx/net/sixlowpan.h +++ b/include/nuttx/net/sixlowpan.h @@ -80,7 +80,7 @@ #define SIXLOWPAN_DISPATCH_FRAG1 0xc0 /* 11000xxx */ #define SIXLOWPAN_DISPATCH_FRAGN 0xe0 /* 11100xxx */ -#define SIXLOWPAN_DISPATCH_FRAG_MASK 0xf1 /* 11111000 */ +#define SIXLOWPAN_DISPATCH_FRAG_MASK 0xf8 /* 11111000 */ /* HC1 encoding */ @@ -398,6 +398,12 @@ struct ieee802154_driver_s uint16_t i_reasstag; + /* i_boffset. Offset to the beginning of data in d_buf. As each fragment + * is received, data is placed at an appriate offset added to this. + */ + + uint16_t i_boffset; + /* The source MAC address of the fragments being merged */ struct rimeaddr_s i_fragsrc; diff --git a/net/sixlowpan/sixlowpan_framelist.c b/net/sixlowpan/sixlowpan_framelist.c index 994b7bfc7b4..8d8900460f5 100644 --- a/net/sixlowpan/sixlowpan_framelist.c +++ b/net/sixlowpan/sixlowpan_framelist.c @@ -202,8 +202,8 @@ static void sixlowpan_copy_protohdr(FAR const struct ipv6_hdr_s *ipv6hdr, memcpy(fptr + g_frame_hdrlen, (FAR uint8_t *)ipv6hdr + g_uncomp_hdrlen, copysize); - g_frame_hdrlen += copysize; - g_uncomp_hdrlen += copysize; + g_frame_hdrlen += copysize; + g_uncomp_hdrlen += copysize; } /**************************************************************************** @@ -252,6 +252,7 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, FAR uint8_t *fptr; int framer_hdrlen; struct rimeaddr_s bcastmac; + uint16_t pktlen; uint16_t paysize; #ifdef CONFIG_NET_6LOWPAN_FRAG uint16_t outlen = 0; @@ -365,6 +366,7 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, */ FAR struct iob_s *qtail; + FAR uint8_t *frame1; int verify; /* The outbound IPv6 packet is too large to fit into a single 15.4 @@ -383,7 +385,9 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, DEBUGASSERT(verify == framer_hdrlen); UNUSED(verify); - /* Move HC1/HC06/IPv6 header */ + /* Move HC1/HC06/IPv6 header to make space for the FRAG1 header at the + * beginning of the frame. + */ memmove(fptr + SIXLOWPAN_FRAG1_HDR_LEN, fptr, g_frame_hdrlen); @@ -402,11 +406,10 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, * bytes for all subsequent headers. */ + pktlen = buflen + g_uncomp_hdrlen; PUTINT16(fptr, RIME_FRAG_DISPATCH_SIZE, - ((SIXLOWPAN_DISPATCH_FRAG1 << 8) | buflen)); - + ((SIXLOWPAN_DISPATCH_FRAG1 << 8) | pktlen)); PUTINT16(fptr, RIME_FRAG_TAG, ieee->i_dgramtag); - ieee->i_dgramtag++; g_frame_hdrlen += SIXLOWPAN_FRAG1_HDR_LEN; @@ -414,9 +417,11 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, sixlowpan_copy_protohdr(destip, fptr); - /* Copy payload and enqueue */ + /* Copy payload and enqueue. NOTE that the size is a multiple of eight + * bytes. + */ - paysize = (CONFIG_NET_6LOWPAN_MAXPAYLOAD - g_frame_hdrlen) & 0xf8; + paysize = (CONFIG_NET_6LOWPAN_MAXPAYLOAD - g_frame_hdrlen) & ~7; memcpy(fptr + g_frame_hdrlen, buf, paysize); /* Set outlen to what we already sent from the IP payload */ @@ -440,10 +445,11 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, /* Create following fragments */ - g_frame_hdrlen = SIXLOWPAN_FRAGN_HDR_LEN; - + frame1 = iob->io_data; while (outlen < buflen) { + uint16_t fragn_hdrlen; + /* Allocate an IOB to hold the next fragment, waiting if * necessary. */ @@ -459,49 +465,44 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, iob->io_pktlen = 0; fptr = iob->io_data; - /* Add the frame header */ + /* Copy the frame header from first frame, into the correct + * location after the FRAGN header. + */ - verify = sixlowpan_framecreate(ieee, iob, ieee->i_panid); - DEBUGASSERT(verify == framer_hdrlen); - UNUSED(verify); + memmove(fptr + SIXLOWPAN_FRAGN_HDR_LEN, + frame1 + SIXLOWPAN_FRAG1_HDR_LEN, + framer_hdrlen); + fragn_hdrlen = framer_hdrlen; - /* Move HC1/HC06/IPv6 header */ - - memmove(fptr + SIXLOWPAN_FRAGN_HDR_LEN, fptr, g_frame_hdrlen); - - /* Setup up the fragment header */ + /* Setup up the FRAGN header at the beginning of the frame */ PUTINT16(fptr, RIME_FRAG_DISPATCH_SIZE, - ((SIXLOWPAN_DISPATCH_FRAGN << 8) | buflen)); + ((SIXLOWPAN_DISPATCH_FRAGN << 8) | pktlen)); PUTINT16(fptr, RIME_FRAG_TAG, ieee->i_dgramtag); fptr[RIME_FRAG_OFFSET] = outlen >> 3; - /* Copy protocol header that follows the IPv6 header */ - - sixlowpan_copy_protohdr(destip, fptr); + fragn_hdrlen += SIXLOWPAN_FRAGN_HDR_LEN; /* Copy payload and enqueue */ /* Check for the last fragment */ + paysize = (CONFIG_NET_6LOWPAN_MAXPAYLOAD - fragn_hdrlen) & + SIXLOWPAN_DISPATCH_FRAG_MASK; if (buflen - outlen < paysize) { /* Last fragment, truncate to the correct length */ paysize = buflen - outlen; } - else - { - paysize = (CONFIG_NET_6LOWPAN_MAXPAYLOAD - g_frame_hdrlen) & 0xf8; - } - memcpy(fptr + g_frame_hdrlen, buf + outlen, paysize); + memcpy(fptr + fragn_hdrlen, buf + outlen, paysize); /* Set outlen to what we already sent from the IP payload */ - iob->io_len = paysize + g_frame_hdrlen; + iob->io_len = paysize + fragn_hdrlen; outlen += paysize; - ninfo("Fragment offset %d, length %d, tag %d\n", + ninfo("Fragment offset=%d, paysize=%d, i_dgramtag=%d\n", outlen >> 3, paysize, ieee->i_dgramtag); sixlowpan_dumpbuffer("Outgoing frame", (FAR const uint8_t *)iob->io_data, @@ -516,6 +517,10 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, ieee->i_framelist->io_pktlen += iob->io_len; } + + /* Update the datagram TAG value */ + + ieee->i_dgramtag++; #else nerr("ERROR: Packet too large: %d\n", buflen); nerr(" Cannot to be sent without fragmentation support\n"); diff --git a/net/sixlowpan/sixlowpan_input.c b/net/sixlowpan/sixlowpan_input.c index 95bb7c1cee2..7144d36e8b4 100644 --- a/net/sixlowpan/sixlowpan_input.c +++ b/net/sixlowpan/sixlowpan_input.c @@ -111,12 +111,26 @@ int sixlowpan_recv_hdrlen(FAR const uint8_t *fptr) { - uint16_t hdrlen; + uint16_t hdrlen = 0; uint8_t addrmode; + uint8_t tmp; + + /* Check for a fragment header preceding the IEEE802.15.4 FCF */ + + tmp = *fptr & SIXLOWPAN_DISPATCH_FRAG_MASK; + if (tmp == SIXLOWPAN_DISPATCH_FRAG1) + { + hdrlen += SIXLOWPAN_FRAG1_HDR_LEN; + } + else if (tmp == SIXLOWPAN_DISPATCH_FRAGN) + { + hdrlen += SIXLOWPAN_FRAGN_HDR_LEN; + } /* Minimum header: 2 byte FCF + 1 byte sequence number */ - hdrlen = 3; + fptr += hdrlen; + hdrlen += 3; /* Account for destination address size */ @@ -209,9 +223,7 @@ int sixlowpan_recv_hdrlen(FAR const uint8_t *fptr) static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, FAR struct iob_s *iob) { - FAR uint8_t *payptr; /* Pointer to the frame payload */ FAR uint8_t *hc1; /* Convenience pointer to HC1 data */ - uint16_t fragsize = 0; /* Size of the IP packet (read from fragment) */ uint16_t paysize; /* Size of the data payload */ uint8_t fragoffset = 0; /* Offset of the fragment in the IP packet */ @@ -221,12 +233,13 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, #ifdef CONFIG_NET_6LOWPAN_FRAG bool isfrag = false; bool isfirstfrag = false; - bool islastfrag = false; uint16_t fragtag = 0; /* Tag of the fragment */ systime_t elapsed; /* Elapsed time */ #endif /* CONFIG_NET_6LOWPAN_FRAG */ - /* Get a pointer to the payload following the IEEE802.15.4 frame header. */ + /* Get a pointer to the payload following the IEEE802.15.4 frame header(s). + * This size includes both fragmentation and FCF headers. + */ hdrsize = sixlowpan_recv_hdrlen(iob->io_data); if (hdrsize < 0) @@ -242,16 +255,13 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, g_uncomp_hdrlen = 0; g_frame_hdrlen = hdrsize; - /* Payload starts after the IEEE802.15.4 header */ - - payptr = &iob->io_data[hdrsize]; - #ifdef CONFIG_NET_6LOWPAN_FRAG /* Since we don't support the mesh and broadcast header, the first header - * we look for is the fragmentation header + * we look for is the fragmentation header. NOTE that g_frame_hdrlen + * already includes the fragementation header, if presetn. */ - switch ((GETINT16(payptr, RIME_FRAG_DISPATCH_SIZE) & 0xf800) >> 8) + switch ((GETINT16(iob->io_data, RIME_FRAG_DISPATCH_SIZE) & 0xf800) >> 8) { /* First fragment of new reassembly */ @@ -259,15 +269,12 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, { /* Set up for the reassembly */ - fragoffset = 0; - fragsize = GETINT16(payptr, RIME_FRAG_DISPATCH_SIZE) & 0x07ff; - fragtag = GETINT16(payptr, RIME_FRAG_TAG); + fragsize = GETINT16(iob->io_data, RIME_FRAG_DISPATCH_SIZE) & 0x07ff; + fragtag = GETINT16(iob->io_data, RIME_FRAG_TAG); - ninfo("FRAG1: size %d, tag %d, offset %d\n", + ninfo("FRAG1: fragsize=%d fragtag=%d fragoffset=%d\n", fragsize, fragtag, fragoffset); - g_frame_hdrlen += SIXLOWPAN_FRAG1_HDR_LEN; - /* Indicate the first fragment of the reassembly */ isfirstfrag = true; @@ -279,32 +286,18 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, { /* Set offset, tag, size. Offset is in units of 8 bytes. */ - fragoffset = payptr[RIME_FRAG_OFFSET]; - fragtag = GETINT16(payptr, RIME_FRAG_TAG); - fragsize = GETINT16(payptr, RIME_FRAG_DISPATCH_SIZE) & 0x07ff; + fragoffset = iob->io_data[RIME_FRAG_OFFSET]; + fragtag = GETINT16(iob->io_data, RIME_FRAG_TAG); + fragsize = GETINT16(iob->io_data, RIME_FRAG_DISPATCH_SIZE) & 0x07ff; - ninfo("FRAGN: size=%d tag=%d offset=%d\n", + ninfo("FRAGN: fragsize=%d fragtag=%d fragoffset=%d\n", fragsize, fragtag, fragoffset); - - g_frame_hdrlen += SIXLOWPAN_FRAGN_HDR_LEN; - ninfo("FRAGN: i_accumlen=%d paysize=%u fragsize=%u\n", ieee->i_accumlen, iob->io_len - g_frame_hdrlen, fragsize); /* Indicate that this frame is a another fragment for reassembly */ isfrag = true; - - /* Check if it is the last fragement to be processed. - * - * If this is the last fragment, we may shave off any extrenous - * bytes at the end. We must be liberal in what we accept. - */ - - if (ieee->i_accumlen + iob->io_len - g_frame_hdrlen >= fragsize) - { - islastfrag = true; - } } break; @@ -393,6 +386,7 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, * compression dispatch logic. */ + g_uncomp_hdrlen = ieee->i_boffset; goto copypayload; } } @@ -413,29 +407,22 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, return OK; } - /* Start reassembly if we received a non-zero length, first fragment */ + /* Drop the packet if it cannot fit into the d_buf */ - if (fragsize > 0) + if (fragsize > CONFIG_NET_6LOWPAN_MTU) { - /* Drop the packet if it cannot fit into the d_buf */ - - if (fragsize > CONFIG_NET_6LOWPAN_MTU) - { - nwarn("WARNING: Reassembled packet size exeeds CONFIG_NET_6LOWPAN_MTU\n"); - return OK; - } - - /* Set up for the reassembly */ - - ieee->i_pktlen = fragsize; - ieee->i_reasstag = fragtag; - ieee->i_time = clock_systimer(); - - ninfo("Starting reassembly: i_pktlen %d, i_pktlen %d\n", - ieee->i_pktlen, ieee->i_reasstag); - - rimeaddr_copy(&ieee->i_fragsrc, &g_pktaddrs[PACKETBUF_ADDR_SENDER]); + nwarn("WARNING: Reassembled packet size exeeds CONFIG_NET_6LOWPAN_MTU\n"); + return OK; } + + ieee->i_pktlen = fragsize; + ieee->i_reasstag = fragtag; + ieee->i_time = clock_systimer(); + + ninfo("Starting reassembly: i_pktlen %u, i_reasstag %d\n", + ieee->i_pktlen, ieee->i_reasstag); + + rimeaddr_copy(&ieee->i_fragsrc, &g_pktaddrs[PACKETBUF_ADDR_SENDER]); } #endif /* CONFIG_NET_6LOWPAN_FRAG */ @@ -446,7 +433,13 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, #ifdef CONFIG_NET_6LOWPAN_COMPRESSION_HC06 if ((hc1[RIME_HC1_DISPATCH] & SIXLOWPAN_DISPATCH_IPHC_MASK) == SIXLOWPAN_DISPATCH_IPHC) { + FAR uint8_t *payptr; + ninfo("IPHC Dispatch\n"); + + /* Payload starts after the IEEE802.15.4 header(s) */ + + payptr = &iob->io_data[g_frame_hdrlen]; sixlowpan_uncompresshdr_hc06(ieee, fragsize, iob, payptr); } else @@ -455,7 +448,13 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, #ifdef CONFIG_NET_6LOWPAN_COMPRESSION_HC1 if (hc1[RIME_HC1_DISPATCH] == SIXLOWPAN_DISPATCH_HC1) { + FAR uint8_t *payptr; + ninfo("HC1 Dispatch\n"); + + /* Payload starts after the IEEE802.15.4 header(s) */ + + payptr = &iob->io_data[g_frame_hdrlen]; sixlowpan_uncompresshdr_hc1(ieee, fragsize, iob, payptr); } else @@ -468,16 +467,9 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, ninfo("IPv6 Dispatch\n"); g_frame_hdrlen += SIXLOWPAN_IPV6_HDR_LEN; - /* payptr was set up to begin just after the IPHC bytes. However, - * those bytes are not present for the case of IPv6 dispatch. Just - * reset back to the begnning of the buffer. - */ - - payptr = iob->io_data; - /* Put uncompressed IP header in d_buf. */ - memcpy(ipv6, payptr + g_frame_hdrlen, IPv6_HDRLEN); + memcpy(ipv6, iob->io_data + g_frame_hdrlen, IPv6_HDRLEN); /* Update g_uncomp_hdrlen and g_frame_hdrlen. */ @@ -493,6 +485,18 @@ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, } #ifdef CONFIG_NET_6LOWPAN_FRAG + /* Non-fragmented and FRAG1 frames pass through here. Remember the + * offset from the beginning of d_buf where be begin placing the data + * payload. + */ + + if (isfirstfrag) + { + ieee->i_boffset = g_uncomp_hdrlen; + } + + /* We branch to here on all good FRAGN frames */ + copypayload: #endif /* CONFIG_NET_6LOWPAN_FRAG */ @@ -512,17 +516,17 @@ copypayload: /* Sanity-check size of incoming packet to avoid buffer overflow */ - reqsize = g_uncomp_hdrlen + (uint16_t) (fragoffset << 3) + paysize; + reqsize = g_uncomp_hdrlen + (fragoffset << 3) + paysize; if (reqsize > CONFIG_NET_6LOWPAN_MTU) { - ninfo("Required buffer size: %d+%d+%d=%d Available: %d\n", - g_uncomp_hdrlen, (int)(fragoffset << 3), paysize, + ninfo("Required buffer size: %u+%u+%u=%u Available=%u\n", + g_uncomp_hdrlen, (fragoffset << 3), paysize, reqsize, CONFIG_NET_6LOWPAN_MTU); return -ENOMEM; } - memcpy((FAR uint8_t *)ieee->i_dev.d_buf + g_uncomp_hdrlen + - (int)(fragoffset << 3), payptr + g_frame_hdrlen, + memcpy(ieee->i_dev.d_buf + g_uncomp_hdrlen + (fragoffset << 3), + iob->io_data + g_frame_hdrlen, paysize); #ifdef CONFIG_NET_6LOWPAN_FRAG @@ -532,27 +536,13 @@ copypayload: if (isfrag) { - /* Add the size of the header only for the first fragment. */ - - if (isfirstfrag) - { - ieee->i_accumlen += g_uncomp_hdrlen; - } - - /* For the last fragment, we are OK if there is extraneous bytes at the - * end of the packet. + /* Check if it is the last fragment to be processed. + * + * If this is the last fragment, we may shave off any extrenous + * bytes at the end. We must be liberal in what we accept. */ - if (islastfrag) - { - ieee->i_accumlen = fragsize; - } - else - { - ieee->i_accumlen += paysize; - } - - ninfo("i_accumlen %d, paysize %d\n", ieee->i_accumlen, paysize); + ieee->i_accumlen = g_uncomp_hdrlen + (fragoffset << 3) + paysize; } else { @@ -563,10 +553,10 @@ copypayload: * the IP stack */ - ninfo("sixlowpan_init i_accumlen %d, ieee->i_pktlen %d\n", - ieee->i_accumlen, ieee->i_pktlen); + ninfo("i_accumlen=%d i_pktlen=%d paysize=%d\n", + ieee->i_accumlen, ieee->i_pktlen, paysize); - if (ieee->i_accumlen == 0 || ieee->i_accumlen == ieee->i_pktlen) + if (ieee->i_accumlen == 0 || ieee->i_accumlen >= ieee->i_pktlen) { ninfo("IP packet ready (length %d)\n", ieee->i_pktlen); diff --git a/net/sixlowpan/sixlowpan_tcpsend.c b/net/sixlowpan/sixlowpan_tcpsend.c index a34bfe0666e..a1a4a5847b8 100644 --- a/net/sixlowpan/sixlowpan_tcpsend.c +++ b/net/sixlowpan/sixlowpan_tcpsend.c @@ -403,6 +403,8 @@ void sixlowpan_tcp_send(FAR struct net_driver_s *dev) else { struct rimeaddr_s destmac; + FAR uint8_t *buf; + size_t buflen; /* Get the Rime MAC address of the destination. This assumes an * encoding of the MAC address in the IPv6 address. @@ -412,9 +414,12 @@ void sixlowpan_tcp_send(FAR struct net_driver_s *dev) /* Convert the outgoing packet into a frame list. */ + buf = dev->d_buf + sizeof(struct ipv6_hdr_s); + buflen = dev->d_len - sizeof(struct ipv6_hdr_s); + (void)sixlowpan_queue_frames( (FAR struct ieee802154_driver_s *)dev, ipv6hdr, - dev->d_buf, dev->d_len, &destmac); + buf, buflen, &destmac); } }