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() \
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) \
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
00135
00136
00137
00138
00139 #define XENO_DEBUG_CHECK_QUEUE()
00140 #define XENO_DEBUG_INSERT_QUEUE()
00141 #define XENO_DEBUG_REMOVE_QUEUE()
00142
00143 #endif
00144
00145 #endif
00146
00147 static inline int insertq (xnqueue_t *qslot,
00148 xnholder_t *head,
00149 xnholder_t *holder)
00150 {
00151
00152 #ifdef CONFIG_RTAI_OPT_DEBUG
00153 XENO_DEBUG_CHECK_QUEUE();
00154 XENO_DEBUG_INSERT_QUEUE();
00155 #endif
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
00164 #ifdef CONFIG_RTAI_OPT_DEBUG
00165 XENO_DEBUG_CHECK_QUEUE();
00166 XENO_DEBUG_INSERT_QUEUE();
00167 #endif
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
00176 #ifdef CONFIG_RTAI_OPT_DEBUG
00177 XENO_DEBUG_CHECK_QUEUE();
00178 XENO_DEBUG_INSERT_QUEUE();
00179 #endif
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
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
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
00259 }
00260
00261
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
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
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
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
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
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
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
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)
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
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;
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)
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