Diff: modules/pico_tcp.c

Revision:

149:5f4cb161cec3

Parent:

137:a1c8bfa9d691

Child:

152:a3d286bf94e5

--- a/modules/pico_tcp.c	Tue Mar 11 15:34:51 2014 +0100
+++ b/modules/pico_tcp.c	Wed Apr 09 14:31:41 2014 +0200
@@ -16,15 +16,15 @@
 #include "pico_queue.h"
 #include "pico_tree.h"
 
-#define TCP_IS_STATE(s, st) (s->state & st)
+#define TCP_IS_STATE(s, st) ((s->state & PICO_SOCKET_STATE_TCP) == st)
 #define TCP_SOCK(s) ((struct pico_socket_tcp *)s)
 #define SEQN(f) ((f) ? (long_be(((struct pico_tcp_hdr *)((f)->transport_hdr))->seq)) : 0)
 #define ACKN(f) ((f) ? (long_be(((struct pico_tcp_hdr *)((f)->transport_hdr))->ack)) : 0)
 
 #define TCP_TIME (PICO_TIME_MS())
 
-#define PICO_TCP_RTO_MIN 50
-#define PICO_TCP_RTO_MAX 120000
+#define PICO_TCP_RTO_MIN (70)
+#define PICO_TCP_RTO_MAX (120000)
 #define PICO_TCP_IW          2
 #define PICO_TCP_SYN_TO  1000u
 #define PICO_TCP_ZOMBIE_TO 30000
@@ -41,7 +41,7 @@
 #define PICO_TCP_WINDOW_FULL    0x06
 
 /* check if the Nagle algorithm is enabled on the socket */
-#define IS_NAGLE_ENABLED(s)     (!(!(!(s->opt_flags & (1 << PICO_SOCKET_OPT_TCPNODELAY)))))
+#define IS_NAGLE_ENABLED(s)     (!(!(!(s->opt_flags & (1u << PICO_SOCKET_OPT_TCPNODELAY)))))
 /* check if tcp connection is "idle" according to Nagle (RFC 896) */
 #define IS_TCP_IDLE(t)          ((t->in_flight == 0) && (t->tcpq_out.size == 0))
 /* check if the hold queue contains data (again Nagle) */
@@ -61,34 +61,39 @@
 
 #ifdef PICO_SUPPORT_MUTEX
 static void *Mutex = NULL;
-#define LOCK(x) { \
+#define PICOTCP_MUTEX_LOCK(x) { \
         if (x == NULL) \
             x = pico_mutex_init(); \
         pico_mutex_lock(x); \
 }
-#define UNLOCK(x) pico_mutex_unlock(x)
+#define PICOTCP_MUTEX_UNLOCK(x) pico_mutex_unlock(x)
 
 #else
-#define LOCK(x) do {} while(0)
-#define UNLOCK(x) do {} while(0)
+#define PICOTCP_MUTEX_LOCK(x) do {} while(0)
+#define PICOTCP_MUTEX_UNLOCK(x) do {} while(0)
 #endif
 
 
-static inline int seq_compare(uint32_t a, uint32_t b)
+static /* inline*/ int32_t seq_compare(uint32_t a, uint32_t b)
 {
     uint32_t thresh = ((uint32_t)(-1)) >> 1;
-    if (((a > thresh) && (b > thresh)) || ((a <= thresh) && (b <= thresh))) {
-        if (a > b)
-            return 1;
-
-        if (b > a)
-            return -1;
-    } else {
-        if (a > b)
-            return -2;
-
-        if (b > a)
-            return 2;
+
+    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;
@@ -112,14 +117,14 @@
 
 static struct tcp_input_segment *segment_from_frame(struct pico_frame *f)
 {
-    struct tcp_input_segment *seg = pico_zalloc(sizeof(struct tcp_input_segment));
+    struct tcp_input_segment *seg = PICO_ZALLOC(sizeof(struct tcp_input_segment));
     if(!seg)
         return NULL;
 
-    seg->payload = pico_zalloc(f->payload_len);
+    seg->payload = PICO_ZALLOC(f->payload_len);
     if(!seg->payload)
     {
-        pico_free(seg);
+        PICO_FREE(seg);
         return NULL;
     }
 
@@ -141,7 +146,6 @@
     uint32_t max_size;
     uint32_t size;
     uint32_t frames;
-    uint16_t overhead;
 };
 
 static void tcp_discard_all_segments(struct pico_tcp_queue *tq);
@@ -192,17 +196,22 @@
 static int32_t pico_enqueue_segment(struct pico_tcp_queue *tq, void *f)
 {
     int32_t ret = -1;
-    uint16_t payload_len = (uint16_t)(IS_INPUT_QUEUE(tq) ?
-                                      ((struct tcp_input_segment *)f)->payload_len :
-                                      ((struct pico_frame *)f)->buffer_len);
+    uint16_t payload_len;
+
+    if (!f)
+        return -1;
+
+    payload_len = (uint16_t)((IS_INPUT_QUEUE(tq)) ?
+                             (((struct tcp_input_segment *)f)->payload_len) :
+                             (((struct pico_frame *)f)->buffer_len));
 
     if (payload_len <= 0) {
         tcp_dbg("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! TRIED TO ENQUEUE INVALID SEGMENT!\n");
-        /* abort(); */
-        return -1;
+        ret = -2; /* Fail harder */
+        goto out;
     }
 
-    LOCK(Mutex);
+    PICOTCP_MUTEX_LOCK(Mutex);
     if ((tq->size + payload_len) > tq->max_size)
     {
         ret = 0;
@@ -215,41 +224,41 @@
         goto out;
     }
 
-    tq->size += (uint16_t)(payload_len + tq->overhead);
+    tq->size += (uint16_t)payload_len;
     if (payload_len > 0)
         tq->frames++;
 
     ret = (int32_t)payload_len;
 
 out:
-    UNLOCK(Mutex);
+    PICOTCP_MUTEX_UNLOCK(Mutex);
     return ret;
 }
 
 static void pico_discard_segment(struct pico_tcp_queue *tq, void *f)
 {
     void *f1;
-    uint16_t payload_len = (uint16_t)(IS_INPUT_QUEUE(tq) ?
-                                      ((struct tcp_input_segment *)f)->payload_len :
-                                      ((struct pico_frame *)f)->buffer_len);
-    LOCK(Mutex);
+    uint16_t payload_len = (uint16_t)((IS_INPUT_QUEUE(tq)) ?
+                                      (((struct tcp_input_segment *)f)->payload_len) :
+                                      (((struct pico_frame *)f)->buffer_len));
+    PICOTCP_MUTEX_LOCK(Mutex);
     f1 = pico_tree_delete(&tq->pool, f);
     if (f1) {
-        tq->size -= (uint16_t)(payload_len + tq->overhead);
+        tq->size -= (uint16_t)payload_len;
         if (payload_len > 0)
             tq->frames--;
     }
 
-    if(IS_INPUT_QUEUE(tq))
+    if(f1 && IS_INPUT_QUEUE(tq))
     {
         struct tcp_input_segment *inp = f1;
-        pico_free(inp->payload);
-        pico_free(inp);
+        PICO_FREE(inp->payload);
+        PICO_FREE(inp);
     }
     else
         pico_frame_discard(f);
 
-    UNLOCK(Mutex);
+    PICOTCP_MUTEX_UNLOCK(Mutex);
 }
 
 /* Structure for TCP socket */
@@ -293,6 +302,7 @@
     uint32_t rcv_processed;
     uint16_t wnd;
     uint16_t wnd_scale;
+    uint16_t remote_closed;
 
     /* options */
     uint32_t ts_nxt;
@@ -338,39 +348,50 @@
 {
     void *head = first_segment(q);
     int ret = 0;
-    int seq_result = 0;
-    if(head)
-        seq_result = IS_INPUT_QUEUE(q) ?
-                     seq_compare(((struct tcp_input_segment *)head)->seq + ((struct tcp_input_segment *)head)->payload_len, seq) :
-                     seq_compare(SEQN((struct pico_frame *)head) + ((struct pico_frame *)head)->payload_len, seq);
-
-    while (head && (seq_result <= 0)) {
+    int32_t seq_result = 0;
+
+    if (!head)
+        return ret;
+
+    do {
         void *cur = head;
-        head = next_segment(q, cur);
-        tcp_dbg("Releasing %p\n", q);
-        pico_discard_segment(q, cur);
-        ret++;
-        if(head)
-            seq_result = IS_INPUT_QUEUE(q) ?
-                         seq_compare(((struct tcp_input_segment *)head)->seq + ((struct tcp_input_segment *)head)->payload_len, seq) :
-                         seq_compare(SEQN((struct pico_frame *)head) + ((struct pico_frame *)head)->payload_len, seq);
-    }
+
+        if (IS_INPUT_QUEUE(q))
+            seq_result = seq_compare(((struct tcp_input_segment *)head)->seq + ((struct tcp_input_segment *)head)->payload_len, seq);
+        else
+            seq_result = seq_compare(SEQN((struct pico_frame *)head) + ((struct pico_frame *)head)->payload_len, seq);
+
+        if (seq_result <= 0)
+        {
+            head = next_segment(q, cur);
+            tcp_dbg("Releasing %08x, len: %d\n", SEQN((struct pico_frame *)head), ((struct pico_frame *)head)->payload_len);
+            pico_discard_segment(q, cur);
+            ret++;
+        } else {
+            break;
+        }
+    } while (head);
+
     return ret;
 }
 
 static int release_all_until(struct pico_tcp_queue *q, uint32_t seq, pico_time *timestamp)
 {
-    void *f = NULL, *tmp __attribute__((unused));
+    void *f = NULL;
     struct pico_tree_node *idx, *temp;
     int seq_result;
     int ret = 0;
     *timestamp = 0;
 
-    pico_tree_foreach_safe(idx, &q->pool, temp){
+    pico_tree_foreach_safe(idx, &q->pool, temp)
+    {
         f = idx->keyValue;
-        seq_result = IS_INPUT_QUEUE(q) ?
-                     seq_compare(((struct tcp_input_segment *)f)->seq + ((struct tcp_input_segment *)f)->payload_len, seq) :
-                     seq_compare(SEQN((struct pico_frame *)f) + ((struct pico_frame *)f)->payload_len, seq);
+
+        if (IS_INPUT_QUEUE(q))
+            seq_result = seq_compare(((struct tcp_input_segment *)f)->seq + ((struct tcp_input_segment *)f)->payload_len, seq);
+        else
+            seq_result = seq_compare(SEQN((struct pico_frame *)f) + ((struct pico_frame *)f)->payload_len, seq);
+
         if (seq_result <= 0) {
             tcp_dbg("Releasing %p\n", f);
             if(seq_result == 0)
@@ -378,12 +399,14 @@
 
             pico_discard_segment(q, f);
             ret++;
-        } else
+        } else {
             return ret;
+        }
     }
     return ret;
 }
 
+
 /* API calls */
 
 uint16_t pico_tcp_checksum_ipv4(struct pico_frame *f)
@@ -412,6 +435,58 @@
     return pico_dualbuffer_checksum(&pseudo, sizeof(struct pico_ipv4_pseudo_hdr), tcp_hdr, f->transport_len);
 }
 
+#ifdef PICO_SUPPORT_IPV6
+uint16_t pico_tcp_checksum_ipv6(struct pico_frame *f)
+{
+    struct pico_ipv6_hdr *ipv6_hdr = (struct pico_ipv6_hdr *)f->net_hdr;
+    struct pico_tcp_hdr *tcp_hdr = (struct pico_tcp_hdr *)f->transport_hdr;
+    struct pico_ipv6_pseudo_hdr pseudo;
+    struct pico_socket *s = f->sock;
+
+    /* XXX If the IPv6 packet contains a Routing header, the Destination
+     *     Address used in the pseudo-header is that of the final destination */
+    if (s) {
+        /* Case of outgoing frame */
+        pseudo.src = s->local_addr.ip6;
+        pseudo.dst = s->remote_addr.ip6;
+    } else {
+        /* Case of incomming frame */
+        pseudo.src = ipv6_hdr->src;
+        pseudo.dst = ipv6_hdr->dst;
+    }
+
+    pseudo.zero[0] = 0;
+    pseudo.zero[1] = 0;
+    pseudo.zero[2] = 0;
+    pseudo.len = long_be(f->transport_len);
+    pseudo.nxthdr = PICO_PROTO_TCP;
+
+    return pico_dualbuffer_checksum(&pseudo, sizeof(struct pico_ipv6_pseudo_hdr), tcp_hdr, f->transport_len);
+}
+#endif
+
+uint16_t pico_tcp_checksum(struct pico_frame *f)
+{
+    (void)f;
+    #ifdef PICO_SUPPORT_IPV4
+    if (IS_IPV4(f))
+        return pico_tcp_checksum_ipv4(f);
+
+    if (f->sock && (f->sock->net == &pico_proto_ipv4))
+        return pico_tcp_checksum_ipv4(f);
+
+    #endif
+    #ifdef PICO_SUPPORT_IPV6
+    if (IS_IPV6(f))
+        return pico_tcp_checksum_ipv6(f);
+
+    if (f->sock && (f->sock->net == &pico_proto_ipv6))
+        return pico_tcp_checksum_ipv6(f);
+
+    #endif
+    return 0xffff;
+}
+
 static void tcp_send_fin(struct pico_socket_tcp *t);
 static int pico_tcp_process_out(struct pico_protocol *self, struct pico_frame *f)
 {
@@ -459,7 +534,7 @@
 {
     static uint32_t _paws = 0;
     _paws = pico_rand();
-    return long_be(_paws); /*XXX: implement paws */
+    return long_be(_paws);
 }
 
 static void tcp_add_options(struct pico_socket_tcp *ts, struct pico_frame *f, uint16_t flags, uint16_t optsiz)
@@ -507,7 +582,7 @@
                 memcpy(f->start + i, sb, 2 * sizeof(uint32_t));
                 i += (2 * (uint32_t)sizeof(uint32_t));
                 f->start[len_off] = (uint8_t)(f->start[len_off] + (2 * sizeof(uint32_t)));
-                pico_free(sb);
+                PICO_FREE(sb);
             }
         }
     }
