hs-test: fixed timed out tests passing in the CI
[vpp.git] / src / svm / message_queue.c
1 /*
2  * Copyright (c) 2018 Cisco and/or its affiliates.
3  * Licensed under the Apache License, Version 2.0 (the "License");
4  * you may not use this file except in compliance with the License.
5  * You may obtain a copy of the License at:
6  *
7  *     http://www.apache.org/licenses/LICENSE-2.0
8  *
9  * Unless required by applicable law or agreed to in writing, software
10  * distributed under the License is distributed on an "AS IS" BASIS,
11  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12  * See the License for the specific language governing permissions and
13  * limitations under the License.
14  */
15
16 #include <svm/message_queue.h>
17 #include <vppinfra/mem.h>
18 #include <vppinfra/format.h>
19 #include <vppinfra/time.h>
20 #include <sys/eventfd.h>
21 #include <sys/socket.h>
22
23 static inline svm_msg_q_ring_t *
24 svm_msg_q_ring_inline (svm_msg_q_t * mq, u32 ring_index)
25 {
26   return vec_elt_at_index (mq->rings, ring_index);
27 }
28
29 svm_msg_q_ring_t *
30 svm_msg_q_ring (svm_msg_q_t * mq, u32 ring_index)
31 {
32   return svm_msg_q_ring_inline (mq, ring_index);
33 }
34
35 static inline void *
36 svm_msg_q_ring_data (svm_msg_q_ring_t * ring, u32 elt_index)
37 {
38   ASSERT (elt_index < ring->nitems);
39   return (ring->shr->data + elt_index * ring->elsize);
40 }
41
42 static void
43 svm_msg_q_init_mutex (svm_msg_q_shared_queue_t *sq)
44 {
45   pthread_mutexattr_t attr;
46   pthread_condattr_t cattr;
47
48   clib_memset (&attr, 0, sizeof (attr));
49   clib_memset (&cattr, 0, sizeof (cattr));
50
51   if (pthread_mutexattr_init (&attr))
52     clib_unix_warning ("mutexattr_init");
53   if (pthread_mutexattr_setpshared (&attr, PTHREAD_PROCESS_SHARED))
54     clib_unix_warning ("pthread_mutexattr_setpshared");
55   if (pthread_mutexattr_setrobust (&attr, PTHREAD_MUTEX_ROBUST))
56     clib_unix_warning ("setrobust");
57   if (pthread_mutex_init (&sq->mutex, &attr))
58     clib_unix_warning ("mutex_init");
59   if (pthread_mutexattr_destroy (&attr))
60     clib_unix_warning ("mutexattr_destroy");
61   if (pthread_condattr_init (&cattr))
62     clib_unix_warning ("condattr_init");
63   if (pthread_condattr_setpshared (&cattr, PTHREAD_PROCESS_SHARED))
64     clib_unix_warning ("condattr_setpshared");
65   if (pthread_cond_init (&sq->condvar, &cattr))
66     clib_unix_warning ("cond_init1");
67   if (pthread_condattr_destroy (&cattr))
68     clib_unix_warning ("cond_init2");
69 }
70
71 svm_msg_q_shared_t *
72 svm_msg_q_init (void *base, svm_msg_q_cfg_t *cfg)
73 {
74   svm_msg_q_ring_shared_t *ring;
75   svm_msg_q_shared_queue_t *sq;
76   svm_msg_q_shared_t *smq;
77   u32 q_sz, offset;
78   int i;
79
80   q_sz = sizeof (*sq) + cfg->q_nitems * sizeof (svm_msg_q_msg_t);
81
82   smq = (svm_msg_q_shared_t *) base;
83   sq = smq->q;
84   clib_memset (sq, 0, sizeof (*sq));
85   sq->elsize = sizeof (svm_msg_q_msg_t);
86   sq->maxsize = cfg->q_nitems;
87   smq->n_rings = cfg->n_rings;
88   ring = (void *) ((u8 *) smq->q + q_sz);
89   for (i = 0; i < cfg->n_rings; i++)
90     {
91       ring->elsize = cfg->ring_cfgs[i].elsize;
92       ring->nitems = cfg->ring_cfgs[i].nitems;
93       ring->cursize = ring->head = ring->tail = 0;
94       offset = sizeof (*ring) + ring->nitems * ring->elsize;
95       ring = (void *) ((u8 *) ring + offset);
96     }
97
98   svm_msg_q_init_mutex (sq);
99
100   return smq;
101 }
102
103 uword
104 svm_msg_q_size_to_alloc (svm_msg_q_cfg_t *cfg)
105 {
106   svm_msg_q_ring_cfg_t *ring_cfg;
107   uword rings_sz = 0, mq_sz;
108   u32 q_sz;
109   int i;
110
111   ASSERT (cfg);
112
113   rings_sz = sizeof (svm_msg_q_ring_shared_t) * cfg->n_rings;
114   for (i = 0; i < cfg->n_rings; i++)
115     {
116       if (cfg->ring_cfgs[i].data)
117         continue;
118       ring_cfg = &cfg->ring_cfgs[i];
119       rings_sz += (uword) ring_cfg->nitems * ring_cfg->elsize;
120     }
121
122   q_sz = sizeof (svm_msg_q_shared_queue_t) +
123          cfg->q_nitems * sizeof (svm_msg_q_msg_t);
124   mq_sz = sizeof (svm_msg_q_shared_t) + q_sz + rings_sz;
125
126   return mq_sz;
127 }
128
129 svm_msg_q_shared_t *
130 svm_msg_q_alloc (svm_msg_q_cfg_t *cfg)
131 {
132   uword mq_sz;
133   u8 *base;
134
135   mq_sz = svm_msg_q_size_to_alloc (cfg);
136   base = clib_mem_alloc_aligned (mq_sz, CLIB_CACHE_LINE_BYTES);
137   if (!base)
138     return 0;
139
140   return svm_msg_q_init (base, cfg);
141 }
142
143 void
144 svm_msg_q_attach (svm_msg_q_t *mq, void *smq_base)
145 {
146   svm_msg_q_ring_shared_t *ring;
147   svm_msg_q_shared_t *smq;
148   u32 i, n_rings, q_sz, offset;
149
150   smq = (svm_msg_q_shared_t *) smq_base;
151   mq->q.shr = smq->q;
152   mq->q.evtfd = -1;
153   n_rings = smq->n_rings;
154   vec_validate (mq->rings, n_rings - 1);
155   q_sz = sizeof (svm_msg_q_shared_queue_t) +
156          mq->q.shr->maxsize * sizeof (svm_msg_q_msg_t);
157   ring = (void *) ((u8 *) smq->q + q_sz);
158   for (i = 0; i < n_rings; i++)
159     {
160       mq->rings[i].nitems = ring->nitems;
161       mq->rings[i].elsize = ring->elsize;
162       mq->rings[i].shr = ring;
163       offset = sizeof (*ring) + ring->nitems * ring->elsize;
164       ring = (void *) ((u8 *) ring + offset);
165     }
166   clib_spinlock_init (&mq->q.lock);
167 }
168
169 void
170 svm_msg_q_cleanup (svm_msg_q_t *mq)
171 {
172   vec_free (mq->rings);
173   clib_spinlock_free (&mq->q.lock);
174   if (mq->q.evtfd != -1)
175     close (mq->q.evtfd);
176 }
177
178 void
179 svm_msg_q_free (svm_msg_q_t * mq)
180 {
181   svm_msg_q_cleanup (mq);
182   clib_mem_free (mq->q.shr);
183   clib_mem_free (mq);
184 }
185
186 static void
187 svm_msg_q_send_signal (svm_msg_q_t *mq, u8 is_consumer)
188 {
189   if (mq->q.evtfd == -1)
190     {
191       if (is_consumer)
192         {
193           int rv = pthread_mutex_lock (&mq->q.shr->mutex);
194           if (PREDICT_FALSE (rv == EOWNERDEAD))
195             {
196               rv = pthread_mutex_consistent (&mq->q.shr->mutex);
197               return;
198             }
199         }
200
201       (void) pthread_cond_broadcast (&mq->q.shr->condvar);
202
203       if (is_consumer)
204         pthread_mutex_unlock (&mq->q.shr->mutex);
205     }
206   else
207     {
208       int __clib_unused rv;
209       u64 data = 1;
210
211       if (mq->q.evtfd < 0)
212         return;
213
214       rv = write (mq->q.evtfd, &data, sizeof (data));
215       if (PREDICT_FALSE (rv < 0))
216         clib_unix_warning ("signal write on %d returned %d", mq->q.evtfd, rv);
217     }
218 }
219
220 svm_msg_q_msg_t
221 svm_msg_q_alloc_msg_w_ring (svm_msg_q_t * mq, u32 ring_index)
222 {
223   svm_msg_q_ring_shared_t *sr;
224   svm_msg_q_ring_t *ring;
225   svm_msg_q_msg_t msg;
226
227   ring = svm_msg_q_ring_inline (mq, ring_index);
228   sr = ring->shr;
229
230   ASSERT (sr->cursize < ring->nitems);
231   msg.ring_index = ring - mq->rings;
232   msg.elt_index = sr->tail;
233   sr->tail = (sr->tail + 1) % ring->nitems;
234   clib_atomic_fetch_add_rel (&sr->cursize, 1);
235   return msg;
236 }
237
238 int
239 svm_msg_q_lock_and_alloc_msg_w_ring (svm_msg_q_t * mq, u32 ring_index,
240                                      u8 noblock, svm_msg_q_msg_t * msg)
241 {
242   if (noblock)
243     {
244       if (svm_msg_q_try_lock (mq))
245         return -1;
246       if (PREDICT_FALSE (svm_msg_q_or_ring_is_full (mq, ring_index)))
247         {
248           svm_msg_q_unlock (mq);
249           return -2;
250         }
251       *msg = svm_msg_q_alloc_msg_w_ring (mq, ring_index);
252     }
253   else
254     {
255       svm_msg_q_lock (mq);
256       while (svm_msg_q_or_ring_is_full (mq, ring_index))
257         svm_msg_q_or_ring_wait_prod (mq, ring_index);
258       *msg = svm_msg_q_alloc_msg_w_ring (mq, ring_index);
259     }
260   return 0;
261 }
262
263 svm_msg_q_msg_t
264 svm_msg_q_alloc_msg (svm_msg_q_t * mq, u32 nbytes)
265 {
266   svm_msg_q_msg_t msg = {.as_u64 = ~0 };
267   svm_msg_q_ring_shared_t *sr;
268   svm_msg_q_ring_t *ring;
269
270   vec_foreach (ring, mq->rings)
271   {
272     sr = ring->shr;
273     if (ring->elsize < nbytes || sr->cursize == ring->nitems)
274       continue;
275     msg.ring_index = ring - mq->rings;
276     msg.elt_index = sr->tail;
277     sr->tail = (sr->tail + 1) % ring->nitems;
278     clib_atomic_fetch_add_relax (&sr->cursize, 1);
279     break;
280   }
281   return msg;
282 }
283
284 void *
285 svm_msg_q_msg_data (svm_msg_q_t * mq, svm_msg_q_msg_t * msg)
286 {
287   svm_msg_q_ring_t *ring = svm_msg_q_ring_inline (mq, msg->ring_index);
288   return svm_msg_q_ring_data (ring, msg->elt_index);
289 }
290
291 void
292 svm_msg_q_free_msg (svm_msg_q_t * mq, svm_msg_q_msg_t * msg)
293 {
294   svm_msg_q_ring_shared_t *sr;
295   svm_msg_q_ring_t *ring;
296   u32 need_signal;
297
298   ASSERT (vec_len (mq->rings) > msg->ring_index);
299   ring = svm_msg_q_ring_inline (mq, msg->ring_index);
300   sr = ring->shr;
301   if (msg->elt_index == sr->head)
302     {
303       sr->head = (sr->head + 1) % ring->nitems;
304     }
305   else
306     {
307       clib_warning ("message out of order: elt %u head %u ring %u",
308                     msg->elt_index, sr->head, msg->ring_index);
309       /* for now, expect messages to be processed in order */
310       ASSERT (0);
311     }
312
313   need_signal = clib_atomic_load_relax_n (&sr->cursize) == ring->nitems;
314   clib_atomic_fetch_sub_relax (&sr->cursize, 1);
315
316   if (PREDICT_FALSE (need_signal))
317     svm_msg_q_send_signal (mq, 1 /* is consumer */);
318 }
319
320 static int
321 svm_msq_q_msg_is_valid (svm_msg_q_t * mq, svm_msg_q_msg_t * msg)
322 {
323   u32 dist1, dist2, tail, head;
324   svm_msg_q_ring_shared_t *sr;
325   svm_msg_q_ring_t *ring;
326
327   if (vec_len (mq->rings) <= msg->ring_index)
328     return 0;
329
330   ring = svm_msg_q_ring_inline (mq, msg->ring_index);
331   sr = ring->shr;
332   tail = sr->tail;
333   head = sr->head;
334
335   dist1 = ((ring->nitems + msg->elt_index) - head) % ring->nitems;
336   if (tail == head)
337     dist2 = (sr->cursize == 0) ? 0 : ring->nitems;
338   else
339     dist2 = ((ring->nitems + tail) - head) % ring->nitems;
340   return (dist1 < dist2);
341 }
342
343 void
344 svm_msg_q_add_raw (svm_msg_q_t *mq, svm_msg_q_msg_t *msg)
345 {
346   svm_msg_q_shared_queue_t *sq = mq->q.shr;
347   i8 *tailp;
348   u32 sz;
349
350   tailp = (i8 *) (&sq->data[0] + sq->elsize * sq->tail);
351   clib_memcpy_fast (tailp, msg, sq->elsize);
352
353   sq->tail = (sq->tail + 1) % sq->maxsize;
354
355   sz = clib_atomic_fetch_add_rel (&sq->cursize, 1);
356   if (!sz)
357     svm_msg_q_send_signal (mq, 0 /* is consumer */);
358 }
359
360 int
361 svm_msg_q_add (svm_msg_q_t * mq, svm_msg_q_msg_t * msg, int nowait)
362 {
363   ASSERT (svm_msq_q_msg_is_valid (mq, msg));
364
365   if (nowait)
366     {
367       /* zero on success */
368       if (svm_msg_q_try_lock (mq))
369         {
370           return (-1);
371         }
372     }
373   else
374     svm_msg_q_lock (mq);
375
376   if (PREDICT_FALSE (svm_msg_q_is_full (mq)))
377     {
378       if (nowait)
379         return (-2);
380       while (svm_msg_q_is_full (mq))
381         svm_msg_q_wait_prod (mq);
382     }
383
384   svm_msg_q_add_raw (mq, msg);
385
386   svm_msg_q_unlock (mq);
387
388   return 0;
389 }
390
391 void
392 svm_msg_q_add_and_unlock (svm_msg_q_t * mq, svm_msg_q_msg_t * msg)
393 {
394   ASSERT (svm_msq_q_msg_is_valid (mq, msg));
395   svm_msg_q_add_raw (mq, msg);
396   svm_msg_q_unlock (mq);
397 }
398
399 int
400 svm_msg_q_sub_raw (svm_msg_q_t *mq, svm_msg_q_msg_t *elem)
401 {
402   svm_msg_q_shared_queue_t *sq = mq->q.shr;
403   i8 *headp;
404   u32 sz;
405
406   ASSERT (!svm_msg_q_is_empty (mq));
407
408   headp = (i8 *) (&sq->data[0] + sq->elsize * sq->head);
409   clib_memcpy_fast (elem, headp, sq->elsize);
410
411   sq->head = (sq->head + 1) % sq->maxsize;
412
413   sz = clib_atomic_fetch_sub_relax (&sq->cursize, 1);
414   if (PREDICT_FALSE (sz == sq->maxsize))
415     svm_msg_q_send_signal (mq, 1 /* is consumer */);
416
417   return 0;
418 }
419
420 int
421 svm_msg_q_sub_raw_batch (svm_msg_q_t *mq, svm_msg_q_msg_t *msg_buf, u32 n_msgs)
422 {
423   svm_msg_q_shared_queue_t *sq = mq->q.shr;
424   u32 sz, to_deq;
425   i8 *headp;
426
427   sz = svm_msg_q_size (mq);
428   ASSERT (sz);
429   to_deq = clib_min (sz, n_msgs);
430
431   headp = (i8 *) (&sq->data[0] + sq->elsize * sq->head);
432
433   if (sq->head + to_deq < sq->maxsize)
434     {
435       clib_memcpy_fast (msg_buf, headp, sq->elsize * to_deq);
436       sq->head += to_deq;
437     }
438   else
439     {
440       u32 first_batch = sq->maxsize - sq->head;
441       clib_memcpy_fast (msg_buf, headp, sq->elsize * first_batch);
442       clib_memcpy_fast (msg_buf + first_batch, sq->data,
443                         sq->elsize * (to_deq - first_batch));
444       sq->head = (sq->head + to_deq) % sq->maxsize;
445     }
446
447   clib_atomic_fetch_sub_relax (&sq->cursize, to_deq);
448   if (PREDICT_FALSE (sz == sq->maxsize))
449     svm_msg_q_send_signal (mq, 1 /* is consumer */);
450
451   return to_deq;
452 }
453
454 int
455 svm_msg_q_sub (svm_msg_q_t *mq, svm_msg_q_msg_t *msg,
456                svm_q_conditional_wait_t cond, u32 time)
457 {
458   int rc = 0;
459
460   if (svm_msg_q_is_empty (mq))
461     {
462       if (cond == SVM_Q_NOWAIT)
463         {
464           return (-2);
465         }
466       else if (cond == SVM_Q_TIMEDWAIT)
467         {
468           if ((rc = svm_msg_q_timedwait (mq, time)))
469             return rc;
470         }
471       else
472         {
473           svm_msg_q_wait (mq, SVM_MQ_WAIT_EMPTY);
474         }
475     }
476
477   svm_msg_q_sub_raw (mq, msg);
478
479   return 0;
480 }
481
482 void
483 svm_msg_q_set_eventfd (svm_msg_q_t *mq, int fd)
484 {
485   mq->q.evtfd = fd;
486 }
487
488 int
489 svm_msg_q_alloc_eventfd (svm_msg_q_t *mq)
490 {
491   int fd;
492   if ((fd = eventfd (0, 0)) < 0)
493     return -1;
494   svm_msg_q_set_eventfd (mq, fd);
495   return 0;
496 }
497
498 int
499 svm_msg_q_wait (svm_msg_q_t *mq, svm_msg_q_wait_type_t type)
500 {
501   u8 (*fn) (svm_msg_q_t *);
502   int rv;
503
504   fn = (type == SVM_MQ_WAIT_EMPTY) ? svm_msg_q_is_empty : svm_msg_q_is_full;
505
506   if (mq->q.evtfd == -1)
507     {
508       rv = pthread_mutex_lock (&mq->q.shr->mutex);
509       if (PREDICT_FALSE (rv == EOWNERDEAD))
510         {
511           rv = pthread_mutex_consistent (&mq->q.shr->mutex);
512           return rv;
513         }
514
515       while (fn (mq))
516         pthread_cond_wait (&mq->q.shr->condvar, &mq->q.shr->mutex);
517
518       pthread_mutex_unlock (&mq->q.shr->mutex);
519     }
520   else
521     {
522       u64 buf;
523
524       while (fn (mq))
525         {
526           while ((rv = read (mq->q.evtfd, &buf, sizeof (buf))) < 0)
527             {
528               if (errno != EAGAIN)
529                 {
530                   clib_unix_warning ("read error");
531                   return rv;
532                 }
533             }
534         }
535     }
536
537   return 0;
538 }
539
540 int
541 svm_msg_q_wait_prod (svm_msg_q_t *mq)
542 {
543   if (mq->q.evtfd == -1)
544     {
545       while (svm_msg_q_is_full (mq))
546         pthread_cond_wait (&mq->q.shr->condvar, &mq->q.shr->mutex);
547     }
548   else
549     {
550       u64 buf;
551       int rv;
552
553       while (svm_msg_q_is_full (mq))
554         {
555           while ((rv = read (mq->q.evtfd, &buf, sizeof (buf))) < 0)
556             {
557               if (errno != EAGAIN)
558                 {
559                   clib_unix_warning ("read error");
560                   return rv;
561                 }
562             }
563         }
564     }
565
566   return 0;
567 }
568
569 int
570 svm_msg_q_or_ring_wait_prod (svm_msg_q_t *mq, u32 ring_index)
571 {
572   if (mq->q.evtfd == -1)
573     {
574       while (svm_msg_q_or_ring_is_full (mq, ring_index))
575         pthread_cond_wait (&mq->q.shr->condvar, &mq->q.shr->mutex);
576     }
577   else
578     {
579       u64 buf;
580       int rv;
581
582       while (svm_msg_q_or_ring_is_full (mq, ring_index))
583         {
584           while ((rv = read (mq->q.evtfd, &buf, sizeof (buf))) < 0)
585             {
586               if (errno != EAGAIN)
587                 {
588                   clib_unix_warning ("read error");
589                   return rv;
590                 }
591             }
592         }
593     }
594
595   return 0;
596 }
597
598 int
599 svm_msg_q_timedwait (svm_msg_q_t *mq, double timeout)
600 {
601   if (mq->q.evtfd == -1)
602     {
603       svm_msg_q_shared_queue_t *sq = mq->q.shr;
604       struct timespec ts;
605       u32 sz;
606       int rv;
607
608       rv = pthread_mutex_lock (&sq->mutex);
609       if (PREDICT_FALSE (rv == EOWNERDEAD))
610         {
611           rv = pthread_mutex_consistent (&sq->mutex);
612           return rv;
613         }
614
615       /* check if we're still in a signalable state after grabbing lock */
616       sz = svm_msg_q_size (mq);
617       if (sz != 0 && sz != sq->maxsize)
618         {
619           pthread_mutex_unlock (&sq->mutex);
620           return 0;
621         }
622
623       ts.tv_sec = unix_time_now () + (u32) timeout;
624       ts.tv_nsec = (timeout - (u32) timeout) * 1e9;
625       rv = pthread_cond_timedwait (&sq->condvar, &sq->mutex, &ts);
626
627       pthread_mutex_unlock (&sq->mutex);
628       return rv;
629     }
630   else
631     {
632       struct timeval tv;
633       u64 buf;
634       int rv;
635
636       tv.tv_sec = (u64) timeout;
637       tv.tv_usec = ((u64) timeout - (u64) timeout) * 1e9;
638       rv = setsockopt (mq->q.evtfd, SOL_SOCKET, SO_RCVTIMEO,
639                        (const char *) &tv, sizeof tv);
640       if (rv < 0)
641         {
642           clib_unix_warning ("setsockopt");
643           return -1;
644         }
645
646       rv = read (mq->q.evtfd, &buf, sizeof (buf));
647       if (rv < 0)
648         clib_warning ("read %u", errno);
649
650       return rv < 0 ? errno : 0;
651     }
652 }
653
654 u8 *
655 format_svm_msg_q (u8 * s, va_list * args)
656 {
657   svm_msg_q_t *mq = va_arg (*args, svm_msg_q_t *);
658   s = format (s, " [Q:%d/%d]", mq->q.shr->cursize, mq->q.shr->maxsize);
659   for (u32 i = 0; i < vec_len (mq->rings); i++)
660     {
661       s = format (s, " [R%d:%d/%d]", i, mq->rings[i].shr->cursize,
662                   mq->rings[i].nitems);
663     }
664   return s;
665 }
666
667 /*
668  * fd.io coding-style-patch-verification: ON
669  *
670  * Local Variables:
671  * eval: (c-set-style "gnu")
672  * End:
673  */