File content as of revision 155:a70f34550c34:

/*********************************************************************
   PicoTCP. Copyright (c) 2012-2015 Altran Intelligent Systems. Some rights reserved.
   See LICENSE and COPYING for usage.

   .

   Authors: Daniele Lacamera
 *********************************************************************/


#include "pico_config.h"
#include "pico_frame.h"
#include "pico_device.h"
#include "pico_protocol.h"
#include "pico_stack.h"
#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"
#include "pico_ipv6.h"
#include "pico_icmp4.h"
#include "pico_icmp6.h"
#include "pico_igmp.h"
#include "pico_udp.h"
#include "pico_tcp.h"
#include "pico_socket.h"
#include "heap.h"

#define IS_LIMITED_BCAST(f) (((struct pico_ipv4_hdr *) f->net_hdr)->dst.addr == PICO_IP4_BCAST)

const uint8_t PICO_ETHADDR_ALL[6] = {
    0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};

# define PICO_SIZE_MCAST 3
const uint8_t PICO_ETHADDR_MCAST[6] = {
    0x01, 0x00, 0x5e, 0x00, 0x00, 0x00
};

#ifdef PICO_SUPPORT_IPV6
# define PICO_SIZE_MCAST6 2
const uint8_t PICO_ETHADDR_MCAST6[6] = {
    0x33, 0x33, 0x00, 0x00, 0x00, 0x00
};
#endif


volatile pico_time pico_tick;
volatile pico_err_t pico_err;

static uint32_t _rand_seed;

void WEAK pico_rand_feed(uint32_t feed)
{
    if (!feed)
        return;

    _rand_seed *= 1664525;
    _rand_seed += 1013904223;
    _rand_seed ^= ~(feed);
}

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.
 */

int pico_notify_socket_unreachable(struct pico_frame *f)
{
    if (0) {}

#ifdef PICO_SUPPORT_ICMP4
    else if (IS_IPV4(f)) {
        pico_icmp4_port_unreachable(f);
    }
#endif
#ifdef PICO_SUPPORT_ICMP6
    else if (IS_IPV6(f)) {
        pico_icmp6_port_unreachable(f);
    }
#endif

    return 0;
}

int pico_notify_proto_unreachable(struct pico_frame *f)
{
    if (0) {}

#ifdef PICO_SUPPORT_ICMP4
    else if (IS_IPV4(f)) {
        pico_icmp4_proto_unreachable(f);
    }
#endif
#ifdef PICO_SUPPORT_ICMP6
    else if (IS_IPV6(f)) {
        pico_icmp6_proto_unreachable(f);
    }
#endif
    return 0;
}

int pico_notify_dest_unreachable(struct pico_frame *f)
{
    if (0) {}

#ifdef PICO_SUPPORT_ICMP4
    else if (IS_IPV4(f)) {
        pico_icmp4_dest_unreachable(f);
    }
#endif
#ifdef PICO_SUPPORT_ICMP6
    else if (IS_IPV6(f)) {
        pico_icmp6_dest_unreachable(f);
    }
#endif
    return 0;
}

