Revision 74:c146c4e346c4, committed 2013-10-01

Comitter:

tass

Date:

Tue Oct 01 06:13:42 2013 +0000

Parent:

73:dfb737147f6e

Child:

75:a9abf10138d6

Commit message:

Complete Issue #17

Changed in this revision

--- a/include/pico_arp.h	Fri Sep 27 12:43:36 2013 +0000
+++ b/include/pico_arp.h	Tue Oct 01 06:13:42 2013 +0000
@@ -22,4 +22,5 @@
 struct pico_eth *pico_arp_lookup(struct pico_ip4 *dst);
 struct pico_ip4 *pico_arp_reverse_lookup(struct pico_eth *dst);
 int pico_arp_create_entry(uint8_t* hwaddr, struct pico_ip4 ipv4, struct pico_device* dev);
+int pico_arp_get_neighbors(struct pico_device *dev, struct pico_ip4 *neighbors, int maxlen);
 #endif

--- a/include/pico_constants.h	Fri Sep 27 12:43:36 2013 +0000
+++ b/include/pico_constants.h	Tue Oct 01 06:13:42 2013 +0000
@@ -8,7 +8,7 @@
 /* Included from pico_config.h */
 /** Endian-dependant constants **/
 
-extern volatile unsigned long pico_tick;
+extern volatile uint32_t pico_tick;
 
 #ifdef PICO_BIGENDIAN
 
@@ -124,10 +124,10 @@
 
 static inline uint32_t pico_hash(const char *name)
 {
-  unsigned long hash = 5381;
-  int8_t c;
-  while ((c = *name++))
-    hash = ((hash << 5) + hash) + (unsigned long)c; /* hash * 33 + c */
+  uint32_t hash = 5381;
+  uint8_t c;
+  while ((c = (uint8_t)*name++))
+    hash = ((hash << 5) + hash) + c; /* hash * 33 + c */
   return hash;
 }
 

--- a/modules/pico_dhcp_client.c	Fri Sep 27 12:43:36 2013 +0000
+++ b/modules/pico_dhcp_client.c	Tue Oct 01 06:13:42 2013 +0000
@@ -781,7 +781,7 @@
 
 static int16_t pico_dhcp_client_opt_parse(void *ptr, uint16_t len)
 {
-  uint32_t optlen = len - sizeof(struct pico_dhcp_hdr);
+  uint32_t optlen = len - (uint32_t)sizeof(struct pico_dhcp_hdr);
   struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)ptr;
   struct pico_dhcp_opt *opt = NULL;
 

--- a/modules/pico_dns_client.c	Fri Sep 27 12:43:36 2013 +0000
+++ b/modules/pico_dns_client.c	Tue Oct 01 06:13:42 2013 +0000
@@ -505,15 +505,19 @@
   uint16_t * paramID = pico_zalloc(sizeof(uint16_t));
   dns_dbg("DNS: sending query to %08X\n", q->q_ns.ns.addr);
   if (!q->s)
-    return -1;
+    goto failure;
   if (pico_socket_connect(q->s, &q->q_ns.ns, short_be(PICO_DNS_NS_PORT)) < 0)
-    return -1;
+    goto failure;
 
   pico_socket_send(q->s, q->query, q->len);
   *paramID = q->id;
   pico_timer_add(PICO_DNS_CLIENT_RETRANS, pico_dns_client_retransmission, paramID);
 
   return 0;
+
+failure:
+  pico_free(paramID);
+  return -1;
 }
 
 static void pico_dns_client_retransmission(uint32_t now, void *arg)

--- a/modules/pico_icmp4.c	Fri Sep 27 12:43:36 2013 +0000
+++ b/modules/pico_icmp4.c	Tue Oct 01 06:13:42 2013 +0000
@@ -225,9 +225,9 @@
 {
   pico_icmp4_send_echo(cookie);
   cookie->timestamp = pico_tick;
-  pico_timer_add((long unsigned int)cookie->timeout, ping_timeout, cookie);
+  pico_timer_add((uint32_t)cookie->timeout, ping_timeout, cookie);
   if (cookie->seq < cookie->count)
-    pico_timer_add((long unsigned int)cookie->interval, next_ping, cookie);
+    pico_timer_add((uint32_t)cookie->interval, next_ping, cookie);
 }
 
 static void next_ping(uint32_t now, void *arg)

--- a/modules/pico_ipv4.c	Fri Sep 27 12:43:36 2013 +0000
+++ b/modules/pico_ipv4.c	Tue Oct 01 06:13:42 2013 +0000
@@ -241,6 +241,18 @@
 #endif /* PICO_SUPPORT_IPFRAG */
 
 #ifdef PICO_SUPPORT_IPFRAG
+
+static inline struct pico_ipv4_fragmented_packet *fragment_find_by_hdr(struct pico_ipv4_hdr *hdr)
+{
+  struct pico_ipv4_fragmented_packet frag = {0};
+  frag.id = short_be(hdr->id);
+  frag.proto = hdr->proto;
+  frag.src.addr = long_be(hdr->src.addr);
+  frag.dst.addr = long_be(hdr->dst.addr);
+  return pico_tree_findKey(&pico_ipv4_fragmented_tree, &frag);
+}
+
+
 static inline int8_t pico_ipv4_fragmented_check(struct pico_protocol *self, struct pico_frame **f)
 {
   uint8_t *running_pointer = NULL;
@@ -250,7 +262,7 @@
   struct pico_ipv4_hdr *f_frag_hdr = NULL, *hdr = (struct pico_ipv4_hdr *) (*f)->net_hdr;
   struct pico_udp_hdr *udp_hdr = NULL;
   struct pico_tcp_hdr *tcp_hdr = NULL;
-  struct pico_ipv4_fragmented_packet *pfrag = NULL, frag; 
+  struct pico_ipv4_fragmented_packet *pfrag = NULL; 
   struct pico_frame *f_new = NULL, *f_frag = NULL;
   struct pico_tree_node *index, *_tmp;
 
@@ -291,11 +303,7 @@
     }
     else {
       reassembly_dbg("REASSEMBLY: intermediate element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
-      frag.id = short_be(hdr->id);
-      frag.proto = hdr->proto;
-      frag.src.addr = long_be(hdr->src.addr);
-      frag.dst.addr = long_be(hdr->dst.addr);
-      pfrag = pico_tree_findKey(&pico_ipv4_fragmented_tree, &frag);
+      pfrag = fragment_find_by_hdr(hdr);
       if (pfrag) {
         pfrag->total_len = (uint16_t)(pfrag->total_len + (short_be(hdr->len) - (*f)->net_len));
         pico_tree_insert(pfrag->t, *f);
@@ -308,11 +316,7 @@
     }
   } else if (offset) {
     reassembly_dbg("REASSEMBLY: last element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
-    frag.id = short_be(hdr->id);
-    frag.proto = hdr->proto;
-    frag.src.addr = long_be(hdr->src.addr);
-    frag.dst.addr = long_be(hdr->dst.addr);
-    pfrag = pico_tree_findKey(&pico_ipv4_fragmented_tree, &frag);
+    pfrag = fragment_find_by_hdr(hdr);
     if (pfrag) {
       pfrag->total_len = (uint16_t)(pfrag->total_len + (short_be(hdr->len) - (*f)->net_len));
       reassembly_dbg("REASSEMBLY: fragmented packet in tree, reassemble packet of %u data bytes\n", pfrag->total_len);
@@ -1317,6 +1321,27 @@
   return NULL;
 }
 
+struct pico_ipv4_link *pico_ipv4_link_by_dev_next(struct pico_device *dev, struct pico_ipv4_link *last)
+{
+  struct pico_tree_node *index = NULL;
+  struct pico_ipv4_link *link = NULL;
+  int valid = 0;
+
+  if (last == NULL)
+    valid = 1;
+
+  pico_tree_foreach(index, &Tree_dev_link) 
+  {
+    link = index->keyValue;
+    if (link->dev == dev) {
+      if (last == link)
+        valid = 1;
+      else if (valid > 0)
+        return link;
+    }
+  }
+  return NULL;
+}
 
 struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address)
 {

--- a/modules/pico_ipv4.h	Fri Sep 27 12:43:36 2013 +0000
+++ b/modules/pico_ipv4.h	Tue Oct 01 06:13:42 2013 +0000
@@ -13,7 +13,7 @@
 
 #define PICO_IPV4_INADDR_ANY 0x00000000U
 
-#define PICO_SIZE_IP4HDR ((sizeof(struct pico_ipv4_hdr)))
+#define PICO_SIZE_IP4HDR (uint32_t)((sizeof(struct pico_ipv4_hdr)))
 #define PICO_IPV4_DONTFRAG 0x4000
 #define PICO_IPV4_MOREFRAG 0x2000
 #define PICO_IPV4_FRAG_MASK 0x1FFF
@@ -82,6 +82,7 @@
 int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto);
 struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address);
 struct pico_ipv4_link *pico_ipv4_link_by_dev(struct pico_device *dev);
+struct pico_ipv4_link *pico_ipv4_link_by_dev_next(struct pico_device *dev, struct pico_ipv4_link *last);
 struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address);
 struct pico_ip4 *pico_ipv4_source_find(const struct pico_ip4 *dst);
 int pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link);

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_olsr.c	Tue Oct 01 06:13:42 2013 +0000
@@ -0,0 +1,869 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+
+#include "pico_stack.h"
+#include "pico_config.h"
+#include "pico_device.h"
+#include "pico_ipv4.h"
+#include "pico_arp.h"
+#include "pico_socket.h"
+#ifdef PICO_SUPPORT_OLSR
+
+
+#define OLSR_MSG_INTERVAL 2000
+#define DGRAM_MAX_SIZE 1800
+static const struct pico_ip4 HOST_NETMASK = { 0xFFFFFFFF };
+#ifndef MIN
+# define MIN(a,b) (a<b?a:b)
+#endif
+
+#define fresher(a,b) ((a>b) || ((b - a) > 32768))
+
+/* Objects */
+struct olsr_route_entry
+{
+	struct olsr_route_entry         *next;
+	uint32_t                        time_left;
+	struct pico_ip4			            destination;
+	struct olsr_route_entry         *gateway;
+	struct pico_device              *iface;
+	uint16_t			                  metric;
+	uint8_t				                  link_type;
+	struct olsr_route_entry         *children;
+	uint16_t                        ansn;
+	uint8_t                         lq, nlq;
+	uint8_t                         *advertised_tc;
+};
+
+struct olsr_dev_entry
+{
+  struct olsr_dev_entry *next;
+  struct pico_device *dev;
+};
+
+
+/* OLSR Protocol */
+#define OLSRMSG_HELLO 	0xc9
+#define OLSRMSG_MID		0x03
+#define OLSRMSG_TC		0xca
+
+#define OLSRLINK_SYMMETRIC 0x06
+#define OLSRLINK_UNKNOWN 0x08
+#define OLSRLINK_MPR	0x0a
+
+#define OLSR_MAX_DEVICES 8
+
+#define OLSR_PORT (short_be((uint16_t)698))
+
+struct __attribute__((packed)) olsr_link
+{
+	uint8_t link_code;
+	uint8_t reserved;
+	uint16_t link_msg_size;
+};
+
+struct __attribute__((packed)) olsr_neighbor
+{
+	uint32_t addr;
+	uint8_t  lq;
+	uint8_t  nlq;
+	uint16_t reserved;
+};
+
+struct __attribute__((packed)) olsr_hmsg_hello
+{
+	uint16_t reserved;
+	uint8_t htime;
+	uint8_t willingness;
+};
+
+struct __attribute__((packed)) olsr_hmsg_tc
+{
+	uint16_t ansn;
+	uint16_t reserved;
+};
+
+
+struct __attribute__((packed)) olsrmsg
+{
+	uint8_t type;
+	uint8_t vtime;
+	uint16_t size;
+	struct pico_ip4 orig;
+	uint8_t ttl;
+	uint8_t hop;
+	uint16_t seq;
+};
+
+struct __attribute__((packed)) olsrhdr
+{
+	uint16_t len;
+	uint16_t seq;
+};
+
+
+struct olsr_setup {
+	int n_ifaces;
+	struct pico_device *ifaces[OLSR_MAX_DEVICES];
+};
+
+
+
+/* Globals */
+static struct pico_socket *udpsock = NULL;
+uint16_t my_ansn = 0;
+uint16_t fresh_ansn = 0;
+static struct olsr_route_entry  *Local_interfaces = NULL;
+static struct olsr_dev_entry    *Local_devices    = NULL;
+
+struct olsr_route_entry *olsr_get_ethentry(struct pico_device *vif)
+{
+  struct olsr_route_entry *cur = Local_interfaces;
+  while(cur) {
+    if (cur->iface == vif)
+      return cur;
+    cur = cur->next;
+  }
+  return NULL;
+}
+
+
+static struct olsr_route_entry *get_next_hop(struct olsr_route_entry *dst)
+{
+	struct olsr_route_entry *hop = dst;
+	while(hop) {
+		if(hop->metric <= 1)
+			return hop;
+		hop = hop->gateway;
+	}
+	return NULL;
+}
+
+static inline void olsr_route_add(struct olsr_route_entry *el)
+{
+	struct olsr_route_entry *nexthop;
+
+	if (fresher(fresh_ansn, my_ansn))
+		my_ansn = (uint16_t)(fresh_ansn + 1u);
+	else
+		my_ansn++;
+
+	if (el->gateway) {
+		/* 2-hops route or more */
+		el->next = el->gateway->children;
+		el->gateway->children = el;
+		nexthop = get_next_hop(el);
+    dbg("[OLSR] Adding route\n");
+    pico_ipv4_route_add(el->destination, HOST_NETMASK, nexthop->destination, el->metric, NULL);
+		el->link_type = OLSRLINK_MPR;
+	} else if (el->iface) {
+		/* neighbor */
+		struct olsr_route_entry *ei = olsr_get_ethentry(el->iface);
+		el->link_type = OLSRLINK_SYMMETRIC;
+		if (ei) {
+			el->next = ei->children;
+			ei->children = el;
+		}
+	}
+}
+
+static inline void olsr_route_del(struct olsr_route_entry *r)
+{
+	struct olsr_route_entry *cur, *prev = NULL, *lst;
+	if (fresher(fresh_ansn, my_ansn))
+		my_ansn = (uint16_t)(fresh_ansn + 1u);
+	if (r->gateway) {
+		lst = r->gateway->children;
+	} else if (r->iface) {
+		lst = olsr_get_ethentry(r->iface);
+	} else {
+		lst = Local_interfaces;
+	}
+	cur = lst, prev = NULL;
+	while(cur) {
+		if (cur == r) {
+			/* found */
+			if (r->gateway) {
+				pico_ipv4_route_del(r->destination, HOST_NETMASK, r->metric);
+				if (!prev)
+					r->gateway->children = r->next;
+				else
+					prev->next = r->next;
+			}
+
+			while (r->children) {
+				olsr_route_del(r->children);
+				/* Orphans must die. */
+				pico_free(r->children);
+			}
+			return;
+		}
+		prev = cur;
+		cur = cur->next;
+	}
+}
+
+static struct olsr_route_entry *get_route_by_address(struct olsr_route_entry *lst, uint32_t ip)
+{
+	struct olsr_route_entry *found;
+	if(lst) {
+		if (lst->destination.addr == ip) {
+			return lst;
+		}
+		found = get_route_by_address(lst->children, ip);
+		if (found)
+			return found;
+		found = get_route_by_address(lst->next, ip);
+		if (found)
+			return found;
+	}
+	return NULL;
+}
+
+#define OLSR_C_SHIFT (uint32_t)4 /* 1/16 */
+#define DEFAULT_VTIME 288UL
+
+uint8_t seconds2olsr(uint32_t seconds)
+{
+    uint8_t a, b;
+
+    /* find largest b such as seconds/C >= 2^b */
+    for (b = 0; b <= 0x0f; b++) {
+        if (seconds * 16 < (1u << b)){
+            b--;
+            break;
+        }
+    }
+    /* compute the expression 16*(T/(C*(2^b))-1), which may not be a
+       integer, and round it up.  This results in the value for 'a' */
+    a = (uint8_t)((uint32_t)(seconds / ((1u << b) >> OLSR_C_SHIFT) - 1u) << 4u);
+    //    a = 16 * (seconds / (OSLR_C * (1 << b)) - 1);
+
+    /* if 'a' is equal to 16: increment 'b' by one, and set 'a' to 0 */
+    if (16u == a) {
+        b++;
+        a = 0u;
+    }
+    return (uint8_t)((a << 4u) + b);
+}
+
+uint32_t olsr2seconds(uint8_t olsr)
+{
+    uint8_t a, b;
+    a = olsr >> 4;
+    b = olsr & 0x0f;
+    return (uint8_t)(( (1u << b) + (uint32_t)((a << b) >> 4u) ) >> OLSR_C_SHIFT);
+}
+
+
+static void refresh_neighbors(struct pico_device *iface)
+{
+	struct pico_ip4 neighbors[256];
+	int i;
+	struct olsr_route_entry *found = NULL, *ancestor = NULL;
+	int n_vec_size = pico_arp_get_neighbors(iface, neighbors, 256);
+
+	ancestor = olsr_get_ethentry(iface);
+	if (!ancestor)
+		return;
+
+	for (i = 0; i < n_vec_size; i++) {
+		found = get_route_by_address(Local_interfaces, neighbors[i].addr);
+		if (found) {
+			if (found->metric > 1) { /* Reposition among neighbors */
+				olsr_route_del(found);
+				found->gateway = olsr_get_ethentry(iface);
+				found->iface = iface;
+				found->metric = 1;
+				found->lq = 0xFF;
+				found->nlq = 0xFF;
+				olsr_route_add(found);
+			}
+			found->link_type = OLSRLINK_SYMMETRIC;
+			found->time_left = (OLSR_MSG_INTERVAL << 2);
+		} else {
+			struct olsr_route_entry *e = pico_zalloc(sizeof (struct olsr_route_entry));
+			if (!e) {
+				dbg("olsr: adding local route entry");
+				return;
+			}
+			e->destination.addr = neighbors[i].addr;
+			e->link_type = OLSRLINK_SYMMETRIC;
+			e->time_left = (OLSR_MSG_INTERVAL << 2);
+			e->gateway = olsr_get_ethentry(iface);
+			e->iface = iface;
+			e->metric = 1;
+			e->lq = 0xFF;
+			e->nlq = 0xFF;
+			olsr_route_add(e);
+		}
+	}
+}
+
+static void olsr_garbage_collector(struct olsr_route_entry *sublist)
+{
+	if(!sublist)
+		return;
+	if ((sublist->time_left--) <= 0) {
+		olsr_route_del(sublist);
+		pico_free(sublist);
+		return;
+	}
+	olsr_garbage_collector(sublist->children);
+	olsr_garbage_collector(sublist->next);
+}
+
+
+static void refresh_routes(void)
+{
+	struct olsr_route_entry *local, *neighbor = NULL;
+  struct olsr_dev_entry *icur = Local_devices;
+
+	/* Refresh local entries */
+
+	/* Step 1: set zero expire time for local addresses and neighbors*/
+	local = Local_interfaces;
+	while(local) {
+		local->time_left = 0;
+		neighbor = local->children;
+		while (neighbor && (neighbor->metric < 2)) {
+			//dbg("Setting to zero. Neigh: %08x metric %d\n", neighbor->destination, neighbor->metric);
+			neighbor->time_left = 0;
+			neighbor = neighbor->next;
+		}
+		local = local->next;
+	}
+
+	/* Step 2: refresh timer for entries that are still valid. 
+	 * Add new entries.
+	 */
+  while(icur) {
+    struct pico_ipv4_link *lnk = NULL;
+    do { 
+      lnk = pico_ipv4_link_by_dev_next(icur->dev, lnk);
+      if (!lnk) break;
+  		local = olsr_get_ethentry(icur->dev);
+  		if (local) {
+  			local->time_left = (OLSR_MSG_INTERVAL << 2);
+  		} else if (lnk) {
+  			struct olsr_route_entry *e = pico_zalloc(sizeof (struct olsr_route_entry));
+  			if (!e) {
+  				dbg("olsr: adding local route entry");
+  				return;
+  			}
+  			e->destination.addr = lnk->address.addr; /* Always pick the first address */
+  			e->time_left = (OLSR_MSG_INTERVAL << 2);
+  			e->iface = icur->dev;
+  			e->metric = 0;
+  			e->lq = 0xFF;
+  			e->nlq = 0xFF;
+  			e->next = Local_interfaces;
+  			Local_interfaces = e;
+  		}
+    } while (lnk);
+
+		refresh_neighbors(icur->dev);
+    icur = icur->next;
+	}
+}
+
+static uint32_t olsr_build_hello_neighbors(uint8_t *buf, uint32_t size)
+{
+	uint32_t ret = 0;
+	struct olsr_route_entry *local, *neighbor;
+	struct olsr_neighbor *dst = (struct olsr_neighbor *) buf;
+	local = Local_interfaces;
+	while (local) {
+		neighbor = local->children;
+		while (neighbor) {
+			struct olsr_link *li = (struct olsr_link *) (buf + ret);
+			li->link_code = neighbor->link_type;
+			li->reserved = 0;
+			li->link_msg_size = short_be(sizeof(struct olsr_neighbor) + sizeof(struct olsr_link));
+			ret += (uint32_t)sizeof(struct olsr_link);
+			dst = (struct olsr_neighbor *) (buf+ret);
+			dst->addr = neighbor->destination.addr;
+			dst->nlq = neighbor->nlq;
+			dst->lq = neighbor->lq;
+			dst->reserved = 0;
+			ret += (uint32_t)sizeof(struct olsr_neighbor);
+			if (ret >= size)
+				return (uint32_t)((uint32_t)(ret - sizeof(struct olsr_neighbor)) - sizeof(struct olsr_link));
+			neighbor = neighbor->next;
+		}
+		local = local->next;
+	}
+	return ret;
+}
+
+static uint32_t olsr_build_tc_neighbors(uint8_t *buf, uint32_t size)
+{
+	uint32_t ret = 0;
+	struct olsr_route_entry *local, *neighbor;
+	struct olsr_neighbor *dst = (struct olsr_neighbor *) buf;
+	local = Local_interfaces;
+	while (local) {
+		neighbor = local->children;
+		while (neighbor) {
+			dst->addr = neighbor->destination.addr;
+			dst->nlq = neighbor->nlq;
+			dst->lq = neighbor->lq;
+			dst->reserved = 0;
+			ret += (uint32_t)sizeof(struct olsr_neighbor);
+			dst = (struct olsr_neighbor *) (buf + ret);
+			if (ret >= size)
+				return (uint32_t)(ret - sizeof(struct olsr_neighbor));
+			neighbor = neighbor->next;
+		}
+		local = local->next;
+	}
+	return ret;
+}
+
+static uint32_t olsr_build_mid(uint8_t *buf, uint32_t size, struct pico_device *excluded)
+{
+	uint32_t ret = 0;
+	struct olsr_route_entry *local;
+	struct pico_ip4 *dst = (struct pico_ip4 *) buf;
+	local = Local_interfaces;
+	while (local) {
+		if (local->iface != excluded) {
+			dst->addr = local->destination.addr;
+			ret += (uint32_t)sizeof(uint32_t);
+			dst = (struct pico_ip4 *) (buf + ret);
+			if (ret >= size)
+				return (uint32_t)(ret - sizeof(uint32_t));
+		}
+		local = local->next;
+	}
+	return ret;
+}
+
+static uint16_t pkt_counter = 0;
+static void olsr_make_dgram(struct pico_device *pdev, int full)
+{
+	uint8_t dgram[DGRAM_MAX_SIZE];
+	uint32_t size = 0, r;
+	struct pico_ipv4_link *ep;
+	struct olsrhdr *ohdr;
+	struct pico_ip4 bcast;
+	struct olsrmsg *msg_hello, *msg_mid, *msg_tc;
+	struct olsr_hmsg_hello *hello;
+	struct olsr_hmsg_tc *tc;
+	static uint8_t hello_counter = 0, mid_counter = 0, tc_counter = 0;
+
+	ohdr = (struct olsrhdr *)dgram;
+	size += (uint32_t)sizeof(struct olsrhdr);
+  ep = pico_ipv4_link_by_dev(pdev);
+	if (!ep)
+		return;
+  bcast.addr = (ep->netmask.addr & ep->address.addr) | (~ep->netmask.addr);
+
+	/* HELLO Message */
+
+	msg_hello = (struct olsrmsg *) (dgram + size);
+	size += (uint32_t)sizeof(struct olsrmsg);
+	msg_hello->type = OLSRMSG_HELLO;
+	msg_hello->vtime = seconds2olsr(DEFAULT_VTIME);
+	msg_hello->orig.addr = ep->address.addr;
+	msg_hello->ttl = 1;
+	msg_hello->hop = 0;
+	msg_hello->seq = short_be(hello_counter++);
+	hello = (struct olsr_hmsg_hello *)(dgram + size);
+	size += (uint32_t)sizeof(struct olsr_hmsg_hello);
+	hello->reserved = 0;
+	hello->htime = 0x05; /* Todo: find and define values */
+	hello->willingness = 0x07;
+	r = olsr_build_hello_neighbors(dgram + size, DGRAM_MAX_SIZE - size);
+	if (r == 0) {
+		dbg("Building hello message");
+		return;
+	}
+	size += r;
+	msg_hello->size = short_be((uint16_t)(sizeof(struct olsrmsg) + sizeof(struct olsr_hmsg_hello) + r));
+
+  if (full) {
+
+  	/* MID Message */
+  
+  	msg_mid = (struct olsrmsg *)(dgram + size);
+  	size += (uint32_t)sizeof(struct olsrmsg);
+  	msg_mid->type = OLSRMSG_MID;
+  	msg_mid->vtime = seconds2olsr(60);
+  	msg_mid->orig.addr = ep->address.addr;
+  	msg_mid->ttl = 0xFF;
+  	msg_mid->hop = 0;
+  	msg_mid->seq = short_be(mid_counter++);
+  	r = olsr_build_mid(dgram + size, DGRAM_MAX_SIZE - size, pdev);
+  	if (r == 0) {
+  		dbg("Building mid message");
+  		return;
+  	}
+  	if (r == 0) {
+  		size -= (uint32_t)sizeof(struct olsrmsg);
+  	} else {
+  		size += r;
+  		msg_mid->size = short_be((uint16_t)(sizeof(struct olsrmsg) + r));
+  	}
+  
+  	msg_tc = (struct olsrmsg *) (dgram + size);
+  	size += (uint32_t)sizeof(struct olsrmsg);
+  	msg_tc->type = OLSRMSG_TC;
+  	msg_tc->vtime = seconds2olsr(DEFAULT_VTIME); 
+  	msg_tc->orig.addr = ep->address.addr;
+  	msg_tc->ttl = 0xFF;
+  	msg_tc->hop = 0;
+  	msg_tc->seq = short_be(tc_counter++);
+  	tc = (struct olsr_hmsg_tc *)(dgram + size);
+  	size += (uint32_t)sizeof(struct olsr_hmsg_tc);
+  	tc->ansn = short_be(my_ansn);
+  	r = olsr_build_tc_neighbors(dgram + size, DGRAM_MAX_SIZE  - size);
+  	if (r == 0) {
+  		dbg("Building tc message");
+  		return;
+  	}
+  	size += r;
+  	msg_tc->size = short_be((uint16_t)(sizeof(struct olsrmsg) + sizeof(struct olsr_hmsg_tc) + r));
+  } /*if full */
+
+	/* Finalize olsr packet */
+	ohdr->len = short_be((uint16_t)size);
+	ohdr->seq = short_be((uint16_t)pkt_counter++);
+
+	/* Send the thing out */
+  if  (0 > pico_socket_sendto(udpsock, dgram, (int)size, &bcast, OLSR_PORT)) {
+	}
+  dbg("Sent %s packet\n", full!=0?"FULL":"HELLO");
+}
+
+static inline void arp_storm(struct pico_ip4 *addr)
+{
+  struct olsr_dev_entry *icur = Local_devices;
+  while(icur) {
+		pico_arp_query(icur->dev, addr);
+    icur = icur->next;
+	}
+}
+
+static void recv_mid(uint8_t *buffer, uint32_t len, struct olsr_route_entry *origin) 
+{
+	uint32_t parsed = 0;
+	uint32_t *address;
+	struct olsr_route_entry *e;
+
+	if (len % sizeof(uint32_t)) /*drop*/
+		return;
+
+	while (len > parsed) {
+		address = (uint32_t *)(buffer + parsed);
+		e = get_route_by_address(Local_interfaces, *address);
+		if (!e) {
+			e = pico_zalloc(sizeof(struct olsr_route_entry));
+			if (!e) {
+				dbg("olsr allocating route");
+				return;
+			}
+			e->time_left = (OLSR_MSG_INTERVAL << 2);
+			e->destination.addr = *address;
+			e->gateway = origin;
+			e->iface = origin->iface;
+			e->metric = (uint16_t)(origin->metric + 1u);
+			e->lq = origin->lq;
+			e->nlq = origin->nlq;
+			olsr_route_add(e);
+			arp_storm(&e->destination);
+		} else if (e->metric > (origin->metric + 1)) {
+			olsr_route_del(e);
+			e->metric = origin->metric;
+			e->gateway = origin;
+			olsr_route_add(e);
+		}
+		parsed += (uint32_t)sizeof(uint32_t);
+	}
+}
+
+static void recv_hello(uint8_t *buffer, uint32_t len, struct olsr_route_entry *origin)
+{
+	struct olsr_link *li;
+	struct olsr_route_entry *e;
+	uint32_t parsed = 0;
+	struct olsr_neighbor *neigh;
+
+	if (!origin)
+		return;
+
+	while (len > parsed) {
+		li = (struct olsr_link *) buffer;
+		neigh = (struct olsr_neighbor *)(buffer + parsed + sizeof(struct olsr_link));
+		parsed += short_be(li->link_msg_size);
+		e = get_route_by_address(Local_interfaces, neigh->addr);
+		if (!e) {
+			e = pico_zalloc(sizeof(struct olsr_route_entry));
+			if (!e) {
+				dbg("olsr allocating route");
+				return;
+			}
+			e->time_left = (OLSR_MSG_INTERVAL << 2);
+			e->destination.addr = neigh->addr;
+			e->gateway = origin;
+			e->iface = origin->iface;
+			e->metric = (uint16_t)(origin->metric + 1u);
+			e->link_type = OLSRLINK_UNKNOWN;
+			e->lq = MIN(origin->lq, neigh->lq);
+			e->nlq = MIN(origin->nlq, neigh->nlq);
+			olsr_route_add(e);
+			arp_storm(&e->destination);
+		} else if ((e->gateway != origin) && (e->metric > (origin->metric + 1))) {
+			olsr_route_del(e);
+			e->metric = (uint16_t)(origin->metric + 1u);
+			e->gateway = origin;
+			olsr_route_add(e);
+		}
+	}
+}
+
+static uint32_t reconsider_topology(uint8_t *buf, uint32_t size, struct olsr_route_entry *e)
+{
+	struct olsr_hmsg_tc *tc = (struct olsr_hmsg_tc *) buf;
+	uint16_t new_ansn = short_be(tc->ansn);
+	uint32_t parsed = sizeof(struct olsr_hmsg_tc);
+	struct olsr_route_entry *rt;
+	struct olsr_neighbor *n;
+
+	if (e->advertised_tc && fresher(new_ansn, e->ansn))
+	{
+		pico_free(e->advertised_tc);
+		e->advertised_tc = NULL;
+	}
+
+	if (fresher(new_ansn, fresh_ansn)) {
+		fresh_ansn = new_ansn;
+	}
+
+	if (!e->advertised_tc) {
+		e->advertised_tc = pico_zalloc(size);
+		if (!e) {
+			dbg("Allocating forward packet");
+			return 0;
+		}
+		memcpy(e->advertised_tc, buf, size);
+		e->ansn = new_ansn;
+		while (parsed < size) {
+			n = (struct olsr_neighbor *) (buf + parsed);
+			parsed += (uint32_t)sizeof(struct olsr_neighbor);
+			rt = get_route_by_address(Local_interfaces, n->addr);
+			if (rt && (rt->gateway == e)) {
+				/* Refresh existing node */
+				rt->time_left = e->time_left;
+			} else if (!rt || (rt->metric > (e->metric + 1)) || (rt->nlq < n->nlq)) {
+				if (!rt) {
+					rt = pico_zalloc(sizeof (struct olsr_route_entry));
+					rt->destination.addr = n->addr;
+				} else {
+					olsr_route_del(rt);
+				}
+				rt->link_type = OLSRLINK_UNKNOWN;
+				rt->iface = e->iface;
+				rt->gateway = e;
+				rt->metric = (uint16_t)(e->metric + 1);
+				rt->lq = n->lq;
+				rt->nlq = n->nlq;
+				rt->time_left = e->time_left;
+				olsr_route_add(rt);
+			}
+		}
+		return 1u;
+	} else {
+		return 0u;
+	}
+}
+
+static void olsr_recv(uint8_t *buffer, uint32_t len)
+{
+	struct olsrmsg *msg;
+	struct olsrhdr *outohdr, *oh = (struct olsrhdr *) buffer;
+	struct olsr_route_entry *ancestor;
+	uint32_t parsed = 0;
+	uint8_t outmsg[DGRAM_MAX_SIZE];
+	uint32_t outsize = 0;
+	if (len != short_be(oh->len)) {
+		return;
+	}
+	parsed += (uint32_t)sizeof(struct olsrhdr);
+
+	outohdr = (struct olsrhdr *)outmsg;
+	outsize += (uint32_t)sizeof(struct olsrhdr);
+
+	while (len > parsed) {
+		struct olsr_route_entry *origin;
+		msg = (struct olsrmsg *) (buffer + parsed);
+		origin = get_route_by_address(Local_interfaces, msg->orig.addr);
+		if (!origin) {
+			/* Discard this msg while it is not from known host */
+			arp_storm(&msg->orig);
+			parsed += short_be(msg->size);
+			continue;
+		}
+		/* We know this is a Master host and a neighbor */
+		origin->link_type = OLSRLINK_MPR;
+		origin->time_left = olsr2seconds(msg->vtime);
+		switch(msg->type) {
+			case OLSRMSG_HELLO:
+				ancestor = olsr_get_ethentry(origin->iface);
+				if ((origin->metric > 1) && ancestor) {
+					olsr_route_del(origin);
+					origin->gateway = ancestor;
+					origin->metric = 1;
+					olsr_route_add(origin);
+				}
+				recv_hello(buffer + (uint32_t)parsed + (uint32_t)sizeof(struct olsrmsg) + (uint32_t)sizeof(struct olsr_hmsg_hello),
+					(uint32_t) ((short_be(msg->size) - (sizeof(struct olsrmsg))) - (uint32_t)sizeof(struct olsr_hmsg_hello)),
+					origin);
+				msg->ttl = 0;
+				break;
+			case OLSRMSG_MID:
+				recv_mid(buffer + parsed + sizeof(struct olsrmsg), (uint32_t)(short_be(msg->size) - (sizeof(struct olsrmsg))), origin);
+				break;
+			case OLSRMSG_TC:
+				if (reconsider_topology(buffer + parsed + sizeof(struct olsrmsg), (uint32_t)(short_be(msg->size) - (sizeof(struct olsrmsg))), origin) < 1)
+					msg->ttl = 0;
+				else {
+					msg->hop = (uint8_t) origin->metric;
+				}
+				break;
+			default:
+				return;
+		}
+		if ((--msg->ttl) > 0) {
+			memcpy(outmsg + outsize, msg, short_be(msg->size));
+			outsize += short_be(msg->size);
+		}
+		parsed += short_be(msg->size);
+	}
+
+	if (outsize > sizeof(struct olsrhdr)) {
+		struct pico_ip4 bcast;
+		struct pico_ipv4_link *addr;
+		struct olsr_dev_entry *pdev = Local_devices;
+		/* Finalize FWD packet */
+		outohdr->len = short_be((uint16_t)outsize);
+		outohdr->seq = short_be((uint16_t)pkt_counter++);
+
+
+		/* Send the thing out */
+   while(pdev) { 
+      addr = pico_ipv4_link_by_dev(pdev->dev);
+			if (!addr)
+				continue;
+      bcast.addr = (addr->netmask.addr & addr->address.addr) | (~addr->netmask.addr);
+			if ( 0 > pico_socket_sendto(udpsock, outmsg, (int)outsize, &bcast, OLSR_PORT) ) {
+				dbg("olsr send");
+			}
+      pdev = pdev->next;
+		}
+	}
+}
+
+static void wakeup(uint16_t ev, struct pico_socket *s)
+{
+  unsigned char recvbuf[DGRAM_MAX_SIZE];
+  int r = 0;
+  struct pico_ip4 ANY = {0};
+  uint16_t port = OLSR_PORT;
+
+  if (ev & PICO_SOCK_EV_RD) {
+    r = pico_socket_recv(s, recvbuf, DGRAM_MAX_SIZE);
+    if (r > 0)
+      olsr_recv(recvbuf, (uint32_t)r);
+  }
+
+  if (ev == PICO_SOCK_EV_ERR) {
+    pico_socket_close(udpsock);
+    udpsock = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &wakeup);
+    if (udpsock)
+      pico_socket_bind(udpsock, &ANY, &port);
+  }
+}
+
+static void olsr_tick(uint32_t when, void *unused)
+{
+  struct olsr_dev_entry *d;
+  static int full = 0;
+  (void)when;
+  (void)unused;
+  olsr_garbage_collector(Local_interfaces);
+	refresh_routes();
+  d = Local_devices;
+  while(d) {
+		olsr_make_dgram(d->dev, full);
+    d = d->next;
+  }
+  if (full++ > 0)
+    full = 0;
+  pico_timer_add(1000, &olsr_tick, NULL);
+}
+
+
+/* Public interface */
+
+void pico_olsr_init(void)
+{
+  struct pico_ip4 ANY = {0};
+  uint16_t port = OLSR_PORT;
+  dbg("OLSR initialized.\n");
+  if (!udpsock) {
+    udpsock = pico_socket_open(PICO_PROTO_IPV4, PICO_PROTO_UDP, &wakeup);
+    if (udpsock)
+      pico_socket_bind(udpsock, &ANY, &port);
+  }
+  pico_timer_add(100, &olsr_tick, NULL);
+}
+
+int pico_olsr_add(struct pico_device *dev)
+{
+  struct pico_ipv4_link *lnk = NULL;
+  struct olsr_dev_entry *od; 
+  if (!dev) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+  dbg("OLSR: Adding device %s\n", dev->name);
+  od = pico_zalloc(sizeof(struct olsr_dev_entry));
+  if (!od) {
+     pico_err = PICO_ERR_ENOMEM;
+     return -1;
+  }
+  od->dev = dev;
+  od->next = Local_devices;
+  Local_devices = od;
+
+  do {
+    lnk = pico_ipv4_link_by_dev_next(dev, lnk);
+    if (lnk) {
+      struct olsr_route_entry *e = pico_zalloc(sizeof(struct olsr_route_entry));
+      dbg("OLSR: Found IP address %08x\n", long_be(lnk->address.addr));
+      if (!e) {
+        pico_err = PICO_ERR_ENOMEM;
+        return -1;
+      }
+      e->destination.addr = lnk->address.addr;
+      e->link_type = OLSRLINK_SYMMETRIC;
+      e->time_left = (OLSR_MSG_INTERVAL << 2);
+      e->gateway = NULL;
+      e->iface = dev;
+			e->metric = 0;
+			e->lq = 0xFF;
+			e->nlq = 0xFF;
+			e->next = Local_interfaces;
+			Local_interfaces = e;
+
+    }
+  } while(lnk);
+  return 0;
+}
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_olsr.h	Tue Oct 01 06:13:42 2013 +0000
@@ -0,0 +1,13 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Authors: Daniele Lacamera
+*********************************************************************/
+#ifndef __PICO_OLSR_H
+#define __PICO_OLSR_H
+
+
+void pico_olsr_init(void);
+int pico_olsr_add(struct pico_device *dev);
+#endif

