Diff: stack/pico_socket.c

Revision:

152:a3d286bf94e5

Parent:

150:551effcf6a39

--- a/stack/pico_socket.c	Wed Apr 09 17:32:25 2014 +0200
+++ b/stack/pico_socket.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.
 
 
@@ -28,22 +28,11 @@
 
 
 #define PROTO(s) ((s)->proto->proto_number)
-#define PICO_SOCKET4_MTU 1480 /* Ethernet MTU(1500) - IP header size(20) */
-#define PICO_SOCKET6_MTU 1460 /* Ethernet MTU(1500) - IP header size(40) */
+#define PICO_MIN_MSS (1280)
 #define TCP_STATE(s) (s->state & PICO_SOCKET_STATE_TCP)
 
 #ifdef PICO_SUPPORT_MUTEX
 static void *Mutex = NULL;
-#define PICOTCP_MUTEX_LOCK(x) { \
-        if (x == NULL) \
-            x = pico_mutex_init(); \
-        pico_mutex_lock(x); \
-}
-#define PICOTCP_MUTEX_UNLOCK(x) pico_mutex_unlock(x)
-
-#else
-#define PICOTCP_MUTEX_LOCK(x) do {} while(0)
-#define PICOTCP_MUTEX_UNLOCK(x) do {} while(0)
 #endif
 
 
@@ -51,18 +40,6 @@
 
 #define PICO_SOCKET_MTU 1480 /* Ethernet MTU(1500) - IP header size(20) */
 
-#ifdef PICO_SUPPORT_IPV4
-# define IS_SOCK_IPV4(s) ((s->net == &pico_proto_ipv4))
-#else
-# define IS_SOCK_IPV4(s) (0)
-#endif
-
-#ifdef PICO_SUPPORT_IPV6
-# define IS_SOCK_IPV6(s) ((s->net == &pico_proto_ipv6))
-#else
-# define IS_SOCK_IPV6(s) (0)
-#endif
-
 # define frag_dbg(...) do {} while(0)
 
 
@@ -171,7 +148,7 @@
 
 #define INIT_SOCKPORT { {&LEAF, socket_cmp}, 0, 0 }
 
