1/*********************************************************************
2 PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved.
3 See LICENSE and COPYING for usage.
4
5 Authors: Jelle De Vleeschouwer
6 *********************************************************************/
7
8#include "pico_ipv6.h"
9#include "pico_stack.h"
10#include "pico_frame.h"
11#include "pico_802154.h"
12#include "pico_6lowpan.h"
13#include "pico_protocol.h"
14#include "pico_addressing.h"
15#include "pico_6lowpan_ll.h"
16
17#ifdef PICO_SUPPORT_6LOWPAN
18
19/*******************************************************************************
20 * Macros
21 ******************************************************************************/
22
23#ifdef DEBUG_6LOWPAN
24#define ll_dbg dbg
25#else
26#define ll_dbg(...) do {} while(0)
27#endif
28
29/*******************************************************************************
30 * Constants
31 ******************************************************************************/
32
33/* Lifetime check interval */
34#define ONE_MINUTE  ((pico_time)(1000 * 60))
35
36/* Number of extensions */
37#define NUM_LL_EXTENSIONS       (2)
38
39/*******************************************************************************
40 * Type definitions
41 ******************************************************************************/
42
43struct extension {
44    int32_t (*estimate)(struct pico_frame *f);
45    int32_t (*out)(struct pico_frame *f);
46    int32_t (*in)(struct pico_frame *f);
47};
48
49/*******************************************************************************
50 * Global Variables
51 ******************************************************************************/
52
53static const struct pico_6lowpan_ll_protocol pico_6lowpan_ll_none = {
54    NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL
55};
56
57/* Declare a global lookup-table for distribution of link layer specific tasks */
58struct pico_6lowpan_ll_protocol pico_6lowpan_lls[PICO_6LOWPAN_LLS + 1];
59
60static struct pico_queue pico_6lowpan_ll_in = {
61    0
62};
63static struct pico_queue pico_6lowpan_ll_out = {
64    0
65};
66
67/*******************************************************************************
68 *  CTX
69 ******************************************************************************/
70
71#ifdef PICO_6LOWPAN_IPHC_ENABLED
72
73/* Compares if the IPv6 prefix of two IPv6 addresses match */
74static int32_t compare_prefix(uint8_t *a, uint8_t *b, int32_t len)
75{
76    uint8_t bitmask = (uint8_t)(0xff << (8 - (len % 8)));
77    size_t bytes = (size_t)len / 8;
78    int32_t ret = 0;
79    if ((ret = memcmp(a, b, bytes)))
80        return ret;
81    return (int32_t)((a[bytes] & bitmask) - (b[bytes] & bitmask));
82}
83
84/* Compares 2 IPHC context entries */
85static int32_t compare_ctx(void *a, void *b)
86{
87    struct iphc_ctx *ca = (struct iphc_ctx *)a;
88    struct iphc_ctx *cb = (struct iphc_ctx *)b;
89    return compare_prefix(ca->prefix.addr, cb->prefix.addr, ca->size);
90}
91
92PICO_TREE_DECLARE(CTXtree, compare_ctx);
93
94/* Searches in the context tree if there's a context entry available with the
95 * prefix of the IPv6 address */
96struct iphc_ctx * ctx_lookup(struct pico_ip6 addr)
97{
98    struct iphc_ctx test = { NULL, addr, 0, 0, 0, 0 };
99    return pico_tree_findKey(&CTXtree, &test);
100}
101
102/* Looks up the context by ID, for decompression */
103struct iphc_ctx * ctx_lookup_id(uint8_t id)
104{
105    struct iphc_ctx *key = NULL;
106    struct pico_tree_node *i = NULL;
107
108    pico_tree_foreach(i, &CTXtree) {
109        key = i->keyValue;
110        if (key && id ==key->id)
111            return key;
112    }
113    return NULL;
114}
115
116/* Tries to insert a new IPHC-context into the Context-tree */
117static int32_t ctx_insert(struct pico_ip6 addr, uint8_t id, uint8_t size, pico_time lifetime, uint8_t flags, struct pico_device *dev)
118{
119    struct iphc_ctx *new = PICO_ZALLOC(sizeof(struct iphc_ctx));
120    if (new) {
121        new->lifetime = lifetime;
122        new->prefix = addr;
123        new->flags = flags;
124        new->size = size;
125        new->dev = dev;
126        new->id = id;
127        if (pico_tree_insert(&CTXtree, new)) {
128            PICO_FREE(new);
129            return -1;
130        }
131    } else {
132        return -1;
133    }
134    return 0;
135}
136
137/* Function to update context table from 6LoWPAN Neighbor Discovery */
138void ctx_update(struct pico_ip6 addr, uint8_t id, uint8_t size, pico_time lifetime, uint8_t flags, struct pico_device *dev)
139{
140    struct iphc_ctx *entry = ctx_lookup_id(id);
141    if (entry && dev == entry->dev) {
142        if (!lifetime) {
143            pico_tree_delete(&CTXtree, entry);
144            PICO_FREE(entry);
145            return;
146        }
147        entry->prefix = addr;
148        entry->size = size;
149        entry->lifetime = lifetime;
150        entry->flags = flags;
151    } else {
152        /* We don't care if it failed */
153        (void)ctx_insert(addr, id, size, lifetime, flags, dev);
154    }
155}
156
157/* Check whether or not particular contexts are expired and remove them if so. Contexts
158 * are reconfirmed before their lifetime expires */
159static void ctx_lifetime_check(pico_time now, void *arg)
160{
161    struct pico_tree_node *i = NULL, *next = NULL;
162    struct pico_ipv6_route *gw = NULL;
163    struct iphc_ctx *key = NULL;
164    IGNORE_PARAMETER(now);
165    IGNORE_PARAMETER(arg);
166
167    pico_tree_foreach_safe(i, &CTXtree, next) {
168        if (i && i->keyValue) {
169            key = i->keyValue;
170            key->lifetime--;
171            if (!key->lifetime) {
172                pico_tree_delete(&CTXtree, key);
173                PICO_FREE(key);
174            } else if (key->lifetime == 5) {
175                /* RFC6775: The host SHOULD unicast one or more RSs to the router well before the
176                 * shortest of the, Router Lifetime, PIO lifetimes and the lifetime of the 6COs. */
177                gw = pico_ipv6_gateway_by_dev(key->dev);
178                while (gw) {
179                    pico_6lp_nd_start_soliciting(pico_ipv6_linklocal_get(key->dev), gw);
180                    gw = pico_ipv6_gateway_by_dev_next(key->dev, gw);
181                }
182            }
183        }
184    }
185
186    (void)pico_timer_add(ONE_MINUTE, ctx_lifetime_check, NULL);
187}
188
189#endif
190
191/*******************************************************************************
192 *  MESH-UNDER ROUTING LAYER
193 ******************************************************************************/
194
195/* XXX: Extensible processing function for outgoing frames. Here, the mesh header
196 * for a Mesh-Under topology can be prepended and the link layer source and
197 * destination addresses can be updated */
198static int32_t
199ll_mesh_header_process_in(struct pico_frame *f)
200{
201    IGNORE_PARAMETER(f);
202    return 0;
203}
204
205/* XXX: Extensible processing function for outgoing frames. Here, the mesh header
206 * for a Mesh-Under topology can be prepended and the link layer source and
207 * destination addresses can be updated */
208static int32_t
209ll_mesh_header_process_out(struct pico_frame *f)
210{
211    IGNORE_PARAMETER(f);
212    return 0;
213}
214
215/* XXX: Extensible function that estimates the size of the mesh header to be
216 * prepended based on the frame, the source and destination link layer address */
217static int32_t
218ll_mesh_header_estimator(struct pico_frame *f)
219{
220    IGNORE_PARAMETER(f);
221    return 0;
222}
223
224/*******************************************************************************
225 *  GENERIC 6LOWPAN LINK LAYER
226 ******************************************************************************/
227
228static int32_t
229ll_mac_header_process_in(struct pico_frame *f)
230{
231    if (f && f->dev && pico_6lowpan_lls[f->dev->mode].process_in) {
232        return (int32_t)pico_6lowpan_lls[f->dev->mode].process_in(f);
233    } else {
234        return -1;
235    }
236}
237
238static int32_t
239ll_mac_header_process_out(struct pico_frame *f)
240{
241    if (f && f->dev && pico_6lowpan_lls[f->dev->mode].process_out) {
242        return (int32_t)pico_6lowpan_lls[f->dev->mode].process_out(f);
243    } else {
244        return -1;
245    }
246}
247
248static int32_t
249ll_mac_header_estimator(struct pico_frame *f)
250{
251    if (f && f->dev && pico_6lowpan_lls[f->dev->mode].estimate) {
252        return (int32_t)pico_6lowpan_lls[f->dev->mode].estimate(f);
253    } else {
254        return -1;
255    }
256}
257
258/* Alloc's a frame with device's overhead and maximum IEEE802.15.4 header size */
259static struct pico_frame *
260pico_6lowpan_frame_alloc(struct pico_protocol *self, struct pico_device *dev, uint16_t size)
261{
262    IGNORE_PARAMETER(self);
263    if (dev && pico_6lowpan_lls[dev->mode].alloc) {
264        return pico_6lowpan_lls[dev->mode].alloc(dev, size);
265    } else {
266        return NULL;
267    }
268}
269
270/*******************************************************************************
271 *  6LOWPAN LINK LAYER PROTOCOL
272 ******************************************************************************/
273
274const struct extension exts[] = {
275    {ll_mesh_header_estimator, ll_mesh_header_process_out, ll_mesh_header_process_in},
276    {ll_mac_header_estimator, ll_mac_header_process_out, ll_mac_header_process_in},
277};
278
279static int32_t
280pico_6lowpan_ll_process_out(struct pico_protocol *self, struct pico_frame *f)
281{
282    uint32_t datalink_len = 0;
283    int32_t ret = 0, i = 0;
284    IGNORE_PARAMETER(self);
285
286    /* Every link layer extension updates the datalink pointer of the frame a little bit. */
287    f->datalink_hdr = f->net_hdr;
288
289    /* Call each of the outgoing processing functions */
290    for (i = 0; i < NUM_LL_EXTENSIONS; i++) {
291        ret = exts[i].out(f);
292        if (ret < 0)  /* Processing failed, no way to recover, discard frame */
293            goto fin;
294        datalink_len = (uint32_t)(datalink_len + (uint32_t)ret);
295        if ((f->net_hdr - datalink_len) < f->buffer) /* Before buffer bound check */
296            goto fin;
297    }
298
299    /* Frame is ready for sending to the device driver */
300    f->start = f->datalink_hdr;
301    f->len = (uint32_t)(f->len + datalink_len);
302    return (int32_t)(pico_sendto_dev(f) <= 0);
303fin:
304    pico_frame_discard(f);
305    return -1;
306}
307
308static int32_t
309pico_6lowpan_ll_process_in(struct pico_protocol *self, struct pico_frame *f)
310{
311    int32_t i = 0, ret = 0;
312    uint32_t len = 0;
313    IGNORE_PARAMETER(self);
314
315    /* net_hdr is the pointer that is dynamically updated by the incoming
316     * processing functions to always point to right after a particular
317     * header, whether it's MAC, MESH, LL_SEC, ... eventually net_hdr will
318     * point to 6LoWPAN header which is exactly what we want */
319    f->net_hdr = f->buffer;
320
321    for (i = NUM_LL_EXTENSIONS - 1; i >= 0; i--) {
322        ret = exts[i].in(f);
323        switch (ret) {
324            case FRAME_6LOWPAN_LL_RELEASE:
325                /* Success, frame is somewhere else now.. */
326                break;
327            case FRAME_6LOWPAN_LL_DISCARD:
328                /* Something went wrong, discard the frame */
329                pico_frame_discard(f);
330                break;
331            default:
332                /* Success, update link layer header length */
333                len = (uint32_t)(len + (uint32_t)ret);
334        }
335    }
336
337    /* Determine size at network layer */
338    f->net_len = (uint16_t)(f->len - len);
339    f->len = (uint32_t)(f->len - len);
340    return pico_6lowpan_pull(f);
341}
342
343/* Entry point for incoming 6LoWPAN frames, proxy for pico_stack_recv. This allows passing the link
344 * layer source and destination address as well */
345int32_t pico_6lowpan_stack_recv(struct pico_device *dev, uint8_t *buffer, uint32_t len, union pico_ll_addr *src, union pico_ll_addr *dst)
346{
347    int32_t ret = 0;
348    ll_dbg("6LoWPAN - Stack recv called!\n");
349    if (PICO_DEV_IS_NOMAC(dev)) {
350        struct pico_frame *f = pico_stack_recv_new_frame(dev, buffer, len);
351        if (f) {
352            f->src = *src;
353            f->dst = *dst;
354            ret = pico_enqueue(dev->q_in, f);
355            if (0 >= ret)
356                pico_frame_discard(f);
357            return ret;
358        }
359    } else {
360        return pico_stack_recv(dev, buffer, len);
361    }
362    return -1; // return ERROR
363}
364
365/* Proxy for pico_devloop_sendto_dev, 6LoWPAN-devices have a different interface with pico. This
366 * allows passing the link layer source and destination address as well */
367int32_t pico_6lowpan_ll_sendto_dev(struct pico_device *dev, struct pico_frame *f)
368{
369    /* FINAL OUTGOING POINT OF 6LOWPAN STACK */
370    return ((struct pico_dev_6lowpan *)dev)->send(dev, f->start, (int32_t)f->len, f->src, f->dst);
371}
372
373/* Initialisation routine for 6LoWPAN specific devices */
374int pico_dev_6lowpan_init(struct pico_dev_6lowpan *dev, const char *name, uint8_t *mac, enum pico_ll_mode ll_mode, uint16_t mtu, uint8_t nomac,
375                          int (* send)(struct pico_device *dev, void *_buf, int len, union pico_ll_addr src, union pico_ll_addr dst),
376                          int (* poll)(struct pico_device *dev, int loop_score))
377{
378    struct pico_device *picodev = (struct pico_device *)dev;
379    if (!dev || !send || !poll) {
380        return -1;
381    }
382
383    picodev->mode = ll_mode;
384    picodev->hostvars.lowpan_flags = PICO_6LP_FLAG_LOWPAN;
385    if (nomac) {
386        picodev->hostvars.lowpan_flags |= PICO_6LP_FLAG_NOMAC;
387    }
388    picodev->mtu = mtu;
389    picodev->poll = poll;
390    picodev->send = NULL;
391    dev->send = send;
392
393    return pico_device_init(picodev, name, mac);
394}
395
396
397/* Push function for 6LoWPAN to call when it wants to try to send te frame to the device-driver */
398int32_t
399pico_6lowpan_ll_push(struct pico_frame *f)
400{
401    uint16_t frame_size, pl_available = 0;
402    int32_t i = 0;
403
404    if (!f || !f->dev)
405        return -1;
406    frame_size = (uint16_t)(f->len);
407
408    /* Restrict frames to be as large as the device's MTU. */
409    pl_available = (uint16_t)f->dev->mtu;
410
411    /* Call each of the estimator functions of the additional headers to
412     * determine if the frame fits inside a single 802.15.4 frame, if it doesn't
413     * in the end, return the available bytes */
414    for (i = 0; i < NUM_LL_EXTENSIONS; i++) {
415        pl_available = (uint16_t)(pl_available - exts[i].estimate(f));
416    }
417    if (frame_size > pl_available)
418        return pl_available;
419
420    /* Make sure these addresses are retrievable from the frame on processing */
421    if (pico_enqueue(pico_proto_6lowpan_ll.q_out,f) > 0) {
422        return 0; // Frame enqueued for later processing
423    }
424    return -1; // Return ERROR
425}
426
427struct pico_protocol pico_proto_6lowpan_ll = {
428    .name = "6lowpan_ll",
429    .layer = PICO_LAYER_DATALINK,
430    .alloc = pico_6lowpan_frame_alloc,
431    .process_in = pico_6lowpan_ll_process_in,
432    .process_out = pico_6lowpan_ll_process_out,
433    .q_in = &pico_6lowpan_ll_in,
434    .q_out = &pico_6lowpan_ll_out
435};
436
437void pico_6lowpan_ll_init(void)
438{
439    int32_t i = 0;
440
441#ifdef PICO_6LOWPAN_IPHC_ENABLED
442    /* We don't care about failure */
443    (void)pico_timer_add(60000, ctx_lifetime_check, NULL);
444#endif
445
446    /* Initialize interface with 6LoWPAN link layer protocols */
447    pico_6lowpan_lls[i++] = pico_6lowpan_ll_none;
448
449#ifdef PICO_SUPPORT_802154
450    pico_6lowpan_lls[i++] = pico_6lowpan_ll_802154;
451#endif
452}
453
454#endif /* PICO_SUPPORT_6LOWPAN */
455