@@ -526,7 +601,7 @@
         size = (uint16_t)(size + PICO_TCPOPTLEN_TIMESTAMP);
 
     size = (uint16_t)(size + PICO_TCPOPTLEN_END);
-    size = (uint16_t)(((size + 3) >> 2) << 2);
+    size = (uint16_t)(((uint16_t)(size + 3u) >> 2u) << 2u);
     return size;
 }
 
@@ -564,25 +639,23 @@
 
 static void tcp_set_space(struct pico_socket_tcp *t)
 {
-    uint32_t mtu;
     int32_t space;
     uint32_t shift = 0;
 
-    mtu = t->mss + PICO_SIZE_TCPHDR + PICO_SIZE_TCPOPT_SYN;
     if (t->tcpq_in.max_size == 0) {
         space = 1024 * 1024 * 1024; /* One Gigabyte, for unlimited sockets. */
     } else {
-        space = (int32_t)(((t->tcpq_in.max_size - t->tcpq_in.size) / mtu) * t->mss);
+        space = (int32_t)(t->tcpq_in.max_size - t->tcpq_in.size);
     }
 
     if (space < 0)
         space = 0;
 
     while(space > 0xFFFF) {
-        space >>= 1;
+        space = (int32_t)(((uint32_t)space >> 1u));
         shift++;
     }
-    if ((space != t->wnd) || (shift != t->wnd_scale) || ((space - t->wnd) > (space >> 2))) {
+    if (((uint32_t)space != t->wnd) || (shift != t->wnd_scale) || ((space - t->wnd) > (int32_t)((uint32_t)space >> 2u))) {
         t->wnd = (uint16_t)space;
         t->wnd_scale = (uint16_t)shift;
 
@@ -623,7 +696,7 @@
         }
     }
 
-    size = (uint16_t)(((size + 3) >> 2) << 2);
+    size = (uint16_t)(((size + 3u) >> 2u) << 2u);
     return size;
 }
 
@@ -684,7 +757,7 @@
     hdr->flags |= PICO_TCP_PSH | PICO_TCP_ACK;
     hdr->ack = long_be(t->rcv_nxt);
     hdr->crc = 0;
-    hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+    hdr->crc = short_be(pico_tcp_checksum(f));
 }
 
 static void tcp_rcv_sack(struct pico_socket_tcp *t, uint8_t *opt, int len)
@@ -825,17 +898,22 @@
     f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
     hdr->rwnd = short_be(ts->wnd);
     hdr->crc = 0;
-    hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+    hdr->crc = short_be(pico_tcp_checksum(f));
 
     /* TCP: ENQUEUE to PROTO ( Transmit ) */
     cpy = pico_frame_copy(f);
