src/svm/svm_fifo.c

   1 /*
   2  * Copyright (c) 2016-2019 Cisco and/or its affiliates.
   3  * Copyright (c) 2019 Arm Limited
   4  * Copyright (c) 2010-2017 Intel Corporation and/or its affiliates.
   5  * Copyright (c) 2007-2009 Kip Macy kmacy@freebsd.org
   6  * Inspired from DPDK rte_ring.h (SPSC only) (derived from freebsd bufring.h).
   7  * Licensed under the Apache License, Version 2.0 (the "License");
   8  * you may not use this file except in compliance with the License.
   9  * You may obtain a copy of the License at:
  10  *
  11  *     http://www.apache.org/licenses/LICENSE-2.0
  12  *
  13  * Unless required by applicable law or agreed to in writing, software
  14  * distributed under the License is distributed on an "AS IS" BASIS,
  15  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  16  * See the License for the specific language governing permissions and
  17  * limitations under the License.
  18  */
  19
  20 #include <svm/svm_fifo.h>
  21 #include <svm/fifo_segment.h>
  22 #include <vppinfra/cpu.h>
  23
  24 #define F_INVALID_CPTR (svm_fifo_chunk_ptr_t) ~0ULL
  25
  26 CLIB_MARCH_FN (svm_fifo_copy_to_chunk, void, svm_fifo_t *f,
  27                svm_fifo_chunk_t *c, u32 tail_idx, const u8 *src, u32 len,
  28                svm_fifo_chunk_ptr_t *last)
  29 {
  30   u32 n_chunk;
  31
  32   ASSERT (f_pos_geq (tail_idx, c->start_byte)
  33           && f_pos_lt (tail_idx, c->start_byte + c->length));
  34
  35   tail_idx -= c->start_byte;
  36   n_chunk = c->length - tail_idx;
  37   if (n_chunk <= len)
  38     {
  39       u32 to_copy = len;
  40       clib_memcpy_fast (&c->data[tail_idx], src, n_chunk);
  41       c = f_cptr (f, c->next);
  42       while ((to_copy -= n_chunk))
  43         {
  44           n_chunk = clib_min (c->length, to_copy);
  45           clib_memcpy_fast (&c->data[0], src + (len - to_copy), n_chunk);
  46           c = c->length <= to_copy ? f_cptr (f, c->next) : c;
  47         }
  48       if (*last)
  49         *last = f_csptr (f, c);
  50     }
  51   else
  52     {
  53       clib_memcpy_fast (&c->data[tail_idx], src, len);
  54     }
  55 }
  56
  57 CLIB_MARCH_FN (svm_fifo_copy_from_chunk, void, svm_fifo_t *f,
  58                svm_fifo_chunk_t *c, u32 head_idx, u8 *dst, u32 len,
  59                svm_fifo_chunk_ptr_t *last)
  60 {
  61   u32 n_chunk;
  62
  63   ASSERT (f_pos_geq (head_idx, c->start_byte)
  64           && f_pos_lt (head_idx, c->start_byte + c->length));
  65
  66   head_idx -= c->start_byte;
  67   n_chunk = c->length - head_idx;
  68   if (n_chunk <= len)
  69     {
  70       u32 to_copy = len;
  71       clib_memcpy_fast (dst, &c->data[head_idx], n_chunk);
  72       c = f_cptr (f, c->next);
  73       while ((to_copy -= n_chunk))
  74         {
  75           CLIB_MEM_UNPOISON (c, sizeof (*c));
  76           CLIB_MEM_UNPOISON (c->data, c->length);
  77           n_chunk = clib_min (c->length, to_copy);
  78           clib_memcpy_fast (dst + (len - to_copy), &c->data[0], n_chunk);
  79           c = c->length <= to_copy ? f_cptr (f, c->next) : c;
  80         }
  81       if (*last)
  82         *last = f_csptr (f, c);
  83     }
  84   else
  85     {
  86       clib_memcpy_fast (dst, &c->data[head_idx], len);
  87     }
  88 }
  89
  90 #ifndef CLIB_MARCH_VARIANT
  91
  92 static inline void
  93 svm_fifo_copy_to_chunk (svm_fifo_t *f, svm_fifo_chunk_t *c, u32 tail_idx,
  94                         const u8 *src, u32 len, svm_fifo_chunk_ptr_t *last)
  95 {
  96   CLIB_MARCH_FN_SELECT (svm_fifo_copy_to_chunk) (f, c, tail_idx, src, len,
  97                                                  last);
  98 }
  99
 100 static inline void
 101 svm_fifo_copy_from_chunk (svm_fifo_t *f, svm_fifo_chunk_t *c, u32 head_idx,
 102                           u8 *dst, u32 len, svm_fifo_chunk_ptr_t *last)
 103 {
 104   CLIB_MARCH_FN_SELECT (svm_fifo_copy_from_chunk) (f, c, head_idx, dst, len,
 105                                                    last);
 106 }
 107
 108 static inline u32
 109 ooo_segment_end_pos (ooo_segment_t * s)
 110 {
 111   return (s->start + s->length);
 112 }
 113
 114 void
 115 svm_fifo_free_ooo_data (svm_fifo_t * f)
 116 {
 117   pool_free (f->ooo_segments);
 118 }
 119
 120 static inline ooo_segment_t *
 121 ooo_segment_prev (svm_fifo_t * f, ooo_segment_t * s)
 122 {
 123   if (s->prev == OOO_SEGMENT_INVALID_INDEX)
 124     return 0;
 125   return pool_elt_at_index (f->ooo_segments, s->prev);
 126 }
 127
 128 static inline ooo_segment_t *
 129 ooo_segment_next (svm_fifo_t * f, ooo_segment_t * s)
 130 {
 131   if (s->next == OOO_SEGMENT_INVALID_INDEX)
 132     return 0;
 133   return pool_elt_at_index (f->ooo_segments, s->next);
 134 }
 135
 136 static inline ooo_segment_t *
 137 ooo_segment_alloc (svm_fifo_t * f, u32 start, u32 length)
 138 {
 139   ooo_segment_t *s;
 140
 141   pool_get (f->ooo_segments, s);
 142
 143   s->start = start;
 144   s->length = length;
 145   s->prev = s->next = OOO_SEGMENT_INVALID_INDEX;
 146
 147   return s;
 148 }
 149
 150 static inline void
 151 ooo_segment_free (svm_fifo_t * f, u32 index)
 152 {
 153   ooo_segment_t *cur, *prev = 0, *next = 0;
 154   cur = pool_elt_at_index (f->ooo_segments, index);
 155
 156   if (cur->next != OOO_SEGMENT_INVALID_INDEX)
 157     {
 158       next = pool_elt_at_index (f->ooo_segments, cur->next);
 159       next->prev = cur->prev;
 160     }
 161
 162   if (cur->prev != OOO_SEGMENT_INVALID_INDEX)
 163     {
 164       prev = pool_elt_at_index (f->ooo_segments, cur->prev);
 165       prev->next = cur->next;
 166     }
 167   else
 168     {
 169       f->ooos_list_head = cur->next;
 170     }
 171
 172   pool_put (f->ooo_segments, cur);
 173 }
 174
 175 /**
 176  * Add segment to fifo's out-of-order segment list. Takes care of merging
 177  * adjacent segments and removing overlapping ones.
 178  */
 179 static void
 180 ooo_segment_add (svm_fifo_t * f, u32 offset, u32 head, u32 tail, u32 length)
 181 {
 182   ooo_segment_t *s, *new_s, *prev, *next, *it;
 183   u32 new_index, s_end_pos, s_index;
 184   u32 offset_pos, offset_end_pos;
 185
 186   ASSERT (offset + length <= f_free_count (f, head, tail));
 187
 188   offset_pos = tail + offset;
 189   offset_end_pos = tail + offset + length;
 190
 191   f->ooos_newest = OOO_SEGMENT_INVALID_INDEX;
 192
 193   if (f->ooos_list_head == OOO_SEGMENT_INVALID_INDEX)
 194     {
 195       s = ooo_segment_alloc (f, offset_pos, length);
 196       f->ooos_list_head = s - f->ooo_segments;
 197       f->ooos_newest = f->ooos_list_head;
 198       return;
 199     }
 200
 201   /* Find first segment that starts after new segment */
 202   s = pool_elt_at_index (f->ooo_segments, f->ooos_list_head);
 203   while (s->next != OOO_SEGMENT_INVALID_INDEX
 204          && f_pos_lt (s->start, offset_pos))
 205     s = pool_elt_at_index (f->ooo_segments, s->next);
 206
 207   /* If we have a previous and we overlap it, use it as starting point */
 208   prev = ooo_segment_prev (f, s);
 209   if (prev && f_pos_leq (offset_pos, ooo_segment_end_pos (prev)))
 210     {
 211       s = prev;
 212       s_end_pos = ooo_segment_end_pos (s);
 213
 214       /* Since we have previous, offset start position cannot be smaller
 215        * than prev->start. Check tail */
 216       ASSERT (f_pos_lt (s->start, offset_pos));
 217       goto check_tail;
 218     }
 219
 220   s_index = s - f->ooo_segments;
 221   s_end_pos = ooo_segment_end_pos (s);
 222
 223   /* No overlap, add before current segment */
 224   if (f_pos_lt (offset_end_pos, s->start))
 225     {
 226       new_s = ooo_segment_alloc (f, offset_pos, length);
 227       new_index = new_s - f->ooo_segments;
 228
 229       /* Pool might've moved, get segment again */
 230       s = pool_elt_at_index (f->ooo_segments, s_index);
 231       if (s->prev != OOO_SEGMENT_INVALID_INDEX)
 232         {
 233           new_s->prev = s->prev;
 234           prev = pool_elt_at_index (f->ooo_segments, new_s->prev);
 235           prev->next = new_index;
 236         }
 237       else
 238         {
 239           /* New head */
 240           f->ooos_list_head = new_index;
 241         }
 242
 243       new_s->next = s_index;
 244       s->prev = new_index;
 245       f->ooos_newest = new_index;
 246       return;
 247     }
 248   /* No overlap, add after current segment */
 249   else if (f_pos_gt (offset_pos, s_end_pos))
 250     {
 251       new_s = ooo_segment_alloc (f, offset_pos, length);
 252       new_index = new_s - f->ooo_segments;
 253
 254       /* Pool might've moved, get segment again */
 255       s = pool_elt_at_index (f->ooo_segments, s_index);
 256
 257       /* Needs to be last */
 258       ASSERT (s->next == OOO_SEGMENT_INVALID_INDEX);
 259
 260       new_s->prev = s_index;
 261       s->next = new_index;
 262       f->ooos_newest = new_index;
 263
 264       return;
 265     }
 266
 267   /*
 268    * Merge needed
 269    */
 270
 271   /* Merge at head */
 272   if (f_pos_lt (offset_pos, s->start))
 273     {
 274       s->start = offset_pos;
 275       s->length = s_end_pos - s->start;
 276       f->ooos_newest = s - f->ooo_segments;
 277     }
 278
 279 check_tail:
 280
 281   /* Overlapping tail */
 282   if (f_pos_gt (offset_end_pos, s_end_pos))
 283     {
 284       s->length = offset_end_pos - s->start;
 285
 286       /* Remove the completely overlapped segments in the tail */
 287       it = ooo_segment_next (f, s);
 288       while (it && f_pos_leq (ooo_segment_end_pos (it), offset_end_pos))
 289         {
 290           next = ooo_segment_next (f, it);
 291           ooo_segment_free (f, it - f->ooo_segments);
 292           it = next;
 293         }
 294
 295       /* If partial overlap with last, merge */
 296       if (it && f_pos_leq (it->start, offset_end_pos))
 297         {
 298           s->length = ooo_segment_end_pos (it) - s->start;
 299           ooo_segment_free (f, it - f->ooo_segments);
 300         }
 301       f->ooos_newest = s - f->ooo_segments;
 302     }
 303 }
 304
 305 /**
 306  * Removes segments that can now be enqueued because the fifo's tail has
 307  * advanced. Returns the number of bytes added to tail.
 308  */
 309 static int
 310 ooo_segment_try_collect (svm_fifo_t * f, u32 n_bytes_enqueued, u32 * tail)
 311 {
 312   u32 s_index, bytes = 0;
 313   ooo_segment_t *s;
 314   i32 diff;
 315
 316   s = pool_elt_at_index (f->ooo_segments, f->ooos_list_head);
 317   diff = *tail - s->start;
 318
 319   ASSERT (diff != n_bytes_enqueued);
 320
 321   if (diff > n_bytes_enqueued)
 322     return 0;
 323
 324   /* If last tail update overlaps one/multiple ooo segments, remove them */
 325   while (0 <= diff && diff < n_bytes_enqueued)
 326     {
 327       s_index = s - f->ooo_segments;
 328
 329       /* Segment end is beyond the tail. Advance tail and remove segment */
 330       if (s->length > diff)
 331         {
 332           bytes = s->length - diff;
 333           *tail = *tail + bytes;
 334           ooo_segment_free (f, s_index);
 335           break;
 336         }
 337
 338       /* If we have next go on */
 339       if (s->next != OOO_SEGMENT_INVALID_INDEX)
 340         {
 341           s = pool_elt_at_index (f->ooo_segments, s->next);
 342           diff = *tail - s->start;
 343           ooo_segment_free (f, s_index);
 344         }
 345       /* End of search */
 346       else
 347         {
 348           ooo_segment_free (f, s_index);
 349           break;
 350         }
 351     }
 352
 353   ASSERT (bytes <= f->shr->size);
 354   return bytes;
 355 }
 356
 357 __clib_unused static ooo_segment_t *
 358 ooo_segment_last (svm_fifo_t * f)
 359 {
 360   ooo_segment_t *s;
 361
 362   if (f->ooos_list_head == OOO_SEGMENT_INVALID_INDEX)
 363     return 0;
 364
 365   s = svm_fifo_first_ooo_segment (f);
 366   while (s->next != OOO_SEGMENT_INVALID_INDEX)
 367     s = pool_elt_at_index (f->ooo_segments, s->next);
 368   return s;
 369 }
 370
 371 void
 372 svm_fifo_init (svm_fifo_t * f, u32 size)
 373 {
 374   svm_fifo_chunk_t *c, *prev;
 375   u32 min_alloc;
 376
 377   f->shr->size = size;
 378   f->ooos_list_head = OOO_SEGMENT_INVALID_INDEX;
 379   f->segment_index = SVM_FIFO_INVALID_INDEX;
 380   f->refcnt = 1;
 381   f->shr->head = f->shr->tail = f->flags = 0;
 382   f->shr->head_chunk = f->shr->tail_chunk = f->shr->start_chunk;
 383   f->ooo_deq = f->ooo_enq = 0;
 384
 385   min_alloc = size > 32 << 10 ? size >> 3 : 4096;
 386   min_alloc = clib_min (min_alloc, 64 << 10);
 387   f->shr->min_alloc = min_alloc;
 388
 389   /*
 390    * Initialize chunks
 391    */
 392   prev = f_start_cptr (f);
 393   prev->start_byte = 0;
 394   prev->enq_rb_index = prev->deq_rb_index = RBTREE_TNIL_INDEX;
 395   c = f_cptr (f, prev->next);
 396
 397   while (c)
 398     {
 399       c->start_byte = prev->start_byte + prev->length;
 400       c->enq_rb_index = c->deq_rb_index = RBTREE_TNIL_INDEX;
 401       ASSERT (c->length >= 1 << FS_MIN_LOG2_CHUNK_SZ);
 402       prev = c;
 403       c = f_cptr (f, c->next);
 404     }
 405 }
 406
 407 void
 408 svm_fifo_init_ooo_lookup (svm_fifo_t * f, u8 ooo_type)
 409 {
 410   if (ooo_type == 0)
 411     {
 412       ASSERT (!rb_tree_is_init (&f->ooo_enq_lookup));
 413       rb_tree_init (&f->ooo_enq_lookup);
 414     }
 415   else
 416     {
 417       ASSERT (!rb_tree_is_init (&f->ooo_deq_lookup));
 418       rb_tree_init (&f->ooo_deq_lookup);
 419     }
 420 }
 421
 422 /**
 423  * Creates a fifo in the current heap. Fails vs blow up the process
 424  */
 425 svm_fifo_t *
 426 svm_fifo_alloc (u32 data_size_in_bytes)
 427 {
 428   u32 rounded_data_size;
 429   svm_fifo_chunk_t *c;
 430   svm_fifo_t *f;
 431
 432   f = clib_mem_alloc_aligned_or_null (sizeof (*f), CLIB_CACHE_LINE_BYTES);
 433   if (f == 0)
 434     return 0;
 435
 436   clib_memset (f, 0, sizeof (*f));
 437
 438   /* always round fifo data size to the next highest power-of-two */
 439   rounded_data_size = (1 << (max_log2 (data_size_in_bytes)));
 440   c = clib_mem_alloc_aligned_or_null (sizeof (*c) + rounded_data_size,
 441                                       CLIB_CACHE_LINE_BYTES);
 442   if (!c)
 443     {
 444       clib_mem_free (f);
 445       return 0;
 446     }
 447
 448   clib_memset (c, 0, sizeof (*c));
 449   c->start_byte = 0;
 450   c->length = data_size_in_bytes;
 451   c->enq_rb_index = RBTREE_TNIL_INDEX;
 452   c->deq_rb_index = RBTREE_TNIL_INDEX;
 453   f->shr->start_chunk = f->shr->end_chunk = f_csptr (f, c);
 454
 455   return f;
 456 }
 457
 458 /**
 459  * Creates a fifo chunk in the current heap
 460  */
 461 svm_fifo_chunk_t *
 462 svm_fifo_chunk_alloc (u32 size)
 463 {
 464   svm_fifo_chunk_t *c;
 465   u32 rounded_size;
 466
 467   /* round chunk size to the next highest power-of-two */
 468   rounded_size = (1 << (max_log2 (size)));
 469   c = clib_mem_alloc_aligned_or_null (sizeof (*c) + rounded_size,
 470                                       CLIB_CACHE_LINE_BYTES);
 471   if (c == 0)
 472     return 0;
 473
 474   clib_memset (c, 0, sizeof (*c));
 475   c->length = rounded_size;
 476   return c;
 477 }
 478
 479 /**
 480  * Find chunk for given byte position
 481  *
 482  * @param f     fifo
 483  * @param pos   normalized position in fifo
 484  *
 485  * @return chunk that includes given position or 0
 486  */
 487 static svm_fifo_chunk_t *
 488 svm_fifo_find_chunk (svm_fifo_t * f, u32 pos)
 489 {
 490   svm_fifo_chunk_t *c;
 491
 492   c = f_start_cptr (f);
 493   while (c && !f_chunk_includes_pos (c, pos))
 494     c = f_cptr (f, c->next);
 495
 496   return c;
 497 }
 498
 499 static svm_fifo_chunk_t *
 500 svm_fifo_find_next_chunk (svm_fifo_t * f, svm_fifo_chunk_t * start, u32 pos)
 501 {
 502   svm_fifo_chunk_t *c;
 503
 504   ASSERT (start != 0);
 505
 506   c = start;
 507   while (c && !f_chunk_includes_pos (c, pos))
 508     c = f_cptr (f, c->next);
 509
 510   return c;
 511 }
 512
 513 u32
 514 svm_fifo_max_read_chunk (svm_fifo_t * f)
 515 {
 516   u32 head, tail, end_chunk;
 517
 518   f_load_head_tail_cons (f, &head, &tail);
 519   ASSERT (!f->shr->head_chunk || f_chunk_includes_pos (f_head_cptr (f), head));
 520
 521   if (!f->shr->head_chunk)
 522     {
 523       f->shr->head_chunk = f_csptr (f, svm_fifo_find_chunk (f, head));
 524       if (PREDICT_FALSE (!f->shr->head_chunk))
 525         return 0;
 526     }
 527
 528   end_chunk = f_chunk_end (f_head_cptr (f));
 529
 530   return f_pos_lt (end_chunk, tail) ? end_chunk - head : tail - head;
 531 }
 532
 533 u32
 534 svm_fifo_max_write_chunk (svm_fifo_t * f)
 535 {
 536   svm_fifo_chunk_t *tail_chunk;
 537   u32 head, tail;
 538
 539   f_load_head_tail_prod (f, &head, &tail);
 540   tail_chunk = f_tail_cptr (f);
 541
 542   ASSERT (!tail_chunk || f_chunk_includes_pos (tail_chunk, tail));
 543
 544   return tail_chunk ? f_chunk_end (tail_chunk) - tail : 0;
 545 }
 546
 547 static rb_node_t *
 548 f_find_node_rbtree (rb_tree_t * rt, u32 pos)
 549 {
 550   rb_node_t *cur, *prev;
 551
 552   cur = rb_node (rt, rt->root);
 553   if (PREDICT_FALSE (rb_node_is_tnil (rt, cur)))
 554     return 0;
 555
 556   while (pos != cur->key)
 557     {
 558       prev = cur;
 559       if (f_pos_lt (pos, cur->key))
 560         {
 561           cur = rb_node_left (rt, cur);
 562           if (rb_node_is_tnil (rt, cur))
 563             {
 564               cur = rb_tree_predecessor (rt, prev);
 565               break;
 566             }
 567         }
 568       else
 569         {
 570           cur = rb_node_right (rt, cur);
 571           if (rb_node_is_tnil (rt, cur))
 572             {
 573               cur = prev;
 574               break;
 575             }
 576         }
 577     }
 578
 579   if (rb_node_is_tnil (rt, cur))
 580     return 0;
 581
 582   return cur;
 583 }
 584
 585 static svm_fifo_chunk_t *
 586 f_find_chunk_rbtree (rb_tree_t * rt, u32 pos)
 587 {
 588   svm_fifo_chunk_t *c;
 589   rb_node_t *n;
 590
 591   if (!rb_tree_is_init (rt))
 592     return 0;
 593
 594   n = f_find_node_rbtree (rt, pos);
 595   if (!n)
 596     return 0;
 597   c = uword_to_pointer (n->opaque, svm_fifo_chunk_t *);
 598   if (f_chunk_includes_pos (c, pos))
 599     return c;
 600
 601   return 0;
 602 }
 603
 604 static void
 605 f_update_ooo_enq (svm_fifo_t * f, u32 start_pos, u32 end_pos)
 606 {
 607   rb_tree_t *rt = &f->ooo_enq_lookup;
 608   svm_fifo_chunk_t *c;
 609   rb_node_t *cur;
 610
 611   /* Use linear search if rbtree is not initialized */
 612   if (PREDICT_FALSE (!rb_tree_is_init (rt)))
 613     {
 614       f->ooo_enq = svm_fifo_find_next_chunk (f, f_tail_cptr (f), start_pos);
 615       return;
 616     }
 617
 618   if (rt->root == RBTREE_TNIL_INDEX)
 619     {
 620       c = f_tail_cptr (f);
 621       ASSERT (c->enq_rb_index == RBTREE_TNIL_INDEX);
 622       c->enq_rb_index = rb_tree_add_custom (rt, c->start_byte,
 623                                             pointer_to_uword (c), f_pos_lt);
 624     }
 625   else
 626     {
 627       cur = f_find_node_rbtree (rt, start_pos);
 628       c = uword_to_pointer (cur->opaque, svm_fifo_chunk_t *);
 629       ASSERT (f_pos_leq (c->start_byte, start_pos));
 630     }
 631
 632   if (f_chunk_includes_pos (c, start_pos))
 633     f->ooo_enq = c;
 634
 635   if (f_chunk_includes_pos (c, end_pos))
 636     return;
 637
 638   do
 639     {
 640       c = f_cptr (f, c->next);
 641       if (!c || c->enq_rb_index != RBTREE_TNIL_INDEX)
 642         break;
 643
 644       c->enq_rb_index = rb_tree_add_custom (rt, c->start_byte,
 645                                             pointer_to_uword (c), f_pos_lt);
 646
 647       if (f_chunk_includes_pos (c, start_pos))
 648         f->ooo_enq = c;
 649     }
 650   while (!f_chunk_includes_pos (c, end_pos));
 651 }
 652
 653 static void
 654 f_update_ooo_deq (svm_fifo_t * f, u32 start_pos, u32 end_pos)
 655 {
 656   rb_tree_t *rt = &f->ooo_deq_lookup;
 657   rb_node_t *cur;
 658   svm_fifo_chunk_t *c;
 659
 660   /* Use linear search if rbtree is not initialized  */
 661   if (PREDICT_FALSE (!rb_tree_is_init (rt)))
 662     {
 663       f->ooo_deq = svm_fifo_find_chunk (f, start_pos);
 664       return;
 665     }
 666
 667   if (rt->root == RBTREE_TNIL_INDEX)
 668     {
 669       c = f_start_cptr (f);
 670       ASSERT (c->deq_rb_index == RBTREE_TNIL_INDEX);
 671       c->deq_rb_index = rb_tree_add_custom (rt, c->start_byte,
 672                                             pointer_to_uword (c), f_pos_lt);
 673     }
 674   else
 675     {
 676       cur = f_find_node_rbtree (rt, start_pos);
 677       c = uword_to_pointer (cur->opaque, svm_fifo_chunk_t *);
 678       ASSERT (f_pos_leq (c->start_byte, start_pos));
 679     }
 680
 681   if (f_chunk_includes_pos (c, start_pos))
 682     f->ooo_deq = c;
 683
 684   if (f_chunk_includes_pos (c, end_pos))
 685     return;
 686
 687   do
 688     {
 689       c = f_cptr (f, c->next);
 690       if (!c || c->deq_rb_index != RBTREE_TNIL_INDEX)
 691         break;
 692
 693       c->deq_rb_index = rb_tree_add_custom (rt, c->start_byte,
 694                                             pointer_to_uword (c), f_pos_lt);
 695
 696       if (f_chunk_includes_pos (c, start_pos))
 697         f->ooo_deq = c;
 698     }
 699   while (!f_chunk_includes_pos (c, end_pos));
 700 }
 701
 702 static svm_fifo_chunk_t *
 703 f_lookup_clear_enq_chunks (svm_fifo_t * f, svm_fifo_chunk_t * start,
 704                            u32 end_pos)
 705 {
 706   rb_tree_t *rt = &f->ooo_enq_lookup;
 707   svm_fifo_chunk_t *c;
 708   rb_node_t *n;
 709
 710   c = start;
 711   while (c && !f_chunk_includes_pos (c, end_pos))
 712     {
 713       if (c->enq_rb_index != RBTREE_TNIL_INDEX)
 714         {
 715           n = rb_node (rt, c->enq_rb_index);
 716           rb_tree_del_node (rt, n);
 717           c->enq_rb_index = RBTREE_TNIL_INDEX;
 718         }
 719
 720       c = f_cptr (f, c->next);
 721     }
 722
 723   /* No ooo segments left, so make sure the current chunk
 724    * is not tracked in the enq rbtree */
 725   if (f->ooos_list_head == OOO_SEGMENT_INVALID_INDEX
 726       && c && c->enq_rb_index != RBTREE_TNIL_INDEX)
 727     {
 728       n = rb_node (rt, c->enq_rb_index);
 729       rb_tree_del_node (rt, n);
 730       c->enq_rb_index = RBTREE_TNIL_INDEX;
 731     }
 732
 733   return c;
 734 }
 735
 736 static svm_fifo_chunk_t *
 737 f_lookup_clear_deq_chunks (svm_fifo_t * f, svm_fifo_chunk_t * start,
 738                            u32 end_pos)
 739 {
 740   rb_tree_t *rt = &f->ooo_deq_lookup;
 741   svm_fifo_chunk_t *c;
 742   rb_node_t *n;
 743
 744   c = start;
 745   while (c && !f_chunk_includes_pos (c, end_pos))
 746     {
 747       if (c->deq_rb_index != RBTREE_TNIL_INDEX)
 748         {
 749           n = rb_node (rt, c->deq_rb_index);
 750           rb_tree_del_node (rt, n);
 751           c->deq_rb_index = RBTREE_TNIL_INDEX;
 752         }
 753
 754       c = f_cptr (f, c->next);
 755     }
 756
 757   return c;
 758 }
 759
 760 void
 761 svm_fifo_free_chunk_lookup (svm_fifo_t * f)
 762 {
 763   rb_tree_free_nodes (&f->ooo_enq_lookup);
 764   rb_tree_free_nodes (&f->ooo_deq_lookup);
 765 }
 766
 767 void
 768 svm_fifo_free (svm_fifo_t * f)
 769 {
 770   ASSERT (f->refcnt > 0);
 771
 772   if (--f->refcnt == 0)
 773     {
 774       /* ooo data is not allocated on segment heap */
 775       svm_fifo_free_chunk_lookup (f);
 776       clib_mem_free (f);
 777     }
 778 }
 779
 780 void
 781 svm_fifo_overwrite_head (svm_fifo_t * f, u8 * src, u32 len)
 782 {
 783   u32 n_chunk;
 784   u32 head, tail, head_idx;
 785   svm_fifo_chunk_t *c;
 786
 787   ASSERT (len <= f->shr->size);
 788
 789   f_load_head_tail_cons (f, &head, &tail);
 790
 791   if (!f->shr->head_chunk)
 792     f->shr->head_chunk = f_csptr (f, svm_fifo_find_chunk (f, head));
 793
 794   c = f_head_cptr (f);
 795   head_idx = head - c->start_byte;
 796   n_chunk = c->length - head_idx;
 797   if (len <= n_chunk)
 798     clib_memcpy_fast (&c->data[head_idx], src, len);
 799   else
 800     {
 801       ASSERT (len - n_chunk <= f_cptr (f, c->next)->length);
 802       clib_memcpy_fast (&c->data[head_idx], src, n_chunk);
 803       clib_memcpy_fast (&f_cptr (f, c->next)->data[0], src + n_chunk,
 804                         len - n_chunk);
 805     }
 806 }
 807
 808 static int
 809 f_try_chunk_alloc (svm_fifo_t * f, u32 head, u32 tail, u32 len)
 810 {
 811   svm_fifo_chunk_t *c, *cur, *prev;
 812   u32 alloc_size, free_alloced;
 813
 814   prev = f_end_cptr (f);
 815   free_alloced = f_chunk_end (prev) - tail;
 816
 817   alloc_size = clib_min (f->shr->min_alloc, f->shr->size - (tail - head));
 818   alloc_size = clib_max (alloc_size, len - free_alloced);
 819
 820   c = fsh_alloc_chunk (f->fs_hdr, f->shr->slice_index, alloc_size);
 821   if (PREDICT_FALSE (!c))
 822     return -1;
 823
 824   cur = c;
 825
 826   while (cur)
 827     {
 828       cur->start_byte = prev->start_byte + prev->length;
 829       cur->enq_rb_index = RBTREE_TNIL_INDEX;
 830       cur->deq_rb_index = RBTREE_TNIL_INDEX;
 831
 832       prev = cur;
 833       cur = f_cptr (f, cur->next);
 834     }
 835
 836   f_csptr_link (f, f->shr->end_chunk, c);
 837   prev->next = 0;
 838   f->shr->end_chunk = f_csptr (f, prev);
 839
 840   if (!f->shr->tail_chunk)
 841     f->shr->tail_chunk = f_csptr (f, c);
 842
 843   return 0;
 844 }
 845
 846 int
 847 svm_fifo_enqueue (svm_fifo_t * f, u32 len, const u8 * src)
 848 {
 849   u32 tail, head, free_count;
 850   svm_fifo_chunk_t *old_tail_c;
 851
 852   f->ooos_newest = OOO_SEGMENT_INVALID_INDEX;
 853
 854   f_load_head_tail_prod (f, &head, &tail);
 855
 856   /* free space in fifo can only increase during enqueue: SPSC */
 857   free_count = f_free_count (f, head, tail);
 858
 859   if (PREDICT_FALSE (free_count == 0))
 860     return SVM_FIFO_EFULL;
 861
 862   /* number of bytes we're going to copy */
 863   len = clib_min (free_count, len);
 864
 865   if (f_pos_gt (tail + len, f_chunk_end (f_end_cptr (f))))
 866     {
 867       if (PREDICT_FALSE (f_try_chunk_alloc (f, head, tail, len)))
 868         {
 869           len = f_chunk_end (f_end_cptr (f)) - tail;
 870           if (!len)
 871             return SVM_FIFO_EGROW;
 872         }
 873     }
 874
 875   old_tail_c = f_tail_cptr (f);
 876
 877   svm_fifo_copy_to_chunk (f, old_tail_c, tail, src, len, &f->shr->tail_chunk);
 878   tail = tail + len;
 879
 880   svm_fifo_trace_add (f, head, len, 2);
 881
 882   /* collect out-of-order segments */
 883   if (PREDICT_FALSE (f->ooos_list_head != OOO_SEGMENT_INVALID_INDEX))
 884     {
 885       len += ooo_segment_try_collect (f, len, &tail);
 886       /* Tail chunk might've changed even if nothing was collected */
 887       f->shr->tail_chunk =
 888         f_csptr (f, f_lookup_clear_enq_chunks (f, old_tail_c, tail));
 889       f->ooo_enq = 0;
 890     }
 891
 892   /* store-rel: producer owned index (paired with load-acq in consumer) */
 893   clib_atomic_store_rel_n (&f->shr->tail, tail);
 894
 895   return len;
 896 }
 897
 898 /**
 899  * Enqueue a future segment.
 900  *
 901  * Two choices: either copies the entire segment, or copies nothing
 902  * Returns 0 of the entire segment was copied
 903  * Returns -1 if none of the segment was copied due to lack of space
 904  */
 905 int
 906 svm_fifo_enqueue_with_offset (svm_fifo_t * f, u32 offset, u32 len, u8 * src)
 907 {
 908   u32 tail, head, free_count, enq_pos;
 909   svm_fifo_chunk_ptr_t last = F_INVALID_CPTR;
 910
 911   f_load_head_tail_prod (f, &head, &tail);
 912
 913   /* free space in fifo can only increase during enqueue: SPSC */
 914   free_count = f_free_count (f, head, tail);
 915   f->ooos_newest = OOO_SEGMENT_INVALID_INDEX;
 916
 917   /* will this request fit? */
 918   if ((len + offset) > free_count)
 919     return SVM_FIFO_EFULL;
 920
 921   enq_pos = tail + offset;
 922
 923   if (f_pos_gt (enq_pos + len, f_chunk_end (f_end_cptr (f))))
 924     {
 925       if (PREDICT_FALSE (f_try_chunk_alloc (f, head, tail, offset + len)))
 926         return SVM_FIFO_EGROW;
 927     }
 928
 929   svm_fifo_trace_add (f, offset, len, 1);
 930   ooo_segment_add (f, offset, head, tail, len);
 931
 932   if (!f->ooo_enq || !f_chunk_includes_pos (f->ooo_enq, enq_pos))
 933     f_update_ooo_enq (f, enq_pos, enq_pos + len);
 934
 935   svm_fifo_copy_to_chunk (f, f->ooo_enq, enq_pos, src, len, &last);
 936   if (last != F_INVALID_CPTR)
 937     f->ooo_enq = f_cptr (f, last);
 938
 939   return 0;
 940 }
 941
 942 /**
 943  * Advance tail
 944  */
 945 void
 946 svm_fifo_enqueue_nocopy (svm_fifo_t * f, u32 len)
 947 {
 948   u32 tail;
 949
 950   ASSERT (len <= svm_fifo_max_enqueue_prod (f));
 951   /* load-relaxed: producer owned index */
 952   tail = f->shr->tail;
 953   tail = tail + len;
 954
 955   if (rb_tree_is_init (&f->ooo_enq_lookup))
 956     {
 957       f->shr->tail_chunk =
 958         f_csptr (f, f_lookup_clear_enq_chunks (f, f_tail_cptr (f), tail));
 959       f->ooo_enq = 0;
 960     }
 961   else
 962     {
 963       f->shr->tail_chunk =
 964         f_csptr (f, svm_fifo_find_next_chunk (f, f_tail_cptr (f), tail));
 965     }
 966
 967   /* store-rel: producer owned index (paired with load-acq in consumer) */
 968   clib_atomic_store_rel_n (&f->shr->tail, tail);
 969 }
 970
 971 int
 972 svm_fifo_enqueue_segments (svm_fifo_t * f, const svm_fifo_seg_t segs[],
 973                            u32 n_segs, u8 allow_partial)
 974 {
 975   u32 tail, head, free_count, len = 0, i;
 976   svm_fifo_chunk_t *old_tail_c;
 977
 978   f->ooos_newest = OOO_SEGMENT_INVALID_INDEX;
 979
 980   f_load_head_tail_prod (f, &head, &tail);
 981
 982   /* free space in fifo can only increase during enqueue: SPSC */
 983   free_count = f_free_count (f, head, tail);
 984
 985   if (PREDICT_FALSE (free_count == 0))
 986     return SVM_FIFO_EFULL;
 987
 988   for (i = 0; i < n_segs; i++)
 989     len += segs[i].len;
 990
 991   old_tail_c = f_tail_cptr (f);
 992
 993   if (!allow_partial)
 994     {
 995       if (PREDICT_FALSE (free_count < len))
 996         return SVM_FIFO_EFULL;
 997
 998       if (f_pos_gt (tail + len, f_chunk_end (f_end_cptr (f))))
 999         {
1000           if (PREDICT_FALSE (f_try_chunk_alloc (f, head, tail, len)))
1001             return SVM_FIFO_EGROW;
1002         }
1003
1004       for (i = 0; i < n_segs; i++)
1005         {
1006           svm_fifo_copy_to_chunk (f, f_tail_cptr (f), tail, segs[i].data,
1007                                   segs[i].len, &f->shr->tail_chunk);
1008           tail += segs[i].len;
1009         }
1010     }
1011   else
1012     {
1013       len = clib_min (free_count, len);
1014
1015       if (f_pos_gt (tail + len, f_chunk_end (f_end_cptr (f))))
1016         {
1017           if (PREDICT_FALSE (f_try_chunk_alloc (f, head, tail, len)))
1018             {
1019               len = f_chunk_end (f_end_cptr (f)) - tail;
1020               if (!len)
1021                 return SVM_FIFO_EGROW;
1022             }
1023         }
1024
1025       i = 0;
1026       while (len)
1027         {
1028           u32 to_copy = clib_min (segs[i].len, len);
1029           svm_fifo_copy_to_chunk (f, f_tail_cptr (f), tail, segs[i].data,
1030                                   to_copy, &f->shr->tail_chunk);
1031           len -= to_copy;
1032           tail += to_copy;
1033           i++;
1034         }
1035     }
1036
1037   /* collect out-of-order segments */
1038   if (PREDICT_FALSE (f->ooos_list_head != OOO_SEGMENT_INVALID_INDEX))
1039     {
1040       len += ooo_segment_try_collect (f, len, &tail);
1041       /* Tail chunk might've changed even if nothing was collected */
1042       f->shr->tail_chunk =
1043         f_csptr (f, f_lookup_clear_enq_chunks (f, old_tail_c, tail));
1044       f->ooo_enq = 0;
1045     }
1046
1047   /* store-rel: producer owned index (paired with load-acq in consumer) */
1048   clib_atomic_store_rel_n (&f->shr->tail, tail);
1049
1050   return len;
1051 }
1052
1053 always_inline svm_fifo_chunk_t *
1054 f_unlink_chunks (svm_fifo_t * f, u32 end_pos, u8 maybe_ooo)
1055 {
1056   svm_fifo_chunk_t *start, *prev = 0, *c;
1057   rb_tree_t *rt;
1058   rb_node_t *n;
1059
1060   if (maybe_ooo)
1061     rt = &f->ooo_deq_lookup;
1062
1063   c = f_start_cptr (f);
1064   ASSERT (!f_chunk_includes_pos (c, end_pos));
1065
1066   do
1067     {
1068       if (maybe_ooo && c->deq_rb_index != RBTREE_TNIL_INDEX)
1069         {
1070           n = rb_node (rt, c->deq_rb_index);
1071           ASSERT (n == f_find_node_rbtree (rt, c->start_byte));
1072           rb_tree_del_node (rt, n);
1073           c->deq_rb_index = RBTREE_TNIL_INDEX;
1074         }
1075       if (!c->next)
1076         break;
1077       prev = c;
1078       c = f_cptr (f, c->next);
1079     }
1080   while (!f_chunk_includes_pos (c, end_pos));
1081
1082   if (maybe_ooo)
1083     {
1084       if (f->ooo_deq && f_pos_lt (f->ooo_deq->start_byte, f_chunk_end (c)))
1085         f->ooo_deq = 0;
1086     }
1087   else
1088     {
1089       if (PREDICT_FALSE (f->ooo_deq != 0))
1090         f->ooo_deq = 0;
1091     }
1092
1093   /* Avoid unlinking the last chunk */
1094   if (!prev)
1095     return 0;
1096
1097   prev->next = 0;
1098   start = f_start_cptr (f);
1099   f->shr->start_chunk = f_csptr (f, c);
1100
1101   return start;
1102 }
1103
1104 int
1105 svm_fifo_dequeue (svm_fifo_t * f, u32 len, u8 * dst)
1106 {
1107   u32 tail, head, cursize;
1108
1109   f_load_head_tail_cons (f, &head, &tail);
1110
1111   /* current size of fifo can only increase during dequeue: SPSC */
1112   cursize = f_cursize (f, head, tail);
1113
1114   if (PREDICT_FALSE (cursize == 0))
1115     return SVM_FIFO_EEMPTY;
1116
1117   len = clib_min (cursize, len);
1118
1119   if (!f->shr->head_chunk)
1120     f->shr->head_chunk = f_csptr (f, svm_fifo_find_chunk (f, head));
1121
1122   svm_fifo_copy_from_chunk (f, f_head_cptr (f), head, dst, len,
1123                             &f->shr->head_chunk);
1124   head = head + len;
1125
1126   /* In order dequeues are not supported in combination with ooo peeking.
1127    * Use svm_fifo_dequeue_drop instead. */
1128   ASSERT (rb_tree_n_nodes (&f->ooo_deq_lookup) <= 1);
1129
1130   if (f_pos_geq (head, f_chunk_end (f_start_cptr (f))))
1131     fsh_collect_chunks (f->fs_hdr, f->shr->slice_index,
1132                         f_unlink_chunks (f, head, 0));
1133
1134   /* store-rel: consumer owned index (paired with load-acq in producer) */
1135   clib_atomic_store_rel_n (&f->shr->head, head);
1136
1137   return len;
1138 }
1139
1140 int
1141 svm_fifo_peek (svm_fifo_t * f, u32 offset, u32 len, u8 * dst)
1142 {
1143   u32 tail, head, cursize, head_idx;
1144   svm_fifo_chunk_ptr_t last = F_INVALID_CPTR;
1145
1146   f_load_head_tail_cons (f, &head, &tail);
1147
1148   /* current size of fifo can only increase during peek: SPSC */
1149   cursize = f_cursize (f, head, tail);
1150
1151   if (PREDICT_FALSE (cursize < offset))
1152     return SVM_FIFO_EEMPTY;
1153
1154   len = clib_min (cursize - offset, len);
1155   head_idx = head + offset;
1156
1157   CLIB_MEM_UNPOISON (f->ooo_deq, sizeof (*f->ooo_deq));
1158   if (!f->ooo_deq || !f_chunk_includes_pos (f->ooo_deq, head_idx))
1159     f_update_ooo_deq (f, head_idx, head_idx + len);
1160
1161   svm_fifo_copy_from_chunk (f, f->ooo_deq, head_idx, dst, len, &last);
1162   if (last != F_INVALID_CPTR)
1163     f->ooo_deq = f_cptr (f, last);
1164   return len;
1165 }
1166
1167 int
1168 svm_fifo_dequeue_drop (svm_fifo_t * f, u32 len)
1169 {
1170   u32 total_drop_bytes, tail, head, cursize;
1171
1172   f_load_head_tail_cons (f, &head, &tail);
1173
1174   /* number of bytes available */
1175   cursize = f_cursize (f, head, tail);
1176   if (PREDICT_FALSE (cursize == 0))
1177     return SVM_FIFO_EEMPTY;
1178
1179   /* number of bytes we're going to drop */
1180   total_drop_bytes = clib_min (cursize, len);
1181
1182   svm_fifo_trace_add (f, tail, total_drop_bytes, 3);
1183
1184   /* move head */
1185   head = head + total_drop_bytes;
1186
1187   if (f_pos_geq (head, f_chunk_end (f_start_cptr (f))))
1188     {
1189       fsh_collect_chunks (f->fs_hdr, f->shr->slice_index,
1190                           f_unlink_chunks (f, head, 1));
1191       f->shr->head_chunk = f_chunk_includes_pos (f_start_cptr (f), head) ?
1192                              f->shr->start_chunk :
1193                              0;
1194     }
1195
1196   /* store-rel: consumer owned index (paired with load-acq in producer) */
1197   clib_atomic_store_rel_n (&f->shr->head, head);
1198
1199   return total_drop_bytes;
1200 }
1201
1202 /**
1203  * Drop all data from fifo
1204  *
1205  */
1206 void
1207 svm_fifo_dequeue_drop_all (svm_fifo_t * f)
1208 {
1209   u32 head, tail;
1210
1211   f_load_head_tail_all_acq (f, &head, &tail);
1212
1213   if (!f->shr->head_chunk || !f_chunk_includes_pos (f_head_cptr (f), head))
1214     f->shr->head_chunk = f_csptr (f, svm_fifo_find_chunk (f, head));
1215
1216   f->shr->head_chunk =
1217     f_csptr (f, f_lookup_clear_deq_chunks (f, f_head_cptr (f), tail));
1218
1219   if (f_pos_geq (tail, f_chunk_end (f_start_cptr (f))))
1220     fsh_collect_chunks (f->fs_hdr, f->shr->slice_index,
1221                         f_unlink_chunks (f, tail, 0));
1222
1223   /* store-rel: consumer owned index (paired with load-acq in producer) */
1224   clib_atomic_store_rel_n (&f->shr->head, tail);
1225 }
1226
1227 int
1228 svm_fifo_fill_chunk_list (svm_fifo_t * f)
1229 {
1230   u32 head, tail;
1231
1232   f_load_head_tail_prod (f, &head, &tail);
1233
1234   if (f_chunk_end (f_end_cptr (f)) - head >= f->shr->size)
1235     return 0;
1236
1237   if (f_try_chunk_alloc (f, head, tail, f->shr->size - (tail - head)))
1238     return SVM_FIFO_EGROW;
1239
1240   return 0;
1241 }
1242
1243 int
1244 svm_fifo_provision_chunks (svm_fifo_t *f, svm_fifo_seg_t *fs, u32 n_segs,
1245                            u32 len)
1246 {
1247   u32 head, tail, n_avail, head_pos, n_bytes, fs_index = 1, clen;
1248   svm_fifo_chunk_t *c;
1249
1250   f_load_head_tail_prod (f, &head, &tail);
1251
1252   if (f_free_count (f, head, tail) < len)
1253     return SVM_FIFO_EFULL;
1254
1255   n_avail = f_chunk_end (f_end_cptr (f)) - tail;
1256
1257   if (n_avail < len && f_try_chunk_alloc (f, head, tail, len))
1258     return SVM_FIFO_EGROW;
1259
1260   c = f_tail_cptr (f);
1261   head_pos = (tail - c->start_byte);
1262   fs[0].data = c->data + head_pos;
1263   fs[0].len = clib_min (c->length - head_pos, len);
1264   n_bytes = fs[0].len;
1265
1266   while (n_bytes < len && fs_index < n_segs)
1267     {
1268       c = f_cptr (f, c->next);
1269       clen = clib_min (c->length, len - n_bytes);
1270       fs[fs_index].data = c->data;
1271       fs[fs_index].len = clen;
1272       n_bytes += clen;
1273       fs_index += 1;
1274     }
1275
1276   return fs_index;
1277 }
1278
1279 int
1280 svm_fifo_segments (svm_fifo_t * f, u32 offset, svm_fifo_seg_t * fs,
1281                    u32 n_segs, u32 max_bytes)
1282 {
1283   u32 cursize, to_read, head, tail, fs_index = 1;
1284   u32 n_bytes, head_pos, len, start;
1285   svm_fifo_chunk_t *c;
1286
1287   f_load_head_tail_cons (f, &head, &tail);
1288
1289   /* consumer function, cursize can only increase while we're working */
1290   cursize = f_cursize (f, head, tail);
1291
1292   if (PREDICT_FALSE (cursize == 0))
1293     return SVM_FIFO_EEMPTY;
1294
1295   if (offset >= cursize)
1296     return SVM_FIFO_EEMPTY;
1297
1298   to_read = clib_min (cursize - offset, max_bytes);
1299   start = head + offset;
1300
1301   if (!f->shr->head_chunk)
1302     f->shr->head_chunk = f_csptr (f, svm_fifo_find_chunk (f, head));
1303
1304   c = f_head_cptr (f);
1305
1306   while (!f_chunk_includes_pos (c, start))
1307     c = f_cptr (f, c->next);
1308
1309   head_pos = start - c->start_byte;
1310   fs[0].data = c->data + head_pos;
1311   fs[0].len = clib_min (c->length - head_pos, to_read);
1312   n_bytes = fs[0].len;
1313
1314   while (n_bytes < to_read && fs_index < n_segs)
1315     {
1316       c = f_cptr (f, c->next);
1317       len = clib_min (c->length, to_read - n_bytes);
1318       fs[fs_index].data = c->data;
1319       fs[fs_index].len = len;
1320       n_bytes += len;
1321       fs_index += 1;
1322     }
1323
1324   return n_bytes;
1325 }
1326
1327 /**
1328  * Clones fifo
1329  *
1330  * Assumptions:
1331  * - no prod and cons are accessing either dest or src fifo
1332  * - fifo is not multi chunk
1333  */
1334 void
1335 svm_fifo_clone (svm_fifo_t * df, svm_fifo_t * sf)
1336 {
1337   u32 head, tail;
1338
1339   /* Support only single chunk clones for now */
1340   ASSERT (svm_fifo_n_chunks (sf) == 1);
1341
1342   clib_memcpy_fast (f_head_cptr (df)->data, f_head_cptr (sf)->data,
1343                     sf->shr->size);
1344
1345   f_load_head_tail_all_acq (sf, &head, &tail);
1346   clib_atomic_store_rel_n (&df->shr->head, head);
1347   clib_atomic_store_rel_n (&df->shr->tail, tail);
1348 }
1349
1350 u32
1351 svm_fifo_n_ooo_segments (svm_fifo_t * f)
1352 {
1353   return pool_elts (f->ooo_segments);
1354 }
1355
1356 ooo_segment_t *
1357 svm_fifo_first_ooo_segment (svm_fifo_t * f)
1358 {
1359   return pool_elt_at_index (f->ooo_segments, f->ooos_list_head);
1360 }
1361
1362 /**
1363  * Set fifo pointers to requested offset
1364  */
1365 void
1366 svm_fifo_init_pointers (svm_fifo_t * f, u32 head, u32 tail)
1367 {
1368   svm_fifo_chunk_t *c;
1369
1370   clib_atomic_store_rel_n (&f->shr->head, head);
1371   clib_atomic_store_rel_n (&f->shr->tail, tail);
1372
1373   c = svm_fifo_find_chunk (f, head);
1374   ASSERT (c != 0);
1375   f->ooo_deq = c;
1376   f->shr->head_chunk = f_csptr (f, c);
1377   c = svm_fifo_find_chunk (f, tail);
1378   ASSERT (c != 0);
1379   f->ooo_enq = c;
1380   f->shr->tail_chunk = f_csptr (f, c);
1381 }
1382
1383 void
1384 svm_fifo_add_subscriber (svm_fifo_t * f, u8 subscriber)
1385 {
1386   if (f->shr->n_subscribers >= SVM_FIFO_MAX_EVT_SUBSCRIBERS)
1387     return;
1388   f->shr->subscribers[f->shr->n_subscribers++] = subscriber;
1389 }
1390
1391 void
1392 svm_fifo_del_subscriber (svm_fifo_t * f, u8 subscriber)
1393 {
1394   int i;
1395
1396   for (i = 0; i < f->shr->n_subscribers; i++)
1397     {
1398       if (f->shr->subscribers[i] != subscriber)
1399         continue;
1400       f->shr->subscribers[i] = f->shr->subscribers[f->shr->n_subscribers - 1];
1401       f->shr->n_subscribers--;
1402       break;
1403     }
1404 }
1405
1406 u8
1407 svm_fifo_is_sane (svm_fifo_t * f)
1408 {
1409   svm_fifo_chunk_t *tmp;
1410
1411   if (f->shr->head_chunk &&
1412       !f_chunk_includes_pos (f_head_cptr (f), f->shr->head))
1413     return 0;
1414   if (f->shr->tail_chunk &&
1415       !f_chunk_includes_pos (f_tail_cptr (f), f->shr->tail))
1416     return 0;
1417   if (f->ooo_deq)
1418     {
1419       if (rb_tree_is_init (&f->ooo_deq_lookup))
1420         {
1421           if (f_pos_lt (f->ooo_deq->start_byte,
1422                         f_start_cptr (f)->start_byte) ||
1423               f_pos_gt (f->ooo_deq->start_byte, f_chunk_end (f_end_cptr (f))))
1424             return 0;
1425
1426           tmp = f_find_chunk_rbtree (&f->ooo_deq_lookup,
1427                                      f->ooo_deq->start_byte);
1428         }
1429       else
1430         tmp = svm_fifo_find_chunk (f, f->ooo_deq->start_byte);
1431       if (tmp != f->ooo_deq)
1432         return 0;
1433     }
1434   if (f->ooo_enq)
1435     {
1436       if (rb_tree_is_init (&f->ooo_enq_lookup))
1437         {
1438           if (f_pos_lt (f->ooo_enq->start_byte,
1439                         f_start_cptr (f)->start_byte) ||
1440               f_pos_gt (f->ooo_enq->start_byte, f_chunk_end (f_end_cptr (f))))
1441             return 0;
1442
1443           tmp = f_find_chunk_rbtree (&f->ooo_enq_lookup,
1444                                      f->ooo_enq->start_byte);
1445         }
1446       else
1447         {
1448           tmp = svm_fifo_find_next_chunk (f, f_tail_cptr (f),
1449                                           f->ooo_enq->start_byte);
1450         }
1451       if (tmp != f->ooo_enq)
1452         return 0;
1453     }
1454
1455   if (f_start_cptr (f)->next)
1456     {
1457       svm_fifo_chunk_t *c, *prev = 0, *tmp;
1458       u32 chunks_bytes = 0;
1459
1460       c = f_start_cptr (f);
1461       do
1462         {
1463           tmp = svm_fifo_find_chunk (f, c->start_byte);
1464           if (tmp != c)
1465             return 0;
1466           if (prev && (prev->start_byte + prev->length != c->start_byte))
1467             return 0;
1468
1469           if (c->enq_rb_index != RBTREE_TNIL_INDEX)
1470             {
1471               tmp = f_find_chunk_rbtree (&f->ooo_enq_lookup, c->start_byte);
1472               if (tmp)
1473                 {
1474                   if (tmp != c)
1475                     return 0;
1476                 }
1477             }
1478           if (c->deq_rb_index != RBTREE_TNIL_INDEX)
1479             {
1480               tmp = f_find_chunk_rbtree (&f->ooo_deq_lookup, c->start_byte);
1481               if (tmp)
1482                 {
1483                   if (tmp != c)
1484                     return 0;
1485                 }
1486             }
1487
1488           chunks_bytes += c->length;
1489           prev = c;
1490           c = f_cptr (f, c->next);
1491         }
1492       while (c);
1493
1494       if (chunks_bytes < f->shr->tail - f->shr->head)
1495         return 0;
1496     }
1497
1498   return 1;
1499 }
1500
1501 u32
1502 svm_fifo_n_chunks (svm_fifo_t * f)
1503 {
1504   svm_fifo_chunk_t *c;
1505   int n_chunks = 0;
1506
1507   c = f_start_cptr (f);
1508   while (c)
1509     {
1510       n_chunks++;
1511       c = f_cptr (f, c->next);
1512     }
1513
1514   return n_chunks;
1515 }
1516
1517 u8 *
1518 format_ooo_segment (u8 * s, va_list * args)
1519 {
1520   svm_fifo_t __clib_unused *f = va_arg (*args, svm_fifo_t *);
1521   ooo_segment_t *seg = va_arg (*args, ooo_segment_t *);
1522   s = format (s, "[%u, %u], len %u, next %d, prev %d", seg->start,
1523               seg->start + seg->length, seg->length, seg->next, seg->prev);
1524   return s;
1525 }
1526
1527 u8 *
1528 svm_fifo_dump_trace (u8 * s, svm_fifo_t * f)
1529 {
1530 #if SVM_FIFO_TRACE
1531   svm_fifo_trace_elem_t *seg = 0;
1532   int i = 0;
1533
1534   if (f->trace)
1535     {
1536       vec_foreach (seg, f->trace)
1537       {
1538         s = format (s, "{%u, %u, %u}, ", seg->offset, seg->len, seg->action);
1539         i++;
1540         if (i % 5 == 0)
1541           s = format (s, "\n");
1542       }
1543       s = format (s, "\n");
1544     }
1545   return s;
1546 #else
1547   return 0;
1548 #endif
1549 }
1550
1551 u8 *
1552 svm_fifo_replay (u8 * s, svm_fifo_t * f, u8 no_read, u8 verbose)
1553 {
1554   int i, trace_len;
1555   u8 *data = 0;
1556   svm_fifo_trace_elem_t *trace;
1557   u32 offset;
1558   svm_fifo_t *placeholder_fifo;
1559
1560   if (!f)
1561     return s;
1562
1563 #if SVM_FIFO_TRACE
1564   trace = f->trace;
1565   trace_len = vec_len (trace);
1566 #else
1567   trace = 0;
1568   trace_len = 0;
1569 #endif
1570
1571   placeholder_fifo = svm_fifo_alloc (f->shr->size);
1572   svm_fifo_init (f, f->shr->size);
1573   clib_memset (f_head_cptr (f)->data, 0xFF, f->shr->size);
1574   vec_validate (data, f->shr->size);
1575   for (i = 0; i < vec_len (data); i++)
1576     data[i] = i;
1577
1578   for (i = 0; i < trace_len; i++)
1579     {
1580       offset = trace[i].offset;
1581       if (trace[i].action == 1)
1582         {
1583           if (verbose)
1584             s = format (s, "adding [%u, %u]:", trace[i].offset,
1585                         (trace[i].offset + trace[i].len));
1586           svm_fifo_enqueue_with_offset (placeholder_fifo, trace[i].offset,
1587                                         trace[i].len, &data[offset]);
1588         }
1589       else if (trace[i].action == 2)
1590         {
1591           if (verbose)
1592             s = format (s, "adding [%u, %u]:", 0, trace[i].len);
1593           svm_fifo_enqueue (placeholder_fifo, trace[i].len, &data[offset]);
1594         }
1595       else if (!no_read)
1596         {
1597           if (verbose)
1598             s = format (s, "read: %u", trace[i].len);
1599           svm_fifo_dequeue_drop (placeholder_fifo, trace[i].len);
1600         }
1601       if (verbose)
1602         s = format (s, "%U", format_svm_fifo, placeholder_fifo, 1);
1603     }
1604
1605   s = format (s, "result: %U", format_svm_fifo, placeholder_fifo, 1);
1606
1607   return s;
1608 }
1609
1610 u8 *
1611 format_ooo_list (u8 * s, va_list * args)
1612 {
1613   svm_fifo_t *f = va_arg (*args, svm_fifo_t *);
1614   u32 indent = va_arg (*args, u32);
1615   u32 ooo_segment_index = f->ooos_list_head;
1616   ooo_segment_t *seg;
1617
1618   while (ooo_segment_index != OOO_SEGMENT_INVALID_INDEX)
1619     {
1620       seg = pool_elt_at_index (f->ooo_segments, ooo_segment_index);
1621       s = format (s, "%U%U\n", format_white_space, indent, format_ooo_segment,
1622                   f, seg);
1623       ooo_segment_index = seg->next;
1624     }
1625
1626   return s;
1627 }
1628
1629 u8 *
1630 format_svm_fifo (u8 * s, va_list * args)
1631 {
1632   svm_fifo_t *f = va_arg (*args, svm_fifo_t *);
1633   int verbose = va_arg (*args, int);
1634   u32 indent;
1635
1636   if (!s)
1637     return s;
1638
1639   indent = format_get_indent (s);
1640   s = format (s, "cursize %u nitems %u has_event %d min_alloc %u\n",
1641               svm_fifo_max_dequeue (f), f->shr->size, f->shr->has_event,
1642               f->shr->min_alloc);
1643   s = format (s, "%Uhead %u tail %u segment manager %u\n", format_white_space,
1644               indent, f->shr->head, f->shr->tail, f->segment_manager);
1645
1646   if (verbose > 1)
1647     s = format (s, "%Uvpp session %d thread %d app session %d thread %d\n",
1648                 format_white_space, indent, f->shr->master_session_index,
1649                 f->master_thread_index, f->shr->client_session_index,
1650                 f->client_thread_index);
1651
1652   if (verbose)
1653     {
1654       s = format (s, "%Uooo pool %d active elts newest %u\n",
1655                   format_white_space, indent, pool_elts (f->ooo_segments),
1656                   f->ooos_newest);
1657       if (svm_fifo_has_ooo_data (f))
1658         s = format (s, " %U", format_ooo_list, f, indent, verbose);
1659     }
1660   return s;
1661 }
1662
1663 #endif
1664 /*
1665  * fd.io coding-style-patch-verification: ON
1666  *
1667  * Local Variables:
1668  * eval: (c-set-style "gnu")
1669  * End:
1670  */