pico_olsr.c

/*********************************************************************
   PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved.
   See COPYING, LICENSE.GPLv2 and LICENSE.GPLv3 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"
#include "pico_olsr.h"
#ifdef PICO_SUPPORT_OLSR
#define DGRAM_MAX_SIZE (100 - 28)
#define MAX_OLSR_MEM (4 * DGRAM_MAX_SIZE)

#ifdef DEBUG_OLSR
#define olsr_dbg dbg
#else
#define olsr_dbg(...) do {} while(0)
#endif

int OOM(void);


#define OLSR_HELLO_INTERVAL   ((uint32_t)5000)
#define OLSR_TC_INTERVAL      ((uint32_t)9000)
#define OLSR_MAXJITTER        ((uint32_t)(OLSR_HELLO_INTERVAL >> 2))
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))


static uint16_t msg_counter; /* Global message sequence number */

/* Objects */
struct olsr_dev_entry
{
    struct olsr_dev_entry *next;
    struct pico_device *dev;
    uint16_t pkt_counter;
};


/* 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_PORT (short_be((uint16_t)698))


/* Headers */

PACKED_STRUCT_DEF olsr_link
{
    uint8_t link_code;
    uint8_t reserved;
    uint16_t link_msg_size;
};

PACKED_STRUCT_DEF olsr_neighbor
{
    uint32_t addr;
    uint8_t lq;
    uint8_t nlq;
    uint16_t reserved;
};

PACKED_STRUCT_DEF olsr_hmsg_hello
{
    uint16_t reserved;
    uint8_t htime;
    uint8_t willingness;
};

PACKED_STRUCT_DEF olsr_hmsg_tc
{
    uint16_t ansn;
    uint16_t reserved;
};


PACKED_STRUCT_DEF olsrmsg
{
    uint8_t type;
    uint8_t vtime;
    uint16_t size;
    struct pico_ip4 orig;
    uint8_t ttl;
    uint8_t hop;
    uint16_t seq;
};

PACKED_STRUCT_DEF olsrhdr
{
    uint16_t len;
    uint16_t seq;
};



/* Globals */
static struct pico_socket *udpsock = NULL;
uint16_t my_ansn = 0;
static struct olsr_route_entry  *Local_interfaces = NULL;
static struct olsr_dev_entry    *Local_devices    = NULL;

static struct olsr_dev_entry *olsr_get_deventry(struct pico_device *dev)
{
    struct olsr_dev_entry *cur = Local_devices;
    while(cur) {
        if (cur->dev == dev)
            return cur;

        cur = cur->next;
    }
    return 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) {
        /* olsr_dbg("Finding next hop to %08x m=%d\n", hop->destination.addr, hop->metric); */
        if(hop->metric <= 1)
            return hop;

        hop = hop->gateway;
    }
    return NULL;
}

static inline void olsr_route_add(struct olsr_route_entry *el)
{
    /* char dest[16],nxdest[16]; */
    struct olsr_route_entry *nexthop;

    if(!el)
        return;

    my_ansn++;

    nexthop = get_next_hop(el);
    if (el->gateway && nexthop && (nexthop->destination.addr != el->destination.addr)) {
        /* 2-hops route or more */
        el->next = el->gateway->children;
        el->gateway->children = el;
        el->link_type = OLSRLINK_MPR;
        olsr_dbg("[OLSR]  ----------Adding route to %08x via %08x metric %d\n", el->destination.addr, nexthop->destination.addr, el->metric);
        pico_ipv4_route_add(el->destination, HOST_NETMASK, nexthop->destination, (int) el->metric, NULL);
    } else if (el->iface) {
        /* neighbor */
        struct olsr_route_entry *ei = olsr_get_ethentry(el->iface);
        struct pico_ip4 no_gw = {
            0U
        };
        if (el->link_type == OLSRLINK_UNKNOWN)
            el->link_type = OLSRLINK_SYMMETRIC;

        if (ei) {
            el->next = ei->children;
            ei->children = el;
        }

        olsr_dbg("[OLSR]  ----------Adding neighbor %08x iface %s\n", el->destination.addr, el->iface->name);

        pico_ipv4_route_add(el->destination, HOST_NETMASK, no_gw, 1, pico_ipv4_link_by_dev(el->iface));
    }
}

