1 /***
2   This file is part of avahi.
3 
4   avahi is free software; you can redistribute it and/or modify it
5   under the terms of the GNU Lesser General Public License as
6   published by the Free Software Foundation; either version 2.1 of the
7   License, or (at your option) any later version.
8 
9   avahi is distributed in the hope that it will be useful, but WITHOUT
10   ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
11   or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General
12   Public License for more details.
13 
14   You should have received a copy of the GNU Lesser General Public
15   License along with avahi; if not, write to the Free Software
16   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
17   USA.
18 ***/
19 
20 #ifdef HAVE_CONFIG_H
21 #include <config.h>
22 #endif
23 
24 #include <assert.h>
25 #include <stdlib.h>
26 
27 #include <avahi-common/malloc.h>
28 
29 #include "prioq.h"
30 
avahi_prio_queue_new(AvahiPQCompareFunc compare)31 AvahiPrioQueue* avahi_prio_queue_new(AvahiPQCompareFunc compare) {
32     AvahiPrioQueue *q;
33     assert(compare);
34 
35     if (!(q = avahi_new(AvahiPrioQueue, 1)))
36         return NULL; /* OOM */
37 
38     q->root = q->last = NULL;
39     q->n_nodes = 0;
40     q->compare = compare;
41 
42     return q;
43 }
44 
avahi_prio_queue_free(AvahiPrioQueue * q)45 void avahi_prio_queue_free(AvahiPrioQueue *q) {
46     assert(q);
47 
48     while (q->last)
49         avahi_prio_queue_remove(q, q->last);
50 
51     assert(!q->n_nodes);
52     avahi_free(q);
53 }
54 
get_node_at_xy(AvahiPrioQueue * q,unsigned x,unsigned y)55 static AvahiPrioQueueNode* get_node_at_xy(AvahiPrioQueue *q, unsigned x, unsigned y) {
56     unsigned r;
57     AvahiPrioQueueNode *n;
58     assert(q);
59 
60     n = q->root;
61     assert(n);
62 
63     for (r = 0; r < y; r++) {
64         assert(n);
65 
66         if ((x >> (y-r-1)) & 1)
67             n = n->right;
68         else
69             n = n->left;
70     }
71 
72     assert(n->x == x);
73     assert(n->y == y);
74 
75     return n;
76 }
77 
exchange_nodes(AvahiPrioQueue * q,AvahiPrioQueueNode * a,AvahiPrioQueueNode * b)78 static void exchange_nodes(AvahiPrioQueue *q, AvahiPrioQueueNode *a, AvahiPrioQueueNode *b) {
79     AvahiPrioQueueNode *l, *r, *p, *ap, *an, *bp, *bn;
80     unsigned t;
81     assert(q);
82     assert(a);
83     assert(b);
84     assert(a != b);
85 
86     /* Swap positions */
87     t = a->x; a->x = b->x; b->x = t;
88     t = a->y; a->y = b->y; b->y = t;
89 
90     if (a->parent == b) {
91         /* B is parent of A */
92 
93         p = b->parent;
94         b->parent = a;
95 
96         if ((a->parent = p)) {
97             if (a->parent->left == b)
98                 a->parent->left = a;
99             else
100                 a->parent->right = a;
101         } else
102             q->root = a;
103 
104         if (b->left == a) {
105             if ((b->left = a->left))
106                 b->left->parent = b;
107             a->left = b;
108 
109             r = a->right;
110             if ((a->right = b->right))
111                 a->right->parent = a;
112             if ((b->right = r))
113                 b->right->parent = b;
114 
115         } else {
116             if ((b->right = a->right))
117                 b->right->parent = b;
118             a->right = b;
119 
120             l = a->left;
121             if ((a->left = b->left))
122                 a->left->parent = a;
123             if ((b->left = l))
124                 b->left->parent = b;
125         }
126     } else if (b->parent == a) {
127         /* A ist parent of B */
128 
129         p = a->parent;
130         a->parent = b;
131 
132         if ((b->parent = p)) {
133             if (b->parent->left == a)
134                 b->parent->left = b;
135             else
136                 b->parent->right = b;
137         } else
138             q->root = b;
139 
140         if (a->left == b) {
141             if ((a->left = b->left))
142                 a->left->parent = a;
143             b->left = a;
144 
145             r = a->right;
146             if ((a->right = b->right))
147                 a->right->parent = a;
148             if ((b->right = r))
149                 b->right->parent = b;
150         } else {
151             if ((a->right = b->right))
152                 a->right->parent = a;
153             b->right = a;
154 
155             l = a->left;
156             if ((a->left = b->left))
157                 a->left->parent = a;
158             if ((b->left = l))
159                 b->left->parent = b;
160         }
161     } else {
162         AvahiPrioQueueNode *apl = NULL, *bpl = NULL;
163 
164         /* Swap parents */
165         ap = a->parent;
166         bp = b->parent;
167 
168         if (ap)
169             apl = ap->left;
170         if (bp)
171             bpl = bp->left;
172 
173         if ((a->parent = bp)) {
174             if (bpl == b)
175                 bp->left = a;
176             else
177                 bp->right = a;
178         } else
179             q->root = a;
180 
181         if ((b->parent = ap)) {
182             if (apl == a)
183                 ap->left = b;
184             else
185                 ap->right = b;
186         } else
187             q->root = b;
188 
189         /* Swap children */
190         l = a->left;
191         r = a->right;
192 
193         if ((a->left = b->left))
194             a->left->parent = a;
195 
196         if ((b->left = l))
197             b->left->parent = b;
198 
199         if ((a->right = b->right))
200             a->right->parent = a;
201 
202         if ((b->right = r))
203             b->right->parent = b;
204     }
205 
206     /* Swap siblings */
207     ap = a->prev; an = a->next;
208     bp = b->prev; bn = b->next;
209 
210     if (a->next == b) {
211         /* A is predecessor of B */
212         a->prev = b;
213         b->next = a;
214 
215         if ((a->next = bn))
216             a->next->prev = a;
217         else
218             q->last = a;
219 
220         if ((b->prev = ap))
221             b->prev->next = b;
222 
223     } else if (b->next == a) {
224         /* B is predecessor of A */
225         a->next = b;
226         b->prev = a;
227 
228         if ((a->prev = bp))
229             a->prev->next = a;
230 
231         if ((b->next = an))
232             b->next->prev = b;
233         else
234             q->last = b;
235 
236     } else {
237         /* A is no neighbour of B */
238 
239         if ((a->prev = bp))
240             a->prev->next = a;
241 
242         if ((a->next = bn))
243             a->next->prev = a;
244         else
245             q->last = a;
246 
247         if ((b->prev = ap))
248             b->prev->next = b;
249 
250         if ((b->next = an))
251             b->next->prev = b;
252         else
253             q->last = b;
254     }
255 }
256 
257 /* Move a node to the correct position */
avahi_prio_queue_shuffle(AvahiPrioQueue * q,AvahiPrioQueueNode * n)258 void avahi_prio_queue_shuffle(AvahiPrioQueue *q, AvahiPrioQueueNode *n) {
259     assert(q);
260     assert(n);
261     assert(n->queue == q);
262 
263     /* Move up until the position is OK */
264     while (n->parent && q->compare(n->parent->data, n->data) > 0)
265         exchange_nodes(q, n, n->parent);
266 
267     /* Move down until the position is OK */
268     for (;;) {
269         AvahiPrioQueueNode *min;
270 
271         if (!(min = n->left)) {
272             /* No children */
273             assert(!n->right);
274             break;
275         }
276 
277         if (n->right && q->compare(n->right->data, min->data) < 0)
278             min = n->right;
279 
280         /* min now contains the smaller one of our two children */
281 
282         if (q->compare(n->data, min->data) <= 0)
283             /* Order OK */
284             break;
285 
286         exchange_nodes(q, n, min);
287     }
288 }
289 
avahi_prio_queue_put(AvahiPrioQueue * q,void * data)290 AvahiPrioQueueNode* avahi_prio_queue_put(AvahiPrioQueue *q, void* data) {
291     AvahiPrioQueueNode *n;
292     assert(q);
293 
294     if (!(n = avahi_new(AvahiPrioQueueNode, 1)))
295         return NULL; /* OOM */
296 
297     n->queue = q;
298     n->data = data;
299 
300     if (q->last) {
301         assert(q->root);
302         assert(q->n_nodes);
303 
304         n->y = q->last->y;
305         n->x = q->last->x+1;
306 
307         if (n->x >= ((unsigned) 1 << n->y)) {
308             n->x = 0;
309             n->y++;
310         }
311 
312         q->last->next = n;
313         n->prev = q->last;
314 
315         assert(n->y > 0);
316         n->parent = get_node_at_xy(q, n->x/2, n->y-1);
317 
318         if (n->x & 1)
319             n->parent->right = n;
320         else
321             n->parent->left = n;
322     } else {
323         assert(!q->root);
324         assert(!q->n_nodes);
325 
326         n->y = n->x = 0;
327         q->root = n;
328         n->prev = n->parent = NULL;
329     }
330 
331     n->next = n->left = n->right = NULL;
332     q->last = n;
333     q->n_nodes++;
334 
335     avahi_prio_queue_shuffle(q, n);
336 
337     return n;
338 }
339 
avahi_prio_queue_remove(AvahiPrioQueue * q,AvahiPrioQueueNode * n)340 void avahi_prio_queue_remove(AvahiPrioQueue *q, AvahiPrioQueueNode *n) {
341     assert(q);
342     assert(n);
343     assert(q == n->queue);
344 
345     if (n != q->last) {
346         AvahiPrioQueueNode *replacement = q->last;
347         exchange_nodes(q, replacement, n);
348         avahi_prio_queue_remove(q, n);
349         avahi_prio_queue_shuffle(q, replacement);
350         return;
351     }
352 
353     assert(n == q->last);
354     assert(!n->next);
355     assert(!n->left);
356     assert(!n->right);
357 
358     q->last = n->prev;
359 
360     if (n->prev) {
361         n->prev->next = NULL;
362         assert(n->parent);
363     } else
364         assert(!n->parent);
365 
366     if (n->parent) {
367         assert(n->prev);
368         if (n->parent->left == n) {
369             assert(n->parent->right == NULL);
370             n->parent->left = NULL;
371         } else {
372             assert(n->parent->right == n);
373             assert(n->parent->left != NULL);
374             n->parent->right = NULL;
375         }
376     } else {
377         assert(q->root == n);
378         assert(!n->prev);
379         assert(q->n_nodes == 1);
380         q->root = NULL;
381     }
382 
383     avahi_free(n);
384 
385     assert(q->n_nodes > 0);
386     q->n_nodes--;
387 }
388 
389