Diff: stack/pico_stack.c

Revision:

152:a3d286bf94e5

Parent:

150:551effcf6a39

Child:

154:6c0e92a80c4a

--- a/stack/pico_stack.c	Wed Apr 09 17:32:25 2014 +0200
+++ b/stack/pico_stack.c	Mon Sep 28 13:16:18 2015 +0200
@@ -1,5 +1,5 @@
 /*********************************************************************
-   PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
+   PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved.
    See LICENSE and COPYING for usage.
 
    .
@@ -16,6 +16,8 @@
 #include "pico_addressing.h"
 #include "pico_dns_client.h"
 
+#include "pico_olsr.h"
+#include "pico_aodv.h"
 #include "pico_eth.h"
 #include "pico_arp.h"
 #include "pico_ipv4.h"
@@ -52,7 +54,7 @@
 
 static uint32_t _rand_seed;
 
-void pico_rand_feed(uint32_t feed)
+void WEAK pico_rand_feed(uint32_t feed)
 {
     if (!feed)
         return;
@@ -62,12 +64,26 @@
     _rand_seed ^= ~(feed);
 }
 
-uint32_t pico_rand(void)
+uint32_t WEAK pico_rand(void)
 {
     pico_rand_feed((uint32_t)pico_tick);
     return _rand_seed;
 }
 
+void pico_to_lowercase(char *str)
+{
+    int i = 0;
+    if (!str)
+        return;
+
+    while(str[i]) {
+        if ((str[i] <= 'Z') && (str[i] >= 'A'))
+            str[i] = (char) (str[i] - (char)('A' - 'a'));
+
+        i++;
+    }
+}
+
 /* NOTIFICATIONS: distributed notifications for stack internal errors.
  */
 
@@ -140,9 +156,43 @@
     return 0;
 }
 
+int pico_notify_frag_expired(struct pico_frame *f)
+{
+    if (0) {}
+
+#ifdef PICO_SUPPORT_ICMP4
+    else if (IS_IPV4(f)) {
+        pico_icmp4_frag_expired(f);
+    }
+#endif
+#ifdef PICO_SUPPORT_ICMP6
+    else if (IS_IPV6(f)) {
+        pico_icmp6_frag_expired(f);
+    }
+#endif
+    return 0;
+}
+
+int pico_notify_pkt_too_big(struct pico_frame *f)
+{
+    if (0) {}
+
+#ifdef PICO_SUPPORT_ICMP4
+    else if (IS_IPV4(f)) {
+        pico_icmp4_mtu_exceeded(f);
+    }
+#endif
+#ifdef PICO_SUPPORT_ICMP6
+    else if (IS_IPV6(f)) {
+        pico_icmp6_pkt_too_big(f);
+    }
+#endif
+    return 0;
+}
+
 
 /* Transport layer */
