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 (fs_sptr_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                fs_sptr_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                fs_sptr_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, fs_sptr_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, fs_sptr_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   fs_sptr_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       u32 n_left = clib_min (free_count, len);
1014
1015       if (f_pos_gt (tail + n_left, f_chunk_end (f_end_cptr (f))))
1016         {
1017           if (PREDICT_FALSE (f_try_chunk_alloc (f, head, tail, n_left)))
1018             {
1019               n_left = f_chunk_end (f_end_cptr (f)) - tail;
1020               if (!n_left)
1021                 return SVM_FIFO_EGROW;
1022             }
1023         }
1024
1025       len = n_left;
1026       i = 0;
1027       while (n_left)
1028         {
1029           u32 to_copy = clib_min (segs[i].len, n_left);
1030           svm_fifo_copy_to_chunk (f, f_tail_cptr (f), tail, segs[i].data,
1031                                   to_copy, &f->shr->tail_chunk);
1032           n_left -= to_copy;
1033           tail += to_copy;
1034           i++;
1035         }
1036     }
1037
1038   /* collect out-of-order segments */
1039   if (PREDICT_FALSE (f->ooos_list_head != OOO_SEGMENT_INVALID_INDEX))
1040     {
1041       len += ooo_segment_try_collect (f, len, &tail);
1042       /* Tail chunk might've changed even if nothing was collected */
1043       f->shr->tail_chunk =
1044         f_csptr (f, f_lookup_clear_enq_chunks (f, old_tail_c, tail));
1045       f->ooo_enq = 0;
1046     }
1047
1048   /* store-rel: producer owned index (paired with load-acq in consumer) */
1049   clib_atomic_store_rel_n (&f->shr->tail, tail);
1050
1051   return len;
1052 }
1053
1054 always_inline svm_fifo_chunk_t *
1055 f_unlink_chunks (svm_fifo_t * f, u32 end_pos, u8 maybe_ooo)
1056 {
1057   svm_fifo_chunk_t *start, *prev = 0, *c;
1058   rb_tree_t *rt;
1059   rb_node_t *n;
1060
1061   if (maybe_ooo)
1062     rt = &f->ooo_deq_lookup;
1063
1064   c = f_start_cptr (f);
1065   ASSERT (!f_chunk_includes_pos (c, end_pos));
1066
1067   do
1068     {
1069       if (maybe_ooo && c->deq_rb_index != RBTREE_TNIL_INDEX)
1070         {
1071           n = rb_node (rt, c->deq_rb_index);
1072           ASSERT (n == f_find_node_rbtree (rt, c->start_byte));
1073           rb_tree_del_node (rt, n);
1074           c->deq_rb_index = RBTREE_TNIL_INDEX;
1075         }
1076       if (!c->next)
1077         break;
1078       prev = c;
1079       c = f_cptr (f, c->next);
1080     }
1081   while (!f_chunk_includes_pos (c, end_pos));
1082
1083   if (maybe_ooo)
1084     {
1085       if (f->ooo_deq && f_pos_lt (f->ooo_deq->start_byte, f_chunk_end (c)))
1086         f->ooo_deq = 0;
1087     }
1088   else
1089     {
1090       if (PREDICT_FALSE (f->ooo_deq != 0))
1091         f->ooo_deq = 0;
1092     }
1093
1094   /* Avoid unlinking the last chunk */
1095   if (!prev)
1096     return 0;
1097
1098   prev->next = 0;
1099   start = f_start_cptr (f);
1100   f->shr->start_chunk = f_csptr (f, c);
1101
1102   return start;
1103 }
1104
1105 int
1106 svm_fifo_dequeue (svm_fifo_t * f, u32 len, u8 * dst)
1107 {
1108   u32 tail, head, cursize;
1109
1110   f_load_head_tail_cons (f, &head, &tail);
1111
1112   /* current size of fifo can only increase during dequeue: SPSC */
1113   cursize = f_cursize (f, head, tail);
1114
1115   if (PREDICT_FALSE (cursize == 0))
1116     return SVM_FIFO_EEMPTY;
1117
1118   len = clib_min (cursize, len);
1119
1120   if (!f->shr->head_chunk)
1121     f->shr->head_chunk = f_csptr (f, svm_fifo_find_chunk (f, head));
1122
1123   svm_fifo_copy_from_chunk (f, f_head_cptr (f), head, dst, len,
1124                             &f->shr->head_chunk);
1125   head = head + len;
1126
1127   /* In order dequeues are not supported in combination with ooo peeking.
1128    * Use svm_fifo_dequeue_drop instead. */
1129   ASSERT (rb_tree_n_nodes (&f->ooo_deq_lookup) <= 1);
1130
1131   if (f_pos_geq (head, f_chunk_end (f_start_cptr (f))))
1132     fsh_collect_chunks (f->fs_hdr, f->shr->slice_index,
1133                         f_unlink_chunks (f, head, 0));
1134
1135   /* store-rel: consumer owned index (paired with load-acq in producer) */
1136   clib_atomic_store_rel_n (&f->shr->head, head);
1137
1138   return len;
1139 }
1140
1141 int
1142 svm_fifo_peek (svm_fifo_t * f, u32 offset, u32 len, u8 * dst)
1143 {
1144   u32 tail, head, cursize, head_idx;
1145   fs_sptr_t last = F_INVALID_CPTR;
1146
1147   f_load_head_tail_cons (f, &head, &tail);
1148
1149   /* current size of fifo can only increase during peek: SPSC */
1150   cursize = f_cursize (f, head, tail);
1151
1152   if (PREDICT_FALSE (cursize < offset))
1153     return SVM_FIFO_EEMPTY;
1154
1155   len = clib_min (cursize - offset, len);
1156   head_idx = head + offset;
1157
1158   clib_mem_unpoison (f->ooo_deq, sizeof (*f->ooo_deq));
1159   if (!f->ooo_deq || !f_chunk_includes_pos (f->ooo_deq, head_idx))
1160     f_update_ooo_deq (f, head_idx, head_idx + len);
1161
1162   svm_fifo_copy_from_chunk (f, f->ooo_deq, head_idx, dst, len, &last);
1163   if (last != F_INVALID_CPTR)
1164     f->ooo_deq = f_cptr (f, last);
1165   return len;
1166 }
1167
1168 int
1169 svm_fifo_dequeue_drop (svm_fifo_t * f, u32 len)
1170 {
1171   u32 total_drop_bytes, tail, head, cursize;
1172
1173   f_load_head_tail_cons (f, &head, &tail);
1174
1175   /* number of bytes available */
1176   cursize = f_cursize (f, head, tail);
1177   if (PREDICT_FALSE (cursize == 0))
1178     return SVM_FIFO_EEMPTY;
1179
1180   /* number of bytes we're going to drop */
1181   total_drop_bytes = clib_min (cursize, len);
1182
1183   svm_fifo_trace_add (f, tail, total_drop_bytes, 3);
1184
1185   /* move head */
1186   head = head + total_drop_bytes;
1187
1188   if (f_pos_geq (head, f_chunk_end (f_start_cptr (f))))
1189     {
1190       fsh_collect_chunks (f->fs_hdr, f->shr->slice_index,
1191                           f_unlink_chunks (f, head, 1));
1192       f->shr->head_chunk = f_chunk_includes_pos (f_start_cptr (f), head) ?
1193                              f->shr->start_chunk :
1194                              0;
1195     }
1196
1197   /* store-rel: consumer owned index (paired with load-acq in producer) */
1198   clib_atomic_store_rel_n (&f->shr->head, head);
1199
1200   return total_drop_bytes;
1201 }
1202
1203 /**
1204  * Drop all data from fifo
1205  *
1206  */
1207 void
1208 svm_fifo_dequeue_drop_all (svm_fifo_t * f)
1209 {
1210   u32 head, tail;
1211
1212   f_load_head_tail_all_acq (f, &head, &tail);
1213
1214   if (!f->shr->head_chunk || !f_chunk_includes_pos (f_head_cptr (f), head))
1215     f->shr->head_chunk = f_csptr (f, svm_fifo_find_chunk (f, head));
1216
1217   f->shr->head_chunk =
1218     f_csptr (f, f_lookup_clear_deq_chunks (f, f_head_cptr (f), tail));
1219
1220   if (f_pos_geq (tail, f_chunk_end (f_start_cptr (f))))
1221     fsh_collect_chunks (f->fs_hdr, f->shr->slice_index,
1222                         f_unlink_chunks (f, tail, 0));
1223
1224   /* store-rel: consumer owned index (paired with load-acq in producer) */
1225   clib_atomic_store_rel_n (&f->shr->head, tail);
1226 }
1227
1228 int
1229 svm_fifo_fill_chunk_list (svm_fifo_t * f)
1230 {
1231   u32 head, tail;
1232
1233   f_load_head_tail_prod (f, &head, &tail);
1234
1235   if (f_chunk_end (f_end_cptr (f)) - head >= f->shr->size)
1236     return 0;
1237
1238   if (f_try_chunk_alloc (f, head, tail, f->shr->size - (tail - head)))
1239     return SVM_FIFO_EGROW;
1240
1241   return 0;
1242 }
1243
1244 int
1245 svm_fifo_provision_chunks (svm_fifo_t *f, svm_fifo_seg_t *fs, u32 n_segs,
1246                            u32 len)
1247 {
1248   u32 head, tail, n_avail, head_pos, n_bytes, fs_index = 1, clen;
1249   svm_fifo_chunk_t *c;
1250
1251   f_load_head_tail_prod (f, &head, &tail);
1252
1253   if (f_free_count (f, head, tail) < len)
1254     return SVM_FIFO_EFULL;
1255
1256   n_avail = f_chunk_end (f_end_cptr (f)) - tail;
1257
1258   if (n_avail < len && f_try_chunk_alloc (f, head, tail, len))
1259     return SVM_FIFO_EGROW;
1260
1261   if (!fs || !n_segs)
1262     return 0;
1263
1264   c = f_tail_cptr (f);
1265   head_pos = (tail - c->start_byte);
1266   fs[0].data = c->data + head_pos;
1267   fs[0].len = clib_min (c->length - head_pos, len);
1268   n_bytes = fs[0].len;
1269
1270   while (n_bytes < len && fs_index < n_segs)
1271     {
1272       c = f_cptr (f, c->next);
1273       clen = clib_min (c->length, len - n_bytes);
1274       fs[fs_index].data = c->data;
1275       fs[fs_index].len = clen;
1276       n_bytes += clen;
1277       fs_index += 1;
1278     }
1279
1280   return fs_index;
1281 }
1282
1283 int
1284 svm_fifo_segments (svm_fifo_t *f, u32 offset, svm_fifo_seg_t *fs, u32 *n_segs,
1285                    u32 max_bytes)
1286 {
1287   u32 cursize, to_read, head, tail, fs_index = 1;
1288   u32 n_bytes, head_pos, len, start;
1289   svm_fifo_chunk_t *c;
1290
1291   f_load_head_tail_cons (f, &head, &tail);
1292
1293   /* consumer function, cursize can only increase while we're working */
1294   cursize = f_cursize (f, head, tail);
1295
1296   if (PREDICT_FALSE (cursize == 0))
1297     return SVM_FIFO_EEMPTY;
1298
1299   if (offset >= cursize)
1300     return SVM_FIFO_EEMPTY;
1301
1302   to_read = clib_min (cursize - offset, max_bytes);
1303   start = head + offset;
1304
1305   if (!f->shr->head_chunk)
1306     f->shr->head_chunk = f_csptr (f, svm_fifo_find_chunk (f, head));
1307
1308   c = f_head_cptr (f);
1309
1310   while (!f_chunk_includes_pos (c, start))
1311     c = f_cptr (f, c->next);
1312
1313   head_pos = start - c->start_byte;
1314   fs[0].data = c->data + head_pos;
1315   fs[0].len = clib_min (c->length - head_pos, to_read);
1316   n_bytes = fs[0].len;
1317
1318   while (n_bytes < to_read && fs_index < *n_segs)
1319     {
1320       c = f_cptr (f, c->next);
1321       len = clib_min (c->length, to_read - n_bytes);
1322       fs[fs_index].data = c->data;
1323       fs[fs_index].len = len;
1324       n_bytes += len;
1325       fs_index += 1;
1326     }
1327   *n_segs = fs_index;
1328
1329   return n_bytes;
1330 }
1331
1332 /**
1333  * Clones fifo
1334  *
1335  * Assumptions:
1336  * - no prod and cons are accessing either dest or src fifo
1337  * - fifo is not multi chunk
1338  */
1339 void
1340 svm_fifo_clone (svm_fifo_t * df, svm_fifo_t * sf)
1341 {
1342   u32 head, tail;
1343
1344   /* Support only single chunk clones for now */
1345   ASSERT (svm_fifo_n_chunks (sf) == 1);
1346
1347   clib_memcpy_fast (f_head_cptr (df)->data, f_head_cptr (sf)->data,
1348                     f_head_cptr (sf)->length);
1349
1350   f_load_head_tail_all_acq (sf, &head, &tail);
1351   clib_atomic_store_rel_n (&df->shr->head, head);
1352   clib_atomic_store_rel_n (&df->shr->tail, tail);
1353 }
1354
1355 u32
1356 svm_fifo_n_ooo_segments (svm_fifo_t * f)
1357 {
1358   return pool_elts (f->ooo_segments);
1359 }
1360
1361 ooo_segment_t *
1362 svm_fifo_first_ooo_segment (svm_fifo_t * f)
1363 {
1364   return pool_elt_at_index (f->ooo_segments, f->ooos_list_head);
1365 }
1366
1367 /**
1368  * Set fifo pointers to requested offset
1369  */
1370 void
1371 svm_fifo_init_pointers (svm_fifo_t * f, u32 head, u32 tail)
1372 {
1373   svm_fifo_chunk_t *c;
1374
1375   clib_atomic_store_rel_n (&f->shr->head, head);
1376   clib_atomic_store_rel_n (&f->shr->tail, tail);
1377
1378   c = svm_fifo_find_chunk (f, head);
1379   ASSERT (c != 0);
1380   f->ooo_deq = c;
1381   f->shr->head_chunk = f_csptr (f, c);
1382   c = svm_fifo_find_chunk (f, tail);
1383   ASSERT (c != 0);
1384   f->ooo_enq = c;
1385   f->shr->tail_chunk = f_csptr (f, c);
1386 }
1387
1388 void
1389 svm_fifo_add_subscriber (svm_fifo_t * f, u8 subscriber)
1390 {
1391   if (f->shr->n_subscribers >= SVM_FIFO_MAX_EVT_SUBSCRIBERS)
1392     return;
1393   f->shr->subscribers[f->shr->n_subscribers++] = subscriber;
1394 }
1395
1396 void
1397 svm_fifo_del_subscriber (svm_fifo_t * f, u8 subscriber)
1398 {
1399   int i;
1400
1401   for (i = 0; i < f->shr->n_subscribers; i++)
1402     {
1403       if (f->shr->subscribers[i] != subscriber)
1404         continue;
1405       f->shr->subscribers[i] = f->shr->subscribers[f->shr->n_subscribers - 1];
1406       f->shr->n_subscribers--;
1407       break;
1408     }
1409 }
1410
1411 u8
1412 svm_fifo_is_sane (svm_fifo_t * f)
1413 {
1414   svm_fifo_chunk_t *tmp;
1415
1416   if (f->shr->head_chunk &&
1417       !f_chunk_includes_pos (f_head_cptr (f), f->shr->head))
1418     return 0;
1419   if (f->shr->tail_chunk &&
1420       !f_chunk_includes_pos (f_tail_cptr (f), f->shr->tail))
1421     return 0;
1422   if (f->ooo_deq)
1423     {
1424       if (rb_tree_is_init (&f->ooo_deq_lookup))
1425         {
1426           if (f_pos_lt (f->ooo_deq->start_byte,
1427                         f_start_cptr (f)->start_byte) ||
1428               f_pos_gt (f->ooo_deq->start_byte, f_chunk_end (f_end_cptr (f))))
1429             return 0;
1430
1431           tmp = f_find_chunk_rbtree (&f->ooo_deq_lookup,
1432                                      f->ooo_deq->start_byte);
1433         }
1434       else
1435         tmp = svm_fifo_find_chunk (f, f->ooo_deq->start_byte);
1436       if (tmp != f->ooo_deq)
1437         return 0;
1438     }
1439   if (f->ooo_enq)
1440     {
1441       if (rb_tree_is_init (&f->ooo_enq_lookup))
1442         {
1443           if (f_pos_lt (f->ooo_enq->start_byte,
1444                         f_start_cptr (f)->start_byte) ||
1445               f_pos_gt (f->ooo_enq->start_byte, f_chunk_end (f_end_cptr (f))))
1446             return 0;
1447
1448           tmp = f_find_chunk_rbtree (&f->ooo_enq_lookup,
1449                                      f->ooo_enq->start_byte);
1450         }
1451       else
1452         {
1453           tmp = svm_fifo_find_next_chunk (f, f_tail_cptr (f),
1454                                           f->ooo_enq->start_byte);
1455         }
1456       if (tmp != f->ooo_enq)
1457         return 0;
1458     }
1459
1460   if (f_start_cptr (f)->next)
1461     {
1462       svm_fifo_chunk_t *c, *prev = 0, *tmp;
1463       u32 chunks_bytes = 0;
1464
1465       c = f_start_cptr (f);
1466       do
1467         {
1468           tmp = svm_fifo_find_chunk (f, c->start_byte);
1469           if (tmp != c)
1470             return 0;
1471           if (prev && (prev->start_byte + prev->length != c->start_byte))
1472             return 0;
1473
1474           if (c->enq_rb_index != RBTREE_TNIL_INDEX)
1475             {
1476               tmp = f_find_chunk_rbtree (&f->ooo_enq_lookup, c->start_byte);
1477               if (tmp)
1478                 {
1479                   if (tmp != c)
1480                     return 0;
1481                 }
1482             }
1483           if (c->deq_rb_index != RBTREE_TNIL_INDEX)
1484             {
1485               tmp = f_find_chunk_rbtree (&f->ooo_deq_lookup, c->start_byte);
1486               if (tmp)
1487                 {
1488                   if (tmp != c)
1489                     return 0;
1490                 }
1491             }
1492
1493           chunks_bytes += c->length;
1494           prev = c;
1495           c = f_cptr (f, c->next);
1496         }
1497       while (c);
1498
1499       if (chunks_bytes < f->shr->tail - f->shr->head)
1500         return 0;
1501     }
1502
1503   return 1;
1504 }
1505
1506 u32
1507 svm_fifo_n_chunks (svm_fifo_t * f)
1508 {
1509   svm_fifo_chunk_t *c;
1510   int n_chunks = 0;
1511
1512   c = f_start_cptr (f);
1513   while (c)
1514     {
1515       n_chunks++;
1516       c = f_cptr (f, c->next);
1517     }
1518
1519   return n_chunks;
1520 }
1521
1522 u8 *
1523 format_ooo_segment (u8 * s, va_list * args)
1524 {
1525   svm_fifo_t __clib_unused *f = va_arg (*args, svm_fifo_t *);
1526   ooo_segment_t *seg = va_arg (*args, ooo_segment_t *);
1527   s = format (s, "[%u, %u], len %u, next %d, prev %d", seg->start,
1528               seg->start + seg->length, seg->length, seg->next, seg->prev);
1529   return s;
1530 }
1531
1532 u8 *
1533 svm_fifo_dump_trace (u8 * s, svm_fifo_t * f)
1534 {
1535 #if SVM_FIFO_TRACE
1536   svm_fifo_trace_elem_t *seg = 0;
1537   int i = 0;
1538
1539   if (f->trace)
1540     {
1541       vec_foreach (seg, f->trace)
1542       {
1543         s = format (s, "{%u, %u, %u}, ", seg->offset, seg->len, seg->action);
1544         i++;
1545         if (i % 5 == 0)
1546           s = format (s, "\n");
1547       }
1548       s = format (s, "\n");
1549     }
1550   return s;
1551 #else
1552   return 0;
1553 #endif
1554 }
1555
1556 u8 *
1557 svm_fifo_replay (u8 * s, svm_fifo_t * f, u8 no_read, u8 verbose)
1558 {
1559   int i, trace_len;
1560   u8 *data = 0;
1561   svm_fifo_trace_elem_t *trace;
1562   u32 offset;
1563   svm_fifo_t *placeholder_fifo;
1564
1565   if (!f)
1566     return s;
1567
1568 #if SVM_FIFO_TRACE
1569   trace = f->trace;
1570   trace_len = vec_len (trace);
1571 #else
1572   trace = 0;
1573   trace_len = 0;
1574 #endif
1575
1576   placeholder_fifo = svm_fifo_alloc (f->shr->size);
1577   svm_fifo_init (f, f->shr->size);
1578   clib_memset (f_head_cptr (f)->data, 0xFF, f->shr->size);
1579   vec_validate (data, f->shr->size);
1580   for (i = 0; i < vec_len (data); i++)
1581     data[i] = i;
1582
1583   for (i = 0; i < trace_len; i++)
1584     {
1585       offset = trace[i].offset;
1586       if (trace[i].action == 1)
1587         {
1588           if (verbose)
1589             s = format (s, "adding [%u, %u]:", trace[i].offset,
1590                         (trace[i].offset + trace[i].len));
1591           svm_fifo_enqueue_with_offset (placeholder_fifo, trace[i].offset,
1592                                         trace[i].len, &data[offset]);
1593         }
1594       else if (trace[i].action == 2)
1595         {
1596           if (verbose)
1597             s = format (s, "adding [%u, %u]:", 0, trace[i].len);
1598           svm_fifo_enqueue (placeholder_fifo, trace[i].len, &data[offset]);
1599         }
1600       else if (!no_read)
1601         {
1602           if (verbose)
1603             s = format (s, "read: %u", trace[i].len);
1604           svm_fifo_dequeue_drop (placeholder_fifo, trace[i].len);
1605         }
1606       if (verbose)
1607         s = format (s, "%U", format_svm_fifo, placeholder_fifo, 1);
1608     }
1609
1610   s = format (s, "result: %U", format_svm_fifo, placeholder_fifo, 1);
1611
1612   return s;
1613 }
1614
1615 u8 *
1616 format_ooo_list (u8 * s, va_list * args)
1617 {
1618   svm_fifo_t *f = va_arg (*args, svm_fifo_t *);
1619   u32 indent = va_arg (*args, u32);
1620   u32 ooo_segment_index = f->ooos_list_head;
1621   ooo_segment_t *seg;
1622
1623   while (ooo_segment_index != OOO_SEGMENT_INVALID_INDEX)
1624     {
1625       seg = pool_elt_at_index (f->ooo_segments, ooo_segment_index);
1626       s = format (s, "%U%U\n", format_white_space, indent, format_ooo_segment,
1627                   f, seg);
1628       ooo_segment_index = seg->next;
1629     }
1630
1631   return s;
1632 }
1633
1634 u8 *
1635 format_svm_fifo (u8 * s, va_list * args)
1636 {
1637   svm_fifo_t *f = va_arg (*args, svm_fifo_t *);
1638   int verbose = va_arg (*args, int);
1639   u32 indent;
1640
1641   if (!s)
1642     return s;
1643
1644   indent = format_get_indent (s);
1645   s = format (s, "cursize %u nitems %u has_event %d min_alloc %u\n",
1646               svm_fifo_max_dequeue (f), f->shr->size, f->shr->has_event,
1647               f->shr->min_alloc);
1648   s = format (s, "%Uhead %u tail %u segment manager %u\n", format_white_space,
1649               indent, f->shr->head, f->shr->tail, f->segment_manager);
1650
1651   if (verbose > 1)
1652     s = format (s, "%Uvpp session %d thread %d app session %d thread %d\n",
1653                 format_white_space, indent, f->shr->master_session_index,
1654                 f->master_thread_index, f->shr->client_session_index,
1655                 f->client_thread_index);
1656
1657   if (verbose)
1658     {
1659       s = format (s, "%Uooo pool %d active elts newest %u\n",
1660                   format_white_space, indent, pool_elts (f->ooo_segments),
1661                   f->ooos_newest);
1662       if (svm_fifo_has_ooo_data (f))
1663         s = format (s, " %U", format_ooo_list, f, indent, verbose);
1664     }
1665   return s;
1666 }
1667
1668 #endif
1669 /*
1670  * fd.io coding-style-patch-verification: ON
1671  *
1672  * Local Variables:
1673  * eval: (c-set-style "gnu")
1674  * End:
1675  */