Free (GPLv2) TCP/IP stack developed by TASS Belgium

Dependents:   lpc1768-picotcp-demo ZeroMQ_PicoTCP_Publisher_demo TCPSocket_HelloWorld_PicoTCP Pico_TCP_UDP_Test ... more

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers pico_socket_multicast.c Source File

pico_socket_multicast.c

00001 #include "pico_config.h"
00002 #include "pico_stack.h"
00003 #include "pico_socket.h"
00004 #include "pico_socket_multicast.h"
00005 #include "pico_tree.h"
00006 #include "pico_ipv4.h"
00007 #include "pico_ipv6.h"
00008 #include "pico_udp.h"
00009 
00010 #ifdef PICO_SUPPORT_MCAST
00011 # define so_mcast_dbg(...) do { }while(0) /* ip_mcast_dbg in pico_ipv4.c */
00012 /* #define so_mcast_dbg dbg */
00013 
00014 /*                       socket
00015 *                         |
00016 *                    MCASTListen                      
00017 *                    |    |     |
00018 *         ------------    |     ------------
00019 *         |               |                |
00020 *   MCASTSources    MCASTSources     MCASTSources
00021 *   |  |  |  |      |  |  |  |       |  |  |  |
00022 *   S  S  S  S      S  S  S  S       S  S  S  S
00023 *
00024 *   MCASTListen: RBTree(mcast_link, mcast_group)
00025 *   MCASTSources: RBTree(source)
00026 */
00027 struct pico_mcast_listen
00028 {
00029     uint8_t filter_mode;
00030     union pico_address mcast_link;
00031     union pico_address mcast_group;
00032     struct pico_tree MCASTSources;
00033     struct pico_tree MCASTSources_ipv6;
00034     uint16_t proto;
00035 };
00036 //Parameters
00037 struct pico_mcast 
00038 {
00039     struct pico_socket *s;
00040     struct pico_ip_mreq *mreq;
00041     struct pico_ip_mreq_source *mreq_s;
00042     union pico_address *address;
00043     union pico_link *mcast_link;
00044     struct pico_mcast_listen *listen;
00045 };
00046 static int mcast_listen_link_cmp(struct pico_mcast_listen *a, struct pico_mcast_listen *b)
00047 {
00048 
00049     if (a->proto < b->proto)
00050         return -1;
00051 
00052     if (a->proto > b->proto)
00053         return 1;
00054 
00055     return pico_address_compare(&a->mcast_link, &b->mcast_link, a->proto);
00056 }
00057 
00058 static int mcast_listen_grp_cmp(struct pico_mcast_listen *a, struct pico_mcast_listen *b)
00059 {
00060     if (a->mcast_group.ip4.addr < b->mcast_group.ip4.addr)
00061         return -1;
00062 
00063     if (a->mcast_group.ip4.addr > b->mcast_group.ip4.addr)
00064         return 1;
00065 
00066     return mcast_listen_link_cmp(a, b);
00067 }
00068 #ifdef PICO_SUPPORT_IPV6
00069 static int mcast_listen_grp_cmp_ipv6(struct pico_mcast_listen *a, struct pico_mcast_listen *b)
00070 {
00071     int tmp = memcmp(&a->mcast_group.ip6, &b->mcast_group.ip6, sizeof(struct pico_ip6));
00072     if(!tmp)
00073         return mcast_listen_link_cmp(a, b);
00074     return tmp;
00075 }
00076 #endif
00077 
00078 static int mcast_listen_cmp(void *ka, void *kb)
00079 {
00080     struct pico_mcast_listen *a = ka, *b = kb;
00081     if (a->proto < b->proto)
00082         return -1;
00083 
00084     if (a->proto > b->proto)
00085         return 1;
00086 
00087     return mcast_listen_grp_cmp(a, b);
00088 }
00089 #ifdef PICO_SUPPORT_IPV6
00090 static int mcast_listen_cmp_ipv6(void *ka, void *kb)
00091 {
00092     struct pico_mcast_listen *a = ka, *b = kb;
00093     if (a->proto < b->proto)
00094         return -1;
00095 
00096     if (a->proto > b->proto)
00097         return 1;
00098 
00099     return mcast_listen_grp_cmp_ipv6(a, b);
00100 }
00101 #endif
00102 static int mcast_sources_cmp(void *ka, void *kb)
00103 {
00104     union pico_address *a = ka, *b = kb;
00105     if (a->ip4.addr < b->ip4.addr)
00106         return -1;
00107 
00108     if (a->ip4.addr > b->ip4.addr)
00109         return 1;
00110 
00111     return 0;
00112 }
00113 #ifdef PICO_SUPPORT_IPV6
00114 static int mcast_sources_cmp_ipv6(void *ka, void *kb)
00115 {
00116     union pico_address *a = ka, *b = kb;
00117     return memcmp(&a->ip6, &b->ip6, sizeof(struct pico_ip6));
00118 }
00119 #endif
00120 static int mcast_socket_cmp(void *ka, void *kb)
00121 {
00122     struct pico_socket *a = ka, *b = kb;
00123     if (a < b)
00124         return -1;
00125 
00126     if (a > b)
00127         return 1;
00128 
00129     return 0;
00130 }
00131 
00132 /* gather all multicast sockets to hasten filter aggregation */
00133 PICO_TREE_DECLARE(MCASTSockets, mcast_socket_cmp);
00134 
00135 static int mcast_filter_cmp(void *ka, void *kb)
00136 {
00137     union pico_address *a = ka, *b = kb;
00138     if (a->ip4.addr < b->ip4.addr)
00139         return -1;
00140 
00141     if (a->ip4.addr > b->ip4.addr)
00142         return 1;
00143 
00144     return 0;
00145 }
00146 /* gather sources to be filtered */
00147 PICO_TREE_DECLARE(MCASTFilter, mcast_filter_cmp);
00148 
00149 static int mcast_filter_cmp_ipv6(void *ka, void *kb)
00150 {
00151     union pico_address *a = ka, *b = kb;
00152     return memcmp(&a->ip6, &b->ip6, sizeof(struct pico_ip6));
00153 }
00154 /* gather sources to be filtered */
00155 PICO_TREE_DECLARE(MCASTFilter_ipv6, mcast_filter_cmp_ipv6);
00156 
00157 inline static struct pico_tree *mcast_get_src_tree(struct pico_socket *s,struct pico_mcast *mcast) {
00158     if( IS_SOCK_IPV4(s)) {
00159         mcast->listen->MCASTSources.compare = mcast_sources_cmp;
00160         return &mcast->listen->MCASTSources;
00161     } 
00162 #ifdef PICO_SUPPORT_IPV6 
00163     else if( IS_SOCK_IPV6(s) ) {
00164         mcast->listen->MCASTSources_ipv6.compare = mcast_sources_cmp_ipv6;
00165         return &mcast->listen->MCASTSources_ipv6;
00166     }
00167 #endif
00168     return NULL;
00169 }
00170 inline static struct pico_tree *mcast_get_listen_tree(struct pico_socket *s) {
00171     if( IS_SOCK_IPV4(s)) 
00172         return s->MCASTListen;
00173 #ifdef PICO_SUPPORT_IPV6
00174     else if( IS_SOCK_IPV6(s) )  
00175         return s->MCASTListen_ipv6;
00176 #endif
00177     return NULL;
00178 }
00179 inline static void mcast_set_listen_tree_p_null(struct pico_socket *s) {
00180     if( IS_SOCK_IPV4(s)) 
00181         s->MCASTListen = NULL;
00182 #ifdef PICO_SUPPORT_IPV6
00183     else if( IS_SOCK_IPV6(s) )  
00184         s->MCASTListen_ipv6 = NULL;
00185 #endif
00186 }
00187 static struct pico_mcast_listen *listen_find(struct pico_socket *s, union pico_address *lnk, union pico_address *grp)
00188 {
00189     struct pico_mcast_listen ltest = {
00190         0
00191     };
00192     ltest.mcast_link = *lnk;
00193     ltest.mcast_group = *grp;
00194 
00195     if(IS_SOCK_IPV4(s)) 
00196         return pico_tree_findKey(s->MCASTListen, &ltest);
00197 #ifdef PICO_SUPPORT_IPV6
00198     else if(IS_SOCK_IPV6(s) ) {
00199         ltest.proto = PICO_PROTO_IPV6;
00200         return pico_tree_findKey(s->MCASTListen_ipv6, &ltest);
00201     }
00202 #endif
00203     return NULL;
00204 }
00205 static union pico_address *pico_mcast_get_link_address(struct pico_socket *s, union pico_link *mcast_link) {
00206     if( IS_SOCK_IPV4(s) ) 
00207         return (union pico_address *) &mcast_link->ipv4.address;
00208 #ifdef PICO_SUPPORT_IPV6
00209     if( IS_SOCK_IPV6(s)) 
00210         return (union pico_address *) &mcast_link->ipv6.address;
00211 #endif
00212     return NULL;
00213 }
00214 static uint8_t pico_mcast_filter_excl_excl(struct pico_mcast_listen *listen)
00215 {
00216     /* filter = intersection of EXCLUDEs */
00217     /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */
00218     /* remove from the interface EXCLUDE filter any source not in the socket EXCLUDE filter */
00219     struct pico_tree_node *index = NULL, *_tmp = NULL;
00220     union pico_address *source = NULL;
00221     if(!pico_tree_empty(&MCASTFilter)) {
00222         pico_tree_foreach_safe(index, &MCASTFilter, _tmp)
00223         {
00224             source = pico_tree_findKey(&listen->MCASTSources, index->keyValue);
00225             if (!source)
00226                 pico_tree_delete(&MCASTFilter, index->keyValue);
00227         }
00228     }
00229 #ifdef PICO_SUPPORT_IPV6
00230     if(!pico_tree_empty(&MCASTFilter_ipv6)) {
00231         pico_tree_foreach_safe(index, &MCASTFilter_ipv6, _tmp)
00232         {
00233             source = pico_tree_findKey(&listen->MCASTSources_ipv6, index->keyValue);
00234             if (!source)
00235                 pico_tree_delete(&MCASTFilter_ipv6, index->keyValue);
00236         }
00237     }
00238 #endif
00239     return PICO_IP_MULTICAST_EXCLUDE;
00240 }
00241 
00242 static uint8_t pico_mcast_filter_excl_incl(struct pico_mcast_listen *listen)
00243 {
00244     /* filter = EXCLUDE - INCLUDE */
00245     /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */
00246     /* remove from the interface EXCLUDE filter any source in the socket INCLUDE filter */
00247     struct pico_tree_node *index = NULL, *_tmp = NULL;
00248     union pico_address *source = NULL;
00249     if(!pico_tree_empty(&listen->MCASTSources)) {
00250         pico_tree_foreach_safe(index, &listen->MCASTSources, _tmp)
00251         {
00252             source = pico_tree_findKey(&MCASTFilter, index->keyValue);
00253             if (source)
00254                 pico_tree_delete(&MCASTFilter, source);
00255         }
00256     }
00257 #ifdef PICO_SUPPORT_IPV6
00258     if(!pico_tree_empty(&listen->MCASTSources_ipv6)) {
00259         pico_tree_foreach_safe(index, &listen->MCASTSources_ipv6, _tmp)
00260         {
00261             source = pico_tree_findKey(&MCASTFilter_ipv6, index->keyValue);
00262             if (source)
00263                 pico_tree_delete(&MCASTFilter_ipv6, source);
00264         }
00265     }
00266 #endif
00267     return PICO_IP_MULTICAST_EXCLUDE;
00268 }
00269 
00270 static uint8_t pico_mcast_filter_incl_excl(struct pico_mcast_listen *listen)
00271 {
00272     /* filter = EXCLUDE - INCLUDE */
00273     /* delete from the interface INCLUDE filter any source NOT in the socket EXCLUDE filter */
00274     struct pico_tree_node *index = NULL, *_tmp = NULL;
00275     union pico_address *source = NULL;
00276     if(!pico_tree_empty(&listen->MCASTSources)) {    
00277         pico_tree_foreach_safe(index, &MCASTFilter, _tmp)
00278         {
00279             source = pico_tree_findKey(&listen->MCASTSources, index->keyValue);
00280             if (!source)
00281                 pico_tree_delete(&MCASTFilter, index->keyValue);
00282         }
00283     }
00284 #ifdef PICO_SUPPORT_IPV6
00285     if(!pico_tree_empty(&listen->MCASTSources_ipv6)) {    
00286         pico_tree_foreach_safe(index, &MCASTFilter_ipv6, _tmp)
00287         {
00288             source = pico_tree_findKey(&listen->MCASTSources_ipv6, index->keyValue);
00289             if (!source)
00290                pico_tree_delete(&MCASTFilter_ipv6, index->keyValue);
00291         }
00292     }
00293 #endif
00294     /* any record with filter mode EXCLUDE, causes the interface mode to be EXCLUDE */
00295 
00296     /* add to the interface EXCLUDE filter any socket source NOT in the former interface INCLUDE filter */
00297     if(!pico_tree_empty(&listen->MCASTSources)) {
00298         pico_tree_foreach_safe(index, &listen->MCASTSources, _tmp)
00299         {
00300             source = pico_tree_insert(&MCASTFilter, index->keyValue);
00301             if (source)
00302                 pico_tree_delete(&MCASTFilter, source);
00303         }
00304     }
00305 #ifdef PICO_SUPPORT_IPV6
00306     if(!pico_tree_empty(&listen->MCASTSources_ipv6)) {
00307         pico_tree_foreach_safe(index, &listen->MCASTSources_ipv6, _tmp)
00308         {
00309             source = pico_tree_insert(&MCASTFilter_ipv6, index->keyValue);
00310             if (source)
00311                 pico_tree_delete(&MCASTFilter_ipv6, source);
00312         }
00313     }
00314 #endif
00315     return PICO_IP_MULTICAST_EXCLUDE;
00316 }
00317 
00318 static uint8_t pico_mcast_filter_incl_incl(struct pico_mcast_listen *listen)
00319 {
00320     /* filter = summation of INCLUDEs */
00321     /* mode stays INCLUDE, add all sources to filter */
00322     struct pico_tree_node *index = NULL, *_tmp = NULL;
00323     union pico_address *source = NULL;
00324     
00325     if( !pico_tree_empty(&listen->MCASTSources)) {
00326         pico_tree_foreach_safe(index, &listen->MCASTSources, _tmp)
00327         {
00328             source = index->keyValue;
00329             pico_tree_insert(&MCASTFilter, source);
00330         }
00331     }
00332 #ifdef PICO_SUPPORT_IPV6
00333     if( !pico_tree_empty(&listen->MCASTSources_ipv6)) {
00334         pico_tree_foreach_safe(index, &listen->MCASTSources_ipv6, _tmp)
00335         {
00336             source = index->keyValue;
00337             pico_tree_insert(&MCASTFilter_ipv6, source);
00338         }
00339     }
00340 #endif
00341     return PICO_IP_MULTICAST_INCLUDE;
00342 }
00343 
00344 struct pico_mcast_filter_aggregation
00345 {
00346     uint8_t (*call)(struct pico_mcast_listen *);
00347 };
00348 
00349 static const struct pico_mcast_filter_aggregation mcast_filter_aggr_call[2][2] =
00350 {
00351     {
00352     /* EXCL + EXCL */ {.call = pico_mcast_filter_excl_excl},
00353     /* EXCL + INCL */ {.call = pico_mcast_filter_excl_incl}
00354     },
00355 
00356     {
00357     /* INCL + EXCL */ {.call = pico_mcast_filter_incl_excl},
00358     /* INCL + INCL */ {.call = pico_mcast_filter_incl_incl}
00359     }
00360 };
00361 
00362 static int mcast_aggr_validate(uint8_t fm, struct pico_mcast_listen *l)
00363 {
00364     if (!l)
00365         return -1;
00366 
00367     if (fm > 1)
00368         return -1;
00369 
00370     if (l->filter_mode > 1)
00371         return -1;
00372 
00373     return 0;
00374 }
00375 
00376 
00377 /* MCASTFilter will be empty if no socket is listening on mcast_group on mcast_link anymore */
00378 static int pico_socket_aggregate_mcastfilters(union pico_address *mcast_link, union pico_address *mcast_group)
00379 {
00380     uint8_t filter_mode = PICO_IP_MULTICAST_INCLUDE;
00381     struct pico_mcast_listen *listen = NULL;
00382     struct pico_socket *mcast_sock = NULL;
00383     struct pico_tree_node *index = NULL, *_tmp = NULL;
00384 
00385     /* cleanup old filter */
00386     if(!pico_tree_empty(&MCASTFilter)) {
00387         pico_tree_foreach_safe(index, &MCASTFilter, _tmp)
00388         {
00389             pico_tree_delete(&MCASTFilter, index->keyValue);
00390          }
00391     }
00392 #ifdef PICO_SUPPORT_IPV6
00393     if(!pico_tree_empty(&MCASTFilter_ipv6)) {
00394         pico_tree_foreach_safe(index, &MCASTFilter_ipv6, _tmp)
00395         {
00396             pico_tree_delete(&MCASTFilter_ipv6, index->keyValue);
00397         }
00398     }
00399 #endif
00400     /* construct new filter */
00401     pico_tree_foreach_safe(index, &MCASTSockets, _tmp)
00402     {
00403         mcast_sock = index->keyValue;
00404         listen = listen_find(mcast_sock, mcast_link, mcast_group);
00405         if (listen) {
00406             if (mcast_aggr_validate(filter_mode, listen) < 0) {
00407                 pico_err = PICO_ERR_EINVAL;
00408                 return -1;
00409             }
00410             if (mcast_filter_aggr_call[filter_mode][listen->filter_mode].call) {
00411                 filter_mode = mcast_filter_aggr_call[filter_mode][listen->filter_mode].call(listen);
00412                 if (filter_mode > 1)
00413                     return -1;
00414             }
00415         } 
00416     }
00417     return filter_mode;
00418 }
00419 
00420 static int pico_socket_mcast_filter_include(struct pico_mcast_listen *listen, union pico_address *src)
00421 {
00422     struct pico_tree_node *index = NULL;
00423 #ifdef PICO_DEBUG_MCAST
00424     char tmp_string[PICO_IPV6_STRING];
00425 #endif
00426     if(!pico_tree_empty(&listen->MCASTSources)) {
00427         pico_tree_foreach(index, &listen->MCASTSources)
00428         {
00429             if (src->ip4.addr == ((union pico_address *)index->keyValue)->ip4.addr) {
00430                 so_mcast_dbg("MCAST: IP %08X in included socket source list\n", src->ip4.addr);
00431                 return 0;
00432             }
00433         }
00434     }
00435 #ifdef PICO_SUPPORT_IPV6
00436     if(!pico_tree_empty(&listen->MCASTSources_ipv6)) {
00437         pico_tree_foreach(index, &listen->MCASTSources_ipv6)
00438         {
00439             if (memcmp(&src->ip6 , &((union pico_address *)index->keyValue)->ip6, sizeof(struct pico_ip6))) {
00440 #ifdef PICO_DEBUG_MCAST                
00441                 pico_ipv6_to_string(tmp_string, src->ip6.addr);
00442                 so_mcast_dbg("MCAST: IP %s in included socket source list\n", tmp_string);
00443 #endif                
00444                 return 0;
00445             }
00446         }
00447     }
00448 #endif
00449     /* XXX IPV6 ADDRESS */
00450     so_mcast_dbg("MCAST: IP %08X NOT in included socket source list\n", src->ip4.addr);
00451     return -1;
00452 
00453 }
00454 
00455 static int pico_socket_mcast_filter_exclude(struct pico_mcast_listen *listen, union pico_address *src)
00456 {
00457     struct pico_tree_node *index = NULL;
00458 #ifdef PICO_DEBUG_MCAST
00459     char tmp_string[PICO_IPV6_STRING];
00460 #endif
00461     if(!pico_tree_empty(&listen->MCASTSources)) {
00462         pico_tree_foreach(index, &listen->MCASTSources)
00463         {
00464             if (src->ip4.addr == ((union pico_address *)index->keyValue)->ip4.addr) {
00465                 so_mcast_dbg("MCAST: IP %08X in excluded socket source list\n", src->ip4.addr);
00466                 return -1;
00467             }
00468         }
00469     }
00470 #ifdef PICO_SUPPORT_IPV6
00471     if(!pico_tree_empty(&listen->MCASTSources_ipv6)) {
00472         pico_tree_foreach(index, &listen->MCASTSources_ipv6)
00473         {
00474             if (memcmp(&src->ip6 , &((union pico_address *)index->keyValue)->ip6, sizeof(struct pico_ip6))) {
00475 #ifdef PICO_DEBUG_MCAST                
00476                 pico_ipv6_to_string(tmp_string, src->ip6.addr);
00477                 so_mcast_dbg("MCAST: IP %s in excluded socket source list\n", tmp_string);
00478 #endif                
00479                 return 0;
00480             }
00481         }
00482     }
00483 #endif
00484     /* XXX IPV6 ADDRESS  */
00485     so_mcast_dbg("MCAST: IP %08X NOT in excluded socket source list\n", src->ip4.addr);
00486     return 0;
00487 }
00488 
00489 static int pico_socket_mcast_source_filtering(struct pico_mcast_listen *listen, union pico_address *src)
00490 {
00491     /* perform source filtering */
00492     if (listen->filter_mode == PICO_IP_MULTICAST_INCLUDE)
00493         return pico_socket_mcast_filter_include(listen, src);
00494 
00495     if (listen->filter_mode == PICO_IP_MULTICAST_EXCLUDE)
00496         return pico_socket_mcast_filter_exclude(listen, src);
00497 
00498     return -1;
00499 }
00500 
00501 static void *pico_socket_mcast_filter_link_get(struct pico_socket *s)
00502 {
00503     /* check if no multicast enabled on socket */
00504     if (!s->MCASTListen)
00505         return NULL;
00506     if( IS_SOCK_IPV4(s) ) {
00507         if (!s->local_addr.ip4.addr)
00508             return pico_ipv4_get_default_mcastlink();
00509 
00510         return pico_ipv4_link_get(&s->local_addr.ip4);
00511     }
00512 #ifdef PICO_SUPPORT_IPV6
00513     else if( IS_SOCK_IPV6(s)) {
00514         if (pico_ipv6_is_null_address(&s->local_addr.ip6))
00515             return pico_ipv6_get_default_mcastlink();
00516 
00517         return pico_ipv6_link_get(&s->local_addr.ip6);
00518     }
00519 #endif
00520     return NULL;
00521 }
00522 
00523 int pico_socket_mcast_filter(struct pico_socket *s, union pico_address *mcast_group, union pico_address *src)
00524 {
00525     void *mcast_link = NULL;
00526     struct pico_mcast_listen *listen = NULL;
00527     mcast_link = pico_socket_mcast_filter_link_get(s);
00528     if (!mcast_link)
00529         return -1;
00530     if(IS_SOCK_IPV4(s))
00531         listen = listen_find(s,(union pico_address *) &((struct pico_ipv4_link*)(mcast_link))->address, mcast_group);
00532 #ifdef PICO_SUPPORT_IPV6
00533     else if(IS_SOCK_IPV6(s))    
00534         listen = listen_find(s, (union pico_address *)&((struct pico_ipv6_link*)(mcast_link))->address, mcast_group);
00535 #endif
00536     if (!listen)
00537         return -1;
00538 
00539     return pico_socket_mcast_source_filtering(listen, src);
00540 }
00541 
00542 
00543 static struct pico_ipv4_link *get_mcast_link(union pico_address *a) {
00544     if (!a->ip4.addr)
00545         return pico_ipv4_get_default_mcastlink();
00546     return pico_ipv4_link_get(&a->ip4);
00547 }
00548 #ifdef PICO_SUPPORT_IPV6
00549 static struct pico_ipv6_link *get_mcast_link_ipv6(union pico_address *a) {
00550 
00551     if (pico_ipv6_is_null_address(&a->ip6)) {
00552         return pico_ipv6_get_default_mcastlink();
00553     }
00554     return pico_ipv6_link_get(&a->ip6);
00555 }
00556 #endif
00557 
00558 static int pico_socket_setoption_pre_validation(struct pico_ip_mreq *mreq)
00559     {
00560     if (!mreq) 
00561         return -1;
00562 
00563     if (!mreq->mcast_group_addr.ip4.addr)   
00564         return -1;
00565 
00566     return 0;
00567 }
00568 #ifdef PICO_SUPPORT_IPV6
00569 static int pico_socket_setoption_pre_validation_ipv6(struct pico_ip_mreq *mreq)
00570 {
00571     if (!mreq)
00572         return -1;
00573 
00574     if (pico_ipv6_is_null_address((struct pico_ip6*)&mreq->mcast_group_addr))
00575         return -1;
00576 
00577     return 0;
00578 }
00579 #endif
00580 
00581 static struct pico_ipv4_link *pico_socket_setoption_validate_mreq(struct pico_ip_mreq *mreq)
00582 {
00583     if (pico_socket_setoption_pre_validation(mreq) < 0)  
00584         return NULL;
00585 
00586     if (pico_ipv4_is_unicast(mreq->mcast_group_addr.ip4.addr)) 
00587         return NULL;
00588 
00589     return get_mcast_link((union pico_address *)&mreq->mcast_link_addr);
00590 }
00591 
00592 #ifdef PICO_SUPPORT_IPV6
00593 static struct pico_ipv6_link *pico_socket_setoption_validate_mreq_ipv6(struct pico_ip_mreq *mreq)
00594 {
00595     if (pico_socket_setoption_pre_validation_ipv6(mreq) < 0)
00596         return NULL;
00597 
00598     if (pico_ipv6_is_unicast((struct pico_ip6 *)&mreq->mcast_group_addr))
00599         return NULL;
00600     return get_mcast_link_ipv6((union pico_address *)&mreq->mcast_link_addr);
00601 }
00602 #endif
00603 
00604 static int pico_socket_setoption_pre_validation_s(struct pico_ip_mreq_source *mreq)
00605 {
00606     if (!mreq)
00607         return -1;
00608 
00609     if (!mreq->mcast_group_addr.ip4.addr)
00610         return -1;
00611 
00612     return 0;
00613 }
00614 #ifdef PICO_SUPPORT_IPV6
00615 static int pico_socket_setoption_pre_validation_s_ipv6(struct pico_ip_mreq_source *mreq)
00616 {
00617     if (!mreq)
00618         return -1;
00619 
00620     if (pico_ipv6_is_null_address((struct pico_ip6 *)&mreq->mcast_group_addr))
00621         return -1;
00622 
00623     return 0;
00624 }
00625 #endif
00626 
00627 static struct pico_ipv4_link *pico_socket_setoption_validate_s_mreq(struct pico_ip_mreq_source *mreq)
00628 {
00629     if (pico_socket_setoption_pre_validation_s(mreq) < 0)
00630         return NULL;
00631 
00632     if (pico_ipv4_is_unicast(mreq->mcast_group_addr.ip4.addr))
00633         return NULL;
00634 
00635     if (!pico_ipv4_is_unicast(mreq->mcast_source_addr.ip4.addr))
00636         return NULL;
00637 
00638     return get_mcast_link((union pico_address *)&mreq->mcast_link_addr);
00639 }
00640 #ifdef PICO_SUPPORT_IPV6
00641 static struct pico_ipv6_link *pico_socket_setoption_validate_s_mreq_ipv6(struct pico_ip_mreq_source *mreq)
00642 {
00643     if (pico_socket_setoption_pre_validation_s_ipv6(mreq) < 0) {
00644         return NULL;
00645     }
00646     if (pico_ipv6_is_unicast((struct pico_ip6 *)&mreq->mcast_group_addr)){
00647         return NULL;
00648     }
00649     if (!pico_ipv6_is_unicast((struct pico_ip6 *)&mreq->mcast_source_addr)){
00650         return NULL;
00651     }
00652 
00653     return get_mcast_link_ipv6(&mreq->mcast_link_addr);
00654 }
00655 #endif
00656 
00657 static struct pico_ipv4_link *setop_multicast_link_search(void *value, int bysource)
00658 {
00659 
00660     struct pico_ip_mreq *mreq = NULL;
00661     struct pico_ipv4_link *mcast_link = NULL;
00662     struct pico_ip_mreq_source *mreq_src = NULL;
00663     if (!bysource) {
00664         mreq = (struct pico_ip_mreq *)value;
00665         mcast_link = pico_socket_setoption_validate_mreq(mreq);
00666         if (!mcast_link) 
00667            return NULL;
00668         if (!mreq->mcast_link_addr.ip4.addr)
00669             mreq->mcast_link_addr.ip4.addr = mcast_link->address.addr;
00670     } else {
00671         mreq_src = (struct pico_ip_mreq_source *)value;
00672         if (!mreq_src) {
00673              return NULL;
00674         }
00675 
00676         mcast_link = pico_socket_setoption_validate_s_mreq(mreq_src);
00677         if (!mcast_link) {
00678             return NULL;
00679         }
00680 
00681         if (!mreq_src->mcast_link_addr.ip4.addr)
00682             mreq_src->mcast_link_addr.ip4 = mcast_link->address;
00683     }
00684 
00685     return mcast_link;
00686 }
00687 #ifdef PICO_SUPPORT_IPV6
00688 static struct pico_ipv6_link *setop_multicast_link_search_ipv6(void *value, int bysource)
00689 {
00690     struct pico_ip_mreq *mreq = NULL;
00691     struct pico_ipv6_link *mcast_link = NULL;
00692     struct pico_ip_mreq_source *mreq_src = NULL;
00693     if (!bysource) {
00694         mreq = (struct pico_ip_mreq *)value;
00695         mcast_link = pico_socket_setoption_validate_mreq_ipv6(mreq);
00696         if (!mcast_link) {
00697            return NULL;
00698         }
00699         if (pico_ipv6_is_null_address(&mreq->mcast_link_addr.ip6))
00700             mreq->mcast_link_addr.ip6 = mcast_link->address;
00701     } else {
00702         mreq_src = (struct pico_ip_mreq_source *)value;
00703         if (!mreq_src) {
00704             return NULL;
00705         }
00706 
00707         mcast_link = pico_socket_setoption_validate_s_mreq_ipv6(mreq_src);
00708         if (!mcast_link) {
00709             return NULL;
00710         }
00711         if (pico_ipv6_is_null_address(&mreq_src->mcast_link_addr.ip6))
00712             mreq_src->mcast_link_addr.ip6 = mcast_link->address;
00713     }
00714     return mcast_link;
00715 }
00716 #endif
00717 static int setop_verify_listen_tree(struct pico_socket *s, int alloc)
00718 {
00719     if(!alloc)
00720         return -1;
00721 
00722     if( IS_SOCK_IPV4(s) ) {
00723 
00724         s->MCASTListen = PICO_ZALLOC(sizeof(struct pico_tree));
00725         if (!s->MCASTListen) {
00726             pico_err = PICO_ERR_ENOMEM;
00727             return -1;
00728         }
00729 
00730         s->MCASTListen->root = &LEAF;
00731         s->MCASTListen->compare = mcast_listen_cmp;
00732         return 0;
00733     } 
00734 #ifdef PICO_SUPPORT_IPV6
00735     else if( IS_SOCK_IPV6(s)){
00736         s->MCASTListen_ipv6 = PICO_ZALLOC(sizeof(struct pico_tree));
00737         if (!s->MCASTListen_ipv6) {
00738             pico_err = PICO_ERR_ENOMEM;
00739             return -1;
00740         }
00741 
00742         s->MCASTListen_ipv6->root = &LEAF;
00743         s->MCASTListen_ipv6->compare = mcast_listen_cmp_ipv6;
00744         return 0;
00745 
00746     }
00747 #endif
00748     return -1;
00749 }
00750 
00751 
00752 static void *setopt_multicast_check(struct pico_socket *s, void *value, int alloc, int bysource)
00753 {
00754     void *mcast_link = NULL;
00755     struct pico_tree *listen_tree = mcast_get_listen_tree(s);
00756     if (!value) {
00757         pico_err = PICO_ERR_EINVAL;
00758         return NULL;
00759     }
00760     if(IS_SOCK_IPV4(s))
00761         mcast_link = setop_multicast_link_search(value, bysource);
00762 #ifdef PICO_SUPPORT_IPV6
00763     else if(IS_SOCK_IPV6(s))
00764         mcast_link = setop_multicast_link_search_ipv6(value, bysource);
00765 #endif    
00766     if (!mcast_link) {
00767         pico_err = PICO_ERR_EINVAL;
00768         return NULL;
00769     }
00770     if (!listen_tree) { /* No RBTree allocated yet */
00771         if (setop_verify_listen_tree(s, alloc) < 0) {
00772             return NULL;
00773         }
00774     }
00775     return mcast_link;
00776 }
00777 
00778 void pico_multicast_delete(struct pico_socket *s)
00779 {
00780     int filter_mode;
00781     struct pico_tree_node *index = NULL, *_tmp = NULL, *index2 = NULL, *_tmp2 = NULL;
00782     struct pico_mcast_listen *listen = NULL;
00783     union pico_address *source = NULL;
00784     struct pico_tree *tree, *listen_tree;
00785     struct pico_mcast mcast;
00786     listen_tree = mcast_get_listen_tree(s);
00787     if(listen_tree) {
00788         pico_tree_delete(&MCASTSockets, s);
00789         pico_tree_foreach_safe(index, listen_tree, _tmp)
00790         {
00791             listen = index->keyValue;
00792             mcast.listen = listen;
00793             tree = mcast_get_src_tree(s, &mcast);
00794             pico_tree_foreach_safe(index2, tree, _tmp2)
00795             {
00796                 source = index->keyValue;
00797                 pico_tree_delete(tree, source);
00798                 PICO_FREE(source);
00799             }
00800             filter_mode = pico_socket_aggregate_mcastfilters((union pico_address *)&listen->mcast_link, (union pico_address *)&listen->mcast_group);
00801             if (filter_mode >= 0) {
00802                 if(IS_SOCK_IPV4(s))
00803                     pico_ipv4_mcast_leave(&listen->mcast_link.ip4, &listen->mcast_group.ip4, 1, (uint8_t)filter_mode, &MCASTFilter);
00804 #ifdef PICO_SUPPORT_IPV6      
00805               else if(IS_SOCK_IPV6(s))
00806                     pico_ipv6_mcast_leave(&listen->mcast_link.ip6, &listen->mcast_group.ip6, 1, (uint8_t)filter_mode, &MCASTFilter_ipv6);
00807 #endif
00808             }
00809             pico_tree_delete(listen_tree, listen);
00810             PICO_FREE(listen);
00811         }
00812         PICO_FREE(listen_tree);
00813         mcast_set_listen_tree_p_null(s);
00814     }
00815 }
00816 
00817 
00818 int pico_getsockopt_mcast(struct pico_socket *s, int option, void *value)
00819 {
00820     switch(option) {
00821     case PICO_IP_MULTICAST_IF:
00822         pico_err = PICO_ERR_EOPNOTSUPP;
00823         return -1;
00824 
00825     case PICO_IP_MULTICAST_TTL:
00826         if (s->proto->proto_number == PICO_PROTO_UDP) {
00827             pico_udp_get_mc_ttl(s, (uint8_t *) value);
00828         } else {
00829             *(uint8_t *)value = 0;
00830             pico_err = PICO_ERR_EINVAL;
00831             return -1;
00832         }
00833 
00834         break;
00835 
00836     case PICO_IP_MULTICAST_LOOP:
00837         if (s->proto->proto_number == PICO_PROTO_UDP) {
00838             *(uint8_t *)value = (uint8_t)PICO_SOCKET_GETOPT(s, PICO_SOCKET_OPT_MULTICAST_LOOP);
00839         } else {
00840             *(uint8_t *)value = 0;
00841             pico_err = PICO_ERR_EINVAL;
00842             return -1;
00843         }
00844 
00845         break;
00846     default:
00847         pico_err = PICO_ERR_EINVAL;
00848         return -1;
00849     }
00850 
00851     return 0;
00852 }
00853 
00854 static int mcast_so_loop(struct pico_socket *s, void *value)
00855 {
00856     uint8_t val = (*(uint8_t *)value);
00857     if (val == 0u) {
00858         PICO_SOCKET_SETOPT_DIS(s, PICO_SOCKET_OPT_MULTICAST_LOOP);
00859         return 0;
00860     } else if (val == 1u) {
00861         PICO_SOCKET_SETOPT_EN(s, PICO_SOCKET_OPT_MULTICAST_LOOP);
00862         return 0;
00863     }
00864 
00865     pico_err = PICO_ERR_EINVAL;
00866     return -1;
00867 }
00868 static int mcast_get_param(struct pico_mcast *mcast, struct pico_socket *s, void *value,int alloc, int by_source) {
00869     if(by_source)
00870         mcast->mreq_s = (struct pico_ip_mreq_source *)value;
00871     else    
00872         mcast->mreq = (struct pico_ip_mreq *)value;
00873     mcast->mcast_link = setopt_multicast_check(s, value, alloc, by_source);
00874     if (!mcast->mcast_link)
00875         return -1;
00876     mcast->address =  pico_mcast_get_link_address(s, mcast->mcast_link);     
00877     if(by_source)
00878         mcast->listen = listen_find(s, &(mcast->mreq_s)->mcast_link_addr, &mcast->mreq_s->mcast_group_addr);
00879     else
00880         mcast->listen = listen_find(s, &(mcast->mreq)->mcast_link_addr, &mcast->mreq->mcast_group_addr);
00881     return 0;
00882 }
00883 static int mcast_so_addm(struct pico_socket *s, void *value)
00884 {
00885     int filter_mode = 0;
00886     struct pico_mcast mcast;
00887     struct pico_tree *tree, *listen_tree;
00888     if(mcast_get_param(&mcast, s, value, 1,0) < 0)
00889         return -1;
00890     
00891     if (mcast.listen) {
00892         if (mcast.listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) { 
00893             so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n");
00894         } else { 
00895             so_mcast_dbg("pico_socket_setoption: ERROR duplicate PICO_IP_ADD_MEMBERSHIP\n");
00896         }
00897         pico_err = PICO_ERR_EINVAL;
00898         return -1;
00899     } 
00900     mcast.listen = PICO_ZALLOC(sizeof(struct pico_mcast_listen));
00901     if (!mcast.listen) {
00902         pico_err = PICO_ERR_ENOMEM;
00903         return -1;
00904     }
00905     mcast.listen->filter_mode = PICO_IP_MULTICAST_EXCLUDE;
00906     mcast.listen->mcast_link = mcast.mreq->mcast_link_addr;
00907     mcast.listen->mcast_group = mcast.mreq->mcast_group_addr;
00908     mcast.listen->proto = s->net->proto_number;
00909 
00910     tree = mcast_get_src_tree(s, &mcast);
00911     listen_tree = mcast_get_listen_tree(s);
00912 #ifdef PICO_SUPPORT_IPV6
00913     if( IS_SOCK_IPV6(s)) 
00914         mcast.listen->proto = PICO_PROTO_IPV6;
00915 #endif
00916     tree->root = &LEAF;
00917     pico_tree_insert(listen_tree, mcast.listen);
00918     
00919     pico_tree_insert(&MCASTSockets, s);
00920     filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq->mcast_group_addr);
00921     if (filter_mode < 0)
00922         return -1;
00923     so_mcast_dbg("PICO_IP_ADD_MEMBERSHIP - success, added %p\n", s);
00924     if(IS_SOCK_IPV4(s)) 
00925         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);
00926 #ifdef PICO_SUPPORT_IPV6   
00927     else if(IS_SOCK_IPV6(s)) { 
00928         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);
00929     }
00930 #endif
00931     return -1;
00932 }
00933 
00934 static int mcast_so_dropm(struct pico_socket *s, void *value)
00935 {
00936     int filter_mode = 0;
00937     union pico_address *source = NULL;
00938     struct pico_tree_node *_tmp,*index;
00939     struct pico_mcast mcast;
00940     struct pico_tree *listen_tree,*tree;
00941     if(mcast_get_param(&mcast, s, value, 0,0) < 0)
00942         return -1;
00943     if (!mcast.listen) {
00944         so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_MEMBERSHIP before PICO_IP_ADD_MEMBERSHIP/SOURCE_MEMBERSHIP\n");
00945         pico_err = PICO_ERR_EADDRNOTAVAIL;
00946         return -1;
00947     } 
00948     tree = mcast_get_src_tree(s,&mcast);
00949     listen_tree = mcast_get_listen_tree(s);
00950 
00951     pico_tree_foreach_safe(index, tree, _tmp)
00952     {
00953         source = index->keyValue;
00954         pico_tree_delete(tree, source);
00955     }
00956     pico_tree_delete(listen_tree, mcast.listen);
00957     PICO_FREE(mcast.listen);
00958     if (pico_tree_empty(listen_tree)) {
00959         PICO_FREE(listen_tree);
00960         mcast_set_listen_tree_p_null(s);
00961         pico_tree_delete(&MCASTSockets, s);
00962     }
00963     
00964     filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq->mcast_group_addr);
00965     if (filter_mode < 0)
00966         return -1;
00967     if(IS_SOCK_IPV4(s)) 
00968         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);
00969 #ifdef PICO_SUPPORT_IPV6    
00970     else if(IS_SOCK_IPV6(s)) { }
00971         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);
00972 #endif
00973     return -1;
00974 }
00975 
00976 static int mcast_so_unblock_src(struct pico_socket *s, void *value)
00977 {
00978     int filter_mode = 0;
00979     union pico_address stest, *source = NULL;
00980     struct pico_mcast mcast;
00981     if(mcast_get_param(&mcast, s, value, 0,1) < 0)
00982         return -1;
00983     
00984     memset(&stest, 0, sizeof(union pico_address));
00985     if (!mcast.listen) {
00986         so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_UNBLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n");
00987         pico_err = PICO_ERR_EINVAL;
00988         return -1;
00989     } 
00990     if (mcast.listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) {
00991         so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n");
00992         pico_err = PICO_ERR_EINVAL;
00993         return -1;
00994     }
00995 
00996     stest = mcast.mreq_s->mcast_source_addr;
00997     if( IS_SOCK_IPV4(s))
00998         source = pico_tree_findKey(&mcast.listen->MCASTSources, &stest);
00999 #ifdef PICO_SUPPORT_IPV6    
01000     else if( IS_SOCK_IPV6(s))
01001         source = pico_tree_findKey(&mcast.listen->MCASTSources_ipv6, &stest);
01002 #endif
01003     if (!source) {
01004         so_mcast_dbg("pico_socket_setoption: ERROR address to unblock not in source list\n");
01005         pico_err = PICO_ERR_EADDRNOTAVAIL;
01006         return -1;
01007     } 
01008     if( IS_SOCK_IPV4(s) )
01009         pico_tree_delete(&mcast.listen->MCASTSources, source);
01010 #ifdef PICO_SUPPORT_IPV6
01011     else if( IS_SOCK_IPV6(s) )
01012         pico_tree_delete(&mcast.listen->MCASTSources_ipv6, source);
01013 #endif
01014 
01015     filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr);
01016     if (filter_mode < 0)
01017         return -1;
01018     if(IS_SOCK_IPV4(s)) 
01019         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);
01020 #ifdef PICO_SUPPORT_IPV6    
01021     else if(IS_SOCK_IPV6(s)) { }
01022         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);
01023 #endif
01024     return -1;
01025 }
01026 
01027 static int mcast_so_block_src(struct pico_socket *s, void *value)
01028 {
01029     int filter_mode = 0;
01030     union pico_address stest, *source = NULL;
01031     struct pico_mcast mcast;
01032     if(mcast_get_param(&mcast, s, value, 0,1) < 0)
01033         return -1;
01034     
01035     memset(&stest, 0, sizeof(union pico_address));
01036     if (!mcast.listen) {
01037         dbg("pico_socket_setoption: ERROR PICO_IP_BLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n");
01038         pico_err = PICO_ERR_EINVAL;
01039         return -1;
01040     } 
01041     if (mcast.listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) {
01042         so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n");
01043         pico_err = PICO_ERR_EINVAL;
01044         return -1;
01045     }
01046     stest = mcast.mreq_s->mcast_source_addr;
01047     if( IS_SOCK_IPV4(s))
01048         source = pico_tree_findKey(&mcast.listen->MCASTSources, &stest);
01049 #ifdef PICO_SUPPORT_IPV6    
01050     else if( IS_SOCK_IPV6(s))
01051         source = pico_tree_findKey(&mcast.listen->MCASTSources_ipv6, &stest);
01052 #endif
01053     if (source) {
01054         so_mcast_dbg("pico_socket_setoption: ERROR address to block already in source list\n");
01055         pico_err = PICO_ERR_ENOMEM;
01056         return -1;
01057     } 
01058     source = PICO_ZALLOC(sizeof(union pico_address));
01059     if (!source) {
01060         pico_err = PICO_ERR_ENOMEM;
01061         return -1;
01062     }
01063     *source = mcast.mreq_s->mcast_source_addr;
01064     if( IS_SOCK_IPV4(s) ) 
01065         pico_tree_insert(&mcast.listen->MCASTSources, source);
01066 #ifdef PICO_SUPPORT_IPV6     
01067      else if( IS_SOCK_IPV6(s) ) 
01068         pico_tree_insert(&mcast.listen->MCASTSources_ipv6, source);
01069 #endif
01070 
01071     filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr);
01072     if (filter_mode < 0)
01073         return -1;
01074     if(IS_SOCK_IPV4(s)) 
01075         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);
01076 #ifdef PICO_SUPPORT_IPV6    
01077     else if(IS_SOCK_IPV6(s)) { }
01078         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);
01079 #endif
01080     return -1;
01081 }
01082 
01083 static int mcast_so_addsrcm(struct pico_socket *s, void *value)
01084 {
01085     int filter_mode = 0, reference_count = 0;
01086     union pico_address stest, *source = NULL;
01087     struct pico_mcast mcast;
01088     struct pico_tree *tree,*listen_tree;
01089     if(mcast_get_param(&mcast, s, value, 1,1) < 0)
01090         return -1;
01091 
01092     memset(&stest, 0, sizeof(union pico_address));
01093     listen_tree = mcast_get_listen_tree(s);
01094     if (mcast.listen) {
01095         tree = mcast_get_src_tree(s,&mcast);
01096         if (mcast.listen->filter_mode != PICO_IP_MULTICAST_INCLUDE) {
01097             so_mcast_dbg("pico_socket_setoption: ERROR source-specific multicast (include) on any-source multicast (exclude)\n");
01098             pico_err = PICO_ERR_EINVAL;
01099             return -1;
01100         }
01101         stest = mcast.mreq_s->mcast_source_addr;
01102         source = pico_tree_findKey(tree, &stest);
01103         if (source) {
01104             so_mcast_dbg("pico_socket_setoption: ERROR source address to allow already in source list\n");
01105             pico_err = PICO_ERR_ENOMEM;
01106             return -1;
01107         } 
01108         source = PICO_ZALLOC(sizeof(union pico_address));
01109         if (!source) {
01110             pico_err = PICO_ERR_EADDRNOTAVAIL;
01111             return -1;
01112         }
01113         *source = mcast.mreq_s->mcast_source_addr;
01114         pico_tree_insert(tree, source);
01115       
01116     } else {
01117         mcast.listen = PICO_ZALLOC(sizeof(struct pico_mcast_listen));
01118         if (!mcast.listen) {
01119             pico_err = PICO_ERR_ENOMEM;
01120             return -1;
01121         }
01122         tree = mcast_get_src_tree(s,&mcast);
01123         mcast.listen->filter_mode = PICO_IP_MULTICAST_INCLUDE;
01124         mcast.listen->mcast_link = mcast.mreq_s->mcast_link_addr;
01125         mcast.listen->mcast_group = mcast.mreq_s->mcast_group_addr;
01126         tree->root = &LEAF;
01127         source = PICO_ZALLOC(sizeof(union pico_address));
01128         if (!source) {
01129             PICO_FREE(mcast.listen);
01130             pico_err = PICO_ERR_ENOMEM;
01131             return -1;
01132         }
01133 #ifdef PICO_SUPPORT_IPV6
01134         if( IS_SOCK_IPV6(s)) 
01135             mcast.listen->proto = PICO_PROTO_IPV6;
01136 #endif
01137         *source = mcast.mreq_s->mcast_source_addr;
01138         pico_tree_insert(tree, source);
01139         pico_tree_insert(listen_tree, mcast.listen);
01140         reference_count = 1;
01141     }
01142     pico_tree_insert(&MCASTSockets, s);
01143     filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr);
01144     if (filter_mode < 0)
01145         return -1;
01146     if(IS_SOCK_IPV4(s)) 
01147         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);
01148 #ifdef PICO_SUPPORT_IPV6    
01149     else if(IS_SOCK_IPV6(s)) { }
01150         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);
01151 #endif
01152     return -1;
01153 }
01154 
01155 static int mcast_so_dropsrcm(struct pico_socket *s, void *value)
01156 {
01157     int filter_mode = 0, reference_count = 0;
01158     union pico_address stest, *source = NULL;
01159     struct pico_mcast mcast;
01160     struct pico_tree *tree,*listen_tree;
01161     if(mcast_get_param(&mcast, s, value, 0,1) < 0)
01162         return -1;
01163 
01164     memset(&stest, 0, sizeof(union pico_address));
01165     listen_tree = mcast_get_listen_tree(s);
01166     if (!mcast.listen) {
01167         so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_SOURCE_MEMBERSHIP before PICO_IP_ADD_SOURCE_MEMBERSHIP\n");
01168         pico_err = PICO_ERR_EADDRNOTAVAIL;
01169         return -1;
01170     } 
01171     if (mcast.listen->filter_mode != PICO_IP_MULTICAST_INCLUDE) {
01172         so_mcast_dbg("pico_socket_setoption: ERROR source-specific multicast (include) on any-source multicast (exclude)\n");
01173         pico_err = PICO_ERR_EINVAL;
01174         return -1;
01175     }
01176     tree = mcast_get_src_tree(s, &mcast);
01177     stest = mcast.mreq_s->mcast_source_addr;
01178     source = pico_tree_findKey(tree, &stest);
01179     if (!source) {
01180         so_mcast_dbg("pico_socket_setoption: ERROR address to drop not in source list\n");
01181         pico_err = PICO_ERR_EADDRNOTAVAIL;
01182         return -1;
01183     } 
01184     pico_tree_delete(tree, source);
01185     if (pico_tree_empty(tree)) { /* 1 if empty, 0 otherwise */
01186         reference_count = 1;
01187         pico_tree_delete(listen_tree, mcast.listen);
01188         PICO_FREE(mcast.listen);
01189         if (pico_tree_empty(listen_tree)) {
01190             PICO_FREE(listen_tree);
01191             mcast_set_listen_tree_p_null(s);
01192             pico_tree_delete(&MCASTSockets, s);
01193         }
01194     }
01195 
01196     filter_mode = pico_socket_aggregate_mcastfilters(mcast.address, &mcast.mreq_s->mcast_group_addr);
01197     if (filter_mode < 0)
01198         return -1;
01199     if(IS_SOCK_IPV4(s)) 
01200         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);
01201 #ifdef PICO_SUPPORT_IPV6    
01202     else if(IS_SOCK_IPV6(s)) { }
01203         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);
01204 #endif
01205     return -1;
01206 }
01207 
01208 
01209 struct pico_setsockopt_mcast_call
01210 {
01211     int option;
01212     int (*call)(struct pico_socket *, void *);
01213 };
01214 
01215 static const struct pico_setsockopt_mcast_call mcast_so_calls[1 + PICO_IP_DROP_SOURCE_MEMBERSHIP - PICO_IP_MULTICAST_IF] =
01216 {
01217     { PICO_IP_MULTICAST_IF,             NULL },
01218     { PICO_IP_MULTICAST_TTL,            pico_udp_set_mc_ttl },
01219     { PICO_IP_MULTICAST_LOOP,           mcast_so_loop },
01220     { PICO_IP_ADD_MEMBERSHIP,           mcast_so_addm },
01221     { PICO_IP_DROP_MEMBERSHIP,          mcast_so_dropm },
01222     { PICO_IP_UNBLOCK_SOURCE,           mcast_so_unblock_src },
01223     { PICO_IP_BLOCK_SOURCE,             mcast_so_block_src },
01224     { PICO_IP_ADD_SOURCE_MEMBERSHIP,    mcast_so_addsrcm },
01225     { PICO_IP_DROP_SOURCE_MEMBERSHIP,   mcast_so_dropsrcm }
01226 };
01227 
01228 
01229 static int mcast_so_check_socket(struct pico_socket *s)
01230 {
01231     pico_err = PICO_ERR_EINVAL;
01232     if (!s)
01233         return -1;
01234 
01235     if (!s->proto)
01236         return -1;
01237 
01238     if (s->proto->proto_number != PICO_PROTO_UDP)
01239         return -1;
01240 
01241     pico_err = PICO_ERR_NOERR;
01242     return 0;
01243 }
01244 
01245 int pico_setsockopt_mcast(struct pico_socket *s, int option, void *value)
01246 {
01247     int arrayn = option - PICO_IP_MULTICAST_IF;
01248     if (option < PICO_IP_MULTICAST_IF || option > PICO_IP_DROP_SOURCE_MEMBERSHIP) {
01249         pico_err = PICO_ERR_EOPNOTSUPP;
01250         return -1;
01251     }
01252 
01253     if (mcast_so_check_socket(s) < 0)
01254         return -1;
01255 
01256     if (!mcast_so_calls[arrayn].call) {
01257         pico_err = PICO_ERR_EOPNOTSUPP;
01258         return -1;
01259     }
01260 
01261     return (mcast_so_calls[arrayn].call(s, value));
01262 }
01263 
01264 int pico_udp_set_mc_ttl(struct pico_socket *s, void  *_ttl)
01265 {
01266     struct pico_socket_udp *u;
01267     uint8_t ttl = *(uint8_t *)_ttl;
01268     if(!s) {
01269         pico_err = PICO_ERR_EINVAL;
01270         return -1;
01271     }
01272 
01273     u = (struct pico_socket_udp *) s;
01274     u->mc_ttl = ttl;
01275     return 0;
01276 }
01277 
01278 int pico_udp_get_mc_ttl(struct pico_socket *s, uint8_t *ttl)
01279 {
01280     struct pico_socket_udp *u;
01281     if(!s)
01282         return -1;
01283 
01284     u = (struct pico_socket_udp *) s;
01285     *ttl = u->mc_ttl;
01286     return 0;
01287 }
01288 #else
01289 int pico_udp_set_mc_ttl(struct pico_socket *s, void  *_ttl)
01290 {
01291     IGNORE_PARAMETER(s);
01292     IGNORE_PARAMETER(_ttl);
01293     pico_err = PICO_ERR_EPROTONOSUPPORT;
01294     return -1;
01295 }
01296 
01297 int pico_udp_get_mc_ttl(struct pico_socket *s, uint8_t *ttl)
01298 {
01299     IGNORE_PARAMETER(s);
01300     IGNORE_PARAMETER(ttl);
01301     pico_err = PICO_ERR_EPROTONOSUPPORT;
01302     return -1;
01303 }
01304 
01305 int pico_socket_mcast_filter(struct pico_socket *s, union pico_address *mcast_group, union pico_address *src)
01306 {
01307     IGNORE_PARAMETER(s);
01308     IGNORE_PARAMETER(mcast_group);
01309     IGNORE_PARAMETER(src);
01310     pico_err = PICO_ERR_EPROTONOSUPPORT;
01311     return -1;
01312 }
01313 
01314 void pico_multicast_delete(struct pico_socket *s)
01315 {
01316     (void)s;
01317 }
01318 
01319 int pico_getsockopt_mcast(struct pico_socket *s, int option, void *value)
01320 {
01321     (void)s;
01322     (void)option;
01323     (void)value;
01324     pico_err = PICO_ERR_EPROTONOSUPPORT;
01325     return -1;
01326 }
01327 
01328 int pico_setsockopt_mcast(struct pico_socket *s, int option, void *value)
01329 {
01330     (void)s;
01331     (void)option;
01332     (void)value;
01333     pico_err = PICO_ERR_EPROTONOSUPPORT;
01334     return -1;
01335 
01336 }
01337 #endif /* PICO_SUPPORT_MCAST */
01338