1 /*
2 * Copyright (c) 2016 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 /**
8 * @file
9 * @brief 802.15.4 6LoWPAN fragment handler implementation
10 */
11
12 #include <zephyr/logging/log.h>
13 LOG_MODULE_REGISTER(net_ieee802154_6lo_fragment, CONFIG_NET_L2_IEEE802154_LOG_LEVEL);
14
15 #include "6lo.h"
16 #include "6lo_private.h"
17 #include "ieee802154_6lo_fragment.h"
18 #include "net_private.h"
19
20 #include <errno.h>
21
22 #include <zephyr/net/net_core.h>
23 #include <zephyr/net/net_if.h>
24 #include <zephyr/net/net_pkt.h>
25 #include <zephyr/net/net_stats.h>
26 #include <zephyr/net/udp.h>
27
28 #define NET_FRAG_DISPATCH_MASK 0xF8
29 #define NET_FRAG_OFFSET_POS (NET_6LO_FRAG_DATAGRAM_SIZE_LEN + NET_6LO_FRAG_DATAGRAM_OFFSET_LEN)
30
31 #define BUF_TIMEOUT K_MSEC(50)
32
33 #define FRAG_REASSEMBLY_TIMEOUT K_SECONDS(CONFIG_NET_L2_IEEE802154_REASSEMBLY_TIMEOUT)
34 #define REASS_CACHE_SIZE CONFIG_NET_L2_IEEE802154_FRAGMENT_REASS_CACHE_SIZE
35
36 /**
37 * Reassemble cache : Depends on cache size it used for reassemble
38 * IPv6 packets simultaneously.
39 */
40 struct frag_cache {
41 struct k_work_delayable timer; /* Reassemble timer */
42 struct net_pkt *pkt; /* Reassemble packet */
43 uint16_t size; /* Datagram size */
44 uint16_t tag; /* Datagram tag */
45 bool used;
46 };
47
48 static struct frag_cache cache[REASS_CACHE_SIZE];
49
50 /**
51 * RFC 4944, section 5.3
52 * If an entire payload (e.g., IPv6) datagram fits within a single 802.15.4
53 * frame, it is unfragmented and the LoWPAN encapsulation should not contain
54 * a fragmentation header. If the datagram does not fit within a single
55 * IEEE 802.15.4 frame, it SHALL be broken into link fragments. As the
56 * fragment offset can only express multiples of eight bytes, all link
57 * fragments for a datagram except the last one MUST be multiples of eight
58 * bytes in length.
59 *
60 * RFC 7668, section 3 (IPv6 over Bluetooth Low Energy)
61 * Functionality is comprised of link-local IPv6 addresses and stateless
62 * IPv6 address autoconfiguration, Neighbor Discovery, and header compression
63 * Fragmentation features from 6LoWPAN standards are not used due to Bluetooth
64 * LE's link-layer fragmentation support.
65 */
66
67 /**
68 * 1 2 3
69 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
70 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
71 * |1 1 0 0 0| datagram_size | datagram_tag |
72 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
73 *
74 * 1 2 3
75 * 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
76 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
77 * |1 1 0 0 0| datagram_size | datagram_tag |
78 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
79 * |datagram_offset|
80 * +-+-+-+-+-+-+-+-+
81 */
82
set_datagram_size(uint8_t * ptr,uint16_t size)83 static inline void set_datagram_size(uint8_t *ptr, uint16_t size)
84 {
85 ptr[0] |= ((size & 0x7FF) >> 8);
86 ptr[1] = (uint8_t)size;
87 }
88
set_datagram_tag(uint8_t * ptr,uint16_t tag)89 static inline void set_datagram_tag(uint8_t *ptr, uint16_t tag)
90 {
91 ptr[0] = tag >> 8;
92 ptr[1] = (uint8_t)tag;
93 }
94
set_up_frag_hdr(struct net_buf * frag,uint16_t size,uint8_t offset)95 static inline void set_up_frag_hdr(struct net_buf *frag, uint16_t size, uint8_t offset)
96 {
97 static uint16_t datagram_tag;
98
99 bool is_first_frag = !offset;
100 uint8_t pos = frag->len;
101
102 if (is_first_frag) {
103 datagram_tag++;
104 net_buf_add(frag, NET_6LO_FRAG1_HDR_LEN);
105 frag->data[pos] = NET_6LO_DISPATCH_FRAG1;
106 } else {
107 net_buf_add(frag, NET_6LO_FRAGN_HDR_LEN);
108 frag->data[pos] = NET_6LO_DISPATCH_FRAGN;
109 }
110
111 set_datagram_size(frag->data + pos, size);
112 pos += NET_6LO_FRAG_DATAGRAM_SIZE_LEN;
113
114 set_datagram_tag(frag->data + pos, datagram_tag);
115 pos += NET_6LO_FRAG_DATAGRAM_OFFSET_LEN;
116
117 if (!is_first_frag) {
118 frag->data[pos] = offset;
119 }
120 }
121
calc_payload_capacity(struct net_buf * frag)122 static inline uint8_t calc_payload_capacity(struct net_buf *frag)
123 {
124 uint8_t capacity = frag->size - frag->len;
125
126 return (capacity & 0xF8);
127 }
128
copy_data(struct ieee802154_6lo_fragment_ctx * ctx,struct net_buf * frame_buf,uint8_t capacity)129 static inline uint8_t copy_data(struct ieee802154_6lo_fragment_ctx *ctx, struct net_buf *frame_buf,
130 uint8_t capacity)
131 {
132 uint8_t remainder = ctx->buf->len - (ctx->pos - ctx->buf->data);
133 uint8_t move = MIN(remainder, capacity);
134
135 memcpy(frame_buf->data + frame_buf->len, ctx->pos, move);
136 net_buf_add(frame_buf, move);
137
138 return move;
139 }
140
update_fragment_ctx(struct ieee802154_6lo_fragment_ctx * ctx,uint8_t moved)141 static inline void update_fragment_ctx(struct ieee802154_6lo_fragment_ctx *ctx, uint8_t moved)
142 {
143 uint8_t remainder = (ctx->buf->len - (ctx->pos - ctx->buf->data));
144 bool next_frag = moved == remainder;
145
146 if (next_frag) {
147 ctx->buf = ctx->buf->frags;
148 if (ctx->buf) {
149 ctx->pos = ctx->buf->data;
150 }
151 } else {
152 ctx->pos += moved;
153 }
154 }
155
156 /**
157 * ch : compressed (IPv6) header(s)
158 * fh : fragment header (dispatch + size + tag + [offset])
159 * p : payload (first fragment holds IPv6 hdr as payload)
160 * e : empty space
161 * ll : link layer
162 *
163 * Input frame_buf to ieee802154_6lo_fragment() looks like below
164 *
165 * | ll |
166 *
167 * After fragment creation, frame_buf will look like:
168 *
169 * | ll + fh + p + e |
170 *
171 * p being taken from current pkt buffer and position.
172 *
173 * Space in every fragment is because fragment payload should be multiple
174 * of 8 octets (we have predefined packets at compile time, data packet mtu
175 * is set already).
176 *
177 * If it's the first fragment being created, fh will not own any offset
178 * (so it will be 1 byte smaller)
179 */
ieee802154_6lo_fragment(struct ieee802154_6lo_fragment_ctx * ctx,struct net_buf * frame_buf,bool iphc)180 struct net_buf *ieee802154_6lo_fragment(struct ieee802154_6lo_fragment_ctx *ctx,
181 struct net_buf *frame_buf, bool iphc)
182 {
183 uint8_t capacity;
184
185 set_up_frag_hdr(frame_buf, ctx->pkt_size, ctx->offset);
186
187 capacity = calc_payload_capacity(frame_buf);
188 ctx->processed += capacity;
189
190 bool is_first_frag = !ctx->offset;
191
192 if (is_first_frag) {
193 /* First fragment needs to take into account 6lo */
194 if (iphc) {
195 capacity -= ctx->hdr_diff;
196 } else {
197 /* Adding IPv6 dispatch header */
198 capacity += 1U;
199 }
200 }
201
202 while (capacity && ctx->buf) {
203 uint8_t moved = copy_data(ctx, frame_buf, capacity);
204
205 update_fragment_ctx(ctx, moved);
206 capacity -= moved;
207 }
208
209 ctx->offset = ctx->processed >> 3;
210
211 return ctx->buf;
212 }
213
get_datagram_type(uint8_t * ptr)214 static inline uint8_t get_datagram_type(uint8_t *ptr)
215 {
216 return ptr[0] & NET_FRAG_DISPATCH_MASK;
217 }
218
get_datagram_size(uint8_t * ptr)219 static inline uint16_t get_datagram_size(uint8_t *ptr)
220 {
221 return ((ptr[0] & 0x1F) << 8) | ptr[1];
222 }
223
get_datagram_tag(uint8_t * ptr)224 static inline uint16_t get_datagram_tag(uint8_t *ptr)
225 {
226 return (ptr[0] << 8) | ptr[1];
227 }
228
update_protocol_header_lengths(struct net_pkt * pkt,uint16_t size)229 static void update_protocol_header_lengths(struct net_pkt *pkt, uint16_t size)
230 {
231 NET_PKT_DATA_ACCESS_DEFINE(ipv6_access, struct net_ipv6_hdr);
232 struct net_ipv6_hdr *ipv6;
233
234 ipv6 = (struct net_ipv6_hdr *)net_pkt_get_data(pkt, &ipv6_access);
235 if (!ipv6) {
236 NET_ERR("could not get IPv6 header");
237 return;
238 }
239
240 net_pkt_set_ip_hdr_len(pkt, NET_IPV6H_LEN);
241 ipv6->len = htons(size - NET_IPV6H_LEN);
242
243 net_pkt_set_data(pkt, &ipv6_access);
244
245 if (ipv6->nexthdr == IPPROTO_UDP) {
246 NET_PKT_DATA_ACCESS_DEFINE(udp_access, struct net_udp_hdr);
247 struct net_udp_hdr *udp;
248
249 udp = (struct net_udp_hdr *)net_pkt_get_data(pkt, &udp_access);
250 if (udp) {
251 udp->len = htons(size - NET_IPV6H_LEN);
252 net_pkt_set_data(pkt, &udp_access);
253 } else {
254 NET_ERR("could not get UDP header");
255 }
256 }
257 }
258
clear_reass_cache(uint16_t size,uint16_t tag)259 static inline void clear_reass_cache(uint16_t size, uint16_t tag)
260 {
261 uint8_t i;
262
263 for (i = 0U; i < REASS_CACHE_SIZE; i++) {
264 if (!(cache[i].size == size && cache[i].tag == tag)) {
265 continue;
266 }
267
268 if (cache[i].pkt) {
269 net_pkt_unref(cache[i].pkt);
270 }
271
272 cache[i].pkt = NULL;
273 cache[i].size = 0U;
274 cache[i].tag = 0U;
275 cache[i].used = false;
276 k_work_cancel_delayable(&cache[i].timer);
277 }
278 }
279
280 /**
281 * If the reassembly not completed within reassembly timeout discard
282 * the whole packet.
283 */
reass_timeout(struct k_work * work)284 static void reass_timeout(struct k_work *work)
285 {
286 struct k_work_delayable *dwork = k_work_delayable_from_work(work);
287 struct frag_cache *fcache = CONTAINER_OF(dwork, struct frag_cache, timer);
288
289 if (fcache->pkt) {
290 net_pkt_unref(fcache->pkt);
291 }
292
293 fcache->pkt = NULL;
294 fcache->size = 0U;
295 fcache->tag = 0U;
296 fcache->used = false;
297 }
298
299 /**
300 * Upon reception of first fragment with respective of size and tag
301 * create a new cache. If number of unused cache are out then
302 * discard the fragments.
303 */
set_reass_cache(struct net_pkt * pkt,uint16_t size,uint16_t tag)304 static inline struct frag_cache *set_reass_cache(struct net_pkt *pkt, uint16_t size, uint16_t tag)
305 {
306 int i;
307
308 for (i = 0; i < REASS_CACHE_SIZE; i++) {
309 if (cache[i].used) {
310 continue;
311 }
312
313 cache[i].pkt = pkt;
314 cache[i].size = size;
315 cache[i].tag = tag;
316 cache[i].used = true;
317
318 k_work_init_delayable(&cache[i].timer, reass_timeout);
319 k_work_reschedule(&cache[i].timer, FRAG_REASSEMBLY_TIMEOUT);
320 return &cache[i];
321 }
322
323 return NULL;
324 }
325
326 /**
327 * Return cache if it matches with size and tag of stored caches,
328 * otherwise return NULL.
329 */
get_reass_cache(uint16_t size,uint16_t tag)330 static inline struct frag_cache *get_reass_cache(uint16_t size, uint16_t tag)
331 {
332 uint8_t i;
333
334 for (i = 0U; i < REASS_CACHE_SIZE; i++) {
335 if (cache[i].used) {
336 if (cache[i].size == size && cache[i].tag == tag) {
337 return &cache[i];
338 }
339 }
340 }
341
342 return NULL;
343 }
344
fragment_append(struct net_pkt * pkt,struct net_buf * frag)345 static inline void fragment_append(struct net_pkt *pkt, struct net_buf *frag)
346 {
347 if (get_datagram_type(frag->data) == NET_6LO_DISPATCH_FRAG1) {
348 /* Always make sure first fragment is inserted first
349 * This will be useful for fragment_cached_pkt_len()
350 */
351 frag->frags = pkt->buffer;
352 pkt->buffer = frag;
353 } else {
354 net_pkt_append_buffer(pkt, frag);
355 }
356 }
357
fragment_cached_pkt_len(struct net_pkt * pkt)358 static inline size_t fragment_cached_pkt_len(struct net_pkt *pkt)
359 {
360 size_t len = 0U;
361 struct net_buf *frag;
362 int hdr_diff;
363 uint8_t *data;
364
365 frag = pkt->buffer;
366 while (frag) {
367 uint16_t frag_hdr_len = NET_6LO_FRAGN_HDR_LEN;
368
369 if (get_datagram_type(frag->data) == NET_6LO_DISPATCH_FRAG1) {
370 frag_hdr_len = NET_6LO_FRAG1_HDR_LEN;
371 }
372
373 len += frag->len - frag_hdr_len;
374
375 frag = frag->frags;
376 }
377
378 /* 6lo assumes that fragment header has been removed,
379 * and in our side we assume first buffer is always the first fragment.
380 */
381 data = pkt->buffer->data;
382 pkt->buffer->data += NET_6LO_FRAG1_HDR_LEN;
383
384 hdr_diff = net_6lo_uncompress_hdr_diff(pkt);
385
386 pkt->buffer->data = data;
387
388 if (hdr_diff == INT_MAX) {
389 return 0;
390 }
391
392 return len + hdr_diff;
393 }
394
fragment_offset(struct net_buf * frag)395 static inline uint16_t fragment_offset(struct net_buf *frag)
396 {
397 if (get_datagram_type(frag->data) == NET_6LO_DISPATCH_FRAG1) {
398 return 0;
399 }
400
401 return ((uint16_t)frag->data[NET_FRAG_OFFSET_POS] << 3);
402 }
403
fragment_move_back(struct net_pkt * pkt,struct net_buf * frag,struct net_buf * stop)404 static void fragment_move_back(struct net_pkt *pkt, struct net_buf *frag, struct net_buf *stop)
405 {
406 struct net_buf *prev, *current;
407
408 prev = NULL;
409 current = pkt->buffer;
410
411 while (current && current != stop) {
412 if (fragment_offset(frag) < fragment_offset(current)) {
413 if (prev) {
414 prev->frags = frag;
415 }
416
417 frag->frags = current;
418 break;
419 }
420
421 prev = current;
422 current = current->frags;
423 }
424 }
425
fragment_remove_headers(struct net_pkt * pkt)426 static inline void fragment_remove_headers(struct net_pkt *pkt)
427 {
428 struct net_buf *frag;
429
430 frag = pkt->buffer;
431 while (frag) {
432 uint16_t frag_hdr_len = NET_6LO_FRAGN_HDR_LEN;
433
434 if (get_datagram_type(frag->data) == NET_6LO_DISPATCH_FRAG1) {
435 frag_hdr_len = NET_6LO_FRAG1_HDR_LEN;
436 }
437
438 memmove(frag->data, frag->data + frag_hdr_len, frag->len - frag_hdr_len);
439 frag->len -= frag_hdr_len;
440
441 frag = frag->frags;
442 }
443 }
444
fragment_reconstruct_packet(struct net_pkt * pkt)445 static inline void fragment_reconstruct_packet(struct net_pkt *pkt)
446 {
447 struct net_buf *prev, *current, *next;
448
449 prev = NULL;
450 current = pkt->buffer;
451
452 while (current) {
453 next = current->frags;
454
455 if (!prev || (fragment_offset(prev) > fragment_offset(current))) {
456 prev = current;
457 } else {
458 fragment_move_back(pkt, current, prev);
459 }
460
461 current = next;
462 }
463
464 /* Let's remove now useless fragmentation headers */
465 fragment_remove_headers(pkt);
466 }
467
fragment_packet_valid(struct net_pkt * pkt)468 static inline bool fragment_packet_valid(struct net_pkt *pkt)
469 {
470 return (get_datagram_type(pkt->buffer->data) == NET_6LO_DISPATCH_FRAG1);
471 }
472
473 /**
474 * Parse size and tag from the fragment, check if we have any cache
475 * related to it. If not create a new cache.
476 * Remove the fragmentation header and uncompress IPv6 and related headers.
477 * Cache Rx part of fragment along with data buf for the first fragment
478 * in the cache, remaining fragments just cache data fragment, unref
479 * RX pkt. So in both the cases caller can assume packet is consumed.
480 */
fragment_add_to_cache(struct net_pkt * pkt)481 static inline enum net_verdict fragment_add_to_cache(struct net_pkt *pkt)
482 {
483 bool first_frag = false;
484 struct frag_cache *fcache;
485 struct net_buf *frag;
486 uint16_t size;
487 uint16_t tag;
488 uint8_t type;
489
490 frag = pkt->buffer;
491 type = get_datagram_type(frag->data);
492
493 if ((type == NET_6LO_DISPATCH_FRAG1 && frag->len < NET_6LO_FRAG1_HDR_LEN) ||
494 (type == NET_6LO_DISPATCH_FRAGN && frag->len < NET_6LO_FRAGN_HDR_LEN)) {
495 return NET_DROP;
496 }
497
498 /* Parse total size of packet */
499 size = get_datagram_size(frag->data);
500
501 /* Parse the datagram tag */
502 tag = get_datagram_tag(frag->data + NET_6LO_FRAG_DATAGRAM_SIZE_LEN);
503
504 /* If there are no fragments in the cache means this frag
505 * is the first one. So cache Rx pkt otherwise not.
506 */
507 pkt->buffer = NULL;
508
509 fcache = get_reass_cache(size, tag);
510 if (!fcache) {
511 fcache = set_reass_cache(pkt, size, tag);
512 if (!fcache) {
513 NET_ERR("Could not get a cache entry");
514 pkt->buffer = frag;
515 return NET_DROP;
516 }
517
518 first_frag = true;
519 }
520
521 fragment_append(fcache->pkt, frag);
522
523 if (fragment_cached_pkt_len(fcache->pkt) == fcache->size) {
524 if (!first_frag) {
525 /* Assign buffer back to input packet. */
526 pkt->buffer = fcache->pkt->buffer;
527 fcache->pkt->buffer = NULL;
528 } else {
529 /* in case pkt == fcache->pkt, we don't want
530 * to unref it while clearing the cach.
531 */
532 fcache->pkt = NULL;
533 }
534
535 clear_reass_cache(size, tag);
536
537 if (!fragment_packet_valid(pkt)) {
538 NET_ERR("Invalid fragmented packet");
539 return NET_DROP;
540 }
541
542 fragment_reconstruct_packet(pkt);
543
544 if (!net_6lo_uncompress(pkt)) {
545 NET_ERR("Could not uncompress. Bogus packet?");
546 return NET_DROP;
547 }
548
549 net_pkt_cursor_init(pkt);
550
551 update_protocol_header_lengths(pkt, size);
552
553 net_pkt_cursor_init(pkt);
554
555 NET_DBG("All fragments received and reassembled");
556
557 return NET_CONTINUE;
558 }
559
560 /* Unref Rx part of original packet */
561 if (!first_frag) {
562 net_pkt_unref(pkt);
563 }
564
565 return NET_OK;
566 }
567
ieee802154_6lo_reassemble(struct net_pkt * pkt)568 enum net_verdict ieee802154_6lo_reassemble(struct net_pkt *pkt)
569 {
570 if (!pkt || !pkt->buffer) {
571 NET_ERR("Nothing to reassemble");
572 return NET_DROP;
573 }
574
575 if (get_datagram_type(pkt->buffer->data) >= NET_6LO_DISPATCH_FRAG1) {
576 return fragment_add_to_cache(pkt);
577 } else {
578 NET_DBG("No frag dispatch (%02x)", pkt->buffer->data[0]);
579 /* Received unfragmented packet, uncompress */
580 if (net_6lo_uncompress(pkt)) {
581 return NET_CONTINUE;
582 }
583
584 NET_ERR("Could not uncompress. Bogus packet?");
585 }
586
587 return NET_DROP;
588 }
589
