include/nucleus/queue.h

00001 /*
00002  * Copyright (C) 2001,2002,2003 Philippe Gerum <rpm@xenomai.org>.
00003  * Copyright (C) 2005 Dmitry Adamushko <dmitry.adamushko@gmail.com>
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/core.h>
00026 
00027 /* debug support */
00028 #include <nucleus/assert.h>
00029 
00030 #ifndef CONFIG_XENO_OPT_DEBUG_QUEUES
00031 #define CONFIG_XENO_OPT_DEBUG_QUEUES 0
00032 #endif
00033 
00034 /* Basic element holder */
00035 
00036 typedef struct xnholder {
00037 
00038         struct xnholder *next;
00039         struct xnholder *last;
00040 
00041 } xnholder_t;
00042 
00043 static inline void inith(xnholder_t *holder)
00044 {
00045         /* Holding queues are doubly-linked and circular */
00046         holder->last = holder;
00047         holder->next = holder;
00048 }
00049 
00050 static inline void ath(xnholder_t *head, xnholder_t *holder)
00051 {
00052         /* Inserts the new element right after the heading one  */
00053         holder->last = head;
00054         holder->next = head->next;
00055         holder->next->last = holder;
00056         head->next = holder;
00057 }
00058 
00059 static inline void dth(xnholder_t *holder)
00060 {
00061         holder->last->next = holder->next;
00062         holder->next->last = holder->last;
00063 }
00064 
00065 /* Basic element queue */
00066 
00067 typedef struct xnqueue {
00068 
00069         xnholder_t head;
00070         int elems;
00071 #if defined(__KERNEL__) && XENO_DEBUG(QUEUES) && defined(CONFIG_SMP)
00072         xnlock_t lock;
00073 #endif                          /* __KERNEL__ && XENO_DEBUG(QUEUES) && CONFIG_SMP */
00074 
00075 } xnqueue_t;
00076 
00077 #if XENO_DEBUG(QUEUES) && defined(CONFIG_SMP)
00078 #define DECLARE_XNQUEUE(q) xnqueue_t q = { { &(q).head, &(q).head }, 0, XNARCH_LOCK_UNLOCKED }
00079 #else /* !(XENO_DEBUG(QUEUES) && CONFIG_SMP) */
00080 #define DECLARE_XNQUEUE(q) xnqueue_t q = { { &(q).head, &(q).head }, 0 }
00081 #endif /* XENO_DEBUG(QUEUES) && CONFIG_SMP */
00082 
00083 static inline void initq(xnqueue_t *qslot)
00084 {
00085         inith(&qslot->head);
00086         qslot->elems = 0;
00087 #if defined(__KERNEL__) && XENO_DEBUG(QUEUES) && defined(CONFIG_SMP)
00088         xnlock_init(&qslot->lock);
00089 #endif /* __KERNEL__ && XENO_DEBUG(QUEUES) && CONFIG_SMP */
00090 }
00091 
00092 #if XENO_DEBUG(QUEUES)
00093 
00094 #if defined(__KERNEL__) || defined(__XENO_SIM__)
00095 
00096 #define XENO_DEBUG_CHECK_QUEUE(__qslot)                         \
00097         do {                                                            \
00098                 xnholder_t *curr;                                       \
00099                 spl_t s;                                                \
00100                 int nelems = 0;                                         \
00101                 xnlock_get_irqsave(&(__qslot)->lock,s);                 \
00102                 curr = (__qslot)->head.last;                            \
00103                 while (curr != &(__qslot)->head && nelems < (__qslot)->elems) \
00104                         curr = curr->last, nelems++;                    \
00105                 if (curr != &(__qslot)->head || nelems != (__qslot)->elems) \
00106                         xnpod_fatal("corrupted queue, qslot->elems=%d/%d, qslot=%p at %s:%d", \
00107                                     nelems,                             \
00108                                     (__qslot)->elems,                   \
00109                                     __qslot,                            \
00110                                     __FILE__,__LINE__);                 \
00111                 xnlock_put_irqrestore(&(__qslot)->lock,s);              \
00112         } while(0)
00113 
00114 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder)                       \
00115         do {                                                            \
00116                 xnholder_t *curr;                                       \
00117                 spl_t s;                                                \
00118                 xnlock_get_irqsave(&(__qslot)->lock,s);                 \
00119                 curr = (__qslot)->head.last;                            \
00120                 while (curr != &(__qslot)->head && (__holder) != curr)  \
00121                         curr = curr->last;                              \
00122                 if (curr == (__holder))                                 \
00123                         xnpod_fatal("inserting element twice, holder=%p, qslot=%p at %s:%d", \
00124                                     __holder,                           \
00125                                     __qslot,                            \
00126                                     __FILE__,__LINE__);                 \
00127                 if ((__holder)->last == NULL)                           \
00128                         xnpod_fatal("holder=%p not initialized, qslot=%p", \
00129                                     __holder,                           \
00130                                     __qslot);                           \
00131                 xnlock_put_irqrestore(&(__qslot)->lock,s);              \
00132         } while(0)
00133 
00134 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder)                       \
00135         do {                                                            \
00136                 xnholder_t *curr;                                       \
00137                 spl_t s;                                                \
00138                 xnlock_get_irqsave(&(__qslot)->lock,s);                 \
00139                 curr = (__qslot)->head.last;                            \
00140                 while (curr != &(__qslot)->head && (__holder) != curr)  \
00141                         curr = curr->last;                              \
00142                 if (curr == &(__qslot)->head)                           \
00143                         xnpod_fatal("removing non-linked element, holder=%p, qslot=%p at %s:%d", \
00144                                     __holder,                           \
00145                                     __qslot,                            \
00146                                     __FILE__,__LINE__);                 \
00147                 xnlock_put_irqrestore(&(__qslot)->lock,s);              \
00148         } while(0)
00149 
00150 #else /* !(__KERNEL__ || __XENO_SIM__) */
00151 
00152 /* Disable queue checks in user-space code which does not run as part
00153    of any virtual machine, e.g. skin call interface libs. */
00154 
00155 #define XENO_DEBUG_CHECK_QUEUE(__qslot)
00156 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder)
00157 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder)
00158 
00159 #endif /* __KERNEL__ || __XENO_SIM__ */
00160 
00161 /* Write the following as macros so that line numbering information
00162    keeps pointing at the real caller in diagnosis messages. */
00163 
00164 #define insertq(__qslot,__head,__holder)                        \
00165         ({ XENO_DEBUG_CHECK_QUEUE(__qslot);                     \
00166                 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder);      \
00167                 ath((__head)->last,__holder);                   \
00168                 ++(__qslot)->elems; })
00169 
00170 #define prependq(__qslot,__holder)                              \
00171         ({ XENO_DEBUG_CHECK_QUEUE(__qslot);                     \
00172                 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder);      \
00173                 ath(&(__qslot)->head,__holder);                 \
00174                 ++(__qslot)->elems; })
00175 
00176 #define appendq(__qslot,__holder)                               \
00177         ({ XENO_DEBUG_CHECK_QUEUE(__qslot);                     \
00178                 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder);      \
00179                 ath((__qslot)->head.last,__holder);             \
00180                 ++(__qslot)->elems; })
00181 
00182 #define removeq(__qslot,__holder)                               \
00183         ({ XENO_DEBUG_CHECK_QUEUE(__qslot);                     \
00184                 XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder);      \
00185                 dth(__holder);                                  \
00186                 --(__qslot)->elems; })
00187 
00188 #else /* !XENO_DEBUG(QUEUES) */
00189 
00190 static inline void insertq(xnqueue_t *qslot,
00191                            xnholder_t *head, xnholder_t *holder)
00192 {
00193         /* Insert the <holder> element before <head> */
00194         ath(head->last, holder);
00195         ++qslot->elems;
00196 }
00197 
00198 static inline void prependq(xnqueue_t *qslot, xnholder_t *holder)
00199 {
00200         /* Prepend the element to the queue */
00201         ath(&qslot->head, holder);
00202         ++qslot->elems;
00203 }
00204 
00205 static inline void appendq(xnqueue_t *qslot, xnholder_t *holder)
00206 {
00207         /* Append the element to the queue */
00208         ath(qslot->head.last, holder);
00209         ++qslot->elems;
00210 }
00211 
00212 static inline void removeq(xnqueue_t *qslot, xnholder_t *holder)
00213 {
00214         dth(holder);
00215         --qslot->elems;
00216 }
00217 
00218 #endif /* XENO_DEBUG(QUEUES) */
00219 
00220 static inline xnholder_t *getheadq(xnqueue_t *qslot)
00221 {
00222         xnholder_t *holder = qslot->head.next;
00223         return holder == &qslot->head ? NULL : holder;
00224 }
00225 
00226 static inline xnholder_t *getq(xnqueue_t *qslot)
00227 {
00228         xnholder_t *holder = getheadq(qslot);
00229         if (holder)
00230                 removeq(qslot, holder);
00231         return holder;
00232 }
00233 
00234 static inline xnholder_t *nextq(xnqueue_t *qslot, xnholder_t *holder)
00235 {
00236         xnholder_t *nextholder = holder->next;
00237         return nextholder == &qslot->head ? NULL : nextholder;
00238 }
00239 
00240 static inline xnholder_t *popq(xnqueue_t *qslot, xnholder_t *holder)
00241 {
00242         xnholder_t *nextholder = nextq(qslot, holder);
00243         removeq(qslot, holder);
00244         return nextholder;
00245 }
00246 
00247 static inline int countq(xnqueue_t *qslot)
00248 {
00249         return qslot->elems;
00250 }
00251 
00252 static inline int emptyq_p(xnqueue_t *qslot)
00253 {
00254         return qslot->head.next == &qslot->head;
00255 }
00256 
00257 static inline void moveq(xnqueue_t *dstq, xnqueue_t *srcq)
00258 {
00259         xnholder_t *headsrc = srcq->head.next;
00260         xnholder_t *tailsrc = srcq->head.last;
00261         xnholder_t *headdst = &dstq->head;
00262 
00263         headsrc->last->next = tailsrc->next;
00264         tailsrc->next->last = headsrc->last;
00265         headsrc->last = headdst;
00266         tailsrc->next = headdst->next;
00267         headdst->next->last = tailsrc;
00268         headdst->next = headsrc;
00269         dstq->elems += srcq->elems;
00270         srcq->elems = 0;
00271 }
00272 
00273 /* Prioritized element holder */
00274 
00275 typedef struct xnpholder {
00276 
00277         xnholder_t plink;
00278         int prio;
00279 
00280 } xnpholder_t;
00281 
00282 static inline void initph(xnpholder_t *holder)
00283 {
00284         inith(&holder->plink);
00285         /* Priority is set upon queue insertion */
00286 }
00287 
00288 /* Prioritized element queue */
00289 
00290 #define xnqueue_up   (-1)
00291 #define xnqueue_down   1
00292 
00293 typedef struct xnpqueue {
00294 
00295         xnqueue_t pqueue;
00296         int qdir;
00297 
00298 } xnpqueue_t;
00299 
00300 static inline void initpq(xnpqueue_t *pqslot, int qdir)
00301 {
00302         initq(&pqslot->pqueue);
00303         pqslot->qdir = qdir;
00304 }
00305 
00306 static inline void insertpq(xnpqueue_t *pqslot,
00307                             xnpholder_t *head, xnpholder_t *holder)
00308 {
00309         /* Insert the <holder> element before <head> */
00310         insertq(&pqslot->pqueue, &head->plink, &holder->plink);
00311 }
00312 
00313 static inline void insertpqf(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
00314 {
00315         /* Insert the element at the end of its priority group (FIFO) */
00316 
00317         xnholder_t *curr;
00318 
00319         if (pqslot->qdir == xnqueue_down) {
00320                 for (curr = pqslot->pqueue.head.last;
00321                      curr != &pqslot->pqueue.head; curr = curr->last) {
00322                         if (prio <= ((xnpholder_t *)curr)->prio)
00323                                 break;
00324                 }
00325         } else {
00326                 for (curr = pqslot->pqueue.head.last;
00327                      curr != &pqslot->pqueue.head; curr = curr->last) {
00328                         if (prio >= ((xnpholder_t *)curr)->prio)
00329                                 break;
00330                 }
00331         }
00332 
00333         holder->prio = prio;
00334 
00335         insertq(&pqslot->pqueue, curr->next, &holder->plink);
00336 }
00337 
00338 static inline void insertpql(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
00339 {
00340         /* Insert the element at the front of its priority group (LIFO) */
00341 
00342         xnholder_t *curr;
00343 
00344         if (pqslot->qdir == xnqueue_down) {
00345                 for (curr = pqslot->pqueue.head.next;
00346                      curr != &pqslot->pqueue.head; curr = curr->next) {
00347                         if (prio >= ((xnpholder_t *)curr)->prio)
00348                                 break;
00349                 }
00350         } else {
00351                 for (curr = pqslot->pqueue.head.next;
00352                      curr != &pqslot->pqueue.head; curr = curr->next) {
00353                         if (prio <= ((xnpholder_t *)curr)->prio)
00354                                 break;
00355                 }
00356         }
00357 
00358         holder->prio = prio;
00359 
00360         insertq(&pqslot->pqueue, curr, &holder->plink);
00361 }
00362 
00363 static inline xnpholder_t *findpqh(xnpqueue_t *pqslot, int prio)
00364 {
00365         /* Find the element heading a given priority group */
00366 
00367         xnholder_t *curr;
00368 
00369         if (pqslot->qdir == xnqueue_down) {
00370                 for (curr = pqslot->pqueue.head.next;
00371                      curr != &pqslot->pqueue.head; curr = curr->next) {
00372                         if (prio >= ((xnpholder_t *)curr)->prio)
00373                                 break;
00374                 }
00375         } else {
00376                 for (curr = pqslot->pqueue.head.next;
00377                      curr != &pqslot->pqueue.head; curr = curr->next) {
00378                         if (prio <= ((xnpholder_t *)curr)->prio)
00379                                 break;
00380                 }
00381         }
00382 
00383         if (curr && ((xnpholder_t *)curr)->prio == prio)
00384                 return (xnpholder_t *)curr;
00385 
00386         return NULL;
00387 }
00388 
00389 static inline void appendpq(xnpqueue_t *pqslot, xnpholder_t *holder)
00390 {
00391         holder->prio = 0;
00392         appendq(&pqslot->pqueue, &holder->plink);
00393 }
00394 
00395 static inline void prependpq(xnpqueue_t *pqslot, xnpholder_t *holder)
00396 {
00397         holder->prio = 0;
00398         prependq(&pqslot->pqueue, &holder->plink);
00399 }
00400 
00401 static inline void removepq(xnpqueue_t *pqslot, xnpholder_t *holder)
00402 {
00403         removeq(&pqslot->pqueue, &holder->plink);
00404 }
00405 
00406 static inline xnpholder_t *getheadpq(xnpqueue_t *pqslot)
00407 {
00408         return (xnpholder_t *)getheadq(&pqslot->pqueue);
00409 }
00410 
00411 static inline xnpholder_t *nextpq(xnpqueue_t *pqslot, xnpholder_t *holder)
00412 {
00413         return (xnpholder_t *)nextq(&pqslot->pqueue, &holder->plink);
00414 }
00415 
00416 static inline xnpholder_t *getpq(xnpqueue_t *pqslot)
00417 {
00418         return (xnpholder_t *)getq(&pqslot->pqueue);
00419 }
00420 
00421 static inline xnpholder_t *poppq(xnpqueue_t *pqslot, xnpholder_t *holder)
00422 {
00423         return (xnpholder_t *)popq(&pqslot->pqueue, &holder->plink);
00424 }
00425 
00426 static inline int countpq(xnpqueue_t *pqslot)
00427 {
00428         return countq(&pqslot->pqueue);
00429 }
00430 
00431 static inline int emptypq_p(xnpqueue_t *pqslot)
00432 {
00433         return emptyq_p(&pqslot->pqueue);
00434 }
00435 
00436 /* Generic prioritized element holder */
00437 
00438 typedef struct xngholder {
00439 
00440         xnpholder_t glink;
00441         void *data;
00442 
00443 } xngholder_t;
00444 
00445 static inline void initgh(xngholder_t *holder, void *data)
00446 {
00447         inith(&holder->glink.plink);
00448         holder->data = data;
00449 }
00450 
00451 /* Generic element queue */
00452 
00453 typedef struct xngqueue {
00454 
00455         xnpqueue_t gqueue;
00456         xnqueue_t *freehq;
00457         void (*starvation) (xnqueue_t *);
00458         int threshold;
00459 
00460 } xngqueue_t;
00461 
00462 static inline void initgq(xngqueue_t *gqslot,
00463                           xnqueue_t *freehq,
00464                           void (*starvation) (xnqueue_t *),
00465                           int threshold, int qdir)
00466 {
00467         initpq(&gqslot->gqueue, qdir);
00468         gqslot->freehq = freehq;
00469         gqslot->starvation = starvation;
00470         gqslot->threshold = threshold;
00471 }
00472 
00473 static inline xngholder_t *allocgh(xngqueue_t *gqslot)
00474 {
00475         if (countq(gqslot->freehq) < gqslot->threshold)
00476                 gqslot->starvation(gqslot->freehq);
00477 
00478         return (xngholder_t *)getq(gqslot->freehq);
00479 }
00480 
00481 static inline void *removegh(xngqueue_t *gqslot, xngholder_t *holder)
00482 {
00483         removepq(&gqslot->gqueue, &holder->glink);
00484         appendq(gqslot->freehq, &holder->glink.plink);
00485         return holder->data;
00486 }
00487 
00488 static inline void insertgqf(xngqueue_t *gqslot, void *data, int prio)
00489 {
00490         xngholder_t *holder = allocgh(gqslot);
00491         holder->data = data;
00492         return insertpqf(&gqslot->gqueue, &holder->glink, prio);
00493 }
00494 
00495 static inline void insertgql(xngqueue_t *gqslot, void *data, int prio)
00496 {
00497         xngholder_t *holder = allocgh(gqslot);
00498         holder->data = data;
00499         insertpql(&gqslot->gqueue, &holder->glink, prio);
00500 }
00501 
00502 static inline void appendgq(xngqueue_t *gqslot, void *data)
00503 {
00504         xngholder_t *holder = allocgh(gqslot);
00505         holder->data = data;
00506         appendpq(&gqslot->gqueue, &holder->glink);
00507 }
00508 
00509 static inline void prependgq(xngqueue_t *gqslot, void *data)
00510 {
00511         xngholder_t *holder = allocgh(gqslot);
00512         holder->data = data;
00513         prependpq(&gqslot->gqueue, &holder->glink);
00514 }
00515 
00516 static inline xngholder_t *getheadgq(xngqueue_t *gqslot)
00517 {
00518         return (xngholder_t *)getheadpq(&gqslot->gqueue);
00519 }
00520 
00521 static inline xngholder_t *nextgq(xngqueue_t *gqslot, xngholder_t *holder)
00522 {
00523         return (xngholder_t *)nextpq(&gqslot->gqueue, &holder->glink);
00524 }
00525 
00526 static inline void *getgq(xngqueue_t *gqslot)
00527 {
00528         xngholder_t *holder = getheadgq(gqslot);
00529         if (!holder)
00530                 return NULL;
00531         appendq(gqslot->freehq, &getpq(&gqslot->gqueue)->plink);
00532         return holder->data;
00533 }
00534 
00535 static inline xngholder_t *popgq(xngqueue_t *gqslot, xngholder_t *holder)
00536 {
00537         xngholder_t *nextholder = nextgq(gqslot, holder);
00538         removegh(gqslot, holder);
00539         return nextholder;
00540 }
00541 
00542 static inline xngholder_t *findgq(xngqueue_t *gqslot, void *data)
00543 {
00544         xnholder_t *holder;
00545 
00546         for (holder = gqslot->gqueue.pqueue.head.next;
00547              holder != &gqslot->gqueue.pqueue.head; holder = holder->next) {
00548                 if (((xngholder_t *)holder)->data == data)
00549                         return (xngholder_t *)holder;
00550         }
00551 
00552         return NULL;
00553 }
00554 
00555 static inline void *removegq(xngqueue_t *gqslot, void *data)
00556 {
00557         xngholder_t *holder = findgq(gqslot, data);
00558         return holder ? removegh(gqslot, holder) : NULL;
00559 }
00560 
00561 static inline int countgq(xngqueue_t *gqslot)
00562 {
00563         return countpq(&gqslot->gqueue);
00564 }
00565 
00566 static inline int emptygq_p(xngqueue_t *gqslot)
00567 {
00568         return emptypq_p(&gqslot->gqueue);
00569 }
00570 
00571 #ifdef CONFIG_XENO_OPT_SCALABLE_SCHED
00572 
00573 /* Multi-level priority queue, suitable for handling the runnable
00574    thread queue. */
00575 
00576 #if BITS_PER_LONG * BITS_PER_LONG < XNCORE_NR_PRIO
00577 #error "Internal bitmap cannot hold so many priority levels"
00578 #endif
00579 
00580 #define __MLQ_LONGS ((XNCORE_NR_PRIO+BITS_PER_LONG-1)/BITS_PER_LONG)
00581 
00582 typedef struct xnmlqueue {
00583 
00584         int qdir, loprio, hiprio, elems;
00585 
00586         u_long himap, lomap[__MLQ_LONGS];
00587 
00588         struct xnqueue queue[XNCORE_NR_PRIO];
00589 
00590 } xnmlqueue_t;
00591 
00592 #undef __MLQ_LONGS
00593 
00594 static inline int countmlq(xnmlqueue_t *mlqslot)
00595 {
00596         return mlqslot->elems;
00597 }
00598 
00599 static inline int emptymlq_p(xnmlqueue_t *mlqslot)
00600 {
00601         return mlqslot->himap == 0;
00602 }
00603 
00604 static inline int indexmlq(xnmlqueue_t *mlqslot, int prio)
00605 {
00606         /* We need to rescale the priority level to a 0-based
00607            range. We use ffnz() to scan the bitmap which MUST be based
00608            on a bit scan forward op. Therefore, the lower the index
00609            value, the higher the priority (since least significant
00610            bits will be found first when scanning). */
00611 
00612         if (mlqslot->qdir == xnqueue_up)
00613                 /* loprio numerically greater than hiprio. */
00614                 return -mlqslot->hiprio + prio;
00615         else
00616                 return mlqslot->hiprio - prio;
00617 }
00618 
00619 static inline int ffsmlq(xnmlqueue_t *mlqslot)
00620 {
00621         int hi = ffnz(mlqslot->himap);
00622         int lo = ffnz(mlqslot->lomap[hi]);
00623         return hi * BITS_PER_LONG + lo; /* Result is undefined if none set. */
00624 }
00625 
00626 static inline void initmlq(xnmlqueue_t *mlqslot, int qdir, int loprio,
00627                            int hiprio)
00628 {
00629         int prio;
00630 
00631         mlqslot->elems = 0;
00632         mlqslot->qdir = qdir;
00633         mlqslot->loprio = loprio;
00634         mlqslot->hiprio = hiprio;
00635         mlqslot->himap = 0;
00636         memset(&mlqslot->lomap, 0, sizeof(mlqslot->lomap));
00637 
00638         for (prio = 0; prio < XNCORE_NR_PRIO; prio++)
00639                 initq(&mlqslot->queue[prio]);
00640 
00641         XENO_ASSERT(NUCLEUS, 
00642                     indexmlq(mlqslot, qdir == xnqueue_up ? loprio : hiprio) < XNCORE_NR_PRIO,
00643                     xnpod_fatal("priority range [%d..%d] is beyond multi-level "
00644                                 "queue indexing capabilities",
00645                                 loprio, hiprio));
00646 }
00647 
00648 #define XNMLQUEUE_APPEND   0
00649 #define XNMLQUEUE_PREPEND  1
00650 
00651 static inline void addmlq(xnmlqueue_t *mlqslot,
00652                           xnpholder_t *holder, int idx, int mode)
00653 {
00654         xnqueue_t *queue = &mlqslot->queue[idx];
00655         int hi = idx / BITS_PER_LONG;
00656         int lo = idx % BITS_PER_LONG;
00657 
00658         if (mode == XNMLQUEUE_PREPEND)  /* Hopefully, this should be optimized away. */
00659                 prependq(queue, &holder->plink);
00660         else
00661                 appendq(queue, &holder->plink);
00662 
00663         holder->prio = idx;
00664         mlqslot->elems++;
00665         __setbits(mlqslot->himap, 1UL << hi);
00666         __setbits(mlqslot->lomap[hi], 1UL << lo);
00667 }
00668 
00669 static inline void insertmlql(xnmlqueue_t *mlqslot,
00670                               xnpholder_t *holder, int prio)
00671 {
00672         addmlq(mlqslot, holder, indexmlq(mlqslot, prio), XNMLQUEUE_PREPEND);
00673 }
00674 
00675 static inline void insertmlqf(xnmlqueue_t *mlqslot,
00676                               xnpholder_t *holder, int prio)
00677 {
00678         addmlq(mlqslot, holder, indexmlq(mlqslot, prio), XNMLQUEUE_APPEND);
00679 }
00680 
00681 static inline void appendmlq(xnmlqueue_t *mlqslot, xnpholder_t *holder)
00682 {
00683         addmlq(mlqslot, holder, indexmlq(mlqslot, mlqslot->hiprio),
00684                XNMLQUEUE_APPEND);
00685 }
00686 
00687 static inline void prependmlq(xnmlqueue_t *mlqslot, xnpholder_t *holder)
00688 {
00689         addmlq(mlqslot, holder, indexmlq(mlqslot, mlqslot->loprio),
00690                XNMLQUEUE_PREPEND);
00691 }
00692 
00693 static inline void removemlq(xnmlqueue_t *mlqslot, xnpholder_t *holder)
00694 {
00695         int idx = holder->prio;
00696         xnqueue_t *queue = &mlqslot->queue[idx];
00697 
00698         mlqslot->elems--;
00699 
00700         removeq(queue, &holder->plink);
00701 
00702         if (emptyq_p(queue)) {
00703                 int hi = idx / BITS_PER_LONG;
00704                 int lo = idx % BITS_PER_LONG;
00705                 __clrbits(mlqslot->lomap[hi], 1UL << lo);
00706                 if (mlqslot->lomap[hi] == 0)
00707                         __clrbits(mlqslot->himap, 1UL << hi);
00708         }
00709 }
00710 
00711 static inline xnpholder_t *findmlqh(xnmlqueue_t *mlqslot, int prio)
00712 {
00713         xnqueue_t *queue = &mlqslot->queue[indexmlq(mlqslot, prio)];
00714         return (xnpholder_t *)getheadq(queue);
00715 }
00716 
00717 static inline xnpholder_t *getheadmlq(xnmlqueue_t *mlqslot)
00718 {
00719         xnpholder_t *holder;
00720         xnqueue_t *queue;
00721 
00722         if (emptymlq_p(mlqslot))
00723                 return NULL;
00724 
00725         queue = &mlqslot->queue[ffsmlq(mlqslot)];
00726         holder = (xnpholder_t *)getheadq(queue);
00727 
00728         XENO_ASSERT(QUEUES, holder,
00729                     xnpod_fatal
00730                     ("corrupted multi-level queue, qslot=%p at %s:%d", mlqslot,
00731                      __FILE__, __LINE__);
00732             );
00733 
00734         return holder;
00735 }
00736 
00737 static inline xnpholder_t *getmlq(xnmlqueue_t *mlqslot)
00738 {
00739         xnholder_t *holder;
00740         xnqueue_t *queue;
00741         int idx, hi, lo;
00742 
00743         if (emptymlq_p(mlqslot))
00744                 return NULL;
00745 
00746         idx = ffsmlq(mlqslot);
00747         queue = &mlqslot->queue[idx];
00748         holder = getq(queue);
00749 
00750         XENO_ASSERT(QUEUES, holder,
00751                     xnpod_fatal
00752                     ("corrupted multi-level queue, qslot=%p at %s:%d", mlqslot,
00753                      __FILE__, __LINE__);
00754             );
00755 
00756         mlqslot->elems--;
00757 
00758         if (emptyq_p(queue)) {
00759                 hi = idx / BITS_PER_LONG;
00760                 lo = idx % BITS_PER_LONG;
00761                 __clrbits(mlqslot->lomap[hi], 1UL << lo);
00762                 if (mlqslot->lomap[hi] == 0)
00763                         __clrbits(mlqslot->himap, 1UL << hi);
00764         }
00765 
00766         return (xnpholder_t *)holder;
00767 }
00768 
00769 #endif /* CONFIG_XENO_OPT_SCALABLE_SCHED */
00770 
00771 #endif /* !_XENO_NUCLEUS_QUEUE_H */

Generated on Sun Dec 9 10:37:17 2007 for Xenomai API by  doxygen 1.5.3