--- a/modules/pico_tcp.c	Fri Sep 27 12:43:36 2013 +0000
+++ b/modules/pico_tcp.c	Tue Oct 01 06:13:42 2013 +0000
@@ -346,7 +346,7 @@
 
 static uint32_t pico_paws(void)
 {
-  static unsigned long _paws = 0;
+  static uint32_t _paws = 0;
   _paws = pico_rand();
   return long_be(_paws); /*XXX: implement paws */
 }
@@ -394,7 +394,7 @@
         sb = ts->sacks;
         ts->sacks = sb->next;
         memcpy(f->start + i, sb, 2 * sizeof(uint32_t));
-        i += (2 * sizeof(uint32_t));
+        i += (2 * (uint32_t)sizeof(uint32_t));
         f->start[len_off] = (uint8_t)(f->start[len_off] + (2 * sizeof(uint32_t)));
         pico_free(sb);
       }
@@ -582,7 +582,7 @@
         }
         t->mss_ok = 1;
         mss = short_from(opt + i);
-        i += sizeof(uint16_t);
+        i += (uint32_t)sizeof(uint16_t);
         if (t->mss > short_be(mss))
           t->mss = short_be(mss);
         break;
@@ -596,10 +596,10 @@
         }
         t->ts_ok = 1;
         tsval = long_from(opt + i);
