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
 137         memcpy(newarray, queue->array, queue->array_size * DATA_SIZE);
 138
 139         XFREE(MTYPE_PQUEUE_DATA, queue->array);
 140         queue->array = newarray;
 141         queue->array_size *= 2;
 142
 143         return 1;
 144 }
 145
 146 void pqueue_enqueue(void *data, struct pqueue *queue)
 147 {
 148         if (queue->size + 2 >= queue->array_size && !pqueue_expand(queue))
 149                 return;
 150
 151         queue->array[queue->size] = data;
 152         if (queue->update != NULL)
 153                 (*queue->update)(data, queue->size);
 154         trickle_up(queue->size, queue);
 155         queue->size++;
 156 }
 157
 158 void *pqueue_dequeue(struct pqueue *queue)
 159 {
 160         void *data = queue->array[0];
 161         queue->array[0] = queue->array[--queue->size];
 162         trickle_down(0, queue);
 163         return data;
 164 }
 165
 166 void pqueue_remove_at(int index, struct pqueue *queue)
 167 {
 168         queue->array[index] = queue->array[--queue->size];
 169
 170         if (index > 0
 171             && (*queue->cmp)(queue->array[index],
 172                              queue->array[PARENT_OF(index)])
 173                        < 0) {
 174                 trickle_up(index, queue);
 175         } else {
 176                 trickle_down(index, queue);
 177         }
 178 }
 179
 180 void pqueue_remove(void *data, struct pqueue *queue)
 181 {
 182         for (int i = 0; i < queue->size; i++)
 183                 if (queue->array[i] == data)
 184                         pqueue_remove_at(i, queue);
 185 }