2 files changed, 185 insertions, 239 deletions

diff --git a/src/svm/svm_fifo.c b/src/svm/svm_fifo.c
index 41cd752914a..f457128ffaa 100644
--- a/src/svm/svm_fifo.c
+++ b/src/svm/svm_fifo.c
@@ -20,6 +20,80 @@
 #include <svm/svm_fifo.h>
 #include <vppinfra/cpu.h>
 
+CLIB_MARCH_FN (svm_fifo_copy_to_chunk, void, svm_fifo_t * f,
+	       svm_fifo_chunk_t * c, u32 tail_idx, const u8 * src, u32 len,
+	       u8 update_tail)
+{
+  u32 n_chunk;
+
+  ASSERT (tail_idx >= c->start_byte && tail_idx < c->start_byte + c->length);
+
+  tail_idx -= c->start_byte;
+  n_chunk = c->length - tail_idx;
+  if (n_chunk < len)
+    {
+      u32 to_copy = len;
+      clib_memcpy_fast (&c->data[tail_idx], src, n_chunk);
+      while ((to_copy -= n_chunk))
+	{
+	  c = c->next;
+	  n_chunk = clib_min (c->length, to_copy);
+	  clib_memcpy_fast (&c->data[0], src + (len - to_copy), n_chunk);
+	}
+      if (update_tail)
+	f->tail_chunk = c;
+    }
+  else
+    {
+      clib_memcpy_fast (&c->data[tail_idx], src, len);
+    }
+}
+
+CLIB_MARCH_FN (svm_fifo_copy_from_chunk, void, svm_fifo_t * f,
+	       svm_fifo_chunk_t * c, u32 head_idx, u8 * dst, u32 len,
+	       u8 update_head)
+{
+  u32 n_chunk;
+
+  head_idx -= c->start_byte;
+  n_chunk = c->length - head_idx;
+  if (n_chunk < len)
+    {
+      u32 to_copy = len;
+      clib_memcpy_fast (dst, &c->data[head_idx], n_chunk);
+      while ((to_copy -= n_chunk))
+	{
+	  c = c->next;
+	  n_chunk = clib_min (c->length, to_copy);
+	  clib_memcpy_fast (dst + (len - to_copy), &c->data[0], n_chunk);
+	}
+      if (update_head)
+	f->head_chunk = c;
+    }
+  else
+    {
+      clib_memcpy_fast (dst, &c->data[head_idx], len);
+    }
+}
+
+#ifndef CLIB_MARCH_VARIANT
+
+static inline void
+svm_fifo_copy_to_chunk (svm_fifo_t * f, svm_fifo_chunk_t * c, u32 tail_idx,
+			const u8 * src, u32 len, u8 update_tail)
+{
+  CLIB_MARCH_FN_SELECT (svm_fifo_copy_to_chunk) (f, c, tail_idx, src, len,
+						 update_tail);
+}
+
+static inline void
+svm_fifo_copy_from_chunk (svm_fifo_t * f, svm_fifo_chunk_t * c, u32 head_idx,
+			  u8 * dst, u32 len, u8 update_head)
+{
+  CLIB_MARCH_FN_SELECT (svm_fifo_copy_from_chunk) (f, c, head_idx, dst, len,
+						   update_head);
+}
+
 static inline u8
 position_lt (svm_fifo_t * f, u32 a, u32 b, u32 tail)
 {
@@ -54,8 +128,6 @@ ooo_segment_end_pos (svm_fifo_t * f, ooo_segment_t * s)
   return s->start + s->length;
 }
 
-#ifndef CLIB_MARCH_VARIANT
-
 u8 *
 format_ooo_segment (u8 * s, va_list * args)
 {
@@ -241,6 +313,63 @@ svm_fifo_create (u32 data_size_in_bytes)
   return f;
 }
 