-int32_t pico_transport_receive(struct pico_frame *f, uint8_t proto)
+MOCKABLE int32_t pico_transport_receive(struct pico_frame *f, uint8_t proto)
 {
     int32_t ret = -1;
     switch (proto) {
@@ -160,7 +210,7 @@
 #endif
 
 
-#ifdef PICO_SUPPORT_IGMP
+#if defined(PICO_SUPPORT_IGMP) && defined(PICO_SUPPORT_MCAST)
     case PICO_PROTO_IGMP:
         ret = pico_enqueue(pico_proto_igmp.q_in, f);
         break;
@@ -210,7 +260,6 @@
     return (int32_t)f->buffer_len;
 }
 
-
 /* Network layer: interface towards socket for frame sending */
 int32_t pico_network_send(struct pico_frame *f)
 {
@@ -238,15 +287,15 @@
 #endif
 #ifdef PICO_SUPPORT_IPV6
     else if (IS_IPV6(f)) {
-        /* XXX */
+        struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr;
+        if (pico_ipv6_is_unspecified(hdr->src.addr) || pico_ipv6_link_find(&hdr->src))
+            return 1;
     }
 #endif
     return 0;
-
-
 }
 
-
+#ifdef PICO_SUPPORT_ETH
 /* DATALINK LEVEL: interface from network to the device
  * and vice versa.
  */
@@ -256,57 +305,6 @@
  * into the stack.
  */
 
-static int32_t pico_ll_receive(struct pico_frame *f)
-{
-    struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr;
-    f->net_hdr = f->datalink_hdr + sizeof(struct pico_eth_hdr);
-
-#if (defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_ETH)
-    if (hdr->proto == PICO_IDETH_ARP)
-        return pico_arp_receive(f);
-#endif
-#if defined (PICO_SUPPORT_IPV4) || defined (PICO_SUPPORT_IPV6)
-    if ((hdr->proto == PICO_IDETH_IPV4) || (hdr->proto == PICO_IDETH_IPV6))
-        return pico_network_receive(f);
-#endif
-    pico_frame_discard(f);
-    return -1;
-}
-
-static void pico_ll_check_bcast(struct pico_frame *f)
-{
-    struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr;
-    /* Indicate a link layer broadcast packet */
-    if (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) == 0)
-        f->flags |= PICO_FRAME_FLAG_BCAST;
-}
-
-int32_t pico_ethernet_receive(struct pico_frame *f)
-{
-    struct pico_eth_hdr *hdr;
-    if (!f || !f->dev || !f->datalink_hdr)
-    {
-        pico_frame_discard(f);
-        return -1;
-    }
-
-    hdr = (struct pico_eth_hdr *) f->datalink_hdr;
-    if ((memcmp(hdr->daddr, f->dev->eth->mac.addr, PICO_SIZE_ETH) != 0) &&
-        (memcmp(hdr->daddr, PICO_ETHADDR_MCAST, PICO_SIZE_MCAST) != 0) &&
-#ifdef PICO_SUPPORT_IPV6
-        (memcmp(hdr->daddr, PICO_ETHADDR_MCAST6, PICO_SIZE_MCAST6) != 0) &&
-#endif
-        (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) != 0))
-    {
-        pico_frame_discard(f);
-        return -1;
-    }
-
-    pico_ll_check_bcast(f);
-
-    return pico_ll_receive(f);
-}
-
 static int destination_is_bcast(struct pico_frame *f)
 {
     if (!f)
@@ -348,6 +346,96 @@
     return ret;
 }
 
+#ifdef PICO_SUPPORT_IPV4
+static int32_t pico_ipv4_ethernet_receive(struct pico_frame *f)
+{
+    if (IS_IPV4(f)) {
+        pico_enqueue(pico_proto_ipv4.q_in, f);
+    } else {
+        (void)pico_icmp4_param_problem(f, 0);
+        pico_frame_discard(f);
+        return -1;
+    }
+
+    return (int32_t)f->buffer_len;
+}
+#endif
+
+#ifdef PICO_SUPPORT_IPV6
+static int32_t pico_ipv6_ethernet_receive(struct pico_frame *f)
+{
+    if (IS_IPV6(f)) {
+        pico_enqueue(pico_proto_ipv6.q_in, f);
+    } else {
+        /* Wrong version for link layer type */
+        pico_frame_discard(f);
+        return -1;
+    }
+
+    return (int32_t)f->buffer_len;
+}
+#endif
+
+static int32_t pico_ll_receive(struct pico_frame *f)
+{
+    struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr;
+    f->net_hdr = f->datalink_hdr + sizeof(struct pico_eth_hdr);
+
+#if (defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_ETH)
+    if (hdr->proto == PICO_IDETH_ARP)
+        return pico_arp_receive(f);
+
+#endif
+
+#if defined (PICO_SUPPORT_IPV4)
+    if (hdr->proto == PICO_IDETH_IPV4)
+        return pico_ipv4_ethernet_receive(f);
+
+#endif
+
+#if defined (PICO_SUPPORT_IPV6)
+    if (hdr->proto == PICO_IDETH_IPV6)
+        return pico_ipv6_ethernet_receive(f);
+
+#endif
+
+    pico_frame_discard(f);
+    return -1;
+}
+
+static void pico_ll_check_bcast(struct pico_frame *f)
+{
+    struct pico_eth_hdr *hdr = (struct pico_eth_hdr *) f->datalink_hdr;
+    /* Indicate a link layer broadcast packet */
+    if (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) == 0)
+        f->flags |= PICO_FRAME_FLAG_BCAST;
+}
+
+int32_t pico_ethernet_receive(struct pico_frame *f)
+{
+    struct pico_eth_hdr *hdr;
+    if (!f || !f->dev || !f->datalink_hdr)
+    {
+        pico_frame_discard(f);
+        return -1;
+    }
+
+    hdr = (struct pico_eth_hdr *) f->datalink_hdr;
+    if ((memcmp(hdr->daddr, f->dev->eth->mac.addr, PICO_SIZE_ETH) != 0) &&
+        (memcmp(hdr->daddr, PICO_ETHADDR_MCAST, PICO_SIZE_MCAST) != 0) &&
+#ifdef PICO_SUPPORT_IPV6
+        (memcmp(hdr->daddr, PICO_ETHADDR_MCAST6, PICO_SIZE_MCAST6) != 0) &&
+#endif
+        (memcmp(hdr->daddr, PICO_ETHADDR_ALL, PICO_SIZE_ETH) != 0))
+    {
+        pico_frame_discard(f);
+        return -1;
+    }
+
+    pico_ll_check_bcast(f);
+    return pico_ll_receive(f);
+}
+
 static struct pico_eth *pico_ethernet_mcast_translate(struct pico_frame *f, uint8_t *pico_mcast_mac)
 {
     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
@@ -376,121 +464,181 @@
 }
 #endif
 
