Merge the rest of the FullyBooted patch
[asterisk/asterisk.git] / main / heap.c
1 /*
2  * Asterisk -- An open source telephony toolkit.
3  *
4  * Copyright (C) 2009, Digium, Inc.
5  *
6  * Russell Bryant <russell@digium.com>
7  *
8  * See http://www.asterisk.org for more information about
9  * the Asterisk project. Please do not directly contact
10  * any of the maintainers of this project for assistance;
11  * the project provides a web site, mailing lists and IRC
12  * channels for your use.
13  *
14  * This program is free software, distributed under the terms of
15  * the GNU General Public License Version 2. See the LICENSE file
16  * at the top of the source tree.
17  */
18
19 /*! \file
20  *
21  * \brief Max Heap data structure
22  *
23  * \author Russell Bryant <russell@digium.com>
24  */
25
26 #include "asterisk.h"
27
28 ASTERISK_FILE_VERSION(__FILE__, "$Revision$")
29
30 #include "asterisk/heap.h"
31 #include "asterisk/utils.h"
32 #include "asterisk/cli.h"
33
34 struct ast_heap {
35         ast_rwlock_t lock;
36         ast_heap_cmp_fn cmp_fn;
37         ssize_t index_offset;
38         size_t cur_len;
39         size_t avail_len;
40         void **heap;
41 };
42
43 static inline int left_node(int i)
44 {
45         return 2 * i;
46 }
47
48 static inline int right_node(int i)
49 {
50         return 2 * i + 1;
51 }
52
53 static inline int parent_node(int i)
54 {
55         return i / 2;
56 }
57
58 static inline void *heap_get(struct ast_heap *h, int i)
59 {
60         return h->heap[i - 1];
61 }
62
63 static inline ssize_t get_index(struct ast_heap *h, void *elm)
64 {
65         ssize_t *index;
66
67         if (h->index_offset < 0) {
68                 return -1;
69         }
70
71         index = elm + h->index_offset;
72
73         return *index;
74 }
75
76 static inline void heap_set(struct ast_heap *h, int i, void *elm)
77 {
78         h->heap[i - 1] = elm;
79
80         if (h->index_offset >= 0) {
81                 ssize_t *index = elm + h->index_offset;
82                 *index = i;
83         }
84 }
85
86 int ast_heap_verify(struct ast_heap *h)
87 {
88         unsigned int i;
89
90         for (i = 1; i <= (h->cur_len / 2); i++) {
91                 int l = left_node(i);
92                 int r = right_node(i);
93
94                 if (l <= h->cur_len) {
95                         if (h->cmp_fn(heap_get(h, i), heap_get(h, l)) < 0) {
96                                 return -1;
97                         }
98                 }
99
100                 if (r <= h->cur_len) {
101                         if (h->cmp_fn(heap_get(h, i), heap_get(h, r)) < 0) {
102                                 return -1;
103                         }
104                 }
105         }
106
107         return 0;
108 }
109
110 #ifdef MALLOC_DEBUG
111 struct ast_heap *_ast_heap_create(unsigned int init_height, ast_heap_cmp_fn cmp_fn,
112                 ssize_t index_offset, const char *file, int lineno, const char *func)
113 #else
114 struct ast_heap *ast_heap_create(unsigned int init_height, ast_heap_cmp_fn cmp_fn,
115                 ssize_t index_offset)
116 #endif
117 {
118         struct ast_heap *h;
119
120         if (!cmp_fn) {
121                 ast_log(LOG_ERROR, "A comparison function must be provided\n");
122                 return NULL;
123         }
124
125         if (!init_height) {
126                 init_height = 8;
127         }
128
129         if (!(h =
130 #ifdef MALLOC_DEBUG
131                         __ast_calloc(1, sizeof(*h), file, lineno, func)
132 #else
133                         ast_calloc(1, sizeof(*h))
134 #endif
135                 )) {
136                 return NULL;
137         }
138
139         h->cmp_fn = cmp_fn;
140         h->index_offset = index_offset;
141         h->avail_len = (1 << init_height) - 1;
142
143         if (!(h->heap =
144 #ifdef MALLOC_DEBUG
145                         __ast_calloc(1, h->avail_len * sizeof(void *), file, lineno, func)
146 #else
147                         ast_calloc(1, h->avail_len * sizeof(void *))
148 #endif
149                 )) {
150                 ast_free(h);
151                 return NULL;
152         }
153
154         ast_rwlock_init(&h->lock);
155
156         return h;
157 }
158
159 struct ast_heap *ast_heap_destroy(struct ast_heap *h)
160 {
161         ast_free(h->heap);
162         h->heap = NULL;
163
164         ast_rwlock_destroy(&h->lock);
165
166         ast_free(h);
167
168         return NULL;
169 }
170
171 /*!
172  * \brief Add a row of additional storage for the heap.
173  */
174 static int grow_heap(struct ast_heap *h
175 #ifdef MALLOC_DEBUG
176 , const char *file, int lineno, const char *func
177 #endif
178 )
179 {
180         h->avail_len = h->avail_len * 2 + 1;
181
182         if (!(h->heap =
183 #ifdef MALLOC_DEBUG
184                         __ast_realloc(h->heap, h->avail_len * sizeof(void *), file, lineno, func)
185 #else
186                         ast_realloc(h->heap, h->avail_len * sizeof(void *))
187 #endif
188                 )) {
189                 h->cur_len = h->avail_len = 0;
190                 return -1;
191         }
192
193         return 0;
194 }
195
196 static inline void heap_swap(struct ast_heap *h, int i, int j)
197 {
198         void *tmp;
199
200         tmp = heap_get(h, i);
201         heap_set(h, i, heap_get(h, j));
202         heap_set(h, j, tmp);
203 }
204
205 static inline void max_heapify(struct ast_heap *h, int i)
206 {
207         for (;;) {
208                 int l = left_node(i);
209                 int r = right_node(i);
210                 int max;
211
212                 if (l <= h->cur_len && h->cmp_fn(heap_get(h, l), heap_get(h, i)) > 0) {
213                         max = l;
214                 } else {
215                         max = i;
216                 }
217
218                 if (r <= h->cur_len && h->cmp_fn(heap_get(h, r), heap_get(h, max)) > 0) {
219                         max = r;
220                 }
221
222                 if (max == i) {
223                         break;
224                 }
225
226                 heap_swap(h, i, max);
227
228                 i = max;
229         }
230 }
231
232 static int bubble_up(struct ast_heap *h, int i)
233 {
234         while (i > 1 && h->cmp_fn(heap_get(h, parent_node(i)), heap_get(h, i)) < 0) {
235                 heap_swap(h, i, parent_node(i));
236                 i = parent_node(i);
237         }
238
239         return i;
240 }
241
242 #ifdef MALLOC_DEBUG
243 int _ast_heap_push(struct ast_heap *h, void *elm, const char *file, int lineno, const char *func)
244 #else
245 int ast_heap_push(struct ast_heap *h, void *elm)
246 #endif
247 {
248         if (h->cur_len == h->avail_len && grow_heap(h
249 #ifdef MALLOC_DEBUG
250                 , file, lineno, func
251 #endif
252                 )) {
253                 return -1;
254         }
255
256         heap_set(h, ++(h->cur_len), elm);
257
258         bubble_up(h, h->cur_len);
259
260         return 0;
261 }
262
263 static void *_ast_heap_remove(struct ast_heap *h, unsigned int index)
264 {
265         void *ret;
266
267         if (!index || index > h->cur_len) {
268                 return NULL;
269         }
270
271         ret = heap_get(h, index);
272         heap_set(h, index, heap_get(h, (h->cur_len)--));
273         index = bubble_up(h, index);
274         max_heapify(h, index);
275
276         return ret;
277 }
278
279 void *ast_heap_remove(struct ast_heap *h, void *elm)
280 {
281         ssize_t i = get_index(h, elm);
282
283         if (i == -1) {
284                 return NULL;
285         }
286
287         return _ast_heap_remove(h, i);
288 }
289
290 void *ast_heap_pop(struct ast_heap *h)
291 {
292         return _ast_heap_remove(h, 1);
293 }
294
295 void *ast_heap_peek(struct ast_heap *h, unsigned int index)
296 {
297         if (!h->cur_len || !index || index > h->cur_len) {
298                 return NULL;
299         }
300
301         return heap_get(h, index);
302 }
303
304 size_t ast_heap_size(struct ast_heap *h)
305 {
306         return h->cur_len;
307 }
308
309 int __ast_heap_wrlock(struct ast_heap *h, const char *file, const char *func, int line)
310 {
311         return __ast_rwlock_wrlock(&h->lock, "&h->lock", file, line, func);
312 }
313
314 int __ast_heap_rdlock(struct ast_heap *h, const char *file, const char *func, int line)
315 {
316         return __ast_rwlock_rdlock(&h->lock, "&h->lock", file, line, func);
317 }
318
319 int __ast_heap_unlock(struct ast_heap *h, const char *file, const char *func, int line)
320 {
321         return __ast_rwlock_unlock(&h->lock, "&h->lock", file, line, func);
322 }