Fix fifo ooo bugs and improve testing

Change-Id: If3c01e318bcb740ca5b240c63f712e2167082a80
Signed-off-by: Dave Barach <dave@barachs.net>
Signed-off-by: Florin Coras <fcoras@cisco.com>

7 files changed, 602 insertions, 94 deletions

diff --git a/src/svm/svm_fifo.c b/src/svm/svm_fifo.c
index 097bab77..bd968aea 100644
--- a/src/svm/svm_fifo.c
+++ b/src/svm/svm_fifo.c
@@ -15,6 +15,36 @@
 
 #include <svm/svm_fifo.h>
 
+#define offset_lt(_a, _b) ((i32)((_a)-(_b)) < 0)
+#define offset_leq(_a, _b) ((i32)((_a)-(_b)) <= 0)
+
+u8 *
+format_ooo_segment (u8 * s, va_list * args)
+{
+  ooo_segment_t *seg = va_arg (*args, ooo_segment_t *);
+
+  s = format (s, "pos %u, len %u, next %d, prev %d",
+	      seg->start, seg->length, seg->next, seg->prev);
+  return s;
+}
+
+u8 *
+format_ooo_list (u8 * s, va_list * args)
+{
+  svm_fifo_t *f = va_arg (*args, svm_fifo_t *);
+  u32 ooo_segment_index = f->ooos_list_head;
+  ooo_segment_t *seg;
+
+  while (ooo_segment_index != OOO_SEGMENT_INVALID_INDEX)
+    {
+      seg = pool_elt_at_index (f->ooo_segments, ooo_segment_index);
+      s = format (s, "\n  %U", format_ooo_segment, seg);
+
+      ooo_segment_index = seg->next;
+    }
+  return s;
+}
+
 /** create an svm fifo, in the current heap. Fails vs blow up the process */
 svm_fifo_t *
 svm_fifo_create (u32 data_size_in_bytes)
@@ -47,7 +77,7 @@ ooo_segment_new (svm_fifo_t * f, u32 start, u32 length)
 
   pool_get (f->ooo_segments, s);
 
-  s->fifo_position = start;
+  s->start = start;
   s->length = length;
 
   s->prev = s->next = OOO_SEGMENT_INVALID_INDEX;
