Xenomai API
2.5.6.1
|
00001 /* 00002 * Copyright (C) 2001,2002,2003 Philippe Gerum <[email protected]>. 00003 * Copyright (C) 2005 Dmitry Adamushko <[email protected]> 00004 * 00005 * Xenomai is free software; you can redistribute it and/or modify 00006 * it under the terms of the GNU General Public License as published 00007 * by the Free Software Foundation; either version 2 of the License, 00008 * or (at your option) any later version. 00009 * 00010 * Xenomai is distributed in the hope that it will be useful, but 00011 * WITHOUT ANY WARRANTY; without even the implied warranty of 00012 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00013 * General Public License for more details. 00014 * 00015 * You should have received a copy of the GNU General Public License 00016 * along with Xenomai; if not, write to the Free Software 00017 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 00018 * 02111-1307, USA. 00019 */ 00020 00021 #ifndef _XENO_NUCLEUS_QUEUE_H 00022 #define _XENO_NUCLEUS_QUEUE_H 00023 00024 #include <nucleus/types.h> 00025 #include <nucleus/assert.h> 00026 00027 /* Basic element holder */ 00028 00029 typedef struct xnholder { 00030 00031 struct xnholder *next; 00032 struct xnholder *last; 00033 00034 } xnholder_t; 00035 00036 static inline void inith(xnholder_t *holder) 00037 { 00038 /* Holding queues are doubly-linked and circular */ 00039 holder->last = holder; 00040 holder->next = holder; 00041 } 00042 00043 static inline void ath(xnholder_t *head, xnholder_t *holder) 00044 { 00045 /* Inserts the new element right after the heading one */ 00046 holder->last = head; 00047 holder->next = head->next; 00048 holder->next->last = holder; 00049 head->next = holder; 00050 } 00051 00052 static inline void dth(xnholder_t *holder) 00053 { 00054 holder->last->next = holder->next; 00055 holder->next->last = holder->last; 00056 } 00057 00058 /* Basic element queue */ 00059 00060 typedef struct xnqueue { 00061 00062 xnholder_t head; 00063 int elems; 00064 #if defined(__KERNEL__) && XENO_DEBUG(QUEUES) 00065 DECLARE_XNLOCK(lock); 00066 #endif /* __KERNEL__ && XENO_DEBUG(QUEUES) */ 00067 00068 } xnqueue_t; 00069 00070 #if XENO_DEBUG(QUEUES) && (defined(CONFIG_SMP) || XENO_DEBUG(XNLOCK)) 00071 #define XNQUEUE_INITIALIZER(q) { { &(q).head, &(q).head }, 0, XNARCH_LOCK_UNLOCKED } 00072 #else /* !(XENO_DEBUG(QUEUES) */ 00073 #define XNQUEUE_INITIALIZER(q) { { &(q).head, &(q).head }, 0 } 00074 #endif /* XENO_DEBUG(QUEUES) */ 00075 00076 #define DEFINE_XNQUEUE(q) xnqueue_t q = XNQUEUE_INITIALIZER(q) 00077 00078 static inline void initq(xnqueue_t *qslot) 00079 { 00080 inith(&qslot->head); 00081 qslot->elems = 0; 00082 #if defined(__KERNEL__) && XENO_DEBUG(QUEUES) 00083 xnlock_init(&qslot->lock); 00084 #endif /* __KERNEL__ && XENO_DEBUG(QUEUES) */ 00085 } 00086 00087 #if XENO_DEBUG(QUEUES) 00088 00089 #if defined(__KERNEL__) || defined(__XENO_SIM__) 00090 00091 #define XENO_DEBUG_CHECK_QUEUE(__qslot) \ 00092 do { \ 00093 xnholder_t *curr; \ 00094 spl_t s; \ 00095 int nelems = 0; \ 00096 xnlock_get_irqsave(&(__qslot)->lock,s); \ 00097 curr = (__qslot)->head.last; \ 00098 while (curr != &(__qslot)->head && nelems < (__qslot)->elems) \ 00099 curr = curr->last, nelems++; \ 00100 if (curr != &(__qslot)->head || nelems != (__qslot)->elems) \ 00101 xnpod_fatal("corrupted queue, qslot->elems=%d/%d, qslot=%p at %s:%d", \ 00102 nelems, \ 00103 (__qslot)->elems, \ 00104 __qslot, \ 00105 __FILE__,__LINE__); \ 00106 xnlock_put_irqrestore(&(__qslot)->lock,s); \ 00107 } while(0) 00108 00109 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder) \ 00110 do { \ 00111 xnholder_t *curr; \ 00112 spl_t s; \ 00113 xnlock_get_irqsave(&(__qslot)->lock,s); \ 00114 curr = (__qslot)->head.last; \ 00115 while (curr != &(__qslot)->head && (__holder) != curr) \ 00116 curr = curr->last; \ 00117 if (curr == (__holder)) \ 00118 xnpod_fatal("inserting element twice, holder=%p, qslot=%p at %s:%d", \ 00119 __holder, \ 00120 __qslot, \ 00121 __FILE__,__LINE__); \ 00122 if ((__holder)->last == NULL) \ 00123 xnpod_fatal("holder=%p not initialized, qslot=%p", \ 00124 __holder, \ 00125 __qslot); \ 00126 xnlock_put_irqrestore(&(__qslot)->lock,s); \ 00127 } while(0) 00128 00129 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder) \ 00130 do { \ 00131 xnholder_t *curr; \ 00132 spl_t s; \ 00133 xnlock_get_irqsave(&(__qslot)->lock,s); \ 00134 curr = (__qslot)->head.last; \ 00135 while (curr != &(__qslot)->head && (__holder) != curr) \ 00136 curr = curr->last; \ 00137 if (curr == &(__qslot)->head) \ 00138 xnpod_fatal("removing non-linked element, holder=%p, qslot=%p at %s:%d", \ 00139 __holder, \ 00140 __qslot, \ 00141 __FILE__,__LINE__); \ 00142 xnlock_put_irqrestore(&(__qslot)->lock,s); \ 00143 } while(0) 00144 00145 #else /* !(__KERNEL__ || __XENO_SIM__) */ 00146 00147 /* Disable queue checks in user-space code which does not run as part 00148 of any virtual machine, e.g. skin call interface libs. */ 00149 00150 #define XENO_DEBUG_CHECK_QUEUE(__qslot) 00151 #define XENO_DEBUG_INSERT_QUEUE(__qslot,__holder) 00152 #define XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder) 00153 00154 #endif /* __KERNEL__ || __XENO_SIM__ */ 00155 00156 /* Write the following as macros so that line numbering information 00157 keeps pointing at the real caller in diagnosis messages. */ 00158 00159 #define insertq(__qslot,__head,__holder) \ 00160 ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \ 00161 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder); \ 00162 ath((__head)->last,__holder); \ 00163 ++(__qslot)->elems; }) 00164 00165 #define prependq(__qslot,__holder) \ 00166 ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \ 00167 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder); \ 00168 ath(&(__qslot)->head,__holder); \ 00169 ++(__qslot)->elems; }) 00170 00171 #define appendq(__qslot,__holder) \ 00172 ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \ 00173 XENO_DEBUG_INSERT_QUEUE(__qslot,__holder); \ 00174 ath((__qslot)->head.last,__holder); \ 00175 ++(__qslot)->elems; }) 00176 00177 #define removeq(__qslot,__holder) \ 00178 ({ XENO_DEBUG_CHECK_QUEUE(__qslot); \ 00179 XENO_DEBUG_REMOVE_QUEUE(__qslot,__holder); \ 00180 dth(__holder); \ 00181 --(__qslot)->elems; }) 00182 00183 #else /* !XENO_DEBUG(QUEUES) */ 00184 00185 static inline void insertq(xnqueue_t *qslot, 00186 xnholder_t *head, xnholder_t *holder) 00187 { 00188 /* Insert the <holder> element before <head> */ 00189 ath(head->last, holder); 00190 ++qslot->elems; 00191 } 00192 00193 static inline void prependq(xnqueue_t *qslot, xnholder_t *holder) 00194 { 00195 /* Prepend the element to the queue */ 00196 ath(&qslot->head, holder); 00197 ++qslot->elems; 00198 } 00199 00200 static inline void appendq(xnqueue_t *qslot, xnholder_t *holder) 00201 { 00202 /* Append the element to the queue */ 00203 ath(qslot->head.last, holder); 00204 ++qslot->elems; 00205 } 00206 00207 static inline void removeq(xnqueue_t *qslot, xnholder_t *holder) 00208 { 00209 dth(holder); 00210 --qslot->elems; 00211 } 00212 00213 #endif /* XENO_DEBUG(QUEUES) */ 00214 00215 static inline xnholder_t *getheadq(xnqueue_t *qslot) 00216 { 00217 xnholder_t *holder = qslot->head.next; 00218 return holder == &qslot->head ? NULL : holder; 00219 } 00220 00221 static inline xnholder_t *getq(xnqueue_t *qslot) 00222 { 00223 xnholder_t *holder = getheadq(qslot); 00224 if (holder) 00225 removeq(qslot, holder); 00226 return holder; 00227 } 00228 00229 static inline xnholder_t *nextq(xnqueue_t *qslot, xnholder_t *holder) 00230 { 00231 xnholder_t *nextholder = holder->next; 00232 return nextholder == &qslot->head ? NULL : nextholder; 00233 } 00234 00235 static inline xnholder_t *popq(xnqueue_t *qslot, xnholder_t *holder) 00236 { 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 { 00244 return qslot->elems; 00245 } 00246 00247 static inline int emptyq_p(xnqueue_t *qslot) 00248 { 00249 return qslot->head.next == &qslot->head; 00250 } 00251 00252 static inline void moveq(xnqueue_t *dstq, xnqueue_t *srcq) 00253 { 00254 xnholder_t *headsrc = srcq->head.next; 00255 xnholder_t *tailsrc = srcq->head.last; 00256 xnholder_t *headdst = &dstq->head; 00257 00258 if (emptyq_p(srcq)) 00259 return; 00260 00261 /* srcq elements are moved to head of dstq (LIFO) */ 00262 headsrc->last->next = tailsrc->next; 00263 tailsrc->next->last = headsrc->last; 00264 headsrc->last = headdst; 00265 tailsrc->next = headdst->next; 00266 headdst->next->last = tailsrc; 00267 headdst->next = headsrc; 00268 dstq->elems += srcq->elems; 00269 srcq->elems = 0; 00270 } 00271 00272 /* Prioritized element holder */ 00273 00274 typedef struct xnpholder { 00275 00276 xnholder_t plink; 00277 int prio; 00278 00279 } xnpholder_t; 00280 00281 static inline void initph(xnpholder_t *holder) 00282 { 00283 inith(&holder->plink); 00284 /* Priority is set upon queue insertion */ 00285 } 00286 00287 /* Prioritized element queue - we only manage a descending queuing 00288 order (highest numbered priorities are linked first). */ 00289 00290 typedef struct xnpqueue { xnqueue_t pqueue; } xnpqueue_t; 00291 00292 static inline void initpq(xnpqueue_t *pqslot) 00293 { 00294 initq(&pqslot->pqueue); 00295 } 00296 00297 static inline void insertpq(xnpqueue_t *pqslot, 00298 xnpholder_t *head, xnpholder_t *holder) 00299 { 00300 /* Insert the <holder> element before <head> */ 00301 insertq(&pqslot->pqueue, &head->plink, &holder->plink); 00302 } 00303 00304 static inline void insertpqf(xnpqueue_t *pqslot, xnpholder_t *holder, int prio) 00305 { 00306 /* Insert the element at the end of its priority group (FIFO) */ 00307 00308 xnholder_t *curr; 00309 00310 for (curr = pqslot->pqueue.head.last; 00311 curr != &pqslot->pqueue.head; curr = curr->last) { 00312 if (prio <= ((xnpholder_t *)curr)->prio) 00313 break; 00314 } 00315 00316 holder->prio = prio; 00317 00318 insertq(&pqslot->pqueue, curr->next, &holder->plink); 00319 } 00320 00321 static inline void insertpql(xnpqueue_t *pqslot, xnpholder_t *holder, int prio) 00322 { 00323 /* Insert the element at the front of its priority group (LIFO) */ 00324 00325 xnholder_t *curr; 00326 00327 for (curr = pqslot->pqueue.head.next; 00328 curr != &pqslot->pqueue.head; curr = curr->next) { 00329 if (prio >= ((xnpholder_t *)curr)->prio) 00330 break; 00331 } 00332 00333 holder->prio = prio; 00334 00335 insertq(&pqslot->pqueue, curr, &holder->plink); 00336 } 00337 00338 static inline xnpholder_t *findpqh(xnpqueue_t *pqslot, int prio) 00339 { 00340 /* Find the element heading a given priority group */ 00341 00342 xnholder_t *curr; 00343 00344 for (curr = pqslot->pqueue.head.next; 00345 curr != &pqslot->pqueue.head; curr = curr->next) { 00346 if (prio >= ((xnpholder_t *)curr)->prio) 00347 break; 00348 } 00349 00350 if (curr && ((xnpholder_t *)curr)->prio == prio) 00351 return (xnpholder_t *)curr; 00352 00353 return NULL; 00354 } 00355 00356 static inline void insertpqfr(xnpqueue_t *pqslot, xnpholder_t *holder, int prio) 00357 { 00358 /* 00359 * Insert the element at the front of its priority group 00360 * (FIFO) - Reverse queueing applied (lowest numbered 00361 * priorities are put at front). 00362 */ 00363 xnholder_t *curr; 00364 00365 for (curr = pqslot->pqueue.head.last; 00366 curr != &pqslot->pqueue.head; curr = curr->last) { 00367 if (prio >= ((xnpholder_t *)curr)->prio) 00368 break; 00369 } 00370 00371 holder->prio = prio; 00372 00373 insertq(&pqslot->pqueue, curr->next, &holder->plink); 00374 } 00375 00376 static inline void insertpqlr(xnpqueue_t *pqslot, xnpholder_t *holder, int prio) 00377 { 00378 /* 00379 * Insert the element at the front of its priority group 00380 * (LIFO) - Reverse queueing applied (lowest numbered 00381 * priorities are put at front). 00382 */ 00383 xnholder_t *curr; 00384 00385 for (curr = pqslot->pqueue.head.next; 00386 curr != &pqslot->pqueue.head; curr = curr->next) { 00387 if (prio <= ((xnpholder_t *)curr)->prio) 00388 break; 00389 } 00390 00391 holder->prio = prio; 00392 00393 insertq(&pqslot->pqueue, curr, &holder->plink); 00394 } 00395 00396 static inline xnpholder_t *findpqhr(xnpqueue_t *pqslot, int prio) 00397 { 00398 /* 00399 * Find the element heading a given priority group - Reverse 00400 * queueing assumed (lowest numbered priorities should be at 00401 * front). 00402 */ 00403 xnholder_t *curr; 00404 00405 for (curr = pqslot->pqueue.head.next; 00406 curr != &pqslot->pqueue.head; curr = curr->next) { 00407 if (prio <= ((xnpholder_t *)curr)->prio) 00408 break; 00409 } 00410 00411 if (curr && ((xnpholder_t *)curr)->prio == prio) 00412 return (xnpholder_t *)curr; 00413 00414 return NULL; 00415 } 00416 00417 static inline void appendpq(xnpqueue_t *pqslot, xnpholder_t *holder) 00418 { 00419 holder->prio = 0; 00420 appendq(&pqslot->pqueue, &holder->plink); 00421 } 00422 00423 static inline void prependpq(xnpqueue_t *pqslot, xnpholder_t *holder) 00424 { 00425 holder->prio = 0; 00426 prependq(&pqslot->pqueue, &holder->plink); 00427 } 00428 00429 static inline void removepq(xnpqueue_t *pqslot, xnpholder_t *holder) 00430 { 00431 removeq(&pqslot->pqueue, &holder->plink); 00432 } 00433 00434 static inline xnpholder_t *getheadpq(xnpqueue_t *pqslot) 00435 { 00436 return (xnpholder_t *)getheadq(&pqslot->pqueue); 00437 } 00438 00439 static inline xnpholder_t *nextpq(xnpqueue_t *pqslot, xnpholder_t *holder) 00440 { 00441 return (xnpholder_t *)nextq(&pqslot->pqueue, &holder->plink); 00442 } 00443 00444 static inline xnpholder_t *getpq(xnpqueue_t *pqslot) 00445 { 00446 return (xnpholder_t *)getq(&pqslot->pqueue); 00447 } 00448 00449 static inline xnpholder_t *poppq(xnpqueue_t *pqslot, xnpholder_t *holder) 00450 { 00451 return (xnpholder_t *)popq(&pqslot->pqueue, &holder->plink); 00452 } 00453 00454 static inline int countpq(xnpqueue_t *pqslot) 00455 { 00456 return countq(&pqslot->pqueue); 00457 } 00458 00459 static inline int emptypq_p(xnpqueue_t *pqslot) 00460 { 00461 return emptyq_p(&pqslot->pqueue); 00462 } 00463 00464 /* Generic prioritized element holder */ 00465 00466 typedef struct xngholder { 00467 00468 xnpholder_t glink; 00469 void *data; 00470 00471 } xngholder_t; 00472 00473 static inline void initgh(xngholder_t *holder, void *data) 00474 { 00475 inith(&holder->glink.plink); 00476 holder->data = data; 00477 } 00478 00479 /* Generic element queue */ 00480 00481 typedef struct xngqueue { 00482 00483 xnpqueue_t gqueue; 00484 xnqueue_t *freehq; 00485 void (*starvation) (xnqueue_t *); 00486 int threshold; 00487 00488 } xngqueue_t; 00489 00490 static inline void initgq(xngqueue_t *gqslot, 00491 xnqueue_t *freehq, 00492 void (*starvation) (xnqueue_t *), 00493 int threshold) 00494 { 00495 initpq(&gqslot->gqueue); 00496 gqslot->freehq = freehq; 00497 gqslot->starvation = starvation; 00498 gqslot->threshold = threshold; 00499 } 00500 00501 static inline xngholder_t *allocgh(xngqueue_t *gqslot) 00502 { 00503 if (countq(gqslot->freehq) < gqslot->threshold) 00504 gqslot->starvation(gqslot->freehq); 00505 00506 return (xngholder_t *)getq(gqslot->freehq); 00507 } 00508 00509 static inline void *removegh(xngqueue_t *gqslot, xngholder_t *holder) 00510 { 00511 removepq(&gqslot->gqueue, &holder->glink); 00512 appendq(gqslot->freehq, &holder->glink.plink); 00513 return holder->data; 00514 } 00515 00516 static inline void insertgqf(xngqueue_t *gqslot, void *data, int prio) 00517 { 00518 xngholder_t *holder = allocgh(gqslot); 00519 holder->data = data; 00520 return insertpqf(&gqslot->gqueue, &holder->glink, prio); 00521 } 00522 00523 static inline void insertgql(xngqueue_t *gqslot, void *data, int prio) 00524 { 00525 xngholder_t *holder = allocgh(gqslot); 00526 holder->data = data; 00527 insertpql(&gqslot->gqueue, &holder->glink, prio); 00528 } 00529 00530 static inline void appendgq(xngqueue_t *gqslot, void *data) 00531 { 00532 xngholder_t *holder = allocgh(gqslot); 00533 holder->data = data; 00534 appendpq(&gqslot->gqueue, &holder->glink); 00535 } 00536 00537 static inline void prependgq(xngqueue_t *gqslot, void *data) 00538 { 00539 xngholder_t *holder = allocgh(gqslot); 00540 holder->data = data; 00541 prependpq(&gqslot->gqueue, &holder->glink); 00542 } 00543 00544 static inline xngholder_t *getheadgq(xngqueue_t *gqslot) 00545 { 00546 return (xngholder_t *)getheadpq(&gqslot->gqueue); 00547 } 00548 00549 static inline xngholder_t *nextgq(xngqueue_t *gqslot, xngholder_t *holder) 00550 { 00551 return (xngholder_t *)nextpq(&gqslot->gqueue, &holder->glink); 00552 } 00553 00554 static inline void *getgq(xngqueue_t *gqslot) 00555 { 00556 xngholder_t *holder = getheadgq(gqslot); 00557 00558 if (!holder) 00559 return NULL; 00560 00561 appendq(gqslot->freehq, &getpq(&gqslot->gqueue)->plink); 00562 00563 return holder->data; 00564 } 00565 00566 static inline xngholder_t *popgq(xngqueue_t *gqslot, xngholder_t *holder) 00567 { 00568 xngholder_t *nextholder = nextgq(gqslot, holder); 00569 removegh(gqslot, holder); 00570 return nextholder; 00571 } 00572 00573 static inline xngholder_t *findgq(xngqueue_t *gqslot, void *data) 00574 { 00575 xnholder_t *holder; 00576 00577 for (holder = gqslot->gqueue.pqueue.head.next; 00578 holder != &gqslot->gqueue.pqueue.head; holder = holder->next) { 00579 if (((xngholder_t *)holder)->data == data) 00580 return (xngholder_t *)holder; 00581 } 00582 00583 return NULL; 00584 } 00585 00586 static inline void *removegq(xngqueue_t *gqslot, void *data) 00587 { 00588 xngholder_t *holder = findgq(gqslot, data); 00589 return holder ? removegh(gqslot, holder) : NULL; 00590 } 00591 00592 static inline int countgq(xngqueue_t *gqslot) 00593 { 00594 return countpq(&gqslot->gqueue); 00595 } 00596 00597 static inline int emptygq_p(xngqueue_t *gqslot) 00598 { 00599 return emptypq_p(&gqslot->gqueue); 00600 } 00601 00602 #endif /* !_XENO_NUCLEUS_QUEUE_H */