queue.h

00001 /*
00002  * Copyright (C) 2001,2002,2003 Philippe Gerum <rpm@xenomai.org>.
00003  *
00004  * RTAI/fusion is free software; you can redistribute it and/or modify
00005  * it under the terms of the GNU General Public License as published
00006  * by the Free Software Foundation; either version 2 of the License,
00007  * or (at your option) any later version.
00008  *
00009  * RTAI/fusion is distributed in the hope that it will be useful, but
00010  * WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00012  * General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with RTAI/fusion; if not, write to the Free Software
00016  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
00017  * 02111-1307, USA.
00018  */
00019 
00020 #ifndef _RTAI_NUCLEUS_QUEUE_H
00021 #define _RTAI_NUCLEUS_QUEUE_H
00022 
00023 #include <nucleus/types.h>
00024 
00025 /* Basic element holder */
00026 
00027 typedef struct xnholder {
00028 
00029     struct xnholder *next;
00030     struct xnholder *last;
00031 
00032 } xnholder_t;
00033 
00034 static inline void inith (xnholder_t *holder) {
00035     /* Holding queues are doubly-linked and circular */
00036     holder->last = holder;
00037     holder->next = holder;
00038 }
00039 
00040 static inline void ath (xnholder_t *head,
00041                         xnholder_t *holder) {
00042     /* Inserts the new element right after the heading one  */
00043     holder->last = head;
00044     holder->next = head->next;
00045     holder->next->last = holder;
00046     head->next = holder;
00047 }
00048 
00049 static inline void dth (xnholder_t *holder) {
00050     holder->last->next = holder->next;
00051     holder->next->last = holder->last;
00052 }
00053 
00054 /* Basic element queue */
00055 
00056 typedef struct xnqueue {
00057 
00058     xnholder_t head;
00059     int elems;
00060 #if defined(__KERNEL__) && defined(CONFIG_RTAI_OPT_DEBUG) && defined(CONFIG_SMP)
00061     xnlock_t lock;
00062 #endif /* __KERNEL__ && CONFIG_RTAI_OPT_DEBUG && CONFIG_SMP */
00063 
00064 } xnqueue_t;
00065 
00066 #if defined(CONFIG_RTAI_OPT_DEBUG) && defined(CONFIG_SMP)
00067 #define DECLARE_XNQUEUE(q) xnqueue_t q = { { &(q).head, &(q).head }, 0, XNARCH_LOCK_UNLOCKED }
00068 #else /* !(CONFIG_RTAI_OPT_DEBUG && CONFIG_SMP) */
00069 #define DECLARE_XNQUEUE(q) xnqueue_t q = { { &(q).head, &(q).head }, 0 }
00070 #endif /* CONFIG_RTAI_OPT_DEBUG && CONFIG_SMP */
00071 
00072 static inline void initq (xnqueue_t *qslot) {
00073     inith(&qslot->head);
00074     qslot->elems = 0;
00075 #if defined(__KERNEL__) && defined(CONFIG_RTAI_OPT_DEBUG) && defined(CONFIG_SMP)
00076     xnlock_init(&qslot->lock);
00077 #endif /* __KERNEL__ && CONFIG_RTAI_OPT_DEBUG && CONFIG_SMP */
00078 }
00079 
00080 #ifdef CONFIG_RTAI_OPT_DEBUG
00081 
00082 #if defined(__KERNEL__) || defined(__RTAI_UVM__) || defined(__RTAI_SIM__)
00083 
00084 #define XENO_DEBUG_CHECK_QUEUE(__qslot)         \
00085 do { \
00086     xnholder_t *curr; \
00087     spl_t s; \
00088     int nelems = 0; \
00089     xnlock_get_irqsave(&(__qslot)->lock,s);     \
00090     curr = (__qslot)->head.last;                                  \
00091     while (curr != &(__qslot)->head && nelems < (__qslot)->elems)       \
00092         curr = curr->last, nelems++; \
00093     if (curr != &(__qslot)->head || nelems != (__qslot)->elems)   \
00094         xnpod_fatal("corrupted queue, qslot->elems=%d, qslot=%p at %s:%d", \
00095                     (__qslot)->elems,                             \
00096                     __qslot,                                      \
00097                     __FILE__,__LINE__);                           \
00098     xnlock_put_irqrestore(&(__qslot)->lock,s);  \
00099 } while(0)
00100 
00101 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder)               \
00102 do { \
00103     xnholder_t *curr; \
00104     spl_t s; \
00105     xnlock_get_irqsave(&(__qslot)->lock,s);     \
00106     curr = (__qslot)->head.last;                           \
00107     while (curr != &(__qslot)->head && (__holder) != curr)      \
00108         curr = curr->last; \
00109     if (curr == (__holder))                                         \
00110         xnpod_fatal("inserting element twice, holder=%p, qslot=%p at %s:%d", \
00111                     __holder, \
00112                     __qslot, \
00113                     __FILE__,__LINE__); \
00114     if ((__holder)->last == NULL)                                  \
00115         xnpod_fatal("holder=%p not initialized, qslot=%p", \
00116                     __holder, \
00117                     __qslot); \
00118     xnlock_put_irqrestore(&(__qslot)->lock,s);  \
00119 } while(0)
00120 
00121 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder)       \
00122 do { \
00123     xnholder_t *curr; \
00124     spl_t s; \
00125     xnlock_get_irqsave(&(__qslot)->lock,s);     \
00126     curr = (__qslot)->head.last;                           \
00127     while (curr != &(__qslot)->head && (__holder) != curr)      \
00128         curr = curr->last; \
00129     if (curr == &(__qslot)->head)                                               \
00130         xnpod_fatal("removing non-linked element, holder=%p, qslot=%p at %s:%d", \
00131                     __holder, \
00132                     __qslot,    \
00133                     __FILE__,__LINE__); \
00134     xnlock_put_irqrestore(&(__qslot)->lock,s);  \
00135 } while(0)
00136 
00137 #else /* !(__KERNEL__ || __RTAI_UVM__ || __RTAI_SIM__) */
00138 
00139 /* Disable queue checks in user-space code which does not run as part
00140    of any virtual machine, e.g. skin syslibs. */
00141 
00142 #define XENO_DEBUG_CHECK_QUEUE(__qslot)
00143 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder)
00144 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder)
00145 
00146 #endif /* __KERNEL__ || __RTAI_UVM__ || __RTAI_SIM__ */
00147 
00148 /* Write the following as macros so that line numbering information
00149    keeps pointing at the real caller in diagnosis messages. */
00150 
00151 #define insertq(__qslot,__head,__holder)        \
00152 ({ XENO_DEBUG_CHECK_QUEUE(__qslot);             \
00153    XENO_DEBUG_INSERT_QUEUE(__qslot,__holder);   \
00154    ath((__head)->last,__holder);                \
00155    ++(__qslot)->elems; })
00156 
00157 #define prependq(__qslot,__holder)              \
00158 ({ XENO_DEBUG_CHECK_QUEUE(__qslot);             \
00159    XENO_DEBUG_INSERT_QUEUE(__qslot,__holder);   \
00160    ath(&(__qslot)->head,__holder);              \
00161    ++(__qslot)->elems; })
00162 
00163 #define appendq(__qslot,__holder)               \
00164 ({ XENO_DEBUG_CHECK_QUEUE(__qslot);             \
00165    XENO_DEBUG_INSERT_QUEUE(__qslot,__holder);   \
00166    ath((__qslot)->head.last,__holder);          \
00167    ++(__qslot)->elems; })
00168 
00169 #define removeq(__qslot,__holder)               \
00170 ({ XENO_DEBUG_CHECK_QUEUE(__qslot);             \
00171    XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder);   \
00172    dth(__holder);                               \
00173    --(__qslot)->elems; })
00174 
00175 #else /* !CONFIG_RTAI_OPT_DEBUG */
00176 
00177 static inline int insertq (xnqueue_t *qslot,
00178                            xnholder_t *head,
00179                            xnholder_t *holder)
00180 {
00181     /* Insert the <holder> element before <head> */
00182     ath(head->last,holder);
00183     return ++qslot->elems;
00184 }
00185 
00186 static inline int prependq (xnqueue_t *qslot,
00187                             xnholder_t *holder)
00188 {
00189     /* Prepend the element to the queue */
00190     ath(&qslot->head,holder);
00191     return ++qslot->elems;
00192 }
00193 
00194 static inline int appendq (xnqueue_t *qslot,
00195                            xnholder_t *holder)
00196 {
00197     /* Append the element to the queue */
00198     ath(qslot->head.last,holder);
00199     return ++qslot->elems;
00200 }
00201 
00202 static inline int removeq (xnqueue_t *qslot,
00203                            xnholder_t *holder)
00204 {
00205     dth(holder);
00206     return --qslot->elems;
00207 }
00208 
00209 #endif /* CONFIG_RTAI_OPT_DEBUG */
00210 
00211 static inline xnholder_t *getheadq (xnqueue_t *qslot)
00212 {
00213     xnholder_t *holder = qslot->head.next;
00214     if (holder == &qslot->head) return NULL;
00215     return holder;
00216 }
00217 
00218 static inline xnholder_t *getq (xnqueue_t *qslot)
00219 {
00220     xnholder_t *holder = getheadq(qslot);
00221     if (holder) removeq(qslot,holder);
00222     return holder;
00223 }
00224 
00225 static inline xnholder_t *nextq (xnqueue_t *qslot,
00226                                  xnholder_t *holder)
00227 {
00228     xnholder_t *nextholder = holder->next;
00229     return nextholder == &qslot->head ? NULL : nextholder;
00230 }
00231 
00232 static inline xnholder_t *popq (xnqueue_t *qslot,
00233                                 xnholder_t *holder)
00234 {
00235     xnholder_t *nextholder = nextq(qslot,holder);
00236     removeq(qslot,holder);
00237     return nextholder;
00238 }
00239 
00240 static inline int countq (xnqueue_t *qslot)
00241 {
00242     return qslot->elems;
00243 }
00244 
00245 static inline int moveq (xnqueue_t *dstq, xnqueue_t *srcq)
00246 {
00247     xnholder_t *headsrc = srcq->head.next;
00248     xnholder_t *tailsrc = srcq->head.last->last;
00249     xnholder_t *headdst = &dstq->head;
00250 
00251     headsrc->last->next = tailsrc->next;
00252     tailsrc->next->last = headsrc->last;
00253     headsrc->last = headdst;
00254     tailsrc->next = headdst->next;
00255     headdst->next->last = tailsrc;
00256     headdst->next = headsrc;
00257     dstq->elems += srcq->elems;
00258     srcq->elems = 0;
00259 
00260     return dstq->elems;
00261 }
00262 
00263 /* Prioritized element holder */
00264 
00265 typedef struct xnpholder {
00266 
00267     xnholder_t plink;
00268     int prio;
00269 
00270 } xnpholder_t;
00271 
00272 static inline void initph (xnpholder_t *holder) {
00273     inith(&holder->plink);
00274     /* Priority is set upon queue insertion */
00275 }
00276 
00277 /* Prioritized element queue */
00278 
00279 #define xnqueue_up   (-1)
00280 #define xnqueue_down   1
00281 
00282 typedef struct xnpqueue {
00283 
00284     xnqueue_t pqueue;
00285     int qdir;
00286 
00287 } xnpqueue_t;
00288 
00289 static inline void initpq (xnpqueue_t *pqslot,
00290                            int qdir,
00291                            int maxpri)
00292 {
00293     initq(&pqslot->pqueue);
00294     pqslot->qdir = qdir;
00295 }
00296 
00297 static inline int insertpq (xnpqueue_t *pqslot,
00298                             xnpholder_t *head,
00299                             xnpholder_t *holder)
00300 {
00301     /* Insert the <holder> element before <head> */
00302     return insertq(&pqslot->pqueue,&head->plink,&holder->plink);
00303 }
00304 
00305 static inline int insertpqf (xnpqueue_t *pqslot,
00306                              xnpholder_t *holder,
00307                              int prio)
00308 {
00309     /* Insert the element at the end of its priority group (FIFO) */
00310 
00311     xnholder_t *curr;
00312 
00313     if (pqslot->qdir == xnqueue_down) {
00314         for (curr = pqslot->pqueue.head.last;
00315              curr != &pqslot->pqueue.head; curr = curr->last) {
00316                 if (prio <= ((xnpholder_t *)curr)->prio)
00317                     break;
00318         }
00319     }
00320     else {
00321         for (curr = pqslot->pqueue.head.last;
00322              curr != &pqslot->pqueue.head; curr = curr->last) {
00323                 if (prio >= ((xnpholder_t *)curr)->prio)
00324                     break;
00325         }
00326     }
00327 
00328     holder->prio = prio;
00329 
00330     return insertq(&pqslot->pqueue,curr->next,&holder->plink);
00331 }
00332 
00333 static inline int insertpql (xnpqueue_t *pqslot,
00334                              xnpholder_t *holder,
00335                              int prio)
00336 {
00337     /* Insert the element at the front of its priority group (LIFO) */
00338 
00339     xnholder_t *curr;
00340 
00341     if (pqslot->qdir == xnqueue_down) {
00342         for (curr = pqslot->pqueue.head.next;
00343              curr != &pqslot->pqueue.head; curr = curr->next) {
00344                 if (prio >= ((xnpholder_t *)curr)->prio)
00345                     break;
00346         }
00347     }
00348     else {
00349         for (curr = pqslot->pqueue.head.next;
00350              curr != &pqslot->pqueue.head; curr = curr->next) {
00351                 if (prio <= ((xnpholder_t *)curr)->prio)
00352                     break;
00353         }
00354     }
00355 
00356     holder->prio = prio;
00357 
00358     return insertq(&pqslot->pqueue,curr,&holder->plink);
00359 }
00360 
00361 static inline xnpholder_t *findpqh (xnpqueue_t *pqslot,
00362                                     int prio)
00363 {
00364     /* Find the element heading a given priority group */
00365 
00366     xnholder_t *curr;
00367 
00368     if (pqslot->qdir == xnqueue_down) {
00369         for (curr = pqslot->pqueue.head.next;
00370              curr != &pqslot->pqueue.head; curr = curr->next) {
00371                 if (prio >= ((xnpholder_t *)curr)->prio)
00372                     break;
00373         }
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 int appendpq (xnpqueue_t *pqslot,
00390                             xnpholder_t *holder)
00391 {
00392     holder->prio = 0;
00393     return appendq(&pqslot->pqueue,&holder->plink);
00394 }
00395 
00396 static inline int prependpq (xnpqueue_t *pqslot,
00397                              xnpholder_t *holder)
00398 {
00399     holder->prio = 0;
00400     return prependq(&pqslot->pqueue,&holder->plink);
00401 }
00402 
00403 static inline int removepq (xnpqueue_t *pqslot,
00404                             xnpholder_t *holder)
00405 {
00406     return removeq(&pqslot->pqueue,&holder->plink);
00407 }
00408 
00409 static inline xnpholder_t *getheadpq (xnpqueue_t *pqslot)
00410 {
00411     return (xnpholder_t *)getheadq(&pqslot->pqueue);
00412 }
00413 
00414 static inline xnpholder_t *nextpq (xnpqueue_t *pqslot,
00415                                    xnpholder_t *holder)
00416 {
00417     return (xnpholder_t *)nextq(&pqslot->pqueue,&holder->plink);
00418 }
00419 
00420 static inline xnpholder_t *getpq (xnpqueue_t *pqslot)
00421 {
00422     return (xnpholder_t *)getq(&pqslot->pqueue);
00423 }
00424 
00425 static inline xnpholder_t *poppq (xnpqueue_t *pqslot,
00426                                   xnpholder_t *holder)
00427 {
00428     return (xnpholder_t *)popq(&pqslot->pqueue,&holder->plink);
00429 }
00430 
00431 static inline int countpq (xnpqueue_t *pqslot)
00432 {
00433     return countq(&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,
00466                            int qdir,
00467                            int maxpri)
00468 {
00469     initpq(&gqslot->gqueue,qdir,maxpri);
00470     gqslot->freehq = freehq;
00471     gqslot->starvation = starvation;
00472     gqslot->threshold = threshold;
00473 }
00474 
00475 static inline xngholder_t *allocgh (xngqueue_t *gqslot)
00476 {
00477     if (countq(gqslot->freehq) < gqslot->threshold)
00478         gqslot->starvation(gqslot->freehq);
00479 
00480     return (xngholder_t *)getq(gqslot->freehq);
00481 }
00482 
00483 static inline void *removegh (xngqueue_t *gqslot,
00484                               xngholder_t *holder)
00485 {
00486     removepq(&gqslot->gqueue,&holder->glink);
00487     appendq(gqslot->freehq,&holder->glink.plink);
00488     return holder->data;
00489 }
00490 
00491 static inline int insertgqf (xngqueue_t *gqslot,
00492                              void *data,
00493                              int prio)
00494 {
00495     xngholder_t *holder = allocgh(gqslot);
00496     holder->data = data;
00497     return insertpqf(&gqslot->gqueue,&holder->glink,prio);
00498 }
00499 
00500 static inline int insertgql (xngqueue_t *gqslot,
00501                              void *data,
00502                              int prio)
00503 {
00504     xngholder_t *holder = allocgh(gqslot);
00505     holder->data = data;
00506     return insertpql(&gqslot->gqueue,&holder->glink,prio);
00507 }
00508 
00509 static inline int appendgq (xngqueue_t *gqslot,
00510                             void *data)
00511 {
00512     xngholder_t *holder = allocgh(gqslot);
00513     holder->data = data;
00514     return appendpq(&gqslot->gqueue,&holder->glink);
00515 }
00516 
00517 static inline int prependgq (xngqueue_t *gqslot,
00518                              void *data)
00519 {
00520     xngholder_t *holder = allocgh(gqslot);
00521     holder->data = data;
00522     return prependpq(&gqslot->gqueue,&holder->glink);
00523 }
00524 
00525 static inline xngholder_t *getheadgq (xngqueue_t *gqslot)
00526 {
00527     return (xngholder_t *)getheadpq(&gqslot->gqueue);
00528 }
00529 
00530 static inline xngholder_t *nextgq (xngqueue_t *gqslot,
00531                                    xngholder_t *holder)
00532 {
00533     return (xngholder_t *)nextpq(&gqslot->gqueue,&holder->glink);
00534 }
00535 
00536 static inline void *getgq (xngqueue_t *gqslot)
00537 {
00538     xngholder_t *holder = getheadgq(gqslot);
00539     if (!holder) return NULL;
00540     appendq(gqslot->freehq,&getpq(&gqslot->gqueue)->plink);
00541     return holder->data;
00542 }
00543 
00544 static inline xngholder_t *popgq (xngqueue_t *gqslot,
00545                                   xngholder_t *holder)
00546 {
00547     xngholder_t *nextholder = nextgq(gqslot,holder);
00548     removegh(gqslot,holder);
00549     return nextholder;
00550 }
00551 
00552 static inline xngholder_t *findgq (xngqueue_t *gqslot,
00553                                    void *data)
00554 {
00555     xnholder_t *holder;
00556 
00557     for (holder = gqslot->gqueue.pqueue.head.next;
00558          holder != &gqslot->gqueue.pqueue.head; holder = holder->next) {
00559          if (((xngholder_t *)holder)->data == data)
00560              return (xngholder_t *)holder;
00561         }
00562 
00563     return NULL;
00564 }
00565 
00566 static inline void *removegq (xngqueue_t *gqslot,
00567                               void *data)
00568 {
00569     xngholder_t *holder = findgq(gqslot,data);
00570     return holder ? removegh(gqslot,holder) : NULL;
00571 }
00572 
00573 static inline int countgq (xngqueue_t *gqslot)
00574 {
00575     return countpq(&gqslot->gqueue);
00576 }
00577 
00578 /* Multi-level priority queue */
00579 
00580 #define XNSPQUEUE_PRIOS      (CONFIG_RTAI_OPT_SCALABLE_PRIOS + 1)
00581 #define XNSPQUEUE_BITMAPSIZE ((XNSPQUEUE_PRIOS + BITS_PER_LONG - 1) / BITS_PER_LONG)
00582 
00583 typedef struct xnspqueue {
00584 
00585     int qdir, maxpri;
00586 
00587     unsigned int elems;
00588 
00589     u_long bitmap[XNSPQUEUE_BITMAPSIZE];
00590 
00591     struct xnqueue queue[XNSPQUEUE_PRIOS];
00592 
00593 } xnspqueue_t;
00594 
00595 static inline unsigned int countspq(xnspqueue_t *spqslot)
00596 {
00597     return spqslot->elems;
00598 }
00599 
00600 static inline unsigned int xnspqueue_prio2idx(xnspqueue_t *spqslot, int prio)
00601 {
00602     if (spqslot->qdir == xnqueue_up) /* minprio > maxprio */
00603         return prio - spqslot->maxpri;
00604     else
00605         return spqslot->maxpri - prio;
00606 }
00607 
00608 static inline unsigned int xnspqueue_find_firstbit(xnspqueue_t *spqslot)
00609 {
00610     int i;
00611 
00612 /* simplest approach for now */
00613     for (i = 0; i < XNSPQUEUE_BITMAPSIZE; i++)
00614         {
00615         if (spqslot->bitmap[i])
00616             return ffnz(spqslot->bitmap[i]) + i*BITS_PER_LONG;
00617         }
00618 
00619     return 0; /* something already had gone wrong before this code was called */
00620 }
00621 
00622 static inline void initspq(xnspqueue_t *spqslot, int qdir, int maxpri)
00623 {
00624     int i;
00625 
00626     spqslot->elems = 0;
00627     spqslot->qdir  = qdir;
00628     spqslot->maxpri = maxpri;
00629     
00630     memset(&spqslot->bitmap, 0, XNSPQUEUE_BITMAPSIZE*sizeof(u_long));
00631     
00632     for (i = 0; i < XNSPQUEUE_PRIOS; i++)
00633         initq(&spqslot->queue[i]);
00634 }
00635 
00636 #define XNSPQUEUE_APPEND        0
00637 #define XNSPQUEUE_PREPEND       1
00638 
00639 static inline void __xnspqueue_insert(xnspqueue_t *spqslot,
00640                                       xnpholder_t  *holder,
00641                                       unsigned int idx,
00642                                       int mode)
00643 {
00644     xnqueue_t *queue = &spqslot->queue[idx];
00645 
00646     if (mode == XNSPQUEUE_PREPEND) /* Hopefully, this should be optimized away. */
00647         prependq(queue, &holder->plink);
00648     else
00649         appendq(queue, &holder->plink);
00650 
00651     spqslot->elems++;
00652     __setbits(spqslot->bitmap[idx / BITS_PER_LONG],1 << (idx % BITS_PER_LONG));
00653 }
00654 
00655 static inline void insertspql(xnspqueue_t *spqslot,
00656                              xnpholder_t *holder,
00657                              int prio)
00658 {
00659     __xnspqueue_insert(spqslot, holder, xnspqueue_prio2idx(spqslot,prio), XNSPQUEUE_PREPEND);
00660     holder->prio = prio;
00661 }
00662 
00663 static inline void insertspqf(xnspqueue_t *spqslot,
00664                              xnpholder_t *holder,
00665                              int prio)
00666 {
00667     __xnspqueue_insert(spqslot, holder, xnspqueue_prio2idx(spqslot,prio), XNSPQUEUE_APPEND);
00668     holder->prio = prio;
00669 }
00670 
00671 static inline void appendspq(xnspqueue_t *spqslot,
00672                             xnpholder_t *holder)
00673 {
00674     __xnspqueue_insert(spqslot, holder, spqslot->maxpri, XNSPQUEUE_APPEND);
00675     holder->prio = spqslot->maxpri;
00676 }
00677 
00678 static inline void prependspq(xnspqueue_t *spqslot,
00679                              xnpholder_t *holder)
00680 {
00681     __xnspqueue_insert(spqslot, holder, 0, XNSPQUEUE_PREPEND);
00682     holder->prio = 0;
00683 }
00684 
00685 static inline void removespq(xnspqueue_t *spqslot,
00686                             xnpholder_t *holder)
00687 {
00688     unsigned int idx = xnspqueue_prio2idx(spqslot,holder->prio);
00689     xnqueue_t *queue = &spqslot->queue[idx];
00690 
00691     spqslot->elems--;
00692     
00693     removeq(queue, &holder->plink);
00694     
00695     if (!countq(queue))
00696         __clrbits(spqslot->bitmap[idx / BITS_PER_LONG],1 << (idx % BITS_PER_LONG));
00697 }
00698 
00699 static inline xnpholder_t* findspqh(xnspqueue_t *spqslot,
00700                                    int prio)
00701 {
00702     xnqueue_t *queue = &spqslot->queue[xnspqueue_prio2idx(spqslot,prio)];
00703     return (xnpholder_t *)getheadq(queue);
00704 }
00705 
00706 static inline xnpholder_t* getheadspq(xnspqueue_t *spqslot)
00707 {
00708     xnqueue_t *queue;
00709 
00710     if (!countspq(spqslot))
00711         return NULL;
00712 
00713     queue = &spqslot->queue[xnspqueue_find_firstbit(spqslot)];
00714     
00715     return (xnpholder_t *)getheadq(queue);
00716 }
00717 
00718 static inline xnpholder_t* getspq(xnspqueue_t *spqslot)
00719 {
00720     unsigned int idx;
00721     xnholder_t *holder;
00722     xnqueue_t *queue;
00723 
00724     if (!countspq(spqslot))
00725         return NULL;
00726 
00727     idx    = xnspqueue_find_firstbit(spqslot);
00728     queue  = &spqslot->queue[idx];
00729     holder = getq(queue);
00730 
00731     if (!holder)
00732         return NULL;
00733         
00734     spqslot->elems--;    
00735 
00736     if (!countq(queue))
00737         __clrbits(spqslot->bitmap[idx / BITS_PER_LONG],1 << (idx % BITS_PER_LONG));
00738     
00739     return (xnpholder_t *)holder;
00740 }
00741 
00742 #endif /* !_RTAI_NUCLEUS_QUEUE_H */

Generated on Sat Sep 3 12:32:47 2005 for RTAI Fusion API by  doxygen 1.4.2