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
  49 trickle_up (int index, struct pqueue *queue)
  50 {
  51   void *tmp;
  52
  53   /* Save current node as tmp node.  */
  54   tmp = queue->array[index];
  55
  56   /* Continue until the node reaches top or the place where the parent
  57      node should be upper than the tmp node.  */
  58   while (index > 0 &&
  59          (*queue->cmp) (tmp, queue->array[PARENT_OF (index)]) < 0)
  60     {
  61       /* actually trickle up */
  62       queue->array[index] = queue->array[PARENT_OF (index)];
  63       if (queue->update != NULL)
  64         (*queue->update) (queue->array[index], index);
  65       index = PARENT_OF (index);
  66     }
  67
  68   /* Restore the tmp node to appropriate place.  */
  69   queue->array[index] = tmp;
  70   if (queue->update != NULL)
  71     (*queue->update) (tmp, index);
  72 }
  73
  74 void
  75 trickle_down (int index, struct pqueue *queue)
  76 {
  77   void *tmp;
  78   int which;
  79
  80   /* Save current node as tmp node.  */
  81   tmp = queue->array[index];
  82
  83   /* Continue until the node have at least one (left) child.  */
  84   while (HAVE_CHILD (index, queue))
  85     {
  86       /* If right child exists, and if the right child is more proper
  87          to be moved upper.  */
  88       if (RIGHT_OF (index) < queue->size &&
  89           (*queue->cmp) (queue->array[LEFT_OF (index)],
  90                          queue->array[RIGHT_OF (index)]) > 0)
  91         which = RIGHT_OF (index);
  92       else
  93         which = LEFT_OF (index);
  94
  95       /* If the tmp node should be upper than the child, break.  */
  96       if ((*queue->cmp) (queue->array[which], tmp) > 0)
  97         break;
  98
  99       /* Actually trickle down the tmp node.  */
 100       queue->array[index] = queue->array[which];
 101        if (queue->update != NULL)
 102          (*queue->update) (queue->array[index], index);
 103       index = which;
 104     }
 105
 106   /* Restore the tmp node to appropriate place.  */
 107   queue->array[index] = tmp;
 108   if (queue->update != NULL)
 109     (*queue->update) (tmp, index);
 110 }
 111
 112 struct pqueue *
 113 pqueue_create (void)
 114 {
 115   struct pqueue *queue;
 116
 117   queue = XCALLOC (MTYPE_PQUEUE, sizeof (struct pqueue));
 118
 119   queue->array = XCALLOC (MTYPE_PQUEUE_DATA,
 120                           DATA_SIZE * PQUEUE_INIT_ARRAYSIZE);
 121   queue->array_size = PQUEUE_INIT_ARRAYSIZE;
 122
 123   /* By default we want nothing to happen when a node changes. */
 124   queue->update = NULL;
 125   return queue;
 126 }
 127
 128 void
 129 pqueue_delete (struct pqueue *queue)
 130 {
 131   XFREE (MTYPE_PQUEUE_DATA, queue->array);
 132   XFREE (MTYPE_PQUEUE, queue);
 133 }
 134
 135 static int
 136 pqueue_expand (struct pqueue *queue)
 137 {
 138   void **newarray;
 139
 140   newarray = XCALLOC (MTYPE_PQUEUE_DATA, queue->array_size * DATA_SIZE * 2);
 141   if (newarray == NULL)
 142     return 0;
 143
 144   memcpy (newarray, queue->array, queue->array_size * DATA_SIZE);
 145
 146   XFREE (MTYPE_PQUEUE_DATA, queue->array);
 147   queue->array = newarray;
 148   queue->array_size *= 2;
 149
 150   return 1;
 151 }
 152
 153 void
 154 pqueue_enqueue (void *data, struct pqueue *queue)
 155 {
 156   if (queue->size + 2 >= queue->array_size && ! pqueue_expand (queue))
 157     return;
 158
 159   queue->array[queue->size] = data;
 160   if (queue->update != NULL)
 161     (*queue->update) (data, queue->size);
 162   trickle_up (queue->size, queue);
 163   queue->size ++;
 164 }
 165
 166 void *
 167 pqueue_dequeue (struct pqueue *queue)
 168 {
 169   void *data = queue->array[0];
 170   queue->array[0] =  queue->array[--queue->size];
 171   trickle_down (0, queue);
 172   return data;
 173 }
 174
 175 void
 176 pqueue_remove_at (int index, struct pqueue *queue)
 177 {
 178   queue->array[index] = queue->array[--queue->size];
 179
 180   if (index > 0
 181       && (*queue->cmp) (queue->array[index],
 182                         queue->array[PARENT_OF(index)]) < 0)
 183     {
 184       trickle_up (index, queue);
 185     }
 186   else
 187     {
 188       trickle_down (index, queue);
 189     }
 190 }
 191
 192 void
 193 pqueue_remove (void *data, struct pqueue *queue)
 194 {
 195   for (int i = 0; i < queue->size; i++)
 196     if (queue->array[i] == data)
 197       pqueue_remove_at (i, queue);
 198 }