-struct pico_eth *pico_ethernet_ipv6_dst(struct pico_frame *f)
+static int pico_ethernet_ipv6_dst(struct pico_frame *f, struct pico_eth *const dstmac)
 {
-    struct pico_eth *dstmac = NULL;
+    int retval = -1;
+    if (!dstmac)
+        return -1;
+
     #ifdef PICO_SUPPORT_IPV6
     if (destination_is_mcast(f)) {
         uint8_t pico_mcast6_mac[6] = {
             0x33, 0x33, 0x00, 0x00, 0x00, 0x00
         };
-        dstmac = pico_ethernet_mcast6_translate(f, pico_mcast6_mac);
+        pico_ethernet_mcast6_translate(f, pico_mcast6_mac);
+        memcpy(dstmac, pico_mcast6_mac, PICO_SIZE_ETH);
+        retval = 0;
     } else {
-        dstmac = pico_nd_get(f);
+        struct pico_eth *neighbor = pico_ipv6_get_neighbor(f);
+        if (neighbor)
+        {
+            memcpy(dstmac, neighbor, PICO_SIZE_ETH);
+            retval = 0;
+        }
     }
 
     #else
     (void)f;
     pico_err = PICO_ERR_EPROTONOSUPPORT;
     #endif
-    return dstmac;
+    return retval;
 }
 
 
