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() \
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", \
00095                     qslot->elems, \
00096                     qslot); \
00097     xnlock_put_irqrestore(&qslot->lock,s); \
00098 } while(0)
00099 
00100 #define XENO_DEBUG_INSERT_QUEUE() \
00101 do { \
00102     xnholder_t *curr; \
00103     spl_t s; \
00104     xnlock_get_irqsave(&qslot->lock,s); \
00105     curr = qslot->head.last; \
00106     while (curr != &qslot->head && holder != curr) \
00107         curr = curr->last; \
00108     if (curr == holder) \
00109         xnpod_fatal("inserting element twice, holder=%p, qslot=%p", \
00110                     holder, \
00111                     qslot); \
00112     if (holder->last == NULL)   /* Just a guess. */ \
00113         xnpod_fatal("holder=%p not initialized, qslot=%p", \
00114                     holder, \
00115                     qslot); \
00116     xnlock_put_irqrestore(&qslot->lock,s); \
00117 } while(0)
00118 
00119 #define XENO_DEBUG_REMOVE_QUEUE() \
00120 do { \
00121     xnholder_t *curr; \
00122     spl_t s; \
00123     xnlock_get_irqsave(&qslot->lock,s); \
00124     curr = qslot->head.last; \
00125     while (curr != &qslot->head && holder != curr) \
00126         curr = curr->last; \
00127     if (curr == &qslot->head) \
00128         xnpod_fatal("removing non-linked element, holder=%p, qslot=%p", \
00129                     holder, \
00130                     qslot); \
00131     xnlock_put_irqrestore(&qslot->lock,s); \
00132 } while(0)
00133 
00134 #else /* !(__KERNEL__ || __RTAI_UVM__ || __RTAI_SIM__) */
00135 
00136 /* Disable queue checks in user-space code which does not run as part
00137    of any virtual machine, e.g. skin syslibs. */
00138 
00139 #define XENO_DEBUG_CHECK_QUEUE()
00140 #define XENO_DEBUG_INSERT_QUEUE()
00141 #define XENO_DEBUG_REMOVE_QUEUE()
00142 
00143 #endif /* __KERNEL__ || __RTAI_UVM__ || __RTAI_SIM__ */
00144 
00145 #endif /* CONFIG_RTAI_OPT_DEBUG */
00146 
00147 static inline int insertq (xnqueue_t *qslot,
00148                            xnholder_t *head,
00149                            xnholder_t *holder)
00150 {
00151     /* Insert the <holder> element before <head> */
00152 #ifdef CONFIG_RTAI_OPT_DEBUG
00153     XENO_DEBUG_CHECK_QUEUE();
00154     XENO_DEBUG_INSERT_QUEUE();
00155 #endif /*CONFIG_RTAI_OPT_DEBUG */
00156     ath(head->last,holder);
00157     return ++qslot->elems;
00158 }
00159 
00160 static inline int prependq (xnqueue_t *qslot,
00161                             xnholder_t *holder)
00162 {
00163     /* Prepend the element to the queue */
00164 #ifdef CONFIG_RTAI_OPT_DEBUG
00165     XENO_DEBUG_CHECK_QUEUE();
00166     XENO_DEBUG_INSERT_QUEUE();
00167 #endif /* CONFIG_RTAI_OPT_DEBUG */
00168     ath(&qslot->head,holder);
00169     return ++qslot->elems;
00170 }
00171 
00172 static inline int appendq (xnqueue_t *qslot,
00173                            xnholder_t *holder)
00174 {
00175     /* Append the element to the queue */
00176 #ifdef CONFIG_RTAI_OPT_DEBUG
00177     XENO_DEBUG_CHECK_QUEUE();
00178     XENO_DEBUG_INSERT_QUEUE();
00179 #endif /* CONFIG_RTAI_OPT_DEBUG */
00180     ath(qslot->head.last,holder);
00181     return ++qslot->elems;
00182 }
00183 
00184 static inline int removeq (xnqueue_t *qslot,
00185                            xnholder_t *holder)
00186 {
00187 #ifdef CONFIG_RTAI_OPT_DEBUG
00188     XENO_DEBUG_CHECK_QUEUE();
00189     XENO_DEBUG_REMOVE_QUEUE();
00190 #endif /* CONFIG_RTAI_OPT_DEBUG */
00191     dth(holder);
00192     return --qslot->elems;
00193 }
00194 
00195 static inline xnholder_t *getheadq (xnqueue_t *qslot)
00196 {
00197     xnholder_t *holder = qslot->head.next;
00198     if (holder == &qslot->head) return NULL;
00199     return holder;
00200 }
00201 
00202 static inline xnholder_t *getq (xnqueue_t *qslot)
00203 {
00204     xnholder_t *holder = getheadq(qslot);
00205     if (holder) removeq(qslot,holder);
00206     return holder;
00207 }
00208 
00209 static inline xnholder_t *nextq (xnqueue_t *qslot,
00210                                  xnholder_t *holder)
00211 {
00212     xnholder_t *nextholder = holder->next;
00213     return nextholder == &qslot->head ? NULL : nextholder;
00214 }
00215 
00216 static inline xnholder_t *popq (xnqueue_t *qslot,
00217                                 xnholder_t *holder)
00218 {
00219     xnholder_t *nextholder = nextq(qslot,holder);
00220     removeq(qslot,holder);
00221     return nextholder;
00222 }
00223 
00224 static inline int countq (xnqueue_t *qslot)
00225 {
00226     return qslot->elems;
00227 }
00228 
00229 static inline int moveq (xnqueue_t *dstq, xnqueue_t *srcq)
00230 {
00231     xnholder_t *headsrc = srcq->head.next;
00232     xnholder_t *tailsrc = srcq->head.last->last;
00233     xnholder_t *headdst = &dstq->head;
00234 
00235     headsrc->last->next = tailsrc->next;
00236     tailsrc->next->last = headsrc->last;
00237     headsrc->last = headdst;
00238     tailsrc->next = headdst->next;
00239     headdst->next->last = tailsrc;
00240     headdst->next = headsrc;
00241     dstq->elems += srcq->elems;
00242     srcq->elems = 0;
00243 
00244     return dstq->elems;
00245 }
00246 
00247 /* Prioritized element holder */
00248 
00249 typedef struct xnpholder {
00250 
00251     xnholder_t plink;
00252     int prio;
00253 
00254 } xnpholder_t;
00255 
00256 static inline void initph (xnpholder_t *holder) {
00257     inith(&holder->plink);
00258     /* Priority is set upon queue insertion */
00259 }
00260 
00261 /* Prioritized element queue */
00262 
00263 #define xnqueue_up   (-1)
00264 #define xnqueue_down   1
00265 
00266 typedef struct xnpqueue {
00267 
00268     xnqueue_t pqueue;
00269     int qdir;
00270 
00271 } xnpqueue_t;
00272 
00273 static inline void initpq (xnpqueue_t *pqslot,
00274                            int qdir,
00275                            int maxpri)
00276 {
00277     initq(&pqslot->pqueue);
00278     pqslot->qdir = qdir;
00279 }
00280 
00281 static inline int insertpq (xnpqueue_t *pqslot,
00282                             xnpholder_t *head,
00283                             xnpholder_t *holder)
00284 {
00285     /* Insert the <holder> element before <head> */
00286     return insertq(&pqslot->pqueue,&head->plink,&holder->plink);
00287 }
00288 
00289 static inline int insertpqf (xnpqueue_t *pqslot,
00290                              xnpholder_t *holder,
00291                              int prio)
00292 {
00293     /* Insert the element at the end of its priority group (FIFO) */
00294 
00295     xnholder_t *curr;
00296 
00297     if (pqslot->qdir == xnqueue_down) {
00298         for (curr = pqslot->pqueue.head.last;
00299              curr != &pqslot->pqueue.head; curr = curr->last) {
00300                 if (prio <= ((xnpholder_t *)curr)->prio)
00301                     break;
00302         }
00303     }
00304     else {
00305         for (curr = pqslot->pqueue.head.last;
00306              curr != &pqslot->pqueue.head; curr = curr->last) {
00307                 if (prio >= ((xnpholder_t *)curr)->prio)
00308                     break;
00309         }
00310     }
00311 
00312     holder->prio = prio;
00313 
00314     return insertq(&pqslot->pqueue,curr->next,&holder->plink);
00315 }
00316 
00317 static inline int insertpql (xnpqueue_t *pqslot,
00318                              xnpholder_t *holder,
00319                              int prio)
00320 {
00321     /* Insert the element at the front of its priority group (LIFO) */
00322 
00323     xnholder_t *curr;
00324 
00325     if (pqslot->qdir == xnqueue_down) {
00326         for (curr = pqslot->pqueue.head.next;
00327              curr != &pqslot->pqueue.head; curr = curr->next) {
00328                 if (prio >= ((xnpholder_t *)curr)->prio)
00329                     break;
00330         }
00331     }
00332     else {
00333         for (curr = pqslot->pqueue.head.next;
00334              curr != &pqslot->pqueue.head; curr = curr->next) {
00335                 if (prio <= ((xnpholder_t *)curr)->prio)
00336                     break;
00337         }
00338     }
00339 
00340     holder->prio = prio;
00341 
00342     return insertq(&pqslot->pqueue,curr,&holder->plink);
00343 }
00344 
00345 static inline xnpholder_t *findpqh (xnpqueue_t *pqslot,
00346                                     int prio)
00347 {
00348     /* Find the element heading a given priority group */
00349 
00350     xnholder_t *curr;
00351 
00352     if (pqslot->qdir == xnqueue_down) {
00353         for (curr = pqslot->pqueue.head.next;
00354              curr != &pqslot->pqueue.head; curr = curr->next) {
00355                 if (prio >= ((xnpholder_t *)curr)->prio)
00356                     break;
00357         }
00358     }
00359     else {
00360         for (curr = pqslot->pqueue.head.next;
00361              curr != &pqslot->pqueue.head; curr = curr->next) {
00362                 if (prio <= ((xnpholder_t *)curr)->prio)
00363                     break;
00364         }
00365     }
00366 
00367     if (curr && ((xnpholder_t *)curr)->prio == prio)
00368         return (xnpholder_t *)curr;
00369 
00370     return NULL;
00371 }
00372 
00373 static inline int appendpq (xnpqueue_t *pqslot,
00374                             xnpholder_t *holder)
00375 {
00376     holder->prio = 0;
00377     return appendq(&pqslot->pqueue,&holder->plink);
00378 }
00379 
00380 static inline int prependpq (xnpqueue_t *pqslot,
00381                              xnpholder_t *holder)
00382 {
00383     holder->prio = 0;
00384     return prependq(&pqslot->pqueue,&holder->plink);
00385 }
00386 
00387 static inline int removepq (xnpqueue_t *pqslot,
00388                             xnpholder_t *holder)
00389 {
00390     return removeq(&pqslot->pqueue,&holder->plink);
00391 }
00392 
00393 static inline xnpholder_t *getheadpq (xnpqueue_t *pqslot)
00394 {
00395     return (xnpholder_t *)getheadq(&pqslot->pqueue);
00396 }
00397 
00398 static inline xnpholder_t *nextpq (xnpqueue_t *pqslot,
00399                                    xnpholder_t *holder)
00400 {
00401     return (xnpholder_t *)nextq(&pqslot->pqueue,&holder->plink);
00402 }
00403 
00404 static inline xnpholder_t *getpq (xnpqueue_t *pqslot)
00405 {
00406     return (xnpholder_t *)getq(&pqslot->pqueue);
00407 }
00408 
00409 static inline xnpholder_t *poppq (xnpqueue_t *pqslot,
00410                                   xnpholder_t *holder)
00411 {
00412     return (xnpholder_t *)popq(&pqslot->pqueue,&holder->plink);
00413 }
00414 
00415 static inline int countpq (xnpqueue_t *pqslot)
00416 {
00417     return countq(&pqslot->pqueue);
00418 }
00419 
00420 /* Generic prioritized element holder */
00421 
00422 typedef struct xngholder {
00423 
00424     xnpholder_t glink;
00425     void *data;
00426 
00427 } xngholder_t;
00428 
00429 static inline void initgh (xngholder_t *holder, void *data)
00430 {
00431     inith(&holder->glink.plink);
00432     holder->data = data;
00433 }
00434 
00435 /* Generic element queue */
00436 
00437 typedef struct xngqueue {
00438 
00439     xnpqueue_t gqueue;
00440     xnqueue_t *freehq;
00441     void (*starvation)(xnqueue_t *);
00442     int threshold;
00443 
00444 } xngqueue_t;
00445 
00446 static inline void initgq (xngqueue_t *gqslot,
00447                            xnqueue_t *freehq,
00448                            void (*starvation)(xnqueue_t *),
00449                            int threshold,
00450                            int qdir,
00451                            int maxpri)
00452 {
00453     initpq(&gqslot->gqueue,qdir,maxpri);
00454     gqslot->freehq = freehq;
00455     gqslot->starvation = starvation;
00456     gqslot->threshold = threshold;
00457 }
00458 
00459 static inline xngholder_t *allocgh (xngqueue_t *gqslot)
00460 {
00461     if (countq(gqslot->freehq) < gqslot->threshold)
00462         gqslot->starvation(gqslot->freehq);
00463 
00464     return (xngholder_t *)getq(gqslot->freehq);
00465 }
00466 
00467 static inline void *removegh (xngqueue_t *gqslot,
00468                               xngholder_t *holder)
00469 {
00470     removepq(&gqslot->gqueue,&holder->glink);
00471     appendq(gqslot->freehq,&holder->glink.plink);
00472     return holder->data;
00473 }
00474 
00475 static inline int insertgqf (xngqueue_t *gqslot,
00476                              void *data,
00477                              int prio)
00478 {
00479     xngholder_t *holder = allocgh(gqslot);
00480     holder->data = data;
00481     return insertpqf(&gqslot->gqueue,&holder->glink,prio);
00482 }
00483 
00484 static inline int insertgql (xngqueue_t *gqslot,
00485                              void *data,
00486                              int prio)
00487 {
00488     xngholder_t *holder = allocgh(gqslot);
00489     holder->data = data;
00490     return insertpql(&gqslot->gqueue,&holder->glink,prio);
00491 }
00492 
00493 static inline int appendgq (xngqueue_t *gqslot,
00494                             void *data)
00495 {
00496     xngholder_t *holder = allocgh(gqslot);
00497     holder->data = data;
00498     return appendpq(&gqslot->gqueue,&holder->glink);
00499 }
00500 
00501 static inline int prependgq (xngqueue_t *gqslot,
00502                              void *data)
00503 {
00504     xngholder_t *holder = allocgh(gqslot);
00505     holder->data = data;
00506     return prependpq(&gqslot->gqueue,&holder->glink);
00507 }
00508 
00509 static inline xngholder_t *getheadgq (xngqueue_t *gqslot)
00510 {
00511     return (xngholder_t *)getheadpq(&gqslot->gqueue);
00512 }
00513 
00514 static inline xngholder_t *nextgq (xngqueue_t *gqslot,
00515                                    xngholder_t *holder)
00516 {
00517     return (xngholder_t *)nextpq(&gqslot->gqueue,&holder->glink);
00518 }
00519 
00520 static inline void *getgq (xngqueue_t *gqslot)
00521 {
00522     xngholder_t *holder = getheadgq(gqslot);
00523     if (!holder) return NULL;
00524     appendq(gqslot->freehq,&getpq(&gqslot->gqueue)->plink);
00525     return holder->data;
00526 }
00527 
00528 static inline xngholder_t *popgq (xngqueue_t *gqslot,
00529                                   xngholder_t *holder)
00530 {
00531     xngholder_t *nextholder = nextgq(gqslot,holder);
00532     removegh(gqslot,holder);
00533     return nextholder;
00534 }
00535 
00536 static inline xngholder_t *findgq (xngqueue_t *gqslot,
00537                                    void *data)
00538 {
00539     xnholder_t *holder;
00540 
00541     for (holder = gqslot->gqueue.pqueue.head.next;
00542          holder != &gqslot->gqueue.pqueue.head; holder = holder->next) {
00543          if (((xngholder_t *)holder)->data == data)
00544              return (xngholder_t *)holder;
00545         }
00546 
00547     return NULL;
00548 }
00549 
00550 static inline void *removegq (xngqueue_t *gqslot,
00551                               void *data)
00552 {
00553     xngholder_t *holder = findgq(gqslot,data);
00554     return holder ? removegh(gqslot,holder) : NULL;
00555 }
00556 
00557 static inline int countgq (xngqueue_t *gqslot)
00558 {
00559     return countpq(&gqslot->gqueue);
00560 }
00561 
00562 /* Multi-level priority queue */
00563 
00564 #define XNSPQUEUE_PRIOS      (CONFIG_RTAI_OPT_SCALABLE_PRIOS + 1)
00565 #define XNSPQUEUE_BITMAPSIZE ((XNSPQUEUE_PRIOS + BITS_PER_LONG - 1) / BITS_PER_LONG)
00566 
00567 typedef struct xnspqueue {
00568 
00569     int qdir, maxpri;
00570 
00571     unsigned int elems;
00572 
00573     u_long bitmap[XNSPQUEUE_BITMAPSIZE];
00574 
00575     struct xnqueue queue[XNSPQUEUE_PRIOS];
00576 
00577 } xnspqueue_t;
00578 
00579 static inline unsigned int countspq(xnspqueue_t *spqslot)
00580 {
00581     return spqslot->elems;
00582 }
00583 
00584 static inline unsigned int xnspqueue_prio2idx(xnspqueue_t *spqslot, int prio)
00585 {
00586     if (spqslot->qdir == xnqueue_up) /* minprio > maxprio */
00587         return prio - spqslot->maxpri;
00588     else
00589         return spqslot->maxpri - prio;
00590 }
00591 
00592 static inline unsigned int xnspqueue_find_firstbit(xnspqueue_t *spqslot)
00593 {
00594     int i;
00595 
00596 /* simplest approach for now */
00597     for (i = 0; i < XNSPQUEUE_BITMAPSIZE; i++)
00598         {
00599         if (spqslot->bitmap[i])
00600             return ffnz(spqslot->bitmap[i]) + i*BITS_PER_LONG;
00601         }
00602 
00603     return 0; /* something already had gone wrong before this code was called */
00604 }
00605 
00606 static inline void initspq(xnspqueue_t *spqslot, int qdir, int maxpri)
00607 {
00608     int i;
00609 
00610     spqslot->elems = 0;
00611     spqslot->qdir  = qdir;
00612     spqslot->maxpri = maxpri;
00613     
00614     memset(&spqslot->bitmap, 0, XNSPQUEUE_BITMAPSIZE*sizeof(u_long));
00615     
00616     for (i = 0; i < XNSPQUEUE_PRIOS; i++)
00617         initq(&spqslot->queue[i]);
00618 }
00619 
00620 #define XNSPQUEUE_APPEND        0
00621 #define XNSPQUEUE_PREPEND       1
00622 
00623 static inline void __xnspqueue_insert(xnspqueue_t *spqslot,
00624                                       xnpholder_t  *holder,
00625                                       unsigned int idx,
00626                                       int mode)
00627 {
00628     xnqueue_t *queue = &spqslot->queue[idx];
00629 
00630     if (mode == XNSPQUEUE_PREPEND) /* Hopefully, this should be optimized away. */
00631         prependq(queue, &holder->plink);
00632     else
00633         appendq(queue, &holder->plink);
00634 
00635     spqslot->elems++;
00636     __setbits(spqslot->bitmap[idx / BITS_PER_LONG],1 << (idx % BITS_PER_LONG));
00637 }
00638 
00639 static inline void insertspql(xnspqueue_t *spqslot,
00640                              xnpholder_t *holder,
00641                              int prio)
00642 {
00643     __xnspqueue_insert(spqslot, holder, xnspqueue_prio2idx(spqslot,prio), XNSPQUEUE_PREPEND);
00644     holder->prio = prio;
00645 }
00646 
00647 static inline void insertspqf(xnspqueue_t *spqslot,
00648                              xnpholder_t *holder,
00649                              int prio)
00650 {
00651     __xnspqueue_insert(spqslot, holder, xnspqueue_prio2idx(spqslot,prio), XNSPQUEUE_APPEND);
00652     holder->prio = prio;
00653 }
00654 
00655 static inline void appendspq(xnspqueue_t *spqslot,
00656                             xnpholder_t *holder)
00657 {
00658     __xnspqueue_insert(spqslot, holder, spqslot->maxpri, XNSPQUEUE_APPEND);
00659     holder->prio = spqslot->maxpri;
00660 }
00661 
00662 static inline void prependspq(xnspqueue_t *spqslot,
00663                              xnpholder_t *holder)
00664 {
00665     __xnspqueue_insert(spqslot, holder, 0, XNSPQUEUE_PREPEND);
00666     holder->prio = 0;
00667 }
00668 
00669 static inline void removespq(xnspqueue_t *spqslot,
00670                             xnpholder_t *holder)
00671 {
00672     unsigned int idx = xnspqueue_prio2idx(spqslot,holder->prio);
00673     xnqueue_t *queue = &spqslot->queue[idx];
00674 
00675     spqslot->elems--;
00676     
00677     removeq(queue, &holder->plink);
00678     
00679     if (!countq(queue))
00680         __clrbits(spqslot->bitmap[idx / BITS_PER_LONG],1 << (idx % BITS_PER_LONG));
00681 }
00682 
00683 static inline xnpholder_t* findspqh(xnspqueue_t *spqslot,
00684                                    int prio)
00685 {
00686     xnqueue_t *queue = &spqslot->queue[xnspqueue_prio2idx(spqslot,prio)];
00687     return (xnpholder_t *)getheadq(queue);
00688 }
00689 
00690 static inline xnpholder_t* getheadspq(xnspqueue_t *spqslot)
00691 {
00692     xnqueue_t *queue;
00693 
00694     if (!countspq(spqslot))
00695         return NULL;
00696 
00697     queue = &spqslot->queue[xnspqueue_find_firstbit(spqslot)];
00698     
00699     return (xnpholder_t *)getheadq(queue);
00700 }
00701 
00702 static inline xnpholder_t* getspq(xnspqueue_t *spqslot)
00703 {
00704     unsigned int idx;
00705     xnholder_t *holder;
00706     xnqueue_t *queue;
00707 
00708     if (!countspq(spqslot))
00709         return NULL;
00710 
00711     idx    = xnspqueue_find_firstbit(spqslot);
00712     queue  = &spqslot->queue[idx];
00713     holder = getq(queue);
00714 
00715     if (!holder)
00716         return NULL;
00717         
00718     spqslot->elems--;    
00719 
00720     if (!countq(queue))
00721         __clrbits(spqslot->bitmap[idx / BITS_PER_LONG],1 << (idx % BITS_PER_LONG));
00722     
00723     return (xnpholder_t *)holder;
00724 }
00725 
00726 #endif /* !_RTAI_NUCLEUS_QUEUE_H */

Generated on Wed Jun 22 22:54:02 2005 for RTAI Fusion API by  doxygen 1.4.1