Add byte queue to simulation utility package

diff --git a/libraries/simulated/util_pkg.vhd b/libraries/simulated/util_pkg.vhd
index 2fc2bb04a76d93be30b0a846e79709101d18ccf3..dc8a08d41be3b1b88f069e57b45ba20afed4591e 100644 (file)
--- a/libraries/simulated/util_pkg.vhd
+++ b/libraries/simulated/util_pkg.vhd
@@ -63,6 +63,28 @@ package sim_utility is
                                   idx:       in    natural;
                                   val:       in    byte_array_t);
 
+       ----------------------------------------------------------------------------
+       -- Byte queue
+
+       type qitem_t;
+       type p_qitem_t is access qitem_t;
+       type qitem_t is record
+               value:  byte_t;
+               p_prev: p_qitem_t;
+               p_next: p_qitem_t;
+       end record;
+       type queue_t is record
+               len:  natural;
+               head: p_qitem_t;
+               tail: p_qitem_t;
+       end record;
+
+       function queue_create return queue_t;
+       procedure queue_enq(queue: inout queue_t; value: in byte_t);
+       procedure queue_deq(queue: inout queue_t; value: out byte_t);
+       function queue_peek(queue: in queue_t) return byte_t;
+       function queue_len(queue: in queue_t) return natural;
+
 end sim_utility;
 
 
@@ -202,4 +224,86 @@ package body sim_utility is
                end loop;
        end procedure;
 
+       ----------------------------------------------------------------------------
+       -- Byte queue
+
+       -- Create a new queue
+       function queue_create return queue_t is
+       begin
+               return (
+                       len  => 0,
+                       head => null,
+                       tail => null
+               );
+       end function;
+
+       -- Insert a byte into a queue's head
+       procedure queue_enq(queue: inout queue_t; value: in byte_t) is
+               variable new_item: p_qitem_t;
+       begin
+               new_item := new qitem_t;
+               new_item.value    := value;
+               new_item.p_next   := null;
+               new_item.p_prev   := queue.head;
+
+               -- Attach to the old head if there is one
+               if queue.head /= null then
+                       queue.head.p_next := new_item;
+               end if;
+
+               -- Insert as new head
+               queue.head        := new_item;
+
+               -- If empty, also make the tail
+               if queue.tail = null then
+                       queue.tail := new_item;
+               end if;
+
+               queue.len := queue.len + 1;
+       end procedure;
+
+       -- Remove a byte from a queue's tail
+       procedure queue_deq(queue: inout queue_t; value: out byte_t) is
+               variable old_item: p_qitem_t;
+       begin
+               if queue.tail = null then
+                       value := (others => 'U');
+               else
+                       old_item := queue.tail;
+
+                       -- New tail is next item after this one
+                       -- Disconnect this item from the new tail
+                       queue.tail := old_item.p_next;
+                       if queue.tail /= null then
+                               queue.tail.p_prev := null;
+                       end if;
+
+                       -- If this was also the head (only item in the queue)
+                       -- null out the head pointer
+                       if queue.head = old_item then
+                               queue.head := null;
+                       end if;
+
+                       value := old_item.value;
+                       deallocate(old_item);
+                       queue.len := queue.len - 1;
+               end if;
+       end procedure;
+
+       -- Get the value of the byte in a queue's tail without removing it
+       function queue_peek(queue: in queue_t) return byte_t is
+       begin
+               if queue.tail = null then
+                       return (others => 'U');
+               else
+                       return queue.tail.value;
+               end if;
+       end function;
+
+       -- Get the number of items currently in a queue
+       function queue_len(queue: in queue_t) return natural is
+       begin
+               return queue.len;
+       end function;
+
 end sim_utility;