+/* Ethernet send, first attempt: try our own address.
+ * Returns 0 if the packet is not for us.
+ * Returns 1 if the packet is cloned to our own receive queue, so the caller can discard the original frame.
+ * */
+static int32_t pico_ethsend_local(struct pico_frame *f, struct pico_eth_hdr *hdr)
+{
+    if (!hdr)
+        return 0;
 
-/* This is called by dev loop in order to ensure correct ethernet addressing.
- * Returns 0 if the destination is unknown, and -1 if the packet is not deliverable
- * due to ethernet addressing (i.e., no arp association was possible.
- *
- * Only IP packets must pass by this. ARP will always use direct dev->send() function, so
- * we assume IP is used.
- */
-
-static int32_t pico_ethsend_local(struct pico_frame *f, struct pico_eth_hdr *hdr, int *ret)
-{
     /* Check own mac */
     if(!memcmp(hdr->daddr, hdr->saddr, PICO_SIZE_ETH)) {
+        struct pico_frame *clone = pico_frame_copy(f);
         dbg("sending out packet destined for our own mac\n");
-        *ret = (int32_t)pico_ethernet_receive(f);
+        (void)pico_ethernet_receive(clone);
         return 1;
     }
 
     return 0;
 }
 
-static int32_t pico_ethsend_bcast(struct pico_frame *f, int *ret)
+/* Ethernet send, second attempt: try bcast.
+ * Returns 0 if the packet is not bcast, so it will be handled somewhere else.
+ * Returns 1 if the packet is handled by the pico_device_broadcast() function, so it can be discarded.
+ * */
+static int32_t pico_ethsend_bcast(struct pico_frame *f)
 {
     if (IS_LIMITED_BCAST(f)) {
-        *ret = pico_device_broadcast(f);
+        (void)pico_device_broadcast(f); /* We can discard broadcast even if it's not sent. */
         return 1;
     }
 
     return 0;
 }
 
-static int32_t pico_ethsend_dispatch(struct pico_frame *f, int *ret)
+/* Ethernet send, third attempt: try unicast.
+ * If the device driver is busy, we return 0, so the stack won't discard the frame.
+ * In case of success, we can safely return 1.
+ */
+static int32_t pico_ethsend_dispatch(struct pico_frame *f)
 {
-    *ret = f->dev->send(f->dev, f->start, (int) f->len);
-    if (*ret <= 0)
-        return 0;
+    int ret = f->dev->send(f->dev, f->start, (int) f->len);
+    if (ret <= 0)
+        return 0; /* Failure to deliver! */
     else {
-        pico_frame_discard(f);
-        return 1;
+        return 1; /* Frame is in flight by now. */
     }
 }
 
-int32_t pico_ethernet_send(struct pico_frame *f)
+
+
+
+/* This function looks for the destination mac address
+ * in order to send the frame being processed.
+ */
+
+int32_t MOCKABLE pico_ethernet_send(struct pico_frame *f)
 {
-    const struct pico_eth *dstmac = NULL;
-    int32_t ret = -1;
+    struct pico_eth dstmac;
+    uint8_t dstmac_valid = 0;
     uint16_t proto = PICO_IDETH_IPV4;
 
+#ifdef PICO_SUPPORT_IPV6
+    /* Step 1: If the frame has an IPv6 packet,
+     * destination address is taken from the ND tables
+     */
     if (IS_IPV6(f)) {
-        dstmac = pico_ethernet_ipv6_dst(f);
-        if (!dstmac)
-            return 0;
+        if (pico_ethernet_ipv6_dst(f, &dstmac) < 0)
+        {
+            pico_ipv6_nd_postpone(f);
+            return 0; /* I don't care if frame was actually postponed. If there is no room in the ND table, discard safely. */
+        }
 
+        dstmac_valid = 1;
         proto = PICO_IDETH_IPV6;
     }
+    else
+#endif
 
-    else if (IS_BCAST(f) || destination_is_bcast(f))
-        dstmac = (const struct pico_eth *) PICO_ETHADDR_ALL;
+    /* In case of broadcast (IPV4 only), dst mac is FF:FF:... */
+    if (IS_BCAST(f) || destination_is_bcast(f))
+    {
+        memcpy(&dstmac, PICO_ETHADDR_ALL, PICO_SIZE_ETH);
+        dstmac_valid = 1;
+    }
 
+    /* In case of multicast, dst mac is translated from the group address */
     else if (destination_is_mcast(f)) {
         uint8_t pico_mcast_mac[6] = {
             0x01, 0x00, 0x5e, 0x00, 0x00, 0x00
         };
-        dstmac = pico_ethernet_mcast_translate(f, pico_mcast_mac);
+        pico_ethernet_mcast_translate(f, pico_mcast_mac);
+        memcpy(&dstmac, pico_mcast_mac, PICO_SIZE_ETH);
+        dstmac_valid = 1;
     }
+
+#if (defined PICO_SUPPORT_IPV4)
     else {
-#if (defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_ETH)
-        dstmac = pico_arp_get(f);
-        if (!dstmac)
+        struct pico_eth *arp_get;
+        arp_get = pico_arp_get(f);
+        if (arp_get) {
+            memcpy(&dstmac, arp_get, PICO_SIZE_ETH);
+            dstmac_valid = 1;
+        } else {
+            /* At this point, ARP will discard the frame in any case.
+             * It is safe to return without discarding.
+             */
+            pico_arp_postpone(f);
+            return 0;
+            /* Same case as for IPv6 ... */
+        }
+
+    }
 #endif
-        return 0;
-    }
 
     /* This sets destination and source address, then pushes the packet to the device. */
-    if (dstmac && (f->start > f->buffer) && ((f->start - f->buffer) >= PICO_SIZE_ETHHDR)) {
+    if (dstmac_valid) {
         struct pico_eth_hdr *hdr;
-        f->start -= PICO_SIZE_ETHHDR;
-        f->len += PICO_SIZE_ETHHDR;
-        f->datalink_hdr = f->start;
         hdr = (struct pico_eth_hdr *) f->datalink_hdr;
-        memcpy(hdr->saddr, f->dev->eth->mac.addr, PICO_SIZE_ETH);
-        memcpy(hdr->daddr, dstmac, PICO_SIZE_ETH);
-        hdr->proto = proto;
+        if ((f->start > f->buffer) && ((f->start - f->buffer) >= PICO_SIZE_ETHHDR))
+        {
+            f->start -= PICO_SIZE_ETHHDR;
+            f->len += PICO_SIZE_ETHHDR;
+            f->datalink_hdr = f->start;
+            hdr = (struct pico_eth_hdr *) f->datalink_hdr;
+            memcpy(hdr->saddr, f->dev->eth->mac.addr, PICO_SIZE_ETH);
+            memcpy(hdr->daddr, &dstmac, PICO_SIZE_ETH);
+            hdr->proto = proto;
+        }
 
-        if (pico_ethsend_local(f, hdr, &ret) || pico_ethsend_bcast(f, &ret) || pico_ethsend_dispatch(f, &ret)) {
-            return ret;
-        } else {
-            return -1;
+        if (pico_ethsend_local(f, hdr) || pico_ethsend_bcast(f) || pico_ethsend_dispatch(f)) {
+            /* one of the above functions has delivered the frame accordingly. (returned != 0)
+             * It is safe to directly return success.
+             * */
+            return 0;
         }
     }
 
+    /* Failure: do not dequeue the frame, keep it for later. */
     return -1;
 }
 
+#endif /* PICO_SUPPORT_ETH */
+
+
 void pico_store_network_origin(void *src, struct pico_frame *f)
 {
   #ifdef PICO_SUPPORT_IPV4
@@ -521,6 +669,54 @@
   #endif
 }
 
+int pico_address_compare(union pico_address *a, union pico_address *b, uint16_t proto)
+{
+    #ifdef PICO_SUPPORT_IPV6
+    if (proto == PICO_PROTO_IPV6) {
+        return pico_ipv6_compare(&a->ip6, &b->ip6);
+    }
+
+    #endif
+    #ifdef PICO_SUPPORT_IPV4
+    if (proto == PICO_PROTO_IPV4) {
+        return pico_ipv4_compare(&a->ip4, &b->ip4);
+    }
+
+    #endif
+    return 0;
+
+}
+
+int pico_frame_dst_is_unicast(struct pico_frame *f)
+{
+    if (0) {
+        return 0;
+    }
+
+#ifdef PICO_SUPPORT_IPV4
+    if (IS_IPV4(f)) {
+        struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+        if (pico_ipv4_is_multicast(hdr->dst.addr) || pico_ipv4_is_broadcast(hdr->dst.addr))
+            return 0;
+
+        return 1;
+    }
+
+#endif
+
+#ifdef PICO_SUPPORT_IPV6
+    if (IS_IPV6(f)) {
+        struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr;
+        if (pico_ipv6_is_multicast(hdr->dst.addr) || pico_ipv6_is_unspecified(hdr->dst.addr))
+            return 0;
+
+        return 1;
+    }
+
+#endif
+    else return 0;
+}
+
 
 /* LOWEST LEVEL: interface towards devices. */
 /* Device driver will call this function which returns immediately.
@@ -547,9 +743,10 @@
     f->start = f->buffer;
     f->len = f->buffer_len;
     if (f->len > 8) {
-        uint32_t mid_frame = (f->buffer_len >> 2) << 1;
+        uint32_t rand, mid_frame = (f->buffer_len >> 2) << 1;
         mid_frame -= (mid_frame % 4);
-        pico_rand_feed(*(uint32_t*)(f->buffer + mid_frame));
+        memcpy(&rand, f->buffer + mid_frame, sizeof(uint32_t));
+        pico_rand_feed(rand);
     }
 
     memcpy(f->buffer, buffer, len);
@@ -561,14 +758,14 @@
     return ret;
 }
 
-int32_t pico_stack_recv_zerocopy(struct pico_device *dev, uint8_t *buffer, uint32_t len)
+static int32_t _pico_stack_recv_zerocopy(struct pico_device *dev, uint8_t *buffer, uint32_t len, int ext_buffer, void (*notify_free)(uint8_t *))
 {
     struct pico_frame *f;
     int ret;
     if (len <= 0)
         return -1;
 
-    f = pico_frame_alloc_skeleton(len);
+    f = pico_frame_alloc_skeleton(len, ext_buffer);
     if (!f)
     {
         dbg("Cannot alloc incoming frame!\n");
@@ -583,6 +780,10 @@
         return -1;
     }
 
+    if (notify_free) {
+        f->notify_free = notify_free;
+    }
+
     f->dev = dev;
     ret = pico_enqueue(dev->q_in, f);
     if (ret <= 0) {
@@ -592,6 +793,21 @@
     return ret;
 }
 
+int32_t pico_stack_recv_zerocopy(struct pico_device *dev, uint8_t *buffer, uint32_t len)
+{
+    return _pico_stack_recv_zerocopy(dev, buffer, len, 0, NULL);
+}
+
+int32_t pico_stack_recv_zerocopy_ext_buffer(struct pico_device *dev, uint8_t *buffer, uint32_t len)
+{
+    return _pico_stack_recv_zerocopy(dev, buffer, len, 1, NULL);
+}
+
+int32_t pico_stack_recv_zerocopy_ext_buffer_notify(struct pico_device *dev, uint8_t *buffer, uint32_t len, void (*notify_free)(uint8_t *buffer))
+{
+    return _pico_stack_recv_zerocopy(dev, buffer, len, 1, notify_free);
+}
+
 int32_t pico_sendto_dev(struct pico_frame *f)
 {
     if (!f->dev) {
@@ -599,9 +815,10 @@
         return -1;
     } else {
         if (f->len > 8) {
-            uint32_t mid_frame = (f->buffer_len >> 2) << 1;
+            uint32_t rand, mid_frame = (f->buffer_len >> 2) << 1;
             mid_frame -= (mid_frame % 4);
-            pico_rand_feed(*(uint32_t*)(f->buffer + mid_frame));
+            memcpy(&rand, f->buffer + mid_frame, sizeof(uint32_t));
+            pico_rand_feed(rand);
         }
 
         return pico_enqueue(f->dev->q_out, f);
@@ -626,7 +843,32 @@
 
 static heap_pico_timer_ref *Timers;
 
-void pico_check_timers(void)
+int32_t pico_seq_compare(uint32_t a, uint32_t b)
+{
+    uint32_t thresh = ((uint32_t)(-1)) >> 1;
+
+    if (a > b) /* return positive number, if not wrapped */
+    {
+        if ((a - b) > thresh) /* b wrapped */
+            return -(int32_t)(b - a); /* b = very small,     a = very big      */
+        else
+            return (int32_t)(a - b); /* a = biggest,        b = a bit smaller */
+
+    }
+
+    if (a < b) /* return negative number, if not wrapped */
+    {
+        if ((b - a) > thresh) /* a wrapped */
+            return (int32_t)(a - b); /* a = very small,     b = very big      */
+        else
+            return -(int32_t)(b - a); /* b = biggest,        a = a bit smaller */
+
+    }
+
+    return 0;
+}
+
+static void pico_check_timers(void)
 {
     struct pico_timer *t;
     struct pico_timer_ref tref_unused, *tref = heap_first(Timers);
@@ -637,7 +879,9 @@
             t->timer(pico_tick, t->arg);
 
         if (t)
+        {
             PICO_FREE(t);
+        }
 
         t = NULL;
         heap_peek(Timers, &tref_unused);
@@ -645,7 +889,7 @@
     }
 }
 
