include/nucleus/bheap.h

00001 /*
00002  * Copyright (C) 2006 Gilles Chanteperdrix <gilles.chanteperdrix@laposte.net>
00003  *
00004  * Xenomai is free software; you can redistribute it and/or modify
00005  * it under the terms of the GNU General Public License as published
00006  * by the Free Software Foundation; either version 2 of the License,
00007  * or (at your option) any later version.
00008  *
00009  * Xenomai is distributed in the hope that it will be useful, but
00010  * WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00012  * General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU General Public License
00015  * along with Xenomai; if not, write to the Free Software
00016  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
00017  * 02111-1307, USA.
00018  */
00019 
00020 #ifndef _XENO_NUCLEUS_BHEAP_H
00021 #define _XENO_NUCLEUS_BHEAP_H
00022 
00023 #include <nucleus/compiler.h>
00024 
00025 /* debug support */
00026 #include <nucleus/assert.h>
00027 
00028 #ifndef CONFIG_XENO_OPT_DEBUG_QUEUES
00029 #define CONFIG_XENO_OPT_DEBUG_QUEUES 0
00030 #endif
00031 
00032 /* Priority queue implementation, using a binary heap. */
00033 
00034 typedef unsigned long long bheap_key_t;
00035 
00036 typedef struct bheaph {
00037         bheap_key_t key;
00038         unsigned prio;
00039         unsigned pos;
00040 } bheaph_t;
00041 
00042 #define bheaph_init(holder) do { } while (0)
00043 #define bheaph_key(holder)  ((holder)->key)
00044 #define bheaph_prio(holder) ((holder)->prio)
00045 #define bheaph_pos(holder)  ((holder)->pos)
00046 #define bheaph_lt(h1, h2)   ((long long) ((h1)->key - (h2)->key) < 0 || \
00047                              ((h1)->key == (h2)->key &&                 \
00048                               (h1)->prio > (h2)->prio))
00049 
00050 typedef struct bheap {
00051         unsigned sz;
00052         unsigned last;
00053         bheaph_t *elems[1]; /* only padding, indexing starts at 1 */
00054 } bheap_t;
00055 
00056 #define DECLARE_BHEAP_CONTAINER(name, sz)       \
00057         struct {                                \
00058                 bheap_t bheap;                  \
00059                 bheaph_t *elems[sz];            \
00060         } name
00061 
00062 /* Check the binary heap invariant. */
00063 static inline int bheap_ordered(bheap_t *heap)
00064 {
00065         unsigned i;
00066         for (i = 2; i < heap->last; i++)
00067                 if (bheaph_lt(heap->elems[i], heap->elems[i / 2]))
00068                         return 0;
00069         return 1;
00070 }
00071 
00072 #define BHEAP_CHECK(heap)                                               \
00073         XENO_BUGON(QUEUES, ((heap)->sz == 0) || !bheap_ordered(heap))
00074 
00075 #define bheap_gethead(heap)                             \
00076         ({                                              \
00077                 bheap_t *_bheap = &(heap)->bheap;       \
00078                 BHEAP_CHECK(_bheap);                    \
00079                 __internal_bheap_gethead(_bheap);       \
00080         })
00081 
00082 static inline bheaph_t *__internal_bheap_gethead(bheap_t *heap)
00083 {
00084         if (heap->last == 1)
00085                 return NULL;
00086 
00087         return heap->elems[1];
00088 }
00089 
00090 #define bheap_next(heap, holder)                        \
00091         ({                                              \
00092                 bheap_t *_bheap = &(heap)->bheap;       \
00093                 BHEAP_CHECK(_bheap);                    \
00094                 __internal_bheap_next(_bheap, holder);  \
00095         })
00096 
00097 static inline bheaph_t *__internal_bheap_next(bheap_t *heap, bheaph_t *holder)
00098 {
00099         unsigned pos;
00100 
00101         if (unlikely(bheaph_pos(holder) >= heap->last
00102                      || heap->elems[bheaph_pos(holder)] != holder))
00103                 return (bheaph_t *) ERR_PTR(-EINVAL);
00104 
00105         pos = bheaph_pos(holder) + 1;
00106 
00107         return likely(pos < heap->last) ? heap->elems[pos] : NULL;
00108 }
00109 
00110 static inline bheaph_t *bheaph_parent(bheap_t *heap, bheaph_t *holder)
00111 {
00112         const unsigned pos = holder->pos;
00113 
00114         return likely(pos > 1) ? heap->elems[pos / 2] : NULL;
00115 }
00116 
00117 static inline bheaph_t *bheaph_child(bheap_t *heap, bheaph_t *holder, int side)
00118 {
00119         const unsigned pos = 2 * holder->pos + side;
00120 
00121         return likely(pos < heap->last) ? heap->elems[pos] : NULL;
00122 }
00123 
00124 #define bheap_init(heap, sz) __internal_bheap_init(&(heap)->bheap, sz)
00125 
00126 static inline void __internal_bheap_init(bheap_t *heap, unsigned sz)
00127 {
00128         heap->sz = sz;
00129         heap->last = 1;
00130 }
00131 
00132 #define bheap_destroy(heap) __internal_bheap_destroy(&(heap)->bheap)
00133 
00134 static inline void __internal_bheap_destroy(bheap_t *heap)
00135 {
00136         heap->sz = 0;
00137         heap->last = 1;
00138 }
00139 
00140 static inline void bheap_swap(bheap_t *heap, bheaph_t *h1, bheaph_t *h2)
00141 {
00142         const unsigned pos2 = bheaph_pos(h2);
00143 
00144         heap->elems[bheaph_pos(h1)] = h2;
00145         bheaph_pos(h2) = bheaph_pos(h1);
00146         heap->elems[pos2] = h1;
00147         bheaph_pos(h1) = pos2;
00148 }
00149 
00150 static inline void bheap_up(bheap_t *heap, bheaph_t *holder)
00151 {
00152         bheaph_t *parent;
00153 
00154         while ((parent = bheaph_parent(heap, holder)) && bheaph_lt(holder, parent))
00155                 bheap_swap(heap, holder, parent);
00156 }
00157 
00158 static inline void bheap_down(bheap_t *heap, bheaph_t *holder)
00159 {
00160         bheaph_t *left, *right, *minchild;
00161 
00162         for (;;) {
00163                 left = bheaph_child(heap, holder, 0);
00164                 right = bheaph_child(heap, holder, 1);
00165 
00166                 if (left && right)
00167                         minchild = bheaph_lt(left, right) ? left : right;
00168                 else
00169                         minchild = left ?: right;
00170 
00171                 if (!minchild || bheaph_lt(holder, minchild))
00172                         break;
00173 
00174                 bheap_swap(heap, minchild, holder);
00175         }
00176 }
00177 
00178 #define bheap_insert(heap, holder)                              \
00179         ({                                                      \
00180                 bheap_t *_bheap = &(heap)->bheap;               \
00181                 BHEAP_CHECK(_bheap);                            \
00182                 __internal_bheap_insert(_bheap, holder);        \
00183         })
00184 
00185 static inline int __internal_bheap_insert(bheap_t *heap, bheaph_t *holder)
00186 {
00187         if (heap->last == heap->sz + 1)
00188                 return EBUSY;
00189 
00190         heap->elems[heap->last] = holder;
00191         bheaph_pos(holder) = heap->last;
00192         ++heap->last;
00193         bheap_up(heap, holder);
00194         return 0;
00195 }
00196 
00197 #define bheap_delete(heap, holder)                              \
00198         ({                                                      \
00199                 bheap_t *_bheap = &(heap)->bheap;               \
00200                 BHEAP_CHECK(_bheap);                            \
00201                 __internal_bheap_delete(_bheap, holder);        \
00202         })
00203 
00204 static inline int __internal_bheap_delete(bheap_t *heap, bheaph_t *holder)
00205 {
00206         bheaph_t *lasth;
00207 
00208         if (unlikely(bheaph_pos(holder) >= heap->last
00209                      || heap->elems[bheaph_pos(holder)] != holder))
00210                 return EINVAL;
00211 
00212         --heap->last;
00213         if (heap->last != bheaph_pos(holder)) {
00214                 bheaph_t *parent;
00215                 lasth = heap->elems[heap->last];
00216                 heap->elems[bheaph_pos(holder)] = lasth;
00217                 bheaph_pos(lasth) = bheaph_pos(holder);
00218                 if ((parent = bheaph_parent(heap, lasth)) && bheaph_lt(lasth, parent))
00219                         bheap_up(heap, lasth);
00220                 else
00221                         bheap_down(heap, lasth);
00222         }
00223 
00224         return 0;
00225 }
00226 
00227 #define bheap_get(heap)                                 \
00228         ({                                              \
00229                 bheap_t *_bheap = &(heap)->bheap;       \
00230                 BHEAP_CHECK(_bheap);                    \
00231                 __internal_bheap_get(_bheap, holder);   \
00232         })
00233 
00234 static inline bheaph_t *__internal_bheap_get(bheap_t *heap)
00235 {
00236         bheaph_t *holder = __internal_bheap_gethead(heap);
00237 
00238         if (!holder)
00239                 return NULL;
00240 
00241         __internal_bheap_delete(heap, holder);
00242 
00243         return holder;
00244 }
00245 
00246 #endif /* _XENO_NUCLEUS_BHEAP_H */

Generated on Mon Mar 24 18:02:40 2008 for Xenomai API by  doxygen 1.5.3