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 /* Address is fully elided */
369 if (net_ipv6_addr_based_on_ll((struct in6_addr *)ipv6->src,
370 net_pkt_lladdr_src(pkt))) {
371 NET_DBG("SAM_11 src address is fully elided");
372
373 *iphc |= NET_6LO_IPHC_SAM_11;
374 return inline_ptr;
375 }
376
377 /* Following 64 bits are 0000:00ff:fe00:XXXX */
378 if (net_6lo_addr_16_bit_compressible((struct in6_addr *)ipv6->src)) {
379 NET_DBG("SAM_10 src addr 16 bit compressible");
380 *iphc |= NET_6LO_IPHC_SAM_10;
381
382 inline_ptr -= sizeof(uint16_t);
383 memmove(inline_ptr, &ipv6->src[14], sizeof(uint16_t));
384
385 return inline_ptr;
386 }
387
388 NET_DBG("SAM_01 src 64 bits are inlined");
389 /* Remaining 64 bits are in-line */
390 *iphc |= NET_6LO_IPHC_SAM_01;
391
392 inline_ptr -= 8U;
393 memmove(inline_ptr, &ipv6->src[8], 8U);
394
395 return inline_ptr;
396 }
397
set_sa_inline(struct net_ipv6_hdr * ipv6,uint8_t * inline_ptr,uint16_t * iphc)398 static uint8_t *set_sa_inline(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
399 uint16_t *iphc)
400 {
401 *iphc |= NET_6LO_IPHC_SAM_00;
402 inline_ptr -= 16U;
403 memmove(inline_ptr, &ipv6->src[0], 16U);
404 return inline_ptr;
405 }
406
407 #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)408 static uint8_t *compress_sa_ctx(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
409 struct net_pkt *pkt, uint16_t *iphc,
410 struct net_6lo_context *src)
411 {
412 NET_DBG("SAC_1 src address context based");
413 *iphc |= NET_6LO_IPHC_SAC_1;
414
415 if (net_ipv6_addr_based_on_ll((struct in6_addr *)ipv6->src,
416 net_pkt_lladdr_src(pkt))) {
417 NET_DBG("SAM_11 src address is fully elided");
418
419 /* Address is fully elided */
420 *iphc |= NET_6LO_IPHC_SAM_11;
421 return inline_ptr;
422 }
423
424 /* Following 64 bits are 0000:00ff:fe00:XXXX */
425 if (net_6lo_addr_16_bit_compressible((struct in6_addr *)ipv6->src)) {
426 NET_DBG("SAM_10 src addr 16 bit compressible");
427
428 *iphc |= NET_6LO_IPHC_SAM_10;
429
430 inline_ptr -= sizeof(uint16_t);
431 memmove(inline_ptr, &ipv6->src[14], sizeof(uint16_t));
432 return inline_ptr;
433 }
434
435 NET_DBG("SAM_01 src remaining 64 bits are inlined");
436
437 /* Remaining 64 bits are in-line */
438 *iphc |= NET_6LO_IPHC_SAM_01;
439
440 inline_ptr -= 8U;
441 memmove(inline_ptr, &ipv6->src[8], 8U);
442
443 return inline_ptr;
444 }
445 #endif
446
447 /* Helpers to compress Destination Address */
compress_da_mcast(struct net_ipv6_hdr * ipv6,uint8_t * inline_ptr,uint16_t * iphc)448 static uint8_t *compress_da_mcast(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
449 uint16_t *iphc)
450 {
451 *iphc |= NET_6LO_IPHC_M_1;
452
453 NET_DBG("M_1 dst is mcast");
454
455 if (net_6lo_maddr_8_bit_compressible((struct in6_addr *)ipv6->dst)) {
456 NET_DBG("DAM_11 dst maddr 8 bit compressible");
457
458 /* last byte */
459 *iphc |= NET_6LO_IPHC_DAM_11;
460
461 inline_ptr -= sizeof(uint8_t);
462 memmove(inline_ptr, &ipv6->dst[15], sizeof(uint8_t));
463
464 return inline_ptr;
465 }
466
467 if (net_6lo_maddr_32_bit_compressible((struct in6_addr *)ipv6->dst)) {
468 NET_DBG("DAM_10 4 bytes: 2nd byte + last three bytes");
469
470 /* 4 bytes: 2nd byte + last three bytes */
471 *iphc |= NET_6LO_IPHC_DAM_10;
472
473 inline_ptr -= 3U;
474 memmove(inline_ptr, &ipv6->dst[13], 3U);
475
476 inline_ptr -= sizeof(uint8_t);
477 memmove(inline_ptr, &ipv6->dst[1], sizeof(uint8_t));
478
479 return inline_ptr;
480 }
481
482 if (net_6lo_maddr_48_bit_compressible((struct in6_addr *)ipv6->dst)) {
483 NET_DBG("DAM_01 6 bytes: 2nd byte + last five bytes");
484
485 /* 6 bytes: 2nd byte + last five bytes */
486 *iphc |= NET_6LO_IPHC_DAM_01;
487
488 inline_ptr -= 5U;
489 memmove(inline_ptr, &ipv6->dst[11], 5U);
490
491 inline_ptr -= sizeof(uint8_t);
492 memmove(inline_ptr, &ipv6->dst[1], sizeof(uint8_t));
493
494 return inline_ptr;
495 }
496
497 NET_DBG("DAM_00 dst complete addr inlined");
498
499 /* complete address NET_6LO_IPHC_DAM_00 */
500 inline_ptr -= 16U;
501 memmove(inline_ptr, &ipv6->dst[0], 16U);
502
503 return inline_ptr;
504 }
505
compress_da(struct net_ipv6_hdr * ipv6,struct net_pkt * pkt,uint8_t * inline_ptr,uint16_t * iphc)506 static uint8_t *compress_da(struct net_ipv6_hdr *ipv6, struct net_pkt *pkt,
507 uint8_t *inline_ptr, uint16_t *iphc)
508 {
509 /* Address is fully elided */
510 if (net_ipv6_addr_based_on_ll((struct in6_addr *)ipv6->dst,
511 net_pkt_lladdr_dst(pkt))) {
512 NET_DBG("DAM_11 dst addr fully elided");
513
514 *iphc |= NET_6LO_IPHC_DAM_11;
515 return inline_ptr;
516 }
517
518 /* Following 64 bits are 0000:00ff:fe00:XXXX */
519 if (net_6lo_addr_16_bit_compressible((struct in6_addr *)ipv6->dst)) {
520 NET_DBG("DAM_10 dst addr 16 bit compressible");
521
522 *iphc |= NET_6LO_IPHC_DAM_10;
523
524 inline_ptr -= sizeof(uint16_t);
525 memmove(inline_ptr, &ipv6->dst[14], sizeof(uint16_t));
526 return inline_ptr;
527 }
528
529 NET_DBG("DAM_01 remaining 64 bits are inlined");
530
531 /* Remaining 64 bits are in-line */
532 *iphc |= NET_6LO_IPHC_DAM_01;
533
534 inline_ptr -= 8U;
535 memmove(inline_ptr, &ipv6->dst[8], 8U);
536
537 return inline_ptr;
538 }
539
set_da_inline(struct net_ipv6_hdr * ipv6,uint8_t * inline_ptr,uint16_t * iphc)540 static uint8_t *set_da_inline(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
541 uint16_t *iphc)
542 {
543 *iphc |= NET_6LO_IPHC_DAM_00;
544 inline_ptr -= 16U;
545 memmove(inline_ptr, &ipv6->dst[0], 16U);
546 return inline_ptr;
547 }
548
549 #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)550 static uint8_t *compress_da_ctx(struct net_ipv6_hdr *ipv6, uint8_t *inline_ptr,
551 struct net_pkt *pkt, uint16_t *iphc,
552 struct net_6lo_context *dst)
553 {
554 *iphc |= NET_6LO_IPHC_DAC_1;
555
556 if (net_ipv6_addr_based_on_ll((struct in6_addr *)ipv6->dst,
557 net_pkt_lladdr_dst(pkt))) {
558 NET_DBG("DAM_11 dst addr fully elided");
559
560 *iphc |= NET_6LO_IPHC_DAM_11;
561 return inline_ptr;
562 }
563
564 /* Following 64 bits are 0000:00ff:fe00:XXXX */
565 if (net_6lo_addr_16_bit_compressible((struct in6_addr *)ipv6->dst)) {
566 NET_DBG("DAM_10 dst addr 16 bit compressible");
567
568 *iphc |= NET_6LO_IPHC_DAM_10;
569 inline_ptr -= sizeof(uint16_t);
570 memmove(inline_ptr, &ipv6->dst[14], sizeof(uint16_t));
571 return inline_ptr;
572 }
573
574 NET_DBG("DAM_01 remaining 64 bits are inlined");
575
576 /* Remaining 64 bits are in-line */
577 *iphc |= NET_6LO_IPHC_DAM_01;
578
579 inline_ptr -= 8U;
580 memmove(inline_ptr, &ipv6->dst[8], 8U);
581
582 return inline_ptr;
583 }
584 #endif
585
586 /* Helper to compress Next header UDP */
compress_nh_udp(struct net_udp_hdr * udp,uint8_t * inline_ptr,bool compress_checksum)587 static inline uint8_t *compress_nh_udp(struct net_udp_hdr *udp, uint8_t *inline_ptr,
588 bool compress_checksum)
589 {
590 uint8_t nhc = NET_6LO_NHC_UDP_BARE;
591 uint8_t *inline_ptr_udp = inline_ptr;
592 uint8_t tmp;
593
594 /* 4.3.3 UDP LOWPAN_NHC Format
595 * 0 1 2 3 4 5 6 7
596 * +---+---+---+---+---+---+---+---+
597 * | 1 | 1 | 1 | 1 | 0 | C | P |
598 * +---+---+---+---+---+---+---+---+
599 */
600
601 /* Port compression
602 * 00: All 16 bits for src and dst are inlined.
603 * 01: All 16 bits for src port inlined. First 8 bits of dst port is
604 * 0xf0 and elided. The remaining 8 bits of dst port inlined.
605 * 10: First 8 bits of src port 0xf0 and elided. The remaining 8 bits
606 * of src port inlined. All 16 bits of dst port inlined.
607 * 11: First 12 bits of both src and dst are 0xf0b and elided. The
608 * remaining 4 bits for each are inlined.
609 */
610
611 if (compress_checksum) {
612 nhc |= NET_6LO_NHC_UDP_CHECKSUM;
613 } else {
614 inline_ptr_udp -= sizeof(udp->chksum);
615 memmove(inline_ptr_udp, &udp->chksum, sizeof(udp->chksum));
616 }
617
618 if ((((htons(udp->src_port) >> 4) & 0xFFF) ==
619 NET_6LO_NHC_UDP_4_BIT_PORT) &&
620 (((htons(udp->dst_port) >> 4) & 0xFFF) ==
621 NET_6LO_NHC_UDP_4_BIT_PORT)) {
622
623 NET_DBG("UDP ports src and dst 4 bits inlined");
624 /** src: first 16 bits elided, next 4 bits inlined
625 * dst: first 16 bits elided, next 4 bits inlined
626 */
627 nhc |= NET_6LO_NHC_UDP_PORT_11;
628
629 tmp = (uint8_t)(htons(udp->src_port));
630 tmp = tmp << 4;
631
632 tmp |= (((uint8_t)(htons(udp->dst_port))) & 0x0F);
633 inline_ptr_udp -= sizeof(tmp);
634 *inline_ptr_udp = tmp;
635
636 } else if (((htons(udp->dst_port) >> 8) & 0xFF) ==
637 NET_6LO_NHC_UDP_8_BIT_PORT) {
638
639 NET_DBG("UDP ports src full, dst 8 bits inlined");
640 /* dst: first 8 bits elided, next 8 bits inlined
641 * src: fully carried inline
642 */
643 nhc |= NET_6LO_NHC_UDP_PORT_01;
644
645 inline_ptr_udp -= sizeof(uint8_t);
646 *inline_ptr_udp = (uint8_t)(htons(udp->dst_port));
647
648 inline_ptr_udp -= sizeof(udp->src_port);
649 memmove(inline_ptr_udp, &udp->src_port, sizeof(udp->src_port));
650
651 } else if (((htons(udp->src_port) >> 8) & 0xFF) ==
652 NET_6LO_NHC_UDP_8_BIT_PORT) {
653
654 NET_DBG("UDP ports src 8bits, dst full inlined");
655 /* src: first 8 bits elided, next 8 bits inlined
656 * dst: fully carried inline
657 */
658 nhc |= NET_6LO_NHC_UDP_PORT_10;
659
660 inline_ptr_udp -= sizeof(udp->dst_port);
661 memmove(inline_ptr_udp, &udp->dst_port, sizeof(udp->dst_port));
662
663 inline_ptr_udp -= sizeof(uint8_t);
664 *inline_ptr_udp = (uint8_t)(htons(udp->src_port));
665
666 } else {
667 NET_DBG("Can not compress ports, ports are inlined");
668
669 /* can not compress ports, ports are inlined */
670 inline_ptr_udp -= sizeof(udp->dst_port) + sizeof(udp->src_port);
671 memmove(inline_ptr_udp, &udp->src_port,
672 sizeof(udp->dst_port) + sizeof(udp->src_port));
673 }
674
675 inline_ptr_udp -= sizeof(nhc);
676 *inline_ptr_udp = nhc;
677
678 return inline_ptr_udp;
679 }
680
681 #if defined(CONFIG_NET_6LO_CONTEXT)
682
get_src_addr_ctx(struct net_pkt * pkt,struct net_ipv6_hdr * ipv6)683 static struct net_6lo_context *get_src_addr_ctx(struct net_pkt *pkt,
684 struct net_ipv6_hdr *ipv6)
685 {
686 /* If compress flag is unset means use only in uncompression. */
687 struct net_6lo_context *src;
688
689 src = get_6lo_context_by_addr(net_pkt_iface(pkt),
690 (struct in6_addr *)ipv6->src);
691 if (!src || !src->compress) {
692 return NULL;
693 }
694
695 return src;
696 }
697
get_dst_addr_ctx(struct net_pkt * pkt,struct net_ipv6_hdr * ipv6)698 static struct net_6lo_context *get_dst_addr_ctx(struct net_pkt *pkt,
699 struct net_ipv6_hdr *ipv6)
700 {
701 /* If compress flag is unset means use only in uncompression. */
702 struct net_6lo_context *dst;
703
704 dst = get_6lo_context_by_addr(net_pkt_iface(pkt),
705 (struct in6_addr *)ipv6->dst);
706 if (!dst || !dst->compress) {
707 return NULL;
708 }
709
710 return dst;
711 }
712 #endif /* CONFIG_NET_6LO_CONTEXT */
713
714 /* RFC 6282 LOWPAN IPHC Encoding format (3.1)
715 * Base Format
716 * 0 1
717 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5
718 * +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
719 * | 0 | 1 | 1 | TF |NH | HLIM |CID|SAC| SAM | M |DAC| DAM |
720 * +---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+---+
721 */
compress_IPHC_header(struct net_pkt * pkt)722 static inline int compress_IPHC_header(struct net_pkt *pkt)
723 {
724 #if defined(CONFIG_NET_6LO_CONTEXT)
725 struct net_6lo_context *src_ctx = NULL;
726 struct net_6lo_context *dst_ctx = NULL;
727 #endif
728 uint8_t compressed = 0;
729 uint16_t iphc = (NET_6LO_DISPATCH_IPHC << 8);
730 struct net_ipv6_hdr *ipv6 = NET_IPV6_HDR(pkt);
731 struct net_udp_hdr *udp;
732 uint8_t *inline_pos;
733
734 if (pkt->frags->len < NET_IPV6H_LEN) {
735 NET_ERR("Invalid length %d, min %d",
736 pkt->frags->len, NET_IPV6H_LEN);
737 return -EINVAL;
738 }
739
740 if (ipv6->nexthdr == IPPROTO_UDP &&
741 pkt->frags->len < NET_IPV6UDPH_LEN) {
742 NET_ERR("Invalid length %d, min %d",
743 pkt->frags->len, NET_IPV6UDPH_LEN);
744 return -EINVAL;
745 }
746
747 inline_pos = pkt->buffer->data + NET_IPV6H_LEN;
748
749 if (ipv6->nexthdr == IPPROTO_UDP) {
750 udp = (struct net_udp_hdr *)inline_pos;
751 inline_pos += NET_UDPH_LEN;
752
753 inline_pos = compress_nh_udp(udp, inline_pos, false);
754 }
755
756 if (net_6lo_ll_prefix_padded_with_zeros((struct in6_addr *)ipv6->dst)) {
757 inline_pos = compress_da(ipv6, pkt, inline_pos, &iphc);
758 goto da_end;
759 }
760
761 if (net_ipv6_is_addr_mcast((struct in6_addr *)ipv6->dst)) {
762 inline_pos = compress_da_mcast(ipv6, inline_pos, &iphc);
763 goto da_end;
764 }
765
766 #if defined(CONFIG_NET_6LO_CONTEXT)
767 dst_ctx = get_dst_addr_ctx(pkt, ipv6);
768 if (dst_ctx) {
769 iphc |= NET_6LO_IPHC_CID_1;
770 inline_pos = compress_da_ctx(ipv6, inline_pos, pkt, &iphc,
771 dst_ctx);
772 goto da_end;
773 }
774 #endif
775 inline_pos = set_da_inline(ipv6, inline_pos, &iphc);
776 da_end:
777
778 if (net_6lo_ll_prefix_padded_with_zeros((struct in6_addr *)ipv6->src)) {
779 inline_pos = compress_sa(ipv6, pkt, inline_pos, &iphc);
780 goto sa_end;
781 }
782
783 if (net_ipv6_is_addr_unspecified((struct in6_addr *)ipv6->src)) {
784 NET_DBG("SAM_00, SAC_1 unspecified src address");
785
786 /* Unspecified IPv6 src address */
787 iphc |= NET_6LO_IPHC_SAC_1;
788 iphc |= NET_6LO_IPHC_SAM_00;
789 goto sa_end;
790 }
791
792 #if defined(CONFIG_NET_6LO_CONTEXT)
793 src_ctx = get_src_addr_ctx(pkt, ipv6);
794 if (src_ctx) {
795 inline_pos = compress_sa_ctx(ipv6, inline_pos, pkt, &iphc,
796 src_ctx);
797 iphc |= NET_6LO_IPHC_CID_1;
798 goto sa_end;
799 }
800 #endif
801 inline_pos = set_sa_inline(ipv6, inline_pos, &iphc);
802 sa_end:
803
804 inline_pos = compress_hoplimit(ipv6, inline_pos, &iphc);
805 inline_pos = compress_nh(ipv6, inline_pos, &iphc);
806 inline_pos = compress_tfl(ipv6, inline_pos, &iphc);
807
808 #if defined(CONFIG_NET_6LO_CONTEXT)
809 if (iphc & NET_6LO_IPHC_CID_1) {
810 inline_pos -= sizeof(uint8_t);
811 *inline_pos = 0;
812
813 if (src_ctx) {
814 *inline_pos = src_ctx->cid << 4;
815 }
816
817 if (dst_ctx) {
818 *inline_pos |= dst_ctx->cid & 0x0F;
819 }
820 }
821 #endif
822
823 inline_pos -= sizeof(iphc);
824 iphc = htons(iphc);
825 memmove(inline_pos, &iphc, sizeof(iphc));
826
827 compressed = inline_pos - pkt->buffer->data;
828
829 net_pkt_cursor_init(pkt);
830 net_pkt_pull(pkt, compressed);
831 net_pkt_compact(pkt);
832
833 return compressed;
834 }
835
836 /* Helper to uncompress Traffic class and Flow label */
uncompress_tfl(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6)837 static inline uint8_t *uncompress_tfl(uint16_t iphc, uint8_t *cursor,
838 struct net_ipv6_hdr *ipv6)
839 {
840 uint8_t tcl;
841
842 /* Uncompress tcl and flow label */
843 switch (iphc & NET_6LO_IPHC_TF_11) {
844 case NET_6LO_IPHC_TF_00:
845 NET_DBG("ECN + DSCP + 4-bit Pad + Flow Label");
846
847 tcl = *cursor;
848 cursor++;
849 tcl = (tcl >> 6) | (tcl << 2);
850
851 ipv6->vtc |= ((tcl & 0xF0) >> 4);
852 ipv6->tcflow = ((tcl & 0x0F) << 4) | (*cursor & 0x0F);
853 cursor++;
854
855 memmove(&ipv6->flow, cursor, sizeof(ipv6->flow));
856 cursor += sizeof(ipv6->flow);
857 break;
858 case NET_6LO_IPHC_TF_01:
859 NET_DBG("ECN + 2-bit Pad + Flow Label, DSCP is elided");
860
861 tcl = ((*cursor & 0xF0) >> 6);
862 ipv6->tcflow = ((tcl & 0x0F) << 4) | (*cursor & 0x0F);
863 cursor++;
864
865 memmove(&ipv6->flow, cursor, sizeof(ipv6->flow));
866 cursor += sizeof(ipv6->flow);
867
868 break;
869 case NET_6LO_IPHC_TF_10:
870 NET_DBG("Flow label elided");
871
872 tcl = *cursor;
873 cursor++;
874 tcl = (tcl >> 6) | (tcl << 2);
875
876 ipv6->vtc |= ((tcl & 0xF0) >> 4);
877 ipv6->tcflow = (tcl & 0x0F) << 4;
878 ipv6->flow = 0U;
879
880 break;
881 case NET_6LO_IPHC_TF_11:
882 NET_DBG("Tcl and Flow label elided");
883
884 ipv6->tcflow = 0U;
885 ipv6->flow = 0U;
886
887 break;
888 }
889
890 return cursor;
891 }
892
893 /* Helper to uncompress Hoplimit */
uncompress_hoplimit(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6)894 static inline uint8_t *uncompress_hoplimit(uint16_t iphc, uint8_t *cursor,
895 struct net_ipv6_hdr *ipv6)
896 {
897 switch (iphc & NET_6LO_IPHC_HLIM_MASK) {
898 case NET_6LO_IPHC_HLIM:
899 ipv6->hop_limit = *cursor;
900 cursor++;
901
902 break;
903 case NET_6LO_IPHC_HLIM1:
904 ipv6->hop_limit = 1U;
905
906 break;
907 case NET_6LO_IPHC_HLIM64:
908 ipv6->hop_limit = 64U;
909
910 break;
911 case NET_6LO_IPHC_HLIM255:
912 ipv6->hop_limit = 255U;
913
914 break;
915 }
916
917 return cursor;
918 }
919
920 /* Helper to uncompress Source Address */
uncompress_sa(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6,struct net_pkt * pkt)921 static inline uint8_t *uncompress_sa(uint16_t iphc, uint8_t *cursor,
922 struct net_ipv6_hdr *ipv6,
923 struct net_pkt *pkt)
924 {
925 struct in6_addr src_ip;
926
927 NET_DBG("SAC_0");
928
929 net_ipv6_addr_copy_raw((uint8_t *)&src_ip, ipv6->src);
930
931 switch (iphc & NET_6LO_IPHC_SAM_MASK) {
932 case NET_6LO_IPHC_SAM_00:
933 NET_DBG("SAM_00 full src addr inlined");
934
935 memmove(src_ip.s6_addr, cursor, sizeof(src_ip.s6_addr));
936 cursor += sizeof(src_ip.s6_addr);
937
938 break;
939 case NET_6LO_IPHC_SAM_01:
940 NET_DBG("SAM_01 last 64 bits are inlined");
941
942 memmove(&src_ip.s6_addr[8], cursor, 8);
943 cursor += 8U;
944
945 src_ip.s6_addr32[0] = 0x00;
946 src_ip.s6_addr32[1] = 0x00;
947 src_ip.s6_addr[0] = 0xFE;
948 src_ip.s6_addr[1] = 0x80;
949
950 break;
951 case NET_6LO_IPHC_SAM_10:
952 NET_DBG("SAM_10 src addr 16 bit compressed");
953
954 memmove(&src_ip.s6_addr[14], cursor, 2);
955 cursor += 2U;
956 src_ip.s6_addr16[6] = 0x00;
957
958 src_ip.s6_addr32[0] = 0x00;
959 src_ip.s6_addr32[1] = 0x00;
960 src_ip.s6_addr32[2] = 0x00;
961 src_ip.s6_addr[0] = 0xFE;
962 src_ip.s6_addr[1] = 0x80;
963 src_ip.s6_addr[11] = 0xFF;
964 src_ip.s6_addr[12] = 0xFE;
965
966 break;
967 case NET_6LO_IPHC_SAM_11:
968 NET_DBG("SAM_11 generate src addr from ll");
969
970 net_ipv6_addr_create_iid(&src_ip, net_pkt_lladdr_src(pkt));
971
972 break;
973 }
974
975 net_ipv6_addr_copy_raw(ipv6->src, (uint8_t *)&src_ip);
976
977 return cursor;
978 }
979
980 #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)981 static inline uint8_t *uncompress_sa_ctx(uint16_t iphc, uint8_t *cursor,
982 struct net_ipv6_hdr *ipv6,
983 struct net_6lo_context *ctx,
984 struct net_pkt *pkt)
985 {
986 struct in6_addr src_ip;
987
988 net_ipv6_addr_copy_raw((uint8_t *)&src_ip, ipv6->src);
989
990 switch (iphc & NET_6LO_IPHC_SAM_MASK) {
991 case NET_6LO_IPHC_SAM_01:
992 NET_DBG("SAM_01 last 64 bits are inlined");
993
994 /* First 8 bytes are from context */
995 memmove(&src_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
996
997 /* And the rest are carried in-line*/
998 memmove(&src_ip.s6_addr[8], cursor, 8);
999 cursor += 8U;
1000
1001 break;
1002 case NET_6LO_IPHC_SAM_10:
1003 NET_DBG("SAM_10 src addr 16 bit compressed");
1004
1005 /* 16 bit carried in-line */
1006 memmove(&src_ip.s6_addr[14], cursor, 2);
1007 cursor += 2U;
1008
1009 /* First 8 bytes are from context */
1010 memmove(&src_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
1011
1012 src_ip.s6_addr32[2] = 0x00;
1013 src_ip.s6_addr16[6] = 0x00;
1014 src_ip.s6_addr[11] = 0xFF;
1015 src_ip.s6_addr[12] = 0xFE;
1016
1017 break;
1018 case NET_6LO_IPHC_SAM_11:
1019 NET_DBG("SAM_11 generate src addr from ll");
1020
1021 /* RFC 6282, 3.1.1. If SAC = 1 and SAM = 11
1022 * Derive addr using context information and
1023 * the encapsulating header.
1024 * (e.g., 802.15.4 or IPv6 source address).
1025 */
1026 net_ipv6_addr_create_iid(&src_ip, net_pkt_lladdr_src(pkt));
1027
1028 /* net_ipv6_addr_create_iid will copy first 8 bytes
1029 * as link local prefix.
1030 * Overwrite first 8 bytes from context prefix here.
1031 */
1032 memmove(&src_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
1033 break;
1034 }
1035
1036 net_ipv6_addr_copy_raw(ipv6->src, (uint8_t *)&src_ip);
1037
1038 return cursor;
1039 }
1040 #endif
1041
1042 /* Helpers to uncompress Destination Address */
uncompress_da_mcast(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6)1043 static inline uint8_t *uncompress_da_mcast(uint16_t iphc, uint8_t *cursor,
1044 struct net_ipv6_hdr *ipv6)
1045 {
1046 struct in6_addr dst_ip;
1047
1048 NET_DBG("Dst is multicast");
1049
1050 net_ipv6_addr_copy_raw((uint8_t *)&dst_ip, ipv6->dst);
1051
1052 if (iphc & NET_6LO_IPHC_DAC_1) {
1053 NET_WARN("Unsupported DAM options");
1054 return 0;
1055 }
1056
1057 /* If M=1 and DAC=0:
1058 * 00: 128 bits, The full address is carried in-line.
1059 * 01: 48 bits, The address takes the form ffXX::00XX:XXXX:XXXX.
1060 * 10: 32 bits, The address takes the form ffXX::00XX:XXXX.
1061 * 11: 8 bits, The address takes the form ff02::00XX.
1062 */
1063
1064 switch (iphc & NET_6LO_IPHC_DAM_MASK) {
1065 case NET_6LO_IPHC_DAM_00:
1066 NET_DBG("DAM_00 full dst addr inlined");
1067
1068 memmove(&dst_ip.s6_addr[0], cursor,
1069 sizeof(dst_ip.s6_addr));
1070
1071 cursor += sizeof(dst_ip.s6_addr);
1072 break;
1073 case NET_6LO_IPHC_DAM_01:
1074 NET_DBG("DAM_01 2nd byte and last five bytes");
1075
1076 dst_ip.s6_addr[1] = *cursor;
1077 cursor++;
1078 memmove(&dst_ip.s6_addr[11], cursor, 5);
1079 cursor += 5U;
1080
1081
1082 dst_ip.s6_addr[0] = 0xFF;
1083 dst_ip.s6_addr16[1] = 0x00;
1084 dst_ip.s6_addr32[1] = 0x00;
1085 dst_ip.s6_addr[10] = 0x00;
1086 dst_ip.s6_addr16[4] = 0x00;
1087
1088 break;
1089 case NET_6LO_IPHC_DAM_10:
1090 NET_DBG("DAM_10 2nd byte and last three bytes");
1091
1092 dst_ip.s6_addr[1] = *cursor;
1093 cursor++;
1094 memmove(&dst_ip.s6_addr[13], cursor, 3);
1095 cursor += 3U;
1096
1097 dst_ip.s6_addr[0] = 0xFF;
1098 dst_ip.s6_addr16[1] = 0x00;
1099 dst_ip.s6_addr32[1] = 0x00;
1100 dst_ip.s6_addr32[2] = 0x00;
1101 dst_ip.s6_addr[12] = 0x00;
1102
1103 break;
1104 case NET_6LO_IPHC_DAM_11:
1105 NET_DBG("DAM_11 8 bit compressed");
1106
1107 dst_ip.s6_addr[15] = *cursor;
1108 cursor++;
1109 dst_ip.s6_addr[14] = 0x00;
1110
1111 dst_ip.s6_addr32[0] = 0x00;
1112 dst_ip.s6_addr32[1] = 0x00;
1113 dst_ip.s6_addr32[2] = 0x00;
1114 dst_ip.s6_addr16[6] = 0x00;
1115 dst_ip.s6_addr[0] = 0xFF;
1116 dst_ip.s6_addr[1] = 0x02;
1117
1118 break;
1119 }
1120
1121 net_ipv6_addr_copy_raw(ipv6->dst, (uint8_t *)&dst_ip);
1122
1123 return cursor;
1124 }
1125
1126 /* Helper to uncompress Destination Address */
uncompress_da(uint16_t iphc,uint8_t * cursor,struct net_ipv6_hdr * ipv6,struct net_pkt * pkt)1127 static inline uint8_t *uncompress_da(uint16_t iphc, uint8_t *cursor,
1128 struct net_ipv6_hdr *ipv6,
1129 struct net_pkt *pkt)
1130 {
1131 struct in6_addr dst_ip;
1132
1133 NET_DBG("DAC_0");
1134
1135 net_ipv6_addr_copy_raw((uint8_t *)&dst_ip, ipv6->dst);
1136
1137 switch (iphc & NET_6LO_IPHC_DAM_MASK) {
1138 case NET_6LO_IPHC_DAM_00:
1139 NET_DBG("DAM_00 full dst addr inlined");
1140
1141 memmove(&dst_ip.s6_addr[0], cursor,
1142 sizeof(dst_ip.s6_addr));
1143 cursor += sizeof(dst_ip.s6_addr);
1144
1145 break;
1146 case NET_6LO_IPHC_DAM_01:
1147 NET_DBG("DAM_01 last 64 bits are inlined");
1148
1149 memmove(&dst_ip.s6_addr[8], cursor, 8);
1150 cursor += 8U;
1151
1152 dst_ip.s6_addr32[0] = 0x00;
1153 dst_ip.s6_addr32[1] = 0x00;
1154 dst_ip.s6_addr[0] = 0xFE;
1155 dst_ip.s6_addr[1] = 0x80;
1156
1157 break;
1158 case NET_6LO_IPHC_DAM_10:
1159 NET_DBG("DAM_10 dst addr 16 bit compressed");
1160
1161 memmove(&dst_ip.s6_addr[14], cursor, 2);
1162 cursor += 2U;
1163
1164 dst_ip.s6_addr32[0] = 0x00;
1165 dst_ip.s6_addr32[1] = 0x00;
1166 dst_ip.s6_addr32[2] = 0x00;
1167 dst_ip.s6_addr16[6] = 0x00;
1168 dst_ip.s6_addr[0] = 0xFE;
1169 dst_ip.s6_addr[1] = 0x80;
1170 dst_ip.s6_addr[11] = 0xFF;
1171 dst_ip.s6_addr[12] = 0xFE;
1172
1173 break;
1174 case NET_6LO_IPHC_DAM_11:
1175 NET_DBG("DAM_11 generate dst addr from ll");
1176
1177 net_ipv6_addr_create_iid(&dst_ip, net_pkt_lladdr_dst(pkt));
1178
1179 break;
1180 }
1181
1182 net_ipv6_addr_copy_raw(ipv6->dst, (uint8_t *)&dst_ip);
1183
1184 return cursor;
1185 }
1186
1187 #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)1188 static inline uint8_t *uncompress_da_ctx(uint16_t iphc, uint8_t *cursor,
1189 struct net_ipv6_hdr *ipv6,
1190 struct net_6lo_context *ctx,
1191 struct net_pkt *pkt)
1192 {
1193 struct in6_addr dst_ip;
1194
1195 NET_DBG("DAC_1");
1196
1197 net_ipv6_addr_copy_raw((uint8_t *)&dst_ip, ipv6->dst);
1198
1199 switch (iphc & NET_6LO_IPHC_DAM_MASK) {
1200 case NET_6LO_IPHC_DAM_01:
1201 NET_DBG("DAM_01 last 64 bits are inlined");
1202
1203 /* Last 8 bytes carried in-line */
1204 memmove(&dst_ip.s6_addr[8], cursor, 8);
1205 cursor += 8U;
1206
1207 /* First 8 bytes are from context */
1208 memmove(&dst_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
1209
1210 break;
1211 case NET_6LO_IPHC_DAM_10:
1212 NET_DBG("DAM_10 src addr 16 bit compressed");
1213
1214 /* 16 bit carried in-line */
1215 memmove(&dst_ip.s6_addr[14], cursor, 2);
1216 cursor += 2U;
1217
1218 /* First 8 bytes are from context */
1219 memmove(&dst_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
1220
1221 dst_ip.s6_addr32[2] = 0x00;
1222 dst_ip.s6_addr16[6] = 0x00;
1223 dst_ip.s6_addr[11] = 0xFF;
1224 dst_ip.s6_addr[12] = 0xFE;
1225
1226 break;
1227 case NET_6LO_IPHC_DAM_11:
1228 NET_DBG("DAM_11 generate src addr from ll");
1229
1230 /* RFC 6282, 3.1.1. If SAC = 1 and SAM = 11
1231 * Derive addr using context information and
1232 * the encapsulating header.
1233 * (e.g., 802.15.4 or IPv6 source address).
1234 */
1235 net_ipv6_addr_create_iid(&dst_ip, net_pkt_lladdr_dst(pkt));
1236
1237 /* net_ipv6_addr_create_iid will copy first 8 bytes
1238 * as link local prefix.
1239 * Overwrite first 8 bytes from context prefix here.
1240 */
1241 memmove(&dst_ip.s6_addr[0], &ctx->prefix.s6_addr[0], 8);
1242
1243 break;
1244 }
1245
1246 net_ipv6_addr_copy_raw(ipv6->dst, (uint8_t *)&dst_ip);
1247
1248 return cursor;
1249 }
1250 #endif
1251
1252 /* Helper to uncompress NH UDP */
uncompress_nh_udp(uint8_t nhc,uint8_t * cursor,struct net_udp_hdr * udp)1253 static uint8_t *uncompress_nh_udp(uint8_t nhc, uint8_t *cursor,
1254 struct net_udp_hdr *udp)
1255 {
1256
1257 /* Port uncompression
1258 * 00: All 16 bits for src and dst are inlined
1259 * 01: src, 16 bits are lined, dst(0xf0) 8 bits are inlined
1260 * 10: dst, 16 bits are lined, src(0xf0) 8 bits are inlined
1261 * 11: src, dst (0xf0b) 4 bits are inlined
1262 */
1263
1264 /* UDP header uncompression */
1265 switch (nhc & NET_6LO_NHC_UDP_PORT_11) {
1266 case NET_6LO_NHC_UDP_PORT_00:
1267 NET_DBG("src and dst ports are inlined");
1268
1269 memmove(&udp->src_port, cursor, sizeof(udp->src_port));
1270 cursor += sizeof(udp->src_port);
1271 memmove(&udp->dst_port, cursor, sizeof(udp->dst_port));
1272 cursor += sizeof(udp->dst_port);
1273
1274 break;
1275 case NET_6LO_NHC_UDP_PORT_01:
1276 NET_DBG("src full, dst 8 bits inlined");
1277
1278 memmove(&udp->src_port, cursor, sizeof(udp->src_port));
1279 cursor += sizeof(udp->src_port);
1280 udp->dst_port = htons(((uint16_t)NET_6LO_NHC_UDP_8_BIT_PORT << 8) |
1281 *cursor);
1282 cursor++;
1283
1284 break;
1285 case NET_6LO_NHC_UDP_PORT_10:
1286 NET_DBG("src 8 bits, dst full inlined");
1287
1288 udp->src_port = htons(((uint16_t)NET_6LO_NHC_UDP_8_BIT_PORT << 8) |
1289 *cursor);
1290 cursor++;
1291 memmove(&udp->dst_port, cursor, sizeof(udp->dst_port));
1292 cursor += sizeof(udp->dst_port);
1293
1294 break;
1295 case NET_6LO_NHC_UDP_PORT_11:
1296 NET_DBG("src and dst 4 bits inlined");
1297
1298 udp->src_port = htons((NET_6LO_NHC_UDP_4_BIT_PORT << 4) |
1299 (*cursor >> 4));
1300
1301 udp->dst_port = htons((NET_6LO_NHC_UDP_4_BIT_PORT << 4) |
1302 (*cursor & 0x0F));
1303 cursor++;
1304
1305 break;
1306 }
1307
1308 if (!(nhc & NET_6LO_NHC_UDP_CHECKSUM)) {
1309 memmove(&udp->chksum, cursor, sizeof(udp->chksum));
1310 cursor += sizeof(udp->chksum);
1311 }
1312
1313 return cursor;
1314 }
1315
1316 #if defined(CONFIG_NET_6LO_CONTEXT)
1317 /* 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)1318 static inline void uncompress_cid(struct net_pkt *pkt, uint8_t cid,
1319 struct net_6lo_context **src,
1320 struct net_6lo_context **dst)
1321 {
1322 uint8_t cid_tmp;
1323
1324 /* Extract source and destination Context Index,
1325 * Either src or dest address is context based or both.
1326 */
1327 cid_tmp = (cid >> 4) & 0x0F;
1328 *src = get_6lo_context_by_cid(net_pkt_iface(pkt), cid_tmp);
1329 if (!(*src)) {
1330 NET_DBG("Unknown src cid %d", cid_tmp);
1331 }
1332
1333 cid_tmp = cid & 0x0F;
1334 *dst = get_6lo_context_by_cid(net_pkt_iface(pkt), cid_tmp);
1335 if (!(*dst)) {
1336 NET_DBG("Unknown dst cid %d", cid_tmp);
1337 }
1338 }
1339 #endif
1340
uncompress_IPHC_header(struct net_pkt * pkt)1341 static bool uncompress_IPHC_header(struct net_pkt *pkt)
1342 {
1343 struct net_udp_hdr *udp = NULL;
1344 struct net_buf *frag = NULL;
1345 uint8_t nhc = 0;
1346 int nhc_inline_size = 0;
1347 struct net_ipv6_hdr *ipv6;
1348 uint16_t len;
1349 uint16_t iphc;
1350 int inline_size, compressed_hdr_size;
1351 size_t diff;
1352 uint8_t *cursor;
1353 #if defined(CONFIG_NET_6LO_CONTEXT)
1354 struct net_6lo_context *src = NULL;
1355 struct net_6lo_context *dst = NULL;
1356 #endif
1357
1358 iphc = ntohs(UNALIGNED_GET((uint16_t *)pkt->buffer->data));
1359
1360 inline_size = get_ihpc_inlined_size(iphc);
1361 if (inline_size < 0) {
1362 return false;
1363 }
1364
1365 compressed_hdr_size = sizeof(iphc) + inline_size;
1366 diff = sizeof(struct net_ipv6_hdr) - compressed_hdr_size;
1367
1368 if (iphc & NET_6LO_IPHC_NH_MASK) {
1369 nhc = *(pkt->buffer->data + sizeof(iphc) + inline_size);
1370 if ((nhc & 0xF8) != NET_6LO_NHC_UDP_BARE) {
1371 NET_ERR("Unsupported next header");
1372 return false;
1373 }
1374
1375 nhc_inline_size = get_udp_nhc_inlined_size(nhc);
1376 compressed_hdr_size += sizeof(uint8_t) + nhc_inline_size;
1377 diff += sizeof(struct net_udp_hdr) - sizeof(uint8_t) -
1378 nhc_inline_size;
1379 }
1380
1381 if (pkt->buffer->len < compressed_hdr_size) {
1382 NET_ERR("Scattered compressed header?");
1383 return false;
1384 }
1385
1386 if (net_buf_tailroom(pkt->buffer) >= diff) {
1387 NET_DBG("Enough tailroom. Uncompress inplace");
1388 frag = pkt->buffer;
1389 net_buf_add(frag, diff);
1390 cursor = frag->data + diff;
1391 memmove(cursor, frag->data, frag->len - diff);
1392 } else {
1393 size_t frag_len = nhc ? NET_IPV6UDPH_LEN : NET_IPV6H_LEN;
1394
1395 NET_DBG("Not enough tailroom. Get new fragment");
1396 cursor = pkt->buffer->data;
1397 frag = net_pkt_get_frag(pkt, frag_len, NET_6LO_RX_PKT_TIMEOUT);
1398 if (!frag) {
1399 NET_ERR("Can't get frag for uncompression");
1400 return false;
1401 }
1402
1403 net_buf_pull(pkt->buffer, compressed_hdr_size);
1404 net_buf_add(frag, frag_len);
1405 }
1406
1407 ipv6 = (struct net_ipv6_hdr *)(frag->data);
1408 cursor += sizeof(iphc);
1409
1410 if (iphc & NET_6LO_IPHC_CID_1) {
1411 #if defined(CONFIG_NET_6LO_CONTEXT)
1412 uncompress_cid(pkt, *cursor, &src, &dst);
1413 cursor++;
1414 #else
1415 NET_ERR("Context based uncompression not enabled");
1416 return false;
1417 #endif
1418 }
1419
1420 /* Version is always 6 */
1421 ipv6->vtc = 0x60;
1422 net_pkt_set_ip_hdr_len(pkt, NET_IPV6H_LEN);
1423
1424 /* Uncompress Traffic class and Flow label */
1425 cursor = uncompress_tfl(iphc, cursor, ipv6);
1426
1427 if (!(iphc & NET_6LO_IPHC_NH_MASK)) {
1428 ipv6->nexthdr = *cursor;
1429 cursor++;
1430 }
1431
1432 /* Uncompress Hoplimit */
1433 cursor = uncompress_hoplimit(iphc, cursor, ipv6);
1434
1435 /* Uncompress Source Address */
1436 if (iphc & NET_6LO_IPHC_SAC_1) {
1437 NET_DBG("SAC_1");
1438
1439 if ((iphc & NET_6LO_IPHC_SAM_MASK) == NET_6LO_IPHC_SAM_00) {
1440 NET_DBG("SAM_00 unspecified address");
1441 memset(&ipv6->src[0], 0,
1442 sizeof(ipv6->src));
1443 } else if (IS_ENABLED(CONFIG_NET_6LO_CONTEXT)) {
1444 #if defined(CONFIG_NET_6LO_CONTEXT)
1445 if (!src) {
1446 NET_ERR("Src context doesn't exists");
1447 goto fail;
1448 }
1449
1450 cursor = uncompress_sa_ctx(iphc, cursor, ipv6, src, pkt);
1451 #endif
1452 } else {
1453 NET_ERR("Context based uncompression not enabled");
1454 goto fail;
1455 }
1456 } else {
1457 cursor = uncompress_sa(iphc, cursor, ipv6, pkt);
1458 }
1459
1460 /* Uncompress Destination Address */
1461 if (iphc & NET_6LO_IPHC_M_1) {
1462 if (iphc & NET_6LO_IPHC_DAC_1) {
1463 /* TODO: DAM00 Unicast-Prefix-based IPv6 Multicast
1464 * Addresses. DAM_01, DAM_10 and DAM_11 are reserved.
1465 */
1466 NET_ERR("DAC_1 and M_1 is not supported");
1467 goto fail;
1468 } else {
1469 cursor = uncompress_da_mcast(iphc, cursor, ipv6);
1470 }
1471 } else {
1472 if (iphc & NET_6LO_IPHC_DAC_1) {
1473 #if defined(CONFIG_NET_6LO_CONTEXT)
1474 if (!dst) {
1475 NET_ERR("Dst context doesn't exists");
1476 goto fail;
1477 }
1478
1479 cursor = uncompress_da_ctx(iphc, cursor, ipv6, dst, pkt);
1480 #else
1481 NET_ERR("Context based uncompression not enabled");
1482 goto fail;
1483 #endif
1484 } else {
1485 cursor = uncompress_da(iphc, cursor, ipv6, pkt);
1486 }
1487 }
1488
1489 if (iphc & NET_6LO_IPHC_NH_MASK) {
1490 ipv6->nexthdr = IPPROTO_UDP;
1491 udp = (struct net_udp_hdr *)(frag->data + NET_IPV6H_LEN);
1492 /* skip nhc */
1493 cursor++;
1494 cursor = uncompress_nh_udp(nhc, cursor, udp);
1495 }
1496
1497 if (frag != pkt->buffer) {
1498 /* Insert the fragment (this one holds uncompressed headers) */
1499 net_pkt_frag_insert(pkt, frag);
1500 }
1501
1502 /* Set IPv6 header and UDP (if next header is) length */
1503 len = net_pkt_get_len(pkt) - NET_IPV6H_LEN;
1504 ipv6->len = htons(len);
1505
1506 if (ipv6->nexthdr == IPPROTO_UDP && udp) {
1507 udp->len = htons(len);
1508
1509 if (nhc & NET_6LO_NHC_UDP_CHECKSUM) {
1510 udp->chksum = net_calc_chksum_udp(pkt);
1511 }
1512 }
1513
1514 net_pkt_cursor_init(pkt);
1515
1516 return true;
1517
1518 fail:
1519 if (frag != pkt->buffer) {
1520 net_pkt_frag_unref(frag);
1521 }
1522
1523 return false;
1524 }
1525
1526 /* Adds IPv6 dispatch as first byte and adjust fragments */
compress_ipv6_header(struct net_pkt * pkt)1527 static inline int compress_ipv6_header(struct net_pkt *pkt)
1528 {
1529 struct net_buf *buffer = pkt->buffer;
1530
1531 if (net_buf_tailroom(buffer) >= 1U) {
1532 memmove(buffer->data + 1U, buffer->data, buffer->len);
1533 net_buf_add(buffer, 1U);
1534 buffer->data[0] = NET_6LO_DISPATCH_IPV6;
1535 return 0;
1536 }
1537
1538 buffer = net_pkt_get_frag(pkt, 1, K_FOREVER);
1539 if (!buffer) {
1540 return -ENOBUFS;
1541 }
1542
1543 buffer->data[0] = NET_6LO_DISPATCH_IPV6;
1544 net_buf_add(buffer, 1);
1545
1546 net_pkt_frag_insert(pkt, buffer);
1547
1548 /* Compact the fragments, so that gaps will be filled */
1549 net_pkt_compact(pkt);
1550
1551 return 0;
1552 }
1553
uncompress_ipv6_header(struct net_pkt * pkt)1554 static inline bool uncompress_ipv6_header(struct net_pkt *pkt)
1555 {
1556 /* Pull off IPv6 dispatch header and adjust data and length */
1557 net_buf_pull(pkt->buffer, 1U);
1558 net_pkt_cursor_init(pkt);
1559
1560 return true;
1561 }
1562
net_6lo_compress(struct net_pkt * pkt,bool iphc)1563 int net_6lo_compress(struct net_pkt *pkt, bool iphc)
1564 {
1565 if (iphc) {
1566 return compress_IPHC_header(pkt);
1567 } else {
1568 return compress_ipv6_header(pkt);
1569 }
1570 }
1571
net_6lo_uncompress(struct net_pkt * pkt)1572 bool net_6lo_uncompress(struct net_pkt *pkt)
1573 {
1574 NET_ASSERT(pkt && pkt->frags);
1575
1576 if ((pkt->frags->data[0] & NET_6LO_DISPATCH_IPHC_MASK) ==
1577 NET_6LO_DISPATCH_IPHC) {
1578 /* Uncompress IPHC header */
1579 return uncompress_IPHC_header(pkt);
1580
1581 } else if (pkt->frags->data[0] == NET_6LO_DISPATCH_IPV6) {
1582 /* Uncompress IPv6 header, it has only IPv6 dispatch in the
1583 * beginning */
1584 return uncompress_ipv6_header(pkt);
1585 }
1586
1587 NET_DBG("pkt %p is not compressed", pkt);
1588
1589 return true;
1590 }
1591
net_6lo_uncompress_hdr_diff(struct net_pkt * pkt)1592 int net_6lo_uncompress_hdr_diff(struct net_pkt *pkt)
1593 {
1594 int inline_size, compressed_hdr_size, nhc_inline_size, diff;
1595 uint16_t iphc;
1596 uint8_t nhc;
1597
1598 if (pkt->frags->data[0] == NET_6LO_DISPATCH_IPV6) {
1599 return -1;
1600 }
1601
1602 if ((pkt->frags->data[0] & NET_6LO_DISPATCH_IPHC_MASK) !=
1603 NET_6LO_DISPATCH_IPHC) {
1604 return 0;
1605 }
1606
1607 iphc = ntohs(UNALIGNED_GET((uint16_t *)pkt->buffer->data));
1608
1609 inline_size = get_ihpc_inlined_size(iphc);
1610 if (inline_size < 0) {
1611 return INT_MAX;
1612 }
1613
1614 compressed_hdr_size = sizeof(iphc) + inline_size;
1615 diff = sizeof(struct net_ipv6_hdr) - compressed_hdr_size;
1616
1617 if (iphc & NET_6LO_IPHC_NH_MASK) {
1618 nhc = *(pkt->buffer->data + sizeof(iphc) + inline_size);
1619 if ((nhc & 0xF8) != NET_6LO_NHC_UDP_BARE) {
1620 NET_ERR("Unsupported next header");
1621 return INT_MAX;
1622 }
1623
1624 nhc_inline_size = get_udp_nhc_inlined_size(nhc);
1625 diff += sizeof(struct net_udp_hdr) - sizeof(uint8_t) -
1626 nhc_inline_size;
1627 }
1628
1629 return diff;
1630 }
1631