static inline void olsr_route_del(struct olsr_route_entry *r)
{
    struct olsr_route_entry *cur, *prev = NULL, *lst;
    /* olsr_dbg("[OLSR] DELETING route..................\n"); */
    my_ansn++;
    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);
                olsr_dbg("[OLSR] Deleting route to %08x \n", r->destination.addr);
                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;
        }

        /* recursive function, could be dangerous for stack overflow if a lot of routes are available... */
        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

static uint8_t seconds2olsr(uint32_t seconds)
{
    uint16_t a, b;
    /* olsr_dbg("seconds=%u\n", (uint16_t)seconds); */

    if (seconds > 32767)
        seconds = 32767;

    /* find largest b such as seconds/C >= 2^b */
    for (b = 1; b <= 0x0fu; b++) {
        if ((uint16_t)(seconds * 16u) < (1u << b)) {
            b--;
            break;
        }
    }
    /* olsr_dbg("b=%u", b); */
    /* 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 = (T / ( C * (1u << b) ) ) - 1u; */
    {
        uint16_t den = ((uint16_t)(1u << b) >> 4u);
        /* olsr_dbg(" den=%u ", den); */
        if (den == 0)
        {
            /* olsr_dbg("div by 0!\n"); */
            den = 1u;
        }

        a = (uint16_t)(((uint16_t)seconds / den) - (uint16_t)1);
    }
    /* a = a & 0x0Fu; */

    /* olsr_dbg(" a=%u\n", a); */

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

static uint32_t olsr2seconds(uint8_t olsr)
{
    uint8_t a, b;
    uint16_t seconds;
    /* olsr_dbg("olsr format: %u -- ", olsr); */
    a = (olsr >> 4) & 0xFu;
    b = olsr & 0x0f;
    /* olsr_dbg("o2s: a=%u, b=%u\n", a,b); */
    if (b < 4)
        seconds = (uint16_t)(((1u << b) + (uint16_t)(((uint16_t)(a << b) >> 4u) & 0xFu)) >> OLSR_C_SHIFT);
    else
        seconds = (uint16_t)(((1u << b) + (uint16_t)(((uint16_t)(a << (b - 4))) & 0xFu)) >> OLSR_C_SHIFT);

    /* olsr_dbg("o2s: seconds: %u\n", seconds); */
    return seconds;
}

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;
    } else {
        /* sublist->time_left -= 2u; */
        sublist->time_left -= 8u;
    }

    olsr_garbage_collector(sublist->children);
    olsr_garbage_collector(sublist->next);
}

struct olsr_fwd_pkt
{
    void *buf;
    uint16_t len;
    struct pico_device *pdev;
};

static uint32_t buffer_mem_used = 0U;

