1/********************************************************************* 2 PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved. 3 See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 for usage. 4 5 Authors: Daniele Lacamera, Kristof Roelants 6 *********************************************************************/ 7 8 9#include "pico_ipv6.h" 10#include "pico_icmp6.h" 11#include "pico_config.h" 12#include "pico_stack.h" 13#include "pico_eth.h" 14#include "pico_udp.h" 15#include "pico_tcp.h" 16#include "pico_socket.h" 17#include "pico_device.h" 18#include "pico_tree.h" 19#include "pico_fragments.h" 20#include "pico_ethernet.h" 21#include "pico_6lowpan_ll.h" 22#include "pico_mld.h" 23#include "pico_mcast.h" 24#ifdef PICO_SUPPORT_IPV6 25 26 27#define PICO_IPV6_EXTHDR_OPT_PAD1 0 28#define PICO_IPV6_EXTHDR_OPT_PADN 1 29#define PICO_IPV6_EXTHDR_OPT_SRCADDR 201 30 31#define PICO_IPV6_EXTHDR_OPT_ACTION_MASK 0xC0 /* highest-order two bits */ 32#define PICO_IPV6_EXTHDR_OPT_ACTION_SKIP 0x00 /* skip and continue processing */ 33#define PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD 0x40 /* discard packet */ 34#define PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SI 0x80 /* discard and send ICMP parameter problem */ 35#define PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SINM 0xC0 /* discard and send ICMP parameter problem if not multicast */ 36 37#define PICO_IPV6_MAX_RTR_SOLICITATION_DELAY 1000 38#define PICO_IPV6_DEFAULT_DAD_RETRANS 1 39 40#ifdef DEBUG_IPV6 41#define ipv6_dbg dbg 42#else 43#define ipv6_dbg(...) do { } while(0) 44#endif 45 46#ifdef PICO_SUPPORT_MCAST 47 48#ifdef DEBUG_MCAST 49#define ipv6_mcast_dbg dbg 50#else 51#define ipv6_mcast_dbg(...) do { } while(0) 52#endif 53 54static struct pico_ipv6_link *mcast_default_link_ipv6 = NULL; 55#endif 56/* queues */ 57static struct pico_queue ipv6_in; 58static struct pico_queue ipv6_out; 59 60const uint8_t PICO_IP6_ANY[PICO_SIZE_IP6] = { 61 0 62}; 63#ifdef PICO_SUPPORT_MCAST 64static int pico_ipv6_mcast_filter(struct pico_frame *f); 65#endif 66 67 68int pico_ipv6_compare(struct pico_ip6 *a, struct pico_ip6 *b) 69{ 70 uint32_t i; 71 for (i = 0; i < sizeof(struct pico_ip6); i++) { 72 if (a->addr[i] < b->addr[i]) 73 return -1; 74 75 if (a->addr[i] > b->addr[i]) 76 return 1; 77 } 78 return 0; 79} 80 81static int ipv6_link_compare(void *ka, void *kb) 82{ 83 struct pico_ipv6_link *a = ka, *b = kb; 84 struct pico_ip6 *a_addr, *b_addr; 85 int ret; 86 a_addr = &a->address; 87 b_addr = &b->address; 88 89 ret = pico_ipv6_compare(a_addr, b_addr); 90 if (ret) 91 return ret; 92 93 /* zero can be assigned multiple times (e.g. for DHCP) */ 94 if (a->dev != NULL && b->dev != NULL && !memcmp(a->address.addr, PICO_IP6_ANY, PICO_SIZE_IP6) && !memcmp(b->address.addr, PICO_IP6_ANY, PICO_SIZE_IP6)) { 95 /* XXX change PICO_IP6_ANY */ 96 if (a->dev < b->dev) 97 return -1; 98 99 if (a->dev > b->dev) 100 return 1; 101 } 102 103 return 0; 104} 105 106static inline int ipv6_compare_metric(struct pico_ipv6_route *a, struct pico_ipv6_route *b) 107{ 108 if (a->metric < b->metric) 109 return -1; 110 111 if (a->metric > b->metric) 112 return 1; 113 114 return 0; 115} 116 117static int ipv6_route_compare(void *ka, void *kb) 118{ 119 struct pico_ipv6_route *a = ka, *b = kb; 120 int ret; 121 122 /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */ 123 ret = pico_ipv6_compare(&a->netmask, &b->netmask); 124 if (ret) 125 return ret; 126 127 ret = pico_ipv6_compare(&a->dest, &b->dest); 128 if (ret) 129 return ret; 130 131 return ipv6_compare_metric(a, b); 132 133} 134 135static PICO_TREE_DECLARE(Tree_dev_ip6_link, ipv6_link_compare); 136PICO_TREE_DECLARE(IPV6Routes, ipv6_route_compare); 137static PICO_TREE_DECLARE(IPV6Links, ipv6_link_compare); 138 139static char pico_ipv6_dec_to_char(uint8_t u) 140{ 141 if (u < 10) 142 return (char)('0' + u); 143 else if (u < 16) 144 return (char)('a' + (u - 10)); 145 else 146 return '0'; 147} 148 149static int pico_ipv6_hex_to_dec(char c) 150{ 151 if (c >= '0' && c <= '9') 152 return c - '0'; 153 154 if (c >= 'a' && c <= 'f') 155 return 10 + (c - 'a'); 156 157 if (c >= 'A' && c <= 'F') 158 return 10 + (c - 'A'); 159 160 return 0; 161} 162 163int pico_ipv6_to_string(char *ipbuf, const uint8_t ip[PICO_SIZE_IP6]) 164{ 165 uint8_t dec = 0, i = 0; 166 167 if (!ipbuf || !ip) { 168 pico_err = PICO_ERR_EINVAL; 169 return -1; 170 } 171 172 /* every nibble is one char */ 173 for (i = 0; i < ((uint8_t)PICO_SIZE_IP6) * 2u; ++i) { 174 if (i % 4 == 0 && i != 0) 175 *ipbuf++ = ':'; 176 177 if (i % 2 == 0) { /* upper nibble */ 178 dec = ip[i / 2] >> 4; 179 } else { /* lower nibble */ 180 dec = ip[i / 2] & 0x0F; 181 } 182 183 *ipbuf++ = pico_ipv6_dec_to_char(dec); 184 } 185 *ipbuf = '\0'; 186 187 return 0; 188} 189 190int pico_string_to_ipv6(const char *ipstr, uint8_t *ip) 191{ 192 uint8_t buf[PICO_SIZE_IP6] = { 193 0 194 }; 195 uint8_t doublecolon = 0, byte = 0; 196 char p = 0; 197 int i = 0, diff = 0, nibble = 0, hex = 0, colons = 0; 198 int zeros = 0, shift = 0; 199 200 pico_err = PICO_ERR_EINVAL; 201 if (!ipstr || !ip) 202 return -1; 203 204 memset(ip, 0, PICO_SIZE_IP6); 205 206 while((p = *ipstr++) != 0) 207 { 208 if (pico_is_hex(p) || (p == ':') || *ipstr == '\0') { /* valid signs */ 209 if (pico_is_hex(p)) { 210 buf[byte] = (uint8_t)((buf[byte] << 4) + pico_ipv6_hex_to_dec(p)); 211 if (++nibble % 2 == 0) 212 ++byte; 213 } 214 215 if (p == ':' || *ipstr == '\0') { /* account for leftout leading zeros */ 216 ++hex; 217 if (p == ':') 218 ++colons; 219 220 diff = (hex * 4) - nibble; 221 nibble += diff; 222 switch (diff) { 223 case 0: 224 /* 16-bit hex block ok f.e. 1db8 */ 225 break; 226 case 1: 227 /* one zero f.e. db8: byte = 1, buf[byte-1] = 0xdb, buf[byte] = 0x08 */ 228 buf[byte] |= (uint8_t)(buf[byte - 1] << 4); 229 buf[byte - 1] >>= 4; 230 byte++; 231 break; 232 case 2: 233 /* two zeros f.e. b8: byte = 1, buf[byte] = 0x00, buf[byte-1] = 0xb8 */ 234 buf[byte] = buf[byte - 1]; 235 buf[byte - 1] = 0x00; 236 byte++; 237 break; 238 case 3: 239 /* three zeros f.e. 8: byte = 0, buf[byte] = 0x08, buf[byte+1] = 0x00 */ 240 buf[byte + 1] = buf[byte]; 241 buf[byte] = 0x00; 242 byte = (uint8_t)(byte + 2); 243 break; 244 case 4: 245 /* case of :: */ 246 if (doublecolon && colons != 2) /* catch case x::x::x but not ::x */ 247 return -1; 248 else 249 doublecolon = byte; 250 251 break; 252 default: 253 /* case of missing colons f.e. 20011db8 instead of 2001:1db8 */ 254 return -1; 255 } 256 } 257 } else { 258 return -1; 259 } 260 } 261 if (colons < 2) /* valid IPv6 has atleast two colons */ 262 return -1; 263 264 /* account for leftout :: zeros */ 265 zeros = PICO_SIZE_IP6 - byte; 266 if (zeros) { 267 shift = PICO_SIZE_IP6 - zeros - doublecolon; 268 for (i = shift; i >= 0; --i) { 269 /* (i-1) as arrays are indexed from 0 onwards */ 270 if ((doublecolon + (i - 1)) >= 0) 271 buf[doublecolon + zeros + (i - 1)] = buf[doublecolon + (i - 1)]; 272 } 273 memset(&buf[doublecolon], 0, (size_t)zeros); 274 } 275 276 memcpy(ip, buf, 16); 277 pico_err = PICO_ERR_NOERR; 278 return 0; 279} 280 281int pico_ipv6_is_linklocal(const uint8_t addr[PICO_SIZE_IP6]) 282{ 283 /* prefix: fe80::/10 */ 284 if ((addr[0] == 0xfe) && ((addr[1] >> 6) == 0x02)) 285 return 1; 286 287 return 0; 288} 289 290int pico_ipv6_is_sitelocal(const uint8_t addr[PICO_SIZE_IP6]) 291{ 292 /* prefix: fec0::/10 */ 293 if ((addr[0] == 0xfe) && ((addr[1] >> 6) == 0x03)) 294 return 1; 295 296 return 0; 297} 298 299int pico_ipv6_is_uniquelocal(const uint8_t addr[PICO_SIZE_IP6]) 300{ 301 /* prefix: fc00::/7 */ 302 if (((addr[0] >> 1) == 0x7e)) 303 return 1; 304 305 return 0; 306} 307 308int pico_ipv6_is_global(const uint8_t addr[PICO_SIZE_IP6]) 309{ 310 /* prefix: 2000::/3 */ 311 if (((addr[0] >> 5) == 0x01)) 312 return 1; 313 314 return 0; 315} 316 317int pico_ipv6_is_localhost(const uint8_t addr[PICO_SIZE_IP6]) 318{ 319 const uint8_t localhost[PICO_SIZE_IP6] = { 320 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 321 }; 322 if (memcmp(addr, localhost, PICO_SIZE_IP6) == 0) 323 return 1; 324 325 return 0; 326 327} 328 329int pico_ipv6_is_unicast(struct pico_ip6 *a) 330{ 331 if (pico_ipv6_is_global(a->addr)) 332 return 1; 333 else if (pico_ipv6_is_uniquelocal(a->addr)) 334 return 1; 335 else if (pico_ipv6_is_sitelocal(a->addr)) 336 return 1; 337 else if (pico_ipv6_is_linklocal(a->addr)) 338 return 1; 339 else if (pico_ipv6_is_localhost(a->addr)) 340 return 1; 341 else if(pico_ipv6_link_get(a)) 342 return 1; 343 else 344 return 0; 345 346} 347 348int pico_ipv6_is_multicast(const uint8_t addr[PICO_SIZE_IP6]) 349{ 350 /* prefix: ff00::/8 */ 351 if ((addr[0] == 0xff)) 352 return 1; 353 354 return 0; 355} 356 357int pico_ipv6_is_allhosts_multicast(const uint8_t addr[PICO_SIZE_IP6]) 358{ 359 struct pico_ip6 allhosts = {{ 0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01 }}; 360 return !memcmp(allhosts.addr, addr, PICO_SIZE_IP6); 361} 362 363int pico_ipv6_is_solicited(const uint8_t addr[PICO_SIZE_IP6]) 364{ 365 struct pico_ip6 solicited_node = {{ 0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0x00, 0x00, 0x00 }}; 366 return !memcmp(solicited_node.addr, addr, 13); 367} 368 369int pico_ipv6_is_solnode_multicast(const uint8_t addr[PICO_SIZE_IP6], struct pico_device *dev) 370{ 371 struct pico_ipv6_link *link; 372 if (pico_ipv6_is_multicast(addr) == 0) 373 return 0; 374 375 link = pico_ipv6_link_by_dev(dev); 376 while(link) { 377 if (pico_ipv6_is_linklocal(link->address.addr)) { 378 int i, match = 0; 379 for(i = 13; i < 16; i++) { 380 if (addr[i] == link->address.addr[i]) 381 ++match; 382 } 383 /* Solicitation: last 3 bytes match a local address. */ 384 if (match == 3) 385 return 1; 386 } 387 388 link = pico_ipv6_link_by_dev_next(dev, link); 389 } 390 return 0; 391} 392 393int pico_ipv6_is_unspecified(const uint8_t addr[PICO_SIZE_IP6]) 394{ 395 return !memcmp(PICO_IP6_ANY, addr, PICO_SIZE_IP6); 396} 397 398static struct pico_ipv6_route *pico_ipv6_route_find(const struct pico_ip6 *addr) 399{ 400 struct pico_tree_node *index = NULL; 401 struct pico_ipv6_route *r = NULL; 402 int i = 0; 403 if (!pico_ipv6_is_localhost(addr->addr) && (pico_ipv6_is_linklocal(addr->addr) || pico_ipv6_is_sitelocal(addr->addr))) { 404 return NULL; 405 } 406 407 pico_tree_foreach_reverse(index, &IPV6Routes) { 408 r = index->keyValue; 409 for (i = 0; i < PICO_SIZE_IP6; ++i) { 410 if ((addr->addr[i] & (r->netmask.addr[i])) != ((r->dest.addr[i]) & (r->netmask.addr[i]))) { 411 break; 412 } 413 414 if (i + 1 == PICO_SIZE_IP6) { 415 return r; 416 } 417 } 418 } 419 return NULL; 420} 421 422struct pico_ip6 *pico_ipv6_source_find(const struct pico_ip6 *dst) 423{ 424 struct pico_ip6 *myself = NULL; 425 struct pico_ipv6_route *rt; 426 427 if(!dst) { 428 pico_err = PICO_ERR_EINVAL; 429 return NULL; 430 } 431 432 rt = pico_ipv6_route_find(dst); 433 if (rt) { 434 myself = &rt->link->address; 435 } else 436 pico_err = PICO_ERR_EHOSTUNREACH; 437 438 return myself; 439} 440 441struct pico_device *pico_ipv6_source_dev_find(const struct pico_ip6 *dst) 442{ 443 struct pico_device *dev = NULL; 444 struct pico_ipv6_route *rt; 445 446 if(!dst) { 447 pico_err = PICO_ERR_EINVAL; 448 return NULL; 449 } 450 451 rt = pico_ipv6_route_find(dst); 452 if (rt && rt->link) { 453 dev = rt->link->dev; 454 } else 455 pico_err = PICO_ERR_EHOSTUNREACH; 456 457 return dev; 458} 459 460static int pico_ipv6_forward_check_dev(struct pico_frame *f) 461{ 462 if(f->dev->mode == LL_MODE_ETHERNET && f->dev->eth != NULL) 463 f->len -= PICO_SIZE_ETHHDR; 464 465 if(f->len > f->dev->mtu) { 466 pico_notify_pkt_too_big(f); 467 return -1; 468 } 469 470 return 0; 471} 472 473static int pico_ipv6_pre_forward_checks(struct pico_frame *f) 474{ 475 struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; 476 477 /* Decrease HOP count, check if expired */ 478 hdr->hop = (uint8_t)(hdr->hop - 1); 479 if (hdr->hop < 1) { 480 pico_notify_ttl_expired(f); 481 dbg(" ------------------- HOP COUNT EXPIRED\n"); 482 return -1; 483 } 484 485 /* If source is local, discard anyway (packets bouncing back and forth) */ 486 if (pico_ipv6_link_get(&hdr->src)) 487 return -1; 488 489 if (pico_ipv6_forward_check_dev(f) < 0) 490 return -1; 491 492 return 0; 493} 494 495static int pico_ipv6_forward(struct pico_frame *f) 496{ 497 struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; 498 struct pico_ipv6_route *rt; 499 if (!hdr) { 500 pico_frame_discard(f); 501 return -1; 502 } 503 504 rt = pico_ipv6_route_find(&hdr->dst); 505 if (!rt) { 506 pico_notify_dest_unreachable(f); 507 pico_frame_discard(f); 508 return -1; 509 } 510 511 f->dev = rt->link->dev; 512 513 if (pico_ipv6_pre_forward_checks(f) < 0) 514 { 515 pico_frame_discard(f); 516 return -1; 517 } 518 519 f->start = f->net_hdr; 520 521 return pico_datalink_send(f); 522} 523 524 525static int pico_ipv6_process_hopbyhop(struct pico_ipv6_exthdr *hbh, struct pico_frame *f) 526{ 527 uint8_t *option = NULL; 528 uint8_t len = 0, optlen = 0; 529 uint32_t ptr = sizeof(struct pico_ipv6_hdr); 530 uint8_t *extensions_start = (uint8_t *)hbh; 531 uint8_t must_align = 1; 532 IGNORE_PARAMETER(f); 533 534 option = ((uint8_t *)&hbh->ext.hopbyhop) + sizeof(struct hopbyhop_s); 535 len = (uint8_t)HBH_LEN(hbh); 536 ipv6_dbg("IPv6: hop by hop extension header length %u\n", len + 2); 537 while (len) { 538 switch (*option) 539 { 540 case PICO_IPV6_EXTHDR_OPT_PAD1: 541 ++option; 542 --len; 543 break; 544 545 case PICO_IPV6_EXTHDR_OPT_PADN: 546 optlen = (uint8_t)((*(option + 1)) + 2); /* plus type and len byte */ 547 option += optlen; 548 len = (uint8_t)(len - optlen); 549 break; 550 case PICO_IPV6_EXTHDR_OPT_ROUTER_ALERT: 551 optlen = (uint8_t)((*(option + 1)) + 2); /* plus type and len byte */ 552 /* MLD package */ 553 if(*(option + 1) == 2) 554 must_align = 0; 555 556 option += optlen; 557 len = (uint8_t)(len - optlen); 558 break; 559 default: 560 /* unknown option */ 561 optlen = (uint8_t)(*(option + 1) + 2); /* plus type and len byte */ 562 switch ((*option) & PICO_IPV6_EXTHDR_OPT_ACTION_MASK) { 563 case PICO_IPV6_EXTHDR_OPT_ACTION_SKIP: 564 break; 565 case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD: 566 return -1; 567 case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SI: 568 pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_IPV6OPT, ptr + (uint32_t)(option - extensions_start)); 569 return -1; 570 case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SINM: 571 if (!pico_ipv6_is_multicast(((struct pico_ipv6_hdr *)(f->net_hdr))->dst.addr)) 572 pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_IPV6OPT, ptr + (uint32_t)(option - extensions_start)); 573 574 return -1; 575 } 576 ipv6_dbg("IPv6: option with type %u and length %u\n", *option, optlen); 577 option += optlen; 578 len = (uint8_t)(len - optlen); 579 } 580 } 581 return must_align; 582} 583 584 585static int pico_ipv6_process_routing(struct pico_ipv6_exthdr *routing, struct pico_frame *f, uint32_t ptr) 586{ 587 IGNORE_PARAMETER(f); 588 589 if (routing->ext.routing.segleft == 0) 590 return 0; 591 592 ipv6_dbg("IPv6: routing extension header with len %u\n", routing->ext.routing.len + 2); 593 switch (routing->ext.routing.routtype) { 594 case 0x00: 595 /* deprecated */ 596 pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_HDRFIELD, ptr + 2); 597 return -1; 598 case 0x02: 599 /* routing type for MIPv6: not supported yet */ 600 break; 601 default: 602 pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_HDRFIELD, ptr + 2); 603 return -1; 604 } 605 return 0; 606} 607 608#define IP6FRAG_MORE(x) ((x & 0x0001)) 609 610static int pico_ipv6_process_destopt(struct pico_ipv6_exthdr *destopt, struct pico_frame *f, uint32_t opt_ptr) 611{ 612 uint8_t *option = NULL; 613 uint8_t len = 0, optlen = 0; 614 opt_ptr += (uint32_t)(2u); /* Skip Dest_opts header */ 615 IGNORE_PARAMETER(f); 616 option = ((uint8_t *)&destopt->ext.destopt) + sizeof(struct destopt_s); 617 len = (uint8_t)(((destopt->ext.destopt.len + 1) << 3) - 2); /* len in bytes, minus nxthdr and len byte */ 618 ipv6_dbg("IPv6: destination option extension header length %u\n", len + 2); 619 while (len) { 620 optlen = (uint8_t)(*(option + 1) + 2); 621 switch (*option) 622 { 623 case PICO_IPV6_EXTHDR_OPT_PAD1: 624 break; 625 626 case PICO_IPV6_EXTHDR_OPT_PADN: 627 break; 628 629 case PICO_IPV6_EXTHDR_OPT_SRCADDR: 630 ipv6_dbg("IPv6: home address option with length %u\n", optlen); 631 break; 632 633 default: 634 ipv6_dbg("IPv6: option with type %u and length %u\n", *option, optlen); 635 switch (*option & PICO_IPV6_EXTHDR_OPT_ACTION_MASK) { 636 case PICO_IPV6_EXTHDR_OPT_ACTION_SKIP: 637 break; 638 case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD: 639 return -1; 640 case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SI: 641 pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_IPV6OPT, opt_ptr); 642 return -1; 643 case PICO_IPV6_EXTHDR_OPT_ACTION_DISCARD_SINM: 644 if (!pico_ipv6_is_multicast(((struct pico_ipv6_hdr *)(f->net_hdr))->dst.addr)) { 645 pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_IPV6OPT, opt_ptr); 646 } 647 648 return -1; 649 } 650 break; 651 } 652 opt_ptr += optlen; 653 option += optlen; 654 len = (uint8_t)(len - optlen); 655 } 656 return 0; 657} 658 659static int pico_ipv6_check_headers_sequence(struct pico_frame *f) 660{ 661 struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; 662 int ptr = sizeof(struct pico_ipv6_hdr); 663 int cur_nexthdr = 6; /* Starts with nexthdr field in ipv6 pkt */ 664 uint8_t nxthdr = hdr->nxthdr; 665 for (;; ) { 666 uint8_t optlen = *(f->net_hdr + ptr + 1); 667 switch (nxthdr) { 668 case PICO_IPV6_EXTHDR_DESTOPT: 669 case PICO_IPV6_EXTHDR_ROUTING: 670 case PICO_IPV6_EXTHDR_HOPBYHOP: 671 case PICO_IPV6_EXTHDR_ESP: 672 case PICO_IPV6_EXTHDR_AUTH: 673 optlen = (uint8_t)IPV6_OPTLEN(optlen); 674 break; 675 case PICO_IPV6_EXTHDR_FRAG: 676 optlen = 8; 677 break; 678 case PICO_IPV6_EXTHDR_NONE: 679 return 0; 680 681 case PICO_PROTO_TCP: 682 case PICO_PROTO_UDP: 683 case PICO_PROTO_ICMP6: 684 return 0; 685 default: 686 /* Invalid next header */ 687 pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_NXTHDR, (uint32_t)cur_nexthdr); 688 return -1; 689 } 690 cur_nexthdr = ptr; 691 nxthdr = *(f->net_hdr + ptr); 692 ptr += optlen; 693 } 694} 695 696static int pico_ipv6_check_aligned(struct pico_frame *f) 697{ 698 struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; 699 if ((short_be(hdr->len) % 8) != 0) { 700 pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_HDRFIELD, 4); 701 return -1; 702 } 703 704 return 0; 705} 706 707static int pico_ipv6_extension_headers(struct pico_frame *f) 708{ 709 struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; 710 uint8_t nxthdr = hdr->nxthdr; 711 struct pico_ipv6_exthdr *exthdr = NULL, *frag_hdr = NULL; 712 uint32_t ptr = sizeof(struct pico_ipv6_hdr); 713 uint16_t cur_optlen; 714 uint32_t cur_nexthdr = 6; 715 int must_align = 0; 716 717 f->net_len = sizeof(struct pico_ipv6_hdr); 718 719 if (pico_ipv6_check_headers_sequence(f) < 0) 720 return -1; 721 722 for (;; ) { 723 exthdr = (struct pico_ipv6_exthdr *)(f->net_hdr + f->net_len); 724 cur_optlen = 0; 725 726 switch (nxthdr) { 727 case PICO_IPV6_EXTHDR_HOPBYHOP: 728 if (cur_nexthdr != 6) { 729 /* The Hop-by-Hop Options header, 730 * when present, must immediately follow the IPv6 header. 731 */ 732 pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_NXTHDR, cur_nexthdr); 733 return -1; 734 } 735 736 cur_optlen = IPV6_OPTLEN(exthdr->ext.hopbyhop.len); 737 f->net_len = (uint16_t) (f->net_len + cur_optlen); 738 must_align = pico_ipv6_process_hopbyhop(exthdr, f); 739 if(must_align < 0) 740 return -1; 741 742 break; 743 case PICO_IPV6_EXTHDR_ROUTING: 744 cur_optlen = IPV6_OPTLEN(exthdr->ext.routing.len); 745 f->net_len = (uint16_t) (f->net_len + cur_optlen); 746 if (pico_ipv6_process_routing(exthdr, f, ptr) < 0) 747 return -1; 748 749 break; 750 case PICO_IPV6_EXTHDR_FRAG: 751 cur_optlen = 8u; 752 f->net_len = (uint16_t) (f->net_len + cur_optlen); 753 frag_hdr = exthdr; 754 f->frag = (uint16_t)((frag_hdr->ext.frag.om[0] << 8) + frag_hdr->ext.frag.om[1]); 755 /* If M-Flag is set, and packet is not 8B aligned, discard and alert */ 756 if (IP6FRAG_MORE(f->frag) && ((short_be(hdr->len) % 8) != 0)) { 757 pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_HDRFIELD, 4); 758 return -1; 759 } 760 761 break; 762 case PICO_IPV6_EXTHDR_DESTOPT: 763 cur_optlen = IPV6_OPTLEN(exthdr->ext.destopt.len); 764 f->net_len = (uint16_t) (f->net_len + cur_optlen); 765 must_align = 1; 766 if (pico_ipv6_process_destopt(exthdr, f, ptr) < 0) 767 return -1; 768 769 break; 770 case PICO_IPV6_EXTHDR_ESP: 771 /* not supported, ignored. */ 772 return 0; 773 case PICO_IPV6_EXTHDR_AUTH: 774 /* not supported, ignored */ 775 return 0; 776 case PICO_IPV6_EXTHDR_NONE: 777 /* no next header */ 778 if (must_align && (pico_ipv6_check_aligned(f) < 0)) 779 return -1; 780 781 return 0; 782 783 case PICO_PROTO_TCP: 784 case PICO_PROTO_UDP: 785 case PICO_PROTO_ICMP6: 786 if (must_align && (pico_ipv6_check_aligned(f) < 0)) 787 return -1; 788 789 f->transport_hdr = f->net_hdr + f->net_len; 790 f->transport_len = (uint16_t)(short_be(hdr->len) - (f->net_len - sizeof(struct pico_ipv6_hdr))); 791 if (frag_hdr) { 792#ifdef PICO_SUPPORT_IPV6FRAG 793 pico_ipv6_process_frag(frag_hdr, f, nxthdr); 794#endif 795 return -1; 796 } else { 797 return nxthdr; 798 } 799 800 default: 801 /* Invalid next header */ 802 pico_icmp6_parameter_problem(f, PICO_ICMP6_PARAMPROB_NXTHDR, cur_nexthdr); 803 return -1; 804 } 805 nxthdr = exthdr->nxthdr; 806 cur_nexthdr = ptr; 807 ptr += cur_optlen; 808 } 809} 810static int pico_ipv6_process_mcast_in(struct pico_frame *f) 811{ 812 struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *) f->net_hdr; 813 struct pico_ipv6_exthdr *hbh = NULL; 814 if (pico_ipv6_is_multicast(hdr->dst.addr)) { 815#ifdef PICO_SUPPORT_MCAST 816 /* Receiving UDP multicast datagram TODO set f->flags? */ 817 if(hdr->nxthdr == 0) { 818 hbh = (struct pico_ipv6_exthdr *) (f->transport_hdr); 819 } 820 821 if (hdr->nxthdr == PICO_PROTO_ICMP6 || (hbh != NULL && hbh->nxthdr == PICO_PROTO_ICMP6)) { 822 pico_transport_receive(f, PICO_PROTO_ICMP6); 823 return 1; 824 } else if ((pico_ipv6_mcast_filter(f) == 0) && (hdr->nxthdr == PICO_PROTO_UDP)) { 825 pico_enqueue(pico_proto_udp.q_in, f); 826 return 1; 827 } 828 829#else 830 IGNORE_PARAMETER(hbh); 831#endif 832 pico_frame_discard(f); 833 return 1; 834 } 835 836 return 0; 837} 838static int pico_ipv6_process_in(struct pico_protocol *self, struct pico_frame *f) 839{ 840 int proto = 0; 841 struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; 842 struct pico_ipv6_exthdr *hbh; 843 IGNORE_PARAMETER(self); 844 /* forward if not local, except if router alert is set */ 845 if (pico_ipv6_is_unicast(&hdr->dst) && !pico_ipv6_link_get(&hdr->dst)) { 846 if(hdr->nxthdr == 0) { 847 hbh = (struct pico_ipv6_exthdr *) f->transport_hdr; 848 if(hbh->ext.routing.routtype == 0) 849 return pico_ipv6_forward(f); 850 } else 851 /* not local, try to forward. */ 852 return pico_ipv6_forward(f); 853 } 854 855 proto = pico_ipv6_extension_headers(f); 856 if (proto <= 0) { 857 pico_frame_discard(f); 858 return 0; 859 } 860 861 f->proto = (uint8_t)proto; 862 ipv6_dbg("IPv6: payload %u net_len %u nxthdr %u\n", short_be(hdr->len), f->net_len, proto); 863 864 if (pico_ipv6_is_unicast(&hdr->dst)) { 865 pico_transport_receive(f, f->proto); 866 } else if (pico_ipv6_is_multicast(hdr->dst.addr)) { 867 /* XXX perform multicast filtering: solicited-node multicast address MUST BE allowed! */ 868 if (pico_ipv6_process_mcast_in(f) > 0) 869 return 0; 870 871 pico_transport_receive(f, f->proto); 872 } 873 874 return 0; 875} 876 877static int pico_ipv6_process_out(struct pico_protocol *self, struct pico_frame *f) 878{ 879 IGNORE_PARAMETER(self); 880 881 f->start = (uint8_t*)f->net_hdr; 882 883 return pico_datalink_send(f); 884} 885 886/* allocates an IPv6 packet without extension headers. If extension headers are needed, 887 * include the len of the extension headers in the size parameter. Once a frame acquired 888 * increment net_len and transport_hdr with the len of the extension headers, decrement 889 * transport_len with this value. 890 */ 891static struct pico_frame *pico_ipv6_alloc(struct pico_protocol *self, struct pico_device *dev, uint16_t size) 892{ 893 struct pico_frame *f = NULL; 894 895 IGNORE_PARAMETER(self); 896 897 if (0) {} 898#ifdef PICO_SUPPORT_6LOWPAN 899 else if (PICO_DEV_IS_6LOWPAN(dev)) { 900 f = pico_proto_6lowpan_ll.alloc(&pico_proto_6lowpan_ll, dev, (uint16_t)(size + PICO_SIZE_IP6HDR)); 901 } 902#endif 903 else { 904#ifdef PICO_SUPPORT_ETH 905 f = pico_proto_ethernet.alloc(&pico_proto_ethernet, dev, (uint16_t)(size + PICO_SIZE_IP6HDR)); 906#else 907 f = pico_frame_alloc(size + PICO_SIZE_IP6HDR + PICO_SIZE_ETHHDR); 908#endif 909 } 910 911 if (!f) 912 return NULL; 913 914 f->net_len = PICO_SIZE_IP6HDR; 915 f->transport_hdr = f->net_hdr + PICO_SIZE_IP6HDR; 916 f->transport_len = (uint16_t)size; 917 918 /* Datalink size is accounted for in pico_datalink_send (link layer) */ 919 f->len = (uint32_t)(size + PICO_SIZE_IP6HDR); 920 921 return f; 922} 923 924static inline int ipv6_pushed_frame_valid(struct pico_frame *f, struct pico_ip6 *dst) 925{ 926 struct pico_ipv6_hdr *hdr = NULL; 927 if(!f || !dst) 928 return -1; 929 930 hdr = (struct pico_ipv6_hdr *)f->net_hdr; 931 if (!hdr) { 932 dbg("IPv6: IP header error\n"); 933 return -1; 934 } 935 936 return 0; 937} 938int pico_ipv6_is_null_address(struct pico_ip6 *ip6) 939{ 940 struct pico_ip6 null_addr = {{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }}; 941 return !memcmp(ip6, &null_addr, sizeof(struct pico_ip6)); 942} 943#ifdef PICO_SUPPORT_MCAST 944/* link 945 * | 946 * MCASTGroups 947 * | | | 948 * ------------ | ------------ 949 * | | | 950 * MCASTSources MCASTSources MCASTSources 951 * | | | | | | | | | | | | 952 * S S S S S S S S S S S S 953 * 954 * MCASTGroups: RBTree(mcast_group) 955 * MCASTSources: RBTree(source) 956 */ 957static int ipv6_mcast_groups_cmp(void *ka, void *kb) 958{ 959 struct pico_mcast_group *a = ka, *b = kb; 960 return pico_ipv6_compare(&a->mcast_addr.ip6, &b->mcast_addr.ip6); 961} 962static int ipv6_mcast_sources_cmp(void *ka, void *kb) 963{ 964 struct pico_ip6 *a = ka, *b = kb; 965 return pico_ipv6_compare(a, b); 966} 967 968static void pico_ipv6_mcast_print_groups(struct pico_ipv6_link *mcast_link) 969{ 970#ifdef PICO_DEBUG_MULTICAST 971 uint16_t i = 0; 972 struct pico_mcast_group *g = NULL; 973 struct pico_ip6 *source = NULL; 974 struct pico_tree_node *index = NULL, *index2 = NULL; 975 char *ipv6_addr; 976 (void) source; 977 ipv6_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); 978 ipv6_mcast_dbg("+ MULTICAST list interface %-16s +\n", mcast_link->dev->name); 979 ipv6_mcast_dbg("+------------------------------------------------------------------------------------------+\n"); 980 ipv6_mcast_dbg("+ nr | interface | host group | reference count | filter mode | source +\n"); 981 ipv6_mcast_dbg("+------------------------------------------------------------------------------------------+\n"); 982 ipv6_addr = PICO_ZALLOC(PICO_IPV6_STRING); 983 pico_tree_foreach(index, mcast_link->MCASTGroups) { 984 g = index->keyValue; 985 pico_ipv6_to_string(ipv6_addr, &g->mcast_addr.addr[0]); 986 ipv6_mcast_dbg("+ %04d | %16s | %s | %05u | %u | %8s +\n", i, mcast_link->dev->name, ipv6_addr, g->reference_count, g->filter_mode, ""); 987 pico_tree_foreach(index2, &g->MCASTSources) { 988 source = index2->keyValue; 989 pico_ipv6_to_string(ipv6_addr, source->addr); 990 ipv6_mcast_dbg("+ %4s | %16s | %8s | %5s | %s | %s +\n", "", "", "", "", "", ipv6_addr); 991 } 992 i++; 993 } 994 PICO_FREE(ipv6_addr); 995 ipv6_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); 996#else 997 IGNORE_PARAMETER(mcast_link); 998#endif 999 1000} 1001 1002static int mcast_group_update_ipv6(struct pico_mcast_group *g, struct pico_tree *_MCASTFilter, uint8_t filter_mode) 1003{ 1004 struct pico_tree_node *index = NULL, *_tmp = NULL; 1005 struct pico_ip6 *source = NULL; 1006 /* cleanup filter */ 1007 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) { 1008 source = index->keyValue; 1009 pico_tree_delete(&g->MCASTSources, source); 1010 PICO_FREE(source); 1011 } 1012 /* insert new filter */ 1013 if (_MCASTFilter) { 1014 pico_tree_foreach(index, _MCASTFilter) { 1015 if (index->keyValue) { 1016 source = PICO_ZALLOC(sizeof(struct pico_ip6)); 1017 if (!source) { 1018 pico_err = PICO_ERR_ENOMEM; 1019 return -1; 1020 } 1021 *source = *((struct pico_ip6 *)index->keyValue); 1022 if (pico_tree_insert(&g->MCASTSources, source)) { 1023 ipv6_mcast_dbg("IPv6 MCAST: Failed to insert source in tree\n"); 1024 PICO_FREE(source); 1025 return -1; 1026 } 1027 } 1028 } 1029 } 1030 g->filter_mode = filter_mode; 1031 return 0; 1032} 1033 1034int pico_ipv6_mcast_join(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *_MCASTFilter) 1035{ 1036 struct pico_mcast_group *g = NULL, test = { 1037 0 1038 }; 1039 struct pico_ipv6_link *link = NULL; 1040 int res = -1; 1041 if (mcast_link) { 1042 link = pico_ipv6_link_get(mcast_link); 1043 } 1044 1045 if (!link) { 1046 link = mcast_default_link_ipv6; 1047 } 1048 1049 test.mcast_addr.ip6 = *mcast_group; 1050 g = pico_tree_findKey(link->MCASTGroups, &test); 1051 if (g) { 1052 if (reference_count) 1053 g->reference_count++; 1054 1055#ifdef PICO_SUPPORT_MLD 1056 res = pico_mld_state_change(mcast_link, mcast_group, filter_mode, _MCASTFilter, PICO_MLD_STATE_UPDATE); 1057#endif 1058 } else { 1059 g = PICO_ZALLOC(sizeof(struct pico_mcast_group)); 1060 if (!g) { 1061 pico_err = PICO_ERR_ENOMEM; 1062 return -1; 1063 } 1064 1065 /* "non-existent" state of filter mode INCLUDE and empty source list */ 1066 g->filter_mode = PICO_IP_MULTICAST_INCLUDE; 1067 g->reference_count = 1; 1068 g->mcast_addr.ip6 = *mcast_group; 1069 g->MCASTSources.root = &LEAF; 1070 g->MCASTSources.compare = ipv6_mcast_sources_cmp; 1071 if (pico_tree_insert(link->MCASTGroups, g)) { 1072 ipv6_mcast_dbg("IPv6 MCAST: Failed to insert group in tree\n"); 1073 PICO_FREE(g); 1074 return -1; 1075 } 1076 1077#ifdef PICO_SUPPORT_MLD 1078 res = pico_mld_state_change(mcast_link, mcast_group, filter_mode, _MCASTFilter, PICO_MLD_STATE_CREATE); 1079#endif 1080 } 1081 1082 if (mcast_group_update_ipv6(g, _MCASTFilter, filter_mode) < 0) { 1083 dbg("Error in mcast_group update\n"); 1084 return -1; 1085 } 1086 1087 pico_ipv6_mcast_print_groups(link); 1088 return res; 1089} 1090 1091int pico_ipv6_mcast_leave(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *_MCASTFilter) 1092{ 1093 struct pico_mcast_group *g = NULL, test = { 1094 0 1095 }; 1096 struct pico_ipv6_link *link = NULL; 1097 struct pico_tree_node *index = NULL, *_tmp = NULL; 1098 struct pico_ip6 *source = NULL; 1099 int res = -1; 1100 if (mcast_link) 1101 link = pico_ipv6_link_get(mcast_link); 1102 1103 if (!link) 1104 link = mcast_default_link_ipv6; 1105 1106 test.mcast_addr.ip6 = *mcast_group; 1107 g = pico_tree_findKey(link->MCASTGroups, &test); 1108 if (!g) { 1109 pico_err = PICO_ERR_EINVAL; 1110 return -1; 1111 } else { 1112 if (reference_count && (--(g->reference_count) < 1)) { 1113#ifdef PICO_SUPPORT_MLD 1114 res = pico_mld_state_change(mcast_link, mcast_group, filter_mode, _MCASTFilter, PICO_MLD_STATE_DELETE); 1115#endif 1116 /* cleanup filter */ 1117 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) { 1118 source = index->keyValue; 1119 pico_tree_delete(&g->MCASTSources, source); 1120 PICO_FREE(source); 1121 } 1122 pico_tree_delete(link->MCASTGroups, g); 1123 PICO_FREE(g); 1124 } else { 1125#ifdef PICO_SUPPORT_MLD 1126 res = pico_mld_state_change(mcast_link, mcast_group, filter_mode, _MCASTFilter, PICO_MLD_STATE_UPDATE); 1127#endif 1128 if (mcast_group_update_ipv6(g, _MCASTFilter, filter_mode) < 0) 1129 return -1; 1130 } 1131 } 1132 1133 pico_ipv6_mcast_print_groups(link); 1134 return res; 1135} 1136 1137struct pico_ipv6_link *pico_ipv6_get_default_mcastlink(void) 1138{ 1139 return mcast_default_link_ipv6; 1140} 1141 1142static int pico_ipv6_mcast_filter(struct pico_frame *f) 1143{ 1144 struct pico_ipv6_link *link = NULL; 1145 struct pico_tree_node *index = NULL, *index2 = NULL; 1146 struct pico_mcast_group *g = NULL, test = { 1147 0 1148 }; 1149 struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *) f->net_hdr; 1150#ifdef PICO_DEBUG_MULTICAST 1151 char ipv6_addr[PICO_IPV6_STRING]; 1152#endif 1153 test.mcast_addr.ip6 = hdr->dst; 1154 1155 pico_tree_foreach(index, &Tree_dev_ip6_link) { 1156 link = index->keyValue; 1157 g = pico_tree_findKey(link->MCASTGroups, &test); 1158 if (g) { 1159 if (f->dev == link->dev) { 1160#ifdef PICO_DEBUG_MULTICAST 1161 pico_ipv6_to_string( ipv6_addr, &hdr->dst.addr[0]); 1162 ipv6_mcast_dbg("MCAST: IP %s is group member of current link %s\n", ipv6_addr, f->dev->name); 1163#endif 1164 /* perform source filtering */ 1165 switch (g->filter_mode) { 1166 case PICO_IP_MULTICAST_INCLUDE: 1167 pico_tree_foreach(index2, &g->MCASTSources) { 1168 if (hdr->src.addr == ((struct pico_ip6 *)index2->keyValue)->addr) { 1169#ifdef PICO_DEBUG_MULTICAST 1170 pico_ipv6_to_string(ipv6_addr, &hdr->src.addr[0]); 1171 ipv6_mcast_dbg("MCAST: IP %s in included interface source list\n", ipv6_addr); 1172#endif 1173 return 0; 1174 } 1175 } 1176#ifdef PICO_DEBUG_MULTICAST 1177 pico_ipv6_to_string(ipv6_addr, &hdr->src.addr[0]); 1178 ipv6_mcast_dbg("MCAST: IP %s NOT in included interface source list\n", ipv6_addr); 1179#endif 1180 return -1; 1181 1182 case PICO_IP_MULTICAST_EXCLUDE: 1183 pico_tree_foreach(index2, &g->MCASTSources) { 1184 if (memcmp(hdr->src.addr, (((struct pico_ip6 *)index2->keyValue)->addr), sizeof(struct pico_ip6)) == 0) { 1185#ifdef PICO_DEBUG_MULTICAST 1186 pico_ipv6_to_string(ipv6_addr, &hdr->src.addr[0]); 1187 ipv6_mcast_dbg("MCAST: IP %s in excluded interface source list\n", ipv6_addr); 1188#endif 1189 return -1; 1190 } 1191 } 1192#ifdef PICO_DEBUG_MULTICAST 1193 pico_ipv6_to_string(ipv6_addr, &hdr->src.addr[0]); 1194 ipv6_mcast_dbg("MCAST: IP %s NOT in excluded interface source list\n", ipv6_addr); 1195#endif 1196 return 0; 1197 1198 default: 1199 return -1; 1200 } 1201 } else { 1202#ifdef PICO_DEBUG_MULTICAST 1203 pico_ipv6_to_string(ipv6_addr, &hdr->dst.addr[0]); 1204 ipv6_mcast_dbg("MCAST: IP %s is group member of different link %s\n", ipv6_addr, link->dev->name); 1205#endif 1206 } 1207 } else { 1208#ifdef PICO_DEBUG_MULTICAST 1209 pico_ipv6_to_string(ipv6_addr, &hdr->dst.addr[0]); 1210 ipv6_mcast_dbg("MCAST: IP %s is not a group member of link %s\n", ipv6_addr, f->dev->name); 1211#endif 1212 } 1213 } 1214 return -1; 1215} 1216 1217#else 1218 1219int pico_ipv6_mcast_join(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *_MCASTFilter) 1220{ 1221 IGNORE_PARAMETER(mcast_link); 1222 IGNORE_PARAMETER(mcast_group); 1223 IGNORE_PARAMETER(reference_count); 1224 IGNORE_PARAMETER(filter_mode); 1225 IGNORE_PARAMETER(_MCASTFilter); 1226 pico_err = PICO_ERR_EPROTONOSUPPORT; 1227 return -1; 1228} 1229 1230int pico_ipv6_mcast_leave(struct pico_ip6 *mcast_link, struct pico_ip6 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *_MCASTFilter) 1231{ 1232 IGNORE_PARAMETER(mcast_link); 1233 IGNORE_PARAMETER(mcast_group); 1234 IGNORE_PARAMETER(reference_count); 1235 IGNORE_PARAMETER(filter_mode); 1236 IGNORE_PARAMETER(_MCASTFilter); 1237 pico_err = PICO_ERR_EPROTONOSUPPORT; 1238 return -1; 1239} 1240 1241struct pico_ipv6_link *pico_ipv6_get_default_mcastlink(void) 1242{ 1243 pico_err = PICO_ERR_EPROTONOSUPPORT; 1244 return NULL; 1245} 1246#endif /* PICO_SUPPORT_MCAST */ 1247static inline struct pico_ipv6_route *ipv6_pushed_frame_checks(struct pico_frame *f, struct pico_ip6 *dst) 1248{ 1249 struct pico_ipv6_route *route = NULL; 1250 1251 if (ipv6_pushed_frame_valid(f, dst) < 0) 1252 return NULL; 1253 1254 if (memcmp(dst->addr, PICO_IP6_ANY, PICO_SIZE_IP6) == 0) { 1255 dbg("IPv6: IP destination address error\n"); 1256 return NULL; 1257 } 1258 1259 route = pico_ipv6_route_find(dst); 1260 if (!route && !f->dev) { 1261 dbg("IPv6: route not found.\n"); 1262 pico_err = PICO_ERR_EHOSTUNREACH; 1263 return NULL; 1264 } 1265 1266 return route; 1267} 1268 1269static inline void ipv6_push_hdr_adjust(struct pico_frame *f, struct pico_ipv6_link *link, struct pico_ip6 *src, struct pico_ip6 *dst, uint8_t proto, int is_dad) 1270{ 1271 struct pico_icmp6_hdr *icmp6_hdr = NULL; 1272 struct pico_ipv6_hdr *hdr = NULL; 1273 struct pico_ipv6_exthdr *hbh = NULL; 1274 const uint8_t vtf = (uint8_t)long_be(0x60000000); /* version 6, traffic class 0, flow label 0 */ 1275 1276 hdr = (struct pico_ipv6_hdr *)f->net_hdr; 1277 hdr->vtf = vtf; 1278 hdr->len = short_be((uint16_t)(f->transport_len + f->net_len - (uint16_t)sizeof(struct pico_ipv6_hdr))); 1279 hdr->nxthdr = proto; 1280 hdr->hop = f->dev->hostvars.hoplimit; 1281 hdr->dst = *dst; 1282 1283 if (!src || !pico_ipv6_is_unicast(src)) 1284 /* Address defaults to the link information: src address selection is done via link */ 1285 hdr->src = link->address; 1286 else { 1287 /* Sender protocol is forcing an IPv6 address */ 1288 hdr->src = *src; 1289 } 1290 1291 if (f->send_ttl) { 1292 hdr->hop = f->send_ttl; 1293 } 1294 1295 if (f->send_tos) { 1296 hdr->vtf |= ((uint32_t)f->send_tos << 20u); 1297 } 1298 1299 /* make adjustments to defaults according to proto */ 1300 switch (proto) 1301 { 1302#ifdef PICO_SUPPORT_MLD 1303 case 0: 1304 { 1305 hbh = (struct pico_ipv6_exthdr *) f->transport_hdr; 1306 switch(hbh->nxthdr) { 1307 case PICO_PROTO_ICMP6: 1308 { 1309 icmp6_hdr = (struct pico_icmp6_hdr *)(f->transport_hdr + sizeof(struct pico_ipv6_exthdr)); 1310 if((icmp6_hdr->type >= PICO_MLD_QUERY && icmp6_hdr->type <= PICO_MLD_DONE) || icmp6_hdr->type == PICO_MLD_REPORTV2) { 1311 hdr->hop = 1; 1312 } 1313 1314 icmp6_hdr->crc = 0; 1315 icmp6_hdr->crc = short_be(pico_mld_checksum(f)); 1316 break; 1317 } 1318 } 1319 break; 1320 } 1321#else 1322 IGNORE_PARAMETER(hbh); 1323#endif 1324 case PICO_PROTO_ICMP6: 1325 { 1326 icmp6_hdr = (struct pico_icmp6_hdr *)f->transport_hdr; 1327 if (icmp6_hdr->type == PICO_ICMP6_NEIGH_SOL || icmp6_hdr->type == PICO_ICMP6_NEIGH_ADV || icmp6_hdr->type == PICO_ICMP6_ROUTER_SOL || icmp6_hdr->type == PICO_ICMP6_ROUTER_ADV) 1328 hdr->hop = 255; 1329 1330 /* RFC6775 $5.5.1: 1331 * ... An unspecified source address MUST NOT be used in NS messages. 1332 */ 1333 if (f->dev->mode == LL_MODE_ETHERNET && (is_dad || link->istentative) && icmp6_hdr->type == PICO_ICMP6_NEIGH_SOL) { 1334 memcpy(hdr->src.addr, PICO_IP6_ANY, PICO_SIZE_IP6); 1335 } 1336 1337 icmp6_hdr->crc = 0; 1338 icmp6_hdr->crc = short_be(pico_icmp6_checksum(f)); 1339 break; 1340 } 1341#ifdef PICO_SUPPORT_UDP 1342 case PICO_PROTO_UDP: 1343 { 1344 struct pico_udp_hdr *udp_hdr = (struct pico_udp_hdr *) f->transport_hdr; 1345 udp_hdr->crc = short_be(pico_udp_checksum_ipv6(f)); 1346 break; 1347 } 1348#endif 1349 1350 default: 1351 break; 1352 } 1353 1354} 1355 1356static int ipv6_frame_push_final(struct pico_frame *f) 1357{ 1358 struct pico_ipv6_hdr *hdr = NULL; 1359 hdr = (struct pico_ipv6_hdr *)f->net_hdr; 1360 if(pico_ipv6_link_get(&hdr->dst)) { 1361 return pico_enqueue(&ipv6_in, f); 1362 } 1363 else { 1364 return pico_enqueue(&ipv6_out, f); 1365 } 1366} 1367 1368struct pico_ipv6_link *pico_ipv6_linklocal_get(struct pico_device *dev); 1369 1370int pico_ipv6_frame_push(struct pico_frame *f, struct pico_ip6 *src, struct pico_ip6 *dst, uint8_t proto, int is_dad) 1371{ 1372 struct pico_ipv6_route *route = NULL; 1373 struct pico_ipv6_link *link = NULL; 1374 1375 if (dst && (pico_ipv6_is_linklocal(dst->addr) || pico_ipv6_is_multicast(dst->addr) || pico_ipv6_is_sitelocal(dst->addr))) { 1376 if (!f->dev) { 1377 pico_frame_discard(f); 1378 return -1; 1379 } 1380 1381 if (pico_ipv6_is_sitelocal(dst->addr)) 1382 link = pico_ipv6_sitelocal_get(f->dev); 1383 else 1384 link = pico_ipv6_linklocal_get(f->dev); 1385 1386 if (link) 1387 goto push_final; 1388 } 1389 1390 if (pico_ipv6_is_localhost(dst->addr)) { 1391 f->dev = pico_get_device("loop"); 1392 } 1393 1394 route = ipv6_pushed_frame_checks(f, dst); 1395 if (!route) { 1396 pico_frame_discard(f); 1397 return -1; 1398 } 1399 1400 link = route->link; 1401 1402 if (f->sock && f->sock->dev) 1403 f->dev = f->sock->dev; 1404 else { 1405 f->dev = link->dev; 1406 if (f->sock) 1407 f->sock->dev = f->dev; 1408 } 1409 1410 1411 #if 0 1412 if (pico_ipv6_is_multicast(hdr->dst.addr)) { 1413 /* XXX: reimplement loopback */ 1414 } 1415 1416 #endif 1417 1418push_final: 1419 ipv6_push_hdr_adjust(f, link, src, dst, proto, is_dad); 1420 return ipv6_frame_push_final(f); 1421} 1422 1423static int pico_ipv6_frame_sock_push(struct pico_protocol *self, struct pico_frame *f) 1424{ 1425 struct pico_ip6 *dst = NULL; 1426 struct pico_remote_endpoint *remote_endpoint = NULL; 1427 1428 IGNORE_PARAMETER(self); 1429 1430 if (!f->sock) { 1431 pico_frame_discard(f); 1432 return -1; 1433 } 1434 1435 remote_endpoint = (struct pico_remote_endpoint *)f->info; 1436 if (remote_endpoint) { 1437 dst = &remote_endpoint->remote_addr.ip6; 1438 } else { 1439 dst = &f->sock->remote_addr.ip6; 1440 } 1441 1442 return pico_ipv6_frame_push(f, NULL, dst, (uint8_t)f->sock->proto->proto_number, 0); 1443} 1444 1445/* interface: protocol definition */ 1446struct pico_protocol pico_proto_ipv6 = { 1447 .name = "ipv6", 1448 .proto_number = PICO_PROTO_IPV6, 1449 .layer = PICO_LAYER_NETWORK, 1450 .alloc = pico_ipv6_alloc, 1451 .process_in = pico_ipv6_process_in, 1452 .process_out = pico_ipv6_process_out, 1453 .push = pico_ipv6_frame_sock_push, 1454 .q_in = &ipv6_in, 1455 .q_out = &ipv6_out, 1456}; 1457 1458#ifdef DEBUG_IPV6_ROUTE 1459static void pico_ipv6_dbg_route(void) 1460{ 1461 struct pico_ipv6_route *r; 1462 struct pico_tree_node *index; 1463 char ipv6_addr[PICO_IPV6_STRING]; 1464 char netmask_addr[PICO_IPV6_STRING]; 1465 char gateway_addr[PICO_IPV6_STRING]; 1466 1467 pico_tree_foreach(index, &Routes){ 1468 r = index->keyValue; 1469 pico_ipv6_to_string(ipv6_addr, r->dest.addr); 1470 pico_ipv6_to_string(netmask_addr, r->netmask.addr); 1471 pico_ipv6_to_string(gateway_addr, r->gateway.addr); 1472 dbg("Route to %s/%s, gw %s, dev: %s, metric: %d\n", ipv6_addr, netmask_addr, gateway_addr, r->link->dev->name, r->metric); 1473 } 1474} 1475#else 1476#define pico_ipv6_dbg_route() do { } while(0) 1477#endif 1478 1479static inline struct pico_ipv6_route *ipv6_route_add_link(struct pico_ip6 gateway) 1480{ 1481 struct pico_ip6 zerogateway = {{0}}; 1482 struct pico_ipv6_route *r = pico_ipv6_route_find(&gateway); 1483 1484 if (!r) { /* Specified Gateway is unreachable */ 1485 pico_err = PICO_ERR_EHOSTUNREACH; 1486 return NULL; 1487 } 1488 1489 if (memcmp(r->gateway.addr, zerogateway.addr, PICO_SIZE_IP6) != 0) { /* Specified Gateway is not a neighbor */ 1490 pico_err = PICO_ERR_ENETUNREACH; 1491 return NULL; 1492 } 1493 1494 return r; 1495} 1496 1497struct pico_ipv6_route *pico_ipv6_gateway_by_dev(struct pico_device *dev) 1498{ 1499 struct pico_ipv6_link *link = pico_ipv6_link_by_dev(dev); 1500 struct pico_ipv6_route *route = NULL; 1501 struct pico_tree_node *node = NULL; 1502 1503 /* Iterate over the IPv6-routes */ 1504 pico_tree_foreach(node, &IPV6Routes) { 1505 route = (struct pico_ipv6_route *)node->keyValue; 1506 /* If the route is a default router, specified by the gw being set */ 1507 if (!pico_ipv6_is_unspecified(route->gateway.addr) && pico_ipv6_is_unspecified(route->netmask.addr)) { 1508 /* Iterate over device's links */ 1509 while (link) { 1510 /* If link is equal to route's link, router list is not empty */ 1511 if (0 == ipv6_link_compare(link, route->link)) 1512 return route; 1513 link = pico_ipv6_link_by_dev_next(dev, link); 1514 } 1515 } 1516 } 1517 1518 return NULL; 1519} 1520 1521struct pico_ipv6_route *pico_ipv6_gateway_by_dev_next(struct pico_device *dev, struct pico_ipv6_route *last) 1522{ 1523 struct pico_ipv6_link *link = NULL; 1524 struct pico_ipv6_route *gw = NULL; 1525 struct pico_tree_node *i = NULL; 1526 int valid = 0; 1527 1528 if (last == NULL) 1529 valid = 1; 1530 1531 pico_tree_foreach(i, &IPV6Routes) { 1532 gw = (struct pico_ipv6_route *)i->keyValue; 1533 /* If the route is a default router, specified by the gw being set */ 1534 if (!pico_ipv6_is_unspecified(gw->gateway.addr) && pico_ipv6_is_unspecified(gw->netmask.addr)) { 1535 /* Iterate over device's links */ 1536 link = pico_ipv6_link_by_dev(dev); 1537 while (link) { 1538 /* If link is equal to route's link, routing list is not empty */ 1539 if (0 == ipv6_link_compare(link, gw->link)) { 1540 if (last == gw) { 1541 valid = 1; 1542 } else if (valid) { 1543 return gw; 1544 } 1545 link = pico_ipv6_link_by_dev_next(dev, link); 1546 } 1547 } 1548 } 1549 } 1550 return NULL; 1551} 1552 1553int pico_ipv6_route_add(struct pico_ip6 address, struct pico_ip6 netmask, struct pico_ip6 gateway, int metric, struct pico_ipv6_link *link) 1554{ 1555 struct pico_ip6 zerogateway = {{0}}; 1556 struct pico_ipv6_route test, *new = NULL; 1557 test.dest = address; 1558 test.netmask = netmask; 1559 test.metric = (uint32_t)metric; 1560 if (pico_tree_findKey(&IPV6Routes, &test)) { 1561 /* Route already exists */ 1562 pico_err = PICO_ERR_EINVAL; 1563 return -1; 1564 } 1565 1566 new = PICO_ZALLOC(sizeof(struct pico_ipv6_route)); 1567 if (!new) { 1568 pico_err = PICO_ERR_ENOMEM; 1569 return -1; 1570 } 1571 1572 ipv6_dbg("Adding IPV6 static route\n"); 1573 new->dest = address; 1574 new->netmask = netmask; 1575 new->gateway = gateway; 1576 new->metric = (uint32_t)metric; 1577 if (memcmp(gateway.addr, zerogateway.addr, PICO_SIZE_IP6) == 0) { 1578 /* No gateway provided, use the link */ 1579 new->link = link; 1580 } else { 1581 struct pico_ipv6_route *r = ipv6_route_add_link(gateway); 1582 if (!r) { 1583 if (link) 1584 new->link = link; 1585 else { 1586 PICO_FREE(new); 1587 return -1; 1588 } 1589 } else { 1590 new->link = r->link; 1591 } 1592 } 1593 1594 if (new->link && (pico_ipv6_is_global(address.addr)) && (!pico_ipv6_is_global(new->link->address.addr))) { 1595 new->link = pico_ipv6_global_get(new->link->dev); 1596 } 1597 1598 if (!new->link) { 1599 pico_err = PICO_ERR_EINVAL; 1600 PICO_FREE(new); 1601 return -1; 1602 } 1603 1604 if (pico_tree_insert(&IPV6Routes, new)) { 1605 ipv6_dbg("IPv6: Failed to insert route in tree\n"); 1606 PICO_FREE(new); 1607 return -1; 1608 } 1609 1610 pico_ipv6_dbg_route(); 1611 return 0; 1612} 1613 1614int pico_ipv6_route_del(struct pico_ip6 address, struct pico_ip6 netmask, struct pico_ip6 gateway, int metric, struct pico_ipv6_link *link) 1615{ 1616 struct pico_ipv6_route test, *found = NULL; 1617 1618 IGNORE_PARAMETER(gateway); 1619 1620 if (!link) { 1621 pico_err = PICO_ERR_EINVAL; 1622 return -1; 1623 } 1624 1625 test.dest = address; 1626 test.netmask = netmask; 1627 test.metric = (uint32_t)metric; 1628 1629 found = pico_tree_findKey(&IPV6Routes, &test); 1630 if (found) { 1631 pico_tree_delete(&IPV6Routes, found); 1632 PICO_FREE(found); 1633 pico_ipv6_dbg_route(); 1634 return 0; 1635 } 1636 1637 pico_err = PICO_ERR_EINVAL; 1638 return -1; 1639} 1640 1641void pico_ipv6_router_down(struct pico_ip6 *address) 1642{ 1643 struct pico_tree_node *index = NULL, *_tmp = NULL; 1644 struct pico_ipv6_route *route = NULL; 1645 if (!address) 1646 return; 1647 1648 pico_tree_foreach_safe(index, &IPV6Routes, _tmp) 1649 { 1650 route = index->keyValue; 1651 if (pico_ipv6_compare(address, &route->gateway) == 0) 1652 pico_ipv6_route_del(route->dest, route->netmask, route->gateway, (int)route->metric, route->link); 1653 } 1654} 1655 1656#ifndef UNIT_TEST 1657static void pico_ipv6_nd_dad(pico_time now, void *arg) 1658{ 1659 struct pico_ip6 *address = (struct pico_ip6 *)arg; 1660 struct pico_ipv6_link *l = NULL; 1661 struct pico_ip6 old_address; 1662 if (!arg) 1663 return; 1664 1665 IGNORE_PARAMETER(now); 1666 1667 l = pico_ipv6_link_istentative(address); 1668 if (!l) 1669 return; 1670 1671 if (pico_device_link_state(l->dev) == 0) { 1672 l->dad_timer = pico_timer_add(100, pico_ipv6_nd_dad, &l->address); 1673 if (!l->dad_timer) { 1674 dbg("IPv6: Failed to start nd_dad timer\n"); 1675 /* TODO does this have disastrous consequences? */ 1676 } 1677 return; 1678 } 1679 1680 if (l->isduplicate) { 1681 dbg("IPv6: duplicate address.\n"); 1682 old_address = *address; 1683 if (pico_ipv6_is_linklocal(address->addr)) { 1684 address->addr[8] = (uint8_t)((uint8_t)(pico_rand() & 0xff) & (uint8_t)(~0x03)); 1685 address->addr[9] = pico_rand() & 0xff; 1686 address->addr[10] = pico_rand() & 0xff; 1687 address->addr[11] = pico_rand() & 0xff; 1688 address->addr[12] = pico_rand() & 0xff; 1689 address->addr[13] = pico_rand() & 0xff; 1690 address->addr[14] = pico_rand() & 0xff; 1691 address->addr[15] = pico_rand() & 0xff; 1692 pico_ipv6_link_add(l->dev, *address, l->netmask); 1693 } 1694 1695 pico_ipv6_link_del(l->dev, old_address); 1696 } 1697 else { 1698 if (l->dup_detect_retrans-- == 0) { 1699 dbg("IPv6: DAD verified valid address.\n"); 1700 1701 l->istentative = 0; 1702 } else { 1703 /* Duplicate Address Detection */ 1704 pico_icmp6_neighbor_solicitation(l->dev, &l->address, PICO_ICMP6_ND_DAD, NULL); 1705 l->dad_timer = pico_timer_add(PICO_ICMP6_MAX_RTR_SOL_DELAY, pico_ipv6_nd_dad, &l->address); 1706 if (!l->dad_timer) { 1707 dbg("IPv6: Failed to start nd_dad timer\n"); 1708 /* TODO does this have disastrous consequences? */ 1709 } 1710 } 1711 } 1712} 1713#endif 1714 1715static struct pico_ipv6_link *pico_ipv6_do_link_add(struct pico_device *dev, struct pico_ip6 address, struct pico_ip6 netmask) 1716{ 1717 struct pico_ipv6_link test = { 1718 0 1719 }, *new = NULL; 1720 struct pico_ip6 network = {{0}}, gateway = {{0}}; 1721 struct pico_ip6 mcast_addr = {{ 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }}; 1722 struct pico_ip6 mcast_nm = {{ 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }}; 1723 struct pico_ip6 mcast_gw = {{0}}; 1724 struct pico_ip6 all_hosts = {{ 0xff, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01 }}; 1725 int i = 0; 1726 if (!dev) { 1727 pico_err = PICO_ERR_EINVAL; 1728 return NULL; 1729 } 1730 1731 test.address = address; 1732 test.dev = dev; 1733 /** XXX: Valid netmask / unicast address test **/ 1734 1735 if (pico_tree_findKey(&IPV6Links, &test)) { 1736 dbg("IPv6: trying to assign an invalid address (in use)\n"); 1737 pico_err = PICO_ERR_EADDRINUSE; 1738 return NULL; 1739 } 1740 1741 /** XXX: Check for network already in use (e.g. trying to assign 10.0.0.1/24 where 10.1.0.1/8 is in use) **/ 1742 new = PICO_ZALLOC(sizeof(struct pico_ipv6_link)); 1743 if (!new) { 1744 dbg("IPv6: out of memory!\n"); 1745 pico_err = PICO_ERR_ENOMEM; 1746 return NULL; 1747 } 1748 1749 new->address = address; 1750 new->netmask = netmask; 1751 new->dev = dev; 1752 new->istentative = 1; 1753 new->isduplicate = 0; 1754#ifdef PICO_SUPPORT_MCAST 1755 new->MCASTGroups = PICO_ZALLOC(sizeof(struct pico_tree)); 1756 if (!new->MCASTGroups) { 1757 PICO_FREE(new); 1758 dbg("IPv6: Out of memory!\n"); 1759 pico_err = PICO_ERR_ENOMEM; 1760 return NULL; 1761 } 1762 1763 new->MCASTGroups->root = &LEAF; 1764 new->MCASTGroups->compare = ipv6_mcast_groups_cmp; 1765#ifdef PICO_SUPPORT_MLD 1766 new->mcast_compatibility = PICO_MLDV2; 1767 new->mcast_last_query_interval = MLD_QUERY_INTERVAL; 1768#endif 1769#endif 1770 if (pico_tree_insert(&IPV6Links, new)) { 1771 ipv6_dbg("IPv6: Failed to insert link in tree\n"); 1772#ifdef PICO_SUPPORT_MCAST 1773 PICO_FREE(new->MCASTGroups); 1774#endif 1775 PICO_FREE(new); 1776 return NULL; 1777 } 1778 for (i = 0; i < PICO_SIZE_IP6; ++i) { 1779 network.addr[i] = address.addr[i] & netmask.addr[i]; 1780 } 1781#ifdef PICO_SUPPORT_MCAST 1782 do { 1783 if (!mcast_default_link_ipv6) { 1784 mcast_default_link_ipv6 = new; 1785 pico_ipv6_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new); 1786 } 1787 1788 pico_ipv6_mcast_join(&address, &all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL); 1789 } while(0); 1790#else 1791 IGNORE_PARAMETER(all_hosts); 1792#endif 1793 pico_ipv6_route_add(network, netmask, gateway, 1, new); 1794#ifdef PICO_SUPPORT_6LOWPAN 1795 if (!PICO_DEV_IS_6LOWPAN(dev)) 1796#endif 1797 pico_ipv6_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new); 1798 /* XXX MUST join the all-nodes multicast address on that interface, as well as 1799 * the solicited-node multicast address corresponding to each of the IP 1800 * addresses assigned to the interface. (RFC 4861 $7.2.1) 1801 * XXX RFC6775 (6LoWPAN): There is no need to join the solicited-node multicast address, since 1802 * nobody multicasts NSs in this type of network. A host MUST join the all-nodes multicast 1803 * address. */ 1804#ifdef PICO_DEBUG_IPV6 1805 pico_ipv6_to_string(ipstr, new->address.addr); 1806 dbg("Assigned ipv6 %s to device %s\n", ipstr, new->dev->name); 1807#endif 1808 return new; 1809} 1810 1811struct pico_ipv6_link *pico_ipv6_link_add_no_dad(struct pico_device *dev, struct pico_ip6 address, struct pico_ip6 netmask) 1812{ 1813 struct pico_ipv6_link *new = pico_ipv6_do_link_add(dev, address, netmask); 1814 if (new) { 1815 new->istentative = 0; 1816 } 1817 return new; 1818} 1819 1820struct pico_ipv6_link *pico_ipv6_link_add(struct pico_device *dev, struct pico_ip6 address, struct pico_ip6 netmask) 1821{ 1822#ifdef DEBUG_IPV6 1823 char ipstr[40] = { 1824 0 1825 }; 1826#endif 1827 /* Try to add the basic link */ 1828 struct pico_ipv6_link *new = pico_ipv6_do_link_add(dev, address, netmask); 1829 if (!new) 1830 return NULL; 1831 1832 /* Apply DAD */ 1833 new->dup_detect_retrans = PICO_IPV6_DEFAULT_DAD_RETRANS; 1834#ifndef UNIT_TEST 1835 /* Duplicate Address Detection */ 1836 new->dad_timer = pico_timer_add(100, pico_ipv6_nd_dad, &new->address); 1837 if (!new->dad_timer) { 1838 dbg("IPv6: Failed to start nd_dad timer\n"); 1839 pico_ipv6_link_del(dev, address); 1840 return NULL; 1841 } 1842#else 1843 new->istentative = 0; 1844#endif 1845 1846#ifdef DEBUG_IPV6 1847 pico_ipv6_to_string(ipstr, new->address.addr); 1848 dbg("Assigned ipv6 %s to device %s\n", ipstr, new->dev->name); 1849#endif 1850 return new; 1851} 1852 1853static int pico_ipv6_cleanup_routes(struct pico_ipv6_link *link) 1854{ 1855 struct pico_tree_node *index = NULL, *_tmp = NULL; 1856 struct pico_ipv6_route *route = NULL; 1857 1858 pico_tree_foreach_safe(index, &IPV6Routes, _tmp) 1859 { 1860 route = index->keyValue; 1861 if (link == route->link) 1862 pico_ipv6_route_del(route->dest, route->netmask, route->gateway, (int)route->metric, route->link); 1863 } 1864 return 0; 1865} 1866 1867int pico_ipv6_cleanup_links(struct pico_device *dev) 1868{ 1869 struct pico_tree_node *index = NULL, *_tmp = NULL; 1870 struct pico_ipv6_link *link = NULL; 1871 1872 pico_tree_foreach_safe(index, &IPV6Links, _tmp) 1873 { 1874 link = index->keyValue; 1875 if (dev == link->dev) 1876 pico_ipv6_link_del(dev, link->address); 1877 } 1878 return 0; 1879} 1880 1881int pico_ipv6_link_del(struct pico_device *dev, struct pico_ip6 address) 1882{ 1883 struct pico_ipv6_link test = { 1884 0 1885 }, *found = NULL; 1886 1887 if (!dev) { 1888 pico_err = PICO_ERR_EINVAL; 1889 return -1; 1890 } 1891 1892 test.address = address; 1893 test.dev = dev; 1894 found = pico_tree_findKey(&IPV6Links, &test); 1895 if (!found) { 1896 pico_err = PICO_ERR_ENXIO; 1897 return -1; 1898 } 1899 1900 pico_ipv6_cleanup_routes(found); 1901 if (found->dad_timer) 1902 pico_timer_cancel(found->dad_timer); 1903 1904 pico_tree_delete(&IPV6Links, found); 1905 /* XXX MUST leave the solicited-node multicast address corresponding to the address (RFC 4861 $7.2.1) */ 1906 PICO_FREE(found); 1907 return 0; 1908} 1909 1910struct pico_ipv6_link *pico_ipv6_link_istentative(struct pico_ip6 *address) 1911{ 1912 struct pico_ipv6_link test = { 1913 0 1914 }, *found = NULL; 1915 test.address = *address; 1916 1917 found = pico_tree_findKey(&IPV6Links, &test); 1918 if (!found) 1919 return NULL; 1920 1921 if (found->istentative) 1922 return found; 1923 1924 return NULL; 1925} 1926 1927struct pico_ipv6_link *pico_ipv6_link_get(struct pico_ip6 *address) 1928{ 1929 struct pico_ipv6_link test = { 1930 0 1931 }, *found = NULL; 1932 test.address = *address; 1933 found = pico_tree_findKey(&IPV6Links, &test); 1934 if (!found) { 1935 return NULL; 1936 } 1937 1938 if (found->istentative) { 1939 return NULL; 1940 } 1941 1942 return found; 1943} 1944 1945struct pico_device *pico_ipv6_link_find(struct pico_ip6 *address) 1946{ 1947 struct pico_ipv6_link test = { 1948 0 1949 }, *found = NULL; 1950 if(!address) { 1951 pico_err = PICO_ERR_EINVAL; 1952 return NULL; 1953 } 1954 1955 test.dev = NULL; 1956 test.address = *address; 1957 found = pico_tree_findKey(&IPV6Links, &test); 1958 if (!found) { 1959 pico_err = PICO_ERR_ENXIO; 1960 return NULL; 1961 } 1962 1963 if (found->istentative) { 1964 return NULL; 1965 } 1966 1967 return found->dev; 1968} 1969 1970struct pico_ip6 pico_ipv6_route_get_gateway(struct pico_ip6 *addr) 1971{ 1972 struct pico_ip6 nullip = {{0}}; 1973 struct pico_ipv6_route *route = NULL; 1974 1975 if (!addr) { 1976 pico_err = PICO_ERR_EINVAL; 1977 return nullip; 1978 } 1979 1980 route = pico_ipv6_route_find(addr); 1981 if (!route) { 1982 pico_err = PICO_ERR_EHOSTUNREACH; 1983 return nullip; 1984 } 1985 else 1986 return route->gateway; 1987} 1988 1989 1990struct pico_ipv6_link *pico_ipv6_link_by_dev(struct pico_device *dev) 1991{ 1992 struct pico_tree_node *index = NULL; 1993 struct pico_ipv6_link *link = NULL; 1994 1995 pico_tree_foreach(index, &IPV6Links) 1996 { 1997 link = index->keyValue; 1998 if (dev == link->dev) 1999 return link; 2000 } 2001 return NULL; 2002} 2003 2004struct pico_ipv6_link *pico_ipv6_link_by_dev_next(struct pico_device *dev, struct pico_ipv6_link *last) 2005{ 2006 struct pico_tree_node *index = NULL; 2007 struct pico_ipv6_link *link = NULL; 2008 int valid = 0; 2009 2010 if (last == NULL) 2011 valid = 1; 2012 2013 pico_tree_foreach(index, &IPV6Links) 2014 { 2015 link = index->keyValue; 2016 if (link->dev == dev) { 2017 if (last == link) 2018 valid = 1; 2019 else if (valid > 0) 2020 return link; 2021 } 2022 } 2023 return NULL; 2024} 2025 2026struct pico_ipv6_link *pico_ipv6_prefix_configured(struct pico_ip6 *prefix) 2027{ 2028 unsigned int nm64_len = 8; 2029 struct pico_tree_node *index = NULL; 2030 struct pico_ipv6_link *link = NULL; 2031 pico_tree_foreach(index, &IPV6Links) { 2032 link = index->keyValue; 2033 if (memcmp(link->address.addr, prefix->addr, nm64_len) == 0) 2034 return link; 2035 } 2036 return NULL; 2037} 2038 2039struct pico_ipv6_link *pico_ipv6_linklocal_get(struct pico_device *dev) 2040{ 2041 struct pico_ipv6_link *link = pico_ipv6_link_by_dev(dev); 2042 while (link && !pico_ipv6_is_linklocal(link->address.addr)) { 2043 link = pico_ipv6_link_by_dev_next(dev, link); 2044 } 2045 return link; 2046} 2047 2048struct pico_ipv6_link *pico_ipv6_sitelocal_get(struct pico_device *dev) 2049{ 2050 struct pico_ipv6_link *link = pico_ipv6_link_by_dev(dev); 2051 while (link && !pico_ipv6_is_sitelocal(link->address.addr)) { 2052 link = pico_ipv6_link_by_dev_next(dev, link); 2053 } 2054 return link; 2055} 2056 2057struct pico_ipv6_link *pico_ipv6_global_get(struct pico_device *dev) 2058{ 2059 struct pico_ipv6_link *link = pico_ipv6_link_by_dev(dev); 2060 while (link && !pico_ipv6_is_global(link->address.addr)) { 2061 dbg("[0x%02X] - is global: %d - %d\n", link->address.addr[0], pico_ipv6_is_global(link->address.addr), link->address.addr[0] >> 0x05); 2062 link = pico_ipv6_link_by_dev_next(dev, link); 2063 } 2064 return link; 2065} 2066 2067#define TWO_HOURS ((pico_time)(1000 * 60 * 60 * 2)) 2068 2069void pico_ipv6_check_lifetime_expired(pico_time now, void *arg) 2070{ 2071 struct pico_tree_node *index = NULL, *temp; 2072 struct pico_ipv6_link *link = NULL; 2073#ifdef PICO_SUPPORT_6LOWPAN 2074 struct pico_ipv6_route *gw = NULL; 2075#endif 2076 (void)arg; 2077 pico_tree_foreach_safe(index, &IPV6Links, temp) { 2078 link = index->keyValue; 2079 if ((link->expire_time > 0) && (link->expire_time < now)) { 2080 dbg("Warning: IPv6 address has expired.\n"); 2081 pico_ipv6_link_del(link->dev, link->address); 2082 } 2083#ifdef PICO_SUPPORT_6LOWPAN 2084 else if (PICO_DEV_IS_6LOWPAN(link->dev) && !pico_ipv6_is_linklocal(link->address.addr) && 2085 (link->expire_time > 0) && (int)(link->expire_time - now) < (int)(TWO_HOURS >> 4)) { 2086 /* RFC6775: The host SHOULD unicast one or more RSs to the router well before the 2087 * shortest of the, Router Lifetime, PIO lifetimes and the lifetime of the 6COs. */ 2088 while ((gw = pico_ipv6_gateway_by_dev_next(link->dev, gw))) { 2089 pico_6lp_nd_start_soliciting(link, gw); 2090 } 2091 } 2092#endif 2093 } 2094 if (!pico_timer_add(1000, pico_ipv6_check_lifetime_expired, NULL)) { 2095 dbg("IPv6: Failed to start check_lifetime timer\n"); 2096 /* TODO No more link lifetime checking now */ 2097 } 2098} 2099 2100int pico_ipv6_lifetime_set(struct pico_ipv6_link *l, pico_time expire) 2101{ 2102 pico_time now = PICO_TIME_MS(); 2103 if (expire <= now) { 2104 return -1; 2105 } 2106 2107 if (expire > 0xFFFFFFFE) { 2108 l->expire_time = 0u; 2109 }else if ((expire > (now + TWO_HOURS)) || (expire > l->expire_time)) { 2110 l->expire_time = expire; 2111 } else { 2112 l->expire_time = now + TWO_HOURS; 2113 } 2114 2115 return 0; 2116} 2117 2118int pico_ipv6_dev_routing_enable(struct pico_device *dev) 2119{ 2120 dev->hostvars.routing = 1; 2121 return 0; 2122} 2123 2124int pico_ipv6_dev_routing_disable(struct pico_device *dev) 2125{ 2126 dev->hostvars.routing = 0; 2127 return 0; 2128} 2129 2130void pico_ipv6_unreachable(struct pico_frame *f, uint8_t code) 2131{ 2132 struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr; 2133#if defined PICO_SUPPORT_TCP || defined PICO_SUPPORT_UDP 2134 pico_transport_error(f, hdr->nxthdr, code); 2135#endif 2136} 2137 2138 2139 2140#endif 2141