int pico_notify_ttl_expired(struct pico_frame *f)
{
    if (0) {}

#ifdef PICO_SUPPORT_ICMP4
    else if (IS_IPV4(f)) {
        pico_icmp4_ttl_expired(f);
    }
#endif
#ifdef PICO_SUPPORT_ICMP6
    else if (IS_IPV6(f)) {
        pico_icmp6_ttl_expired(f);
    }
#endif
    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 */
MOCKABLE int32_t pico_transport_receive(struct pico_frame *f, uint8_t proto)
{
    int32_t ret = -1;
    switch (proto) {

#ifdef PICO_SUPPORT_ICMP4
    case PICO_PROTO_ICMP4:
        ret = pico_enqueue(pico_proto_icmp4.q_in, f);
        break;
#endif

#ifdef PICO_SUPPORT_ICMP6
    case PICO_PROTO_ICMP6:
        ret = pico_enqueue(pico_proto_icmp6.q_in, f);
        break;
#endif


#if defined(PICO_SUPPORT_IGMP) && defined(PICO_SUPPORT_MCAST)
    case PICO_PROTO_IGMP:
        ret = pico_enqueue(pico_proto_igmp.q_in, f);
        break;
#endif

#ifdef PICO_SUPPORT_UDP
    case PICO_PROTO_UDP:
        ret = pico_enqueue(pico_proto_udp.q_in, f);
        break;
#endif

#ifdef PICO_SUPPORT_TCP
    case PICO_PROTO_TCP:
        ret = pico_enqueue(pico_proto_tcp.q_in, f);
        break;
#endif

    default:
        /* Protocol not available */
        dbg("pkt: no such protocol (%d)\n", proto);
        pico_notify_proto_unreachable(f);
        pico_frame_discard(f);
        ret = -1;
    }
    return ret;
}

int32_t pico_network_receive(struct pico_frame *f)
{
    if (0) {}

#ifdef PICO_SUPPORT_IPV4
    else if (IS_IPV4(f)) {
        pico_enqueue(pico_proto_ipv4.q_in, f);
    }
#endif
#ifdef PICO_SUPPORT_IPV6
    else if (IS_IPV6(f)) {
        pico_enqueue(pico_proto_ipv6.q_in, f);
    }
#endif
    else {
        dbg("Network not found.\n");
        pico_frame_discard(f);
        return -1;
    }
    return (int32_t)f->buffer_len;
}

/* Network layer: interface towards socket for frame sending */
int32_t pico_network_send(struct pico_frame *f)
{
    if (!f || !f->sock || !f->sock->net) {
        pico_frame_discard(f);
        return -1;
    }

    return f->sock->net->push(f->sock->net, f);
}

int pico_source_is_local(struct pico_frame *f)
{
    if (0) { }

#ifdef PICO_SUPPORT_IPV4
    else if (IS_IPV4(f)) {
        struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
        if (hdr->src.addr == PICO_IPV4_INADDR_ANY)
            return 1;

        if (pico_ipv4_link_find(&hdr->src))
            return 1;
    }
#endif
#ifdef PICO_SUPPORT_IPV6
    else if (IS_IPV6(f)) {
        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.
 */

/* The pico_ethernet_receive() function is used by
 * those devices supporting ETH in order to push packets up
 * into the stack.
 */

static int destination_is_bcast(struct pico_frame *f)
{
    if (!f)
        return 0;

    if (IS_IPV6(f))
        return 0;

#ifdef PICO_SUPPORT_IPV4
    else {
        struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
        return pico_ipv4_is_broadcast(hdr->dst.addr);
    }
#else
    return 0;
#endif
}

static int destination_is_mcast(struct pico_frame *f)
{
    int ret = 0;
    if (!f)
        return 0;

#ifdef PICO_SUPPORT_IPV6
    if (IS_IPV6(f)) {
        struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *) f->net_hdr;
        ret = pico_ipv6_is_multicast(hdr->dst.addr);
    }

#endif
#ifdef PICO_SUPPORT_IPV4
    else {
        struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
        ret = pico_ipv4_is_multicast(hdr->dst.addr);
    }
#endif

    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;

    /* place 23 lower bits of IP in lower 23 bits of MAC */
    pico_mcast_mac[5] = (long_be(hdr->dst.addr) & 0x000000FFu);
    pico_mcast_mac[4] = (uint8_t)((long_be(hdr->dst.addr) & 0x0000FF00u) >> 8u);
    pico_mcast_mac[3] = (uint8_t)((long_be(hdr->dst.addr) & 0x007F0000u) >> 16u);

    return (struct pico_eth *)pico_mcast_mac;
}


#ifdef PICO_SUPPORT_IPV6
static struct pico_eth *pico_ethernet_mcast6_translate(struct pico_frame *f, uint8_t *pico_mcast6_mac)
{
    struct pico_ipv6_hdr *hdr = (struct pico_ipv6_hdr *)f->net_hdr;

    /* first 2 octets are 0x33, last four are the last four of dst */
    pico_mcast6_mac[5] = hdr->dst.addr[PICO_SIZE_IP6 - 1];
    pico_mcast6_mac[4] = hdr->dst.addr[PICO_SIZE_IP6 - 2];
    pico_mcast6_mac[3] = hdr->dst.addr[PICO_SIZE_IP6 - 3];
    pico_mcast6_mac[2] = hdr->dst.addr[PICO_SIZE_IP6 - 4];

    return (struct pico_eth *)pico_mcast6_mac;
}
#endif

static int pico_ethernet_ipv6_dst(struct pico_frame *f, struct pico_eth *const dstmac)
{
    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
        };
        pico_ethernet_mcast6_translate(f, pico_mcast6_mac);
        memcpy(dstmac, pico_mcast6_mac, PICO_SIZE_ETH);
        retval = 0;
    } else {
        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 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;

    /* 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");
        (void)pico_ethernet_receive(clone);
        return 1;
    }

    return 0;
}

/* 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)) {
        (void)pico_device_broadcast(f); /* We can discard broadcast even if it's not sent. */
        return 1;
    }

    return 0;
}