static void olsr_process_out(pico_time now, void *arg)
{
    struct olsr_fwd_pkt *p = (struct olsr_fwd_pkt *)arg;
    struct pico_ip4 bcast;
    struct pico_ipv4_link *addr;
    struct olsr_dev_entry *pdev = Local_devices;
    struct olsrhdr *ohdr;
    (void)now;

    /* Send the thing out */
    ohdr = (struct olsrhdr *)p->buf;
    ohdr->len = short_be((uint16_t)p->len);

    if (p->pdev) {
        struct olsr_dev_entry *odev = olsr_get_deventry(p->pdev);
        if (!odev) {
            goto out_free;
        }

        addr = pico_ipv4_link_by_dev(p->pdev);
        if (!addr)
            goto out_free;

        ohdr->seq = short_be((uint16_t)(odev->pkt_counter)++);
        if (addr->address.addr)
            bcast.addr = (addr->netmask.addr & addr->address.addr) | (~addr->netmask.addr);
        else
            bcast.addr = 0xFFFFFFFFu;

        if ( 0 > pico_socket_sendto(udpsock, p->buf, p->len, &bcast, OLSR_PORT)) {
            olsr_dbg("olsr send\n");
        }
    } else {
        while(pdev) {
            ohdr->seq = short_be((uint16_t)(pdev->pkt_counter++));
            addr = pico_ipv4_link_by_dev(pdev->dev);
            if (!addr)
                continue;

            if (addr->address.addr)
                bcast.addr = (addr->netmask.addr & addr->address.addr) | (~addr->netmask.addr);
            else
                bcast.addr = 0xFFFFFFFFu;

            if ( 0 > pico_socket_sendto(udpsock, p->buf, p->len, &bcast, OLSR_PORT)) {
                olsr_dbg("olsr send\n");
            }

            pdev = pdev->next;
        }
    }

out_free:
    PICO_FREE(p->buf); /* XXX <-- broken? */
    buffer_mem_used -= DGRAM_MAX_SIZE;
    PICO_FREE(p);
}

static void olsr_scheduled_output(uint32_t when, void *buffer, uint16_t size, struct pico_device *pdev)
{
    struct olsr_fwd_pkt *p;
    /* olsr_dbg("Scheduling olsr packet, type:%s, size: %x\n", when == OLSR_HELLO_INTERVAL?"HELLO":"TC", size); */
    if ((buffer_mem_used + DGRAM_MAX_SIZE) > MAX_OLSR_MEM) {
        PICO_FREE(buffer);
        return;
    }

    p = PICO_ZALLOC(sizeof(struct olsr_fwd_pkt));
    if (!p) {
        OOM();
        PICO_FREE(buffer);
        return;
    }

    p->buf = buffer;
    p->len = size;
    p->pdev = pdev;
    buffer_mem_used += DGRAM_MAX_SIZE;
    if (!pico_timer_add(1 + when - ((pico_rand() % OLSR_MAXJITTER)), &olsr_process_out, p)) {
        olsr_dbg("OLSR: Failed to start process timer\n");
        OOM();
        PICO_FREE(p);
        PICO_FREE(buffer);
    }
}


static void refresh_routes(void)
{
    struct olsr_route_entry *local;
    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 = 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_HELLO_INTERVAL << 2);
            } else if (lnk) {
                struct olsr_route_entry *e = PICO_ZALLOC(sizeof (struct olsr_route_entry));
                if (!e) {
                    olsr_dbg("olsr: adding local route entry\n");
                    OOM();
                    return;
                }

                e->destination.addr = lnk->address.addr; /* Always pick the first address */
                e->time_left = (OLSR_HELLO_INTERVAL << 2);
                e->iface = icur->dev;
                e->metric = 0;
                e->lq = 0xFF;
                e->nlq = 0xFF;
                e->next = Local_interfaces;
                Local_interfaces = e;
            }
        } while (lnk);

        /* disabled if device type != eth */
        /* refresh_neighbors(icur->dev); */
        icur = icur->next;
    }
}

static uint32_t olsr_build_hello_neighbors(uint8_t *buf, uint32_t size, struct olsr_route_entry **bookmark)
{
    uint32_t ret = 0;
    struct olsr_route_entry *local, *neighbor;
    struct olsr_neighbor *dst = (struct olsr_neighbor *) buf;
    uint32_t total_link_size = sizeof(struct olsr_neighbor) + sizeof(struct olsr_link);
    local = Local_interfaces;
    while (local) {
        neighbor = local->children;
        if (*bookmark) {
            while ((neighbor) && *bookmark != neighbor)
                neighbor = neighbor->next;
        }

        while (neighbor) {
            struct olsr_link *li = (struct olsr_link *) (buf + ret);

            if ((size - ret) < total_link_size) {
                /* Incomplete list, new datagram needed. */
                *bookmark = neighbor;
                return ret;
            }

            li->link_code = neighbor->link_type;
            li->reserved = 0;
            li->link_msg_size = short_be((uint16_t)total_link_size);
            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);
            neighbor = neighbor->next;
        }
        local = local->next;
    }
    *bookmark = NULL; /* All the list was visited, no more dgrams needed */
    return ret;
}

