diff --git a/crates/fayalite/src/util/ready_valid.rs b/crates/fayalite/src/util/ready_valid.rs index ec761c2..94154c9 100644 --- a/crates/fayalite/src/util/ready_valid.rs +++ b/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 { @@ -34,3 +35,130 @@ impl ReadyValid { mapped } } + +// TODO: needs testing +#[hdl_module] +pub fn queue( + 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 = m.input(ReadyValid[ty]); + #[hdl] + let out: ReadyValid = 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::>() << (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); + } + } + } +}