add WIP version of queue()

crates/fayalite/src/util/ready_valid.rs

@@ -1,6 +1,7 @@
 // SPDX-License-Identifier: LGPL-3.0-or-later
 // See Notices.txt for copyright information
-use crate::prelude::*;
+use crate::{memory::splat_mask, prelude::*};
+use std::num::NonZeroUsize;
 
 #[hdl]
 pub struct ReadyValid<T> {
@@ -34,3 +35,130 @@ impl<T: Type> ReadyValid<T> {
         mapped
     }
 }
+
+// TODO: needs testing
+#[hdl_module]
+pub fn queue<T: Type>(
+    ty: T,
+    capacity: NonZeroUsize,
+    inp_ready_is_comb: bool,
+    out_valid_is_comb: bool,
+) {
+    let count_ty = UInt::range_inclusive(0..=capacity.get());
+    let index_ty = UInt::range(0..capacity.get());
+
+    #[hdl]
+    let cd: ClockDomain = m.input();
+    #[hdl]
+    let inp: ReadyValid<T> = m.input(ReadyValid[ty]);
+    #[hdl]
+    let out: ReadyValid<T> = m.output(ReadyValid[ty]);
+    #[hdl]
+    let count: UInt = m.output(count_ty);
+
+    #[hdl]
+    let inp_index = reg_builder().clock_domain(cd).reset(0.cast_to(index_ty));
+    #[hdl]
+    let out_index = reg_builder().clock_domain(cd).reset(0.cast_to(index_ty));
+    #[hdl]
+    let maybe_full = reg_builder().clock_domain(cd).reset(false);
+
+    #[hdl]
+    let mut mem = memory(ty);
+    mem.depth(capacity.get());
+    let read_port = mem.new_read_port();
+    let write_port = mem.new_write_port();
+
+    #[hdl]
+    let inp_fire: Bool = wire();
+    connect(inp_fire, ReadyValid::fire(inp));
+    #[hdl]
+    let out_fire: Bool = wire();
+    connect(out_fire, ReadyValid::fire(out));
+    #[hdl]
+    let indexes_equal: Bool = wire();
+    connect(indexes_equal, inp_index.cmp_eq(out_index));
+    #[hdl]
+    let empty: Bool = wire();
+    connect(empty, indexes_equal & !maybe_full);
+    #[hdl]
+    let full: Bool = wire();
+    connect(full, indexes_equal & maybe_full);
+
+    connect(read_port.addr, out_index);
+    connect(read_port.en, true);
+    connect(read_port.clk, cd.clk);
+    connect(write_port.addr, inp_index);
+    connect(write_port.en, inp_fire);
+    connect(write_port.clk, cd.clk);
+    connect(write_port.data, HdlOption::unwrap_or(inp.data, ty.uninit()));
+    connect(write_port.mask, splat_mask(ty, true.to_expr()));
+
+    connect(inp.ready, !full);
+    if inp_ready_is_comb {
+        #[hdl]
+        if out.ready {
+            connect(inp.ready, true);
+        }
+    }
+
+    #[hdl]
+    if !empty {
+        connect(out.data, HdlSome(read_port.data));
+    } else {
+        if out_valid_is_comb {
+            connect(out.data, inp.data);
+        } else {
+            connect(out.data, HdlOption[ty].HdlNone());
+        }
+    }
+
+    #[hdl]
+    if inp_fire.cmp_ne(out_fire) {
+        connect(maybe_full, inp_fire);
+    }
+
+    #[hdl]
+    if inp_fire {
+        #[hdl]
+        if inp_index.cmp_eq(capacity) {
+            connect_any(inp_index, 0_hdl_u0);
+        } else {
+            connect_any(inp_index, inp_index + 1_hdl_u1);
+        }
+    }
+
+    #[hdl]
+    if out_fire {
+        #[hdl]
+        if out_index.cmp_eq(capacity) {
+            connect_any(out_index, 0_hdl_u0);
+        } else {
+            connect_any(out_index, out_index + 1_hdl_u1);
+        }
+    }
+
+    #[hdl]
+    if indexes_equal {
+        connect(
+            count,
+            maybe_full.cast_to_static::<UInt<1>>() << (count_ty.width() - 1),
+        );
+    } else {
+        if capacity.is_power_of_two() {
+            debug_assert_eq!(count_ty.width(), index_ty.width() + 1);
+            #[hdl]
+            let count_lower = wire(index_ty);
+            connect(count_lower, (inp_index - out_index).cast_to(index_ty)); // wrap
+            connect(count, count_lower.cast_to(count_ty));
+        } else {
+            debug_assert_eq!(count_ty.width(), index_ty.width());
+            #[hdl]
+            if inp_index.cmp_lt(out_index) {
+                connect(count, inp_index + capacity - out_index);
+            } else {
+                connect(count, inp_index - out_index);
+            }
+        }
+    }
+}