static uint32_t olsr_build_tc_neighbors(uint8_t *buf, uint32_t size, struct olsr_route_entry **bookmark)
{
    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;
        if (*bookmark) {
            while ((neighbor) && *bookmark != neighbor)
                neighbor = neighbor->next;
        }

        while (neighbor) {
            if (size - ret < sizeof(struct olsr_neighbor)) {
                /* Incomplete list, new datagram needed. */
                *bookmark = neighbor;
                return ret;
            }

            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);
            neighbor = neighbor->next;
        }
        local = local->next;
    }
    *bookmark = NULL; /* All the list was visited, no more dgrams needed */
    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 void olsr_compose_tc_dgram(struct pico_device *pdev, struct pico_ipv4_link *ep)
{
    struct olsrmsg *msg_tc, *msg_mid;
    uint32_t size = 0, r;
    struct olsr_route_entry *last_neighbor = NULL;
    uint8_t *dgram;
    struct olsr_hmsg_tc *tc;
    do {
        dgram = PICO_ZALLOC(DGRAM_MAX_SIZE);
        if (!dgram) {
            OOM();
            return;
        }

        size = (uint32_t)sizeof(struct olsrhdr);
        ep = pico_ipv4_link_by_dev(pdev);
        if (!ep) {
            PICO_FREE(dgram);
            return;
        }


        if (!last_neighbor) {
            /* 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(msg_counter++);
            r = olsr_build_mid(dgram + size, DGRAM_MAX_SIZE - size, pdev);
            if (r == 0) {
                size -= (uint32_t)sizeof(struct olsrmsg);
            } else {
                if ((size + r) > DGRAM_MAX_SIZE)
                    return;

                size += r;
                msg_mid->size = short_be((uint16_t)(sizeof(struct olsrmsg) + r));
            }
        }

        if (size + sizeof(struct olsrmsg) > DGRAM_MAX_SIZE)
            return;

        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(msg_counter++);
        tc = (struct olsr_hmsg_tc *)(dgram + size);
        size += (uint32_t)sizeof(struct olsr_hmsg_tc);
        if (size > DGRAM_MAX_SIZE)
            return;

        tc->ansn = short_be(my_ansn);
        r = olsr_build_tc_neighbors(dgram + size, DGRAM_MAX_SIZE  - size, &last_neighbor);
        size += r;
        msg_tc->size = short_be((uint16_t)(sizeof(struct olsrmsg) + sizeof(struct olsr_hmsg_tc) + r));
        olsr_scheduled_output(OLSR_TC_INTERVAL, dgram, (uint16_t)size, pdev );
    } while(last_neighbor);
}

static void olsr_compose_hello_dgram(struct pico_device *pdev, struct pico_ipv4_link *ep)
{
    struct olsrmsg *msg_hello;
    uint32_t size = 0, r;
    struct olsr_route_entry *last_neighbor = NULL;
    uint8_t *dgram;
    struct olsr_hmsg_hello *hello;
    /* HELLO Message */
    do {
        dgram = PICO_ZALLOC(DGRAM_MAX_SIZE);
        if (!dgram) {
            OOM();
            return;
        }

        size = (uint32_t)sizeof(struct olsrhdr);
        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(msg_counter++);
        hello = (struct olsr_hmsg_hello *)(dgram + size);
        size += (uint32_t)sizeof(struct olsr_hmsg_hello);
        hello->reserved = 0;
        hello->htime = seconds2olsr(OLSR_HELLO_INTERVAL);
        hello->htime = 0x05; /* Todo: find and define values */
        hello->willingness = 0x07;
        if (DGRAM_MAX_SIZE > size) {
            r = olsr_build_hello_neighbors(dgram + size, DGRAM_MAX_SIZE - size, &last_neighbor);
            if (r == 0) {
                /* olsr_dbg("Building hello message\n"); */
                PICO_FREE(dgram);
                return;
            }
        }

        size += r;
        msg_hello->size = short_be((uint16_t)(sizeof(struct olsrmsg) + sizeof(struct olsr_hmsg_hello) + r));
        olsr_scheduled_output(OLSR_HELLO_INTERVAL, dgram, (uint16_t)size, pdev );
    } while(last_neighbor);
}

static void olsr_make_dgram(struct pico_device *pdev, int full)
{
    struct pico_ipv4_link *ep;
    ep = pico_ipv4_link_by_dev(pdev);
    if (!ep) {
        return;
    }

    if (!full) {
        olsr_compose_hello_dgram(pdev, ep);
    } else {
        olsr_compose_tc_dgram(pdev, ep);
    } /*if full */

}

/* Old code was relying on ethernet arp requests */
#define arp_storm(...) do {} while(0)

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) {
                olsr_dbg("olsr allocating route\n");
                OOM();
                return;
            }

            e->time_left = (OLSR_HELLO_INTERVAL << 2);
            e->destination.addr = *address;
            e->gateway = origin;
            /* e->iface = origin->iface; */
            e->iface = NULL;
            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 = (uint16_t)(origin->metric + 1u);
            e->gateway = origin;
            e->time_left = (OLSR_HELLO_INTERVAL << 2);
            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) */
