src/svm/svm_fifo.c

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