+static inline void
+svm_fifo_size_update (svm_fifo_t * f, svm_fifo_chunk_t * c)
+{
+  svm_fifo_chunk_t *prev;
+  u32 add_bytes = 0;
+
+  prev = f->end_chunk;
+  while (c)
+    {
+      c->start_byte = prev->start_byte + prev->length;
+      add_bytes += c->length;
+      prev->next = c;
+      prev = c;
+      c = c->next;
+    }
+  f->end_chunk = prev;
+  prev->next = f->start_chunk;
+  f->size += add_bytes;
+  f->nitems = f->size - 1;
+  f->new_chunks = 0;
+}
+
+static void
+svm_fifo_try_size_update (svm_fifo_t * f, u32 new_head)
+{
+  if (new_head % f->size > f->tail % f->size)
+    return;
+
+  svm_fifo_size_update (f, f->new_chunks);
+  f->flags &= ~SVM_FIFO_F_SIZE_UPDATE;
+}
+
+void
+svm_fifo_add_chunk (svm_fifo_t * f, svm_fifo_chunk_t * c)
+{
+  if (svm_fifo_is_wrapped (f))
+    {
+      if (f->new_chunks)
+	{
+	  svm_fifo_chunk_t *prev;
+
+	  prev = f->new_chunks;
+	  while (prev->next)
+	    prev = prev->next;
+	  prev->next = c;
+	}
+      else
+	{
+	  f->new_chunks = c;
+	}
+      f->flags |= SVM_FIFO_F_SIZE_UPDATE;
+      return;
+    }
+
+  svm_fifo_size_update (f, c);
+}
+
 void
 svm_fifo_free (svm_fifo_t * f)
 {
@@ -252,7 +381,6 @@ svm_fifo_free (svm_fifo_t * f)
       clib_mem_free (f);
     }
 }
-#endif
 
 always_inline ooo_segment_t *
 ooo_segment_new (svm_fifo_t * f, u32 start, u32 length)
@@ -480,12 +608,34 @@ ooo_segment_try_collect (svm_fifo_t * f, u32 n_bytes_enqueued, u32 * tail)
   return bytes;
 }
 
