diff --git a/net/sixlowpan/sixlowpan_framelist.c b/net/sixlowpan/sixlowpan_framelist.c index dbd902a1cf..d98caff602 100644 --- a/net/sixlowpan/sixlowpan_framelist.c +++ b/net/sixlowpan/sixlowpan_framelist.c @@ -125,8 +125,6 @@ static void sixlowpan_compress_ipv6hdr(FAR const struct ipv6_hdr_s *ipv6hdr, FAR uint8_t *fptr) { - uint16_t protosize; - /* Indicate the IPv6 dispatch and length */ fptr[g_frame_hdrlen] = SIXLOWPAN_DISPATCH_IPV6; @@ -134,52 +132,69 @@ static void sixlowpan_compress_ipv6hdr(FAR const struct ipv6_hdr_s *ipv6hdr, /* Copy the IPv6 header and adjust pointers */ - memcpy(&fptr[g_frame_hdrlen] , ipv6hdr, IPv6_HDRLEN); + memcpy(&fptr[g_frame_hdrlen], ipv6hdr, IPv6_HDRLEN); g_frame_hdrlen += IPv6_HDRLEN; g_uncomp_hdrlen += IPv6_HDRLEN; +} - /* Copy the following protocol header, */ +/**************************************************************************** + * Name: sixlowpan_protosize + * + * Description: + * Get the size of any uncompresssed protocol header that follows the + * IPv6 header. + * + ****************************************************************************/ - switch (ipv6hdr->proto) - { +static uint16_t sixlowpan_protosize(FAR const struct ipv6_hdr_s *ipv6hdr, + FAR uint8_t *fptr) +{ + uint16_t protosize; + + /* Do we already have an encoded protocol header? If not, it needs to + * coped as raw data in the fist packet of a fragement. + */ + + if (!g_have_protohdr) + { + /* Copy the following protocol header, */ + + switch (ipv6hdr->proto) + { #ifdef CONFIG_NET_TCP - case IP_PROTO_TCP: - { - FAR struct tcp_hdr_s *tcp = &((FAR struct ipv6tcp_hdr_s *)ipv6hdr)->tcp; + case IP_PROTO_TCP: + { + FAR struct tcp_hdr_s *tcp = + &((FAR struct ipv6tcp_hdr_s *)ipv6hdr)->tcp; - /* The TCP header length is encoded in the top 4 bits of the - * tcpoffset field (in units of 32-bit words). - */ + /* The TCP header length is encoded in the top 4 bits of the + * tcpoffset field (in units of 32-bit words). + */ - protosize = ((uint16_t)tcp->tcpoffset >> 4) << 2; - } - break; + protosize = ((uint16_t)tcp->tcpoffset >> 4) << 2; + } + break; #endif #ifdef CONFIG_NET_UDP - case IP_PROTO_UDP: - protosize = sizeof(struct udp_hdr_s); - break; + case IP_PROTO_UDP: + protosize = UDP_HDRLEN; + break; #endif #ifdef CONFIG_NET_ICMPv6 - case IP_PROTO_ICMP6: - protosize = sizeof(struct icmpv6_hdr_s); - break; + case IP_PROTO_ICMP6: + protosize = ICMPv6_HDRLEN; + break; #endif - default: - nwarn("WARNING: Unrecognized proto: %u\n", ipv6hdr->proto); - return; - } + default: + nwarn("WARNING: Unrecognized proto: %u\n", ipv6hdr->proto); + return 0; + } + } - /* Copy the protocol header. */ - - memcpy(fptr + g_frame_hdrlen, (FAR uint8_t *)ipv6hdr + g_uncomp_hdrlen, - protosize); - - g_frame_hdrlen += protosize; - g_uncomp_hdrlen += protosize; + return protosize; } /**************************************************************************** @@ -234,6 +249,7 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, #ifdef CONFIG_NET_6LOWPAN_FRAG uint16_t outlen = 0; #endif + uint8_t protosize; int ret; ninfo("buflen=%lu\n", (unsigned long)buflen); @@ -244,6 +260,7 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, g_uncomp_hdrlen = 0; g_frame_hdrlen = 0; + g_have_protohdr = false; /* Reset frame meta data */ @@ -381,9 +398,13 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, ninfo("Header of length %d\n", g_frame_hdrlen); + /* Get the size of any uncompressed protocol headers */ + + protosize = sixlowpan_protosize(destip, fptr); + /* Check if we need to fragment the packet into several frames */ - if (buflen > (SIXLOWPAN_FRAMELEN - g_frame_hdrlen)) + if (buflen > (SIXLOWPAN_FRAMELEN - g_frame_hdrlen - protosize)) { #ifdef CONFIG_NET_6LOWPAN_FRAG /* qhead will hold the generated frame list; frames will be @@ -420,29 +441,39 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, * The fragment header contains three fields: Datagram size, datagram * tag and datagram offset: * - * 1. Datagram size describes the total (un-fragmented) payload. - * 2. Datagram tag identifies the set of fragments and is used to - * match fragments of the same payload. - * 3. Datagram offset identifies the fragment’s offset within the un- - * fragmented payload. + * 1. Datagram size describes the total (un-fragmented) payload. + * 2. Datagram tag identifies the set of fragments and is used to + * match fragments of the same payload. + * 3. Datagram offset identifies the fragment’s offset within the un- + * fragmented payload. * * The fragment header length is 4 bytes for the first header and 5 * bytes for all subsequent headers. */ - pktlen = buflen + g_uncomp_hdrlen; + pktlen = buflen + g_uncomp_hdrlen + protosize; PUTHOST16(fragptr, SIXLOWPAN_FRAG_DISPATCH_SIZE, ((SIXLOWPAN_DISPATCH_FRAG1 << 8) | pktlen)); PUTHOST16(fragptr, SIXLOWPAN_FRAG_TAG, ieee->i_dgramtag); g_frame_hdrlen += SIXLOWPAN_FRAG1_HDR_LEN; + /* Copy any uncompressed protocol headers that must appear only in th + * first fragment. + */ + + if (protosize > 0) + { + FAR uint8_t *src = (FAR uint8_t *)destip + IPv6_HDRLEN; + memcpy(fptr + g_frame_hdrlen, src, protosize); + } + /* Copy payload and enqueue. NOTE that the size is a multiple of eight * bytes. */ paysize = (SIXLOWPAN_FRAMELEN - g_frame_hdrlen) & ~7; - memcpy(fptr + g_frame_hdrlen, buf, paysize); + memcpy(fptr + g_frame_hdrlen + protosize, buf, paysize - protosize); /* Set outlen to what we already sent from the IP payload */ @@ -471,7 +502,7 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, frame1 = iob->io_data; frag1_hdrlen = g_frame_hdrlen; - while (outlen < buflen) + while (outlen < (buflen + protosize)) { uint16_t fragn_hdrlen; @@ -515,14 +546,14 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, paysize = (SIXLOWPAN_FRAMELEN - fragn_hdrlen) & SIXLOWPAN_DISPATCH_FRAG_MASK; - if (buflen - outlen < paysize) + if (paysize > buflen - outlen + protosize) { /* Last fragment, truncate to the correct length */ - paysize = buflen - outlen; + paysize = buflen - outlen + protosize; } - memcpy(fptr + fragn_hdrlen, buf + outlen, paysize); + memcpy(fptr + fragn_hdrlen, buf + outlen - protosize, paysize); /* Set outlen to what we already sent from the IP payload */ @@ -587,10 +618,20 @@ int sixlowpan_queue_frames(FAR struct ieee802154_driver_s *ieee, * and send in one frame. */ + /* Copy any uncompressed protocol headers that must appear only in th + * first fragment. + */ + + if (protosize > 0) + { + FAR uint8_t *src = (FAR uint8_t *)destip + IPv6_HDRLEN; + memcpy(fptr + g_frame_hdrlen, src, protosize); + } + /* Copy the payload into the frame. */ - memcpy(fptr + g_frame_hdrlen, buf, buflen); - iob->io_len = buflen + g_frame_hdrlen; + memcpy(fptr + g_frame_hdrlen + protosize, buf, buflen); + iob->io_len = buflen + g_frame_hdrlen + protosize; iob->io_pktlen = iob->io_len; ninfo("Non-fragmented: length %d\n", iob->io_len); diff --git a/net/sixlowpan/sixlowpan_globals.c b/net/sixlowpan/sixlowpan_globals.c index 6d52082df1..0d9aa8f441 100644 --- a/net/sixlowpan/sixlowpan_globals.c +++ b/net/sixlowpan/sixlowpan_globals.c @@ -67,4 +67,8 @@ uint8_t g_uncomp_hdrlen; uint8_t g_frame_hdrlen; +/* g_have_protohdr: true=Protocal header copied. */ + +bool g_have_protohdr; + #endif /* CONFIG_NET_6LOWPAN */ diff --git a/net/sixlowpan/sixlowpan_hc06.c b/net/sixlowpan/sixlowpan_hc06.c index 97f56e85ca..1ed058a024 100644 --- a/net/sixlowpan/sixlowpan_hc06.c +++ b/net/sixlowpan/sixlowpan_hc06.c @@ -878,132 +878,87 @@ void sixlowpan_compresshdr_hc06(FAR struct ieee802154_driver_s *ieee, g_uncomp_hdrlen = IPv6_HDRLEN; - /* Add protocol header */ - - switch (ipv6->proto) - { #ifdef CONFIG_NET_UDP - /* UDP header compression */ + /* UDP header compression */ - case IP_PROTO_UDP: + if (ipv6->proto == IP_PROTO_UDP) + { + /* The UDP header will follow the IPv6 header */ + + FAR struct udp_hdr_s *udp = + (FAR struct udp_hdr_s *)((FAR uint8_t *)ipv6 + IPv6_HDRLEN); + + ninfo("Uncompressed UDP ports: srcport=%04x destport=%04x\n", + ntohs(udp->srcport), ntohs(udp->destport)); + + /* Mask out the last 4 bits can be used as a mask */ + + if (((ntohs(udp->srcport) & 0xfff0) == SIXLOWPAN_UDP_4_BIT_PORT_MIN) && + ((ntohs(udp->destport) & 0xfff0) == SIXLOWPAN_UDP_4_BIT_PORT_MIN)) { - /* The UDP header will follow the IPv6 header */ + /* We can compress 12 bits of both source and dest */ - FAR struct udp_hdr_s *udp = - (FAR struct udp_hdr_s *)((FAR uint8_t *)ipv6 + IPv6_HDRLEN); + *g_hc06ptr = SIXLOWPAN_NHC_UDP_CS_P_11; - ninfo("Uncompressed UDP ports: srcport=%04x destport=%04x\n", - ntohs(udp->srcport), ntohs(udp->destport)); + ninfo("Remove 12b of both source & dest with prefix 0xf0b*\n"); - /* Mask out the last 4 bits can be used as a mask */ + *(g_hc06ptr + 1) = + (uint8_t)((ntohs(udp->srcport) - SIXLOWPAN_UDP_4_BIT_PORT_MIN) << 4) + + (uint8_t)((ntohs(udp->destport) - SIXLOWPAN_UDP_4_BIT_PORT_MIN)); - if (((ntohs(udp->srcport) & 0xfff0) == SIXLOWPAN_UDP_4_BIT_PORT_MIN) && - ((ntohs(udp->destport) & 0xfff0) == SIXLOWPAN_UDP_4_BIT_PORT_MIN)) - { - /* We can compress 12 bits of both source and dest */ - - *g_hc06ptr = SIXLOWPAN_NHC_UDP_CS_P_11; - - ninfo("Remove 12b of both source & dest with prefix 0xf0b*\n"); - - *(g_hc06ptr + 1) = - (uint8_t)((ntohs(udp->srcport) - SIXLOWPAN_UDP_4_BIT_PORT_MIN) << 4) + - (uint8_t)((ntohs(udp->destport) - SIXLOWPAN_UDP_4_BIT_PORT_MIN)); - - g_hc06ptr += 2; - } - else if ((ntohs(udp->destport) & 0xff00) == - SIXLOWPAN_UDP_8_BIT_PORT_MIN) - { - /* We can compress 8 bits of dest, leave source. */ - - *g_hc06ptr = SIXLOWPAN_NHC_UDP_CS_P_01; - - ninfo("Leave source, remove 8 bits of dest with prefix 0xF0\n"); - - memcpy(g_hc06ptr + 1, &udp->srcport, 2); - *(g_hc06ptr + 3) = - (uint8_t) ((ntohs(udp->destport) - - SIXLOWPAN_UDP_8_BIT_PORT_MIN)); - g_hc06ptr += 4; - } - else if ((ntohs(udp->srcport) & 0xff00) == - SIXLOWPAN_UDP_8_BIT_PORT_MIN) - { - /* We can compress 8 bits of src, leave dest. Copy compressed port */ - - *g_hc06ptr = SIXLOWPAN_NHC_UDP_CS_P_10; - - ninfo("Remove 8 bits of source with prefix 0xF0, leave dest. hch: %u\n", - *g_hc06ptr); - - *(g_hc06ptr + 1) = - (uint8_t)((ntohs(udp->srcport) - SIXLOWPAN_UDP_8_BIT_PORT_MIN)); - - memcpy(g_hc06ptr + 2, &udp->destport, 2); - g_hc06ptr += 4; - } - else - { - /* we cannot compress. Copy uncompressed ports, full checksum */ - - *g_hc06ptr = SIXLOWPAN_NHC_UDP_CS_P_00; - - nwarn("WARNING: Cannot compress headers\n"); - - memcpy(g_hc06ptr + 1, &udp->srcport, 4); - g_hc06ptr += 5; - } - - /* Always inline the checksum */ - - if (1) - { - memcpy(g_hc06ptr, &udp->udpchksum, 2); - g_hc06ptr += 2; - } - - g_uncomp_hdrlen += UDP_HDRLEN; + g_hc06ptr += 2; } - break; -#endif /* CONFIG_NET_UDP */ - -#ifdef CONFIG_NET_TCP - /* TCP header -- not compressed */ - - case IP_PROTO_TCP: + else if ((ntohs(udp->destport) & 0xff00) == + SIXLOWPAN_UDP_8_BIT_PORT_MIN) { - FAR struct tcp_hdr_s *tcp = - (FAR struct tcp_hdr_s *)((FAR uint8_t *)ipv6 + IPv6_HDRLEN); - unsigned int hdrsize; + /* We can compress 8 bits of dest, leave source. */ - hdrsize = ((uint16_t)tcp->tcpoffset >> 4) << 2; - memcpy(g_hc06ptr, tcp, hdrsize); + *g_hc06ptr = SIXLOWPAN_NHC_UDP_CS_P_01; - g_uncomp_hdrlen += hdrsize; - g_hc06ptr += hdrsize; + ninfo("Leave source, remove 8 bits of dest with prefix 0xF0\n"); + + memcpy(g_hc06ptr + 1, &udp->srcport, 2); + *(g_hc06ptr + 3) = + (uint8_t) ((ntohs(udp->destport) - + SIXLOWPAN_UDP_8_BIT_PORT_MIN)); + g_hc06ptr += 4; } - break; -#endif - -#ifdef CONFIG_NET_ICMPv6 - /* TCP header -- not compressed */ - - case IP_PROTO_ICMP: + else if ((ntohs(udp->srcport) & 0xff00) == + SIXLOWPAN_UDP_8_BIT_PORT_MIN) { - FAR const uint8_t *src = (FAR const uint8_t *)ipv6 + IPv6_HDRLEN; + /* We can compress 8 bits of src, leave dest. Copy compressed port */ - memcpy(g_hc06ptr, src, ICMPv6_HDRLEN) - g_uncomp_hdrlen += ICMPv6_HDRLEN; - g_hc06ptr += ICMPv6_HDRLEN; + *g_hc06ptr = SIXLOWPAN_NHC_UDP_CS_P_10; + + ninfo("Remove 8 bits of source with prefix 0xF0, leave dest. hch: %u\n", + *g_hc06ptr); + + *(g_hc06ptr + 1) = + (uint8_t)((ntohs(udp->srcport) - SIXLOWPAN_UDP_8_BIT_PORT_MIN)); + + memcpy(g_hc06ptr + 2, &udp->destport, 2); + g_hc06ptr += 4; } - break; -#endif + else + { + /* we cannot compress. Copy uncompressed ports, full checksum */ - default: - nerr("ERROR: Unsupported protocol: %02x\n", ipv6->proto); - break; + *g_hc06ptr = SIXLOWPAN_NHC_UDP_CS_P_00; + + nwarn("WARNING: Cannot compress headers\n"); + + memcpy(g_hc06ptr + 1, &udp->srcport, 4); + g_hc06ptr += 5; + } + + /* Always inline the checksum */ + + memcpy(g_hc06ptr, &udp->udpchksum, 2); + g_hc06ptr += 2; + g_uncomp_hdrlen += UDP_HDRLEN; + g_have_protohdr = true; } +#endif /* CONFIG_NET_UDP */ /* Before the g_frame_hdrlen operation */ diff --git a/net/sixlowpan/sixlowpan_hc1.c b/net/sixlowpan/sixlowpan_hc1.c index 1a106d4506..82019aaae7 100644 --- a/net/sixlowpan/sixlowpan_hc1.c +++ b/net/sixlowpan/sixlowpan_hc1.c @@ -265,6 +265,8 @@ void sixlowpan_compresshdr_hc1(FAR struct ieee802154_driver_s *ieee, hc1[SIXLOWPAN_HC1_TTL] = ipv6->ttl; g_frame_hdrlen += SIXLOWPAN_HC1_HDR_LEN; } + + g_have_protohdr = true; } break; #endif /* CONFIG_NET_UDP */ diff --git a/net/sixlowpan/sixlowpan_internal.h b/net/sixlowpan/sixlowpan_internal.h index 6b5c0953fc..8a26313169 100644 --- a/net/sixlowpan/sixlowpan_internal.h +++ b/net/sixlowpan/sixlowpan_internal.h @@ -58,7 +58,9 @@ ****************************************************************************/ #include + #include +#include #include #include @@ -207,6 +209,10 @@ extern uint8_t g_uncomp_hdrlen; extern uint8_t g_frame_hdrlen; +/* g_have_protohdr: true=Protocal header copied. */ + +extern bool g_have_protohdr; + /**************************************************************************** * Public Types ****************************************************************************/