1 /** @file
2 * @brief 6lopan related functions
3 */
4
5 /*
6 * Copyright (c) 2016 Intel Corporation
7 * Copyright (c) 2019 Alexander Wachter
8 *
9 * SPDX-License-Identifier: Apache-2.0
10 */
11
12 #include <zephyr/logging/log.h>
13 LOG_MODULE_REGISTER(net_6lo, CONFIG_NET_6LO_LOG_LEVEL);
14
15 #include <errno.h>
16 #include <zephyr/net/net_core.h>
17 #include <zephyr/net/net_if.h>
18 #include <zephyr/net/net_stats.h>
19 #include <zephyr/net/udp.h>
20
21 #include "net_private.h"
22 #include "6lo.h"
23 #include "6lo_private.h"
24
25 #if defined(CONFIG_NET_6LO_CONTEXT)
26 struct net_6lo_context {
27 struct in6_addr prefix;
28 struct net_if *iface;
29 uint16_t lifetime;
30 uint8_t is_used : 1;
31 uint8_t compress : 1;
32 uint8_t cid : 4;
33 uint8_t unused : 2;
34 };
35
get_6co_compress(struct net_icmpv6_nd_opt_6co * opt)36 static inline uint8_t get_6co_compress(struct net_icmpv6_nd_opt_6co *opt)
37 {
38 return (opt->flag & 0x10) >> 4;
39 }
40
get_6co_cid(struct net_icmpv6_nd_opt_6co * opt)41 static inline uint8_t get_6co_cid(struct net_icmpv6_nd_opt_6co *opt)
42 {
43 return opt->flag & 0x0F;
44 }
45
46 static struct net_6lo_context ctx_6co[CONFIG_NET_MAX_6LO_CONTEXTS];
47 #endif
48
49 static const uint8_t udp_nhc_inline_size_table[] = {4, 3, 3, 1};
50
51 static const uint8_t tf_inline_size_table[] = {4, 3, 1, 0};
52 /* The first bit of the index is SAC | SAC=0 | SAC=1 |*/
53 static const uint8_t sa_inline_size_table[] = {16, 8, 2, 0, 0, 8, 2, 0};
54
55 /* The first bit is M, the second DAC
56 * | M=0 DAC=0 | M=0 DAC=1 | M=1 DAC=0 | M=1 DAC=1 (DAM always 00)
57 */
58 static const uint8_t da_inline_size_table[] = {
59 16, 8, 2, 0, 0, 8, 2, 0, 16, 6, 4, 1, 6
60 };
61
get_udp_nhc_inlined_size(uint8_t nhc)62 static int get_udp_nhc_inlined_size(uint8_t nhc)
63 {
64 int size = 0;
65
66 if ((nhc & 0xF8) != NET_6LO_NHC_UDP_BARE) {
67 NET_DBG("UDP NHC dispatch doesn't match");
68 return 0;
69 }
70
71 if (!(nhc & NET_6LO_NHC_UDP_CHECKSUM)) {
72 size += 2U;
73 }
74
75 size += udp_nhc_inline_size_table[(nhc & NET_6LO_NHC_UDP_PORT_MASK)];
76
77 NET_DBG("Size of inlined UDP HDR data: %d", size);
78
79 return size;
80 }
81
get_ihpc_inlined_size(uint16_t iphc)82 static int get_ihpc_inlined_size(uint16_t iphc)
83 {
84 int size = 0;
85
86 if (((iphc >> 8) & NET_6LO_DISPATCH_IPHC_MASK) !=
87 NET_6LO_DISPATCH_IPHC) {
88 NET_DBG("IPHC dispatch doesn't match");
89 return -1;
90 }
91
92 size += tf_inline_size_table[(iphc & NET_6LO_IPHC_TF_MASK) >>
93 NET_6LO_IPHC_TF_POS];
94
95 if (!(iphc & NET_6LO_IPHC_NH_MASK)) {
96 size += 1U;
97 }
98
99 if (!(iphc & NET_6LO_IPHC_HLIM_MASK)) {
100 size += 1U;
101 }
102
103 if (iphc & NET_6LO_IPHC_CID_MASK) {
104 size += 1U;
105 }
106
107 size += sa_inline_size_table[(iphc & NET_6LO_IPHC_SA_MASK) >>
108 NET_6LO_IPHC_SAM_POS];
109
110 size += da_inline_size_table[(iphc & NET_6LO_IPHC_DA_MASK) >>
111 NET_6LO_IPHC_DAM_POS];
112
113 NET_DBG("Size of inlined IP HDR data: %d", size);
114
115 return size;
116 }
117
118 /* TODO: Unicast-Prefix based IPv6 Multicast(dst) address compression
119 * Mesh header compression
120 */
121
net_6lo_ll_prefix_padded_with_zeros(struct in6_addr * addr)122 static inline bool net_6lo_ll_prefix_padded_with_zeros(struct in6_addr *addr)
123 {
124 return (net_ipv6_is_ll_addr(addr) &&
125 (UNALIGNED_GET(&addr->s6_addr16[1]) == 0x00) &&
126 (UNALIGNED_GET(&addr->s6_addr32[1]) == 0x00));
127 }
128
net_6lo_addr_16_bit_compressible(struct in6_addr * addr)129 static inline bool net_6lo_addr_16_bit_compressible(struct in6_addr *addr)
130 {
131 return ((UNALIGNED_GET(&addr->s6_addr32[2]) == htonl(0xFF)) &&
132 (UNALIGNED_GET(&addr->s6_addr16[6]) == htons(0xFE00)));
133 }
134
net_6lo_maddr_8_bit_compressible(struct in6_addr * addr)135 static inline bool net_6lo_maddr_8_bit_compressible(struct in6_addr *addr)
136 {
137 return ((addr->s6_addr[1] == 0x02) &&
138 (UNALIGNED_GET(&addr->s6_addr16[1]) == 0x00) &&
139 (UNALIGNED_GET(&addr->s6_addr32[1]) == 0x00) &&
140 (UNALIGNED_GET(&addr->s6_addr32[2]) == 0x00) &&
141 (addr->s6_addr[14] == 0x00));
142 }
143
net_6lo_maddr_32_bit_compressible(struct in6_addr * addr)144 static inline bool net_6lo_maddr_32_bit_compressible(struct in6_addr *addr)
145 {
146 return ((UNALIGNED_GET(&addr->s6_addr32[1]) == 0x00) &&
147 (UNALIGNED_GET(&addr->s6_addr32[2]) == 0x00) &&
148 (addr->s6_addr[12] == 0x00));
149 }
150
net_6lo_maddr_48_bit_compressible(struct in6_addr * addr)151 static inline bool net_6lo_maddr_48_bit_compressible(struct in6_addr *addr)
152 {
153 return ((UNALIGNED_GET(&addr->s6_addr32[1]) == 0x00) &&
154 (UNALIGNED_GET(&addr->s6_addr16[4]) == 0x00) &&
155 (addr->s6_addr[10] == 0x00));
156 }
157
158 #if defined(CONFIG_NET_6LO_CONTEXT)
159 /* RFC 6775, 4.2, 5.4.2, 5.4.3 and 7.2*/
set_6lo_context(struct net_if * iface,uint8_t index,struct net_icmpv6_nd_opt_6co * context)160 static inline void set_6lo_context(struct net_if *iface, uint8_t index,
161 struct net_icmpv6_nd_opt_6co *context)
162
163 {
164 ctx_6co[index].is_used = true;
165 ctx_6co[index].iface = iface;
166
167 /*TODO: Start timer */
168 ctx_6co[index].lifetime = context->lifetime;
169 ctx_6co[index].compress = get_6co_compress(context);
170 ctx_6co[index].cid = get_6co_cid(context);
171
172 net_ipv6_addr_copy_raw((uint8_t *)&ctx_6co[index].prefix, context->prefix);
173 }
174
net_6lo_set_context(struct net_if * iface,struct net_icmpv6_nd_opt_6co * context)175 void net_6lo_set_context(struct net_if *iface,
176 struct net_icmpv6_nd_opt_6co *context)
177 {
178 int unused = -1;
179 uint8_t i;
180
181 /* If the context information already exists, update or remove
182 * as per data.
183 */
184 for (i = 0U; i < CONFIG_NET_MAX_6LO_CONTEXTS; i++) {
185 if (!ctx_6co[i].is_used) {
186 unused = i;
187 continue;
188 }
189
190 if (ctx_6co[i].iface == iface &&
191 ctx_6co[i].cid == get_6co_cid(context)) {
192 /* Remove if lifetime is zero */
193 if (!context->lifetime) {
194 ctx_6co[i].is_used = false;
195 return;
196 }
197
198 /* Update the context */
199 set_6lo_context(iface, i, context);
200 return;
201 }
202 }
203
204 /* Cache the context information. */
205 if (unused != -1) {
206 set_6lo_context(iface, unused, context);
207 return;
208 }
209
210 NET_DBG("Either no free slots in the table or exceeds limit");
211 }
212
213 /* Get the context by matching cid */
214 static inline struct net_6lo_context *
get_6lo_context_by_cid(struct net_if * iface,uint8_t cid)215 get_6lo_context_by_cid(struct net_if *iface, uint8_t cid)
216 {
217 uint8_t i;
218
219 for (i = 0U; i < CONFIG_NET_MAX_6LO_CONTEXTS; i++) {
220 if (!ctx_6co[i].is_used) {
221 continue;
222 }
223
224 if (ctx_6co[i].iface == iface && ctx_6co[i].cid == cid) {
225 return &ctx_6co[i];
226 }
227 }
228
229 return NULL;
230 }
231
232 /* Get the context by addr */
233 static inline struct net_6lo_context *
get_6lo_context_by_addr(struct net_if * iface,struct in6_addr * addr)234 get_6lo_context_by_addr(struct net_if *iface, struct in6_addr *addr)
235 {
236 uint8_t i;
237
238 for (i = 0U; i < CONFIG_NET_MAX_6LO_CONTEXTS; i++) {
239 if (!ctx_6co[i].is_used) {
240 continue;
241 }
242
243 if (ctx_6co[i].iface == iface &&
244 !memcmp(ctx_6co[i].prefix.s6_addr, addr->s6_addr, 8)) {
245 return &ctx_6co[i];
246 }
247 }
248
249 return NULL;
250 }
251
252 #endif
253
254 /* Helper routine to compress Traffic class and Flow label */
255 /* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
256 * |Version| Traffic Class | Flow Label |
257 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
258 * version: 4 bits, Traffic Class: 8 bits, Flow label: 20 bits
259 * The Traffic Class field in the IPv6 header comprises 6 bits of
260 * Diffserv extension [RFC2474] and 2 bits of Explicit Congestion
261 * Notification (ECN) [RFC3168]
262 */
263
264 /* IPHC (compressed) format of traffic class is ECN, DSCP but original
265 * IPv6 traffic class format is DSCP, ECN.
266 * DSCP(6), ECN(2).
267 */
compress_tfl(struct net_ipv6_hdr * ipv6,uint8_t * inline_ptr,uint16_t * iphc)268 static uint8_t *compress_tfl(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
269 uint16_t *iphc)
270 {
271 uint8_t tcl;
272
273 tcl = ((ipv6->vtc & 0x0F) << 4) | ((ipv6->tcflow & 0xF0) >> 4);
274 tcl = (tcl << 6) | (tcl >> 2); /* ECN(2), DSCP(6) */
275
276 if (((ipv6->tcflow & 0x0F) == 0U) && (ipv6->flow == 0U)) {
277 if (((ipv6->vtc & 0x0F) == 0U) && ((ipv6->tcflow & 0xF0) == 0U)) {
278 NET_DBG("Traffic class and Flow label elided");
279
280 /* Traffic class and Flow label elided */
281 *iphc |= NET_6LO_IPHC_TF_11;
282 } else {
283 NET_DBG("Flow label elided");
284
285 /* Flow label elided */
286 *iphc |= NET_6LO_IPHC_TF_10;
287
288 inline_ptr -= sizeof(tcl);
289 *inline_ptr = tcl;
290 }
291 } else {
292 if (((ipv6->vtc & 0x0F) == 0U) && (ipv6->tcflow & 0x30)) {
293 NET_DBG("ECN + 2-bit Pad + Flow Label, DSCP is elided");
294
295 /* ECN + 2-bit Pad + Flow Label, DSCP is elided.*/
296 *iphc |= NET_6LO_IPHC_TF_01;
297
298 inline_ptr -= sizeof(ipv6->flow);
299 memmove(inline_ptr, &ipv6->flow, sizeof(ipv6->flow));
300
301 inline_ptr -= sizeof(uint8_t);
302 *inline_ptr = (tcl & 0xC0) | (ipv6->tcflow & 0x0F);
303 } else {
304 NET_DBG("ECN + DSCP + 4-bit Pad + Flow Label");
305
306 /* ECN + DSCP + 4-bit Pad + Flow Label */
307 *iphc |= NET_6LO_IPHC_TF_00;
308
309 inline_ptr -= sizeof(ipv6->flow);
310 memmove(inline_ptr, &ipv6->flow, sizeof(ipv6->flow));
311
312 inline_ptr -= sizeof(uint8_t);
313 *inline_ptr = ipv6->tcflow & 0x0F;
314 inline_ptr -= sizeof(tcl);
315 *inline_ptr = tcl;
316 }
317 }
318
319 return inline_ptr;
320 }
321
322 /* Helper to compress Hop limit */
compress_hoplimit(struct net_ipv6_hdr * ipv6,uint8_t * inline_ptr,uint16_t * iphc)323 static uint8_t *compress_hoplimit(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
324 uint16_t *iphc)
325 {
326 /* Hop Limit */
327 switch (ipv6->hop_limit) {
328 case 1:
329 NET_DBG("HLIM compressed (1)");
330 *iphc |= NET_6LO_IPHC_HLIM1;
331 break;
332 case 64:
333 NET_DBG("HLIM compressed (64)");
334 *iphc |= NET_6LO_IPHC_HLIM64;
335 break;
336 case 255:
337 NET_DBG("HLIM compressed (255)");
338 *iphc |= NET_6LO_IPHC_HLIM255;
339 break;
340 default:
341 inline_ptr -= sizeof(ipv6->hop_limit);
342 *inline_ptr = ipv6->hop_limit;
343 break;
344 }
345
346 return inline_ptr;
347 }
348
349 /* Helper to compress Next header */
compress_nh(struct net_ipv6_hdr * ipv6,uint8_t * inline_ptr,uint16_t * iphc)350 static uint8_t *compress_nh(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
351 uint16_t *iphc)
352 {
353 /* Next header */
354 if (ipv6->nexthdr == IPPROTO_UDP) {
355 *iphc |= NET_6LO_IPHC_NH_1;
356 } else {
357 inline_ptr -= sizeof(ipv6->nexthdr);
358 *inline_ptr = ipv6->nexthdr;
359 }
360
361 return inline_ptr;
362 }
363
364 /* Helpers to compress Source Address */
compress_sa(struct net_ipv6_hdr * ipv6,struct net_pkt * pkt,uint8_t * inline_ptr,uint16_t * iphc)365 static uint8_t *compress_sa(struct net_ipv6_hdr *ipv6, struct net_pkt *pkt,
366 uint8_t *inline_ptr, uint16_t *iphc)
367 {
368 NET_ASSERT(net_pkt_lladdr_src(pkt)->addr);
369
370 /* Address is fully elided */
371 if (net_ipv6_addr_based_on_ll((struct in6_addr *)ipv6->src,
372 net_pkt_lladdr_src(pkt))) {
373 NET_DBG("SAM_11 src address is fully elided");
374
375 *iphc |= NET_6LO_IPHC_SAM_11;
376 return inline_ptr;
377 }
378
379 /* Following 64 bits are 0000:00ff:fe00:XXXX */
380 if (net_6lo_addr_16_bit_compressible((struct in6_addr *)ipv6->src)) {
381 NET_DBG("SAM_10 src addr 16 bit compressible");
382 *iphc |= NET_6LO_IPHC_SAM_10;
383
384 inline_ptr -= sizeof(uint16_t);
385 memmove(inline_ptr, &ipv6->src[14], sizeof(uint16_t));
386
387 return inline_ptr;
388 }
389
390 NET_DBG("SAM_01 src 64 bits are inlined");
391 /* Remaining 64 bits are in-line */
392 *iphc |= NET_6LO_IPHC_SAM_01;
393
394 inline_ptr -= 8U;
395 memmove(inline_ptr, &ipv6->src[8], 8U);
396
397 return inline_ptr;
398 }
399
set_sa_inline(struct net_ipv6_hdr * ipv6,uint8_t * inline_ptr,uint16_t * iphc)400 static uint8_t *set_sa_inline(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
401 uint16_t *iphc)
402 {
403 *iphc |= NET_6LO_IPHC_SAM_00;
404 inline_ptr -= 16U;
405 memmove(inline_ptr, &ipv6->src[0], 16U);
406 return inline_ptr;
407 }
408
409 #if defined(CONFIG_NET_6LO_CONTEXT)
compress_sa_ctx(struct net_ipv6_hdr * ipv6,uint8_t * inline_ptr,struct net_pkt * pkt,uint16_t * iphc,struct net_6lo_context * src)410 static uint8_t *compress_sa_ctx(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
411 struct net_pkt *pkt, uint16_t *iphc,
412 struct net_6lo_context *src)
413 {
414 NET_ASSERT(net_pkt_lladdr_src(pkt)->addr);
415
416 NET_DBG("SAC_1 src address context based");
417 *iphc |= NET_6LO_IPHC_SAC_1;
418
419 if (net_ipv6_addr_based_on_ll((struct in6_addr *)ipv6->src,
420 net_pkt_lladdr_src(pkt))) {
421 NET_DBG("SAM_11 src address is fully elided");
422
423 /* Address is fully elided */
424 *iphc |= NET_6LO_IPHC_SAM_11;
425 return inline_ptr;
426 }
427
428 /* Following 64 bits are 0000:00ff:fe00:XXXX */
429 if (net_6lo_addr_16_bit_compressible((struct in6_addr *)ipv6->src)) {
430 NET_DBG("SAM_10 src addr 16 bit compressible");
431
432 *iphc |= NET_6LO_IPHC_SAM_10;
433
434 inline_ptr -= sizeof(uint16_t);
435 memmove(inline_ptr, &ipv6->src[14], sizeof(uint16_t));
436 return inline_ptr;
437 }
438
439 NET_DBG("SAM_01 src remaining 64 bits are inlined");
440
441 /* Remaining 64 bits are in-line */
442 *iphc |= NET_6LO_IPHC_SAM_01;
443
444 inline_ptr -= 8U;
445 memmove(inline_ptr, &ipv6->src[8], 8U);
446
447 return inline_ptr;
448 }
449 #endif
450
451 /* Helpers to compress Destination Address */
compress_da_mcast(struct net_ipv6_hdr * ipv6,uint8_t * inline_ptr,uint16_t * iphc)452 static uint8_t *compress_da_mcast(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
453 uint16_t *iphc)
454 {
455 *iphc |= NET_6LO_IPHC_M_1;
456
457 NET_DBG("M_1 dst is mcast");
458
459 if (net_6lo_maddr_8_bit_compressible((struct in6_addr *)ipv6->dst)) {
460 NET_DBG("DAM_11 dst maddr 8 bit compressible");
461
462 /* last byte */
463 *iphc |= NET_6LO_IPHC_DAM_11;
464
465 inline_ptr -= sizeof(uint8_t);
466 memmove(inline_ptr, &ipv6->dst[15], sizeof(uint8_t));
467
468 return inline_ptr;
469 }
470
471 if (net_6lo_maddr_32_bit_compressible((struct in6_addr *)ipv6->dst)) {
472 NET_DBG("DAM_10 4 bytes: 2nd byte + last three bytes");
473
474 /* 4 bytes: 2nd byte + last three bytes */
475 *iphc |= NET_6LO_IPHC_DAM_10;
476
477 inline_ptr -= 3U;
478 memmove(inline_ptr, &ipv6->dst[13], 3U);
479
480 inline_ptr -= sizeof(uint8_t);
481 memmove(inline_ptr, &ipv6->dst[1], sizeof(uint8_t));
482
483 return inline_ptr;
484 }
485
486 if (net_6lo_maddr_48_bit_compressible((struct in6_addr *)ipv6->dst)) {
487 NET_DBG("DAM_01 6 bytes: 2nd byte + last five bytes");
488
489 /* 6 bytes: 2nd byte + last five bytes */
490 *iphc |= NET_6LO_IPHC_DAM_01;
491
492 inline_ptr -= 5U;
493 memmove(inline_ptr, &ipv6->dst[11], 5U);
494
495 inline_ptr -= sizeof(uint8_t);
496 memmove(inline_ptr, &ipv6->dst[1], sizeof(uint8_t));
497
498 return inline_ptr;
499 }
500
501 NET_DBG("DAM_00 dst complete addr inlined");
502
503 /* complete address NET_6LO_IPHC_DAM_00 */
504 inline_ptr -= 16U;
505 memmove(inline_ptr, &ipv6->dst[0], 16U);
506
507 return inline_ptr;
508 }
509
compress_da(struct net_ipv6_hdr * ipv6,struct net_pkt * pkt,uint8_t * inline_ptr,uint16_t * iphc)510 static uint8_t *compress_da(struct net_ipv6_hdr *ipv6, struct net_pkt *pkt,
511 uint8_t *inline_ptr, uint16_t *iphc)
512 {
513 NET_ASSERT(net_pkt_lladdr_dst(pkt)->addr);
514
515 /* Address is fully elided */
516 if (net_ipv6_addr_based_on_ll((struct in6_addr *)ipv6->dst,
517 net_pkt_lladdr_dst(pkt))) {
518 NET_DBG("DAM_11 dst addr fully elided");
519
520 *iphc |= NET_6LO_IPHC_DAM_11;
521 return inline_ptr;
522 }
523
524 /* Following 64 bits are 0000:00ff:fe00:XXXX */
525 if (net_6lo_addr_16_bit_compressible((struct in6_addr *)ipv6->dst)) {
526 NET_DBG("DAM_10 dst addr 16 bit compressible");
527
528 *iphc |= NET_6LO_IPHC_DAM_10;
529
530 inline_ptr -= sizeof(uint16_t);
531 memmove(inline_ptr, &ipv6->dst[14], sizeof(uint16_t));
532 return inline_ptr;
533 }
534
535 NET_DBG("DAM_01 remaining 64 bits are inlined");
536
537 /* Remaining 64 bits are in-line */
538 *iphc |= NET_6LO_IPHC_DAM_01;
539
540 inline_ptr -= 8U;
541 memmove(inline_ptr, &ipv6->dst[8], 8U);
542
543 return inline_ptr;
544 }
545
set_da_inline(struct net_ipv6_hdr * ipv6,uint8_t * inline_ptr,uint16_t * iphc)546 static uint8_t *set_da_inline(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
547 uint16_t *iphc)
548 {
549 *iphc |= NET_6LO_IPHC_DAM_00;
550 inline_ptr -= 16U;
551 memmove(inline_ptr, &ipv6->dst[0], 16U);
552 return inline_ptr;
553 }
554
555 #if defined(CONFIG_NET_6LO_CONTEXT)
compress_da_ctx(struct net_ipv6_hdr * ipv6,uint8_t * inline_ptr,struct net_pkt * pkt,uint16_t * iphc,struct net_6lo_context * dst)556 static uint8_t *compress_da_ctx(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
557 struct net_pkt *pkt, uint16_t *iphc,
558 struct net_6lo_context *dst)
559 {
560 *iphc |= NET_6LO_IPHC_DAC_1;
561
562 if (net_ipv6_addr_based_on_ll((struct in6_addr *)ipv6->dst,
563 net_pkt_lladdr_dst(pkt))) {
564 NET_DBG("DAM_11 dst addr fully elided");
565
566 *iphc |= NET_6LO_IPHC_DAM_11;
567 return inline_ptr;
568 }
569
570 /* Following 64 bits are 0000:00ff:fe00:XXXX */
571 if (net_6lo_addr_16_bit_compressible((struct in6_addr *)ipv6->dst)) {
572 NET_DBG("DAM_10 dst addr 16 bit compressible");
573
574 *iphc |= NET_6LO_IPHC_DAM_10;
575 inline_ptr -= sizeof(uint16_t);
576 memmove(inline_ptr, &ipv6->dst[14], sizeof(uint16_t));
577 return inline_ptr;
578 }
579
580 NET_DBG("DAM_01 remaining 64 bits are inlined");
581
582 /* Remaining 64 bits are in-line */
583 *iphc |= NET_6LO_IPHC_DAM_01;
584
585 inline_ptr -= 8U;
586 memmove(inline_ptr, &ipv6->dst[8], 8U);
587
588 return inline_ptr;
589 }
590 #endif
591
592 /* Helper to compress Next header UDP */
compress_nh_udp(struct net_udp_hdr * udp,uint8_t * inline_ptr,bool compress_checksum)593 static inline uint8_t *compress_nh_udp(struct net_udp_hdr *udp, uint8_t *inline_ptr,
594 bool compress_checksum)
595 {
596 uint8_t nhc = NET_6LO_NHC_UDP_BARE;
597 uint8_t *inline_ptr_udp = inline_ptr;
598 uint8_t tmp;
599
600 /* 4.3.3 UDP LOWPAN_NHC Format
601 * 0 1 2 3 4 5 6 7
602 * +---+---+---+---+---+---+---+---+
603 * | 1 | 1 | 1 | 1 | 0 | C | P |
604 * +---+---+---+---+---+---+---+---+
605 */
606
607 /* Port compression
608 * 00: All 16 bits for src and dst are inlined.
609 * 01: All 16 bits for src port inlined. First 8 bits of dst port is
610 * 0xf0 and elided. The remaining 8 bits of dst port inlined.
611 * 10: First 8 bits of src port 0xf0 and elided. The remaining 8 bits
612 * of src port inlined. All 16 bits of dst port inlined.
613 * 11: First 12 bits of both src and dst are 0xf0b and elided. The
614 * remaining 4 bits for each are inlined.
615 */
616
617 if (compress_checksum) {
618 nhc |= NET_6LO_NHC_UDP_CHECKSUM;
619 } else {
620 inline_ptr_udp -= sizeof(udp->chksum);
621 memmove(inline_ptr_udp, &udp->chksum, sizeof(udp->chksum));
622 }
623
624 if ((((htons(udp->src_port) >> 4) & 0xFFF) ==
625 NET_6LO_NHC_UDP_4_BIT_PORT) &&
626 (((htons(udp->dst_port) >> 4) & 0xFFF) ==
627 NET_6LO_NHC_UDP_4_BIT_PORT)) {
628
629 NET_DBG("UDP ports src and dst 4 bits inlined");
630 /** src: first 16 bits elided, next 4 bits inlined
631 * dst: first 16 bits elided, next 4 bits inlined
632 */
633 nhc |= NET_6LO_NHC_UDP_PORT_11;
634
635 tmp = (uint8_t)(htons(udp->src_port));
636 tmp = tmp << 4;
637
638 tmp |= (((uint8_t)(htons(udp->dst_port))) & 0x0F);
639 inline_ptr_udp -= sizeof(tmp);
640 *inline_ptr_udp = tmp;
641
642 } else if (((htons(udp->dst_port) >> 8) & 0xFF) ==
643 NET_6LO_NHC_UDP_8_BIT_PORT) {
644
645 NET_DBG("UDP ports src full, dst 8 bits inlined");
646 /* dst: first 8 bits elided, next 8 bits inlined
647 * src: fully carried inline
648 */
649 nhc |= NET_6LO_NHC_UDP_PORT_01;
650
651 inline_ptr_udp -= sizeof(uint8_t);
652 *inline_ptr_udp = (uint8_t)(htons(udp->dst_port));
653
654 inline_ptr_udp -= sizeof(udp->src_port);
655 memmove(inline_ptr_udp, &udp->src_port, sizeof(udp->src_port));
656
657 } else if (((htons(udp->src_port) >> 8) & 0xFF) ==
658 NET_6LO_NHC_UDP_8_BIT_PORT) {
659
660 NET_DBG("UDP ports src 8bits, dst full inlined");
661 /* src: first 8 bits elided, next 8 bits inlined
662 * dst: fully carried inline
663 */
664 nhc |= NET_6LO_NHC_UDP_PORT_10;
665
666 inline_ptr_udp -= sizeof(udp->dst_port);
667 memmove(inline_ptr_udp, &udp->dst_port, sizeof(udp->dst_port));
668
669 inline_ptr_udp -= sizeof(uint8_t);
670 *inline_ptr_udp = (uint8_t)(htons(udp->src_port));
671
672 } else {
673 NET_DBG("Can not compress ports, ports are inlined");
674
675 /* can not compress ports, ports are inlined */
676 inline_ptr_udp -= sizeof(udp->dst_port) + sizeof(udp->src_port);
677 memmove(inline_ptr_udp, &udp->src_port,
678 sizeof(udp->dst_port) + sizeof(udp->src_port));
679 }
680
681 inline_ptr_udp -= sizeof(nhc);
682 *inline_ptr_udp = nhc;
683
684 return inline_ptr_udp;
685 }
686
687 #if defined(CONFIG_NET_6LO_CONTEXT)
688
get_src_addr_ctx(struct net_pkt * pkt,struct net_ipv6_hdr * ipv6)689 static struct net_6lo_context *get_src_addr_ctx(struct net_pkt *pkt,
690 struct net_ipv6_hdr *ipv6)
691 {
692 /* If compress flag is unset means use only in uncompression. */
693 struct net_6lo_context *src;
694
695 src = get_6lo_context_by_addr(net_pkt_iface(pkt),
696 (struct in6_addr *)ipv6->src);
697 if (!src || !src->compress) {
698 return NULL;
699 }
700
701 return src;
702 }
703
get_dst_addr_ctx(struct net_pkt * pkt,struct net_ipv6_hdr * ipv6)704 static struct net_6lo_context *get_dst_addr_ctx(struct net_pkt *pkt,
705 struct net_ipv6_hdr *ipv6)
706 {
707 /* If compress flag is unset means use only in uncompression. */
708 struct net_6lo_context *dst;
709
710 dst = get_6lo_context_by_addr(net_pkt_iface(pkt),
711 (struct in6_addr *)ipv6->dst);
712 if (!dst || !dst->compress) {
713 return NULL;
714 }
715
716 return dst;
717 }
718 #endif /* CONFIG_NET_6LO_CONTEXT */
719
720 /* RFC 6282 LOWPAN IPHC Encoding format (3.1)
721 * Base Format
722 * 0 1
723 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
724 * +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
725 * | 0 | 1 | 1 | TF |NH | HLIM |CID|SAC| SAM | M |DAC| DAM |
726 * +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
727 */
compress_IPHC_header(struct net_pkt * pkt)728 static inline int compress_IPHC_header(struct net_pkt *pkt)
729 {
730 #if defined(CONFIG_NET_6LO_CONTEXT)
731 struct net_6lo_context *src_ctx = NULL;
732 struct net_6lo_context *dst_ctx = NULL;
733 #endif
734 uint8_t compressed = 0;
735 uint16_t iphc = (NET_6LO_DISPATCH_IPHC << 8);
736 struct net_ipv6_hdr *ipv6 = NET_IPV6_HDR(pkt);
737 struct net_udp_hdr *udp;
738 uint8_t *inline_pos;
739
740 if (pkt->frags->len < NET_IPV6H_LEN) {
741 NET_ERR("Invalid length %d, min %d",
742 pkt->frags->len, NET_IPV6H_LEN);
743 return -EINVAL;
744 }
745
746 if (ipv6->nexthdr == IPPROTO_UDP &&
747 pkt->frags->len < NET_IPV6UDPH_LEN) {
748 NET_ERR("Invalid length %d, min %d",
749 pkt->frags->len, NET_IPV6UDPH_LEN);
750 return -EINVAL;
751 }
752
753 inline_pos = pkt->buffer->data + NET_IPV6H_LEN;
754
755 if (ipv6->nexthdr == IPPROTO_UDP) {
756 udp = (struct net_udp_hdr *)inline_pos;
757 inline_pos += NET_UDPH_LEN;
758
759 inline_pos = compress_nh_udp(udp, inline_pos, false);
760 }
761
762 if (net_6lo_ll_prefix_padded_with_zeros((struct in6_addr *)ipv6->dst)) {
763 inline_pos = compress_da(ipv6, pkt, inline_pos, &iphc);
764 goto da_end;
765 }
766
767 if (net_ipv6_is_addr_mcast((struct in6_addr *)ipv6->dst)) {
768 inline_pos = compress_da_mcast(ipv6, inline_pos, &iphc);
769 goto da_end;
770 }
771
772 #if defined(CONFIG_NET_6LO_CONTEXT)
773 dst_ctx = get_dst_addr_ctx(pkt, ipv6);
774 if (dst_ctx) {
775 iphc |= NET_6LO_IPHC_CID_1;
776 inline_pos = compress_da_ctx(ipv6, inline_pos, pkt, &iphc,
777 dst_ctx);
778 goto da_end;
779 }
780 #endif
781 inline_pos = set_da_inline(ipv6, inline_pos, &iphc);
782 da_end:
783
784 if (net_6lo_ll_prefix_padded_with_zeros((struct in6_addr *)ipv6->src)) {
785 inline_pos = compress_sa(ipv6, pkt, inline_pos, &iphc);
786 goto sa_end;
787 }
788
789 if (net_ipv6_is_addr_unspecified((struct in6_addr *)ipv6->src)) {
790 NET_DBG("SAM_00, SAC_1 unspecified src address");
791
792 /* Unspecified IPv6 src address */
793 iphc |= NET_6LO_IPHC_SAC_1;
794 iphc |= NET_6LO_IPHC_SAM_00;
795 goto sa_end;
796 }
797
798 #if defined(CONFIG_NET_6LO_CONTEXT)
799 src_ctx = get_src_addr_ctx(pkt, ipv6);
800 if (src_ctx) {
801 inline_pos = compress_sa_ctx(ipv6, inline_pos, pkt, &iphc,
802 src_ctx);
803 iphc |= NET_6LO_IPHC_CID_1;
804 goto sa_end;
805 }
806 #endif
807 inline_pos = set_sa_inline(ipv6, inline_pos, &iphc);
808 sa_end:
809
810 inline_pos = compress_hoplimit(ipv6, inline_pos, &iphc);
811 inline_pos = compress_nh(ipv6, inline_pos, &iphc);
812 inline_pos = compress_tfl(ipv6, inline_pos, &iphc);
813
814 #if defined(CONFIG_NET_6LO_CONTEXT)
815 if (iphc & NET_6LO_IPHC_CID_1) {
816 inline_pos -= sizeof(uint8_t);
817 *inline_pos = 0;
818
819 if (src_ctx) {
820 *inline_pos = src_ctx->cid << 4;
821 }
822
823 if (dst_ctx) {
824 *inline_pos |= dst_ctx->cid & 0x0F;
825 }
826 }
827 #endif
828
829 inline_pos -= sizeof(iphc);
830 iphc = htons(iphc);
831 memmove(inline_pos, &iphc, sizeof(iphc));
832
833 compressed = inline_pos - pkt->buffer->data;
834
835 net_pkt_cursor_init(pkt);
836 net_pkt_pull(pkt, compressed);
837 net_pkt_compact(pkt);
838
839 return compressed;
840 }
841
842 /* Helper to uncompress Traffic class and Flow label */
uncompress_tfl(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6)843 static inline uint8_t *uncompress_tfl(uint16_t iphc, uint8_t *cursor,
844 struct net_ipv6_hdr *ipv6)
845 {
846 uint8_t tcl;
847
848 /* Uncompress tcl and flow label */
849 switch (iphc & NET_6LO_IPHC_TF_11) {
850 case NET_6LO_IPHC_TF_00:
851 NET_DBG("ECN + DSCP + 4-bit Pad + Flow Label");
852
853 tcl = *cursor;
854 cursor++;
855 tcl = (tcl >> 6) | (tcl << 2);
856
857 ipv6->vtc |= ((tcl & 0xF0) >> 4);
858 ipv6->tcflow = ((tcl & 0x0F) << 4) | (*cursor & 0x0F);
859 cursor++;
860
861 memmove(&ipv6->flow, cursor, sizeof(ipv6->flow));
862 cursor += sizeof(ipv6->flow);
863 break;
864 case NET_6LO_IPHC_TF_01:
865 NET_DBG("ECN + 2-bit Pad + Flow Label, DSCP is elided");
866
867 tcl = ((*cursor & 0xF0) >> 6);
868 ipv6->tcflow = ((tcl & 0x0F) << 4) | (*cursor & 0x0F);
869 cursor++;
870
871 memmove(&ipv6->flow, cursor, sizeof(ipv6->flow));
872 cursor += sizeof(ipv6->flow);
873
874 break;
875 case NET_6LO_IPHC_TF_10:
876 NET_DBG("Flow label elided");
877
878 tcl = *cursor;
879 cursor++;
880 tcl = (tcl >> 6) | (tcl << 2);
881
882 ipv6->vtc |= ((tcl & 0xF0) >> 4);
883 ipv6->tcflow = (tcl & 0x0F) << 4;
884 ipv6->flow = 0U;
885
886 break;
887 case NET_6LO_IPHC_TF_11:
888 NET_DBG("Tcl and Flow label elided");
889
890 ipv6->tcflow = 0U;
891 ipv6->flow = 0U;
892
893 break;
894 }
895
896 return cursor;
897 }
898
899 /* Helper to uncompress Hoplimit */
uncompress_hoplimit(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6)900 static inline uint8_t *uncompress_hoplimit(uint16_t iphc, uint8_t *cursor,
901 struct net_ipv6_hdr *ipv6)
902 {
903 switch (iphc & NET_6LO_IPHC_HLIM_MASK) {
904 case NET_6LO_IPHC_HLIM:
905 ipv6->hop_limit = *cursor;
906 cursor++;
907
908 break;
909 case NET_6LO_IPHC_HLIM1:
910 ipv6->hop_limit = 1U;
911
912 break;
913 case NET_6LO_IPHC_HLIM64:
914 ipv6->hop_limit = 64U;
915
916 break;
917 case NET_6LO_IPHC_HLIM255:
918 ipv6->hop_limit = 255U;
919
920 break;
921 }
922
923 return cursor;
924 }
925
926 /* Helper to uncompress Source Address */
uncompress_sa(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6,struct net_pkt * pkt)927 static inline uint8_t *uncompress_sa(uint16_t iphc, uint8_t *cursor,
928 struct net_ipv6_hdr *ipv6,
929 struct net_pkt *pkt)
930 {
931 struct in6_addr src_ip;
932
933 NET_DBG("SAC_0");
934
935 net_ipv6_addr_copy_raw((uint8_t *)&src_ip, ipv6->src);
936
937 switch (iphc & NET_6LO_IPHC_SAM_MASK) {
938 case NET_6LO_IPHC_SAM_00:
939 NET_DBG("SAM_00 full src addr inlined");
940
941 memmove(src_ip.s6_addr, cursor, sizeof(src_ip.s6_addr));
942 cursor += sizeof(src_ip.s6_addr);
943
944 break;
945 case NET_6LO_IPHC_SAM_01:
946 NET_DBG("SAM_01 last 64 bits are inlined");
947
948 memmove(&src_ip.s6_addr[8], cursor, 8);
949 cursor += 8U;
950
951 src_ip.s6_addr32[0] = 0x00;
952 src_ip.s6_addr32[1] = 0x00;
953 src_ip.s6_addr[0] = 0xFE;
954 src_ip.s6_addr[1] = 0x80;
955
956 break;
957 case NET_6LO_IPHC_SAM_10:
958 NET_DBG("SAM_10 src addr 16 bit compressed");
959
960 memmove(&src_ip.s6_addr[14], cursor, 2);
961 cursor += 2U;
962 src_ip.s6_addr16[6] = 0x00;
963
964 src_ip.s6_addr32[0] = 0x00;
965 src_ip.s6_addr32[1] = 0x00;
966 src_ip.s6_addr32[2] = 0x00;
967 src_ip.s6_addr[0] = 0xFE;
968 src_ip.s6_addr[1] = 0x80;
969 src_ip.s6_addr[11] = 0xFF;
970 src_ip.s6_addr[12] = 0xFE;
971
972 break;
973 case NET_6LO_IPHC_SAM_11:
974 NET_DBG("SAM_11 generate src addr from ll");
975
976 net_ipv6_addr_create_iid(&src_ip, net_pkt_lladdr_src(pkt));
977
978 break;
979 }
980
981 net_ipv6_addr_copy_raw(ipv6->src, (uint8_t *)&src_ip);
982
983 return cursor;
984 }
985
986 #if defined(CONFIG_NET_6LO_CONTEXT)
uncompress_sa_ctx(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6,struct net_6lo_context * ctx,struct net_pkt * pkt)987 static inline uint8_t *uncompress_sa_ctx(uint16_t iphc, uint8_t *cursor,
988 struct net_ipv6_hdr *ipv6,
989 struct net_6lo_context *ctx,
990 struct net_pkt *pkt)
991 {
992 struct in6_addr src_ip;
993
994 net_ipv6_addr_copy_raw((uint8_t *)&src_ip, ipv6->src);
995
996 switch (iphc & NET_6LO_IPHC_SAM_MASK) {
997 case NET_6LO_IPHC_SAM_01:
998 NET_DBG("SAM_01 last 64 bits are inlined");
999
1000 /* First 8 bytes are from context */
1001 memmove(&src_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
1002
1003 /* And the rest are carried in-line*/
1004 memmove(&src_ip.s6_addr[8], cursor, 8);
1005 cursor += 8U;
1006
1007 break;
1008 case NET_6LO_IPHC_SAM_10:
1009 NET_DBG("SAM_10 src addr 16 bit compressed");
1010
1011 /* 16 bit carried in-line */
1012 memmove(&src_ip.s6_addr[14], cursor, 2);
1013 cursor += 2U;
1014
1015 /* First 8 bytes are from context */
1016 memmove(&src_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
1017
1018 src_ip.s6_addr32[2] = 0x00;
1019 src_ip.s6_addr16[6] = 0x00;
1020 src_ip.s6_addr[11] = 0xFF;
1021 src_ip.s6_addr[12] = 0xFE;
1022
1023 break;
1024 case NET_6LO_IPHC_SAM_11:
1025 NET_DBG("SAM_11 generate src addr from ll");
1026
1027 /* RFC 6282, 3.1.1. If SAC = 1 and SAM = 11
1028 * Derive addr using context information and
1029 * the encapsulating header.
1030 * (e.g., 802.15.4 or IPv6 source address).
1031 */
1032 net_ipv6_addr_create_iid(&src_ip, net_pkt_lladdr_src(pkt));
1033
1034 /* net_ipv6_addr_create_iid will copy first 8 bytes
1035 * as link local prefix.
1036 * Overwrite first 8 bytes from context prefix here.
1037 */
1038 memmove(&src_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
1039 break;
1040 }
1041
1042 net_ipv6_addr_copy_raw(ipv6->src, (uint8_t *)&src_ip);
1043
1044 return cursor;
1045 }
1046 #endif
1047
1048 /* Helpers to uncompress Destination Address */
uncompress_da_mcast(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6)1049 static inline uint8_t *uncompress_da_mcast(uint16_t iphc, uint8_t *cursor,
1050 struct net_ipv6_hdr *ipv6)
1051 {
1052 struct in6_addr dst_ip;
1053
1054 NET_DBG("Dst is multicast");
1055
1056 net_ipv6_addr_copy_raw((uint8_t *)&dst_ip, ipv6->dst);
1057
1058 if (iphc & NET_6LO_IPHC_DAC_1) {
1059 NET_WARN("Unsupported DAM options");
1060 return 0;
1061 }
1062
1063 /* If M=1 and DAC=0:
1064 * 00: 128 bits, The full address is carried in-line.
1065 * 01: 48 bits, The address takes the form ffXX::00XX:XXXX:XXXX.
1066 * 10: 32 bits, The address takes the form ffXX::00XX:XXXX.
1067 * 11: 8 bits, The address takes the form ff02::00XX.
1068 */
1069
1070 switch (iphc & NET_6LO_IPHC_DAM_MASK) {
1071 case NET_6LO_IPHC_DAM_00:
1072 NET_DBG("DAM_00 full dst addr inlined");
1073
1074 memmove(&dst_ip.s6_addr[0], cursor,
1075 sizeof(dst_ip.s6_addr));
1076
1077 cursor += sizeof(dst_ip.s6_addr);
1078 break;
1079 case NET_6LO_IPHC_DAM_01:
1080 NET_DBG("DAM_01 2nd byte and last five bytes");
1081
1082 dst_ip.s6_addr[1] = *cursor;
1083 cursor++;
1084 memmove(&dst_ip.s6_addr[11], cursor, 5);
1085 cursor += 5U;
1086
1087
1088 dst_ip.s6_addr[0] = 0xFF;
1089 dst_ip.s6_addr16[1] = 0x00;
1090 dst_ip.s6_addr32[1] = 0x00;
1091 dst_ip.s6_addr[10] = 0x00;
1092 dst_ip.s6_addr16[4] = 0x00;
1093
1094 break;
1095 case NET_6LO_IPHC_DAM_10:
1096 NET_DBG("DAM_10 2nd byte and last three bytes");
1097
1098 dst_ip.s6_addr[1] = *cursor;
1099 cursor++;
1100 memmove(&dst_ip.s6_addr[13], cursor, 3);
1101 cursor += 3U;
1102
1103 dst_ip.s6_addr[0] = 0xFF;
1104 dst_ip.s6_addr16[1] = 0x00;
1105 dst_ip.s6_addr32[1] = 0x00;
1106 dst_ip.s6_addr32[2] = 0x00;
1107 dst_ip.s6_addr[12] = 0x00;
1108
1109 break;
1110 case NET_6LO_IPHC_DAM_11:
1111 NET_DBG("DAM_11 8 bit compressed");
1112
1113 dst_ip.s6_addr[15] = *cursor;
1114 cursor++;
1115 dst_ip.s6_addr[14] = 0x00;
1116
1117 dst_ip.s6_addr32[0] = 0x00;
1118 dst_ip.s6_addr32[1] = 0x00;
1119 dst_ip.s6_addr32[2] = 0x00;
1120 dst_ip.s6_addr16[6] = 0x00;
1121 dst_ip.s6_addr[0] = 0xFF;
1122 dst_ip.s6_addr[1] = 0x02;
1123
1124 break;
1125 }
1126
1127 net_ipv6_addr_copy_raw(ipv6->dst, (uint8_t *)&dst_ip);
1128
1129 return cursor;
1130 }
1131
1132 /* Helper to uncompress Destination Address */
uncompress_da(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6,struct net_pkt * pkt)1133 static inline uint8_t *uncompress_da(uint16_t iphc, uint8_t *cursor,
1134 struct net_ipv6_hdr *ipv6,
1135 struct net_pkt *pkt)
1136 {
1137 struct in6_addr dst_ip;
1138
1139 NET_DBG("DAC_0");
1140
1141 net_ipv6_addr_copy_raw((uint8_t *)&dst_ip, ipv6->dst);
1142
1143 switch (iphc & NET_6LO_IPHC_DAM_MASK) {
1144 case NET_6LO_IPHC_DAM_00:
1145 NET_DBG("DAM_00 full dst addr inlined");
1146
1147 memmove(&dst_ip.s6_addr[0], cursor,
1148 sizeof(dst_ip.s6_addr));
1149 cursor += sizeof(dst_ip.s6_addr);
1150
1151 break;
1152 case NET_6LO_IPHC_DAM_01:
1153 NET_DBG("DAM_01 last 64 bits are inlined");
1154
1155 memmove(&dst_ip.s6_addr[8], cursor, 8);
1156 cursor += 8U;
1157
1158 dst_ip.s6_addr32[0] = 0x00;
1159 dst_ip.s6_addr32[1] = 0x00;
1160 dst_ip.s6_addr[0] = 0xFE;
1161 dst_ip.s6_addr[1] = 0x80;
1162
1163 break;
1164 case NET_6LO_IPHC_DAM_10:
1165 NET_DBG("DAM_10 dst addr 16 bit compressed");
1166
1167 memmove(&dst_ip.s6_addr[14], cursor, 2);
1168 cursor += 2U;
1169
1170 dst_ip.s6_addr32[0] = 0x00;
1171 dst_ip.s6_addr32[1] = 0x00;
1172 dst_ip.s6_addr32[2] = 0x00;
1173 dst_ip.s6_addr16[6] = 0x00;
1174 dst_ip.s6_addr[0] = 0xFE;
1175 dst_ip.s6_addr[1] = 0x80;
1176 dst_ip.s6_addr[11] = 0xFF;
1177 dst_ip.s6_addr[12] = 0xFE;
1178
1179 break;
1180 case NET_6LO_IPHC_DAM_11:
1181 NET_DBG("DAM_11 generate dst addr from ll");
1182
1183 net_ipv6_addr_create_iid(&dst_ip, net_pkt_lladdr_dst(pkt));
1184
1185 break;
1186 }
1187
1188 net_ipv6_addr_copy_raw(ipv6->dst, (uint8_t *)&dst_ip);
1189
1190 return cursor;
1191 }
1192
1193 #if defined(CONFIG_NET_6LO_CONTEXT)
uncompress_da_ctx(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6,struct net_6lo_context * ctx,struct net_pkt * pkt)1194 static inline uint8_t *uncompress_da_ctx(uint16_t iphc, uint8_t *cursor,
1195 struct net_ipv6_hdr *ipv6,
1196 struct net_6lo_context *ctx,
1197 struct net_pkt *pkt)
1198 {
1199 struct in6_addr dst_ip;
1200
1201 NET_DBG("DAC_1");
1202
1203 net_ipv6_addr_copy_raw((uint8_t *)&dst_ip, ipv6->dst);
1204
1205 switch (iphc & NET_6LO_IPHC_DAM_MASK) {
1206 case NET_6LO_IPHC_DAM_01:
1207 NET_DBG("DAM_01 last 64 bits are inlined");
1208
1209 /* Last 8 bytes carried in-line */
1210 memmove(&dst_ip.s6_addr[8], cursor, 8);
1211 cursor += 8U;
1212
1213 /* First 8 bytes are from context */
1214 memmove(&dst_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
1215
1216 break;
1217 case NET_6LO_IPHC_DAM_10:
1218 NET_DBG("DAM_10 src addr 16 bit compressed");
1219
1220 /* 16 bit carried in-line */
1221 memmove(&dst_ip.s6_addr[14], cursor, 2);
1222 cursor += 2U;
1223
1224 /* First 8 bytes are from context */
1225 memmove(&dst_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
1226
1227 dst_ip.s6_addr32[2] = 0x00;
1228 dst_ip.s6_addr16[6] = 0x00;
1229 dst_ip.s6_addr[11] = 0xFF;
1230 dst_ip.s6_addr[12] = 0xFE;
1231
1232 break;
1233 case NET_6LO_IPHC_DAM_11:
1234 NET_DBG("DAM_11 generate src addr from ll");
1235
1236 /* RFC 6282, 3.1.1. If SAC = 1 and SAM = 11
1237 * Derive addr using context information and
1238 * the encapsulating header.
1239 * (e.g., 802.15.4 or IPv6 source address).
1240 */
1241 net_ipv6_addr_create_iid(&dst_ip, net_pkt_lladdr_dst(pkt));
1242
1243 /* net_ipv6_addr_create_iid will copy first 8 bytes
1244 * as link local prefix.
1245 * Overwrite first 8 bytes from context prefix here.
1246 */
1247 memmove(&dst_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
1248
1249 break;
1250 }
1251
1252 net_ipv6_addr_copy_raw(ipv6->dst, (uint8_t *)&dst_ip);
1253
1254 return cursor;
1255 }
1256 #endif
1257
1258 /* Helper to uncompress NH UDP */
uncompress_nh_udp(uint8_t nhc,uint8_t * cursor,struct net_udp_hdr * udp)1259 static uint8_t *uncompress_nh_udp(uint8_t nhc, uint8_t *cursor,
1260 struct net_udp_hdr *udp)
1261 {
1262
1263 /* Port uncompression
1264 * 00: All 16 bits for src and dst are inlined
1265 * 01: src, 16 bits are lined, dst(0xf0) 8 bits are inlined
1266 * 10: dst, 16 bits are lined, src(0xf0) 8 bits are inlined
1267 * 11: src, dst (0xf0b) 4 bits are inlined
1268 */
1269
1270 /* UDP header uncompression */
1271 switch (nhc & NET_6LO_NHC_UDP_PORT_11) {
1272 case NET_6LO_NHC_UDP_PORT_00:
1273 NET_DBG("src and dst ports are inlined");
1274
1275 memmove(&udp->src_port, cursor, sizeof(udp->src_port));
1276 cursor += sizeof(udp->src_port);
1277 memmove(&udp->dst_port, cursor, sizeof(udp->dst_port));
1278 cursor += sizeof(udp->dst_port);
1279
1280 break;
1281 case NET_6LO_NHC_UDP_PORT_01:
1282 NET_DBG("src full, dst 8 bits inlined");
1283
1284 memmove(&udp->src_port, cursor, sizeof(udp->src_port));
1285 cursor += sizeof(udp->src_port);
1286 udp->dst_port = htons(((uint16_t)NET_6LO_NHC_UDP_8_BIT_PORT << 8) |
1287 *cursor);
1288 cursor++;
1289
1290 break;
1291 case NET_6LO_NHC_UDP_PORT_10:
1292 NET_DBG("src 8 bits, dst full inlined");
1293
1294 udp->src_port = htons(((uint16_t)NET_6LO_NHC_UDP_8_BIT_PORT << 8) |
1295 *cursor);
1296 cursor++;
1297 memmove(&udp->dst_port, cursor, sizeof(udp->dst_port));
1298 cursor += sizeof(udp->dst_port);
1299
1300 break;
1301 case NET_6LO_NHC_UDP_PORT_11:
1302 NET_DBG("src and dst 4 bits inlined");
1303
1304 udp->src_port = htons((NET_6LO_NHC_UDP_4_BIT_PORT << 4) |
1305 (*cursor >> 4));
1306
1307 udp->dst_port = htons((NET_6LO_NHC_UDP_4_BIT_PORT << 4) |
1308 (*cursor & 0x0F));
1309 cursor++;
1310
1311 break;
1312 }
1313
1314 if (!(nhc & NET_6LO_NHC_UDP_CHECKSUM)) {
1315 memmove(&udp->chksum, cursor, sizeof(udp->chksum));
1316 cursor += sizeof(udp->chksum);
1317 }
1318
1319 return cursor;
1320 }
1321
1322 #if defined(CONFIG_NET_6LO_CONTEXT)
1323 /* Helper function to uncompress src and dst contexts */
uncompress_cid(struct net_pkt * pkt,uint8_t cid,struct net_6lo_context ** src,struct net_6lo_context ** dst)1324 static inline void uncompress_cid(struct net_pkt *pkt, uint8_t cid,
1325 struct net_6lo_context **src,
1326 struct net_6lo_context **dst)
1327 {
1328 uint8_t cid_tmp;
1329
1330 /* Extract source and destination Context Index,
1331 * Either src or dest address is context based or both.
1332 */
1333 cid_tmp = (cid >> 4) & 0x0F;
1334 *src = get_6lo_context_by_cid(net_pkt_iface(pkt), cid_tmp);
1335 if (!(*src)) {
1336 NET_DBG("Unknown src cid %d", cid_tmp);
1337 }
1338
1339 cid_tmp = cid & 0x0F;
1340 *dst = get_6lo_context_by_cid(net_pkt_iface(pkt), cid_tmp);
1341 if (!(*dst)) {
1342 NET_DBG("Unknown dst cid %d", cid_tmp);
1343 }
1344 }
1345 #endif
1346
uncompress_IPHC_header(struct net_pkt * pkt)1347 static bool uncompress_IPHC_header(struct net_pkt *pkt)
1348 {
1349 struct net_udp_hdr *udp = NULL;
1350 struct net_buf *frag = NULL;
1351 uint8_t nhc = 0;
1352 int nhc_inline_size = 0;
1353 struct net_ipv6_hdr *ipv6;
1354 uint16_t len;
1355 uint16_t iphc;
1356 int inline_size, compressed_hdr_size;
1357 size_t diff;
1358 uint8_t *cursor;
1359 #if defined(CONFIG_NET_6LO_CONTEXT)
1360 struct net_6lo_context *src = NULL;
1361 struct net_6lo_context *dst = NULL;
1362 #endif
1363
1364 iphc = ntohs(UNALIGNED_GET((uint16_t *)pkt->buffer->data));
1365
1366 inline_size = get_ihpc_inlined_size(iphc);
1367 if (inline_size < 0) {
1368 return false;
1369 }
1370
1371 compressed_hdr_size = sizeof(iphc) + inline_size;
1372 diff = sizeof(struct net_ipv6_hdr) - compressed_hdr_size;
1373
1374 if (iphc & NET_6LO_IPHC_NH_MASK) {
1375 nhc = *(pkt->buffer->data + sizeof(iphc) + inline_size);
1376 if ((nhc & 0xF8) != NET_6LO_NHC_UDP_BARE) {
1377 NET_ERR("Unsupported next header");
1378 return false;
1379 }
1380
1381 nhc_inline_size = get_udp_nhc_inlined_size(nhc);
1382 compressed_hdr_size += sizeof(uint8_t) + nhc_inline_size;
1383 diff += sizeof(struct net_udp_hdr) - sizeof(uint8_t) -
1384 nhc_inline_size;
1385 }
1386
1387 if (pkt->buffer->len < compressed_hdr_size) {
1388 NET_ERR("Scattered compressed header?");
1389 return false;
1390 }
1391
1392 if (net_buf_tailroom(pkt->buffer) >= diff) {
1393 NET_DBG("Enough tailroom. Uncompress inplace");
1394 frag = pkt->buffer;
1395 net_buf_add(frag, diff);
1396 cursor = frag->data + diff;
1397 memmove(cursor, frag->data, frag->len - diff);
1398 } else {
1399 size_t frag_len = nhc ? NET_IPV6UDPH_LEN : NET_IPV6H_LEN;
1400
1401 NET_DBG("Not enough tailroom. Get new fragment");
1402 cursor = pkt->buffer->data;
1403 frag = net_pkt_get_frag(pkt, frag_len, NET_6LO_RX_PKT_TIMEOUT);
1404 if (!frag) {
1405 NET_ERR("Can't get frag for uncompression");
1406 return false;
1407 }
1408
1409 net_buf_pull(pkt->buffer, compressed_hdr_size);
1410 net_buf_add(frag, frag_len);
1411 }
1412
1413 ipv6 = (struct net_ipv6_hdr *)(frag->data);
1414 cursor += sizeof(iphc);
1415
1416 if (iphc & NET_6LO_IPHC_CID_1) {
1417 #if defined(CONFIG_NET_6LO_CONTEXT)
1418 uncompress_cid(pkt, *cursor, &src, &dst);
1419 cursor++;
1420 #else
1421 NET_ERR("Context based uncompression not enabled");
1422 return false;
1423 #endif
1424 }
1425
1426 /* Version is always 6 */
1427 ipv6->vtc = 0x60;
1428 net_pkt_set_ip_hdr_len(pkt, NET_IPV6H_LEN);
1429
1430 /* Uncompress Traffic class and Flow label */
1431 cursor = uncompress_tfl(iphc, cursor, ipv6);
1432
1433 if (!(iphc & NET_6LO_IPHC_NH_MASK)) {
1434 ipv6->nexthdr = *cursor;
1435 cursor++;
1436 }
1437
1438 /* Uncompress Hoplimit */
1439 cursor = uncompress_hoplimit(iphc, cursor, ipv6);
1440
1441 /* Uncompress Source Address */
1442 if (iphc & NET_6LO_IPHC_SAC_1) {
1443 NET_DBG("SAC_1");
1444
1445 if ((iphc & NET_6LO_IPHC_SAM_MASK) == NET_6LO_IPHC_SAM_00) {
1446 NET_DBG("SAM_00 unspecified address");
1447 memset(&ipv6->src[0], 0,
1448 sizeof(ipv6->src));
1449 } else if (IS_ENABLED(CONFIG_NET_6LO_CONTEXT)) {
1450 #if defined(CONFIG_NET_6LO_CONTEXT)
1451 if (!src) {
1452 NET_ERR("Src context doesn't exists");
1453 goto fail;
1454 }
1455
1456 cursor = uncompress_sa_ctx(iphc, cursor, ipv6, src, pkt);
1457 #endif
1458 } else {
1459 NET_ERR("Context based uncompression not enabled");
1460 goto fail;
1461 }
1462 } else {
1463 cursor = uncompress_sa(iphc, cursor, ipv6, pkt);
1464 }
1465
1466 /* Uncompress Destination Address */
1467 if (iphc & NET_6LO_IPHC_M_1) {
1468 if (iphc & NET_6LO_IPHC_DAC_1) {
1469 /* TODO: DAM00 Unicast-Prefix-based IPv6 Multicast
1470 * Addresses. DAM_01, DAM_10 and DAM_11 are reserved.
1471 */
1472 NET_ERR("DAC_1 and M_1 is not supported");
1473 goto fail;
1474 } else {
1475 cursor = uncompress_da_mcast(iphc, cursor, ipv6);
1476 }
1477 } else {
1478 if (iphc & NET_6LO_IPHC_DAC_1) {
1479 #if defined(CONFIG_NET_6LO_CONTEXT)
1480 if (!dst) {
1481 NET_ERR("Dst context doesn't exists");
1482 goto fail;
1483 }
1484
1485 cursor = uncompress_da_ctx(iphc, cursor, ipv6, dst, pkt);
1486 #else
1487 NET_ERR("Context based uncompression not enabled");
1488 goto fail;
1489 #endif
1490 } else {
1491 cursor = uncompress_da(iphc, cursor, ipv6, pkt);
1492 }
1493 }
1494
1495 if (iphc & NET_6LO_IPHC_NH_MASK) {
1496 ipv6->nexthdr = IPPROTO_UDP;
1497 udp = (struct net_udp_hdr *)(frag->data + NET_IPV6H_LEN);
1498 /* skip nhc */
1499 cursor++;
1500 cursor = uncompress_nh_udp(nhc, cursor, udp);
1501 }
1502
1503 if (frag != pkt->buffer) {
1504 /* Insert the fragment (this one holds uncompressed headers) */
1505 net_pkt_frag_insert(pkt, frag);
1506 }
1507
1508 /* Set IPv6 header and UDP (if next header is) length */
1509 len = net_pkt_get_len(pkt) - NET_IPV6H_LEN;
1510 ipv6->len = htons(len);
1511
1512 if (ipv6->nexthdr == IPPROTO_UDP && udp) {
1513 udp->len = htons(len);
1514
1515 if (nhc & NET_6LO_NHC_UDP_CHECKSUM) {
1516 udp->chksum = net_calc_chksum_udp(pkt);
1517 }
1518 }
1519
1520 net_pkt_cursor_init(pkt);
1521
1522 return true;
1523
1524 fail:
1525 if (frag != pkt->buffer) {
1526 net_pkt_frag_unref(frag);
1527 }
1528
1529 return false;
1530 }
1531
1532 /* Adds IPv6 dispatch as first byte and adjust fragments */
compress_ipv6_header(struct net_pkt * pkt)1533 static inline int compress_ipv6_header(struct net_pkt *pkt)
1534 {
1535 struct net_buf *buffer = pkt->buffer;
1536
1537 if (net_buf_tailroom(buffer) >= 1U) {
1538 memmove(buffer->data + 1U, buffer->data, buffer->len);
1539 net_buf_add(buffer, 1U);
1540 buffer->data[0] = NET_6LO_DISPATCH_IPV6;
1541 return 0;
1542 }
1543
1544 buffer = net_pkt_get_frag(pkt, 1, K_FOREVER);
1545 if (!buffer) {
1546 return -ENOBUFS;
1547 }
1548
1549 buffer->data[0] = NET_6LO_DISPATCH_IPV6;
1550 net_buf_add(buffer, 1);
1551
1552 net_pkt_frag_insert(pkt, buffer);
1553
1554 /* Compact the fragments, so that gaps will be filled */
1555 net_pkt_compact(pkt);
1556
1557 return 0;
1558 }
1559
uncompress_ipv6_header(struct net_pkt * pkt)1560 static inline bool uncompress_ipv6_header(struct net_pkt *pkt)
1561 {
1562 /* Pull off IPv6 dispatch header and adjust data and length */
1563 net_buf_pull(pkt->buffer, 1U);
1564 net_pkt_cursor_init(pkt);
1565
1566 return true;
1567 }
1568
net_6lo_compress(struct net_pkt * pkt,bool iphc)1569 int net_6lo_compress(struct net_pkt *pkt, bool iphc)
1570 {
1571 if (iphc) {
1572 return compress_IPHC_header(pkt);
1573 } else {
1574 return compress_ipv6_header(pkt);
1575 }
1576 }
1577
net_6lo_uncompress(struct net_pkt * pkt)1578 bool net_6lo_uncompress(struct net_pkt *pkt)
1579 {
1580 NET_ASSERT(pkt && pkt->frags);
1581
1582 if ((pkt->frags->data[0] & NET_6LO_DISPATCH_IPHC_MASK) ==
1583 NET_6LO_DISPATCH_IPHC) {
1584 /* Uncompress IPHC header */
1585 return uncompress_IPHC_header(pkt);
1586
1587 } else if (pkt->frags->data[0] == NET_6LO_DISPATCH_IPV6) {
1588 /* Uncompress IPv6 header, it has only IPv6 dispatch in the
1589 * beginning */
1590 return uncompress_ipv6_header(pkt);
1591 }
1592
1593 NET_DBG("pkt %p is not compressed", pkt);
1594
1595 return true;
1596 }
1597
net_6lo_uncompress_hdr_diff(struct net_pkt * pkt)1598 int net_6lo_uncompress_hdr_diff(struct net_pkt *pkt)
1599 {
1600 int inline_size, compressed_hdr_size, nhc_inline_size, diff;
1601 uint16_t iphc;
1602 uint8_t nhc;
1603
1604 if (pkt->frags->data[0] == NET_6LO_DISPATCH_IPV6) {
1605 return -1;
1606 }
1607
1608 if ((pkt->frags->data[0] & NET_6LO_DISPATCH_IPHC_MASK) !=
1609 NET_6LO_DISPATCH_IPHC) {
1610 return 0;
1611 }
1612
1613 iphc = ntohs(UNALIGNED_GET((uint16_t *)pkt->buffer->data));
1614
1615 inline_size = get_ihpc_inlined_size(iphc);
1616 if (inline_size < 0) {
1617 return INT_MAX;
1618 }
1619
1620 compressed_hdr_size = sizeof(iphc) + inline_size;
1621 diff = sizeof(struct net_ipv6_hdr) - compressed_hdr_size;
1622
1623 if (iphc & NET_6LO_IPHC_NH_MASK) {
1624 nhc = *(pkt->buffer->data + sizeof(iphc) + inline_size);
1625 if ((nhc & 0xF8) != NET_6LO_NHC_UDP_BARE) {
1626 NET_ERR("Unsupported next header");
1627 return INT_MAX;
1628 }
1629
1630 nhc_inline_size = get_udp_nhc_inlined_size(nhc);
1631 diff += sizeof(struct net_udp_hdr) - sizeof(uint8_t) -
1632 nhc_inline_size;
1633 }
1634
1635 return diff;
1636 }
1637
