pico_tcp.c

00001 /*********************************************************************
00002 PicoTCP. Copyright (c) 2012 TASS Belgium NV. Some rights reserved.
00003 See LICENSE and COPYING for usage.
00004 
00005 .
00006 
00007 Authors: Daniele Lacamera, Philippe Mariman
00008 *********************************************************************/
00009 
00010 #include "pico_tcp.h"
00011 #include "pico_config.h"
00012 #include "pico_eth.h"
00013 #include "pico_socket.h"
00014 #include "pico_stack.h"
00015 #include "pico_socket.h"
00016 #include "pico_queue.h"
00017 #include "pico_tree.h"
00018 
00019 #define TCP_SOCK(s) ((struct pico_socket_tcp *)s)
00020 #define SEQN(f) (f?(long_be(((struct pico_tcp_hdr *)(f->transport_hdr))->seq)):0)
00021 #define ACKN(f) (f?(long_be(((struct pico_tcp_hdr *)(f->transport_hdr))->ack)):0)
00022 
00023 #define PICO_TCP_RTO_MIN 10
00024 #define PICO_TCP_RTO_MAX 120000
00025 #define PICO_TCP_IW          2
00026 #define PICO_TCP_SYN_TO  1000
00027 
00028 #define PICO_TCP_MAX_CONNECT_RETRIES 7
00029 
00030 #define PICO_TCP_LOOKAHEAD      0x00
00031 #define PICO_TCP_FIRST_DUPACK   0x01
00032 #define PICO_TCP_SECOND_DUPACK  0x02
00033 #define PICO_TCP_RECOVER        0x03
00034 #define PICO_TCP_BLACKOUT       0x04
00035 #define PICO_TCP_UNREACHABLE    0x05
00036 #define PICO_TCP_WINDOW_FULL    0x06
00037 
00038 /* check if the Nagle algorithm is enabled on the socket */
00039 #define IS_NAGLE_ENABLED(s)     (!(!(!(s->opt_flags & (1 << PICO_SOCKET_OPT_TCPNODELAY)))))
00040 /* check if tcp connection is "idle" according to Nagle (RFC 896) */
00041 #define IS_TCP_IDLE(t)          ((t->in_flight == 0) && (t->tcpq_out.size == 0))
00042 /* check if the hold queue contains data (again Nagle) */
00043 #define IS_TCP_HOLDQ_EMPTY(t)   (t->tcpq_hold.size == 0)
00044 
00045 #ifdef PICO_SUPPORT_TCP
00046 #define tcp_dbg_nagle(...) do{}while(0)
00047 #define tcp_dbg_options(...) do{}while(0)
00048 
00049 
00050 #define tcp_dbg(...) do{}while(0)
00051 //#define tcp_dbg dbg
00052 
00053 
00054 #ifdef PICO_SUPPORT_MUTEX
00055 static void * Mutex = NULL;
00056 #define LOCK(x) {\
00057   if (x == NULL) \
00058     x = pico_mutex_init(); \
00059   pico_mutex_lock(x); \
00060 }
00061 #define UNLOCK(x) pico_mutex_unlock(x);
00062 
00063 #else
00064 #define LOCK(x) do{}while(0)
00065 #define UNLOCK(x) do{}while(0)
00066 #endif
00067 
00068 
00069 static inline int seq_compare(uint32_t a, uint32_t b)
00070 {
00071   uint32_t thresh = ((uint32_t)(-1))>>1;
00072   if (((a > thresh) && (b > thresh)) || ((a <= thresh) && (b <= thresh))) {
00073     if (a > b)
00074       return 1;
00075     if (b > a)
00076       return -1;
00077   } else {
00078     if (a > b)
00079       return -2;
00080     if (b > a)
00081       return 2;
00082   }
00083   return 0;
00084 }
00085 
00086 static int segment_compare(void * ka, void * kb)
00087 {
00088   struct pico_frame *a = ka, *b = kb;
00089   return seq_compare(SEQN(a), SEQN(b));
00090 }
00091 
00092 struct pico_tcp_queue
00093 {
00094   struct pico_tree pool;
00095   uint32_t max_size;
00096   uint32_t size;
00097   uint32_t frames;
00098 };
00099 
00100 static struct pico_frame *peek_segment(struct pico_tcp_queue *tq, uint32_t seq)
00101 {
00102   struct pico_tcp_hdr H;
00103   struct pico_frame f = {};
00104   f.transport_hdr = (uint8_t *) (&H);
00105   H.seq = long_be(seq);
00106 
00107   return pico_tree_findKey(&tq->pool,&f);
00108 }
00109 
00110 static struct pico_frame *first_segment(struct pico_tcp_queue *tq)
00111 {
00112   return pico_tree_first(&tq->pool);
00113 }
00114 
00115 static struct pico_frame *next_segment(struct pico_tcp_queue *tq, struct pico_frame *cur)
00116 {
00117   if (!cur)
00118     return NULL;
00119   return peek_segment(tq, SEQN(cur) + cur->payload_len);
00120 }
00121 
00122 static int pico_enqueue_segment(struct pico_tcp_queue *tq, struct pico_frame *f)
00123 {
00124     int ret = -1;
00125   if (f->payload_len <= 0) {
00126     tcp_dbg("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! TRIED TO ENQUEUE INVALID SEGMENT!\n");
00127     //abort();
00128     return -1;
00129   }
00130     LOCK(Mutex);
00131   if ((tq->size + f->payload_len) > tq->max_size)
00132   {
00133     ret = 0;
00134     goto out;
00135   }
00136   if (pico_tree_insert(&tq->pool,f) != 0)
00137   {
00138     ret = 0;
00139     goto out;
00140   }
00141   tq->size += f->payload_len;
00142   if (f->payload_len > 0)
00143     tq->frames++;
00144   ret = f->payload_len;
00145 
00146 out :
00147   UNLOCK(Mutex);
00148   return ret;
00149 }
00150 
00151 static void pico_discard_segment(struct pico_tcp_queue *tq, struct pico_frame *f)
00152 {
00153   struct pico_frame *f1;
00154   LOCK(Mutex);
00155   f1 = pico_tree_delete(&tq->pool,f);
00156   if (f1) {
00157     tq->size -= f->payload_len;
00158     if (f->payload_len > 0)
00159       tq->frames--;
00160   }
00161   pico_frame_discard(f);
00162   UNLOCK(Mutex);
00163 }
00164 
00165 /* Structure for TCP socket */
00166 struct tcp_sack_block {
00167   uint32_t left;
00168   uint32_t right;
00169   struct tcp_sack_block *next;
00170 };
00171 
00172 struct pico_socket_tcp {
00173   struct pico_socket sock;
00174 
00175   /* Tree/queues */
00176   struct pico_tcp_queue tcpq_in;
00177   struct pico_tcp_queue tcpq_out;
00178   struct pico_tcp_queue tcpq_hold;  /* buffer to hold delayed frames according to Nagle */
00179 
00180   /* tcp_output */
00181   uint32_t snd_nxt;
00182   uint32_t snd_last;
00183   uint32_t snd_old_ack;
00184   uint32_t snd_retry;
00185   uint32_t snd_last_out;
00186 
00187   /* congestion control */
00188   uint32_t avg_rtt;
00189   uint32_t rttvar;
00190   uint32_t rto;
00191   uint32_t in_flight;
00192   uint8_t  timer_running;
00193   uint8_t  keepalive_timer_running;
00194   uint16_t cwnd_counter;
00195   uint16_t cwnd;
00196   uint16_t ssthresh;
00197   uint16_t recv_wnd;
00198   uint16_t recv_wnd_scale;
00199 
00200   /* tcp_input */
00201   uint32_t rcv_nxt;
00202   uint32_t rcv_ackd;
00203   uint32_t rcv_processed;
00204   uint16_t wnd;
00205   uint16_t wnd_scale;
00206 
00207   /* options */
00208   uint32_t ts_nxt;
00209   uint16_t mss;
00210   uint8_t sack_ok;
00211   uint8_t ts_ok;
00212   uint8_t mss_ok;
00213   uint8_t scale_ok;
00214   struct tcp_sack_block *sacks;
00215   uint8_t jumbo;
00216 
00217   /* Transmission */
00218   uint8_t  x_mode;
00219   uint8_t  dupacks;
00220   uint8_t  backoff;
00221 
00222 };
00223 
00224 /* Queues */
00225 static struct pico_queue tcp_in = {};
00226 static struct pico_queue tcp_out = {};
00227 
00228 /* If Nagle enabled, this function can make 1 new segment from smaller segments in hold queue */
00229 static struct pico_frame * pico_hold_segment_make(struct pico_socket_tcp *t);
00230 
00231 /* checks if tcpq_in is empty */
00232 int pico_tcp_queue_in_is_empty(struct pico_socket *s)
00233 {
00234   struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
00235 
00236   if (t->tcpq_in.frames == 0)
00237     return 1;
00238   else
00239     return 0;
00240 }
00241 
00242 /* Useful for getting rid of the beginning of the buffer (read() op) */
00243 static int release_until(struct pico_tcp_queue *q, uint32_t seq)
00244 {
00245   struct pico_frame *head = first_segment(q);
00246   int ret = 0;
00247   while (head && (seq_compare(SEQN(head) + head->payload_len, seq) <= 0)) {
00248     struct pico_frame *cur = head;
00249     head = next_segment(q, cur);
00250     tcp_dbg("Releasing %p\n", q);
00251     pico_discard_segment(q, cur);
00252     ret++;
00253   }
00254   return ret;
00255 }
00256 
00257 static int release_all_until(struct pico_tcp_queue *q, uint32_t seq)
00258 {
00259   struct pico_frame *f = NULL, *tmp __attribute__((unused));
00260   struct pico_tree_node * idx, * temp;
00261   int ret = 0;
00262 
00263   pico_tree_foreach_safe(idx,&q->pool,temp){
00264   f = idx->keyValue;
00265     if (seq_compare(SEQN(f) + f->payload_len, seq) <= 0) {
00266       tcp_dbg("Releasing %p\n", f);
00267       pico_discard_segment(q, f);
00268       ret++;
00269     } else
00270       return ret;
00271   }
00272   return ret;
00273 }
00274 
00275 /* API calls */
00276 
00277 uint16_t pico_tcp_checksum_ipv4(struct pico_frame *f)
00278 {
00279   struct pico_ipv4_hdr *hdr = (struct pico_ipv4_hdr *) f->net_hdr;
00280   struct pico_tcp_hdr *tcp_hdr = (struct pico_tcp_hdr *) f->transport_hdr;
00281   struct pico_socket *s = f->sock;
00282   struct pico_ipv4_pseudo_hdr pseudo;
00283 
00284   if (s) {
00285     /* Case of outgoing frame */
00286     //dbg("TCP CRC: on outgoing frame\n");
00287     pseudo.src.addr = s->local_addr.ip4.addr;
00288     pseudo.dst.addr = s->remote_addr.ip4.addr;
00289   } else {
00290     /* Case of incomming frame */
00291     //dbg("TCP CRC: on incomming frame\n");
00292     pseudo.src.addr = hdr->src.addr;
00293     pseudo.dst.addr = hdr->dst.addr;
00294   }
00295   pseudo.zeros = 0;
00296   pseudo.proto = PICO_PROTO_TCP;
00297   pseudo.len = short_be(f->transport_len);
00298 
00299   return pico_dualbuffer_checksum(&pseudo, sizeof(struct pico_ipv4_pseudo_hdr), tcp_hdr, f->transport_len);
00300 }
00301 
00302 static void tcp_send_fin(struct pico_socket_tcp *t);
00303 static int pico_tcp_process_out(struct pico_protocol *self, struct pico_frame *f)
00304 {
00305   struct pico_tcp_hdr *hdr;
00306   struct pico_socket_tcp *t = (struct pico_socket_tcp *)f->sock;
00307   hdr = (struct pico_tcp_hdr *)f->transport_hdr;
00308 
00309   if (f->payload_len > 0) {
00310     tcp_dbg("Process out: sending %p (%d bytes)\n",f, f->payload_len);
00311   } else {
00312     tcp_dbg("Sending empty packet\n");
00313   }
00314 
00315   if (f->payload_len > 0) {
00316     if (seq_compare(SEQN(f) + f->payload_len, t->snd_nxt) > 0) {
00317       t->snd_nxt = SEQN(f) + f->payload_len;
00318       tcp_dbg("%s: snd_nxt is now %08x\n", __FUNCTION__, t->snd_nxt);
00319     }
00320   } else if (hdr->flags == PICO_TCP_ACK) { /* pure ack */
00321     //hdr->seq = long_be(t->snd_nxt);   /* XXX disabled this to not to mess with seq nrs of ACKs anymore */
00322   } else {
00323     tcp_dbg("%s: non-pure ACK with len=0, fl:%04x\n", __FUNCTION__, hdr->flags);
00324   }
00325 
00326   pico_network_send(f);
00327   return 0;
00328 }
00329 
00330 int pico_tcp_push(struct pico_protocol *self, struct pico_frame *data);
00331 
00332 /* Interface: protocol definition */
00333 struct pico_protocol pico_proto_tcp = {
00334   .name = "tcp",
00335   .proto_number = PICO_PROTO_TCP,
00336   .layer = PICO_LAYER_TRANSPORT,
00337   .process_in = pico_transport_process_in,
00338   .process_out = pico_tcp_process_out,
00339   .push = pico_tcp_push,
00340   .q_in = &tcp_in,
00341   .q_out = &tcp_out,
00342 };
00343 
00344 static uint32_t pico_paws(void)
00345 {
00346   static unsigned long _paws = 0;
00347   _paws = pico_rand();
00348   return long_be(_paws); /*XXX: implement paws */
00349 }
00350 
00351 static void tcp_add_options(struct pico_socket_tcp *ts, struct pico_frame *f, uint16_t flags, int optsiz)
00352 {
00353   uint32_t tsval = long_be(pico_tick);
00354   uint32_t tsecr = long_be(ts->ts_nxt);
00355   int i = 0;
00356   f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
00357 
00358   memset(f->start, PICO_TCP_OPTION_NOOP, optsiz); /* fill blanks with noop */
00359 
00360   if (flags & PICO_TCP_SYN) {
00361     f->start[i++] = PICO_TCP_OPTION_MSS;
00362     f->start[i++] = PICO_TCPOPTLEN_MSS;
00363     f->start[i++] = (ts->mss >> 8) & 0xFF;
00364     f->start[i++] = ts->mss & 0xFF;
00365     f->start[i++] = PICO_TCP_OPTION_SACK_OK;
00366     f->start[i++] = PICO_TCPOPTLEN_SACK_OK;
00367   }
00368 
00369   f->start[i++] = PICO_TCP_OPTION_WS;
00370   f->start[i++] = PICO_TCPOPTLEN_WS;
00371   f->start[i++] = ts->wnd_scale;
00372 
00373   if (optsiz >= 12) {
00374     f->start[i++] = PICO_TCP_OPTION_TIMESTAMP;
00375     f->start[i++] = PICO_TCPOPTLEN_TIMESTAMP;
00376     memcpy(f->start + i, &tsval, 4);
00377     i += 4;
00378     memcpy(f->start + i, &tsecr, 4);
00379     i += 4;
00380   }
00381 
00382   if (flags & PICO_TCP_ACK) {
00383     struct tcp_sack_block *sb;
00384     int len_off;
00385 
00386     if (ts->sack_ok && ts->sacks) {
00387       f->start[i++] = PICO_TCP_OPTION_SACK;
00388       len_off = i;
00389       f->start[i++] = PICO_TCPOPTLEN_SACK;
00390       while(ts->sacks) {
00391         sb = ts->sacks;
00392         ts->sacks = sb->next;
00393         memcpy(f->start + i, sb, 2 * sizeof(uint32_t));
00394         i += (2 * sizeof(uint32_t));
00395         f->start[len_off] += (2 * sizeof(uint32_t));
00396         pico_free(sb);
00397       }
00398     }
00399   }
00400   if (i < optsiz)
00401     f->start[ optsiz - 1 ] = PICO_TCP_OPTION_END;
00402 }
00403 
00404 static void tcp_send_ack(struct pico_socket_tcp *t);
00405 
00406 static void tcp_set_space(struct pico_socket_tcp *t)
00407 {
00408   int mtu, space;
00409   int shift = 0;
00410 
00411   mtu = t->mss + PICO_SIZE_TCPHDR + PICO_SIZE_TCPOPT_SYN ;
00412   if (t->tcpq_in.max_size == 0) {
00413     space = 1024 * 1024 * 1024; /* One Gigabyte, for unlimited sockets. */
00414   } else {
00415     space = ((t->tcpq_in.max_size - t->tcpq_in.size) / mtu) * t->mss;
00416   }
00417   if (space < 0)
00418     space = 0;
00419   while(space > 0xFFFF) {
00420     space >>= 1;
00421     shift++;
00422   }
00423   if ((space != t->wnd) || (shift != t->wnd_scale) || ((space - t->wnd) > (space>>2))) {
00424     t->wnd = space;
00425     t->wnd_scale = shift;
00426   }
00427 }
00428 
00429 /* Return 32-bit aligned option size */
00430 static int tcp_options_size(struct pico_socket_tcp *t, uint16_t flags)
00431 {
00432   int size = 0;
00433   struct tcp_sack_block *sb = t->sacks;
00434 
00435   if (flags & PICO_TCP_SYN) {  /* Full options */
00436     size = PICO_TCPOPTLEN_MSS + PICO_TCP_OPTION_SACK_OK + PICO_TCPOPTLEN_WS + PICO_TCPOPTLEN_TIMESTAMP;
00437   } else {
00438 
00439    /* Always update window scale. */
00440     size += PICO_TCPOPTLEN_WS;
00441 
00442     if (t->ts_ok)
00443       size += PICO_TCPOPTLEN_TIMESTAMP;
00444 
00445     size+= PICO_TCPOPTLEN_END;
00446   }
00447   if ((flags & PICO_TCP_ACK) && (t->sack_ok && sb)) {
00448     size += 2;
00449     while(sb) {
00450       size += (2 * sizeof(uint32_t));
00451       sb = sb->next;
00452     }
00453   }
00454   size = (((size + 3) >> 2) << 2);
00455   return size;
00456 }
00457 
00458 int pico_tcp_overhead(struct pico_socket *s)
00459 {
00460   if (!s)
00461     return 0;
00462 
00463   return PICO_SIZE_TCPHDR + tcp_options_size((struct pico_socket_tcp *)s, 0); /* hdr + Options size for data pkt */
00464 
00465 }
00466 
00467 static void tcp_process_sack(struct pico_socket_tcp *t, uint32_t start, uint32_t end)
00468 {
00469   struct pico_frame *f;
00470   struct pico_tree_node * index, * temp;
00471   int cmp;
00472   int count = 0;
00473 
00474   pico_tree_foreach_safe(index,&t->tcpq_out.pool,temp){
00475     f = index->keyValue;
00476     cmp = seq_compare(SEQN(f), start);
00477     if (cmp > 0)
00478       goto done;
00479 
00480     if (cmp == 0) {
00481       cmp = seq_compare(SEQN(f) + f->payload_len, end);
00482       if (cmp > 0) {
00483         tcp_dbg("Invalid SACK: ignoring.\n");
00484       }
00485 
00486       tcp_dbg("Marking (by SACK) segment %08x BLK:[%08x::%08x]\n", SEQN(f), start, end);
00487       f->flags |= PICO_FRAME_FLAG_SACKED;
00488       count++;
00489 
00490       if (cmp == 0) {
00491         /* that was last segment sacked. Job done */
00492         goto done;
00493       }
00494     }
00495   }
00496 
00497 done:
00498   if (t->x_mode > PICO_TCP_LOOKAHEAD) {
00499     if (t->in_flight > (count))
00500       t->in_flight -= (count);
00501     else
00502       t->in_flight = 0;
00503   }
00504 }
00505 
00506 static void tcp_rcv_sack(struct pico_socket_tcp *t, uint8_t *opt, int len)
00507 {
00508   uint32_t start, end;
00509   int i = 0;
00510   if (len % 8) {
00511     tcp_dbg("SACK: Invalid len.\n");
00512     return;
00513   }
00514   while (i < len) {
00515     start = long_from(opt + i);
00516     i += 4;
00517     end = long_from(opt + i);
00518     i += 4;
00519     tcp_process_sack(t, long_be(start), long_be(end));
00520   }
00521 }
00522 
00523 static void tcp_parse_options(struct pico_frame *f)
00524 {
00525   struct pico_socket_tcp *t = (struct pico_socket_tcp *)f->sock;
00526   uint8_t *opt = f->transport_hdr + PICO_SIZE_TCPHDR;
00527   int i = 0;
00528   f->timestamp = 0;
00529   while (i < (f->transport_len - PICO_SIZE_TCPHDR)) {
00530     uint8_t type =  opt[i++];
00531     uint8_t len;
00532     if(i < (f->transport_len - PICO_SIZE_TCPHDR) && (type > 1))
00533       len =  opt[i++];
00534     else
00535       len = 1;
00536     if (f->payload && ((opt + i) > f->payload))
00537       break;
00538     tcp_dbg_options("Received option '%d', len = %d \n", type, len);
00539     switch (type) {
00540       case PICO_TCP_OPTION_NOOP:
00541       case PICO_TCP_OPTION_END:
00542         break;
00543       case PICO_TCP_OPTION_WS:
00544         if (len != PICO_TCPOPTLEN_WS) {
00545           tcp_dbg_options("TCP Window scale: bad len received (%d).\n", len);
00546           i += len - 2;
00547           break;
00548         }
00549         t->recv_wnd_scale = opt[i++];
00550         tcp_dbg_options("TCP Window scale: received %d\n", t->recv_wnd_scale);
00551         break;
00552       case PICO_TCP_OPTION_SACK_OK:
00553         if (len != PICO_TCPOPTLEN_SACK_OK) {
00554           tcp_dbg_options("TCP option sack: bad len received.\n");
00555           i += len - 2;
00556           break;
00557         }
00558         t->sack_ok = 1;
00559         break;
00560       case PICO_TCP_OPTION_MSS: {
00561         uint16_t mss;
00562         if (len != PICO_TCPOPTLEN_MSS) {
00563           tcp_dbg_options("TCP option mss: bad len received.\n");
00564           i += len - 2;
00565           break;
00566         }
00567         t->mss_ok = 1;
00568         mss = short_from(opt + i);
00569         i += sizeof(uint16_t);
00570         if (t->mss > short_be(mss))
00571           t->mss = short_be(mss);
00572         break;
00573       }
00574       case PICO_TCP_OPTION_TIMESTAMP: {
00575         uint32_t tsval, tsecr;
00576         if (len != PICO_TCPOPTLEN_TIMESTAMP) {
00577           tcp_dbg_options("TCP option timestamp: bad len received.\n");
00578           i += len - 2;
00579           break;
00580         }
00581         t->ts_ok = 1;
00582         tsval = long_from(opt + i);
00583         i += sizeof(uint32_t);
00584         tsecr = long_from(opt + i);
00585         f->timestamp = long_be(tsecr);
00586         i += sizeof(uint32_t);
00587         t->ts_nxt = long_be(tsval);
00588         break;
00589       }
00590       case PICO_TCP_OPTION_SACK:
00591       {
00592         tcp_rcv_sack(t, opt + i, len - 2);
00593         i += len - 2;
00594         break;
00595       }
00596       default:
00597         tcp_dbg_options("TCP: received unsupported option %u\n", type);
00598         i += len - 2;
00599     }
00600   }
00601 }
00602 
00603 static int tcp_send(struct pico_socket_tcp *ts, struct pico_frame *f)
00604 {
00605   struct pico_tcp_hdr *hdr= (struct pico_tcp_hdr *) f->transport_hdr;
00606   struct pico_frame *cpy;
00607   hdr->trans.sport = ts->sock.local_port;
00608   hdr->trans.dport = ts->sock.remote_port;
00609   if (!hdr->seq)
00610     hdr->seq = long_be(ts->snd_nxt);
00611 
00612   if (ts->rcv_nxt != 0) {
00613     if ( (ts->rcv_ackd == 0) || (seq_compare(ts->rcv_ackd, ts->rcv_nxt) != 0) || (hdr->flags & PICO_TCP_ACK)) {
00614       hdr->flags |= PICO_TCP_ACK;
00615       hdr->ack = long_be(ts->rcv_nxt);
00616       ts->rcv_ackd = ts->rcv_nxt;
00617     }
00618   }
00619 
00620   if (hdr->flags & PICO_TCP_SYN) {
00621     ts->snd_nxt++;
00622   }
00623   if (f->payload_len > 0) {
00624     hdr->flags |= PICO_TCP_PSH | PICO_TCP_ACK;
00625     hdr->ack = long_be(ts->rcv_nxt);
00626     ts->rcv_ackd = ts->rcv_nxt;
00627     ts->keepalive_timer_running = 2;    /* XXX TODO check fix: added 1 to counter to postpone sending keepalive, ACK is in data segments */
00628   }
00629 
00630   f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
00631   hdr->rwnd = short_be(ts->wnd);
00632   hdr->crc = 0;
00633   hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
00634 
00635   /* TCP: ENQUEUE to PROTO ( Transmit ) */
00636   cpy = pico_frame_copy(f);
00637   if ((pico_enqueue(&tcp_out, cpy) > 0)) {
00638     if (f->payload_len > 0) {
00639       ts->in_flight++;
00640       ts->snd_nxt += f->payload_len;  /* update next pointer here to prevent sending same segment twice when called twice in same tick */
00641     }
00642     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",
00643       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 );
00644   } else {
00645     pico_frame_discard(cpy);
00646   }
00647   return 0;
00648 }
00649 
00650 //#define PICO_TCP_SUPPORT_SOCKET_STATS
00651 
00652 #ifdef PICO_TCP_SUPPORT_SOCKET_STATS
00653 static void sock_stats(unsigned long when, void *arg)
00654 {
00655   struct pico_socket_tcp *t = (struct pico_socket_tcp *)arg;
00656   tcp_dbg("STATISTIC> [%lu] socket state: %02x --> local port:%d remote port: %d queue size: %d snd_una: %08x snd_nxt: %08x timer: %d cwnd: %d\n",
00657     when, t->sock.state, short_be(t->sock.local_port), short_be(t->sock.remote_port), t->tcpq_out.size, SEQN(first_segment(&t->tcpq_out)), t->snd_nxt, t->timer_running, t->cwnd);
00658   pico_timer_add(2000, sock_stats, t);
00659 }
00660 #endif
00661 
00662 struct pico_socket *pico_tcp_open(void)
00663 {
00664   struct pico_socket_tcp *t = pico_zalloc(sizeof(struct pico_socket_tcp));
00665   if (!t)
00666     return NULL;
00667   t->mss = PICO_TCP_DEFAULT_MSS;
00668 
00669   t->tcpq_in.pool.root = t->tcpq_hold.pool.root = t->tcpq_out.pool.root = &LEAF;
00670   t->tcpq_hold.pool.compare = t->tcpq_in.pool.compare = t->tcpq_out.pool.compare = segment_compare;
00671 
00672   t->tcpq_in.max_size = PICO_DEFAULT_SOCKETQ;
00673   t->tcpq_out.max_size = PICO_DEFAULT_SOCKETQ;
00674   t->tcpq_hold.max_size = 2*PICO_TCP_DEFAULT_MSS;
00675 
00676   /* disable Nagle by default */
00677   t->sock.opt_flags |= (1 << PICO_SOCKET_OPT_TCPNODELAY);
00678 
00679 #ifdef PICO_TCP_SUPPORT_SOCKET_STATS
00680   pico_timer_add(2000, sock_stats, t);
00681 #endif
00682   tcp_set_space(t);
00683 
00684   return &t->sock;
00685 }
00686 
00687 int pico_tcp_read(struct pico_socket *s, void *buf, int len)
00688 {
00689   struct pico_socket_tcp *t = TCP_SOCK(s);
00690   struct pico_frame *f;
00691   uint32_t in_frame_off, in_frame_len;
00692   int tot_rd_len = 0;
00693 
00694   while (tot_rd_len < len) {
00695     /* To be sure we don't have garbage at the beginning */
00696     release_until(&t->tcpq_in, t->rcv_processed);
00697     f = first_segment(&t->tcpq_in);
00698     if (!f) {
00699       tcp_set_space(t);
00700       goto out;
00701     }
00702 
00703     /* Hole at the beginning of data, awaiting retransmissions. */
00704     if (seq_compare(t->rcv_processed, SEQN(f)) < 0) {
00705       tcp_dbg("TCP> read hole beginning of data, %08x - %08x. rcv_nxt is %08x\n",t->rcv_processed, SEQN(f), t->rcv_nxt);
00706       goto out;
00707     }
00708 
00709     if(seq_compare(t->rcv_processed, SEQN(f)) > 0) {
00710       in_frame_off = t->rcv_processed - SEQN(f);
00711       in_frame_len = f->payload_len - in_frame_off;
00712     } else {
00713       in_frame_off = 0;
00714       in_frame_len = f->payload_len;
00715     }
00716     if ((in_frame_len + tot_rd_len) > len) {
00717       in_frame_len = len - tot_rd_len;
00718     }
00719 
00720     if (in_frame_len > f->payload_len - in_frame_off)
00721       in_frame_len = f->payload_len - in_frame_off;
00722 
00723     memcpy((uint8_t *)buf + tot_rd_len, f->payload + in_frame_off, in_frame_len);
00724     tot_rd_len += in_frame_len;
00725     t->rcv_processed += in_frame_len;
00726 
00727     if ((in_frame_len == 0) || (in_frame_len == f->payload_len)) {
00728       pico_discard_segment(&t->tcpq_in, f);
00729     }
00730   }
00731 
00732 out:
00733   tcp_set_space(t);
00734   if (t->tcpq_in.size == 0) {
00735     s->ev_pending &= (~PICO_SOCK_EV_RD);
00736   }
00737   return tot_rd_len;
00738 }
00739 
00740 int pico_tcp_initconn(struct pico_socket *s);
00741 static void initconn_retry(unsigned long when, void *arg)
00742 {
00743   struct pico_socket_tcp *t = (struct pico_socket_tcp *)arg;
00744   if (TCPSTATE(&t->sock) == PICO_SOCKET_STATE_TCP_SYN_SENT) {
00745     if (t->backoff > PICO_TCP_MAX_CONNECT_RETRIES) {
00746       tcp_dbg("TCP> Connection timeout. \n");
00747       if (t->sock.wakeup)
00748         t->sock.wakeup(PICO_SOCK_EV_ERR, &t->sock);
00749       return;
00750     }
00751     tcp_dbg("TCP> SYN retry %d...\n", t->backoff);
00752     t->backoff++;
00753     pico_tcp_initconn(&t->sock);
00754   } else {
00755     tcp_dbg("TCP> Connection is already established: no retry needed. good.\n");
00756   }
00757 }
00758 
00759 int pico_tcp_initconn(struct pico_socket *s)
00760 {
00761   struct pico_socket_tcp *ts = TCP_SOCK(s);
00762   struct pico_frame *syn;
00763   struct pico_tcp_hdr *hdr;
00764   int opt_len = tcp_options_size(ts, PICO_TCP_SYN);
00765 
00766   syn = s->net->alloc(s->net, PICO_SIZE_TCPHDR + opt_len);
00767   if (!syn)
00768     return -1;
00769   hdr = (struct pico_tcp_hdr *) syn->transport_hdr;
00770 
00771   if (!ts->snd_nxt)
00772     ts->snd_nxt = long_be(pico_paws());
00773   ts->snd_last = ts->snd_nxt;
00774   ts->cwnd = PICO_TCP_IW;
00775   ts->ssthresh = 40;
00776   syn->sock = s;
00777   hdr->seq = long_be(ts->snd_nxt);
00778   hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | ts->jumbo;
00779   hdr->flags = PICO_TCP_SYN;
00780   tcp_set_space(ts);
00781   hdr->rwnd = short_be(ts->wnd);
00782   tcp_add_options(ts,syn, PICO_TCP_SYN, opt_len);
00783   hdr->trans.sport = ts->sock.local_port;
00784   hdr->trans.dport = ts->sock.remote_port;
00785 
00786   hdr->crc = 0;
00787   hdr->crc = short_be(pico_tcp_checksum_ipv4(syn));
00788 
00789   /* TCP: ENQUEUE to PROTO ( SYN ) */
00790   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);
00791   pico_enqueue(&tcp_out, syn);
00792   pico_timer_add(PICO_TCP_SYN_TO << ts->backoff, initconn_retry, ts);
00793   return 0;
00794 }
00795 
00796 static int tcp_send_synack(struct pico_socket *s)
00797 {
00798   struct pico_socket_tcp *ts = TCP_SOCK(s);
00799   struct pico_frame *synack;
00800   struct pico_tcp_hdr *hdr;
00801   int opt_len = tcp_options_size(ts, PICO_TCP_SYN | PICO_TCP_ACK);
00802 
00803   synack = s->net->alloc(s->net, PICO_SIZE_TCPHDR + opt_len);
00804   if (!synack)
00805     return -1;
00806   hdr = (struct pico_tcp_hdr *) synack->transport_hdr;
00807 
00808   synack->sock = s;
00809   hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | ts->jumbo;
00810   hdr->flags = PICO_TCP_SYN | PICO_TCP_ACK;
00811   hdr->rwnd = short_be(ts->wnd);
00812   hdr->seq = long_be(ts->snd_nxt);
00813   ts->rcv_processed = long_be(hdr->seq);
00814   ts->snd_last = ts->snd_nxt;
00815   tcp_set_space(ts);
00816   tcp_add_options(ts,synack, hdr->flags, opt_len);
00817   synack->payload_len = 0;
00818   synack->timestamp = pico_tick;
00819   tcp_send(ts, synack);
00820   pico_frame_discard(synack);
00821   return 0;
00822 }
00823 
00824 static void tcp_send_empty(struct pico_socket_tcp *t, uint16_t flags)
00825 {
00826   struct pico_frame *f;
00827   struct pico_tcp_hdr *hdr;
00828   int opt_len = tcp_options_size(t, flags);
00829   f = t->sock.net->alloc(t->sock.net, PICO_SIZE_TCPHDR + opt_len);
00830   if (!f) {
00831     return;
00832   }
00833   f->sock = &t->sock;
00834   hdr = (struct pico_tcp_hdr *) f->transport_hdr;
00835   hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo;
00836   hdr->flags = flags;
00837   hdr->rwnd = short_be(t->wnd);
00838   tcp_set_space(t);
00839   tcp_add_options(t,f, flags, opt_len);
00840   hdr->trans.sport = t->sock.local_port;
00841   hdr->trans.dport = t->sock.remote_port;
00842   hdr->seq = long_be(t->snd_nxt);
00843   if ((flags & PICO_TCP_ACK) != 0)
00844     hdr->ack = long_be(t->rcv_nxt);
00845   t->rcv_ackd = t->rcv_nxt;
00846 
00847   f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
00848   hdr->rwnd = short_be(t->wnd);
00849   hdr->crc = 0;
00850   hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
00851 
00852   /* TCP: ENQUEUE to PROTO */
00853   pico_enqueue(&tcp_out, f);
00854 }
00855 
00856 static void tcp_send_ack(struct pico_socket_tcp *t)
00857 {
00858   return tcp_send_empty(t, PICO_TCP_ACK);
00859 }
00860 
00861 static int tcp_send_rst(struct pico_socket *s, struct pico_frame *fr)
00862 {
00863   struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
00864   struct pico_frame *f;
00865   struct pico_tcp_hdr *hdr, *hdr_rcv;
00866   int opt_len = tcp_options_size(t, PICO_TCP_RST);
00867   int close;
00868 
00869   tcp_dbg("TCP SEND_RST >>>>>>>>>>>>>>> START\n");
00870 
00871   f = t->sock.net->alloc(t->sock.net, PICO_SIZE_TCPHDR + opt_len);
00872 
00873   if (!f) {
00874     return -1;
00875   }
00876 
00877   hdr_rcv = (struct pico_tcp_hdr *) fr->transport_hdr;
00878 
00879   f->sock = &t->sock;
00880   hdr = (struct pico_tcp_hdr *) f->transport_hdr;
00881   hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo;
00882   hdr->flags = PICO_TCP_RST;
00883   hdr->rwnd = short_be(t->wnd);
00884   tcp_set_space(t);
00885   tcp_add_options(t,f, PICO_TCP_RST, opt_len);
00886   hdr->trans.sport = t->sock.local_port;
00887   hdr->trans.dport = t->sock.remote_port;
00888   hdr->seq = long_be(t->snd_nxt);
00889 
00890   /* check if state is synchronized */
00891   if (((s->state & PICO_SOCKET_STATE_TCP) > PICO_SOCKET_STATE_TCP_SYN_RECV)) {
00892     /* in synchronized state: send RST with seq = ack from previous segment */
00893     hdr->seq = hdr_rcv->ack;
00894     close = 0;
00895   } else {
00896     /* non-synchronized state */
00897     /* go to CLOSED here to prevent timer callback to go on after timeout */
00898     (t->sock).state &= 0x00FFU;
00899     (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED;
00900     close = 1;
00901   }
00902 
00903   hdr->ack = long_be(t->rcv_nxt);
00904   t->rcv_ackd = t->rcv_nxt;
00905   f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
00906   hdr->rwnd = short_be(t->wnd);
00907   hdr->crc = 0;
00908   hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
00909 
00910   /* TCP: ENQUEUE to PROTO */
00911   pico_enqueue(&tcp_out, f);
00912 
00913   /* goto CLOSED */
00914   if (close) {
00915     (t->sock).state &= 0xFF00U;
00916     (t->sock).state |= PICO_SOCKET_STATE_CLOSED;
00917 
00918     /* call EV_FIN wakeup before deleting */
00919     if ((t->sock).wakeup)
00920       (t->sock).wakeup(PICO_SOCK_EV_FIN, &(t->sock));
00921 
00922     /* delete socket */
00923       pico_socket_del(&t->sock);
00924 
00925     tcp_dbg("TCP SEND_RST >>>>>>>>>>>>>>> DONE, deleted socket\n");
00926   }
00927 
00928   return 0;
00929 }
00930 
00931 int pico_tcp_reply_rst(struct pico_frame *fr)
00932 {
00933   struct pico_tcp_hdr *hdr;
00934   struct pico_frame *f;
00935   int size = PICO_SIZE_TCPHDR;
00936 
00937   tcp_dbg("TCP>>>>>>>>>>>>>>>> sending RST ... <<<<<<<<<<<<<<<<<<\n");
00938 
00939   f = fr->sock->net->alloc(fr->sock->net, size);
00940 
00941   /* fill in IP data from original frame */
00942   // TODO if IPv4
00943   ((struct pico_ipv4_hdr *)(f->net_hdr))->dst.addr = ((struct pico_ipv4_hdr *)(fr->net_hdr))->src.addr;
00944   ((struct pico_ipv4_hdr *)(f->net_hdr))->src.addr = ((struct pico_ipv4_hdr *)(fr->net_hdr))->dst.addr;
00945 
00946   /* fill in TCP data from original frame */
00947   ((struct pico_tcp_hdr *)(f->transport_hdr))->trans.dport = ((struct pico_tcp_hdr *)(fr->transport_hdr))->trans.sport;
00948   ((struct pico_tcp_hdr *)(f->transport_hdr))->trans.sport = ((struct pico_tcp_hdr *)(fr->transport_hdr))->trans.dport;
00949   hdr = (struct pico_tcp_hdr *) f->transport_hdr;
00950   hdr->len   = size << 2;
00951   hdr->flags = PICO_TCP_RST | PICO_TCP_ACK;
00952   hdr->rwnd  = 0;
00953   if (((struct pico_tcp_hdr *)(fr->transport_hdr))->flags & PICO_TCP_ACK) {
00954     hdr->seq = ((struct pico_tcp_hdr *)(fr->transport_hdr))->ack;
00955   } else {
00956     hdr->seq = 0U;
00957   }
00958 
00959   hdr->ack = ((struct pico_tcp_hdr *)(fr->transport_hdr))->seq + short_be(fr->payload_len);
00960 
00961   /* enqueue for transmission */
00962   pico_ipv4_frame_push(f,&(((struct pico_ipv4_hdr *)(f->net_hdr))->dst),PICO_PROTO_TCP);
00963 
00964   return 0;
00965 }
00966 
00967 static int tcp_nosync_rst(struct pico_socket *s, struct pico_frame *fr)
00968 {
00969   struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
00970   struct pico_frame *f;
00971   struct pico_tcp_hdr *hdr, *hdr_rcv;
00972   int opt_len = tcp_options_size(t, PICO_TCP_RST | PICO_TCP_ACK);
00973 
00974   tcp_dbg("TCP SEND RST (NON-SYNC) >>>>>>>>>>>>>>>>>> state %x\n",(s->state & PICO_SOCKET_STATE_TCP));
00975   if (((s->state & PICO_SOCKET_STATE_TCP) ==  PICO_SOCKET_STATE_TCP_LISTEN)) {
00976     /* XXX TODO NOTE: to prevent the parent socket from trying to send, because this socket has no knowledge of dst IP !!! */
00977     return pico_tcp_reply_rst(fr);
00978   }
00979 
00980   /***************************************************************************/
00981   /* sending RST */
00982   f = t->sock.net->alloc(t->sock.net, PICO_SIZE_TCPHDR + opt_len);
00983 
00984   if (!f) {
00985     return -1;
00986   }
00987 
00988   hdr_rcv = (struct pico_tcp_hdr *) fr->transport_hdr;
00989 
00990   f->sock = &t->sock;
00991   hdr = (struct pico_tcp_hdr *) f->transport_hdr;
00992   hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo;
00993   hdr->flags = PICO_TCP_RST | PICO_TCP_ACK;
00994   hdr->rwnd = short_be(t->wnd);
00995   tcp_set_space(t);
00996   tcp_add_options(t,f, PICO_TCP_RST | PICO_TCP_ACK, opt_len);
00997   hdr->trans.sport = t->sock.local_port;
00998   hdr->trans.dport = t->sock.remote_port;
00999 
01000   /* non-synchronized state */
01001   if (hdr_rcv->flags & PICO_TCP_ACK) {
01002     hdr->seq = hdr_rcv->ack;
01003   } else {
01004     hdr->seq = 0U;
01005   }
01006 
01007   hdr->ack = hdr_rcv->seq + short_be(fr->payload_len);
01008 
01009   t->rcv_ackd = t->rcv_nxt;
01010   f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
01011   hdr->rwnd = short_be(t->wnd);
01012   hdr->crc = 0;
01013   hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
01014 
01015   /* TCP: ENQUEUE to PROTO */
01016   pico_enqueue(&tcp_out, f);
01017 
01018   /***************************************************************************/
01019 
01020   tcp_dbg("TCP SEND_RST (NON_SYNC) >>>>>>>>>>>>>>> DONE, ...\n");
01021 
01022   return 0;
01023 }
01024 
01025 static void tcp_send_fin(struct pico_socket_tcp *t)
01026 {
01027   struct pico_frame *f;
01028   struct pico_tcp_hdr *hdr;
01029   int opt_len = tcp_options_size(t, PICO_TCP_FIN);
01030   f = t->sock.net->alloc(t->sock.net, PICO_SIZE_TCPHDR + opt_len);
01031   if (!f) {
01032     return;
01033   }
01034   f->sock = &t->sock;
01035   hdr = (struct pico_tcp_hdr *) f->transport_hdr;
01036   hdr->len = (PICO_SIZE_TCPHDR + opt_len) << 2 | t->jumbo;
01037   hdr->flags = PICO_TCP_FIN | PICO_TCP_ACK;
01038   hdr->ack = long_be(t->rcv_nxt);
01039   t->rcv_ackd = t->rcv_nxt;
01040   hdr->rwnd = short_be(t->wnd);
01041   tcp_set_space(t);
01042   tcp_add_options(t,f, PICO_TCP_FIN, opt_len);
01043   hdr->trans.sport = t->sock.local_port;
01044   hdr->trans.dport = t->sock.remote_port;
01045   hdr->seq = long_be(t->snd_nxt);   /* XXX TODO check correct ?? --> snd_last? otherwise maybe data after FIN */
01046 
01047   f->start = f->transport_hdr + PICO_SIZE_TCPHDR;
01048   hdr->rwnd = short_be(t->wnd);
01049   hdr->crc = 0;
01050   hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
01051   //tcp_dbg("SENDING FIN...\n");
01052   /* TCP: ENQUEUE to PROTO ( Pure ACK ) */
01053   pico_enqueue(&tcp_out, f);
01054   t->snd_nxt++;
01055 }
01056 
01057 static void tcp_sack_prepare(struct pico_socket_tcp *t)
01058 {
01059   struct pico_frame *pkt;
01060   uint32_t left=0, right=0;
01061   struct tcp_sack_block *sb;
01062   int n = 0;
01063   if (t->sacks) /* previous sacks are pending */
01064     return;
01065 
01066   pkt = first_segment(&t->tcpq_in);
01067   while(n < 3) {
01068     if (!pkt) {
01069       if(left) {
01070         sb = pico_zalloc(sizeof(struct tcp_sack_block));
01071         if (!sb)
01072           break;
01073         sb->left = long_be(left);
01074         sb->right = long_be(right);
01075         n++;
01076         sb->next = t->sacks;
01077         t->sacks = sb;
01078         left = 0;
01079         right = 0;
01080       }
01081       break;
01082     }
01083     if ((SEQN(pkt) < t->rcv_nxt)) {
01084       pkt = next_segment(&t->tcpq_in, pkt);
01085       continue;
01086     }
01087     if (!left) {
01088       left = SEQN(pkt);
01089       right = SEQN(pkt) + pkt->payload_len;
01090       pkt = next_segment(&t->tcpq_in, pkt);
01091       continue;
01092     }
01093     if(SEQN(pkt) == (right + 1)) {
01094       right += pkt->payload_len;
01095       pkt = next_segment(&t->tcpq_in, pkt);
01096       continue;
01097     } else {
01098       sb = pico_zalloc(sizeof(struct tcp_sack_block));
01099       if (!sb)
01100         break;
01101       sb->left = long_be(left);
01102       sb->right = long_be(right);
01103       n++;
01104       sb->next = t->sacks;
01105       t->sacks = sb;
01106       left = 0;
01107       right = 0;
01108       pkt = next_segment(&t->tcpq_in, pkt);
01109     }
01110   }
01111 }
01112 
01113 static int tcp_data_in(struct pico_socket *s, struct pico_frame *f)
01114 {
01115   struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
01116   struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) f->transport_hdr;
01117 
01118   if (((hdr->len & 0xf0) >> 2) <= f->transport_len) {
01119     tcp_parse_options(f);
01120     f->payload = f->transport_hdr + ((hdr->len & 0xf0) >>2);
01121     f->payload_len = f->transport_len - ((hdr->len & 0xf0) >>2);
01122     tcp_dbg("TCP> Received segment. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f));
01123 
01124     if (seq_compare(SEQN(f), t->rcv_nxt) <= 0) {
01125       struct pico_frame *nxt;
01126       if (seq_compare(SEQN(f), t->rcv_nxt) == 0) { /* Exactly what we expected */
01127         struct pico_frame *cpy = pico_frame_copy(f);
01128         /* Enqueue: try to put into RCV buffer */
01129         if(pico_enqueue_segment(&t->tcpq_in, cpy) <= 0) {
01130           pico_frame_discard(cpy);
01131         }
01132         t->rcv_nxt = SEQN(f) + f->payload_len;
01133         nxt = peek_segment(&t->tcpq_in, t->rcv_nxt);
01134         while(nxt) {
01135           tcp_dbg("scrolling rcv_nxt...%08x\n", t->rcv_nxt);
01136           t->rcv_nxt += f->payload_len;
01137           nxt = peek_segment(&t->tcpq_in, t->rcv_nxt);
01138         }
01139         t->sock.ev_pending |= PICO_SOCK_EV_RD;
01140         t->rcv_nxt = SEQN(f) + f->payload_len;
01141       } else {
01142         tcp_dbg("TCP> lo segment. Uninteresting retransmission. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f));
01143       }
01144     } else {
01145       tcp_dbg("TCP> hi segment. Possible packet loss. I'll dupack this. (exp: %x got: %x)\n", t->rcv_nxt, SEQN(f));
01146       if (t->sack_ok) {
01147         tcp_sack_prepare(t);
01148       }
01149     }
01150     /* In either case, ack til recv_nxt. */
01151     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)) {
01152       //tcp_dbg("SENDACK CALLED FROM OUTSIDE tcp_synack, state %x\n",t->sock.state);
01153       tcp_send_ack(t);
01154     } else {
01155       //tcp_dbg("SENDACK PREVENTED IN SYNSENT STATE\n");
01156     }
01157     return 0;
01158   } else {
01159     tcp_dbg("TCP: invalid data in pkt len, exp: %d, got %d\n", (hdr->len & 0xf0) >> 2, f->transport_len);
01160     return -1;
01161   }
01162 }
01163 
01164 static int tcp_ack_advance_una(struct pico_socket_tcp *t, struct pico_frame *f)
01165 {
01166   int ret =  release_all_until(&t->tcpq_out, ACKN(f));
01167   if (ret > 0)
01168     t->sock.ev_pending |= PICO_SOCK_EV_WR;
01169   return ret;
01170 }
01171 
01172 static uint16_t time_diff(unsigned long a, unsigned long b)
01173 {
01174   if (a >= b)
01175     return (a - b);
01176   else
01177     return (b - a);
01178 }
01179 
01180 static void tcp_rtt(struct pico_socket_tcp *t, uint32_t rtt)
01181 {
01182 
01183   uint32_t avg = t->avg_rtt;
01184   uint32_t rvar = t->rttvar;
01185   if (!avg) {
01186     /* This follows RFC2988
01187      * (2.2) When the first RTT measurement R is made, the host MUST set
01188      *
01189      * SRTT <- R
01190      * RTTVAR <- R/2
01191      * RTO <- SRTT + max (G, K*RTTVAR)
01192      */
01193     t->avg_rtt = rtt;
01194     t->rttvar = rtt >> 1;
01195     t->rto = t->avg_rtt + (t->rttvar << 4);
01196   } else {
01197     int var = (t->avg_rtt - rtt);
01198     if (var < 0)
01199       var = 0-var;
01200     /* RFC2988, section (2.3). Alpha and beta are the ones suggested. */
01201 
01202     /* First, evaluate a new value for the rttvar */
01203     t->rttvar <<= 2;
01204     t->rttvar -= rvar;
01205     t->rttvar += var;
01206     t->rttvar >>= 2;
01207 
01208     /* Then, calculate the new avg_rtt */
01209     t->avg_rtt <<= 3;
01210     t->avg_rtt -= avg;
01211     t->avg_rtt += rtt;
01212     t->avg_rtt >>= 3;
01213 
01214     /* Finally, assign a new value for the RTO, as specified in the RFC, with K=4 */
01215     t->rto = t->avg_rtt + (t->rttvar << 2);
01216   }
01217   tcp_dbg(" -----=============== RTT CUR: %u AVG: %u RTTVAR: %u RTO: %u ======================----\n", rtt, t->avg_rtt, t->rttvar, t->rto);
01218 }
01219 
01220 static void tcp_congestion_control(struct pico_socket_tcp *t)
01221 {
01222   if (t->x_mode > PICO_TCP_LOOKAHEAD)
01223     return;
01224   if (t->cwnd > t->tcpq_out.frames) {
01225     tcp_dbg("Limited by app: %d\n", t->cwnd);
01226     if (t->sock.wakeup)
01227       t->sock.wakeup(PICO_SOCK_EV_WR, &t->sock);
01228     return;
01229   }
01230   tcp_dbg("Doing congestion control\n");
01231   if (t->cwnd < t->ssthresh) {
01232     t->cwnd++;
01233   } else {
01234     t->cwnd_counter++;
01235     if (t->cwnd_counter >= t->cwnd) {
01236       t->cwnd++;
01237       t->cwnd_counter = 0;
01238     }
01239   }
01240   tcp_dbg("TCP_CWND, %lu, %u, %u, %u\n", pico_tick, t->cwnd, t->ssthresh, t->in_flight);
01241 }
01242 
01243 static void add_retransmission_timer(struct pico_socket_tcp *t, unsigned long next_ts);
01244 static void tcp_retrans_timeout(unsigned long val, void *sock)
01245 {
01246   struct pico_socket_tcp *t = (struct pico_socket_tcp *) sock;
01247   struct pico_frame *f = NULL;
01248   unsigned long limit = val - t->rto;
01249   struct pico_tcp_hdr *hdr;
01250   if( t->sock.net && ((t->sock.state & 0xFF00) == PICO_SOCKET_STATE_TCP_ESTABLISHED
01251         || (t->sock.state & 0xFF00) == PICO_SOCKET_STATE_TCP_CLOSE_WAIT) )
01252   {
01253         tcp_dbg("\n\nTIMEOUT! backoff = %d\n", t->backoff);
01254         /* was timer cancelled? */
01255         if (t->timer_running == 0) {
01256             add_retransmission_timer(t, 0);
01257             return;
01258         }
01259         t->timer_running--;
01260 
01261         f = first_segment(&t->tcpq_out);
01262         while (f) {
01263             if ((t->x_mode == PICO_TCP_WINDOW_FULL) ||
01264                     ((f->timestamp != 0) && (f->timestamp <= limit))) {
01265                 struct pico_frame *cpy;
01266                 hdr = (struct pico_tcp_hdr *)f->transport_hdr;
01267                 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);
01268                 if ((t->x_mode != PICO_TCP_WINDOW_FULL) ) {
01269                     t->x_mode = PICO_TCP_BLACKOUT;
01270                     tcp_dbg("Mode: Blackout.\n");
01271                     t->cwnd = PICO_TCP_IW;
01272                     t->in_flight = 0;
01273                 }
01274                 f->timestamp = pico_tick;
01275                 tcp_add_options(t, f, 0, f->transport_len - f->payload_len - PICO_SIZE_TCPHDR);
01276                 hdr->rwnd = short_be(t->wnd);
01277                 hdr->flags |= PICO_TCP_PSH;
01278                 hdr->ack = long_be(t->rcv_nxt);
01279                 hdr->crc = 0;
01280                 hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
01281                 /* TCP: ENQUEUE to PROTO ( retransmit )*/
01282                 cpy = pico_frame_copy(f);
01283                 if (pico_enqueue(&tcp_out, cpy) > 0) {
01284                     t->backoff++;
01285                     add_retransmission_timer(t, (t->rto << t->backoff) + pico_tick);
01286                     tcp_dbg("TCP_CWND, %lu, %u, %u, %u\n", pico_tick, t->cwnd, t->ssthresh, t->in_flight);
01287                     return;
01288                 } else {
01289                     add_retransmission_timer(t, (t->rto << t->backoff) + pico_tick);
01290                     pico_frame_discard(cpy);
01291                 }
01292             }
01293             f = next_segment(&t->tcpq_out, f);
01294         }
01295         t->backoff = 0;
01296         add_retransmission_timer(t, 0);
01297         if (t->tcpq_out.size < t->tcpq_out.max_size)
01298              t->sock.ev_pending |= PICO_SOCK_EV_WR;
01299         return;
01300     }
01301 }
01302 
01303 static void add_retransmission_timer(struct pico_socket_tcp *t, unsigned long next_ts)
01304 {
01305   struct pico_tree_node * index;
01306 
01307   if (t->timer_running > 0)
01308     return;
01309 
01310   if (next_ts == 0) {
01311     struct pico_frame *f;
01312 
01313     pico_tree_foreach(index,&t->tcpq_out.pool){
01314       f = index->keyValue;
01315       if (((next_ts == 0) || (f->timestamp < next_ts)) && (f->timestamp > 0)) {
01316         next_ts = f->timestamp;
01317       }
01318     }
01319   }
01320   if (next_ts > 0) {
01321     if ((next_ts + t->rto) > pico_tick) {
01322       pico_timer_add(next_ts + t->rto - pico_tick, tcp_retrans_timeout, t);
01323     } else {
01324       pico_timer_add(1, tcp_retrans_timeout, t);
01325     }
01326     t->timer_running++;
01327   }
01328 }
01329 
01330 static int tcp_retrans(struct pico_socket_tcp *t, struct pico_frame *f)
01331 {
01332   struct pico_frame *cpy;
01333   struct pico_tcp_hdr *hdr;
01334   if (f) {
01335     hdr = (struct pico_tcp_hdr *)f->transport_hdr;
01336     tcp_dbg("TCP> RETRANS (by dupack) frame %08x, len= %d\n", SEQN(f), f->payload_len);
01337     f->timestamp = pico_tick;
01338     tcp_add_options(t, f, 0, f->transport_len - f->payload_len - PICO_SIZE_TCPHDR);
01339     hdr->rwnd = short_be(t->wnd);
01340     hdr->flags |= PICO_TCP_PSH;
01341     hdr->ack = long_be(t->rcv_nxt);
01342     hdr->crc = 0;
01343     hdr->crc = short_be(pico_tcp_checksum_ipv4(f));
01344     /* TCP: ENQUEUE to PROTO ( retransmit )*/
01345     cpy = pico_frame_copy(f);
01346     if (pico_enqueue(&tcp_out, cpy) > 0) {
01347       t->in_flight++;
01348       t->snd_last_out = SEQN(cpy);
01349       add_retransmission_timer(t, pico_tick + t->rto);
01350     } else {
01351       pico_frame_discard(cpy);
01352     }
01353     return(f->payload_len);
01354   }
01355   return 0;
01356 }
01357 
01358 #ifdef TCP_ACK_DBG
01359 static void tcp_ack_dbg(struct pico_socket *s, struct pico_frame *f)
01360 {
01361   uint32_t una, nxt, ack, cur;
01362   struct pico_frame *una_f = NULL, *cur_f;
01363   struct pico_tree_node *idx;
01364   struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
01365   char info[64];
01366   char tmp[64];
01367   ack = ACKN(f);
01368   nxt = t->snd_nxt;
01369   tcp_dbg("===================================\n");
01370   tcp_dbg("Queue out (%d/%d). ACKED=%08x\n", t->tcpq_out.size, t->tcpq_out.max_size, ack);
01371 
01372   pico_tree_foreach(idx, &t->tcpq_out.pool) {
01373     info[0] = 0;
01374     cur_f = idx->keyValue;
01375     cur = SEQN(cur_f);
01376     if (!una_f) {
01377       una_f = cur_f;
01378       una = SEQN(una_f);
01379     }
01380 
01381     if (cur == nxt) {
01382       strncpy(tmp, info, strlen(info));
01383       snprintf(info,64, "%s SND_NXT", tmp);
01384     }
01385     if (cur == ack) {
01386       strncpy(tmp, info, strlen(info));
01387       snprintf(info,64, "%s ACK", tmp);
01388     }
01389     if (cur == una) {
01390       strncpy(tmp, info, strlen(info));
01391       snprintf(info,64, "%s SND_UNA", tmp);
01392     }
01393     if (cur == t->snd_last) {
01394       strncpy(tmp, info, strlen(info));
01395       snprintf(info,64, "%s SND_LAST", tmp);
01396     }
01397     tcp_dbg("%08x %d%s\n", cur, cur_f->payload_len, info);
01398 
01399   }
01400   tcp_dbg("SND_NXT is %08x, snd_LAST is %08x", nxt, t->snd_last);
01401   tcp_dbg("===================================\n");
01402   tcp_dbg("\n\n");
01403 }
01404 #endif
01405 
01406 static int tcp_ack(struct pico_socket *s, struct pico_frame *f)
01407 {
01408   struct pico_frame *f_new; /* use with Nagle to push to out queue */
01409   struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
01410   struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) f->transport_hdr;
01411   uint32_t rtt = 0;
01412   int acked = 0;
01413   struct pico_frame *una = NULL;
01414   if ((hdr->flags & PICO_TCP_ACK) == 0)
01415     return -1;
01416 
01417 #ifdef TCP_ACK_DBG
01418   tcp_ack_dbg(s,f);
01419 #endif
01420 
01421   tcp_parse_options(f);
01422   t->recv_wnd = short_be(hdr->rwnd);
01423 
01424   acked = tcp_ack_advance_una(t, f);
01425   una = first_segment(&t->tcpq_out);
01426 
01427   if ((t->x_mode == PICO_TCP_BLACKOUT) || 
01428     ((t->x_mode == PICO_TCP_WINDOW_FULL) && ((t->recv_wnd << t->recv_wnd_scale) > t->mss))) {
01429     tcp_dbg("Re-entering look-ahead...\n\n\n");
01430     t->x_mode = PICO_TCP_LOOKAHEAD;
01431     t->backoff = 0;
01432   }
01433 
01434   /* One should be acked. */
01435 //  if ((acked == 0) && (t->in_flight > 0))
01436   if ((acked == 0) && (f->payload_len  == 0) && (t->in_flight > 0))
01437     t->in_flight--;
01438   if (!una || acked > 0) {
01439     t->x_mode = PICO_TCP_LOOKAHEAD;
01440     tcp_dbg("Mode: Look-ahead. In flight: %d/%d buf: %d\n", t->in_flight, t->cwnd, t->tcpq_out.frames);
01441     t->backoff = 0;
01442 
01443     /* Do rtt/rttvar/rto calculations */
01444     /* First, try with timestamps, using the value from options */
01445     if(f && (f->timestamp != 0)) {
01446       rtt = time_diff(pico_tick, f->timestamp);
01447       if (rtt)
01448         tcp_rtt(t, rtt);
01449     } else if(una && (una->timestamp != 0)) {
01450       /* If no timestamps are there, use conservatve estimation on the una */
01451         rtt = time_diff(pico_tick, una->timestamp);
01452         if (rtt)
01453           tcp_rtt(t, rtt);
01454     }
01455 
01456     tcp_dbg("TCP ACK> FRESH ACK %08x (acked %d) Queue size: %u/%u frames: %u cwnd: %u in_flight: %u snd_una: %u\n", ACKN(f), acked, t->tcpq_out.size, t->tcpq_out.max_size, t->tcpq_out.frames, t->cwnd, t->in_flight, SEQN(una));
01457     if (acked > t->in_flight) {
01458       tcp_dbg("WARNING: in flight < 0\n");
01459       t->in_flight = 0;
01460     } else
01461       t->in_flight -= (acked);
01462 
01463   } else if ((t->snd_old_ack == ACKN(f)) && /* We've just seen this ack, and... */
01464       ((0 == (hdr->flags & (PICO_TCP_PSH | PICO_TCP_SYN))) &&
01465           (f->payload_len == 0)) && /* This is a pure ack, and... */
01466       (ACKN(f) != t->snd_nxt)) /* There is something in flight awaiting to be acked... */
01467   {
01468     /* Process incoming duplicate ack. */
01469     if (t->x_mode < PICO_TCP_RECOVER) {
01470       t->x_mode++;
01471       tcp_dbg("Mode: DUPACK %d, due to PURE ACK %0x, len = %d\n", t->x_mode, SEQN(f), f->payload_len);
01472       tcp_dbg("ACK: %x - QUEUE: %x\n",ACKN(f), SEQN(first_segment(&t->tcpq_out)));
01473       if (t->x_mode == PICO_TCP_RECOVER) { /* Switching mode */
01474         t->snd_retry = SEQN(first_segment(&t->tcpq_out));
01475         if (t->ssthresh > t->cwnd)
01476           t->ssthresh >>=2;
01477         else
01478           t->ssthresh = (t->cwnd >> 1);
01479         if (t->ssthresh < 2)
01480           t->ssthresh = 2;
01481       }
01482     } else if (t->x_mode == PICO_TCP_RECOVER) {
01483      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));
01484       if (t->in_flight <= t->cwnd) {
01485         struct pico_frame *nxt = peek_segment(&t->tcpq_out, t->snd_retry);
01486         if (!nxt)
01487           nxt = first_segment(&t->tcpq_out);
01488 
01489         while (nxt && (nxt->flags & PICO_FRAME_FLAG_SACKED) && (nxt != first_segment(&t->tcpq_out))) {
01490           tcp_dbg("Skipping %08x because it is sacked.\n", SEQN(nxt));
01491           nxt = next_segment(&t->tcpq_out, nxt);
01492         }
01493 
01494         if (nxt && (seq_compare(SEQN(nxt), t->snd_nxt)) > 0)
01495           nxt = NULL;
01496         if (nxt && (seq_compare(SEQN(nxt), SEQN(first_segment(&t->tcpq_out))) > (t->recv_wnd << t->recv_wnd_scale)))
01497           nxt = NULL;
01498 
01499         if(!nxt)
01500           nxt = first_segment(&t->tcpq_out);
01501         if (nxt) {
01502           tcp_retrans(t, peek_segment(&t->tcpq_out, t->snd_retry));
01503           t->snd_retry = SEQN(nxt);
01504         }
01505       }
01506 
01507       if (++t->cwnd_counter > 1) {
01508         t->cwnd--;
01509         if (t->cwnd < 2)
01510           t->cwnd = 2;
01511         t->cwnd_counter = 0;
01512       }
01513     } else {
01514       tcp_dbg("DUPACK in mode %d \n", t->x_mode);
01515 
01516     }
01517   } /* End case duplicate ack detection */
01518 
01519   /* Do congestion control */
01520   tcp_congestion_control(t);
01521   if ((acked > 0) && t->sock.wakeup) {
01522     if (t->tcpq_out.size < t->tcpq_out.max_size)
01523       t->sock.wakeup(PICO_SOCK_EV_WR, &(t->sock));
01524       //t->sock.ev_pending |= PICO_SOCK_EV_WR;
01525   }
01526 
01527   /* if Nagle enabled, check if no unack'ed data and fill out queue (till window) */
01528   if (IS_NAGLE_ENABLED((&(t->sock)))) {
01529     while (!IS_TCP_HOLDQ_EMPTY(t) && ((t->tcpq_out.max_size - t->tcpq_out.size) >= PICO_TCP_DEFAULT_MSS)) {
01530       tcp_dbg_nagle("TCP_ACK - NAGLE add new segment\n");
01531       f_new = pico_hold_segment_make(t);
01532       if (f_new == NULL)
01533         break;            /* XXX corrupt !!! (or no memory) */
01534       if (pico_enqueue_segment(&t->tcpq_out,f_new) <= 0)
01535         // handle error
01536         tcp_dbg_nagle("TCP_ACK - NAGLE FAILED to enqueue in out\n");
01537     }
01538   }
01539 
01540   /* If some space was created, put a few segments out. */
01541   tcp_dbg("TCP_CWND, %lu, %u, %u, %u\n", pico_tick, t->cwnd, t->ssthresh, t->in_flight);
01542   if (t->x_mode ==  PICO_TCP_LOOKAHEAD) {
01543     if ((t->cwnd >= t->in_flight) && (t->snd_nxt > t->snd_last_out)) {
01544       pico_tcp_output(&t->sock, t->cwnd - t->in_flight);
01545     }
01546   }
01547 
01548   t->snd_old_ack = ACKN(f);
01549   return 0;
01550 }
01551 
01552 static int tcp_finwaitack(struct pico_socket *s, struct pico_frame *f)
01553 {
01554   tcp_dbg("RECEIVED ACK IN FIN_WAIT1\nTCP> IN STATE FIN_WAIT2\n");
01555 
01556   /* acking part */
01557   tcp_ack(s,f);
01558   /* update TCP state */
01559   s->state &= 0x00FFU;
01560   s->state |= PICO_SOCKET_STATE_TCP_FIN_WAIT2;
01561 
01562   return 0;
01563 }
01564 
01565 static void tcp_deltcb(unsigned long when, void *arg)
01566 {
01567   struct pico_socket_tcp *t = (struct pico_socket_tcp *)arg;
01568   if (TCPSTATE(&t->sock) == PICO_SOCKET_STATE_TCP_TIME_WAIT) {
01569     tcp_dbg("TCP> state: time_wait, final timer expired, going to closed state\n");
01570     /* update state */
01571     (t->sock).state &= 0x00FFU;
01572     (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED;
01573     (t->sock).state &= 0xFF00U;
01574     (t->sock).state |= PICO_SOCKET_STATE_CLOSED;
01575     /* call EV_FIN wakeup before deleting */
01576     if (t->sock.wakeup) {
01577       (t->sock).wakeup(PICO_SOCK_EV_FIN, &(t->sock));
01578     }
01579     /* delete socket */
01580     pico_socket_del(&t->sock);
01581   } else {
01582     tcp_dbg("TCP> trying to go to closed, wrong state\n");
01583   }
01584 }
01585 
01586 static int tcp_finwaitfin(struct pico_socket *s, struct pico_frame *f)
01587 {
01588   struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
01589   struct pico_tcp_hdr *hdr  = (struct pico_tcp_hdr *) (f->transport_hdr);
01590   tcp_dbg("TCP> received fin in FIN_WAIT2\n");
01591   /* received FIN, increase ACK nr */
01592   t->rcv_nxt = long_be(hdr->seq) + 1;
01593   s->state &= 0x00FFU;
01594   s->state |= PICO_SOCKET_STATE_TCP_TIME_WAIT;
01595   /* set SHUT_REMOTE */
01596   s->state |= PICO_SOCKET_STATE_SHUT_REMOTE;
01597   if (s->wakeup)
01598     s->wakeup(PICO_SOCK_EV_CLOSE, s);
01599   if (f->payload_len > 0) /* needed?? */
01600     tcp_data_in(s,f);
01601   /* send ACK */
01602   tcp_send_ack(t);
01603   /* set timer */
01604   pico_timer_add(200, tcp_deltcb, t);
01605   return 0;
01606 }
01607 
01608 static int tcp_closewaitack(struct pico_socket *s, struct pico_frame *f)
01609 {
01610   struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
01611   tcp_dbg("TCP> received ack in CLOSING\n");
01612   /* acking part */
01613   tcp_ack(s,f);
01614   /* update TCP state */
01615   s->state &= 0x00FFU;
01616   s->state |= PICO_SOCKET_STATE_TCP_TIME_WAIT;
01617   /* set timer */
01618   pico_timer_add(200, tcp_deltcb, t);
01619   return 0;
01620 }
01621 
01622 static int tcp_lastackwait(struct pico_socket *s, struct pico_frame *f)
01623 {
01624   tcp_dbg("TCP> state: last_ack, received ack, to closed\n");
01625   s->state &= 0x00FFU;
01626   s->state |= PICO_SOCKET_STATE_TCP_CLOSED;
01627   s->state &= 0xFF00U;
01628   s->state |= PICO_SOCKET_STATE_CLOSED;
01629   /* call socket wakeup with EV_FIN */
01630   if (s->wakeup)
01631     s->wakeup(PICO_SOCK_EV_FIN, s);
01632   /* delete socket */
01633   pico_socket_del(s);
01634   return 0;
01635 }
01636 
01637 static int tcp_syn(struct pico_socket *s, struct pico_frame *f)
01638 {
01639   /* TODO: Check against backlog length */
01640   struct pico_socket_tcp *new = (struct pico_socket_tcp *)pico_socket_clone(s);
01641   struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *)f->transport_hdr;
01642   if (!new)
01643     return -1;
01644 
01645 #ifdef PICO_TCP_SUPPORT_SOCKET_STATS
01646   pico_timer_add(2000, sock_stats, s);
01647 #endif
01648 
01649   new->sock.remote_port = ((struct pico_trans *)f->transport_hdr)->sport;
01650 #ifdef PICO_SUPPORT_IPV4
01651   if (IS_IPV4(f)) {
01652     new->sock.remote_addr.ip4.addr = ((struct pico_ipv4_hdr *)(f->net_hdr))->src.addr;
01653     new->sock.local_addr.ip4.addr = ((struct pico_ipv4_hdr *)(f->net_hdr))->dst.addr;
01654   }
01655 #endif
01656 #ifdef PICO_SUPPORT_IPV6
01657   if (IS_IPV6(f)) {
01658     new->sock.remote_addr.ip6 = ((struct pico_ipv6_hdr *)(f->net_hdr))->src;
01659     new->sock.local_addr.ip6 = ((struct pico_ipv6_hdr *)(f->net_hdr))->dst;
01660   }
01661 #endif
01662 
01663   /* Set socket limits */
01664   new->tcpq_in.max_size = PICO_DEFAULT_SOCKETQ;
01665   new->tcpq_out.max_size = PICO_DEFAULT_SOCKETQ;
01666   new->tcpq_hold.max_size = 2*PICO_TCP_DEFAULT_MSS;
01667 
01668   f->sock = &new->sock;
01669   tcp_parse_options(f);
01670   new->mss = PICO_TCP_DEFAULT_MSS;
01671   new->rcv_nxt = long_be(hdr->seq) + 1;
01672   new->snd_nxt = long_be(pico_paws());
01673   new->snd_last = new->snd_nxt;
01674   new->cwnd = PICO_TCP_IW;
01675   new->ssthresh = 40;
01676   new->recv_wnd = short_be(hdr->rwnd);
01677   new->jumbo = hdr->len & 0x07;
01678   new->sock.parent = s;
01679   new->sock.wakeup = s->wakeup;
01680   /* Initialize timestamp values */
01681   new->sock.state = PICO_SOCKET_STATE_BOUND | PICO_SOCKET_STATE_CONNECTED | PICO_SOCKET_STATE_TCP_SYN_RECV;
01682   pico_socket_add(&new->sock);
01683   tcp_send_synack(&new->sock);
01684   tcp_dbg("SYNACK sent, socket added. snd_nxt is %08x\n", new->snd_nxt);
01685   return 0;
01686 }
01687 
01688 static void tcp_set_init_point(struct pico_socket *s)
01689 {
01690   struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
01691   t->rcv_processed = t->rcv_nxt;
01692 }
01693 
01694 static int tcp_synack(struct pico_socket *s, struct pico_frame *f)
01695 {
01696   struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
01697   struct pico_tcp_hdr *hdr  = (struct pico_tcp_hdr *)f->transport_hdr;
01698 
01699   if (ACKN(f) ==  (1 + t->snd_nxt)) {
01700     t->rcv_nxt = long_be(hdr->seq);
01701     t->rcv_processed = t->rcv_nxt + 1;
01702     tcp_ack(s, f);
01703 
01704     s->state &= 0x00FFU;
01705     s->state |= PICO_SOCKET_STATE_TCP_ESTABLISHED;
01706     tcp_dbg("TCP> Established. State: %x\n", s->state);
01707 
01708     if (s->wakeup)
01709       s->wakeup(PICO_SOCK_EV_CONN, s);
01710     s->ev_pending |= PICO_SOCK_EV_WR;
01711 
01712     t->rcv_nxt++;
01713     t->snd_nxt++;
01714     tcp_send_ack(t);  /* return ACK */
01715 
01716     return 0;
01717 
01718   } else {
01719     tcp_dbg("TCP> Not established, RST sent.\n");
01720     tcp_nosync_rst(s,f);
01721     return 0;
01722   }
01723 }
01724 
01725 static int tcp_first_ack(struct pico_socket *s, struct pico_frame *f)
01726 {
01727   struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
01728   tcp_dbg("ACK in SYN_RECV: expecting %08x got %08x\n", t->snd_nxt, ACKN(f));
01729   if (t->snd_nxt == ACKN(f)) {
01730     tcp_set_init_point(s);
01731     tcp_ack(s, f);
01732     s->state &= 0x00FFU;
01733     s->state |= PICO_SOCKET_STATE_TCP_ESTABLISHED;
01734     tcp_dbg("TCP: Established. State now: %04x\n", s->state);
01735     if( !s->parent && s->wakeup) { /* If the socket has no parent, -> sending socket that has a sim_open */
01736         tcp_dbg("FIRST ACK - No parent found -> sending socket\n");
01737         s->wakeup(PICO_SOCK_EV_CONN,  s);
01738     }
01739     if (s->parent && s->parent->wakeup) {
01740       tcp_dbg("FIRST ACK - Parent found -> listening socket\n");
01741       s->wakeup = s->parent->wakeup;
01742       s->parent->wakeup(PICO_SOCK_EV_CONN, s->parent);
01743     }
01744     s->ev_pending |= PICO_SOCK_EV_WR;
01745     tcp_dbg("%s: snd_nxt is now %08x\n", __FUNCTION__, t->snd_nxt);
01746     return 0;
01747   } else {
01748     tcp_nosync_rst(s,f);
01749     return 0;
01750   }
01751 }
01752 
01753 static int tcp_closewait(struct pico_socket *s, struct pico_frame *f)
01754 {
01755 
01756   struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
01757   struct pico_tcp_hdr *hdr  = (struct pico_tcp_hdr *) (f->transport_hdr);
01758 
01759 
01760   if (f->payload_len > 0)
01761     tcp_data_in(s,f);
01762   if (f->flags & PICO_TCP_ACK)
01763     tcp_ack(s,f);
01764   if (seq_compare(SEQN(f), t->rcv_nxt) == 0) {
01765     /* received FIN, increase ACK nr */
01766     t->rcv_nxt = long_be(hdr->seq) + 1;
01767         s->state &= 0x00FFU;
01768         s->state |= PICO_SOCKET_STATE_TCP_CLOSE_WAIT;
01769         /* set SHUT_REMOTE */
01770     s->state |= PICO_SOCKET_STATE_SHUT_REMOTE;
01771       tcp_dbg("TCP> Close-wait\n");
01772 
01773     if (s->wakeup){
01774       if(f->payload_len>0){
01775         struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
01776         t->sock.ev_pending |=PICO_SOCK_EV_CLOSE;
01777       }else
01778         s->wakeup(PICO_SOCK_EV_CLOSE, s);
01779     }
01780   } else {
01781     tcp_send_ack(t);  /* return ACK */
01782   }
01783   return 0;
01784 }
01785 
01786 /*static int tcp_fin(struct pico_socket *s, struct pico_frame *f)
01787 {
01788   return 0;
01789 }*/
01790 
01791 static int tcp_rcvfin(struct pico_socket *s, struct pico_frame *f)
01792 {
01793   struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
01794   tcp_dbg("TCP> Received FIN in FIN_WAIT1\n");
01795   s->state &= 0x00FFU;
01796   s->state |= PICO_SOCKET_STATE_TCP_CLOSING;
01797   t->rcv_processed = t->rcv_nxt + 1;
01798   t->rcv_nxt++;
01799   /* send ACK */
01800   tcp_send_ack(t);
01801   return 0;
01802 }
01803 
01804 static int tcp_finack(struct pico_socket *s, struct pico_frame *f)
01805 {
01806   struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
01807   tcp_dbg("TCP> ENTERED finack\n");
01808   t->rcv_nxt++;
01809   /* send ACK */
01810   tcp_send_ack(t);
01811 
01812   /* call socket wakeup with EV_FIN */
01813   if (s->wakeup)
01814     s->wakeup(PICO_SOCK_EV_FIN, s);
01815   s->state &= 0x00FFU;
01816   s->state |= PICO_SOCKET_STATE_TCP_TIME_WAIT;
01817   /* set SHUT_REMOTE */
01818   s->state |= PICO_SOCKET_STATE_SHUT_REMOTE;
01819   pico_timer_add(2000, tcp_deltcb, t);
01820 
01821   return 0;
01822 }
01823 
01824 static int tcp_rst(struct pico_socket *s, struct pico_frame *f)
01825 {
01826   struct pico_socket_tcp *t = (struct pico_socket_tcp *) s;
01827   struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr);
01828 
01829   tcp_dbg("TCP >>>>>>>>>>>>>> received RST <<<<<<<<<<<<<<<<<<<<\n");
01830   if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_SYN_SENT) {
01831     /* the RST is acceptable if the ACK field acknowledges the SYN */
01832     if ((t->snd_nxt + 1) == ACKN(f)) {  /* valid, got to closed state */
01833       /* update state */
01834       (t->sock).state &= 0x00FFU;
01835       (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED;
01836       (t->sock).state &= 0xFF00U;
01837       (t->sock).state |= PICO_SOCKET_STATE_CLOSED;
01838 
01839       /* call EV_FIN wakeup before deleting */
01840       if ((t->sock).wakeup)
01841         (t->sock).wakeup(PICO_SOCK_EV_FIN, &(t->sock));
01842 
01843       /* call EV_ERR wakeup before deleting */
01844       pico_err = PICO_ERR_ECONNRESET;
01845       if ((t->sock).wakeup)
01846         (t->sock).wakeup(PICO_SOCK_EV_ERR, &(t->sock));
01847 
01848       /* delete socket */
01849       pico_socket_del(&t->sock);
01850     } else {                      /* not valid, ignore */
01851       tcp_dbg("TCP RST> IGNORE\n");
01852       return 0;
01853     }
01854   } else {  /* all other states */
01855     /* all reset (RST) segments are validated by checking their SEQ-fields,
01856     a reset is valid if its sequence number is in the window */
01857     if ((long_be(hdr->seq) >= t->rcv_ackd) && (long_be(hdr->seq) <= ((short_be(hdr->rwnd)<<(t->wnd_scale)) + t->rcv_ackd))) {
01858       if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_SYN_RECV) {
01859         /* go to closed */
01860         (t->sock).state &= 0x00FFU;
01861         (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED;
01862         (t->sock).state &= 0xFF00U;
01863         (t->sock).state |= PICO_SOCKET_STATE_CLOSED;
01864         /* call EV_ERR wakeup */
01865         pico_err = PICO_ERR_ECONNRESET;
01866         if ((t->sock).wakeup)
01867           (t->sock).wakeup(PICO_SOCK_EV_ERR, &(t->sock));
01868         tcp_dbg("TCP RST> SOCKET BACK TO LISTEN\n");
01869         pico_socket_del(s);
01870       } else {
01871         /* go to closed */
01872         (t->sock).state &= 0x00FFU;
01873         (t->sock).state |= PICO_SOCKET_STATE_TCP_CLOSED;
01874         (t->sock).state &= 0xFF00U;
01875         (t->sock).state |= PICO_SOCKET_STATE_CLOSED;
01876 
01877         /* call EV_FIN wakeup before deleting */
01878         if ((t->sock).wakeup)
01879           (t->sock).wakeup(PICO_SOCK_EV_FIN, &(t->sock));
01880         /* call EV_ERR wakeup before deleting */
01881         pico_err = PICO_ERR_ECONNRESET;
01882         if ((t->sock).wakeup)
01883           (t->sock).wakeup(PICO_SOCK_EV_ERR, &(t->sock));
01884 
01885         /* delete socket */
01886         pico_socket_del(&t->sock);
01887       }
01888     } else {                      /* not valid, ignore */
01889       tcp_dbg("TCP RST> IGNORE\n");
01890       return 0;
01891     }
01892   }
01893 
01894   return 0;
01895 }
01896 
01897 struct tcp_action_entry {
01898   uint16_t tcpstate;
01899   int (*syn)(struct pico_socket *s, struct pico_frame *f);
01900   int (*synack)(struct pico_socket *s, struct pico_frame *f);
01901   int (*ack)(struct pico_socket *s, struct pico_frame *f);
01902   int (*data)(struct pico_socket *s, struct pico_frame *f);
01903   int (*fin)(struct pico_socket *s, struct pico_frame *f);
01904   int (*finack)(struct pico_socket *s, struct pico_frame *f);
01905   int (*rst)(struct pico_socket *s, struct pico_frame *f);
01906 };
01907 
01908 static struct tcp_action_entry tcp_fsm[] = {
01909     /* State                            syn              synack             ack                data             fin              finack           rst*/
01910   { PICO_SOCKET_STATE_TCP_UNDEF,        NULL,            NULL,              NULL,              NULL,            NULL,            NULL,            NULL     },
01911   { PICO_SOCKET_STATE_TCP_CLOSED,       NULL,            NULL,              NULL,              NULL,            NULL,            NULL,            NULL     },
01912   { PICO_SOCKET_STATE_TCP_LISTEN,       &tcp_syn,        &tcp_nosync_rst,   &tcp_nosync_rst,   &tcp_nosync_rst, &tcp_nosync_rst, &tcp_nosync_rst, NULL     },
01913   { PICO_SOCKET_STATE_TCP_SYN_SENT,     &tcp_nosync_rst, &tcp_synack,       &tcp_nosync_rst,   &tcp_nosync_rst, &tcp_nosync_rst, &tcp_nosync_rst, &tcp_rst },
01914   { PICO_SOCKET_STATE_TCP_SYN_RECV,     NULL,            &tcp_nosync_rst,   &tcp_first_ack,    &tcp_nosync_rst, &tcp_nosync_rst, &tcp_nosync_rst, &tcp_rst },
01915   { PICO_SOCKET_STATE_TCP_ESTABLISHED,  NULL,            &tcp_ack,          &tcp_ack,          &tcp_data_in,    &tcp_closewait,  &tcp_closewait,  &tcp_rst },
01916   { PICO_SOCKET_STATE_TCP_CLOSE_WAIT,   NULL,            &tcp_ack,          &tcp_ack,          &tcp_send_rst,   &tcp_closewait,  &tcp_closewait,  &tcp_rst },
01917   { PICO_SOCKET_STATE_TCP_LAST_ACK,     NULL,            &tcp_ack,          &tcp_lastackwait,  &tcp_send_rst,   &tcp_send_rst,   &tcp_send_rst,   &tcp_rst },
01918   { PICO_SOCKET_STATE_TCP_FIN_WAIT1,    NULL,            &tcp_ack,          &tcp_finwaitack,   &tcp_data_in,    &tcp_rcvfin,     &tcp_finack,     &tcp_rst },
01919   { PICO_SOCKET_STATE_TCP_FIN_WAIT2,    NULL,            &tcp_ack,          &tcp_ack,          &tcp_data_in,    &tcp_finwaitfin, &tcp_finack,     &tcp_rst },
01920   { PICO_SOCKET_STATE_TCP_CLOSING,      NULL,            &tcp_ack,          &tcp_closewaitack, &tcp_send_rst,   &tcp_send_rst,   &tcp_send_rst,   &tcp_rst },
01921   { PICO_SOCKET_STATE_TCP_TIME_WAIT,    NULL,            &tcp_ack,          &tcp_send_rst,     &tcp_send_rst,   &tcp_send_rst,   &tcp_send_rst,   &tcp_rst }
01922 };
01923 
01924 /*
01925    NOTE: in SYN-RECV receiving syn when cloned by default (see yellow pos-it), should send reset.
01926 */
01927 
01928 int pico_tcp_input(struct pico_socket *s, struct pico_frame *f)
01929 {
01930   struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *) (f->transport_hdr);
01931   int ret = 0;
01932   uint8_t flags = hdr->flags;
01933   struct tcp_action_entry *action = &tcp_fsm[s->state >> 8];
01934 
01935   f->payload = (f->transport_hdr + ((hdr->len & 0xf0) >> 2));
01936   f->payload_len = f->transport_len - ((hdr->len & 0xf0) >> 2);
01937 
01938   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", pico_tick,
01939       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 );
01940 
01941   /* This copy of the frame has the current socket as owner */
01942   f->sock = s;
01943 
01944   /* Those are not supported at this time. */
01945   flags &= ~(PICO_TCP_CWR | PICO_TCP_URG | PICO_TCP_ECN);
01946   if (flags == PICO_TCP_SYN) {
01947     if (action->syn)
01948       action->syn(s,f);
01949   } else if (flags == (PICO_TCP_SYN | PICO_TCP_ACK)) {
01950     if (action->synack)
01951       action->synack(s,f);
01952   } else {
01953     if ((flags == PICO_TCP_ACK) || (flags == (PICO_TCP_ACK | PICO_TCP_PSH))) {
01954       if (action->ack) {
01955         action->ack(s,f);
01956       }
01957     }
01958     if (f->payload_len > 0) {
01959       ret = f->payload_len;
01960       if (action->data)
01961         action->data(s,f);
01962     }
01963     if (flags == PICO_TCP_FIN) {
01964       if (action->fin)
01965         action->fin(s,f);
01966     }
01967     if ((flags == (PICO_TCP_FIN | PICO_TCP_ACK)) || (flags == (PICO_TCP_FIN | PICO_TCP_ACK | PICO_TCP_PSH))) {
01968       if (action->finack)
01969         action->finack(s,f);
01970     }
01971     if (flags & PICO_TCP_RST) {
01972       if (action->rst)
01973         action->rst(s,f);
01974     }
01975   }
01976 
01977 //discard:
01978   pico_frame_discard(f);
01979   return ret;
01980 }
01981 
01982 static void tcp_send_keepalive(unsigned long when, void *_t)
01983 {
01984   struct pico_socket_tcp *t = (struct pico_socket_tcp *)_t;
01985   tcp_dbg("\n\nSending keepalive (%d), [State = %d]...\n", t->backoff,t->sock.state );
01986   if( t->sock.net && ((t->sock.state & 0xFF00) == PICO_SOCKET_STATE_TCP_ESTABLISHED) )
01987   {
01988         tcp_send_ack(t);
01989 
01990         if (t->keepalive_timer_running > 0) {
01991             t->keepalive_timer_running--;
01992         }
01993 
01994         if (t->keepalive_timer_running == 0) {
01995             t->keepalive_timer_running++;
01996             tcp_dbg("[Self] Adding timer(retransmit keepalive)\n");
01997             pico_timer_add(t->rto << (++t->backoff), tcp_send_keepalive, t);
01998         }
01999   }
02000 }
02001 
02002 int pico_tcp_output(struct pico_socket *s, int loop_score)
02003 {
02004   struct pico_socket_tcp *t = (struct pico_socket_tcp *)s;
02005   struct pico_frame *f, *una;
02006   struct pico_tcp_hdr *hdr;
02007   int sent = 0;
02008 
02009   una = first_segment(&t->tcpq_out);
02010 
02011   f = peek_segment(&t->tcpq_out, t->snd_nxt);
02012   while((f) && (t->cwnd >= t->in_flight)) {
02013     hdr = (struct pico_tcp_hdr *)f->transport_hdr;
02014     f->timestamp = pico_tick;
02015     tcp_add_options(t, f, hdr->flags, tcp_options_size(t, hdr->flags));
02016     if (seq_compare(SEQN(f) + f->payload_len, SEQN(una) + (t->recv_wnd << t->recv_wnd_scale)) > 0) {
02017       t->cwnd = t->in_flight;
02018       if (t->cwnd < 1)
02019         t->cwnd = 1;
02020       if (t->x_mode != PICO_TCP_WINDOW_FULL) {
02021         tcp_dbg("TCP> RIGHT SIZING (rwnd: %d, frame len: %d\n",t->recv_wnd << t->recv_wnd_scale, f->payload_len);
02022         tcp_dbg("In window full...\n");
02023         t->snd_nxt = SEQN(una);   /* XXX prevent out-of-order-packets ! */ /*DLA re-enabled.*/
02024         t->snd_retry = SEQN(una);   /* XXX replace by retry pointer? */
02025 
02026         /* Alternative to the line above:  (better performance, but seems to lock anyway with larger buffers)
02027         if (seq_compare(t->snd_nxt, SEQN(una)) > 0)
02028           t->snd_nxt -= f->payload_len;
02029         */
02030 
02031         t->x_mode = PICO_TCP_WINDOW_FULL;
02032         if (t->keepalive_timer_running == 0) {
02033           tcp_dbg("[Window full] Adding timer(send keepalive)\n");
02034           tcp_send_keepalive(0, t);
02035         }
02036       }
02037       break;
02038     }
02039     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);
02040     tcp_send(t, f);
02041     sent++;
02042     loop_score--;
02043     t->snd_last_out = SEQN(f);
02044     if (loop_score < 1)
02045       break;
02046     if (f->payload_len > 0) {
02047       f = next_segment(&t->tcpq_out, f);
02048     } else {
02049       f = NULL;
02050     }
02051   }
02052   if (sent > 0) {
02053     if (t->rto < PICO_TCP_RTO_MIN)
02054       t->rto = PICO_TCP_RTO_MIN;
02055     //if (s->wakeup)
02056     //  t->sock.wakeup(PICO_SOCK_EV_WR, &t->sock);
02057     add_retransmission_timer(t, pico_tick + t->rto);
02058   } else {
02059     /* Nothing to transmit. */
02060   }
02061 
02062   if ((t->tcpq_out.frames == 0) && (s->state & PICO_SOCKET_STATE_SHUT_LOCAL)) {    /* if no more packets in queue, XXX replacled !f by tcpq check */
02063     if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_ESTABLISHED) {
02064       tcp_dbg("TCP> buffer empty, shutdown established ...\n");
02065       /* send fin if queue empty and in state shut local (write) */
02066       tcp_send_fin(t);
02067       /* change tcp state to FIN_WAIT1 */
02068       s->state &= 0x00FFU;
02069       s->state |= PICO_SOCKET_STATE_TCP_FIN_WAIT1;
02070     } else if ((s->state & PICO_SOCKET_STATE_TCP) == PICO_SOCKET_STATE_TCP_CLOSE_WAIT) {
02071       /* send fin if queue empty and in state shut local (write) */
02072       tcp_send_fin(t);
02073       /* change tcp state to LAST_ACK */
02074       s->state &= 0x00FFU;
02075       s->state |= PICO_SOCKET_STATE_TCP_LAST_ACK;
02076       tcp_dbg("TCP> STATE: LAST_ACK.\n");
02077     }
02078   }
02079   return loop_score;
02080 }
02081 
02082 /* function to make new segment from hold queue with specific size (mss) */
02083 static struct pico_frame * pico_hold_segment_make(struct pico_socket_tcp *t)
02084 {
02085   struct pico_frame *f_temp,*f_new;
02086   struct pico_socket *s = (struct pico_socket *) &t->sock;
02087   struct pico_tcp_hdr *hdr;
02088   int total_len = 0, total_payload_len = 0;
02089   int off = 0, test = 0;
02090 
02091   off = pico_tcp_overhead(s);
02092 
02093   /* init with first frame in hold queue */
02094   f_temp = first_segment(&t->tcpq_hold);
02095   total_len = f_temp->payload_len;
02096   f_temp = next_segment(&t->tcpq_hold, f_temp);
02097 
02098   /* check till total_len <= MSS */
02099   while ((f_temp != NULL) && ((total_len+f_temp->payload_len) <= PICO_TCP_DEFAULT_MSS)) {
02100     total_len += f_temp->payload_len;
02101     f_temp = next_segment(&t->tcpq_hold, f_temp);
02102     if (f_temp == NULL)
02103       break;
02104   }
02105   /* alloc new frame with payload size = off + total_len */
02106   f_new = pico_socket_frame_alloc(s, off + total_len);
02107   if (!f_new) {
02108     pico_err = PICO_ERR_ENOMEM;
02109     return f_new;
02110   }
02111 
02112   hdr = (struct pico_tcp_hdr *) f_new->transport_hdr;
02113   /* init new frame */
02114   f_new->payload += off;
02115   f_new->payload_len -= off;
02116   f_new->sock = s;
02117 
02118   f_temp = first_segment(&t->tcpq_hold);
02119   hdr->seq = ((struct pico_tcp_hdr *)(f_temp->transport_hdr))->seq;  /* get sequence number of first frame */
02120   hdr->trans.sport = t->sock.local_port;
02121   hdr->trans.dport = t->sock.remote_port;
02122 
02123   /* check till total_payload_len <= MSS */
02124   while ((f_temp != NULL) && ((total_payload_len + f_temp->payload_len) <= PICO_TCP_DEFAULT_MSS)) {
02125     /* cpy data and discard frame */
02126     test++;
02127     memcpy(f_new->payload + total_payload_len, f_temp->payload, f_temp->payload_len);
02128     total_payload_len += f_temp->payload_len;
02129     pico_discard_segment(&t->tcpq_hold, f_temp);
02130     f_temp = first_segment(&t->tcpq_hold);
02131   }
02132 
02133   hdr->len = (f_new->payload - f_new->transport_hdr) << 2 | t->jumbo;
02134 
02135   tcp_dbg_nagle("NAGLE make - joined %d segments, len %d bytes\n",test,total_payload_len);
02136 
02137   return f_new;
02138 }
02139 
02140 /* original behavior kept when Nagle disabled;
02141    Nagle algorithm added here, keeping hold frame queue instead of eg linked list of data */
02142 int pico_tcp_push(struct pico_protocol *self, struct pico_frame *f)
02143 {
02144   struct pico_tcp_hdr *hdr = (struct pico_tcp_hdr *)f->transport_hdr;
02145   struct pico_socket_tcp *t = (struct pico_socket_tcp *) f->sock;
02146   struct pico_frame *f_new;
02147   int total_len = 0;
02148 
02149   hdr->trans.sport = t->sock.local_port;
02150   hdr->trans.dport = t->sock.remote_port;
02151   hdr->seq = long_be(t->snd_last + 1);
02152   hdr->len = (f->payload - f->transport_hdr) << 2 | t->jumbo;
02153 
02154   if (f->payload_len > (t->tcpq_out.max_size - t->tcpq_out.size))
02155     t->sock.ev_pending &= (~PICO_SOCK_EV_WR);
02156 
02157   /***************************************************************************/
02158 
02159   if (!IS_NAGLE_ENABLED((&(t->sock)))) {
02160     /* TCP_NODELAY enabled, original behavior */
02161     if (pico_enqueue_segment(&t->tcpq_out,f) > 0) {
02162       tcp_dbg_nagle("TCP_PUSH - NO NAGLE - Pushing segment %08x, len %08x to socket %p\n", t->snd_last + 1, f->payload_len, t);
02163       t->snd_last += f->payload_len;
02164       return f->payload_len;
02165     } else {
02166       tcp_dbg("Enqueue failed.\n");
02167       return 0;
02168     }
02169   }
02170   /***************************************************************************/
02171   else {
02172     /* Nagle's algorithm enabled, check if ready to send, or put frame in hold queue */
02173     if (IS_TCP_IDLE(t) && IS_TCP_HOLDQ_EMPTY(t)) {  /* opt 1. send frame */
02174       if (pico_enqueue_segment(&t->tcpq_out,f) > 0) {
02175         tcp_dbg_nagle("TCP_PUSH - NAGLE - Pushing segment %08x, len %08x to socket %p\n", t->snd_last + 1, f->payload_len, t);
02176         t->snd_last += f->payload_len;
02177         return f->payload_len;
02178       } else {
02179         tcp_dbg("Enqueue failed.\n");
02180         return 0;
02181       }
02182     } else {                                        /* opt 2. hold data back */
02183       total_len = f->payload_len + t->tcpq_hold.size;
02184       if ((total_len >= PICO_TCP_DEFAULT_MSS) && ((t->tcpq_out.max_size - t->tcpq_out.size) >= PICO_TCP_DEFAULT_MSS)) {/* TODO check mss socket */
02185         /* IF enough data in hold (>mss) AND space in out queue (>mss) */
02186         /* add current frame in hold and make new segment */
02187         if (pico_enqueue_segment(&t->tcpq_hold,f) > 0 ) {
02188           tcp_dbg_nagle("TCP_PUSH - NAGLE - Pushed into hold, make new (enqueued frames out %d)\n",t->tcpq_out.frames);
02189           t->snd_last += f->payload_len;    /* XXX  WATCH OUT */
02190           f_new = pico_hold_segment_make(t);
02191         } else {
02192           tcp_dbg_nagle("TCP_PUSH - NAGLE - enqueue hold failed 1\n");
02193           return 0;
02194         }
02195         /* and put new frame in out queue */
02196         if ((f_new != NULL) && (pico_enqueue_segment(&t->tcpq_out,f_new) > 0)) {
02197           return f_new->payload_len;
02198         } else {
02199           tcp_dbg_nagle("TCP_PUSH - NAGLE - enqueue out failed, f_new = %p\n",f_new);
02200           return -1;                        /* XXX something seriously wrong */
02201         }
02202       } else {
02203         /* ELSE put frame in hold queue */
02204         if (pico_enqueue_segment(&t->tcpq_hold,f) > 0) {
02205           tcp_dbg_nagle("TCP_PUSH - NAGLE - Pushed into hold (enqueued frames out %d)\n",t->tcpq_out.frames);
02206           t->snd_last += f->payload_len;    /* XXX  WATCH OUT */
02207           return f->payload_len;
02208         } else {
02209           tcp_dbg_nagle("TCP_PUSH - NAGLE - enqueue hold failed 2\n");
02210           return 0;
02211         }
02212       }
02213     }
02214   }
02215   /***************************************************************************/
02216 }
02217 #endif //PICO_SUPPORT_TCP