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, Markian Yskout 6 *********************************************************************/ 7 8 9#include "pico_config.h" 10#include "pico_ipfilter.h" 11#include "pico_ipv4.h" 12#include "pico_icmp4.h" 13#include "pico_stack.h" 14#include "pico_eth.h" 15#include "pico_udp.h" 16#include "pico_tcp.h" 17#include "pico_socket.h" 18#include "pico_device.h" 19#include "pico_nat.h" 20#include "pico_igmp.h" 21#include "pico_tree.h" 22#include "pico_aodv.h" 23#include "pico_socket_multicast.h" 24#include "pico_fragments.h" 25#include "pico_ethernet.h" 26#include "pico_mcast.h" 27 28#ifdef PICO_SUPPORT_IPV4 29 30#ifdef PICO_SUPPORT_MCAST 31 32#ifdef DEBUG_MCAST 33#define ip_mcast_dbg dbg 34#else 35#define ip_mcast_dbg(...) do {} while(0) 36#endif 37 38# define PICO_MCAST_ALL_HOSTS long_be(0xE0000001) /* 224.0.0.1 */ 39/* Default network interface for multicast transmission */ 40static struct pico_ipv4_link *mcast_default_link = NULL; 41#endif 42 43/* Queues */ 44static struct pico_queue in = { 45 0 46}; 47static struct pico_queue out = { 48 0 49}; 50 51/* Functions */ 52static int ipv4_route_compare(void *ka, void *kb); 53static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, struct pico_device *dev, uint16_t size); 54 55 56int pico_ipv4_compare(struct pico_ip4 *a, struct pico_ip4 *b) 57{ 58 if (a->addr < b->addr) 59 return -1; 60 61 if (a->addr > b->addr) 62 return 1; 63 64 return 0; 65} 66 67int pico_ipv4_to_string(char *ipbuf, const uint32_t ip) 68{ 69 const unsigned char *addr = (const unsigned char *) &ip; 70 int i; 71 72 if (!ipbuf) { 73 pico_err = PICO_ERR_EINVAL; 74 return -1; 75 } 76 77 for(i = 0; i < 4; i++) 78 { 79 if (addr[i] > 99) { 80 *ipbuf++ = (char)('0' + (addr[i] / 100)); 81 *ipbuf++ = (char)('0' + ((addr[i] % 100) / 10)); 82 *ipbuf++ = (char)('0' + ((addr[i] % 100) % 10)); 83 } else if (addr[i] > 9) { 84 *ipbuf++ = (char)('0' + (addr[i] / 10)); 85 *ipbuf++ = (char)('0' + (addr[i] % 10)); 86 } else { 87 *ipbuf++ = (char)('0' + addr[i]); 88 } 89 90 if (i < 3) 91 *ipbuf++ = '.'; 92 } 93 *ipbuf = '\0'; 94 95 return 0; 96} 97 98static int pico_string_check_null_args(const char *ipstr, uint32_t *ip) 99{ 100 101 if (!ipstr || !ip) { 102 pico_err = PICO_ERR_EINVAL; 103 return -1; 104 } 105 106 return 0; 107 108} 109 110int pico_string_to_ipv4(const char *ipstr, uint32_t *ip) 111{ 112 unsigned char buf[PICO_SIZE_IP4] = { 113 0 114 }; 115 int cnt = 0; 116 char p; 117 118 if (pico_string_check_null_args(ipstr, ip) < 0) 119 return -1; 120 121 while((p = *ipstr++) != 0 && cnt < PICO_SIZE_IP4) 122 { 123 if (pico_is_digit(p)) { 124 buf[cnt] = (uint8_t)((10 * buf[cnt]) + (p - '0')); 125 } else if (p == '.') { 126 cnt++; 127 } else { 128 return -1; 129 } 130 } 131 /* Handle short notation */ 132 if (cnt == 1) { 133 buf[3] = buf[1]; 134 buf[1] = 0; 135 buf[2] = 0; 136 } else if (cnt == 2) { 137 buf[3] = buf[2]; 138 buf[2] = 0; 139 } else if (cnt != 3) { 140 /* String could not be parsed, return error */ 141 return -1; 142 } 143 144 *ip = long_from(buf); 145 146 return 0; 147} 148 149int pico_ipv4_valid_netmask(uint32_t mask) 150{ 151 int cnt = 0; 152 int end = 0; 153 int i; 154 uint32_t mask_swap = long_be(mask); 155 156 /* 157 * Swap bytes for convenient parsing 158 * e.g. 0x..f8ff will become 0xfff8.. 159 * Then, we count the consecutive bits 160 * 161 * */ 162 163 for(i = 0; i < 32; i++) { 164 if ((mask_swap << i) & 0x80000000) { 165 if (end) { 166 pico_err = PICO_ERR_EINVAL; 167 return -1; 168 } 169 170 cnt++; 171 } else { 172 end = 1; 173 } 174 } 175 return cnt; 176} 177 178int pico_ipv4_is_unicast(uint32_t address) 179{ 180 const unsigned char *addr = (unsigned char *) &address; 181 if ((addr[0] & 0xe0) == 0xe0) 182 return 0; /* multicast */ 183 184 return 1; 185} 186 187int pico_ipv4_is_multicast(uint32_t address) 188{ 189 const unsigned char *addr = (unsigned char *) &address; 190 if ((addr[0] != 0xff) && ((addr[0] & 0xe0) == 0xe0)) 191 return 1; /* multicast */ 192 193 return 0; 194} 195 196int pico_ipv4_is_loopback(uint32_t address) 197{ 198 const unsigned char *addr = (unsigned char *) &address; 199 if (addr[0] == 0x7f) 200 return 1; 201 202 return 0; 203} 204 205static int pico_ipv4_is_invalid_loopback(uint32_t address, struct pico_device *dev) 206{ 207 return pico_ipv4_is_loopback(address) && ((!dev) || strcmp(dev->name, "loop")); 208} 209 210int pico_ipv4_is_valid_src(uint32_t address, struct pico_device *dev) 211{ 212 if (pico_ipv4_is_broadcast(address)) { 213 dbg("Source is a broadcast address, discard packet\n"); 214 return 0; 215 } else if ( pico_ipv4_is_multicast(address)) { 216 dbg("Source is a multicast address, discard packet\n"); 217 return 0; 218 } else if (pico_ipv4_is_invalid_loopback(address, dev)) { 219 dbg("Source is a loopback address, discard packet\n"); 220 return 0; 221 } else { 222#ifdef PICO_SUPPORT_AODV 223 union pico_address src; 224 src.ip4.addr = address; 225 pico_aodv_refresh(&src); 226#endif 227 return 1; 228 } 229} 230 231static int pico_ipv4_checksum(struct pico_frame *f) 232{ 233 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 234 if (!hdr) 235 return -1; 236 237 hdr->crc = 0; 238 hdr->crc = short_be(pico_checksum(hdr, f->net_len)); 239 return 0; 240} 241 242 243#ifdef PICO_SUPPORT_CRC 244static inline int pico_ipv4_crc_check(struct pico_frame *f) 245{ 246 uint16_t checksum_invalid = 1; 247 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 248 249 checksum_invalid = short_be(pico_checksum(hdr, f->net_len)); 250 if (checksum_invalid) { 251 dbg("IP: checksum failed!\n"); 252 pico_frame_discard(f); 253 return 0; 254 } 255 256 return 1; 257} 258#else 259static inline int pico_ipv4_crc_check(struct pico_frame *f) 260{ 261 IGNORE_PARAMETER(f); 262 return 1; 263} 264#endif /* PICO_SUPPORT_CRC */ 265 266static int pico_ipv4_forward(struct pico_frame *f); 267#ifdef PICO_SUPPORT_MCAST 268static int pico_ipv4_mcast_filter(struct pico_frame *f); 269#endif 270 271static int ipv4_link_compare(void *ka, void *kb) 272{ 273 struct pico_ipv4_link *a = ka, *b = kb; 274 int cmp = pico_ipv4_compare(&a->address, &b->address); 275 if (cmp) 276 return cmp; 277 278 /* zero can be assigned multiple times (e.g. for DHCP) */ 279 if (a->dev != NULL && b->dev != NULL && a->address.addr == PICO_IP4_ANY && b->address.addr == PICO_IP4_ANY) { 280 if (a->dev < b->dev) 281 return -1; 282 283 if (a->dev > b->dev) 284 return 1; 285 } 286 287 return 0; 288} 289 290static PICO_TREE_DECLARE(Tree_dev_link, ipv4_link_compare); 291 292static int pico_ipv4_process_bcast_in(struct pico_frame *f) 293{ 294 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 295#ifdef PICO_SUPPORT_UDP 296 if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_UDP)) { 297 /* Receiving UDP broadcast datagram */ 298 f->flags |= PICO_FRAME_FLAG_BCAST; 299 pico_enqueue(pico_proto_udp.q_in, f); 300 return 1; 301 } 302 303#endif 304 305#ifdef PICO_SUPPORT_ICMP4 306 if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_ICMP4)) { 307 /* Receiving ICMP4 bcast packet */ 308 f->flags |= PICO_FRAME_FLAG_BCAST; 309 pico_enqueue(pico_proto_icmp4.q_in, f); 310 return 1; 311 } 312 313#endif 314 return 0; 315} 316 317static int pico_ipv4_process_mcast_in(struct pico_frame *f) 318{ 319 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 320 if (pico_ipv4_is_multicast(hdr->dst.addr)) { 321#ifdef PICO_SUPPORT_IGMP 322 /* Receiving UDP multicast datagram TODO set f->flags? */ 323 if (hdr->proto == PICO_PROTO_IGMP) { 324 ip_mcast_dbg("MCAST: received IGMP message\n"); 325 pico_transport_receive(f, PICO_PROTO_IGMP); 326 return 1; 327 } else if ((pico_ipv4_mcast_filter(f) == 0) && (hdr->proto == PICO_PROTO_UDP)) { 328 pico_enqueue(pico_proto_udp.q_in, f); 329 return 1; 330 } 331 332#endif 333 pico_frame_discard(f); 334 return 1; 335 } 336 337 return 0; 338} 339 340static int pico_ipv4_process_local_unicast_in(struct pico_frame *f) 341{ 342 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 343 struct pico_ipv4_link test = { 344 .address = {.addr = PICO_IP4_ANY}, .dev = NULL 345 }; 346 if (pico_ipv4_link_find(&hdr->dst)) { 347 if (pico_ipv4_nat_inbound(f, &hdr->dst) == 0) 348 pico_enqueue(pico_proto_ipv4.q_in, f); /* dst changed, reprocess */ 349 else 350 pico_transport_receive(f, hdr->proto); 351 352 return 1; 353 } else if (pico_tree_findKey(&Tree_dev_link, &test)) { 354#ifdef PICO_SUPPORT_UDP 355 /* address of this device is apparently 0.0.0.0; might be a DHCP packet */ 356 /* XXX KRO: is obsolete. Broadcast flag is set on outgoing DHCP messages. 357 * incomming DHCP messages are to be broadcasted. Our current DHCP server 358 * implementation does not take this flag into account yet though ... */ 359 pico_enqueue(pico_proto_udp.q_in, f); 360 return 1; 361#endif 362 } 363 364 return 0; 365} 366 367static void pico_ipv4_process_finally_try_forward(struct pico_frame *f) 368{ 369 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 370 if ((pico_ipv4_is_broadcast(hdr->dst.addr)) || ((f->flags & PICO_FRAME_FLAG_BCAST) != 0)) { 371 /* don't forward broadcast frame, discard! */ 372 pico_frame_discard(f); 373 } else if (pico_ipv4_forward(f) != 0) { 374 pico_frame_discard(f); 375 /* dbg("Forward failed.\n"); */ 376 } 377} 378 379 380 381static int pico_ipv4_process_in(struct pico_protocol *self, struct pico_frame *f) 382{ 383 uint8_t option_len = 0; 384 int ret = 0; 385 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 386 uint16_t max_allowed = (uint16_t) ((int)f->buffer_len - (f->net_hdr - f->buffer) - (int)PICO_SIZE_IP4HDR); 387 388 if (!hdr) 389 return -1; 390 391 (void)self; 392 393 /* NAT needs transport header information */ 394 if (((hdr->vhl) & 0x0F) > 5) { 395 option_len = (uint8_t)(4 * (((hdr->vhl) & 0x0F) - 5)); 396 } 397 398 f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR + option_len; 399 f->transport_len = (uint16_t)(short_be(hdr->len) - PICO_SIZE_IP4HDR - option_len); 400 f->net_len = (uint16_t)(PICO_SIZE_IP4HDR + option_len); 401#if defined(PICO_SUPPORT_IPV4FRAG) || defined(PICO_SUPPORT_IPV6FRAG) 402 f->frag = short_be(hdr->frag); 403#endif 404 405 if (f->transport_len > max_allowed) { 406 pico_frame_discard(f); 407 return 0; /* Packet is discarded due to unfeasible length */ 408 } 409 410#ifdef PICO_SUPPORT_IPFILTER 411 if (ipfilter(f)) { 412 /*pico_frame is discarded as result of the filtering*/ 413 return 0; 414 } 415 416#endif 417 418 419 /* ret == 1 indicates to continue the function */ 420 ret = pico_ipv4_crc_check(f); 421 if (ret < 1) 422 return ret; 423 424 /* Validate source IP address. Discard quietly if invalid */ 425 if (!pico_ipv4_is_valid_src(hdr->src.addr, f->dev)) { 426 pico_frame_discard(f); 427 return 0; 428 } 429 430#if defined(PICO_SUPPORT_IPV4FRAG) || defined(PICO_SUPPORT_IPV6FRAG) 431 if (f->frag & PICO_IPV4_EVIL) { 432 (void)pico_icmp4_param_problem(f, 0); 433 pico_frame_discard(f); /* RFC 3514 */ 434 return 0; 435 } 436#endif 437 438 if ((hdr->vhl & 0x0f) < 5) { 439 /* RFC 791: IHL minimum value is 5 */ 440 (void)pico_icmp4_param_problem(f, 0); 441 pico_frame_discard(f); 442 return 0; 443 } 444 445#if defined(PICO_SUPPORT_IPV4FRAG) || defined(PICO_SUPPORT_IPV6FRAG) 446 if (f->frag & (PICO_IPV4_MOREFRAG | PICO_IPV4_FRAG_MASK)) 447 { 448#ifdef PICO_SUPPORT_IPV4FRAG 449 pico_ipv4_process_frag(hdr, f, hdr->proto); 450 /* Frame can be discarded, frag will handle its own copy */ 451#endif 452 /* We do not support fragmentation, discard quietly */ 453 pico_frame_discard(f); 454 return 0; 455 } 456#endif 457 458 if (pico_ipv4_process_bcast_in(f) > 0) 459 return 0; 460 461 if (pico_ipv4_process_mcast_in(f) > 0) 462 return 0; 463 464 if (pico_ipv4_process_local_unicast_in(f) > 0) 465 return 0; 466 467 pico_ipv4_process_finally_try_forward(f); 468 469 return 0; 470} 471 472PICO_TREE_DECLARE(Routes, ipv4_route_compare); 473 474 475static int pico_ipv4_process_out(struct pico_protocol *self, struct pico_frame *f) 476{ 477 IGNORE_PARAMETER(self); 478 f->start = (uint8_t*) f->net_hdr; 479#ifdef PICO_SUPPORT_IPFILTER 480 if (ipfilter(f)) { 481 /*pico_frame is discarded as result of the filtering*/ 482 return 0; 483 } 484 485#endif 486 return pico_datalink_send(f); 487} 488 489 490static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, struct pico_device *dev, uint16_t size) 491{ 492 struct pico_frame *f = NULL; 493 IGNORE_PARAMETER(self); 494 495 f = pico_proto_ethernet.alloc(&pico_proto_ethernet, dev, (uint16_t)(size + PICO_SIZE_IP4HDR)); 496 /* TODO: In 6LoWPAN topic branch update to make use of dev->ll_mode */ 497 498 if (!f) 499 return NULL; 500 501 f->net_len = PICO_SIZE_IP4HDR; 502 f->transport_hdr = f->net_hdr + PICO_SIZE_IP4HDR; 503 f->transport_len = (uint16_t)size; 504 505 /* Datalink size is accounted for in pico_datalink_send (link layer) */ 506 f->len = (uint32_t)(size + PICO_SIZE_IP4HDR); 507 508 return f; 509} 510 511static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f); 512 513/* Interface: protocol definition */ 514struct pico_protocol pico_proto_ipv4 = { 515 .name = "ipv4", 516 .proto_number = PICO_PROTO_IPV4, 517 .layer = PICO_LAYER_NETWORK, 518 .alloc = pico_ipv4_alloc, 519 .process_in = pico_ipv4_process_in, 520 .process_out = pico_ipv4_process_out, 521 .push = pico_ipv4_frame_sock_push, 522 .q_in = &in, 523 .q_out = &out, 524}; 525 526 527static int ipv4_route_compare(void *ka, void *kb) 528{ 529 struct pico_ipv4_route *a = ka, *b = kb; 530 uint32_t a_nm, b_nm; 531 int cmp; 532 533 a_nm = long_be(a->netmask.addr); 534 b_nm = long_be(b->netmask.addr); 535 536 /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */ 537 if (a_nm < b_nm) 538 return -1; 539 540 if (b_nm < a_nm) 541 return 1; 542 543 cmp = pico_ipv4_compare(&a->dest, &b->dest); 544 if (cmp) 545 return cmp; 546 547 if (a->metric < b->metric) 548 return -1; 549 550 if (a->metric > b->metric) 551 return 1; 552 553 return 0; 554} 555 556 557static struct pico_ipv4_route default_bcast_route = { 558 .dest = {PICO_IP4_BCAST}, 559 .netmask = {PICO_IP4_BCAST}, 560 .gateway = { 0 }, 561 .link = NULL, 562 .metric = 1000 563}; 564 565static struct pico_ipv4_route *route_find_default_bcast(void) 566{ 567 return &default_bcast_route; 568} 569 570 571static struct pico_ipv4_route *route_find(const struct pico_ip4 *addr) 572{ 573 struct pico_ipv4_route *r; 574 struct pico_tree_node *index; 575 576 if (addr->addr == PICO_IP4_ANY) { 577 return NULL; 578 } 579 580 if (addr->addr != PICO_IP4_BCAST) { 581 pico_tree_foreach_reverse(index, &Routes) { 582 r = index->keyValue; 583 if ((addr->addr & (r->netmask.addr)) == (r->dest.addr)) { 584 return r; 585 } 586 } 587 return NULL; 588 } 589 590 return route_find_default_bcast(); 591} 592 593struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr) 594{ 595 struct pico_ip4 nullip; 596 struct pico_ipv4_route *route; 597 nullip.addr = 0U; 598 599 if (!addr) { 600 pico_err = PICO_ERR_EINVAL; 601 return nullip; 602 } 603 604 route = route_find(addr); 605 if (!route) { 606 pico_err = PICO_ERR_EHOSTUNREACH; 607 return nullip; 608 } 609 else 610 return route->gateway; 611} 612 613struct pico_ip4 *pico_ipv4_source_find(const struct pico_ip4 *dst) 614{ 615 struct pico_ip4 *myself = NULL; 616 struct pico_ipv4_route *rt; 617#ifdef PICO_SUPPORT_AODV 618 union pico_address node_address; 619#endif 620 621 if (!dst) { 622 pico_err = PICO_ERR_EINVAL; 623 return NULL; 624 } 625 626#ifdef PICO_SUPPORT_AODV 627 node_address.ip4.addr = dst->addr; 628 if (dst->addr && pico_ipv4_is_unicast(dst->addr)) 629 pico_aodv_lookup(&node_address); 630 631#endif 632 633 rt = route_find(dst); 634 if (rt && rt->link) { 635 myself = &rt->link->address; 636 } else { 637 pico_err = PICO_ERR_EHOSTUNREACH; 638 } 639 640 return myself; 641} 642 643struct pico_device *pico_ipv4_source_dev_find(const struct pico_ip4 *dst) 644{ 645 struct pico_device *dev = NULL; 646 struct pico_ipv4_route *rt; 647 648 if (!dst) { 649 pico_err = PICO_ERR_EINVAL; 650 return NULL; 651 } 652 653 rt = route_find(dst); 654 if (rt && rt->link) { 655 dev = rt->link->dev; 656 } else { 657 pico_err = PICO_ERR_EHOSTUNREACH; 658 } 659 660 return dev; 661} 662 663 664#ifdef PICO_SUPPORT_MCAST 665/* link 666 * | 667 * MCASTGroups 668 * | | | 669 * ------------ | ------------ 670 * | | | 671 * MCASTSources MCASTSources MCASTSources 672 * | | | | | | | | | | | | 673 * S S S S S S S S S S S S 674 * 675 * MCASTGroups: RBTree(mcast_group) 676 * MCASTSources: RBTree(source) 677 */ 678static int ipv4_mcast_groups_cmp(void *ka, void *kb) 679{ 680 struct pico_mcast_group *a = ka, *b = kb; 681 return pico_ipv4_compare(&a->mcast_addr.ip4, &b->mcast_addr.ip4); 682} 683 684static int ipv4_mcast_sources_cmp(void *ka, void *kb) 685{ 686 struct pico_ip4 *a = ka, *b = kb; 687 return pico_ipv4_compare(a, b); 688} 689 690static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link) 691{ 692 uint16_t i = 0; 693 struct pico_mcast_group *g = NULL; 694 struct pico_ip4 *source = NULL; 695 struct pico_tree_node *index = NULL, *index2 = NULL; 696 (void) source; 697 698 ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); 699 ip_mcast_dbg("+ MULTICAST list interface %-16s +\n", mcast_link->dev->name); 700 ip_mcast_dbg("+---------------------------------------------------------------------------------+\n"); 701 ip_mcast_dbg("+ nr | interface | host group | reference count | filter mode | source +\n"); 702 ip_mcast_dbg("+---------------------------------------------------------------------------------+\n"); 703 704 pico_tree_foreach(index, mcast_link->MCASTGroups) { 705 g = index->keyValue; 706 ip_mcast_dbg("+ %04d | %16s | %08X | %05u | %u | %8s +\n", i, mcast_link->dev->name, g->mcast_addr.ip4.addr, g->reference_count, g->filter_mode, ""); 707 pico_tree_foreach(index2, &g->MCASTSources) { 708 source = index2->keyValue; 709 ip_mcast_dbg("+ %4s | %16s | %8s | %5s | %s | %08X +\n", "", "", "", "", "", source->addr); 710 } 711 i++; 712 } 713 ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"); 714} 715 716static int mcast_group_update(struct pico_mcast_group *g, struct pico_tree *MCASTFilter, uint8_t filter_mode) 717{ 718 struct pico_tree_node *index = NULL, *_tmp = NULL; 719 struct pico_ip4 *source = NULL; 720 /* cleanup filter */ 721 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) { 722 source = index->keyValue; 723 pico_tree_delete(&g->MCASTSources, source); 724 PICO_FREE(source); 725 } 726 /* insert new filter */ 727 if (MCASTFilter) { 728 pico_tree_foreach(index, MCASTFilter) { 729 if (index->keyValue) { 730 source = PICO_ZALLOC(sizeof(struct pico_ip4)); 731 if (!source) { 732 pico_err = PICO_ERR_ENOMEM; 733 return -1; 734 } 735 source->addr = ((struct pico_ip4 *)index->keyValue)->addr; 736 if (pico_tree_insert(&g->MCASTSources, source)) { 737 dbg("IPv4: Failed to insert source in tree\n"); 738 PICO_FREE(source); 739 return -1; 740 } 741 } 742 } 743 } 744 745 g->filter_mode = filter_mode; 746 return 0; 747} 748 749int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) 750{ 751 struct pico_mcast_group *g = NULL, test = { 752 0 753 }; 754 struct pico_ipv4_link *link = NULL; 755 756 if (mcast_link) 757 link = pico_ipv4_link_get(mcast_link); 758 759 if (!link) 760 link = mcast_default_link; 761 762 test.mcast_addr.ip4 = *mcast_group; 763 g = pico_tree_findKey(link->MCASTGroups, &test); 764 if (g) { 765 if (reference_count) 766 g->reference_count++; 767 768#ifdef PICO_SUPPORT_IGMP 769 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE); 770#endif 771 } else { 772 g = PICO_ZALLOC(sizeof(struct pico_mcast_group)); 773 if (!g) { 774 pico_err = PICO_ERR_ENOMEM; 775 return -1; 776 } 777 778 /* "non-existent" state of filter mode INCLUDE and empty source list */ 779 g->filter_mode = PICO_IP_MULTICAST_INCLUDE; 780 g->reference_count = 1; 781 g->mcast_addr.ip4 = *mcast_group; 782 g->MCASTSources.root = &LEAF; 783 g->MCASTSources.compare = ipv4_mcast_sources_cmp; 784 if (pico_tree_insert(link->MCASTGroups, g)) { 785 dbg("IPv4: Failed to insert group in tree\n"); 786 PICO_FREE(g); 787 return -1; 788 } 789 790#ifdef PICO_SUPPORT_IGMP 791 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE); 792#endif 793 } 794 795 if (mcast_group_update(g, MCASTFilter, filter_mode) < 0) { 796 dbg("Error in mcast_group update\n"); 797 return -1; 798 } 799 800 pico_ipv4_mcast_print_groups(link); 801 return 0; 802} 803 804int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) 805{ 806 807 struct pico_mcast_group *g = NULL, test = { 808 0 809 }; 810 struct pico_ipv4_link *link = NULL; 811 struct pico_tree_node *index = NULL, *_tmp = NULL; 812 struct pico_ip4 *source = NULL; 813 814 if (mcast_link) 815 link = pico_ipv4_link_get(mcast_link); 816 817 if (!link) 818 link = mcast_default_link; 819 820 if (!link) 821 return -1; 822 823 test.mcast_addr.ip4 = *mcast_group; 824 g = pico_tree_findKey(link->MCASTGroups, &test); 825 if (!g) { 826 pico_err = PICO_ERR_EINVAL; 827 return -1; 828 } else { 829 if (reference_count && (--(g->reference_count) < 1)) { 830#ifdef PICO_SUPPORT_IGMP 831 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_DELETE); 832#endif 833 /* cleanup filter */ 834 pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) { 835 source = index->keyValue; 836 pico_tree_delete(&g->MCASTSources, source); 837 PICO_FREE(source); 838 } 839 pico_tree_delete(link->MCASTGroups, g); 840 PICO_FREE(g); 841 } else { 842#ifdef PICO_SUPPORT_IGMP 843 pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE); 844#endif 845 if (mcast_group_update(g, MCASTFilter, filter_mode) < 0) 846 return -1; 847 } 848 } 849 850 pico_ipv4_mcast_print_groups(link); 851 return 0; 852} 853 854struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void) 855{ 856 return mcast_default_link; 857} 858 859static int pico_ipv4_mcast_filter(struct pico_frame *f) 860{ 861 struct pico_ipv4_link *link = NULL; 862 struct pico_tree_node *index = NULL, *index2 = NULL; 863 struct pico_mcast_group *g = NULL, test = { 864 0 865 }; 866 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 867 868 test.mcast_addr.ip4 = hdr->dst; 869 870 pico_tree_foreach(index, &Tree_dev_link) { 871 link = index->keyValue; 872 g = pico_tree_findKey(link->MCASTGroups, &test); 873 if (g) { 874 if (f->dev == link->dev) { 875 ip_mcast_dbg("MCAST: IP %08X is group member of current link %s\n", hdr->dst.addr, f->dev->name); 876 /* perform source filtering */ 877 switch (g->filter_mode) { 878 case PICO_IP_MULTICAST_INCLUDE: 879 pico_tree_foreach(index2, &g->MCASTSources) { 880 if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) { 881 ip_mcast_dbg("MCAST: IP %08X in included interface source list\n", hdr->src.addr); 882 return 0; 883 } 884 } 885 ip_mcast_dbg("MCAST: IP %08X NOT in included interface source list\n", hdr->src.addr); 886 return -1; 887 888 case PICO_IP_MULTICAST_EXCLUDE: 889 pico_tree_foreach(index2, &g->MCASTSources) { 890 if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) { 891 ip_mcast_dbg("MCAST: IP %08X in excluded interface source list\n", hdr->src.addr); 892 return -1; 893 } 894 } 895 ip_mcast_dbg("MCAST: IP %08X NOT in excluded interface source list\n", hdr->src.addr); 896 return 0; 897 898 default: 899 return -1; 900 } 901 } else { 902 ip_mcast_dbg("MCAST: IP %08X is group member of different link %s\n", hdr->dst.addr, link->dev->name); 903 } 904 } else { 905 ip_mcast_dbg("MCAST: IP %08X is not a group member of link %s\n", hdr->dst.addr, f->dev->name); 906 } 907 } 908 return -1; 909} 910 911#else 912 913int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) 914{ 915 IGNORE_PARAMETER(mcast_link); 916 IGNORE_PARAMETER(mcast_group); 917 IGNORE_PARAMETER(reference_count); 918 IGNORE_PARAMETER(filter_mode); 919 IGNORE_PARAMETER(MCASTFilter); 920 pico_err = PICO_ERR_EPROTONOSUPPORT; 921 return -1; 922} 923 924int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter) 925{ 926 IGNORE_PARAMETER(mcast_link); 927 IGNORE_PARAMETER(mcast_group); 928 IGNORE_PARAMETER(reference_count); 929 IGNORE_PARAMETER(filter_mode); 930 IGNORE_PARAMETER(MCASTFilter); 931 pico_err = PICO_ERR_EPROTONOSUPPORT; 932 return -1; 933} 934 935struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void) 936{ 937 pico_err = PICO_ERR_EPROTONOSUPPORT; 938 return NULL; 939} 940#endif /* PICO_SUPPORT_MCAST */ 941 942/* #define DEBUG_ROUTE */ 943#ifdef DEBUG_ROUTE 944void dbg_route(void) 945{ 946 struct pico_ipv4_route *r; 947 struct pico_tree_node *index; 948 int count_hosts = 0; 949 dbg("==== ROUTING TABLE =====\n"); 950 pico_tree_foreach(index, &Routes) { 951 r = index->keyValue; 952 dbg("Route to %08x/%08x, gw %08x, dev: %s, metric: %d\n", r->dest.addr, r->netmask.addr, r->gateway.addr, r->link->dev->name, r->metric); 953 if (r->netmask.addr == 0xFFFFFFFF) 954 count_hosts++; 955 } 956 dbg("================ total HOST nodes: %d ======\n\n\n", count_hosts); 957} 958#else 959#define dbg_route() do { } while(0) 960#endif 961 962int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto) 963{ 964 965 struct pico_ipv4_route *route; 966 struct pico_ipv4_link *link; 967 struct pico_ipv4_hdr *hdr; 968 uint8_t ttl = PICO_IPV4_DEFAULT_TTL; 969 uint8_t vhl = 0x45; /* version 4, header length 20 */ 970 int32_t retval = 0; 971 static uint16_t ipv4_progressive_id = 0x91c0; 972#ifdef PICO_SUPPORT_MCAST 973 struct pico_tree_node *index; 974#endif 975 976 if (!f || !dst) { 977 pico_err = PICO_ERR_EINVAL; 978 goto drop; 979 } 980 981 982 hdr = (struct pico_ipv4_hdr *) f->net_hdr; 983 if (!hdr) { 984 dbg("IP header error\n"); 985 pico_err = PICO_ERR_EINVAL; 986 goto drop; 987 } 988 989 if (dst->addr == 0) { 990 dbg("IP destination addr error\n"); 991 pico_err = PICO_ERR_EINVAL; 992 goto drop; 993 } 994 995 route = route_find(dst); 996 if (!route) { 997 /* dbg("Route to %08x not found.\n", long_be(dst->addr)); */ 998 999 1000 pico_err = PICO_ERR_EHOSTUNREACH; 1001 goto drop; 1002 } else { 1003 link = route->link; 1004#ifdef PICO_SUPPORT_MCAST 1005 if (pico_ipv4_is_multicast(dst->addr)) { /* if multicast */ 1006 switch (proto) { 1007 case PICO_PROTO_UDP: 1008 if (pico_udp_get_mc_ttl(f->sock, &ttl) < 0) 1009 ttl = PICO_IP_DEFAULT_MULTICAST_TTL; 1010 1011 break; 1012#ifdef PICO_SUPPORT_IGMP 1013 case PICO_PROTO_IGMP: 1014 vhl = 0x46; /* header length 24 */ 1015 ttl = 1; 1016 /* router alert (RFC 2113) */ 1017 hdr->options[0] = 0x94; 1018 hdr->options[1] = 0x04; 1019 hdr->options[2] = 0x00; 1020 hdr->options[3] = 0x00; 1021 if (f->dev && link->dev != f->dev) { /* default link is not requested link */ 1022 pico_tree_foreach(index, &Tree_dev_link) { 1023 link = index->keyValue; 1024 if (link->dev == f->dev) 1025 break; 1026 } 1027 } 1028 1029 break; 1030#endif 1031 default: 1032 ttl = PICO_IPV4_DEFAULT_TTL; 1033 } 1034 } 1035 1036#endif 1037 } 1038 1039 hdr->vhl = vhl; 1040 hdr->len = short_be((uint16_t)(f->transport_len + f->net_len)); 1041 hdr->id = short_be(ipv4_progressive_id); 1042 1043 if ( 1044#ifdef PICO_SUPPORT_IPV4FRAG 1045 (0 == (f->frag & PICO_IPV4_MOREFRAG)) && 1046#endif 1047 1 ) 1048 ipv4_progressive_id++; 1049 1050 if (f->send_ttl > 0) { 1051 ttl = f->send_ttl; 1052 } 1053 1054 hdr->dst.addr = dst->addr; 1055 hdr->src.addr = link->address.addr; 1056 hdr->ttl = ttl; 1057 hdr->tos = f->send_tos; 1058 hdr->proto = proto; 1059 hdr->frag = short_be(PICO_IPV4_DONTFRAG); 1060 1061#ifdef PICO_SUPPORT_IPV4FRAG 1062# ifdef PICO_SUPPORT_UDP 1063 if (proto == PICO_PROTO_UDP) { 1064 /* first fragment, can not use transport_len to calculate IP length */ 1065 if (f->transport_hdr != f->payload) 1066 hdr->len = short_be((uint16_t)(f->payload_len + sizeof(struct pico_udp_hdr) + f->net_len)); 1067 1068 /* set fragmentation flags and offset calculated in socket layer */ 1069 hdr->frag = short_be(f->frag); 1070 } 1071 1072 if (proto == PICO_PROTO_ICMP4) 1073 { 1074 hdr->frag = short_be(f->frag); 1075 } 1076 1077# endif 1078#endif /* PICO_SUPPORT_IPV4FRAG */ 1079 pico_ipv4_checksum(f); 1080 1081 if (f->sock && f->sock->dev) { 1082 /* if the socket has its device set, use that (currently used for DHCP) */ 1083 f->dev = f->sock->dev; 1084 } else { 1085 f->dev = link->dev; 1086 if (f->sock) 1087 f->sock->dev = f->dev; 1088 } 1089 1090#ifdef PICO_SUPPORT_MCAST 1091 if (pico_ipv4_is_multicast(hdr->dst.addr)) { 1092 struct pico_frame *cpy; 1093 /* Sending UDP multicast datagram, am I member? If so, loopback copy */ 1094 if ((proto != PICO_PROTO_IGMP) && (pico_ipv4_mcast_filter(f) == 0)) { 1095 ip_mcast_dbg("MCAST: sender is member of group, loopback copy\n"); 1096 cpy = pico_frame_copy(f); 1097 if (!cpy) { 1098 pico_err = PICO_ERR_ENOMEM; 1099 ip_mcast_dbg("MCAST: Failed to copy frame\n"); 1100 goto drop; 1101 } 1102 1103 retval = pico_enqueue(&in, cpy); 1104 if (retval <= 0) 1105 pico_frame_discard(cpy); 1106 } 1107 } 1108 1109#endif 1110 1111/* #ifdef PICO_SUPPORT_AODV */ 1112#if 0 1113 { 1114 union pico_address node_address; 1115 node_address.ip4.addr = hdr->dst.addr; 1116 if(hdr->dst.addr && pico_ipv4_is_unicast(hdr->dst.addr)) 1117 pico_aodv_lookup(&node_address); 1118 } 1119#endif 1120 1121 if (pico_ipv4_link_get(&hdr->dst)) { 1122 /* it's our own IP */ 1123 retval = pico_enqueue(&in, f); 1124 if (retval > 0) 1125 return retval; 1126 } else{ 1127 /* TODO: Check if there are members subscribed here */ 1128 retval = pico_enqueue(&out, f); 1129 if (retval > 0) 1130 return retval; 1131 } 1132 1133drop: 1134 pico_frame_discard(f); 1135 return -1; 1136} 1137 1138 1139static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f) 1140{ 1141 struct pico_ip4 *dst; 1142 struct pico_remote_endpoint *remote_endpoint = (struct pico_remote_endpoint *) f->info; 1143 IGNORE_PARAMETER(self); 1144 1145 if (!f->sock) { 1146 pico_frame_discard(f); 1147 return -1; 1148 } 1149 1150 if (remote_endpoint) { 1151 dst = &remote_endpoint->remote_addr.ip4; 1152 } else { 1153 dst = &f->sock->remote_addr.ip4; 1154 } 1155 1156 return pico_ipv4_frame_push(f, dst, (uint8_t)f->sock->proto->proto_number); 1157} 1158 1159 1160int MOCKABLE pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link) 1161{ 1162 struct pico_ipv4_route test, *new; 1163 test.dest.addr = address.addr; 1164 test.netmask.addr = netmask.addr; 1165 test.metric = (uint32_t)metric; 1166 1167 if (pico_tree_findKey(&Routes, &test)) { 1168 pico_err = PICO_ERR_EINVAL; 1169 return -1; 1170 } 1171 1172 new = PICO_ZALLOC(sizeof(struct pico_ipv4_route)); 1173 if (!new) { 1174 pico_err = PICO_ERR_ENOMEM; 1175 return -1; 1176 } 1177 1178 new->dest.addr = address.addr; 1179 new->netmask.addr = netmask.addr; 1180 new->gateway.addr = gateway.addr; 1181 new->metric = (uint32_t)metric; 1182 if (gateway.addr == 0) { 1183 /* No gateway provided, use the link */ 1184 new->link = link; 1185 } else { 1186 struct pico_ipv4_route *r = route_find(&gateway); 1187 if (!r ) { /* Specified Gateway is unreachable */ 1188 pico_err = PICO_ERR_EHOSTUNREACH; 1189 PICO_FREE(new); 1190 return -1; 1191 } 1192 1193 if (r->gateway.addr) { /* Specified Gateway is not a neighbor */ 1194 pico_err = PICO_ERR_ENETUNREACH; 1195 PICO_FREE(new); 1196 return -1; 1197 } 1198 1199 new->link = r->link; 1200 } 1201 1202 if (!new->link) { 1203 pico_err = PICO_ERR_EINVAL; 1204 PICO_FREE(new); 1205 return -1; 1206 } 1207 1208 if (pico_tree_insert(&Routes, new)) { 1209 dbg("IPv4: Failed to insert route in tree\n"); 1210 PICO_FREE(new); 1211 return -1; 1212 } 1213 1214 dbg_route(); 1215 return 0; 1216} 1217 1218int pico_ipv4_route_del(struct pico_ip4 address, struct pico_ip4 netmask, int metric) 1219{ 1220 struct pico_ipv4_route test, *found; 1221 1222 test.dest.addr = address.addr; 1223 test.netmask.addr = netmask.addr; 1224 test.metric = (uint32_t)metric; 1225 1226 found = pico_tree_findKey(&Routes, &test); 1227 if (found) { 1228 1229 pico_tree_delete(&Routes, found); 1230 PICO_FREE(found); 1231 1232 dbg_route(); 1233 return 0; 1234 } 1235 1236 pico_err = PICO_ERR_EINVAL; 1237 return -1; 1238} 1239 1240 1241int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask) 1242{ 1243 struct pico_ipv4_link test, *new; 1244 struct pico_ip4 network, gateway; 1245 char ipstr[30]; 1246 1247 if (!dev) { 1248 pico_err = PICO_ERR_EINVAL; 1249 return -1; 1250 } 1251 1252 test.address.addr = address.addr; 1253 test.netmask.addr = netmask.addr; 1254 test.dev = dev; 1255 /** XXX: Valid netmask / unicast address test **/ 1256 1257 if (pico_tree_findKey(&Tree_dev_link, &test)) { 1258 pico_err = PICO_ERR_EADDRINUSE; 1259 return -1; 1260 } 1261 1262 /** 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) **/ 1263 new = PICO_ZALLOC(sizeof(struct pico_ipv4_link)); 1264 if (!new) { 1265 dbg("IPv4: Out of memory!\n"); 1266 pico_err = PICO_ERR_ENOMEM; 1267 return -1; 1268 } 1269 1270 new->address.addr = address.addr; 1271 new->netmask.addr = netmask.addr; 1272 new->dev = dev; 1273#ifdef PICO_SUPPORT_MCAST 1274 new->MCASTGroups = PICO_ZALLOC(sizeof(struct pico_tree)); 1275 if (!new->MCASTGroups) { 1276 PICO_FREE(new); 1277 dbg("IPv4: Out of memory!\n"); 1278 pico_err = PICO_ERR_ENOMEM; 1279 return -1; 1280 } 1281 1282 new->MCASTGroups->root = &LEAF; 1283 new->MCASTGroups->compare = ipv4_mcast_groups_cmp; 1284#ifdef PICO_SUPPORT_IGMP 1285 new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */ 1286 new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL; 1287#endif 1288#endif 1289 1290 if (pico_tree_insert(&Tree_dev_link, new)) { 1291 dbg("IPv4: Failed to insert link in tree\n"); 1292#ifdef PICO_SUPPORT_MCAST 1293 PICO_FREE(new->MCASTGroups); 1294#endif 1295 PICO_FREE(new); 1296 return -1; 1297 } 1298 1299#ifdef PICO_SUPPORT_MCAST 1300 do { 1301 struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw; 1302 if (!mcast_default_link) { 1303 mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */ 1304 mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */ 1305 mcast_gw.addr = long_be(0x00000000); 1306 mcast_default_link = new; 1307 pico_ipv4_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new); 1308 } 1309 1310 mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS; 1311 pico_ipv4_mcast_join(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL); 1312 } while(0); 1313#endif 1314 1315 network.addr = address.addr & netmask.addr; 1316 gateway.addr = 0U; 1317 pico_ipv4_route_add(network, netmask, gateway, 1, new); 1318 pico_ipv4_to_string(ipstr, new->address.addr); 1319 dbg("Assigned ipv4 %s to device %s\n", ipstr, new->dev->name); 1320 if (default_bcast_route.link == NULL) 1321 default_bcast_route.link = new; 1322 1323 return 0; 1324} 1325 1326static int pico_ipv4_cleanup_routes(struct pico_ipv4_link *link) 1327{ 1328 struct pico_tree_node *index = NULL, *tmp = NULL; 1329 struct pico_ipv4_route *route = NULL; 1330 1331 pico_tree_foreach_safe(index, &Routes, tmp) { 1332 route = index->keyValue; 1333 if (link == route->link) 1334 pico_ipv4_route_del(route->dest, route->netmask, (int)route->metric); 1335 } 1336 return 0; 1337} 1338 1339void MOCKABLE pico_ipv4_route_set_bcast_link(struct pico_ipv4_link *link) 1340{ 1341 if (link) 1342 default_bcast_route.link = link; 1343} 1344 1345int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address) 1346{ 1347 struct pico_ipv4_link test, *found; 1348 1349 if (!dev) { 1350 pico_err = PICO_ERR_EINVAL; 1351 return -1; 1352 } 1353 1354 test.address.addr = address.addr; 1355 test.dev = dev; 1356 found = pico_tree_findKey(&Tree_dev_link, &test); 1357 if (!found) { 1358 pico_err = PICO_ERR_ENXIO; 1359 return -1; 1360 } 1361 1362#ifdef PICO_SUPPORT_MCAST 1363 do { 1364 struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm; 1365 struct pico_mcast_group *g = NULL; 1366 struct pico_tree_node *index, *_tmp; 1367 if (found == mcast_default_link) { 1368 mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */ 1369 mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */ 1370 mcast_default_link = NULL; 1371 pico_ipv4_route_del(mcast_addr, mcast_nm, 1); 1372 } 1373 1374 mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS; 1375 pico_ipv4_mcast_leave(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL); 1376 pico_tree_foreach_safe(index, found->MCASTGroups, _tmp) { 1377 g = index->keyValue; 1378 pico_tree_delete(found->MCASTGroups, g); 1379 PICO_FREE(g); 1380 } 1381 } while(0); 1382 PICO_FREE(found->MCASTGroups); 1383#endif 1384 1385 pico_ipv4_cleanup_routes(found); 1386 pico_tree_delete(&Tree_dev_link, found); 1387 if (default_bcast_route.link == found) 1388 default_bcast_route.link = NULL; 1389 1390 PICO_FREE(found); 1391 1392 return 0; 1393} 1394 1395 1396struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address) 1397{ 1398 struct pico_ipv4_link test = { 1399 0 1400 }, *found = NULL; 1401 test.address.addr = address->addr; 1402 1403 found = pico_tree_findKey(&Tree_dev_link, &test); 1404 if (!found) 1405 return NULL; 1406 else 1407 return found; 1408} 1409 1410struct pico_ipv4_link *MOCKABLE pico_ipv4_link_by_dev(struct pico_device *dev) 1411{ 1412 struct pico_tree_node *index = NULL; 1413 struct pico_ipv4_link *link = NULL; 1414 1415 pico_tree_foreach(index, &Tree_dev_link) { 1416 link = index->keyValue; 1417 if (link->dev == dev) 1418 return link; 1419 } 1420 return NULL; 1421} 1422 1423struct pico_ipv4_link *pico_ipv4_link_by_dev_next(struct pico_device *dev, struct pico_ipv4_link *last) 1424{ 1425 struct pico_tree_node *index = NULL; 1426 struct pico_ipv4_link *link = NULL; 1427 int valid = 0; 1428 1429 if (last == NULL) 1430 valid = 1; 1431 1432 pico_tree_foreach(index, &Tree_dev_link) { 1433 link = index->keyValue; 1434 if (link->dev == dev) { 1435 if (last == link) 1436 valid = 1; 1437 else if (valid > 0) 1438 return link; 1439 } 1440 } 1441 return NULL; 1442} 1443 1444struct pico_device *MOCKABLE pico_ipv4_link_find(struct pico_ip4 *address) 1445{ 1446 struct pico_ipv4_link test, *found; 1447 if (!address) { 1448 pico_err = PICO_ERR_EINVAL; 1449 return NULL; 1450 } 1451 1452 test.dev = NULL; 1453 test.address.addr = address->addr; 1454 found = pico_tree_findKey(&Tree_dev_link, &test); 1455 if (!found) { 1456 pico_err = PICO_ERR_ENXIO; 1457 return NULL; 1458 } 1459 1460 return found->dev; 1461} 1462 1463 1464static int pico_ipv4_rebound_large(struct pico_frame *f) 1465{ 1466#ifdef PICO_SUPPORT_IPV4FRAG 1467 uint16_t total_payload_written = 0; 1468 uint32_t len = f->transport_len; 1469 struct pico_frame *fr; 1470 struct pico_ip4 dst; 1471 struct pico_ipv4_hdr *hdr; 1472 hdr = (struct pico_ipv4_hdr *) f->net_hdr; 1473 dst.addr = hdr->src.addr; 1474 1475 while(total_payload_written < len) { 1476 uint32_t space = (uint32_t)len - total_payload_written; 1477 if (space > PICO_IPV4_MAXPAYLOAD) 1478 space = PICO_IPV4_MAXPAYLOAD; 1479 1480 fr = pico_ipv4_alloc(&pico_proto_ipv4, NULL, (uint16_t)space); 1481 if (!fr) { 1482 pico_err = PICO_ERR_ENOMEM; 1483 return -1; 1484 } 1485 1486 if (space + total_payload_written < len) 1487 { 1488 fr->frag |= PICO_IPV4_MOREFRAG; 1489 } 1490 else 1491 { 1492 fr->frag &= PICO_IPV4_FRAG_MASK; 1493 } 1494 1495 fr->frag = (((total_payload_written) >> 3u) & 0xffffu) | fr->frag; 1496 1497 memcpy(fr->transport_hdr, f->transport_hdr + total_payload_written, fr->transport_len); 1498 if (pico_ipv4_frame_push(fr, &dst, hdr->proto) > 0) { 1499 total_payload_written = (uint16_t)((uint16_t)fr->transport_len + total_payload_written); 1500 } else { 1501 /* No need to discard frame here, pico_ipv4_frame_push() already did that */ 1502 break; 1503 } 1504 } /* while() */ 1505 return (int)total_payload_written; 1506#else 1507 (void)f; 1508 return -1; 1509#endif 1510} 1511 1512int pico_ipv4_rebound(struct pico_frame *f) 1513{ 1514 struct pico_ip4 dst; 1515 struct pico_ipv4_hdr *hdr; 1516 if (!f) { 1517 pico_err = PICO_ERR_EINVAL; 1518 return -1; 1519 } 1520 1521 hdr = (struct pico_ipv4_hdr *) f->net_hdr; 1522 if (!hdr) { 1523 pico_err = PICO_ERR_EINVAL; 1524 return -1; 1525 } 1526 1527 dst.addr = hdr->src.addr; 1528 if (f->transport_len > PICO_IPV4_MAXPAYLOAD) { 1529 return pico_ipv4_rebound_large(f); 1530 } 1531 1532 return pico_ipv4_frame_push(f, &dst, hdr->proto); 1533} 1534 1535static int pico_ipv4_pre_forward_checks(struct pico_frame *f) 1536{ 1537 static uint16_t last_id = 0; 1538 static uint16_t last_proto = 0; 1539 static struct pico_ip4 last_src = { 1540 0 1541 }; 1542 static struct pico_ip4 last_dst = { 1543 0 1544 }; 1545 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr; 1546 1547 /* Decrease TTL, check if expired */ 1548 hdr->ttl = (uint8_t)(hdr->ttl - 1); 1549 if (hdr->ttl < 1) { 1550 pico_notify_ttl_expired(f); 1551 dbg(" ------------------- TTL EXPIRED\n"); 1552 return -1; 1553 } 1554 1555 /* HACK: increase crc to compensate decreased TTL */ 1556 hdr->crc++; 1557 1558 /* If source is local, discard anyway (packets bouncing back and forth) */ 1559 if (pico_ipv4_link_get(&hdr->src)) 1560 return -1; 1561 1562 /* If this was the last forwarded packet, silently discard to prevent duplications */ 1563 if ((last_src.addr == hdr->src.addr) && (last_id == hdr->id) 1564 && (last_dst.addr == hdr->dst.addr) && (last_proto == hdr->proto)) { 1565 return -1; 1566 } else { 1567 last_src.addr = hdr->src.addr; 1568 last_dst.addr = hdr->dst.addr; 1569 last_id = hdr->id; 1570 last_proto = hdr->proto; 1571 } 1572 1573 return 0; 1574} 1575 1576static int pico_ipv4_forward_check_dev(struct pico_frame *f) 1577{ 1578 if (f->dev->eth != NULL) 1579 f->len -= PICO_SIZE_ETHHDR; 1580 1581 if (f->len > f->dev->mtu) { 1582 pico_notify_pkt_too_big(f); 1583 return -1; 1584 } 1585 1586 return 0; 1587} 1588 1589static int pico_ipv4_forward(struct pico_frame *f) 1590{ 1591 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr; 1592 struct pico_ipv4_route *rt; 1593 if (!hdr) { 1594 return -1; 1595 } 1596 1597 rt = route_find(&hdr->dst); 1598 if (!rt) { 1599 pico_notify_dest_unreachable(f); 1600 return -1; 1601 } 1602 1603 f->dev = rt->link->dev; 1604 1605 if (pico_ipv4_pre_forward_checks(f) < 0) 1606 return -1; 1607 1608 pico_ipv4_nat_outbound(f, &rt->link->address); 1609 1610 f->start = f->net_hdr; 1611 1612 if (pico_ipv4_forward_check_dev(f) < 0) 1613 return -1; 1614 1615 pico_datalink_send(f); 1616 return 0; 1617 1618} 1619 1620int pico_ipv4_is_broadcast(uint32_t addr) 1621{ 1622 struct pico_ipv4_link *link; 1623 struct pico_tree_node *index; 1624 if (addr == PICO_IP4_BCAST) 1625 return 1; 1626 1627 pico_tree_foreach(index, &Tree_dev_link) { 1628 link = index->keyValue; 1629 if ((link->address.addr | (~link->netmask.addr)) == addr) 1630 return 1; 1631 } 1632 return 0; 1633} 1634 1635void pico_ipv4_unreachable(struct pico_frame *f, int err) 1636{ 1637 struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr; 1638#if defined PICO_SUPPORT_TCP || defined PICO_SUPPORT_UDP 1639 f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR; 1640 pico_transport_error(f, hdr->proto, err); 1641#endif 1642} 1643 1644int pico_ipv4_cleanup_links(struct pico_device *dev) 1645{ 1646 struct pico_tree_node *index = NULL, *_tmp = NULL; 1647 struct pico_ipv4_link *link = NULL; 1648 1649 pico_tree_foreach_safe(index, &Tree_dev_link, _tmp) { 1650 link = index->keyValue; 1651 if (dev == link->dev) 1652 pico_ipv4_link_del(dev, link->address); 1653 } 1654 return 0; 1655} 1656 1657 1658#endif 1659