lib/pqueue.c

   1 /* Priority queue functions.
   2  * Copyright (C) 2003 Yasuhiro Ohara
   3  *
   4  * This file is part of GNU Zebra.
   5  *
   6  * GNU Zebra is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published
   8  * by the Free Software Foundation; either version 2, or (at your
   9  * option) any later version.
  10  *
  11  * GNU Zebra is distributed in the hope that it will be useful, but
  12  * WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14  * General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU General Public License along
  17  * with this program; see the file COPYING; if not, write to the Free Software
  18  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
  19  */
  20
  21 #include <zebra.h>
  22
  23 #include "memory.h"
  24 #include "pqueue.h"
  25
  26 DEFINE_MTYPE_STATIC(LIB, PQUEUE, "Priority queue")
  27 DEFINE_MTYPE_STATIC(LIB, PQUEUE_DATA, "Priority queue data")
  28
  29 /* priority queue using heap sort */
  30
  31 /* pqueue->cmp() controls the order of sorting (i.e, ascending or
  32    descending). If you want the left node to move upper of the heap
  33    binary tree, make cmp() to return less than 0.  for example, if cmp
  34    (10, 20) returns -1, the sorting is ascending order. if cmp (10,
  35    20) returns 1, the sorting is descending order. if cmp (10, 20)
  36    returns 0, this library does not do sorting (which will not be what
  37    you want).  To be brief, if the contents of cmp_func (left, right)
  38    is left - right, dequeue () returns the smallest node.  Otherwise
  39    (if the contents is right - left), dequeue () returns the largest
  40    node.  */
  41
  42 #define DATA_SIZE (sizeof (void *))
  43 #define PARENT_OF(x) ((x - 1) / 2)
  44 #define LEFT_OF(x)  (2 * x + 1)
  45 #define RIGHT_OF(x) (2 * x + 2)
  46 #define HAVE_CHILD(x,q) (x < (q)->size / 2)
  47
  48 void trickle_up(int index, struct pqueue *queue)
  49 {
  50         void *tmp;
  51
  52         /* Save current node as tmp node.  */
  53         tmp = queue->array[index];
  54
  55         /* Continue until the node reaches top or the place where the parent
  56            node should be upper than the tmp node.  */
  57         while (index > 0
  58                && (*queue->cmp)(tmp, queue->array[PARENT_OF(index)]) < 0) {
  59                 /* actually trickle up */
  60                 queue->array[index] = queue->array[PARENT_OF(index)];
  61                 if (queue->update != NULL)
  62                         (*queue->update)(queue->array[index], index);
  63                 index = PARENT_OF(index);
  64         }
  65
  66         /* Restore the tmp node to appropriate place.  */
  67         queue->array[index] = tmp;
  68         if (queue->update != NULL)
  69                 (*queue->update)(tmp, index);
  70 }
  71
  72 void trickle_down(int index, struct pqueue *queue)
  73 {
  74         void *tmp;
  75         int which;
  76
  77         /* Save current node as tmp node.  */
  78         tmp = queue->array[index];
  79
  80         /* Continue until the node have at least one (left) child.  */
  81         while (HAVE_CHILD(index, queue)) {
  82                 /* If right child exists, and if the right child is more proper
  83                    to be moved upper.  */
  84                 if (RIGHT_OF(index) < queue->size
  85                     && (*queue->cmp)(queue->array[LEFT_OF(index)],
  86                                      queue->array[RIGHT_OF(index)])
  87                                > 0)
  88                         which = RIGHT_OF(index);
  89                 else
  90                         which = LEFT_OF(index);
  91
  92                 /* If the tmp node should be upper than the child, break.  */
  93                 if ((*queue->cmp)(queue->array[which], tmp) > 0)
  94                         break;
  95
  96                 /* Actually trickle down the tmp node.  */
  97                 queue->array[index] = queue->array[which];
  98                 if (queue->update != NULL)
  99                         (*queue->update)(queue->array[index], index);
 100                 index = which;
 101         }
 102
 103         /* Restore the tmp node to appropriate place.  */
 104         queue->array[index] = tmp;
 105         if (queue->update != NULL)
 106                 (*queue->update)(tmp, index);
 107 }
 108
 109 struct pqueue *pqueue_create(void)
 110 {
 111         struct pqueue *queue;
 112
 113         queue = XCALLOC(MTYPE_PQUEUE, sizeof(struct pqueue));
 114
 115         queue->array =
 116                 XCALLOC(MTYPE_PQUEUE_DATA, DATA_SIZE * PQUEUE_INIT_ARRAYSIZE);
 117         queue->array_size = PQUEUE_INIT_ARRAYSIZE;
 118
 119         /* By default we want nothing to happen when a node changes. */
 120         queue->update = NULL;
 121         return queue;
 122 }
 123
 124 void pqueue_delete(struct pqueue *queue)
 125 {
 126         XFREE(MTYPE_PQUEUE_DATA, queue->array);
 127         XFREE(MTYPE_PQUEUE, queue);
 128 }
 129
 130 static int pqueue_expand(struct pqueue *queue)
 131 {
 132         void **newarray;
 133
 134         newarray =
 135                 XCALLOC(MTYPE_PQUEUE_DATA, queue->array_size * DATA_SIZE * 2);
 136         if (newarray == NULL)
 137                 return 0;
 138
 139         memcpy(newarray, queue->array, queue->array_size * DATA_SIZE);
 140
 141         XFREE(MTYPE_PQUEUE_DATA, queue->array);
 142         queue->array = newarray;
 143         queue->array_size *= 2;
 144
 145         return 1;
 146 }
 147
 148 void pqueue_enqueue(void *data, struct pqueue *queue)
 149 {
 150         if (queue->size + 2 >= queue->array_size && !pqueue_expand(queue))
 151                 return;
 152
 153         queue->array[queue->size] = data;
 154         if (queue->update != NULL)
 155                 (*queue->update)(data, queue->size);
 156         trickle_up(queue->size, queue);
 157         queue->size++;
 158 }
 159
 160 void *pqueue_dequeue(struct pqueue *queue)
 161 {
 162         void *data = queue->array[0];
 163         queue->array[0] = queue->array[--queue->size];
 164         trickle_down(0, queue);
 165         return data;
 166 }
 167
 168 void pqueue_remove_at(int index, struct pqueue *queue)
 169 {
 170         queue->array[index] = queue->array[--queue->size];
 171
 172         if (index > 0
 173             && (*queue->cmp)(queue->array[index],
 174                              queue->array[PARENT_OF(index)])
 175                        < 0) {
 176                 trickle_up(index, queue);
 177         } else {
 178                 trickle_down(index, queue);
 179         }
 180 }
 181
 182 void pqueue_remove(void *data, struct pqueue *queue)
 183 {
 184         for (int i = 0; i < queue->size; i++)
 185                 if (queue->array[i] == data)
 186                         pqueue_remove_at(i, queue);
 187 }