-void pico_timer_cancel(struct pico_timer *t)
+void MOCKABLE pico_timer_cancel(struct pico_timer *t)
 {
     uint32_t i;
     struct pico_timer_ref *tref = Timers->top;
@@ -666,7 +910,7 @@
 #define PROTO_DEF_SCORE   32
 #define PROTO_MIN_SCORE   32
 #define PROTO_MAX_SCORE   128
-#define PROTO_LAT_IND     3   /* latecy indication 0-3 (lower is better latency performance), x1, x2, x4, x8 */
+#define PROTO_LAT_IND     3   /* latency indication 0-3 (lower is better latency performance), x1, x2, x4, x8 */
 #define PROTO_MAX_LOOP    (PROTO_MAX_SCORE << PROTO_LAT_IND) /* max global loop score, so per tick */
 
 static int calc_score(int *score, int *index, int avg[][PROTO_DEF_AVG_NR], int *ret)
@@ -754,41 +998,6 @@
     return 0;
 }
 
-
-
-/*
-
-         .
-       .vS.
-     <aoSo.
-    .XoS22.
-    .S2S22.             ._...              ......            ..._.
-   :=|2S2X2|=++;      <vSX2XX2z+          |vSSSXSSs>.      :iXXZUZXXe=
-   )2SS2SS2S2S2I    =oS2S2S2S2X22;.    _vuXS22S2S2S22i  ._wZZXZZZXZZXZX=
-   )22S2S2S2S2Sl    |S2S2S22S2SSSXc:  .S2SS2S2S22S2SS= .]#XZZZXZXZZZZZZ:
-   )oSS2SS2S2Sol     |2}!"""!32S22S(. uS2S2Se**12oS2e  ]dXZZXX2?YYXXXZ*
-   .:2S2So:..-.      .      :]S2S2e;=X2SS2o     .)oc  ]XZZXZ(     =nX:
-    .S2S22.          ___s_i,.)oS2So(;2SS2So,       `  3XZZZZc,      -
-    .S2SSo.        =oXXXSSS2XoS2S2o( XS2S2XSos;.      ]ZZZZXXXX|=
-    .S2S22.      .)S2S2S22S2S2S2S2o( "X2SS2S2S2Sus,,  +3XZZZZZXZZoos_
-    .S2S22.     .]2S2SS22S222S2SS2o(  ]S22S2S2S222So   :3XXZZZZZZZZXXv
-    .S2S22.     =u2SS2e"~---"{2S2So(   -"12S2S2SSS2Su.   "?SXXXZXZZZZXo
-    .S2SSo.     )SS22z;      :S2S2o(       ={vS2S2S22v      .<vXZZZZZZZ;
-    .S2S2S:     ]oSS2c;      =22S2o(          -"S2SS2n          ~4XXZXZ(
-    .2S2S2i     )2S2S2[.    .)XS2So(  <;.      .2S2S2o :<.       ]XZZZX(
-     nX2S2S,,_s_=3oSS2SoaasuXXS2S2o( .oXoasi_aioSSS22l.]dZoaas_aadXZZXZ'
-     vS2SSSXXX2; )S2S2S2SoS2S2S2S2o( iS2S222XSoSS22So.)nXZZXXXZZXXZZXZo
-     x32S22S2Sn  -+S2S2S2S2So22S2So( 12S2SS2S2SS22S}- )SXXZZZZZZZZZXX!-
-      .)S22222i    .i2S2S2o>;:S2S2o(  .<vSoSoSo2S(;     :nXXXXXZXXX(
-       .-~~~~-        --- .   - -        --~~~--           --^^~~-
-                                  .
-
-
-   ... curious about our source code? We are hiring! mailto:<recruiting@tass.be>
-
-
- */
-
 void pico_stack_tick(void)
 {
     static int score[PROTO_DEF_NR] = {
@@ -860,7 +1069,7 @@
     }
 }
 
