Xenomai API  2.5.6.1
include/nucleus/queue.h
00001 /*
00002  * Copyright (C) 2001,2002,2003 Philippe Gerum <[email protected]>.
00003  * Copyright (C) 2005 Dmitry Adamushko <[email protected]>
00004  *
00005  * Xenomai is free software; you can redistribute it and/or modify
00006  * it under the terms of the GNU General Public License as published
00007  * by the Free Software Foundation; either version 2 of the License,
00008  * or (at your option) any later version.
00009  *
00010  * Xenomai is distributed in the hope that it will be useful, but
00011  * WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013  * General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with Xenomai; if not, write to the Free Software
00017  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
00018  * 02111-1307, USA.
00019  */
00020 
00021 #ifndef _XENO_NUCLEUS_QUEUE_H
00022 #define _XENO_NUCLEUS_QUEUE_H
00023 
00024 #include <nucleus/types.h>
00025 #include <nucleus/assert.h>
00026 
00027 /* Basic element holder */
00028 
00029 typedef struct xnholder {
00030 
00031         struct xnholder *next;
00032         struct xnholder *last;
00033 
00034 } xnholder_t;
00035 
00036 static inline void inith(xnholder_t *holder)
00037 {
00038         /* Holding queues are doubly-linked and circular */
00039         holder->last = holder;
00040         holder->next = holder;
00041 }
00042 
00043 static inline void ath(xnholder_t *head, xnholder_t *holder)
00044 {
00045         /* Inserts the new element right after the heading one  */
00046         holder->last = head;
00047         holder->next = head->next;
00048         holder->next->last = holder;
00049         head->next = holder;
00050 }
00051 
00052 static inline void dth(xnholder_t *holder)
00053 {
00054         holder->last->next = holder->next;
00055         holder->next->last = holder->last;
00056 }
00057 
00058 /* Basic element queue */
00059 
00060 typedef struct xnqueue {
00061 
00062         xnholder_t head;
00063         int elems;
00064 #if defined(__KERNEL__) && XENO_DEBUG(QUEUES)
00065         DECLARE_XNLOCK(lock);
00066 #endif /* __KERNEL__ && XENO_DEBUG(QUEUES) */
00067 
00068 } xnqueue_t;
00069 
00070 #if XENO_DEBUG(QUEUES) && (defined(CONFIG_SMP) || XENO_DEBUG(XNLOCK))
00071 #define XNQUEUE_INITIALIZER(q) { { &(q).head, &(q).head }, 0, XNARCH_LOCK_UNLOCKED }
00072 #else /* !(XENO_DEBUG(QUEUES) */
00073 #define XNQUEUE_INITIALIZER(q) { { &(q).head, &(q).head }, 0 }
00074 #endif /* XENO_DEBUG(QUEUES) */
00075 
00076 #define DEFINE_XNQUEUE(q) xnqueue_t q = XNQUEUE_INITIALIZER(q)
00077 
00078 static inline void initq(xnqueue_t *qslot)
00079 {
00080         inith(&qslot->head);
00081         qslot->elems = 0;
00082 #if defined(__KERNEL__) && XENO_DEBUG(QUEUES)
00083         xnlock_init(&qslot->lock);
00084 #endif /* __KERNEL__ && XENO_DEBUG(QUEUES) */
00085 }
00086 
00087 #if XENO_DEBUG(QUEUES)
00088 
00089 #if defined(__KERNEL__) || defined(__XENO_SIM__)
00090 
00091 #define XENO_DEBUG_CHECK_QUEUE(__qslot)                                 \
00092         do {                                                            \
00093                 xnholder_t *curr;                                       \
00094                 spl_t s;                                                \
00095                 int nelems = 0;                                         \
00096                 xnlock_get_irqsave(&(__qslot)->lock,s);                 \
00097                 curr = (__qslot)->head.last;                            \
00098                 while (curr != &(__qslot)->head && nelems < (__qslot)->elems) \
00099                         curr = curr->last, nelems++;                    \
00100                 if (curr != &(__qslot)->head || nelems != (__qslot)->elems) \
00101                         xnpod_fatal("corrupted queue, qslot->elems=%d/%d, qslot=%p at %s:%d", \
00102                                     nelems,                             \
00103                                     (__qslot)->elems,                   \
00104                                     __qslot,                            \
00105                                     __FILE__,__LINE__);                 \
00106                 xnlock_put_irqrestore(&(__qslot)->lock,s);              \
00107         } while(0)
00108 
00109 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder)                       \
00110         do {                                                            \
00111                 xnholder_t *curr;                                       \
00112                 spl_t s;                                                \
00113                 xnlock_get_irqsave(&(__qslot)->lock,s);                 \
00114                 curr = (__qslot)->head.last;                            \
00115                 while (curr != &(__qslot)->head && (__holder) != curr)  \
00116                         curr = curr->last;                              \
00117                 if (curr == (__holder))                                 \
00118                         xnpod_fatal("inserting element twice, holder=%p, qslot=%p at %s:%d", \
00119                                     __holder,                           \
00120                                     __qslot,                            \
00121                                     __FILE__,__LINE__);                 \
00122                 if ((__holder)->last == NULL)                           \
00123                         xnpod_fatal("holder=%p not initialized, qslot=%p", \
00124                                     __holder,                           \
00125                                     __qslot);                           \
00126                 xnlock_put_irqrestore(&(__qslot)->lock,s);              \
00127         } while(0)
00128 
00129 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder)                       \
00130         do {                                                            \
00131                 xnholder_t *curr;                                       \
00132                 spl_t s;                                                \
00133                 xnlock_get_irqsave(&(__qslot)->lock,s);                 \
00134                 curr = (__qslot)->head.last;                            \
00135                 while (curr != &(__qslot)->head && (__holder) != curr)  \
00136                         curr = curr->last;                              \
00137                 if (curr == &(__qslot)->head)                           \
00138                         xnpod_fatal("removing non-linked element, holder=%p, qslot=%p at %s:%d", \
00139                                     __holder,                           \
00140                                     __qslot,                            \
00141                                     __FILE__,__LINE__);                 \
00142                 xnlock_put_irqrestore(&(__qslot)->lock,s);              \
00143         } while(0)
00144 
00145 #else /* !(__KERNEL__ || __XENO_SIM__) */
00146 
00147 /* Disable queue checks in user-space code which does not run as part
00148    of any virtual machine, e.g. skin call interface libs. */
00149 
00150 #define XENO_DEBUG_CHECK_QUEUE(__qslot)
00151 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder)
00152 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder)
00153 
00154 #endif /* __KERNEL__ || __XENO_SIM__ */
00155 
00156 /* Write the following as macros so that line numbering information
00157    keeps pointing at the real caller in diagnosis messages. */
00158 
00159 #define insertq(__qslot,__head,__holder)                        \
00160         ({ XENO_DEBUG_CHECK_QUEUE(__qslot);                     \
00161                 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder);      \
00162                 ath((__head)->last,__holder);                   \
00163                 ++(__qslot)->elems; })
00164 
00165 #define prependq(__qslot,__holder)                              \
00166         ({ XENO_DEBUG_CHECK_QUEUE(__qslot);                     \
00167                 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder);      \
00168                 ath(&(__qslot)->head,__holder);                 \
00169                 ++(__qslot)->elems; })
00170 
00171 #define appendq(__qslot,__holder)                               \
00172         ({ XENO_DEBUG_CHECK_QUEUE(__qslot);                     \
00173                 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder);      \
00174                 ath((__qslot)->head.last,__holder);             \
00175                 ++(__qslot)->elems; })
00176 
00177 #define removeq(__qslot,__holder)                               \
00178         ({ XENO_DEBUG_CHECK_QUEUE(__qslot);                     \
00179                 XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder);      \
00180                 dth(__holder);                                  \
00181                 --(__qslot)->elems; })
00182 
00183 #else /* !XENO_DEBUG(QUEUES) */
00184 
00185 static inline void insertq(xnqueue_t *qslot,
00186                            xnholder_t *head, xnholder_t *holder)
00187 {
00188         /* Insert the <holder> element before <head> */
00189         ath(head->last, holder);
00190         ++qslot->elems;
00191 }
00192 
00193 static inline void prependq(xnqueue_t *qslot, xnholder_t *holder)
00194 {
00195         /* Prepend the element to the queue */
00196         ath(&qslot->head, holder);
00197         ++qslot->elems;
00198 }
00199 
00200 static inline void appendq(xnqueue_t *qslot, xnholder_t *holder)
00201 {
00202         /* Append the element to the queue */
00203         ath(qslot->head.last, holder);
00204         ++qslot->elems;
00205 }
00206 
00207 static inline void removeq(xnqueue_t *qslot, xnholder_t *holder)
00208 {
00209         dth(holder);
00210         --qslot->elems;
00211 }
00212 
00213 #endif /* XENO_DEBUG(QUEUES) */
00214 
00215 static inline xnholder_t *getheadq(xnqueue_t *qslot)
00216 {
00217         xnholder_t *holder = qslot->head.next;
00218         return holder == &qslot->head ? NULL : holder;
00219 }
00220 
00221 static inline xnholder_t *getq(xnqueue_t *qslot)
00222 {
00223         xnholder_t *holder = getheadq(qslot);
00224         if (holder)
00225                 removeq(qslot, holder);
00226         return holder;
00227 }
00228 
00229 static inline xnholder_t *nextq(xnqueue_t *qslot, xnholder_t *holder)
00230 {
00231         xnholder_t *nextholder = holder->next;
00232         return nextholder == &qslot->head ? NULL : nextholder;
00233 }
00234 
00235 static inline xnholder_t *popq(xnqueue_t *qslot, xnholder_t *holder)
00236 {
00237         xnholder_t *nextholder = nextq(qslot, holder);
00238         removeq(qslot, holder);
00239         return nextholder;
00240 }
00241 
00242 static inline int countq(xnqueue_t *qslot)
00243 {
00244         return qslot->elems;
00245 }
00246 
00247 static inline int emptyq_p(xnqueue_t *qslot)
00248 {
00249         return qslot->head.next == &qslot->head;
00250 }
00251 
00252 static inline void moveq(xnqueue_t *dstq, xnqueue_t *srcq)
00253 {
00254         xnholder_t *headsrc = srcq->head.next;
00255         xnholder_t *tailsrc = srcq->head.last;
00256         xnholder_t *headdst = &dstq->head;
00257 
00258         if (emptyq_p(srcq))
00259                 return;
00260 
00261         /* srcq elements are moved to head of dstq (LIFO) */
00262         headsrc->last->next = tailsrc->next;
00263         tailsrc->next->last = headsrc->last;
00264         headsrc->last = headdst;
00265         tailsrc->next = headdst->next;
00266         headdst->next->last = tailsrc;
00267         headdst->next = headsrc;
00268         dstq->elems += srcq->elems;
00269         srcq->elems = 0;
00270 }
00271 
00272 /* Prioritized element holder */
00273 
00274 typedef struct xnpholder {
00275 
00276         xnholder_t plink;
00277         int prio;
00278 
00279 } xnpholder_t;
00280 
00281 static inline void initph(xnpholder_t *holder)
00282 {
00283         inith(&holder->plink);
00284         /* Priority is set upon queue insertion */
00285 }
00286 
00287 /* Prioritized element queue - we only manage a descending queuing
00288    order (highest numbered priorities are linked first). */
00289 
00290 typedef struct xnpqueue { xnqueue_t pqueue; } xnpqueue_t;
00291 
00292 static inline void initpq(xnpqueue_t *pqslot)
00293 {
00294         initq(&pqslot->pqueue);
00295 }
00296 
00297 static inline void insertpq(xnpqueue_t *pqslot,
00298                             xnpholder_t *head, xnpholder_t *holder)
00299 {
00300         /* Insert the <holder> element before <head> */
00301         insertq(&pqslot->pqueue, &head->plink, &holder->plink);
00302 }
00303 
00304 static inline void insertpqf(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
00305 {
00306         /* Insert the element at the end of its priority group (FIFO) */
00307 
00308         xnholder_t *curr;
00309 
00310         for (curr = pqslot->pqueue.head.last;
00311              curr != &pqslot->pqueue.head; curr = curr->last) {
00312                 if (prio <= ((xnpholder_t *)curr)->prio)
00313                         break;
00314         }
00315 
00316         holder->prio = prio;
00317 
00318         insertq(&pqslot->pqueue, curr->next, &holder->plink);
00319 }
00320 
00321 static inline void insertpql(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
00322 {
00323         /* Insert the element at the front of its priority group (LIFO) */
00324 
00325         xnholder_t *curr;
00326 
00327         for (curr = pqslot->pqueue.head.next;
00328              curr != &pqslot->pqueue.head; curr = curr->next) {
00329                 if (prio >= ((xnpholder_t *)curr)->prio)
00330                         break;
00331         }
00332 
00333         holder->prio = prio;
00334 
00335         insertq(&pqslot->pqueue, curr, &holder->plink);
00336 }
00337 
00338 static inline xnpholder_t *findpqh(xnpqueue_t *pqslot, int prio)
00339 {
00340         /* Find the element heading a given priority group */
00341 
00342         xnholder_t *curr;
00343 
00344         for (curr = pqslot->pqueue.head.next;
00345              curr != &pqslot->pqueue.head; curr = curr->next) {
00346                 if (prio >= ((xnpholder_t *)curr)->prio)
00347                         break;
00348         }
00349 
00350         if (curr && ((xnpholder_t *)curr)->prio == prio)
00351                 return (xnpholder_t *)curr;
00352 
00353         return NULL;
00354 }
00355 
00356 static inline void insertpqfr(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
00357 {
00358         /*
00359          * Insert the element at the front of its priority group
00360          * (FIFO) - Reverse queueing applied (lowest numbered
00361          * priorities are put at front).
00362          */
00363         xnholder_t *curr;
00364 
00365         for (curr = pqslot->pqueue.head.last;
00366              curr != &pqslot->pqueue.head; curr = curr->last) {
00367                 if (prio >= ((xnpholder_t *)curr)->prio)
00368                         break;
00369         }
00370 
00371         holder->prio = prio;
00372 
00373         insertq(&pqslot->pqueue, curr->next, &holder->plink);
00374 }
00375 
00376 static inline void insertpqlr(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
00377 {
00378         /*
00379          * Insert the element at the front of its priority group
00380          * (LIFO) - Reverse queueing applied (lowest numbered
00381          * priorities are put at front).
00382          */
00383         xnholder_t *curr;
00384 
00385         for (curr = pqslot->pqueue.head.next;
00386              curr != &pqslot->pqueue.head; curr = curr->next) {
00387                 if (prio <= ((xnpholder_t *)curr)->prio)
00388                         break;
00389         }
00390 
00391         holder->prio = prio;
00392 
00393         insertq(&pqslot->pqueue, curr, &holder->plink);
00394 }
00395 
00396 static inline xnpholder_t *findpqhr(xnpqueue_t *pqslot, int prio)
00397 {
00398         /*
00399          * Find the element heading a given priority group - Reverse
00400          * queueing assumed (lowest numbered priorities should be at
00401          * front).
00402          */
00403         xnholder_t *curr;
00404 
00405         for (curr = pqslot->pqueue.head.next;
00406              curr != &pqslot->pqueue.head; curr = curr->next) {
00407                 if (prio <= ((xnpholder_t *)curr)->prio)
00408                         break;
00409         }
00410 
00411         if (curr && ((xnpholder_t *)curr)->prio == prio)
00412                 return (xnpholder_t *)curr;
00413 
00414         return NULL;
00415 }
00416 
00417 static inline void appendpq(xnpqueue_t *pqslot, xnpholder_t *holder)
00418 {
00419         holder->prio = 0;
00420         appendq(&pqslot->pqueue, &holder->plink);
00421 }
00422 
00423 static inline void prependpq(xnpqueue_t *pqslot, xnpholder_t *holder)
00424 {
00425         holder->prio = 0;
00426         prependq(&pqslot->pqueue, &holder->plink);
00427 }
00428 
00429 static inline void removepq(xnpqueue_t *pqslot, xnpholder_t *holder)
00430 {
00431         removeq(&pqslot->pqueue, &holder->plink);
00432 }
00433 
00434 static inline xnpholder_t *getheadpq(xnpqueue_t *pqslot)
00435 {
00436         return (xnpholder_t *)getheadq(&pqslot->pqueue);
00437 }
00438 
00439 static inline xnpholder_t *nextpq(xnpqueue_t *pqslot, xnpholder_t *holder)
00440 {
00441         return (xnpholder_t *)nextq(&pqslot->pqueue, &holder->plink);
00442 }
00443 
00444 static inline xnpholder_t *getpq(xnpqueue_t *pqslot)
00445 {
00446         return (xnpholder_t *)getq(&pqslot->pqueue);
00447 }
00448 
00449 static inline xnpholder_t *poppq(xnpqueue_t *pqslot, xnpholder_t *holder)
00450 {
00451         return (xnpholder_t *)popq(&pqslot->pqueue, &holder->plink);
00452 }
00453 
00454 static inline int countpq(xnpqueue_t *pqslot)
00455 {
00456         return countq(&pqslot->pqueue);
00457 }
00458 
00459 static inline int emptypq_p(xnpqueue_t *pqslot)
00460 {
00461         return emptyq_p(&pqslot->pqueue);
00462 }
00463 
00464 /* Generic prioritized element holder */
00465 
00466 typedef struct xngholder {
00467 
00468         xnpholder_t glink;
00469         void *data;
00470 
00471 } xngholder_t;
00472 
00473 static inline void initgh(xngholder_t *holder, void *data)
00474 {
00475         inith(&holder->glink.plink);
00476         holder->data = data;
00477 }
00478 
00479 /* Generic element queue */
00480 
00481 typedef struct xngqueue {
00482 
00483         xnpqueue_t gqueue;
00484         xnqueue_t *freehq;
00485         void (*starvation) (xnqueue_t *);
00486         int threshold;
00487 
00488 } xngqueue_t;
00489 
00490 static inline void initgq(xngqueue_t *gqslot,
00491                           xnqueue_t *freehq,
00492                           void (*starvation) (xnqueue_t *),
00493                           int threshold)
00494 {
00495         initpq(&gqslot->gqueue);
00496         gqslot->freehq = freehq;
00497         gqslot->starvation = starvation;
00498         gqslot->threshold = threshold;
00499 }
00500 
00501 static inline xngholder_t *allocgh(xngqueue_t *gqslot)
00502 {
00503         if (countq(gqslot->freehq) < gqslot->threshold)
00504                 gqslot->starvation(gqslot->freehq);
00505 
00506         return (xngholder_t *)getq(gqslot->freehq);
00507 }
00508 
00509 static inline void *removegh(xngqueue_t *gqslot, xngholder_t *holder)
00510 {
00511         removepq(&gqslot->gqueue, &holder->glink);
00512         appendq(gqslot->freehq, &holder->glink.plink);
00513         return holder->data;
00514 }
00515 
00516 static inline void insertgqf(xngqueue_t *gqslot, void *data, int prio)
00517 {
00518         xngholder_t *holder = allocgh(gqslot);
00519         holder->data = data;
00520         return insertpqf(&gqslot->gqueue, &holder->glink, prio);
00521 }
00522 
00523 static inline void insertgql(xngqueue_t *gqslot, void *data, int prio)
00524 {
00525         xngholder_t *holder = allocgh(gqslot);
00526         holder->data = data;
00527         insertpql(&gqslot->gqueue, &holder->glink, prio);
00528 }
00529 
00530 static inline void appendgq(xngqueue_t *gqslot, void *data)
00531 {
00532         xngholder_t *holder = allocgh(gqslot);
00533         holder->data = data;
00534         appendpq(&gqslot->gqueue, &holder->glink);
00535 }
00536 
00537 static inline void prependgq(xngqueue_t *gqslot, void *data)
00538 {
00539         xngholder_t *holder = allocgh(gqslot);
00540         holder->data = data;
00541         prependpq(&gqslot->gqueue, &holder->glink);
00542 }
00543 
00544 static inline xngholder_t *getheadgq(xngqueue_t *gqslot)
00545 {
00546         return (xngholder_t *)getheadpq(&gqslot->gqueue);
00547 }
00548 
00549 static inline xngholder_t *nextgq(xngqueue_t *gqslot, xngholder_t *holder)
00550 {
00551         return (xngholder_t *)nextpq(&gqslot->gqueue, &holder->glink);
00552 }
00553 
00554 static inline void *getgq(xngqueue_t *gqslot)
00555 {
00556         xngholder_t *holder = getheadgq(gqslot);
00557 
00558         if (!holder)
00559                 return NULL;
00560 
00561         appendq(gqslot->freehq, &getpq(&gqslot->gqueue)->plink);
00562 
00563         return holder->data;
00564 }
00565 
00566 static inline xngholder_t *popgq(xngqueue_t *gqslot, xngholder_t *holder)
00567 {
00568         xngholder_t *nextholder = nextgq(gqslot, holder);
00569         removegh(gqslot, holder);
00570         return nextholder;
00571 }
00572 
00573 static inline xngholder_t *findgq(xngqueue_t *gqslot, void *data)
00574 {
00575         xnholder_t *holder;
00576 
00577         for (holder = gqslot->gqueue.pqueue.head.next;
00578              holder != &gqslot->gqueue.pqueue.head; holder = holder->next) {
00579                 if (((xngholder_t *)holder)->data == data)
00580                         return (xngholder_t *)holder;
00581         }
00582 
00583         return NULL;
00584 }
00585 
00586 static inline void *removegq(xngqueue_t *gqslot, void *data)
00587 {
00588         xngholder_t *holder = findgq(gqslot, data);
00589         return holder ? removegh(gqslot, holder) : NULL;
00590 }
00591 
00592 static inline int countgq(xngqueue_t *gqslot)
00593 {
00594         return countpq(&gqslot->gqueue);
00595 }
00596 
00597 static inline int emptygq_p(xngqueue_t *gqslot)
00598 {
00599         return emptypq_p(&gqslot->gqueue);
00600 }
00601 
00602 #endif /* !_XENO_NUCLEUS_QUEUE_H */
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines