00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #ifndef _XENO_NUCLEUS_QUEUE_H
00022 #define _XENO_NUCLEUS_QUEUE_H
00023
00024 #include <nucleus/types.h>
00025 #include <nucleus/core.h>
00026
00027
00028 #include <nucleus/assert.h>
00029
00030 #ifndef CONFIG_XENO_OPT_DEBUG_QUEUES
00031 #define CONFIG_XENO_OPT_DEBUG_QUEUES 0
00032 #endif
00033
00034
00035
00036 typedef struct xnholder {
00037
00038 struct xnholder *next;
00039 struct xnholder *last;
00040
00041 } xnholder_t;
00042
00043 static inline void inith(xnholder_t *holder)
00044 {
00045
00046 holder->last = holder;
00047 holder->next = holder;
00048 }
00049
00050 static inline void ath(xnholder_t *head, xnholder_t *holder)
00051 {
00052
00053 holder->last = head;
00054 holder->next = head->next;
00055 holder->next->last = holder;
00056 head->next = holder;
00057 }
00058
00059 static inline void dth(xnholder_t *holder)
00060 {
00061 holder->last->next = holder->next;
00062 holder->next->last = holder->last;
00063 }
00064
00065
00066
00067 typedef struct xnqueue {
00068
00069 xnholder_t head;
00070 int elems;
00071 #if defined(__KERNEL__) && XENO_DEBUG(QUEUES) && defined(CONFIG_SMP)
00072 xnlock_t lock;
00073 #endif
00074
00075 } xnqueue_t;
00076
00077 #if XENO_DEBUG(QUEUES) && defined(CONFIG_SMP)
00078 #define XNQUEUE_INITIALIZER(q) { { &(q).head, &(q).head }, 0, XNARCH_LOCK_UNLOCKED }
00079 #else
00080 #define XNQUEUE_INITIALIZER(q) { { &(q).head, &(q).head }, 0 }
00081 #endif
00082
00083 #define DEFINE_XNQUEUE(q) xnqueue_t q = XNQUEUE_INITIALIZER(q)
00084
00085 static inline void initq(xnqueue_t *qslot)
00086 {
00087 inith(&qslot->head);
00088 qslot->elems = 0;
00089 #if defined(__KERNEL__) && XENO_DEBUG(QUEUES) && defined(CONFIG_SMP)
00090 xnlock_init(&qslot->lock);
00091 #endif
00092 }
00093
00094 #if XENO_DEBUG(QUEUES)
00095
00096 #if defined(__KERNEL__) || defined(__XENO_SIM__)
00097
00098 #define XENO_DEBUG_CHECK_QUEUE(__qslot) \
00099 do { \
00100 xnholder_t *curr; \
00101 spl_t s; \
00102 int nelems = 0; \
00103 xnlock_get_irqsave(&(__qslot)->lock,s); \
00104 curr = (__qslot)->head.last; \
00105 while (curr != &(__qslot)->head && nelems < (__qslot)->elems) \
00106 curr = curr->last, nelems++; \
00107 if (curr != &(__qslot)->head || nelems != (__qslot)->elems) \
00108 xnpod_fatal("corrupted queue, qslot->elems=%d/%d, qslot=%p at %s:%d", \
00109 nelems, \
00110 (__qslot)->elems, \
00111 __qslot, \
00112 __FILE__,__LINE__); \
00113 xnlock_put_irqrestore(&(__qslot)->lock,s); \
00114 } while(0)
00115
00116 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder) \
00117 do { \
00118 xnholder_t *curr; \
00119 spl_t s; \
00120 xnlock_get_irqsave(&(__qslot)->lock,s); \
00121 curr = (__qslot)->head.last; \
00122 while (curr != &(__qslot)->head && (__holder) != curr) \
00123 curr = curr->last; \
00124 if (curr == (__holder)) \
00125 xnpod_fatal("inserting element twice, holder=%p, qslot=%p at %s:%d", \
00126 __holder, \
00127 __qslot, \
00128 __FILE__,__LINE__); \
00129 if ((__holder)->last == NULL) \
00130 xnpod_fatal("holder=%p not initialized, qslot=%p", \
00131 __holder, \
00132 __qslot); \
00133 xnlock_put_irqrestore(&(__qslot)->lock,s); \
00134 } while(0)
00135
00136 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder) \
00137 do { \
00138 xnholder_t *curr; \
00139 spl_t s; \
00140 xnlock_get_irqsave(&(__qslot)->lock,s); \
00141 curr = (__qslot)->head.last; \
00142 while (curr != &(__qslot)->head && (__holder) != curr) \
00143 curr = curr->last; \
00144 if (curr == &(__qslot)->head) \
00145 xnpod_fatal("removing non-linked element, holder=%p, qslot=%p at %s:%d", \
00146 __holder, \
00147 __qslot, \
00148 __FILE__,__LINE__); \
00149 xnlock_put_irqrestore(&(__qslot)->lock,s); \
00150 } while(0)
00151
00152 #else
00153
00154
00155
00156
00157 #define XENO_DEBUG_CHECK_QUEUE(__qslot)
00158 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder)
00159 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder)
00160
00161 #endif
00162
00163
00164
00165
00166 #define insertq(__qslot,__head,__holder) \
00167 ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \
00168 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder); \
00169 ath((__head)->last,__holder); \
00170 ++(__qslot)->elems; })
00171
00172 #define prependq(__qslot,__holder) \
00173 ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \
00174 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder); \
00175 ath(&(__qslot)->head,__holder); \
00176 ++(__qslot)->elems; })
00177
00178 #define appendq(__qslot,__holder) \
00179 ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \
00180 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder); \
00181 ath((__qslot)->head.last,__holder); \
00182 ++(__qslot)->elems; })
00183
00184 #define removeq(__qslot,__holder) \
00185 ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \
00186 XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder); \
00187 dth(__holder); \
00188 --(__qslot)->elems; })
00189
00190 #else
00191
00192 static inline void insertq(xnqueue_t *qslot,
00193 xnholder_t *head, xnholder_t *holder)
00194 {
00195
00196 ath(head->last, holder);
00197 ++qslot->elems;
00198 }
00199
00200 static inline void prependq(xnqueue_t *qslot, xnholder_t *holder)
00201 {
00202
00203 ath(&qslot->head, holder);
00204 ++qslot->elems;
00205 }
00206
00207 static inline void appendq(xnqueue_t *qslot, xnholder_t *holder)
00208 {
00209
00210 ath(qslot->head.last, holder);
00211 ++qslot->elems;
00212 }
00213
00214 static inline void removeq(xnqueue_t *qslot, xnholder_t *holder)
00215 {
00216 dth(holder);
00217 --qslot->elems;
00218 }
00219
00220 #endif
00221
00222 static inline xnholder_t *getheadq(xnqueue_t *qslot)
00223 {
00224 xnholder_t *holder = qslot->head.next;
00225 return holder == &qslot->head ? NULL : holder;
00226 }
00227
00228 static inline xnholder_t *getq(xnqueue_t *qslot)
00229 {
00230 xnholder_t *holder = getheadq(qslot);
00231 if (holder)
00232 removeq(qslot, holder);
00233 return holder;
00234 }
00235
00236 static inline xnholder_t *nextq(xnqueue_t *qslot, xnholder_t *holder)
00237 {
00238 xnholder_t *nextholder = holder->next;
00239 return nextholder == &qslot->head ? NULL : nextholder;
00240 }
00241
00242 static inline xnholder_t *popq(xnqueue_t *qslot, xnholder_t *holder)
00243 {
00244 xnholder_t *nextholder = nextq(qslot, holder);
00245 removeq(qslot, holder);
00246 return nextholder;
00247 }
00248
00249 static inline int countq(xnqueue_t *qslot)
00250 {
00251 return qslot->elems;
00252 }
00253
00254 static inline int emptyq_p(xnqueue_t *qslot)
00255 {
00256 return qslot->head.next == &qslot->head;
00257 }
00258
00259 static inline void moveq(xnqueue_t *dstq, xnqueue_t *srcq)
00260 {
00261 xnholder_t *headsrc = srcq->head.next;
00262 xnholder_t *tailsrc = srcq->head.last;
00263 xnholder_t *headdst = &dstq->head;
00264
00265 headsrc->last->next = tailsrc->next;
00266 tailsrc->next->last = headsrc->last;
00267 headsrc->last = headdst;
00268 tailsrc->next = headdst->next;
00269 headdst->next->last = tailsrc;
00270 headdst->next = headsrc;
00271 dstq->elems += srcq->elems;
00272 srcq->elems = 0;
00273 }
00274
00275
00276
00277 typedef struct xnpholder {
00278
00279 xnholder_t plink;
00280 int prio;
00281
00282 } xnpholder_t;
00283
00284 static inline void initph(xnpholder_t *holder)
00285 {
00286 inith(&holder->plink);
00287
00288 }
00289
00290
00291
00292
00293 typedef struct xnpqueue { xnqueue_t pqueue; } xnpqueue_t;
00294
00295 static inline void initpq(xnpqueue_t *pqslot)
00296 {
00297 initq(&pqslot->pqueue);
00298 }
00299
00300 static inline void insertpq(xnpqueue_t *pqslot,
00301 xnpholder_t *head, xnpholder_t *holder)
00302 {
00303
00304 insertq(&pqslot->pqueue, &head->plink, &holder->plink);
00305 }
00306
00307 static inline void insertpqf(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
00308 {
00309
00310
00311 xnholder_t *curr;
00312
00313 for (curr = pqslot->pqueue.head.last;
00314 curr != &pqslot->pqueue.head; curr = curr->last) {
00315 if (prio <= ((xnpholder_t *)curr)->prio)
00316 break;
00317 }
00318
00319 holder->prio = prio;
00320
00321 insertq(&pqslot->pqueue, curr->next, &holder->plink);
00322 }
00323
00324 static inline void insertpql(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
00325 {
00326
00327
00328 xnholder_t *curr;
00329
00330 for (curr = pqslot->pqueue.head.next;
00331 curr != &pqslot->pqueue.head; curr = curr->next) {
00332 if (prio >= ((xnpholder_t *)curr)->prio)
00333 break;
00334 }
00335
00336 holder->prio = prio;
00337
00338 insertq(&pqslot->pqueue, curr, &holder->plink);
00339 }
00340
00341 static inline xnpholder_t *findpqh(xnpqueue_t *pqslot, int prio)
00342 {
00343
00344
00345 xnholder_t *curr;
00346
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 if (curr && ((xnpholder_t *)curr)->prio == prio)
00354 return (xnpholder_t *)curr;
00355
00356 return NULL;
00357 }
00358
00359 static inline void insertpqfr(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
00360 {
00361
00362
00363
00364
00365
00366 xnholder_t *curr;
00367
00368 for (curr = pqslot->pqueue.head.last;
00369 curr != &pqslot->pqueue.head; curr = curr->last) {
00370 if (prio >= ((xnpholder_t *)curr)->prio)
00371 break;
00372 }
00373
00374 holder->prio = prio;
00375
00376 insertq(&pqslot->pqueue, curr->next, &holder->plink);
00377 }
00378
00379 static inline void insertpqlr(xnpqueue_t *pqslot, xnpholder_t *holder, int prio)
00380 {
00381
00382
00383
00384
00385
00386 xnholder_t *curr;
00387
00388 for (curr = pqslot->pqueue.head.next;
00389 curr != &pqslot->pqueue.head; curr = curr->next) {
00390 if (prio <= ((xnpholder_t *)curr)->prio)
00391 break;
00392 }
00393
00394 holder->prio = prio;
00395
00396 insertq(&pqslot->pqueue, curr, &holder->plink);
00397 }
00398
00399 static inline xnpholder_t *findpqhr(xnpqueue_t *pqslot, int prio)
00400 {
00401
00402
00403
00404
00405
00406 xnholder_t *curr;
00407
00408 for (curr = pqslot->pqueue.head.next;
00409 curr != &pqslot->pqueue.head; curr = curr->next) {
00410 if (prio <= ((xnpholder_t *)curr)->prio)
00411 break;
00412 }
00413
00414 if (curr && ((xnpholder_t *)curr)->prio == prio)
00415 return (xnpholder_t *)curr;
00416
00417 return NULL;
00418 }
00419
00420 static inline void appendpq(xnpqueue_t *pqslot, xnpholder_t *holder)
00421 {
00422 holder->prio = 0;
00423 appendq(&pqslot->pqueue, &holder->plink);
00424 }
00425
00426 static inline void prependpq(xnpqueue_t *pqslot, xnpholder_t *holder)
00427 {
00428 holder->prio = 0;
00429 prependq(&pqslot->pqueue, &holder->plink);
00430 }
00431
00432 static inline void removepq(xnpqueue_t *pqslot, xnpholder_t *holder)
00433 {
00434 removeq(&pqslot->pqueue, &holder->plink);
00435 }
00436
00437 static inline xnpholder_t *getheadpq(xnpqueue_t *pqslot)
00438 {
00439 return (xnpholder_t *)getheadq(&pqslot->pqueue);
00440 }
00441
00442 static inline xnpholder_t *nextpq(xnpqueue_t *pqslot, xnpholder_t *holder)
00443 {
00444 return (xnpholder_t *)nextq(&pqslot->pqueue, &holder->plink);
00445 }
00446
00447 static inline xnpholder_t *getpq(xnpqueue_t *pqslot)
00448 {
00449 return (xnpholder_t *)getq(&pqslot->pqueue);
00450 }
00451
00452 static inline xnpholder_t *poppq(xnpqueue_t *pqslot, xnpholder_t *holder)
00453 {
00454 return (xnpholder_t *)popq(&pqslot->pqueue, &holder->plink);
00455 }
00456
00457 static inline int countpq(xnpqueue_t *pqslot)
00458 {
00459 return countq(&pqslot->pqueue);
00460 }
00461
00462 static inline int emptypq_p(xnpqueue_t *pqslot)
00463 {
00464 return emptyq_p(&pqslot->pqueue);
00465 }
00466
00467
00468
00469 typedef struct xngholder {
00470
00471 xnpholder_t glink;
00472 void *data;
00473
00474 } xngholder_t;
00475
00476 static inline void initgh(xngholder_t *holder, void *data)
00477 {
00478 inith(&holder->glink.plink);
00479 holder->data = data;
00480 }
00481
00482
00483
00484 typedef struct xngqueue {
00485
00486 xnpqueue_t gqueue;
00487 xnqueue_t *freehq;
00488 void (*starvation) (xnqueue_t *);
00489 int threshold;
00490
00491 } xngqueue_t;
00492
00493 static inline void initgq(xngqueue_t *gqslot,
00494 xnqueue_t *freehq,
00495 void (*starvation) (xnqueue_t *),
00496 int threshold)
00497 {
00498 initpq(&gqslot->gqueue);
00499 gqslot->freehq = freehq;
00500 gqslot->starvation = starvation;
00501 gqslot->threshold = threshold;
00502 }
00503
00504 static inline xngholder_t *allocgh(xngqueue_t *gqslot)
00505 {
00506 if (countq(gqslot->freehq) < gqslot->threshold)
00507 gqslot->starvation(gqslot->freehq);
00508
00509 return (xngholder_t *)getq(gqslot->freehq);
00510 }
00511
00512 static inline void *removegh(xngqueue_t *gqslot, xngholder_t *holder)
00513 {
00514 removepq(&gqslot->gqueue, &holder->glink);
00515 appendq(gqslot->freehq, &holder->glink.plink);
00516 return holder->data;
00517 }
00518
00519 static inline void insertgqf(xngqueue_t *gqslot, void *data, int prio)
00520 {
00521 xngholder_t *holder = allocgh(gqslot);
00522 holder->data = data;
00523 return insertpqf(&gqslot->gqueue, &holder->glink, prio);
00524 }
00525
00526 static inline void insertgql(xngqueue_t *gqslot, void *data, int prio)
00527 {
00528 xngholder_t *holder = allocgh(gqslot);
00529 holder->data = data;
00530 insertpql(&gqslot->gqueue, &holder->glink, prio);
00531 }
00532
00533 static inline void appendgq(xngqueue_t *gqslot, void *data)
00534 {
00535 xngholder_t *holder = allocgh(gqslot);
00536 holder->data = data;
00537 appendpq(&gqslot->gqueue, &holder->glink);
00538 }
00539
00540 static inline void prependgq(xngqueue_t *gqslot, void *data)
00541 {
00542 xngholder_t *holder = allocgh(gqslot);
00543 holder->data = data;
00544 prependpq(&gqslot->gqueue, &holder->glink);
00545 }
00546
00547 static inline xngholder_t *getheadgq(xngqueue_t *gqslot)
00548 {
00549 return (xngholder_t *)getheadpq(&gqslot->gqueue);
00550 }
00551
00552 static inline xngholder_t *nextgq(xngqueue_t *gqslot, xngholder_t *holder)
00553 {
00554 return (xngholder_t *)nextpq(&gqslot->gqueue, &holder->glink);
00555 }
00556
00557 static inline void *getgq(xngqueue_t *gqslot)
00558 {
00559 xngholder_t *holder = getheadgq(gqslot);
00560
00561 if (!holder)
00562 return NULL;
00563
00564 appendq(gqslot->freehq, &getpq(&gqslot->gqueue)->plink);
00565
00566 return holder->data;
00567 }
00568
00569 static inline xngholder_t *popgq(xngqueue_t *gqslot, xngholder_t *holder)
00570 {
00571 xngholder_t *nextholder = nextgq(gqslot, holder);
00572 removegh(gqslot, holder);
00573 return nextholder;
00574 }
00575
00576 static inline xngholder_t *findgq(xngqueue_t *gqslot, void *data)
00577 {
00578 xnholder_t *holder;
00579
00580 for (holder = gqslot->gqueue.pqueue.head.next;
00581 holder != &gqslot->gqueue.pqueue.head; holder = holder->next) {
00582 if (((xngholder_t *)holder)->data == data)
00583 return (xngholder_t *)holder;
00584 }
00585
00586 return NULL;
00587 }
00588
00589 static inline void *removegq(xngqueue_t *gqslot, void *data)
00590 {
00591 xngholder_t *holder = findgq(gqslot, data);
00592 return holder ? removegh(gqslot, holder) : NULL;
00593 }
00594
00595 static inline int countgq(xngqueue_t *gqslot)
00596 {
00597 return countpq(&gqslot->gqueue);
00598 }
00599
00600 static inline int emptygq_p(xngqueue_t *gqslot)
00601 {
00602 return emptypq_p(&gqslot->gqueue);
00603 }
00604
00605 #ifdef CONFIG_XENO_OPT_SCALABLE_SCHED
00606
00607
00608
00609
00610
00611 #if BITS_PER_LONG * BITS_PER_LONG < XNCORE_NR_PRIO
00612 #error "Internal bitmap cannot hold so many priority levels"
00613 #endif
00614
00615 #define __MLQ_LONGS ((XNCORE_NR_PRIO+BITS_PER_LONG-1)/BITS_PER_LONG)
00616
00617 typedef struct xnmlqueue {
00618
00619 int loprio, hiprio, elems;
00620
00621 u_long himap, lomap[__MLQ_LONGS];
00622
00623 struct xnqueue queue[XNCORE_NR_PRIO];
00624
00625 } xnmlqueue_t;
00626
00627 #undef __MLQ_LONGS
00628
00629 static inline int countmlq(xnmlqueue_t *mlqslot)
00630 {
00631 return mlqslot->elems;
00632 }
00633
00634 static inline int emptymlq_p(xnmlqueue_t *mlqslot)
00635 {
00636 return mlqslot->himap == 0;
00637 }
00638
00639 static inline int indexmlq(xnmlqueue_t *mlqslot, int prio)
00640 {
00641
00642
00643
00644
00645
00646 return mlqslot->hiprio - prio;
00647 }
00648
00649 static inline int ffsmlq(xnmlqueue_t *mlqslot)
00650 {
00651 int hi = ffnz(mlqslot->himap);
00652 int lo = ffnz(mlqslot->lomap[hi]);
00653 return hi * BITS_PER_LONG + lo;
00654 }
00655
00656 static inline void initmlq(xnmlqueue_t *mlqslot, int loprio, int hiprio)
00657 {
00658 int prio;
00659
00660 mlqslot->elems = 0;
00661 mlqslot->loprio = loprio;
00662 mlqslot->hiprio = hiprio;
00663 mlqslot->himap = 0;
00664 memset(&mlqslot->lomap, 0, sizeof(mlqslot->lomap));
00665
00666 for (prio = 0; prio < XNCORE_NR_PRIO; prio++)
00667 initq(&mlqslot->queue[prio]);
00668
00669 XENO_ASSERT(NUCLEUS,
00670 hiprio - loprio < XNCORE_NR_PRIO,
00671 xnpod_fatal("priority range [%d..%d] is beyond multi-level "
00672 "queue indexing capabilities",
00673 loprio, hiprio));
00674 }
00675
00676 #define XNMLQUEUE_APPEND 0
00677 #define XNMLQUEUE_PREPEND 1
00678
00679 static inline void addmlq(xnmlqueue_t *mlqslot,
00680 xnpholder_t *holder, int idx, int mode)
00681 {
00682 xnqueue_t *queue = &mlqslot->queue[idx];
00683 int hi = idx / BITS_PER_LONG;
00684 int lo = idx % BITS_PER_LONG;
00685
00686 if (mode == XNMLQUEUE_PREPEND)
00687 prependq(queue, &holder->plink);
00688 else
00689 appendq(queue, &holder->plink);
00690
00691 holder->prio = idx;
00692 mlqslot->elems++;
00693 __setbits(mlqslot->himap, 1UL << hi);
00694 __setbits(mlqslot->lomap[hi], 1UL << lo);
00695 }
00696
00697 static inline void insertmlql(xnmlqueue_t *mlqslot,
00698 xnpholder_t *holder, int prio)
00699 {
00700 addmlq(mlqslot, holder, indexmlq(mlqslot, prio), XNMLQUEUE_PREPEND);
00701 }
00702
00703 static inline void insertmlqf(xnmlqueue_t *mlqslot,
00704 xnpholder_t *holder, int prio)
00705 {
00706 addmlq(mlqslot, holder, indexmlq(mlqslot, prio), XNMLQUEUE_APPEND);
00707 }
00708
00709 static inline void appendmlq(xnmlqueue_t *mlqslot, xnpholder_t *holder)
00710 {
00711 addmlq(mlqslot, holder, indexmlq(mlqslot, mlqslot->hiprio),
00712 XNMLQUEUE_APPEND);
00713 }
00714
00715 static inline void prependmlq(xnmlqueue_t *mlqslot, xnpholder_t *holder)
00716 {
00717 addmlq(mlqslot, holder, indexmlq(mlqslot, mlqslot->loprio),
00718 XNMLQUEUE_PREPEND);
00719 }
00720
00721 static inline void removemlq(xnmlqueue_t *mlqslot, xnpholder_t *holder)
00722 {
00723 int idx = holder->prio;
00724 xnqueue_t *queue = &mlqslot->queue[idx];
00725
00726 mlqslot->elems--;
00727
00728 removeq(queue, &holder->plink);
00729
00730 if (emptyq_p(queue)) {
00731 int hi = idx / BITS_PER_LONG;
00732 int lo = idx % BITS_PER_LONG;
00733 __clrbits(mlqslot->lomap[hi], 1UL << lo);
00734 if (mlqslot->lomap[hi] == 0)
00735 __clrbits(mlqslot->himap, 1UL << hi);
00736 }
00737 }
00738
00739 static inline xnpholder_t *findmlqh(xnmlqueue_t *mlqslot, int prio)
00740 {
00741 xnqueue_t *queue = &mlqslot->queue[indexmlq(mlqslot, prio)];
00742 return (xnpholder_t *)getheadq(queue);
00743 }
00744
00745 static inline xnpholder_t *getheadmlq(xnmlqueue_t *mlqslot)
00746 {
00747 xnpholder_t *holder;
00748 xnqueue_t *queue;
00749
00750 if (emptymlq_p(mlqslot))
00751 return NULL;
00752
00753 queue = &mlqslot->queue[ffsmlq(mlqslot)];
00754 holder = (xnpholder_t *)getheadq(queue);
00755
00756 XENO_ASSERT(QUEUES, holder,
00757 xnpod_fatal
00758 ("corrupted multi-level queue, qslot=%p at %s:%d", mlqslot,
00759 __FILE__, __LINE__);
00760 );
00761
00762 return holder;
00763 }
00764
00765 static inline xnpholder_t *getmlq(xnmlqueue_t *mlqslot)
00766 {
00767 xnholder_t *holder;
00768 xnqueue_t *queue;
00769 int idx, hi, lo;
00770
00771 if (emptymlq_p(mlqslot))
00772 return NULL;
00773
00774 idx = ffsmlq(mlqslot);
00775 queue = &mlqslot->queue[idx];
00776 holder = getq(queue);
00777
00778 XENO_ASSERT(QUEUES, holder,
00779 xnpod_fatal
00780 ("corrupted multi-level queue, qslot=%p at %s:%d", mlqslot,
00781 __FILE__, __LINE__);
00782 );
00783
00784 mlqslot->elems--;
00785
00786 if (emptyq_p(queue)) {
00787 hi = idx / BITS_PER_LONG;
00788 lo = idx % BITS_PER_LONG;
00789 __clrbits(mlqslot->lomap[hi], 1UL << lo);
00790 if (mlqslot->lomap[hi] == 0)
00791 __clrbits(mlqslot->himap, 1UL << hi);
00792 }
00793
00794 return (xnpholder_t *)holder;
00795 }
00796
00797 #endif
00798
00799 #endif