diff --git a/net/yaip/6lo.c b/net/yaip/6lo.c index 19b5210b183..46ba06deb5a 100644 --- a/net/yaip/6lo.c +++ b/net/yaip/6lo.c @@ -751,20 +751,64 @@ fail: return false; } -bool net_6lo_compress(struct net_buf *buf) +/* Adds IPv6 dispatch as first byte and adjust fragments */ +static inline bool compress_ipv6_header(struct net_buf *buf) { - return compress_IPHC_header(buf); + struct net_buf *frag; + + frag = net_nbuf_get_reserve_data(net_nbuf_ll_reserve(buf)); + if (!frag) { + return false; + } + + frag->data[0] = NET6LO_DISPATCH_IPV6; + net_buf_add(frag, 1); + + net_buf_frag_insert(buf, frag); + + /* compact the fragments, so that gaps will be filled */ + buf->frags = net_nbuf_compact(buf->frags); + + return true; +} + +static inline bool uncompress_ipv6_header(struct net_buf *buf) +{ + struct net_buf *frag = buf->frags; + + /* Pull off IPv6 dispatch header and adjust data and length */ + memmove(frag->data, frag->data + 1, frag->len - 1); + frag->len -= 1; + + return true; +} + +bool net_6lo_compress(struct net_buf *buf, bool iphc) +{ + if (iphc) { + return compress_IPHC_header(buf); + } else { + return compress_ipv6_header(buf); + } } bool net_6lo_uncompress(struct net_buf *buf) { if (!buf || !buf->frags) { - return NULL; + return false; } - if (!(buf->frags->data[0] & NET6LO_DISPATCH_IPHC)) { - return NULL; + if ((buf->frags->data[0] & NET6LO_DISPATCH_IPHC) == + NET6LO_DISPATCH_IPHC) { + /* uncompress IPHC header */ + return uncompress_IPHC_header(buf); + + } else if ((buf->frags->data[0] & NET6LO_DISPATCH_IPV6) == + NET6LO_DISPATCH_IPV6) { + /* uncompress IPv6 header, it has only IPv6 dispatch in the + * beginning */ + return uncompress_ipv6_header(buf); } - return uncompress_IPHC_header(buf); + return false; } diff --git a/net/yaip/6lo.h b/net/yaip/6lo.h index 91a3c35d148..ce12f43ff65 100644 --- a/net/yaip/6lo.h +++ b/net/yaip/6lo.h @@ -36,10 +36,11 @@ * will be adjusted according to remaining space in fragments. * * @param Pointer to network buffer + * @param iphc true for IPHC compression, false for IPv6 dispatch header * * @return True on success, false otherwise */ -bool net_6lo_compress(struct net_buf *buf); +bool net_6lo_compress(struct net_buf *buf, bool iphc); /** * @brief Unompress IPv6 packet as per RFC 6282 diff --git a/tests/net/6lo/src/main.c b/tests/net/6lo/src/main.c index 2bfbdca5c37..6e860235c26 100644 --- a/tests/net/6lo/src/main.c +++ b/tests/net/6lo/src/main.c @@ -374,7 +374,7 @@ static int test_6lo(struct net_6lo_data *data) net_hexdump_frags("before-compression", buf); #endif - if (!net_6lo_compress(buf)) { + if (!net_6lo_compress(buf, true)) { TC_PRINT("compression failed\n"); goto end; }