/* 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)
{
    int ret = f->dev->send(f->dev, f->start, (int) f->len);
    if (ret <= 0)
        return 0; /* Failure to deliver! */
    else {
        return 1; /* Frame is in flight by now. */
    }
}




/* 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)
{
    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)) {
        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

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

    /* This sets destination and source address, then pushes the packet to the device. */
    if (dstmac_valid) {
        struct pico_eth_hdr *hdr;
        hdr = (struct pico_eth_hdr *) f->datalink_hdr;
        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) || 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
    struct pico_ip4 *ip4;
  #endif

  #ifdef PICO_SUPPORT_IPV6
    struct pico_ip6 *ip6;
  #endif

  #ifdef PICO_SUPPORT_IPV4
    if (IS_IPV4(f)) {
        struct pico_ipv4_hdr *hdr;
        hdr = (struct pico_ipv4_hdr *) f->net_hdr;
        ip4 = (struct pico_ip4 *) src;
        ip4->addr = hdr->src.addr;
    }

  #endif
  #ifdef PICO_SUPPORT_IPV6
    if (IS_IPV6(f)) {
        struct pico_ipv6_hdr *hdr;
        hdr = (struct pico_ipv6_hdr *) f->net_hdr;
        ip6 = (struct pico_ip6 *) src;
        memcpy(ip6->addr, hdr->src.addr, PICO_SIZE_IP6);
    }

  #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.
 * Incoming packet will be processed later on in the dev loop.
 */
int32_t pico_stack_recv(struct pico_device *dev, uint8_t *buffer, uint32_t len)
{
    struct pico_frame *f;
    int32_t ret;
    if (len == 0)
        return -1;

    f = pico_frame_alloc(len);
    if (!f)
    {
        dbg("Cannot alloc incoming frame!\n");
        return -1;
    }

    /* Association to the device that just received the frame. */
    f->dev = dev;

    /* Setup the start pointer, length. */
    f->start = f->buffer;
    f->len = f->buffer_len;
    if (f->len > 8) {
        uint32_t rand, mid_frame = (f->buffer_len >> 2) << 1;
        mid_frame -= (mid_frame % 4);
        memcpy(&rand, f->buffer + mid_frame, sizeof(uint32_t));
        pico_rand_feed(rand);
    }

    memcpy(f->buffer, buffer, len);
    ret = pico_enqueue(dev->q_in, f);
    if (ret <= 0) {
        pico_frame_discard(f);
    }

    return ret;
}

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, ext_buffer);
    if (!f)
    {
        dbg("Cannot alloc incoming frame!\n");
        return -1;
    }

    if (pico_frame_skeleton_set_buffer(f, buffer) < 0)
    {
        dbg("Invalid zero-copy buffer!\n");
        PICO_FREE(f->usage_count);
        PICO_FREE(f);
        return -1;
    }

    if (notify_free) {
        f->notify_free = notify_free;
    }

    f->dev = dev;
    ret = pico_enqueue(dev->q_in, f);
    if (ret <= 0) {
        pico_frame_discard(f);
    }

    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) {
        pico_frame_discard(f);
        return -1;
    } else {
        if (f->len > 8) {
            uint32_t rand, mid_frame = (f->buffer_len >> 2) << 1;
            mid_frame -= (mid_frame % 4);
            memcpy(&rand, f->buffer + mid_frame, sizeof(uint32_t));
            pico_rand_feed(rand);
        }

        return pico_enqueue(f->dev->q_out, f);
    }
}

struct pico_timer
{
    void *arg;
    void (*timer)(pico_time timestamp, void *arg);
};

struct pico_timer_ref
{
    pico_time expire;
    struct pico_timer *tmr;
};