static void recv_hello(uint8_t *buffer, uint32_t len, struct olsr_route_entry *origin, uint16_t hops)
{
    struct olsr_link *li;
    struct olsr_route_entry *e;
    uint32_t parsed = 0;
    struct olsr_neighbor *neigh;

    if (!origin)
        return;

    /* Don't parse hello messages that were forwarded */
    if (hops > 0 || origin->metric > 1)
        return;

    if (pico_ipv4_link_get(&origin->destination))
        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) {
                olsr_dbg("olsr allocating route\n");
                OOM();
                return;
            }

            e->time_left = (OLSR_HELLO_INTERVAL << 2);
            e->destination.addr = neigh->addr;
            e->gateway = origin;
            e->iface = NULL;
            e->metric = (uint16_t)(origin->metric + hops + 1);
            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) && (origin->metric > 1) && (e->metric > (origin->metric + hops + 1))) {
            olsr_route_del(e);
            e->metric = (uint16_t)(origin->metric + hops + 1);
            e->gateway = origin;
            e->time_left = (OLSR_HELLO_INTERVAL << 2);
            olsr_route_add(e);
        } else {
            e->time_left = (OLSR_HELLO_INTERVAL << 2);
        }
    }
}

/* static uint32_t reconsider_topology(uint8_t *buf, uint32_t size, struct olsr_route_entry *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;
    uint32_t retval = 0;

    if (!e->advertised_tc)
        retval = 1;

    if (e->advertised_tc && fresher(new_ansn, e->ansn))
    {
        PICO_FREE(e->advertised_tc); /* <--- XXX check invalid free? */
        e->advertised_tc = NULL;
        retval = 1;
    }

    /* Ignore self packets */
    if (pico_ipv4_link_get(&e->destination))
        return 0;

    if (!e->advertised_tc) {
        e->advertised_tc = PICO_ZALLOC(size);
        if (!e->advertised_tc) {
            OOM();
            olsr_dbg("Allocating forward packet\n");
            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));
                    if (!rt) {
                        OOM();
                        return retval;
                    }

                    rt->destination.addr = n->addr;
                    rt->link_type = OLSRLINK_UNKNOWN;
                } else {
                    olsr_route_del(rt);
                }

                rt->iface = e->iface;
                rt->gateway = e;
                rt->metric = (uint16_t)(e->metric + 1);
                rt->lq = n->lq; /* 0xff */
                rt->nlq = n->nlq; /* 0xff */
                rt->time_left = e->time_left; /* 256 */
                olsr_route_add(rt);
            }
        }
        /* olsr_dbg("Routes changed...\n"); */
    }

    return retval;
}


