Xenomai API  2.6.3
queue.h
1 /*
2  * Copyright (C) 2001,2002,2003 Philippe Gerum <rpm@xenomai.org>.
3  * Copyright (C) 2005 Dmitry Adamushko <dmitry.adamushko@gmail.com>
4  *
5  * Xenomai is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published
7  * by the Free Software Foundation; either version 2 of the License,
8  * or (at your option) any later version.
9  *
10  * Xenomai is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13  * General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with Xenomai; if not, write to the Free Software
17  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
18  * 02111-1307, USA.
19  */
20 
21 #ifndef _XENO_NUCLEUS_QUEUE_H
22 #define _XENO_NUCLEUS_QUEUE_H
23 
24 #include <nucleus/types.h>
25 #include <nucleus/assert.h>
26 
27 /* Basic element holder */
28 
29 typedef struct xnholder {
30 
31  struct xnholder *next;
32  struct xnholder *last;
33 
34 } xnholder_t;
35 
36 static inline void inith(xnholder_t *holder)
37 {
38  /* Holding queues are doubly-linked and circular */
39  holder->last = holder;
40  holder->next = holder;
41 }
42 
43 static inline void ath(xnholder_t *head, xnholder_t *holder)
44 {
45  /* Inserts the new element right after the heading one */
46  holder->last = head;
47  holder->next = head->next;
48  holder->next->last = holder;
49  head->next = holder;
50 }
51 
52 static inline void dth(xnholder_t *holder)
53 {
54  holder->last->next = holder->next;
55  holder->next->last = holder->last;
56 }
57 
58 /* Basic element queue */
59 
60 typedef struct xnqueue {
61 
62  xnholder_t head;
63  int elems;
64 #if defined(__KERNEL__) && XENO_DEBUG(QUEUES)
65  DECLARE_XNLOCK(lock);
66 #endif /* __KERNEL__ && XENO_DEBUG(QUEUES) */
67 
68 } xnqueue_t;
69 
70 #if XENO_DEBUG(QUEUES) && (defined(CONFIG_SMP) || XENO_DEBUG(XNLOCK))
71 #define XNQUEUE_INITIALIZER(q) { { &(q).head, &(q).head }, 0, XNARCH_LOCK_UNLOCKED }
72 #else /* !(XENO_DEBUG(QUEUES) */
73 #define XNQUEUE_INITIALIZER(q) { { &(q).head, &(q).head }, 0 }
74 #endif /* XENO_DEBUG(QUEUES) */
75 
76 #define DEFINE_XNQUEUE(q) xnqueue_t q = XNQUEUE_INITIALIZER(q)
77 
78 static inline void initq(xnqueue_t *qslot)
79 {
80  inith(&qslot->head);
81  qslot->elems = 0;
82 #if defined(__KERNEL__) && XENO_DEBUG(QUEUES)
83  xnlock_init(&qslot->lock);
84 #endif /* __KERNEL__ && XENO_DEBUG(QUEUES) */
85 }
86 
87 #if XENO_DEBUG(QUEUES)
88 
89 #if defined(__KERNEL__) || defined(__XENO_SIM__)
90 
91 #define XENO_DEBUG_CHECK_QUEUE(__qslot) \
92  do { \
93  xnholder_t *curr; \
94  spl_t s; \
95  int nelems = 0; \
96  xnlock_get_irqsave(&(__qslot)->lock,s); \
97  curr = (__qslot)->head.last; \
98  while (curr != &(__qslot)->head && nelems < (__qslot)->elems) \
99  curr = curr->last, nelems++; \
100  if (curr != &(__qslot)->head || nelems != (__qslot)->elems) \
101  xnpod_fatal("corrupted queue, qslot->elems=%d/%d, qslot=%p at %s:%d", \
102  nelems, \
103  (__qslot)->elems, \
104  __qslot, \
105  __FILE__,__LINE__); \
106  xnlock_put_irqrestore(&(__qslot)->lock,s); \
107  } while(0)
108 
109 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder) \
110  do { \
111  xnholder_t *curr; \
112  spl_t s; \
113  xnlock_get_irqsave(&(__qslot)->lock,s); \
114  curr = (__qslot)->head.last; \
115  while (curr != &(__qslot)->head && (__holder) != curr) \
116  curr = curr->last; \
117  if (curr == (__holder)) \
118  xnpod_fatal("inserting element twice, holder=%p, qslot=%p at %s:%d", \
119  __holder, \
120  __qslot, \
121  __FILE__,__LINE__); \
122  if ((__holder)->last == NULL) \
123  xnpod_fatal("holder=%p not initialized, qslot=%p", \
124  __holder, \
125  __qslot); \
126  xnlock_put_irqrestore(&(__qslot)->lock,s); \
127  } while(0)
128 
129 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder) \
130  do { \
131  xnholder_t *curr; \
132  spl_t s; \
133  xnlock_get_irqsave(&(__qslot)->lock,s); \
134  curr = (__qslot)->head.last; \
135  while (curr != &(__qslot)->head && (__holder) != curr) \
136  curr = curr->last; \
137  if (curr == &(__qslot)->head) \
138  xnpod_fatal("removing non-linked element, holder=%p, qslot=%p at %s:%d", \
139  __holder, \
140  __qslot, \
141  __FILE__,__LINE__); \
142  xnlock_put_irqrestore(&(__qslot)->lock,s); \
143  } while(0)
144 
145 #else /* !(__KERNEL__ || __XENO_SIM__) */
146 
147 /* Disable queue checks in user-space code which does not run as part
148  of any virtual machine, e.g. skin call interface libs. */
149 
150 #define XENO_DEBUG_CHECK_QUEUE(__qslot)
151 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder)
152 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder)
153 
154 #endif /* __KERNEL__ || __XENO_SIM__ */
155 
156 /* Write the following as macros so that line numbering information
157  keeps pointing at the real caller in diagnosis messages. */
158 
159 #define insertq(__qslot,__head,__holder) \
160  ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \
161  XENO_DEBUG_INSERT_QUEUE(__qslot,__holder); \
162  ath((__head)->last,__holder); \
163  ++(__qslot)->elems; })
164 
165 #define prependq(__qslot,__holder) \
166  ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \
167  XENO_DEBUG_INSERT_QUEUE(__qslot,__holder); \
168  ath(&(__qslot)->head,__holder); \
169  ++(__qslot)->elems; })
170 
171 #define appendq(__qslot,__holder) \
172  ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \
173  XENO_DEBUG_INSERT_QUEUE(__qslot,__holder); \
174  ath((__qslot)->head.last,__holder); \
175  ++(__qslot)->elems; })
176 
177 #define removeq(__qslot,__holder) \
178  ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \
179  XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder); \
180  dth(__holder); \
181  --(__qslot)->elems; })
182 
183 #else /* !XENO_DEBUG(QUEUES) */
184 
185 static inline void insertq(xnqueue_t *qslot,
186  xnholder_t *head, xnholder_t *holder)
187 {
188  /* Insert the <holder> element before <head> */
189  ath(head->last, holder);
190  ++qslot->elems;
191 }
192 
193 static inline void prependq(xnqueue_t *qslot, xnholder_t *holder)
194 {
195  /* Prepend the element to the queue */
196  ath(&qslot->head, holder);
197  ++qslot->elems;
198 }
199 
200 static inline void appendq(xnqueue_t *qslot, xnholder_t *holder)
201 {
202  /* Append the element to the queue */
203  ath(qslot->head.last, holder);
204  ++qslot->elems;
205 }
206 
207 static inline void removeq(xnqueue_t *qslot, xnholder_t *holder)
208 {
209  dth(holder);
210  --qslot->elems;
211 }
212 
213 #endif /* XENO_DEBUG(QUEUES) */
214 
215 static inline xnholder_t *getheadq(xnqueue_t *qslot)
216 {
217  xnholder_t *holder = qslot->head.next;
218  return holder == &qslot->head ? NULL : holder;
219 }
220 
221 static inline xnholder_t *getq(xnqueue_t *qslot)
222 {
223  xnholder_t *holder = getheadq(qslot);
224  if (holder)
225  removeq(qslot, holder);
226  return holder;
227 }
228 
229 static inline xnholder_t *nextq(xnqueue_t *qslot, xnholder_t *holder)
230 {
231  xnholder_t *nextholder = holder->next;
232  return nextholder == &qslot->head ? NULL : nextholder;
233 }
234 
235 static inline xnholder_t *popq(xnqueue_t *qslot, xnholder_t *holder)
236 {
237  xnholder_t *nextholder = nextq(qslot, holder);
238  removeq(qslot, holder);
239  return nextholder;
240 }
241 
242 static inline int countq(xnqueue_t *qslot)
243 {
244  return qslot->elems;
245 }
246 
247 static inline int emptyq_p(xnqueue_t *qslot)
248 {
249  return qslot->head.next == &qslot->head;
250 }
251 
252 static inline void moveq(xnqueue_t *dstq, xnqueue_t *srcq)
253 {
254  xnholder_t *headsrc = srcq->head.next;
255  xnholder_t *tailsrc = srcq->head.last;
256  xnholder_t *headdst = &dstq->head;
257 
258  if (emptyq_p(srcq))
259  return;
260 
261  /* srcq elements are moved to head of dstq (LIFO) */
262  headsrc->last->next = tailsrc->next;
263  tailsrc->next->last = headsrc->last;
264  headsrc->last = headdst;
265  tailsrc->next = headdst->next;
266  headdst->next->last = tailsrc;
267  headdst->next = headsrc;
268  dstq->elems += srcq->elems;
269  srcq->elems = 0;
270 }
271 
272 /* Prioritized element holder */
273 
274 typedef struct xnpholder {
275 
276  xnholder_t plink;
277  int prio;
278 
279 } xnpholder_t;
280 
281 static inline void initph(xnpholder_t *holder)
282 {
283  inith(&holder->plink);
284  /* Priority is set upon queue insertion */
285 }
286 
287 /* Prioritized element queue - we only manage a descending queuing
288  order (highest numbered priorities are linked first). */
289 
290 typedef struct xnpqueue { xnqueue_t pqueue; } xnpqueue_t;
291 
292 static inline void initpq(xnpqueue_t *pqslot)
293 {
294  initq(&pqslot->pqueue);
295 }
296 
297 static inline void insertpq(xnpqueue_t *pqslot,
298  xnpholder_t *head, xnpholder_t *holder)
299 {
300  /* Insert the <holder> element before <head> */
301  insertq(&pqslot->pqueue, &head->plink, &holder->plink);
302 }
303 
304 static inline void insertpqf(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
305 {
306  /* Insert the element at the end of its priority group (FIFO) */
307 
308  xnholder_t *curr;
309 
310  for (curr = pqslot->pqueue.head.last;
311  curr != &pqslot->pqueue.head; curr = curr->last) {
312  if (prio <= ((xnpholder_t *)curr)->prio)
313  break;
314  }
315 
316  holder->prio = prio;
317 
318  insertq(&pqslot->pqueue, curr->next, &holder->plink);
319 }
320 
321 static inline void insertpql(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
322 {
323  /* Insert the element at the front of its priority group (LIFO) */
324 
325  xnholder_t *curr;
326 
327  for (curr = pqslot->pqueue.head.next;
328  curr != &pqslot->pqueue.head; curr = curr->next) {
329  if (prio >= ((xnpholder_t *)curr)->prio)
330  break;
331  }
332 
333  holder->prio = prio;
334 
335  insertq(&pqslot->pqueue, curr, &holder->plink);
336 }
337 
338 static inline xnpholder_t *findpqh(xnpqueue_t *pqslot, int prio)
339 {
340  /* Find the element heading a given priority group */
341 
342  xnholder_t *curr;
343 
344  for (curr = pqslot->pqueue.head.next;
345  curr != &pqslot->pqueue.head; curr = curr->next) {
346  if (prio >= ((xnpholder_t *)curr)->prio)
347  break;
348  }
349 
350  if (curr && ((xnpholder_t *)curr)->prio == prio)
351  return (xnpholder_t *)curr;
352 
353  return NULL;
354 }
355 
356 static inline void insertpqfr(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
357 {
358  /*
359  * Insert the element at the front of its priority group
360  * (FIFO) - Reverse queueing applied (lowest numbered
361  * priorities are put at front).
362  */
363  xnholder_t *curr;
364 
365  for (curr = pqslot->pqueue.head.last;
366  curr != &pqslot->pqueue.head; curr = curr->last) {
367  if (prio >= ((xnpholder_t *)curr)->prio)
368  break;
369  }
370 
371  holder->prio = prio;
372 
373  insertq(&pqslot->pqueue, curr->next, &holder->plink);
374 }
375 
376 static inline void insertpqlr(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
377 {
378  /*
379  * Insert the element at the front of its priority group
380  * (LIFO) - Reverse queueing applied (lowest numbered
381  * priorities are put at front).
382  */
383  xnholder_t *curr;
384 
385  for (curr = pqslot->pqueue.head.next;
386  curr != &pqslot->pqueue.head; curr = curr->next) {
387  if (prio <= ((xnpholder_t *)curr)->prio)
388  break;
389  }
390 
391  holder->prio = prio;
392 
393  insertq(&pqslot->pqueue, curr, &holder->plink);
394 }
395 
396 static inline xnpholder_t *findpqhr(xnpqueue_t *pqslot, int prio)
397 {
398  /*
399  * Find the element heading a given priority group - Reverse
400  * queueing assumed (lowest numbered priorities should be at
401  * front).
402  */
403  xnholder_t *curr;
404 
405  for (curr = pqslot->pqueue.head.next;
406  curr != &pqslot->pqueue.head; curr = curr->next) {
407  if (prio <= ((xnpholder_t *)curr)->prio)
408  break;
409  }
410 
411  if (curr && ((xnpholder_t *)curr)->prio == prio)
412  return (xnpholder_t *)curr;
413 
414  return NULL;
415 }
416 
417 static inline void appendpq(xnpqueue_t *pqslot, xnpholder_t *holder)
418 {
419  holder->prio = 0;
420  appendq(&pqslot->pqueue, &holder->plink);
421 }
422 
423 static inline void prependpq(xnpqueue_t *pqslot, xnpholder_t *holder)
424 {
425  holder->prio = 0;
426  prependq(&pqslot->pqueue, &holder->plink);
427 }
428 
429 static inline void removepq(xnpqueue_t *pqslot, xnpholder_t *holder)
430 {
431  removeq(&pqslot->pqueue, &holder->plink);
432 }
433 
434 static inline xnpholder_t *getheadpq(xnpqueue_t *pqslot)
435 {
436  return (xnpholder_t *)getheadq(&pqslot->pqueue);
437 }
438 
439 static inline xnpholder_t *nextpq(xnpqueue_t *pqslot, xnpholder_t *holder)
440 {
441  return (xnpholder_t *)nextq(&pqslot->pqueue, &holder->plink);
442 }
443 
444 static inline xnpholder_t *getpq(xnpqueue_t *pqslot)
445 {
446  return (xnpholder_t *)getq(&pqslot->pqueue);
447 }
448 
449 static inline xnpholder_t *poppq(xnpqueue_t *pqslot, xnpholder_t *holder)
450 {
451  return (xnpholder_t *)popq(&pqslot->pqueue, &holder->plink);
452 }
453 
454 static inline int countpq(xnpqueue_t *pqslot)
455 {
456  return countq(&pqslot->pqueue);
457 }
458 
459 static inline int emptypq_p(xnpqueue_t *pqslot)
460 {
461  return emptyq_p(&pqslot->pqueue);
462 }
463 
464 /* Generic prioritized element holder */
465 
466 typedef struct xngholder {
467 
468  xnpholder_t glink;
469  void *data;
470 
471 } xngholder_t;
472 
473 static inline void initgh(xngholder_t *holder, void *data)
474 {
475  inith(&holder->glink.plink);
476  holder->data = data;
477 }
478 
479 /* Generic element queue */
480 
481 typedef struct xngqueue {
482 
483  xnpqueue_t gqueue;
484  xnqueue_t *freehq;
485  void (*starvation) (xnqueue_t *);
486  int threshold;
487 
488 } xngqueue_t;
489 
490 static inline void initgq(xngqueue_t *gqslot,
491  xnqueue_t *freehq,
492  void (*starvation) (xnqueue_t *),
493  int threshold)
494 {
495  initpq(&gqslot->gqueue);
496  gqslot->freehq = freehq;
497  gqslot->starvation = starvation;
498  gqslot->threshold = threshold;
499 }
500 
501 static inline xngholder_t *allocgh(xngqueue_t *gqslot)
502 {
503  if (countq(gqslot->freehq) < gqslot->threshold)
504  gqslot->starvation(gqslot->freehq);
505 
506  return (xngholder_t *)getq(gqslot->freehq);
507 }
508 
509 static inline void *removegh(xngqueue_t *gqslot, xngholder_t *holder)
510 {
511  removepq(&gqslot->gqueue, &holder->glink);
512  appendq(gqslot->freehq, &holder->glink.plink);
513  return holder->data;
514 }
515 
516 static inline void insertgqf(xngqueue_t *gqslot, void *data, int prio)
517 {
518  xngholder_t *holder = allocgh(gqslot);
519  holder->data = data;
520  insertpqf(&gqslot->gqueue, &holder->glink, prio);
521 }
522 
523 static inline void insertgql(xngqueue_t *gqslot, void *data, int prio)
524 {
525  xngholder_t *holder = allocgh(gqslot);
526  holder->data = data;
527  insertpql(&gqslot->gqueue, &holder->glink, prio);
528 }
529 
530 static inline void appendgq(xngqueue_t *gqslot, void *data)
531 {
532  xngholder_t *holder = allocgh(gqslot);
533  holder->data = data;
534  appendpq(&gqslot->gqueue, &holder->glink);
535 }
536 
537 static inline void prependgq(xngqueue_t *gqslot, void *data)
538 {
539  xngholder_t *holder = allocgh(gqslot);
540  holder->data = data;
541  prependpq(&gqslot->gqueue, &holder->glink);
542 }
543 
544 static inline xngholder_t *getheadgq(xngqueue_t *gqslot)
545 {
546  return (xngholder_t *)getheadpq(&gqslot->gqueue);
547 }
548 
549 static inline xngholder_t *nextgq(xngqueue_t *gqslot, xngholder_t *holder)
550 {
551  return (xngholder_t *)nextpq(&gqslot->gqueue, &holder->glink);
552 }
553 
554 static inline void *getgq(xngqueue_t *gqslot)
555 {
556  xngholder_t *holder = getheadgq(gqslot);
557 
558  if (!holder)
559  return NULL;
560 
561  appendq(gqslot->freehq, &getpq(&gqslot->gqueue)->plink);
562 
563  return holder->data;
564 }
565 
566 static inline xngholder_t *popgq(xngqueue_t *gqslot, xngholder_t *holder)
567 {
568  xngholder_t *nextholder = nextgq(gqslot, holder);
569  removegh(gqslot, holder);
570  return nextholder;
571 }
572 
573 static inline xngholder_t *findgq(xngqueue_t *gqslot, void *data)
574 {
575  xnholder_t *holder;
576 
577  for (holder = gqslot->gqueue.pqueue.head.next;
578  holder != &gqslot->gqueue.pqueue.head; holder = holder->next) {
579  if (((xngholder_t *)holder)->data == data)
580  return (xngholder_t *)holder;
581  }
582 
583  return NULL;
584 }
585 
586 static inline void *removegq(xngqueue_t *gqslot, void *data)
587 {
588  xngholder_t *holder = findgq(gqslot, data);
589  return holder ? removegh(gqslot, holder) : NULL;
590 }
591 
592 static inline int countgq(xngqueue_t *gqslot)
593 {
594  return countpq(&gqslot->gqueue);
595 }
596 
597 static inline int emptygq_p(xngqueue_t *gqslot)
598 {
599  return emptypq_p(&gqslot->gqueue);
600 }
601 
602 #endif /* !_XENO_NUCLEUS_QUEUE_H */