-CLIB_MARCH_FN (svm_fifo_enqueue_nowait, int, svm_fifo_t * f, u32 len,
-	       const u8 * src)
+void
+svm_fifo_overwrite_head (svm_fifo_t * f, u8 * data, u32 len)
 {
-  u32 n_chunk, to_copy, tail, head, free_count, tail_idx;
+  u32 n_chunk;
+  u32 head, tail, head_idx;
   svm_fifo_chunk_t *c;
 
+  ASSERT (len <= f->nitems);
+
+  f_load_head_tail_cons (f, &head, &tail);
+  c = f->head_chunk;
+  head_idx = head % f->size;
+  head_idx -= c->start_byte;
+  n_chunk = c->length - head_idx;
+  if (len <= n_chunk)
+    clib_memcpy_fast (&c->data[head_idx], data, len);
+  else
+    {
+      clib_memcpy_fast (&c->data[head_idx], data, n_chunk);
+      clib_memcpy_fast (&c->next->data[0], data + n_chunk, len - n_chunk);
+    }
+}
+
+int
+svm_fifo_enqueue_nowait (svm_fifo_t * f, u32 len, const u8 * src)
+{
+  u32 tail, head, free_count;
+
   f_load_head_tail_prod (f, &head, &tail);
 
   /* free space in fifo can only increase during enqueue: SPSC */
@@ -497,29 +647,10 @@ CLIB_MARCH_FN (svm_fifo_enqueue_nowait, int, svm_fifo_t * f, u32 len,
     return SVM_FIFO_FULL;
 
   /* number of bytes we're going to copy */
-  to_copy = len = clib_min (free_count, len);
+  len = clib_min (free_count, len);
 
-  c = f->tail_chunk;
-  tail_idx = tail % f->size;
-  ASSERT (tail_idx >= c->start_byte);
-  tail_idx -= c->start_byte;
-  n_chunk = c->length - tail_idx;
-
-  if (n_chunk < to_copy)
-    {
-      clib_memcpy_fast (&c->data[tail_idx], src, n_chunk);
-      while ((to_copy -= n_chunk))
-	{
-	  c = c->next;
-	  n_chunk = clib_min (c->length, to_copy);
-	  clib_memcpy_fast (&c->data[0], src + (len - to_copy), n_chunk);
-	}
-      f->tail_chunk = c;
-    }
-  else
-    {
-      clib_memcpy_fast (&c->data[tail_idx], src, to_copy);
-    }
+  svm_fifo_copy_to_chunk (f, f->tail_chunk, tail % f->size, src, len,
+			  1 /* update tail */ );
   tail += len;
 
   svm_fifo_trace_add (f, head, n_total, 2);
@@ -528,24 +659,12 @@ CLIB_MARCH_FN (svm_fifo_enqueue_nowait, int, svm_fifo_t * f, u32 len,
   if (PREDICT_FALSE (f->ooos_list_head != OOO_SEGMENT_INVALID_INDEX))
     len += ooo_segment_try_collect (f, len, &tail);
 
-  ASSERT (len <= free_count);
-
   /* store-rel: producer owned index (paired with load-acq in consumer) */
   clib_atomic_store_rel_n (&f->tail, tail);
 
   return len;
 }
 
-#ifndef CLIB_MARCH_VARIANT
-int
-svm_fifo_enqueue_nowait (svm_fifo_t * f, u32 max_bytes,
-			 const u8 * copy_from_here)
-{
-  return CLIB_MARCH_FN_SELECT (svm_fifo_enqueue_nowait) (f, max_bytes,
-							 copy_from_here);
-}
-#endif
-
 /**
  * Enqueue a future segment.
  *
@@ -553,11 +672,10 @@ svm_fifo_enqueue_nowait (svm_fifo_t * f, u32 max_bytes,
  * Returns 0 of the entire segment was copied
  * Returns -1 if none of the segment was copied due to lack of space
  */
-CLIB_MARCH_FN (svm_fifo_enqueue_with_offset, int, svm_fifo_t * f,
-	       u32 offset, u32 len, u8 * src)
+int
+svm_fifo_enqueue_with_offset (svm_fifo_t * f, u32 offset, u32 len, u8 * src)
 {
-  u32 to_copy, n_chunk, tail, head, free_count, tail_offset_idx;
-  svm_fifo_chunk_t *c;
+  u32 tail, head, free_count;
 
   f_load_head_tail_prod (f, &head, &tail);
 
@@ -570,75 +688,20 @@ CLIB_MARCH_FN (svm_fifo_enqueue_with_offset, int, svm_fifo_t * f,
 
   f->ooos_newest = OOO_SEGMENT_INVALID_INDEX;
 
-  ASSERT (len < f->nitems);
   svm_fifo_trace_add (f, offset, len, 1);
 
   ooo_segment_add (f, offset, head, tail, len);
 
-  c = f->tail_chunk;
-  tail_offset_idx = (tail + offset) % f->size;
-  tail_offset_idx -= c->start_byte;
-  n_chunk = c->length - tail_offset_idx;
-  to_copy = len;
-
-  if (n_chunk < to_copy)
-    {
-      clib_memcpy_fast (&c->data[tail_offset_idx], src, n_chunk);
-      while ((to_copy -= n_chunk))
-	{
-	  c = c->next;
-	  n_chunk = clib_min (c->length, to_copy);
-	  clib_memcpy_fast (&c->data[0], src + (len - to_copy), n_chunk);
-	}
-    }
-  else
-    {
-      clib_memcpy_fast (&c->data[tail_offset_idx], src, len);
-    }
+  svm_fifo_copy_to_chunk (f, f->tail_chunk, (tail + offset) % f->size, src,
+			  len, 0 /* update tail */ );
 
   return 0;
 }
 
-#ifndef CLIB_MARCH_VARIANT
-
 int
-svm_fifo_enqueue_with_offset (svm_fifo_t * f, u32 offset, u32 required_bytes,
-			      u8 * copy_from_here)
-{
-  return CLIB_MARCH_FN_SELECT (svm_fifo_enqueue_with_offset) (f, offset,
-							      required_bytes,
-							      copy_from_here);
-}
-
-void
-svm_fifo_overwrite_head (svm_fifo_t * f, u8 * data, u32 len)
+svm_fifo_dequeue_nowait (svm_fifo_t * f, u32 len, u8 * dst)
 {
-  u32 n_chunk;
-  u32 head, tail, head_idx;
-  svm_fifo_chunk_t *c;
-
-  ASSERT (len <= f->nitems);
-
-  f_load_head_tail_cons (f, &head, &tail);
-  c = f->head_chunk;
-  head_idx = head % f->size;
-  head_idx -= c->start_byte;
-  n_chunk = c->length - head_idx;
-  if (len <= n_chunk)
-    clib_memcpy_fast (&c->data[head_idx], data, len);
-  else
-    {
-      clib_memcpy_fast (&c->data[head_idx], data, n_chunk);
-      clib_memcpy_fast (&c->next->data[0], data + n_chunk, len - n_chunk);
-    }
-}
-#endif
-
-CLIB_MARCH_FN (svm_fifo_dequeue_nowait, int, svm_fifo_t * f, u32 len,
-	       u8 * dst)
-{
-  u32 to_copy, n_chunk, tail, head, cursize, head_idx;
-  svm_fifo_chunk_t *c;
+  u32 tail, head, cursize;
 
   f_load_head_tail_cons (f, &head, &tail);
 
@@ -648,29 +711,11 @@ CLIB_MARCH_FN (svm_fifo_dequeue_nowait, int, svm_fifo_t * f, u32 len,
   if (PREDICT_FALSE (cursize == 0))
     return -2;			/* nothing in the fifo */
 
-  to_copy = len = clib_min (cursize, len);
-  ASSERT (cursize >= to_copy);
-
-  c = f->head_chunk;
-  head_idx = head % f->size;
-  head_idx -= c->start_byte;
-  n_chunk = c->length - head_idx;
+  len = clib_min (cursize, len);
+  ASSERT (cursize >= len);
 
-  if (n_chunk < to_copy)
-    {
-      clib_memcpy_fast (dst, &c->data[head_idx], n_chunk);
-      while ((to_copy -= n_chunk))
-	{
-	  c = c->next;
-	  n_chunk = clib_min (c->length, to_copy);
-	  clib_memcpy_fast (dst + (len - to_copy), &c->data[0], n_chunk);
-	}
-      f->head_chunk = c;
-    }
-  else
-    {
-      clib_memcpy_fast (dst, &c->data[head_idx], to_copy);
-    }
+  svm_fifo_copy_from_chunk (f, f->head_chunk, head % f->size, dst, len,
+			    1 /* update head */ );
   head += len;
 
   if (PREDICT_FALSE (f->flags & SVM_FIFO_F_SIZE_UPDATE))
@@ -682,21 +727,10 @@ CLIB_MARCH_FN (svm_fifo_dequeue_nowait, int, svm_fifo_t * f, u32 len,
   return len;
 }
 
-#ifndef CLIB_MARCH_VARIANT
-
 int
-svm_fifo_dequeue_nowait (svm_fifo_t * f, u32 max_bytes, u8 * copy_here)
+svm_fifo_peek (svm_fifo_t * f, u32 relative_offset, u32 len, u8 * dst)
 {
-  return CLIB_MARCH_FN_SELECT (svm_fifo_dequeue_nowait) (f, max_bytes,
-							 copy_here);
-}
-#endif
-
-CLIB_MARCH_FN (svm_fifo_peek, int, svm_fifo_t * f, u32 relative_offset,
-	       u32 len, u8 * dst)
-{
-  u32 to_copy, n_chunk, tail, head, cursize, head_idx;
-  svm_fifo_chunk_t *c;
+  u32 tail, head, cursize;
 
   f_load_head_tail_cons (f, &head, &tail);
 
@@ -706,41 +740,14 @@ CLIB_MARCH_FN (svm_fifo_peek, int, svm_fifo_t * f, u32 relative_offset,
   if (PREDICT_FALSE (cursize < relative_offset))
     return -2;			/* nothing in the fifo */
 
-  to_copy = len = clib_min (cursize - relative_offset, len);
+  len = clib_min (cursize - relative_offset, len);
 
-  c = f->head_chunk;
-  head_idx = (head + relative_offset) % f->size;
-  head_idx -= c->start_byte;
-  n_chunk = c->length - head_idx;
-
-  if (n_chunk < to_copy)
-    {
-      clib_memcpy_fast (dst, &c->data[head_idx], n_chunk);
-      while ((to_copy -= n_chunk))
-	{
-	  c = c->next;
-	  n_chunk = clib_min (c->length, to_copy);
-	  clib_memcpy_fast (dst + (len - to_copy), &c->data[0], n_chunk);
-	}
-      f->head_chunk = c;
-    }
-  else
-    {
-      clib_memcpy_fast (dst, &c->data[head_idx], to_copy);
-    }
+  svm_fifo_copy_from_chunk (f, f->head_chunk,
+			    (head + relative_offset) % f->size, dst, len,
+			    0 /* update head */ );
   return len;
 }
 
-#ifndef CLIB_MARCH_VARIANT
-
-int
-svm_fifo_peek (svm_fifo_t * f, u32 relative_offset, u32 max_bytes,
-	       u8 * copy_here)
-{
-  return CLIB_MARCH_FN_SELECT (svm_fifo_peek) (f, relative_offset, max_bytes,
-					       copy_here);
-}
-
 int
 svm_fifo_dequeue_drop (svm_fifo_t * f, u32 max_bytes)
 {
diff --git a/src/svm/svm_fifo.h b/src/svm/svm_fifo.h
index 39f052fb9fd..82d2b391ab6 100644
--- a/src/svm/svm_fifo.h
+++ b/src/svm/svm_fifo.h
@@ -21,11 +21,8 @@
 
 #include <vppinfra/clib.h>
 #include <vppinfra/vec.h>
-#include <vppinfra/mheap.h>
-#include <vppinfra/heap.h>
 #include <vppinfra/pool.h>
 #include <vppinfra/format.h>
-#include <pthread.h>
 
 /** Out-of-order segment */
 typedef struct
@@ -37,9 +34,6 @@ typedef struct
   u32 length;	/**< Length of segment */
 } ooo_segment_t;
 
-format_function_t format_ooo_segment;
-format_function_t format_ooo_list;
-
 #define SVM_FIFO_TRACE 			(0)
 #define OOO_SEGMENT_INVALID_INDEX 	((u32)~0)
 #define SVM_FIFO_INVALID_SESSION_INDEX 	((u32)~0)
@@ -264,6 +258,14 @@ svm_fifo_is_empty (svm_fifo_t * f)
   return (svm_fifo_max_dequeue (f) == 0);
 }
 
+static inline u8
+svm_fifo_is_wrapped (svm_fifo_t * f)
+{
+  u32 head, tail;
+  f_load_head_tail_all_acq (f, &head, &tail);
+  return head % f->size > tail % f->size;
+}
+
 /* used by producer*/
 static inline u32
 svm_fifo_max_enqueue_prod (svm_fifo_t * f)
@@ -325,6 +327,7 @@ svm_fifo_unset_event (svm_fifo_t * f)
 
 svm_fifo_t *svm_fifo_create (u32 data_size_in_bytes);
 void svm_fifo_init (svm_fifo_t * f, u32 size);
+void svm_fifo_add_chunk (svm_fifo_t * f, svm_fifo_chunk_t * c);
 void svm_fifo_free (svm_fifo_t * f);
 
 int svm_fifo_enqueue_nowait (svm_fifo_t * f, u32 max_bytes,
@@ -520,70 +523,6 @@ ooo_segment_next (svm_fifo_t * f, ooo_segment_t * s)
   return pool_elt_at_index (f->ooo_segments, s->next);
 }
 
-static inline u8
-svm_fifo_is_wrapped (svm_fifo_t * f)
-{
-  u32 head, tail;
-  f_load_head_tail_all_acq (f, &head, &tail);
-  return head % f->size > tail % f->size;
-}
-
-static inline void
-svm_fifo_size_update (svm_fifo_t * f, svm_fifo_chunk_t * c)
-{
-  svm_fifo_chunk_t *prev;
-  u32 add_bytes = 0;
-
-  prev = f->end_chunk;
-  while (c)
-    {
-      c->start_byte = prev->start_byte + prev->length;
-      add_bytes += c->length;
-      prev->next = c;
-      prev = c;
-      c = c->next;
-    }
-  f->end_chunk = prev;
-  prev->next = f->start_chunk;
-  f->size += add_bytes;
-  f->nitems = f->size - 1;
-  f->new_chunks = 0;
-}
-
-static inline void
-svm_fifo_add_chunk (svm_fifo_t * f, svm_fifo_chunk_t * c)
-{
-  if (svm_fifo_is_wrapped (f))
-    {
-      if (f->new_chunks)
-	{
-	  svm_fifo_chunk_t *prev;
-
-	  prev = f->new_chunks;
-	  while (prev->next)
-	    prev = prev->next;
-	  prev->next = c;
-	}
-      else
-	{
-	  f->new_chunks = c;
-	}
-      f->flags |= SVM_FIFO_F_SIZE_UPDATE;
-      return;
-    }
-
-  svm_fifo_size_update (f, c);
-}
-
-static inline void
-svm_fifo_try_size_update (svm_fifo_t * f, u32 new_head)
-{
-  if (new_head % f->size > f->tail % f->size)
-    return;
-
-  svm_fifo_size_update (f, f->new_chunks);
-  f->flags &= ~SVM_FIFO_F_SIZE_UPDATE;
-}
 #endif /* __included_ssvm_fifo_h__ */
 
 /*