static void olsr_recv(uint8_t *buffer, uint32_t len)
{
    struct olsrmsg *msg;
    struct olsrhdr *oh = (struct olsrhdr *) buffer;
    struct olsr_route_entry *ancestor;
    uint32_t parsed = 0;
    uint16_t outsize = 0;
    uint8_t *datagram;

    if (len != short_be(oh->len)) {
        return;
    }

    /* RFC 3626, section 3.4, if a packet is too small, it is silently discarded */
    if (len < 16) {
        return;
    }

    parsed += (uint32_t)sizeof(struct olsrhdr);

    datagram = PICO_ZALLOC(DGRAM_MAX_SIZE);
    if (!datagram) {
        OOM();
        return;
    }

    outsize = (uint16_t) (outsize + (sizeof(struct olsrhdr)));
    /* Section 1: parsing received messages. */
    while (len > parsed) {
        struct olsr_route_entry *origin;
        msg = (struct olsrmsg *) (buffer + parsed);
        origin = get_route_by_address(Local_interfaces, msg->orig.addr);

        if(pico_ipv4_link_find(&msg->orig) != NULL) {
            /* olsr_dbg("rebound\n"); */
            parsed += short_be(msg->size);
            continue;
        }

        /* OLSR's TTL expired. */
        if (msg->ttl < 1u) {
            parsed += short_be(msg->size);
            continue;
        }

        if (!origin) {
            if (msg->hop == 0) {
                struct olsr_route_entry *e = PICO_ZALLOC(sizeof (struct olsr_route_entry));
                if (!e) {
                    parsed += short_be(msg->size);
                    OOM();
                    break;
                }

                e->destination.addr = msg->orig.addr;
                e->link_type = OLSRLINK_SYMMETRIC;
                e->time_left = (OLSR_HELLO_INTERVAL << 2);
                e->iface = Local_devices->dev;
                e->gateway = olsr_get_ethentry(e->iface);
                e->metric = 1;
                e->lq = 0xFF;
                e->nlq = 0xFF;
                olsr_route_add(e);
            }

            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->hop);
            msg->ttl = 0;
            break;
        case OLSRMSG_MID:
            if ((origin->seq != 0) && (!fresher(short_be(msg->seq), origin->seq))) {
                msg->ttl = 0;
            } else {
                recv_mid(buffer + parsed + sizeof(struct olsrmsg), (uint32_t)(short_be(msg->size) - (sizeof(struct olsrmsg))), origin);
                /* olsr_dbg("MID forwarded from origin %08x (seq: %u)\n", long_be(msg->orig.addr), short_be(msg->seq)); */
                origin->seq = short_be(msg->seq);
            }

            break;
        case OLSRMSG_TC:
            if(!pico_ipv4_link_find(&origin->destination)) {
                reconsider_topology(buffer + parsed + sizeof(struct olsrmsg), (uint32_t)(short_be(msg->size) - (sizeof(struct olsrmsg))), origin);
                if ((origin->seq != 0) && (!fresher(short_be(msg->seq), origin->seq))) {
                    msg->ttl = 0;
                } else {
                    /* olsr_dbg("TC forwarded from origin %08x (seq: %u)\n", long_be(msg->orig.addr), short_be(msg->seq)); */
                    origin->seq = short_be(msg->seq);
                }
            }

            break;
        default:
            PICO_FREE(datagram);
            return;
        }

        if (msg->ttl > 1) {
            msg->hop++;
            msg->ttl--;
            memcpy(datagram + outsize, msg, short_be(msg->size));
            outsize = (uint16_t)(outsize + short_be(msg->size));
        }

        parsed += short_be(msg->size);
    }
    /* Section 2: forwarding parsed messages that got past the filter. */
    if ((outsize > sizeof(struct olsrhdr))) {
        /* Finalize FWD packet */
        olsr_scheduled_output(OLSR_MAXJITTER, datagram, outsize, NULL);
    } else {
        /* Nothing to forward. */
        PICO_FREE(datagram);
    }
}

