1#include "pico_config.h" 2#include "pico_stack.h" 3#include "pico_socket.h" 4#include "pico_socket_multicast.h" 5#include "pico_tree.h" 6#include "pico_ipv4.h" 7#include "pico_ipv6.h" 8#include "pico_udp.h" 9 10#ifdef PICO_SUPPORT_MCAST 11 12#ifdef DEBUG_MCAST 13#define so_mcast_dbg dbg 14#else 15#define so_mcast_dbg(...) do { } while(0) 16#endif 17 18/* socket 19 * | 20 * MCASTListen 21 * | | | 22 * ------------ | ------------ 23 * | | | 24 * MCASTSources MCASTSources MCASTSources 25 * | | | | | | | | | | | | 26 * S S S S S S S S S S S S 27 * 28 * MCASTListen: RBTree(mcast_link, mcast_group) 29 * MCASTSources: RBTree(source) 30 */ 31struct pico_mcast_listen 32{ 33 int8_t filter_mode; 34 union pico_address mcast_link; 35 union pico_address mcast_group; 36 struct pico_tree MCASTSources; 37 struct pico_tree MCASTSources_ipv6; 38 uint16_t proto; 39}; 40/* Parameters */ 41struct pico_mcast 42{ 43 struct pico_socket *s; 44 struct pico_ip_mreq *mreq; 45 struct pico_ip_mreq_source *mreq_s; 46 union pico_address *address; 47 union pico_link *mcast_link; 48 struct pico_mcast_listen *listen; 49}; 50static int mcast_listen_link_cmp(struct pico_mcast_listen *a, struct pico_mcast_listen *b) 51{ 52 53 if (a->proto < b->proto) 54 return -1; 55 56 if (a->proto > b->proto) 57 return 1; 58 59 return pico_address_compare(&a->mcast_link, &b->mcast_link, a->proto); 60} 61 62static int mcast_listen_grp_cmp(struct pico_mcast_listen *a, struct pico_mcast_listen *b) 63{ 64 if (a->mcast_group.ip4.addr < b->mcast_group.ip4.addr) 65 return -1; 66 67 if (a->mcast_group.ip4.addr > b->mcast_group.ip4.addr) 68 return 1; 69 70 return mcast_listen_link_cmp(a, b); 71} 72#ifdef PICO_SUPPORT_IPV6 73static int mcast_listen_grp_cmp_ipv6(struct pico_mcast_listen *a, struct pico_mcast_listen *b) 74{ 75 int tmp = memcmp(&a->mcast_group.ip6, &b->mcast_group.ip6, sizeof(struct pico_ip6)); 76 if(!tmp) 77 return mcast_listen_link_cmp(a, b); 78 79 return tmp; 80} 81#endif 82 83static int mcast_listen_cmp(void *ka, void *kb) 84{ 85 struct pico_mcast_listen *a = ka, *b = kb; 86 if (a->proto < b->proto) 87 return -1; 88 89 if (a->proto > b->proto) 90 return 1; 91 92 return mcast_listen_grp_cmp(a, b); 93} 94#ifdef PICO_SUPPORT_IPV6 95static int mcast_listen_cmp_ipv6(void *ka, void *kb) 96{ 97 struct pico_mcast_listen *a = ka, *b = kb; 98 if (a->proto < b->proto) 99 return -1; 100 101 if (a->proto > b->proto) 102 return 1; 103 104 return mcast_listen_grp_cmp_ipv6(a, b); 105} 106#endif 107static int mcast_sources_cmp(void *ka, void *kb) 108{ 109 union pico_address *a = ka, *b = kb; 110 if (a->ip4.addr < b->ip4.addr) 111 return -1; 112 113 if (a->ip4.addr > b->ip4.addr) 114 return 1; 115 116 return 0; 117} 118#ifdef PICO_SUPPORT_IPV6 119static int mcast_sources_cmp_ipv6(void *ka, void *kb) 120{ 121 union pico_address *a = ka, *b = kb; 122 return memcmp(&a->ip6, &b->ip6, sizeof(struct pico_ip6)); 123} 124#endif 125static int mcast_socket_cmp(void *ka, void *kb) 126{ 127 struct pico_socket *a = ka, *b = kb; 128 if (a < b) 129 return -1; 130 131 if (a > b) 132 return 1; 133 134 return 0; 135} 136 137/* gather all multicast sockets to hasten filter aggregation */ 138static PICO_TREE_DECLARE(MCASTSockets, mcast_socket_cmp); 139 140static int mcast_filter_cmp(void *ka, void *kb) 141{ 142 union pico_address *a = ka, *b = kb; 143 if (a->ip4.addr < b->ip4.addr) 144 return -1; 145 146 if (a->ip4.addr > b->ip4.addr) 147 return 1; 148 149 return 0; 150} 151/* gather sources to be filtered */ 152static PICO_TREE_DECLARE(MCASTFilter, mcast_filter_cmp); 153 154static int mcast_filter_cmp_ipv6(void *ka, void *kb) 155{ 156 union pico_address *a = ka, *b = kb; 157 return memcmp(&a->ip6, &b->ip6, sizeof(struct pico_ip6)); 158} 159/* gather sources to be filtered */ 160static PICO_TREE_DECLARE(MCASTFilter_ipv6, mcast_filter_cmp_ipv6); 161 162inline static struct pico_tree *mcast_get_src_tree(struct pico_socket *s, struct pico_mcast *mcast) 163{ 164 if( IS_SOCK_IPV4(s)) { 165 mcast->listen->MCASTSources.compare = mcast_sources_cmp; 166 return &mcast->listen->MCASTSources; 167 } 168 169#ifdef PICO_SUPPORT_IPV6 170 else if( IS_SOCK_IPV6(s)) { 171 mcast->listen->MCASTSources_ipv6.compare = mcast_sources_cmp_ipv6; 172 return &mcast->listen->MCASTSources_ipv6; 173 } 174#endif 175 return NULL; 176} 177inline static struct pico_tree *mcast_get_listen_tree(struct pico_socket *s) 178{ 179 if( IS_SOCK_IPV4(s)) 180 return s->MCASTListen; 181 182#ifdef PICO_SUPPORT_IPV6 183 else if( IS_SOCK_IPV6(s)) 184 return s->MCASTListen_ipv6; 185#endif 186 return NULL; 187} 188inline static void mcast_set_listen_tree_p_null(struct pico_socket *s) 189{ 190 if( IS_SOCK_IPV4(s)) 191 s->MCASTListen = NULL; 192 193#ifdef PICO_SUPPORT_IPV6 194 else if( IS_SOCK_IPV6(s)) 195 s->MCASTListen_ipv6 = NULL; 196#endif 197} 198static struct pico_mcast_listen *listen_find(struct pico_socket *s, union pico_address *lnk, union pico_address *grp) 199{ 200 struct pico_mcast_listen ltest = { 201 0 202 }; 203 ltest.mcast_link = *lnk; 204 ltest.mcast_group = *grp; 205 206 if(IS_SOCK_IPV4(s)) 207 return pico_tree_findKey(s->MCASTListen, <est); 208 209#ifdef PICO_SUPPORT_IPV6 210 else if(IS_SOCK_IPV6(s)) { 211 ltest.proto = PICO_PROTO_IPV6; 212 return pico_tree_findKey(s->MCASTListen_ipv6, <est); 213 } 214#endif 215 return NULL; 216} 217static union pico_address *pico_mcast_get_link_address(struct pico_socket *s, union pico_link *mcast_link) 218{ 219 if( IS_SOCK_IPV4(s)) 220 return (union pico_address *) &mcast_link->ipv4.address; 221 222#ifdef PICO_SUPPORT_IPV6 223 if( IS_SOCK_IPV6(s)) 224 return (union pico_address *) &mcast_link->ipv6.address; 225 226#endif 227 return NULL; 228} 229static int8_t pico_mcast_filter_excl_excl(struct pico_mcast_listen *listen) 230{ 231 /* filter = intersection of EXCLUDEs */ 232 /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */ 233 /* remove from the interface EXCLUDE filter any source not in the socket EXCLUDE filter */ 234 struct pico_tree_node *index = NULL, *_tmp = NULL; 235 union pico_address *source = NULL; 236 if(!pico_tree_empty(&MCASTFilter)) { 237 pico_tree_foreach_safe(index, &MCASTFilter, _tmp) 238 { 239 source = pico_tree_findKey(&listen->MCASTSources, index->keyValue); 240 if (!source) 241 pico_tree_delete(&MCASTFilter, index->keyValue); 242 } 243 } 244 245#ifdef PICO_SUPPORT_IPV6 246 if(!pico_tree_empty(&MCASTFilter_ipv6)) { 247 pico_tree_foreach_safe(index, &MCASTFilter_ipv6, _tmp) 248 { 249 source = pico_tree_findKey(&listen->MCASTSources_ipv6, index->keyValue); 250 if (!source) 251 pico_tree_delete(&MCASTFilter_ipv6, index->keyValue); 252 } 253 } 254 255#endif 256 return PICO_IP_MULTICAST_EXCLUDE; 257} 258 259static int8_t pico_mcast_filter_excl_incl(struct pico_mcast_listen *listen) 260{ 261 /* filter = EXCLUDE - INCLUDE */ 262 /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */ 263 /* remove from the interface EXCLUDE filter any source in the socket INCLUDE filter */ 264 struct pico_tree_node *index = NULL, *_tmp = NULL; 265 union pico_address *source = NULL; 266 if(!pico_tree_empty(&listen->MCASTSources)) { 267 pico_tree_foreach_safe(index, &listen->MCASTSources, _tmp) 268 { 269 source = pico_tree_findKey(&MCASTFilter, index->keyValue); 270 if (source) 271 pico_tree_delete(&MCASTFilter, source); 272 } 273 } 274 275#ifdef PICO_SUPPORT_IPV6 276 if(!pico_tree_empty(&listen->MCASTSources_ipv6)) { 277 pico_tree_foreach_safe(index, &listen->MCASTSources_ipv6, _tmp) 278 { 279 source = pico_tree_findKey(&MCASTFilter_ipv6, index->keyValue); 280 if (source) 281 pico_tree_delete(&MCASTFilter_ipv6, source); 282 } 283 } 284 285#endif 286 return PICO_IP_MULTICAST_EXCLUDE; 287} 288 289static int8_t pico_mcast_filter_incl_excl(struct pico_mcast_listen *listen) 290{ 291 /* filter = EXCLUDE - INCLUDE */ 292 /* delete from the interface INCLUDE filter any source NOT in the socket EXCLUDE filter */ 293 struct pico_tree_node *index = NULL, *_tmp = NULL; 294 union pico_address *source = NULL; 295 if(!pico_tree_empty(&listen->MCASTSources)) { 296 pico_tree_foreach_safe(index, &MCASTFilter, _tmp) 297 { 298 source = pico_tree_findKey(&listen->MCASTSources, index->keyValue); 299 if (!source) 300 pico_tree_delete(&MCASTFilter, index->keyValue); 301 } 302 } 303 304#ifdef PICO_SUPPORT_IPV6 305 if(!pico_tree_empty(&listen->MCASTSources_ipv6)) { 306 pico_tree_foreach_safe(index, &MCASTFilter_ipv6, _tmp) 307 { 308 source = pico_tree_findKey(&listen->MCASTSources_ipv6, index->keyValue); 309 if (!source) 310 pico_tree_delete(&MCASTFilter_ipv6, index->keyValue); 311 } 312 } 313 314#endif 315 /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */ 316 317 /* add to the interface EXCLUDE filter any socket source NOT in the former interface INCLUDE filter */ 318 if(!pico_tree_empty(&listen->MCASTSources)) { 319 pico_tree_foreach_safe(index, &listen->MCASTSources, _tmp) 320 { 321 source = pico_tree_insert(&MCASTFilter, index->keyValue); 322 if (source) { 323 if ((void *)source == (void *)&LEAF) 324 return -1; 325 else 326 pico_tree_delete(&MCASTFilter, source); 327 } 328 } 329 } 330 331#ifdef PICO_SUPPORT_IPV6 332 if(!pico_tree_empty(&listen->MCASTSources_ipv6)) { 333 pico_tree_foreach_safe(index, &listen->MCASTSources_ipv6, _tmp) 334 { 335 source = pico_tree_insert(&MCASTFilter_ipv6, index->keyValue); 336 if (source) { 337 if ((void *)source == (void *)&LEAF) 338 return -1; 339 else 340 pico_tree_delete(&MCASTFilter_ipv6, source); 341 } 342 } 343 } 344 345#endif 346 return PICO_IP_MULTICAST_EXCLUDE; 347} 348 349static int8_t pico_mcast_filter_incl_incl(struct pico_mcast_listen *listen) 350{ 351 /* filter = summation of INCLUDEs */ 352 /* mode stays INCLUDE, add all sources to filter */ 353 struct pico_tree_node *index = NULL, *_tmp = NULL; 354 union pico_address *source = NULL; 355 356 if( !pico_tree_empty(&listen->MCASTSources)) { 357 pico_tree_foreach_safe(index, &listen->MCASTSources, _tmp) 358 { 359 source = index->keyValue; 360 if (pico_tree_insert(&MCASTFilter, source) == &LEAF) 361 return -1; 362 } 363 } 364 365#ifdef PICO_SUPPORT_IPV6 366 if( !pico_tree_empty(&listen->MCASTSources_ipv6)) { 367 pico_tree_foreach_safe(index, &listen->MCASTSources_ipv6, _tmp) 368 { 369 source = index->keyValue; 370 if (pico_tree_insert(&MCASTFilter_ipv6, source) == &LEAF) 371 return -1; 372 } 373 } 374 375#endif 376 return PICO_IP_MULTICAST_INCLUDE; 377} 378 379struct pico_mcast_filter_aggregation 380{ 381 int8_t (*call)(struct pico_mcast_listen *); 382}; 383 384static const struct pico_mcast_filter_aggregation mcast_filter_aggr_call[2][2] = 385{ 386 { 387 /* EXCL + EXCL */ {.call = pico_mcast_filter_excl_excl}, 388 /* EXCL + INCL */ {.call = pico_mcast_filter_excl_incl} 389 }, 390 391 { 392 /* INCL + EXCL */ {.call = pico_mcast_filter_incl_excl}, 393 /* INCL + INCL */ {.call = pico_mcast_filter_incl_incl} 394 } 395}; 396 397static int mcast_aggr_validate(int8_t fm, struct pico_mcast_listen *l) 398{ 399 if (!l) 400 return -1; 401 402 if (fm > 1 || fm < 0) 403 return -1; 404 405 if (l->filter_mode > 1) 406 return -1; 407 408 return 0; 409} 410 411 412/* MCASTFilter will be empty if no socket is listening on mcast_group on mcast_link anymore */ 413static int pico_socket_aggregate_mcastfilters(union pico_address *mcast_link, union pico_address *mcast_group) 414{ 415 int8_t filter_mode = PICO_IP_MULTICAST_INCLUDE; 416 struct pico_mcast_listen *listen = NULL; 417 struct pico_socket *mcast_sock = NULL; 418 struct pico_tree_node *index = NULL, *_tmp = NULL; 419 420 /* cleanup old filter */ 421 if(!pico_tree_empty(&MCASTFilter)) { 422 pico_tree_foreach_safe(index, &MCASTFilter, _tmp) 423 { 424 pico_tree_delete(&MCASTFilter, index->keyValue); 425 } 426 } 427 428#ifdef PICO_SUPPORT_IPV6 429 if(!pico_tree_empty(&MCASTFilter_ipv6)) { 430 pico_tree_foreach_safe(index, &MCASTFilter_ipv6, _tmp) 431 { 432 pico_tree_delete(&MCASTFilter_ipv6, index->keyValue); 433 } 434 } 435 436#endif 437 /* construct new filter */ 438 pico_tree_foreach_safe(index, &MCASTSockets, _tmp) 439 { 440 mcast_sock = index->keyValue; 441 listen = listen_find(mcast_sock, mcast_link, mcast_group); 442 if (listen) { 443 if (mcast_aggr_validate(filter_mode, listen) < 0) { 444 pico_err = PICO_ERR_EINVAL; 445 return -1; 446 } 447 448 if (mcast_filter_aggr_call[filter_mode][listen->filter_mode].call) { 449 filter_mode = mcast_filter_aggr_call[filter_mode][listen->filter_mode].call(listen); 450 if (filter_mode > 1 || filter_mode < 0) 451 return -1; 452 } 453 } 454 } 455 return filter_mode; 456} 457 458static int pico_socket_mcast_filter_include(struct pico_mcast_listen *listen, union pico_address *src) 459{ 460 struct pico_tree_node *index = NULL; 461#ifdef PICO_DEBUG_MCAST 462 char tmp_string[PICO_IPV6_STRING]; 463#endif 464 if(!pico_tree_empty(&listen->MCASTSources)) { 465 pico_tree_foreach(index, &listen->MCASTSources) 466 { 467 if (src->ip4.addr == ((union pico_address *)index->keyValue)->ip4.addr) { 468 so_mcast_dbg("MCAST: IP %08X in included socket source list\n", src->ip4.addr); 469 return 0; 470 } 471 } 472 } 473 474#ifdef PICO_SUPPORT_IPV6 475 if(!pico_tree_empty(&listen->MCASTSources_ipv6)) { 476 pico_tree_foreach(index, &listen->MCASTSources_ipv6) 477 { 478 if (memcmp(&src->ip6, &((union pico_address *)index->keyValue)->ip6, sizeof(struct pico_ip6))) { 479#ifdef PICO_DEBUG_MCAST 480 pico_ipv6_to_string(tmp_string, src->ip6.addr); 481 so_mcast_dbg("MCAST: IP %s in included socket source list\n", tmp_string); 482#endif 483 return 0; 484 } 485 } 486 } 487 488#endif 489 /* XXX IPV6 ADDRESS */ 490 so_mcast_dbg("MCAST: IP %08X NOT in included socket source list\n", src->ip4.addr); 491 return -1; 492 493} 494 495static int pico_socket_mcast_filter_exclude(struct pico_mcast_listen *listen, union pico_address *src) 496{ 497 struct pico_tree_node *index = NULL; 498#ifdef PICO_DEBUG_MCAST 499 char tmp_string[PICO_IPV6_STRING]; 500#endif 501 if(!pico_tree_empty(&listen->MCASTSources)) { 502 pico_tree_foreach(index, &listen->MCASTSources) 503 { 504 if (src->ip4.addr == ((union pico_address *)index->keyValue)->ip4.addr) { 505 so_mcast_dbg("MCAST: IP %08X in excluded socket source list\n", src->ip4.addr); 506 return -1; 507 } 508 } 509 } 510 511#ifdef PICO_SUPPORT_IPV6 512 if(!pico_tree_empty(&listen->MCASTSources_ipv6)) { 513 pico_tree_foreach(index, &listen->MCASTSources_ipv6) 514 { 515 if (memcmp(&src->ip6, &((union pico_address *)index->keyValue)->ip6, sizeof(struct pico_ip6))) { 516#ifdef PICO_DEBUG_MCAST 517 pico_ipv6_to_string(tmp_string, src->ip6.addr); 518 so_mcast_dbg("MCAST: IP %s in excluded socket source list\n", tmp_string); 519#endif 520 return 0; 521 } 522 } 523 } 524 525#endif 526 /* XXX IPV6 ADDRESS */ 527 so_mcast_dbg("MCAST: IP %08X NOT in excluded socket source list\n", src->ip4.addr); 528 return 0; 529} 530 531static int pico_socket_mcast_source_filtering(struct pico_mcast_listen *listen, union pico_address *src) 532{ 533 /* perform source filtering */ 534 if (listen->filter_mode == PICO_IP_MULTICAST_INCLUDE) 535 return pico_socket_mcast_filter_include(listen, src); 536 537 if (listen->filter_mode == PICO_IP_MULTICAST_EXCLUDE) 538 return pico_socket_mcast_filter_exclude(listen, src); 539 540 return -1; 541} 542 543static void *pico_socket_mcast_filter_link_get(struct pico_socket *s) 544{ 545 /* check if no multicast enabled on socket */ 546 if (!s->MCASTListen) 547 return NULL; 548 549 if( IS_SOCK_IPV4(s)) { 550 if (!s->local_addr.ip4.addr) 551 return pico_ipv4_get_default_mcastlink(); 552 553 return pico_ipv4_link_get(&s->local_addr.ip4); 554 } 555 556#ifdef PICO_SUPPORT_IPV6 557 else if( IS_SOCK_IPV6(s)) { 558 if (pico_ipv6_is_null_address(&s->local_addr.ip6)) 559 return pico_ipv6_get_default_mcastlink(); 560 561 return pico_ipv6_link_get(&s->local_addr.ip6); 562 } 563#endif 564 return NULL; 565} 566 567int pico_socket_mcast_filter(struct pico_socket *s, union pico_address *mcast_group, union pico_address *src) 568{ 569 void *mcast_link = NULL; 570 struct pico_mcast_listen *listen = NULL; 571 mcast_link = pico_socket_mcast_filter_link_get(s); 572 if (!mcast_link) 573 return -1; 574 575 if(IS_SOCK_IPV4(s)) 576 listen = listen_find(s, (union pico_address *) &((struct pico_ipv4_link*)(mcast_link))->address, mcast_group); 577 578#ifdef PICO_SUPPORT_IPV6 579 else if(IS_SOCK_IPV6(s)) 580 listen = listen_find(s, (union pico_address *)&((struct pico_ipv6_link*)(mcast_link))->address, mcast_group); 581#endif 582 if (!listen) 583 return -1; 584 585 return pico_socket_mcast_source_filtering(listen, src); 586} 587 588 589static struct pico_ipv4_link *get_mcast_link(union pico_address *a) 590{ 591 if (!a->ip4.addr) 592 return pico_ipv4_get_default_mcastlink(); 593 594 return pico_ipv4_link_get(&a->ip4); 595} 596#ifdef PICO_SUPPORT_IPV6 597static struct pico_ipv6_link *get_mcast_link_ipv6(union pico_address *a) 598{ 599 600 if (pico_ipv6_is_null_address(&a->ip6)) { 601 return pico_ipv6_get_default_mcastlink(); 602 } 603 604 return pico_ipv6_link_get(&a->ip6); 605} 606#endif 607 608static int pico_socket_setoption_pre_validation(struct pico_ip_mreq *mreq) 609{ 610 if (!mreq) 611 return -1; 612 613 if (!mreq->mcast_group_addr.ip4.addr) 614 return -1; 615 616 return 0; 617} 618#ifdef PICO_SUPPORT_IPV6 619static int pico_socket_setoption_pre_validation_ipv6(struct pico_ip_mreq *mreq) 620{ 621 if (!mreq) 622 return -1; 623 624 if (pico_ipv6_is_null_address((struct pico_ip6*)&mreq->mcast_group_addr)) 625 return -1; 626 627 return 0; 628} 629#endif 630 631static struct pico_ipv4_link *pico_socket_setoption_validate_mreq(struct pico_ip_mreq *mreq) 632{ 633 if (pico_socket_setoption_pre_validation(mreq) < 0) 634 return NULL; 635 636 if (pico_ipv4_is_unicast(mreq->mcast_group_addr.ip4.addr)) 637 return NULL; 638 639 return get_mcast_link((union pico_address *)&mreq->mcast_link_addr); 640} 641 642#ifdef PICO_SUPPORT_IPV6 643static struct pico_ipv6_link *pico_socket_setoption_validate_mreq_ipv6(struct pico_ip_mreq *mreq) 644{ 645 if (pico_socket_setoption_pre_validation_ipv6(mreq) < 0) 646 return NULL; 647 648 if (pico_ipv6_is_unicast((struct pico_ip6 *)&mreq->mcast_group_addr)) 649 return NULL; 650 651 return get_mcast_link_ipv6((union pico_address *)&mreq->mcast_link_addr); 652} 653#endif 654 655static int pico_socket_setoption_pre_validation_s(struct pico_ip_mreq_source *mreq) 656{ 657 if (!mreq) 658 return -1; 659 660 if (!mreq->mcast_group_addr.ip4.addr) 661 return -1; 662 663 return 0; 664} 665#ifdef PICO_SUPPORT_IPV6 666static int pico_socket_setoption_pre_validation_s_ipv6(struct pico_ip_mreq_source *mreq) 667{ 668 if (!mreq) 669 return -1; 670 671 if (pico_ipv6_is_null_address((struct pico_ip6 *)&mreq->mcast_group_addr)) 672 return -1; 673 674 return 0; 675} 676#endif 677 678static struct pico_ipv4_link *pico_socket_setoption_validate_s_mreq(struct pico_ip_mreq_source *mreq) 679{ 680 if (pico_socket_setoption_pre_validation_s(mreq) < 0) 681 return NULL; 682 683 if (pico_ipv4_is_unicast(mreq->mcast_group_addr.ip4.addr)) 684 return NULL; 685 686 if (!pico_ipv4_is_unicast(mreq->mcast_source_addr.ip4.addr)) 687 return NULL; 688 689 return get_mcast_link((union pico_address *)&mreq->mcast_link_addr); 690} 691#ifdef PICO_SUPPORT_IPV6 692static struct pico_ipv6_link *pico_socket_setoption_validate_s_mreq_ipv6(struct pico_ip_mreq_source *mreq) 693{ 694 if (pico_socket_setoption_pre_validation_s_ipv6(mreq) < 0) { 695 return NULL; 696 } 697 698 if (pico_ipv6_is_unicast((struct pico_ip6 *)&mreq->mcast_group_addr)) { 699 return NULL; 700 } 701 702 if (!pico_ipv6_is_unicast((struct pico_ip6 *)&mreq->mcast_source_addr)) { 703 return NULL; 704 } 705 706 return get_mcast_link_ipv6(&mreq->mcast_link_addr); 707} 708#endif 709 710static struct pico_ipv4_link *setop_multicast_link_search(void *value, int bysource) 711{ 712 713 struct pico_ip_mreq *mreq = NULL; 714 struct pico_ipv4_link *mcast_link = NULL; 715 struct pico_ip_mreq_source *mreq_src = NULL; 716 if (!bysource) { 717 mreq = (struct pico_ip_mreq *)value; 718 mcast_link = pico_socket_setoption_validate_mreq(mreq); 719 if (!mcast_link) 720 return NULL; 721 722 if (!mreq->mcast_link_addr.ip4.addr) 723 mreq->mcast_link_addr.ip4.addr = mcast_link->address.addr; 724 } else { 725 mreq_src = (struct pico_ip_mreq_source *)value; 726 if (!mreq_src) { 727 return NULL; 728 } 729 730 mcast_link = pico_socket_setoption_validate_s_mreq(mreq_src); 731 if (!mcast_link) { 732 return NULL; 733 } 734 735 if (!mreq_src->mcast_link_addr.ip4.addr) 736 mreq_src->mcast_link_addr.ip4 = mcast_link->address; 737 } 738 739 return mcast_link; 740} 741#ifdef PICO_SUPPORT_IPV6 742static struct pico_ipv6_link *setop_multicast_link_search_ipv6(void *value, int bysource) 743{ 744 struct pico_ip_mreq *mreq = NULL; 745 struct pico_ipv6_link *mcast_link = NULL; 746 struct pico_ip_mreq_source *mreq_src = NULL; 747 if (!bysource) { 748 mreq = (struct pico_ip_mreq *)value; 749 mcast_link = pico_socket_setoption_validate_mreq_ipv6(mreq); 750 if (!mcast_link) { 751 return NULL; 752 } 753 754 if (pico_ipv6_is_null_address(&mreq->mcast_link_addr.ip6)) 755 mreq->mcast_link_addr.ip6 = mcast_link->address; 756 } else { 757 mreq_src = (struct pico_ip_mreq_source *)value; 758 if (!mreq_src) { 759 return NULL; 760 } 761 762 mcast_link = pico_socket_setoption_validate_s_mreq_ipv6(mreq_src); 763 if (!mcast_link) { 764 return NULL; 765 } 766 767 if (pico_ipv6_is_null_address(&mreq_src->mcast_link_addr.ip6)) 768 mreq_src->mcast_link_addr.ip6 = mcast_link->address; 769 } 770 771 return mcast_link; 772} 773#endif 774static int setop_verify_listen_tree(struct pico_socket *s, int alloc) 775{ 776 if(!alloc) 777 return -1; 778 779 if( IS_SOCK_IPV4(s)) { 780 781 s->MCASTListen = PICO_ZALLOC(sizeof(struct pico_tree)); 782 if (!s->MCASTListen) { 783 pico_err = PICO_ERR_ENOMEM; 784 return -1; 785 } 786 787 s->MCASTListen->root = &LEAF; 788 s->MCASTListen->compare = mcast_listen_cmp; 789 return 0; 790 } 791 792#ifdef PICO_SUPPORT_IPV6 793 else if( IS_SOCK_IPV6(s)) { 794 s->MCASTListen_ipv6 = PICO_ZALLOC(sizeof(struct pico_tree)); 795 if (!s->MCASTListen_ipv6) { 796 pico_err = PICO_ERR_ENOMEM; 797 return -1; 798 } 799 800 s->MCASTListen_ipv6->root = &LEAF; 801 s->MCASTListen_ipv6->compare = mcast_listen_cmp_ipv6; 802 return 0; 803 804 } 805#endif 806 return -1; 807} 808 809 810static void *setopt_multicast_check(struct pico_socket *s, void *value, int alloc, int bysource) 811{ 812 void *mcast_link = NULL; 813 struct pico_tree *listen_tree = mcast_get_listen_tree(s); 814 if (!value) { 815 pico_err = PICO_ERR_EINVAL; 816 return NULL; 817 } 818 819 if(IS_SOCK_IPV4(s)) 820 mcast_link = setop_multicast_link_search(value, bysource); 821 822#ifdef PICO_SUPPORT_IPV6 823 else if(IS_SOCK_IPV6(s)) 824 mcast_link = setop_multicast_link_search_ipv6(value, bysource); 825#endif 826 if (!mcast_link) { 827 pico_err = PICO_ERR_EINVAL; 828 return NULL; 829 } 830 831 if (!listen_tree) { /* No RBTree allocated yet */ 832 if (setop_verify_listen_tree(s, alloc) < 0) { 833 return NULL; 834 } 835 } 836 837 return mcast_link; 838} 839 840void pico_multicast_delete(struct pico_socket *s) 841{ 842 int filter_mode; 843 struct pico_tree_node *index = NULL, *_tmp = NULL, *index2 = NULL, *_tmp2 = NULL; 844 struct pico_mcast_listen *listen = NULL; 845 union pico_address *source = NULL; 846 struct pico_tree *tree, *listen_tree; 847 struct pico_mcast mcast; 848 listen_tree = mcast_get_listen_tree(s); 849 if(listen_tree) { 850 pico_tree_delete(&MCASTSockets, s); 851 pico_tree_foreach_safe(index, listen_tree, _tmp) 852 { 853 listen = index->keyValue; 854 mcast.listen = listen; 855 tree = mcast_get_src_tree(s, &mcast); 856 if (tree) { 857 pico_tree_foreach_safe(index2, tree, _tmp2) 858 { 859 source = index2->keyValue; 860 pico_tree_delete(tree, source); 861 PICO_FREE(source); 862 } 863 } 864 865 filter_mode = pico_socket_aggregate_mcastfilters((union pico_address *)&listen->mcast_link, (union pico_address *)&listen->mcast_group); 866 if (filter_mode >= 0) { 867 if(IS_SOCK_IPV4(s)) 868 pico_ipv4_mcast_leave(&listen->mcast_link.ip4, &listen->mcast_group.ip4, 1, (uint8_t)filter_mode, &MCASTFilter); 869 870#ifdef PICO_SUPPORT_IPV6 871 else if(IS_SOCK_IPV6(s)) 872 pico_ipv6_mcast_leave(&listen->mcast_link.ip6, &listen->mcast_group.ip6, 1, (uint8_t)filter_mode, &MCASTFilter_ipv6); 873#endif 874 } 875 876 pico_tree_delete(listen_tree, listen); 877 PICO_FREE(listen); 878 } 879 PICO_FREE(listen_tree); 880 mcast_set_listen_tree_p_null(s); 881 } 882} 883 884 885int pico_getsockopt_mcast(struct pico_socket *s, int option, void *value) 886{ 887 switch(option) { 888 case PICO_IP_MULTICAST_IF: 889 pico_err = PICO_ERR_EOPNOTSUPP; 890 return -1; 891 892 case PICO_IP_MULTICAST_TTL: 893 if (s->proto->proto_number == PICO_PROTO_UDP) { 894 pico_udp_get_mc_ttl(s, (uint8_t *) value); 895 } else { 896 *(uint8_t *)value = 0; 897 pico_err = PICO_ERR_EINVAL; 898 return -1; 899 } 900 901 break; 902 903 case PICO_IP_MULTICAST_LOOP: 904 if (s->proto->proto_number == PICO_PROTO_UDP) { 905 *(uint8_t *)value = (uint8_t)PICO_SOCKET_GETOPT(s, PICO_SOCKET_OPT_MULTICAST_LOOP); 906 } else { 907 *(uint8_t *)value = 0; 908 pico_err = PICO_ERR_EINVAL; 909 return -1; 910 } 911 912 break; 913 default: 914 pico_err = PICO_ERR_EINVAL; 915 return -1; 916 } 917 918 return 0; 919} 920 921static int mcast_so_loop(struct pico_socket *s, void *value) 922{ 923 uint8_t val = (*(uint8_t *)value); 924 if (val == 0u) { 925 PICO_SOCKET_SETOPT_DIS(s, PICO_SOCKET_OPT_MULTICAST_LOOP); 926 return 0; 927 } else if (val == 1u) { 928 PICO_SOCKET_SETOPT_EN(s, PICO_SOCKET_OPT_MULTICAST_LOOP); 929 return 0; 930 } 931 932 pico_err = PICO_ERR_EINVAL; 933 return -1; 934} 935static int mcast_get_param(struct pico_mcast *mcast, struct pico_socket *s, void *value, int alloc, int by_source) 936{ 937 if(by_source) 938 mcast->mreq_s = (struct pico_ip_mreq_source *)value; 939 else 940 mcast->mreq = (struct pico_ip_mreq *)value; 941 942 mcast->mcast_link = setopt_multicast_check(s, value, alloc, by_source); 943 if (!mcast->mcast_link) 944 return -1; 945 946 mcast->address = pico_mcast_get_link_address(s, mcast->mcast_link); 947 if(by_source) 948 mcast->listen = listen_find(s, &(mcast->mreq_s)->mcast_link_addr, &mcast->mreq_s->mcast_group_addr); 949 else 950 mcast->listen = listen_find(s, &(mcast->mreq)->mcast_link_addr, &mcast->mreq->mcast_group_addr); 951 952 return 0; 953} 954static int mcast_so_addm(struct pico_socket *s, void *value) 955{ 956 int filter_mode = 0; 957 struct pico_mcast mcast; 958 struct pico_tree *tree, *listen_tree; 959 if(mcast_get_param(&mcast, s, value, 1, 0) < 0) 960 return -1; 961 962 if (mcast.listen) { 963 if (mcast.listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) { 964 so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n"); 965 } else { 966 so_mcast_dbg("pico_socket_setoption: ERROR duplicate PICO_IP_ADD_MEMBERSHIP\n"); 967 } 968 969 pico_err = PICO_ERR_EINVAL; 970 return -1; 971 } 972 973 mcast.listen = PICO_ZALLOC(sizeof(struct pico_mcast_listen)); 974 if (!mcast.listen) { 975 pico_err = PICO_ERR_ENOMEM; 976 return -1; 977 } 978 979 mcast.listen->filter_mode = PICO_IP_MULTICAST_EXCLUDE; 980 mcast.listen->mcast_link = mcast.mreq->mcast_link_addr; 981 mcast.listen->mcast_group = mcast.mreq->mcast_group_addr; 982 mcast.listen->proto = s->net->proto_number; 983 984 tree = mcast_get_src_tree(s, &mcast); 985 listen_tree = mcast_get_listen_tree(s); 986#ifdef PICO_SUPPORT_IPV6 987 if( IS_SOCK_IPV6(s)) 988 mcast.listen->proto = PICO_PROTO_IPV6; 989 990#endif 991 tree->root = &LEAF; 992 if (pico_tree_insert(listen_tree, mcast.listen)) { 993 PICO_FREE(mcast.listen); 994 return -1; 995 } 996 997 if (pico_tree_insert(&MCASTSockets, s) == &LEAF) { 998 pico_tree_delete(listen_tree, mcast.listen); 999 PICO_FREE(mcast.listen); 1000 return -1; 1001 } 1002 1003 filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq->mcast_group_addr); 1004 if (filter_mode < 0) 1005 return -1; 1006 1007 so_mcast_dbg("PICO_IP_ADD_MEMBERSHIP - success, added %p\n", s); 1008 if(IS_SOCK_IPV4(s)) 1009 return pico_ipv4_mcast_join((struct pico_ip4*)&mcast.mreq->mcast_link_addr, (struct pico_ip4*) &mcast.mreq->mcast_group_addr, 1, (uint8_t)filter_mode, &MCASTFilter); 1010 1011#ifdef PICO_SUPPORT_IPV6 1012 else if(IS_SOCK_IPV6(s)) { 1013 return pico_ipv6_mcast_join((struct pico_ip6*)&mcast.mreq->mcast_link_addr, (struct pico_ip6*)&mcast.mreq->mcast_group_addr, 1, (uint8_t)filter_mode, &MCASTFilter_ipv6); 1014 } 1015#endif 1016 return -1; 1017} 1018 1019static int mcast_so_dropm(struct pico_socket *s, void *value) 1020{ 1021 int filter_mode = 0; 1022 union pico_address *source = NULL; 1023 struct pico_tree_node *_tmp, *index; 1024 struct pico_mcast mcast; 1025 struct pico_tree *listen_tree, *tree; 1026 if(mcast_get_param(&mcast, s, value, 0, 0) < 0) 1027 return -1; 1028 1029 if (!mcast.listen) { 1030 so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_MEMBERSHIP before PICO_IP_ADD_MEMBERSHIP/SOURCE_MEMBERSHIP\n"); 1031 pico_err = PICO_ERR_EADDRNOTAVAIL; 1032 return -1; 1033 } 1034 1035 tree = mcast_get_src_tree(s, &mcast); 1036 listen_tree = mcast_get_listen_tree(s); 1037 1038 pico_tree_foreach_safe(index, tree, _tmp) 1039 { 1040 source = index->keyValue; 1041 pico_tree_delete(tree, source); 1042 } 1043 pico_tree_delete(listen_tree, mcast.listen); 1044 PICO_FREE(mcast.listen); 1045 if (pico_tree_empty(listen_tree)) { 1046 PICO_FREE(listen_tree); 1047 mcast_set_listen_tree_p_null(s); 1048 pico_tree_delete(&MCASTSockets, s); 1049 } 1050 1051 filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq->mcast_group_addr); 1052 if (filter_mode < 0) 1053 return -1; 1054 1055 if(IS_SOCK_IPV4(s)) 1056 return pico_ipv4_mcast_leave((struct pico_ip4*) &mcast.mreq->mcast_link_addr, (struct pico_ip4 *) &mcast.mreq->mcast_group_addr, 1, (uint8_t)filter_mode, &MCASTFilter); 1057 1058#ifdef PICO_SUPPORT_IPV6 1059 else if(IS_SOCK_IPV6(s)) { } 1060 return pico_ipv6_mcast_leave((struct pico_ip6*)&mcast.mreq->mcast_link_addr, (struct pico_ip6*)&mcast.mreq->mcast_group_addr, 1, (uint8_t)filter_mode, &MCASTFilter_ipv6); 1061#endif 1062 return -1; 1063} 1064 1065static int mcast_so_unblock_src(struct pico_socket *s, void *value) 1066{ 1067 int filter_mode = 0; 1068 union pico_address stest, *source = NULL; 1069 struct pico_mcast mcast; 1070 if(mcast_get_param(&mcast, s, value, 0, 1) < 0) 1071 return -1; 1072 1073 memset(&stest, 0, sizeof(union pico_address)); 1074 if (!mcast.listen) { 1075 so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_UNBLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n"); 1076 pico_err = PICO_ERR_EINVAL; 1077 return -1; 1078 } 1079 1080 if (mcast.listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) { 1081 so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n"); 1082 pico_err = PICO_ERR_EINVAL; 1083 return -1; 1084 } 1085 1086 stest = mcast.mreq_s->mcast_source_addr; 1087 if( IS_SOCK_IPV4(s)) 1088 source = pico_tree_findKey(&mcast.listen->MCASTSources, &stest); 1089 1090#ifdef PICO_SUPPORT_IPV6 1091 else if( IS_SOCK_IPV6(s)) 1092 source = pico_tree_findKey(&mcast.listen->MCASTSources_ipv6, &stest); 1093#endif 1094 if (!source) { 1095 so_mcast_dbg("pico_socket_setoption: ERROR address to unblock not in source list\n"); 1096 pico_err = PICO_ERR_EADDRNOTAVAIL; 1097 return -1; 1098 } 1099 1100 if( IS_SOCK_IPV4(s)) 1101 pico_tree_delete(&mcast.listen->MCASTSources, source); 1102 1103#ifdef PICO_SUPPORT_IPV6 1104 else if( IS_SOCK_IPV6(s)) 1105 pico_tree_delete(&mcast.listen->MCASTSources_ipv6, source); 1106#endif 1107 1108 filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr); 1109 if (filter_mode < 0) 1110 return -1; 1111 1112 if(IS_SOCK_IPV4(s)) 1113 return pico_ipv4_mcast_leave((struct pico_ip4 *)&mcast.mreq_s->mcast_link_addr, (struct pico_ip4*) &mcast.mreq_s->mcast_group_addr, 0, (uint8_t)filter_mode, &MCASTFilter); 1114 1115#ifdef PICO_SUPPORT_IPV6 1116 else if(IS_SOCK_IPV6(s)) { } 1117 return pico_ipv6_mcast_leave((struct pico_ip6*)&mcast.mreq_s->mcast_link_addr, (struct pico_ip6*)&mcast.mreq_s->mcast_group_addr, 0, (uint8_t)filter_mode, &MCASTFilter_ipv6); 1118#endif 1119 return -1; 1120} 1121 1122static int mcast_so_block_src(struct pico_socket *s, void *value) 1123{ 1124 int filter_mode = 0; 1125 union pico_address stest, *source = NULL; 1126 struct pico_mcast mcast; 1127 if(mcast_get_param(&mcast, s, value, 0, 1) < 0) 1128 return -1; 1129 1130 memset(&stest, 0, sizeof(union pico_address)); 1131 if (!mcast.listen) { 1132 dbg("pico_socket_setoption: ERROR PICO_IP_BLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n"); 1133 pico_err = PICO_ERR_EINVAL; 1134 return -1; 1135 } 1136 1137 if (mcast.listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) { 1138 so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n"); 1139 pico_err = PICO_ERR_EINVAL; 1140 return -1; 1141 } 1142 1143 stest = mcast.mreq_s->mcast_source_addr; 1144 if( IS_SOCK_IPV4(s)) 1145 source = pico_tree_findKey(&mcast.listen->MCASTSources, &stest); 1146 1147#ifdef PICO_SUPPORT_IPV6 1148 else if( IS_SOCK_IPV6(s)) 1149 source = pico_tree_findKey(&mcast.listen->MCASTSources_ipv6, &stest); 1150#endif 1151 if (source) { 1152 so_mcast_dbg("pico_socket_setoption: ERROR address to block already in source list\n"); 1153 pico_err = PICO_ERR_ENOMEM; 1154 return -1; 1155 } 1156 1157 source = PICO_ZALLOC(sizeof(union pico_address)); 1158 if (!source) { 1159 pico_err = PICO_ERR_ENOMEM; 1160 return -1; 1161 } 1162 1163 *source = mcast.mreq_s->mcast_source_addr; 1164 if( IS_SOCK_IPV4(s)) { 1165 if (pico_tree_insert(&mcast.listen->MCASTSources, source)) { 1166 PICO_FREE(source); 1167 return -1; 1168 } 1169 } 1170 1171#ifdef PICO_SUPPORT_IPV6 1172 else if( IS_SOCK_IPV6(s)) 1173 if (pico_tree_insert(&mcast.listen->MCASTSources_ipv6, source)) { 1174 PICO_FREE(source); 1175 return -1; 1176 } 1177#endif 1178 1179 filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr); 1180 if (filter_mode < 0) 1181 return -1; 1182 1183 if(IS_SOCK_IPV4(s)) 1184 return pico_ipv4_mcast_join((struct pico_ip4 *) &mcast.mreq_s->mcast_link_addr, (struct pico_ip4*)&mcast.mreq_s->mcast_group_addr, 0, (uint8_t)filter_mode, &MCASTFilter); 1185 1186#ifdef PICO_SUPPORT_IPV6 1187 else if(IS_SOCK_IPV6(s)) { } 1188 return pico_ipv6_mcast_join((struct pico_ip6 *)&mcast.mreq_s->mcast_link_addr, (struct pico_ip6*)&mcast.mreq_s->mcast_group_addr, 0, (uint8_t)filter_mode, &MCASTFilter_ipv6); 1189#endif 1190 return -1; 1191} 1192 1193static int mcast_so_addsrcm(struct pico_socket *s, void *value) 1194{ 1195 int filter_mode = 0, reference_count = 0; 1196 union pico_address stest, *source = NULL; 1197 struct pico_mcast mcast; 1198 struct pico_tree *tree, *listen_tree; 1199 if(mcast_get_param(&mcast, s, value, 1, 1) < 0) 1200 return -1; 1201 1202 memset(&stest, 0, sizeof(union pico_address)); 1203 listen_tree = mcast_get_listen_tree(s); 1204 if (mcast.listen) { 1205 tree = mcast_get_src_tree(s, &mcast); 1206 if (mcast.listen->filter_mode != PICO_IP_MULTICAST_INCLUDE) { 1207 so_mcast_dbg("pico_socket_setoption: ERROR source-specific multicast (include) on any-source multicast (exclude)\n"); 1208 pico_err = PICO_ERR_EINVAL; 1209 return -1; 1210 } 1211 1212 stest = mcast.mreq_s->mcast_source_addr; 1213 source = pico_tree_findKey(tree, &stest); 1214 if (source) { 1215 so_mcast_dbg("pico_socket_setoption: ERROR source address to allow already in source list\n"); 1216 pico_err = PICO_ERR_EADDRNOTAVAIL; 1217 return -1; 1218 } 1219 1220 source = PICO_ZALLOC(sizeof(union pico_address)); 1221 if (!source) { 1222 pico_err = PICO_ERR_ENOMEM; 1223 return -1; 1224 } 1225 1226 *source = mcast.mreq_s->mcast_source_addr; 1227 if (pico_tree_insert(tree, source)) { 1228 PICO_FREE(source); 1229 return -1; 1230 } 1231 1232 } else { 1233 mcast.listen = PICO_ZALLOC(sizeof(struct pico_mcast_listen)); 1234 if (!mcast.listen) { 1235 pico_err = PICO_ERR_ENOMEM; 1236 return -1; 1237 } 1238 1239 tree = mcast_get_src_tree(s, &mcast); 1240 mcast.listen->filter_mode = PICO_IP_MULTICAST_INCLUDE; 1241 mcast.listen->mcast_link = mcast.mreq_s->mcast_link_addr; 1242 mcast.listen->mcast_group = mcast.mreq_s->mcast_group_addr; 1243 tree->root = &LEAF; 1244 source = PICO_ZALLOC(sizeof(union pico_address)); 1245 if (!source) { 1246 PICO_FREE(mcast.listen); 1247 pico_err = PICO_ERR_ENOMEM; 1248 return -1; 1249 } 1250 1251#ifdef PICO_SUPPORT_IPV6 1252 if( IS_SOCK_IPV6(s)) 1253 mcast.listen->proto = PICO_PROTO_IPV6; 1254 1255#endif 1256 *source = mcast.mreq_s->mcast_source_addr; 1257 if (pico_tree_insert(tree, source)) { 1258 PICO_FREE(mcast.listen); 1259 PICO_FREE(source); 1260 return -1; 1261 } 1262 1263 if (pico_tree_insert(listen_tree, mcast.listen)) { 1264 pico_tree_delete(tree, source); 1265 PICO_FREE(source); 1266 PICO_FREE(mcast.listen); 1267 return -1; 1268 } 1269 reference_count = 1; 1270 } 1271 1272 if (pico_tree_insert(&MCASTSockets, s) == &LEAF) { 1273 return -1; 1274 } 1275 1276 filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr); 1277 if (filter_mode < 0) 1278 return -1; 1279 1280 if(IS_SOCK_IPV4(s)) 1281 return pico_ipv4_mcast_join((struct pico_ip4 *)&mcast.mreq_s->mcast_link_addr, (struct pico_ip4*)&mcast.mreq_s->mcast_group_addr, (uint8_t)reference_count, (uint8_t)filter_mode, &MCASTFilter); 1282 1283#ifdef PICO_SUPPORT_IPV6 1284 else if(IS_SOCK_IPV6(s)) { } 1285 return pico_ipv6_mcast_join((struct pico_ip6 *) &mcast.mreq_s->mcast_link_addr, (struct pico_ip6*)&mcast.mreq_s->mcast_group_addr, (uint8_t)reference_count, (uint8_t)filter_mode, &MCASTFilter_ipv6); 1286#endif 1287 return -1; 1288} 1289 1290static int mcast_so_dropsrcm(struct pico_socket *s, void *value) 1291{ 1292 int filter_mode = 0, reference_count = 0; 1293 union pico_address stest, *source = NULL; 1294 struct pico_mcast mcast; 1295 struct pico_tree *tree, *listen_tree; 1296 if(mcast_get_param(&mcast, s, value, 0, 1) < 0) 1297 return -1; 1298 1299 memset(&stest, 0, sizeof(union pico_address)); 1300 listen_tree = mcast_get_listen_tree(s); 1301 if (!mcast.listen) { 1302 so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_SOURCE_MEMBERSHIP before PICO_IP_ADD_SOURCE_MEMBERSHIP\n"); 1303 pico_err = PICO_ERR_EADDRNOTAVAIL; 1304 return -1; 1305 } 1306 1307 if (mcast.listen->filter_mode != PICO_IP_MULTICAST_INCLUDE) { 1308 so_mcast_dbg("pico_socket_setoption: ERROR source-specific multicast (include) on any-source multicast (exclude)\n"); 1309 pico_err = PICO_ERR_EINVAL; 1310 return -1; 1311 } 1312 1313 tree = mcast_get_src_tree(s, &mcast); 1314 stest = mcast.mreq_s->mcast_source_addr; 1315 source = pico_tree_findKey(tree, &stest); 1316 if (!source) { 1317 so_mcast_dbg("pico_socket_setoption: ERROR address to drop not in source list\n"); 1318 pico_err = PICO_ERR_EADDRNOTAVAIL; 1319 return -1; 1320 } 1321 1322 pico_tree_delete(tree, source); 1323 if (pico_tree_empty(tree)) { /* 1 if empty, 0 otherwise */ 1324 reference_count = 1; 1325 pico_tree_delete(listen_tree, mcast.listen); 1326 PICO_FREE(mcast.listen); 1327 if (pico_tree_empty(listen_tree)) { 1328 PICO_FREE(listen_tree); 1329 mcast_set_listen_tree_p_null(s); 1330 pico_tree_delete(&MCASTSockets, s); 1331 } 1332 } 1333 1334 filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr); 1335 if (filter_mode < 0) 1336 return -1; 1337 1338 if(IS_SOCK_IPV4(s)) 1339 return pico_ipv4_mcast_leave((struct pico_ip4 *) &mcast.mreq_s->mcast_link_addr, (struct pico_ip4*)&mcast.mreq_s->mcast_group_addr, (uint8_t)reference_count, (uint8_t)filter_mode, &MCASTFilter); 1340 1341#ifdef PICO_SUPPORT_IPV6 1342 else if(IS_SOCK_IPV6(s)) { } 1343 return pico_ipv6_mcast_leave((struct pico_ip6 *)&mcast.mreq_s->mcast_link_addr, (struct pico_ip6*)&mcast.mreq_s->mcast_group_addr, (uint8_t)reference_count, (uint8_t)filter_mode, &MCASTFilter_ipv6); 1344#endif 1345 return -1; 1346} 1347 1348 1349struct pico_setsockopt_mcast_call 1350{ 1351 int option; 1352 int (*call)(struct pico_socket *, void *); 1353}; 1354 1355static const struct pico_setsockopt_mcast_call mcast_so_calls[1 + PICO_IP_DROP_SOURCE_MEMBERSHIP - PICO_IP_MULTICAST_IF] = 1356{ 1357 { PICO_IP_MULTICAST_IF, NULL }, 1358 { PICO_IP_MULTICAST_TTL, pico_udp_set_mc_ttl }, 1359 { PICO_IP_MULTICAST_LOOP, mcast_so_loop }, 1360 { PICO_IP_ADD_MEMBERSHIP, mcast_so_addm }, 1361 { PICO_IP_DROP_MEMBERSHIP, mcast_so_dropm }, 1362 { PICO_IP_UNBLOCK_SOURCE, mcast_so_unblock_src }, 1363 { PICO_IP_BLOCK_SOURCE, mcast_so_block_src }, 1364 { PICO_IP_ADD_SOURCE_MEMBERSHIP, mcast_so_addsrcm }, 1365 { PICO_IP_DROP_SOURCE_MEMBERSHIP, mcast_so_dropsrcm } 1366}; 1367 1368 1369static int mcast_so_check_socket(struct pico_socket *s) 1370{ 1371 pico_err = PICO_ERR_EINVAL; 1372 if (!s) 1373 return -1; 1374 1375 if (!s->proto) 1376 return -1; 1377 1378 if (s->proto->proto_number != PICO_PROTO_UDP) 1379 return -1; 1380 1381 pico_err = PICO_ERR_NOERR; 1382 return 0; 1383} 1384 1385int pico_setsockopt_mcast(struct pico_socket *s, int option, void *value) 1386{ 1387 int arrayn = option - PICO_IP_MULTICAST_IF; 1388 if (option < PICO_IP_MULTICAST_IF || option > PICO_IP_DROP_SOURCE_MEMBERSHIP) { 1389 pico_err = PICO_ERR_EOPNOTSUPP; 1390 return -1; 1391 } 1392 1393 if (mcast_so_check_socket(s) < 0) 1394 return -1; 1395 1396 if (!mcast_so_calls[arrayn].call) { 1397 pico_err = PICO_ERR_EOPNOTSUPP; 1398 return -1; 1399 } 1400 1401 return (mcast_so_calls[arrayn].call(s, value)); 1402} 1403 1404int pico_udp_set_mc_ttl(struct pico_socket *s, void *_ttl) 1405{ 1406 struct pico_socket_udp *u; 1407 uint8_t ttl = *(uint8_t *)_ttl; 1408 if(!s) { 1409 pico_err = PICO_ERR_EINVAL; 1410 return -1; 1411 } 1412 1413 u = (struct pico_socket_udp *) s; 1414 u->mc_ttl = ttl; 1415 return 0; 1416} 1417 1418int pico_udp_get_mc_ttl(struct pico_socket *s, uint8_t *ttl) 1419{ 1420 struct pico_socket_udp *u; 1421 if(!s) 1422 return -1; 1423 1424 u = (struct pico_socket_udp *) s; 1425 *ttl = u->mc_ttl; 1426 return 0; 1427} 1428#else 1429int pico_udp_set_mc_ttl(struct pico_socket *s, void *_ttl) 1430{ 1431 IGNORE_PARAMETER(s); 1432 IGNORE_PARAMETER(_ttl); 1433 pico_err = PICO_ERR_EPROTONOSUPPORT; 1434 return -1; 1435} 1436 1437int pico_udp_get_mc_ttl(struct pico_socket *s, uint8_t *ttl) 1438{ 1439 IGNORE_PARAMETER(s); 1440 IGNORE_PARAMETER(ttl); 1441 pico_err = PICO_ERR_EPROTONOSUPPORT; 1442 return -1; 1443} 1444 1445int pico_socket_mcast_filter(struct pico_socket *s, union pico_address *mcast_group, union pico_address *src) 1446{ 1447 IGNORE_PARAMETER(s); 1448 IGNORE_PARAMETER(mcast_group); 1449 IGNORE_PARAMETER(src); 1450 pico_err = PICO_ERR_EPROTONOSUPPORT; 1451 return -1; 1452} 1453 1454void pico_multicast_delete(struct pico_socket *s) 1455{ 1456 (void)s; 1457} 1458 1459int pico_getsockopt_mcast(struct pico_socket *s, int option, void *value) 1460{ 1461 (void)s; 1462 (void)option; 1463 (void)value; 1464 pico_err = PICO_ERR_EPROTONOSUPPORT; 1465 return -1; 1466} 1467 1468int pico_setsockopt_mcast(struct pico_socket *s, int option, void *value) 1469{ 1470 (void)s; 1471 (void)option; 1472 (void)value; 1473 pico_err = PICO_ERR_EPROTONOSUPPORT; 1474 return -1; 1475 1476} 1477#endif /* PICO_SUPPORT_MCAST */ 1478 1479