-        i += sizeof(uint32_t);
+        i += (uint32_t)sizeof(uint32_t);
         tsecr = long_from(opt + i);
         f->timestamp = long_be(tsecr);
-        i += sizeof(uint32_t);
+        i += (uint32_t)sizeof(uint32_t);
         t->ts_nxt = long_be(tsval);
         break;
       }

--- a/modules/pico_tcp.h	Fri Sep 27 12:43:36 2013 +0000
+++ b/modules/pico_tcp.h	Tue Oct 01 06:13:42 2013 +0000
@@ -35,7 +35,7 @@
 
 #define PICO_TCPHDR_SIZE 20
 #define PICO_SIZE_TCPOPT_SYN 20
-#define PICO_SIZE_TCPHDR (sizeof(struct pico_tcp_hdr))
+#define PICO_SIZE_TCPHDR (uint32_t)(sizeof(struct pico_tcp_hdr))
 
 #define PICO_TCP_DEFAULT_MSS 1444
 

--- a/stack/pico_arp.c	Fri Sep 27 12:43:36 2013 +0000
+++ b/stack/pico_arp.c	Tue Oct 01 06:13:42 2013 +0000
@@ -318,3 +318,19 @@
   pico_frame_discard(q);
   return ret;
 }
+
+int pico_arp_get_neighbors(struct pico_device *dev, struct pico_ip4 *neighbors, int maxlen)
+{
+  struct pico_arp* search;
+  struct pico_tree_node * index;
+  int i = 0;
+  pico_tree_foreach(index,&arp_tree){
+    search = index->keyValue;
+    if (search->dev == dev) {
+      neighbors[i++].addr = search->ipv4.addr;
+      if (i >= maxlen)
+        return i;
+    }
+  }
+  return i;
+}

