Diff: modules/pico_igmp.c

Revision:

2:540f6e142d59

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_igmp.c	Sat Aug 03 13:16:14 2013 +0000
@@ -0,0 +1,1120 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+RFC 1112, 2236, 3376, 3569, 3678, 4607
+
+Authors: Kristof Roelants (IGMPv3), Simon Maes, Brecht Van Cauwenberghe 
+*********************************************************************/
+
+#include "pico_stack.h"
+#include "pico_ipv4.h"
+#include "pico_igmp.h"
+#include "pico_config.h"
+#include "pico_eth.h"
+#include "pico_addressing.h"
+#include "pico_frame.h"
+#include "pico_tree.h"
+#include "pico_device.h"
+#include "pico_socket.h"
+
+#define igmp_dbg(...) do{}while(0)
+//#define igmp_dbg dbg
+
+/* membership states */
+#define IGMP_STATE_NON_MEMBER             (0x0)
+#define IGMP_STATE_DELAYING_MEMBER        (0x1)
+#define IGMP_STATE_IDLE_MEMBER            (0x2)
+
+/* events */ 
+#define IGMP_EVENT_DELETE_GROUP           (0x0)
+#define IGMP_EVENT_CREATE_GROUP           (0x1)
+#define IGMP_EVENT_UPDATE_GROUP           (0x2)
+#define IGMP_EVENT_QUERY_RECV             (0x3)
+#define IGMP_EVENT_REPORT_RECV            (0x4)
+#define IGMP_EVENT_TIMER_EXPIRED          (0x5)
+
+/* message types */
+#define IGMP_TYPE_MEM_QUERY               (0x11)
+#define IGMP_TYPE_MEM_REPORT_V1           (0x12)
+#define IGMP_TYPE_MEM_REPORT_V2           (0x16)
+#define IGMP_TYPE_LEAVE_GROUP             (0x17)
+#define IGMP_TYPE_MEM_REPORT_V3           (0x22)
+
+/* group record types */
+#define IGMP_MODE_IS_INCLUDE              (1)
+#define IGMP_MODE_IS_EXCLUDE              (2)
+#define IGMP_CHANGE_TO_INCLUDE_MODE       (3)
+#define IGMP_CHANGE_TO_EXCLUDE_MODE       (4)
+#define IGMP_ALLOW_NEW_SOURCES            (5)
+#define IGMP_BLOCK_OLD_SOURCES            (6)
+
+/* host flag */
+#define IGMP_HOST_LAST                    (0x1)
+#define IGMP_HOST_NOT_LAST                (0x0)
+
+/* list of timers, counters and their default values */
+#define IGMP_ROBUSTNESS                   (2)
+#define IGMP_QUERY_INTERVAL               (125) /* secs */
+#define IGMP_QUERY_RESPONSE_INTERVAL      (10) /* secs */
+#define IGMP_STARTUP_QUERY_INTERVAL       (IGMPV3_QUERY_INTERVAL / 4)
+#define IGMP_STARTUP_QUERY_COUNT          (IGMPV3_ROBUSTNESS)
+#define IGMP_LAST_MEMBER_QUERY_INTERVAL   (1) /* secs */
+#define IGMP_LAST_MEMBER_QUERY_COUNT      (IGMPV3_ROBUSTNESS)
+#define IGMP_UNSOLICITED_REPORT_INTERVAL  (1) /* secs */
+#define IGMP_DEFAULT_MAX_RESPONSE_TIME    (100)
+
+/* custom timers types */
+#define IGMP_TIMER_GROUP_REPORT           (1)
+#define IGMP_TIMER_V1_QUERIER             (2)
+#define IGMP_TIMER_V2_QUERIER             (3)
+
+/* IGMP groups */
+#define IGMP_ALL_HOST_GROUP               long_be(0xE0000001) /* 224.0.0.1 */
+#define IGMP_ALL_ROUTER_GROUP             long_be(0xE0000002) /* 224.0.0.2 */
+#define IGMPV3_ALL_ROUTER_GROUP           long_be(0xE0000016) /* 224.0.0.22 */
+
+/* misc */
+#define IGMP_TIMER_STOPPED                (1)
+#define IP_OPTION_ROUTER_ALERT_LEN        (4)
+#define IGMP_MAX_GROUPS                   (32) /* max 255 */
+
+struct __attribute__((packed)) igmp_message {
+  uint8_t type;
+  uint8_t max_resp_time;
+  uint16_t crc;
+  uint32_t mcast_group;
+};
+
+struct __attribute__((packed)) igmpv3_query {
+  uint8_t type;
+  uint8_t max_resp_time;
+  uint16_t crc;
+  uint32_t mcast_group;
+  uint8_t rsq;
+  uint8_t qqic;
+  uint16_t sources;
+  uint32_t source_addr[0];
+};
+
+struct __attribute__((packed)) igmpv3_group_record {
+  uint8_t type;
+  uint8_t aux;
+  uint16_t sources;
+  uint32_t mcast_group;
+  uint32_t source_addr[0];
+};
+
+struct __attribute__((packed)) igmpv3_report {
+  uint8_t type;
+  uint8_t res0;
+  uint16_t crc;
+  uint16_t res1;
+  uint16_t groups;
+  struct igmpv3_group_record record[0];
+};
+
+struct igmp_parameters {
+  uint8_t event;
+  uint8_t state;
+  uint8_t last_host;
+  uint8_t filter_mode;
+  uint8_t max_resp_time;
+  struct pico_ip4 mcast_link;
+  struct pico_ip4 mcast_group;
+  struct pico_tree *MCASTFilter;
+  struct pico_frame *f;
+};
+
+struct igmp_timer {
+  uint8_t type;
+  uint8_t stopped;
+  unsigned long start;
+  unsigned long delay;
+  struct pico_ip4 mcast_link;
+  struct pico_ip4 mcast_group;
+  struct pico_frame *f;
+  void (*callback)(struct igmp_timer *t);
+};
+
+/* queues */
+static struct pico_queue igmp_in = {};
+static struct pico_queue igmp_out = {};
+
+/* finite state machine caller */
+static int pico_igmp_process_event(struct igmp_parameters *p);
+
+/* state callback prototype */
+typedef int (*callback)(struct igmp_parameters *);
+
+/* redblack trees */
+static int igmp_timer_cmp(void *ka, void *kb)
+{
+  struct igmp_timer *a = ka, *b =kb;
+  if (a->type < b->type)
+    return -1;
+  if (a->type > b->type)
+    return 1;
+  if (a->mcast_group.addr < b->mcast_group.addr)
+    return -1;
+  if (a->mcast_group.addr > b->mcast_group.addr)
+    return 1;
+  if (a->mcast_link.addr < b->mcast_link.addr)
+    return -1;
+  if (a->mcast_link.addr > b->mcast_link.addr)
+    return 1;
+  return 0;
+}
+PICO_TREE_DECLARE(IGMPTimers, igmp_timer_cmp);
+
+static int igmp_parameters_cmp(void *ka, void *kb)
+{
+  struct igmp_parameters *a = ka, *b = kb;
+  if (a->mcast_group.addr < b->mcast_group.addr)
+    return -1;
+  if (a->mcast_group.addr > b->mcast_group.addr)
+    return 1;
+  if (a->mcast_link.addr < b->mcast_link.addr)
+    return -1;
+  if (a->mcast_link.addr > b->mcast_link.addr)
+    return 1;
+  return 0;
+}
+PICO_TREE_DECLARE(IGMPParameters, igmp_parameters_cmp);
+
+static int igmp_sources_cmp(void *ka, void *kb)
+{
+  struct pico_ip4 *a = ka, *b = kb;
+  if (a->addr < b->addr)
+    return -1;
+  if (a->addr > b->addr)
+    return 1;
+  return 0;
+}
+PICO_TREE_DECLARE(IGMPAllow, igmp_sources_cmp);
+PICO_TREE_DECLARE(IGMPBlock, igmp_sources_cmp);
+
+static struct igmp_parameters *pico_igmp_find_parameter(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group)
+{
+  struct igmp_parameters test = {0};
+  test.mcast_link.addr = mcast_link->addr;
+  test.mcast_group.addr = mcast_group->addr;
+  return pico_tree_findKey(&IGMPParameters, &test);
+}
+
+static int pico_igmp_delete_parameter(struct igmp_parameters *p)
+{
+  if (pico_tree_delete(&IGMPParameters, p))
+    pico_free(p);
+  else
+    return -1;
+
+  return 0;
+}
+
+static void pico_igmp_timer_expired(unsigned long now, void *arg)
+{
+  struct igmp_timer *t = NULL, *timer = NULL, test = {0};
+
+  t = (struct igmp_timer *)arg;
+  test.type = t->type;
+  test.mcast_link = t->mcast_link;
+  test.mcast_group = t->mcast_group;
+  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);
+  timer = pico_tree_findKey(&IGMPTimers, &test);
+  if (!timer) {
+    return;
+  }
+  if (timer->stopped == IGMP_TIMER_STOPPED) {
+    pico_free(t);
+    return;
+  }
+  if (timer->start + timer->delay < PICO_TIME_MS()) {
+    pico_tree_delete(&IGMPTimers, timer);
+    if (timer->callback)
+      timer->callback(timer);
+    pico_free(timer);
+  } else {
+    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());
+    pico_timer_add((timer->start + timer->delay) - PICO_TIME_MS(), &pico_igmp_timer_expired, timer);
+  }
+  return;
+}
+
+static int pico_igmp_timer_reset(struct igmp_timer *t)
+{
+  struct igmp_timer *timer = NULL, test = {0};
+
+  igmp_dbg("IGMP: reset timer for %08X, delay %lu\n", t->mcast_group.addr, t->delay);
+  test.type = t->type;
+  test.mcast_link = t->mcast_link;
+  test.mcast_group = t->mcast_group;
+  timer = pico_tree_findKey(&IGMPTimers, &test);
+  if (!timer)
+    return -1;
+
+  *timer = *t;
+  timer->start = PICO_TIME_MS();
+  return 0;
+}
+
+static int pico_igmp_timer_start(struct igmp_timer *t)
+{
+  struct igmp_timer *timer = NULL, test = {0};
+
+  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);
+  test.type = t->type;
+  test.mcast_link = t->mcast_link;
+  test.mcast_group = t->mcast_group;
+  timer = pico_tree_findKey(&IGMPTimers, &test);
+  if (timer)
+    return pico_igmp_timer_reset(t);
+
+  timer = pico_zalloc(sizeof(struct igmp_timer));
+  if (!timer) {
+    pico_err = PICO_ERR_ENOMEM;
+    return -1;
+  }
+  *timer = *t;
+  timer->start = PICO_TIME_MS();
+
+  pico_tree_insert(&IGMPTimers, timer);
+  pico_timer_add(timer->delay, &pico_igmp_timer_expired, timer);
+  return 0;
+}
+
+static int pico_igmp_timer_stop(struct igmp_timer *t)
+{
+  struct igmp_timer *timer = NULL, test = {0};
+
+  test.type = t->type;
+  test.mcast_link = t->mcast_link;
+  test.mcast_group = t->mcast_group;
+  timer = pico_tree_findKey(&IGMPTimers, &test);
+  if (!timer)
+    return 0;
+
+  igmp_dbg("IGMP: stop timer for %08X, delay %lu\n", timer->mcast_group.addr, timer->delay);
+  timer->stopped = IGMP_TIMER_STOPPED;
+  return 0;
+}
+
+static int pico_igmp_timer_is_running(struct igmp_timer *t)
+{
+  struct igmp_timer *timer = NULL, test = {0};
+
+  test.type = t->type;
+  test.mcast_link = t->mcast_link;
+  test.mcast_group = t->mcast_group;
+  timer = pico_tree_findKey(&IGMPTimers, &test);
+  if (timer)
+    return 1;
+  return 0;
+}
+
+static struct igmp_timer *pico_igmp_find_timer(uint8_t type, struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group)
+{
+  struct igmp_timer test = {0};
+
+  test.type = type;
+  test.mcast_link = *mcast_link;
+  test.mcast_group = *mcast_group;
+  return pico_tree_findKey(&IGMPTimers, &test);
+}
+
+static void pico_igmp_report_expired(struct igmp_timer *t)
+{
+  struct igmp_parameters *p = NULL;
+
+  p = pico_igmp_find_parameter(&t->mcast_link, &t->mcast_group);
+  if (!p)
+    return;
+
+  p->event = IGMP_EVENT_TIMER_EXPIRED;
+  pico_igmp_process_event(p);
+}
+
+static void pico_igmp_v2querier_expired(struct igmp_timer *t)
+{
+  struct pico_ipv4_link *link = NULL;
+  struct pico_tree_node *index = NULL, *_tmp = NULL;
+
+  link = pico_ipv4_link_by_dev(t->f->dev);
+  if (!link)
+    return;
+
+  /* When changing compatibility mode, cancel all pending response 
+   * and retransmission timers.
+   */
+  pico_tree_foreach_safe(index, &IGMPTimers, _tmp) 
+  {
+    ((struct igmp_timer *)index->keyValue)->stopped = IGMP_TIMER_STOPPED;
+    pico_tree_delete(&IGMPTimers, index->keyValue);
+  }
+  igmp_dbg("IGMP: switch to compatibility mode IGMPv3\n");
+  link->mcast_compatibility = PICO_IGMPV3;
+  return;
+}
+
+static int pico_igmp_is_checksum_valid(struct pico_frame *f)
+{
+  struct pico_ipv4_hdr *hdr = NULL;
+  uint8_t ihl = 24, datalen = 0;
+
+  hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+  ihl = (hdr->vhl & 0x0F) * 4; /* IHL is in 32bit words */
+  datalen = short_be(hdr->len) - ihl;
+
+  if (short_be(pico_checksum(f->transport_hdr, datalen)) == 0)
+    return 1;
+  igmp_dbg("IGMP: invalid checksum\n");
+  return 0;
+}
+
+/* RFC 3376 $7.1 */
+static int pico_igmp_compatibility_mode(struct pico_frame *f)
+{
+  struct pico_ipv4_hdr *hdr = NULL;
+  struct pico_ipv4_link *link = NULL;
+  struct pico_tree_node *index = NULL, *_tmp = NULL;
+  struct igmp_timer t = {0};
+  uint8_t ihl = 24, datalen = 0;
+
+  link = pico_ipv4_link_by_dev(f->dev);
+  if (!link)
+    return -1;
+
+  hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+  ihl = (hdr->vhl & 0x0F) * 4; /* IHL is in 32bit words */
+  datalen = short_be(hdr->len) - ihl;
+  igmp_dbg("IGMP: IHL = %u, LEN = %u, OCTETS = %u\n", ihl, short_be(hdr->len), datalen);
+
+  if (datalen > 12) {
+    /* IGMPv3 query */
+    t.type = IGMP_TIMER_V2_QUERIER;
+    if (pico_igmp_timer_is_running(&t)) { /* IGMPv2 querier present timer still running */
+      return -1;
+    } else {
+      link->mcast_compatibility = PICO_IGMPV3;
+      return 0;
+    }
+  } else if (datalen == 8) {
+    struct igmp_message *query = (struct igmp_message *)f->transport_hdr;
+    if (query->max_resp_time != 0) {
+      /* IGMPv2 query */
+      /* When changing compatibility mode, cancel all pending response 
+       * and retransmission timers.
+       */
+      pico_tree_foreach_safe(index, &IGMPTimers, _tmp) 
+      {
+        ((struct igmp_timer *)index->keyValue)->stopped = IGMP_TIMER_STOPPED;
+        pico_tree_delete(&IGMPTimers, index->keyValue);
+      }
+      igmp_dbg("IGMP: switch to compatibility mode IGMPv2\n");
+      link->mcast_compatibility = PICO_IGMPV2;
+      t.type = IGMP_TIMER_V2_QUERIER;
+      t.delay = ((IGMP_ROBUSTNESS * link->mcast_last_query_interval) + IGMP_QUERY_RESPONSE_INTERVAL) * 1000;
+      t.f = f;
+      t.callback = pico_igmp_v2querier_expired;
+      /* only one of this type of timer may exist! */
+      pico_igmp_timer_start(&t);
+    } else {
+      /* IGMPv1 query, not supported */
+      return -1;
+    }
+  } else {
+    /* invalid query, silently ignored */
+    return -1;
+  }
+  return 0;
+}
+
+static struct igmp_parameters *pico_igmp_analyse_packet(struct pico_frame *f)
+{
+  struct igmp_message *message = NULL;
+  struct igmp_parameters *p = NULL;
+  struct pico_ipv4_link *link = NULL;
+  struct pico_ip4 mcast_group = {0};
+
+  link = pico_ipv4_link_by_dev(f->dev);
+  if (!link)
+    return NULL;
+
+  /* IGMPv2 and IGMPv3 have a similar structure for the first 8 bytes */ 
+  message = (struct igmp_message *)f->transport_hdr;
+  mcast_group.addr = message->mcast_group;
+  p = pico_igmp_find_parameter(&link->address, &mcast_group);
+  if (!p && mcast_group.addr == 0) { /* general query */
+    p = pico_zalloc(sizeof(struct igmp_parameters));
+    if (!p)
+      return NULL;
+    p->state = IGMP_STATE_NON_MEMBER;
+    p->mcast_link.addr = link->address.addr;
+    p->mcast_group.addr = mcast_group.addr;
+    pico_tree_insert(&IGMPParameters, p);
+  } else if (!p) {
+    return NULL;
+  }
+
+  switch (message->type) {
+    case IGMP_TYPE_MEM_QUERY:
+       p->event = IGMP_EVENT_QUERY_RECV;
+       break;
+    case IGMP_TYPE_MEM_REPORT_V1:
+       p->event = IGMP_EVENT_REPORT_RECV;
+       break;
+    case IGMP_TYPE_MEM_REPORT_V2:
+       p->event = IGMP_EVENT_REPORT_RECV;
+       break;
+    case IGMP_TYPE_MEM_REPORT_V3:
+       p->event = IGMP_EVENT_REPORT_RECV;
+       break;
+    default:
+       return NULL;
+  }
+  p->max_resp_time = message->max_resp_time; /* if IGMPv3 report this will be 0 (res0 field) */
+  p->f = f;
+
+  return p;
+}
+
+static int pico_igmp_process_in(struct pico_protocol *self, struct pico_frame *f)
+{
+  struct igmp_parameters *p = NULL;
+ 
+  if (!pico_igmp_is_checksum_valid(f))
+    goto out;
+  if (pico_igmp_compatibility_mode(f) < 0)
+    goto out;
+  p = pico_igmp_analyse_packet(f);
+  if (!p)
+    goto out;
+
+  return pico_igmp_process_event(p);
+
+  out:
+    pico_frame_discard(f);
+    return 0;
+}
+
+static int pico_igmp_process_out(struct pico_protocol *self, struct pico_frame *f) {
+  /* packets are directly transferred to the IP layer by calling pico_ipv4_frame_push */
+  return 0;
+}
+
+/* Interface: protocol definition */
+struct pico_protocol pico_proto_igmp = {
+  .name = "igmp",
+  .proto_number = PICO_PROTO_IGMP,
+  .layer = PICO_LAYER_TRANSPORT,
+  .process_in = pico_igmp_process_in,
+  .process_out = pico_igmp_process_out,
+  .q_in = &igmp_in,
+  .q_out = &igmp_out,
+};
+
+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) 
+{
+  struct igmp_parameters *p = NULL;
+  
+  if (mcast_group->addr == IGMP_ALL_HOST_GROUP)
+    return 0;
+
+  p = pico_igmp_find_parameter(mcast_link, mcast_group);
+  if (!p && state == PICO_IGMP_STATE_CREATE) {
+    p = pico_zalloc(sizeof(struct igmp_parameters));
+    if (!p) {
+      pico_err = PICO_ERR_ENOMEM;
+      return -1;
+    }
+    p->state = IGMP_STATE_NON_MEMBER;
+    p->mcast_link = *mcast_link;
+    p->mcast_group = *mcast_group;
+    pico_tree_insert(&IGMPParameters, p);
+  } else if (!p) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+
+  switch (state) {
+    case PICO_IGMP_STATE_CREATE:
+      p->event = IGMP_EVENT_CREATE_GROUP;
+      break;
+
+    case PICO_IGMP_STATE_UPDATE:
+      p->event = IGMP_EVENT_UPDATE_GROUP;
+      break;
+    
+    case PICO_IGMP_STATE_DELETE:
+      p->event = IGMP_EVENT_DELETE_GROUP;
+      break;
+
+    default:
+      return -1;
+  }
+  p->filter_mode = filter_mode;
+  p->MCASTFilter = MCASTFilter;
+
+  return pico_igmp_process_event(p);
+}
+
+static int pico_igmp_send_report(struct igmp_parameters *p, struct pico_frame *f)
+{
+  struct pico_ip4 dst = {0};
+  struct pico_ip4 mcast_group = {0};
+  struct pico_ipv4_link *link = NULL;
+  
+  link = pico_ipv4_link_get(&p->mcast_link);
+  if (!link)
+    return -1;
+
+  mcast_group.addr = p->mcast_group.addr;
+  switch (link->mcast_compatibility) {
+    case PICO_IGMPV2:
+      if (p->event == IGMP_EVENT_DELETE_GROUP)
+        dst.addr = IGMP_ALL_ROUTER_GROUP;
+      else
+        dst.addr = mcast_group.addr;
+      break;
+
+    case PICO_IGMPV3:
+      dst.addr = IGMPV3_ALL_ROUTER_GROUP;
+      break;
+
+    default:
+      pico_err = PICO_ERR_EPROTONOSUPPORT;
+      return -1;
+  }
+
+  igmp_dbg("IGMP: send membership report on group %08X to %08X\n", mcast_group.addr, dst.addr);
+  pico_ipv4_frame_push(f, &dst, PICO_PROTO_IGMP);
+  return 0;
+}
+
+static int pico_igmp_generate_report(struct igmp_parameters *p)
+{
+  struct pico_ipv4_link *link = NULL;
+  int i = 0;
+
+  link = pico_ipv4_link_get(&p->mcast_link);
+  if (!link) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+
+  switch (link->mcast_compatibility) {
+    case PICO_IGMPV1:
+      pico_err = PICO_ERR_EPROTONOSUPPORT;
+      return -1;
+      
+    case PICO_IGMPV2:
+    {
+      struct igmp_message *report = NULL;
+      uint8_t report_type = IGMP_TYPE_MEM_REPORT_V2;
+      if (p->event == IGMP_EVENT_DELETE_GROUP)
+        report_type = IGMP_TYPE_LEAVE_GROUP;
+
+      p->f = pico_proto_ipv4.alloc(&pico_proto_ipv4, IP_OPTION_ROUTER_ALERT_LEN + sizeof(struct igmp_message));
+      p->f->net_len += IP_OPTION_ROUTER_ALERT_LEN;
+      p->f->transport_hdr += IP_OPTION_ROUTER_ALERT_LEN;
+      p->f->transport_len -= IP_OPTION_ROUTER_ALERT_LEN;
+      p->f->dev = pico_ipv4_link_find(&p->mcast_link);
+      /* p->f->len is correctly set by alloc */
+
+      report = (struct igmp_message *)p->f->transport_hdr;
+      report->type = report_type;
+      report->max_resp_time = IGMP_DEFAULT_MAX_RESPONSE_TIME;
+      report->mcast_group = p->mcast_group.addr;
+
+      report->crc = 0;
+      report->crc = short_be(pico_checksum(report, sizeof(struct igmp_message)));
+      break;
+    }
+    case PICO_IGMPV3:
+    {
+      struct igmpv3_report *report = NULL;
+      struct igmpv3_group_record *record = NULL;
+      struct pico_mcast_group *g = NULL, test = {0};
+      struct pico_tree_node *index = NULL, *_tmp = NULL;
+      struct pico_tree *IGMPFilter = NULL;
+      struct pico_ip4 *source = NULL;
+      uint8_t record_type = 0;
+      uint8_t sources = 0;
+      int len = 0;
+
+      test.mcast_addr = p->mcast_group;
+      g = pico_tree_findKey(link->MCASTGroups, &test);
+      if (!g) {
+        pico_err = PICO_ERR_EINVAL;
+        return -1;
+      }
+
+      if (p->event == IGMP_EVENT_DELETE_GROUP) { /* "non-existent" state of filter mode INCLUDE and empty source list */
+        p->filter_mode = PICO_IP_MULTICAST_INCLUDE;
+        p->MCASTFilter = NULL;
+      }
+
+      /* cleanup filters */
+      pico_tree_foreach_safe(index, &IGMPAllow, _tmp) 
+      {
+        pico_tree_delete(&IGMPAllow, index->keyValue);
+      }
+      pico_tree_foreach_safe(index, &IGMPBlock, _tmp) 
+      {
+        pico_tree_delete(&IGMPBlock, index->keyValue);
+      }
+
+      switch (g->filter_mode) {
+
+        case PICO_IP_MULTICAST_INCLUDE:
+          switch (p->filter_mode) {
+            case PICO_IP_MULTICAST_INCLUDE:
+              if (p->event == IGMP_EVENT_DELETE_GROUP) { /* all ADD_SOURCE_MEMBERSHIP had an equivalent DROP_SOURCE_MEMBERSHIP */
+                /* TO_IN (B) */
+                record_type = IGMP_CHANGE_TO_INCLUDE_MODE;
+                IGMPFilter = &IGMPAllow;
+                if (p->MCASTFilter) {
+                  pico_tree_foreach(index, p->MCASTFilter) /* B */
+                  {
+                    pico_tree_insert(&IGMPAllow, index->keyValue);
+                    sources++;
+                  }
+                } /* else { IGMPAllow stays empty } */
+                break;
+              }
+
+              /* ALLOW (B-A) */
+              /* if event is CREATE A will be empty, thus only ALLOW (B-A) has sense */
+              if (p->event == IGMP_EVENT_CREATE_GROUP) /* first ADD_SOURCE_MEMBERSHIP */
+                record_type = IGMP_CHANGE_TO_INCLUDE_MODE;
+              else
+                record_type = IGMP_ALLOW_NEW_SOURCES;
+              IGMPFilter = &IGMPAllow;
+              pico_tree_foreach(index, p->MCASTFilter) /* B */
+              {
+                pico_tree_insert(&IGMPAllow, index->keyValue);
+                sources++;
+              }
+              pico_tree_foreach(index, &g->MCASTSources) /* A */
+              {
+                source = pico_tree_findKey(&IGMPAllow, index->keyValue);
+                if (source) {
+                  pico_tree_delete(&IGMPAllow, source);
+                  sources--;
+                }
+              }
+              if (!pico_tree_empty(&IGMPAllow)) /* record type is ALLOW */
+                break;
+
+              /* BLOCK (A-B) */
+              record_type = IGMP_BLOCK_OLD_SOURCES;
+              IGMPFilter = &IGMPBlock;
+              pico_tree_foreach(index, &g->MCASTSources) /* A */
+              {
+                pico_tree_insert(&IGMPBlock, index->keyValue);
+                sources++;
+              }
+              pico_tree_foreach(index, p->MCASTFilter) /* B */
+              {
+                source = pico_tree_findKey(&IGMPBlock, index->keyValue);
+                if (source) {
+                  pico_tree_delete(&IGMPBlock, source);
+                  sources--;
+                }
+              }
+              if (!pico_tree_empty(&IGMPBlock)) /* record type is BLOCK */
+                break;
+
+              /* ALLOW (B-A) and BLOCK (A-B) are empty: do not send report (RFC 3376 $5.1) */
+              p->f = NULL;
+              return 0;
+
+            case PICO_IP_MULTICAST_EXCLUDE:
+              /* TO_EX (B) */
+              record_type = IGMP_CHANGE_TO_EXCLUDE_MODE;
+              IGMPFilter = &IGMPBlock;
+              pico_tree_foreach(index, p->MCASTFilter) /* B */
+              {
+                pico_tree_insert(&IGMPBlock, index->keyValue);
+                sources++;
+              }
+              break;
+
+            default:
+              pico_err = PICO_ERR_EINVAL;
+              return -1;
+          }
+          break;
+
+        case PICO_IP_MULTICAST_EXCLUDE:
+          switch (p->filter_mode) {
+            case PICO_IP_MULTICAST_INCLUDE:
+              /* TO_IN (B) */
+              record_type = IGMP_CHANGE_TO_INCLUDE_MODE;
+              IGMPFilter = &IGMPAllow;
+              if (p->MCASTFilter) {
+                pico_tree_foreach(index, p->MCASTFilter) /* B */
+                {
+                  pico_tree_insert(&IGMPAllow, index->keyValue);
+                  sources++;
+                }
+              } /* else { IGMPAllow stays empty } */
+              break;
+
+            case PICO_IP_MULTICAST_EXCLUDE:
+              /* BLOCK (B-A) */
+              record_type = IGMP_BLOCK_OLD_SOURCES;
+              IGMPFilter = &IGMPBlock;
+              pico_tree_foreach(index, p->MCASTFilter)
+              {
+                pico_tree_insert(&IGMPBlock, index->keyValue);
+                sources++;
+              }
+              pico_tree_foreach(index, &g->MCASTSources) /* A */
+              {
+                source = pico_tree_findKey(&IGMPBlock, index->keyValue); /* B */
+                if (source) {
+                  pico_tree_delete(&IGMPBlock, source);
+                  sources--;
+                }
+              }
+              if (!pico_tree_empty(&IGMPBlock)) /* record type is BLOCK */
+                break;
+
+              /* ALLOW (A-B) */
+              record_type = IGMP_ALLOW_NEW_SOURCES;
+              IGMPFilter = &IGMPAllow;
+              pico_tree_foreach(index, &g->MCASTSources)
+              {
+                pico_tree_insert(&IGMPAllow, index->keyValue);
+                sources++;
+              }
+              pico_tree_foreach(index, p->MCASTFilter) /* B */
+              {
+                source = pico_tree_findKey(&IGMPAllow, index->keyValue); /* A */
+                if (source) {
+                  pico_tree_delete(&IGMPAllow, source);
+                  sources--;
+                }
+              }
+              if (!pico_tree_empty(&IGMPAllow)) /* record type is ALLOW */
+                break;
+
+              /* BLOCK (B-A) and ALLOW (A-B) are empty: do not send report (RFC 3376 $5.1) */
+              p->f = NULL;
+              return 0;
+
+            default:
+              pico_err = PICO_ERR_EINVAL;
+              return -1;
+          }
+          break;
+
+        default:
+          pico_err = PICO_ERR_EINVAL;
+          return -1;
+      }
+
+      len = sizeof(struct igmpv3_report) + sizeof(struct igmpv3_group_record) + (sources * sizeof(struct pico_ip4));
+      p->f = pico_proto_ipv4.alloc(&pico_proto_ipv4, IP_OPTION_ROUTER_ALERT_LEN + len);
+      p->f->net_len += IP_OPTION_ROUTER_ALERT_LEN;
+      p->f->transport_hdr += IP_OPTION_ROUTER_ALERT_LEN;
+      p->f->transport_len -= IP_OPTION_ROUTER_ALERT_LEN;
+      p->f->dev = pico_ipv4_link_find(&p->mcast_link);
+      /* p->f->len is correctly set by alloc */
+
+      report = (struct igmpv3_report *)p->f->transport_hdr;
+      report->type = IGMP_TYPE_MEM_REPORT_V3;
+      report->res0 = 0;
+      report->crc = 0;
+      report->res1 = 0;
+      report->groups = short_be(1);
+
+      record = &report->record[0];
+      record->type = record_type;
+      record->aux = 0;
+      record->sources = short_be(sources);
+      record->mcast_group = p->mcast_group.addr;
+      if (!pico_tree_empty(IGMPFilter)) {
+        i = 0;
+        pico_tree_foreach(index, IGMPFilter)
+        {
+          record->source_addr[i] = ((struct pico_ip4 *)index->keyValue)->addr;
+          i++;
+        }
+      }
+      report->crc = short_be(pico_checksum(report, len));
+      break;
+    }
+
+    default:
+      pico_err = PICO_ERR_EINVAL;
+      return -1;
+  }
+  return 0;
+}
+
+/* stop timer, send leave if flag set */
+static int stslifs(struct igmp_parameters *p)
+{
+  struct igmp_timer t = {0};
+
+  igmp_dbg("IGMP: event = leave group | action = stop timer, send leave if flag set\n");
+
+  t.type = IGMP_TIMER_GROUP_REPORT;
+  t.mcast_link = p->mcast_link;
+  t.mcast_group = p->mcast_group;
+  if (pico_igmp_timer_stop(&t) < 0)
+    return -1;
+
+  /* always send leave, even if not last host */
+  if (pico_igmp_send_report(p, p->f) < 0)
+    return -1;
+
+  pico_igmp_delete_parameter(p);
+  igmp_dbg("IGMP: new state = non-member\n");
+  return 0;
+}
+
+/* send report, set flag, start timer */
+static int srsfst(struct igmp_parameters *p)
+{
+  struct igmp_timer t = {0};
+  struct pico_frame *copy_frame = NULL;
+
+  igmp_dbg("IGMP: event = join group | action = send report, set flag, start timer\n");
+
+  p->last_host = IGMP_HOST_LAST;
+
+  if (pico_igmp_generate_report(p) < 0)
+    return -1;
+  if (!p->f)
+    return 0;
+  copy_frame = pico_frame_copy(p->f);
+  if (!copy_frame) {
+    pico_err = PICO_ERR_ENOMEM;
+    return -1;
+  }
+  if (pico_igmp_send_report(p, copy_frame) < 0)
+    return -1;
+
+  t.type = IGMP_TIMER_GROUP_REPORT;
+  t.mcast_link = p->mcast_link;
+  t.mcast_group = p->mcast_group;
+  t.delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000)); 
+  t.f = p->f;
+  t.callback = pico_igmp_report_expired;
+  pico_igmp_timer_start(&t);
+
+  p->state = IGMP_STATE_DELAYING_MEMBER;
+  igmp_dbg("IGMP: new state = delaying member\n");
+  return 0;
+}
+
+/* merge report, send report, reset timer (IGMPv3 only) */
+static int mrsrrt(struct igmp_parameters *p)
+{
+  struct igmp_timer *t = NULL;
+  struct pico_frame *copy_frame = NULL;
+  struct pico_ipv4_link *link = NULL;
+
+  igmp_dbg("IGMP: event = update group | action = merge report, send report, reset timer (IGMPv3 only)\n");
+
+  link = pico_ipv4_link_get(&p->mcast_link);
+  if (!link)
+    return -1;
+
+  if (link->mcast_compatibility != PICO_IGMPV3) {
+    igmp_dbg("IGMP: no IGMPv3 compatible router on network\n");
+    return -1;
+  }
+
+  /* XXX: merge with pending report rfc 3376 $5.1 */
+
+  copy_frame = pico_frame_copy(p->f);
+  if (!copy_frame)
+    return -1;
+  if (pico_igmp_send_report(p, copy_frame) < 0)
+    return -1;
+
+  t = pico_igmp_find_timer(IGMP_TIMER_GROUP_REPORT, &p->mcast_link, &p->mcast_group);
+  if (!t)
+    return -1;
+  t->delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000)); 
+  pico_igmp_timer_reset(t);
+
+  p->state = IGMP_STATE_DELAYING_MEMBER;
+  igmp_dbg("IGMP: new state = delaying member\n");
+  return 0;
+}
+
+/* send report, start timer (IGMPv3 only) */
+static int srst(struct igmp_parameters *p)
+{
+  struct igmp_timer t = {0};
+  struct pico_frame *copy_frame = NULL;
+  struct pico_ipv4_link *link = NULL;
+
+  igmp_dbg("IGMP: event = update group | action = send report, start timer (IGMPv3 only)\n");
+
+  link = pico_ipv4_link_get(&p->mcast_link);
+  if (!link)
+    return -1;
+
+  if (link->mcast_compatibility != PICO_IGMPV3) {
+    igmp_dbg("IGMP: no IGMPv3 compatible router on network\n");
+    return -1;
+  }
+
+  if (pico_igmp_generate_report(p) < 0)
+    return -1;
+  if (!p->f)
+    return 0;
+  copy_frame = pico_frame_copy(p->f);
+  if (!copy_frame)
+    return -1;
+  if (pico_igmp_send_report(p, copy_frame) < 0)
+    return -1;
+
+  t.type = IGMP_TIMER_GROUP_REPORT;
+  t.mcast_link = p->mcast_link;
+  t.mcast_group = p->mcast_group;
+  t.delay = (pico_rand() % (IGMP_UNSOLICITED_REPORT_INTERVAL * 10000)); 
+  t.f = p->f;
+  t.callback = pico_igmp_report_expired;
+  pico_igmp_timer_start(&t);
+
+  p->state = IGMP_STATE_DELAYING_MEMBER;
+  igmp_dbg("IGMP: new state = delaying member\n");
+  return 0;
+}
+
+/* send leave if flag set */
+static int slifs(struct igmp_parameters *p)
+{
+  igmp_dbg("IGMP: event = leave group | action = send leave if flag set\n");
+
+  /* always send leave, even if not last host */
+  if (pico_igmp_send_report(p, p->f) < 0)
+    return -1;
+
+  pico_igmp_delete_parameter(p);
+  igmp_dbg("IGMP: new state = non-member\n");
+  return 0;
+}
+
+/* start timer */
+static int st(struct igmp_parameters *p)
+{
+  struct igmp_timer t = {0};
+
+  igmp_dbg("IGMP: event = query received | action = start timer\n");
+
+  if (pico_igmp_generate_report(p) < 0)
+    return -1;
+  if (!p->f)
+    return -1;
+
+  t.type = IGMP_TIMER_GROUP_REPORT;
+  t.mcast_link = p->mcast_link;
+  t.mcast_group = p->mcast_group;
+  t.delay = (pico_rand() % (p->max_resp_time * 100)); 
+  t.f = p->f;
+  t.callback = pico_igmp_report_expired;
+  pico_igmp_timer_start(&t);
+
+  p->state = IGMP_STATE_DELAYING_MEMBER;
+  igmp_dbg("IGMP: new state = delaying member\n");
+  return 0;
+}
+
+/* stop timer, clear flag */
+static int stcl(struct igmp_parameters *p)
+{
+  struct igmp_timer t = {0};
+
+  igmp_dbg("IGMP: event = report received | action = stop timer, clear flag\n");
+
+  t.type = IGMP_TIMER_GROUP_REPORT;
+  t.mcast_link = p->mcast_link;
+  t.mcast_group = p->mcast_group;
+  if (pico_igmp_timer_stop(&t) < 0)
+    return -1;
+
+  p->last_host = IGMP_HOST_NOT_LAST;
+  p->state = IGMP_STATE_IDLE_MEMBER;
+  igmp_dbg("IGMP: new state = idle member\n");
+  return 0;
+}
+
+/* send report, set flag */
+static int srsf(struct igmp_parameters *p)
+{
+  igmp_dbg("IGMP: event = timer expired | action = send report, set flag\n");
+
+  if (pico_igmp_send_report(p, p->f) < 0)
+    return -1;
+
+  p->state = IGMP_STATE_IDLE_MEMBER;
+  igmp_dbg("IGMP: new state = idle member\n"); 
+  return 0;
+}
+
+/* reset timer if max response time < current timer */
+static int rtimrtct(struct igmp_parameters *p)
+{
+  struct igmp_timer *t = NULL;
+  unsigned long time_to_run = 0;
+
+  igmp_dbg("IGMP: event = query received | action = reset timer if max response time < current timer\n");
+
+  t = pico_igmp_find_timer(IGMP_TIMER_GROUP_REPORT, &p->mcast_link, &p->mcast_group);
+  if (!t)
+    return -1;
+
+  time_to_run = t->start + t->delay - PICO_TIME_MS();
+  if ((p->max_resp_time * 100) < time_to_run) { /* max_resp_time in units of 1/10 seconds */
+    t->delay = pico_rand() % (p->max_resp_time * 100); 
+    pico_igmp_timer_reset(t);
+  }
+
+  p->state = IGMP_STATE_DELAYING_MEMBER;
+  igmp_dbg("IGMP: new state = delaying member\n"); 
+  return 0;
+}
+
+static int discard(struct igmp_parameters *p){
+  igmp_dbg("IGMP: ignore and discard frame\n");
+  pico_frame_discard(p->f);
+  return 0;
+}
+
+/* finite state machine table */
+const callback host_membership_diagram_table[3][6] =
+{ /* event                    |Delete Group  |Create Group |Update Group |Query Received  |Report Received  |Timer Expired */
+/* state Non-Member      */ { discard,       srsfst,       srsfst,       discard,         discard,          discard },
+/* state Delaying Member */ { stslifs,       mrsrrt,       mrsrrt,       rtimrtct,        stcl,             srsf    },
+/* state Idle Member     */ { slifs,         srst,         srst,         st,              discard,          discard }
+};
+
+static int pico_igmp_process_event(struct igmp_parameters *p)
+{
+  struct pico_tree_node *index = NULL;
+  struct igmp_parameters *_p = NULL;
+
+  igmp_dbg("IGMP: process event on group address %08X\n", p->mcast_group.addr);
+  if (p->event == IGMP_EVENT_QUERY_RECV && p->mcast_group.addr == 0) { /* general query */
+    pico_tree_foreach(index, &IGMPParameters) {
+      _p = index->keyValue;
+      _p->max_resp_time = p->max_resp_time;
+      _p->event = IGMP_EVENT_QUERY_RECV;
+      igmp_dbg("IGMP: for each mcast_group = %08X | state = %u\n", _p->mcast_group.addr, _p->state);
+      host_membership_diagram_table[_p->state][_p->event](_p);
+    }
+  } else {
+    igmp_dbg("IGMP: state = %u (0: non-member - 1: delaying member - 2: idle member)\n", p->state); 
+    host_membership_diagram_table[p->state][p->event](p);
+  }
+  return 0;
+}
+