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