From 7a4af75fcf577097d8292babc549cd38ef8de903 Mon Sep 17 00:00:00 2001 From: Gregory Nutt Date: Sat, 1 Apr 2017 12:34:08 -0600 Subject: [PATCH] 6lowWPAN: Add frame decompression logic to IEEE802.15.4 input --- include/nuttx/net/sixlowpan.h | 44 +++- net/sixlowpan/Kconfig | 3 +- net/sixlowpan/sixlowpan_input.c | 371 +++++++++++++++++++++++++++++++- net/sixlowpan/sixlowpan_send.c | 13 ++ 4 files changed, 426 insertions(+), 5 deletions(-) diff --git a/include/nuttx/net/sixlowpan.h b/include/nuttx/net/sixlowpan.h index f31c453c43..9cfa3e0973 100644 --- a/include/nuttx/net/sixlowpan.h +++ b/include/nuttx/net/sixlowpan.h @@ -53,6 +53,7 @@ #include +#include #include #include @@ -71,9 +72,13 @@ #define SIXLOWPAN_DISPATCH_IPV6 0x41 /* 01000001 = 65 */ #define SIXLOWPAN_DISPATCH_HC1 0x42 /* 01000010 = 66 */ -#define SIXLOWPAN_DISPATCH_IPHC 0x60 /* 011xxxxx = ... */ + +#define SIXLOWPAN_DISPATCH_IPHC 0x60 /* 011xxxxx */ +#define SIXLOWPAN_DISPATCH_IPHC_MASK 0xe0 /* 11100000 */ + #define SIXLOWPAN_DISPATCH_FRAG1 0xc0 /* 11000xxx */ #define SIXLOWPAN_DISPATCH_FRAGN 0xe0 /* 11100xxx */ +#define SIXLOWPAN_DISPATCH_FRAG_MASK 0xf1 /* 11111000 */ /* HC1 encoding */ @@ -402,6 +407,7 @@ struct ieee802154_driver_s FAR struct iob_s *i_framelist; + /* Driver Configuration ***************************************************/ /* i_panid. The PAN ID is 16-bit number that identifies the network. It * must be unique to differentiate a network. All the nodes in the same * network should have the same PAN ID. This value must be provided to @@ -433,6 +439,42 @@ struct ieee802154_driver_s */ uint16_t i_dgramtag; + +#if CONFIG_NET_6LOWPAN_FRAG + /* Fragmentation Support *************************************************/ + /* Fragementation is handled frame by frame and requires that certain + * state information be retained from frame to frame. + */ + + /* i_pktlen. The total length of the IPv6 packet to be re-assembled in + * d_buf. + */ + + uint16_t i_pktlen; + + /* The current accumulated length of the packet being received in d_buf. + * Included IPv6 and protocol headers. + */ + + uint16_t i_accumlen; + + /* i_reasstag. Each frame in the reassembly has a tag. That tag must + * match the reassembly tag in the fragments being merged. + */ + + uint16_t i_reasstag; + + /* The source MAC address of the fragments being merged */ + + struct rimeaddr_s i_fragsrc; + + /* That time at which reassembly was started. If the elapsed time + * exceeds CONFIG_NET_6LOWPAN_MAXAGE, then the reassembly will + * be cancelled. + */ + + systime_t i_time; +#endif /* CONFIG_NET_6LOWPAN_FRAG */ }; /* The structure of a next header compressor. This compressor is provided diff --git a/net/sixlowpan/Kconfig b/net/sixlowpan/Kconfig index 4d0be0b660..49f4756a02 100644 --- a/net/sixlowpan/Kconfig +++ b/net/sixlowpan/Kconfig @@ -132,7 +132,8 @@ config NET_6LOWPAN_MAXAGE int "Packet reassembly timeout" default 20 ---help--- - Timeout for packet reassembly at the 6lowpan layer (should be < 60s) + Timeout for packet reassembly at the 6lowpan layer in units of + seconds (should be < 60s) config NET_6LOWPAN_MAX_MACTRANSMITS int "Max MAC transmissions" diff --git a/net/sixlowpan/sixlowpan_input.c b/net/sixlowpan/sixlowpan_input.c index 151d205f85..18e499f0d5 100644 --- a/net/sixlowpan/sixlowpan_input.c +++ b/net/sixlowpan/sixlowpan_input.c @@ -1,6 +1,6 @@ /**************************************************************************** * net/sixlowpan/sixlowpan_input.c - * 6lowpan implementation (RFC4944 and draft-ietf-6lowpan-hc-06) + * 6loWPAN implementation (RFC4944 and draft-ietf-6loWPAN-hc-06) * * Copyright (C) 2017, Gregory Nutt, all rights reserved * Author: Gregory Nutt @@ -49,10 +49,15 @@ #include +#include #include #include #include +#if CONFIG_NET_6LOWPAN_FRAG +# include "nuttx/clock.h" +#endif + #include "nuttx/net/netdev.h" #include "nuttx/net/ip.h" #include "nuttx/net/sixlowpan.h" @@ -74,6 +79,14 @@ #define INPUT_PARTIAL 0 /* Frame processed successful, packet incomplete */ #define INPUT_COMPLETE 1 /* Frame processed successful, packet complete */ +/* Re-assembly timeout in clock ticks */ + +#define NET_6LOWPAN_TIMEOUT SEC2TICK(CONFIG_NET_6LOWPAN_MAXAGE) + +/* Buffer access helpers */ + +#define IPv6BUF(dev) ((FAR struct ipv6_hdr_s *)((dev)->d_buf)) + /**************************************************************************** * Public Functions ****************************************************************************/ @@ -185,13 +198,365 @@ static void sixlowpan_set_pktattrs(FAR struct ieee802154_driver_s *ieee, * Returned Value: * Ok is returned on success; Othewise a negated errno value is returned. * + * Assumptions: + * Network is locked + * ****************************************************************************/ static int sixlowpan_frame_process(FAR struct ieee802154_driver_s *ieee, FAR struct iob_s *iob) { - /* REVISIT: To be provided */ - return -ENOSYS; + FAR uint8_t *hc1 = RIME_HC1_PTR; + + uint16_t fragsize = 0; /* Size of the IP packet (read from fragment) */ + uint8_t fragoffset = 0; /* Offset of the fragment in the IP packet */ + bool isfrag = false; + int reqsize; /* Required buffer size */ + +#if CONFIG_NET_6LOWPAN_FRAG + bool isfirstfrag = false; + bool islastfrag = false; + uint16_t fragtag = 0; /* Tag of the fragment */ + systime_t elapsed; /* Elapsed time */ +#endif /* CONFIG_NET_6LOWPAN_FRAG */ + + /* Initialize global data. Locking the network guarantees that we have + * exclusive use of the global values for intermediate calculations. + */ + + g_uncomp_hdrlen = 0; + g_rime_hdrlen = 0; + + /* The MAC puts the 15.4 payload inside the RIME data buffer */ + + g_rimeptr = &iob->io_data[PACKETBUF_HDR_SIZE]; + +#if CONFIG_NET_6LOWPAN_FRAG + /* If reassembly timed out, cancel it */ + + elapsed = clock_systimer() - ieee->i_time; + if (elapsed > NET_6LOWPAN_TIMEOUT) + { + nwarn("WARNING: Reassembly timed out\n"); + ieee->i_pktlen = 0; + ieee->i_accumlen = 0; + } + + /* Since we don't support the mesh and broadcast header, the first header + * we look for is the fragmentation header + */ + + switch ((GETINT16(RIME_FRAG_PTR, RIME_FRAG_DISPATCH_SIZE) & 0xf800) >> 8) + { + /* First fragment of new reassembly */ + + case SIXLOWPAN_DISPATCH_FRAG1: + { + /* Set up for the reassembly */ + + fragoffset = 0; + fragsize = GETINT16(RIME_FRAG_PTR, RIME_FRAG_DISPATCH_SIZE) & 0x07ff; + fragtag = GETINT16(RIME_FRAG_PTR, RIME_FRAG_TAG); + + ninfo("FRAG1: size %d, tag %d, offset %d\n", + fragsize, fragtag, fragoffset); + + g_rime_hdrlen += SIXLOWPAN_FRAG1_HDR_LEN; + + /* Indicate the first fragment of the reassembly */ + + isfirstfrag = true; + isfrag = true; + } + break; + + case SIXLOWPAN_DISPATCH_FRAGN: + { + /* Set offset, tag, size. Offset is in units of 8 bytes. */ + + fragoffset = RIME_FRAG_PTR[RIME_FRAG_OFFSET]; + fragtag = GETINT16(RIME_FRAG_PTR, RIME_FRAG_TAG); + fragsize = GETINT16(RIME_FRAG_PTR, RIME_FRAG_DISPATCH_SIZE) & 0x07ff; + + ninfo("FRAGN: size %d, tag %d, offset %d\n", + fragsize, fragtag, fragoffset); + + g_rime_hdrlen += SIXLOWPAN_FRAGN_HDR_LEN; + + ninfo("islastfrag?: i_accumlen %d g_rime_payloadlen %d fragsize %d\n", + ieee->i_accumlen, iob->io_len - g_rime_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_rime_hdrlen >= fragsize) + { + islastfrag = true; + } + } + break; + + /* Not a fragment */ + + default: + break; + } + + /* Check if we are currently reassembling a packet */ + + if (ieee->i_accumlen > 0) + { + /* In this case what we expect is that the next frame will hold the + * next FRAGN of the sequence. We have to handle a few exeptional + * cases that we need to handle: + * + * 1. If we are currently reassembling a packet, but have just received + * the first fragment of another packet. We can either ignore it and + * hope to receive the rest of the under-reassembly packet fragments, + * or we can discard the previous packet altogether, and start + * reassembling the new packet. Here we discard the previous packet, + * and start reassembling the new packet. + * 2. The new frame is not a fragment. We should be able to handle this + * case, but we cannot because that would require two packet buffers. + * It could be handled with a more extensive design. + * 3. The fragment came from a different sender. What would this mean? + * + */ + + if (!isfrag) + { + /* Discard the partially assembled packet */ + + nwarn("WARNING: Non-fragment frame received during reassembly\n"); + ieee->i_pktlen = 0; + ieee->i_accumlen = 0; + } + + /* It is a fragment of some kind. Drop any zero length fragments */ + + else if (fragsize == 0) + { + nwarn("WARNING: Dropping zero-length 6loWPAN fragment\n"); + return OK; + } + + /* A non-zero, first fragement received while we are in the middle of + * rassembly. Discard the partially assembled packet and start over. + */ + + else if (isfirstfrag) + { + nwarn("WARNING: First fragment frame received during reassembly\n"); + ieee->i_pktlen = 0; + ieee->i_accumlen = 0; + } + + /* Verify that this fragment is part of that reassembly sequence */ + + else if (fragsize != ieee->i_pktlen || ieee->i_reasstag != fragtag || + !rimeaddr_cmp(&ieee->i_fragsrc, &g_pktaddrs[PACKETBUF_ADDR_SENDER])) + { + /* The packet is a fragment that does not belong to the packet + * being reassembled or the packet is not a fragment. + */ + + nwarn("WARNING: Dropping 6loWPAN packet that is not a fragment of " + "the packet currently being reassembled\n"); + return OK; + } + + /* Looks good. We are currently processing a reassembling sequence and + * we recieved a valid FRAGN fragment. Skip the header compression + * dispatch logic. + */ + + goto copypayload; + } + + /* There is no reassembly in progress. Check if we received a fragment */ + + else if (isfrag) + { + /* Another case that we have to handle is if a FRAGN fragment of a + * reassembly is received, but we are not currently reassembling a + * packet. I think we have no choice but to drop the packet in this + * case. + */ + + if (!isfirstfrag) + { + nwarn("WARNING: FRAGN 6loWPAN fragment while not reassembling\n"); + return OK; + } + + /* Start reassembly if we received a non-zero length, first fragment */ + + if (fragsize > 0) + { + /* 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]); + } + } +#endif /* CONFIG_NET_6LOWPAN_FRAG */ + + /* Process next dispatch and headers */ + +#ifdef CONFIG_NET_6LOWPAN_COMPRESSION_HC06 + if ((hc1[RIME_HC1_DISPATCH] & SIXLOWPAN_DISPATCH_IPHC_MASK) == SIXLOWPAN_DISPATCH_IPHC) + { + ninfo("IPHC Dispatch\n"); + sixlowpan_uncompresshdr_hc06(ieee, fragsize); + } + else +#endif /* CONFIG_NET_6LOWPAN_COMPRESSION_HC06 */ + +#ifdef CONFIG_NET_6LOWPAN_COMPRESSION_HC1 + if (hc1[RIME_HC1_DISPATCH] == SIXLOWPAN_DISPATCH_HC1) + { + ninfo("HC1 Dispatch\n"); + sixlowpan_uncompresshdr_hc1(ieee, fragsize); + } + else +#endif /* CONFIG_NET_6LOWPAN_COMPRESSION_HC1 */ + if (hc1[RIME_HC1_DISPATCH] == SIXLOWPAN_DISPATCH_IPV6) + { + ninfo("IPV6 Dispatch\n"); + g_rime_hdrlen += SIXLOWPAN_IPV6_HDR_LEN; + + /* Put uncompressed IP header in d_buf. */ + + memcpy(ieee->i_dev.d_buf, g_rimeptr + g_rime_hdrlen, IPv6_HDRLEN); + + /* Update g_uncomp_hdrlen and g_rime_hdrlen. */ + + g_rime_hdrlen += IPv6_HDRLEN; + g_uncomp_hdrlen += IPv6_HDRLEN; + } + else + { + /* Unknown or unsupported header */ + + nwarn("WARNING unknown dispatch: %u\n", hc1[RIME_HC1_DISPATCH]); + return OK; + } + +#if CONFIG_NET_6LOWPAN_FRAG +copypayload: +#endif /* CONFIG_NET_6LOWPAN_FRAG */ + + /* Copy "payload" from the rime buffer to the d_buf. If this frame is a + * first fragment or not part of a fragmented packet, we have already + * copied the compressed headers, g_uncomp_hdrlen and g_rime_hdrlen are + * non-zerio, fragoffset is. + */ + + if (ieee->i_dev.d_len < g_rime_hdrlen) + { + ninfo("SIXLOWPAN: packet dropped due to header > total packet\n"); + return OK; + } + + g_rime_payloadlen = ieee->i_dev.d_len - g_rime_hdrlen; + + /* Sanity-check size of incoming packet to avoid buffer overflow */ + + reqsize = g_uncomp_hdrlen + (uint16_t) (fragoffset << 3) + g_rime_payloadlen; + if (reqsize > CONFIG_NET_6LOWPAN_MTU) + { + ninfo("Required buffer size: %d+%d+%d=%d Available: %d\n", + g_uncomp_hdrlen, (int)(fragoffset << 3), g_rime_payloadlen, + reqsize, CONFIG_NET_6LOWPAN_MTU); + return OK; + } + + memcpy((FAR uint8_t *)ieee->i_dev.d_buf + g_uncomp_hdrlen + + (int)(fragoffset << 3), g_rimeptr + g_rime_hdrlen, + g_rime_payloadlen); + +#if CONFIG_NET_6LOWPAN_FRAG + /* Update ieee->i_accumlen if the frame is a fragment, ieee->i_pktlen + * otherwise. + */ + + 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. + */ + + if (islastfrag) + { + ieee->i_accumlen = fragsize; + } + else + { + ieee->i_accumlen += g_rime_payloadlen; + } + + ninfo("i_accumlen %d, g_rime_payloadlen %d\n", + ieee->i_accumlen, g_rime_payloadlen); + } + else +#endif /* CONFIG_NET_6LOWPAN_FRAG */ + { + ieee->i_pktlen = g_rime_payloadlen + g_uncomp_hdrlen; + } + +#if CONFIG_NET_6LOWPAN_FRAG + /* If we have a full IP packet in sixlowpan_buf, deliver it to + * the IP stack + */ + + ninfo("sixlowpan_init i_accumlen %d, ieee->i_pktlen %d\n", + ieee->i_accumlen, ieee->i_pktlen); + + if (ieee->i_accumlen == 0 || ieee->i_accumlen == ieee->i_pktlen) + { + FAR struct ipv6_hdr_s *ipv6 = IPv6BUF(&ieee->i_dev); + + ninfo("IP packet ready (length %d)\n", ieee->i_pktlen); + + /* REVISIT -- clearly wrong. */ + memcpy((FAR uint8_t *)ipv6, (FAR uint8_t *)ipv6, ieee->i_pktlen); + + ieee->i_pktlen = 0; + ieee->i_accumlen = 0; + + } +#endif /* CONFIG_NET_6LOWPAN_FRAG */ + + ninfodumpbuffer("IPv6 header", IPv6BUF(ieee->i_dev), IPv6_HDRLEN) + return OK; } /**************************************************************************** diff --git a/net/sixlowpan/sixlowpan_send.c b/net/sixlowpan/sixlowpan_send.c index cff92eecee..f8076449b9 100644 --- a/net/sixlowpan/sixlowpan_send.c +++ b/net/sixlowpan/sixlowpan_send.c @@ -54,6 +54,17 @@ #ifdef CONFIG_NET_6LOWPAN +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* These are temporary stubs. Something like this would be needed to + * monitor the health of a IPv6 neighbor. + */ + +#define neighbor_reachable(dev) +#define neighbor_notreachable(dev) + /**************************************************************************** * Private Types ****************************************************************************/ @@ -172,6 +183,7 @@ static uint16_t send_interrupt(FAR struct net_driver_s *dev, sinfo->s_destmac); flags &= ~WPAN_POLL; + neighbor_reachable(dev); goto end_wait; } @@ -183,6 +195,7 @@ static uint16_t send_interrupt(FAR struct net_driver_s *dev, nwarn("WARNING: SEND timeout\n"); sinfo->s_result = -ETIMEDOUT; + neighbor_notreachable(dev); goto end_wait; }