+    if (!cpy) {
+        pico_err = PICO_ERR_ENOMEM;
+        return -1;
+    }
+
     if ((pico_enqueue(&tcp_out, cpy) > 0)) {
         if (f->payload_len > 0) {
             ts->in_flight++;
             ts->snd_nxt += f->payload_len; /* update next pointer here to prevent sending same segment twice when called twice in same tick */
         }
 
-        tcp_dbg("DBG> [tcp output] state: %02x --> local port:%d remote port: %d seq: %08x ack: %08x flags: %02x = t_len: %d, hdr: %u payload: %d\n",
+        tcp_dbg("DBG> [tcp output] state: %02x --> local port:%u remote port: %u seq: %08x ack: %08x flags: %02x = t_len: %u, hdr: %u payload: %d\n",
                 TCPSTATE(&ts->sock) >> 8, short_be(hdr->trans.sport), short_be(hdr->trans.dport), SEQN(f), ACKN(f), hdr->flags, f->transport_len, (hdr->len & 0xf0) >> 2, f->payload_len );
     } else {
         pico_frame_discard(cpy);
@@ -856,23 +934,21 @@
 }
 #endif
 
-struct pico_socket *pico_tcp_open(void)
+struct pico_socket *pico_tcp_open(uint16_t family)
 {
-    struct pico_socket_tcp *t = pico_zalloc(sizeof(struct pico_socket_tcp));
+    struct pico_socket_tcp *t = PICO_ZALLOC(sizeof(struct pico_socket_tcp));
     if (!t)
         return NULL;
 
     t->sock.timestamp = TCP_TIME;
-    t->mss = PICO_TCP_DEFAULT_MSS;
-
+    pico_socket_set_family(&t->sock, family);
+    t->mss = (uint16_t)(pico_socket_get_mtu(&t->sock) - PICO_SIZE_TCPHDR);
     t->tcpq_in.pool.root = t->tcpq_hold.pool.root = t->tcpq_out.pool.root = &LEAF;
     t->tcpq_hold.pool.compare = t->tcpq_out.pool.compare = segment_compare;
     t->tcpq_in.pool.compare = input_segment_compare;
     t->tcpq_in.max_size = PICO_DEFAULT_SOCKETQ;
     t->tcpq_out.max_size = PICO_DEFAULT_SOCKETQ;
-    t->tcpq_hold.max_size = 2 * PICO_TCP_DEFAULT_MSS;
-    t->tcpq_in.overhead = (sizeof(struct tcp_input_segment) + sizeof(struct pico_tree_node));
-    t->tcpq_out.overhead = t->tcpq_hold.overhead = sizeof(struct pico_frame) + sizeof(struct pico_tree_node);
+    t->tcpq_hold.max_size = 2u * t->mss;
     /* disable Nagle by default */
     t->sock.opt_flags |= (1 << PICO_SOCKET_OPT_TCPNODELAY);
     /* Nagle is enabled by default */
@@ -890,7 +966,8 @@
 {
     struct pico_socket_tcp *t = TCP_SOCK(s);
     struct tcp_input_segment *f;
-    uint32_t in_frame_off, in_frame_len;
+    int32_t in_frame_off;
+    uint32_t in_frame_len;
     uint32_t tot_rd_len = 0;
 
     while (tot_rd_len < len) {
@@ -900,27 +977,28 @@
         if (!f)
             goto out;
 
+        in_frame_off = seq_compare(t->rcv_processed, f->seq);
         /* Hole at the beginning of data, awaiting retransmissions. */
-        if (seq_compare(t->rcv_processed, f->seq) < 0) {
+        if (in_frame_off < 0) {
             tcp_dbg("TCP> read hole beginning of data, %08x - %08x. rcv_nxt is %08x\n", t->rcv_processed, f->seq, t->rcv_nxt);
             goto out;
         }
 
-        if(seq_compare(t->rcv_processed, f->seq) > 0) {
-            in_frame_off = t->rcv_processed - f->seq;
-            in_frame_len = f->payload_len - in_frame_off;
+        else if (in_frame_off > 0)
+        {
+            if ((uint32_t)in_frame_off > f->payload_len)
+                dbg("FATAL TCP ERR: in_frame_off > f->payload_len\n");
+
+            in_frame_len = f->payload_len - (uint32_t)in_frame_off;
         } else {
-            in_frame_off = 0;
             in_frame_len = f->payload_len;
         }
 
+
         if ((in_frame_len + tot_rd_len) > (uint32_t)len) {
             in_frame_len = len - tot_rd_len;
         }
 
-        if (in_frame_len > f->payload_len - in_frame_off)
-            in_frame_len = f->payload_len - in_frame_off;
-
         memcpy((uint8_t *)buf + tot_rd_len, f->payload + in_frame_off, in_frame_len);
         tot_rd_len += in_frame_len;
         t->rcv_processed += in_frame_len;
@@ -935,6 +1013,17 @@
         s->ev_pending &= (uint16_t)(~PICO_SOCK_EV_RD);
     }
 
+    if (t->remote_closed) {
+        s->ev_pending |= (uint16_t)(PICO_SOCK_EV_CLOSE);
+        s->state &= 0x00FFU;
+        s->state |= PICO_SOCKET_STATE_TCP_CLOSE_WAIT;
+        /* set SHUT_REMOTE */
+        s->state |= PICO_SOCKET_STATE_SHUT_REMOTE;
+        if (s->wakeup) {
+            s->wakeup(PICO_SOCK_EV_CLOSE, s);
+        }
+    }
+
     return tot_rd_len;
 }
 
@@ -967,7 +1056,7 @@
     struct pico_socket_tcp *ts = TCP_SOCK(s);
     struct pico_frame *syn;
     struct pico_tcp_hdr *hdr;
-    uint16_t opt_len = tcp_options_size(ts, PICO_TCP_SYN);
+    uint16_t mtu, opt_len = tcp_options_size(ts, PICO_TCP_SYN);
 
     syn = s->net->alloc(s->net, (uint16_t)(PICO_SIZE_TCPHDR + opt_len));
     if (!syn)
@@ -980,7 +1069,9 @@
 
     ts->snd_last = ts->snd_nxt;
     ts->cwnd = PICO_TCP_IW;
-    ts->ssthresh = 40;
+    mtu = pico_socket_get_mtu(s);
+    ts->mss = (uint16_t)(mtu - PICO_SIZE_TCPHDR);
+    ts->ssthresh = (uint16_t)((uint16_t)(PICO_DEFAULT_SOCKETQ / ts->mss) -  (((uint16_t)(PICO_DEFAULT_SOCKETQ / ts->mss)) >> 3u));
     syn->sock = s;
     hdr->seq = long_be(ts->snd_nxt);
     hdr->len = (uint8_t)((PICO_SIZE_TCPHDR + opt_len) << 2 | ts->jumbo);
@@ -992,12 +1083,12 @@
     hdr->trans.dport = ts->sock.remote_port;
 
     hdr->crc = 0;
-    hdr->crc = short_be(pico_tcp_checksum_ipv4(syn));
+    hdr->crc = short_be(pico_tcp_checksum(syn));
 
     /* TCP: ENQUEUE to PROTO ( SYN ) */
     tcp_dbg("Sending SYN... (ports: %d - %d) size: %d\n", short_be(ts->sock.local_port), short_be(ts->sock.remote_port), syn->buffer_len);
     pico_enqueue(&tcp_out, syn);
-    pico_timer_add(PICO_TCP_SYN_TO << ts->backoff, initconn_retry, ts);
+    ts->retrans_tmr = pico_timer_add(PICO_TCP_SYN_TO << ts->backoff, initconn_retry, ts);
     return 0;
 }
 
@@ -1061,7 +1152,7 @@
     f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
     hdr->rwnd = short_be(t->wnd);
     hdr->crc = 0;
-    hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+    hdr->crc = short_be(pico_tcp_checksum(f));
 
     /* TCP: ENQUEUE to PROTO */
     pico_enqueue(&tcp_out, f);
@@ -1069,13 +1160,13 @@
 
 static void tcp_send_ack(struct pico_socket_tcp *t)
 {
-    return tcp_send_empty(t, PICO_TCP_ACK, 0);
+    tcp_send_empty(t, PICO_TCP_ACK, 0);
 }
 
 static void tcp_send_probe(struct pico_socket_tcp *t)
 {
     /* tcp_dbg("Sending probe\n"); */
-    return tcp_send_empty(t, PICO_TCP_PSH, 0);
+    tcp_send_empty(t, PICO_TCP_PSHACK, 1);
 }
 
 static int tcp_send_rst(struct pico_socket *s, struct pico_frame *fr)
@@ -1125,7 +1216,7 @@
     f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
     hdr->rwnd = short_be(t->wnd);
     hdr->crc = 0;
-    hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+    hdr->crc = short_be(pico_tcp_checksum(f));
 
     /* TCP: ENQUEUE to PROTO */
     pico_enqueue(&tcp_out, f);
@@ -1154,15 +1245,31 @@
     struct pico_frame *f;
     uint16_t size = PICO_SIZE_TCPHDR;
 
-    tcp_dbg("TCP>>>>>>>>>>>>>>>> sending RST ... <<<<<<<<<<<<<<<<<<\n");
 
     hdr1 = (struct pico_tcp_hdr *) (fr->transport_hdr);
+    if ((hdr1->flags & PICO_TCP_RST) != 0)
+        return -1;
+
+    tcp_dbg("TCP> sending RST ... \n");
+
     f = fr->sock->net->alloc(fr->sock->net, size);
+    if (!f) {
+        pico_err = PICO_ERR_ENOMEM;
+        return -1;
+    }
 
     /* fill in IP data from original frame */
-    /* TODO if IPv4 */
-    ((struct pico_ipv4_hdr *)(f->net_hdr))->dst.addr = ((struct pico_ipv4_hdr *)(fr->net_hdr))->src.addr;
-    ((struct pico_ipv4_hdr *)(f->net_hdr))->src.addr = ((struct pico_ipv4_hdr *)(fr->net_hdr))->dst.addr;
+    if (IS_IPV4(fr)) {
+        memcpy(f->net_hdr, fr->net_hdr, sizeof(struct pico_ipv4_hdr));
+        ((struct pico_ipv4_hdr *)(f->net_hdr))->dst.addr = ((struct pico_ipv4_hdr *)(fr->net_hdr))->src.addr;
+        ((struct pico_ipv4_hdr *)(f->net_hdr))->src.addr = ((struct pico_ipv4_hdr *)(fr->net_hdr))->dst.addr;
+        tcp_dbg("Making IPv4 reset frame...\n");
+
+    } else {
+        memcpy(f->net_hdr, fr->net_hdr, sizeof(struct pico_ipv6_hdr));
+        ((struct pico_ipv6_hdr *)(f->net_hdr))->dst = ((struct pico_ipv6_hdr *)(fr->net_hdr))->src;
+        ((struct pico_ipv6_hdr *)(f->net_hdr))->src = ((struct pico_ipv6_hdr *)(fr->net_hdr))->dst;
+    }
 
     /* fill in TCP data from original frame */
     ((struct pico_tcp_hdr *)(f->transport_hdr))->trans.dport = ((struct pico_tcp_hdr *)(fr->transport_hdr))->trans.sport;
@@ -1184,9 +1291,16 @@
     if(!(hdr1->flags & PICO_TCP_ACK))
         hdr->ack = long_be(long_be(((struct pico_tcp_hdr *)(fr->transport_hdr))->seq) + fr->payload_len);
 
-    hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
-    /* enqueue for transmission */
-    pico_ipv4_frame_push(f, &(((struct pico_ipv4_hdr *)(f->net_hdr))->dst), PICO_PROTO_TCP);
+    hdr->crc = short_be(pico_tcp_checksum(f));
+    if (IS_IPV4(f)) {
+        tcp_dbg("Pushing IPv4 reset frame...\n");
+        pico_ipv4_frame_push(f, &(((struct pico_ipv4_hdr *)(f->net_hdr))->dst), PICO_PROTO_TCP);
+#ifdef PICO_SUPPORT_IPV6
+    } else {
+        pico_ipv6_frame_push(f, &(((struct pico_ipv6_hdr *)(f->net_hdr))->dst), PICO_PROTO_TCP);
+#endif
+    }
+
 
     return 0;
 }
@@ -1197,10 +1311,13 @@
     struct pico_frame *f;
     struct pico_tcp_hdr *hdr, *hdr_rcv;
     uint16_t opt_len = tcp_options_size(t, PICO_TCP_RST | PICO_TCP_ACK);
+    hdr_rcv = (struct pico_tcp_hdr *) fr->transport_hdr;
 
     tcp_dbg("TCP SEND RST (NON-SYNC) >>>>>>>>>>>>>>>>>> state %x\n", (s->state & PICO_SOCKET_STATE_TCP));
     if (((s->state & PICO_SOCKET_STATE_TCP) ==  PICO_SOCKET_STATE_TCP_LISTEN)) {
-        /* XXX TODO NOTE: to prevent the parent socket from trying to send, because this socket has no knowledge of dst IP !!! */
+        if ((fr->flags & PICO_TCP_RST) != 0)
+            return 0;
+
         return pico_tcp_reply_rst(fr);
     }
 
@@ -1212,7 +1329,6 @@
         return -1;
     }
 
-    hdr_rcv = (struct pico_tcp_hdr *) fr->transport_hdr;
 
     f->sock = &t->sock;
     hdr = (struct pico_tcp_hdr *) f->transport_hdr;
@@ -1237,7 +1353,7 @@
     f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
     hdr->rwnd = short_be(t->wnd);
     hdr->crc = 0;
-    hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+    hdr->crc = short_be(pico_tcp_checksum(f));
 
     /* TCP: ENQUEUE to PROTO */
     pico_enqueue(&tcp_out, f);
@@ -1275,7 +1391,7 @@
     f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
     hdr->rwnd = short_be(t->wnd);
     hdr->crc = 0;
-    hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
+    hdr->crc = short_be(pico_tcp_checksum(f));
     /* tcp_dbg("SENDING FIN...\n"); */
     /* TCP: ENQUEUE to PROTO ( Pure ACK ) */
     pico_enqueue(&tcp_out, f);
@@ -1295,7 +1411,7 @@
     while(n < 3) {
         if (!pkt) {
             if(left) {
-                sb = pico_zalloc(sizeof(struct tcp_sack_block));
+                sb = PICO_ZALLOC(sizeof(struct tcp_sack_block));
                 if (!sb)
                     break;
 
@@ -1328,7 +1444,7 @@
             pkt = next_segment(&t->tcpq_in, pkt);
             continue;
         } else {
-            sb = pico_zalloc(sizeof(struct tcp_sack_block));
+            sb = PICO_ZALLOC(sizeof(struct tcp_sack_block));
             if (!sb)
                 break;
 
@@ -1348,17 +1464,11 @@
 {
     struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
     struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) f->transport_hdr;
-    uint16_t payload_len = (uint16_t)(f->transport_len - ((hdr->len & 0xf0) >> 2));
-
-    if (payload_len == 0 && (hdr->flags & PICO_TCP_PSH)) {
-        tcp_send_ack(t);
-        return 0;
-    }
-
-
-    if (((hdr->len & 0xf0) >> 2) <= f->transport_len) {
+    uint16_t payload_len = (uint16_t)(f->transport_len - ((hdr->len & 0xf0u) >> 2u));
+    int ret = 0;
+    if (((hdr->len & 0xf0u) >> 2u) <= f->transport_len) {
         tcp_parse_options(f);
-        f->payload = f->transport_hdr + ((hdr->len & 0xf0) >> 2);
+        f->payload = f->transport_hdr + ((hdr->len & 0xf0u) >> 2u);
         f->payload_len = payload_len;
         tcp_dbg("TCP> Received segment. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f));
 
@@ -1367,21 +1477,27 @@
             if (seq_compare(SEQN(f), t->rcv_nxt) == 0) { /* Exactly what we expected */
                 /* Create new segment and enqueue it */
                 struct tcp_input_segment *input = segment_from_frame(f);
-                if(input && pico_enqueue_segment(&t->tcpq_in, input) <= 0)
+                if (!input) {
+                    pico_err = PICO_ERR_ENOMEM;
+                    return -1;
+                }
+
+                if(pico_enqueue_segment(&t->tcpq_in, input) <= 0)
                 {
                     /* failed to enqueue, destroy segment */
-                    pico_free(input->payload);
-                    pico_free(input);
+                    PICO_FREE(input->payload);
+                    PICO_FREE(input);
+                    ret = -1;
+                } else {
+                    t->rcv_nxt = SEQN(f) + f->payload_len;
+                    nxt = peek_segment(&t->tcpq_in, t->rcv_nxt);
+                    while(nxt) {
+                        tcp_dbg("scrolling rcv_nxt...%08x\n", t->rcv_nxt);
+                        t->rcv_nxt += nxt->payload_len;
+                        nxt = peek_segment(&t->tcpq_in, t->rcv_nxt);
+                    }
+                    t->sock.ev_pending |= PICO_SOCK_EV_RD;
                 }
-
-                t->rcv_nxt = SEQN(f) + f->payload_len;
-                nxt = peek_segment(&t->tcpq_in, t->rcv_nxt);
-                while(nxt) {
-                    tcp_dbg("scrolling rcv_nxt...%08x\n", t->rcv_nxt);
-                    t->rcv_nxt += nxt->payload_len;
-                    nxt = peek_segment(&t->tcpq_in, t->rcv_nxt);
-                }
-                t->sock.ev_pending |= PICO_SOCK_EV_RD;
             } else {
                 tcp_dbg("TCP> lo segment. Uninteresting retransmission. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f));
             }
@@ -1389,10 +1505,16 @@
             tcp_dbg("TCP> hi segment. Possible packet loss. I'll dupack this. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f));
             if (t->sack_ok) {
                 struct tcp_input_segment *input = segment_from_frame(f);
-                if(input && pico_enqueue_segment(&t->tcpq_in, input) <= 0) {
+                if (!input) {
+                    pico_err = PICO_ERR_ENOMEM;
+                    return -1;
+                }
+
+                if(pico_enqueue_segment(&t->tcpq_in, input) <= 0) {
                     /* failed to enqueue, destroy segment */
-                    pico_free(input->payload);
-                    pico_free(input);
+                    PICO_FREE(input->payload);
+                    PICO_FREE(input);
+                    return -1;
                 }
 
                 tcp_sack_prepare(t);
@@ -1402,12 +1524,13 @@
         /* In either case, ack til recv_nxt. */
         if (((t->sock.state & PICO_SOCKET_STATE_TCP) != PICO_SOCKET_STATE_TCP_CLOSE_WAIT) && ((t->sock.state & PICO_SOCKET_STATE_TCP) != PICO_SOCKET_STATE_TCP_SYN_SENT) && ((t->sock.state & PICO_SOCKET_STATE_TCP) != PICO_SOCKET_STATE_TCP_SYN_RECV)) {
             /* tcp_dbg("SENDACK CALLED FROM OUTSIDE tcp_synack, state %x\n",t->sock.state); */
+            if (hdr->flags & PICO_TCP_RST)
+                return -1;
+
             tcp_send_ack(t);
-        } else {
-            /* tcp_dbg("SENDACK PREVENTED IN SYNSENT STATE\n"); */
         }
 
-        return 0;
+        return ret;
     } else {
         tcp_dbg("TCP: invalid data in pkt len, exp: %d, got %d\n", (hdr->len & 0xf0) >> 2, f->transport_len);
         return -1;
@@ -1432,6 +1555,17 @@
         return (uint16_t)(b - a);
 }
 
+static inline void rto_set(struct pico_socket_tcp *t, uint32_t rto)
+{
+    if (rto < PICO_TCP_RTO_MIN)
+        rto = PICO_TCP_RTO_MIN;
+
+    if (rto > PICO_TCP_RTO_MAX)
+        rto = PICO_TCP_RTO_MAX;
+
+    t->rto = rto;
+}
+
 static void tcp_rtt(struct pico_socket_tcp *t, uint32_t rtt)
 {
 
@@ -1447,9 +1581,9 @@
          */
         t->avg_rtt = rtt;
         t->rttvar = rtt >> 1;
-        t->rto = t->avg_rtt + (t->rttvar << 4);
+        rto_set(t, t->avg_rtt + (t->rttvar << 2));
     } else {
-        int32_t var = (int32_t)(t->avg_rtt - rtt);
+        int32_t var = (int32_t)t->avg_rtt - (int32_t)rtt;
         if (var < 0)
             var = 0 - var;
 
@@ -1468,7 +1602,7 @@
         t->avg_rtt >>= 3;
 
         /* Finally, assign a new value for the RTO, as specified in the RFC, with K=4 */
-        t->rto = t->avg_rtt + (t->rttvar << 2);
+        rto_set(t, t->avg_rtt + (t->rttvar << 2));
     }
 
     tcp_dbg(" -----=============== RTT CUR: %u AVG: %u RTTVAR: %u RTO: %u ======================----\n", rtt, t->avg_rtt, t->rttvar, t->rto);
@@ -1479,14 +1613,6 @@
     if (t->x_mode > PICO_TCP_LOOKAHEAD)
         return;
 
-    if (t->cwnd > t->tcpq_out.frames) {
-        tcp_dbg("Limited by app: %d\n", t->cwnd);
-        if (t->sock.wakeup)
-            t->sock.wakeup(PICO_SOCK_EV_WR, &t->sock);
-
-        return;
-    }
-
     tcp_dbg("Doing congestion control\n");
     if (t->cwnd < t->ssthresh) {
         t->cwnd++;
@@ -1518,6 +1644,11 @@
     struct pico_frame *cpy;
     /* TCP: ENQUEUE to PROTO ( retransmit )*/
     cpy = pico_frame_copy(f);
+    if (!cpy) {
+        add_retransmission_timer(t, (t->rto << t->backoff) + TCP_TIME);
+        return -1;
+    }
+
     if (pico_enqueue(&tcp_out, cpy) > 0) {
         t->snd_last_out = SEQN(cpy);
         add_retransmission_timer(t, (t->rto << (++t->backoff)) + TCP_TIME);
@@ -1525,6 +1656,7 @@
         tcp_dbg("Sending RTO!\n");
         return 1;
     } else {
+        tcp_dbg("RTO fail, retry!\n");
         add_retransmission_timer(t, (t->rto << t->backoff) + TCP_TIME);
         pico_frame_discard(cpy);
         return 0;
@@ -1555,25 +1687,25 @@
 
     t->retrans_tmr = NULL;
 
-    if (t->retrans_tmr_due == 0ull)
+    if (t->retrans_tmr_due == 0ull) {
         return;
+    }
 
     if (t->retrans_tmr_due > val) {
         /* Timer was postponed... */
-        add_retransmission_timer(t, (t->rto << (t->backoff)) + TCP_TIME);
+        add_retransmission_timer(t, t->retrans_tmr_due);
         return;
     }
 
     tcp_dbg("TIMEOUT! backoff = %d, rto: %d\n", t->backoff, t->rto);
-    tcp_dbg("TIMEOUT! backoff = %d, rto: %d\n", t->backoff, t->rto);
     t->retrans_tmr_due = 0ull;
 
     if (tcp_is_allowed_to_send(t)) {
         f = first_segment(&t->tcpq_out);
         while (f) {
+            tcp_dbg("Checking frame in queue \n");
             if (t->x_mode == PICO_TCP_WINDOW_FULL) {
                 tcp_dbg("TCP BLACKOUT> TIMED OUT (output) frame %08x, len= %d rto=%d Win full: %d frame flags: %04x\n", SEQN(f), f->payload_len, t->rto, t->x_mode == PICO_TCP_WINDOW_FULL, f->flags);
-                tcp_dbg("TCP BLACKOUT> TIMED OUT (output) frame %08x, len= %d rto=%d Win full: %d frame flags: %04x\n", SEQN(f), f->payload_len, t->rto, t->x_mode == PICO_TCP_WINDOW_FULL, f->flags);
                 tcp_next_zerowindow_probe(t);
                 return;
             }
@@ -1592,12 +1724,14 @@
     }
     else if(t->backoff >= PICO_TCP_MAX_RETRANS && (t->sock.state & 0xFF00) == PICO_SOCKET_STATE_TCP_ESTABLISHED )
     {
-        dbg("Connection timeout!\n");
+        tcp_dbg("Connection timeout!\n");
         /* the retransmission timer, failed to get an ack for a frame, gives up on the connection */
         tcp_discard_all_segments(&t->tcpq_out);
         if(t->sock.wakeup)
             t->sock.wakeup(PICO_SOCK_EV_FIN, &t->sock);
 
+        /* delete socket */
+        pico_socket_del(&t->sock);
         return;
     } else {
         tcp_dbg("Retransmission not allowed, rescheduling\n");
@@ -1607,8 +1741,10 @@
 static void add_retransmission_timer(struct pico_socket_tcp *t, pico_time next_ts)
 {
     struct pico_tree_node *index;
+    pico_time now = TCP_TIME;
     pico_time val = 0;
 
+
     if (next_ts == 0) {
         struct pico_frame *f;
 
@@ -1623,17 +1759,15 @@
         val = next_ts;
     }
 
-    if (val > 0) {
-        if (val > TCP_TIME) {
-            t->retrans_tmr_due = val;
-        } else {
-            t->retrans_tmr_due = TCP_TIME + 1;
-        }
+    if ((val > 0) || (val > now)) {
+        t->retrans_tmr_due = val;
+    } else {
+        t->retrans_tmr_due = now + 1;
     }
 
     if (!t->retrans_tmr) {
-        t->retrans_tmr = pico_timer_add(t->retrans_tmr_due - TCP_TIME, tcp_retrans_timeout, t);
-    } else {
+        t->retrans_tmr = pico_timer_add(t->retrans_tmr_due - now, tcp_retrans_timeout, t);
+        tcp_dbg("Next timeout in %u msec\n", (uint32_t) (t->retrans_tmr_due - now));
     }
 }
 
@@ -1645,6 +1779,10 @@
         tcp_add_header(t, f);
         /* TCP: ENQUEUE to PROTO ( retransmit )*/
         cpy = pico_frame_copy(f);
+        if (!cpy) {
+            return -1;
+        }
+
         if (pico_enqueue(&tcp_out, cpy) > 0) {
             t->in_flight++;
             t->snd_last_out = SEQN(cpy);
@@ -1789,8 +1927,9 @@
         if (t->x_mode < PICO_TCP_RECOVER) {
             t->x_mode++;
             tcp_dbg("Mode: DUPACK %d, due to PURE ACK %0x, len = %d\n", t->x_mode, SEQN(f), f->payload_len);
-            tcp_dbg("ACK: %x - QUEUE: %x\n", ACKN(f), SEQN(first_segment(&t->tcpq_out)));
+            /* tcp_dbg("ACK: %x - QUEUE: %x\n", ACKN(f), SEQN(first_segment(&t->tcpq_out))); */
             if (t->x_mode == PICO_TCP_RECOVER) {              /* Switching mode */
+                t->cwnd = (uint16_t)t->in_flight;
                 t->snd_retry = SEQN((struct pico_frame *)first_segment(&t->tcpq_out));
                 if (t->ssthresh > t->cwnd)
                     t->ssthresh >>= 2;
@@ -1801,7 +1940,7 @@
                     t->ssthresh = 2;
             }
         } else if (t->x_mode == PICO_TCP_RECOVER) {
-            tcp_dbg("TCP RECOVER> DUPACK! snd_una: %08x, snd_nxt: %08x, acked now: %08x\n", SEQN(first_segment(&t->tcpq_out)), t->snd_nxt, ACKN(f));
+            /* tcp_dbg("TCP RECOVER> DUPACK! snd_una: %08x, snd_nxt: %08x, acked now: %08x\n", SEQN(first_segment(&t->tcpq_out)), t->snd_nxt, ACKN(f)); */
             if (t->in_flight <= t->cwnd) {
                 struct pico_frame *nxt = peek_segment(&t->tcpq_out, t->snd_retry);
                 if (!nxt)
@@ -1814,7 +1953,7 @@
                 if (nxt && (seq_compare(SEQN(nxt), t->snd_nxt)) > 0)
                     nxt = NULL;
 
-                if (nxt && (seq_compare(SEQN(nxt), SEQN((struct pico_frame *)first_segment(&t->tcpq_out))) > (t->recv_wnd << t->recv_wnd_scale)))
+                if (nxt && (seq_compare(SEQN(nxt), SEQN((struct pico_frame *)first_segment(&t->tcpq_out))) > (int)(t->recv_wnd << t->recv_wnd_scale)))
                     nxt = NULL;
 
                 if(!nxt)
@@ -1839,6 +1978,15 @@
         }
     }              /* End case duplicate ack detection */
 
+    /* Linux very special zero-window probe detection (see bug #107) */
+    if ((0 == (hdr->flags & (PICO_TCP_PSH | PICO_TCP_SYN))) && /* This is a pure ack, and... */
+        (ACKN(f) == t->snd_nxt) &&                           /* it's acking our snd_nxt, and... */
+        (seq_compare(SEQN(f), t->rcv_nxt) < 0))             /* Has an old seq number */
+    {
+        tcp_send_ack(t);
+    }
+
+
     /* Do congestion control */
     tcp_congestion_control(t);
     if ((acked > 0) && t->sock.wakeup) {
@@ -1850,7 +1998,7 @@
 
     /* if Nagle enabled, check if no unack'ed data and fill out queue (till window) */
     if (IS_NAGLE_ENABLED((&(t->sock)))) {
-        while (!IS_TCP_HOLDQ_EMPTY(t) && ((t->tcpq_out.max_size - t->tcpq_out.size) >= PICO_TCP_DEFAULT_MSS)) {
+        while (!IS_TCP_HOLDQ_EMPTY(t) && ((t->tcpq_out.max_size - t->tcpq_out.size) >= t->mss)) {
             tcp_dbg_nagle("TCP_ACK - NAGLE add new segment\n");
             f_new = pico_hold_segment_make(t);
             if (f_new == NULL)
@@ -1972,6 +2120,7 @@
 {
     struct pico_socket_tcp *new = NULL;
     struct pico_tcp_hdr *hdr = NULL;
+    uint16_t mtu;
     if(s->number_of_pending_conn >= s->max_backlog)
         return -1;
 
@@ -2000,21 +2149,19 @@
 
 #endif
 
-    /* Set socket limits */
-    new->tcpq_in.max_size = PICO_DEFAULT_SOCKETQ;
-    new->tcpq_out.max_size = PICO_DEFAULT_SOCKETQ;
-    new->tcpq_hold.max_size = 2 * PICO_TCP_DEFAULT_MSS;
-    new->tcpq_in.overhead = (sizeof(struct tcp_input_segment) + sizeof(struct pico_tree_node));
-    new->tcpq_out.overhead = new->tcpq_hold.overhead = sizeof(struct pico_frame) + sizeof(struct pico_tree_node);
 
     f->sock = &new->sock;
     tcp_parse_options(f);
-    new->mss = PICO_TCP_DEFAULT_MSS;
+    mtu = pico_socket_get_mtu(s);
+    new->mss = (uint16_t)(mtu - PICO_SIZE_TCPHDR);
+    new->tcpq_in.max_size = PICO_DEFAULT_SOCKETQ;
+    new->tcpq_out.max_size = PICO_DEFAULT_SOCKETQ;
+    new->tcpq_hold.max_size = 2u * mtu;
     new->rcv_nxt = long_be(hdr->seq) + 1;
     new->snd_nxt = long_be(pico_paws());
     new->snd_last = new->snd_nxt;
     new->cwnd = PICO_TCP_IW;
-    new->ssthresh = 40;
+    new->ssthresh = (uint16_t)((uint16_t)(PICO_DEFAULT_SOCKETQ / new->mss) -  (((uint16_t)(PICO_DEFAULT_SOCKETQ / new->mss)) >> 3u));
     new->recv_wnd = short_be(hdr->rwnd);
     new->jumbo = hdr->len & 0x07;
     s->number_of_pending_conn++;
@@ -2028,18 +2175,53 @@
     return 0;
 }
 
+static int tcp_synrecv_syn(struct pico_socket *s, struct pico_frame *f)
+{
+    struct pico_tcp_hdr *hdr = NULL;
+    struct pico_socket_tcp *t = TCP_SOCK(s);
+    hdr = (struct pico_tcp_hdr *)f->transport_hdr;
+    if (t->rcv_nxt == long_be(hdr->seq) + 1) {
+        /* take back our own SEQ number to its original value,
+         * so the synack retransmitted is identical to the original. 
+         */
+        t->snd_nxt--; 
+        tcp_send_synack(s);
+    } else {
+        tcp_send_rst(s, f);
+        return -1;
+    }
+
+    return 0;
+}
+
 static void tcp_set_init_point(struct pico_socket *s)
 {
     struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
     t->rcv_processed = t->rcv_nxt;
 }
 
+
+uint16_t pico_tcp_get_socket_mtu(struct pico_socket *s)
+{
+    struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
+    if (t->mss > 0)
+        return (uint16_t)(t->mss + PICO_SIZE_TCPHDR);
+    else
+        return pico_socket_get_mtu(s);
+}
+
 static int tcp_synack(struct pico_socket *s, struct pico_frame *f)
 {
     struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
     struct pico_tcp_hdr *hdr  = (struct pico_tcp_hdr *)f->transport_hdr;
 
-    if (ACKN(f) ==  (1 + t->snd_nxt)) {
+    if (ACKN(f) ==  (1u + t->snd_nxt)) {
+        /* Get rid of initconn retry */
+        if(t->retrans_tmr) {
+            pico_timer_cancel(t->retrans_tmr);
+            t->retrans_tmr = NULL;
+        }
+
         t->rcv_nxt = long_be(hdr->seq);
         t->rcv_processed = t->rcv_nxt + 1;
         tcp_ack(s, f);
@@ -2059,16 +2241,20 @@
 
         return 0;
 
-    } else {
+    } else if ((hdr->flags & PICO_TCP_RST) == 0) {
         tcp_dbg("TCP> Not established, RST sent.\n");
         tcp_nosync_rst(s, f);
         return 0;
+    } else {
+        /* The segment has the reset flag on: Ignore! */
+        return 0;
     }
 }
 
 static int tcp_first_ack(struct pico_socket *s, struct pico_frame *f)
 {
     struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+    struct pico_tcp_hdr *hdr  = (struct pico_tcp_hdr *)f->transport_hdr;
     tcp_dbg("ACK in SYN_RECV: expecting %08x got %08x\n", t->snd_nxt, ACKN(f));
     if (t->snd_nxt == ACKN(f)) {
         tcp_set_init_point(s);
@@ -2090,8 +2276,11 @@
         s->ev_pending |= PICO_SOCK_EV_WR;
         tcp_dbg("%s: snd_nxt is now %08x\n", __FUNCTION__, t->snd_nxt);
         return 0;
+    } else if ((hdr->flags & PICO_TCP_RST) == 0) {
+        tcp_nosync_rst(s, f);
+        return 0;
     } else {
-        tcp_nosync_rst(s, f);
+        /* The segment has the reset flag on: Ignore! */
         return 0;
     }
 }
@@ -2109,34 +2298,45 @@
     if (f->flags & PICO_TCP_ACK)
         tcp_ack(s, f);
 
+    tcp_dbg("called close_wait, in state %08x\n", s->state);
+
     if (seq_compare(SEQN(f), t->rcv_nxt) == 0) {
         /* received FIN, increase ACK nr */
         t->rcv_nxt = long_be(hdr->seq) + 1;
-        s->state &= 0x00FFU;
-        s->state |= PICO_SOCKET_STATE_TCP_CLOSE_WAIT;
-        /* set SHUT_REMOTE */
-        s->state |= PICO_SOCKET_STATE_SHUT_REMOTE;
-        tcp_dbg("TCP> Close-wait\n");
-
-        if (s->wakeup) {
-            if(f->payload_len > 0) {
-                struct pico_socket_tcp *_t = (struct pico_socket_tcp *)s;
-                _t->sock.ev_pending |= PICO_SOCK_EV_CLOSE;
-            }else
+        if (seq_compare(SEQN(f), t->rcv_processed) == 0) {
+            if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_ESTABLISHED) {
+                tcp_dbg("Changing state to CLOSE_WAIT\n");
+                s->state &= 0x00FFU;
+                s->state |= PICO_SOCKET_STATE_TCP_CLOSE_WAIT;
+            }
+
+            /* set SHUT_REMOTE */
+            s->state |= PICO_SOCKET_STATE_SHUT_REMOTE;
+            tcp_dbg("TCP> Close-wait\n");
+            if (s->wakeup) {
                 s->wakeup(PICO_SOCK_EV_CLOSE, s);
+            }
+        } else {
+            t->remote_closed = 1;
         }
-    } else {
-        tcp_send_ack(t);              /* return ACK */
+    }
+
+    /* Ensure that the notification given to the socket
+     * did not put us in LAST_ACK state before sending the ACK: i.e. if
+     * pico_socket_close() has been called in the socket callback, we don't need to send
+     * an ACK here.
+     *
+     */
+    if (((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_CLOSE_WAIT) ||
+        ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_ESTABLISHED))
+    {
+        tcp_dbg("In closewait: Sending ack! (state is %08x)\n", s->state);
+        tcp_send_ack(t);
     }
 
     return 0;
 }
 
-/*static int tcp_fin(struct pico_socket *s, struct pico_frame *f)
-   {
-   return 0;
-   }*/
-
 static int tcp_rcvfin(struct pico_socket *s, struct pico_frame *f)
 {
     struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
@@ -2199,7 +2399,7 @@
     tcp_dbg("TCP >>>>>>>>>>>>>> received RST <<<<<<<<<<<<<<<<<<<<\n");
     if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_SYN_SENT) {
         /* the RST is acceptable if the ACK field acknowledges the SYN */
-        if ((t->snd_nxt + 1) == ACKN(f)) {              /* valid, got to closed state */
+        if ((t->snd_nxt + 1u) == ACKN(f)) {              /* valid, got to closed state */
             tcp_force_closed(s);
             pico_err = PICO_ERR_ECONNRESET;
             tcp_wakeup_pending(s, PICO_SOCK_EV_ERR);
@@ -2211,7 +2411,8 @@
     } else {              /* all other states */
         /* all reset (RST) segments are validated by checking their SEQ-fields,
            a reset is valid if its sequence number is in the window */
-        if ((long_be(hdr->seq) >= t->rcv_ackd) && (long_be(hdr->seq) <= ((uint32_t)(short_be(hdr->rwnd) << (t->wnd_scale)) + t->rcv_ackd))) {
+        uint32_t this_seq = long_be(hdr->seq);
+        if ((this_seq >= t->rcv_ackd) && (this_seq <= ((uint32_t)(short_be(hdr->rwnd) << (t->wnd_scale)) + t->rcv_ackd))) {
             if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_SYN_RECV) {
                 tcp_force_closed(s);
                 pico_err = PICO_ERR_ECONNRESET;
@@ -2272,14 +2473,14 @@
     int (*rst)(struct pico_socket *s, struct pico_frame *f);
 };
 
-static struct tcp_action_entry tcp_fsm[] = {
+static const struct tcp_action_entry tcp_fsm[] = {
     /* State                              syn              synack             ack                data             fin              finack           rst*/
     { PICO_SOCKET_STATE_TCP_UNDEF,        NULL,            NULL,              NULL,              NULL,            NULL,            NULL,            NULL     },
     { PICO_SOCKET_STATE_TCP_CLOSED,       NULL,            NULL,              NULL,              NULL,            NULL,            NULL,            NULL     },
     { PICO_SOCKET_STATE_TCP_LISTEN,       &tcp_syn,        NULL,              NULL,              NULL,            NULL,            NULL,            NULL     },
     { PICO_SOCKET_STATE_TCP_SYN_SENT,     NULL,            &tcp_synack,       NULL,              NULL,            NULL,            NULL,            &tcp_rst },
-    { PICO_SOCKET_STATE_TCP_SYN_RECV,     NULL,            NULL,              &tcp_first_ack,    &tcp_data_in,    NULL,            &tcp_closeconn,  &tcp_rst },
-    { PICO_SOCKET_STATE_TCP_ESTABLISHED,  &tcp_halfopencon, &tcp_ack,          &tcp_ack,          &tcp_data_in,    &tcp_closewait,  &tcp_closewait,  &tcp_rst },
+    { PICO_SOCKET_STATE_TCP_SYN_RECV,     &tcp_synrecv_syn, NULL,              &tcp_first_ack,    &tcp_data_in,    NULL,            &tcp_closeconn,  &tcp_rst },
+    { PICO_SOCKET_STATE_TCP_ESTABLISHED,  &tcp_halfopencon, &tcp_ack,         &tcp_ack,          &tcp_data_in,    &tcp_closewait,  &tcp_closewait,  &tcp_rst },
     { PICO_SOCKET_STATE_TCP_CLOSE_WAIT,   NULL,            &tcp_ack,          &tcp_ack,          &tcp_send_rst,   &tcp_closewait,  &tcp_closewait,  &tcp_rst },
     { PICO_SOCKET_STATE_TCP_LAST_ACK,     NULL,            &tcp_ack,          &tcp_lastackwait,  &tcp_send_rst,   &tcp_send_rst,   &tcp_send_rst,   &tcp_rst },
     { PICO_SOCKET_STATE_TCP_FIN_WAIT1,    NULL,            &tcp_ack,          &tcp_finwaitack,   &tcp_data_in,    &tcp_rcvfin,     &tcp_finack,     &tcp_rst },
@@ -2288,19 +2489,16 @@
     { PICO_SOCKET_STATE_TCP_TIME_WAIT,    NULL,            &tcp_ack,          &tcp_send_rst,     &tcp_send_rst,   &tcp_send_rst,   &tcp_send_rst,   &tcp_rst }
 };
 
-/*
-   NOTE: in SYN-RECV receiving syn when cloned by default (see yellow pos-it), should send reset.
- */
 #define MAX_VALID_FLAGS  9  /* Maximum number of valid flag combinations */
 static uint8_t invalid_flags(struct pico_socket *s, uint8_t flags)
 {
     uint8_t i;
-    static uint8_t valid_flags[PICO_SOCKET_STATE_TCP_ARRAYSIZ][MAX_VALID_FLAGS] = {
-        { /* PICO_SOCKET_STATE_TCP_UNDEF      */ },
-        { /* PICO_SOCKET_STATE_TCP_CLOSED     */ },
+    static const uint8_t valid_flags[PICO_SOCKET_STATE_TCP_ARRAYSIZ][MAX_VALID_FLAGS] = {
+        { /* PICO_SOCKET_STATE_TCP_UNDEF      */ 0, },
+        { /* PICO_SOCKET_STATE_TCP_CLOSED     */ 0, },
         { /* PICO_SOCKET_STATE_TCP_LISTEN     */ PICO_TCP_SYN },
         { /* PICO_SOCKET_STATE_TCP_SYN_SENT   */ PICO_TCP_SYNACK, PICO_TCP_RST, PICO_TCP_RSTACK},
-        { /* PICO_SOCKET_STATE_TCP_SYN_RECV   */ PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST},
+        { /* PICO_SOCKET_STATE_TCP_SYN_RECV   */ PICO_TCP_SYN, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST},
         { /* PICO_SOCKET_STATE_TCP_ESTABLISHED*/ PICO_TCP_SYN, PICO_TCP_SYNACK, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FIN, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST},
         { /* PICO_SOCKET_STATE_TCP_CLOSE_WAIT */ PICO_TCP_SYNACK, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FIN, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST},
         { /* PICO_SOCKET_STATE_TCP_LAST_ACK   */ PICO_TCP_SYNACK, PICO_TCP_ACK, PICO_TCP_PSH, PICO_TCP_PSHACK, PICO_TCP_FIN, PICO_TCP_FINACK, PICO_TCP_FINPSHACK, PICO_TCP_RST},
@@ -2312,10 +2510,10 @@
     if(!flags)
         return 1;
 
-    for(i = 0; i < MAX_VALID_FLAGS; i++)
+    for(i = 0; i < MAX_VALID_FLAGS; i++) {
         if(valid_flags[s->state >> 8u][i] == flags)
             return 0;
-
+    }
     return 1;
 }
 int pico_tcp_input(struct pico_socket *s, struct pico_frame *f)
@@ -2323,21 +2521,24 @@
     struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr);
     int ret = 0;
     uint8_t flags = hdr->flags;
-    struct tcp_action_entry *action = &tcp_fsm[s->state >> 8];
-
-    f->payload = (f->transport_hdr + ((hdr->len & 0xf0) >> 2));
-    f->payload_len = (uint16_t)(f->transport_len - ((hdr->len & 0xf0) >> 2));
-
-    tcp_dbg("[%lu] TCP> [tcp input] socket: %p state: %d <-- local port:%d remote port: %d seq: %08x ack: %08x flags: %02x = t_len: %d, hdr: %u payload: %d\n", TCP_TIME,
-            s, s->state >> 8, short_be(hdr->trans.dport), short_be(hdr->trans.sport), SEQN(f), ACKN(f), hdr->flags, f->transport_len, (hdr->len & 0xf0) >> 2, f->payload_len );
+    const struct tcp_action_entry *action = &tcp_fsm[s->state >> 8];
+
+    f->payload = (f->transport_hdr + ((hdr->len & 0xf0u) >> 2u));
+    f->payload_len = (uint16_t)(f->transport_len - ((hdr->len & 0xf0u) >> 2u));
+
+    tcp_dbg("[sam] TCP> [tcp input] t_len: %u\n", f->transport_len);
+    tcp_dbg("[sam] TCP> flags = %02x\n", hdr->flags);
+    tcp_dbg("[sam] TCP> s->state >> 8 = %u\n", s->state >> 8);
+    tcp_dbg("[%lu] TCP> [tcp input] socket: %p state: %d <-- local port:%u remote port: %u seq: %08x ack: %08x flags: %02x t_len: %u, hdr: %u payload: %d\n", TCP_TIME, s, s->state >> 8, short_be(hdr->trans.dport), short_be(hdr->trans.sport), SEQN(f), ACKN(f), hdr->flags, f->transport_len, (hdr->len & 0xf0) >> 2, f->payload_len );
 
     /* This copy of the frame has the current socket as owner */
     f->sock = s;
     s->timestamp = TCP_TIME;
     /* Those are not supported at this time. */
     /* flags &= (uint8_t) ~(PICO_TCP_CWR | PICO_TCP_URG | PICO_TCP_ECN); */
-    if(invalid_flags(s, flags))
+    if(invalid_flags(s, flags)) {
         pico_tcp_reply_rst(f);
+    }
     else if (flags == PICO_TCP_SYN) {
         if (action->syn)
             action->syn(s, f);
@@ -2375,6 +2576,9 @@
         }
     }
 
+    if (s->ev_pending)
+        tcp_wakeup_pending(s, s->ev_pending);
+
 /* discard: */
     pico_frame_discard(f);
     return ret;
@@ -2384,6 +2588,66 @@
 inline static int checkLocalClosing(struct pico_socket *s);
 inline static int checkRemoteClosing(struct pico_socket *s);
 
+static struct pico_frame *tcp_split_segment(struct pico_socket_tcp *t, struct pico_frame *f, uint16_t size)
+{
+    struct pico_frame *f1, *f2;
+    uint16_t size1, size2, size_f;
+    uint16_t overhead;
+    struct pico_tcp_hdr *hdr1, *hdr2, *hdr = (struct pico_tcp_hdr *)f->transport_hdr;
+    overhead = pico_tcp_overhead(&t->sock);
+    size_f = f->payload_len;
+
+
+    if (size >= size_f)
+        return f; /* no need to split! */
+
+    size1 = size;
+    size2 = (uint16_t)(size_f - size);
+
+    f1 = pico_socket_frame_alloc(&t->sock, (uint16_t) (size1 + overhead));
+    f2 = pico_socket_frame_alloc(&t->sock, (uint16_t) (size2 + overhead));
+
+    if (!f1 || !f2) {
+        pico_err = PICO_ERR_ENOMEM;
+        return NULL;
+    }
+
+    /* Advance payload pointer to the beginning of segment data */
+    f1->payload += overhead;
+    f1->payload_len = (uint16_t)(f1->payload_len - overhead);
+    f2->payload += overhead;
+    f2->payload_len = (uint16_t)(f2->payload_len - overhead);
+
+    hdr1 = (struct pico_tcp_hdr *)f1->transport_hdr;
+    hdr2 = (struct pico_tcp_hdr *)f2->transport_hdr;
+
+    /* Copy payload */
+    memcpy(f1->payload, f->payload, size1);
+    memcpy(f2->payload, f->payload + size1, size2);
+
+    /* Copy tcp hdr */
+    memcpy(hdr1, hdr, sizeof(struct pico_tcp_hdr));
+    memcpy(hdr2, hdr, sizeof(struct pico_tcp_hdr));
+
+    /* Adjust f2's sequence number */
+    hdr2->seq = long_be(SEQN(f) + size1);
+
+    /* Add TCP options */
+    pico_tcp_flags_update(f1, &t->sock);
+    pico_tcp_flags_update(f2, &t->sock);
+    tcp_add_options_frame(t, f1);
+    tcp_add_options_frame(t, f2);
+
+    /* Get rid of the full frame */
+    pico_discard_segment(&t->tcpq_out, f);
+
+    /* Enqueue f2 for later send... */
+    pico_enqueue_segment(&t->tcpq_out, f2);
+
+    /* Return the partial frame */
+    return f1;
+}
+
 
 int pico_tcp_output(struct pico_socket *s, int loop_score)
 {
@@ -2391,35 +2655,46 @@
     struct pico_frame *f, *una;
     int sent = 0;
     int data_sent = 0;
+    int32_t seq_diff = 0;
 
     una = first_segment(&t->tcpq_out);
     f = peek_segment(&t->tcpq_out, t->snd_nxt);
 
     while((f) && (t->cwnd >= t->in_flight)) {
         f->timestamp = TCP_TIME;
+        add_retransmission_timer(t, t->rto + TCP_TIME);
         tcp_add_options_frame(t, f);
-        if (seq_compare((SEQN(f) + f->payload_len), (SEQN(una) + (uint32_t)(t->recv_wnd << t->recv_wnd_scale))) > 0) {
-            t->cwnd = (uint16_t)t->in_flight;
-            if (t->cwnd < 1)
-                t->cwnd = 1;
-
+        seq_diff = seq_compare(SEQN(f), SEQN(una));
+        if (seq_diff < 0) {
+            dbg(">>> FATAL: seq diff is negative!\n");
+            break;
+        }
+
+        /* Check if advertised window is full */
+        if ((uint32_t)seq_diff >= (uint32_t)(t->recv_wnd << t->recv_wnd_scale)) {
             if (t->x_mode != PICO_TCP_WINDOW_FULL) {
                 tcp_dbg("TCP> RIGHT SIZING (rwnd: %d, frame len: %d\n", t->recv_wnd << t->recv_wnd_scale, f->payload_len);
                 tcp_dbg("In window full...\n");
-                t->snd_nxt = SEQN(una);              /* XXX prevent out-of-order-packets ! */ /*DLA re-enabled.*/
-                t->snd_retry = SEQN(una);              /* XXX replace by retry pointer? */
-
-                /* Alternative to the line above:  (better performance, but seems to lock anyway with larger buffers)
-                   if (seq_compare(t->snd_nxt, SEQN(una)) > 0)
-                   t->snd_nxt -= f->payload_len;
-                 */
-
+                t->snd_nxt = SEQN(una);
+                t->snd_retry = SEQN(una);
                 t->x_mode = PICO_TCP_WINDOW_FULL;
             }
 
             break;
         }
 
+        /* Check if the advertised window is too small to receive the current frame */
+        if ((uint32_t)(seq_diff + f->payload_len) > (uint32_t)(t->recv_wnd << t->recv_wnd_scale)) {
+            f = tcp_split_segment(t, f, (uint16_t)(t->recv_wnd << t->recv_wnd_scale));
+            if (!f)
+                break;
+
+            /* Limit sending window to packets in flight (right sizing) */
+            t->cwnd = (uint16_t)t->in_flight;
+            if (t->cwnd < 1)
+                t->cwnd = 1;
+        }
+
         tcp_dbg("TCP> DEQUEUED (for output) frame %08x, acks %08x len= %d, remaining frames %d\n", SEQN(f), ACKN(f), f->payload_len, t->tcpq_out.frames);
         tcp_send(t, f);
         sent++;
@@ -2436,8 +2711,7 @@
         }
     }
     if ((sent > 0 && data_sent > 0)) {
-        if (t->rto < PICO_TCP_RTO_MIN)
-            t->rto = PICO_TCP_RTO_MIN;
+        rto_set(t, t->rto);
     } else {
         /* Nothing to transmit. */
     }
@@ -2469,7 +2743,7 @@
     f_temp = next_segment(&t->tcpq_hold, f_temp);
 
     /* check till total_len <= MSS */
-    while ((f_temp != NULL) && ((total_len + f_temp->payload_len) <= PICO_TCP_DEFAULT_MSS)) {
+    while ((f_temp != NULL) && ((total_len + f_temp->payload_len) <= t->mss)) {
         total_len = (uint16_t)(total_len + f_temp->payload_len);
         f_temp = next_segment(&t->tcpq_hold, f_temp);
         if (f_temp == NULL)
@@ -2482,6 +2756,7 @@
         return f_new;
     }
 
+    pico_tcp_flags_update(f_new,&t->sock);
     hdr = (struct pico_tcp_hdr *) f_new->transport_hdr;
     /* init new frame */
     f_new->payload += off;
@@ -2494,7 +2769,7 @@
     hdr->trans.dport = t->sock.remote_port;
 
     /* check till total_payload_len <= MSS */
-    while ((f_temp != NULL) && ((total_payload_len + f_temp->payload_len) <= PICO_TCP_DEFAULT_MSS)) {
+    while ((f_temp != NULL) && ((total_payload_len + f_temp->payload_len) <= t->mss)) {
         /* cpy data and discard frame */
         test++;
         memcpy(f_new->payload + total_payload_len, f_temp->payload, f_temp->payload_len);
@@ -2502,9 +2777,10 @@
         pico_discard_segment(&t->tcpq_hold, f_temp);
         f_temp = first_segment(&t->tcpq_hold);
     }
-    hdr->len = (uint8_t)((f_new->payload - f_new->transport_hdr) << 2 | t->jumbo);
+    hdr->len = (uint8_t)((f_new->payload - f_new->transport_hdr) << 2u | (int8_t)t->jumbo);
 
     tcp_dbg_nagle("NAGLE make - joined %d segments, len %d bytes\n", test, total_payload_len);
+    tcp_add_options_frame(t,f_new);
 
     return f_new;
 }
@@ -2522,7 +2798,7 @@
     hdr->trans.sport = t->sock.local_port;
     hdr->trans.dport = t->sock.remote_port;
     hdr->seq = long_be(t->snd_last + 1);
-    hdr->len = (uint8_t)((f->payload - f->transport_hdr) << 2 | t->jumbo);
+    hdr->len = (uint8_t)((f->payload - f->transport_hdr) << 2u | (int8_t)t->jumbo);
 
     if ((uint32_t)f->payload_len > (uint32_t)(t->tcpq_out.max_size - t->tcpq_out.size))
         t->sock.ev_pending &= (uint16_t)(~PICO_SOCK_EV_WR);
@@ -2554,7 +2830,7 @@
             }
         } else {                                    /* opt 2. hold data back */
             total_len = f->payload_len + t->tcpq_hold.size;
-            if ((total_len >= PICO_TCP_DEFAULT_MSS) && ((t->tcpq_out.max_size - t->tcpq_out.size) >= PICO_TCP_DEFAULT_MSS)) {              /* TODO check mss socket */
+            if ((total_len >= t->mss) && ((t->tcpq_out.max_size - t->tcpq_out.size) >= t->mss)) {              /* TODO check mss socket */
                 /* IF enough data in hold (>mss) AND space in out queue (>mss) */
                 /* add current frame in hold and make new segment */
                 if (pico_enqueue_segment(&t->tcpq_hold, f) > 0 ) {
@@ -2594,7 +2870,7 @@
 inline static void tcp_discard_all_segments(struct pico_tcp_queue *tq)
 {
     struct pico_tree_node *index = NULL, *index_safe = NULL;
-    LOCK(Mutex);
+    PICOTCP_MUTEX_LOCK(Mutex);
     pico_tree_foreach_safe(index, &tq->pool, index_safe)
     {
         void *f = index->keyValue;
@@ -2605,15 +2881,15 @@
         if(IS_INPUT_QUEUE(tq))
         {
             struct tcp_input_segment *inp = (struct tcp_input_segment *)f;
-            pico_free(inp->payload);
-            pico_free(inp);
+            PICO_FREE(inp->payload);
+            PICO_FREE(inp);
         }
         else
             pico_frame_discard(f);
     }
     tq->frames = 0;
     tq->size = 0;
-    UNLOCK(Mutex);
+    PICOTCP_MUTEX_UNLOCK(Mutex);
 }
 
 void pico_tcp_cleanup_queues(struct pico_socket *sck)
@@ -2678,8 +2954,47 @@
     }
 }
 
-void transport_flags_update(struct pico_frame *f, struct pico_socket *s)
+
+int pico_tcp_check_listen_close(struct pico_socket *s)
+{
+    if (TCP_IS_STATE(s, PICO_SOCKET_STATE_TCP_LISTEN)) {
+        pico_socket_del(s);
+        return 0;
+    }
+    return -1;
+}
+
+void pico_tcp_flags_update(struct pico_frame *f, struct pico_socket *s)
 {
     f->transport_flags_saved = ((struct pico_socket_tcp *)s)->ts_ok;
 }
+
+int pico_tcp_set_bufsize_in(struct pico_socket *s, uint32_t value)
+{
+    struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+    t->tcpq_in.max_size = value;
+    return 0;
+}
+
+int pico_tcp_set_bufsize_out(struct pico_socket *s, uint32_t value)
+{
+    struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+    t->tcpq_out.max_size = value;
+    return 0;
+}
+
+int pico_tcp_get_bufsize_in(struct pico_socket *s, uint32_t *value)
+{
+    struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+    *value = t->tcpq_in.max_size;
+    return 0;
+}
+
+int pico_tcp_get_bufsize_out(struct pico_socket *s, uint32_t *value)
+{
+    struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
+    *value = t->tcpq_out.max_size;
+    return 0;
+}
+
 #endif /* PICO_SUPPORT_TCP */