queue.h

00001 /*
00002  * Copyright (C) 2001,2002,2003 Philippe Gerum <rpm@xenomai.org>.
00003  *
00004  * Xenomai is free software; you can redistribute it and/or modify it
00005  * under the terms of the GNU General Public License as published by
00006  * the Free Software Foundation; either version 2 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * Xenomai 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 Xenomai; if not, write to the Free Software Foundation,
00016  * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00017  *
00018  * As a special exception, the RTAI project gives permission
00019  * for additional uses of the text contained in its release of
00020  * Xenomai.
00021  *
00022  * The exception is that, if you link the Xenomai libraries with other
00023  * files to produce an executable, this does not by itself cause the
00024  * resulting executable to be covered by the GNU General Public License.
00025  * Your use of that executable is in no way restricted on account of
00026  * linking the Xenomai libraries code into it.
00027  *
00028  * This exception does not however invalidate any other reasons why
00029  * the executable file might be covered by the GNU General Public
00030  * License.
00031  *
00032  * This exception applies only to the code released by the
00033  * RTAI project under the name Xenomai.  If you copy code from other
00034  * RTAI project releases into a copy of Xenomai, as the General Public
00035  * License permits, the exception does not apply to the code that you
00036  * add in this way.  To avoid misleading anyone as to the status of
00037  * such modified files, you must delete this exception notice from
00038  * them.
00039  *
00040  * If you write modifications of your own for Xenomai, it is your
00041  * choice whether to permit this exception to apply to your
00042  * modifications. If you do not wish that, delete this exception
00043  * notice.
00044  */
00045 
00046 #ifndef _RTAI_NUCLEUS_QUEUE_H
00047 #define _RTAI_NUCLEUS_QUEUE_H
00048 
00049 #include <nucleus/types.h>
00050 
00051 /* Basic element holder */
00052 
00053 typedef struct xnholder {
00054 
00055     struct xnholder *next;
00056     struct xnholder *last;
00057 
00058 } xnholder_t;
00059 
00060 static inline void inith (xnholder_t *holder) {
00061     /* Holding queues are doubly-linked and circular */
00062     holder->last = holder;
00063     holder->next = holder;
00064 }
00065 
00066 static inline void ath (xnholder_t *head,
00067                         xnholder_t *holder) {
00068     /* Inserts the new element right after the heading one  */
00069     holder->last = head;
00070     holder->next = head->next;
00071     holder->next->last = holder;
00072     head->next = holder;
00073 }
00074 
00075 static inline void dth (xnholder_t *holder) {
00076     holder->last->next = holder->next;
00077     holder->next->last = holder->last;
00078 }
00079 
00080 /* Basic element queue */
00081 
00082 typedef struct xnqueue {
00083 
00084     xnholder_t head;
00085     int elems;
00086 #if defined(CONFIG_RTAI_OPT_DEBUG) && defined(CONFIG_SMP)
00087     xnlock_t lock;
00088 #endif /* CONFIG_RTAI_OPT_DEBUG && CONFIG_SMP */
00089 
00090 } xnqueue_t;
00091 
00092 #if defined(CONFIG_RTAI_OPT_DEBUG) && defined(CONFIG_SMP)
00093 #define DECLARE_XNQUEUE(q) xnqueue_t q = { { &(q).head, &(q).head }, 0, XNARCH_LOCK_UNLOCKED }
00094 #else /* !(CONFIG_RTAI_OPT_DEBUG && CONFIG_SMP) */
00095 #define DECLARE_XNQUEUE(q) xnqueue_t q = { { &(q).head, &(q).head }, 0 }
00096 #endif /* CONFIG_RTAI_OPT_DEBUG && CONFIG_SMP */
00097 
00098 static inline void initq (xnqueue_t *qslot) {
00099     inith(&qslot->head);
00100     qslot->elems = 0;
00101 #if defined(CONFIG_RTAI_OPT_DEBUG) && defined(CONFIG_SMP)
00102     xnlock_init(&qslot->lock);
00103 #endif /* CONFIG_RTAI_OPT_DEBUG && CONFIG_SMP */
00104 }
00105 
00106 #ifdef CONFIG_RTAI_OPT_DEBUG
00107 
00108 #if defined(__KERNEL__) || defined(__RTAI_UVM__) || defined(__RTAI_SIM__)
00109 
00110 #define XENO_DEBUG_CHECK_QUEUE() \
00111 do { \
00112     xnholder_t *curr; \
00113     spl_t s; \
00114     int nelems = 0; \
00115     xnlock_get_irqsave(&qslot->lock,s); \
00116     curr = qslot->head.last; \
00117     while (curr != &qslot->head && nelems < qslot->elems) \
00118         curr = curr->last, nelems++; \
00119     if (curr != &qslot->head || nelems != qslot->elems) \
00120         xnpod_fatal("corrupted queue, qslot->elems=%d, qslot=%p", \
00121                     qslot->elems, \
00122                     qslot); \
00123     xnlock_put_irqrestore(&qslot->lock,s); \
00124 } while(0)
00125 
00126 #define XENO_DEBUG_INSERT_QUEUE() \
00127 do { \
00128     xnholder_t *curr; \
00129     spl_t s; \
00130     xnlock_get_irqsave(&qslot->lock,s); \
00131     curr = qslot->head.last; \
00132     while (curr != &qslot->head && holder != curr) \
00133         curr = curr->last; \
00134     if (curr == holder) \
00135         xnpod_fatal("inserting element twice, holder=%p, qslot=%p", \
00136                     holder, \
00137                     qslot); \
00138     if (holder->last == NULL)   /* Just a guess. */ \
00139         xnpod_fatal("holder=%p not initialized, qslot=%p", \
00140                     holder, \
00141                     qslot); \
00142     xnlock_put_irqrestore(&qslot->lock,s); \
00143 } while(0)
00144 
00145 #define XENO_DEBUG_REMOVE_QUEUE() \
00146 do { \
00147     xnholder_t *curr; \
00148     spl_t s; \
00149     xnlock_get_irqsave(&qslot->lock,s); \
00150     curr = qslot->head.last; \
00151     while (curr != &qslot->head && holder != curr) \
00152         curr = curr->last; \
00153     if (curr == &qslot->head) \
00154         xnpod_fatal("removing non-linked element, holder=%p, qslot=%p", \
00155                     holder, \
00156                     qslot); \
00157     xnlock_put_irqrestore(&qslot->lock,s); \
00158 } while(0)
00159 
00160 #else /* !(__KERNEL__ || __RTAI_UVM__ || __RTAI_SIM__) */
00161 
00162 /* Disable queue checks in user-space code which does not run as part
00163    of any virtual machine, e.g. skin syslibs. */
00164 
00165 #define XENO_DEBUG_CHECK_QUEUE()
00166 #define XENO_DEBUG_INSERT_QUEUE()
00167 #define XENO_DEBUG_REMOVE_QUEUE()
00168 
00169 #endif /* __KERNEL__ || __RTAI_UVM__ || __RTAI_SIM__ */
00170 
00171 #endif /* CONFIG_RTAI_OPT_DEBUG */
00172 
00173 static inline int insertq (xnqueue_t *qslot,
00174                            xnholder_t *head,
00175                            xnholder_t *holder) {
00176     /* Insert the <holder> element before <head> */
00177 #ifdef CONFIG_RTAI_OPT_DEBUG
00178     XENO_DEBUG_CHECK_QUEUE();
00179     XENO_DEBUG_INSERT_QUEUE();
00180 #endif /*CONFIG_RTAI_OPT_DEBUG */
00181     ath(head->last,holder);
00182     return ++qslot->elems;
00183 }
00184 
00185 static inline int prependq (xnqueue_t *qslot,
00186                             xnholder_t *holder) {
00187     /* Prepend the element to the queue */
00188 #ifdef CONFIG_RTAI_OPT_DEBUG
00189     XENO_DEBUG_CHECK_QUEUE();
00190     XENO_DEBUG_INSERT_QUEUE();
00191 #endif /* CONFIG_RTAI_OPT_DEBUG */
00192     ath(&qslot->head,holder);
00193     return ++qslot->elems;
00194 }
00195 
00196 static inline int appendq (xnqueue_t *qslot,
00197                            xnholder_t *holder) {
00198     /* Append the element to the queue */
00199 #ifdef CONFIG_RTAI_OPT_DEBUG
00200     XENO_DEBUG_CHECK_QUEUE();
00201     XENO_DEBUG_INSERT_QUEUE();
00202 #endif /* CONFIG_RTAI_OPT_DEBUG */
00203     ath(qslot->head.last,holder);
00204     return ++qslot->elems;
00205 }
00206 
00207 static inline int removeq (xnqueue_t *qslot,
00208                            xnholder_t *holder) {
00209 #ifdef CONFIG_RTAI_OPT_DEBUG
00210     XENO_DEBUG_CHECK_QUEUE();
00211     XENO_DEBUG_REMOVE_QUEUE();
00212 #endif /* CONFIG_RTAI_OPT_DEBUG */
00213     dth(holder);
00214     return --qslot->elems;
00215 }
00216 
00217 static inline xnholder_t *getheadq (xnqueue_t *qslot) {
00218     xnholder_t *holder = qslot->head.next;
00219     if (holder == &qslot->head) return NULL;
00220     return holder;
00221 }
00222 
00223 static inline xnholder_t *getq (xnqueue_t *qslot) {
00224     xnholder_t *holder = getheadq(qslot);
00225     if (holder) removeq(qslot,holder);
00226     return holder;
00227 }
00228 
00229 static inline xnholder_t *nextq (xnqueue_t *qslot,
00230                                  xnholder_t *holder) {
00231     xnholder_t *nextholder = holder->next;
00232     return nextholder == &qslot->head ? NULL : nextholder;
00233 }
00234 
00235 static inline xnholder_t *popq (xnqueue_t *qslot,
00236                                 xnholder_t *holder) {
00237     xnholder_t *nextholder = nextq(qslot,holder);
00238     removeq(qslot,holder);
00239     return nextholder;
00240 }
00241 
00242 static inline int countq (xnqueue_t *qslot) {
00243     return qslot->elems;
00244 }
00245 
00246 static inline int moveq (xnqueue_t *dstq, xnqueue_t *srcq) {
00247     
00248     xnholder_t *headsrc = srcq->head.next;
00249     xnholder_t *tailsrc = srcq->head.last->last;
00250     xnholder_t *headdst = &dstq->head;
00251 
00252     headsrc->last->next = tailsrc->next;
00253     tailsrc->next->last = headsrc->last;
00254     headsrc->last = headdst;
00255     tailsrc->next = headdst->next;
00256     headdst->next->last = tailsrc;
00257     headdst->next = headsrc;
00258     dstq->elems += srcq->elems;
00259     srcq->elems = 0;
00260 
00261     return dstq->elems;
00262 }
00263 
00264 /* Prioritized element holder */
00265 
00266 typedef struct xnpholder {
00267 
00268     xnholder_t plink;
00269     int prio;
00270 
00271 } xnpholder_t;
00272 
00273 static inline void initph (xnpholder_t *holder) {
00274     inith(&holder->plink);
00275     /* Priority is set upon queue insertion */
00276 }
00277 
00278 /* Prioritized element queue */
00279 
00280 #define xnqueue_up   (-1)
00281 #define xnqueue_down   1
00282 
00283 typedef struct xnpqueue {
00284 
00285     xnqueue_t pqueue;
00286     int qdir;
00287 
00288 } xnpqueue_t;
00289 
00290 static inline void initpq (xnpqueue_t *pqslot,
00291                            int qdir) {
00292     initq(&pqslot->pqueue);
00293     pqslot->qdir = qdir;
00294 }
00295 
00296 static inline int insertpq (xnpqueue_t *pqslot,
00297                             xnpholder_t *head,
00298                             xnpholder_t *holder) {
00299     /* Insert the <holder> element before <head> */
00300     return insertq(&pqslot->pqueue,&head->plink,&holder->plink);
00301 }
00302 
00303 static inline int insertpqf (xnpqueue_t *pqslot,
00304                              xnpholder_t *holder,
00305                              int prio) {
00306 
00307     /* Insert the element at the end of its priority group (FIFO) */
00308 
00309     xnholder_t *curr;
00310 
00311     if (pqslot->qdir == xnqueue_down) {
00312         for (curr = pqslot->pqueue.head.last;
00313              curr != &pqslot->pqueue.head; curr = curr->last) {
00314                 if (prio <= ((xnpholder_t *)curr)->prio)
00315                     break;
00316         }
00317     }
00318     else {
00319         for (curr = pqslot->pqueue.head.last;
00320              curr != &pqslot->pqueue.head; curr = curr->last) {
00321                 if (prio >= ((xnpholder_t *)curr)->prio)
00322                     break;
00323         }
00324     }
00325 
00326     holder->prio = prio;
00327 
00328     return insertq(&pqslot->pqueue,curr->next,&holder->plink);
00329 }
00330 
00331 static inline int insertpql (xnpqueue_t *pqslot,
00332                              xnpholder_t *holder,
00333                              int prio) {
00334 
00335     /* Insert the element at the front of its priority group (LIFO) */
00336 
00337     xnholder_t *curr;
00338 
00339     if (pqslot->qdir == xnqueue_down) {
00340         for (curr = pqslot->pqueue.head.next;
00341              curr != &pqslot->pqueue.head; curr = curr->next) {
00342                 if (prio >= ((xnpholder_t *)curr)->prio)
00343                     break;
00344         }
00345     }
00346     else {
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 
00354     holder->prio = prio;
00355 
00356     return insertq(&pqslot->pqueue,curr,&holder->plink);
00357 }
00358 
00359 static inline xnpholder_t *findpqh (xnpqueue_t *pqslot,
00360                                     int prio) {
00361 
00362     /* Find the element heading a given priority group */
00363 
00364     xnholder_t *curr;
00365 
00366     if (pqslot->qdir == xnqueue_down) {
00367         for (curr = pqslot->pqueue.head.next;
00368              curr != &pqslot->pqueue.head; curr = curr->next) {
00369                 if (prio >= ((xnpholder_t *)curr)->prio)
00370                     break;
00371         }
00372     }
00373     else {
00374         for (curr = pqslot->pqueue.head.next;
00375              curr != &pqslot->pqueue.head; curr = curr->next) {
00376                 if (prio <= ((xnpholder_t *)curr)->prio)
00377                     break;
00378         }
00379     }
00380 
00381     if (curr && ((xnpholder_t *)curr)->prio == prio)
00382         return (xnpholder_t *)curr;
00383 
00384     return NULL;
00385 }
00386 
00387 static inline int appendpq (xnpqueue_t *pqslot,
00388                             xnpholder_t *holder) {
00389     holder->prio = 0;
00390     return appendq(&pqslot->pqueue,&holder->plink);
00391 }
00392 
00393 static inline int prependpq (xnpqueue_t *pqslot,
00394                              xnpholder_t *holder) {
00395     holder->prio = 0;
00396     return prependq(&pqslot->pqueue,&holder->plink);
00397 }
00398 
00399 static inline int removepq (xnpqueue_t *pqslot,
00400                             xnpholder_t *holder) {
00401     return removeq(&pqslot->pqueue,&holder->plink);
00402 }
00403 
00404 static inline xnpholder_t *getheadpq (xnpqueue_t *pqslot) {
00405     return (xnpholder_t *)getheadq(&pqslot->pqueue);
00406 }
00407 
00408 static inline xnpholder_t *nextpq (xnpqueue_t *pqslot,
00409                                    xnpholder_t *holder) {
00410     return (xnpholder_t *)nextq(&pqslot->pqueue,&holder->plink);
00411 }
00412 
00413 static inline xnpholder_t *getpq (xnpqueue_t *pqslot) {
00414     return (xnpholder_t *)getq(&pqslot->pqueue);
00415 }
00416 
00417 static inline xnpholder_t *poppq (xnpqueue_t *pqslot,
00418                                   xnpholder_t *holder) {
00419     return (xnpholder_t *)popq(&pqslot->pqueue,&holder->plink);
00420 }
00421 
00422 static inline int countpq (xnpqueue_t *pqslot) {
00423     return countq(&pqslot->pqueue);
00424 }
00425 
00426 /* Generic prioritized element holder */
00427 
00428 typedef struct xngholder {
00429 
00430     xnpholder_t glink;
00431     void *data;
00432 
00433 } xngholder_t;
00434 
00435 static inline void initgh (xngholder_t *holder, void *data) {
00436     inith(&holder->glink.plink);
00437     holder->data = data;
00438 }
00439 
00440 /* Generic element queue */
00441 
00442 typedef struct xngqueue {
00443 
00444     xnpqueue_t gqueue;
00445     xnqueue_t *freehq;
00446     void (*starvation)(xnqueue_t *);
00447     int threshold;
00448 
00449 } xngqueue_t;
00450 
00451 static inline void initgq (xngqueue_t *gqslot,
00452                            xnqueue_t *freehq,
00453                            void (*starvation)(xnqueue_t *),
00454                            int threshold,
00455                            int qdir) {
00456     initpq(&gqslot->gqueue,qdir);
00457     gqslot->freehq = freehq;
00458     gqslot->starvation = starvation;
00459     gqslot->threshold = threshold;
00460 }
00461 
00462 static inline xngholder_t *allocgh (xngqueue_t *gqslot) {
00463 
00464     if (countq(gqslot->freehq) < gqslot->threshold)
00465         gqslot->starvation(gqslot->freehq);
00466 
00467     return (xngholder_t *)getq(gqslot->freehq);
00468 }
00469 
00470 static inline void *removegh (xngqueue_t *gqslot,
00471                               xngholder_t *holder) {
00472     removepq(&gqslot->gqueue,&holder->glink);
00473     appendq(gqslot->freehq,&holder->glink.plink);
00474     return holder->data;
00475 }
00476 
00477 static inline int insertgqf (xngqueue_t *gqslot,
00478                              void *data,
00479                              int prio) {
00480     xngholder_t *holder = allocgh(gqslot);
00481     holder->data = data;
00482     return insertpqf(&gqslot->gqueue,&holder->glink,prio);
00483 }
00484 
00485 static inline int insertgql (xngqueue_t *gqslot,
00486                              void *data,
00487                              int prio) {
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     xngholder_t *holder = allocgh(gqslot);
00496     holder->data = data;
00497     return appendpq(&gqslot->gqueue,&holder->glink);
00498 }
00499 
00500 static inline int prependgq (xngqueue_t *gqslot,
00501                              void *data) {
00502     xngholder_t *holder = allocgh(gqslot);
00503     holder->data = data;
00504     return prependpq(&gqslot->gqueue,&holder->glink);
00505 }
00506 
00507 static inline xngholder_t *getheadgq (xngqueue_t *gqslot) {
00508     return (xngholder_t *)getheadpq(&gqslot->gqueue);
00509 }
00510 
00511 static inline xngholder_t *nextgq (xngqueue_t *gqslot,
00512                                    xngholder_t *holder) {
00513     return (xngholder_t *)nextpq(&gqslot->gqueue,&holder->glink);
00514 }
00515 
00516 static inline void *getgq (xngqueue_t *gqslot) {
00517     xngholder_t *holder = getheadgq(gqslot);
00518     if (!holder) return NULL;
00519     appendq(gqslot->freehq,&getpq(&gqslot->gqueue)->plink);
00520     return holder->data;
00521 }
00522 
00523 static inline xngholder_t *popgq (xngqueue_t *gqslot,
00524                                   xngholder_t *holder) {
00525     xngholder_t *nextholder = nextgq(gqslot,holder);
00526     removegh(gqslot,holder);
00527     return nextholder;
00528 }
00529 
00530 static inline xngholder_t *findgq (xngqueue_t *gqslot,
00531                                    void *data) {
00532     xnholder_t *holder;
00533 
00534     for (holder = gqslot->gqueue.pqueue.head.next;
00535          holder != &gqslot->gqueue.pqueue.head; holder = holder->next) {
00536          if (((xngholder_t *)holder)->data == data)
00537              return (xngholder_t *)holder;
00538         }
00539 
00540     return NULL;
00541 }
00542 
00543 static inline void *removegq (xngqueue_t *gqslot,
00544                               void *data) {
00545     xngholder_t *holder = findgq(gqslot,data);
00546     return holder ? removegh(gqslot,holder) : NULL;
00547 }
00548 
00549 static inline int countgq (xngqueue_t *gqslot) {
00550     return countpq(&gqslot->gqueue);
00551 }
00552 
00553 #endif /* !_RTAI_NUCLEUS_QUEUE_H */

Generated on Mon Dec 13 09:49:49 2004 for RTAI API by  doxygen 1.3.9.1