Xenomai API  2.6.4
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 
20 #ifndef _XENO_NUCLEUS_BHEAP_H
21 #define _XENO_NUCLEUS_BHEAP_H
22 
23 #include <nucleus/compiler.h>
24 
25 /* debug support */
26 #include <nucleus/assert.h>
27 
28 /* Priority queue implementation, using a binary heap. */
29 
30 typedef unsigned long long bheap_key_t;
31 
32 typedef struct bheaph {
33  bheap_key_t key;
34  unsigned prio;
35  unsigned pos;
36 } bheaph_t;
37 
38 #define bheaph_init(holder) do { } while (0)
39 #define bheaph_key(holder) ((holder)->key)
40 #define bheaph_prio(holder) ((holder)->prio)
41 #define bheaph_pos(holder) ((holder)->pos)
42 #define bheaph_lt(h1, h2) ((long long) ((h1)->key - (h2)->key) < 0 || \
43  ((h1)->key == (h2)->key && \
44  (h1)->prio > (h2)->prio))
45 
46 typedef struct bheap {
47  unsigned sz;
48  unsigned last;
49  bheaph_t *elems[];
50 } bheap_t;
51 
52 #define DECLARE_BHEAP_CONTAINER(name, sz) \
53  struct { \
54  bheap_t bheap; \
55  bheaph_t *elems[sz + 1]; \
56  } name
57 
58 /* Check the binary heap invariant. */
59 static inline int bheap_ordered(bheap_t *heap)
60 {
61  unsigned i;
62  for (i = 2; i < heap->last; i++)
63  if (bheaph_lt(heap->elems[i], heap->elems[i / 2]))
64  return 0;
65  return 1;
66 }
67 
68 #define BHEAP_CHECK(heap) \
69  XENO_BUGON(QUEUES, ((heap)->sz == 0) || !bheap_ordered(heap))
70 
71 #define bheap_gethead(heap) \
72  ({ \
73  bheap_t *_bheap = &(heap)->bheap; \
74  BHEAP_CHECK(_bheap); \
75  __internal_bheap_gethead(_bheap); \
76  })
77 
78 static inline bheaph_t *__internal_bheap_gethead(bheap_t *heap)
79 {
80  if (heap->last == 1)
81  return NULL;
82 
83  return heap->elems[1];
84 }
85 
86 #define bheap_next(heap, holder) \
87  ({ \
88  bheap_t *_bheap = &(heap)->bheap; \
89  BHEAP_CHECK(_bheap); \
90  __internal_bheap_next(_bheap, holder); \
91  })
92 
93 static inline bheaph_t *__internal_bheap_next(bheap_t *heap, bheaph_t *holder)
94 {
95  unsigned pos;
96 
97  if (unlikely(bheaph_pos(holder) >= heap->last
98  || heap->elems[bheaph_pos(holder)] != holder))
99  return (bheaph_t *) ERR_PTR(-EINVAL);
100 
101  pos = bheaph_pos(holder) + 1;
102 
103  return likely(pos < heap->last) ? heap->elems[pos] : NULL;
104 }
105 
106 static inline bheaph_t *bheaph_parent(bheap_t *heap, bheaph_t *holder)
107 {
108  const unsigned pos = holder->pos;
109 
110  return likely(pos > 1) ? heap->elems[pos / 2] : NULL;
111 }
112 
113 static inline bheaph_t *bheaph_child(bheap_t *heap, bheaph_t *holder, int side)
114 {
115  const unsigned pos = 2 * holder->pos + side;
116 
117  return likely(pos < heap->last) ? heap->elems[pos] : NULL;
118 }
119 
120 #define bheap_init(heap, sz) __internal_bheap_init(&(heap)->bheap, sz)
121 
122 static inline void __internal_bheap_init(bheap_t *heap, unsigned sz)
123 {
124  heap->sz = sz;
125  heap->last = 1;
126 }
127 
128 #define bheap_destroy(heap) __internal_bheap_destroy(&(heap)->bheap)
129 
130 static inline void __internal_bheap_destroy(bheap_t *heap)
131 {
132  heap->sz = 0;
133  heap->last = 1;
134 }
135 
136 static inline void bheap_swap(bheap_t *heap, bheaph_t *h1, bheaph_t *h2)
137 {
138  const unsigned pos2 = bheaph_pos(h2);
139 
140  heap->elems[bheaph_pos(h1)] = h2;
141  bheaph_pos(h2) = bheaph_pos(h1);
142  heap->elems[pos2] = h1;
143  bheaph_pos(h1) = pos2;
144 }
145 
146 static inline void bheap_up(bheap_t *heap, bheaph_t *holder)
147 {
148  bheaph_t *parent;
149 
150  while ((parent = bheaph_parent(heap, holder)) && bheaph_lt(holder, parent))
151  bheap_swap(heap, holder, parent);
152 }
153 
154 static inline void bheap_down(bheap_t *heap, bheaph_t *holder)
155 {
156  bheaph_t *left, *right, *minchild;
157 
158  for (;;) {
159  left = bheaph_child(heap, holder, 0);
160  right = bheaph_child(heap, holder, 1);
161 
162  if (left && right)
163  minchild = bheaph_lt(left, right) ? left : right;
164  else
165  minchild = left ?: right;
166 
167  if (!minchild || bheaph_lt(holder, minchild))
168  break;
169 
170  bheap_swap(heap, minchild, holder);
171  }
172 }
173 
174 #define bheap_insert(heap, holder) \
175  ({ \
176  bheap_t *_bheap = &(heap)->bheap; \
177  BHEAP_CHECK(_bheap); \
178  __internal_bheap_insert(_bheap, holder); \
179  })
180 
181 static inline int __internal_bheap_insert(bheap_t *heap, bheaph_t *holder)
182 {
183  if (heap->last == heap->sz + 1)
184  return EBUSY;
185 
186  heap->elems[heap->last] = holder;
187  bheaph_pos(holder) = heap->last;
188  ++heap->last;
189  bheap_up(heap, holder);
190  return 0;
191 }
192 
193 #define bheap_delete(heap, holder) \
194  ({ \
195  bheap_t *_bheap = &(heap)->bheap; \
196  BHEAP_CHECK(_bheap); \
197  __internal_bheap_delete(_bheap, holder); \
198  })
199 
200 static inline int __internal_bheap_delete(bheap_t *heap, bheaph_t *holder)
201 {
202  bheaph_t *lasth;
203 
204  if (unlikely(bheaph_pos(holder) >= heap->last
205  || heap->elems[bheaph_pos(holder)] != holder))
206  return EINVAL;
207 
208  --heap->last;
209  if (heap->last != bheaph_pos(holder)) {
210  bheaph_t *parent;
211  lasth = heap->elems[heap->last];
212  heap->elems[bheaph_pos(holder)] = lasth;
213  bheaph_pos(lasth) = bheaph_pos(holder);
214  if ((parent = bheaph_parent(heap, lasth)) && bheaph_lt(lasth, parent))
215  bheap_up(heap, lasth);
216  else
217  bheap_down(heap, lasth);
218  }
219 
220  return 0;
221 }
222 
223 #define bheap_get(heap) \
224  ({ \
225  bheap_t *_bheap = &(heap)->bheap; \
226  BHEAP_CHECK(_bheap); \
227  __internal_bheap_get(_bheap, holder); \
228  })
229 
230 static inline bheaph_t *__internal_bheap_get(bheap_t *heap)
231 {
232  bheaph_t *holder = __internal_bheap_gethead(heap);
233 
234  if (!holder)
235  return NULL;
236 
237  __internal_bheap_delete(heap, holder);
238 
239  return holder;
240 }
241 
242 #endif /* _XENO_NUCLEUS_BHEAP_H */