-struct pico_timer *pico_timer_add(pico_time expire, void (*timer)(pico_time, void *), void *arg)
+MOCKABLE struct pico_timer *pico_timer_add(pico_time expire, void (*timer)(pico_time, void *), void *arg)
 {
     struct pico_timer *t = PICO_ZALLOC(sizeof(struct pico_timer));
     struct pico_timer_ref tref;
@@ -875,7 +1084,7 @@
     tref.tmr = t;
     heap_insert(Timers, &tref);
     if (Timers->n > PICO_MAX_TIMERS) {
-        dbg("Warning: I have %d timers\n", Timers->n);
+        dbg("Warning: I have %d timers\n", (int)Timers->n);
     }
 
     return t;
@@ -900,7 +1109,7 @@
     pico_protocol_init(&pico_proto_icmp6);
 #endif
 
-#ifdef PICO_SUPPORT_IGMP
+#if defined(PICO_SUPPORT_IGMP) && defined(PICO_SUPPORT_MCAST)
     pico_protocol_init(&pico_proto_igmp);
 #endif
 
@@ -932,6 +1141,14 @@
     /* Initialize Neighbor discovery module */
     pico_ipv6_nd_init();
 #endif
+
+#ifdef PICO_SUPPORT_OLSR
+    pico_olsr_init();
+#endif
+#ifdef PICO_SUPPORT_AODV
+    pico_aodv_init();
+#endif
+
     pico_stack_tick();
     pico_stack_tick();
     pico_stack_tick();