00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 #ifndef _RTAI_NUCLEUS_QUEUE_H
00047 #define _RTAI_NUCLEUS_QUEUE_H
00048
00049 #include <nucleus/types.h>
00050
00051
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
00062 holder->last = holder;
00063 holder->next = holder;
00064 }
00065
00066 static inline void ath (xnholder_t *head,
00067 xnholder_t *holder) {
00068
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
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
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
00095 #define DECLARE_XNQUEUE(q) xnqueue_t q = { { &(q).head, &(q).head }, 0 }
00096 #endif
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
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) \
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
00161
00162
00163
00164
00165 #define XENO_DEBUG_CHECK_QUEUE()
00166 #define XENO_DEBUG_INSERT_QUEUE()
00167 #define XENO_DEBUG_REMOVE_QUEUE()
00168
00169 #endif
00170
00171 #endif
00172
00173 static inline int insertq (xnqueue_t *qslot,
00174 xnholder_t *head,
00175 xnholder_t *holder) {
00176
00177 #ifdef CONFIG_RTAI_OPT_DEBUG
00178 XENO_DEBUG_CHECK_QUEUE();
00179 XENO_DEBUG_INSERT_QUEUE();
00180 #endif
00181 ath(head->last,holder);
00182 return ++qslot->elems;
00183 }
00184
00185 static inline int prependq (xnqueue_t *qslot,
00186 xnholder_t *holder) {
00187
00188 #ifdef CONFIG_RTAI_OPT_DEBUG
00189 XENO_DEBUG_CHECK_QUEUE();
00190 XENO_DEBUG_INSERT_QUEUE();
00191 #endif
00192 ath(&qslot->head,holder);
00193 return ++qslot->elems;
00194 }
00195
00196 static inline int appendq (xnqueue_t *qslot,
00197 xnholder_t *holder) {
00198
00199 #ifdef CONFIG_RTAI_OPT_DEBUG
00200 XENO_DEBUG_CHECK_QUEUE();
00201 XENO_DEBUG_INSERT_QUEUE();
00202 #endif
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
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
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
00276 }
00277
00278
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
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
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
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
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
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
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