Diff: modules/pico_ipv4.c

Revision:

152:a3d286bf94e5

Parent:

149:5f4cb161cec3

--- a/modules/pico_ipv4.c	Wed Apr 09 17:32:25 2014 +0200
+++ b/modules/pico_ipv4.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.
 
    Authors: Daniele Lacamera, Markian Yskout
@@ -19,7 +19,9 @@
 #include "pico_nat.h"
 #include "pico_igmp.h"
 #include "pico_tree.h"
+#include "pico_aodv.h"
 #include "pico_socket_multicast.h"
+#include "pico_fragments.h"
 
 #ifdef PICO_SUPPORT_IPV4
 
@@ -30,7 +32,7 @@
 /* Default network interface for multicast transmission */
 static struct pico_ipv4_link *mcast_default_link = NULL;
 #endif
-#ifdef PICO_SUPPORT_IPFRAG
+#ifdef PICO_SUPPORT_IPV4FRAG
 /* # define reassembly_dbg dbg */
 # define reassembly_dbg(...) do {} while(0)
 #endif
@@ -47,6 +49,18 @@
 static int ipv4_route_compare(void *ka, void *kb);
 static struct pico_frame *pico_ipv4_alloc(struct pico_protocol *self, uint16_t size);
 
+
+int pico_ipv4_compare(struct pico_ip4 *a, struct pico_ip4 *b)
+{
+    if (a->addr < b->addr)
+        return -1;
+
+    if (a->addr > b->addr)
+        return 1;
+
+    return 0;
+}
+
 int pico_ipv4_to_string(char *ipbuf, const uint32_t ip)
 {
     const unsigned char *addr = (const unsigned char *) &ip;
@@ -59,18 +73,18 @@
 
     for(i = 0; i < 4; i++)
     {
-        if(addr[i] > 99) {
+        if (addr[i] > 99) {
             *ipbuf++ = (char)('0' + (addr[i] / 100));
             *ipbuf++ = (char)('0' + ((addr[i] % 100) / 10));
             *ipbuf++ = (char)('0' + ((addr[i] % 100) % 10));
-        }else if(addr[i] > 9) {
+        } else if (addr[i] > 9) {
             *ipbuf++ = (char)('0' + (addr[i] / 10));
             *ipbuf++ = (char)('0' + (addr[i] % 10));
-        }else{
+        } else {
             *ipbuf++ = (char)('0' + addr[i]);
         }
 
-        if(i < 3)
+        if (i < 3)
             *ipbuf++ = '.';
     }
     *ipbuf = '\0';
@@ -81,7 +95,7 @@
 static int pico_string_check_null_args(const char *ipstr, uint32_t *ip)
 {
 
-    if(!ipstr || !ip) {
+    if (!ipstr || !ip) {
         pico_err = PICO_ERR_EINVAL;
         return -1;
     }
@@ -92,7 +106,7 @@
 
 int pico_string_to_ipv4(const char *ipstr, uint32_t *ip)
 {
-    unsigned char buf[4] = {
+    unsigned char buf[PICO_SIZE_IP4] = {
         0
     };
     int cnt = 0;
@@ -101,26 +115,25 @@
     if (pico_string_check_null_args(ipstr, ip) < 0)
         return -1;
 
-
-    while((p = *ipstr++) != 0)
+    while((p = *ipstr++) != 0 && cnt < PICO_SIZE_IP4)
     {
-        if(pico_is_digit(p)) {
+        if (pico_is_digit(p)) {
             buf[cnt] = (uint8_t)((10 * buf[cnt]) + (p - '0'));
-        }else if(p == '.') {
+        } else if (p == '.') {
             cnt++;
-        }else{
+        } else {
             return -1;
         }
     }
     /* Handle short notation */
-    if(cnt == 1) {
+    if (cnt == 1) {
         buf[3] = buf[1];
         buf[1] = 0;
         buf[2] = 0;
-    }else if (cnt == 2) {
+    } else if (cnt == 2) {
         buf[3] = buf[2];
         buf[2] = 0;
-    }else if(cnt != 3) {
+    } else if (cnt != 3) {
         /* String could not be parsed, return error */
         return -1;
     }
@@ -128,7 +141,6 @@
     *ip = long_from(buf);
 
     return 0;
-
 }
 
 int pico_ipv4_valid_netmask(uint32_t mask)
@@ -146,14 +158,14 @@
      * */
 
     for(i = 0; i < 32; i++) {
-        if((mask_swap << i) & 0x80000000) {
-            if(end) {
+        if ((mask_swap << i) & 0x80000000) {
+            if (end) {
                 pico_err = PICO_ERR_EINVAL;
                 return -1;
             }
 
             cnt++;
-        }else{
+        } else {
             end = 1;
         }
     }
@@ -163,7 +175,7 @@
 int pico_ipv4_is_unicast(uint32_t address)
 {
     const unsigned char *addr = (unsigned char *) &address;
-    if((addr[0] & 0xe0) == 0xe0)
+    if ((addr[0] & 0xe0) == 0xe0)
         return 0; /* multicast */
 
     return 1;
@@ -172,7 +184,7 @@
 int pico_ipv4_is_multicast(uint32_t address)
 {
     const unsigned char *addr = (unsigned char *) &address;
-    if((addr[0] != 0xff) && ((addr[0] & 0xe0) == 0xe0))
+    if ((addr[0] != 0xff) && ((addr[0] & 0xe0) == 0xe0))
         return 1; /* multicast */
 
     return 0;
@@ -181,7 +193,7 @@
 int pico_ipv4_is_loopback(uint32_t address)
 {
     const unsigned char *addr = (unsigned char *) &address;
-    if(addr[0] == 0x7f)
+    if (addr[0] == 0x7f)
         return 1;
 
     return 0;
@@ -197,16 +209,18 @@
     if (pico_ipv4_is_broadcast(address)) {
         dbg("Source is a broadcast address, discard packet\n");
         return 0;
-    }
-    else if( pico_ipv4_is_multicast(address)) {
+    } else if ( pico_ipv4_is_multicast(address)) {
         dbg("Source is a multicast address, discard packet\n");
         return 0;
-    }
-    else if (pico_ipv4_is_invalid_loopback(address, dev)) {
+    } else if (pico_ipv4_is_invalid_loopback(address, dev)) {
         dbg("Source is a loopback address, discard packet\n");
         return 0;
-    }
-    else {
+    } else {
+#ifdef PICO_SUPPORT_AODV
+        union pico_address src;
+        src.ip4.addr = address;
+        pico_aodv_refresh(&src);
+#endif
         return 1;
     }
 }
@@ -222,270 +236,6 @@
     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;
-    uint16_t a_frag, b_frag;
-    a = (struct pico_ipv4_hdr *) frame_a->net_hdr;
-    b = (struct pico_ipv4_hdr *) frame_b->net_hdr;
-    a_frag = short_be(a->frag & PICO_IPV4_FRAG_MASK);
-    b_frag = short_be(b->frag & PICO_IPV4_FRAG_MASK);
-
-    if (a_frag < b_frag)
-        return -1;
-
-    if (b_frag < a_frag)
-        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 struct pico_ipv4_fragmented_packet *fragment_find_by_hdr(struct pico_ipv4_hdr *hdr)
-{
-    struct pico_ipv4_fragmented_packet frag = {
-        0
-    };
-    frag.id = short_be(hdr->id);
-    frag.proto = hdr->proto;
-    frag.src.addr = long_be(hdr->src.addr);
-    frag.dst.addr = long_be(hdr->dst.addr);
-    return pico_tree_findKey(&pico_ipv4_fragmented_tree, &frag);
-}
-
-
-static inline int8_t pico_ipv4_fragmented_check(struct pico_protocol *self, struct pico_frame **f)
-{
-    uint8_t *running_pointer = NULL;
-    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_ipv4_fragmented_packet *pfrag = NULL;
-    struct pico_frame *f_new = NULL, *f_frag = NULL;
-    struct pico_tree_node *index, *_tmp;
-
-    data_len = (uint16_t)(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 = (uint16_t)(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);
-            pfrag = fragment_find_by_hdr(hdr);
-            if (pfrag) {
-                pfrag->total_len = (uint16_t)(pfrag->total_len + (short_be(hdr->len) - (*f)->net_len));
-                if (pfrag->total_len > PICO_IPV4_FRAG_MAX_SIZE) {
-                    reassembly_dbg("BIG frame!!!\n");
-                    pfrag = pico_tree_first(&pico_ipv4_fragmented_tree);
-                    pico_ipv4_fragmented_cleanup(pfrag);
-                    pico_frame_discard(*f);
-                    return 0;
-                }
-
-                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);
-        pfrag = fragment_find_by_hdr(hdr);
-        if (pfrag) {
-            pfrag->total_len = (uint16_t)(pfrag->total_len + (short_be(hdr->len) - (*f)->net_len));
-            reassembly_dbg("REASSEMBLY: fragmented packet in tree, reassemble packet of %u data bytes\n", pfrag->total_len);
-            if (pfrag->total_len > PICO_IPV4_FRAG_MAX_SIZE) {
-                reassembly_dbg("BIG frame!!!\n");
-                pfrag = pico_tree_first(&pico_ipv4_fragmented_tree);
-                pico_ipv4_fragmented_cleanup(pfrag);
-                pico_frame_discard(*f);
-                return 0;
-            }
-
-            f_new = self->alloc(self, pfrag->total_len);
-            if (!f_new)
-                return -1;
-
-            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 = (uint16_t)(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);
-            f_new->dev = f_frag->dev;
-            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 = (uint16_t)(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 = (uint16_t)(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 = (uint16_t)(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) {
-                struct pico_tcp_hdr *tcp_hdr = NULL;
-                tcp_hdr = (struct pico_tcp_hdr *) f_new->transport_hdr;
-                tcp_hdr->crc = 0;
-                tcp_hdr->crc = short_be(pico_tcp_checksum(f_new));
-  #endif
-  #ifdef PICO_SUPPORT_UDP
-            } else if (hdr->proto == PICO_PROTO_UDP) {
-                struct pico_udp_hdr *udp_hdr = NULL;
-                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 int8_t 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)
@@ -518,11 +268,9 @@
 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;
+    int cmp = pico_ipv4_compare(&a->address, &b->address);
+    if (cmp)
+        return cmp;
 
     /* 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) {
@@ -549,6 +297,9 @@
         return 1;
     }
 
+#endif
+
+#ifdef PICO_SUPPORT_ICMP4
     if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_ICMP4)) {
         /* Receiving ICMP4 bcast packet */
         f->flags |= PICO_FRAME_FLAG_BCAST;
@@ -564,7 +315,7 @@
 {
     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
     if (pico_ipv4_is_multicast(hdr->dst.addr)) {
-#ifdef PICO_SUPPORT_MCAST
+#ifdef PICO_SUPPORT_IGMP
         /* Receiving UDP multicast datagram TODO set f->flags? */
         if (hdr->proto == PICO_PROTO_IGMP) {
             ip_mcast_dbg("MCAST: received IGMP message\n");
@@ -613,8 +364,7 @@
 static void pico_ipv4_process_finally_try_forward(struct pico_frame *f)
 {
     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
-    if((pico_ipv4_is_broadcast(hdr->dst.addr)))
-    {
+    if ((pico_ipv4_is_broadcast(hdr->dst.addr)) || ((f->flags & PICO_FRAME_FLAG_BCAST)!= 0)) {
         /* don't forward broadcast frame, discard! */
         pico_frame_discard(f);
     } else if (pico_ipv4_forward(f) != 0) {
@@ -630,9 +380,12 @@
     uint8_t option_len = 0;
     int ret = 0;
     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+    uint16_t max_allowed = (uint16_t) ((int)f->buffer_len - (f->net_hdr - f->buffer) - (int)PICO_SIZE_IP4HDR);
+    uint16_t flag = short_be(hdr->frag);
 
+    (void)self;
     /* NAT needs transport header information */
-    if(((hdr->vhl) & 0x0F) > 5) {
+    if (((hdr->vhl) & 0x0F) > 5) {
         option_len =  (uint8_t)(4 * (((hdr->vhl) & 0x0F) - 5));
     }
 
@@ -640,6 +393,11 @@
     f->transport_len = (uint16_t)(short_be(hdr->len) - PICO_SIZE_IP4HDR - option_len);
     f->net_len = (uint16_t)(PICO_SIZE_IP4HDR + option_len);
 
+    if (f->transport_len > max_allowed) {
+        pico_frame_discard(f);
+        return 0; /* Packet is discarded due to unfeasible length */
+    }
+
 #ifdef PICO_SUPPORT_IPFILTER
     if (ipfilter(f)) {
         /*pico_frame is discarded as result of the filtering*/
@@ -648,26 +406,42 @@
 
 #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;
-
     /* Validate source IP address. Discard quietly if invalid */
     if (!pico_ipv4_is_valid_src(hdr->src.addr, f->dev)) {
         pico_frame_discard(f);
         return 0;
     }
 
-    if (hdr->frag & 0x80) {
+    if (hdr->frag & short_be(PICO_IPV4_EVIL)) {
+        (void)pico_icmp4_param_problem(f, 0);
         pico_frame_discard(f); /* RFC 3514 */
         return 0;
     }
 
+    if ((hdr->vhl & 0x0f) < 5) {
+        /* RFC 791: IHL minimum value is 5 */
+        (void)pico_icmp4_param_problem(f, 0);
+        pico_frame_discard(f);
+        return 0;
+    }
+
+    if (flag & (PICO_IPV4_MOREFRAG | PICO_IPV4_FRAG_MASK))
+    {
+#ifdef PICO_SUPPORT_IPV4FRAG
+        pico_ipv4_process_frag(hdr, f, hdr ? hdr->proto : 0 );
+        /* Frame can be discarded, frag will handle its own copy */
+#endif
+        /* We do not support fragmentation, discard quietly */
+        pico_frame_discard(f);
+        return 0;
+    }
+
     if (pico_ipv4_process_bcast_in(f) > 0)
         return 0;
 
@@ -689,13 +463,13 @@
 {
     IGNORE_PARAMETER(self);
     f->start = (uint8_t*) f->net_hdr;
-  #ifdef PICO_SUPPORT_IPFILTER
+#ifdef PICO_SUPPORT_IPFILTER
     if (ipfilter(f)) {
         /*pico_frame is discarded as result of the filtering*/
         return 0;
     }
 
-  #endif
+#endif
     return pico_sendto_dev(f);
 }
 
@@ -732,20 +506,12 @@
     .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;
     uint32_t a_nm, b_nm;
+    int cmp;
 
     a_nm = long_be(a->netmask.addr);
     b_nm = long_be(b->netmask.addr);
@@ -757,11 +523,9 @@
     if (b_nm < a_nm)
         return 1;
 
-    if (a->dest.addr < b->dest.addr)
-        return -1;
-
-    if (a->dest.addr > b->dest.addr)
-        return 1;
+    cmp = pico_ipv4_compare(&a->dest, &b->dest);
+    if (cmp)
+        return cmp;
 
     if (a->metric < b->metric)
         return -1;
@@ -772,34 +536,37 @@
     return 0;
 }
 
+
+static struct pico_ipv4_route default_bcast_route = {
+    .dest = {PICO_IP4_BCAST},
+    .netmask = {PICO_IP4_BCAST},
+    .gateway  = { 0 },
+    .link = NULL,
+    .metric = 1000
+};
+
+static struct pico_ipv4_route *route_find_default_bcast(void)
+{
+    return &default_bcast_route;
+}
+
+
 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)
-    {
+    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;
     }
 
-    return NULL;
+    return route_find_default_bcast();
 }
 
 struct pico_ip4 pico_ipv4_route_get_gateway(struct pico_ip4 *addr)
@@ -808,7 +575,7 @@
     struct pico_ipv4_route *route;
     nullip.addr = 0U;
 
-    if(!addr) {
+    if (!addr) {
         pico_err = PICO_ERR_EINVAL;
         return nullip;
     }
@@ -826,21 +593,49 @@
 {
     struct pico_ip4 *myself = NULL;
     struct pico_ipv4_route *rt;
+#ifdef PICO_SUPPORT_AODV
+    union pico_address node_address;
+    node_address.ip4.addr = dst->addr;
+    if (dst->addr && pico_ipv4_is_unicast(dst->addr))
+        pico_aodv_lookup(&node_address);
 
-    if(!dst) {
+#endif
+
+    if (!dst) {
         pico_err = PICO_ERR_EINVAL;
         return NULL;
     }
 
     rt = route_find(dst);
-    if (rt) {
+    if (rt && rt->link) {
         myself = &rt->link->address;
-    } else
+    } else {
         pico_err = PICO_ERR_EHOSTUNREACH;
+    }
 
     return myself;
 }
 
+struct pico_device *pico_ipv4_source_dev_find(const struct pico_ip4 *dst)
+{
+    struct pico_device *dev = NULL;
+    struct pico_ipv4_route *rt;
+
+    if (!dst) {
+        pico_err = PICO_ERR_EINVAL;
+        return NULL;
+    }
+
+    rt = route_find(dst);
+    if (rt && rt->link) {
+        dev = rt->link->dev;
+    } else {
+        pico_err = PICO_ERR_EHOSTUNREACH;
+    }
+
+    return dev;
+}
+
 
 #ifdef PICO_SUPPORT_MCAST
 /*                        link
@@ -859,25 +654,13 @@
 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;
-    }
+    return pico_ipv4_compare(&a->mcast_addr, &b->mcast_addr);
 }
 
 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;
+    return pico_ipv4_compare(a, b);
 }
 
 static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link)
@@ -894,12 +677,10 @@
     ip_mcast_dbg("+  nr  |    interface     | host group | reference count | filter mode |  source  +\n");
     ip_mcast_dbg("+---------------------------------------------------------------------------------+\n");
 
-    pico_tree_foreach(index, mcast_link->MCASTGroups)
-    {
+    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)
-        {
+        pico_tree_foreach(index2, &g->MCASTSources) {
             source = index2->keyValue;
             ip_mcast_dbg("+ %4s | %16s |  %8s  |      %5s      |      %s      | %08X +\n", "", "", "", "", "", source->addr);
         }
@@ -913,16 +694,14 @@
     struct pico_tree_node *index = NULL, *_tmp = NULL;
     struct pico_ip4 *source = NULL;
     /* cleanup filter */
-    pico_tree_foreach_safe(index, &g->MCASTSources, _tmp)
-    {
+    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)
-        {
+        pico_tree_foreach(index, MCASTFilter) {
             if (index->keyValue) {
                 source = PICO_ZALLOC(sizeof(struct pico_ip4));
                 if (!source) {
@@ -949,7 +728,8 @@
 
     if (mcast_link)
         link = pico_ipv4_link_get(mcast_link);
-    else
+
+    if (!link)
         link = mcast_default_link;
 
     test.mcast_addr = *mcast_group;
@@ -997,7 +777,8 @@
 
     if (mcast_link)
         link = pico_ipv4_link_get(mcast_link);
-    else
+
+    if (!link)
         link = mcast_default_link;
 
     test.mcast_addr = *mcast_group;
@@ -1009,8 +790,7 @@
         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)
-            {
+            pico_tree_foreach_safe(index, &g->MCASTSources, _tmp) {
                 source = index->keyValue;
                 pico_tree_delete(&g->MCASTSources, source);
                 PICO_FREE(source);
@@ -1044,19 +824,16 @@
 
     test.mcast_addr = hdr->dst;
 
-    pico_tree_foreach(index, &Tree_dev_link)
-    {
+    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)
-                {
+                switch (g->filter_mode) {
                 case PICO_IP_MULTICAST_INCLUDE:
-                    pico_tree_foreach(index2, &g->MCASTSources)
-                    {
+                    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;
@@ -1066,8 +843,7 @@
                     return -1;
 
                 case PICO_IP_MULTICAST_EXCLUDE:
-                    pico_tree_foreach(index2, &g->MCASTSources)
-                    {
+                    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;
@@ -1096,11 +872,13 @@
     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;
@@ -1108,15 +886,21 @@
 }
 #endif /* PICO_SUPPORT_MCAST */
 
+/* #define DEBUG_ROUTE */
 #ifdef DEBUG_ROUTE
-static void dbg_route(void)
+void dbg_route(void)
 {
     struct pico_ipv4_route *r;
     struct pico_tree_node *index;
-    pico_tree_foreach(index, &Routes){
+    int count_hosts = 0;
+    dbg("==== ROUTING TABLE =====\n");
+    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);
+        if (r->netmask.addr == 0xFFFFFFFF)
+            count_hosts++;
     }
+    dbg("================ total HOST nodes: %d ======\n\n\n", count_hosts);
 }
 #else
 #define dbg_route() do { } while(0)
@@ -1135,11 +919,12 @@
     struct pico_tree_node *index;
 #endif
 
-    if(!f || !dst) {
+    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");
@@ -1166,10 +951,11 @@
         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)
+                if (pico_udp_get_mc_ttl(f->sock, &ttl) < 0)
                     ttl = PICO_IP_DEFAULT_MULTICAST_TTL;
 
                 break;
+#ifdef PICO_SUPPORT_IGMP
             case PICO_PROTO_IGMP:
                 vhl = 0x46; /* header length 24 */
                 ttl = 1;
@@ -1187,6 +973,7 @@
                 }
 
                 break;
+#endif
             default:
                 ttl = PICO_IPV4_DEFAULT_TTL;
             }
@@ -1198,19 +985,27 @@
     hdr->vhl = vhl;
     hdr->len = short_be((uint16_t)(f->transport_len + f->net_len));
     if ((f->transport_hdr != f->payload)  &&
-#ifdef PICO_SUPPORT_IPFRAG
-        (0 == (f->frag & PICO_IPV4_MOREFRAG)) &&
+#ifdef PICO_SUPPORT_IPV4FRAG
+        ( (0 == (f->frag & PICO_IPV4_MOREFRAG)) ||
+          (0 == (f->frag & PICO_IPV4_FRAG_MASK)) )
+        &&
 #endif
         1 )
         ipv4_progressive_id++;
 
+    if (f->send_ttl > 0) {
+        ttl = f->send_ttl;
+    }
+
     hdr->id = short_be(ipv4_progressive_id);
     hdr->dst.addr = dst->addr;
     hdr->src.addr = link->address.addr;
     hdr->ttl = ttl;
+    hdr->tos = f->send_tos;
     hdr->proto = proto;
     hdr->frag = short_be(PICO_IPV4_DONTFRAG);
-#ifdef PICO_SUPPORT_IPFRAG
+
+#ifdef PICO_SUPPORT_IPV4FRAG
 #  ifdef PICO_SUPPORT_UDP
     if (proto == PICO_PROTO_UDP) {
         /* first fragment, can not use transport_len to calculate IP length */
@@ -1218,14 +1013,16 @@
             hdr->len = short_be((uint16_t)(f->payload_len + sizeof(struct pico_udp_hdr) + f->net_len));
 
         /* set fragmentation flags and offset calculated in socket layer */
-        hdr->frag = f->frag;
+        hdr->frag = short_be(f->frag);
     }
 
     if (proto == PICO_PROTO_ICMP4)
-        hdr->frag = f->frag;
+    {
+        hdr->frag = short_be(f->frag);
+    }
 
 #   endif
-#endif /* PICO_SUPPORT_IPFRAG */
+#endif /* PICO_SUPPORT_IPV4FRAG */
     pico_ipv4_checksum(f);
 
     if (f->sock && f->sock->dev) {
@@ -1233,6 +1030,8 @@
         f->dev = f->sock->dev;
     } else {
         f->dev = link->dev;
+        if (f->sock)
+            f->sock->dev = f->dev;
     }
 
 #ifdef PICO_SUPPORT_MCAST
@@ -1248,10 +1047,20 @@
 
 #endif
 
-    if(pico_ipv4_link_get(&hdr->dst)) {
+/* #ifdef PICO_SUPPORT_AODV */
+#if 0
+    {
+        union pico_address node_address;
+        node_address.ip4.addr = hdr->dst.addr;
+        if(hdr->dst.addr && pico_ipv4_is_unicast(hdr->dst.addr))
+            pico_aodv_lookup(&node_address);
+    }
+#endif
+
+    if (pico_ipv4_link_get(&hdr->dst)) {
         /* it's our own IP */
         return pico_enqueue(&in, f);
-    }else{
+    } else{
         /* TODO: Check if there are members subscribed here */
         return pico_enqueue(&out, f);
     }
@@ -1283,14 +1092,14 @@
 }
 
 
-int pico_ipv4_route_add(struct pico_ip4 address, struct pico_ip4 netmask, struct pico_ip4 gateway, int metric, struct pico_ipv4_link *link)
+int MOCKABLE 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 = (uint32_t)metric;
 
-    if(pico_tree_findKey(&Routes, &test)) {
+    if (pico_tree_findKey(&Routes, &test)) {
         pico_err = PICO_ERR_EINVAL;
         return -1;
     }
@@ -1332,7 +1141,7 @@
     }
 
     pico_tree_insert(&Routes, new);
-    /* dbg_route(); */
+    dbg_route();
     return 0;
 }
 
@@ -1350,7 +1159,7 @@
         pico_tree_delete(&Routes, found);
         PICO_FREE(found);
 
-        /* dbg_route(); */
+        dbg_route();
         return 0;
     }
 
@@ -1365,7 +1174,7 @@
     struct pico_ip4 network, gateway;
     char ipstr[30];
 
-    if(!dev) {
+    if (!dev) {
         pico_err = PICO_ERR_EINVAL;
         return -1;
     }
@@ -1375,8 +1184,7 @@
     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");
+    if (pico_tree_findKey(&Tree_dev_link, &test)) {
         pico_err = PICO_ERR_EADDRINUSE;
         return -1;
     }
@@ -1403,9 +1211,11 @@
 
     new->MCASTGroups->root = &LEAF;
     new->MCASTGroups->compare = ipv4_mcast_groups_cmp;
+#ifdef PICO_SUPPORT_IGMP
     new->mcast_compatibility = PICO_IGMPV3; /* default RFC 3376 $7.2.1 */
     new->mcast_last_query_interval = PICO_IGMP_QUERY_INTERVAL;
 #endif
+#endif
 
     pico_tree_insert(&Tree_dev_link, new);
 #ifdef PICO_SUPPORT_MCAST
@@ -1429,6 +1239,9 @@
     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);
+    if (default_bcast_route.link == NULL)
+        default_bcast_route.link = new;
+
     return 0;
 }
 
@@ -1437,8 +1250,7 @@
     struct pico_tree_node *index = NULL, *tmp = NULL;
     struct pico_ipv4_route *route = NULL;
 
-    pico_tree_foreach_safe(index, &Routes, tmp)
-    {
+    pico_tree_foreach_safe(index, &Routes, tmp) {
         route = index->keyValue;
         if (link == route->link)
             pico_ipv4_route_del(route->dest, route->netmask, (int)route->metric);
@@ -1446,11 +1258,17 @@
     return 0;
 }
 
+void MOCKABLE pico_ipv4_route_set_bcast_link(struct pico_ipv4_link *link)
+{
+    if (link)
+        default_bcast_route.link = link;
+}
+
 int pico_ipv4_link_del(struct pico_device *dev, struct pico_ip4 address)
 {
     struct pico_ipv4_link test, *found;
 
-    if(!dev) {
+    if (!dev) {
         pico_err = PICO_ERR_EINVAL;
         return -1;
     }
@@ -1477,8 +1295,7 @@
 
         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)
-        {
+        pico_tree_foreach_safe(index, found->MCASTGroups, _tmp) {
             g = index->keyValue;
             pico_tree_delete(found->MCASTGroups, g);
             PICO_FREE(g);
@@ -1488,6 +1305,9 @@
 
     pico_ipv4_cleanup_routes(found);
     pico_tree_delete(&Tree_dev_link, found);
+    if (default_bcast_route.link == found)
+        default_bcast_route.link = NULL;
+
     PICO_FREE(found);
 
     return 0;
@@ -1508,13 +1328,12 @@
         return found;
 }
 
-struct pico_ipv4_link *pico_ipv4_link_by_dev(struct pico_device *dev)
+struct pico_ipv4_link *MOCKABLE 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)
-    {
+    pico_tree_foreach(index, &Tree_dev_link) {
         link = index->keyValue;
         if (link->dev == dev)
             return link;
@@ -1531,8 +1350,7 @@
     if (last == NULL)
         valid = 1;
 
-    pico_tree_foreach(index, &Tree_dev_link)
-    {
+    pico_tree_foreach(index, &Tree_dev_link) {
         link = index->keyValue;
         if (link->dev == dev) {
             if (last == link)
@@ -1544,10 +1362,10 @@
     return NULL;
 }
 
-struct pico_device *pico_ipv4_link_find(struct pico_ip4 *address)
+struct pico_device *MOCKABLE pico_ipv4_link_find(struct pico_ip4 *address)
 {
     struct pico_ipv4_link test, *found;
-    if(!address) {
+    if (!address) {
         pico_err = PICO_ERR_EINVAL;
         return NULL;
     }
@@ -1564,10 +1382,10 @@
 }
 
 
-
 static int pico_ipv4_rebound_large(struct pico_frame *f)
 {
-    uint32_t total_payload_written = 0;
+#ifdef PICO_SUPPORT_IPV4FRAG
+    uint16_t total_payload_written = 0;
     uint32_t len = f->transport_len;
     struct pico_frame *fr;
     struct pico_ip4 dst;
@@ -1575,7 +1393,6 @@
     hdr = (struct pico_ipv4_hdr *) f->net_hdr;
     dst.addr = hdr->src.addr;
 
-#ifdef PICO_SUPPORT_IPFRAG
     while(total_payload_written < len) {
         uint32_t space = (uint32_t)len - total_payload_written;
         if (space > PICO_IPV4_MAXPAYLOAD)
@@ -1588,15 +1405,19 @@
         }
 
         if (space + total_payload_written < len)
-            fr->frag |= short_be(PICO_IPV4_MOREFRAG);
+        {
+            fr->frag |= PICO_IPV4_MOREFRAG;
+        }
         else
-            fr->frag &= short_be(PICO_IPV4_FRAG_MASK);
+        {
+            fr->frag &= PICO_IPV4_FRAG_MASK;
+        }
 
-        fr->frag |= short_be((uint16_t)((total_payload_written) >> 3u));
+        fr->frag = (((total_payload_written) >> 3u) & 0xffffu) | fr->frag;
 
         memcpy(fr->transport_hdr, f->transport_hdr + total_payload_written, fr->transport_len);
         if (pico_ipv4_frame_push(fr, &dst, hdr->proto) > 0) {
-            total_payload_written += fr->transport_len;
+            total_payload_written = (uint16_t)((uint16_t)fr->transport_len + total_payload_written);
         } else {
             pico_frame_discard(fr);
             break;
@@ -1604,6 +1425,7 @@
     } /* while() */
     return (int)total_payload_written;
 #else
+    (void)f;
     return -1;
 #endif
 }
@@ -1612,7 +1434,7 @@
 {
     struct pico_ip4 dst;
     struct pico_ipv4_hdr *hdr;
-    if(!f) {
+    if (!f) {
         pico_err = PICO_ERR_EINVAL;
         return -1;
     }
@@ -1631,6 +1453,60 @@
     return pico_ipv4_frame_push(f, &dst, hdr->proto);
 }
 
+static int pico_ipv4_pre_forward_checks(struct pico_frame *f)
+{
+    static uint16_t last_id = 0;
+    static uint16_t last_proto = 0;
+    static struct pico_ip4 last_src = {
+        0
+    };
+    static struct pico_ip4 last_dst = {
+        0
+    };
+    struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
+
+    /* Decrease TTL, check if expired */
+    hdr->ttl = (uint8_t)(hdr->ttl - 1);
+    if (hdr->ttl < 1) {
+        pico_notify_ttl_expired(f);
+        dbg(" ------------------- TTL EXPIRED\n");
+        return -1;
+    }
+
+    /* HACK: increase crc to compensate decreased TTL */
+    hdr->crc++;
+
+    /* If source is local, discard anyway (packets bouncing back and forth) */
+    if (pico_ipv4_link_get(&hdr->src))
+        return -1;
+
+    /* If this was the last forwarded packet, silently discard to prevent duplications */
+    if ((last_src.addr == hdr->src.addr) && (last_id == hdr->id)
+        && (last_dst.addr == hdr->dst.addr) && (last_proto == hdr->proto)) {
+        return -1;
+    } else {
+        last_src.addr = hdr->src.addr;
+        last_dst.addr = hdr->dst.addr;
+        last_id = hdr->id;
+        last_proto = hdr->proto;
+    }
+
+    return 0;
+}
+
+static int pico_ipv4_forward_check_dev(struct pico_frame *f)
+{
+    if (f->dev->eth != NULL)
+        f->len -= PICO_SIZE_ETHHDR;
+
+    if (f->len > f->dev->mtu) {
+        pico_notify_pkt_too_big(f);
+        return -1;
+    }
+
+    return 0;
+}
+
 static int pico_ipv4_forward(struct pico_frame *f)
 {
     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *)f->net_hdr;
@@ -1646,20 +1522,16 @@
     }
 
     f->dev = rt->link->dev;
-    hdr->ttl = (uint8_t)(hdr->ttl - 1);
-    if (hdr->ttl < 1) {
-        pico_notify_ttl_expired(f);
-        dbg(" ------------------- TTL EXPIRED\n");
+
+    if (pico_ipv4_pre_forward_checks(f) < 0)
         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;
+
+    if (pico_ipv4_forward_check_dev(f) < 0)
+        return -1;
 
     pico_sendto_dev(f);
     return 0;
@@ -1695,8 +1567,7 @@
     struct pico_tree_node *index = NULL, *_tmp = NULL;
     struct pico_ipv4_link *link = NULL;
 
-    pico_tree_foreach_safe(index, &Tree_dev_link, _tmp)
-    {
+    pico_tree_foreach_safe(index, &Tree_dev_link, _tmp) {
         link = index->keyValue;
         if (dev == link->dev)
             pico_ipv4_link_del(dev, link->address);