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