Diff: modules/pico_ipv4.c

Revision:

68:0847e35d08a6

Child:

70:cd218dd180e5

diff -r fb3098601f18 -r 0847e35d08a6 modules/pico_ipv4.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/modules/pico_ipv4.c	Thu Sep 19 13:26:14 2013 +0000
@@ -0,0 +1,1423 @@
+/*********************************************************************
+PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+See LICENSE and COPYING for usage.
+
+Authors: Daniele Lacamera, Markian Yskout
+*********************************************************************/
+
+
+#include "pico_config.h"
+#include "pico_ipfilter.h"
+#include "pico_ipv4.h"
+#include "pico_icmp4.h"
+#include "pico_stack.h"
+#include "pico_eth.h"
+#include "pico_udp.h"
+#include "pico_tcp.h"
+#include "pico_socket.h"
+#include "pico_device.h"
+#include "pico_nat.h"
+#include "pico_igmp.h"
+#include "pico_tree.h"
+
+#ifdef PICO_SUPPORT_IPV4
+
+#ifdef PICO_SUPPORT_MCAST
+# define ip_mcast_dbg(...) do{}while(0) /* so_mcast_dbg in pico_socket.c */
+# define PICO_MCAST_ALL_HOSTS long_be(0xE0000001) /* 224.0.0.1 */
+/* Default network interface for multicast transmission */
+static struct pico_ipv4_link *mcast_default_link = NULL;
+#endif
+#ifdef PICO_SUPPORT_IPFRAG
+# define reassembly_dbg(...) do{}while(0) 
+#endif
+
+/* Queues */
+static struct pico_queue in = {0};
+static struct pico_queue out = {0};
+
+/* Functions */
+static int ipv4_route_compare(void *ka, void * kb);
+
+int pico_ipv4_to_string(char *ipbuf, const uint32_t ip)
+{
+  const unsigned char *addr = (const unsigned char *) &ip;
+  int i;
+
+  if (!ipbuf) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+
+  for(i = 0; i < 4; i++)
+  {
+    if(addr[i] > 99){
+      *ipbuf++ = '0' + (addr[i] / 100);
+      *ipbuf++ = '0' + ((addr[i] % 100) / 10);
+      *ipbuf++ = '0' + ((addr[i] % 100) % 10);
+    }else if(addr[i] > 9){
+      *ipbuf++ = '0' + (addr[i] / 10);
+      *ipbuf++ = '0' + (addr[i] % 10);
+    }else{
+      *ipbuf++ = '0' + addr[i];
+    }
+    if(i < 3)
+      *ipbuf++ = '.';
+  }
+  *ipbuf = '\0';
+  
+  return 0;
+}
+    
+int pico_string_to_ipv4(const char *ipstr, uint32_t *ip)
+{
+  unsigned char buf[4] = {0};
+  int cnt = 0;
+  int p;
+
+  if(!ipstr || !ip) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+
+  while((p = *ipstr++) != 0)
+  {
+    if(pico_is_digit(p)){
+      buf[cnt] = (10 * buf[cnt]) + (p - '0');
+    }else if(p == '.'){
+        cnt++;
+    }else{
+      return -1;
+    }
+  }   
+  
+  /* Handle short notation */
+  if(cnt == 1){
+    buf[3] = buf[1];
+    buf[1] = 0;
+    buf[2] = 0;
+  }else if (cnt == 2){
+    buf[3] = buf[2];
+    buf[2] = 0;
+  }else if(cnt != 3){
+    /* String could not be parsed, return error */
+    return -1;
+  }   
+
+  *ip = long_from(buf);
+
+  return 0;
+
+}  
+
+int pico_ipv4_valid_netmask(uint32_t mask)
+{
+  int cnt = 0;
+  int end = 0;
+  int i;
+  uint32_t mask_swap = long_be(mask);
+
+  /* 
+   * Swap bytes for convenient parsing 
+   * e.g. 0x..f8ff will become 0xfff8..
+   * Then, we count the consecutive bits
+   *
+   * */
+
+  for(i = 0; i < 32; i++){
+    if((mask_swap << i) & 0x80000000){
+      if(end) {
+        pico_err = PICO_ERR_EINVAL;
+        return -1;
+      }
+      cnt++;
+    }else{
+      end = 1;
+    }        
+  }
+  return cnt;
+}
+
+int pico_ipv4_is_unicast(uint32_t address) 
+{
+  const unsigned char *addr = (unsigned char *) &address;
+  if((addr[0] & 0xe0) == 0xe0)
+    return 0; /* multicast */
+    
+  return 1;
+}
+
+int pico_ipv4_is_multicast(uint32_t address) 
+{
+  const unsigned char *addr = (unsigned char *) &address;
+  if((addr[0] != 0xff) && ((addr[0] & 0xe0) == 0xe0))
+    return 1; /* multicast */
+    
+  return 0;
+}
+
+static int pico_ipv4_checksum(struct pico_frame *f)
+{
+  struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+  if (!hdr)
+    return -1;
+  hdr->crc = 0;
+  hdr->crc = short_be(pico_checksum(hdr, f->net_len));
+  return 0;
+}
+
+#ifdef PICO_SUPPORT_IPFRAG
+struct pico_ipv4_fragmented_packet {
+  uint16_t id;
+  uint8_t proto;
+  struct pico_ip4 src;
+  struct pico_ip4 dst;
+  uint16_t total_len;
+  struct pico_tree *t;
+};
+
+static int pico_ipv4_fragmented_packet_cmp(void *ka, void *kb)
+{
+  struct pico_ipv4_fragmented_packet *a = ka, *b = kb;
+
+  if (a->id < b->id)
+    return -1; 
+  else if (a->id > b->id)
+    return 1;
+  else {
+    if (a->proto < b->proto)
+      return -1;
+    else if (a->proto > b->proto)
+      return 1;
+    else {
+      if (a->src.addr < b->src.addr)
+        return -1;
+      else if (a->src.addr > b->src.addr)
+        return 1;
+      else {
+        if (a->dst.addr < b->dst.addr)
+          return -1;
+        else if (a->dst.addr > b->dst.addr)
+          return 1;
+        else
+          return 0;
+      }
+    }
+  }
+} 
+
+static int pico_ipv4_fragmented_element_cmp(void *ka, void *kb)
+{
+  struct pico_frame *frame_a = ka, *frame_b = kb;
+  struct pico_ipv4_hdr *a, *b;
+  a = (struct pico_ipv4_hdr *) frame_a->net_hdr;
+  b = (struct pico_ipv4_hdr *) frame_b->net_hdr;
+
+  if (short_be((a->frag & PICO_IPV4_FRAG_MASK)) < short_be((b->frag & PICO_IPV4_FRAG_MASK)))
+    return -1; 
+  else if (short_be((a->frag & PICO_IPV4_FRAG_MASK)) > short_be((b->frag & PICO_IPV4_FRAG_MASK)))
+    return 1;
+  else
+    return 0;
+} 
+    
+PICO_TREE_DECLARE(pico_ipv4_fragmented_tree, pico_ipv4_fragmented_packet_cmp);
+
+static inline void pico_ipv4_fragmented_cleanup(struct pico_ipv4_fragmented_packet *pfrag)
+{
+  struct pico_tree_node *index = NULL, *_tmp = NULL;
+  struct pico_frame *f_frag = NULL;
+
+  pico_tree_foreach_safe(index, pfrag->t, _tmp) {
+    f_frag = index->keyValue;
+    reassembly_dbg("REASSEMBLY: remove packet with offset %u\n", short_be(((struct pico_ipv4_hdr *)f_frag->net_hdr)->frag) & PICO_IPV4_FRAG_MASK);
+    pico_tree_delete(pfrag->t, f_frag);
+    pico_frame_discard(f_frag);
+  }
+  pico_tree_delete(&pico_ipv4_fragmented_tree, pfrag);
+  pico_free(pfrag->t);
+  pico_free(pfrag);
+}
+#endif /* PICO_SUPPORT_IPFRAG */
+
+#ifdef PICO_SUPPORT_IPFRAG
+static inline int pico_ipv4_fragmented_check(struct pico_protocol *self, struct pico_frame **f)
+{
+  uint8_t *running_pointer = NULL;
+  uint16_t running_offset = 0;
+  uint16_t offset = 0;
+  uint16_t data_len = 0;
+  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_frame *f_new = NULL, *f_frag = NULL;
+  struct pico_tree_node *index, *_tmp;
+
+  data_len = short_be(hdr->len) - (*f)->net_len;
+  offset = short_be(hdr->frag) & PICO_IPV4_FRAG_MASK;
+  if (short_be(hdr->frag) & PICO_IPV4_MOREFRAG) {
+    if (!offset) {
+      reassembly_dbg("REASSEMBLY: first element of a fragmented packet with id %X and offset %u\n", short_be(hdr->id), offset);
+      if (!pico_tree_empty(&pico_ipv4_fragmented_tree)) {
+        reassembly_dbg("REASSEMBLY: cleanup tree\n");
+        // only one entry allowed in this tree
+        pfrag = pico_tree_first(&pico_ipv4_fragmented_tree);
+        pico_ipv4_fragmented_cleanup(pfrag);
+      }
+      // add entry in tree for this ID and create secondary tree to contain fragmented elements
+      pfrag = pico_zalloc(sizeof(struct pico_ipv4_fragmented_packet));
+      if (!pfrag) {
+        pico_err = PICO_ERR_ENOMEM;
+        return -1;
+      }
+      pfrag->id = short_be(hdr->id);
+      pfrag->proto = hdr->proto;
+      pfrag->src.addr = long_be(hdr->src.addr);
+      pfrag->dst.addr = long_be(hdr->dst.addr);
+      pfrag->total_len = short_be(hdr->len) - (*f)->net_len;
+      pfrag->t = pico_zalloc(sizeof(struct pico_tree));
+      if (!pfrag->t) {
+        pico_free(pfrag);
+        pico_err = PICO_ERR_ENOMEM;
+        return -1;
+      }
+      pfrag->t->root = &LEAF;
+      pfrag->t->compare = pico_ipv4_fragmented_element_cmp;
+       
+      pico_tree_insert(pfrag->t, *f);
+      pico_tree_insert(&pico_ipv4_fragmented_tree, pfrag);
+      return 0;
+    }
+    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);
+      if (pfrag) {
+        pfrag->total_len += (short_be(hdr->len) - (*f)->net_len);
+        pico_tree_insert(pfrag->t, *f);
+        return 0;
+      } else {
+        reassembly_dbg("REASSEMBLY: silently discard intermediate frame, first packet was lost or disallowed (one fragmented packet at a time)\n");
+        pico_frame_discard(*f);
+        return 0;
+      }
+    }
+  } 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);
+    if (pfrag) {
+      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);
+      f_new = self->alloc(self, pfrag->total_len);
+
+      f_frag = pico_tree_first(pfrag->t);
+      reassembly_dbg("REASSEMBLY: copy IP header information len = %lu\n", f_frag->net_len);
+      f_frag_hdr = (struct pico_ipv4_hdr *)f_frag->net_hdr;
+      data_len = short_be(f_frag_hdr->len) - f_frag->net_len; 
+      memcpy(f_new->net_hdr, f_frag->net_hdr, f_frag->net_len);
+      memcpy(f_new->transport_hdr, f_frag->transport_hdr, data_len);
+      running_pointer = f_new->transport_hdr + data_len;
+      offset = short_be(f_frag_hdr->frag) & PICO_IPV4_FRAG_MASK;
+      running_offset = data_len / 8;
+      pico_tree_delete(pfrag->t, f_frag);
+      pico_frame_discard(f_frag);
+      reassembly_dbg("REASSEMBLY: reassembled first packet of %u data bytes, offset = %u next expected offset = %u\n", data_len, offset, running_offset);
+
+      pico_tree_foreach_safe(index, pfrag->t, _tmp)
+      {
+        f_frag = index->keyValue;
+        f_frag_hdr = (struct pico_ipv4_hdr *)f_frag->net_hdr;
+        data_len = short_be(f_frag_hdr->len) - f_frag->net_len; 
+        memcpy(running_pointer, f_frag->transport_hdr, data_len);
+        running_pointer += data_len;
+        offset = short_be(f_frag_hdr->frag) & PICO_IPV4_FRAG_MASK;
+        if (offset != running_offset) {
+          reassembly_dbg("REASSEMBLY: error reassembling intermediate packet: offset %u != expected offset %u (missing fragment)\n", offset, running_offset);
+          pico_ipv4_fragmented_cleanup(pfrag);
+          return -1;
+        }
+        running_offset += (data_len / 8);
+        pico_tree_delete(pfrag->t, f_frag);
+        pico_frame_discard(f_frag);
+        reassembly_dbg("REASSEMBLY: reassembled intermediate packet of %u data bytes, offset = %u next expected offset = %u\n", data_len, offset, running_offset);
+      }
+      pico_tree_delete(&pico_ipv4_fragmented_tree, pfrag);
+      pico_free(pfrag);
+
+      data_len = short_be(hdr->len) - (*f)->net_len;
+      memcpy(running_pointer, (*f)->transport_hdr, data_len);
+      offset = short_be(hdr->frag) & PICO_IPV4_FRAG_MASK;
+      pico_frame_discard(*f);
+      reassembly_dbg("REASSEMBLY: reassembled last packet of %u data bytes, offset = %u\n", data_len, offset);
+      
+      hdr = (struct pico_ipv4_hdr *)f_new->net_hdr;
+      hdr->len = pfrag->total_len;
+      hdr->frag = 0; /* flags cleared and no offset */
+      hdr->crc = 0;
+      hdr->crc = short_be(pico_checksum(hdr, f_new->net_len));
+      /* Optional, the UDP/TCP CRC should already be correct */
+      if (0) {
+  #ifdef PICO_SUPPORT_TCP
+      } else if (hdr->proto == PICO_PROTO_TCP) {
+        tcp_hdr = (struct pico_tcp_hdr *) f_new->transport_hdr;
+        tcp_hdr->crc = 0;
+        tcp_hdr->crc = short_be(pico_tcp_checksum_ipv4(f_new));
+  #endif
+  #ifdef PICO_SUPPORT_UDP
+      } else if (hdr->proto == PICO_PROTO_UDP){
+        udp_hdr = (struct pico_udp_hdr *) f_new->transport_hdr;
+        udp_hdr->crc = 0;
+        udp_hdr->crc = short_be(pico_udp_checksum_ipv4(f_new));
+  #endif
+      }
+      reassembly_dbg("REASSEMBLY: packet with id %X reassembled correctly\n", short_be(hdr->id));
+      *f = f_new;
+      return 1;
+    } else {
+      reassembly_dbg("REASSEMBLY: silently discard last frame, first packet was lost or disallowed (one fragmented packet at a time)\n");
+      pico_frame_discard(*f);
+      return 0;
+    }
+  } else {
+    return 1;
+  }
+}
+#else
+static inline int pico_ipv4_fragmented_check(struct pico_protocol *self, struct pico_frame **f)
+{
+  return 1;
+}
+#endif /* PICO_SUPPORT_IPFRAG */
+
+#ifdef PICO_SUPPORT_CRC
+static inline int pico_ipv4_crc_check(struct pico_frame *f)
+{
+  uint16_t checksum_invalid = 1;
+  struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+
+  checksum_invalid = short_be(pico_checksum(hdr, f->net_len));
+  if (checksum_invalid) {
+    dbg("IP: checksum failed!\n");
+    pico_frame_discard(f);
+    return 0;
+  }
+  return 1;
+}
+#else
+static inline int pico_ipv4_crc_check(struct pico_frame *f)
+{
+	IGNORE_PARAMETER(f);
+  return 1;
+}
+#endif /* PICO_SUPPORT_CRC */
+
+static int pico_ipv4_forward(struct pico_frame *f);
+#ifdef PICO_SUPPORT_MCAST
+static int pico_ipv4_mcast_filter(struct pico_frame *f);
+#endif
+
+static int ipv4_link_compare(void *ka, void *kb)
+{
+  struct pico_ipv4_link *a = ka, *b =kb;
+  if (a->address.addr < b->address.addr)
+    return -1;
+  if (a->address.addr > b->address.addr)
+    return 1;
+
+  //zero can be assigned multiple times (e.g. for DHCP)
+  if (a->dev != NULL && b->dev != NULL && a->address.addr == PICO_IP4_ANY && b->address.addr == PICO_IP4_ANY){
+    if (a->dev < b->dev)
+      return -1;
+    if (a->dev > b->dev)
+      return 1;
+  }
+  return 0;
+}
+
+PICO_TREE_DECLARE(Tree_dev_link, ipv4_link_compare);
+
+static int pico_ipv4_process_in(struct pico_protocol *self, struct pico_frame *f)
+{
+  uint8_t option_len = 0;
+  int ret = 0;
+  struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+  struct pico_ipv4_link test = {.address = {.addr = PICO_IP4_ANY}, .dev = NULL};
+
+  /* NAT needs transport header information */
+  if(((hdr->vhl) & 0x0F )> 5){
+     option_len =  4*(((hdr->vhl) & 0x0F)-5);
+  }
+  f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR + option_len;
+  f->transport_len = short_be(hdr->len) - PICO_SIZE_IP4HDR - option_len;
+  f->net_len = PICO_SIZE_IP4HDR + option_len;
+
+#ifdef PICO_SUPPORT_IPFILTER
+  if (ipfilter(f)) {
+    /*pico_frame is discarded as result of the filtering*/
+    return 0;
+  }
+#endif
+
+  /* ret == 1 indicates to continue the function */
+  ret = pico_ipv4_crc_check(f);
+  if (ret < 1)
+    return ret;
+  ret = pico_ipv4_fragmented_check(self, &f);
+  if (ret < 1)
+    return ret;
+
+#ifdef PICO_SUPPORT_MCAST
+  /* Multicast address in source, discard quietly */
+  if (pico_ipv4_is_multicast(hdr->src.addr)) {
+    ip_mcast_dbg("MCAST: ERROR multicast address %08X in source address\n", hdr->src.addr);
+    pico_frame_discard(f);
+    return 0;
+  }
+#endif
+  if (hdr->frag & 0x80) {
+    pico_frame_discard(f); //RFC 3514
+    return 0;
+  }
+  if (0) {
+#ifdef PICO_SUPPORT_UDP
+  } else if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_UDP)) {
+      /* Receiving UDP broadcast datagram */
+      f->flags |= PICO_FRAME_FLAG_BCAST;
+      pico_enqueue(pico_proto_udp.q_in, f);
+#endif
+  } else if (pico_ipv4_is_multicast(hdr->dst.addr)) {
+#ifdef PICO_SUPPORT_MCAST
+    /* Receiving UDP multicast datagram TODO set f->flags? */
+    if (hdr->proto == PICO_PROTO_IGMP) {
+      ip_mcast_dbg("MCAST: received IGMP message\n");
+      pico_transport_receive(f, PICO_PROTO_IGMP);
+    } else if ((pico_ipv4_mcast_filter(f) == 0) && (hdr->proto == PICO_PROTO_UDP)) {
+      pico_enqueue(pico_proto_udp.q_in, f);
+    } else {
+      pico_frame_discard(f);
+    }
+#endif
+  } else if (pico_ipv4_link_find(&hdr->dst)) {
+    if (pico_ipv4_nat_inbound(f, &hdr->dst) == 0)
+      pico_enqueue(pico_proto_ipv4.q_in, f); /* dst changed, reprocess */
+    else 
+      pico_transport_receive(f, hdr->proto);
+  } else if (pico_tree_findKey(&Tree_dev_link, &test)){
+#ifdef PICO_SUPPORT_UDP
+    //address of this device is apparently 0.0.0.0; might be a DHCP packet
+    /* XXX KRO: is obsolete. Broadcast flag is set on outgoing DHCP messages.
+     * incomming DHCP messages are to be broadcasted. Our current DHCP server
+     * implementation does not take this flag into account yet though ... */
+    pico_enqueue(pico_proto_udp.q_in, f);
+#endif
+  } else {
+    
+    if((pico_ipv4_is_broadcast(hdr->dst.addr)))
+    {
+      /* don't forward broadcast frame, discard! */
+      pico_frame_discard(f);
+    } else if (pico_ipv4_forward(f) != 0) {
+        /* Packet is not local. Try to forward. */
+        pico_ipv4_forward(f);
+    }
+  }
+  return 0;
+}
+
+PICO_TREE_DECLARE(Routes, ipv4_route_compare);
+
+
+static int pico_ipv4_process_out(struct pico_protocol *self, struct pico_frame *f)
+{
+	IGNORE_PARAMETER(self);
+	f->start = (uint8_t*) f->net_hdr;
+  #ifdef PICO_SUPPORT_IPFILTER
+  if (ipfilter(f)) {
+    /*pico_frame is discarded as result of the filtering*/
+    return 0;
+  }
+  #endif
+  return pico_sendto_dev(f);
+}
+
+
+static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, int size)
+{
+	struct pico_frame *f =  pico_frame_alloc(size + PICO_SIZE_IP4HDR + PICO_SIZE_ETHHDR);
+	IGNORE_PARAMETER(self);
+
+	if (!f)
+    return NULL;
+  f->datalink_hdr = f->buffer;
+  f->net_hdr = f->buffer + PICO_SIZE_ETHHDR;
+  f->net_len = PICO_SIZE_IP4HDR;
+  f->transport_hdr = f->net_hdr + PICO_SIZE_IP4HDR;
+  f->transport_len = size;
+  f->len =  size + PICO_SIZE_IP4HDR;
+  return f;
+}
+
+static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f);
+
+/* Interface: protocol definition */
+struct pico_protocol pico_proto_ipv4 = {
+  .name = "ipv4",
+  .proto_number = PICO_PROTO_IPV4,
+  .layer = PICO_LAYER_NETWORK,
+  .alloc = pico_ipv4_alloc,
+  .process_in = pico_ipv4_process_in,
+  .process_out = pico_ipv4_process_out,
+  .push = pico_ipv4_frame_sock_push,
+  .q_in = &in,
+  .q_out = &out,
+};
+
+struct pico_ipv4_route
+{
+  struct pico_ip4 dest;
+  struct pico_ip4 netmask;
+  struct pico_ip4 gateway;
+  struct pico_ipv4_link *link;
+  uint32_t metric;
+};
+
+
+static int ipv4_route_compare(void *ka, void * kb)
+{
+  struct pico_ipv4_route *a = ka, *b = kb;
+
+  /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */
+  if (long_be(a->netmask.addr) < long_be(b->netmask.addr))
+    return -1;
+
+  if (long_be(a->netmask.addr) > long_be(b->netmask.addr))
+    return 1;
+
+  if (a->dest.addr < b->dest.addr)
+    return -1;
+
+  if (a->dest.addr > b->dest.addr)
+    return 1;
+
+  if (a->metric < b->metric)
+    return -1;
+
+  if (a->metric > b->metric)
+    return 1;
+
+  return 0;
+}
+
+static struct pico_ipv4_route *route_find(const struct pico_ip4 *addr)
+{
+  struct pico_ipv4_route *r;
+  struct pico_tree_node * index;
+
+  if(addr->addr != PICO_IP4_BCAST)
+  {
+    pico_tree_foreach_reverse(index, &Routes) {
+      r = index->keyValue;
+      if ((addr->addr & (r->netmask.addr)) == (r->dest.addr)) {
+        return r;
+      }
+    }
+  }
+  else
+  {
+    r = pico_tree_first(&Routes);
+    if(!r->netmask.addr)
+    {
+      return r;
+    }
+    else
+    {
+      dbg("WARNING: no default route for a global broadcast found\n");
+    }
+  }
+
+  return NULL;
+}
+
+struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr)
+{
+  struct pico_ip4 nullip;
+  struct pico_ipv4_route *route;
+  nullip.addr = 0U;
+
+  if(!addr) {
+    pico_err = PICO_ERR_EINVAL;
+    return nullip;
+  }
+
+  route = route_find(addr);
+  if (!route) {
+    pico_err = PICO_ERR_EHOSTUNREACH;
+    return nullip;
+  }
+  else
+    return route->gateway;
+}
+
+struct pico_ip4 *pico_ipv4_source_find(const struct pico_ip4 *dst)
+{
+  struct pico_ip4 *myself = NULL;
+  struct pico_ipv4_route *rt;
+
+  if(!dst) {
+    pico_err = PICO_ERR_EINVAL;
+    return NULL;
+  }
+
+  rt = route_find(dst);
+  if (rt) {
+    myself = &rt->link->address;
+  } else
+    pico_err = PICO_ERR_EHOSTUNREACH;
+  return myself;
+}
+
+
+#ifdef PICO_SUPPORT_MCAST
+/*                        link
+ *                         |  
+ *                    MCASTGroups
+ *                    |    |     |
+ *         ------------    |     ------------
+ *         |               |                |
+ *   MCASTSources    MCASTSources     MCASTSources    
+ *   |  |  |  |      |  |  |  |       |  |  |  |
+ *   S  S  S  S      S  S  S  S       S  S  S  S
+ *
+ *   MCASTGroups: RBTree(mcast_group)
+ *   MCASTSources: RBTree(source)
+ */
+static int ipv4_mcast_groups_cmp(void * ka, void * kb)
+{
+  struct pico_mcast_group *a = ka, *b = kb;
+  if (a->mcast_addr.addr < b->mcast_addr.addr) {
+    return -1;
+  } else if (a->mcast_addr.addr > b->mcast_addr.addr) {
+    return 1;
+  } else {
+    return 0;
+  }
+}
+
+static int ipv4_mcast_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;
+}
+
+static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link)
+{
+  uint16_t i = 0;
+  struct pico_mcast_group __attribute__ ((unused)) *g = NULL;
+  struct pico_ip4 __attribute__ ((unused)) *source = NULL;
+  struct pico_tree_node *index = NULL, *index2 = NULL;
+
+  ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
+  ip_mcast_dbg("+                           MULTICAST list interface %-16s             +\n", mcast_link->dev->name);
+  ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
+  ip_mcast_dbg("+  nr  |    interface     | host group | reference count | filter mode |  source  +\n");
+  ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
+
+  pico_tree_foreach(index, mcast_link->MCASTGroups)
+  {
+    g = index->keyValue;
+    ip_mcast_dbg("+ %04d | %16s |  %08X  |      %05u      |      %u      | %8s +\n", i, mcast_link->dev->name, g->mcast_addr.addr, g->reference_count, g->filter_mode, "");
+    pico_tree_foreach(index2, &g->MCASTSources)
+    {
+      source = index2->keyValue;
+      ip_mcast_dbg("+ %4s | %16s |  %8s  |      %5s      |      %s      | %08X +\n", "", "", "", "", "", source->addr);
+    }
+    i++;
+  }
+  ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
+}
+
+int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
+{
+  struct pico_mcast_group *g = NULL, test = {0};
+  struct pico_ipv4_link *link = NULL;
+  struct pico_tree_node *index = NULL, *_tmp = NULL;
+  struct pico_ip4 *source = NULL;
+
+  if (mcast_link)
+    link = pico_ipv4_link_get(mcast_link);
+  else
+    link = mcast_default_link;
+
+  test.mcast_addr = *mcast_group;
+  g = pico_tree_findKey(link->MCASTGroups, &test);
+  if (g) {
+    if (reference_count)
+      g->reference_count++;
+    pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
+  } else {
+    g = pico_zalloc(sizeof(struct pico_mcast_group));
+    if (!g) {
+      pico_err = PICO_ERR_ENOMEM;
+      return -1;
+    }
+    /* "non-existent" state of filter mode INCLUDE and empty source list */
+    g->filter_mode = PICO_IP_MULTICAST_INCLUDE;
+    g->reference_count = 1;
+    g->mcast_addr = *mcast_group;
+    g->MCASTSources.root = &LEAF;
+    g->MCASTSources.compare = ipv4_mcast_sources_cmp;
+    pico_tree_insert(link->MCASTGroups, g);
+    pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE);
+  }
+
+  /* cleanup filter */
+  pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
+  {
+    source = index->keyValue;
+    pico_tree_delete(&g->MCASTSources, source);
+    pico_free(source);
+  }
+  /* insert new filter */
+  if (MCASTFilter) {
+    pico_tree_foreach(index, MCASTFilter)
+    {
+      source = pico_zalloc(sizeof(struct pico_ip4));
+      if (!source) {
+        pico_err = PICO_ERR_ENOMEM;
+        return -1;
+      }
+      source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
+      pico_tree_insert(&g->MCASTSources, source);
+    }
+  }
+  g->filter_mode = filter_mode;
+
+  pico_ipv4_mcast_print_groups(link);
+  return 0;
+}
+
+int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
+{
+
+  struct pico_mcast_group *g = NULL, test = {0};
+  struct pico_ipv4_link *link = NULL;
+  struct pico_tree_node *index = NULL, *_tmp = NULL;
+  struct pico_ip4 *source = NULL;
+
+  if (mcast_link)
+    link = pico_ipv4_link_get(mcast_link);
+  else
+    link = mcast_default_link;
+
+  test.mcast_addr = *mcast_group;
+  g = pico_tree_findKey(link->MCASTGroups, &test);
+  if (!g) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  } else {
+    if (reference_count && (--(g->reference_count) < 1)) {
+      pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_DELETE);
+      /* cleanup filter */
+      pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
+      {
+        source = index->keyValue;
+        pico_tree_delete(&g->MCASTSources, source);
+        pico_free(source);
+      }
+      pico_tree_delete(link->MCASTGroups, g);
+      pico_free(g); 
+    } else {
+      pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
+      /* cleanup filter */
+      pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
+      {
+        source = index->keyValue;
+        pico_tree_delete(&g->MCASTSources, source);
+        pico_free(source);
+      }
+      /* insert new filter */
+      if (MCASTFilter) {
+        pico_tree_foreach(index, MCASTFilter)
+        {
+          source = pico_zalloc(sizeof(struct pico_ip4));
+          if (!source) {
+            pico_err = PICO_ERR_ENOMEM;
+            return -1;
+          }
+          source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
+          pico_tree_insert(&g->MCASTSources, source);
+        }
+      }
+      g->filter_mode = filter_mode;
+    }
+  }
+
+  pico_ipv4_mcast_print_groups(link);
+  return 0;
+}
+
+struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
+{
+  return mcast_default_link;
+}
+
+static int pico_ipv4_mcast_filter(struct pico_frame *f)
+{
+  struct pico_ipv4_link *link = NULL;
+  struct pico_tree_node *index = NULL, *index2 = NULL;
+  struct pico_mcast_group *g = NULL, test = {0};
+  struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+
+  test.mcast_addr = hdr->dst; 
+
+  pico_tree_foreach(index, &Tree_dev_link) 
+  {
+    link = index->keyValue;
+    g = pico_tree_findKey(link->MCASTGroups, &test);
+    if (g) {
+      if (f->dev == link->dev) {
+        ip_mcast_dbg("MCAST: IP %08X is group member of current link %s\n", hdr->dst.addr, f->dev->name);
+        /* perform source filtering */
+        switch (g->filter_mode)
+        {
+          case PICO_IP_MULTICAST_INCLUDE:
+            pico_tree_foreach(index2, &g->MCASTSources)
+            {
+              if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
+                ip_mcast_dbg("MCAST: IP %08X in included interface source list\n", hdr->src.addr);
+                return 0;
+              }
+            }
+            ip_mcast_dbg("MCAST: IP %08X NOT in included interface source list\n", hdr->src.addr);
+            return -1;
+
+          case PICO_IP_MULTICAST_EXCLUDE:
+            pico_tree_foreach(index2, &g->MCASTSources)
+            {
+              if (hdr->src.addr == ((struct pico_ip4 *)index2->keyValue)->addr) {
+                ip_mcast_dbg("MCAST: IP %08X in excluded interface source list\n", hdr->src.addr);
+                return -1;
+              }
+            }
+            ip_mcast_dbg("MCAST: IP %08X NOT in excluded interface source list\n", hdr->src.addr);
+            return 0;
+
+          default:
+            return -1;
+        }
+      } else {
+        ip_mcast_dbg("MCAST: IP %08X is group member of different link %s\n", hdr->dst.addr, link->dev->name);
+      }
+    } else {
+      ip_mcast_dbg("MCAST: IP %08X is not a group member of link %s\n", hdr->dst.addr, f->dev->name);
+    }
+  }
+  return -1;
+}
+
+#else 
+
+int pico_ipv4_mcast_join(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
+{
+  pico_err = PICO_ERR_EPROTONOSUPPORT;
+  return -1;
+}
+int pico_ipv4_mcast_leave(struct pico_ip4 *mcast_link, struct pico_ip4 *mcast_group, uint8_t reference_count, uint8_t filter_mode, struct pico_tree *MCASTFilter)
+{
+  pico_err = PICO_ERR_EPROTONOSUPPORT;
+  return -1;
+}
+struct pico_ipv4_link *pico_ipv4_get_default_mcastlink(void)
+{
+  pico_err = PICO_ERR_EPROTONOSUPPORT;
+  return NULL;
+}
+#endif /* PICO_SUPPORT_MCAST */
+
+int pico_ipv4_frame_push(struct pico_frame *f, struct pico_ip4 *dst, uint8_t proto)
+{
+
+  struct pico_ipv4_route *route;
+  struct pico_ipv4_link *link;
+  struct pico_ipv4_hdr *hdr;
+  uint8_t ttl = PICO_IPV4_DEFAULT_TTL;
+  uint8_t vhl = 0x45; /* version 4, header length 20 */
+  static uint16_t ipv4_progressive_id = 0x91c0;
+#ifdef PICO_SUPPORT_MCAST
+  struct pico_tree_node *index;
+#endif
+
+  if(!f || !dst) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+  hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+  if (!hdr) {
+    dbg("IP header error\n");
+    pico_err = PICO_ERR_EINVAL;
+    goto drop;
+  }
+
+  if (dst->addr == 0) {
+    dbg("IP destination addr error\n");
+    pico_err = PICO_ERR_EINVAL;
+    goto drop;
+  }
+
+  route = route_find(dst);
+  if (!route) {
+    dbg("Route to %08x not found.\n", long_be(dst->addr));
+    pico_err = PICO_ERR_EHOSTUNREACH;
+    goto drop;
+  } else {
+    link = route->link;
+#ifdef PICO_SUPPORT_MCAST
+    if (pico_ipv4_is_multicast(dst->addr)) { /* if multicast */
+      switch (proto) {
+        case PICO_PROTO_UDP:
+          if(pico_udp_get_mc_ttl(f->sock, &ttl) < 0)
+            ttl = PICO_IP_DEFAULT_MULTICAST_TTL;
+          break;
+        case PICO_PROTO_IGMP:
+          vhl = 0x46; /* header length 24 */
+          ttl = 1;
+          /* router alert (RFC 2113) */ 
+          hdr->options[0] = 0x94;
+          hdr->options[1] = 0x04;
+          hdr->options[2] = 0x00;
+          hdr->options[3] = 0x00;
+          if (f->dev && link->dev != f->dev) { /* default link is not requested link */
+            pico_tree_foreach(index, &Tree_dev_link) {
+              link = index->keyValue;
+              if (link->dev == f->dev)
+                break;
+            }
+          }
+          break;
+        default:
+          ttl = PICO_IPV4_DEFAULT_TTL;
+      }
+    }
+#endif
+  }
+
+  hdr->vhl = vhl;
+  hdr->len = short_be(f->transport_len + f->net_len);
+  if (f->transport_hdr != f->payload)
+    ipv4_progressive_id++;
+  hdr->id = short_be(ipv4_progressive_id);
+  hdr->dst.addr = dst->addr;
+  hdr->src.addr = link->address.addr;
+  hdr->ttl = ttl;
+  hdr->proto = proto;
+  hdr->frag = short_be(PICO_IPV4_DONTFRAG);
+#ifdef PICO_SUPPORT_IPFRAG
+#  ifdef PICO_SUPPORT_UDP
+  if (proto == PICO_PROTO_UDP) {
+    /* first fragment, can not use transport_len to calculate IP length */
+    if (f->transport_hdr != f->payload)
+      hdr->len = short_be(f->payload_len + sizeof(struct pico_udp_hdr) + f->net_len);
+    /* set fragmentation flags and offset calculated in socket layer */
+    hdr->frag = f->frag;
+  }
+#  endif /* PICO_SUPPORT_UDP */
+#endif /* PICO_SUPPORT_IPFRAG */
+  pico_ipv4_checksum(f);
+
+  if (f->sock && f->sock->dev){
+    //if the socket has its device set, use that (currently used for DHCP)
+    f->dev = f->sock->dev;
+  } else {
+    f->dev = link->dev;
+  }
+
+#ifdef PICO_SUPPORT_MCAST
+  if (pico_ipv4_is_multicast(hdr->dst.addr)) {
+    struct pico_frame *cpy;
+    /* Sending UDP multicast datagram, am I member? If so, loopback copy */
+    if ((proto != PICO_PROTO_IGMP) && (pico_ipv4_mcast_filter(f) == 0)) {
+      ip_mcast_dbg("MCAST: sender is member of group, loopback copy\n");
+      cpy = pico_frame_copy(f);
+      pico_enqueue(&in, cpy);
+    }
+  }
+#endif
+
+  if(pico_ipv4_link_get(&hdr->dst)){
+    //it's our own IP
+    return pico_enqueue(&in, f);
+  }else{
+    /* TODO: Check if there are members subscribed here */
+    return pico_enqueue(&out, f);
+  }
+
+drop:
+  pico_frame_discard(f);
+  return -1;
+}
+
+
+static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f)
+{
+	struct pico_ip4 *dst;
+  struct pico_remote_duple *remote_duple = (struct pico_remote_duple *) f->info;
+  IGNORE_PARAMETER(self);
+
+  if (!f->sock) {
+    pico_frame_discard(f);
+    return -1;
+  }
+
+  if (remote_duple) {
+    dst = &remote_duple->remote_addr.ip4;
+  } else {
+    dst = &f->sock->remote_addr.ip4;
+  }
+
+  return pico_ipv4_frame_push(f, dst, f->sock->proto->proto_number);
+}
+
+
+#ifdef DEBUG_ROUTE
+static void dbg_route(void)
+{
+  struct pico_ipv4_route *r;
+  struct pico_tree_node * index;
+  pico_tree_foreach(index,&Routes){
+    r = index->keyValue;
+    dbg("Route to %08x/%08x, gw %08x, dev: %s, metric: %d\n", r->dest.addr, r->netmask.addr, r->gateway.addr, r->link->dev->name, r->metric);
+  }
+}
+#else
+#define dbg_route() do{ }while(0)
+#endif
+
+int pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link)
+{
+  struct pico_ipv4_route test, *new;
+  test.dest.addr = address.addr;
+  test.netmask.addr = netmask.addr;
+  test.metric = metric;
+
+  if(pico_tree_findKey(&Routes,&test)){
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+  
+  new = pico_zalloc(sizeof(struct pico_ipv4_route));
+  if (!new) {
+    pico_err = PICO_ERR_ENOMEM;
+    return -1;
+  }
+  new->dest.addr = address.addr;
+  new->netmask.addr = netmask.addr;
+  new->gateway.addr = gateway.addr;
+  new->metric = metric;
+  if (gateway.addr == 0) {
+    /* No gateway provided, use the link */
+    new->link = link;
+  } else {
+    struct pico_ipv4_route *r = route_find(&gateway);
+    if (!r ) { /* Specified Gateway is unreachable */
+      pico_err = PICO_ERR_EHOSTUNREACH;
+      pico_free(new);
+      return -1;
+    }
+    if (r->gateway.addr) { /* Specified Gateway is not a neighbor */
+      pico_err = PICO_ERR_ENETUNREACH;
+      pico_free(new);
+      return -1;
+    }
+    new->link = r->link;
+  }
+  if (!new->link) {
+      pico_err = PICO_ERR_EINVAL;
+      pico_free(new);
+      return -1;
+  }
+
+  pico_tree_insert(&Routes,new);
+  dbg_route();
+  return 0;
+}
+
+int pico_ipv4_route_del(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link)
+{
+	struct pico_ipv4_route test, *found;
+	IGNORE_PARAMETER(gateway);
+
+	if (!link) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+  test.dest.addr = address.addr;
+  test.netmask.addr = netmask.addr;
+  test.metric = metric;
+
+  found = pico_tree_findKey(&Routes,&test);
+  if (found) {
+
+    pico_tree_delete(&Routes,found);
+    pico_free(found);
+
+    dbg_route();
+    return 0;
+  }
+  pico_err = PICO_ERR_EINVAL;
+  return -1;
+}
+
+
+int pico_ipv4_link_add(struct pico_device *dev, struct pico_ip4 address, struct pico_ip4 netmask)
+{
+  struct pico_ipv4_link test, *new;
+  struct pico_ip4 network, gateway;
+  char ipstr[30];
+
+  if(!dev) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+  test.address.addr = address.addr;
+  test.netmask.addr = netmask.addr;
+  test.dev = dev;
+  /** XXX: Valid netmask / unicast address test **/
+
+  if(pico_tree_findKey(&Tree_dev_link, &test)) {
+    dbg("IPv4: Trying to assign an invalid address (in use)\n");
+    pico_err = PICO_ERR_EADDRINUSE;
+    return -1;
+  }
+
+  /** XXX: Check for network already in use (e.g. trying to assign 10.0.0.1/24 where 10.1.0.1/8 is in use) **/
+  new = pico_zalloc(sizeof(struct pico_ipv4_link));
+  if (!new) {
+    dbg("IPv4: Out of memory!\n");
+    pico_err = PICO_ERR_ENOMEM;
+    return -1;
+  }
+  new->address.addr = address.addr;
+  new->netmask.addr = netmask.addr;
+  new->dev = dev;
+#ifdef PICO_SUPPORT_MCAST
+  new->MCASTGroups = pico_zalloc(sizeof(struct pico_tree));
+  if (!new->MCASTGroups) {
+    pico_free(new);
+    dbg("IPv4: Out of memory!\n");
+    pico_err = PICO_ERR_ENOMEM;
+    return -1;
+  }
+
+  new->MCASTGroups->root = &LEAF;
+  new->MCASTGroups->compare = ipv4_mcast_groups_cmp;
+  new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */
+  new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL;
+#endif
+
+  pico_tree_insert(&Tree_dev_link, new);
+#ifdef PICO_SUPPORT_MCAST
+  do {
+    struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
+    if (!mcast_default_link) {
+      mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
+      mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
+      mcast_gw.addr = long_be(0x00000000);
+      mcast_default_link = new;
+      pico_ipv4_route_add(mcast_addr, mcast_nm, mcast_gw, 1, new);
+    }
+    mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
+    pico_ipv4_mcast_join(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
+  } while(0);
+#endif
+
+  network.addr = address.addr & netmask.addr;
+  gateway.addr = 0U;
+  pico_ipv4_route_add(network, netmask, gateway, 1, new);
+  pico_ipv4_to_string(ipstr, new->address.addr);
+  dbg("Assigned ipv4 %s to device %s\n", ipstr, new->dev->name);
+  return 0;
+}
+
+
+int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address)
+{
+  struct pico_ipv4_link test, *found;
+  struct pico_ip4 network;
+
+  if(!dev) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+  test.address.addr = address.addr;
+  test.dev = dev;
+  found = pico_tree_findKey(&Tree_dev_link, &test);
+  if (!found) {
+    pico_err = PICO_ERR_ENXIO;
+    return -1;
+  }
+
+  network.addr = found->address.addr & found->netmask.addr;
+  pico_ipv4_route_del(network, found->netmask,pico_ipv4_route_get_gateway(&found->address), 1, found);
+#ifdef PICO_SUPPORT_MCAST
+  do {
+    struct pico_ip4 mcast_all_hosts, mcast_addr, mcast_nm, mcast_gw;
+    struct pico_mcast_group *g = NULL;
+    struct pico_tree_node * index, * _tmp;
+    if (found == mcast_default_link) {
+      mcast_addr.addr = long_be(0xE0000000); /* 224.0.0.0 */
+      mcast_nm.addr = long_be(0xF0000000); /* 15.0.0.0 */
+      mcast_gw.addr = long_be(0x00000000);
+      mcast_default_link = NULL;
+      pico_ipv4_route_del(mcast_addr, mcast_nm, mcast_gw, 1, found);
+    }
+    mcast_all_hosts.addr = PICO_MCAST_ALL_HOSTS;
+    pico_ipv4_mcast_leave(&address, &mcast_all_hosts, 1, PICO_IP_MULTICAST_EXCLUDE, NULL);
+    pico_tree_foreach_safe(index,found->MCASTGroups, _tmp)
+    {
+      g = index->keyValue;
+      pico_tree_delete(found->MCASTGroups, g);
+      pico_free(g);
+    }
+  } while(0);
+#endif
+
+  pico_tree_delete(&Tree_dev_link, found);
+  /* XXX: pico_free(found); */
+  /* XXX: cleanup all routes containing the removed link */
+  return 0;
+}
+
+
+struct pico_ipv4_link *pico_ipv4_link_get(struct pico_ip4 *address)
+{
+  struct pico_ipv4_link test = {0}, *found = NULL;
+  test.address.addr = address->addr;
+
+  found = pico_tree_findKey(&Tree_dev_link, &test);
+  if (!found)
+    return NULL;
+  else
+    return found;
+}
+
+struct pico_ipv4_link *pico_ipv4_link_by_dev(struct pico_device *dev)
+{
+  struct pico_tree_node *index = NULL;
+  struct pico_ipv4_link *link = NULL;
+
+  pico_tree_foreach(index, &Tree_dev_link) 
+  {
+    link = index->keyValue;
+    if (link->dev == dev)
+      return link;
+  }
+  return NULL;
+}
+
+
+struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address)
+{
+  struct pico_ipv4_link test, *found;
+  if(!address) {
+    pico_err = PICO_ERR_EINVAL;
+    return NULL;
+  }
+  test.dev = NULL;
+  test.address.addr = address->addr;
+  found = pico_tree_findKey(&Tree_dev_link, &test);
+  if (!found) {
+    pico_err = PICO_ERR_ENXIO;
+    return NULL;
+  }
+  return found->dev;
+}
+
+int pico_ipv4_rebound(struct pico_frame *f)
+{
+  struct pico_ip4 dst;
+  struct pico_ipv4_hdr *hdr;
+  if(!f) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+
+  hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+  if (!hdr) {
+    pico_err = PICO_ERR_EINVAL;
+    return -1;
+  }
+  dst.addr = hdr->src.addr;
+  return pico_ipv4_frame_push(f, &dst, hdr->proto);
+}
+
+static int pico_ipv4_forward(struct pico_frame *f)
+{
+  struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+  struct pico_ipv4_route *rt;
+  if (!hdr) {
+    return -1;
+  }
+
+  rt = route_find(&hdr->dst);
+  if (!rt) {
+    pico_notify_dest_unreachable(f);
+    return -1;
+  }
+
+  f->dev = rt->link->dev;
+  hdr->ttl-=1;
+  if (hdr->ttl < 1) {
+    pico_notify_ttl_expired(f);
+    return -1;
+  }
+  hdr->crc++;
+
+  pico_ipv4_nat_outbound(f, &rt->link->address);
+
+  f->start = f->net_hdr;
+  if(f->dev->eth != NULL)
+    f->len -= PICO_SIZE_ETHHDR;
+  pico_sendto_dev(f);
+  return 0;
+
+}
+
+int pico_ipv4_is_broadcast(uint32_t addr)
+{
+  struct pico_ipv4_link *link;
+  struct pico_tree_node * index;
+  if (addr == PICO_IP4_ANY)
+    return 1;
+  if (addr == PICO_IP4_BCAST)
+    return 1;
+
+  pico_tree_foreach(index,&Tree_dev_link) {
+    link = index->keyValue;
+    if ((link->address.addr | (~link->netmask.addr)) == addr)
+      return 1;
+  }
+  return 0;
+}
+
+void pico_ipv4_unreachable(struct pico_frame *f, int err)
+{
+  struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+#if defined PICO_SUPPORT_TCP || defined PICO_SUPPORT_UDP
+  f->transport_hdr = ((uint8_t *)f->net_hdr) + PICO_SIZE_IP4HDR;
+  pico_transport_error(f, hdr->proto, err);
+#endif
+}
+
+#endif