Diff: modules/pico_ipv4.c

Revision:

149:5f4cb161cec3

Parent:

137:a1c8bfa9d691

Child:

152:a3d286bf94e5

--- a/modules/pico_ipv4.c	Tue Mar 11 15:34:51 2014 +0100
+++ b/modules/pico_ipv4.c	Wed Apr 09 14:31:41 2014 +0200
@@ -19,16 +19,19 @@
 #include "pico_nat.h"
 #include "pico_igmp.h"
 #include "pico_tree.h"
+#include "pico_socket_multicast.h"
 
 #ifdef PICO_SUPPORT_IPV4
 
 #ifdef PICO_SUPPORT_MCAST
 # define ip_mcast_dbg(...) do {} while(0) /* so_mcast_dbg in pico_socket.c */
+/* #define ip_mcast_dbg dbg */
 # define PICO_MCAST_ALL_HOSTS long_be(0xE0000001) /* 224.0.0.1 */
 /* Default network interface for multicast transmission */
 static struct pico_ipv4_link *mcast_default_link = NULL;
 #endif
 #ifdef PICO_SUPPORT_IPFRAG
+/* # define reassembly_dbg dbg */
 # define reassembly_dbg(...) do {} while(0)
 #endif
 
@@ -42,6 +45,7 @@
 
 /* Functions */
 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_to_string(char *ipbuf, const uint32_t ip)
 {
@@ -74,6 +78,18 @@
     return 0;
 }
 
+static int pico_string_check_null_args(const char *ipstr, uint32_t *ip)
+{
+
+    if(!ipstr || !ip) {
+        pico_err = PICO_ERR_EINVAL;
+        return -1;
+    }
+
+    return 0;
+
+}
+
 int pico_string_to_ipv4(const char *ipstr, uint32_t *ip)
 {
     unsigned char buf[4] = {
@@ -82,10 +98,9 @@
     int cnt = 0;
     char p;
 
-    if(!ipstr || !ip) {
-        pico_err = PICO_ERR_EINVAL;
+    if (pico_string_check_null_args(ipstr, ip) < 0)
         return -1;
-    }
+
 
     while((p = *ipstr++) != 0)
     {
@@ -172,7 +187,12 @@
     return 0;
 }
 
-int pico_ipv4_is_valid_src(uint32_t address)
+static int pico_ipv4_is_invalid_loopback(uint32_t address, struct pico_device *dev)
+{
+    return pico_ipv4_is_loopback(address) && ((!dev) || strcmp(dev->name, "loop"));
+}
+
+int pico_ipv4_is_valid_src(uint32_t address, struct pico_device *dev)
 {
     if (pico_ipv4_is_broadcast(address)) {
         dbg("Source is a broadcast address, discard packet\n");
@@ -182,7 +202,7 @@
         dbg("Source is a multicast address, discard packet\n");
         return 0;
     }
-    else if (pico_ipv4_is_loopback(address)) {
+    else if (pico_ipv4_is_invalid_loopback(address, dev)) {
         dbg("Source is a loopback address, discard packet\n");
         return 0;
     }
@@ -246,12 +266,16 @@
 {
     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 (short_be((a->frag & PICO_IPV4_FRAG_MASK)) < short_be((b->frag & PICO_IPV4_FRAG_MASK)))
+    if (a_frag < b_frag)
         return -1;
-    else if (short_be((a->frag & PICO_IPV4_FRAG_MASK)) > short_be((b->frag & PICO_IPV4_FRAG_MASK)))
+
+    if (b_frag < a_frag)
         return 1;
     else
         return 0;
@@ -271,8 +295,8 @@
         pico_frame_discard(f_frag);
     }
     pico_tree_delete(&pico_ipv4_fragmented_tree, pfrag);
-    pico_free(pfrag->t);
-    pico_free(pfrag);
+    PICO_FREE(pfrag->t);
+    PICO_FREE(pfrag);
 }
 #endif /* PICO_SUPPORT_IPFRAG */
 
@@ -298,8 +322,6 @@
     uint16_t offset = 0;
     uint16_t data_len = 0;
     struct pico_ipv4_hdr *f_frag_hdr = NULL, *hdr = (struct pico_ipv4_hdr *) (*f)->net_hdr;
-    struct pico_udp_hdr *udp_hdr = NULL;
-    struct pico_tcp_hdr *tcp_hdr = NULL;
     struct pico_ipv4_fragmented_packet *pfrag = NULL;
     struct pico_frame *f_new = NULL, *f_frag = NULL;
     struct pico_tree_node *index, *_tmp;
@@ -317,7 +339,7 @@
             }
 
             /* add entry in tree for this ID and create secondary tree to contain fragmented elements */
-            pfrag = pico_zalloc(sizeof(struct pico_ipv4_fragmented_packet));
+            pfrag = PICO_ZALLOC(sizeof(struct pico_ipv4_fragmented_packet));
             if (!pfrag) {
                 pico_err = PICO_ERR_ENOMEM;
                 return -1;
@@ -328,9 +350,9 @@
             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));
+            pfrag->t = PICO_ZALLOC(sizeof(struct pico_tree));
             if (!pfrag->t) {
-                pico_free(pfrag);
+                PICO_FREE(pfrag);
                 pico_err = PICO_ERR_ENOMEM;
                 return -1;
             }
@@ -378,6 +400,8 @@
             }
 
             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);
@@ -385,6 +409,7 @@
             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;
@@ -412,7 +437,7 @@
                 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);
+            PICO_FREE(pfrag);
 
             data_len = (uint16_t)(short_be(hdr->len) - (*f)->net_len);
             memcpy(running_pointer, (*f)->transport_hdr, data_len);
@@ -429,12 +454,14 @@
             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_ipv4(f_new));
+                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));
@@ -511,14 +538,98 @@
 
 PICO_TREE_DECLARE(Tree_dev_link, ipv4_link_compare);
 
+static int pico_ipv4_process_bcast_in(struct pico_frame *f)
+{
+    struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+#ifdef PICO_SUPPORT_UDP
+    if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_UDP)) {
+        /* Receiving UDP broadcast datagram */
+        f->flags |= PICO_FRAME_FLAG_BCAST;
+        pico_enqueue(pico_proto_udp.q_in, f);
+        return 1;
+    }
+
+    if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_ICMP4)) {
+        /* Receiving ICMP4 bcast packet */
+        f->flags |= PICO_FRAME_FLAG_BCAST;
+        pico_enqueue(pico_proto_icmp4.q_in, f);
+        return 1;
+    }
+
+#endif
+    return 0;
+}
+
+static int pico_ipv4_process_mcast_in(struct pico_frame *f)
+{
+    struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+    if (pico_ipv4_is_multicast(hdr->dst.addr)) {
+#ifdef PICO_SUPPORT_MCAST
+        /* Receiving UDP multicast datagram TODO set f->flags? */
+        if (hdr->proto == PICO_PROTO_IGMP) {
+            ip_mcast_dbg("MCAST: received IGMP message\n");
+            pico_transport_receive(f, PICO_PROTO_IGMP);
+            return 1;
+        } else if ((pico_ipv4_mcast_filter(f) == 0) && (hdr->proto == PICO_PROTO_UDP)) {
+            pico_enqueue(pico_proto_udp.q_in, f);
+            return 1;
+        }
+
+#endif
+        pico_frame_discard(f);
+        return 1;
+    }
+
+    return 0;
+}
+
+static int pico_ipv4_process_local_unicast_in(struct pico_frame *f)
+{
+    struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
+    struct pico_ipv4_link test = {
+        .address = {.addr = PICO_IP4_ANY}, .dev = NULL
+    };
+    if (pico_ipv4_link_find(&hdr->dst)) {
+        if (pico_ipv4_nat_inbound(f, &hdr->dst) == 0)
+            pico_enqueue(pico_proto_ipv4.q_in, f); /* dst changed, reprocess */
+        else
+            pico_transport_receive(f, hdr->proto);
+
+        return 1;
+    } else if (pico_tree_findKey(&Tree_dev_link, &test)) {
+#ifdef PICO_SUPPORT_UDP
+        /* address of this device is apparently 0.0.0.0; might be a DHCP packet */
+        /* XXX KRO: is obsolete. Broadcast flag is set on outgoing DHCP messages.
+         * incomming DHCP messages are to be broadcasted. Our current DHCP server
+         * implementation does not take this flag into account yet though ... */
+        pico_enqueue(pico_proto_udp.q_in, f);
+        return 1;
+#endif
+    }
+
+    return 0;
+}
+
+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)))
+    {
+        /* don't forward broadcast frame, discard! */
+        pico_frame_discard(f);
+    } else if (pico_ipv4_forward(f) != 0) {
+        pico_frame_discard(f);
+        /* dbg("Forward failed.\n"); */
+    }
+}
+
+
+
 static int pico_ipv4_process_in(struct pico_protocol *self, struct pico_frame *f)
 {
     uint8_t option_len = 0;
     int ret = 0;
     struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
-    struct pico_ipv4_link test = {
-        .address = {.addr = PICO_IP4_ANY}, .dev = NULL
-    };
 
     /* NAT needs transport header information */
     if(((hdr->vhl) & 0x0F) > 5) {
@@ -547,7 +658,7 @@
         return ret;
 
     /* Validate source IP address. Discard quietly if invalid */
-    if (!pico_ipv4_is_valid_src(hdr->src.addr)) {
+    if (!pico_ipv4_is_valid_src(hdr->src.addr, f->dev)) {
         pico_frame_discard(f);
         return 0;
     }
@@ -557,50 +668,16 @@
         return 0;
     }
 
-    if (0) {
-#ifdef PICO_SUPPORT_UDP
-    } else if (pico_ipv4_is_broadcast(hdr->dst.addr) && (hdr->proto == PICO_PROTO_UDP)) {
-        /* Receiving UDP broadcast datagram */
-        f->flags |= PICO_FRAME_FLAG_BCAST;
-        pico_enqueue(pico_proto_udp.q_in, f);
-#endif
-    } else if (pico_ipv4_is_multicast(hdr->dst.addr)) {
-#ifdef PICO_SUPPORT_MCAST
-        /* Receiving UDP multicast datagram TODO set f->flags? */
-        if (hdr->proto == PICO_PROTO_IGMP) {
-            ip_mcast_dbg("MCAST: received IGMP message\n");
-            pico_transport_receive(f, PICO_PROTO_IGMP);
-        } else if ((pico_ipv4_mcast_filter(f) == 0) && (hdr->proto == PICO_PROTO_UDP)) {
-            pico_enqueue(pico_proto_udp.q_in, f);
-        } else {
-            pico_frame_discard(f);
-        }
+    if (pico_ipv4_process_bcast_in(f) > 0)
+        return 0;
 
-#endif
-    } else if (pico_ipv4_link_find(&hdr->dst)) {
-        if (pico_ipv4_nat_inbound(f, &hdr->dst) == 0)
-            pico_enqueue(pico_proto_ipv4.q_in, f); /* dst changed, reprocess */
-        else
-            pico_transport_receive(f, hdr->proto);
-    } else if (pico_tree_findKey(&Tree_dev_link, &test)) {
-#ifdef PICO_SUPPORT_UDP
-        /* address of this device is apparently 0.0.0.0; might be a DHCP packet */
-        /* XXX KRO: is obsolete. Broadcast flag is set on outgoing DHCP messages.
-         * incomming DHCP messages are to be broadcasted. Our current DHCP server
-         * implementation does not take this flag into account yet though ... */
-        pico_enqueue(pico_proto_udp.q_in, f);
-#endif
-    } else {
+    if (pico_ipv4_process_mcast_in(f) > 0)
+        return 0;
 
-        if((pico_ipv4_is_broadcast(hdr->dst.addr)))
-        {
-            /* don't forward broadcast frame, discard! */
-            pico_frame_discard(f);
-        } else if (pico_ipv4_forward(f) != 0) {
-            pico_frame_discard(f);
-            /* dbg("Forward failed.\n"); */
-        }
-    }
+    if (pico_ipv4_process_local_unicast_in(f) > 0)
+        return 0;
+
+    pico_ipv4_process_finally_try_forward(f);
 
     return 0;
 }
@@ -668,12 +745,16 @@
 static int ipv4_route_compare(void *ka, void *kb)
 {
     struct pico_ipv4_route *a = ka, *b = kb;
+    uint32_t a_nm, b_nm;
+
+    a_nm = long_be(a->netmask.addr);
+    b_nm = long_be(b->netmask.addr);
 
     /* Routes are sorted by (host side) netmask len, then by addr, then by metric. */
-    if (long_be(a->netmask.addr) < long_be(b->netmask.addr))
+    if (a_nm < b_nm)
         return -1;
 
-    if (long_be(a->netmask.addr) > long_be(b->netmask.addr))
+    if (b_nm < a_nm)
         return 1;
 
     if (a->dest.addr < b->dest.addr)
@@ -802,9 +883,10 @@
 static void pico_ipv4_mcast_print_groups(struct pico_ipv4_link *mcast_link)
 {
     uint16_t i = 0;
-    struct pico_mcast_group __attribute__ ((unused)) *g = NULL;
-    struct pico_ip4 __attribute__ ((unused)) *source = NULL;
+    struct pico_mcast_group *g = NULL;
+    struct pico_ip4 *source = NULL;
     struct pico_tree_node *index = NULL, *index2 = NULL;
+    (void) source;
 
     ip_mcast_dbg("+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
     ip_mcast_dbg("+                           MULTICAST list interface %-16s             +\n", mcast_link->dev->name);
@@ -835,20 +917,22 @@
     {
         source = index->keyValue;
         pico_tree_delete(&g->MCASTSources, source);
-        pico_free(source);
+        PICO_FREE(source);
     }
     /* insert new filter */
     if (MCASTFilter) {
         pico_tree_foreach(index, MCASTFilter)
         {
-            source = pico_zalloc(sizeof(struct pico_ip4));
-            if (!source) {
-                pico_err = PICO_ERR_ENOMEM;
-                return -1;
+            if (index->keyValue) {
+                source = PICO_ZALLOC(sizeof(struct pico_ip4));
+                if (!source) {
+                    pico_err = PICO_ERR_ENOMEM;
+                    return -1;
+                }
+
+                source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
+                pico_tree_insert(&g->MCASTSources, source);
             }
-
-            source->addr = ((struct pico_ip4 *)index->keyValue)->addr;
-            pico_tree_insert(&g->MCASTSources, source);
         }
     }
 
@@ -876,7 +960,7 @@
 
         pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
     } else {
-        g = pico_zalloc(sizeof(struct pico_mcast_group));
+        g = PICO_ZALLOC(sizeof(struct pico_mcast_group));
         if (!g) {
             pico_err = PICO_ERR_ENOMEM;
             return -1;
@@ -892,8 +976,10 @@
         pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_CREATE);
     }
 
-    if (mcast_group_update(g, MCASTFilter, filter_mode) < 0)
+    if (mcast_group_update(g, MCASTFilter, filter_mode) < 0) {
+        dbg("Error in mcast_group update\n");
         return -1;
+    }
 
     pico_ipv4_mcast_print_groups(link);
     return 0;
@@ -927,10 +1013,10 @@
             {
                 source = index->keyValue;
                 pico_tree_delete(&g->MCASTSources, source);
-                pico_free(source);
+                PICO_FREE(source);
             }
             pico_tree_delete(link->MCASTGroups, g);
-            pico_free(g);
+            PICO_FREE(g);
         } else {
             pico_igmp_state_change(mcast_link, mcast_group, filter_mode, MCASTFilter, PICO_IGMP_STATE_UPDATE);
             if (mcast_group_update(g, MCASTFilter, filter_mode) < 0)
@@ -1111,7 +1197,11 @@
 
     hdr->vhl = vhl;
     hdr->len = short_be((uint16_t)(f->transport_len + f->net_len));
-    if (f->transport_hdr != f->payload)
+    if ((f->transport_hdr != f->payload)  &&
+#ifdef PICO_SUPPORT_IPFRAG
+        (0 == (f->frag & PICO_IPV4_MOREFRAG)) &&
+#endif
+        1 )
         ipv4_progressive_id++;
 
     hdr->id = short_be(ipv4_progressive_id);
@@ -1131,7 +1221,10 @@
         hdr->frag = f->frag;
     }
 
-#  endif /* PICO_SUPPORT_UDP */
+    if (proto == PICO_PROTO_ICMP4)
+        hdr->frag = f->frag;
+
+#   endif
 #endif /* PICO_SUPPORT_IPFRAG */
     pico_ipv4_checksum(f);
 
@@ -1172,7 +1265,7 @@
 static int pico_ipv4_frame_sock_push(struct pico_protocol *self, struct pico_frame *f)
 {
     struct pico_ip4 *dst;
-    struct pico_remote_duple *remote_duple = (struct pico_remote_duple *) f->info;
+    struct pico_remote_endpoint *remote_endpoint = (struct pico_remote_endpoint *) f->info;
     IGNORE_PARAMETER(self);
 
     if (!f->sock) {
@@ -1180,8 +1273,8 @@
         return -1;
     }
 
-    if (remote_duple) {
-        dst = &remote_duple->remote_addr.ip4;
+    if (remote_endpoint) {
+        dst = &remote_endpoint->remote_addr.ip4;
     } else {
         dst = &f->sock->remote_addr.ip4;
     }
@@ -1202,7 +1295,7 @@
         return -1;
     }
 
-    new = pico_zalloc(sizeof(struct pico_ipv4_route));
+    new = PICO_ZALLOC(sizeof(struct pico_ipv4_route));
     if (!new) {
         pico_err = PICO_ERR_ENOMEM;
         return -1;
@@ -1219,13 +1312,13 @@
         struct pico_ipv4_route *r = route_find(&gateway);
         if (!r ) { /* Specified Gateway is unreachable */
             pico_err = PICO_ERR_EHOSTUNREACH;
-            pico_free(new);
+            PICO_FREE(new);
             return -1;
         }
 
         if (r->gateway.addr) { /* Specified Gateway is not a neighbor */
             pico_err = PICO_ERR_ENETUNREACH;
-            pico_free(new);
+            PICO_FREE(new);
             return -1;
         }
 
@@ -1234,7 +1327,7 @@
 
     if (!new->link) {
         pico_err = PICO_ERR_EINVAL;
-        pico_free(new);
+        PICO_FREE(new);
         return -1;
     }
 
@@ -1255,7 +1348,7 @@
     if (found) {
 
         pico_tree_delete(&Routes, found);
-        pico_free(found);
+        PICO_FREE(found);
 
         /* dbg_route(); */
         return 0;
@@ -1289,7 +1382,7 @@
     }
 
     /** XXX: Check for network already in use (e.g. trying to assign 10.0.0.1/24 where 10.1.0.1/8 is in use) **/
-    new = pico_zalloc(sizeof(struct pico_ipv4_link));
+    new = PICO_ZALLOC(sizeof(struct pico_ipv4_link));
     if (!new) {
         dbg("IPv4: Out of memory!\n");
         pico_err = PICO_ERR_ENOMEM;
@@ -1300,9 +1393,9 @@
     new->netmask.addr = netmask.addr;
     new->dev = dev;
 #ifdef PICO_SUPPORT_MCAST
-    new->MCASTGroups = pico_zalloc(sizeof(struct pico_tree));
+    new->MCASTGroups = PICO_ZALLOC(sizeof(struct pico_tree));
     if (!new->MCASTGroups) {
-        pico_free(new);
+        PICO_FREE(new);
         dbg("IPv4: Out of memory!\n");
         pico_err = PICO_ERR_ENOMEM;
         return -1;
@@ -1388,14 +1481,14 @@
         {
             g = index->keyValue;
             pico_tree_delete(found->MCASTGroups, g);
-            pico_free(g);
+            PICO_FREE(g);
         }
     } while(0);
 #endif
 
     pico_ipv4_cleanup_routes(found);
     pico_tree_delete(&Tree_dev_link, found);
-    pico_free(found);
+    PICO_FREE(found);
 
     return 0;
 }
@@ -1470,6 +1563,51 @@
     return found->dev;
 }
 
+
+
+static int pico_ipv4_rebound_large(struct pico_frame *f)
+{
+    uint32_t total_payload_written = 0;
+    uint32_t len = f->transport_len;
+    struct pico_frame *fr;
+    struct pico_ip4 dst;
+    struct pico_ipv4_hdr *hdr;
+    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)
+            space = PICO_IPV4_MAXPAYLOAD;
+
+        fr = pico_ipv4_alloc(&pico_proto_ipv4, (uint16_t)space);
+        if (!fr) {
+            pico_err = PICO_ERR_ENOMEM;
+            return -1;
+        }
+
+        if (space + total_payload_written < len)
+            fr->frag |= short_be(PICO_IPV4_MOREFRAG);
+        else
+            fr->frag &= short_be(PICO_IPV4_FRAG_MASK);
+
+        fr->frag |= short_be((uint16_t)((total_payload_written) >> 3u));
+
+        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;
+        } else {
+            pico_frame_discard(fr);
+            break;
+        }
+    } /* while() */
+    return (int)total_payload_written;
+#else
+    return -1;
+#endif
+}
+
 int pico_ipv4_rebound(struct pico_frame *f)
 {
     struct pico_ip4 dst;
@@ -1486,6 +1624,10 @@
     }
 
     dst.addr = hdr->src.addr;
+    if (f->transport_len > PICO_IPV4_MAXPAYLOAD) {
+        return pico_ipv4_rebound_large(f);
+    }
+
     return pico_ipv4_frame_push(f, &dst, hdr->proto);
 }
 
@@ -1503,9 +1645,6 @@
         return -1;
     }
 
-    if (f->dev == rt->link->dev)
-        return -1;
-
     f->dev = rt->link->dev;
     hdr->ttl = (uint8_t)(hdr->ttl - 1);
     if (hdr->ttl < 1) {
@@ -1551,4 +1690,19 @@
 #endif
 }
 
+int pico_ipv4_cleanup_links(struct pico_device *dev)
+{
+    struct pico_tree_node *index = NULL, *_tmp = NULL;
+    struct pico_ipv4_link *link = NULL;
+
+    pico_tree_foreach_safe(index, &Tree_dev_link, _tmp)
+    {
+        link = index->keyValue;
+        if (dev == link->dev)
+            pico_ipv4_link_del(dev, link->address);
+    }
+    return 0;
+}
+
+
 #endif