Xenomai  3.0-rc7
bheap.h
1 /*
2  * Copyright (C) 2006 Gilles Chanteperdrix <gilles.chanteperdrix@xenomai.org>
3  *
4  * Xenomai is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License as published
6  * by the Free Software Foundation; either version 2 of the License,
7  * or (at your option) any later version.
8  *
9  * Xenomai is distributed in the hope that it will be useful, but
10  * WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12  * General Public License for more details.
13  *
14  * You should have received a copy of the GNU General Public License
15  * along with Xenomai; if not, write to the Free Software
16  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA
17  * 02111-1307, USA.
18  */
19 #ifndef _COBALT_KERNEL_BHEAP_H
20 #define _COBALT_KERNEL_BHEAP_H
21 
22 /* debug support */
23 #include <cobalt/kernel/assert.h>
24 
25 /* Priority queue implementation, using a binary heap. */
26 
27 typedef unsigned long long bheap_key_t;
28 
29 typedef struct bheaph {
30  bheap_key_t key;
31  unsigned prio;
32  unsigned pos;
33 } bheaph_t;
34 
35 #define bheaph_init(holder) do { } while (0)
36 #define bheaph_key(holder) ((holder)->key)
37 #define bheaph_prio(holder) ((holder)->prio)
38 #define bheaph_pos(holder) ((holder)->pos)
39 #define bheaph_lt(h1, h2) ((long long) ((h1)->key - (h2)->key) < 0 || \
40  ((h1)->key == (h2)->key && \
41  (h1)->prio > (h2)->prio))
42 
43 typedef struct bheap {
44  unsigned sz;
45  unsigned last;
46  bheaph_t *elems[]; /* only padding, indexing starts at 1 */
47 } bheap_t;
48 
49 #define DECLARE_BHEAP_CONTAINER(name, sz) \
50  struct { \
51  bheap_t bheap; \
52  bheaph_t *elems[sz + 1]; \
53  } name
54 
55 /* Check the binary heap invariant. */
56 static inline int bheap_ordered(bheap_t *heap)
57 {
58  unsigned i;
59  for (i = 2; i < heap->last; i++)
60  if (bheaph_lt(heap->elems[i], heap->elems[i / 2]))
61  return 0;
62  return 1;
63 }
64 
65 #define BHEAP_CHECK(heap) \
66  XENO_BUG_ON(COBALT, ((heap)->sz == 0) || !bheap_ordered(heap))
67 
68 #define bheap_gethead(heap) \
69  ({ \
70  bheap_t *_bheap = &(heap)->bheap; \
71  BHEAP_CHECK(_bheap); \
72  __internal_bheap_gethead(_bheap); \
73  })
74 
75 static inline bheaph_t *__internal_bheap_gethead(bheap_t *heap)
76 {
77  if (heap->last == 1)
78  return NULL;
79 
80  return heap->elems[1];
81 }
82 
83 #define bheap_second(heap) \
84  ({ \
85  bheap_t *_bheap = &(heap)->bheap; \
86  BHEAP_CHECK(_bheap); \
87  __internal_bheap_second(_bheap); \
88  })
89 
90 #define bheap_next(heap, holder) \
91  ({ \
92  bheap_t *_bheap = &(heap)->bheap; \
93  BHEAP_CHECK(_bheap); \
94  __internal_bheap_next(_bheap, holder); \
95  })
96 
97 static inline bheaph_t *__internal_bheap_next(bheap_t *heap, bheaph_t *holder)
98 {
99  unsigned pos;
100 
101  if (unlikely(bheaph_pos(holder) >= heap->last
102  || heap->elems[bheaph_pos(holder)] != holder))
103  return (bheaph_t *) ERR_PTR(-EINVAL);
104 
105  pos = bheaph_pos(holder) + 1;
106 
107  return likely(pos < heap->last) ? heap->elems[pos] : NULL;
108 }
109 
110 static inline bheaph_t *bheaph_parent(bheap_t *heap, bheaph_t *holder)
111 {
112  const unsigned pos = holder->pos;
113 
114  return likely(pos > 1) ? heap->elems[pos / 2] : NULL;
115 }
116 
117 static inline bheaph_t *bheaph_child(bheap_t *heap, bheaph_t *holder, int side)
118 {
119  const unsigned pos = 2 * holder->pos + side;
120 
121  return likely(pos < heap->last) ? heap->elems[pos] : NULL;
122 }
123 
124 static inline bheaph_t *__internal_bheap_second(bheap_t *heap)
125 {
126  bheaph_t *left, *right, *first = __internal_bheap_gethead(heap);
127 
128  left = bheaph_child(heap, first, 0);
129  right = bheaph_child(heap, first, 1);
130 
131  if (!left || !right)
132  return left ?: right;
133 
134  return bheaph_lt(left, right) ? left : right;
135 }
136 
137 #define bheap_init(heap, sz) __internal_bheap_init(&(heap)->bheap, sz)
138 
139 static inline void __internal_bheap_init(bheap_t *heap, unsigned sz)
140 {
141  heap->sz = sz;
142  heap->last = 1;
143 }
144 
145 #define bheap_destroy(heap) __internal_bheap_destroy(&(heap)->bheap)
146 
147 static inline void __internal_bheap_destroy(bheap_t *heap)
148 {
149  heap->sz = 0;
150  heap->last = 1;
151 }
152 
153 static inline void bheap_swap(bheap_t *heap, bheaph_t *h1, bheaph_t *h2)
154 {
155  const unsigned pos2 = bheaph_pos(h2);
156 
157  heap->elems[bheaph_pos(h1)] = h2;
158  bheaph_pos(h2) = bheaph_pos(h1);
159  heap->elems[pos2] = h1;
160  bheaph_pos(h1) = pos2;
161 }
162 
163 static inline void bheap_up(bheap_t *heap, bheaph_t *holder)
164 {
165  bheaph_t *parent;
166 
167  while ((parent = bheaph_parent(heap, holder)) && bheaph_lt(holder, parent))
168  bheap_swap(heap, holder, parent);
169 }
170 
171 static inline void bheap_down(bheap_t *heap, bheaph_t *holder)
172 {
173  bheaph_t *left, *right, *minchild;
174 
175  for (;;) {
176  left = bheaph_child(heap, holder, 0);
177  right = bheaph_child(heap, holder, 1);
178 
179  if (left && right)
180  minchild = bheaph_lt(left, right) ? left : right;
181  else
182  minchild = left ?: right;
183 
184  if (!minchild || bheaph_lt(holder, minchild))
185  break;
186 
187  bheap_swap(heap, minchild, holder);
188  }
189 }
190 
191 #define bheap_insert(heap, holder) \
192  ({ \
193  bheap_t *_bheap = &(heap)->bheap; \
194  BHEAP_CHECK(_bheap); \
195  __internal_bheap_insert(_bheap, holder); \
196  })
197 
198 static inline int __internal_bheap_insert(bheap_t *heap, bheaph_t *holder)
199 {
200  if (heap->last == heap->sz + 1)
201  return EBUSY;
202 
203  heap->elems[heap->last] = holder;
204  bheaph_pos(holder) = heap->last;
205  ++heap->last;
206  bheap_up(heap, holder);
207  return 0;
208 }
209 
210 #define bheap_delete(heap, holder) \
211  ({ \
212  bheap_t *_bheap = &(heap)->bheap; \
213  BHEAP_CHECK(_bheap); \
214  __internal_bheap_delete(_bheap, holder); \
215  })
216 
217 static inline int __internal_bheap_delete(bheap_t *heap, bheaph_t *holder)
218 {
219  bheaph_t *lasth;
220 
221  if (unlikely(bheaph_pos(holder) >= heap->last
222  || heap->elems[bheaph_pos(holder)] != holder))
223  return EINVAL;
224 
225  --heap->last;
226  if (heap->last != bheaph_pos(holder)) {
227  bheaph_t *parent;
228  lasth = heap->elems[heap->last];
229  heap->elems[bheaph_pos(holder)] = lasth;
230  bheaph_pos(lasth) = bheaph_pos(holder);
231  if ((parent = bheaph_parent(heap, lasth)) && bheaph_lt(lasth, parent))
232  bheap_up(heap, lasth);
233  else
234  bheap_down(heap, lasth);
235  }
236 
237  return 0;
238 }
239 
240 #define bheap_get(heap) \
241  ({ \
242  bheap_t *_bheap = &(heap)->bheap; \
243  BHEAP_CHECK(_bheap); \
244  __internal_bheap_get(_bheap, holder); \
245  })
246 
247 static inline bheaph_t *__internal_bheap_get(bheap_t *heap)
248 {
249  bheaph_t *holder = __internal_bheap_gethead(heap);
250 
251  if (!holder)
252  return NULL;
253 
254  __internal_bheap_delete(heap, holder);
255 
256  return holder;
257 }
258 
259 #define bheap_empty(heap) \
260  ({ \
261  bheap_t *_bheap = &(heap)->bheap; \
262  BHEAP_CHECK(_bheap); \
263  _bheap->last == 1; \
264  })
265 
266 #endif /* _COBALT_KERNEL_BHEAP_H */