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 _xenomai_queue_h
00047 #define _xenomai_queue_h
00048
00049 #include "xenomai/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
00087 } xnqueue_t;
00088
00089 static inline void initq (xnqueue_t *qslot) {
00090 inith(&qslot->head);
00091 qslot->elems = 0;
00092 }
00093
00094 #ifdef CONFIG_RTAI_XENOMAI_DEBUG
00095
00096 #define XENO_DEBUG_CHECK_QUEUE() \
00097 do { \
00098 xnholder_t *curr; \
00099 spl_t s; \
00100 int nelems = 0; \
00101 splhigh(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, qslot=%p", \
00107 qslot->elems, \
00108 qslot); \
00109 splexit(s); \
00110 } while(0)
00111
00112 #define XENO_DEBUG_INSERT_QUEUE() \
00113 do { \
00114 xnholder_t *curr; \
00115 spl_t s; \
00116 splhigh(s); \
00117 curr = qslot->head.last; \
00118 while (curr != &qslot->head && holder != curr) \
00119 curr = curr->last; \
00120 if (curr == holder) \
00121 xnpod_fatal("inserting element twice, holder=%p, qslot=%p", \
00122 holder, \
00123 qslot); \
00124 if (holder->last == NULL) \
00125 xnpod_fatal("holder=%p not initialized, qslot=%p", \
00126 holder, \
00127 qslot); \
00128 splexit(s); \
00129 } while(0)
00130
00131 #define XENO_DEBUG_REMOVE_QUEUE() \
00132 do { \
00133 xnholder_t *curr; \
00134 spl_t s; \
00135 splhigh(s); \
00136 curr = qslot->head.last; \
00137 while (curr != &qslot->head && holder != curr) \
00138 curr = curr->last; \
00139 if (curr == &qslot->head) \
00140 xnpod_fatal("removing non-linked element, holder=%p, qslot=%p", \
00141 holder, \
00142 qslot); \
00143 splexit(s); \
00144 } while(0)
00145 #endif
00146
00147 static inline int insertq (xnqueue_t *qslot,
00148 xnholder_t *head,
00149 xnholder_t *holder) {
00150
00151 #ifdef CONFIG_RTAI_XENOMAI_DEBUG
00152 XENO_DEBUG_CHECK_QUEUE();
00153 XENO_DEBUG_INSERT_QUEUE();
00154 #endif
00155 ath(head->last,holder);
00156 return ++qslot->elems;
00157 }
00158
00159 static inline int prependq (xnqueue_t *qslot,
00160 xnholder_t *holder) {
00161
00162 #ifdef CONFIG_RTAI_XENOMAI_DEBUG
00163 XENO_DEBUG_CHECK_QUEUE();
00164 XENO_DEBUG_INSERT_QUEUE();
00165 #endif
00166 ath(&qslot->head,holder);
00167 return ++qslot->elems;
00168 }
00169
00170 static inline int appendq (xnqueue_t *qslot,
00171 xnholder_t *holder) {
00172
00173 #ifdef CONFIG_RTAI_XENOMAI_DEBUG
00174 XENO_DEBUG_CHECK_QUEUE();
00175 XENO_DEBUG_INSERT_QUEUE();
00176 #endif
00177 ath(qslot->head.last,holder);
00178 return ++qslot->elems;
00179 }
00180
00181 static inline int removeq (xnqueue_t *qslot,
00182 xnholder_t *holder) {
00183 #ifdef CONFIG_RTAI_XENOMAI_DEBUG
00184 XENO_DEBUG_CHECK_QUEUE();
00185 XENO_DEBUG_REMOVE_QUEUE();
00186 #endif
00187 dth(holder);
00188 return --qslot->elems;
00189 }
00190
00191 static inline xnholder_t *getheadq (xnqueue_t *qslot) {
00192 xnholder_t *holder = qslot->head.next;
00193 if (holder == &qslot->head) return NULL;
00194 return holder;
00195 }
00196
00197 static inline xnholder_t *getq (xnqueue_t *qslot) {
00198 xnholder_t *holder = getheadq(qslot);
00199 if (holder) removeq(qslot,holder);
00200 return holder;
00201 }
00202
00203 static inline xnholder_t *nextq (xnqueue_t *qslot,
00204 xnholder_t *holder) {
00205 xnholder_t *nextholder = holder->next;
00206 return nextholder == &qslot->head ? NULL : nextholder;
00207 }
00208
00209 static inline xnholder_t *popq (xnqueue_t *qslot,
00210 xnholder_t *holder) {
00211 xnholder_t *nextholder = nextq(qslot,holder);
00212 removeq(qslot,holder);
00213 return nextholder;
00214 }
00215
00216 static inline int countq (xnqueue_t *qslot) {
00217 return qslot->elems;
00218 }
00219
00220 static inline int moveq (xnqueue_t *dstq, xnqueue_t *srcq) {
00221
00222 xnholder_t *headsrc = srcq->head.next;
00223 xnholder_t *tailsrc = srcq->head.last->last;
00224 xnholder_t *headdst = &dstq->head;
00225
00226 headsrc->last->next = tailsrc->next;
00227 tailsrc->next->last = headsrc->last;
00228 headsrc->last = headdst;
00229 tailsrc->next = headdst->next;
00230 headdst->next->last = tailsrc;
00231 headdst->next = headsrc;
00232 dstq->elems += srcq->elems;
00233 srcq->elems = 0;
00234
00235 return dstq->elems;
00236 }
00237
00238
00239
00240 typedef struct xnpholder {
00241
00242 xnholder_t plink;
00243 int prio;
00244
00245 } xnpholder_t;
00246
00247 static inline void initph (xnpholder_t *holder) {
00248 inith(&holder->plink);
00249
00250 }
00251
00252
00253
00254 #define xnqueue_up (-1)
00255 #define xnqueue_down 1
00256
00257 typedef struct xnpqueue {
00258
00259 xnqueue_t pqueue;
00260 int qdir;
00261
00262 } xnpqueue_t;
00263
00264 static inline void initpq (xnpqueue_t *pqslot,
00265 int qdir) {
00266 initq(&pqslot->pqueue);
00267 pqslot->qdir = qdir;
00268 }
00269
00270 static inline int insertpq (xnpqueue_t *pqslot,
00271 xnpholder_t *head,
00272 xnpholder_t *holder) {
00273
00274 return insertq(&pqslot->pqueue,&head->plink,&holder->plink);
00275 }
00276
00277 static inline int insertpqf (xnpqueue_t *pqslot,
00278 xnpholder_t *holder,
00279 int prio) {
00280
00281
00282
00283 xnholder_t *curr;
00284
00285 if (pqslot->qdir == xnqueue_down) {
00286 for (curr = pqslot->pqueue.head.last;
00287 curr != &pqslot->pqueue.head; curr = curr->last) {
00288 if (prio <= ((xnpholder_t *)curr)->prio)
00289 break;
00290 }
00291 }
00292 else {
00293 for (curr = pqslot->pqueue.head.last;
00294 curr != &pqslot->pqueue.head; curr = curr->last) {
00295 if (prio >= ((xnpholder_t *)curr)->prio)
00296 break;
00297 }
00298 }
00299
00300 holder->prio = prio;
00301
00302 return insertq(&pqslot->pqueue,curr->next,&holder->plink);
00303 }
00304
00305 static inline int insertpql (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.next;
00315 curr != &pqslot->pqueue.head; curr = curr->next) {
00316 if (prio >= ((xnpholder_t *)curr)->prio)
00317 break;
00318 }
00319 }
00320 else {
00321 for (curr = pqslot->pqueue.head.next;
00322 curr != &pqslot->pqueue.head; curr = curr->next) {
00323 if (prio <= ((xnpholder_t *)curr)->prio)
00324 break;
00325 }
00326 }
00327
00328 holder->prio = prio;
00329
00330 return insertq(&pqslot->pqueue,curr,&holder->plink);
00331 }
00332
00333 static inline xnpholder_t *findpqh (xnpqueue_t *pqslot,
00334 int prio) {
00335
00336
00337
00338 xnholder_t *curr;
00339
00340 if (pqslot->qdir == xnqueue_down) {
00341 for (curr = pqslot->pqueue.head.next;
00342 curr != &pqslot->pqueue.head; curr = curr->next) {
00343 if (prio >= ((xnpholder_t *)curr)->prio)
00344 break;
00345 }
00346 }
00347 else {
00348 for (curr = pqslot->pqueue.head.next;
00349 curr != &pqslot->pqueue.head; curr = curr->next) {
00350 if (prio <= ((xnpholder_t *)curr)->prio)
00351 break;
00352 }
00353 }
00354
00355 if (curr && ((xnpholder_t *)curr)->prio == prio)
00356 return (xnpholder_t *)curr;
00357
00358 return NULL;
00359 }
00360
00361 static inline int appendpq (xnpqueue_t *pqslot,
00362 xnpholder_t *holder) {
00363 holder->prio = 0;
00364 return appendq(&pqslot->pqueue,&holder->plink);
00365 }
00366
00367 static inline int prependpq (xnpqueue_t *pqslot,
00368 xnpholder_t *holder) {
00369 holder->prio = 0;
00370 return prependq(&pqslot->pqueue,&holder->plink);
00371 }
00372
00373 static inline int removepq (xnpqueue_t *pqslot,
00374 xnpholder_t *holder) {
00375 return removeq(&pqslot->pqueue,&holder->plink);
00376 }
00377
00378 static inline xnpholder_t *getheadpq (xnpqueue_t *pqslot) {
00379 return (xnpholder_t *)getheadq(&pqslot->pqueue);
00380 }
00381
00382 static inline xnpholder_t *nextpq (xnpqueue_t *pqslot,
00383 xnpholder_t *holder) {
00384 return (xnpholder_t *)nextq(&pqslot->pqueue,&holder->plink);
00385 }
00386
00387 static inline xnpholder_t *getpq (xnpqueue_t *pqslot) {
00388 return (xnpholder_t *)getq(&pqslot->pqueue);
00389 }
00390
00391 static inline xnpholder_t *poppq (xnpqueue_t *pqslot,
00392 xnpholder_t *holder) {
00393 return (xnpholder_t *)popq(&pqslot->pqueue,&holder->plink);
00394 }
00395
00396 static inline int countpq (xnpqueue_t *pqslot) {
00397 return countq(&pqslot->pqueue);
00398 }
00399
00400
00401
00402 typedef struct xngholder {
00403
00404 xnpholder_t glink;
00405 void *data;
00406
00407 } xngholder_t;
00408
00409 static inline void initgh (xngholder_t *holder, void *data) {
00410 inith(&holder->glink.plink);
00411 holder->data = data;
00412 }
00413
00414
00415
00416 typedef struct xngqueue {
00417
00418 xnpqueue_t gqueue;
00419 xnqueue_t *freehq;
00420 void (*starvation)(xnqueue_t *);
00421 int threshold;
00422
00423 } xngqueue_t;
00424
00425 static inline void initgq (xngqueue_t *gqslot,
00426 xnqueue_t *freehq,
00427 void (*starvation)(xnqueue_t *),
00428 int threshold,
00429 int qdir) {
00430 initpq(&gqslot->gqueue,qdir);
00431 gqslot->freehq = freehq;
00432 gqslot->starvation = starvation;
00433 gqslot->threshold = threshold;
00434 }
00435
00436 static inline xngholder_t *allocgh (xngqueue_t *gqslot) {
00437
00438 if (countq(gqslot->freehq) < gqslot->threshold)
00439 gqslot->starvation(gqslot->freehq);
00440
00441 return (xngholder_t *)getq(gqslot->freehq);
00442 }
00443
00444 static inline void *removegh (xngqueue_t *gqslot,
00445 xngholder_t *holder) {
00446 removepq(&gqslot->gqueue,&holder->glink);
00447 appendq(gqslot->freehq,&holder->glink.plink);
00448 return holder->data;
00449 }
00450
00451 static inline int insertgqf (xngqueue_t *gqslot,
00452 void *data,
00453 int prio) {
00454 xngholder_t *holder = allocgh(gqslot);
00455 holder->data = data;
00456 return insertpqf(&gqslot->gqueue,&holder->glink,prio);
00457 }
00458
00459 static inline int insertgql (xngqueue_t *gqslot,
00460 void *data,
00461 int prio) {
00462 xngholder_t *holder = allocgh(gqslot);
00463 holder->data = data;
00464 return insertpql(&gqslot->gqueue,&holder->glink,prio);
00465 }
00466
00467 static inline int appendgq (xngqueue_t *gqslot,
00468 void *data) {
00469 xngholder_t *holder = allocgh(gqslot);
00470 holder->data = data;
00471 return appendpq(&gqslot->gqueue,&holder->glink);
00472 }
00473
00474 static inline int prependgq (xngqueue_t *gqslot,
00475 void *data) {
00476 xngholder_t *holder = allocgh(gqslot);
00477 holder->data = data;
00478 return prependpq(&gqslot->gqueue,&holder->glink);
00479 }
00480
00481 static inline xngholder_t *getheadgq (xngqueue_t *gqslot) {
00482 return (xngholder_t *)getheadpq(&gqslot->gqueue);
00483 }
00484
00485 static inline xngholder_t *nextgq (xngqueue_t *gqslot,
00486 xngholder_t *holder) {
00487 return (xngholder_t *)nextpq(&gqslot->gqueue,&holder->glink);
00488 }
00489
00490 static inline void *getgq (xngqueue_t *gqslot) {
00491 xngholder_t *holder = getheadgq(gqslot);
00492 if (!holder) return NULL;
00493 appendq(gqslot->freehq,&getpq(&gqslot->gqueue)->plink);
00494 return holder->data;
00495 }
00496
00497 static inline xngholder_t *popgq (xngqueue_t *gqslot,
00498 xngholder_t *holder) {
00499 xngholder_t *nextholder = nextgq(gqslot,holder);
00500 removegh(gqslot,holder);
00501 return nextholder;
00502 }
00503
00504 static inline xngholder_t *findgq (xngqueue_t *gqslot,
00505 void *data) {
00506 xnholder_t *holder;
00507
00508 for (holder = gqslot->gqueue.pqueue.head.next;
00509 holder != &gqslot->gqueue.pqueue.head; holder = holder->next) {
00510 if (((xngholder_t *)holder)->data == data)
00511 return (xngholder_t *)holder;
00512 }
00513
00514 return NULL;
00515 }
00516
00517 static inline void *removegq (xngqueue_t *gqslot,
00518 void *data) {
00519 xngholder_t *holder = findgq(gqslot,data);
00520 return holder ? removegh(gqslot,holder) : NULL;
00521 }
00522
00523 static inline int countgq (xngqueue_t *gqslot) {
00524 return countpq(&gqslot->gqueue);
00525 }
00526
00527 #endif