pico_igmp.c

00001 /*********************************************************************
00002 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
00003 See LICENSE and COPYING for usage.
00004 
00005 RFC 1112, 2236, 3376, 3569, 3678, 4607
00006 
00007 Authors: Kristof Roelants (IGMPv3), Simon Maes, Brecht Van Cauwenberghe 
00008 *********************************************************************/
00009 
00010 #include "pico_stack.h"
00011 #include "pico_ipv4.h"
00012 #include "pico_igmp.h"
00013 #include "pico_config.h"
00014 #include "pico_eth.h"
00015 #include "pico_addressing.h"
00016 #include "pico_frame.h"
00017 #include "pico_tree.h"
00018 #include "pico_device.h"
00019 #include "pico_socket.h"
00020 
00021 #define igmp_dbg(...) do{}while(0)
00022 //#define igmp_dbg dbg
00023 
00024 /* membership states */
00025 #define IGMP_STATE_NON_MEMBER             (0x0)
00026 #define IGMP_STATE_DELAYING_MEMBER        (0x1)
00027 #define IGMP_STATE_IDLE_MEMBER            (0x2)
00028 
00029 /* events */ 
00030 #define IGMP_EVENT_DELETE_GROUP           (0x0)
00031 #define IGMP_EVENT_CREATE_GROUP           (0x1)
00032 #define IGMP_EVENT_UPDATE_GROUP           (0x2)
00033 #define IGMP_EVENT_QUERY_RECV             (0x3)
00034 #define IGMP_EVENT_REPORT_RECV            (0x4)
00035 #define IGMP_EVENT_TIMER_EXPIRED          (0x5)
00036 
00037 /* message types */
00038 #define IGMP_TYPE_MEM_QUERY               (0x11)
00039 #define IGMP_TYPE_MEM_REPORT_V1           (0x12)
00040 #define IGMP_TYPE_MEM_REPORT_V2           (0x16)
00041 #define IGMP_TYPE_LEAVE_GROUP             (0x17)
00042 #define IGMP_TYPE_MEM_REPORT_V3           (0x22)
00043 
00044 /* group record types */
00045 #define IGMP_MODE_IS_INCLUDE              (1)
00046 #define IGMP_MODE_IS_EXCLUDE              (2)
00047 #define IGMP_CHANGE_TO_INCLUDE_MODE       (3)
00048 #define IGMP_CHANGE_TO_EXCLUDE_MODE       (4)
00049 #define IGMP_ALLOW_NEW_SOURCES            (5)
00050 #define IGMP_BLOCK_OLD_SOURCES            (6)
00051 
00052 /* host flag */
00053 #define IGMP_HOST_LAST                    (0x1)
00054 #define IGMP_HOST_NOT_LAST                (0x0)
00055 
00056 /* list of timers, counters and their default values */
00057 #define IGMP_ROBUSTNESS                   (2)
00058 #define IGMP_QUERY_INTERVAL               (125) /* secs */
00059 #define IGMP_QUERY_RESPONSE_INTERVAL      (10) /* secs */
00060 #define IGMP_STARTUP_QUERY_INTERVAL       (IGMPV3_QUERY_INTERVAL / 4)
00061 #define IGMP_STARTUP_QUERY_COUNT          (IGMPV3_ROBUSTNESS)
00062 #define IGMP_LAST_MEMBER_QUERY_INTERVAL   (1) /* secs */
00063 #define IGMP_LAST_MEMBER_QUERY_COUNT      (IGMPV3_ROBUSTNESS)
00064 #define IGMP_UNSOLICITED_REPORT_INTERVAL  (1) /* secs */
00065 #define IGMP_DEFAULT_MAX_RESPONSE_TIME    (100)
00066 
00067 /* custom timers types */
00068 #define IGMP_TIMER_GROUP_REPORT           (1)
00069 #define IGMP_TIMER_V1_QUERIER             (2)
00070 #define IGMP_TIMER_V2_QUERIER             (3)
00071 
00072 /* IGMP groups */
00073 #define IGMP_ALL_HOST_GROUP               long_be(0xE0000001) /* 224.0.0.1 */
00074 #define IGMP_ALL_ROUTER_GROUP             long_be(0xE0000002) /* 224.0.0.2 */
00075 #define IGMPV3_ALL_ROUTER_GROUP           long_be(0xE0000016) /* 224.0.0.22 */
00076 
00077 /* misc */
00078 #define IGMP_TIMER_STOPPED                (1)
00079 #define IP_OPTION_ROUTER_ALERT_LEN        (4)
00080 #define IGMP_MAX_GROUPS                   (32) /* max 255 */
00081 
00082 struct __attribute__((packed)) igmp_message {
00083   uint8_t type;
00084   uint8_t max_resp_time;
00085   uint16_t crc;
00086   uint32_t mcast_group;
00087 };
00088 
00089 struct __attribute__((packed)) igmpv3_query {
00090   uint8_t type;
00091   uint8_t max_resp_time;
00092   uint16_t crc;
00093   uint32_t mcast_group;
00094   uint8_t rsq;
00095   uint8_t qqic;
00096   uint16_t sources;
00097   uint32_t source_addr[0];
00098 };
00099 
00100 struct __attribute__((packed)) igmpv3_group_record {
00101   uint8_t type;
00102   uint8_t aux;
00103   uint16_t sources;
00104   uint32_t mcast_group;
00105   uint32_t source_addr[0];
00106 };
00107 
00108 struct __attribute__((packed)) igmpv3_report {
00109   uint8_t type;
00110   uint8_t res0;
00111   uint16_t crc;
00112   uint16_t res1;
00113   uint16_t groups;
00114   struct igmpv3_group_record record[0];
00115 };
00116 
00117 struct igmp_parameters {
00118   uint8_t event;
00119   uint8_t state;
00120   uint8_t last_host;
00121   uint8_t filter_mode;
00122   uint8_t max_resp_time;
00123   struct pico_ip4 mcast_link;
00124   struct pico_ip4 mcast_group;
00125   struct pico_tree *MCASTFilter;
00126   struct pico_frame *f;
00127 };
00128 
00129 struct igmp_timer {
00130   uint8_t type;
00131   uint8_t stopped;
00132   unsigned long start;
00133   unsigned long delay;
00134   struct pico_ip4 mcast_link;
00135   struct pico_ip4 mcast_group;
00136   struct pico_frame *f;
00137   void (*callback)(struct igmp_timer *t);
00138 };
00139 
00140 /* queues */
00141 static struct pico_queue igmp_in = {};
00142 static struct pico_queue igmp_out = {};
00143 
00144 /* finite state machine caller */
00145 static int pico_igmp_process_event(struct igmp_parameters *p);
00146 
00147 /* state callback prototype */
00148 typedef int (*callback)(struct igmp_parameters *);
00149 
00150 /* redblack trees */
00151 static int igmp_timer_cmp(void *ka, void *kb)
00152 {
00153   struct igmp_timer *a = ka, *b =kb;
00154   if (a->type < b->type)
00155     return -1;
00156   if (a->type > b->type)
00157     return 1;
00158   if (a->mcast_group.addr < b->mcast_group.addr)
00159     return -1;
00160   if (a->mcast_group.addr > b->mcast_group.addr)
00161     return 1;
00162   if (a->mcast_link.addr < b->mcast_link.addr)
00163     return -1;
00164   if (a->mcast_link.addr > b->mcast_link.addr)
00165     return 1;
00166   return 0;
00167 }
00168 PICO_TREE_DECLARE(IGMPTimers, igmp_timer_cmp);
00169 
00170 static int igmp_parameters_cmp(void *ka, void *kb)
00171 {
00172   struct igmp_parameters *a = ka, *b = kb;
00173   if (a->mcast_group.addr < b->mcast_group.addr)
00174     return -1;
00175   if (a->mcast_group.addr > b->mcast_group.addr)
00176     return 1;
00177   if (a->mcast_link.addr < b->mcast_link.addr)
00178     return -1;
00179   if (a->mcast_link.addr > b->mcast_link.addr)
00180     return 1;
00181   return 0;
00182 }
00183 PICO_TREE_DECLARE(IGMPParameters, igmp_parameters_cmp);
00184 
00185 static int igmp_sources_cmp(void *ka, void *kb)
00186 {
00187   struct pico_ip4 *a = ka, *b = kb;
00188   if (a->addr < b->addr)
00189     return -1;
00190   if (a->addr > b->addr)
00191     return 1;
00192   return 0;
00193 }
00194 PICO_TREE_DECLARE(IGMPAllow, igmp_sources_cmp);
00195 PICO_TREE_DECLARE(IGMPBlock, igmp_sources_cmp);
00196 
00197 static struct igmp_parameters *pico_igmp_find_parameter(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group)
00198 {
00199   struct igmp_parameters test = {0};
00200   test.mcast_link.addr = mcast_link->addr;
00201   test.mcast_group.addr = mcast_group->addr;
00202   return pico_tree_findKey(&IGMPParameters, &test);
00203 }
00204 
00205 static int pico_igmp_delete_parameter(struct igmp_parameters *p)
00206 {
00207   if (pico_tree_delete(&IGMPParameters, p))
00208     pico_free(p);
00209   else
00210     return -1;
00211 
00212   return 0;
00213 }
00214 
00215 static void pico_igmp_timer_expired(unsigned long now, void *arg)
00216 {
00217   struct igmp_timer *t = NULL, *timer = NULL, test = {0};
00218 
00219   t = (struct igmp_timer *)arg;
00220   test.type = t->type;
00221   test.mcast_link = t->mcast_link;
00222   test.mcast_group = t->mcast_group;
00223   igmp_dbg("IGMP: timer expired for %08X link %08X type %u, delay %lu\n", t->mcast_group.addr, t->mcast_link.addr, t->type, t->delay);
00224   timer = pico_tree_findKey(&IGMPTimers, &test);
00225   if (!timer) {
00226     return;
00227   }
00228   if (timer->stopped == IGMP_TIMER_STOPPED) {
00229     pico_free(t);
00230     return;
00231   }
00232   if (timer->start + timer->delay < PICO_TIME_MS()) {
00233     pico_tree_delete(&IGMPTimers, timer);
00234     if (timer->callback)
00235       timer->callback(timer);
00236     pico_free(timer);
00237   } else {
00238     igmp_dbg("IGMP: restart timer for %08X, delay %lu, new delay %lu\n", t->mcast_group.addr, t->delay,  (timer->start + timer->delay) - PICO_TIME_MS());
00239     pico_timer_add((timer->start + timer->delay) - PICO_TIME_MS(), &pico_igmp_timer_expired, timer);
00240   }
00241   return;
00242 }
00243 
00244 static int pico_igmp_timer_reset(struct igmp_timer *t)
00245 {
00246   struct igmp_timer *timer = NULL, test = {0};
00247 
00248   igmp_dbg("IGMP: reset timer for %08X, delay %lu\n", t->mcast_group.addr, t->delay);
00249   test.type = t->type;
00250   test.mcast_link = t->mcast_link;
00251   test.mcast_group = t->mcast_group;
00252   timer = pico_tree_findKey(&IGMPTimers, &test);
00253   if (!timer)
00254     return -1;
00255 
00256   *timer = *t;
00257   timer->start = PICO_TIME_MS();
00258   return 0;
00259 }
00260 
00261 static int pico_igmp_timer_start(struct igmp_timer *t)
00262 {
00263   struct igmp_timer *timer = NULL, test = {0};
00264 
00265   igmp_dbg("IGMP: start timer for %08X link %08X type %u, delay %lu\n", t->mcast_group.addr, t->mcast_link.addr, t->type, t->delay);
00266   test.type = t->type;
00267   test.mcast_link = t->mcast_link;
00268   test.mcast_group = t->mcast_group;
00269   timer = pico_tree_findKey(&IGMPTimers, &test);
00270   if (timer)
00271     return pico_igmp_timer_reset(t);
00272 
00273   timer = pico_zalloc(sizeof(struct igmp_timer));
00274   if (!timer) {
00275     pico_err = PICO_ERR_ENOMEM;
00276     return -1;
00277   }
00278   *timer = *t;
00279   timer->start = PICO_TIME_MS();
00280 
00281   pico_tree_insert(&IGMPTimers, timer);
00282   pico_timer_add(timer->delay, &pico_igmp_timer_expired, timer);
00283   return 0;
00284 }
00285 
00286 static int pico_igmp_timer_stop(struct igmp_timer *t)
00287 {
00288   struct igmp_timer *timer = NULL, test = {0};
00289 
00290   test.type = t->type;
00291   test.mcast_link = t->mcast_link;
00292   test.mcast_group = t->mcast_group;
00293   timer = pico_tree_findKey(&IGMPTimers, &test);
00294   if (!timer)
00295     return 0;
00296 
00297   igmp_dbg("IGMP: stop timer for %08X, delay %lu\n", timer->mcast_group.addr, timer->delay);
00298   timer->stopped = IGMP_TIMER_STOPPED;
00299   return 0;
00300 }
00301 
00302 static int pico_igmp_timer_is_running(struct igmp_timer *t)
00303 {
00304   struct igmp_timer *timer = NULL, test = {0};
00305 
00306   test.type = t->type;
00307   test.mcast_link = t->mcast_link;
00308   test.mcast_group = t->mcast_group;
00309   timer = pico_tree_findKey(&IGMPTimers, &test);
00310   if (timer)
00311     return 1;
00312   return 0;
00313 }
00314 
00315 static struct igmp_timer *pico_igmp_find_timer(uint8_t type, struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group)
00316 {
00317   struct igmp_timer test = {0};
00318 
00319   test.type = type;
00320   test.mcast_link = *mcast_link;
00321   test.mcast_group = *mcast_group;
00322   return pico_tree_findKey(&IGMPTimers, &test);
00323 }
00324 
00325 static void pico_igmp_report_expired(struct igmp_timer *t)
00326 {
00327   struct igmp_parameters *p = NULL;
00328 
00329   p = pico_igmp_find_parameter(&t->mcast_link, &t->mcast_group);
00330   if (!p)
00331     return;
00332 
00333   p->event = IGMP_EVENT_TIMER_EXPIRED;
00334   pico_igmp_process_event(p);
00335 }
00336 
00337 static void pico_igmp_v2querier_expired(struct igmp_timer *t)
00338 {
00339   struct pico_ipv4_link *link = NULL;
00340   struct pico_tree_node *index = NULL, *_tmp = NULL;
00341 
00342   link = pico_ipv4_link_by_dev(t->f->dev);
00343   if (!link)
00344     return;
00345 
00346   /* When changing compatibility mode, cancel all pending response 
00347    * and retransmission timers.
00348    */
00349   pico_tree_foreach_safe(index, &IGMPTimers, _tmp) 
00350   {
00351     ((struct igmp_timer *)index->keyValue)->stopped = IGMP_TIMER_STOPPED;
00352     pico_tree_delete(&IGMPTimers, index->keyValue);
00353   }
00354   igmp_dbg("IGMP: switch to compatibility mode IGMPv3\n");
00355   link->mcast_compatibility = PICO_IGMPV3;
00356   return;
00357 }
00358 
00359 static int pico_igmp_is_checksum_valid(struct pico_frame *f)
00360 {
00361   struct pico_ipv4_hdr *hdr = NULL;
00362   uint8_t ihl = 24, datalen = 0;
00363 
00364   hdr = (struct pico_ipv4_hdr *)f->net_hdr;
00365   ihl = (hdr->vhl & 0x0F) * 4; /* IHL is in 32bit words */
00366   datalen = short_be(hdr->len) - ihl;
00367 
00368   if (short_be(pico_checksum(f->transport_hdr, datalen)) == 0)
00369     return 1;
00370   igmp_dbg("IGMP: invalid checksum\n");
00371   return 0;
00372 }
00373 
00374 /* RFC 3376 $7.1 */
00375 static int pico_igmp_compatibility_mode(struct pico_frame *f)
00376 {
00377   struct pico_ipv4_hdr *hdr = NULL;
00378   struct pico_ipv4_link *link = NULL;
00379   struct pico_tree_node *index = NULL, *_tmp = NULL;
00380   struct igmp_timer t = {0};
00381   uint8_t ihl = 24, datalen = 0;
00382 
00383   link = pico_ipv4_link_by_dev(f->dev);
00384   if (!link)
00385     return -1;
00386 
00387   hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00388   ihl = (hdr->vhl & 0x0F) * 4; /* IHL is in 32bit words */
00389   datalen = short_be(hdr->len) - ihl;
00390   igmp_dbg("IGMP: IHL = %u, LEN = %u, OCTETS = %u\n", ihl, short_be(hdr->len), datalen);
00391 
00392   if (datalen > 12) {
00393     /* IGMPv3 query */
00394     t.type = IGMP_TIMER_V2_QUERIER;
00395     if (pico_igmp_timer_is_running(&t)) { /* IGMPv2 querier present timer still running */
00396       return -1;
00397     } else {
00398       link->mcast_compatibility = PICO_IGMPV3;
00399       return 0;
00400     }
00401   } else if (datalen == 8) {
00402     struct igmp_message *query = (struct igmp_message *)f->transport_hdr;
00403     if (query->max_resp_time != 0) {
00404       /* IGMPv2 query */
00405       /* When changing compatibility mode, cancel all pending response 
00406        * and retransmission timers.
00407        */
00408       pico_tree_foreach_safe(index, &IGMPTimers, _tmp) 
00409       {
00410         ((struct igmp_timer *)index->keyValue)->stopped = IGMP_TIMER_STOPPED;
00411         pico_tree_delete(&IGMPTimers, index->keyValue);
00412       }
00413       igmp_dbg("IGMP: switch to compatibility mode IGMPv2\n");
00414       link->mcast_compatibility = PICO_IGMPV2;
00415       t.type = IGMP_TIMER_V2_QUERIER;
00416       t.delay = ((IGMP_ROBUSTNESS * link->mcast_last_query_interval) + IGMP_QUERY_RESPONSE_INTERVAL) * 1000;
00417       t.f = f;
00418       t.callback = pico_igmp_v2querier_expired;
00419       /* only one of this type of timer may exist! */
00420       pico_igmp_timer_start(&t);
00421     } else {
00422       /* IGMPv1 query, not supported */
00423       return -1;
00424     }
00425   } else {
00426     /* invalid query, silently ignored */
00427     return -1;
00428   }
00429   return 0;
00430 }
00431 
00432 static struct igmp_parameters *pico_igmp_analyse_packet(struct pico_frame *f)
00433 {
00434   struct igmp_message *message = NULL;
00435   struct igmp_parameters *p = NULL;
00436   struct pico_ipv4_link *link = NULL;
00437   struct pico_ip4 mcast_group = {0};
00438 
00439   link = pico_ipv4_link_by_dev(f->dev);
00440   if (!link)
00441     return NULL;
00442 
00443   /* IGMPv2 and IGMPv3 have a similar structure for the first 8 bytes */ 
00444   message = (struct igmp_message *)f->transport_hdr;
00445   mcast_group.addr = message->mcast_group;
00446   p = pico_igmp_find_parameter(&link->address, &mcast_group);
00447   if (!p && mcast_group.addr == 0) { /* general query */
00448     p = pico_zalloc(sizeof(struct igmp_parameters));
00449     if (!p)
00450       return NULL;
00451     p->state = IGMP_STATE_NON_MEMBER;
00452     p->mcast_link.addr = link->address.addr;
00453     p->mcast_group.addr = mcast_group.addr;
00454     pico_tree_insert(&IGMPParameters, p);
00455   } else if (!p) {
00456     return NULL;
00457   }
00458 
00459   switch (message->type) {
00460     case IGMP_TYPE_MEM_QUERY:
00461        p->event = IGMP_EVENT_QUERY_RECV;
00462        break;
00463     case IGMP_TYPE_MEM_REPORT_V1:
00464        p->event = IGMP_EVENT_REPORT_RECV;
00465        break;
00466     case IGMP_TYPE_MEM_REPORT_V2:
00467        p->event = IGMP_EVENT_REPORT_RECV;
00468        break;
00469     case IGMP_TYPE_MEM_REPORT_V3:
00470        p->event = IGMP_EVENT_REPORT_RECV;
00471        break;
00472     default:
00473        return NULL;
00474   }
00475   p->max_resp_time = message->max_resp_time; /* if IGMPv3 report this will be 0 (res0 field) */
00476   p->f = f;
00477 
00478   return p;
00479 }
00480 
00481 static int pico_igmp_process_in(struct pico_protocol *self, struct pico_frame *f)
00482 {
00483   struct igmp_parameters *p = NULL;
00484  
00485   if (!pico_igmp_is_checksum_valid(f))
00486     goto out;
00487   if (pico_igmp_compatibility_mode(f) < 0)
00488     goto out;
00489   p = pico_igmp_analyse_packet(f);
00490   if (!p)
00491     goto out;
00492 
00493   return pico_igmp_process_event(p);
00494 
00495   out:
00496     pico_frame_discard(f);
00497     return 0;
00498 }
00499 
00500 static int pico_igmp_process_out(struct pico_protocol *self, struct pico_frame *f) {
00501   /* packets are directly transferred to the IP layer by calling pico_ipv4_frame_push */
00502   return 0;
00503 }
00504 
00505 /* Interface: protocol definition */
00506 struct pico_protocol pico_proto_igmp = {
00507   .name = "igmp",
00508   .proto_number = PICO_PROTO_IGMP,
00509   .layer = PICO_LAYER_TRANSPORT,
00510   .process_in = pico_igmp_process_in,
00511   .process_out = pico_igmp_process_out,
00512   .q_in = &igmp_in,
00513   .q_out = &igmp_out,
00514 };
00515 
00516 int pico_igmp_state_change(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t filter_mode, struct pico_tree *MCASTFilter, uint8_t state) 
00517 {
00518   struct igmp_parameters *p = NULL;
00519   
00520   if (mcast_group->addr == IGMP_ALL_HOST_GROUP)
00521     return 0;
00522 
00523   p = pico_igmp_find_parameter(mcast_link, mcast_group);
00524   if (!p && state == PICO_IGMP_STATE_CREATE) {
00525     p = pico_zalloc(sizeof(struct igmp_parameters));
00526     if (!p) {
00527       pico_err = PICO_ERR_ENOMEM;
00528       return -1;
00529     }
00530     p->state = IGMP_STATE_NON_MEMBER;
00531     p->mcast_link = *mcast_link;
00532     p->mcast_group = *mcast_group;
00533     pico_tree_insert(&IGMPParameters, p);
00534   } else if (!p) {
00535     pico_err = PICO_ERR_EINVAL;
00536     return -1;
00537   }
00538 
00539   switch (state) {
00540     case PICO_IGMP_STATE_CREATE:
00541       p->event = IGMP_EVENT_CREATE_GROUP;
00542       break;
00543 
00544     case PICO_IGMP_STATE_UPDATE:
00545       p->event = IGMP_EVENT_UPDATE_GROUP;
00546       break;
00547     
00548     case PICO_IGMP_STATE_DELETE:
00549       p->event = IGMP_EVENT_DELETE_GROUP;
00550       break;
00551 
00552     default:
00553       return -1;
00554   }
00555   p->filter_mode = filter_mode;
00556   p->MCASTFilter = MCASTFilter;
00557 
00558   return pico_igmp_process_event(p);
00559 }
00560 
00561 static int pico_igmp_send_report(struct igmp_parameters *p, struct pico_frame *f)
00562 {
00563   struct pico_ip4 dst = {0};
00564   struct pico_ip4 mcast_group = {0};
00565   struct pico_ipv4_link *link = NULL;
00566   
00567   link = pico_ipv4_link_get(&p->mcast_link);
00568   if (!link)
00569     return -1;
00570 
00571   mcast_group.addr = p->mcast_group.addr;
00572   switch (link->mcast_compatibility) {
00573     case PICO_IGMPV2:
00574       if (p->event == IGMP_EVENT_DELETE_GROUP)
00575         dst.addr = IGMP_ALL_ROUTER_GROUP;
00576       else
00577         dst.addr = mcast_group.addr;
00578       break;
00579 
00580     case PICO_IGMPV3:
00581       dst.addr = IGMPV3_ALL_ROUTER_GROUP;
00582       break;
00583 
00584     default:
00585       pico_err = PICO_ERR_EPROTONOSUPPORT;
00586       return -1;
00587   }
00588 
00589   igmp_dbg("IGMP: send membership report on group %08X to %08X\n", mcast_group.addr, dst.addr);
00590   pico_ipv4_frame_push(f, &dst, PICO_PROTO_IGMP);
00591   return 0;
00592 }
00593 
00594 static int pico_igmp_generate_report(struct igmp_parameters *p)
00595 {
00596   struct pico_ipv4_link *link = NULL;
00597   int i = 0;
00598 
00599   link = pico_ipv4_link_get(&p->mcast_link);
00600   if (!link) {
00601     pico_err = PICO_ERR_EINVAL;
00602     return -1;
00603   }
00604 
00605   switch (link->mcast_compatibility) {
00606     case PICO_IGMPV1:
00607       pico_err = PICO_ERR_EPROTONOSUPPORT;
00608       return -1;
00609       
00610     case PICO_IGMPV2:
00611     {
00612       struct igmp_message *report = NULL;
00613       uint8_t report_type = IGMP_TYPE_MEM_REPORT_V2;
00614       if (p->event == IGMP_EVENT_DELETE_GROUP)
00615         report_type = IGMP_TYPE_LEAVE_GROUP;
00616 
00617       p->f = pico_proto_ipv4.alloc(&pico_proto_ipv4, IP_OPTION_ROUTER_ALERT_LEN + sizeof(struct igmp_message));
00618       p->f->net_len += IP_OPTION_ROUTER_ALERT_LEN;
00619       p->f->transport_hdr += IP_OPTION_ROUTER_ALERT_LEN;
00620       p->f->transport_len -= IP_OPTION_ROUTER_ALERT_LEN;
00621       p->f->dev = pico_ipv4_link_find(&p->mcast_link);
00622       /* p->f->len is correctly set by alloc */
00623 
00624       report = (struct igmp_message *)p->f->transport_hdr;
00625       report->type = report_type;
00626       report->max_resp_time = IGMP_DEFAULT_MAX_RESPONSE_TIME;
00627       report->mcast_group = p->mcast_group.addr;
00628 
00629       report->crc = 0;
00630       report->crc = short_be(pico_checksum(report, sizeof(struct igmp_message)));
00631       break;
00632     }
00633     case PICO_IGMPV3:
00634     {
00635       struct igmpv3_report *report = NULL;
00636       struct igmpv3_group_record *record = NULL;
00637       struct pico_mcast_group *g = NULL, test = {0};
00638       struct pico_tree_node *index = NULL, *_tmp = NULL;
00639       struct pico_tree *IGMPFilter = NULL;
00640       struct pico_ip4 *source = NULL;
00641       uint8_t record_type = 0;
00642       uint8_t sources = 0;
00643       int len = 0;
00644 
00645       test.mcast_addr = p->mcast_group;
00646       g = pico_tree_findKey(link->MCASTGroups, &test);
00647       if (!g) {
00648         pico_err = PICO_ERR_EINVAL;
00649         return -1;
00650       }
00651 
00652       if (p->event == IGMP_EVENT_DELETE_GROUP) { /* "non-existent" state of filter mode INCLUDE and empty source list */
00653         p->filter_mode = PICO_IP_MULTICAST_INCLUDE;
00654         p->MCASTFilter = NULL;
00655       }
00656 
00657       /* cleanup filters */
00658       pico_tree_foreach_safe(index, &IGMPAllow, _tmp) 
00659       {
00660         pico_tree_delete(&IGMPAllow, index->keyValue);
00661       }
00662       pico_tree_foreach_safe(index, &IGMPBlock, _tmp) 
00663       {
00664         pico_tree_delete(&IGMPBlock, index->keyValue);
00665       }
00666 
00667       switch (g->filter_mode) {
00668 
00669         case PICO_IP_MULTICAST_INCLUDE:
00670           switch (p->filter_mode) {
00671             case PICO_IP_MULTICAST_INCLUDE:
00672               if (p->event == IGMP_EVENT_DELETE_GROUP) { /* all ADD_SOURCE_MEMBERSHIP had an equivalent DROP_SOURCE_MEMBERSHIP */
00673                 /* TO_IN (B) */
00674                 record_type = IGMP_CHANGE_TO_INCLUDE_MODE;
00675                 IGMPFilter = &IGMPAllow;
00676                 if (p->MCASTFilter) {
00677                   pico_tree_foreach(index, p->MCASTFilter) /* B */
00678                   {
00679                     pico_tree_insert(&IGMPAllow, index->keyValue);
00680                     sources++;
00681                   }
00682                 } /* else { IGMPAllow stays empty } */
00683                 break;
00684               }
00685 
00686               /* ALLOW (B-A) */
00687               /* if event is CREATE A will be empty, thus only ALLOW (B-A) has sense */
00688               if (p->event == IGMP_EVENT_CREATE_GROUP) /* first ADD_SOURCE_MEMBERSHIP */
00689                 record_type = IGMP_CHANGE_TO_INCLUDE_MODE;
00690               else
00691                 record_type = IGMP_ALLOW_NEW_SOURCES;
00692               IGMPFilter = &IGMPAllow;
00693               pico_tree_foreach(index, p->MCASTFilter) /* B */
00694               {
00695                 pico_tree_insert(&IGMPAllow, index->keyValue);
00696                 sources++;
00697               }
00698               pico_tree_foreach(index, &g->MCASTSources) /* A */
00699               {
00700                 source = pico_tree_findKey(&IGMPAllow, index->keyValue);
00701                 if (source) {
00702                   pico_tree_delete(&IGMPAllow, source);
00703                   sources--;
00704                 }
00705               }
00706               if (!pico_tree_empty(&IGMPAllow)) /* record type is ALLOW */
00707                 break;
00708 
00709               /* BLOCK (A-B) */
00710               record_type = IGMP_BLOCK_OLD_SOURCES;
00711               IGMPFilter = &IGMPBlock;
00712               pico_tree_foreach(index, &g->MCASTSources) /* A */
00713               {
00714                 pico_tree_insert(&IGMPBlock, index->keyValue);
00715                 sources++;
00716               }
00717               pico_tree_foreach(index, p->MCASTFilter) /* B */
00718               {
00719                 source = pico_tree_findKey(&IGMPBlock, index->keyValue);
00720                 if (source) {
00721                   pico_tree_delete(&IGMPBlock, source);
00722                   sources--;
00723                 }
00724               }
00725               if (!pico_tree_empty(&IGMPBlock)) /* record type is BLOCK */
00726                 break;
00727 
00728               /* ALLOW (B-A) and BLOCK (A-B) are empty: do not send report (RFC 3376 $5.1) */
00729               p->f = NULL;
00730               return 0;
00731 
00732             case PICO_IP_MULTICAST_EXCLUDE:
00733               /* TO_EX (B) */
00734               record_type = IGMP_CHANGE_TO_EXCLUDE_MODE;
00735               IGMPFilter = &IGMPBlock;
00736               pico_tree_foreach(index, p->MCASTFilter) /* B */
00737               {
00738                 pico_tree_insert(&IGMPBlock, index->keyValue);
00739                 sources++;
00740               }
00741               break;
00742 
00743             default:
00744               pico_err = PICO_ERR_EINVAL;
00745               return -1;
00746           }
00747           break;
00748 
00749         case PICO_IP_MULTICAST_EXCLUDE:
00750           switch (p->filter_mode) {
00751             case PICO_IP_MULTICAST_INCLUDE:
00752               /* TO_IN (B) */
00753               record_type = IGMP_CHANGE_TO_INCLUDE_MODE;
00754               IGMPFilter = &IGMPAllow;
00755               if (p->MCASTFilter) {
00756                 pico_tree_foreach(index, p->MCASTFilter) /* B */
00757                 {
00758                   pico_tree_insert(&IGMPAllow, index->keyValue);
00759                   sources++;
00760                 }
00761               } /* else { IGMPAllow stays empty } */
00762               break;
00763 
00764             case PICO_IP_MULTICAST_EXCLUDE:
00765               /* BLOCK (B-A) */
00766               record_type = IGMP_BLOCK_OLD_SOURCES;
00767               IGMPFilter = &IGMPBlock;
00768               pico_tree_foreach(index, p->MCASTFilter)
00769               {
00770                 pico_tree_insert(&IGMPBlock, index->keyValue);
00771                 sources++;
00772               }
00773               pico_tree_foreach(index, &g->MCASTSources) /* A */
00774               {
00775                 source = pico_tree_findKey(&IGMPBlock, index->keyValue); /* B */
00776                 if (source) {
00777                   pico_tree_delete(&IGMPBlock, source);
00778                   sources--;
00779                 }
00780               }
00781               if (!pico_tree_empty(&IGMPBlock)) /* record type is BLOCK */
00782                 break;
00783 
00784               /* ALLOW (A-B) */
00785               record_type = IGMP_ALLOW_NEW_SOURCES;
00786               IGMPFilter = &IGMPAllow;
00787               pico_tree_foreach(index, &g->MCASTSources)
00788               {
00789                 pico_tree_insert(&IGMPAllow, index->keyValue);
00790                 sources++;
00791               }
00792               pico_tree_foreach(index, p->MCASTFilter) /* B */
00793               {
00794                 source = pico_tree_findKey(&IGMPAllow, index->keyValue); /* A */
00795                 if (source) {
00796                   pico_tree_delete(&IGMPAllow, source);
00797                   sources--;
00798                 }
00799               }
00800               if (!pico_tree_empty(&IGMPAllow)) /* record type is ALLOW */
00801                 break;
00802 
00803               /* BLOCK (B-A) and ALLOW (A-B) are empty: do not send report (RFC 3376 $5.1) */
00804               p->f = NULL;
00805               return 0;
00806 
00807             default:
00808               pico_err = PICO_ERR_EINVAL;
00809               return -1;
00810           }
00811           break;
00812 
00813         default:
00814           pico_err = PICO_ERR_EINVAL;
00815           return -1;
00816       }
00817 
00818       len = sizeof(struct igmpv3_report) + sizeof(struct igmpv3_group_record) + (sources * sizeof(struct pico_ip4));
00819       p->f = pico_proto_ipv4.alloc(&pico_proto_ipv4, IP_OPTION_ROUTER_ALERT_LEN + len);
00820       p->f->net_len += IP_OPTION_ROUTER_ALERT_LEN;
00821       p->f->transport_hdr += IP_OPTION_ROUTER_ALERT_LEN;
00822       p->f->transport_len -= IP_OPTION_ROUTER_ALERT_LEN;
00823       p->f->dev = pico_ipv4_link_find(&p->mcast_link);
00824       /* p->f->len is correctly set by alloc */
00825 
00826       report = (struct igmpv3_report *)p->f->transport_hdr;
00827       report->type = IGMP_TYPE_MEM_REPORT_V3;
00828       report->res0 = 0;
00829       report->crc = 0;
00830       report->res1 = 0;
00831       report->groups = short_be(1);
00832 
00833       record = &report->record[0];
00834       record->type = record_type;
00835       record->aux = 0;
00836       record->sources = short_be(sources);
00837       record->mcast_group = p->mcast_group.addr;
00838       if (!pico_tree_empty(IGMPFilter)) {
00839         i = 0;
00840         pico_tree_foreach(index, IGMPFilter)
00841         {
00842           record->source_addr[i] = ((struct pico_ip4 *)index->keyValue)->addr;
00843           i++;
00844         }
00845       }
00846       report->crc = short_be(pico_checksum(report, len));
00847       break;
00848     }
00849 
00850     default:
00851       pico_err = PICO_ERR_EINVAL;
00852       return -1;
00853   }
00854   return 0;
00855 }
00856 
00857 /* stop timer, send leave if flag set */
00858 static int stslifs(struct igmp_parameters *p)
00859 {
00860   struct igmp_timer t = {0};
00861 
00862   igmp_dbg("IGMP: event = leave group | action = stop timer, send leave if flag set\n");
00863 
00864   t.type = IGMP_TIMER_GROUP_REPORT;
00865   t.mcast_link = p->mcast_link;
00866   t.mcast_group = p->mcast_group;
00867   if (pico_igmp_timer_stop(&t) < 0)
00868     return -1;
00869 
00870   /* always send leave, even if not last host */
00871   if (pico_igmp_send_report(p, p->f) < 0)
00872     return -1;
00873 
00874   pico_igmp_delete_parameter(p);
00875   igmp_dbg("IGMP: new state = non-member\n");
00876   return 0;
00877 }
00878 
00879 /* send report, set flag, start timer */
00880 static int srsfst(struct igmp_parameters *p)
00881 {
00882   struct igmp_timer t = {0};
00883   struct pico_frame *copy_frame = NULL;
00884 
00885   igmp_dbg("IGMP: event = join group | action = send report, set flag, start timer\n");
00886 
00887   p->last_host = IGMP_HOST_LAST;
00888 
00889   if (pico_igmp_generate_report(p) < 0)
00890     return -1;
00891   if (!p->f)
00892     return 0;
00893   copy_frame = pico_frame_copy(p->f);
00894   if (!copy_frame) {
00895     pico_err = PICO_ERR_ENOMEM;
00896     return -1;
00897   }
00898   if (pico_igmp_send_report(p, copy_frame) < 0)
00899     return -1;
00900 
00901   t.type = IGMP_TIMER_GROUP_REPORT;
00902   t.mcast_link = p->mcast_link;
00903   t.mcast_group = p->mcast_group;
00904   t.delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000)); 
00905   t.f = p->f;
00906   t.callback = pico_igmp_report_expired;
00907   pico_igmp_timer_start(&t);
00908 
00909   p->state = IGMP_STATE_DELAYING_MEMBER;
00910   igmp_dbg("IGMP: new state = delaying member\n");
00911   return 0;
00912 }
00913 
00914 /* merge report, send report, reset timer (IGMPv3 only) */
00915 static int mrsrrt(struct igmp_parameters *p)
00916 {
00917   struct igmp_timer *t = NULL;
00918   struct pico_frame *copy_frame = NULL;
00919   struct pico_ipv4_link *link = NULL;
00920 
00921   igmp_dbg("IGMP: event = update group | action = merge report, send report, reset timer (IGMPv3 only)\n");
00922 
00923   link = pico_ipv4_link_get(&p->mcast_link);
00924   if (!link)
00925     return -1;
00926 
00927   if (link->mcast_compatibility != PICO_IGMPV3) {
00928     igmp_dbg("IGMP: no IGMPv3 compatible router on network\n");
00929     return -1;
00930   }
00931 
00932   /* XXX: merge with pending report rfc 3376 $5.1 */
00933 
00934   copy_frame = pico_frame_copy(p->f);
00935   if (!copy_frame)
00936     return -1;
00937   if (pico_igmp_send_report(p, copy_frame) < 0)
00938     return -1;
00939 
00940   t = pico_igmp_find_timer(IGMP_TIMER_GROUP_REPORT, &p->mcast_link, &p->mcast_group);
00941   if (!t)
00942     return -1;
00943   t->delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000)); 
00944   pico_igmp_timer_reset(t);
00945 
00946   p->state = IGMP_STATE_DELAYING_MEMBER;
00947   igmp_dbg("IGMP: new state = delaying member\n");
00948   return 0;
00949 }
00950 
00951 /* send report, start timer (IGMPv3 only) */
00952 static int srst(struct igmp_parameters *p)
00953 {
00954   struct igmp_timer t = {0};
00955   struct pico_frame *copy_frame = NULL;
00956   struct pico_ipv4_link *link = NULL;
00957 
00958   igmp_dbg("IGMP: event = update group | action = send report, start timer (IGMPv3 only)\n");
00959 
00960   link = pico_ipv4_link_get(&p->mcast_link);
00961   if (!link)
00962     return -1;
00963 
00964   if (link->mcast_compatibility != PICO_IGMPV3) {
00965     igmp_dbg("IGMP: no IGMPv3 compatible router on network\n");
00966     return -1;
00967   }
00968 
00969   if (pico_igmp_generate_report(p) < 0)
00970     return -1;
00971   if (!p->f)
00972     return 0;
00973   copy_frame = pico_frame_copy(p->f);
00974   if (!copy_frame)
00975     return -1;
00976   if (pico_igmp_send_report(p, copy_frame) < 0)
00977     return -1;
00978 
00979   t.type = IGMP_TIMER_GROUP_REPORT;
00980   t.mcast_link = p->mcast_link;
00981   t.mcast_group = p->mcast_group;
00982   t.delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000)); 
00983   t.f = p->f;
00984   t.callback = pico_igmp_report_expired;
00985   pico_igmp_timer_start(&t);
00986 
00987   p->state = IGMP_STATE_DELAYING_MEMBER;
00988   igmp_dbg("IGMP: new state = delaying member\n");
00989   return 0;
00990 }
00991 
00992 /* send leave if flag set */
00993 static int slifs(struct igmp_parameters *p)
00994 {
00995   igmp_dbg("IGMP: event = leave group | action = send leave if flag set\n");
00996 
00997   /* always send leave, even if not last host */
00998   if (pico_igmp_send_report(p, p->f) < 0)
00999     return -1;
01000 
01001   pico_igmp_delete_parameter(p);
01002   igmp_dbg("IGMP: new state = non-member\n");
01003   return 0;
01004 }
01005 
01006 /* start timer */
01007 static int st(struct igmp_parameters *p)
01008 {
01009   struct igmp_timer t = {0};
01010 
01011   igmp_dbg("IGMP: event = query received | action = start timer\n");
01012 
01013   if (pico_igmp_generate_report(p) < 0)
01014     return -1;
01015   if (!p->f)
01016     return -1;
01017 
01018   t.type = IGMP_TIMER_GROUP_REPORT;
01019   t.mcast_link = p->mcast_link;
01020   t.mcast_group = p->mcast_group;
01021   t.delay = (pico_rand() % (p->max_resp_time * 100)); 
01022   t.f = p->f;
01023   t.callback = pico_igmp_report_expired;
01024   pico_igmp_timer_start(&t);
01025 
01026   p->state = IGMP_STATE_DELAYING_MEMBER;
01027   igmp_dbg("IGMP: new state = delaying member\n");
01028   return 0;
01029 }
01030 
01031 /* stop timer, clear flag */
01032 static int stcl(struct igmp_parameters *p)
01033 {
01034   struct igmp_timer t = {0};
01035 
01036   igmp_dbg("IGMP: event = report received | action = stop timer, clear flag\n");
01037 
01038   t.type = IGMP_TIMER_GROUP_REPORT;
01039   t.mcast_link = p->mcast_link;
01040   t.mcast_group = p->mcast_group;
01041   if (pico_igmp_timer_stop(&t) < 0)
01042     return -1;
01043 
01044   p->last_host = IGMP_HOST_NOT_LAST;
01045   p->state = IGMP_STATE_IDLE_MEMBER;
01046   igmp_dbg("IGMP: new state = idle member\n");
01047   return 0;
01048 }
01049 
01050 /* send report, set flag */
01051 static int srsf(struct igmp_parameters *p)
01052 {
01053   igmp_dbg("IGMP: event = timer expired | action = send report, set flag\n");
01054 
01055   if (pico_igmp_send_report(p, p->f) < 0)
01056     return -1;
01057 
01058   p->state = IGMP_STATE_IDLE_MEMBER;
01059   igmp_dbg("IGMP: new state = idle member\n"); 
01060   return 0;
01061 }
01062 
01063 /* reset timer if max response time < current timer */
01064 static int rtimrtct(struct igmp_parameters *p)
01065 {
01066   struct igmp_timer *t = NULL;
01067   unsigned long time_to_run = 0;
01068 
01069   igmp_dbg("IGMP: event = query received | action = reset timer if max response time < current timer\n");
01070 
01071   t = pico_igmp_find_timer(IGMP_TIMER_GROUP_REPORT, &p->mcast_link, &p->mcast_group);
01072   if (!t)
01073     return -1;
01074 
01075   time_to_run = t->start + t->delay - PICO_TIME_MS();
01076   if ((p->max_resp_time * 100) < time_to_run) { /* max_resp_time in units of 1/10 seconds */
01077     t->delay = pico_rand() % (p->max_resp_time * 100); 
01078     pico_igmp_timer_reset(t);
01079   }
01080 
01081   p->state = IGMP_STATE_DELAYING_MEMBER;
01082   igmp_dbg("IGMP: new state = delaying member\n"); 
01083   return 0;
01084 }
01085 
01086 static int discard(struct igmp_parameters *p){
01087   igmp_dbg("IGMP: ignore and discard frame\n");
01088   pico_frame_discard(p->f);
01089   return 0;
01090 }
01091 
01092 /* finite state machine table */
01093 const callback host_membership_diagram_table[3][6] =
01094 { /* event                    |Delete Group  |Create Group |Update Group |Query Received  |Report Received  |Timer Expired */
01095 /* state Non-Member      */ { discard,       srsfst,       srsfst,       discard,         discard,          discard },
01096 /* state Delaying Member */ { stslifs,       mrsrrt,       mrsrrt,       rtimrtct,        stcl,             srsf    },
01097 /* state Idle Member     */ { slifs,         srst,         srst,         st,              discard,          discard }
01098 };
01099 
01100 static int pico_igmp_process_event(struct igmp_parameters *p)
01101 {
01102   struct pico_tree_node *index = NULL;
01103   struct igmp_parameters *_p = NULL;
01104 
01105   igmp_dbg("IGMP: process event on group address %08X\n", p->mcast_group.addr);
01106   if (p->event == IGMP_EVENT_QUERY_RECV && p->mcast_group.addr == 0) { /* general query */
01107     pico_tree_foreach(index, &IGMPParameters) {
01108       _p = index->keyValue;
01109       _p->max_resp_time = p->max_resp_time;
01110       _p->event = IGMP_EVENT_QUERY_RECV;
01111       igmp_dbg("IGMP: for each mcast_group = %08X | state = %u\n", _p->mcast_group.addr, _p->state);
01112       host_membership_diagram_table[_p->state][_p->event](_p);
01113     }
01114   } else {
01115     igmp_dbg("IGMP: state = %u (0: non-member - 1: delaying member - 2: idle member)\n", p->state); 
01116     host_membership_diagram_table[p->state][p->event](p);
01117   }
01118   return 0;
01119 }
01120