File content as of revision 2:540f6e142d59:

/*********************************************************************
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;
}