static void wakeup(uint16_t ev, struct pico_socket *s)
{
    unsigned char *recvbuf;
    int r = 0;
    struct pico_ip4 ANY = {
        0
    };
    uint16_t port = OLSR_PORT;
    recvbuf = PICO_ZALLOC(DGRAM_MAX_SIZE);
    if (!recvbuf) {
        OOM();
        return;
    }

    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);
    }

    PICO_FREE(recvbuf);
}

static void olsr_hello_tick(pico_time when, void *unused)
{
    struct olsr_dev_entry *d;
    (void)when;
    (void)unused;
    olsr_garbage_collector(Local_interfaces);
    refresh_routes();
    d = Local_devices;
    while(d) {
        olsr_make_dgram(d->dev, 0);
        d = d->next;
    }
    if (!pico_timer_add(OLSR_HELLO_INTERVAL, &olsr_hello_tick, NULL)) {
        olsr_dbg("OLSR: Failed to start hello_tick timer\n");
        /* TODO no more ticks now */
    }
}

static void olsr_tc_tick(pico_time when, void *unused)
{
    struct olsr_dev_entry *d;
    (void)when;
    (void)unused;
    d = Local_devices;
    while(d) {
        olsr_make_dgram(d->dev, 1);
        d = d->next;
    }
    if (!pico_timer_add(OLSR_TC_INTERVAL, &olsr_tc_tick, NULL)) {
        olsr_dbg("OLSR: Failed to start tc_tick timer\n");
        /* TODO no more ticks now */
    }
}


/* Public interface */

void pico_olsr_init(void)
{
    uint32_t hello_timer = 0;
    struct pico_ip4 ANY = {
        0
    };
    uint16_t port = OLSR_PORT;
    olsr_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);
    }

    hello_timer = pico_timer_add(pico_rand() % 100, &olsr_hello_tick, NULL);
    if (!hello_timer) {
        olsr_dbg("OLSR: Failed to start hello_tick timer\n");
        return;
    }
    if (!pico_timer_add(pico_rand() % 900, &olsr_tc_tick, NULL)) {
        olsr_dbg("OLSR: Failed to start tc_tick timer\n");
        pico_timer_cancel(hello_timer);
    }
}


int OOM(void)
{
    volatile int c = 3600;
    c++;
    c++;
    c++;
    return -1;
}

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

    /* olsr_dbg("OLSR: Adding device %s\n", dev->name); */
    od = PICO_ZALLOC(sizeof(struct olsr_dev_entry));
    if (!od) {
        pico_err = PICO_ERR_ENOMEM;
        /* OOM(); */
        return -1;
    }

    od->dev = dev;
    od->next = Local_devices;
    Local_devices = od;

    do {
        char ipaddr[20];
        lnk = pico_ipv4_link_by_dev_next(dev, lnk);
        if (lnk) {
            struct olsr_route_entry *e = PICO_ZALLOC(sizeof(struct olsr_route_entry));
            /* olsr_dbg("OLSR: Found IP address %08x\n", long_be(lnk->address.addr)); */
            pico_ipv4_to_string(ipaddr, (lnk->address.addr));
            /* olsr_dbg("OLSR: Found IP address %s\n", ipaddr); */
            if (!e) {
                olsr_dbg("olsr allocating route\n");
                pico_err = PICO_ERR_ENOMEM;
                return -1;
            }

            e->destination.addr = lnk->address.addr;
            e->link_type = OLSRLINK_SYMMETRIC;
            e->time_left = (OLSR_HELLO_INTERVAL << 2);
            e->gateway = NULL;
            e->children = 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