Xenomai  3.0-rc3
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_BUGON(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_next(heap, holder) \
84  ({ \
85  bheap_t *_bheap = &(heap)->bheap; \
86  BHEAP_CHECK(_bheap); \
87  __internal_bheap_next(_bheap, holder); \
88  })
89 
90 static inline bheaph_t *__internal_bheap_next(bheap_t *heap, bheaph_t *holder)
91 {
92  unsigned pos;
93 
94  if (unlikely(bheaph_pos(holder) >= heap->last
95  || heap->elems[bheaph_pos(holder)] != holder))
96  return (bheaph_t *) ERR_PTR(-EINVAL);
97 
98  pos = bheaph_pos(holder) + 1;
99 
100  return likely(pos < heap->last) ? heap->elems[pos] : NULL;
101 }
102 
103 static inline bheaph_t *bheaph_parent(bheap_t *heap, bheaph_t *holder)
104 {
105  const unsigned pos = holder->pos;
106 
107  return likely(pos > 1) ? heap->elems[pos / 2] : NULL;
108 }
109 
110 static inline bheaph_t *bheaph_child(bheap_t *heap, bheaph_t *holder, int side)
111 {
112  const unsigned pos = 2 * holder->pos + side;
113 
114  return likely(pos < heap->last) ? heap->elems[pos] : NULL;
115 }
116 
117 #define bheap_init(heap, sz) __internal_bheap_init(&(heap)->bheap, sz)
118 
119 static inline void __internal_bheap_init(bheap_t *heap, unsigned sz)
120 {
121  heap->sz = sz;
122  heap->last = 1;
123 }
124 
125 #define bheap_destroy(heap) __internal_bheap_destroy(&(heap)->bheap)
126 
127 static inline void __internal_bheap_destroy(bheap_t *heap)
128 {
129  heap->sz = 0;
130  heap->last = 1;
131 }
132 
133 static inline void bheap_swap(bheap_t *heap, bheaph_t *h1, bheaph_t *h2)
134 {
135  const unsigned pos2 = bheaph_pos(h2);
136 
137  heap->elems[bheaph_pos(h1)] = h2;
138  bheaph_pos(h2) = bheaph_pos(h1);
139  heap->elems[pos2] = h1;
140  bheaph_pos(h1) = pos2;
141 }
142 
143 static inline void bheap_up(bheap_t *heap, bheaph_t *holder)
144 {
145  bheaph_t *parent;
146 
147  while ((parent = bheaph_parent(heap, holder)) && bheaph_lt(holder, parent))
148  bheap_swap(heap, holder, parent);
149 }
150 
151 static inline void bheap_down(bheap_t *heap, bheaph_t *holder)
152 {
153  bheaph_t *left, *right, *minchild;
154 
155  for (;;) {
156  left = bheaph_child(heap, holder, 0);
157  right = bheaph_child(heap, holder, 1);
158 
159  if (left && right)
160  minchild = bheaph_lt(left, right) ? left : right;
161  else
162  minchild = left ?: right;
163 
164  if (!minchild || bheaph_lt(holder, minchild))
165  break;
166 
167  bheap_swap(heap, minchild, holder);
168  }
169 }
170 
171 #define bheap_insert(heap, holder) \
172  ({ \
173  bheap_t *_bheap = &(heap)->bheap; \
174  BHEAP_CHECK(_bheap); \
175  __internal_bheap_insert(_bheap, holder); \
176  })
177 
178 static inline int __internal_bheap_insert(bheap_t *heap, bheaph_t *holder)
179 {
180  if (heap->last == heap->sz + 1)
181  return EBUSY;
182 
183  heap->elems[heap->last] = holder;
184  bheaph_pos(holder) = heap->last;
185  ++heap->last;
186  bheap_up(heap, holder);
187  return 0;
188 }
189 
190 #define bheap_delete(heap, holder) \
191  ({ \
192  bheap_t *_bheap = &(heap)->bheap; \
193  BHEAP_CHECK(_bheap); \
194  __internal_bheap_delete(_bheap, holder); \
195  })
196 
197 static inline int __internal_bheap_delete(bheap_t *heap, bheaph_t *holder)
198 {
199  bheaph_t *lasth;
200 
201  if (unlikely(bheaph_pos(holder) >= heap->last
202  || heap->elems[bheaph_pos(holder)] != holder))
203  return EINVAL;
204 
205  --heap->last;
206  if (heap->last != bheaph_pos(holder)) {
207  bheaph_t *parent;
208  lasth = heap->elems[heap->last];
209  heap->elems[bheaph_pos(holder)] = lasth;
210  bheaph_pos(lasth) = bheaph_pos(holder);
211  if ((parent = bheaph_parent(heap, lasth)) && bheaph_lt(lasth, parent))
212  bheap_up(heap, lasth);
213  else
214  bheap_down(heap, lasth);
215  }
216 
217  return 0;
218 }
219 
220 #define bheap_get(heap) \
221  ({ \
222  bheap_t *_bheap = &(heap)->bheap; \
223  BHEAP_CHECK(_bheap); \
224  __internal_bheap_get(_bheap, holder); \
225  })
226 
227 static inline bheaph_t *__internal_bheap_get(bheap_t *heap)
228 {
229  bheaph_t *holder = __internal_bheap_gethead(heap);
230 
231  if (!holder)
232  return NULL;
233 
234  __internal_bheap_delete(heap, holder);
235 
236  return holder;
237 }
238 
239 #define bheap_empty(heap) \
240  ({ \
241  bheap_t *_bheap = &(heap)->bheap; \
242  BHEAP_CHECK(_bheap); \
243  _bheap->last == 1; \
244  })
245 
246 #endif /* _COBALT_KERNEL_BHEAP_H */