1/*********************************************************************
2   PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
3   See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage.
4
5   Authors: Laurens Miers, Daniele Lacamera
6 *********************************************************************/
7
8
9#include "pico_config.h"
10#ifdef PICO_SUPPORT_IPV6
11#include "pico_ipv6.h"
12#include "pico_icmp6.h"
13#endif
14#ifdef PICO_SUPPORT_IPV4
15#include "pico_ipv4.h"
16#include "pico_icmp4.h"
17#endif
18#include "pico_stack.h"
19#include "pico_eth.h"
20#include "pico_udp.h"
21#include "pico_tcp.h"
22#include "pico_socket.h"
23#include "pico_device.h"
24#include "pico_tree.h"
25#include "pico_constants.h"
26#include "pico_fragments.h"
27
28#ifdef DEBUG_FRAG
29    #define frag_dbg dbg
30#else
31    #define frag_dbg(...) do {} while(0)
32#endif
33
34#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG)
35#define IP6_FRAG_OFF(x)         ((x & 0xFFF8u))
36#define IP6_FRAG_MORE(x)        ((x & 0x0001))
37#define IP6_FRAG_ID(x)          ((uint32_t)(((uint32_t)x->ext.frag.id[0] << 24) + ((uint32_t)x->ext.frag.id[1] << 16) + \
38                                            ((uint32_t)x->ext.frag.id[2] << 8) + (uint32_t)x->ext.frag.id[3]))
39
40#else
41#define IP6_FRAG_OFF(x)         (0)
42#define IP6_FRAG_MORE(x)        (0)
43#define IP6_FRAG_ID(x)          (0)
44#endif
45
46#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG)
47#define IP4_FRAG_OFF(frag)      (((uint32_t)frag & PICO_IPV4_FRAG_MASK) << 3ul)
48#define IP4_FRAG_MORE(frag)     ((frag & PICO_IPV4_MOREFRAG) ? 1 : 0)
49#define IP4_FRAG_ID(hdr)        (hdr->id)
50#else
51#define IP4_FRAG_OFF(frag)      (0)
52#define IP4_FRAG_MORE(frag)     (0)
53#define IP4_FRAG_ID(hdr)        (0)
54#endif
55
56#define PICO_IPV6_FRAG_TIMEOUT   60000
57#define PICO_IPV4_FRAG_TIMEOUT   15000
58
59static void pico_frag_expire(pico_time now, void *arg);
60static void pico_fragments_complete(unsigned int bookmark, uint8_t proto, uint8_t net);
61static int pico_fragments_check_complete(struct pico_tree *tree, uint8_t proto, uint8_t net);
62static int pico_fragments_reassemble(struct pico_tree *tree, unsigned int len, uint8_t proto, uint8_t net);
63static int pico_fragments_get_more_flag(struct pico_frame *frame, uint8_t net);
64static uint32_t pico_fragments_get_offset(struct pico_frame *frame, uint8_t net);
65static void pico_fragments_send_notify(struct pico_frame *first);
66static uint16_t pico_fragments_get_header_length(uint8_t net);
67static void pico_fragments_empty_tree(struct pico_tree *tree);
68
69#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG)
70static uint32_t ipv6_cur_frag_id = 0u;
71static uint32_t ipv6_fragments_timer = 0u;
72
73static int pico_ipv6_frag_compare(void *ka, void *kb)
74{
75    struct pico_frame *a = ka, *b = kb;
76    if (IP6_FRAG_OFF(a->frag) > IP6_FRAG_OFF(b->frag))
77        return 1;
78
79    if (IP6_FRAG_OFF(a->frag) < IP6_FRAG_OFF(b->frag))
80        return -1;
81
82    return 0;
83}
84static PICO_TREE_DECLARE(ipv6_fragments, pico_ipv6_frag_compare);
85
86static void pico_ipv6_fragments_complete(unsigned int len, uint8_t proto)
87{
88    if (pico_fragments_reassemble(&ipv6_fragments, len, proto, PICO_PROTO_IPV6) == 0)
89    {
90        pico_timer_cancel(ipv6_fragments_timer);
91        ipv6_fragments_timer = 0;
92    }
93}
94
95static void pico_ipv6_frag_timer_on(void)
96{
97    ipv6_fragments_timer = pico_timer_add(PICO_IPV6_FRAG_TIMEOUT, pico_frag_expire, &ipv6_fragments);
98    if (!ipv6_fragments_timer) {
99        frag_dbg("FRAG: Failed to start IPv6 expiration timer\n");
100        pico_fragments_empty_tree(&ipv6_fragments);
101    }
102}
103
104static int pico_ipv6_frag_match(struct pico_frame *a, struct pico_frame *b)
105{
106    struct pico_ipv6_hdr *ha = NULL, *hb = NULL;
107    if (!a || !b)
108        return -1;
109
110    ha = (struct pico_ipv6_hdr *)a->net_hdr;
111    hb = (struct pico_ipv6_hdr *)b->net_hdr;
112    if (!ha || !hb)
113        return -2;
114
115    if (memcmp(ha->src.addr, hb->src.addr, PICO_SIZE_IP6) != 0)
116        return 1;
117
118    if (memcmp(ha->dst.addr, hb->dst.addr, PICO_SIZE_IP6) != 0)
119        return 2;
120
121    return 0;
122}
123#endif
124
125#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG)
126static uint32_t ipv4_cur_frag_id = 0u;
127static uint32_t ipv4_fragments_timer = 0u;
128
129static int pico_ipv4_frag_compare(void *ka, void *kb)
130{
131    struct pico_frame *a = ka, *b = kb;
132    if (IP4_FRAG_OFF(a->frag) > IP4_FRAG_OFF(b->frag))
133        return 1;
134
135    if (IP4_FRAG_OFF(a->frag) < IP4_FRAG_OFF(b->frag))
136        return -1;
137
138    return 0;
139}
140static PICO_TREE_DECLARE(ipv4_fragments, pico_ipv4_frag_compare);
141
142static void pico_ipv4_fragments_complete(unsigned int len, uint8_t proto)
143{
144    if (pico_fragments_reassemble(&ipv4_fragments, len, proto, PICO_PROTO_IPV4) == 0)
145    {
146        pico_timer_cancel(ipv4_fragments_timer);
147        ipv4_fragments_timer = 0;
148    }
149}
150
151static void pico_ipv4_frag_timer_on(void)
152{
153    ipv4_fragments_timer = pico_timer_add( PICO_IPV4_FRAG_TIMEOUT, pico_frag_expire, &ipv4_fragments);
154    if (!ipv4_fragments_timer) {
155        frag_dbg("FRAG: Failed to start IPv4 expiration timer\n");
156        pico_fragments_empty_tree(&ipv4_fragments);
157    }
158}
159
160static int pico_ipv4_frag_match(struct pico_frame *a, struct pico_frame *b)
161{
162    struct pico_ipv4_hdr *ha, *hb;
163    if (!a || !b)
164        return -1;
165
166    ha = (struct pico_ipv4_hdr *)a->net_hdr;
167    hb = (struct pico_ipv4_hdr *)b->net_hdr;
168    if (!ha || !hb)
169        return -2;
170
171    if (memcmp(&(ha->src.addr), &(hb->src.addr), PICO_SIZE_IP4) != 0)
172        return 1;
173
174    if (memcmp(&(ha->dst.addr), &(hb->dst.addr), PICO_SIZE_IP4) != 0)
175        return 2;
176
177    return 0;
178}
179#endif
180
181static void pico_fragments_complete(unsigned int bookmark, uint8_t proto, uint8_t net)
182{
183    if (0) {}
184
185#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG)
186    else if (net == PICO_PROTO_IPV4)
187    {
188        pico_ipv4_fragments_complete(bookmark, proto);
189    }
190#endif
191#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG)
192    else if (net == PICO_PROTO_IPV6)
193    {
194        pico_ipv6_fragments_complete(bookmark, proto);
195    }
196#endif
197}
198
199static void pico_fragments_empty_tree(struct pico_tree *tree)
200{
201    struct pico_tree_node *index, *tmp;
202
203    if (!tree)
204    {
205        return;
206    }
207
208    pico_tree_foreach_safe(index, tree, tmp) {
209        struct pico_frame * old = index->keyValue;
210        pico_tree_delete(tree, old);
211        pico_frame_discard(old);
212    }
213
214}
215
216static int pico_fragments_check_complete(struct pico_tree *tree, uint8_t proto, uint8_t net)
217{
218    struct pico_tree_node *index, *temp;
219    struct pico_frame *cur;
220    unsigned int bookmark = 0;
221
222    if (!tree)
223        return 0;
224
225    pico_tree_foreach_safe(index, tree, temp) {
226        cur = index->keyValue;
227        if (cur) {
228            if (pico_fragments_get_offset(cur, net) != bookmark)
229                return -1;
230
231            bookmark += cur->transport_len;
232            if (!pico_fragments_get_more_flag(cur, net)) {
233                pico_fragments_complete(bookmark, proto, net);
234                return 0;
235            }
236        }
237    }
238    return 1;
239}
240
241static void pico_frag_expire(pico_time now, void *arg)
242{
243    struct pico_tree *tree = (struct pico_tree *) arg;
244    struct pico_frame *first = NULL;
245    IGNORE_PARAMETER(now);
246
247    if (!tree)
248    {
249        frag_dbg("Expired packet but no tree supplied!\n");
250        return;
251    }
252
253    first = pico_tree_first(tree);
254
255    if (!first) {
256        frag_dbg("Empty tree - not sending notify\n");
257        return;
258    }
259
260    pico_fragments_send_notify(first);
261
262    pico_fragments_empty_tree(tree);
263}
264
265static void pico_fragments_send_notify(struct pico_frame *first)
266{
267    uint8_t net = 0;
268
269    if (!first)
270    {
271        return;
272    }
273
274    if (0) {}
275
276#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG)
277    else if (IS_IPV4(first))
278    {
279        net = PICO_PROTO_IPV4;
280        frag_dbg("Packet expired! ID:%hu\n", ipv4_cur_frag_id);
281    }
282
283#endif
284#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG)
285    else if (IS_IPV6(first))
286    {
287        net = PICO_PROTO_IPV6;
288        frag_dbg("Packet expired! ID:%hu\n", ipv6_cur_frag_id);
289    }
290
291#endif
292
293    if (((pico_fragments_get_offset(first, net) == 0) && (pico_frame_dst_is_unicast(first))))
294    {
295        frag_dbg("sending notify\n");
296        pico_notify_frag_expired(first);
297    }
298    else
299    {
300        frag_dbg("Not first packet or not unicast address, not sending notify");
301    }
302}
303
304static int pico_fragments_reassemble(struct pico_tree *tree, unsigned int len, uint8_t proto, uint8_t net)
305{
306    struct pico_tree_node *index, *tmp;
307    struct pico_frame *f;
308    uint16_t header_length = 0;
309    unsigned int bookmark = 0;
310    struct pico_frame *full = NULL;
311    struct pico_frame *first = NULL;
312
313    if (!tree)
314    {
315        frag_dbg("Cannot reassemble packet, no tree supplied!\n");
316        return -1;
317    }
318
319    first = pico_tree_first(tree);
320
321    if (!first)
322    {
323        frag_dbg("Cannot reassemble packet, empty tree supplied!\n");
324        return -2;
325    }
326
327    header_length = pico_fragments_get_header_length(net);
328
329    if (!header_length)
330    {
331        return -3;
332    }
333
334    full = pico_frame_alloc((uint16_t)(header_length + len));
335    if (full) {
336        full->net_hdr = full->buffer;
337        full->net_len = header_length;
338        memcpy(full->net_hdr, first->net_hdr, full->net_len);
339        full->transport_hdr = full->net_hdr + full->net_len;
340        full->transport_len = (uint16_t)len;
341        full->dev = first->dev;
342        pico_tree_foreach_safe(index, tree, tmp) {
343            f = index->keyValue;
344            memcpy(full->transport_hdr + bookmark, f->transport_hdr, f->transport_len);
345            bookmark += f->transport_len;
346            pico_tree_delete(tree, f);
347            pico_frame_discard(f);
348        }
349        if (pico_transport_receive(full, proto) == -1)
350        {
351            pico_frame_discard(full);
352        }
353
354        return 0;
355    }
356
357    return 1;
358}
359
360static uint16_t pico_fragments_get_header_length(uint8_t net)
361{
362    if (0) {}
363
364#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG)
365    else if (net == PICO_PROTO_IPV4)
366    {
367        return PICO_SIZE_IP4HDR;
368    }
369#endif
370#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG)
371    else if (net == PICO_PROTO_IPV6)
372    {
373        return PICO_SIZE_IP6HDR;
374    }
375#endif
376
377    return 0;
378}
379
380static int pico_fragments_get_more_flag(struct pico_frame *frame, uint8_t net)
381{
382    if (!frame)
383    {
384      frag_dbg("no frame given to determine more flag\n");
385      return 0;
386    }
387
388    if (0) {}
389
390#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG)
391    else if (net == PICO_PROTO_IPV4)
392    {
393      return IP4_FRAG_MORE(frame->frag);
394    }
395#endif
396#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG)
397    else if (net == PICO_PROTO_IPV6)
398    {
399      return IP6_FRAG_MORE(frame->frag);
400    }
401#endif
402
403    return 0;
404}
405
406static uint32_t pico_fragments_get_offset(struct pico_frame *frame, uint8_t net)
407{
408    if (!frame)
409    {
410      frag_dbg("no frame given to determine offset\n");
411      return 0;
412    }
413
414    if (0) {}
415
416#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG)
417    else if (net == PICO_PROTO_IPV4)
418    {
419      return IP4_FRAG_OFF(frame->frag);
420    }
421#endif
422#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG)
423    else if (net == PICO_PROTO_IPV6)
424    {
425      return IP6_FRAG_OFF(frame->frag);
426    }
427#endif
428
429    return 0;
430}
431
432void pico_ipv6_process_frag(struct pico_ipv6_exthdr *frag, struct pico_frame *f, uint8_t proto)
433{
434#if defined(PICO_SUPPORT_IPV6) && defined(PICO_SUPPORT_IPV6FRAG)
435    struct pico_frame *first = NULL;
436
437    if (!f || !frag)
438    {
439        frag_dbg("Bad arguments provided to pico_ipv6_process_frag\n");
440        return;
441    }
442
443    first = pico_tree_first(&ipv6_fragments);
444
445    if (first)
446    {
447        if ((pico_ipv6_frag_match(f, first) == 0 && (IP6_FRAG_ID(frag) == ipv6_cur_frag_id))) {
448            struct pico_frame *temp = NULL;
449
450            temp = pico_frame_copy(f);
451
452            if (!temp) {
453                frag_dbg("Could not allocate memory to continue reassembly of IPV6 fragmented packet (id: %hu)\n", ipv6_cur_frag_id);
454                return;
455            }
456
457            if (pico_tree_insert(&ipv6_fragments, temp)) {
458                frag_dbg("FRAG: Could not insert picoframe in tree\n");
459                pico_frame_discard(temp);
460                return;
461            }
462        }
463    }
464    else
465    {
466        struct pico_frame *temp = NULL;
467
468        if (ipv6_cur_frag_id && (IP6_FRAG_ID(frag) == ipv6_cur_frag_id)) {
469            /* Discard late arrivals, without firing the timer. */
470            frag_dbg("discarded late arrival, exp:%hu found:%hu\n", ipv6_cur_frag_id, IP6_FRAG_ID(frag));
471            return;
472        }
473
474        temp = pico_frame_copy(f);
475
476        if (!temp) {
477            frag_dbg("Could not allocate memory to start reassembly of fragmented packet\n");
478            return;
479        }
480
481        pico_ipv6_frag_timer_on();
482        ipv6_cur_frag_id = IP6_FRAG_ID(frag);
483        frag_dbg("Started new reassembly, ID:%hu\n", ipv6_cur_frag_id);
484
485        if (pico_tree_insert(&ipv6_fragments, temp)) {
486            frag_dbg("FRAG: Could not insert picoframe in tree\n");
487            pico_frame_discard(temp);
488            return;
489        }
490    }
491
492    pico_fragments_check_complete(&ipv6_fragments, proto, PICO_PROTO_IPV6);
493#else
494    IGNORE_PARAMETER(frag);
495    IGNORE_PARAMETER(f);
496    IGNORE_PARAMETER(proto);
497#endif
498}
499
500void pico_ipv4_process_frag(struct pico_ipv4_hdr *hdr, struct pico_frame *f, uint8_t proto)
501{
502#if defined(PICO_SUPPORT_IPV4) && defined(PICO_SUPPORT_IPV4FRAG)
503    struct pico_frame *first = NULL;
504
505    if (!f || !hdr)
506    {
507        frag_dbg("Bad arguments provided to pico_ipv4_process_frag\n");
508        return;
509    }
510
511    first = pico_tree_first(&ipv4_fragments);
512
513    if (first)
514    {
515        /* fragments from old packets still in tree, and new first fragment ? */
516        if ((IP4_FRAG_ID(hdr) != ipv4_cur_frag_id) && (IP4_FRAG_OFF(f->frag) == 0)) {
517            pico_fragments_empty_tree(&ipv4_fragments);
518
519            first = NULL;
520            ipv4_cur_frag_id = 0;
521        }
522
523        if ((pico_ipv4_frag_match(f, first) == 0 && (IP4_FRAG_ID(hdr) == ipv4_cur_frag_id))) {
524            struct pico_frame *temp = NULL;
525
526            temp = pico_frame_copy(f);
527
528            if (!temp) {
529                frag_dbg("Could not allocate memory to continue reassembly of IPV4 fragmented packet (id: %hu)\n", ipv4_cur_frag_id);
530                return;
531            }
532
533            if (pico_tree_insert(&ipv4_fragments, temp)) {
534            	frag_dbg("FRAG: Could not insert picoframe in tree\n");
535                pico_frame_discard(temp);
536                return;
537			}
538        }
539    }
540    else
541    {
542        struct pico_frame *temp = NULL;
543
544        if (ipv4_cur_frag_id && (IP4_FRAG_ID(hdr) == ipv4_cur_frag_id)) {
545            /* Discard late arrivals, without firing the timer */
546            return;
547        }
548
549        temp = pico_frame_copy(f);
550
551        if (!temp) {
552            frag_dbg("Could not allocate memory to start reassembly fragmented packet\n");
553            return;
554        }
555
556        pico_ipv4_frag_timer_on();
557        ipv4_cur_frag_id = IP4_FRAG_ID(hdr);
558        frag_dbg("Started new reassembly, ID:%hu\n", ipv4_cur_frag_id);
559
560        if (pico_tree_insert(&ipv4_fragments, temp)) {
561            frag_dbg("FRAG: Could not insert picoframe in tree\n");
562            pico_frame_discard(temp);
563            return;
564        }
565    }
566
567    pico_fragments_check_complete(&ipv4_fragments, proto, PICO_PROTO_IPV4);
568#else
569    IGNORE_PARAMETER(hdr);
570    IGNORE_PARAMETER(f);
571    IGNORE_PARAMETER(proto);
572#endif
573}
574