--- a/stack/pico_device.c	Fri Sep 27 12:43:36 2013 +0000
+++ b/stack/pico_device.c	Tue Oct 01 06:13:42 2013 +0000
@@ -29,7 +29,7 @@
 
 int pico_device_init(struct pico_device *dev, const char *name, uint8_t *mac)
 {
-	uint32_t len = strlen(name);
+	uint32_t len = (uint32_t)strlen(name);
 	if(len>MAX_DEVICE_NAME)
 		len = MAX_DEVICE_NAME;
   memcpy(dev->name, name, len);

--- a/stack/pico_socket.c	Fri Sep 27 12:43:36 2013 +0000
+++ b/stack/pico_socket.c	Tue Oct 01 06:13:42 2013 +0000
@@ -220,17 +220,23 @@
 /* gather sources to be filtered */
 PICO_TREE_DECLARE(MCASTFilter, mcast_filter_cmp);
 
+static struct pico_mcast_listen *listen_find(struct pico_socket *s, struct pico_ip4 *lnk, struct pico_ip4 *grp)
+{
+  struct pico_mcast_listen ltest = {0};
+  ltest.mcast_link.addr = lnk->addr;
+  ltest.mcast_group.addr = grp->addr;
+  return pico_tree_findKey(s->MCASTListen, &ltest);
+}
+
 /* MCASTFilter will be empty if no socket is listening on mcast_group on mcast_link anymore */
 static int pico_socket_aggregate_mcastfilters(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group)
 {
   uint8_t filter_mode = PICO_IP_MULTICAST_INCLUDE;
-  struct pico_mcast_listen *listen = NULL, ltest = {0};
+  struct pico_mcast_listen *listen = NULL;
   struct pico_ip4 *source = NULL;
   struct pico_socket *mcast_sock = NULL;
   struct pico_tree_node *index = NULL, *_tmp = NULL, *index2 = NULL, *_tmp2 = NULL;
 
-  ltest.mcast_link = *mcast_link;
-  ltest.mcast_group = *mcast_group;
 
   /* cleanup old filter */
   pico_tree_foreach_safe(index, &MCASTFilter, _tmp)
@@ -242,7 +248,7 @@
   pico_tree_foreach(index, &MCASTSockets)
   {
     mcast_sock = index->keyValue;
-    listen = pico_tree_findKey(mcast_sock->MCASTListen, &ltest);
+    listen = listen_find(mcast_sock, mcast_link, mcast_group);
     if (listen) {
       /* aggregate filter */
       switch(filter_mode)
@@ -328,7 +334,7 @@
 static int pico_socket_mcast_filter(struct pico_socket *s, struct pico_ip4 *mcast_group, struct pico_ip4 *src)
 {
   struct pico_ipv4_link *mcast_link = NULL;
-  struct pico_mcast_listen *listen = NULL, ltest = {0};
+  struct pico_mcast_listen *listen = NULL;
   struct pico_tree_node *index = NULL;
 
   /* no multicast enabled on socket */
@@ -338,10 +344,7 @@
   mcast_link = pico_ipv4_link_get(&s->local_addr.ip4);
   if (!mcast_link) 
     return -1;
-
-  ltest.mcast_link.addr = mcast_link->address.addr;
-  ltest.mcast_group = *mcast_group;
-  listen = pico_tree_findKey(s->MCASTListen, &ltest);
+  listen = listen_find(s, &mcast_link->address, mcast_group);
   if (!listen)
     return -1;
 
@@ -1481,6 +1484,48 @@
 #define PICO_SOCKET_SETOPT_EN(socket,index)  (socket->opt_flags |=  (1 << index))
 #define PICO_SOCKET_SETOPT_DIS(socket,index) (socket->opt_flags &= (uint16_t)~(1 << index))
 
+#ifdef PICO_SUPPORT_MCAST
+static struct pico_ipv4_link *setopt_multicast_check(struct pico_socket *s, void *value, int alloc, int bysource)
+{
+    struct pico_ip_mreq *mreq = NULL;
+    struct pico_ipv4_link *mcast_link = NULL;
+    struct pico_ip_mreq_source *mreq_src = NULL;
+
+    if (!value) {
+      pico_err = PICO_ERR_EINVAL;
+      return NULL;
+    }
+    if (!bysource) {
+      mreq = (struct pico_ip_mreq *) value;
+      mcast_link = pico_socket_setoption_mcastargs_validation(mreq, NULL);
+      if (!mreq->mcast_link_addr.addr)
+        mreq->mcast_link_addr.addr = mcast_link->address.addr;
+    } else {
+      mreq_src = (struct pico_ip_mreq_source *) value;
+      mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq_src);
+      if (!mreq_src->mcast_link_addr.addr)
+        mreq_src->mcast_link_addr.addr = mcast_link->address.addr;
+    }
+    if (!mcast_link) {
+      pico_err = PICO_ERR_EINVAL;
+      return NULL;
+    }
+
+    if (!s->MCASTListen) { /* No RBTree allocated yet */
+      if (alloc) {
+        s->MCASTListen = pico_zalloc(sizeof(struct pico_tree));
+        if (!s->MCASTListen) {
+          pico_err = PICO_ERR_ENOMEM;
+          return NULL;
+        }
+        s->MCASTListen->root = &LEAF;
+        s->MCASTListen->compare = mcast_listen_cmp;
+      } else return NULL;
+    }
+    return mcast_link;
+}
+#endif
+
 int pico_socket_setoption(struct pico_socket *s, int option, void *value) // XXX no check against proto (vs setsockopt) or implicit by socket?
 {
   if (s == NULL) {
@@ -1551,35 +1596,12 @@
       /* EXCLUDE mode */
       if (s->proto->proto_number == PICO_PROTO_UDP) {
         int filter_mode;
-        struct pico_ip_mreq *mreq = NULL;
-        struct pico_mcast_listen *listen = NULL, ltest = {0};
-        struct pico_ipv4_link *mcast_link = NULL;
-
-        if (!value) {
-          pico_err = PICO_ERR_EINVAL;
-          return -1;
-        }
-        mreq = (struct pico_ip_mreq *) value;
-        mcast_link = pico_socket_setoption_mcastargs_validation(mreq, NULL);
-        if (!mcast_link) {
-          pico_err = PICO_ERR_EINVAL;
+        struct pico_mcast_listen *listen;
+        struct pico_ip_mreq *mreq = (struct pico_ip_mreq *)value;
+        struct pico_ipv4_link *mcast_link = setopt_multicast_check(s, value, 1, 0);
+        if (!mcast_link)
           return -1;
-        }
-        if (!mreq->mcast_link_addr.addr)
-          mreq->mcast_link_addr.addr = mcast_link->address.addr;
-
-        if (!s->MCASTListen) { /* No RBTree allocated yet */
-          s->MCASTListen = pico_zalloc(sizeof(struct pico_tree));
-          if (!s->MCASTListen) {
-            pico_err = PICO_ERR_ENOMEM;
-            return -1;
-          }
-          s->MCASTListen->root = &LEAF;
-          s->MCASTListen->compare = mcast_listen_cmp;
-        }
-        ltest.mcast_link = mreq->mcast_link_addr;
-        ltest.mcast_group = mreq->mcast_group_addr;
-        listen = pico_tree_findKey(s->MCASTListen, &ltest);
+        listen = listen_find(s, &mreq->mcast_link_addr, &mreq->mcast_group_addr);
         if (listen) {
           if (listen->filter_mode != PICO_IP_MULTICAST_EXCLUDE) {
             so_mcast_dbg("pico_socket_setoption: ERROR any-source multicast (exclude) on source-specific multicast (include)\n");
@@ -1617,33 +1639,15 @@
       /* EXCLUDE mode */
       if (s->proto->proto_number == PICO_PROTO_UDP) {
         int filter_mode = 0;
-        struct pico_ip_mreq *mreq = NULL;
-        struct pico_mcast_listen *listen = NULL, ltest = {0};
+        struct pico_mcast_listen *listen;
+        struct pico_ip_mreq *mreq = (struct pico_ip_mreq *)value;
         struct pico_ip4 *source = NULL;
-        struct pico_ipv4_link *mcast_link = NULL;
         struct pico_tree_node *index, *_tmp;
-
-        if (!value) {
-          pico_err = PICO_ERR_EINVAL;
+        struct pico_ipv4_link *mcast_link = setopt_multicast_check(s, value, 0, 0);
+        if (!mcast_link)
           return -1;
-        }
-        mreq = (struct pico_ip_mreq *) value;
-        mcast_link = pico_socket_setoption_mcastargs_validation(mreq, NULL);
-        if (!mcast_link) {
-          pico_err = PICO_ERR_EINVAL;
-          return -1;
-        }
-        if (!mreq->mcast_link_addr.addr)
-          mreq->mcast_link_addr.addr = mcast_link->address.addr;
 
-        if (!s->MCASTListen) { /* No RBTree allocated yet */
-          so_mcast_dbg("socket_setoption: ERROR PICO_IP_DROP_MEMBERSHIP before any PICO_IP_ADD_MEMBERSHIP/SOURCE_MEMBERSHIP\n");
-          pico_err = PICO_ERR_EINVAL;
-          return -1;
-        }
-        ltest.mcast_link = mreq->mcast_link_addr;
-        ltest.mcast_group = mreq->mcast_group_addr;
-        listen = pico_tree_findKey(s->MCASTListen, &ltest);
+        listen = listen_find(s, &mreq->mcast_link_addr, &mreq->mcast_group_addr);
         if (!listen) {
           so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_MEMBERSHIP before PICO_IP_ADD_MEMBERSHIP/SOURCE_MEMBERSHIP\n");
           pico_err = PICO_ERR_EADDRNOTAVAIL;
@@ -1676,32 +1680,13 @@
       /* EXCLUDE mode */
       if (s->proto->proto_number == PICO_PROTO_UDP) {
         int filter_mode = 0;
-        struct pico_ip_mreq_source *mreq = NULL;
-        struct pico_mcast_listen *listen = NULL, ltest = {0};
+        struct pico_ip_mreq_source *mreq = (struct pico_ip_mreq_source *)value;
+        struct pico_mcast_listen *listen = NULL; 
         struct pico_ip4 *source = NULL, stest = {0};
-        struct pico_ipv4_link *mcast_link = NULL;
-
-        if (!value) {
-          pico_err = PICO_ERR_EINVAL;
+        struct pico_ipv4_link *mcast_link = setopt_multicast_check(s, value, 0, 1);
+        if (!mcast_link)
           return -1;
-        }
-        mreq = (struct pico_ip_mreq_source *) value;
-        mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq);
-        if (!mcast_link) {
-          pico_err = PICO_ERR_EINVAL;
-          return -1;
-        }
-        if (!mreq->mcast_link_addr.addr)
-          mreq->mcast_link_addr.addr = mcast_link->address.addr;
-
-        if (!s->MCASTListen) { /* No RBTree allocated yet */
-          so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_UNBLOCK_SOURCE before any PICO_IP_ADD_MEMBERSHIP\n");
-          pico_err = PICO_ERR_EINVAL;
-          return -1;
-        }
-        ltest.mcast_link = mreq->mcast_link_addr;
-        ltest.mcast_group = mreq->mcast_group_addr;
-        listen = pico_tree_findKey(s->MCASTListen, &ltest);
+        listen = listen_find(s, &mreq->mcast_link_addr, &mreq->mcast_group_addr);
         if (!listen) {
           so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_UNBLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n");
           pico_err = PICO_ERR_EINVAL;
@@ -1736,34 +1721,15 @@
       /* EXCLUDE mode */
       if (s->proto->proto_number == PICO_PROTO_UDP) {
         int filter_mode = 0;
-        struct pico_ip_mreq_source *mreq = NULL;
-        struct pico_mcast_listen *listen = NULL, ltest = {0};
+        struct pico_ip_mreq_source *mreq = (struct pico_ip_mreq_source *)value;
+        struct pico_mcast_listen *listen = NULL;
         struct pico_ip4 *source = NULL, stest = {0};
-        struct pico_ipv4_link *mcast_link = NULL;
-
-        if (!value) {
-          pico_err = PICO_ERR_EINVAL;
-          return -1;
-        }
-        mreq = (struct pico_ip_mreq_source *) value;
-        mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq);
-        if (!mcast_link) {
-          pico_err = PICO_ERR_EINVAL;
+        struct pico_ipv4_link *mcast_link = setopt_multicast_check(s, value, 0, 1);
+        if (!mcast_link)
           return -1;
-        }
-        if (!mreq->mcast_link_addr.addr)
-          mreq->mcast_link_addr.addr = mcast_link->address.addr;
-
-        if (!s->MCASTListen) { /* No RBTree allocated yet */
-          so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_BLOCK_SOURCE before any PICO_IP_ADD_MEMBERSHIP\n");
-          pico_err = PICO_ERR_EINVAL;
-          return -1;
-        }
-        ltest.mcast_link = mreq->mcast_link_addr;
-        ltest.mcast_group = mreq->mcast_group_addr;
-        listen = pico_tree_findKey(s->MCASTListen, &ltest);
+        listen = listen_find(s, &mreq->mcast_link_addr, &mreq->mcast_group_addr);
         if (!listen) {
-          so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_BLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n");
+          dbg("pico_socket_setoption: ERROR PICO_IP_BLOCK_SOURCE before PICO_IP_ADD_MEMBERSHIP\n");
           pico_err = PICO_ERR_EINVAL;
           return -1;
         } else {
@@ -1801,36 +1767,13 @@
       /* INCLUDE mode */
       if (s->proto->proto_number == PICO_PROTO_UDP) {
         int filter_mode = 0, reference_count = 0;
-        struct pico_ip_mreq_source *mreq = NULL;
-        struct pico_mcast_listen *listen = NULL, ltest = {0};
+        struct pico_ip_mreq_source *mreq = (struct pico_ip_mreq_source *)value;
+        struct pico_mcast_listen *listen = NULL; 
         struct pico_ip4 *source = NULL, stest = {0};
-        struct pico_ipv4_link *mcast_link = NULL;
-
-        if (!value) {
-          pico_err = PICO_ERR_EINVAL;
-          return -1;
-        }
-        mreq = (struct pico_ip_mreq_source *) value;
-        mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq);
-        if (!mcast_link) {
-          pico_err = PICO_ERR_EINVAL;
+        struct pico_ipv4_link *mcast_link = setopt_multicast_check(s, value, 1, 1);
+        if (!mcast_link)
           return -1;
-        }
-        if (!mreq->mcast_link_addr.addr)
-          mreq->mcast_link_addr.addr = mcast_link->address.addr;
-
-        if (!s->MCASTListen) { /* No RBTree allocated yet */
-          s->MCASTListen = pico_zalloc(sizeof(struct pico_tree));
-          if (!s->MCASTListen) {
-            pico_err = PICO_ERR_ENOMEM;
-            return -1;
-          }
-          s->MCASTListen->root = &LEAF;
-          s->MCASTListen->compare = mcast_listen_cmp;
-        }
-        ltest.mcast_link = mreq->mcast_link_addr;
-        ltest.mcast_group = mreq->mcast_group_addr;
-        listen = pico_tree_findKey(s->MCASTListen, &ltest);
+        listen = listen_find(s, &mreq->mcast_link_addr, &mreq->mcast_group_addr);
         if (listen) {
           if (listen->filter_mode != PICO_IP_MULTICAST_INCLUDE) {
             so_mcast_dbg("pico_socket_setoption: ERROR source-specific multicast (include) on any-source multicast (exclude)\n");
@@ -1888,32 +1831,13 @@
       /* INCLUDE mode */
       if (s->proto->proto_number == PICO_PROTO_UDP) {
         int filter_mode = 0, reference_count = 0;
-        struct pico_ip_mreq_source *mreq = NULL;
-        struct pico_mcast_listen *listen = NULL, ltest = {0};
+        struct pico_ip_mreq_source *mreq = (struct pico_ip_mreq_source *)value;
+        struct pico_mcast_listen *listen = NULL;
         struct pico_ip4 *source = NULL, stest = {0};
-        struct pico_ipv4_link *mcast_link = NULL;
-
-        if (!value) {
-          pico_err = PICO_ERR_EINVAL;
+        struct pico_ipv4_link *mcast_link = setopt_multicast_check(s, value, 0, 1);
+        if (!mcast_link)
           return -1;
-        }
-        mreq = (struct pico_ip_mreq_source *) value;
-        mcast_link = pico_socket_setoption_mcastargs_validation(NULL, mreq);
-        if (!mcast_link) {
-          pico_err = PICO_ERR_EINVAL;
-          return -1;
-        }
-        if (!mreq->mcast_link_addr.addr)
-          mreq->mcast_link_addr.addr = mcast_link->address.addr;
-
-        if (!s->MCASTListen) { /* No RBTree allocated yet */
-          so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_SOURCE_MEMBERSHIP before any PICO_IP_ADD_SOURCE_MEMBERSHIP\n");
-          pico_err = PICO_ERR_EINVAL;
-          return -1;
-        }
-        ltest.mcast_link = mreq->mcast_link_addr;
-        ltest.mcast_group = mreq->mcast_group_addr;
-        listen = pico_tree_findKey(s->MCASTListen, &ltest);
+        listen = listen_find(s, &mreq->mcast_link_addr, &mreq->mcast_group_addr);
         if (!listen) {
           so_mcast_dbg("pico_socket_setoption: ERROR PICO_IP_DROP_SOURCE_MEMBERSHIP before PICO_IP_ADD_SOURCE_MEMBERSHIP\n");
           pico_err = PICO_ERR_EADDRNOTAVAIL;

--- a/stack/pico_stack.c	Fri Sep 27 12:43:36 2013 +0000
+++ b/stack/pico_stack.c	Tue Oct 01 06:13:42 2013 +0000
@@ -34,7 +34,7 @@
   const uint8_t PICO_ETHADDR_MCAST[6] = {0x01, 0x00, 0x5e, 0x00, 0x00, 0x00};
 #endif
 
-volatile unsigned long pico_tick;
+volatile uint32_t pico_tick;
 volatile pico_err_t pico_err;
 
 static uint32_t _rand_seed;