@@ -88,14 +118,13 @@ static void
 ooo_segment_add (svm_fifo_t * f, u32 offset, u32 length)
 {
   ooo_segment_t *s, *new_s, *prev, *next, *it;
-  u32 new_index, position, end_offset, s_sof, s_eof, s_index;
+  u32 new_index, end_offset, s_sof, s_eof, s_index;
 
-  position = (f->tail + offset) % f->nitems;
   end_offset = offset + length;
 
   if (f->ooos_list_head == OOO_SEGMENT_INVALID_INDEX)
     {
-      s = ooo_segment_new (f, position, length);
+      s = ooo_segment_new (f, offset, length);
       f->ooos_list_head = s - f->ooo_segments;
       f->ooos_newest = f->ooos_list_head;
       return;
@@ -104,26 +133,26 @@ ooo_segment_add (svm_fifo_t * f, u32 offset, u32 length)
   /* Find first segment that starts after new segment */
   s = pool_elt_at_index (f->ooo_segments, f->ooos_list_head);
   while (s->next != OOO_SEGMENT_INVALID_INDEX
-	 && ooo_segment_offset (f, s) <= offset)
+	 && offset_leq (ooo_segment_offset (f, s), offset))
     s = pool_elt_at_index (f->ooo_segments, s->next);
 
   s_index = s - f->ooo_segments;
   s_sof = ooo_segment_offset (f, s);
   s_eof = ooo_segment_end_offset (f, s);
+  prev = ooo_segment_get_prev (f, s);
 
   /* No overlap, add before current segment */
-  if (end_offset < s_sof)
+  if (offset_lt (end_offset, s_sof)
+      && (!prev || offset_lt (prev->start + prev->length, offset)))
     {
-      new_s = ooo_segment_new (f, position, length);
+      new_s = ooo_segment_new (f, offset, length);
       new_index = new_s - f->ooo_segments;
 
       /* Pool might've moved, get segment again */
       s = pool_elt_at_index (f->ooo_segments, s_index);
-
       if (s->prev != OOO_SEGMENT_INVALID_INDEX)
 	{
 	  new_s->prev = s->prev;
-
 	  prev = pool_elt_at_index (f->ooo_segments, new_s->prev);
 	  prev->next = new_index;
 	}
@@ -139,9 +168,9 @@ ooo_segment_add (svm_fifo_t * f, u32 offset, u32 length)
       return;
     }
   /* No overlap, add after current segment */
-  else if (s_eof < offset)
+  else if (offset_lt (s_eof, offset))
     {
-      new_s = ooo_segment_new (f, position, length);
+      new_s = ooo_segment_new (f, offset, length);
       new_index = new_s - f->ooo_segments;
 
       /* Pool might've moved, get segment again */
@@ -150,7 +179,6 @@ ooo_segment_add (svm_fifo_t * f, u32 offset, u32 length)
       if (s->next != OOO_SEGMENT_INVALID_INDEX)
 	{
 	  new_s->next = s->next;
-
 	  next = pool_elt_at_index (f->ooo_segments, new_s->next);
 	  next->prev = new_index;
 	}
@@ -167,7 +195,7 @@ ooo_segment_add (svm_fifo_t * f, u32 offset, u32 length)
    */
 
   /* Merge at head */
-  if (offset <= s_sof)
+  if (offset_leq (offset, s_sof))
     {
       /* If we have a previous, check if we overlap */
       if (s->prev != OOO_SEGMENT_INVALID_INDEX)
@@ -176,26 +204,31 @@ ooo_segment_add (svm_fifo_t * f, u32 offset, u32 length)
 
 	  /* New segment merges prev and current. Remove previous and
 	   * update position of current. */
-	  if (ooo_segment_end_offset (f, prev) >= offset)
+	  if (offset_leq (offset, ooo_segment_end_offset (f, prev)))
 	    {
-	      s->fifo_position = prev->fifo_position;
+	      s->start = prev->start;
 	      s->length = s_eof - ooo_segment_offset (f, prev);
 	      ooo_segment_del (f, s->prev);
 	    }
+	  else
+	    {
+	      s->start = offset;
+	      s->length = s_eof - ooo_segment_offset (f, s);
+	    }
 	}
       else
 	{
-	  s->fifo_position = position;
+	  s->start = offset;
 	  s->length = s_eof - ooo_segment_offset (f, s);
 	}
 
       /* The new segment's tail may cover multiple smaller ones */
-      if (s_eof < end_offset)
+      if (offset_lt (s_eof, end_offset))
 	{
 	  /* Remove segments completely covered */
 	  it = (s->next != OOO_SEGMENT_INVALID_INDEX) ?
 	    pool_elt_at_index (f->ooo_segments, s->next) : 0;
-	  while (it && ooo_segment_end_offset (f, it) < end_offset)
+	  while (it && offset_lt (ooo_segment_end_offset (f, it), end_offset))
 	    {
 	      next = (it->next != OOO_SEGMENT_INVALID_INDEX) ?
 		pool_elt_at_index (f->ooo_segments, it->next) : 0;
@@ -207,7 +240,7 @@ ooo_segment_add (svm_fifo_t * f, u32 offset, u32 length)
 	  s->length = end_offset - ooo_segment_offset (f, s);
 
 	  /* If partial overlap with last, merge */
-	  if (it && ooo_segment_offset (f, it) < end_offset)
+	  if (it && offset_lt (ooo_segment_offset (f, it), end_offset))
 	    {
 	      s->length +=
 		it->length - (ooo_segment_offset (f, it) - end_offset);
@@ -216,7 +249,7 @@ ooo_segment_add (svm_fifo_t * f, u32 offset, u32 length)
 	}
     }
   /* Last but overlapping previous */
-  else if (s_eof <= end_offset)
+  else if (offset_leq (s_eof, end_offset))
     {
       s->length = end_offset - ooo_segment_offset (f, s);
     }
@@ -247,7 +280,7 @@ ooo_segment_try_collect (svm_fifo_t * f, u32 n_bytes_enqueued)
   s = pool_elt_at_index (f->ooo_segments, f->ooos_list_head);
 
   /* If last tail update overlaps one/multiple ooo segments, remove them */
-  diff = (f->nitems + f->tail - s->fifo_position) % f->nitems;
+  diff = (f->nitems + ((int) s->start - f->tail)) % f->nitems;
   while (0 < diff && diff < n_bytes_enqueued)
     {
       /* Segment end is beyond the tail. Advance tail and be done */
@@ -262,7 +295,7 @@ ooo_segment_try_collect (svm_fifo_t * f, u32 n_bytes_enqueued)
 	{
 	  index = s - f->ooo_segments;
 	  s = pool_elt_at_index (f->ooo_segments, s->next);
-	  diff = (f->nitems + f->tail - s->fifo_position) % f->nitems;
+	  diff = (f->nitems + ((int) s->start - f->tail)) % f->nitems;
 	  ooo_segment_del (f, index);
 	}
       /* End of search */
@@ -368,9 +401,20 @@ svm_fifo_enqueue_with_offset_internal (svm_fifo_t * f,
 {
   u32 total_copy_bytes, first_copy_bytes, second_copy_bytes;
   u32 cursize, nitems;
-  u32 tail_plus_offset;
+  u32 normalized_offset;
+  int rv;
 
-  ASSERT (offset > 0);
+  /* Safety: don't wrap more than nitems/2 */
+  ASSERT ((f->nitems + offset - f->tail) % f->nitems < f->nitems / 2);
+
+  /* Users would do do well to avoid this */
+  if (PREDICT_FALSE (f->tail == (offset % f->nitems)))
+    {
+      rv = svm_fifo_enqueue_internal (f, pid, required_bytes, copy_from_here);
+      if (rv > 0)
+	return 0;
+      return -1;
+    }
 
   /* read cursize, which can only increase while we're working */
   cursize = svm_fifo_max_dequeue (f);
@@ -384,24 +428,24 @@ svm_fifo_enqueue_with_offset_internal (svm_fifo_t * f,
 
   /* Number of bytes we're going to copy */
   total_copy_bytes = required_bytes;
-  tail_plus_offset = (f->tail + offset) % nitems;
+  normalized_offset = offset % nitems;
 
   /* Number of bytes in first copy segment */
-  first_copy_bytes = ((nitems - tail_plus_offset) < total_copy_bytes)
-    ? (nitems - tail_plus_offset) : total_copy_bytes;
+  first_copy_bytes = ((nitems - normalized_offset) < total_copy_bytes)
+    ? (nitems - normalized_offset) : total_copy_bytes;
 
-  clib_memcpy (&f->data[tail_plus_offset], copy_from_here, first_copy_bytes);
+  clib_memcpy (&f->data[normalized_offset], copy_from_here, first_copy_bytes);
 
   /* Number of bytes in second copy segment, if any */
   second_copy_bytes = total_copy_bytes - first_copy_bytes;
   if (second_copy_bytes)
     {
-      tail_plus_offset += first_copy_bytes;
-      tail_plus_offset %= nitems;
+      normalized_offset += first_copy_bytes;
+      normalized_offset %= nitems;
 
-      ASSERT (tail_plus_offset == 0);
+      ASSERT (normalized_offset == 0);
 
-      clib_memcpy (&f->data[tail_plus_offset],
+      clib_memcpy (&f->data[normalized_offset],
 		   copy_from_here + first_copy_bytes, second_copy_bytes);
     }
 
@@ -573,8 +617,8 @@ format_svm_fifo (u8 * s, va_list * args)
       ooo_segment_t *seg;
       u32 seg_index;
 
-      s =
-	format (s, "ooo pool %d active elts\n", pool_elts (f->ooo_segments));
+      s = format (s, "ooo pool %d active elts\n",
+		  pool_elts (f->ooo_segments));
 
       seg_index = f->ooos_list_head;
 
@@ -582,13 +626,25 @@ format_svm_fifo (u8 * s, va_list * args)
 	{
 	  seg = pool_elt_at_index (f->ooo_segments, seg_index);
 	  s = format (s, "  pos %u, len %u next %d\n",
-		      seg->fifo_position, seg->length, seg->next);
+		      seg->start, seg->length, seg->next);
 	  seg_index = seg->next;
 	}
     }
   return s;
 }
 
+u32
+svm_fifo_number_ooo_segments (svm_fifo_t * f)
+{
+  return pool_elts (f->ooo_segments);
+}
+
+ooo_segment_t *
+svm_fifo_first_ooo_segment (svm_fifo_t * f)
+{
+  return pool_elt_at_index (f->ooo_segments, f->ooos_list_head);
+}
+
 /*
  * fd.io coding-style-patch-verification: ON
  *
diff --git a/src/svm/svm_fifo.h b/src/svm/svm_fifo.h
index 9beb63f5..0fff2577 100644
--- a/src/svm/svm_fifo.h
+++ b/src/svm/svm_fifo.h
@@ -36,10 +36,13 @@ typedef struct
   u32 next;	/**< Next linked-list element pool index */
   u32 prev;	/**< Previous linked-list element pool index */
 
-  u32 fifo_position;	/**< Start of segment, normalized*/
+  u32 start;	/**< Start of segment, normalized*/
   u32 length;		/**< Length of segment */
 } ooo_segment_t;
 
+format_function_t format_ooo_segment;
+format_function_t format_ooo_list;
+
 #define OOO_SEGMENT_INVALID_INDEX ((u32)~0)
 
 typedef struct
@@ -127,6 +130,8 @@ int svm_fifo_dequeue_nowait (svm_fifo_t * f, int pid, u32 max_bytes,
 int svm_fifo_peek (svm_fifo_t * f, int pid, u32 offset, u32 max_bytes,
 		   u8 * copy_here);
 int svm_fifo_dequeue_drop (svm_fifo_t * f, int pid, u32 max_bytes);
+u32 svm_fifo_number_ooo_segments (svm_fifo_t * f);
+ooo_segment_t *svm_fifo_first_ooo_segment (svm_fifo_t * f);
 
 format_function_t format_svm_fifo;
 
@@ -139,13 +144,23 @@ svm_fifo_newest_ooo_segment (svm_fifo_t * f)
 always_inline u32
 ooo_segment_offset (svm_fifo_t * f, ooo_segment_t * s)
 {
-  return ((f->nitems + s->fifo_position - f->tail) % f->nitems);
+//  return ((f->nitems + s->fifo_position - f->tail) % f->nitems);
+  return s->start;
 }
 
 always_inline u32
 ooo_segment_end_offset (svm_fifo_t * f, ooo_segment_t * s)
 {
-  return ((f->nitems + s->fifo_position + s->length - f->tail) % f->nitems);
+//  return ((f->nitems + s->fifo_position + s->length - f->tail) % f->nitems);
+  return s->start + s->length;
+}
+
+always_inline ooo_segment_t *
+ooo_segment_get_prev (svm_fifo_t * f, ooo_segment_t * s)
+{
+  if (s->prev == OOO_SEGMENT_INVALID_INDEX)
+    return 0;
+  return pool_elt_at_index (f->ooo_segments, s->prev);
 }
 
 #endif /* __included_ssvm_fifo_h__ */
diff --git a/src/vnet/tcp/tcp.c b/src/vnet/tcp/tcp.c
index a0c66b9f..12982589 100644
--- a/src/vnet/tcp/tcp.c
+++ b/src/vnet/tcp/tcp.c
@@ -447,7 +447,7 @@ format_tcp_state (u8 * s, va_list * args)
   if (*state < TCP_N_STATES)
     s = format (s, "%s", tcp_fsm_states[*state]);
   else
-    s = format (s, "UNKNOWN");
+    s = format (s, "UNKNOWN (%d (0x%x))", *state, *state);
 
   return s;
 }
diff --git a/src/vnet/tcp/tcp.h b/src/vnet/tcp/tcp.h
index 225b26da..2ac6a9b8 100644
--- a/src/vnet/tcp/tcp.h
+++ b/src/vnet/tcp/tcp.h
@@ -58,6 +58,7 @@ typedef enum _tcp_state
 } tcp_state_t;
 
 format_function_t format_tcp_state;
+format_function_t format_tcp_flags;
 
 /** TCP timers */
 #define foreach_tcp_timer               \
diff --git a/src/vnet/tcp/tcp_format.c b/src/vnet/tcp/tcp_format.c
index 7136741d..994ccfd6 100644
--- a/src/vnet/tcp/tcp_format.c
+++ b/src/vnet/tcp/tcp_format.c
@@ -40,7 +40,7 @@
 #include <vnet/ip/ip.h>
 #include <vnet/tcp/tcp.h>
 
-static u8 *
+u8 *
 format_tcp_flags (u8 * s, va_list * args)
 {
   int flags = va_arg (*args, int);
diff --git a/src/vnet/tcp/tcp_input.c b/src/vnet/tcp/tcp_input.c
index a12ad8c0..97679aaf 100644
--- a/src/vnet/tcp/tcp_input.c
+++ b/src/vnet/tcp/tcp_input.c
@@ -211,8 +211,6 @@ tcp_options_parse (tcp_header_t * th, tcp_options_t * to)
 always_inline int
 tcp_segment_check_paws (tcp_connection_t * tc)
 {
-  /* XXX normally test for timestamp should be lt instead of leq, but for
-   * local testing this is not enough */
   return tcp_opts_tstamp (&tc->opt) && tc->tsval_recent
     && timestamp_lt (tc->opt.tsval, tc->tsval_recent);
 }
@@ -999,7 +997,7 @@ tcp_session_enqueue_ooo (tcp_connection_t * tc, vlib_buffer_t * b,
 			 u16 data_len)
 {
   stream_session_t *s0;
-  u32 offset, seq;
+  u32 offset;
   int rv;
 
   /* Pure ACK. Do nothing */
@@ -1009,8 +1007,9 @@ tcp_session_enqueue_ooo (tcp_connection_t * tc, vlib_buffer_t * b,
     }
 
   s0 = stream_session_get (tc->c_s_index, tc->c_thread_index);
-  seq = vnet_buffer (b)->tcp.seq_number;
-  offset = seq - tc->rcv_nxt;
+  offset = vnet_buffer (b)->tcp.seq_number - tc->irs;
+
+  clib_warning ("ooo: offset %d len %d", offset, data_len);
 
   rv = svm_fifo_enqueue_with_offset (s0->server_rx_fifo, s0->pid, offset,
 				     data_len, vlib_buffer_get_current (b));
@@ -1032,8 +1031,8 @@ tcp_session_enqueue_ooo (tcp_connection_t * tc, vlib_buffer_t * b,
 
       /* Get the newest segment from the fifo */
       newest = svm_fifo_newest_ooo_segment (s0->server_rx_fifo);
-      start = tc->rcv_nxt + ooo_segment_offset (s0->server_rx_fifo, newest);
-      end = tc->rcv_nxt + ooo_segment_end_offset (s0->server_rx_fifo, newest);
+      start = ooo_segment_offset (s0->server_rx_fifo, newest);
+      end = ooo_segment_end_offset (s0->server_rx_fifo, newest);
 
       tcp_update_sack_list (tc, start, end);
     }
@@ -1072,6 +1071,7 @@ tcp_segment_rcv (tcp_main_t * tm, tcp_connection_t * tc, vlib_buffer_t * b,
     {
       /* Old sequence numbers allowed through because they overlapped
        * the rx window */
+
       if (seq_lt (vnet_buffer (b)->tcp.seq_number, tc->rcv_nxt))
 	{
 	  error = TCP_ERROR_SEGMENT_OLD;
@@ -1181,6 +1181,7 @@ tcp46_established_inline (vlib_main_t * vm, vlib_node_runtime_t * node,
   u32 n_left_from, next_index, *from, *to_next;
   u32 my_thread_index = vm->thread_index, errors = 0;
   tcp_main_t *tm = vnet_get_tcp_main ();
+  u8 is_fin = 0;
 
   from = vlib_frame_vector_args (from_frame);
   n_left_from = from_frame->n_vectors;
@@ -1243,9 +1244,11 @@ tcp46_established_inline (vlib_main_t * vm, vlib_node_runtime_t * node,
 	      n_advance_bytes0 += sizeof (ip60[0]);
 	    }
 
+	  is_fin = (th0->flags & TCP_FLAG_FIN) != 0;
+
 	  /* SYNs, FINs and data consume sequence numbers */
 	  vnet_buffer (b0)->tcp.seq_end = vnet_buffer (b0)->tcp.seq_number
-	    + tcp_is_syn (th0) + tcp_is_fin (th0) + n_data_bytes0;
+	    + tcp_is_syn (th0) + is_fin + n_data_bytes0;
 
 	  /* TODO header prediction fast path */
 
@@ -1272,8 +1275,11 @@ tcp46_established_inline (vlib_main_t * vm, vlib_node_runtime_t * node,
 	  vlib_buffer_advance (b0, n_advance_bytes0);
 	  error0 = tcp_segment_rcv (tm, tc0, b0, n_data_bytes0, &next0);
 
+	  /* N.B. buffer is rewritten if segment is ooo. Thus, th0 becomes a
+	   * dangling reference. */
+
 	  /* 8: check the FIN bit */
-	  if (tcp_fin (th0))
+	  if (is_fin)
 	    {
 	      /* Enter CLOSE-WAIT and notify session. Don't send ACK, instead
 	       * wait for session to call close. To avoid lingering
@@ -2365,8 +2371,12 @@ tcp46_input_inline (vlib_main_t * vm, vlib_node_runtime_t * node,
 
 	      if (PREDICT_FALSE (error0 == TCP_ERROR_DISPATCH))
 		{
+		  tcp_state_t state0 = tc0->state;
 		  /* Overload tcp flags to store state */
 		  vnet_buffer (b0)->tcp.flags = tc0->state;
+		  clib_warning ("disp error state %U flags %U",
+				format_tcp_state, &state0,
+				format_tcp_flags, flags0);
 		}
 	    }
 	  else
diff --git a/src/vnet/tcp/tcp_test.c b/src/vnet/tcp/tcp_test.c
index 3dbbdf6f..12579632 100644
--- a/src/vnet/tcp/tcp_test.c
+++ b/src/vnet/tcp/tcp_test.c
@@ -173,17 +173,145 @@ tcp_test_sack ()
   return 0;
 }
 
-static int
-tcp_test_fifo (vlib_main_t * vm, unformat_input_t * input)
+typedef struct
+{
+  u32 offset;
+  u32 len;
+} test_pattern_t;
+
+/* *INDENT-OFF* */
+test_pattern_t test_pattern[] = {
+  {380, 8}, {768, 8}, {1156, 8}, {1544, 8}, {1932, 8}, {2320, 8}, {2708, 8},
+  {2992, 8}, {372, 8}, {760, 8}, {1148, 8}, {1536, 8}, {1924, 8}, {2312, 8},
+  {2700, 8}, {2984, 8}, {364, 8}, {752, 8}, {1140, 8}, {1528, 8}, {1916, 8},
+  {2304, 8}, {2692, 8}, {2976, 8}, {356, 8}, {744, 8}, {1132, 8}, {1520, 8},
+  {1908, 8}, {2296, 8}, {2684, 8}, {2968, 8}, {348, 8}, {736, 8}, {1124, 8},
+  {1512, 8}, {1900, 8}, {2288, 8}, {2676, 8}, {2960, 8}, {340, 8}, {728, 8},
+  {1116, 8}, {1504, 8}, {1892, 8}, {2280, 8}, {2668, 8}, {2952, 8}, {332, 8},
+  {720, 8}, {1108, 8}, {1496, 8}, {1884, 8}, {2272, 8}, {2660, 8}, {2944, 8},
+  {324, 8}, {712, 8}, {1100, 8}, {1488, 8}, {1876, 8}, {2264, 8}, {2652, 8},
+  {2936, 8}, {316, 8}, {704, 8}, {1092, 8}, {1480, 8}, {1868, 8}, {2256, 8},
+  {2644, 8}, {2928, 8}, {308, 8}, {696, 8}, {1084, 8}, {1472, 8}, {1860, 8},
+  {2248, 8}, {2636, 8}, {2920, 8}, {300, 8}, {688, 8}, {1076, 8}, {1464, 8},
+  {1852, 8}, {2240, 8}, {2628, 8}, {2912, 8}, {292, 8}, {680, 8}, {1068, 8},
+  {1456, 8}, {1844, 8}, {2232, 8}, {2620, 8}, {2904, 8}, {284, 8}, {672, 8},
+  {1060, 8}, {1448, 8}, {1836, 8}, {2224, 8}, {2612, 8}, {2896, 8}, {276, 8},
+  {664, 8}, {1052, 8}, {1440, 8}, {1828, 8},  {2216, 8}, {2604, 8}, {2888, 8},
+  {268, 8}, {656, 8}, {1044, 8}, {1432, 8}, {1820, 8}, {2208, 8}, {2596, 8},
+  {2880, 8}, {260, 8}, {648, 8}, {1036, 8}, {1424, 8}, {1812, 8}, {2200, 8},
+  {2588, 8}, {2872, 8}, {252, 8}, {640, 8}, {1028, 8}, {1416, 8}, {1804, 8},
+  {2192, 8}, {2580, 8}, {2864, 8}, {244, 8}, {632, 8}, {1020, 8}, {1408, 8},
+  {1796, 8}, {2184, 8}, {2572, 8}, {2856, 8}, {236, 8}, {624, 8}, {1012, 8},
+  {1400, 8}, {1788, 8}, {2176, 8}, {2564, 8}, {2848, 8}, {228, 8}, {616, 8},
+  {1004, 8}, {1392, 8}, {1780, 8}, {2168, 8}, {2556, 8}, {2840, 8}, {220, 8},
+  {608, 8}, {996, 8}, {1384, 8}, {1772, 8}, {2160, 8}, {2548, 8}, {2832, 8},
+  {212, 8}, {600, 8}, {988, 8}, {1376, 8}, {1764, 8}, {2152, 8}, {2540, 8},
+  {2824, 8}, {204, 8}, {592, 8}, {980, 8}, {1368, 8}, {1756, 8}, {2144, 8},
+  {2532, 8}, {2816, 8}, {196, 8}, {584, 8}, {972, 8}, {1360, 8}, {1748, 8},
+  {2136, 8}, {2524, 8}, {2808, 8}, {188, 8}, {576, 8}, {964, 8}, {1352, 8},
+  {1740, 8}, {2128, 8}, {2516, 8}, {2800, 8}, {180, 8}, {568, 8}, {956, 8},
+  {1344, 8}, {1732, 8}, {2120, 8}, {2508, 8}, {2792, 8}, {172, 8}, {560, 8},
+  {948, 8}, {1336, 8}, {1724, 8}, {2112, 8}, {2500, 8}, {2784, 8}, {164, 8},
+  {552, 8}, {940, 8}, {1328, 8}, {1716, 8}, {2104, 8}, {2492, 8}, {2776, 8},
+  {156, 8}, {544, 8}, {932, 8}, {1320, 8}, {1708, 8}, {2096, 8}, {2484, 8},
+  {2768, 8}, {148, 8}, {536, 8}, {924, 8}, {1312, 8}, {1700, 8}, {2088, 8},
+  {2476, 8}, {2760, 8}, {140, 8}, {528, 8}, {916, 8}, {1304, 8}, {1692, 8},
+  {2080, 8}, {2468, 8}, {2752, 8}, {132, 8}, {520, 8}, {908, 8}, {1296, 8},
+  {1684, 8}, {2072, 8}, {2460, 8}, {2744, 8}, {124, 8}, {512, 8}, {900, 8},
+  {1288, 8}, {1676, 8}, {2064, 8}, {2452, 8}, {2736, 8}, {116, 8}, {504, 8},
+  {892, 8}, {1280, 8}, {1668, 8}, {2056, 8}, {2444, 8}, {2728, 8}, {108, 8},
+  {496, 8}, {884, 8}, {1272, 8}, {1660, 8}, {2048, 8}, {2436, 8}, {2720, 8},
+  {100, 8}, {488, 8}, {876, 8}, {1264, 8}, {1652, 8}, {2040, 8}, {2428, 8},
+  {2716, 4}, {92, 8}, {480, 8}, {868, 8}, {1256, 8}, {1644, 8}, {2032, 8},
+  {2420, 8}, {84, 8}, {472, 8}, {860, 8}, {1248, 8}, {1636, 8}, {2024, 8},
+  {2412, 8}, {76, 8}, {464, 8}, {852, 8}, {1240, 8}, {1628, 8}, {2016, 8},
+  {2404, 8}, {68, 8}, {456, 8}, {844, 8}, {1232, 8}, {1620, 8}, {2008, 8},
+  {2396, 8}, {60, 8}, {448, 8}, {836, 8}, {1224, 8}, {1612, 8}, {2000, 8},
+  {2388, 8}, {52, 8}, {440, 8}, {828, 8}, {1216, 8}, {1604, 8}, {1992, 8},
+  {2380, 8}, {44, 8}, {432, 8}, {820, 8}, {1208, 8}, {1596, 8}, {1984, 8},
+  {2372, 8}, {36, 8}, {424, 8}, {812, 8}, {1200, 8}, {1588, 8}, {1976, 8},
+  {2364, 8}, {28, 8}, {416, 8}, {804, 8}, {1192, 8}, {1580, 8}, {1968, 8},
+  {2356, 8}, {20, 8}, {408, 8}, {796, 8}, {1184, 8}, {1572, 8}, {1960, 8},
+  {2348, 8}, {12, 8}, {400, 8}, {788, 8}, {1176, 8}, {1564, 8}, {1952, 8},
+  {2340, 8}, {4, 8}, {392, 8}, {780, 8}, {1168, 8}, {1556, 8}, {1944, 8},
+  {2332, 8},
+  /* missing from original data set */
+  {388, 4}, {776, 4}, {1164, 4}, {1552, 4}, {1940, 4}, {2328, 4},
+};
+/* *INDENT-ON* */
+
+int
+pattern_cmp (const void *arg1, const void *arg2)
+{
+  test_pattern_t *a1 = (test_pattern_t *) arg1;
+  test_pattern_t *a2 = (test_pattern_t *) arg2;
+
+  if (a1->offset < a2->offset)
+    return -1;
+  else if (a1->offset > a2->offset)
+    return 1;
+  return 0;
+}
+
+static u8
+fifo_validate_pattern (vlib_main_t * vm, test_pattern_t * pattern,
+		       u32 pattern_length)
+{
+  test_pattern_t *tp = pattern;
+  int i;
+
+  /* Go through the pattern and make 100% sure it's sane */
+  for (i = 0; i < pattern_length - 1; i++)
+    {
+      if (tp->offset + tp->len != (tp + 1)->offset)
+	{
+	  vlib_cli_output (vm, "[%d] missing {%d, %d}", i,
+			   (tp->offset + tp->len),
+			   (tp + 1)->offset - (tp->offset + tp->len));
+	  return 0;
+	}
+      tp++;
+    }
+  return 1;
+}
+
+static test_pattern_t *
+fifo_get_validate_pattern (vlib_main_t * vm, test_pattern_t * test_data,
+			   u32 test_data_len)
+{
+  test_pattern_t *validate_pattern = 0;
+
+  /* Validate, and try segments in order... */
+  vec_validate (validate_pattern, test_data_len - 1);
+  memcpy (validate_pattern, test_data,
+	  test_data_len * sizeof (test_pattern_t));
+  qsort ((u8 *) validate_pattern, test_data_len, sizeof (test_pattern_t),
+	 pattern_cmp);
+
+  if (fifo_validate_pattern (vm, validate_pattern, test_data_len) == 0)
+    return 0;
+
+  return validate_pattern;
+}
+
+int
+tcp_test_fifo1 (vlib_main_t * vm, unformat_input_t * input)
 {
   svm_fifo_t *f;
   u32 fifo_size = 1 << 20;
   u32 *test_data = 0;
   u32 offset;
-  int i, rv;
+  int i, rv, verbose = 0;
   u32 data_word, test_data_len;
+  ooo_segment_t *ooo_seg;
+  u8 *data;
+
+  while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (input, "verbose"))
+	verbose = 1;
+    }
 
-  /* $$$ parse args */
   test_data_len = fifo_size / sizeof (u32);
   vec_validate (test_data, test_data_len - 1);
 
@@ -198,12 +326,8 @@ tcp_test_fifo (vlib_main_t * vm, unformat_input_t * input)
   /* Enqueue an initial (un-dequeued) chunk */
   rv = svm_fifo_enqueue_nowait (f, 0 /* pid */ ,
 				sizeof (u32), (u8 *) test_data);
-
-  if (rv != sizeof (u32))
-    {
-      clib_warning ("enqueue returned %d", rv);
-      goto out;
-    }
+  TCP_TEST ((rv == sizeof (u32)), "enqueued %d", rv);
+  TCP_TEST ((f->tail == 4), "fifo tail %u", f->tail);
 
   /*
    * Create 3 chunks in the future. The offsets are relative
@@ -212,51 +336,62 @@ tcp_test_fifo (vlib_main_t * vm, unformat_input_t * input)
   for (i = 0; i < 3; i++)
     {
       offset = (2 * i + 1) * sizeof (u32);
-      vlib_cli_output (vm, "add offset %d", offset);
-
-      rv = svm_fifo_enqueue_with_offset
-	(f, 0 /* pid */ , offset, sizeof (u32),
-	 (u8 *) (test_data + ((offset + sizeof (u32)) / sizeof (u32))));
-
+      data = (u8 *) (test_data + (2 * i + 1));
+      rv =
+	svm_fifo_enqueue_with_offset (f, 0 /* pid */ , offset, sizeof (u32),
+				      data);
+      if (verbose)
+	vlib_cli_output (vm, "add [%d] [%d, %d]", 2 * i + 1, offset,
+			 offset + sizeof (u32));
       if (rv)
 	{
 	  clib_warning ("enqueue returned %d", rv);
-	  goto out;
+	  goto err;
 	}
     }
 
-  /* Paint missing data backwards */
-  for (i = 3; i > 0; i--)
+  if (verbose)
+    vlib_cli_output (vm, "fifo after odd segs: %U", format_svm_fifo, f, 1);
+  TCP_TEST ((f->tail == 8), "fifo tail %u", f->tail);
+
+  /* Paint some of missing data backwards */
+  for (i = 3; i > 1; i--)
     {
       offset = (2 * i + 0) * sizeof (u32);
-
-      vlib_cli_output (vm, "add offset %d", offset);
-
-      rv = svm_fifo_enqueue_with_offset
-	(f, 0 /* pid */ , offset, sizeof (u32),
-	 (u8 *) (test_data + ((offset + sizeof (u32)) / sizeof (u32))));
-
+      data = (u8 *) (test_data + (2 * i + 0));
+      rv =
+	svm_fifo_enqueue_with_offset (f, 0 /* pid */ , offset, sizeof (u32),
+				      data);
+      if (verbose)
+	vlib_cli_output (vm, "add [%d] [%d, %d]", 2 * i, offset,
+			 offset + sizeof (u32));
       if (rv)
 	{
 	  clib_warning ("enqueue returned %d", rv);
-	  goto out;
+	  goto err;
 	}
     }
 
-  vlib_cli_output (vm, "fifo before missing link: %U",
-		   format_svm_fifo, f, 1 /* verbose */ );
+  if (verbose)
+    vlib_cli_output (vm, "fifo before missing link: %U", format_svm_fifo, f,
+		     1);
+  TCP_TEST ((svm_fifo_number_ooo_segments (f) == 1),
+	    "number of ooo segments %u", svm_fifo_number_ooo_segments (f));
+  ooo_seg = svm_fifo_first_ooo_segment (f);
+  TCP_TEST ((ooo_seg->start == 12),
+	    "first ooo seg position %u", ooo_seg->start);
+  TCP_TEST ((ooo_seg->length == 16),
+	    "first ooo seg length %u", ooo_seg->length);
 
   /* Enqueue the missing u32 */
-  rv = svm_fifo_enqueue_nowait (f, 0 /* pid */ ,
-				sizeof (u32), (u8 *) (test_data + 1));
-  if (rv != 7 * sizeof (u32))
-    {
-      clib_warning ("enqueue returned %d", rv);
-      goto out;
-    }
-
-  vlib_cli_output (vm, "fifo after missing link: %U",
-		   format_svm_fifo, f, 1 /* verbose */ );
+  rv = svm_fifo_enqueue_nowait (f, 0 /* pid */ , sizeof (u32),
+				(u8 *) (test_data + 2));
+  if (verbose)
+    vlib_cli_output (vm, "fifo after missing link: %U", format_svm_fifo, f,
+		     1);
+  TCP_TEST ((rv == 20), "bytes to be enqueued %u", rv);
+  TCP_TEST ((svm_fifo_number_ooo_segments (f) == 0),
+	    "number of ooo segments %u", svm_fifo_number_ooo_segments (f));
 
   /* Collect results */
   for (i = 0; i < 7; i++)
@@ -265,25 +400,316 @@ tcp_test_fifo (vlib_main_t * vm, unformat_input_t * input)
 				    (u8 *) & data_word);
       if (rv != sizeof (u32))
 	{
-	  clib_warning ("dequeue returned %d", rv);
-	  goto out;
+	  clib_warning ("bytes dequeues %u", rv);
+	  goto err;
 	}
       if (data_word != test_data[i])
 	{
-	  clib_warning ("recovered data %d not %d", data_word, test_data[i]);
-	  goto out;
+	  clib_warning ("recovered [%d] %d not %d", i, data_word,
+			test_data[i]);
+	  goto err;
 	}
     }
 
-  clib_warning ("test complete...");
+  svm_fifo_free (f);
+  vec_free (test_data);
+  return 0;
 
-out:
+err:
   svm_fifo_free (f);
   vec_free (test_data);
+  return -1;
+}
+
+static int
+tcp_test_fifo2 (vlib_main_t * vm)
+{
+  svm_fifo_t *f;
+  u32 fifo_size = 1 << 20;
+  int i, rv, test_data_len;
+  u64 data64;
+  test_pattern_t *tp, *vp, *test_data;
+  ooo_segment_t *ooo_seg;
+
+  test_data = test_pattern;
+  test_data_len = ARRAY_LEN (test_pattern);
+
+  vp = fifo_get_validate_pattern (vm, test_data, test_data_len);
+
+  /* Create a fifo */
+  f = svm_fifo_create (fifo_size);
+
+  /* Paint the fifo data vector with -1's */
+  memset (f->data, 0xFF, 1 << 20);
+
+  /*
+   * Try with sorted data
+   */
+  for (i = 0; i < test_data_len; i++)
+    {
+      tp = vp + i;
+      data64 = tp->offset;
+      rv = svm_fifo_enqueue_with_offset (f, 0, tp->offset, tp->len,
+					 (u8 *) & data64);
+    }
+
+  /* Expected result: one big fat chunk at offset 4 */
+  TCP_TEST ((svm_fifo_number_ooo_segments (f) == 1),
+	    "number of ooo segments %u", svm_fifo_number_ooo_segments (f));
+  ooo_seg = svm_fifo_first_ooo_segment (f);
+  TCP_TEST ((ooo_seg->start == 4),
+	    "first ooo seg position %u", ooo_seg->start);
+  TCP_TEST ((ooo_seg->length == 2996),
+	    "first ooo seg length %u", ooo_seg->length);
+
+  data64 = 0;
+  rv = svm_fifo_enqueue_nowait (f, 0, sizeof (u32), (u8 *) & data64);
+  TCP_TEST ((rv == 3000), "bytes to be enqueued %u", rv);
+
+  svm_fifo_free (f);
+  vec_free (vp);
+
+  /*
+   * Now try it again w/ unsorted data...
+   */
+
+  f = svm_fifo_create (fifo_size);
+
+  /* Paint fifo data vector with -1's */
+  memset (f->data, 0xFF, 1 << 20);
+
+  for (i = 0; i < test_data_len; i++)
+    {
+      tp = &test_data[i];
+      data64 = tp->offset;
+      rv = svm_fifo_enqueue_with_offset (f, 0, tp->offset, tp->len,
+					 (u8 *) & data64);
+      if (rv)
+	{
+	  clib_warning ("enqueue returned %d", rv);
+	}
+    }
+
+  /* Expecting the same result: one big fat chunk at offset 4 */
+  TCP_TEST ((svm_fifo_number_ooo_segments (f) == 1),
+	    "number of ooo segments %u", svm_fifo_number_ooo_segments (f));
+  ooo_seg = svm_fifo_first_ooo_segment (f);
+  TCP_TEST ((ooo_seg->start == 4),
+	    "first ooo seg position %u", ooo_seg->start);
+  TCP_TEST ((ooo_seg->length == 2996),
+	    "first ooo seg length %u", ooo_seg->length);
+
+  data64 = 0;
+  rv = svm_fifo_enqueue_nowait (f, 0, sizeof (u32), (u8 *) & data64);
+
+  TCP_TEST ((rv == 3000), "bytes to be enqueued %u", rv);
+
+  svm_fifo_free (f);
+
   return 0;
 }
 
+static int
+tcp_test_fifo3 (vlib_main_t * vm, unformat_input_t * input)
+{
+  svm_fifo_t *f;
+  u32 fifo_size = 4 << 10;
+  u32 fifo_initial_offset = 0;
+  u32 total_size = 2 << 10;
+  int overlap = 0;
+  int i, rv;
+  u8 *data_pattern = 0;
+  test_pattern_t *tp, *generate = 0;
+  u32 nsegs = 2;
+  u32 seg_size, length_so_far;
+  u32 current_offset, offset_increment, len_this_chunk;
+  u32 seed = 0xdeaddabe;
+  int verbose = 0;
+  int randomize = 1;
+
+  while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (input, "fifo-size %d", &fifo_size))
+	;
+      else if (unformat (input, "total-size %d", &total_size))
+	;
+      else if (unformat (input, "verbose"))
+	verbose = 1;
+      else if (unformat (input, "overlap"))
+	overlap = 1;
+      else if (unformat (input, "initial-offset %d", &fifo_initial_offset))
+	;
+      else if (unformat (input, "seed %d", &seed))
+	;
+      else if (unformat (input, "nsegs %d", &nsegs))
+	;
+      else if (unformat (input, "no-randomize"))
+	randomize = 0;
+      else
+	{
+	  clib_error_t *e = clib_error_return
+	    (0, "unknown input `%U'", format_unformat_error, input);
+	  clib_error_report (e);
+	  return -1;
+	}
+    }
 
+  /*
+   * Generate data
+   */
+  vec_validate (data_pattern, total_size - 1);
+  for (i = 0; i < vec_len (data_pattern); i++)
+    data_pattern[i] = i & 0xff;
+
+  seg_size = total_size / nsegs;
+  length_so_far = 0;
+  current_offset = 1;
+  while (length_so_far < total_size)
+    {
+      vec_add2 (generate, tp, 1);
+      len_this_chunk = clib_min (seg_size, total_size - length_so_far);
+      tp->offset = current_offset;
+      tp->len = len_this_chunk;
+
+      if (overlap && (len_this_chunk == seg_size))
+	do
+	  {
+	    offset_increment = len_this_chunk
+	      % (1 + (random_u32 (&seed) % len_this_chunk));
+	  }
+	while (offset_increment == 0);
+      else
+	offset_increment = len_this_chunk;
+
+      current_offset += offset_increment;
+      length_so_far = tp->offset + tp->len;
+    }
+
+  /*
+   * Validate segment list. Only valid for non-overlap cases.
+   */
+  if (overlap == 0)
+    fifo_validate_pattern (vm, generate, vec_len (generate));
+
+  if (verbose)
+    {
+      vlib_cli_output (vm, "raw data pattern:");
+      for (i = 0; i < vec_len (generate); i++)
+	{
+	  vlib_cli_output (vm, "[%d] offset %u len %u", i,
+			   generate[i].offset, generate[i].len);
+	}
+    }
+
+  /* Randomize data pattern */
+  if (randomize)
+    {
+      for (i = 0; i < vec_len (generate) / 2; i++)
+	{
+	  u32 src_index, dst_index;
+	  test_pattern_t _tmp, *tmp = &_tmp;
+
+	  src_index = random_u32 (&seed) % vec_len (generate);
+	  dst_index = random_u32 (&seed) % vec_len (generate);
+
+	  tmp[0] = generate[dst_index];
+	  generate[dst_index] = generate[src_index];
+	  generate[src_index] = tmp[0];
+	}
+    }
+
+  if (verbose)
+    {
+      vlib_cli_output (vm, "randomized data pattern:");
+      for (i = 0; i < vec_len (generate); i++)
+	{
+	  vlib_cli_output (vm, "[%d] offset %u len %u", i,
+			   generate[i].offset, generate[i].len);
+	}
+    }
+
+  /* Create a fifo */
+  f = svm_fifo_create (fifo_size);
+
+  /* Paint the fifo data vector with -1's */
+  memset (f->data, 0xFF, fifo_size);
+
+  /* manually set head and tail pointers to validate modular arithmetic */
+  f->head = fifo_initial_offset % fifo_size;
+  f->tail = fifo_initial_offset % fifo_size;
+
+  for (i = 0; i < vec_len (generate); i++)
+    {
+      tp = generate + i;
+      rv = svm_fifo_enqueue_with_offset (f, 0, tp->offset, tp->len,
+					 (u8 *) data_pattern + tp->offset);
+    }
+
+  /* Expected result: one big fat chunk at offset 1 */
+
+  if (verbose)
+    vlib_cli_output (vm, "fifo before missing link: %U",
+		     format_svm_fifo, f, 1 /* verbose */ );
+
+  rv = svm_fifo_enqueue_nowait (f, 0, 1 /* count */ , data_pattern + 0);
+
+  if (verbose)
+    vlib_cli_output (vm, "in-order enqueue returned %d", rv);
+
+  TCP_TEST ((rv == total_size), "retrieved %u expected %u", rv, total_size);
+  TCP_TEST ((svm_fifo_number_ooo_segments (f) == 0),
+	    "number of ooo segments %u", svm_fifo_number_ooo_segments (f));
+  svm_fifo_free (f);
+  vec_free (data_pattern);
+
+  return 0;
+}
+
+static int
+tcp_test_fifo (vlib_main_t * vm, unformat_input_t * input)
+{
+  int res = 0;
+
+  /* Run all tests */
+  if (unformat_check_input (input) == UNFORMAT_END_OF_INPUT)
+    {
+      res = tcp_test_fifo1 (vm, input);
+      if (res)
+	return res;
+
+      res = tcp_test_fifo2 (vm);
+      if (res)
+	return res;
+
+      /* Run a number of fifo3 configs */
+      unformat_init_cstring (input, "nsegs 3 overlap seed 123");
+      if (tcp_test_fifo3 (vm, input))
+	return -1;
+      unformat_free (input);
+
+      unformat_init_cstring (input, "nsegs 10");
+      if (tcp_test_fifo3 (vm, input))
+	return -1;
+      unformat_free (input);
+    }
+  else
+    {
+      if (unformat (input, "fifo3"))
+	{
+	  res = tcp_test_fifo3 (vm, input);
+	}
+      else if (unformat (input, "fifo2"))
+	{
+	  res = tcp_test_fifo2 (vm);
+	}
+      else if (unformat (input, "fifo1"))
+	{
+	  res = tcp_test_fifo1 (vm, input);
+	}
+    }
+
+  return res;
+}
 
 static clib_error_t *
 tcp_test (vlib_main_t * vm,