diff --git a/lib/pqueue.c b/lib/pqueue.c
new file mode 100644
index 0000000..1e41b09
--- /dev/null
+++ b/lib/pqueue.c
@@ -0,0 +1,160 @@
+/* Priority queue functions.
+   Copyright (C) 2003 Yasuhiro Ohara
+
+This file is part of GNU Zebra.
+
+GNU Zebra is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published
+by the Free Software Foundation; either version 2, or (at your
+option) any later version.
+
+GNU Zebra is distributed in the hope that it will be useful, but
+WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with GNU Zebra; see the file COPYING.  If not, write to the
+Free Software Foundation, Inc., 59 Temple Place - Suite 330,
+Boston, MA 02111-1307, USA.  */
+
+#include <zebra.h>
+
+#include "pqueue.h"
+
+/* priority queue using heap sort */
+
+/* pqueue->cmp() controls the order of sorting (i.e, ascending or
+   descending). If you want the left node to move upper of the heap
+   binary tree, make cmp() to return less than 0.  for example, if cmp
+   (10, 20) returns -1, the sorting is ascending order. if cmp (10,
+   20) returns 1, the sorting is descending order. if cmp (10, 20)
+   returns 0, this library does not do sorting (which will not be what
+   you want).  To be brief, if the contents of cmp_func (left, right)
+   is left - right, dequeue () returns the smallest node.  Otherwise
+   (if the contents is right - left), dequeue () returns the largest
+   node.  */
+
+#define DATA_SIZE (sizeof (void *))
+#define PARENT_OF(x) ((x - 1) / 2)
+#define LEFT_OF(x)  (2 * x + 1)
+#define RIGHT_OF(x) (2 * x + 2)
+#define HAVE_CHILD(x,q) (x < (q)->size / 2)
+
+static void
+trickle_up (int index, struct pqueue *queue)
+{
+  void *tmp;
+
+  /* Save current node as tmp node.  */
+  tmp = queue->array[index];
+
+  /* Continue until the node reaches top or the place where the parent
+     node should be upper than the tmp node.  */
+  while (index > 0 &&
+         (*queue->cmp) (tmp, queue->array[PARENT_OF (index)]) < 0)
+    {
+      /* actually trickle up */
+      queue->array[index] = queue->array[PARENT_OF (index)];
+      index = PARENT_OF (index);
+    }
+
+  /* Restore the tmp node to appropriate place.  */
+  queue->array[index] = tmp;
+}
+
+static void
+trickle_down (int index, struct pqueue *queue)
+{
+  void *tmp;
+  int which;
+
+  /* Save current node as tmp node.  */
+  tmp = queue->array[index];
+
+  /* Continue until the node have at least one (left) child.  */
+  while (HAVE_CHILD (index, queue))
+    {
+      /* If right child exists, and if the right child is more proper
+         to be moved upper.  */
+      if (RIGHT_OF (index) < queue->size &&
+          (*queue->cmp) (queue->array[LEFT_OF (index)],
+                         queue->array[RIGHT_OF (index)]) > 0)
+        which = RIGHT_OF (index);
+      else
+        which = LEFT_OF (index);
+
+      /* If the tmp node should be upper than the child, break.  */
+      if ((*queue->cmp) (queue->array[which], tmp) > 0)
+        break;
+
+      /* Actually trickle down the tmp node.  */
+      queue->array[index] = queue->array[which];
+      index = which;
+    }
+
+  /* Restore the tmp node to appropriate place.  */
+  queue->array[index] = tmp;
+}
+
+struct pqueue *
+pqueue_create ()
+{
+  struct pqueue *queue;
+
+  queue = (struct pqueue *) malloc (sizeof (struct pqueue));
+  memset (queue, 0, sizeof (struct pqueue));
+
+  queue->array = (void **)
+    malloc (DATA_SIZE * PQUEUE_INIT_ARRAYSIZE);
+  memset (queue->array, 0, DATA_SIZE * PQUEUE_INIT_ARRAYSIZE);
+  queue->array_size = PQUEUE_INIT_ARRAYSIZE;
+
+  return queue;
+}
+
+void
+pqueue_delete (struct pqueue *queue)
+{
+  free (queue->array);
+  free (queue);
+}
+
+static int
+pqueue_expand (struct pqueue *queue)
+{
+  void **newarray;
+
+  newarray = (void **) malloc (queue->array_size * DATA_SIZE * 2);
+  if (newarray == NULL)
+    return 0;
+
+  memset (newarray, 0, queue->array_size * DATA_SIZE * 2);
+  memcpy (newarray, queue->array, queue->array_size * DATA_SIZE);
+
+  free (queue->array);
+  queue->array = newarray;
+  queue->array_size *= 2;
+
+  return 1;
+}
+
+void
+pqueue_enqueue (void *data, struct pqueue *queue)
+{
+  if (queue->size + 2 >= queue->array_size && ! pqueue_expand (queue))
+    return;
+
+  queue->array[queue->size] = data;
+  trickle_up (queue->size, queue);
+  queue->size ++;
+}
+
+void *
+pqueue_dequeue (struct pqueue *queue)
+{
+  void *data = queue->array[0];
+  queue->array[0] =  queue->array[--queue->size];
+  trickle_down (0, queue);
+  return data;
+}