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

Generated on Mon Mar 24 18:02:40 2008 for Xenomai API by  doxygen 1.5.3