-int sockport_cmp(void *ka, void *kb)
+static int sockport_cmp(void *ka, void *kb)
 {
     struct pico_sockport *a = ka, *b = kb;
     if (a->number < b->number)
@@ -286,9 +263,9 @@
     struct pico_ip6 ip;
     /* IPv6 */
     if (addr)
-        memcpy(&ip.addr, ((struct pico_ip6 *)addr)->addr, sizeof(struct pico_ip6));
+        memcpy(ip.addr, ((struct pico_ip6 *)addr)->addr, sizeof(struct pico_ip6));
     else
-        memcpy(&ip.addr, PICO_IP6_ANY, sizeof(struct pico_ip6));
+        memcpy(ip.addr, PICO_IP6_ANY, sizeof(struct pico_ip6));
 
     if (memcmp(ip.addr, PICO_IP6_ANY, sizeof(struct pico_ip6)) ==  0) {
         if (!sp)
@@ -305,22 +282,28 @@
 
 
 
-static int pico_generic_port_in_use(uint16_t proto, uint16_t port, struct pico_sockport *sp, void *addr)
+static int pico_generic_port_in_use(uint16_t proto, uint16_t port, struct pico_sockport *sp, void *addr, void *net)
 {
 #ifdef PICO_SUPPORT_IPV4
-    if (pico_port_in_use_by_nat(proto, port)) {
-        return 1;
-    }
+    if (net == &pico_proto_ipv4)
+    {
+        if (pico_port_in_use_by_nat(proto, port)) {
+            return 1;
+        }
 
-    if (pico_port_in_use_ipv4(sp, addr)) {
-        return 1;
+        if (pico_port_in_use_ipv4(sp, addr)) {
+            return 1;
+        }
     }
 
 #endif
 
 #ifdef PICO_SUPPORT_IPV6
-    if (pico_port_in_use_ipv6(sp, addr)) {
-        return 1;
+    if (net == &pico_proto_ipv6)
+    {
+        if (pico_port_in_use_ipv6(sp, addr)) {
+            return 1;
+        }
     }
 
 #endif
@@ -331,10 +314,9 @@
 int pico_is_port_free(uint16_t proto, uint16_t port, void *addr, void *net)
 {
     struct pico_sockport *sp;
-    (void) net;
     sp = pico_get_sockport(proto, port);
 
-    if (pico_generic_port_in_use(proto, port, sp, addr))
+    if (pico_generic_port_in_use(proto, port, sp, addr, net))
         return 0;
 
     return 1;
@@ -417,7 +399,7 @@
     pico_tree_insert(&sp->socks, s);
     s->state |= PICO_SOCKET_STATE_BOUND;
     PICOTCP_MUTEX_UNLOCK(Mutex);
-#if DEBUG_SOCKET_TREE
+#ifdef DEBUG_SOCKET_TREE
     {
         struct pico_tree_node *index;
         /* RB_FOREACH(s, socket_tree, &sp->socks) { */
@@ -450,6 +432,8 @@
             f_out = pico_dequeue(&sock->q_out);
         }
     }
+    pico_queue_deinit(&sock->q_in);
+    pico_queue_deinit(&sock->q_out);
     pico_socket_tcp_cleanup(sock);
 }
 
@@ -499,7 +483,7 @@
     pico_multicast_delete(s);
     pico_socket_tcp_delete(s);
     s->state = PICO_SOCKET_STATE_CLOSED;
-    pico_timer_add(3000, socket_garbage_collect, s);
+    pico_timer_add((pico_time)10, socket_garbage_collect, s);
     PICOTCP_MUTEX_UNLOCK(Mutex);
     return 0;
 }
@@ -536,12 +520,18 @@
 
 static int pico_socket_transport_deliver(struct pico_protocol *p, struct pico_sockport *sp, struct pico_frame *f)
 {
+#ifdef PICO_SUPPORT_TCP
     if (p->proto_number == PICO_PROTO_TCP)
         return pico_socket_tcp_deliver(sp, f);
 
+#endif
+
+#ifdef PICO_SUPPORT_UDP
     if (p->proto_number == PICO_PROTO_UDP)
         return pico_socket_udp_deliver(sp, f);
 
+#endif
+
     return -1;
 }
 
@@ -588,12 +578,19 @@
 static struct pico_socket *pico_socket_transport_open(uint16_t proto, uint16_t family)
 {
     struct pico_socket *s = NULL;
+    (void)family;
+#ifdef PICO_SUPPORT_UDP
     if (proto == PICO_PROTO_UDP)
         s = pico_socket_udp_open();
 
+#endif
+
+#ifdef PICO_SUPPORT_TCP
     if (proto == PICO_PROTO_TCP)
         s = pico_socket_tcp_open(family);
 
+#endif
+
     return s;
 
 }
@@ -676,7 +673,15 @@
 static int pico_socket_transport_read(struct pico_socket *s, void *buf, int len)
 {
     if (PROTO(s) == PICO_PROTO_UDP)
+    {
+        /* make sure cast to uint16_t doesn't give unexpected results */
+        if(len > 0xFFFF) {
+            pico_err = PICO_ERR_EINVAL;
+            return -1;
+        }
+
         return pico_socket_udp_recv(s, buf, (uint16_t)len, NULL, NULL);
+    }
     else if (PROTO(s) == PICO_PROTO_TCP)
         return pico_socket_tcp_read(s, buf, (uint32_t)len);
     else return 0;
@@ -750,14 +755,14 @@
     return pico_socket_write_attempt(s, buf, len);
 }
 
-uint16_t pico_socket_high_port(uint16_t proto)
+static uint16_t pico_socket_high_port(uint16_t proto)
 {
     uint16_t port;
     if (0 ||
 #ifdef PICO_SUPPORT_TCP
         (proto == PICO_PROTO_TCP) ||
 #endif
-#ifdef PICO_SUPPORT_TCP
+#ifdef PICO_SUPPORT_UDP
         (proto == PICO_PROTO_UDP) ||
 #endif
         0) {
@@ -869,9 +874,7 @@
 
 static int pico_socket_sendto_initial_checks(struct pico_socket *s, const void *buf, const int len, void *dst, uint16_t remote_port)
 {
-    if (len == 0) {
-        return 0;
-    } else if (len < 0) {
+    if (len < 0) {
         pico_err = PICO_ERR_EINVAL;
         return -1;
     }
@@ -969,6 +972,8 @@
             pico_err = PICO_ERR_EINVAL;
             return -1;
         }
+
+        pico_socket_alter_state(s, PICO_SOCKET_STATE_BOUND, 0, 0);
     }
 
     return s->local_port;
@@ -1007,8 +1012,10 @@
 
 static void pico_xmit_frame_set_nofrag(struct pico_frame *f)
 {
-#ifdef PICO_SUPPORT_IPFRAG
-    f->frag = short_be(PICO_IPV4_DONTFRAG);
+#ifdef PICO_SUPPORT_IPV4FRAG
+    f->frag = PICO_IPV4_DONTFRAG;
+#else
+    (void)f;
 #endif
 }
 
@@ -1022,13 +1029,14 @@
     }
 }
 
-static int pico_socket_xmit_one(struct pico_socket *s, const void *buf, const int len, void *src, struct pico_remote_endpoint *ep)
+static int pico_socket_xmit_one(struct pico_socket *s, const void *buf, const int len, void *src,
+                                struct pico_remote_endpoint *ep, struct pico_msginfo *msginfo)
 {
     struct pico_frame *f;
     uint16_t hdr_offset = (uint16_t)pico_socket_sendto_transport_offset(s);
     int ret = 0;
     (void)src;
-
+    
     f = pico_socket_frame_alloc(s, (uint16_t)(len + hdr_offset));
     if (!f) {
         pico_err = PICO_ERR_ENOMEM;
@@ -1040,7 +1048,7 @@
     f->sock = s;
     transport_flags_update(f, s);
     pico_xmit_frame_set_nofrag(f);
-    if (ep) {
+    if (ep && !f->info) {
         f->info = pico_socket_set_info(ep);
         if (!f->info) {
             pico_frame_discard(f);
@@ -1048,6 +1056,19 @@
         }
     }
 
+    if (msginfo) {
+        f->send_ttl = (uint8_t)msginfo->ttl;
+        f->send_tos = (uint8_t)msginfo->tos;
+        f->dev = msginfo->dev;
+    }
+#ifdef PICO_SUPPORT_IPV6
+    if(IS_SOCK_IPV6(s) && ep && pico_ipv6_is_multicast(&ep->remote_addr.ip6.addr[0])) {
+        f->dev = pico_ipv6_link_find(src);
+        if(!f->dev) {
+            return -1;
+        }
+    }
+#endif
     memcpy(f->payload, (const uint8_t *)buf, f->payload_len);
     /* dbg("Pushing segment, hdr len: %d, payload_len: %d\n", header_offset, f->payload_len); */
     ret = pico_socket_final_xmit(s, f);
@@ -1056,57 +1077,67 @@
 
 static int pico_socket_xmit_avail_space(struct pico_socket *s);
 
+#ifdef PICO_SUPPORT_IPV4FRAG
 static void pico_socket_xmit_first_fragment_setup(struct pico_frame *f, int space, int hdr_offset)
 {
     frag_dbg("FRAG: first fragmented frame %p | len = %u offset = 0\n", f, f->payload_len);
     /* transport header length field contains total length + header length */
     f->transport_len = (uint16_t)(space);
-#ifdef PICO_SUPPORT_IPFRAG
-    f->frag = short_be(PICO_IPV4_MOREFRAG);
-#endif
+    f->frag = PICO_IPV4_MOREFRAG;
     f->payload += hdr_offset;
-    f->payload_len = (uint16_t) space;
 }
 
 static void pico_socket_xmit_next_fragment_setup(struct pico_frame *f, int hdr_offset, int total_payload_written, int len)
 {
     /* no transport header in fragmented IP */
     f->payload = f->transport_hdr;
-    f->payload_len = (uint16_t)(f->payload_len + hdr_offset);
     /* set offset in octets */
-#ifdef PICO_SUPPORT_IPFRAG
-    f->frag = short_be((uint16_t)((uint16_t)(total_payload_written + (uint16_t)hdr_offset) >> 3u));
-#endif
+    f->frag = (uint16_t)((total_payload_written + (uint16_t)hdr_offset) >> 3u); /* first fragment had a header offset */
     if (total_payload_written + f->payload_len < len) {
         frag_dbg("FRAG: intermediate fragmented frame %p | len = %u offset = %u\n", f, f->payload_len, short_be(f->frag));
-#ifdef PICO_SUPPORT_IPFRAG
-        f->frag |= short_be(PICO_IPV4_MOREFRAG);
-#endif
+        f->frag |= PICO_IPV4_MOREFRAG;
     } else {
         frag_dbg("FRAG: last fragmented frame %p | len = %u offset = %u\n", f, f->payload_len, short_be(f->frag));
-#ifdef PICO_SUPPORT_IPFRAG
-        f->frag &= short_be(PICO_IPV4_FRAG_MASK);
-#endif
+        f->frag &= PICO_IPV4_FRAG_MASK;
     }
 }
+#endif
 
-static int pico_socket_xmit_fragments(struct pico_socket *s, const void *buf, const int len, void *src, struct pico_remote_endpoint *ep)
+/* Implies ep discarding! */
+static int pico_socket_xmit_fragments(struct pico_socket *s, const void *buf, const int len,
+                                      void *src, struct pico_remote_endpoint *ep, struct pico_msginfo *msginfo)
 {
     int space = pico_socket_xmit_avail_space(s);
     int hdr_offset = pico_socket_sendto_transport_offset(s);
     int total_payload_written = 0;
+    int retval = 0;
     struct pico_frame *f = NULL;
 
     if (space > len) {
-        return pico_socket_xmit_one(s, buf, len, src, ep);
+        retval = pico_socket_xmit_one(s, buf, len, src, ep, msginfo);
+        pico_endpoint_free(ep);
+        return retval;
     }
 
-#ifdef PICO_SUPPORT_IPFRAG
+#ifdef PICO_SUPPORT_IPV6
+    /* Can't fragment IPv6 */
+    if (is_sock_ipv6(s)) {
+        retval =  pico_socket_xmit_one(s, buf, space, src, ep, msginfo);
+        pico_endpoint_free(ep);
+        return retval;
+    }
+
+#endif
+
+#ifdef PICO_SUPPORT_IPV4FRAG
     while(total_payload_written < len) {
         /* Always allocate the max space available: space + offset */
         if (len < space)
             space = len;
 
+        if (space > len - total_payload_written) /* update space for last fragment */
+            space = len - total_payload_written;
+
         f = pico_socket_frame_alloc(s, (uint16_t)(space + hdr_offset));
         if (!f) {
             pico_err = PICO_ERR_ENOMEM;
@@ -1124,12 +1155,15 @@
             }
         }
 
-        if (!total_payload_written == 0) {
+        f->payload_len = (uint16_t) space;
+        if (total_payload_written == 0) {
             /* First fragment: no payload written yet! */
             pico_socket_xmit_first_fragment_setup(f, space, hdr_offset);
+            space += hdr_offset; /* only first fragments contains transport header */
+            hdr_offset = 0;
         } else {
             /* Next fragment */
-            pico_socket_xmit_next_fragment_setup(f, hdr_offset, total_payload_written, len);
+            pico_socket_xmit_next_fragment_setup(f, pico_socket_sendto_transport_offset(s), total_payload_written, len);
         }
 
         memcpy(f->payload, (const uint8_t *)buf + total_payload_written, f->payload_len);
@@ -1152,20 +1186,56 @@
     (void) f;
     (void) hdr_offset;
     (void) total_payload_written;
-    return pico_socket_xmit_one(s, buf, space, src, ep);
+    retval = pico_socket_xmit_one(s, buf, space, src, ep, msginfo);
+    pico_endpoint_free(ep);
+    return retval;
 
 #endif
 }
 
-uint16_t pico_socket_get_mtu(struct pico_socket *s)
+static void get_sock_dev(struct pico_socket *s)
 {
-    (void)s;
+    if (0) {}
+
+#ifdef PICO_SUPPORT_IPV6
+    else if (is_sock_ipv6(s))
+        s->dev = pico_ipv6_source_dev_find(&s->remote_addr.ip6);
+#endif
+#ifdef PICO_SUPPORT_IPV4
+    else if (is_sock_ipv4(s))
+        s->dev = pico_ipv4_source_dev_find(&s->remote_addr.ip4);
+#endif
+
+}
+
+
+static uint32_t pico_socket_adapt_mss_to_proto(struct pico_socket *s, uint32_t mss)
+{
 #ifdef PICO_SUPPORT_IPV6
     if (is_sock_ipv6(s))
-        return PICO_SOCKET6_MTU;
+        mss -= PICO_SIZE_IP6HDR;
+    else
+#endif
+    mss -= PICO_SIZE_IP4HDR;
+    return mss;
+}
 
-#endif
-    return PICO_SOCKET4_MTU;
+uint32_t pico_socket_get_mss(struct pico_socket *s)
+{
+    uint32_t mss = PICO_MIN_MSS;
+    if (!s)
+        return mss;
+
+    if (!s->dev)
+        get_sock_dev(s);
+
+    if (!s->dev) {
+        mss = PICO_MIN_MSS;
+    } else {
+        mss = s->dev->mtu;
+    }
+
+    return pico_socket_adapt_mss_to_proto(s, mss);
 }
 
 
@@ -1176,10 +1246,10 @@
 
 #ifdef PICO_SUPPORT_TCP
     if (PROTO(s) == PICO_PROTO_TCP) {
-        transport_len = pico_tcp_get_socket_mtu(s);
+        transport_len = (uint16_t)pico_tcp_get_socket_mss(s);
     } else
 #endif
-    transport_len = pico_socket_get_mtu(s);
+    transport_len = (uint16_t)pico_socket_get_mss(s);
     header_offset = pico_socket_sendto_transport_offset(s);
     if (header_offset < 0) {
         pico_err = PICO_ERR_EPROTONOSUPPORT;
@@ -1191,18 +1261,22 @@
 }
 
 
-static int pico_socket_xmit(struct pico_socket *s, const void *buf, const int len, void *src, struct pico_remote_endpoint *ep)
+static int pico_socket_xmit(struct pico_socket *s, const void *buf, const int len, void *src,
+                            struct pico_remote_endpoint *ep, struct pico_msginfo *msginfo)
 {
     int space = pico_socket_xmit_avail_space(s);
     int total_payload_written = 0;
 
     if (space < 0) {
         pico_err = PICO_ERR_EPROTONOSUPPORT;
+        pico_endpoint_free(ep);
         return -1;
     }
 
     if ((PROTO(s) == PICO_PROTO_UDP) && (len > space)) {
-        return pico_socket_xmit_fragments(s, buf, len, src, ep);
+        total_payload_written = pico_socket_xmit_fragments(s, buf, len, src, ep, msginfo);
+        /* Implies ep discarding */
+        return total_payload_written;
     }
 
     while (total_payload_written < len) {
@@ -1210,7 +1284,7 @@
         if (chunk_len > space)
             chunk_len = space;
 
-        w = pico_socket_xmit_one(s, (const void *)((const uint8_t *)buf + total_payload_written), chunk_len, src, ep);
+        w = pico_socket_xmit_one(s, (const void *)((const uint8_t *)buf + total_payload_written), chunk_len, src, ep, msginfo);
         if (w <= 0) {
             break;
         }
@@ -1233,11 +1307,14 @@
 }
 
 
-int pico_socket_sendto(struct pico_socket *s, const void *buf, const int len, void *dst, uint16_t remote_port)
+int MOCKABLE pico_socket_sendto_extended(struct pico_socket *s, const void *buf, const int len,
+                                         void *dst, uint16_t remote_port, struct pico_msginfo *msginfo)
 {
     struct pico_remote_endpoint *remote_endpoint = NULL;
     void *src = NULL;
 
+    if(len == 0)
+        return 0;
 
     if (pico_socket_sendto_initial_checks(s, buf, len, dst, remote_port) < 0)
         return -1;
@@ -1245,16 +1322,33 @@
 
     src = pico_socket_sendto_get_src(s, dst);
     if (!src) {
+#ifdef PICO_SUPPORT_IPV6
+        if((s->net->proto_number == PICO_PROTO_IPV6)
+           && msginfo && msginfo->dev
+           && pico_ipv6_is_linklocal(((struct pico_ip6 *)dst)->addr))
+        {
+            src = &(pico_ipv6_linklocal_get(msginfo->dev)->address);
+            if(!src)
+                return -1;
+        }
+        else
+#endif
         return -1;
     }
 
     remote_endpoint = pico_socket_sendto_destination(s, dst, remote_port);
-    if (pico_socket_sendto_set_localport(s) < 0)
+    if (pico_socket_sendto_set_localport(s) < 0) {
+        pico_endpoint_free(remote_endpoint);
         return -1;
+    }
 
     pico_socket_sendto_set_dport(s, remote_port);
+    return pico_socket_xmit(s, buf, len, src, remote_endpoint, msginfo); /* Implies discarding the endpoint */
+}
 
-    return pico_socket_xmit(s, buf, len, src, remote_endpoint);
+int MOCKABLE pico_socket_sendto(struct pico_socket *s, const void *buf, const int len, void *dst, uint16_t remote_port)
+{
+    return pico_socket_sendto_extended(s, buf, len, dst, remote_port, NULL);
 }
 
 int pico_socket_send(struct pico_socket *s, const void *buf, int len)
@@ -1279,7 +1373,8 @@
     return pico_socket_sendto(s, buf, len, &s->remote_addr, s->remote_port);
 }
 
-int pico_socket_recvfrom(struct pico_socket *s, void *buf, int len, void *orig, uint16_t *remote_port)
+int pico_socket_recvfrom_extended(struct pico_socket *s, void *buf, int len, void *orig,
+                                  uint16_t *remote_port, struct pico_msginfo *msginfo)
 {
     if (!s || buf == NULL) { /* / || orig == NULL || remote_port == NULL) { */
         pico_err = PICO_ERR_EINVAL;
@@ -1300,7 +1395,13 @@
 
 #ifdef PICO_SUPPORT_UDP
     if (PROTO(s) == PICO_PROTO_UDP) {
-        return pico_udp_recv(s, buf, (uint16_t)len, orig, remote_port);
+        /* make sure cast to uint16_t doesn't give unexpected results */
+        if(len > 0xFFFF) {
+            pico_err = PICO_ERR_EINVAL;
+            return -1;
+        }
+
+        return pico_udp_recv(s, buf, (uint16_t)len, orig, remote_port, msginfo);
     }
 
 #endif
@@ -1321,6 +1422,13 @@
     return 0;
 }
 
+int pico_socket_recvfrom(struct pico_socket *s, void *buf, int len, void *orig,
+                         uint16_t *remote_port)
+{
+    return pico_socket_recvfrom_extended(s, buf, len, orig, remote_port, NULL);
+
+}
+
 int pico_socket_recv(struct pico_socket *s, void *buf, int len)
 {
     return pico_socket_recvfrom(s, buf, len, NULL, NULL);
@@ -1351,10 +1459,45 @@
         pico_err = PICO_ERR_EINVAL;
         return -1;
     }
+
     *port = s->local_port;
     return 0;
 }
 
+int pico_socket_getpeername(struct pico_socket *s, void *remote_addr, uint16_t *port, uint16_t *proto)
+{
+    if (!s || !remote_addr || !port || !proto) {
+        pico_err = PICO_ERR_EINVAL;
+        return -1;
+    }
+
+    if ((s->state & PICO_SOCKET_STATE_CONNECTED) == 0) {
+        pico_err = PICO_ERR_ENOTCONN;
+        return -1;
+    }
+
+    if (is_sock_ipv4(s)) {
+    #ifdef PICO_SUPPORT_IPV4
+        struct pico_ip4 *ip = (struct pico_ip4 *)remote_addr;
+        ip->addr = s->remote_addr.ip4.addr;
+        *proto = PICO_PROTO_IPV4;
+    #endif
+    } else if (is_sock_ipv6(s)) {
+    #ifdef PICO_SUPPORT_IPV6
+        struct pico_ip6 *ip = (struct pico_ip6 *)remote_addr;
+        memcpy(ip->addr, s->remote_addr.ip6.addr, PICO_SIZE_IP6);
+        *proto = PICO_PROTO_IPV6;
+    #endif
+    } else {
+        pico_err = PICO_ERR_EINVAL;
+        return -1;
+    }
+
+    *port = s->remote_port;
+    return 0;
+
+}
+
 int pico_socket_bind(struct pico_socket *s, void *local_addr, uint16_t *port)
 {
     if (!s || !local_addr || !port) {
@@ -1389,7 +1532,6 @@
         return -1;
     }
 
-
     /* When given port = 0, get a random high port to bind to. */
     if (*port == 0) {
         *port = pico_socket_high_port(PROTO(s));
@@ -1451,6 +1593,7 @@
         s->remote_addr.ip4 = *ip;
         local = pico_ipv4_source_find(ip);
         if (local) {
+            get_sock_dev(s);
             s->local_addr.ip4 = *local;
         } else {
             pico_err = PICO_ERR_EHOSTUNREACH;
@@ -1465,6 +1608,7 @@
         s->remote_addr.ip6 = *ip;
         local = pico_ipv6_source_find(ip);
         if (local) {
+            get_sock_dev(s);
             s->local_addr.ip6 = *local;
         } else {
             pico_err = PICO_ERR_EHOSTUNREACH;
@@ -1491,7 +1635,7 @@
 #ifdef PICO_SUPPORT_TCP
     if (PROTO(s) == PICO_PROTO_TCP) {
         if (pico_tcp_initconn(s) == 0) {
-            pico_socket_alter_state(s, PICO_SOCKET_STATE_CONNECTED | PICO_SOCKET_STATE_TCP_SYN_SENT, 0, 0);
+            pico_socket_alter_state(s, PICO_SOCKET_STATE_CONNECTED | PICO_SOCKET_STATE_TCP_SYN_SENT, PICO_SOCKET_STATE_CLOSED, 0);
             pico_err = PICO_ERR_NOERR;
             ret = 0;
         } else {
@@ -1663,9 +1807,16 @@
         return -1;
     }
 
+    /* unbound sockets can be deleted immediately */
+    if (!(s->state & PICO_SOCKET_STATE_BOUND))
+    {
+        socket_garbage_collect((pico_time)10, s);
+        return 0;
+    }
+
 #ifdef PICO_SUPPORT_UDP
     if (PROTO(s) == PICO_PROTO_UDP) {
-        if (mode & PICO_SHUT_RDWR)
+        if ((mode & PICO_SHUT_RDWR) == PICO_SHUT_RDWR)
             pico_socket_alter_state(s, PICO_SOCKET_STATE_CLOSED, PICO_SOCKET_STATE_CLOSING | PICO_SOCKET_STATE_BOUND | PICO_SOCKET_STATE_CONNECTED, 0);
         else if (mode & PICO_SHUT_RD)
             pico_socket_alter_state(s, 0, PICO_SOCKET_STATE_BOUND, 0);
@@ -1674,14 +1825,15 @@
 #endif
 #ifdef PICO_SUPPORT_TCP
     if (PROTO(s) == PICO_PROTO_TCP) {
-        if(mode & PICO_SHUT_RDWR)
+        if ((mode & PICO_SHUT_RDWR) == PICO_SHUT_RDWR)
         {
             pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_LOCAL | PICO_SOCKET_STATE_SHUT_REMOTE, 0, 0);
             pico_tcp_notify_closing(s);
         }
-        else if (mode & PICO_SHUT_WR)
+        else if (mode & PICO_SHUT_WR) {
             pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_LOCAL, 0, 0);
-        else if (mode & PICO_SHUT_RD)
+            pico_tcp_notify_closing(s);
+        } else if (mode & PICO_SHUT_RD)
             pico_socket_alter_state(s, PICO_SOCKET_STATE_SHUT_REMOTE, 0, 0);
 
     }
@@ -1690,15 +1842,17 @@
     return 0;
 }
 
-int pico_socket_close(struct pico_socket *s)
+int MOCKABLE pico_socket_close(struct pico_socket *s)
 {
     if (!s)
         return -1;
+
 #ifdef PICO_SUPPORT_TCP
     if (PROTO(s) == PICO_PROTO_TCP) {
         if (pico_tcp_check_listen_close(s) == 0)
             return 0;
     }
+
 #endif
     return pico_socket_shutdown(s, PICO_SHUT_RDWR);
 }
@@ -1712,6 +1866,7 @@
 
     switch (net_hdr->proto)
     {
+#ifdef PICO_SUPPORT_TCP
     case PICO_PROTO_TCP:
         checksum_invalid = short_be(pico_tcp_checksum(f));
         /* dbg("TCP CRC validation == %u\n", checksum_invalid); */
@@ -1722,7 +1877,9 @@
         }
 
         break;
+#endif /* PICO_SUPPORT_TCP */
 
+#ifdef PICO_SUPPORT_UDP
     case PICO_PROTO_UDP:
         udp_hdr = (struct pico_udp_hdr *) f->transport_hdr;
         if (short_be(udp_hdr->crc)) {
@@ -1745,6 +1902,7 @@
         }
 
         break;
+#endif /* PICO_SUPPORT_UDP */
 
     default:
         /* Do nothing */
@@ -1793,7 +1951,7 @@
 #define SL_LOOP_MIN 1
 
 #ifdef PICO_SUPPORT_TCP
-static int checkSocketSanity(struct pico_socket *s)
+static int check_socket_sanity(struct pico_socket *s)
 {
 
     /* checking for pending connections */
@@ -1801,13 +1959,6 @@
         if((PICO_TIME_MS() - s->timestamp) >= PICO_SOCKET_BOUND_TIMEOUT)
             return -1;
     }
-
-    if((PICO_TIME_MS() - s->timestamp) >= PICO_SOCKET_TIMEOUT) {
-        /* checking for hanging sockets */
-        if((TCP_STATE(s) != PICO_SOCKET_STATE_TCP_LISTEN) && (TCP_STATE(s) != PICO_SOCKET_STATE_TCP_ESTABLISHED))
-            return -1;
-    }
-
     return 0;
 }
 #endif
@@ -1841,7 +1992,8 @@
             while (f && (loop_score > 0)) {
                 pico_proto_udp.push(&pico_proto_udp, f);
                 loop_score -= 1;
-                f = pico_dequeue(&s->q_out);
+                if (loop_score > 0) /* only dequeue if there is still loop_score, otherwise f might get lost */
+                    f = pico_dequeue(&s->q_out);
             }
         }
 
@@ -1892,7 +2044,7 @@
                 break;
             }
 
-            if(checkSocketSanity(s) < 0)
+            if(check_socket_sanity(s) < 0)
             {
                 pico_socket_del(s);
                 index_tcp = NULL; /* forcing the restart of loop */
@@ -1930,6 +2082,35 @@
     return loop_score;
 }
 
+int pico_count_sockets(uint8_t proto)
+{
+    struct pico_sockport *sp;
+    struct pico_tree_node *idx_sp, *idx_s;
+    int count = 0;
+
+    if ((proto == 0) || (proto == PICO_PROTO_TCP)) {
+        pico_tree_foreach(idx_sp, &TCPTable) {
+            sp = idx_sp->keyValue;
+            if (sp) {
+                pico_tree_foreach(idx_s, &sp->socks)
+                count++;
+            }
+        }
+    }
+
+    if ((proto == 0) || (proto == PICO_PROTO_UDP)) {
+        pico_tree_foreach(idx_sp, &UDPTable) {
+            sp = idx_sp->keyValue;
+            if (sp) {
+                pico_tree_foreach(idx_s, &sp->socks)
+                count++;
+            }
+        }
+    }
+
+    return count;
+}
+
 
 struct pico_frame *pico_socket_frame_alloc(struct pico_socket *s, uint16_t len)
 {