typedef struct pico_timer_ref pico_timer_ref;

DECLARE_HEAP(pico_timer_ref, expire);

static heap_pico_timer_ref *Timers;

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);
    pico_tick = PICO_TIME_MS();
    while((tref) && (tref->expire < pico_tick)) {
        t = tref->tmr;
        if (t && t->timer)
            t->timer(pico_tick, t->arg);

        if (t)
        {
            PICO_FREE(t);
        }

        t = NULL;
        heap_peek(Timers, &tref_unused);
        tref = heap_first(Timers);
    }
}

void MOCKABLE pico_timer_cancel(struct pico_timer *t)
{
    uint32_t i;
    struct pico_timer_ref *tref = Timers->top;
    if (!t)
        return;

    for (i = 1; i <= Timers->n; i++) {
        if (tref[i].tmr == t) {
            Timers->top[i].tmr = NULL;
            PICO_FREE(t);
            break;
        }
    }
}

#define PROTO_DEF_NR      11
#define PROTO_DEF_AVG_NR  4
#define PROTO_DEF_SCORE   32
#define PROTO_MIN_SCORE   32
#define PROTO_MAX_SCORE   128
#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)
{
    int temp, i, j, sum;
    int max_total = PROTO_MAX_LOOP, total = 0;

    /* dbg("USED SCORES> "); */

    for (i = 0; i < PROTO_DEF_NR; i++) {

        /* if used looped score */
        if (ret[i] < score[i]) {
            temp = score[i] - ret[i]; /* remaining loop score */

            /* dbg("%3d - ",temp); */

            if (index[i] >= PROTO_DEF_AVG_NR)
                index[i] = 0;   /* reset index */

            j = index[i];
            avg[i][j] = temp;

            index[i]++;

            if (ret[i] == 0 && ((score[i] * 2) <= PROTO_MAX_SCORE) && ((total + (score[i] * 2)) < max_total)) { /* used all loop score -> increase next score directly */
                score[i] *= 2;
                total += score[i];
                continue;
            }

            sum = 0;
            for (j = 0; j < PROTO_DEF_AVG_NR; j++)
                sum += avg[i][j]; /* calculate sum */

            sum /= 4;           /* divide by 4 to get average used score */

            /* criterion to increase next loop score */
            if (sum > (score[i] - (score[i] / 4))  && ((score[i] * 2) <= PROTO_MAX_SCORE) && ((total + (score[i] / 2)) < max_total)) { /* > 3/4 */
                score[i] *= 2; /* double loop score */
                total += score[i];
                continue;
            }

            /* criterion to decrease next loop score */
            if ((sum < (score[i] / 4)) && ((score[i] / 2) >= PROTO_MIN_SCORE)) { /* < 1/4 */
                score[i] /= 2; /* half loop score */
                total += score[i];
                continue;
            }

            /* also add non-changed scores */
            total += score[i];
        }
        else if (ret[i] == score[i]) {
            /* no used loop score - gradually decrease */

            /*  dbg("%3d - ",0); */

            if (index[i] >= PROTO_DEF_AVG_NR)
                index[i] = 0;   /* reset index */

            j = index[i];
            avg[i][j] = 0;

            index[i]++;

            sum = 0;
            for (j = 0; j < PROTO_DEF_AVG_NR; j++)
                sum += avg[i][j]; /* calculate sum */

            sum /= 2;          /* divide by 4 to get average used score */

            if ((sum == 0) && ((score[i] / 2) >= PROTO_MIN_SCORE)) {
                score[i] /= 2; /* half loop score */
                total += score[i];
                for (j = 0; j < PROTO_DEF_AVG_NR; j++)
                    avg[i][j] = score[i];
            }

        }
    }
    /* dbg("\n"); */

    return 0;
}

