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