00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef _RTAI_NUCLEUS_QUEUE_H
00021 #define _RTAI_NUCLEUS_QUEUE_H
00022
00023 #include <nucleus/types.h>
00024
00025
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
00036 holder->last = holder;
00037 holder->next = holder;
00038 }
00039
00040 static inline void ath (xnholder_t *head,
00041 xnholder_t *holder) {
00042
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
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
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
00069 #define DECLARE_XNQUEUE(q) xnqueue_t q = { { &(q).head, &(q).head }, 0 }
00070 #endif
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
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
00138
00139
00140
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
00147
00148
00149
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
00176
00177 static inline int insertq (xnqueue_t *qslot,
00178 xnholder_t *head,
00179 xnholder_t *holder)
00180 {
00181
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
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
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
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
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
00275 }
00276
00277
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
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
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
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
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
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
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
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)
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
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;
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)
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