void pico_stack_tick(void)
{
    static int score[PROTO_DEF_NR] = {
        PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE, PROTO_DEF_SCORE
    };
    static int index[PROTO_DEF_NR] = {
        0, 0, 0, 0, 0, 0
    };
    static int avg[PROTO_DEF_NR][PROTO_DEF_AVG_NR];
    static int ret[PROTO_DEF_NR] = {
        0
    };

    pico_check_timers();

    /* dbg("LOOP_SCORES> %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d - %3d\n",score[0],score[1],score[2],score[3],score[4],score[5],score[6],score[7],score[8],score[9],score[10]); */

    /* score = pico_protocols_loop(100); */

    ret[0] = pico_devices_loop(score[0], PICO_LOOP_DIR_IN);
    pico_rand_feed((uint32_t)ret[0]);

    ret[1] = pico_protocol_datalink_loop(score[1], PICO_LOOP_DIR_IN);
    pico_rand_feed((uint32_t)ret[1]);

    ret[2] = pico_protocol_network_loop(score[2], PICO_LOOP_DIR_IN);
    pico_rand_feed((uint32_t)ret[2]);

    ret[3] = pico_protocol_transport_loop(score[3], PICO_LOOP_DIR_IN);
    pico_rand_feed((uint32_t)ret[3]);


    ret[5] = score[5];
#if defined (PICO_SUPPORT_IPV4) || defined (PICO_SUPPORT_IPV6)
#if defined (PICO_SUPPORT_TCP) || defined (PICO_SUPPORT_UDP)
    ret[5] = pico_sockets_loop(score[5]); /* swapped */
    pico_rand_feed((uint32_t)ret[5]);
#endif
#endif

    ret[4] = pico_protocol_socket_loop(score[4], PICO_LOOP_DIR_IN);
    pico_rand_feed((uint32_t)ret[4]);


    ret[6] = pico_protocol_socket_loop(score[6], PICO_LOOP_DIR_OUT);
    pico_rand_feed((uint32_t)ret[6]);

    ret[7] = pico_protocol_transport_loop(score[7], PICO_LOOP_DIR_OUT);
    pico_rand_feed((uint32_t)ret[7]);

    ret[8] = pico_protocol_network_loop(score[8], PICO_LOOP_DIR_OUT);
    pico_rand_feed((uint32_t)ret[8]);

    ret[9] = pico_protocol_datalink_loop(score[9], PICO_LOOP_DIR_OUT);
    pico_rand_feed((uint32_t)ret[9]);

    ret[10] = pico_devices_loop(score[10], PICO_LOOP_DIR_OUT);
    pico_rand_feed((uint32_t)ret[10]);

    /* calculate new loop scores for next iteration */
    calc_score(score, index, (int (*)[])avg, ret);
}

void pico_stack_loop(void)
{
    while(1) {
        pico_stack_tick();
        PICO_IDLE();
    }
}

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;
    if (!t) {
        pico_err = PICO_ERR_ENOMEM;
        return NULL;
    }

    tref.expire = PICO_TIME_MS() + expire;
    t->arg = arg;
    t->timer = timer;
    tref.tmr = t;
    heap_insert(Timers, &tref);
    if (Timers->n > PICO_MAX_TIMERS) {
        dbg("Warning: I have %d timers\n", (int)Timers->n);
    }

    return t;
}

int pico_stack_init(void)
{

#ifdef PICO_SUPPORT_IPV4
    pico_protocol_init(&pico_proto_ipv4);
#endif

#ifdef PICO_SUPPORT_IPV6
    pico_protocol_init(&pico_proto_ipv6);
#endif

#ifdef PICO_SUPPORT_ICMP4
    pico_protocol_init(&pico_proto_icmp4);
#endif

#ifdef PICO_SUPPORT_ICMP6
    pico_protocol_init(&pico_proto_icmp6);
#endif

#if defined(PICO_SUPPORT_IGMP) && defined(PICO_SUPPORT_MCAST)
    pico_protocol_init(&pico_proto_igmp);
#endif

#ifdef PICO_SUPPORT_UDP
    pico_protocol_init(&pico_proto_udp);
#endif

#ifdef PICO_SUPPORT_TCP
    pico_protocol_init(&pico_proto_tcp);
#endif

#ifdef PICO_SUPPORT_DNS_CLIENT
    pico_dns_client_init();
#endif

    pico_rand_feed(123456);

    /* Initialize timer heap */
    Timers = heap_init();
    if (!Timers)
        return -1;

#if ((defined PICO_SUPPORT_IPV4) && (defined PICO_SUPPORT_ETH))
    /* Initialize ARP module */
    pico_arp_init();
#endif

#ifdef PICO_SUPPORT_IPV6
    /* 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();
    return 0;
}