Commit 7bb56989 authored by egousiou's avatar egousiou

added missing ip_cores:-/

parent d2fce695
--! @file gc_big_adder.vhd
--! @brief Formerly eca_adder.vhd A pipelined adder for fast and wide arithmetic
--! @author Wesley W. Terpstra <w.terpstra@gsi.de>
--!
--! Copyright (C) 2013 GSI Helmholtz Centre for Heavy Ion Research GmbH
--!
--! The pipeline has three stages: partial sum, propogate carry, full sum
--! The inputs should be registers.
--! The outputs are provided at the end of each stage and should be registered.
--! The high carry bit is available one pipeline stage earlier than the full sum.
--! The high carry bit can be used to implement a comparator.
--!
--------------------------------------------------------------------------------
-- Renamed and moved to common -Mathias Kreider
--
--! This library is free software; you can redistribute it and/or
--! modify it under the terms of the GNU Lesser General Public
--! License as published by the Free Software Foundation; either
--! version 3 of the License, or (at your option) any later version.
--!
--! This library 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
--! Lesser General Public License for more details.
--!
--! You should have received a copy of the GNU Lesser General Public
--! License along with this library. If not, see <http://www.gnu.org/licenses/>.
---------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.wishbone_pkg.all;
use work.gencores_pkg.all;
-- Expects registers for inputs. Async outputs.
-- c1_o is available after 1 cycle (2 once registered)
-- c2_o, x2_o are available after 2 cycles (3 once registered)
entity gc_big_adder is
generic(
g_data_bits : natural := 64;
g_parts : natural := 4);
port(
clk_i : in std_logic;
stall_i : in std_logic := '0';
a_i : in std_logic_vector(g_data_bits-1 downto 0);
b_i : in std_logic_vector(g_data_bits-1 downto 0);
c_i : in std_logic := '0';
c1_o : out std_logic;
x2_o : out std_logic_vector(g_data_bits-1 downto 0);
c2_o : out std_logic);
end gc_big_adder;
architecture rtl of gc_big_adder is
-- Out of principle, tell quartus to leave my design alone.
attribute altera_attribute : string;
attribute altera_attribute of rtl : architecture is "-name AUTO_SHIFT_REGISTER_RECOGNITION OFF";
constant c_parts : natural := g_parts; -- Must divide g_data_bits
constant c_sub_bits : natural := g_data_bits / c_parts;
subtype t_part is std_logic_vector(c_sub_bits-1 downto 0);
subtype t_carry is std_logic_vector(c_parts -1 downto 0);
type t_part_array is array(c_parts-1 downto 0) of t_part;
constant c_zeros : t_part := (others => '0');
-- Pipeline:
-- s1: partial sums / prop+gen
-- s2: carry bits
-- s3: full sum
-- Registers
signal r1_sum0 : t_part_array;
--signal r1_sum1 : t_part_array;
signal r1_p : t_carry;
signal r1_g : t_carry;
signal r1_c : std_logic;
signal r2_sum0 : t_part_array;
--signal r2_sum1 : t_part_array;
signal r2_c : t_carry;
signal r2_cH : std_logic;
-- Signals
signal s1_r : std_logic_vector(c_parts downto 0);
begin
s1_r <= f_big_ripple(r1_p, r1_g, r1_c);
main : process(clk_i) is
variable sum0, sum1 : std_logic_vector(c_sub_bits downto 0);
begin
if rising_edge(clk_i) then
if stall_i = '0' then
for i in 0 to c_parts-1 loop
sum0 := f_big_ripple(a_i((i+1)*c_sub_bits-1 downto i*c_sub_bits),
b_i((i+1)*c_sub_bits-1 downto i*c_sub_bits), '0');
sum1 := f_big_ripple(a_i((i+1)*c_sub_bits-1 downto i*c_sub_bits),
b_i((i+1)*c_sub_bits-1 downto i*c_sub_bits), '1');
r1_sum0(i) <= sum0(c_sub_bits-1 downto 0);
--r1_sum1(i) <= sum1(c_sub_bits-1 downto 0);
r1_g(i) <= sum0(c_sub_bits);
r1_p(i) <= sum1(c_sub_bits);
end loop;
r1_c <= c_i;
r2_c <= s1_r(c_parts-1 downto 0) xor (r1_p xor r1_g);
r2_cH <= s1_r(c_parts);
r2_sum0 <= r1_sum0;
--r2_sum1 <= r1_sum1;
end if;
end if;
end process;
c1_o <= s1_r(c_parts);
c2_o <= r2_cH;
output : for i in 0 to c_parts-1 generate
-- save the sum1 registers by using an adder instead of MUX
x2_o((i+1)*c_sub_bits-1 downto i*c_sub_bits) <=
f_big_ripple(r2_sum0(i), c_zeros, r2_c(i))
(c_sub_bits-1 downto 0);
-- r2_sum1(i) when r2_c(i)='1' else r2_sum0(i);
end generate;
end rtl;
--==============================================================================
-- CERN (BE-CO-HT)
-- Finite State Machine Watchdog Timer
--==============================================================================
--
-- author: Theodor Stana (t.stana@cern.ch)
--
-- date of creation: 2013-11-22
--
-- version: 1.0
--
-- description:
--
-- dependencies:
--
-- references:
--
--==============================================================================
-- GNU LESSER GENERAL PUBLIC LICENSE
--==============================================================================
-- This source file is free software; you can redistribute it and/or modify it
-- under the terms of the GNU Lesser General Public License as published by the
-- Free Software Foundation; either version 2.1 of the License, or (at your
-- option) any later version. This source 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 Lesser General Public License for more details. You should have
-- received a copy of the GNU Lesser General Public License along with this
-- source; if not, download it from http://www.gnu.org/licenses/lgpl-2.1.html
--==============================================================================
-- last changes:
-- 2013-11-22 Theodor Stana File created
--==============================================================================
-- TODO: -
--==============================================================================
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.genram_pkg.all;
entity gc_fsm_watchdog is
generic
(
-- Maximum value of watchdog timer in clk_i cycles
g_wdt_max : positive := 65535
);
port
(
-- Clock and active-low reset line
clk_i : in std_logic;
rst_n_i : in std_logic;
-- Active-high watchdog timer reset line, synchronous to clk_i
wdt_rst_i : in std_logic;
-- Active-high reset output, synchronous to clk_i
fsm_rst_o : out std_logic
);
end entity gc_fsm_watchdog;
architecture behav of gc_fsm_watchdog is
--============================================================================
-- Signal declarations
--============================================================================
signal wdt : unsigned(f_log2_size(g_wdt_max)-1 downto 0);
--==============================================================================
-- architecture begin
--==============================================================================
begin
--============================================================================
-- Watchdog timer process
--============================================================================
p_wdt : process (clk_i) is
begin
if rising_edge(clk_i) then
if (rst_n_i = '0') or (wdt_rst_i = '1') then
wdt <= (others => '0');
fsm_rst_o <= '0';
else
wdt <= wdt + 1;
if (wdt = g_wdt_max-1) then
fsm_rst_o <= '1';
end if;
end if;
end if;
end process p_wdt;
end architecture behav;
--==============================================================================
-- architecture end
--==============================================================================
--==============================================================================
-- CERN (BE-CO-HT)
-- Glitch filter with selectable length
--==============================================================================
--
-- author: Theodor Stana (t.stana@cern.ch)
--
-- date of creation: 2013-03-12
--
-- version: 1.0
--
-- description:
--
-- dependencies:
--
-- references:
--
--==============================================================================
-- GNU LESSER GENERAL PUBLIC LICENSE
--==============================================================================
-- This source file is free software; you can redistribute it and/or modify it
-- under the terms of the GNU Lesser General Public License as published by the
-- Free Software Foundation; either version 2.1 of the License, or (at your
-- option) any later version. This source 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 Lesser General Public License for more details. You should have
-- received a copy of the GNU Lesser General Public License along with this
-- source; if not, download it from http://www.gnu.org/licenses/lgpl-2.1.html
--==============================================================================
-- last changes:
-- 2013-03-12 Theodor Stana t.stana@cern.ch File created
--==============================================================================
-- TODO: -
--==============================================================================
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.gencores_pkg.all;
entity gc_glitch_filt is
generic
(
-- Length of glitch filter:
-- g_len = 1 => data width should be > 1 clk_i cycle
-- g_len = 2 => data width should be > 2 clk_i cycle
-- etc.
g_len : natural := 4
);
port
(
clk_i : in std_logic;
rst_n_i : in std_logic;
-- Data input
dat_i : in std_logic;
-- Data output
-- latency: g_len+1 clk_i cycles
dat_o : out std_logic
);
end entity gc_glitch_filt;
architecture behav of gc_glitch_filt is
--============================================================================
-- Signal declarations
--============================================================================
signal glitch_filt : std_logic_vector(g_len downto 0);
signal dat_synced : std_logic;
--==============================================================================
-- architecture begin
--==============================================================================
begin
--============================================================================
-- Glitch filtration logic
--============================================================================
-- First, synchronize the data input in the clk_i domain
cmp_sync : gc_sync_ffs
port map
(
clk_i => clk_i,
rst_n_i => rst_n_i,
data_i => dat_i,
synced_o => dat_synced,
npulse_o => open,
ppulse_o => open
);
-- Then, assign the current sample of the glitch filter
glitch_filt(0) <= dat_synced;
-- Generate glitch filter FFs when the filter length is > 0
gen_glitch_filt: if (g_len > 0) generate
p_glitch_filt: process (clk_i)
begin
if rising_edge(clk_i) then
if (rst_n_i = '0') then
glitch_filt(g_len downto 1) <= (others => '0');
else
glitch_filt(g_len downto 1) <= glitch_filt(g_len-1 downto 0);
end if;
end if;
end process p_glitch_filt;
end generate gen_glitch_filt;
-- and set the data output based on the state of the glitch filter
p_output: process(clk_i)
begin
if rising_edge(clk_i) then
if (rst_n_i = '0') then
dat_o <= '0';
elsif (glitch_filt = (glitch_filt'range => '1')) then
dat_o <= '1';
elsif (glitch_filt = (glitch_filt'range => '0')) then
dat_o <= '0';
end if;
end if;
end process p_output;
end architecture behav;
--==============================================================================
-- architecture end
--==============================================================================
--==============================================================================
-- CERN (BE-CO-HT)
-- I2C slave core
--==============================================================================
--
-- author: Theodor Stana (t.stana@cern.ch)
--
-- date of creation: 2013-03-13
--
-- version: 1.0
--
-- description:
--
-- Simple I2C slave interface, providing the basic low-level functionality
-- of the I2C protocol.
--
-- The gc_i2c_slave module waits for a master to initiate a transfer via
-- a start condition. The address is sent next and if the address matches
-- the slave address set via the i2c_addr_i input, the addr_good_p_o output
-- is set. Based on the eighth bit of the first I2C transfer byte, the module
-- then starts shifting in or out each byte in the transfer, setting the
-- r/w_done_p_o output after each received/sent byte.
--
-- For master write (slave read) transfers, the received byte can be read at
-- the rx_byte_o output when the r_done_p_o pin is high. For master read (slave
-- write) transfers, the slave sends the byte at the tx_byte_i input, which
-- should be set when the w_done_p_o output is high, either after I2C address
-- reception, or a successful send of a previous byte.
--
-- dependencies:
-- OHWR general-cores library
--
-- references:
-- [1] The I2C bus specification, version 2.1, NXP Semiconductor, Jan. 2000
-- http://www.nxp.com/documents/other/39340011.pdf
--
--==============================================================================
-- GNU LESSER GENERAL PUBLIC LICENSE
--==============================================================================
-- This source file is free software; you can redistribute it and/or modify it
-- under the terms of the GNU Lesser General Public License as published by the
-- Free Software Foundation; either version 2.1 of the License, or (at your
-- option) any later version. This source 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 Lesser General Public License for more details. You should have
-- received a copy of the GNU Lesser General Public License along with this
-- source; if not, download it from http://www.gnu.org/licenses/lgpl-2.1.html
--==============================================================================
-- last changes:
-- 2013-03-13 Theodor Stana File created
-- 2013-11-22 Theodor Stana Changed to sampling SDA on SCL rising edge
--==============================================================================
-- TODO:
-- - Stop condition
--==============================================================================
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.gencores_pkg.all;
entity gc_i2c_slave is
generic
(
-- Length of glitch filter
-- 0 - SCL and SDA lines are passed only through synchronizer
-- 1 - one clk_i glitches filtered
-- 2 - two clk_i glitches filtered
g_gf_len : natural := 0
);
port
(
-- Clock, reset ports
clk_i : in std_logic;
rst_n_i : in std_logic;
-- I2C lines
scl_i : in std_logic;
scl_o : out std_logic;
scl_en_o : out std_logic;
sda_i : in std_logic;
sda_o : out std_logic;
sda_en_o : out std_logic;
-- Slave address
i2c_addr_i : in std_logic_vector(6 downto 0);
-- ACK input, should be set after done_p_o = '1'
-- (note that the bit is reversed wrt I2C ACK bit)
-- '1' - ACK
-- '0' - NACK
ack_i : in std_logic;
-- Byte to send, should be loaded while done_p_o = '1'
tx_byte_i : in std_logic_vector(7 downto 0);
-- Received byte, valid after done_p_o = '1'
rx_byte_o : out std_logic_vector(7 downto 0);
-- Pulse outputs signaling various I2C actions
-- Start and stop conditions
i2c_sta_p_o : out std_logic;
i2c_sto_p_o : out std_logic;
-- Received address corresponds i2c_addr_i
addr_good_p_o : out std_logic;
-- Read and write done
r_done_p_o : out std_logic;
w_done_p_o : out std_logic;
-- I2C bus operation, set after address detection
-- '0' - write
-- '1' - read
op_o : out std_logic
);
end entity gc_i2c_slave;
architecture behav of gc_i2c_slave is
--============================================================================
-- Type declarations
--============================================================================
type t_state is
(
IDLE, -- idle
ADDR, -- shift in I2C address bits
ADDR_CHECK, -- check received I2C address
ADDR_ACK, -- ACK/NACK to I2C address
RD, -- shift in byte to read
RD_ACK, -- ACK/NACK to received byte
WR_LOAD_TXSR, -- load byte to send via I2C
WR, -- shift out byte
WR_ACK -- get ACK/NACK from master
);
--============================================================================
-- Signal declarations
--============================================================================
-- Deglitched signals and delays for SCL and SDA lines
signal scl_deglitched : std_logic;
signal scl_deglitched_d0 : std_logic;
signal sda_deglitched : std_logic;
signal sda_deglitched_d0 : std_logic;
signal scl_r_edge_p : std_logic;
signal scl_f_edge_p : std_logic;
signal sda_f_edge_p : std_logic;
signal sda_r_edge_p : std_logic;
-- FSM signals
signal state : t_state;
signal inhibit : std_logic;
-- RX and TX shift registers
signal txsr : std_logic_vector(7 downto 0);
signal rxsr : std_logic_vector(7 downto 0);
-- Bit counter on RX & TX
signal bit_cnt : unsigned(2 downto 0);
-- Start and stop condition pulse signals
signal sta_p, sto_p : std_logic;
-- Master ACKed after it has read a byte from the slave
signal mst_acked : std_logic;
--==============================================================================
-- architecture begin
--==============================================================================
begin
--============================================================================
-- I/O logic
--============================================================================
-- No clock stretching implemented, always disable SCL line
scl_o <= '0';
scl_en_o <= '0';
-- SDA line driven low; SDA_EN line controls when the tristate buffer is enabled
sda_o <= '0';
-- Assign RX byte output
rx_byte_o <= rxsr;
--============================================================================
-- Deglitching logic
--============================================================================
-- Generate deglitched SCL signal with 54-ns max. glitch width
cmp_scl_deglitch : gc_glitch_filt
generic map
(
g_len => g_gf_len
)
port map
(
clk_i => clk_i,
rst_n_i => rst_n_i,
dat_i => scl_i,
dat_o => scl_deglitched
);
-- and create a delayed version of this signal, together with one-tick-long
-- falling-edge detection signal
p_scl_degl_d0 : process(clk_i) is
begin
if rising_edge(clk_i) then
if (rst_n_i = '0') then
scl_deglitched_d0 <= '0';
scl_f_edge_p <= '0';
scl_r_edge_p <= '0';
else
scl_deglitched_d0 <= scl_deglitched;
scl_f_edge_p <= (not scl_deglitched) and scl_deglitched_d0;
scl_r_edge_p <= scl_deglitched and (not scl_deglitched_d0);
end if;
end if;
end process p_scl_degl_d0;
-- Generate deglitched SDA signal with 54-ns max. glitch width
cmp_sda_deglitch : gc_glitch_filt
generic map
(
g_len => g_gf_len
)
port map
(
clk_i => clk_i,
rst_n_i => rst_n_i,
dat_i => sda_i,
dat_o => sda_deglitched
);
-- and create a delayed version of this signal, together with one-tick-long
-- falling- and rising-edge detection signals
p_sda_deglitched_d0 : process(clk_i) is
begin
if rising_edge(clk_i) then
if (rst_n_i = '0') then
sda_deglitched_d0 <= '0';
sda_f_edge_p <= '0';
sda_r_edge_p <= '0';
else
sda_deglitched_d0 <= sda_deglitched;
sda_f_edge_p <= (not sda_deglitched) and sda_deglitched_d0;
sda_r_edge_p <= sda_deglitched and (not sda_deglitched_d0);
end if;
end if;
end process p_sda_deglitched_d0;
--============================================================================
-- Start and stop condition outputs
--============================================================================
-- First the process to set the start and stop conditions as per I2C standard
p_sta_sto : process (clk_i) is
begin
if rising_edge(clk_i) then
if (rst_n_i = '0') then
sta_p <= '0';
sto_p <= '0';
else
sta_p <= sda_f_edge_p and scl_deglitched;
sto_p <= sda_r_edge_p and scl_deglitched;
end if;
end if;
end process p_sta_sto;
-- Finally, set the outputs
i2c_sta_p_o <= sta_p;
i2c_sto_p_o <= sto_p;
--============================================================================
-- FSM logic
--============================================================================
p_fsm: process (clk_i) is
begin
if rising_edge(clk_i) then
if (rst_n_i = '0') then
state <= IDLE;
inhibit <= '0';
bit_cnt <= (others => '0');
rxsr <= (others => '0');
txsr <= (others => '0');
mst_acked <= '0';
sda_en_o <= '0';
r_done_p_o <= '0';
w_done_p_o <= '0';
addr_good_p_o <= '0';
op_o <= '0';
-- start and stop conditions are followed by I2C address, so any byte
-- following would be an address byte; therefore, it is safe to deinhibit
-- the FSM
elsif (sta_p = '1') or (sto_p = '1') then
state <= IDLE;
inhibit <= '0';
-- state machine logic
else
case state is
---------------------------------------------------------------------
-- IDLE
---------------------------------------------------------------------
when IDLE =>
-- clear outputs and bit counter
bit_cnt <= (others => '0');
sda_en_o <= '0';
mst_acked <= '0';
r_done_p_o <= '0';
w_done_p_o <= '0';
addr_good_p_o <= '0';
if (scl_f_edge_p = '1') and (inhibit = '0') then
state <= ADDR;
end if;
---------------------------------------------------------------------
-- ADDR
---------------------------------------------------------------------
when ADDR =>
-- Shifting in is done on rising edge of SCL
if (scl_r_edge_p = '1') then
rxsr <= rxsr(6 downto 0) & sda_deglitched;
bit_cnt <= bit_cnt + 1;
end if;
if (scl_f_edge_p = '1') then
-- If we've shifted in 8 bits, go to ADDR_CHECK
if (bit_cnt = 0) then
state <= ADDR_CHECK;
end if;
end if;
---------------------------------------------------------------------
-- ADDR_CHECK
---------------------------------------------------------------------
when ADDR_CHECK =>
-- if the address is ours, set the OP output and go to ACK state
if (rxsr(7 downto 1) = i2c_addr_i) then
op_o <= rxsr(0);
addr_good_p_o <= '1';
state <= ADDR_ACK;
-- if the address is not ours, the FSM should be inhibited so a
-- byte sent to another slave doesn't get interpreted as this
-- slave's address
else
inhibit <= '1';
state <= IDLE;
end if;
---------------------------------------------------------------------
-- ADDR_ACK
---------------------------------------------------------------------
when ADDR_ACK =>
-- clear addr_good pulse
addr_good_p_o <= '0';
-- send ACK from input and go start reading or writing based on OP
sda_en_o <= ack_i;
if (scl_f_edge_p = '1') then
if (rxsr(0) = '0') then
state <= RD;
else
state <= WR_LOAD_TXSR;
end if;
end if;
---------------------------------------------------------------------
-- RD
---------------------------------------------------------------------
-- Shift in bits sent by the master
---------------------------------------------------------------------
when RD =>
-- not controlling SDA, clear enable signal
sda_en_o <= '0';
-- shift in on rising-edge
if (scl_r_edge_p = '1') then
rxsr <= rxsr(6 downto 0) & sda_deglitched;
bit_cnt <= bit_cnt + 1;
end if;
if (scl_f_edge_p = '1') then
-- Received 8 bits, go to RD_ACK and signal external module
if (bit_cnt = 0) then
state <= RD_ACK;
r_done_p_o <= '1';
end if;
end if;
---------------------------------------------------------------------
-- RD_ACK
---------------------------------------------------------------------
when RD_ACK =>
-- Clear done pulse
r_done_p_o <= '0';
-- we write the ACK bit, so control sda_en_o signal to send ACK/NACK
sda_en_o <= ack_i;
-- based on the ACK received by external command, we read the next
-- bit (ACK) or go back to idle state (NACK)
if (scl_f_edge_p = '1') then
if (ack_i = '1') then
state <= RD;
else
state <= IDLE;
end if;
end if;
---------------------------------------------------------------------
-- WR_LOAD_TXSR
---------------------------------------------------------------------
when WR_LOAD_TXSR =>
txsr <= tx_byte_i;
state <= WR;
---------------------------------------------------------------------
-- WR
---------------------------------------------------------------------
when WR =>
-- slave writes, SDA output enable is the negated value of the bit
-- to send (since on I2C, '1' is a release of the bus)
sda_en_o <= not txsr(7);
-- increment bit counter on rising edge
if (scl_r_edge_p = '1') then
bit_cnt <= bit_cnt + 1;
end if;
-- Shift TXSR on falling edge of SCL
if (scl_f_edge_p = '1') then
txsr <= txsr(6 downto 0) & '0';
-- Eight bits sent, disable SDA and go to WR_ACK
if (bit_cnt = 0) then
state <= WR_ACK;
w_done_p_o <= '1';
end if;
end if;
---------------------------------------------------------------------
-- WR_ACK
---------------------------------------------------------------------
when WR_ACK =>
-- master controls SDA, clear sda_en_o
sda_en_o <= '0';
-- clear done pulse
w_done_p_o <= '0';
-- sample in ACK from master on rising edge
if (scl_r_edge_p = '1') then
if (sda_deglitched = '0') then
mst_acked <= '1';
else
mst_acked <= '0';
end if;
end if;
-- and check it on falling edge
if (scl_f_edge_p = '1') then
if (mst_acked = '1') then
state <= WR_LOAD_TXSR;
else
state <= IDLE;
end if;
end if;
---------------------------------------------------------------------
-- Any other state: go back to IDLE
---------------------------------------------------------------------
when others =>
state <= IDLE;
end case;
end if;
end if;
end process p_fsm;
end architecture behav;
--==============================================================================
-- architecture end
--==============================================================================
-------------------------------------------------------------------------------
-- Title : Pulse synchronizer
-- Project : General Cores Library
-------------------------------------------------------------------------------
-- File : gc_pulse_synchronizer2.vhd
-- Author : Tomasz Wlostowski, Wesley Terpstra
-- Company : CERN BE-CO-HT
-- Created : 2012-01-10
-- Last update: 2012-08-29
-- Platform : FPGA-generic
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Full feedback pulse synchronizer (works independently of the
-- input/output clock domain frequency ratio)
-------------------------------------------------------------------------------
--
-- Copyright (c) 2012 CERN / BE-CO-HT
--
-- This source file is free software; you can redistribute it
-- and/or modify it under the terms of the GNU Lesser General
-- Public License as published by the Free Software Foundation;
-- either version 2.1 of the License, or (at your option) any
-- later version.
--
-- This source 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 Lesser General Public License for more
-- details.
--
-- You should have received a copy of the GNU Lesser General
-- Public License along with this source; if not, download it
-- from http://www.gnu.org/licenses/lgpl-2.1.html
--
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2012-01-12 1.0 twlostow Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use work.gencores_pkg.all;
entity gc_pulse_synchronizer2 is
port (
-- pulse input clock
clk_in_i : in std_logic;
rst_in_n_i : in std_logic;
-- pulse output clock
clk_out_i : in std_logic;
rst_out_n_i : in std_logic;
-- pulse input ready (clk_in_i domain). When HI, a pulse coming to d_p_i will be
-- correctly transferred to q_p_o.
d_ready_o : out std_logic;
-- pulse input (clk_in_i domain)
d_p_i : in std_logic;
-- pulse output (clk_out_i domain)
q_p_o : out std_logic);
end gc_pulse_synchronizer2;
architecture rtl of gc_pulse_synchronizer2 is
constant c_sync_stages : integer := 3;
signal ready, d_p_d0 : std_logic;
signal in_ext, out_ext : std_logic;
signal out_feedback : std_logic;
signal d_in2out : std_logic_vector(c_sync_stages-1 downto 0);
signal d_out2in : std_logic_vector(c_sync_stages-1 downto 0);
begin -- rtl
process(clk_out_i, rst_out_n_i)
begin
if rst_out_n_i = '0' then
d_in2out <= (others => '0');
out_ext <= '0';
elsif rising_edge(clk_out_i) then
d_in2out <= d_in2out(c_sync_stages-2 downto 0) & in_ext;
out_ext <= d_in2out(c_sync_stages-1);
end if;
end process;
process(clk_in_i, rst_in_n_i)
begin
if rst_in_n_i = '0' then
d_out2in <= (others => '0');
elsif rising_edge(clk_in_i) then
d_out2in <= d_out2in(c_sync_stages-2 downto 0) & out_ext;
end if;
end process;
out_feedback <= d_out2in(c_sync_stages-1);
p_input_ack : process(clk_in_i, rst_in_n_i)
begin
if rst_in_n_i = '0' then
ready <= '1';
in_ext <= '0';
d_p_d0 <= '0';
elsif rising_edge(clk_in_i) then
d_p_d0 <= d_p_i;
if(ready = '1' and d_p_i = '1' and d_p_d0 = '0') then
in_ext <= not in_ext;
ready <= '0';
elsif(in_ext = out_feedback) then
ready <= '1';
end if;
end if;
end process;
p_drive_output : process(clk_out_i, rst_out_n_i)
begin
if rst_out_n_i = '0' then
q_p_o <= '0';
elsif rising_edge(clk_out_i) then
q_p_o <= out_ext xor d_in2out(c_sync_stages-1);
end if;
end process;
d_ready_o <= ready;
end rtl;
coregen_ip
\ No newline at end of file
files = [
"genram_pkg.vhd",
"memory_loader_pkg.vhd",
"generic_shiftreg_fifo.vhd",
"inferred_sync_fifo.vhd",
"inferred_async_fifo.vhd"];
if (target == "altera"):
modules = {"local" : "altera"}
elif (target == "xilinx" and syn_device[0:4].upper()=="XC6V"):
modules = {"local" : ["xilinx", "xilinx/virtex6"]}
elif (target == "xilinx"):
modules = {"local" : ["xilinx", "generic"]}
files = [
"generic_async_fifo.vhd",
"generic_simple_dpram.vhd",
"generic_dpram.vhd",
"generic_spram.vhd",
"generic_sync_fifo.vhd",
"gc_shiftreg.vhd"]
library ieee;
use ieee.STD_LOGIC_1164.all;
use ieee.NUMERIC_STD.all;
use work.genram_pkg.all;
entity gc_shiftreg is
generic (
g_size : integer);
port (
clk_i : in std_logic;
en_i : in std_logic;
d_i : in std_logic;
q_o : out std_logic;
a_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0));
end gc_shiftreg;
architecture rtl of gc_shiftreg is
signal a : std_logic_vector(4 downto 0);
signal sr : std_logic_vector(g_size-1 downto 0);
begin
a <= std_logic_vector(resize(unsigned(a_i), 5));
p_srl : process(clk_i)
begin
if rising_edge(clk_i) then
if en_i = '1' then
sr <= sr(sr'left - 1 downto 0) & d_i;
end if;
end if;
end process;
q_o <= sr(TO_INTEGER(unsigned(a_i)));
end rtl;
-------------------------------------------------------------------------------
-- Title : Parametrizable asynchronous FIFO (Altera version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_async_fifo.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-CO-HT
-- Created : 2011-01-25
-- Last update: 2011-01-25
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Dual-clock asynchronous FIFO.
-- - configurable data width and size
-- - "show ahead" mode
-- - configurable full/empty/almost full/almost empty/word count signals
-- Todo:
-- - optimize almost empty / almost full flags generation for speed
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library altera_mf;
use altera_mf.all;
use work.genram_pkg.all;
entity generic_async_fifo is
generic (
g_data_width : natural;
g_size : natural;
g_show_ahead : boolean := false;
-- Read-side flag selection
g_with_rd_empty : boolean := true; -- with empty flag
g_with_rd_full : boolean := false; -- with full flag
g_with_rd_almost_empty : boolean := false;
g_with_rd_almost_full : boolean := false;
g_with_rd_count : boolean := false; -- with words counter
g_with_wr_empty : boolean := false;
g_with_wr_full : boolean := true;
g_with_wr_almost_empty : boolean := false;
g_with_wr_almost_full : boolean := false;
g_with_wr_count : boolean := false;
g_almost_empty_threshold : integer; -- threshold for almost empty flag
g_almost_full_threshold : integer -- threshold for almost full flag
);
port (
rst_n_i : in std_logic := '1';
-- write port
clk_wr_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
wr_empty_o : out std_logic;
wr_full_o : out std_logic;
wr_almost_empty_o : out std_logic;
wr_almost_full_o : out std_logic;
wr_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0);
-- read port
clk_rd_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
rd_empty_o : out std_logic;
rd_full_o : out std_logic;
rd_almost_empty_o : out std_logic;
rd_almost_full_o : out std_logic;
rd_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0)
);
end generic_async_fifo;
architecture syn of generic_async_fifo is
component dcfifo
generic (
lpm_numwords : natural;
lpm_showahead : string;
lpm_type : string;
lpm_width : natural;
lpm_widthu : natural;
overflow_checking : string;
rdsync_delaypipe : natural;
underflow_checking : string;
use_eab : string;
read_aclr_synch : string;
write_aclr_synch : string;
wrsync_delaypipe : natural
);
port (
wrclk : in std_logic;
rdempty : out std_logic;
wrempty : out std_logic;
rdreq : in std_logic;
aclr : in std_logic;
rdfull : out std_logic;
wrfull : out std_logic;
rdclk : in std_logic;
q : out std_logic_vector (g_data_width-1 downto 0);
wrreq : in std_logic;
data : in std_logic_vector (g_data_width-1 downto 0);
wrusedw : out std_logic_vector (f_log2_size(g_size)-1 downto 0);
rdusedw : out std_logic_vector (f_log2_size(g_size)-1 downto 0)
);
end component;
function f_bool_2_string (x : boolean) return string is
begin
if(x) then
return "ON";
else
return "OFF";
end if;
end f_bool_2_string;
signal rdempty : std_logic;
signal wrempty : std_logic;
signal aclr : std_logic;
signal rdfull : std_logic;
signal wrfull : std_logic;
signal wrusedw : std_logic_vector (f_log2_size(g_size)-1 downto 0);
signal rdusedw : std_logic_vector (f_log2_size(g_size)-1 downto 0);
begin -- syn
aclr <= not rst_n_i;
dcfifo_inst : dcfifo
generic map (
lpm_numwords => g_size,
lpm_showahead => f_bool_2_string(g_show_ahead),
lpm_type => "dcfifo",
lpm_width => g_data_width,
lpm_widthu => f_log2_size(g_size),
overflow_checking => "ON",
rdsync_delaypipe => 5, -- 2 sync stages
underflow_checking => "ON",
use_eab => "ON",
read_aclr_synch => "ON",
write_aclr_synch => "ON",
wrsync_delaypipe => 5 -- 2 sync stages
)
port map (
wrclk => clk_wr_i,
rdempty => rdempty,
wrempty => wrempty,
rdreq => rd_i,
aclr => aclr,
rdfull => rdfull,
wrfull => wrfull,
rdclk => clk_rd_i,
q => q_o,
wrreq => we_i,
data => d_i,
wrusedw => wrusedw,
rdusedw => rdusedw);
wr_count_o <= wrusedw;
rd_count_o <= rdusedw;
wr_empty_o <= wrempty;
rd_empty_o <= rdempty;
wr_full_o <= wrfull;
rd_full_o <= rdfull;
wr_almost_empty_o <= '1' when unsigned(wrusedw) < to_unsigned(g_almost_empty_threshold, wrusedw'length) else '0';
rd_almost_empty_o <= '1' when unsigned(rdusedw) < to_unsigned(g_almost_empty_threshold, rdusedw'length) else '0';
wr_almost_full_o <= '1' when unsigned(wrusedw) > to_unsigned(g_almost_full_threshold, wrusedw'length) else '0';
rd_almost_full_o <= '1' when unsigned(rdusedw) > to_unsigned(g_almost_full_threshold,rdusedw'length) else '0';
end syn;
-------------------------------------------------------------------------------
-- Title : Parametrizable dual-port synchronous RAM (Altera version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_dpram.vhd
-- Author : Wesley W. Terpstra
-- Company : GSI
-- Created : 2011-01-25
-- Last update: 2013-03-04
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: True dual-port synchronous RAM for Altera FPGAs with:
-- - configurable address and data bus width
-- - byte-addressing mode (data bus width restricted to multiple of 8 bits)
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-- Copyright (c) 2013 GSI
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-- 2012-03-13 1.1 wterpstra Added initial value as array
-- 2013-03-04 2.0 wterpstra Rewrote using altsyncram
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.genram_pkg.all;
library altera_mf;
use altera_mf.altera_mf_components.all;
entity generic_dpram is
generic(
-- standard parameters
g_data_width : natural;
g_size : natural;
g_with_byte_enable : boolean := false;
g_addr_conflict_resolution : string := "dont_care";
g_init_file : string := "none";
g_dual_clock : boolean := true);
port(
rst_n_i : in std_logic := '1'; -- synchronous reset, active LO
-- Port A
clka_i : in std_logic;
bwea_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
wea_i : in std_logic;
aa_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
da_i : in std_logic_vector(g_data_width-1 downto 0);
qa_o : out std_logic_vector(g_data_width-1 downto 0);
-- Port B
clkb_i : in std_logic;
bweb_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
web_i : in std_logic;
ab_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
db_i : in std_logic_vector(g_data_width-1 downto 0);
qb_o : out std_logic_vector(g_data_width-1 downto 0)
);
end generic_dpram;
architecture syn of generic_dpram is
function f_sameport_order(x : string) return string is
begin
if x = "read_first" then
return "OLD_DATA";
elsif x = "write_first" then
--return "NEW_DATA_NO_NBE_READ"; -- unwritten bytes='X'
return "NEW_DATA_WITH_NBE_READ"; -- unwritten bytes=OLD_DATA (ie: whole result = NEW_DATA)
elsif x = "dont_care" then
return "DONT_CARE";
else
assert (false) report "generic_dpram: g_addr_conflict_resolution must be: read_first, write_first, dont_care" severity failure;
return "DONT_CARE";
end if;
end f_sameport_order;
function f_diffport_order(x : string) return string is
begin
if x = "read_first" then
return "OLD_DATA";
elsif x = "write_first" then
return "DONT_CARE"; -- "NEW_DATA" is unsupported; we use a bypass MUX in this case
elsif x = "dont_care" then
return "DONT_CARE";
else
assert (false) report "generic_dpram: g_addr_conflict_resolution must be: read_first, write_first, dont_care" severity failure;
return "DONT_CARE";
end if;
end f_diffport_order;
function f_filename(x : string) return string is
begin
if x'length = 0 or x = "none" then
return "UNUSED";
else
return x;
end if;
end f_filename;
constant c_num_bytes : integer := (g_data_width+7)/8;
constant c_addr_width : integer := f_log2_size(g_size);
constant sameport_order : string := f_sameport_order(g_addr_conflict_resolution);
constant diffport_order : string := f_diffport_order(g_addr_conflict_resolution);
constant c_init_file : string := f_filename(g_init_file);
signal qa : std_logic_vector(g_data_width-1 downto 0);
signal qb : std_logic_vector(g_data_width-1 downto 0);
signal da : std_logic_vector(g_data_width-1 downto 0);
signal db : std_logic_vector(g_data_width-1 downto 0);
signal nba : boolean;
signal nbb : boolean;
begin
assert (g_addr_conflict_resolution /= "write_first" or (g_dual_clock = false and g_with_byte_enable = false))
report "generic_dpram: write_first is only possible when dual_clock and g_with_byte_enable are false"
severity failure;
assert (g_addr_conflict_resolution /= "read_first" or g_dual_clock = false)
report "generic_dpram: read_first is only possible when dual_clock is false"
severity failure;
assert (g_addr_conflict_resolution /= "write_first")
report "generic_dpram: write_first requires a bypass MUX"
severity note;
case_qb_raw : if (g_addr_conflict_resolution /= "write_first") generate
qa_o <= qa;
qb_o <= qb;
end generate;
case_qb_bypass : if (g_addr_conflict_resolution = "write_first") generate
qa_o <= qa when nba else db;
qb_o <= qb when nbb else da;
memoize : process(clka_i) is
begin
if rising_edge(clka_i) then
nba <= aa_i /= ab_i or web_i = '0';
nbb <= aa_i /= ab_i or wea_i = '0';
da <= da_i;
db <= db_i;
end if;
end process;
end generate;
case_be_dual : if (g_with_byte_enable = true and g_dual_clock = true) generate
memory : altsyncram
generic map(
byte_size => 8,
numwords_a => g_size,
numwords_b => g_size,
widthad_a => c_addr_width,
widthad_b => c_addr_width,
width_a => g_data_width,
width_b => g_data_width,
width_byteena_a => c_num_bytes,
width_byteena_b => c_num_bytes,
operation_mode => "BIDIR_DUAL_PORT",
read_during_write_mode_mixed_ports => diffport_order,
read_during_write_mode_port_a => sameport_order,
read_during_write_mode_port_b => sameport_order,
outdata_reg_a => "UNREGISTERED",
outdata_reg_b => "UNREGISTERED",
address_reg_b => "CLOCK1",
wrcontrol_wraddress_reg_b => "CLOCK1",
byteena_reg_b => "CLOCK1",
indata_reg_b => "CLOCK1",
rdcontrol_reg_b => "CLOCK1",
init_file => c_init_file)
port map(
clock0 => clka_i,
wren_a => wea_i,
address_a => aa_i,
data_a => da_i,
byteena_a => bwea_i,
q_a => qa,
clock1 => clkb_i,
wren_b => web_i,
address_b => ab_i,
data_b => db_i,
byteena_b => bweb_i,
q_b => qb);
end generate;
case_be_single : if (g_with_byte_enable = true and g_dual_clock = false) generate
memory : altsyncram
generic map(
byte_size => 8,
numwords_a => g_size,
numwords_b => g_size,
widthad_a => c_addr_width,
widthad_b => c_addr_width,
width_a => g_data_width,
width_b => g_data_width,
width_byteena_a => c_num_bytes,
width_byteena_b => c_num_bytes,
operation_mode => "BIDIR_DUAL_PORT",
read_during_write_mode_mixed_ports => diffport_order,
read_during_write_mode_port_a => sameport_order,
read_during_write_mode_port_b => sameport_order,
outdata_reg_a => "UNREGISTERED",
outdata_reg_b => "UNREGISTERED",
address_reg_b => "CLOCK0",
wrcontrol_wraddress_reg_b => "CLOCK0",
byteena_reg_b => "CLOCK0",
indata_reg_b => "CLOCK0",
rdcontrol_reg_b => "CLOCK0",
init_file => c_init_file)
port map(
clock0 => clka_i,
wren_a => wea_i,
address_a => aa_i,
data_a => da_i,
byteena_a => bwea_i,
q_a => qa,
wren_b => web_i,
address_b => ab_i,
data_b => db_i,
byteena_b => bweb_i,
q_b => qb);
end generate;
case_nobe_dual : if (g_with_byte_enable = false and g_dual_clock = true) generate
memory : altsyncram
generic map(
byte_size => 8,
numwords_a => g_size,
numwords_b => g_size,
widthad_a => c_addr_width,
widthad_b => c_addr_width,
width_a => g_data_width,
width_b => g_data_width,
operation_mode => "BIDIR_DUAL_PORT",
read_during_write_mode_mixed_ports => diffport_order,
read_during_write_mode_port_a => sameport_order,
read_during_write_mode_port_b => sameport_order,
outdata_reg_a => "UNREGISTERED",
outdata_reg_b => "UNREGISTERED",
address_reg_b => "CLOCK1",
wrcontrol_wraddress_reg_b => "CLOCK1",
byteena_reg_b => "CLOCK1",
indata_reg_b => "CLOCK1",
rdcontrol_reg_b => "CLOCK1",
init_file => c_init_file)
port map(
clock0 => clka_i,
wren_a => wea_i,
address_a => aa_i,
data_a => da_i,
q_a => qa,
clock1 => clkb_i,
wren_b => web_i,
address_b => ab_i,
data_b => db_i,
q_b => qb);
end generate;
case_nobe_single : if (g_with_byte_enable = false and g_dual_clock = false) generate
memory : altsyncram
generic map(
byte_size => 8,
numwords_a => g_size,
numwords_b => g_size,
widthad_a => c_addr_width,
widthad_b => c_addr_width,
width_a => g_data_width,
width_b => g_data_width,
operation_mode => "BIDIR_DUAL_PORT",
read_during_write_mode_mixed_ports => diffport_order,
read_during_write_mode_port_a => sameport_order,
read_during_write_mode_port_b => sameport_order,
outdata_reg_a => "UNREGISTERED",
outdata_reg_b => "UNREGISTERED",
address_reg_b => "CLOCK0",
wrcontrol_wraddress_reg_b => "CLOCK0",
byteena_reg_b => "CLOCK0",
indata_reg_b => "CLOCK0",
rdcontrol_reg_b => "CLOCK0",
init_file => c_init_file)
port map(
clock0 => clka_i,
wren_a => wea_i,
address_a => aa_i,
data_a => da_i,
q_a => qa,
wren_b => web_i,
address_b => ab_i,
data_b => db_i,
q_b => qb);
end generate;
end syn;
-------------------------------------------------------------------------------
-- Title : Parametrizable dual-port synchronous RAM (Altera version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_simple_dpram.vhd
-- Author : Wesley W. Terpstra
-- Company : GSI
-- Created : 2013-03-04
-- Last update: 2013-03-04
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Simple dual-port synchronous RAM for Altera FPGAs with:
-- - configurable address and data bus width
-- - byte-addressing mode (data bus width restricted to multiple of 8 bits)
-------------------------------------------------------------------------------
-- Copyright (c) 2013 GSI
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2013-03-04 1.0 wterpstra Adapted from generic_dpram.vhd
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.genram_pkg.all;
library altera_mf;
use altera_mf.altera_mf_components.all;
entity generic_simple_dpram is
generic(
g_data_width : natural;
g_size : natural;
g_with_byte_enable : boolean := false;
g_addr_conflict_resolution : string := "dont_care";
g_init_file : string := "none";
g_dual_clock : boolean := true);
port(
rst_n_i : in std_logic := '1'; -- synchronous reset, active LO
-- Port A
clka_i : in std_logic;
bwea_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
wea_i : in std_logic;
aa_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
da_i : in std_logic_vector(g_data_width-1 downto 0);
-- Port B
clkb_i : in std_logic;
ab_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
qb_o : out std_logic_vector(g_data_width-1 downto 0)
);
end generic_simple_dpram;
architecture syn of generic_simple_dpram is
function f_diffport_order(x : string) return string is
begin
if x = "read_first" then
return "OLD_DATA";
elsif x = "write_first" then
return "DONT_CARE"; -- "NEW_DATA" is unsupported; we use a bypass MUX in this case
elsif x = "dont_care" then
return "DONT_CARE";
else
assert (false) report "generic_simple_dpram: g_addr_conflict_resolution must be: read_first, write_first, dont_care" severity failure;
return "DONT_CARE";
end if;
end f_diffport_order;
function f_filename(x : string) return string is
begin
if x'length = 0 or x = "none" then
return "UNUSED";
else
return x;
end if;
end f_filename;
constant c_num_bytes : integer := (g_data_width+7)/8;
constant c_addr_width : integer := f_log2_size(g_size);
constant diffport_order : string := f_diffport_order(g_addr_conflict_resolution);
constant c_init_file : string := f_filename(g_init_file);
signal qb : std_logic_vector(g_data_width-1 downto 0);
signal da : std_logic_vector(g_data_width-1 downto 0);
signal nbb : boolean;
begin
assert (g_addr_conflict_resolution /= "write_first" or (g_dual_clock = false and g_with_byte_enable = false))
report "generic_simple_dpram: write_first is only possible when dual_clock and g_with_byte_enable are false"
severity failure;
assert (g_addr_conflict_resolution /= "read_first" or g_dual_clock = false)
report "generic_simple_dpram: read_first is only possible when dual_clock is false"
severity failure;
assert (g_addr_conflict_resolution /= "write_first")
report "generic_simple_dpram: write_first requires a bypass MUX"
severity note;
case_qb_raw : if (g_addr_conflict_resolution /= "write_first") generate
qb_o <= qb;
end generate;
case_qb_bypass : if (g_addr_conflict_resolution = "write_first") generate
qb_o <= qb when nbb else da;
memoize : process(clka_i) is
begin
if rising_edge(clka_i) then
nbb <= aa_i /= ab_i or wea_i = '0';
da <= da_i;
end if;
end process;
end generate;
case_be_dual : if (g_with_byte_enable = true and g_dual_clock = true) generate
memory : altsyncram
generic map(
byte_size => 8,
numwords_a => g_size,
numwords_b => g_size,
widthad_a => c_addr_width,
widthad_b => c_addr_width,
width_a => g_data_width,
width_b => g_data_width,
width_byteena_a => c_num_bytes,
operation_mode => "DUAL_PORT",
read_during_write_mode_mixed_ports => diffport_order,
outdata_reg_a => "UNREGISTERED",
outdata_reg_b => "UNREGISTERED",
address_reg_b => "CLOCK1",
wrcontrol_wraddress_reg_b => "CLOCK1",
byteena_reg_b => "CLOCK1",
indata_reg_b => "CLOCK1",
rdcontrol_reg_b => "CLOCK1",
init_file => c_init_file)
port map(
clock0 => clka_i,
wren_a => wea_i,
address_a => aa_i,
data_a => da_i,
byteena_a => bwea_i,
clock1 => clkb_i,
address_b => ab_i,
q_b => qb);
end generate;
case_be_single : if (g_with_byte_enable = true and g_dual_clock = false) generate
memory : altsyncram
generic map(
byte_size => 8,
numwords_a => g_size,
numwords_b => g_size,
widthad_a => c_addr_width,
widthad_b => c_addr_width,
width_a => g_data_width,
width_b => g_data_width,
width_byteena_a => c_num_bytes,
operation_mode => "DUAL_PORT",
read_during_write_mode_mixed_ports => diffport_order,
outdata_reg_a => "UNREGISTERED",
outdata_reg_b => "UNREGISTERED",
address_reg_b => "CLOCK0",
wrcontrol_wraddress_reg_b => "CLOCK0",
byteena_reg_b => "CLOCK0",
indata_reg_b => "CLOCK0",
rdcontrol_reg_b => "CLOCK0",
init_file => c_init_file)
port map(
clock0 => clka_i,
wren_a => wea_i,
address_a => aa_i,
data_a => da_i,
byteena_a => bwea_i,
address_b => ab_i,
q_b => qb);
end generate;
case_nobe_dual : if (g_with_byte_enable = false and g_dual_clock = true) generate
memory : altsyncram
generic map(
byte_size => 8,
numwords_a => g_size,
numwords_b => g_size,
widthad_a => c_addr_width,
widthad_b => c_addr_width,
width_a => g_data_width,
width_b => g_data_width,
operation_mode => "DUAL_PORT",
read_during_write_mode_mixed_ports => diffport_order,
outdata_reg_a => "UNREGISTERED",
outdata_reg_b => "UNREGISTERED",
address_reg_b => "CLOCK1",
wrcontrol_wraddress_reg_b => "CLOCK1",
byteena_reg_b => "CLOCK1",
indata_reg_b => "CLOCK1",
rdcontrol_reg_b => "CLOCK1",
init_file => c_init_file)
port map(
clock0 => clka_i,
wren_a => wea_i,
address_a => aa_i,
data_a => da_i,
clock1 => clkb_i,
address_b => ab_i,
q_b => qb);
end generate;
case_nobe_single : if (g_with_byte_enable = false and g_dual_clock = false) generate
memory : altsyncram
generic map(
byte_size => 8,
numwords_a => g_size,
numwords_b => g_size,
widthad_a => c_addr_width,
widthad_b => c_addr_width,
width_a => g_data_width,
width_b => g_data_width,
operation_mode => "DUAL_PORT",
read_during_write_mode_mixed_ports => diffport_order,
outdata_reg_a => "UNREGISTERED",
outdata_reg_b => "UNREGISTERED",
address_reg_b => "CLOCK0",
wrcontrol_wraddress_reg_b => "CLOCK0",
byteena_reg_b => "CLOCK0",
indata_reg_b => "CLOCK0",
rdcontrol_reg_b => "CLOCK0",
init_file => c_init_file)
port map(
clock0 => clka_i,
wren_a => wea_i,
address_a => aa_i,
data_a => da_i,
address_b => ab_i,
q_b => qb);
end generate;
end syn;
-------------------------------------------------------------------------------
-- Title : Parametrizable single-port synchronous RAM (Altera version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_spram.vhd
-- Author : Wesley W. Terpstra
-- Company : GSI
-- Created : 2011-01-25
-- Last update: 2013-03-04
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Single-port synchronous RAM for Altera FPGAs with:
-- - configurable address and data bus width
-- - byte-addressing mode (data bus width restricted to multiple of 8 bits)
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-- 2013-03-04 2.0 wterpstra Rewrote using altsyncram
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.genram_pkg.all;
library altera_mf;
use altera_mf.altera_mf_components.all;
entity generic_spram is
generic(
g_data_width : natural := 32;
g_size : natural := 1024;
g_with_byte_enable : boolean := false;
g_addr_conflict_resolution : string := "dont_care";
g_init_file : string := "");
port(
rst_n_i : in std_logic := '1';
clk_i : in std_logic;
bwe_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
we_i : in std_logic;
a_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
d_i : in std_logic_vector(g_data_width-1 downto 0);
q_o : out std_logic_vector(g_data_width-1 downto 0));
end generic_spram;
architecture syn of generic_spram is
function f_sameport_order(x : string) return string is
begin
if x = "read_first" then
return "OLD_DATA";
elsif x = "write_first" then
--return "NEW_DATA_NO_NBE_READ"; -- unwritten bytes='X'
return "NEW_DATA_WITH_NBE_READ"; -- unwritten bytes=OLD_DATA (ie: whole result = NEW_DATA)
elsif x = "dont_care" then
return "DONT_CARE";
else
assert (false) report "generic_spram: g_addr_conflict_resolution must be: read_first, write_first, dont_care" severity failure;
return "DONT_CARE";
end if;
end f_sameport_order;
function f_filename(x : string) return string is
begin
if x'length = 0 or x = "none" then
return "UNUSED";
else
return x;
end if;
end f_filename;
constant c_num_bytes : integer := (g_data_width+7)/8;
constant c_addr_width : integer := f_log2_size(g_size);
constant sameport_order : string := f_sameport_order(g_addr_conflict_resolution);
constant c_init_file : string := f_filename(g_init_file);
begin
case_be : if (g_with_byte_enable = true) generate
memory : altsyncram
generic map(
byte_size => 8,
numwords_a => g_size,
widthad_a => c_addr_width,
width_a => g_data_width,
width_byteena_a => c_num_bytes,
operation_mode => "SINGLE_PORT",
read_during_write_mode_port_a => sameport_order,
outdata_reg_a => "UNREGISTERED",
init_file => c_init_file)
port map(
clock0 => clk_i,
wren_a => we_i,
address_a => a_i,
data_a => d_i,
byteena_a => bwe_i,
q_a => q_o);
end generate;
case_nobe : if (g_with_byte_enable = false) generate
memory : altsyncram
generic map(
byte_size => 8,
numwords_a => g_size,
widthad_a => c_addr_width,
width_a => g_data_width,
operation_mode => "SINGLE_PORT",
read_during_write_mode_port_a => sameport_order,
outdata_reg_a => "UNREGISTERED",
init_file => c_init_file)
port map(
clock0 => clk_i,
wren_a => we_i,
address_a => a_i,
data_a => d_i,
q_a => q_o);
end generate;
end syn;
-------------------------------------------------------------------------------
-- Title : Parametrizable synchronous FIFO (Altera version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_sync_fifo.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-CO-HT
-- Created : 2011-01-25
-- Last update: 2011-01-25
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Single-clock FIFO.
-- - configurable data width and size
-- - "show ahead" mode
-- - configurable full/empty/almost full/almost empty/word count signals
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library altera_mf;
use altera_mf.all;
use work.genram_pkg.all;
entity generic_sync_fifo is
generic (
g_data_width : natural;
g_size : natural;
g_show_ahead : boolean := false;
-- Read-side flag selection
g_with_empty : boolean := true; -- with empty flag
g_with_full : boolean := true; -- with full flag
g_with_almost_empty : boolean := false;
g_with_almost_full : boolean := false;
g_with_count : boolean := false; -- with words counter
g_almost_empty_threshold : integer; -- threshold for almost empty flag
g_almost_full_threshold : integer -- threshold for almost full flag
);
port (
rst_n_i : in std_logic := '1';
clk_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
empty_o : out std_logic;
full_o : out std_logic;
almost_empty_o : out std_logic;
almost_full_o : out std_logic;
count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0)
);
end generic_sync_fifo;
architecture syn of generic_sync_fifo is
component scfifo
generic (
add_ram_output_register : string;
almost_empty_value : natural;
almost_full_value : natural;
lpm_numwords : natural;
lpm_showahead : string;
lpm_type : string;
lpm_width : natural;
lpm_widthu : natural;
overflow_checking : string;
underflow_checking : string;
use_eab : string);
port (
clock : in std_logic;
sclr : in std_logic;
usedw : out std_logic_vector (f_log2_size(g_size)-1 downto 0);
empty : out std_logic;
full : out std_logic;
q : out std_logic_vector (g_data_width-1 downto 0);
wrreq : in std_logic;
almost_empty : out std_logic;
almost_full : out std_logic;
data : in std_logic_vector (g_data_width-1 downto 0);
rdreq : in std_logic);
end component;
function f_bool_2_string (x : boolean) return string is
begin
if(x) then
return "ON";
else
return "OFF";
end if;
end f_bool_2_string;
signal empty : std_logic;
signal almost_empty : std_logic;
signal almost_full : std_logic;
signal sclr : std_logic;
signal full : std_logic;
signal usedw : std_logic_vector (f_log2_size(g_size)-1 downto 0);
begin -- syn
sclr <= not rst_n_i;
scfifo_inst: scfifo
generic map (
add_ram_output_register => "OFF",
almost_empty_value => g_almost_empty_threshold,
almost_full_value => g_almost_full_threshold,
lpm_numwords => g_size,
lpm_showahead => f_bool_2_string(g_show_ahead),
lpm_type => "scfifo",
lpm_width => g_data_width,
lpm_widthu => f_log2_size(g_size),
overflow_checking => "ON",
underflow_checking => "ON",
use_eab => "ON" )
port map (
clock => clk_i,
sclr => sclr,
usedw => usedw,
empty => empty,
full => full,
q => q_o,
wrreq => we_i,
almost_empty => almost_empty,
almost_full => almost_full,
data => d_i,
rdreq => rd_i);
count_o <= usedw;
empty_o <= empty;
full_o <= full;
almost_empty_o <= almost_empty;
almost_full_o <= almost_full;
end syn;
files = ["generic_async_fifo.vhd",
"generic_sync_fifo.vhd"]
-------------------------------------------------------------------------------
-- Title : Parametrizable asynchronous FIFO (Generic version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_async_fifo.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-CO-HT
-- Created : 2011-01-25
-- Last update: 2012-07-03
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Dual-clock asynchronous FIFO.
-- - configurable data width and size
-- - configurable full/empty/almost full/almost empty/word count signals
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.genram_pkg.all;
entity generic_async_fifo is
generic (
g_data_width : natural;
g_size : natural;
g_show_ahead : boolean := false;
-- Read-side flag selection
g_with_rd_empty : boolean := true; -- with empty flag
g_with_rd_full : boolean := false; -- with full flag
g_with_rd_almost_empty : boolean := false;
g_with_rd_almost_full : boolean := false;
g_with_rd_count : boolean := false; -- with words counter
g_with_wr_empty : boolean := false;
g_with_wr_full : boolean := true;
g_with_wr_almost_empty : boolean := false;
g_with_wr_almost_full : boolean := false;
g_with_wr_count : boolean := false;
g_almost_empty_threshold : integer; -- threshold for almost empty flag
g_almost_full_threshold : integer -- threshold for almost full flag
);
port (
rst_n_i : in std_logic := '1';
-- write port
clk_wr_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
wr_empty_o : out std_logic;
wr_full_o : out std_logic;
wr_almost_empty_o : out std_logic;
wr_almost_full_o : out std_logic;
wr_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0);
-- read port
clk_rd_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
rd_empty_o : out std_logic;
rd_full_o : out std_logic;
rd_almost_empty_o : out std_logic;
rd_almost_full_o : out std_logic;
rd_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0)
);
end generic_async_fifo;
architecture syn of generic_async_fifo is
component inferred_async_fifo
generic (
g_data_width : natural;
g_size : natural;
g_show_ahead : boolean;
g_with_rd_empty : boolean;
g_with_rd_full : boolean;
g_with_rd_almost_empty : boolean;
g_with_rd_almost_full : boolean;
g_with_rd_count : boolean;
g_with_wr_empty : boolean;
g_with_wr_full : boolean;
g_with_wr_almost_empty : boolean;
g_with_wr_almost_full : boolean;
g_with_wr_count : boolean;
g_almost_empty_threshold : integer;
g_almost_full_threshold : integer);
port (
rst_n_i : in std_logic := '1';
clk_wr_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
wr_empty_o : out std_logic;
wr_full_o : out std_logic;
wr_almost_empty_o : out std_logic;
wr_almost_full_o : out std_logic;
wr_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0);
clk_rd_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
rd_empty_o : out std_logic;
rd_full_o : out std_logic;
rd_almost_empty_o : out std_logic;
rd_almost_full_o : out std_logic;
rd_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0));
end component;
begin -- syn
U_Inferred_FIFO : inferred_async_fifo
generic map (
g_data_width => g_data_width,
g_size => g_size,
g_show_ahead => g_show_ahead,
g_with_rd_empty => g_with_rd_empty,
g_with_rd_full => g_with_rd_full,
g_with_rd_almost_empty => g_with_rd_almost_empty,
g_with_rd_almost_full => g_with_rd_almost_full,
g_with_rd_count => g_with_rd_count,
g_with_wr_empty => g_with_wr_empty,
g_with_wr_full => g_with_wr_full,
g_with_wr_almost_empty => g_with_wr_almost_empty,
g_with_wr_almost_full => g_with_wr_almost_full,
g_with_wr_count => g_with_wr_count,
g_almost_empty_threshold => g_almost_empty_threshold,
g_almost_full_threshold => g_almost_full_threshold)
port map (
rst_n_i => rst_n_i,
clk_wr_i => clk_wr_i,
d_i => d_i,
we_i => we_i,
wr_empty_o => wr_empty_o,
wr_full_o => wr_full_o,
wr_almost_empty_o => wr_almost_empty_o,
wr_almost_full_o => wr_almost_full_o,
wr_count_o => wr_count_o,
clk_rd_i => clk_rd_i,
q_o => q_o,
rd_i => rd_i,
rd_empty_o => rd_empty_o,
rd_full_o => rd_full_o,
rd_almost_empty_o => rd_almost_empty_o,
rd_almost_full_o => rd_almost_full_o,
rd_count_o => rd_count_o);
end syn;
-------------------------------------------------------------------------------
-- Title : Parametrizable synchronous FIFO (Xilinx version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_sync_fifo.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-CO-HT
-- Created : 2011-01-25
-- Last update: 2012-07-03
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Single-clock FIFO.
-- - configurable data width and size
-- - "show ahead" mode
-- - configurable full/empty/almost full/almost empty/word count signals
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.genram_pkg.all;
entity generic_sync_fifo is
generic (
g_data_width : natural;
g_size : natural;
g_show_ahead : boolean := false;
-- Read-side flag selection
g_with_empty : boolean := true; -- with empty flag
g_with_full : boolean := true; -- with full flag
g_with_almost_empty : boolean := false;
g_with_almost_full : boolean := false;
g_with_count : boolean := false; -- with words counter
g_almost_empty_threshold : integer; -- threshold for almost empty flag
g_almost_full_threshold : integer; -- threshold for almost full flag
g_register_flag_outputs : boolean := true
);
port (
rst_n_i : in std_logic := '1';
clk_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
empty_o : out std_logic;
full_o : out std_logic;
almost_empty_o : out std_logic;
almost_full_o : out std_logic;
count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0)
);
end generic_sync_fifo;
architecture syn of generic_sync_fifo is
component inferred_sync_fifo
generic (
g_data_width : natural;
g_size : natural;
g_show_ahead : boolean;
g_with_empty : boolean;
g_with_full : boolean;
g_with_almost_empty : boolean;
g_with_almost_full : boolean;
g_with_count : boolean;
g_almost_empty_threshold : integer;
g_almost_full_threshold : integer;
g_register_flag_outputs : boolean);
port (
rst_n_i : in std_logic := '1';
clk_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
empty_o : out std_logic;
full_o : out std_logic;
almost_empty_o : out std_logic;
almost_full_o : out std_logic;
count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0));
end component;
begin -- syn
U_Inferred_FIFO : inferred_sync_fifo
generic map (
g_data_width => g_data_width,
g_size => g_size,
g_show_ahead => g_show_ahead,
g_with_empty => g_with_empty,
g_with_full => g_with_full,
g_with_almost_empty => g_with_almost_empty,
g_with_almost_full => g_with_almost_full,
g_with_count => g_with_count,
g_almost_empty_threshold => g_almost_empty_threshold,
g_almost_full_threshold => g_almost_full_threshold,
g_register_flag_outputs => g_register_flag_outputs)
port map (
rst_n_i => rst_n_i,
clk_i => clk_i,
d_i => d_i,
we_i => we_i,
q_o => q_o,
rd_i => rd_i,
empty_o => empty_o,
full_o => full_o,
almost_empty_o => almost_empty_o,
almost_full_o => almost_full_o,
count_o => count_o);
end syn;
files = [
"generic_dpram.vhd",
"generic_dpram_sameclock.vhd",
"generic_dpram_dualclock.vhd",
"generic_simple_dpram.vhd",
"generic_spram.vhd",
"gc_shiftreg.vhd"]
library ieee;
use ieee.STD_LOGIC_1164.all;
use ieee.NUMERIC_STD.all;
use work.genram_pkg.all;
entity gc_shiftreg is
generic (
g_size : integer);
port (
clk_i : in std_logic;
en_i : in std_logic;
d_i : in std_logic;
q_o : out std_logic;
a_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0));
end gc_shiftreg;
architecture wrapper of gc_shiftreg is
component SRLC32E
port (
Q : out std_ulogic;
A : in std_logic_vector (4 downto 0);
CE : in std_ulogic;
CLK : in std_ulogic;
D : in std_ulogic);
end component;
signal a : std_logic_vector(4 downto 0);
signal sr : std_logic_vector(g_size-1 downto 0);
begin -- wrapper
assert (g_size <= 32) report "gc_shiftreg[xilinx]: forced SRL32 implementation can be done only for 32-bit or smaller shift registers" severity warning;
a <= std_logic_vector(resize(unsigned(a_i), 5));
gen_srl32 : if(g_size <= 32) generate
U_SRLC32 : SRLC32E
port map (
Q => q_o,
A => a,
CE => en_i,
CLK => clk_i,
D => d_i);
end generate gen_srl32;
gen_inferred : if(g_size > 32) generate
p_srl : process(clk_i)
begin
if rising_edge(clk_i) then
if en_i = '1' then
sr <= sr(sr'left - 1 downto 0) & d_i;
end if;
end if;
end process;
q_o <= sr(TO_INTEGER(unsigned(a_i)));
end generate gen_inferred;
end wrapper;
-------------------------------------------------------------------------------
-- Title : Parametrizable dual-port synchronous RAM (Xilinx version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_dpram.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-CO-HT
-- Created : 2011-01-25
-- Last update: 2012-03-16
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: True dual-port synchronous RAM for Xilinx FPGAs with:
-- - configurable address and data bus width
-- - byte-addressing mode (data bus width restricted to multiple of 8 bits)
-- Todo:
-- - loading initial contents from file
-- - add support for read-first/write-first address conflict resulution (only
-- supported by Xilinx in VHDL templates)
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-- 2012-03-13 1.1 wterpstra Added initial value as array
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use std.textio.all;
library work;
use work.genram_pkg.all;
use work.memory_loader_pkg.all;
entity generic_dpram is
generic (
-- standard parameters
g_data_width : natural := 32;
g_size : natural := 16384;
g_with_byte_enable : boolean := false;
g_addr_conflict_resolution : string := "read_first";
g_init_file : string := "";
g_dual_clock : boolean := true;
g_fail_if_file_not_found : boolean := true
);
port (
rst_n_i : in std_logic := '1'; -- synchronous reset, active LO
-- Port A
clka_i : in std_logic;
bwea_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
wea_i : in std_logic;
aa_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
da_i : in std_logic_vector(g_data_width-1 downto 0);
qa_o : out std_logic_vector(g_data_width-1 downto 0);
-- Port B
clkb_i : in std_logic;
bweb_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
web_i : in std_logic;
ab_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
db_i : in std_logic_vector(g_data_width-1 downto 0);
qb_o : out std_logic_vector(g_data_width-1 downto 0)
);
end generic_dpram;
architecture syn of generic_dpram is
component generic_dpram_sameclock
generic (
g_data_width : natural;
g_size : natural;
g_with_byte_enable : boolean;
g_addr_conflict_resolution : string;
g_init_file : string;
g_fail_if_file_not_found : boolean);
port (
rst_n_i : in std_logic := '1';
clk_i : in std_logic;
bwea_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
wea_i : in std_logic;
aa_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
da_i : in std_logic_vector(g_data_width-1 downto 0);
qa_o : out std_logic_vector(g_data_width-1 downto 0);
bweb_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
web_i : in std_logic;
ab_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
db_i : in std_logic_vector(g_data_width-1 downto 0);
qb_o : out std_logic_vector(g_data_width-1 downto 0));
end component;
component generic_dpram_dualclock
generic (
g_data_width : natural;
g_size : natural;
g_with_byte_enable : boolean;
g_addr_conflict_resolution : string;
g_init_file : string;
g_fail_if_file_not_found : boolean);
port (
rst_n_i : in std_logic := '1';
clka_i : in std_logic;
bwea_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
wea_i : in std_logic;
aa_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
da_i : in std_logic_vector(g_data_width-1 downto 0);
qa_o : out std_logic_vector(g_data_width-1 downto 0);
clkb_i : in std_logic;
bweb_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
web_i : in std_logic;
ab_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
db_i : in std_logic_vector(g_data_width-1 downto 0);
qb_o : out std_logic_vector(g_data_width-1 downto 0));
end component;
begin
gen_single_clk : if(g_dual_clock = false) generate
U_RAM_SC: generic_dpram_sameclock
generic map (
g_data_width => g_data_width,
g_size => g_size,
g_with_byte_enable => g_with_byte_enable,
g_addr_conflict_resolution => g_addr_conflict_resolution,
g_init_file => g_init_file,
g_fail_if_file_not_found => g_fail_if_file_not_found)
port map (
rst_n_i => rst_n_i,
clk_i => clka_i,
bwea_i => bwea_i,
wea_i => wea_i,
aa_i => aa_i,
da_i => da_i,
qa_o => qa_o,
bweb_i => bweb_i,
web_i => web_i,
ab_i => ab_i,
db_i => db_i,
qb_o => qb_o);
end generate gen_single_clk;
gen_dual_clk : if(g_dual_clock = true) generate
U_RAM_DC: generic_dpram_dualclock
generic map (
g_data_width => g_data_width,
g_size => g_size,
g_with_byte_enable => g_with_byte_enable,
g_addr_conflict_resolution => g_addr_conflict_resolution,
g_init_file => g_init_file,
g_fail_if_file_not_found => g_fail_if_file_not_found)
port map (
rst_n_i => rst_n_i,
clka_i => clka_i,
bwea_i => bwea_i,
wea_i => wea_i,
aa_i => aa_i,
da_i => da_i,
qa_o => qa_o,
clkb_i => clkb_i,
bweb_i => bweb_i,
web_i => web_i,
ab_i => ab_i,
db_i => db_i,
qb_o => qb_o);
end generate gen_dual_clk;
end syn;
-------------------------------------------------------------------------------
-- Title : Parametrizable dual-port synchronous RAM (Xilinx version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_dpram.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-CO-HT
-- Created : 2011-01-25
-- Last update: 2012-03-28
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: True dual-port synchronous RAM for Xilinx FPGAs with:
-- - configurable address and data bus width
-- - byte-addressing mode (data bus width restricted to multiple of 8 bits)
-- Todo:
-- - loading initial contents from file
-- - add support for read-first/write-first address conflict resulution (only
-- supported by Xilinx in VHDL templates)
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use std.textio.all;
library work;
use work.genram_pkg.all;
use work.memory_loader_pkg.all;
entity generic_dpram_dualclock is
generic (
-- standard parameters
g_data_width : natural := 32;
g_size : natural := 16384;
g_with_byte_enable : boolean := false;
g_addr_conflict_resolution : string := "read_first";
g_init_file : string := "";
g_fail_if_file_not_found : boolean := true
);
port (
rst_n_i : in std_logic := '1'; -- synchronous reset, active LO
-- Port A
clka_i : in std_logic;
bwea_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
wea_i : in std_logic;
aa_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
da_i : in std_logic_vector(g_data_width-1 downto 0);
qa_o : out std_logic_vector(g_data_width-1 downto 0);
-- Port B
clkb_i : in std_logic;
bweb_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
web_i : in std_logic;
ab_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
db_i : in std_logic_vector(g_data_width-1 downto 0);
qb_o : out std_logic_vector(g_data_width-1 downto 0)
);
end generic_dpram_dualclock;
architecture syn of generic_dpram_dualclock is
constant c_num_bytes : integer := (g_data_width+7)/8;
type t_ram_type is array(0 to g_size-1) of std_logic_vector(g_data_width-1 downto 0);
function f_memarray_to_ramtype(arr : t_meminit_array) return t_ram_type is
variable tmp : t_ram_type;
variable n, pos : integer;
begin
pos := 0;
while(pos < g_size)loop
n := 0;
-- avoid ISE loop iteration limit
while (pos < g_size and n < 4096) loop
for i in 0 to g_data_width-1 loop
tmp(pos)(i) := arr(pos, i);
end loop; -- i
n := n+1;
pos := pos + 1;
end loop;
end loop;
return tmp;
end f_memarray_to_ramtype;
function f_file_contents return t_meminit_array is
begin
return f_load_mem_from_file(g_init_file, g_size, g_data_width, g_fail_if_file_not_found);
end f_file_contents;
shared variable ram : t_ram_type := f_memarray_to_ramtype(f_file_contents);
signal s_we_a : std_logic_vector(c_num_bytes-1 downto 0);
signal s_ram_in_a : std_logic_vector(g_data_width-1 downto 0);
signal s_we_b : std_logic_vector(c_num_bytes-1 downto 0);
signal s_ram_in_b : std_logic_vector(g_data_width-1 downto 0);
signal clka_int : std_logic;
signal clkb_int : std_logic;
signal wea_rep, web_rep : std_logic_vector(c_num_bytes-1 downto 0);
begin
wea_rep <= (others => wea_i);
web_rep <= (others => web_i);
s_we_a <= bwea_i and wea_rep;
s_we_b <= bweb_i and web_rep;
gen_with_byte_enable_readfirst : if(g_with_byte_enable = true and (g_addr_conflict_resolution = "read_first" or
g_addr_conflict_resolution = "dont_care")) generate
process (clka_i)
begin
if rising_edge(clka_i) then
qa_o <= ram(to_integer(unsigned(aa_i)));
for i in 0 to c_num_bytes-1 loop
if s_we_a(i) = '1' then
ram(to_integer(unsigned(aa_i)))((i+1)*8-1 downto i*8) := da_i((i+1)*8-1 downto i*8);
end if;
end loop;
end if;
end process;
process (clkb_i)
begin
if rising_edge(clkb_i) then
qb_o <= ram(to_integer(unsigned(ab_i)));
for i in 0 to c_num_bytes-1 loop
if s_we_b(i) = '1' then
ram(to_integer(unsigned(ab_i)))((i+1)*8-1 downto i*8)
:= db_i((i+1)*8-1 downto i*8);
end if;
end loop;
end if;
end process;
end generate gen_with_byte_enable_readfirst;
gen_without_byte_enable_readfirst : if(g_with_byte_enable = false and (g_addr_conflict_resolution = "read_first" or
g_addr_conflict_resolution = "dont_care")) generate
process(clka_i)
begin
if rising_edge(clka_i) then
qa_o <= ram(to_integer(unsigned(aa_i)));
if(wea_i = '1') then
ram(to_integer(unsigned(aa_i))) := da_i;
end if;
end if;
end process;
process(clkb_i)
begin
if rising_edge(clkb_i) then
qb_o <= ram(to_integer(unsigned(ab_i)));
if(web_i = '1') then
ram(to_integer(unsigned(ab_i))) := db_i;
end if;
end if;
end process;
end generate gen_without_byte_enable_readfirst;
gen_without_byte_enable_writefirst : if(g_with_byte_enable = false and g_addr_conflict_resolution = "write_first") generate
process(clka_i)
begin
if rising_edge(clka_i) then
if(wea_i = '1') then
ram(to_integer(unsigned(aa_i))) := da_i;
qa_o <= da_i;
else
qa_o <= ram(to_integer(unsigned(aa_i)));
end if;
end if;
end process;
process(clkb_i)
begin
if rising_edge(clkb_i) then
if(web_i = '1') then
ram(to_integer(unsigned(ab_i))) := db_i;
qb_o <= db_i;
else
qb_o <= ram(to_integer(unsigned(ab_i)));
end if;
end if;
end process;
end generate gen_without_byte_enable_writefirst;
gen_without_byte_enable_nochange : if(g_with_byte_enable = false and g_addr_conflict_resolution = "no_change") generate
process(clka_i)
begin
if rising_edge(clka_i) then
if(wea_i = '1') then
ram(to_integer(unsigned(aa_i))) := da_i;
else
qa_o <= ram(to_integer(unsigned(aa_i)));
end if;
end if;
end process;
process(clkb_i)
begin
if rising_edge(clkb_i) then
if(web_i = '1') then
ram(to_integer(unsigned(ab_i))) := db_i;
else
qb_o <= ram(to_integer(unsigned(ab_i)));
end if;
end if;
end process;
end generate gen_without_byte_enable_nochange;
end syn;
-------------------------------------------------------------------------------
-- Title : Parametrizable dual-port synchronous RAM (Xilinx version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_dpram_sameclock.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-CO-HT
-- Created : 2011-01-25
-- Last update: 2012-07-09
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: True dual-port synchronous RAM for Xilinx FPGAs with:
-- - configurable address and data bus width
-- - byte-addressing mode (data bus width restricted to multiple of 8 bits)
-- Todo:
-- - loading initial contents from file
-- - add support for read-first/write-first address conflict resulution (only
-- supported by Xilinx in VHDL templates)
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use std.textio.all;
library work;
use work.genram_pkg.all;
use work.memory_loader_pkg.all;
entity generic_dpram_sameclock is
generic (
g_data_width : natural := 32;
g_size : natural := 16384;
g_with_byte_enable : boolean := false;
g_addr_conflict_resolution : string := "read_first";
g_init_file : string := "";
g_fail_if_file_not_found : boolean := true
);
port (
rst_n_i : in std_logic := '1'; -- synchronous reset, active LO
-- Port A
clk_i : in std_logic;
bwea_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
wea_i : in std_logic;
aa_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
da_i : in std_logic_vector(g_data_width-1 downto 0);
qa_o : out std_logic_vector(g_data_width-1 downto 0);
-- Port B
bweb_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
web_i : in std_logic;
ab_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
db_i : in std_logic_vector(g_data_width-1 downto 0);
qb_o : out std_logic_vector(g_data_width-1 downto 0)
);
end generic_dpram_sameclock;
architecture syn of generic_dpram_sameclock is
constant c_num_bytes : integer := (g_data_width+7)/8;
type t_ram_type is array(0 to g_size-1) of std_logic_vector(g_data_width-1 downto 0);
function f_memarray_to_ramtype(arr : t_meminit_array) return t_ram_type is
variable tmp : t_ram_type;
variable n, pos : integer;
begin
pos := 0;
while(pos < g_size)loop
n := 0;
-- avoid ISE loop iteration limit
while (pos < g_size and n < 4096) loop
for i in 0 to g_data_width-1 loop
tmp(pos)(i) := arr(pos, i);
end loop; -- i
n := n+1;
pos := pos + 1;
end loop;
end loop;
return tmp;
end f_memarray_to_ramtype;
function f_file_contents return t_meminit_array is
begin
return f_load_mem_from_file(g_init_file, g_size, g_data_width, g_fail_if_file_not_found);
end f_file_contents;
shared variable ram : t_ram_type := f_memarray_to_ramtype(f_file_contents);
signal s_we_a : std_logic_vector(c_num_bytes-1 downto 0);
signal s_ram_in_a : std_logic_vector(g_data_width-1 downto 0);
signal s_we_b : std_logic_vector(c_num_bytes-1 downto 0);
signal s_ram_in_b : std_logic_vector(g_data_width-1 downto 0);
signal wea_rep, web_rep : std_logic_vector(c_num_bytes-1 downto 0);
function f_check_bounds(x : integer; minx : integer; maxx : integer) return integer is
begin
if(x < minx) then
return minx;
elsif(x > maxx) then
return maxx;
else
return x;
end if;
end f_check_bounds;
begin
wea_rep <= (others => wea_i);
web_rep <= (others => web_i);
s_we_a <= bwea_i and wea_rep;
s_we_b <= bweb_i and web_rep;
gen_with_byte_enable_readfirst : if(g_with_byte_enable = true and (g_addr_conflict_resolution = "read_first" or
g_addr_conflict_resolution = "dont_care")) generate
process (clk_i)
begin
if rising_edge(clk_i) then
qa_o <= ram(f_check_bounds(to_integer(unsigned(aa_i)), 0, g_size-1));
qb_o <= ram(f_check_bounds(to_integer(unsigned(ab_i)), 0, g_size-1));
for i in 0 to c_num_bytes-1 loop
if s_we_a(i) = '1' then
ram(f_check_bounds(to_integer(unsigned(aa_i)), 0, g_size-1))((i+1)*8-1 downto i*8) := da_i((i+1)*8-1 downto i*8);
end if;
if(s_we_b(i) = '1') then
ram(f_check_bounds(to_integer(unsigned(ab_i)), 0, g_size-1))((i+1)*8-1 downto i*8) := db_i((i+1)*8-1 downto i*8);
end if;
end loop;
end if;
end process;
end generate gen_with_byte_enable_readfirst;
gen_without_byte_enable_readfirst : if(g_with_byte_enable = false and (g_addr_conflict_resolution = "read_first" or
g_addr_conflict_resolution = "dont_care")) generate
process(clk_i)
begin
if rising_edge(clk_i) then
qa_o <= ram(to_integer(unsigned(aa_i)));
qb_o <= ram(to_integer(unsigned(ab_i)));
if(wea_i = '1') then
ram(to_integer(unsigned(aa_i))) := da_i;
end if;
if(web_i = '1') then
ram(to_integer(unsigned(ab_i))) := db_i;
end if;
end if;
end process;
end generate gen_without_byte_enable_readfirst;
gen_without_byte_enable_writefirst : if(g_with_byte_enable = false and g_addr_conflict_resolution = "write_first") generate
process(clk_i)
begin
if rising_edge(clk_i) then
if(wea_i = '1') then
ram(to_integer(unsigned(aa_i))) := da_i;
qa_o <= da_i;
else
qa_o <= ram(to_integer(unsigned(aa_i)));
end if;
if(web_i = '1') then
ram(to_integer(unsigned(ab_i))) := db_i;
qb_o <= db_i;
else
qb_o <= ram(to_integer(unsigned(ab_i)));
end if;
end if;
end process;
end generate gen_without_byte_enable_writefirst;
gen_without_byte_enable_nochange : if(g_with_byte_enable = false and g_addr_conflict_resolution = "no_change") generate
process(clk_i)
begin
if rising_edge(clk_i) then
if(wea_i = '1') then
ram(to_integer(unsigned(aa_i))) := da_i;
else
qa_o <= ram(to_integer(unsigned(aa_i)));
end if;
if(web_i = '1') then
ram(to_integer(unsigned(ab_i))) := db_i;
else
qb_o <= ram(to_integer(unsigned(ab_i)));
end if;
end if;
end process;
end generate gen_without_byte_enable_nochange;
end syn;
-------------------------------------------------------------------------------
-- Title : Parametrizable dual-port synchronous RAM (Xilinx version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_simple_dpram.vhd
-- Author : Wesley W. Terpstra
-- Company : GSI
-- Created : 2013-03-04
-- Last update: 2013-10-30
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: True dual-port synchronous RAM for Xilinx FPGAs with:
-- - configurable address and data bus width
-- - byte-addressing mode (data bus width restricted to multiple of 8 bits)
-- Todo:
-- - loading initial contents from file
-- - add support for read-first/write-first address conflict resulution (only
-- supported by Xilinx in VHDL templates)
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2013-03-04 1.0 wterpstra Initial version: wrapper to generic_dpram
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use std.textio.all;
library work;
use work.genram_pkg.all;
use work.memory_loader_pkg.all;
entity generic_simple_dpram is
generic (
-- standard parameters
g_data_width : natural := 32;
g_size : natural := 16384;
g_with_byte_enable : boolean := false;
g_addr_conflict_resolution : string := "read_first";
g_init_file : string := "";
g_dual_clock : boolean := true;
g_fail_if_file_not_found : boolean := true
);
port (
rst_n_i : in std_logic := '1'; -- synchronous reset, active LO
-- Port A
clka_i : in std_logic;
bwea_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
wea_i : in std_logic;
aa_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
da_i : in std_logic_vector(g_data_width-1 downto 0);
-- Port B
clkb_i : in std_logic;
ab_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
qb_o : out std_logic_vector(g_data_width-1 downto 0)
);
end generic_simple_dpram;
architecture syn of generic_simple_dpram is
begin
-- Works well enough until a Xilinx guru can optimize it.
true_dp : generic_dpram
generic map(
g_data_width => g_data_width,
g_size => g_size,
g_with_byte_enable => g_with_byte_enable,
g_addr_conflict_resolution => g_addr_conflict_resolution,
g_init_file => g_init_file,
g_dual_clock => g_dual_clock)
port map(
rst_n_i => rst_n_i,
clka_i => clka_i,
bwea_i => bwea_i,
wea_i => wea_i,
aa_i => aa_i,
da_i => da_i,
qa_o => open,
clkb_i => clkb_i,
bweb_i => f_zeros((g_data_width+7)/8),
web_i => '0',
ab_i => ab_i,
db_i => f_zeros(g_data_width),
qb_o => qb_o);
end syn;
library ieee;
use ieee.std_logic_1164.all;
use ieee.std_logic_arith.all;
library work;
use work.genram_pkg.all;
entity generic_spram is
generic (
-- standard parameters
g_data_width : natural := 32;
g_size : natural := 1024;
-- if true, the user can write individual bytes by using bwe_i
g_with_byte_enable : boolean := false;
-- RAM read-on-write conflict resolution. Can be "read_first" (read-then-write)
-- or "write_first" (write-then-read)
g_addr_conflict_resolution : string := "write_first";
g_init_file : string := ""
);
port (
rst_n_i : in std_logic; -- synchronous reset, active LO
clk_i : in std_logic; -- clock input
-- byte write enable, actiwe when g_
bwe_i : in std_logic_vector((g_data_width+7)/8-1 downto 0);
-- global write enable (masked by bwe_i if g_with_byte_enable = true)
we_i : in std_logic;
-- address input
a_i : in std_logic_vector(f_log2_size(g_size)-1 downto 0);
-- data input
d_i : in std_logic_vector(g_data_width-1 downto 0);
-- data output
q_o : out std_logic_vector(g_data_width-1 downto 0)
);
end generic_spram;
architecture syn of generic_spram is
constant c_num_bytes : integer := (g_data_width+7)/8;
type t_ram_type is array(0 to g_size-1) of std_logic_vector(g_data_width-1 downto 0);
type t_string_file_type is file of string;
impure function f_bitstring_2_slv(s : string; num_bits : integer) return std_logic_vector is
begin
end function f_bitstring_2_slv;
impure function f_load_from_file(file_name : string) return t_ram_type is
file f : t_string_file_type;
variable fstatus : file_open_status;
begin
file_open(fstatus, f, file_name, read_mode);
if(fstatus /= open_ok) then
report "generic_spram: Cannot open memory initialization file: " & file_name severity failure;
end if;
end function f_load_from_file;
signal ram : t_ram_type;
signal s_we : std_logic_vector(c_num_bytes-1 downto 0);
signal s_ram_in : std_logic_vector(g_data_width-1 downto 0);
signal s_ram_out : std_logic_vector(g_data_width-1 downto 0);
begin
assert (g_init_file = "" or g_init_file = "none")
report "generic_spram: Memory initialization files not supported yet. Sorry :("
severity failure;
gen_with_byte_enable_writefirst : if(g_with_byte_enable = true and g_addr_conflict_resolution = "write_first") generate
s_we <= bwe_i when we_i = '1' else (others => '0');
process(s_we, d_i)
begin
for i in 0 to c_num_bytes-1 loop
if s_we(i) = '1' then
s_ram_in(8*i+7 downto 8*i) <= d_i(8*i+7 downto 8*i);
s_ram_out(8*i+7 downto 8*i) <= d_i(8*i+7 downto 8*i);
else
s_ram_in(8*i+7 downto 8*i) <= ram(conv_integer(unsigned(a_i)))(8*i+7 downto 8*i);
s_ram_out(8*i+7 downto 8*i) <= ram(conv_integer(unsigned(a_i)))(8*i+7 downto 8*i);
end if;
end loop; -- i
end process;
process(clk_i)
begin
if rising_edge(clk_i) then
ram(conv_integer(unsigned(a_i))) <= s_ram_in;
q_o <= s_ram_out;
end if;
end process;
end generate gen_with_byte_enable_writefirst;
gen_with_byte_enable_readfirst : if(g_with_byte_enable = true and (g_addr_conflict_resolution = "read_first" or
g_addr_conflict_resolution = "dont_care")) generate
s_we <= bwe_i when we_i = '1' else (others => '0');
process(s_we, d_i)
begin
for i in 0 to c_num_bytes-1 loop
if (s_we(i) = '1') then
s_ram_in(8*i+7 downto 8*i) <= d_i(8*i+7 downto 8*i);
else
s_ram_in(8*i+7 downto 8*i) <= ram(conv_integer(unsigned(a_i)))(8*i+7 downto 8*i);
end if;
end loop;
end process;
process(clk_i)
begin
if rising_edge(clk_i) then
ram(conv_integer(unsigned(a_i))) <= s_ram_in;
q_o <= ram(conv_integer(unsigned(a_i)));
end if;
end process;
end generate gen_with_byte_enable_readfirst;
gen_without_byte_enable_writefirst : if(g_with_byte_enable = false and g_addr_conflict_resolution = "write_first") generate
process(clk_i)
begin
if rising_edge(clk_i) then
if(we_i = '1') then
ram(conv_integer(unsigned(a_i))) <= d_i;
q_o <= d_i;
else
q_o <= ram(conv_integer(unsigned(a_i)));
end if;
end if;
end process;
end generate gen_without_byte_enable_writefirst;
gen_without_byte_enable_readfirst : if(g_with_byte_enable = false and (g_addr_conflict_resolution = "read_first" or
g_addr_conflict_resolution = "dont_care")) generate
process(clk_i)
begin
if rising_edge(clk_i) then
if(we_i = '1') then
ram(conv_integer(unsigned(a_i))) <= d_i;
end if;
q_o <= ram(conv_integer(unsigned(a_i)));
end if;
end process;
end generate gen_without_byte_enable_readfirst;
end syn;
files =["v6_fifo_pkg.vhd", "v6_hwfifo_wrapper.vhd", "generic_async_fifo.vhd", "generic_sync_fifo.vhd"];
-------------------------------------------------------------------------------
-- Title : Parametrizable asynchronous FIFO (Generic version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_async_fifo.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-CO-HT
-- Created : 2011-01-25
-- Last update: 2012-07-03
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Dual-clock asynchronous FIFO.
-- - configurable data width and size
-- - configurable full/empty/almost full/almost empty/word count signals
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.genram_pkg.all;
use work.v6_fifo_pkg.all;
entity generic_async_fifo is
generic (
g_data_width : natural;
g_size : natural;
g_show_ahead : boolean := false;
-- Read-side flag selection
g_with_rd_empty : boolean := true; -- with empty flag
g_with_rd_full : boolean := false; -- with full flag
g_with_rd_almost_empty : boolean := false;
g_with_rd_almost_full : boolean := false;
g_with_rd_count : boolean := false; -- with words counter
g_with_wr_empty : boolean := false;
g_with_wr_full : boolean := true;
g_with_wr_almost_empty : boolean := false;
g_with_wr_almost_full : boolean := false;
g_with_wr_count : boolean := false;
g_almost_empty_threshold : integer; -- threshold for almost empty flag
g_almost_full_threshold : integer -- threshold for almost full flag
);
port (
rst_n_i : in std_logic := '1';
-- write port
clk_wr_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
wr_empty_o : out std_logic;
wr_full_o : out std_logic;
wr_almost_empty_o : out std_logic;
wr_almost_full_o : out std_logic;
wr_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0);
-- read port
clk_rd_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
rd_empty_o : out std_logic;
rd_full_o : out std_logic;
rd_almost_empty_o : out std_logic;
rd_almost_full_o : out std_logic;
rd_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0)
);
end generic_async_fifo;
architecture syn of generic_async_fifo is
component inferred_async_fifo
generic (
g_data_width : natural;
g_size : natural;
g_show_ahead : boolean;
g_with_rd_empty : boolean;
g_with_rd_full : boolean;
g_with_rd_almost_empty : boolean;
g_with_rd_almost_full : boolean;
g_with_rd_count : boolean;
g_with_wr_empty : boolean;
g_with_wr_full : boolean;
g_with_wr_almost_empty : boolean;
g_with_wr_almost_full : boolean;
g_with_wr_count : boolean;
g_almost_empty_threshold : integer;
g_almost_full_threshold : integer);
port (
rst_n_i : in std_logic := '1';
clk_wr_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
wr_empty_o : out std_logic;
wr_full_o : out std_logic;
wr_almost_empty_o : out std_logic;
wr_almost_full_o : out std_logic;
wr_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0);
clk_rd_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
rd_empty_o : out std_logic;
rd_full_o : out std_logic;
rd_almost_empty_o : out std_logic;
rd_almost_full_o : out std_logic;
rd_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0));
end component;
component v6_hwfifo_wraper
generic (
g_data_width : natural;
g_size : natural;
g_dual_clock : boolean;
g_almost_empty_threshold : integer;
g_almost_full_threshold : integer);
port (
rst_n_i : in std_logic := '1';
clk_wr_i : in std_logic;
clk_rd_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
rd_empty_o : out std_logic;
wr_full_o : out std_logic;
rd_almost_empty_o : out std_logic;
wr_almost_full_o : out std_logic;
rd_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0);
wr_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0));
end component;
constant m : t_v6_fifo_mapping := f_v6_fifo_find_mapping(g_data_width, g_size);
begin -- syn
gen_inferred : if(m.d_width = 0 or g_with_wr_count or g_with_rd_count) generate
assert false report "generic_async_fifo[xilinx]: using inferred BRAM-based FIFO." severity note;
U_Inferred_FIFO: inferred_async_fifo
generic map (
g_data_width => g_data_width,
g_size => g_size,
g_show_ahead => g_show_ahead,
g_with_rd_empty => g_with_rd_empty,
g_with_rd_full => g_with_rd_full,
g_with_rd_almost_empty => g_with_rd_almost_empty,
g_with_rd_almost_full => g_with_rd_almost_full,
g_with_rd_count => g_with_rd_count,
g_with_wr_empty => g_with_wr_empty,
g_with_wr_full => g_with_wr_full,
g_with_wr_almost_empty => g_with_wr_almost_empty,
g_with_wr_almost_full => g_with_wr_almost_full,
g_with_wr_count => g_with_wr_count,
g_almost_empty_threshold => g_almost_empty_threshold,
g_almost_full_threshold => g_almost_full_threshold)
port map (
rst_n_i => rst_n_i,
clk_wr_i => clk_wr_i,
d_i => d_i,
we_i => we_i,
wr_empty_o => wr_empty_o,
wr_full_o => wr_full_o,
wr_almost_empty_o => wr_almost_empty_o,
wr_almost_full_o => wr_almost_full_o,
wr_count_o => wr_count_o,
clk_rd_i => clk_rd_i,
q_o => q_o,
rd_i => rd_i,
rd_empty_o => rd_empty_o,
rd_full_o => rd_full_o,
rd_almost_empty_o => rd_almost_empty_o,
rd_almost_full_o => rd_almost_full_o,
rd_count_o => rd_count_o);
end generate gen_inferred;
gen_native : if(m.d_width > 0 and not g_with_wr_count and not g_with_rd_count) generate
U_Native_FIFO: v6_hwfifo_wraper
generic map (
g_data_width => g_data_width,
g_size => g_size,
g_dual_clock => true,
g_almost_empty_threshold => f_empty_thr(g_with_rd_almost_empty, g_almost_empty_threshold, g_size),
g_almost_full_threshold => f_empty_thr(g_with_wr_almost_full, g_almost_full_threshold, g_size))
port map (
rst_n_i => rst_n_i,
clk_wr_i => clk_wr_i,
clk_rd_i => clk_rd_i,
d_i => d_i,
we_i => we_i,
q_o => q_o,
rd_i => rd_i,
rd_empty_o => rd_empty_o,
wr_full_o => wr_full_o,
rd_almost_empty_o => rd_almost_empty_o,
wr_almost_full_o => wr_almost_full_o,
rd_count_o => rd_count_o,
wr_count_o => wr_count_o);
end generate gen_native;
end syn;
-------------------------------------------------------------------------------
-- Title : Parametrizable synchronous FIFO (Xilinx version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_sync_fifo.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-CO-HT
-- Created : 2011-01-25
-- Last update: 2012-07-03
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Single-clock FIFO.
-- - configurable data width and size
-- - "show ahead" mode
-- - configurable full/empty/almost full/almost empty/word count signals
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library unisim;
use unisim.vcomponents.all;
use work.genram_pkg.all;
use work.v6_fifo_pkg.all;
entity generic_sync_fifo is
generic (
g_data_width : natural;
g_size : natural;
g_show_ahead : boolean := false;
-- Read-side flag selection
g_with_empty : boolean := true; -- with empty flag
g_with_full : boolean := true; -- with full flag
g_with_almost_empty : boolean := false;
g_with_almost_full : boolean := false;
g_with_count : boolean := false; -- with words counter
g_almost_empty_threshold : integer; -- threshold for almost empty flag
g_almost_full_threshold : integer; -- threshold for almost full flag
g_register_flag_outputs : boolean := true
);
port (
rst_n_i : in std_logic := '1';
clk_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
empty_o : out std_logic;
full_o : out std_logic;
almost_empty_o : out std_logic;
almost_full_o : out std_logic;
count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0)
);
end generic_sync_fifo;
architecture syn of generic_sync_fifo is
component inferred_sync_fifo
generic (
g_data_width : natural;
g_size : natural;
g_show_ahead : boolean;
g_with_empty : boolean;
g_with_full : boolean;
g_with_almost_empty : boolean;
g_with_almost_full : boolean;
g_with_count : boolean;
g_almost_empty_threshold : integer;
g_almost_full_threshold : integer;
g_register_flag_outputs : boolean);
port (
rst_n_i : in std_logic := '1';
clk_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
empty_o : out std_logic;
full_o : out std_logic;
almost_empty_o : out std_logic;
almost_full_o : out std_logic;
count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0));
end component;
component v6_hwfifo_wraper
generic (
g_data_width : natural;
g_size : natural;
g_dual_clock : boolean;
g_almost_empty_threshold : integer;
g_almost_full_threshold : integer);
port (
rst_n_i : in std_logic := '1';
clk_wr_i : in std_logic;
clk_rd_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
rd_empty_o : out std_logic;
wr_full_o : out std_logic;
rd_almost_empty_o : out std_logic;
wr_almost_full_o : out std_logic;
rd_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0);
wr_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0));
end component;
constant m : t_v6_fifo_mapping := f_v6_fifo_find_mapping(g_data_width, g_size);
begin -- syn
gen_inferred : if(m.d_width = 0) generate
assert false report "generic_sync_fifo[xilinx]: using inferred BRAM-based FIFO." severity note;
U_Inferred_FIFO : inferred_sync_fifo
generic map (
g_data_width => g_data_width,
g_size => g_size,
g_show_ahead => g_show_ahead,
g_with_empty => g_with_empty,
g_with_full => g_with_full,
g_with_almost_empty => g_with_almost_empty,
g_with_almost_full => g_with_almost_full,
g_with_count => g_with_count,
g_almost_empty_threshold => g_almost_empty_threshold,
g_almost_full_threshold => g_almost_full_threshold,
g_register_flag_outputs => g_register_flag_outputs)
port map (
rst_n_i => rst_n_i,
clk_i => clk_i,
d_i => d_i,
we_i => we_i,
q_o => q_o,
rd_i => rd_i,
empty_o => empty_o,
full_o => full_o,
almost_empty_o => almost_empty_o,
almost_full_o => almost_full_o,
count_o => count_o);
end generate gen_inferred;
gen_native : if(m.d_width > 0) generate
U_Native_FIFO: v6_hwfifo_wraper
generic map (
g_data_width => g_data_width,
g_size => g_size,
g_dual_clock => false,
g_almost_empty_threshold => f_empty_thr(g_with_almost_empty, g_almost_empty_threshold, g_size),
g_almost_full_threshold => f_empty_thr(g_with_almost_full, g_almost_full_threshold, g_size))
port map (
rst_n_i => rst_n_i,
clk_wr_i => clk_i,
clk_rd_i => clk_i,
d_i => d_i,
we_i => we_i,
q_o => q_o,
rd_i => rd_i,
rd_empty_o => empty_o,
wr_full_o => full_o,
rd_almost_empty_o => almost_empty_o,
wr_almost_full_o => almost_full_o,
rd_count_o => count_o,
wr_count_o => open);
end generate gen_native;
end syn;
package v6_fifo_pkg is
type t_v6_fifo_mapping is record
d_width : integer;
dp_width : integer;
entries : integer;
is_36 : boolean;
end record;
type t_v6_fifo_mapping_array is array(integer range <>) of t_v6_fifo_mapping;
constant c_v6_fifo_mappings : t_v6_fifo_mapping_array (0 to 9) := (
0 => (d_width => 4, dp_width => 0, entries => 4096, is_36 => false),
1 => (d_width => 8, dp_width => 1, entries => 2048, is_36 => false),
2 => (d_width => 16, dp_width => 2, entries => 1024, is_36 => false),
3 => (d_width => 32, dp_width => 4, entries => 512, is_36 => false),
4 => (d_width => 32, dp_width => 4, entries => 1024, is_36 => true),
5 => (d_width => 4, dp_width => 0, entries => 8192, is_36 => true),
6 => (d_width => 8, dp_width => 1, entries => 4096, is_36 => true),
7 => (d_width => 16, dp_width => 2, entries => 2048, is_36 => true),
8 => (d_width => 32, dp_width => 4, entries => 1024, is_36 => true),
9 => (d_width => 64, dp_width => 8, entries => 512, is_36 => true));
impure function f_v6_fifo_find_mapping
(data_width : integer; size : integer) return t_v6_fifo_mapping;
function f_v6_fifo_mode (m : t_v6_fifo_mapping) return string;
function f_empty_thr(a: boolean; thr: integer; size:integer) return integer;
end v6_fifo_pkg;
package body v6_fifo_pkg is
impure function f_v6_fifo_find_mapping
(data_width : integer; size : integer) return t_v6_fifo_mapping is
variable tmp : t_v6_fifo_mapping;
begin
for i in 0 to c_v6_fifo_mappings'length-1 loop
if(c_v6_fifo_mappings(i).d_width + c_v6_fifo_mappings(i).dp_width >= data_width and c_v6_fifo_mappings(i).entries >= size) then
return c_v6_fifo_mappings(i);
end if;
end loop;
tmp.d_width := 0;
return tmp;
end f_v6_fifo_find_mapping;
function f_v6_fifo_mode (m : t_v6_fifo_mapping) return string is
begin
if(m.d_width = 64 and m.is_36 = true) then
return "FIFO36_72";
elsif(m.d_width = 32 and m.is_36 = false) then
return "FIFO18_36";
elsif(m.is_36 = true) then
return "FIFO36";
else
return "FIFO18";
end if;
return "";
end f_v6_fifo_mode;
function f_empty_thr
(a: boolean; thr: integer; size:integer) return integer is
begin
if(a = true) then
return thr;
else
return size/2;
end if;
end f_empty_thr;
end v6_fifo_pkg;
-------------------------------------------------------------------------------
-- Title : Parametrizable synchronous FIFO (Xilinx version)
-- Project : Generics RAMs and FIFOs collection
-------------------------------------------------------------------------------
-- File : generic_sync_fifo.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-CO-HT
-- Created : 2011-01-25
-- Last update: 2012-10-02
-- Platform :
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Single-clock FIFO.
-- - configurable data width and size
-- - "show ahead" mode
-- - configurable full/empty/almost full/almost empty/word count signals
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-01-25 1.0 twlostow Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library unisim;
use unisim.vcomponents.all;
use work.genram_pkg.all;
use work.v6_fifo_pkg.all;
entity v6_hwfifo_wraper is
generic (
g_data_width : natural;
g_size : natural;
g_dual_clock : boolean;
-- Read-side flag selection
g_almost_empty_threshold : integer; -- threshold for almost empty flag
g_almost_full_threshold : integer -- threshold for almost full flag
);
port (
rst_n_i : in std_logic := '1';
clk_wr_i : in std_logic;
clk_rd_i : in std_logic;
d_i : in std_logic_vector(g_data_width-1 downto 0);
we_i : in std_logic;
q_o : out std_logic_vector(g_data_width-1 downto 0);
rd_i : in std_logic;
rd_empty_o : out std_logic;
wr_full_o : out std_logic;
rd_almost_empty_o : out std_logic;
wr_almost_full_o : out std_logic;
rd_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0);
wr_count_o : out std_logic_vector(f_log2_size(g_size)-1 downto 0)
);
end v6_hwfifo_wraper;
architecture syn of v6_hwfifo_wraper is
constant m : t_v6_fifo_mapping := f_v6_fifo_find_mapping(g_data_width, g_size);
signal di_wide_18, do_wide_18 : std_logic_vector(35 downto 0);
signal di_18, do_18 : std_logic_vector(31 downto 0);
signal dip_18, dop_18 : std_logic_vector(3 downto 0);
signal di_36, do_36 : std_logic_vector(63 downto 0);
signal dip_36, dop_36 : std_logic_vector(7 downto 0);
signal di_wide_36, do_wide_36 : std_logic_vector(79 downto 0);
signal srst, srstreg : std_logic;
signal rd_ptr, wr_ptr : std_logic_vector(12 downto 0);
function f_bool_2_int (x : boolean) return integer is
begin
if(x) then
return 1;
else
return 0;
end if;
end f_bool_2_int;
function f_clamp (x : integer; min_val : integer; max_val : integer)
return integer is
begin
if(x < min_val) then
return min_val;
elsif(x > max_val) then
return max_val;
else
return x;
end if;
end f_clamp;
begin -- syn
srst <= not rst_n_i;
srstreg <= '0' when g_dual_clock = true else srst;
gen_fifo36 : if(m.is_36 and m.d_width > 0) generate
assert false report "generic_sync_fifo[xilinx]: using FIFO36E1 primitive." severity note;
di_wide_36 <= std_logic_vector(resize(unsigned(d_i), di_wide_36'length));
di_36(m.d_width-1 downto 0) <= di_wide_36(m.d_width-1 downto 0);
dip_36(m.dp_width-1 downto 0) <= di_wide_36(m.dp_width + m.d_width-1 downto m.d_width);
do_wide_36 (m.d_width-1 downto 0) <= do_36(m.d_width-1 downto 0);
do_wide_36 (m.d_width + m.dp_width-1 downto m.d_width) <= dop_36(m.dp_width-1 downto 0);
q_o <= do_wide_36(g_data_width-1 downto 0);
U_Wrapped_FIFO36 : FIFO36E1
generic map (
ALMOST_FULL_OFFSET => to_bitvector(std_logic_vector(to_unsigned(g_almost_full_threshold, 16))),
ALMOST_EMPTY_OFFSET => to_bitvector(std_logic_vector(to_unsigned(g_almost_empty_threshold, 16))),
DATA_WIDTH => m.d_width + m.dp_width,
DO_REG => f_bool_2_int(g_dual_clock),
EN_SYN => not g_dual_clock,
FIFO_MODE => f_v6_fifo_mode(m),
FIRST_WORD_FALL_THROUGH => false)
port map (
ALMOSTEMPTY => rd_almost_empty_o,
ALMOSTFULL => wr_almost_full_o,
DO => do_36,
DOP => dop_36,
EMPTY => rd_empty_o,
FULL => wr_full_o,
RDCOUNT => rd_ptr(12 downto 0),
WRCOUNT => wr_ptr(12 downto 0),
INJECTDBITERR => '0',
INJECTSBITERR => '0',
DI => di_36,
DIP => dip_36,
RDCLK => clk_rd_i,
RDEN => rd_i,
REGCE => '1',
RST => srst,
RSTREG => srstreg,
WRCLK => clk_wr_i,
WREN => we_i);
end generate gen_fifo36;
gen_fifo18 : if(not m.is_36 and m.d_width > 0) generate
di_wide_18 <= std_logic_vector(resize(unsigned(d_i), di_wide_18'length));
di_18(m.d_width-1 downto 0) <= di_wide_18(m.d_width-1 downto 0);
dip_18(m.dp_width-1 downto 0) <= di_wide_18(m.dp_width + m.d_width-1 downto m.d_width);
do_wide_18 (m.d_width-1 downto 0) <= do_18(m.d_width-1 downto 0);
do_wide_18 (m.d_width + m.dp_width-1 downto m.d_width) <= dop_18(m.dp_width-1 downto 0);
q_o <= do_wide_18(g_data_width-1 downto 0);
assert false report "generic_sync_fifo[xilinx]: using FIFO18E1 primitive [dw=" & integer'image(g_data_width) &"]." severity note;
U_Wrapped_FIFO18 : FIFO18E1
generic map (
ALMOST_FULL_OFFSET => to_bitvector(std_logic_vector(to_unsigned(f_clamp(g_almost_full_threshold, 5, 2043), 16))),
ALMOST_EMPTY_OFFSET => to_bitvector(std_logic_vector(to_unsigned(f_clamp(g_almost_empty_threshold, 5, 2043), 16))),
DATA_WIDTH => m.d_width + m.dp_width,
DO_REG => f_bool_2_int(g_dual_clock),
EN_SYN => not g_dual_clock,
FIFO_MODE => f_v6_fifo_mode(m),
FIRST_WORD_FALL_THROUGH => false)
port map (
ALMOSTEMPTY => rd_almost_empty_o,
ALMOSTFULL => wr_almost_full_o,
DO => do_18,
DOP => dop_18,
EMPTY => rd_empty_o,
FULL => wr_full_o,
RDCOUNT => rd_ptr(11 downto 0),
WRCOUNT => wr_ptr(11 downto 0),
DI => di_18,
DIP => dip_18,
RDCLK => clk_rd_i,
RDEN => rd_i,
REGCE => '1',
RST => srst,
RSTREG => srstreg,
WRCLK => clk_wr_i,
WREN => we_i);
rd_ptr(12) <= '0';
wr_ptr(12) <= '0';
end generate gen_fifo18;
gen_pointers : if(g_dual_clock = false) generate
rd_count_o <= std_logic_vector(resize(unsigned(wr_ptr) - unsigned(rd_ptr), rd_count_o'length));
wr_count_o <= std_logic_vector(resize(unsigned(wr_ptr) - unsigned(rd_ptr), wr_count_o'length));
end generate gen_pointers;
end syn;
files = ["wb_async_bridge.vhd", "xwb_async_bridge.vhd"]
------------------------------------------------------------------------------
-- Title : Atmel EBI asynchronous bus <-> Wishbone bridge
-- Project : White Rabbit Switch
------------------------------------------------------------------------------
-- Author : Tomasz Wlostowski
-- Company : CERN BE-Co-HT
-- Created : 2010-05-18
-- Last update: 2011-09-23
-- Platform : FPGA-generic
-- Standard : VHDL'87
-------------------------------------------------------------------------------
-- Description: An interface between AT91SAM9x-series ARM CPU External Bus Interface
-- and FPGA-internal Wishbone bus:
-- - does clock domain synchronisation
-- - provides configurable number of independent WB master ports at fixed base addresses
-- TODO:
-- - implement write queueing and read prefetching (for speed improvement)
-------------------------------------------------------------------------------
-- Copyright (c) 2010 Tomasz Wlostowski
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2010-05-18 1.0 twlostow Created
-- 2011-09-21 1.1 twlostow Added support for pipelined mode
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use ieee.math_real.log2;
use ieee.math_real.ceil;
use work.gencores_pkg.all;
use work.wishbone_pkg.all;
entity wb_async_bridge is
generic (
g_simulation : integer := 0;
g_interface_mode : t_wishbone_interface_mode;
g_address_granularity : t_wishbone_address_granularity;
g_cpu_address_width : integer := 32);
port(
rst_n_i : in std_logic; -- global reset
clk_sys_i : in std_logic; -- system clock
-------------------------------------------------------------------------------
-- Atmel EBI bus
-------------------------------------------------------------------------------
cpu_cs_n_i : in std_logic;
-- async write, active LOW
cpu_wr_n_i : in std_logic;
-- async read, active LOW
cpu_rd_n_i : in std_logic;
-- byte select, active LOW (not used due to weird CPU pin layout - NBS2 line is
-- shared with 100 Mbps Ethernet PHY)
cpu_bs_n_i : in std_logic_vector(3 downto 0);
-- address input
cpu_addr_i : in std_logic_vector(g_cpu_address_width-1 downto 0);
-- data bus (bidirectional)
cpu_data_b : inout std_logic_vector(31 downto 0);
-- async wait, active LOW
cpu_nwait_o : out std_logic;
-------------------------------------------------------------------------------
-- Wishbone master I/F
-------------------------------------------------------------------------------
-- wishbone master address output (m->s, common for all slaves)
wb_adr_o : out std_logic_vector(c_wishbone_address_width - 1 downto 0);
-- wishbone master data output (m->s common for all slaves)
wb_dat_o : out std_logic_vector(31 downto 0);
-- wishbone cycle strobe (m->s, common for all slaves)
wb_stb_o : out std_logic;
-- wishbone write enable (m->s, common for all slaves)
wb_we_o : out std_logic;
-- wishbone byte select output (m->s, common for all slaves)
wb_sel_o : out std_logic_vector(3 downto 0);
-- wishbone cycle select (m->s)
wb_cyc_o : out std_logic;
-- wishbone master data input (s->m)
wb_dat_i : in std_logic_vector (c_wishbone_data_width-1 downto 0);
-- wishbone ACK input (s->m)
wb_ack_i : in std_logic;
wb_stall_i : in std_logic
);
end wb_async_bridge;
architecture behavioral of wb_async_bridge is
signal rw_sel, cycle_in_progress, cs_synced, rd_pulse, wr_pulse : std_logic;
signal cpu_data_reg : std_logic_vector(31 downto 0);
signal long_cycle : std_logic;
signal wb_in : t_wishbone_master_in;
signal wb_out : t_wishbone_master_out;
begin
U_Adapter : wb_slave_adapter
generic map (
g_master_use_struct => false,
g_master_mode => g_interface_mode,
g_master_granularity => g_address_granularity,
g_slave_use_struct => true,
g_slave_mode => CLASSIC,
g_slave_granularity => WORD)
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
slave_i => wb_out,
slave_o => wb_in,
ma_adr_o => wb_adr_o,
ma_dat_o => wb_dat_o,
ma_sel_o => wb_sel_o,
ma_cyc_o => wb_cyc_o,
ma_stb_o => wb_stb_o,
ma_we_o => wb_we_o,
ma_dat_i => wb_dat_i,
ma_ack_i => wb_ack_i,
ma_stall_i => wb_stall_i);
gen_sync_chains_nosim : if(g_simulation = 0) generate
sync_ffs_cs : gc_sync_ffs
generic map (
g_sync_edge => "positive")
port map
(rst_n_i => rst_n_i,
clk_i => clk_sys_i,
data_i => cpu_cs_n_i,
synced_o => cs_synced,
npulse_o => open
);
sync_ffs_wr : gc_sync_ffs
generic map (
g_sync_edge => "positive")
port map (
rst_n_i => rst_n_i,
clk_i => clk_sys_i,
data_i => cpu_wr_n_i,
synced_o => open,
npulse_o => wr_pulse
);
sync_ffs_rd : gc_sync_ffs
generic map (
g_sync_edge => "positive")
port map (
rst_n_i => rst_n_i,
clk_i => clk_sys_i,
data_i => cpu_rd_n_i,
synced_o => open,
npulse_o => rd_pulse
);
end generate gen_sync_chains_nosim;
gen_sim : if(g_simulation = 1) generate
wr_pulse <= not cpu_wr_n_i;
rd_pulse <= not cpu_rd_n_i;
cs_synced <= cpu_cs_n_i;
end generate gen_sim;
process(clk_sys_i)
begin
if(rising_edge(clk_sys_i)) then
if(rst_n_i = '0') then
cpu_data_reg <= (others => '0');
cycle_in_progress <= '0';
rw_sel <= '0';
cpu_nwait_o <= '1';
long_cycle <= '0';
wb_out.adr <= (others => '0');
wb_out.dat <= (others => '0');
wb_out.sel <= (others => '1');
wb_out.stb <= '0';
wb_out.we <= '0';
wb_out.cyc <= '0';
else
if(cs_synced = '0') then
wb_out.adr <= std_logic_vector(resize(unsigned(cpu_addr_i), c_wishbone_address_width));
if(cycle_in_progress = '1') then
if(wb_in.ack = '1') then
if(rw_sel = '0') then
cpu_data_reg <= wb_in.dat;
end if;
cycle_in_progress <= '0';
wb_out.cyc <= '0';
wb_out.sel <= (others => '1');
wb_out.stb <= '0';
wb_out.we <= '0';
cpu_nwait_o <= '1';
long_cycle <= '0';
else
cpu_nwait_o <= not long_cycle;
long_cycle <= '1';
end if;
elsif(rd_pulse = '1' or wr_pulse = '1') then
wb_out.cyc <= '1';
wb_out.stb <= '1';
wb_out.we <= wr_pulse;
long_cycle <= '0';
rw_sel <= wr_pulse;
if(wr_pulse = '1') then
wb_out.dat <= cpu_data_b;
end if;
cycle_in_progress <= '1';
end if;
end if;
end if;
end if;
end process;
process(cpu_cs_n_i, cpu_rd_n_i, cpu_data_reg)
begin
if(cpu_cs_n_i = '0' and cpu_rd_n_i = '0') then
cpu_data_b <= cpu_data_reg;
else
cpu_data_b <= (others => 'Z');
end if;
end process;
end behavioral;
library ieee;
use ieee.std_logic_1164.all;
use work.wishbone_pkg.all;
entity xwb_async_bridge is
generic (
g_simulation : integer := 0;
g_interface_mode : t_wishbone_interface_mode;
g_address_granularity : t_wishbone_address_granularity;
g_cpu_address_width: integer := 32);
port(
rst_n_i : in std_logic; -- global reset
clk_sys_i : in std_logic; -- system clock
-------------------------------------------------------------------------------
-- Atmel EBI bus
-------------------------------------------------------------------------------
cpu_cs_n_i : in std_logic;
-- async write, active LOW
cpu_wr_n_i : in std_logic;
-- async read, active LOW
cpu_rd_n_i : in std_logic;
-- byte select, active LOW (not used due to weird CPU pin layout - NBS2 line is
-- shared with 100 Mbps Ethernet PHY)
cpu_bs_n_i : in std_logic_vector(3 downto 0);
-- address input
cpu_addr_i : in std_logic_vector(g_cpu_address_width-1 downto 0);
-- data bus (bidirectional)
cpu_data_b : inout std_logic_vector(31 downto 0);
-- async wait, active LOW
cpu_nwait_o : out std_logic;
-------------------------------------------------------------------------------
-- Wishbone master I/F
-------------------------------------------------------------------------------
master_o : out t_wishbone_master_out;
master_i : in t_wishbone_master_in
);
end xwb_async_bridge;
architecture wrapper of xwb_async_bridge is
begin
U_Wrapped_Bridge : wb_async_bridge
generic map (
g_simulation => g_simulation,
g_interface_mode => g_interface_mode,
g_address_granularity => g_address_granularity,
g_cpu_address_width => g_cpu_address_width)
port map (
rst_n_i => rst_n_i,
clk_sys_i => clk_sys_i,
cpu_cs_n_i => cpu_cs_n_i,
cpu_wr_n_i => cpu_wr_n_i,
cpu_rd_n_i => cpu_rd_n_i,
cpu_bs_n_i => cpu_bs_n_i,
cpu_addr_i => cpu_addr_i,
cpu_data_b => cpu_data_b,
cpu_nwait_o => cpu_nwait_o,
wb_adr_o => master_o.adr,
wb_dat_o => master_o.dat,
wb_stb_o => master_o.stb,
wb_we_o => master_o.we,
wb_sel_o => master_o.sel,
wb_cyc_o => master_o.cyc,
wb_dat_i => master_i.dat,
wb_ack_i => master_i.ack);
end wrapper;
library ieee;
use ieee.STD_LOGIC_1164.all;
use ieee.numeric_std.all;
use work.wishbone_pkg.all;
entity xwb_bus_fanout is
generic (
g_num_outputs : natural;
g_bits_per_slave : integer := 14;
g_address_granularity : t_wishbone_address_granularity;
g_slave_interface_mode : t_wishbone_interface_mode);
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
slave_i : in t_wishbone_slave_in;
slave_o : out t_wishbone_slave_out;
master_i : in t_wishbone_master_in_array(0 to g_num_outputs-1);
master_o : out t_wishbone_master_out_array(0 to g_num_outputs-1)
);
end xwb_bus_fanout;
architecture rtl of xwb_bus_fanout is
function f_log2_ceil(N : natural) return positive is
begin
if N <= 2 then
return 1;
elsif N mod 2 = 0 then
return 1 + f_log2_ceil(N/2);
else
return 1 + f_log2_ceil((N+1)/2);
end if;
end;
constant c_periph_addr_bits : integer := f_log2_ceil(g_num_outputs);
signal periph_addr : std_logic_vector(c_periph_addr_bits - 1 downto 0);
signal periph_addr_reg : std_logic_vector(c_periph_addr_bits - 1 downto 0);
signal periph_sel : std_logic_vector(2**c_periph_addr_bits-1 downto 0);
signal periph_sel_reg : std_logic_vector(2**c_periph_addr_bits-1 downto 0);
signal ack_muxed : std_logic;
signal data_in_muxed : std_logic_vector(31 downto 0);
signal cycle_in_progress : std_logic;
signal ack_prev : std_logic;
signal adp_in : t_wishbone_master_in;
signal adp_out : t_wishbone_master_out;
begin -- rtl
U_Slave_Adapter : wb_slave_adapter
generic map (
g_master_use_struct => true,
g_master_mode => CLASSIC,
g_master_granularity => g_address_granularity,
g_slave_use_struct => true,
g_slave_mode => g_slave_interface_mode,
g_slave_granularity => g_address_granularity)
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
slave_i => slave_i,
slave_o => slave_o,
master_i => adp_in,
master_o => adp_out);
periph_addr <= adp_out.adr(g_bits_per_slave+c_periph_addr_bits-1 downto g_bits_per_slave);
onehot_decode : process (periph_addr) -- periph_sel <= onehot_decode(periph_addr)
variable temp1 : std_logic_vector (periph_sel'high downto 0);
variable temp2 : integer range 0 to periph_sel'high;
begin
temp1 := (others => '0');
temp2 := 0;
for i in periph_addr'range loop
if (periph_addr(i) = '1') then
temp2 := 2*temp2+1;
else
temp2 := 2*temp2;
end if;
end loop;
temp1(temp2) := '1';
periph_sel <= temp1;
end process;
ACK_MUX : process (periph_addr_reg, master_i)
begin
if(to_integer(unsigned(periph_addr_reg)) < g_num_outputs) then
ack_muxed <= master_i(to_integer(unsigned(periph_addr_reg))).ack;
else
ack_muxed <= '0';
end if;
end process;
DIN_MUX : process (periph_addr_reg, master_i)
begin
if(to_integer(unsigned(periph_addr_reg))) < g_num_outputs then
data_in_muxed <= master_i(to_integer(unsigned(periph_addr_reg))).dat;
else
data_in_muxed <= (others => 'X');
end if;
end process;
p_arbitrate : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
cycle_in_progress <= '0';
ack_prev <= '0';
periph_addr_reg <= (others => '0');
periph_sel_reg <= (others => '0');
adp_in.dat <= (others => '0');
adp_in.ack <= '0';
else
periph_sel_reg <= periph_sel;
periph_addr_reg <= periph_addr;
if(cycle_in_progress = '0') then
if(adp_out.cyc = '1' and adp_in.ack = '0') then
cycle_in_progress <= '1';
end if;
ack_prev <= '0';
adp_in.ack<='0';
else
adp_in.dat <= data_in_muxed;
ack_prev <= ack_muxed;
if(ack_prev = '0' and ack_muxed = '1') then
adp_in.ack <= '1';
else
adp_in.ack <= '0';
end if;
if(ack_muxed = '1') then
cycle_in_progress <= '0';
end if;
end if;
end if;
end if;
end process;
-- adp_in.ack <= ack_prev and adp_out.stb;
gen_outputs : for i in 0 to g_num_outputs-1 generate
master_o(i).cyc <= adp_out.cyc and periph_sel(i);
master_o(i).adr <= adp_out.adr;
master_o(i).dat <= adp_out.dat;
master_o(i).stb <= adp_out.stb and not (not cycle_in_progress and ack_prev);
master_o(i).we <= adp_out.we;
master_o(i).sel <= adp_out.sel;
end generate gen_outputs;
adp_in.err <= '0';
adp_in.stall <= '0';
adp_in.rty <= '0';
end rtl;
K 25
svn:wc:ra_dav:version-url
V 61
/fmc-tdc/!svn/ver/119/hdl/ip_cores/wishbone/wb_clock_crossing
END
xwb_clock_crossing.vhd
K 25
svn:wc:ra_dav:version-url
V 84
/fmc-tdc/!svn/ver/119/hdl/ip_cores/wishbone/wb_clock_crossing/xwb_clock_crossing.vhd
END
Manifest.py
K 25
svn:wc:ra_dav:version-url
V 73
/fmc-tdc/!svn/ver/119/hdl/ip_cores/wishbone/wb_clock_crossing/Manifest.py
END
10
dir
147
http://svn.ohwr.org/fmc-tdc/hdl/ip_cores/wishbone/wb_clock_crossing
http://svn.ohwr.org/fmc-tdc
2013-10-02T17:30:59.859470Z
119
egousiou
85dfdc96-de2c-444c-878d-45b388be74a9
xwb_clock_crossing.vhd
file
2014-01-23T10:16:42.389794Z
c749c75280b50ac9ffc906bc3476357e
2013-10-02T17:30:59.859470Z
119
egousiou
6338
Manifest.py
file
2014-01-23T10:16:42.389794Z
ba3dcaf6ade684ae8bae04aa00db326b
2013-10-02T17:30:59.859470Z
119
egousiou
39
files = ["wb_conmax_pri_dec.vhd", "wb_conmax_pri_enc.vhd", "wb_conmax_arb.vhd", "wb_conmax_msel.vhd",
"wbconmax_pkg.vhd", "wb_conmax_slave_if.vhd", "wb_conmax_master_if.vhd", "wb_conmax_rf.vhd",
"xwb_conmax.vhd" ];
-------------------------------------------------------------------------------
-- Title : Wishbone interconnect matrix for WR Core
-- Project : WhiteRabbit
-------------------------------------------------------------------------------
-- File : wb_conmax_arb.vhd
-- Author : Grzegorz Daniluk
-- Company : Elproma
-- Created : 2011-02-12
-- Last update: 2011-02-16
-- Platform : FPGA-generics
-- Standard : VHDL
-------------------------------------------------------------------------------
-- Description:
-- Simple arbiter with round robin. It does not use any prioritization for
-- WB Masters.
--
-------------------------------------------------------------------------------
-- Copyright (C) 2000-2002 Rudolf Usselmann
-- Copyright (c) 2011 Grzegorz Daniluk (VHDL port)
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-02-12 1.0 greg.d Created
-------------------------------------------------------------------------------
-- TODO:
-- Code optimization. (now it is more like dummy translation from Verilog)
--
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
entity wb_conmax_arb is
port(
clk_i : in std_logic;
rst_i : in std_logic;
--req_i(n) <- wb_cyc from n-th Master
req_i : in std_logic_vector(7 downto 0);
next_i : in std_logic;
--which master (0 to 7) is granted
gnt_o : out std_logic_vector(2 downto 0)
);
end wb_conmax_arb;
architecture behaviour of wb_conmax_arb is
type t_arb_states is (GRANT0, GRANT1, GRANT2, GRANT3, GRANT4, GRANT5,
GRANT6, GRANT7);
signal s_state : t_arb_states;
begin
--state transitions
process(clk_i)
begin
if(clk_i'event and clk_i='1') then
if(rst_i = '1') then
s_state <= GRANT0;
else
case s_state is
when GRANT0 =>
--if this req is dropped or next is asserted, check for other req's
if(req_i(0)='0' or next_i='1') then
if ( req_i(1)='1' ) then
s_state <= GRANT1;
elsif( req_i(2)='1' ) then
s_state <= GRANT2;
elsif( req_i(3)='1' ) then
s_state <= GRANT3;
elsif( req_i(4)='1' ) then
s_state <= GRANT4;
elsif( req_i(5)='1' ) then
s_state <= GRANT5;
elsif( req_i(6)='1' ) then
s_state <= GRANT6;
elsif( req_i(7)='1' ) then
s_state <= GRANT7;
end if;
end if;
when GRANT1 =>
if(req_i(1)='0' or next_i='1') then
if ( req_i(2)='1' ) then
s_state <= GRANT2;
elsif( req_i(3)='1' ) then
s_state <= GRANT3;
elsif( req_i(4)='1' ) then
s_state <= GRANT4;
elsif( req_i(5)='1' ) then
s_state <= GRANT5;
elsif( req_i(6)='1' ) then
s_state <= GRANT6;
elsif( req_i(7)='1' ) then
s_state <= GRANT7;
elsif( req_i(0)='1' ) then
s_state <= GRANT0;
end if;
end if;
when GRANT2 =>
if(req_i(2)='0' or next_i='1') then
if ( req_i(3)='1' ) then
s_state <= GRANT3;
elsif( req_i(4)='1' ) then
s_state <= GRANT4;
elsif( req_i(5)='1' ) then
s_state <= GRANT5;
elsif( req_i(6)='1' ) then
s_state <= GRANT6;
elsif( req_i(7)='1' ) then
s_state <= GRANT7;
elsif( req_i(0)='1' ) then
s_state <= GRANT0;
elsif( req_i(1)='1' ) then
s_state <= GRANT1;
end if;
end if;
when GRANT3 =>
if(req_i(3)='0' or next_i='1') then
if ( req_i(4)='1' ) then
s_state <= GRANT4;
elsif( req_i(5)='1' ) then
s_state <= GRANT5;
elsif( req_i(6)='1' ) then
s_state <= GRANT6;
elsif( req_i(7)='1' ) then
s_state <= GRANT7;
elsif( req_i(0)='1' ) then
s_state <= GRANT0;
elsif( req_i(1)='1' ) then
s_state <= GRANT1;
elsif( req_i(2)='1' ) then
s_state <= GRANT2;
end if;
end if;
when GRANT4 =>
if(req_i(4)='0' or next_i='1') then
if ( req_i(5)='1' ) then
s_state <= GRANT5;
elsif( req_i(6)='1' ) then
s_state <= GRANT6;
elsif( req_i(7)='1' ) then
s_state <= GRANT7;
elsif( req_i(0)='1' ) then
s_state <= GRANT0;
elsif( req_i(1)='1' ) then
s_state <= GRANT1;
elsif( req_i(2)='1' ) then
s_state <= GRANT2;
elsif( req_i(3)='1' ) then
s_state <= GRANT3;
end if;
end if;
when GRANT5 =>
if(req_i(5)='0' or next_i='1') then
if ( req_i(6)='1' ) then
s_state <= GRANT6;
elsif( req_i(7)='1' ) then
s_state <= GRANT7;
elsif( req_i(0)='1' ) then
s_state <= GRANT0;
elsif( req_i(1)='1' ) then
s_state <= GRANT1;
elsif( req_i(2)='1' ) then
s_state <= GRANT2;
elsif( req_i(3)='1' ) then
s_state <= GRANT3;
elsif( req_i(4)='1' ) then
s_state <= GRANT4;
end if;
end if;
when GRANT6 =>
if(req_i(6)='0' or next_i='1') then
if ( req_i(7)='1' ) then
s_state <= GRANT7;
elsif( req_i(0)='1' ) then
s_state <= GRANT0;
elsif( req_i(1)='1' ) then
s_state <= GRANT1;
elsif( req_i(2)='1' ) then
s_state <= GRANT2;
elsif( req_i(3)='1' ) then
s_state <= GRANT3;
elsif( req_i(4)='1' ) then
s_state <= GRANT4;
elsif( req_i(5)='1' ) then
s_state <= GRANT5;
end if;
end if;
when GRANT7 =>
if(req_i(7)='0' or next_i='1') then
if ( req_i(0)='1' ) then
s_state <= GRANT0;
elsif( req_i(1)='1' ) then
s_state <= GRANT1;
elsif( req_i(2)='1' ) then
s_state <= GRANT2;
elsif( req_i(3)='1' ) then
s_state <= GRANT3;
elsif( req_i(4)='1' ) then
s_state <= GRANT4;
elsif( req_i(5)='1' ) then
s_state <= GRANT5;
elsif( req_i(6)='1' ) then
s_state <= GRANT6;
end if;
end if;
when others =>
s_state <= GRANT0;
end case;
end if;
end if;
end process;
process(s_state)
begin
case(s_state) is
when GRANT0 => gnt_o <= "000";
when GRANT1 => gnt_o <= "001";
when GRANT2 => gnt_o <= "010";
when GRANT3 => gnt_o <= "011";
when GRANT4 => gnt_o <= "100";
when GRANT5 => gnt_o <= "101";
when GRANT6 => gnt_o <= "110";
when GRANT7 => gnt_o <= "111";
when others => gnt_o <= "000";
end case;
end process;
end behaviour;
-------------------------------------------------------------------------------
-- Title : Wishbone interconnect matrix for WR Core
-- Project : WhiteRabbit
-------------------------------------------------------------------------------
-- File : wb_conmax_master_if.vhd
-- Author : Grzegorz Daniluk
-- Company : Elproma
-- Created : 2011-02-12
-- Last update: 2011-09-12
-- Platform : FPGA-generics
-- Standard : VHDL
-------------------------------------------------------------------------------
-- Description:
-- Wishbone interface to connect WB master from outside. Decodes 4 most
-- significant bits of Address bus. Using the selection it multiplexes the
-- Master's WB interface to appropriate Slave interface.
--
-------------------------------------------------------------------------------
-- Copyright (C) 2000-2002 Rudolf Usselmann
-- Copyright (c) 2011 Grzegorz Daniluk (VHDL port)
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-02-12 1.0 greg.d Created
-- 2011-02-16 1.1 greg.d Using generates and types
-------------------------------------------------------------------------------
-- TODO:
-- Code optimization. (now it is more like dummy translation from Verilog)
--
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wbconmax_pkg.all;
use work.wishbone_pkg.all;
entity wb_conmax_master_if is
generic(
g_adr_width : integer;
g_sel_width : integer;
g_dat_width : integer);
port(
clk_i : in std_logic;
rst_i : in std_logic;
--Master interface
wb_master_i : in t_wishbone_slave_in;
wb_master_o : out t_wishbone_slave_out;
--Slaves(0 to 15) interface
wb_slaves_i : in t_wishbone_master_in_array(0 to 15);
wb_slaves_o : out t_wishbone_master_out_array(0 to 15)
);
end wb_conmax_master_if;
architecture behaviour of wb_conmax_master_if is
signal s_slv_sel : std_logic_vector(3 downto 0);
signal s_cyc_o_next : std_logic_vector(15 downto 0);
signal s_cyc_o : std_logic_vector(15 downto 0);
begin
--Select logic
s_slv_sel <= wb_master_i.adr(g_adr_width-1 downto g_adr_width-4);
--Address & Data Pass
GEN_OUTS :
for I in 0 to 15 generate
wb_slaves_o(I).adr <= wb_master_i.adr;
wb_slaves_o(I).sel <= wb_master_i.sel;
wb_slaves_o(I).dat <= wb_master_i.dat;
wb_slaves_o(I).we <= wb_master_i.we;
wb_slaves_o(I).cyc <= s_cyc_o(I);
wb_slaves_o(I).stb <= wb_master_i.stb when(s_slv_sel = std_logic_vector(
to_unsigned(I, 4)) ) else '0';
end generate;
wb_master_o.dat <= wb_slaves_i(0).dat when(s_slv_sel = "0000") else
wb_slaves_i(1).dat when(s_slv_sel = "0001") else
wb_slaves_i(2).dat when(s_slv_sel = "0010") else
wb_slaves_i(3).dat when(s_slv_sel = "0011") else
wb_slaves_i(4).dat when(s_slv_sel = "0100") else
wb_slaves_i(5).dat when(s_slv_sel = "0101") else
wb_slaves_i(6).dat when(s_slv_sel = "0110") else
wb_slaves_i(7).dat when(s_slv_sel = "0111") else
wb_slaves_i(8).dat when(s_slv_sel = "1000") else
wb_slaves_i(9).dat when(s_slv_sel = "1001") else
wb_slaves_i(10).dat when(s_slv_sel = "1010") else
wb_slaves_i(11).dat when(s_slv_sel = "1011") else
wb_slaves_i(12).dat when(s_slv_sel = "1100") else
wb_slaves_i(13).dat when(s_slv_sel = "1101") else
wb_slaves_i(14).dat when(s_slv_sel = "1110") else
wb_slaves_i(15).dat when(s_slv_sel = "1111") else
(others => '0');
--Control Signal Pass
G1 : for I in 0 to 15 generate
s_cyc_o_next(I) <= s_cyc_o(I) when (wb_master_i.cyc = '1' and wb_master_i.stb = '0') else
wb_master_i.cyc when (s_slv_sel = std_logic_vector(to_unsigned(I, 4))) else
'0';
end generate;
process(clk_i)
begin
if(clk_i'event and clk_i = '1') then
if(rst_i = '1') then
s_cyc_o(15 downto 0) <= (others => '0');
else
s_cyc_o(15 downto 0) <= s_cyc_o_next(15 downto 0);
end if;
end if;
end process;
wb_master_o.ack <= wb_slaves_i(0).ack when(s_slv_sel = "0000") else
wb_slaves_i(1).ack when(s_slv_sel = "0001") else
wb_slaves_i(2).ack when(s_slv_sel = "0010") else
wb_slaves_i(3).ack when(s_slv_sel = "0011") else
wb_slaves_i(4).ack when(s_slv_sel = "0100") else
wb_slaves_i(5).ack when(s_slv_sel = "0101") else
wb_slaves_i(6).ack when(s_slv_sel = "0110") else
wb_slaves_i(7).ack when(s_slv_sel = "0111") else
wb_slaves_i(8).ack when(s_slv_sel = "1000") else
wb_slaves_i(9).ack when(s_slv_sel = "1001") else
wb_slaves_i(10).ack when(s_slv_sel = "1010") else
wb_slaves_i(11).ack when(s_slv_sel = "1011") else
wb_slaves_i(12).ack when(s_slv_sel = "1100") else
wb_slaves_i(13).ack when(s_slv_sel = "1101") else
wb_slaves_i(14).ack when(s_slv_sel = "1110") else
wb_slaves_i(15).ack when(s_slv_sel = "1111") else
'0';
wb_master_o.err <= wb_slaves_i(0).err when(s_slv_sel = "0000") else
wb_slaves_i(1).err when(s_slv_sel = "0001") else
wb_slaves_i(2).err when(s_slv_sel = "0010") else
wb_slaves_i(3).err when(s_slv_sel = "0011") else
wb_slaves_i(4).err when(s_slv_sel = "0100") else
wb_slaves_i(5).err when(s_slv_sel = "0101") else
wb_slaves_i(6).err when(s_slv_sel = "0110") else
wb_slaves_i(7).err when(s_slv_sel = "0111") else
wb_slaves_i(8).err when(s_slv_sel = "1000") else
wb_slaves_i(9).err when(s_slv_sel = "1001") else
wb_slaves_i(10).err when(s_slv_sel = "1010") else
wb_slaves_i(11).err when(s_slv_sel = "1011") else
wb_slaves_i(12).err when(s_slv_sel = "1100") else
wb_slaves_i(13).err when(s_slv_sel = "1101") else
wb_slaves_i(14).err when(s_slv_sel = "1110") else
wb_slaves_i(15).err when(s_slv_sel = "1111") else
'0';
wb_master_o.rty <= wb_slaves_i(0).rty when(s_slv_sel = "0000") else
wb_slaves_i(1).rty when(s_slv_sel = "0001") else
wb_slaves_i(2).rty when(s_slv_sel = "0010") else
wb_slaves_i(3).rty when(s_slv_sel = "0011") else
wb_slaves_i(4).rty when(s_slv_sel = "0100") else
wb_slaves_i(5).rty when(s_slv_sel = "0101") else
wb_slaves_i(6).rty when(s_slv_sel = "0110") else
wb_slaves_i(7).rty when(s_slv_sel = "0111") else
wb_slaves_i(8).rty when(s_slv_sel = "1000") else
wb_slaves_i(9).rty when(s_slv_sel = "1001") else
wb_slaves_i(10).rty when(s_slv_sel = "1010") else
wb_slaves_i(11).rty when(s_slv_sel = "1011") else
wb_slaves_i(12).rty when(s_slv_sel = "1100") else
wb_slaves_i(13).rty when(s_slv_sel = "1101") else
wb_slaves_i(14).rty when(s_slv_sel = "1110") else
wb_slaves_i(15).rty when(s_slv_sel = "1111") else
'0';
end behaviour;
-------------------------------------------------------------------------------
-- Title : Wishbone interconnect matrix for WR Core
-- Project : WhiteRabbit
-------------------------------------------------------------------------------
-- File : wb_conmax_msel.vhd
-- Author : Grzegorz Daniluk
-- Company : Elproma
-- Created : 2011-02-12
-- Last update: 2010-02-12
-- Platform : FPGA-generics
-- Standard : VHDL
-------------------------------------------------------------------------------
-- Description:
-- Prioritizing arbiter for Slave interface. Uses simple arbitrer
-- (wb_conmax_arb) and takes Master's priorities into account.
--
-------------------------------------------------------------------------------
-- Copyright (C) 2000-2002 Rudolf Usselmann
-- Copyright (c) 2011 Grzegorz Daniluk (VHDL port)
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-02-12 1.0 greg.d Created
-------------------------------------------------------------------------------
-- TODO:
-- Code optimization. (now it is more like dummy translation from Verilog)
--
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
entity wb_conmax_msel is
generic(
g_pri_sel : integer := 0
);
port(
clk_i : in std_logic;
rst_i : in std_logic;
conf_i : in std_logic_vector(15 downto 0);
req_i : in std_logic_vector(7 downto 0);
next_i : in std_logic;
sel : out std_logic_vector(2 downto 0)
);
end wb_conmax_msel;
architecture behavioral of wb_conmax_msel is
component wb_conmax_pri_enc is
generic(
-- :=1 means 2 priority levels, :=2 means 4 priority levels
g_pri_sel : integer := 0
);
port(
valid_i : in std_logic_vector(7 downto 0);
pri0_i : in std_logic_vector(1 downto 0);
pri1_i : in std_logic_vector(1 downto 0);
pri2_i : in std_logic_vector(1 downto 0);
pri3_i : in std_logic_vector(1 downto 0);
pri4_i : in std_logic_vector(1 downto 0);
pri5_i : in std_logic_vector(1 downto 0);
pri6_i : in std_logic_vector(1 downto 0);
pri7_i : in std_logic_vector(1 downto 0);
pri_o : out std_logic_vector(1 downto 0)
);
end component;
component wb_conmax_arb is
port(
clk_i : in std_logic;
rst_i : in std_logic;
req_i : in std_logic_vector(7 downto 0);
gnt_o : out std_logic_vector(2 downto 0);
next_i : in std_logic
);
end component;
signal s_pri0, s_pri1, s_pri2, s_pri3, s_pri4,
s_pri5, s_pri6, s_pri7 : std_logic_vector(1 downto 0);
signal s_pri_out_d, s_pri_out : std_logic_vector(1 downto 0);
signal s_req_p0, s_req_p1, s_req_p2, s_req_p3 : std_logic_vector(7 downto 0);
signal s_gnt_p0, s_gnt_p1, s_gnt_p2, s_gnt_p3 : std_logic_vector(2 downto 0);
signal s_sel1, s_sel2 : std_logic_vector(2 downto 0);
begin
--------------------------------------
--Priority Select logic
G1: if(g_pri_sel=0) generate
s_pri0 <= "00";
s_pri1 <= "00";
s_pri2 <= "00";
s_pri3 <= "00";
s_pri4 <= "00";
s_pri5 <= "00";
s_pri6 <= "00";
s_pri7 <= "00";
end generate;
G2: if(g_pri_sel=2) generate
s_pri0 <= conf_i(1 downto 0);
s_pri1 <= conf_i(3 downto 2);
s_pri2 <= conf_i(5 downto 4);
s_pri3 <= conf_i(7 downto 6);
s_pri4 <= conf_i(9 downto 8);
s_pri5 <= conf_i(11 downto 10);
s_pri6 <= conf_i(13 downto 12);
s_pri7 <= conf_i(15 downto 14);
end generate;
G3: if(g_pri_sel/=0 and g_pri_sel/=2) generate
s_pri0 <= '0' & conf_i(0);
s_pri1 <= '0' & conf_i(2);
s_pri2 <= '0' & conf_i(4);
s_pri3 <= '0' & conf_i(6);
s_pri4 <= '0' & conf_i(8);
s_pri5 <= '0' & conf_i(10);
s_pri6 <= '0' & conf_i(12);
s_pri7 <= '0' & conf_i(14);
end generate;
PRI_ENC: wb_conmax_pri_enc
generic map(
-- :=1 means 2 priority levels, :=2 means 4 priority levels
g_pri_sel => g_pri_sel
)
port map(
valid_i => req_i,
pri0_i => s_pri0,
pri1_i => s_pri1,
pri2_i => s_pri2,
pri3_i => s_pri3,
pri4_i => s_pri4,
pri5_i => s_pri5,
pri6_i => s_pri6,
pri7_i => s_pri7,
pri_o => s_pri_out_d
);
process(clk_i)
begin
if(clk_i'event and clk_i='1') then
if(rst_i = '1') then
s_pri_out <= "00";
elsif(next_i = '1') then
s_pri_out <= s_pri_out_d;
end if;
end if;
end process;
-----------------------------------------------
--Arbiters
s_req_p0(0) <= req_i(0) when(s_pri0 = "00") else '0';
s_req_p0(1) <= req_i(1) when(s_pri1 = "00") else '0';
s_req_p0(2) <= req_i(2) when(s_pri2 = "00") else '0';
s_req_p0(3) <= req_i(3) when(s_pri3 = "00") else '0';
s_req_p0(4) <= req_i(4) when(s_pri4 = "00") else '0';
s_req_p0(5) <= req_i(5) when(s_pri5 = "00") else '0';
s_req_p0(6) <= req_i(6) when(s_pri6 = "00") else '0';
s_req_p0(7) <= req_i(7) when(s_pri7 = "00") else '0';
s_req_p1(0) <= req_i(0) when(s_pri0 = "01") else '0';
s_req_p1(1) <= req_i(1) when(s_pri1 = "01") else '0';
s_req_p1(2) <= req_i(2) when(s_pri2 = "01") else '0';
s_req_p1(3) <= req_i(3) when(s_pri3 = "01") else '0';
s_req_p1(4) <= req_i(4) when(s_pri4 = "01") else '0';
s_req_p1(5) <= req_i(5) when(s_pri5 = "01") else '0';
s_req_p1(6) <= req_i(6) when(s_pri6 = "01") else '0';
s_req_p1(7) <= req_i(7) when(s_pri7 = "01") else '0';
s_req_p2(0) <= req_i(0) when(s_pri0 = "10") else '0';
s_req_p2(1) <= req_i(1) when(s_pri1 = "10") else '0';
s_req_p2(2) <= req_i(2) when(s_pri2 = "10") else '0';
s_req_p2(3) <= req_i(3) when(s_pri3 = "10") else '0';
s_req_p2(4) <= req_i(4) when(s_pri4 = "10") else '0';
s_req_p2(5) <= req_i(5) when(s_pri5 = "10") else '0';
s_req_p2(6) <= req_i(6) when(s_pri6 = "10") else '0';
s_req_p2(7) <= req_i(7) when(s_pri7 = "10") else '0';
s_req_p3(0) <= req_i(0) when(s_pri0 = "11") else '0';
s_req_p3(1) <= req_i(1) when(s_pri1 = "11") else '0';
s_req_p3(2) <= req_i(2) when(s_pri2 = "11") else '0';
s_req_p3(3) <= req_i(3) when(s_pri3 = "11") else '0';
s_req_p3(4) <= req_i(4) when(s_pri4 = "11") else '0';
s_req_p3(5) <= req_i(5) when(s_pri5 = "11") else '0';
s_req_p3(6) <= req_i(6) when(s_pri6 = "11") else '0';
s_req_p3(7) <= req_i(7) when(s_pri7 = "11") else '0';
ARB0: wb_conmax_arb
port map(
clk_i => clk_i,
rst_i => rst_i,
req_i => s_req_p0,
gnt_o => s_gnt_p0,
next_i => '0'
);
ARB1: wb_conmax_arb
port map(
clk_i => clk_i,
rst_i => rst_i,
req_i => s_req_p1,
gnt_o => s_gnt_p1,
next_i => '0'
);
ARB2: wb_conmax_arb
port map(
clk_i => clk_i,
rst_i => rst_i,
req_i => s_req_p2,
gnt_o => s_gnt_p2,
next_i => '0'
);
ARB3: wb_conmax_arb
port map(
clk_i => clk_i,
rst_i => rst_i,
req_i => s_req_p3,
gnt_o => s_gnt_p3,
next_i => '0'
);
-----------------------------------------------
--Final Master Select
s_sel1 <= s_gnt_p1 when( s_pri_out(0)='1' ) else
s_gnt_p0;
s_sel2 <= s_gnt_p0 when( s_pri_out="00" ) else
s_gnt_p1 when( s_pri_out="01" ) else
s_gnt_p2 when( s_pri_out="10" ) else
s_gnt_p3;
G4: if(g_pri_sel=0) generate
sel <= s_gnt_p0;
end generate;
G5: if(g_pri_sel=1) generate
sel <= s_sel1;
end generate;
G6: if(g_pri_sel/=0 and g_pri_sel/=1) generate
sel <= s_sel2;
end generate;
end behavioral;
-------------------------------------------------------------------------------
-- Title : Wishbone interconnect matrix for WR Core
-- Project : WhiteRabbit
-------------------------------------------------------------------------------
-- File : wb_conmax_pri_dec.vhd
-- Author : Grzegorz Daniluk
-- Company : Elproma
-- Created : 2011-02-12
-- Last update: 2010-02-12
-- Platform : FPGA-generics
-- Standard : VHDL
-------------------------------------------------------------------------------
-- Description:
-- Simple Master's priority encoder
--
-------------------------------------------------------------------------------
-- Copyright (C) 2000-2002 Rudolf Usselmann
-- Copyright (c) 2011 Grzegorz Daniluk (VHDL port)
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-02-12 1.0 greg.d Created
-------------------------------------------------------------------------------
-- TODO:
-- Code optimization. (now it is more like dummy translation from Verilog)
-- (eg. <if..generate> instead of pri_out_d0 and pri_out_d1)
--
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
entity wb_conmax_pri_dec is
generic(
-- :=1 means 2 priority levels, :=2 means 4 priority levels
g_pri_sel : integer := 0
);
port(
valid_i : in std_logic;
pri_i : in std_logic_vector(1 downto 0);
pri_o : out std_logic_vector(3 downto 0)
);
end wb_conmax_pri_dec;
architecture behaviour of wb_conmax_pri_dec is
signal pri_out_d0 : std_logic_vector(3 downto 0);
signal pri_out_d1 : std_logic_vector(3 downto 0);
begin
--4 priority levels
process(valid_i,pri_i)
begin
if( valid_i='0' ) then
pri_out_d1 <= "0001";
elsif( pri_i="00" ) then
pri_out_d1 <= "0001";
elsif( pri_i="01" ) then
pri_out_d1 <= "0010";
elsif( pri_i="10" ) then
pri_out_d1 <= "0100";
else
pri_out_d1 <= "1000";
end if;
end process;
--2 priority levels
process(valid_i, pri_i)
begin
if( valid_i='0' ) then
pri_out_d0 <= "0001";
elsif( pri_i="00" ) then
pri_out_d0 <= "0001";
else
pri_out_d0 <= "0010";
end if;
end process;
--select how many pririty levels
G1: if(g_pri_sel=0) generate
pri_o <= "0000";
end generate;
G2: if(g_pri_sel=1) generate
pri_o <= pri_out_d0;
end generate;
G3: if(g_pri_sel/=0 and g_pri_sel/=1) generate
pri_o <= pri_out_d1;
end generate;
end behaviour;
-------------------------------------------------------------------------------
-- Title : Wishbone interconnect matrix for WR Core
-- Project : WhiteRabbit
-------------------------------------------------------------------------------
-- File : wb_conmax_pri_enc.vhd
-- Author : Grzegorz Daniluk
-- Company : Elproma
-- Created : 2011-02-12
-- Last update: 2010-02-12
-- Platform : FPGA-generics
-- Standard : VHDL
-------------------------------------------------------------------------------
-- Description:
-- The set of priority encoders for all Master interfaces.
--
-------------------------------------------------------------------------------
-- Copyright (C) 2000-2002 Rudolf Usselmann
-- Copyright (c) 2011 Grzegorz Daniluk (VHDL port)
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-02-12 1.0 greg.d Created
-------------------------------------------------------------------------------
-- TODO:
-- Code optimization. (now it is more like dummy translation from Verilog)
--
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
entity wb_conmax_pri_enc is
generic(
-- :=1 means 2 priority levels, :=2 means 4 priority levels
g_pri_sel : integer := 0
);
port(
valid_i : in std_logic_vector(7 downto 0);
pri0_i : in std_logic_vector(1 downto 0);
pri1_i : in std_logic_vector(1 downto 0);
pri2_i : in std_logic_vector(1 downto 0);
pri3_i : in std_logic_vector(1 downto 0);
pri4_i : in std_logic_vector(1 downto 0);
pri5_i : in std_logic_vector(1 downto 0);
pri6_i : in std_logic_vector(1 downto 0);
pri7_i : in std_logic_vector(1 downto 0);
pri_o : out std_logic_vector(1 downto 0)
);
end wb_conmax_pri_enc;
architecture behaviour of wb_conmax_pri_enc is
component wb_conmax_pri_dec is
generic(
-- :=1 means 2 priority levels, :=2 means 4 priority levels
g_pri_sel : integer := 0
);
port(
valid_i : in std_logic;
pri_i : in std_logic_vector(1 downto 0);
pri_o : out std_logic_vector(3 downto 0)
);
end component;
signal s_pri0_o : std_logic_vector(3 downto 0);
signal s_pri1_o : std_logic_vector(3 downto 0);
signal s_pri2_o : std_logic_vector(3 downto 0);
signal s_pri3_o : std_logic_vector(3 downto 0);
signal s_pri4_o : std_logic_vector(3 downto 0);
signal s_pri5_o : std_logic_vector(3 downto 0);
signal s_pri6_o : std_logic_vector(3 downto 0);
signal s_pri7_o : std_logic_vector(3 downto 0);
signal s_pritmp_o : std_logic_vector(3 downto 0);
signal s_pri_out0 : std_logic_vector(1 downto 0);
signal s_pri_out1 : std_logic_vector(1 downto 0);
begin
PD0: wb_conmax_pri_dec
generic map(
g_pri_sel => g_pri_sel
)
port map(
valid_i => valid_i(0),
pri_i => pri0_i,
pri_o => s_pri0_o
);
PD1: wb_conmax_pri_dec
generic map(
g_pri_sel => g_pri_sel
)
port map(
valid_i => valid_i(1),
pri_i => pri1_i,
pri_o => s_pri1_o
);
PD2: wb_conmax_pri_dec
generic map(
g_pri_sel => g_pri_sel
)
port map(
valid_i => valid_i(2),
pri_i => pri2_i,
pri_o => s_pri2_o
);
PD3: wb_conmax_pri_dec
generic map(
g_pri_sel => g_pri_sel
)
port map(
valid_i => valid_i(3),
pri_i => pri3_i,
pri_o => s_pri3_o
);
PD4: wb_conmax_pri_dec
generic map(
g_pri_sel => g_pri_sel
)
port map(
valid_i => valid_i(4),
pri_i => pri4_i,
pri_o => s_pri4_o
);
PD5: wb_conmax_pri_dec
generic map(
g_pri_sel => g_pri_sel
)
port map(
valid_i => valid_i(5),
pri_i => pri5_i,
pri_o => s_pri5_o
);
PD6: wb_conmax_pri_dec
generic map(
g_pri_sel => g_pri_sel
)
port map(
valid_i => valid_i(6),
pri_i => pri6_i,
pri_o => s_pri6_o
);
PD7: wb_conmax_pri_dec
generic map(
g_pri_sel => g_pri_sel
)
port map(
valid_i => valid_i(7),
pri_i => pri7_i,
pri_o => s_pri7_o
);
s_pritmp_o <= s_pri0_o or s_pri1_o or s_pri2_o or s_pri3_o or s_pri4_o or
s_pri5_o or s_pri6_o or s_pri7_o;
--4 priority levels
s_pri_out1 <= "11" when ( s_pritmp_o(3)='1' ) else
"10" when ( s_pritmp_o(2)='1' ) else
"01" when ( s_pritmp_o(1)='1' ) else
"00";
--2 priority levels
s_pri_out0 <= "01" when ( s_pritmp_o(1)='1' ) else
"00";
G1: if (g_pri_sel=0) generate
pri_o <= "00";
end generate;
G2: if (g_pri_sel=1) generate
pri_o <= s_pri_out0;
end generate;
G3: if (g_pri_sel/=0 and g_pri_sel/=1) generate
pri_o <= s_pri_out1;
end generate;
end behaviour;
-------------------------------------------------------------------------------
-- Title : Wishbone interconnect matrix for WR Core
-- Project : WhiteRabbit
-------------------------------------------------------------------------------
-- File : wb_conmax_rf.vhd
-- Author : Grzegorz Daniluk
-- Company : Elproma
-- Created : 2011-02-12
-- Last update: 2011-09-12
-- Platform : FPGA-generics
-- Standard : VHDL
-------------------------------------------------------------------------------
-- Description:
-- Register File - consists of 16 registers. Each stores configuration for
-- different Slave.
-- Each of those 16 registers is the Slave's personal priority register,
-- where the priorities for each Master are stored. The Register File is
-- accessible from Master through Slave 15th interface.
--
-------------------------------------------------------------------------------
-- Copyright (C) 2000-2002 Rudolf Usselmann
-- Copyright (c) 2011 Grzegorz Daniluk (VHDL port)
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-02-12 1.0 greg.d Created
-- 2011-02-16 1.1 greg.d Using generates and types
-------------------------------------------------------------------------------
-- TODO:
-- Code optimization. (now it is more like dummy translation from Verilog)
--
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wbconmax_pkg.all;
use work.wishbone_pkg.all;
entity wb_conmax_rf is
generic(
g_rf_addr : integer range 0 to 15 := 15; --0xF
g_adr_width : integer;
g_sel_width : integer;
g_dat_width : integer);
port(
clk_i : in std_logic;
rst_i : in std_logic;
--Internal WB interface
int_wb_i : in t_wishbone_slave_in;
int_wb_o : out t_wishbone_slave_out;
--External WB interface
ext_wb_i : in t_wishbone_master_in;
ext_wb_o : out t_wishbone_master_out;
--Configuration regs
conf_o : out t_conmax_rf_conf
);
end wb_conmax_rf;
architecture behaviour of wb_conmax_rf is
signal s_rf_sel : std_logic;
signal s_rf_dout : std_logic_vector(15 downto 0);
signal s_rf_ack : std_logic;
signal s_rf_we : std_logic;
signal s_conf : t_conmax_rf_conf;
signal s_rf_addr : std_logic_vector(3 downto 0);
begin
--Register File select logic
s_rf_addr <= std_logic_vector(to_unsigned(g_rf_addr, 4));
s_rf_sel <= int_wb_i.cyc and int_wb_i.stb when(int_wb_i.adr(g_adr_width-5 downto g_adr_width-8) = s_rf_addr )
else '0';
--Register File logic
process(clk_i)
begin
if(clk_i'event and clk_i='1') then
s_rf_we <= s_rf_sel and int_wb_i.we and not(s_rf_we);
s_rf_ack <= s_rf_sel and not(s_rf_ack);
end if;
end process;
--Write logic
process(clk_i)
begin
if(clk_i'event and clk_i='1') then
if(rst_i = '1') then
s_conf <= (others=> (others=>'0'));
elsif(s_rf_we='1') then
if (int_wb_i.adr(5 downto 2)=x"0") then s_conf(0) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"1") then s_conf(1) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"2") then s_conf(2) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"3") then s_conf(3) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"4") then s_conf(4) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"5") then s_conf(5) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"6") then s_conf(6) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"7") then s_conf(7) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"8") then s_conf(8) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"9") then s_conf(9) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"a") then s_conf(10) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"b") then s_conf(11) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"c") then s_conf(12) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"d") then s_conf(13) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"e") then s_conf(14) <= int_wb_i.dat(15 downto 0);
elsif(int_wb_i.adr(5 downto 2)=x"f") then s_conf(15) <= int_wb_i.dat(15 downto 0);
end if;
end if;
end if;
end process;
--Read logic
process(clk_i)
begin
if(clk_i'event and clk_i='1') then
if(s_rf_sel='0') then
s_rf_dout <= x"0000";
else
if ( int_wb_i.adr(5 downto 2)=x"0" ) then s_rf_dout <= s_conf(0);
elsif( int_wb_i.adr(5 downto 2)=x"1" ) then s_rf_dout <= s_conf(1);
elsif( int_wb_i.adr(5 downto 2)=x"2" ) then s_rf_dout <= s_conf(2);
elsif( int_wb_i.adr(5 downto 2)=x"3" ) then s_rf_dout <= s_conf(3);
elsif( int_wb_i.adr(5 downto 2)=x"4" ) then s_rf_dout <= s_conf(4);
elsif( int_wb_i.adr(5 downto 2)=x"5" ) then s_rf_dout <= s_conf(5);
elsif( int_wb_i.adr(5 downto 2)=x"6" ) then s_rf_dout <= s_conf(6);
elsif( int_wb_i.adr(5 downto 2)=x"7" ) then s_rf_dout <= s_conf(7);
elsif( int_wb_i.adr(5 downto 2)=x"8" ) then s_rf_dout <= s_conf(8);
elsif( int_wb_i.adr(5 downto 2)=x"9" ) then s_rf_dout <= s_conf(9);
elsif( int_wb_i.adr(5 downto 2)=x"A" ) then s_rf_dout <= s_conf(10);
elsif( int_wb_i.adr(5 downto 2)=x"B" ) then s_rf_dout <= s_conf(11);
elsif( int_wb_i.adr(5 downto 2)=x"C" ) then s_rf_dout <= s_conf(12);
elsif( int_wb_i.adr(5 downto 2)=x"D" ) then s_rf_dout <= s_conf(13);
elsif( int_wb_i.adr(5 downto 2)=x"E" ) then s_rf_dout <= s_conf(14);
elsif( int_wb_i.adr(5 downto 2)=x"F" ) then s_rf_dout <= s_conf(15);
end if;
end if;
end if;
end process;
--Register File bypass logic
ext_wb_o.adr <= int_wb_i.adr;
ext_wb_o.sel <= int_wb_i.sel;
ext_wb_o.dat <= int_wb_i.dat;
ext_wb_o.cyc <= int_wb_i.cyc when(s_rf_sel='0') else '0';
ext_wb_o.stb <= int_wb_i.stb;
ext_wb_o.we <= int_wb_i.we;
int_wb_o.dat <= ( (g_dat_width-1 downto 16 => '0') & s_rf_dout ) when(s_rf_sel='1')
else ext_wb_i.dat;
int_wb_o.ack <= s_rf_ack when(s_rf_sel='1') else ext_wb_i.ack;
int_wb_o.err <= '0' when(s_rf_sel='1') else ext_wb_i.err;
int_wb_o.rty <= '0' when(s_rf_sel='1') else ext_wb_i.rty;
conf_o <= s_conf;
end behaviour;
-------------------------------------------------------------------------------
-- Title : Wishbone interconnect matrix for WR Core
-- Project : WhiteRabbit
-------------------------------------------------------------------------------
-- File : wb_conmax_slave_if.vhd
-- Author : Grzegorz Daniluk
-- Company : Elproma
-- Created : 2011-02-12
-- Last update: 2011-09-12
-- Platform : FPGA-generics
-- Standard : VHDL
-------------------------------------------------------------------------------
-- Description:
-- Full interface for a single Wishbone Slave. Consists of WB Master interface,
-- Prioritizing Arbiter and multiplexer for selecting appropriate Master.
--
-------------------------------------------------------------------------------
-- Copyright (C) 2000-2002 Rudolf Usselmann
-- Copyright (c) 2011 Grzegorz Daniluk (VHDL port)
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-02-12 1.0 greg.d Created
-- 2011-02-16 1.1 greg.d Using generates and types
-------------------------------------------------------------------------------
-- TODO:
-- Code optimization. (now it is more like dummy translation from Verilog)
--
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wbconmax_pkg.all;
use work.wishbone_pkg.all;
entity wb_conmax_slave_if is
generic(
g_pri_sel : integer := 2
);
port(
clk_i : in std_logic;
rst_i : in std_logic;
conf_i : in std_logic_vector(15 downto 0);
--Slave interface
wb_slave_i : in t_wishbone_master_in;
wb_slave_o : out t_wishbone_master_out;
--Master (0 to 7) interfaces
wb_masters_i : in t_wishbone_slave_in_array(0 to 7);
wb_masters_o : out t_wishbone_slave_out_array(0 to 7)
);
end wb_conmax_slave_if;
architecture bahaviour of wb_conmax_slave_if is
component wb_conmax_arb is
port(
clk_i : in std_logic;
rst_i : in std_logic;
req_i : in std_logic_vector(7 downto 0);
gnt_o : out std_logic_vector(2 downto 0);
next_i : in std_logic
);
end component;
component wb_conmax_msel is
generic(
g_pri_sel : integer := 0
);
port(
clk_i : in std_logic;
rst_i : in std_logic;
conf_i : in std_logic_vector(15 downto 0);
req_i : in std_logic_vector(7 downto 0);
next_i : in std_logic;
sel : out std_logic_vector(2 downto 0)
);
end component;
signal s_wb_cyc_o : std_logic;
signal s_msel_simple, s_msel_pe, s_msel : std_logic_vector(2 downto 0);
signal s_next : std_logic;
signal s_mcyc : std_logic_vector(7 downto 0);
signal s_arb_req_i : std_logic_vector(7 downto 0);
begin
wb_slave_o.cyc <= s_wb_cyc_o;
process(clk_i)
begin
if(clk_i'event and clk_i = '1') then
s_next <= not(s_wb_cyc_o);
end if;
end process;
s_arb_req_i <= wb_masters_i(7).cyc & wb_masters_i(6).cyc & wb_masters_i(5).cyc &
wb_masters_i(4).cyc & wb_masters_i(3).cyc & wb_masters_i(2).cyc &
wb_masters_i(1).cyc & wb_masters_i(0).cyc;
--Prioritizing Arbiter
ARB : wb_conmax_arb
port map(
clk_i => clk_i,
rst_i => rst_i,
req_i => s_arb_req_i,
gnt_o => s_msel_simple,
next_i => '0' --no round robin
);
MSEL : wb_conmax_msel
generic map(
g_pri_sel => g_pri_sel
)
port map(
clk_i => clk_i,
rst_i => rst_i,
conf_i => conf_i,
req_i => s_arb_req_i,
next_i => s_next,
sel => s_msel_pe
);
G1 : if(g_pri_sel = 0) generate
s_msel <= s_msel_simple;
end generate;
G2 : if(g_pri_sel /= 0) generate
s_msel <= s_msel_pe;
end generate;
-------------------------------------
--Address & Data Pass
wb_slave_o.adr <= wb_masters_i(0).adr when(s_msel = "000") else
wb_masters_i(1).adr when(s_msel = "001") else
wb_masters_i(2).adr when(s_msel = "010") else
wb_masters_i(3).adr when(s_msel = "011") else
wb_masters_i(4).adr when(s_msel = "100") else
wb_masters_i(5).adr when(s_msel = "101") else
wb_masters_i(6).adr when(s_msel = "110") else
wb_masters_i(7).adr when(s_msel = "111") else
(others => '0');
wb_slave_o.sel <= wb_masters_i(0).sel when(s_msel = "000") else
wb_masters_i(1).sel when(s_msel = "001") else
wb_masters_i(2).sel when(s_msel = "010") else
wb_masters_i(3).sel when(s_msel = "011") else
wb_masters_i(4).sel when(s_msel = "100") else
wb_masters_i(5).sel when(s_msel = "101") else
wb_masters_i(6).sel when(s_msel = "110") else
wb_masters_i(7).sel when(s_msel = "111") else
(others => '0');
wb_slave_o.dat <= wb_masters_i(0).dat when(s_msel = "000") else
wb_masters_i(1).dat when(s_msel = "001") else
wb_masters_i(2).dat when(s_msel = "010") else
wb_masters_i(3).dat when(s_msel = "011") else
wb_masters_i(4).dat when(s_msel = "100") else
wb_masters_i(5).dat when(s_msel = "101") else
wb_masters_i(6).dat when(s_msel = "110") else
wb_masters_i(7).dat when(s_msel = "111") else
(others => '0');
G_OUT : for I in 0 to 7 generate
wb_masters_o(I).dat <= wb_slave_i.dat;
wb_masters_o(I).ack <= wb_slave_i.ack when(s_msel = std_logic_vector(
to_unsigned(I, 3)) ) else '0';
wb_masters_o(I).err <= wb_slave_i.err when(s_msel = std_logic_vector(
to_unsigned(I, 3)) ) else '0';
wb_masters_o(I).rty <= wb_slave_i.rty when(s_msel = std_logic_vector(
to_unsigned(I, 3)) ) else '0';
end generate;
------------------------------------
--Control Signal Pass
wb_slave_o.we <= wb_masters_i(0).we when(s_msel = "000") else
wb_masters_i(1).we when(s_msel = "001") else
wb_masters_i(2).we when(s_msel = "010") else
wb_masters_i(3).we when(s_msel = "011") else
wb_masters_i(4).we when(s_msel = "100") else
wb_masters_i(5).we when(s_msel = "101") else
wb_masters_i(6).we when(s_msel = "110") else
wb_masters_i(7).we when(s_msel = "111") else
'0';
process(clk_i)
begin
if(clk_i'event and clk_i = '1') then
s_mcyc(7 downto 0) <= wb_masters_i(7).cyc & wb_masters_i(6).cyc &
wb_masters_i(5).cyc & wb_masters_i(4).cyc & wb_masters_i(3).cyc &
wb_masters_i(2).cyc & wb_masters_i(1).cyc & wb_masters_i(0).cyc;
end if;
end process;
s_wb_cyc_o <= wb_masters_i(0).cyc and s_mcyc(0) when(s_msel = "000") else
wb_masters_i(1).cyc and s_mcyc(1) when(s_msel = "001") else
wb_masters_i(2).cyc and s_mcyc(2) when(s_msel = "010") else
wb_masters_i(3).cyc and s_mcyc(3) when(s_msel = "011") else
wb_masters_i(4).cyc and s_mcyc(4) when(s_msel = "100") else
wb_masters_i(5).cyc and s_mcyc(5) when(s_msel = "101") else
wb_masters_i(6).cyc and s_mcyc(6) when(s_msel = "110") else
wb_masters_i(7).cyc and s_mcyc(7) when(s_msel = "111") else
'0';
wb_slave_o.stb <= wb_masters_i(0).stb when(s_msel = "000") else
wb_masters_i(1).stb when(s_msel = "001") else
wb_masters_i(2).stb when(s_msel = "010") else
wb_masters_i(3).stb when(s_msel = "011") else
wb_masters_i(4).stb when(s_msel = "100") else
wb_masters_i(5).stb when(s_msel = "101") else
wb_masters_i(6).stb when(s_msel = "110") else
wb_masters_i(7).stb when(s_msel = "111") else
'0';
end bahaviour;
-------------------------------------------------------------------------------
-- Title : Wishbone interconnect matrix for WR Core
-- Project : WhiteRabbit
-------------------------------------------------------------------------------
-- File : wb_conmax_top.vhd
-- Author : Grzegorz Daniluk
-- Company : Elproma
-- Created : 2011-02-12
-- Last update: 2011-09-14
-- Platform : FPGA-generics
-- Standard : VHDL
-------------------------------------------------------------------------------
-- Description:
-- Wishbone Interconnect Matrix, up to 8 Masters and 16 Slaves. Features
-- prioritized arbiter inside each Slave Interface (1, 2 of 4 priority levels).
-- Allows the parallel communication between masters and slaves on
-- different interfaces.
-- It is the WISHBONE Conmax IP Core from opencores.org rewritten in VHDL (from
-- Verilog) with some code optimalization.
--
-------------------------------------------------------------------------------
-- Copyright (C) 2000-2002 Rudolf Usselmann
-- Copyright (c) 2011 Grzegorz Daniluk (VHDL port)
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-02-12 1.0 greg.d Created
-- 2011-02-16 1.1 greg.d Using generates and types
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
library work;
use work.wbconmax_pkg.all;
entity wb_conmax_top is
generic(
g_rf_addr : integer range 0 to 15 := 15 --0xf
);
port(
clk_i : std_logic;
rst_i : std_logic;
wb_masters_i : in t_conmax_masters_i;
wb_masters_o : out t_conmax_masters_o;
wb_slaves_i : in t_conmax_slaves_i;
wb_slaves_o : out t_conmax_slaves_o
);
end wb_conmax_top;
architecture struct of wb_conmax_top is
component wb_conmax_master_if is
port(
clk_i : in std_logic;
rst_i : in std_logic;
--Master interface
wb_master_i : in t_wb_i;
wb_master_o : out t_wb_o;
--Slaves(0 to 15) interface
wb_slaves_i : in t_conmax_slaves_i;
wb_slaves_o : out t_conmax_slaves_o
);
end component;
component wb_conmax_slave_if is
generic(
g_pri_sel : integer := 2
);
port(
clk_i : in std_logic;
rst_i : in std_logic;
conf_i : in std_logic_vector(15 downto 0);
--Slave interface
wb_slave_i : in t_wb_o;
wb_slave_o : out t_wb_i;
--Master (0 to 7) interfaces
wb_masters_i : in t_conmax_masters_i;
wb_masters_o : out t_conmax_masters_o
);
end component;
component wb_conmax_rf is
generic(
g_rf_addr : integer range 0 to 15 := 15 --0xF
);
port(
clk_i : in std_logic;
rst_i : in std_logic;
--Internal WB interface
int_wb_i : in t_wb_i;
int_wb_o : out t_wb_o;
--External WB interface
ext_wb_i : in t_wb_o;
ext_wb_o : out t_wb_i;
--Configuration regs
conf_o : out t_rf_conf
);
end component;
signal intwb_s15_i : t_wishbone_master_in;
signal intwb_s15_o : t_wishbone_master_out;
--M0Sx
signal m0_slaves_i : t_conmax_slaves_i;
signal m0_slaves_o : t_conmax_slaves_o;
signal m1_slaves_i : t_conmax_slaves_i;
signal m1_slaves_o : t_conmax_slaves_o;
signal m2_slaves_i : t_conmax_slaves_i;
signal m2_slaves_o : t_conmax_slaves_o;
signal m3_slaves_i : t_conmax_slaves_i;
signal m3_slaves_o : t_conmax_slaves_o;
signal m4_slaves_i : t_conmax_slaves_i;
signal m4_slaves_o : t_conmax_slaves_o;
signal m5_slaves_i : t_conmax_slaves_i;
signal m5_slaves_o : t_conmax_slaves_o;
signal m6_slaves_i : t_conmax_slaves_i;
signal m6_slaves_o : t_conmax_slaves_o;
signal m7_slaves_i : t_conmax_slaves_i;
signal m7_slaves_o : t_conmax_slaves_o;
signal s_conf : t_rf_conf;
signal s15_wb_masters_i : t_conmax_masters_i;
signal s15_wb_masters_o : t_conmax_masters_o;
begin
--Master interfaces
M0: wb_conmax_master_if
port map(
clk_i => clk_i,
rst_i => rst_i,
--Master interface
wb_master_i => wb_masters_i(0),
wb_master_o => wb_masters_o(0),
--Slaves(0 to 15) interface
wb_slaves_i => m0_slaves_i,
wb_slaves_o => m0_slaves_o
);
M1: wb_conmax_master_if
port map(
clk_i => clk_i,
rst_i => rst_i,
wb_master_i => wb_masters_i(1),
wb_master_o => wb_masters_o(1),
wb_slaves_i => m1_slaves_i,
wb_slaves_o => m1_slaves_o
);
M2: wb_conmax_master_if
port map(
clk_i => clk_i,
rst_i => rst_i,
wb_master_i => wb_masters_i(2),
wb_master_o => wb_masters_o(2),
wb_slaves_i => m2_slaves_i,
wb_slaves_o => m2_slaves_o
);
M3: wb_conmax_master_if
port map(
clk_i => clk_i,
rst_i => rst_i,
wb_master_i => wb_masters_i(3),
wb_master_o => wb_masters_o(3),
wb_slaves_i => m3_slaves_i,
wb_slaves_o => m3_slaves_o
);
M4: wb_conmax_master_if
port map(
clk_i => clk_i,
rst_i => rst_i,
wb_master_i => wb_masters_i(4),
wb_master_o => wb_masters_o(4),
wb_slaves_i => m4_slaves_i,
wb_slaves_o => m4_slaves_o
);
M5: wb_conmax_master_if
port map(
clk_i => clk_i,
rst_i => rst_i,
wb_master_i => wb_masters_i(5),
wb_master_o => wb_masters_o(5),
wb_slaves_i => m5_slaves_i,
wb_slaves_o => m5_slaves_o
);
M6: wb_conmax_master_if
port map(
clk_i => clk_i,
rst_i => rst_i,
wb_master_i => wb_masters_i(6),
wb_master_o => wb_masters_o(6),
wb_slaves_i => m6_slaves_i,
wb_slaves_o => m6_slaves_o
);
M7: wb_conmax_master_if
port map(
clk_i => clk_i,
rst_i => rst_i,
wb_master_i => wb_masters_i(7),
wb_master_o => wb_masters_o(7),
wb_slaves_i => m7_slaves_i,
wb_slaves_o => m7_slaves_o
);
--------------------------------------------------
--Slave interfaces
S_GEN: for I in 0 to 14 generate
SLV: wb_conmax_slave_if
generic map(
g_pri_sel => g_pri_sel(I)
)
port map(
clk_i => clk_i,
rst_i => rst_i,
conf_i => s_conf(I),
--Slave interface
wb_slave_i => wb_slaves_i(I),
wb_slave_o => wb_slaves_o(I),
--Interfaces to masters
wb_masters_i(0) => m0_slaves_o(I),
wb_masters_i(1) => m1_slaves_o(I),
wb_masters_i(2) => m2_slaves_o(I),
wb_masters_i(3) => m3_slaves_o(I),
wb_masters_i(4) => m4_slaves_o(I),
wb_masters_i(5) => m5_slaves_o(I),
wb_masters_i(6) => m6_slaves_o(I),
wb_masters_i(7) => m7_slaves_o(I),
wb_masters_o(0) => m0_slaves_i(I),
wb_masters_o(1) => m1_slaves_i(I),
wb_masters_o(2) => m2_slaves_i(I),
wb_masters_o(3) => m3_slaves_i(I),
wb_masters_o(4) => m4_slaves_i(I),
wb_masters_o(5) => m5_slaves_i(I),
wb_masters_o(6) => m6_slaves_i(I),
wb_masters_o(7) => m7_slaves_i(I)
);
end generate;
s15_wb_masters_i(0) <= m0_slaves_o(15);
s15_wb_masters_i(1) <= m1_slaves_o(15);
s15_wb_masters_i(2) <= m2_slaves_o(15);
s15_wb_masters_i(3) <= m3_slaves_o(15);
s15_wb_masters_i(4) <= m4_slaves_o(15);
s15_wb_masters_i(5) <= m5_slaves_o(15);
s15_wb_masters_i(6) <= m6_slaves_o(15);
s15_wb_masters_i(7) <= m7_slaves_o(15);
m0_slaves_i(15) <= s15_wb_masters_o(0);
m1_slaves_i(15) <= s15_wb_masters_o(1);
m2_slaves_i(15) <= s15_wb_masters_o(2);
m3_slaves_i(15) <= s15_wb_masters_o(3);
m4_slaves_i(15) <= s15_wb_masters_o(4);
m5_slaves_i(15) <= s15_wb_masters_o(5);
m6_slaves_i(15) <= s15_wb_masters_o(6);
m7_slaves_i(15) <= s15_wb_masters_o(7);
SLV15: wb_conmax_slave_if
generic map(
g_pri_sel => g_pri_sel(15)
)
port map(
clk_i => clk_i,
rst_i => rst_i,
conf_i => s_conf(15),
--Slave interface
wb_slave_i => intwb_s15_i,
wb_slave_o => intwb_s15_o,
--Interfaces to masters
wb_masters_i => s15_wb_masters_i,
wb_masters_o => s15_wb_masters_o
);
---------------------------------------
--Register File
RF: wb_conmax_rf
generic map(
g_rf_addr => g_rf_addr
)
port map(
clk_i => clk_i,
rst_i => rst_i,
int_wb_i => intwb_s15_o,
int_wb_o => intwb_s15_i,
ext_wb_i => wb_slaves_i(15),
ext_wb_o => wb_slaves_o(15),
conf_o => s_conf
);
end struct;
-------------------------------------------------------------------------------
-- Title : Wishbone interconnect matrix for WR Core
-- Project : WhiteRabbit
-------------------------------------------------------------------------------
-- File : wbconmax_pkg.vhd
-- Author : Grzegorz Daniluk
-- Company : Elproma
-- Created : 2011-02-16
-- Last update: 2011-09-12
-- Platform : FPGA-generics
-- Standard : VHDL
-------------------------------------------------------------------------------
-- Description:
-- Package for WB interconnect matrix. Defines basic constants and types used
-- to simplify WB interface connections.
-------------------------------------------------------------------------------
-- Copyright (c) 2011 Grzegorz Daniluk
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-02-16 1.1 greg.d Using generates and types
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
package wbconmax_pkg is
type t_conmax_rf_conf is array(0 to 15) of std_logic_vector(15 downto 0);
type t_conmax_pri_sel is array(0 to 15) of integer range 0 to 3;
constant c_conmax_default_pri_sel : t_conmax_pri_sel := (2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2);
end wbconmax_pkg;
library ieee;
use ieee.std_logic_1164.all;
use work.wishbone_pkg.all;
use work.wbconmax_pkg.all;
entity xwb_conmax is
generic (
g_rf_addr : integer := 15;
g_num_slaves : integer;
g_num_masters : integer;
g_adr_width : integer;
g_sel_width : integer;
g_dat_width : integer;
g_priority_sel : t_conmax_pri_sel := c_conmax_default_pri_sel);
port(
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
slave_i : in t_wishbone_slave_in_array(0 to g_num_slaves-1);
slave_o : out t_wishbone_slave_out_array(0 to g_num_slaves-1);
master_i : in t_wishbone_master_in_array(0 to g_num_masters-1);
master_o : out t_wishbone_master_out_array(0 to g_num_masters-1)
);
end xwb_conmax;
architecture rtl of xwb_conmax is
component wb_conmax_master_if
generic (
g_adr_width : integer;
g_sel_width : integer;
g_dat_width : integer);
port (
clk_i : in std_logic;
rst_i : in std_logic;
wb_master_i : in t_wishbone_slave_in;
wb_master_o : out t_wishbone_slave_out;
wb_slaves_i : in t_wishbone_master_in_array(0 to 15);
wb_slaves_o : out t_wishbone_master_out_array(0 to 15));
end component;
component wb_conmax_slave_if
generic (
g_pri_sel : integer);
port (
clk_i : in std_logic;
rst_i : in std_logic;
conf_i : in std_logic_vector(15 downto 0);
wb_slave_i : in t_wishbone_master_in;
wb_slave_o : out t_wishbone_master_out;
wb_masters_i : in t_wishbone_slave_in_array(0 to 7);
wb_masters_o : out t_wishbone_slave_out_array(0 to 7));
end component;
component wb_conmax_rf
generic (
g_rf_addr : integer range 0 to 15;
g_adr_width : integer;
g_sel_width : integer;
g_dat_width : integer);
port (
clk_i : in std_logic;
rst_i : in std_logic;
int_wb_i : in t_wishbone_slave_in;
int_wb_o : out t_wishbone_slave_out;
ext_wb_i : in t_wishbone_master_in;
ext_wb_o : out t_wishbone_master_out;
conf_o : out t_conmax_rf_conf);
end component;
signal intwb_s15_i : t_wishbone_master_in;
signal intwb_s15_o : t_wishbone_master_out;
type t_conmax_slave_mux_in_array is array(integer range <>) of t_wishbone_master_in_array(0 to 15);
type t_conmax_slave_mux_out_array is array(integer range <>) of t_wishbone_master_out_array(0 to 15);
--M0Sx
signal m_slaves_in : t_conmax_slave_mux_in_array(0 to 7);
signal m_slaves_out : t_conmax_slave_mux_out_array(0 to 7);
signal s_conf : t_conmax_rf_conf;
signal s15_wb_masters_i : t_wishbone_slave_in_array(0 to 7);
signal s15_wb_masters_o : t_wishbone_slave_out_array(0 to 7);
signal rst : std_logic;
signal slave_i_int : t_wishbone_slave_in_array(0 to 7);
signal slave_o_int : t_wishbone_slave_out_array(0 to 7);
signal master_i_int : t_wishbone_master_in_array(0 to 15);
signal master_o_int : t_wishbone_master_out_array(0 to 15);
begin -- rtl
rst <= not rst_n_i;
gen_real_masters : for i in 0 to g_num_slaves-1 generate
slave_i_int(i) <= slave_i(i);
slave_o(i) <= slave_o_int(i);
end generate gen_real_masters;
gen_dummy_masters : for i in g_num_slaves to 7 generate
slave_i_int(i).cyc <= '0';
slave_i_int(i).stb <= '0';
slave_i_int(i).we <= '0';
slave_i_int(i).adr <= (others => '0');
slave_i_int(i).dat <= (others => '0');
end generate gen_dummy_masters;
gen_master_ifs : for i in 0 to 7 generate
U_Master_IF : wb_conmax_master_if
generic map (
g_adr_width => g_adr_width,
g_sel_width => g_sel_width,
g_dat_width => g_dat_width)
port map(
clk_i => clk_sys_i,
rst_i => rst,
--Master interface
wb_master_i => slave_i_int(i),
wb_master_o => slave_o_int(i),
--Slaves(0 to 15) interface
wb_slaves_i => m_slaves_in(i),
wb_slaves_o => m_slaves_out(i)
);
end generate gen_master_ifs;
gen_slave_ifs : for i in 0 to 14 generate
U_Slave_IF : wb_conmax_slave_if
generic map(
g_pri_sel => g_priority_sel(i)
)
port map(
clk_i => clk_sys_i,
rst_i => rst,
conf_i => s_conf(i),
--Slave interface
wb_slave_i => master_i_int(I),
wb_slave_o => master_o_int(I),
--Interfaces to masters
wb_masters_i(0) => m_slaves_out(0)(I),
wb_masters_i(1) => m_slaves_out(1)(I),
wb_masters_i(2) => m_slaves_out(2)(I),
wb_masters_i(3) => m_slaves_out(3)(I),
wb_masters_i(4) => m_slaves_out(4)(I),
wb_masters_i(5) => m_slaves_out(5)(I),
wb_masters_i(6) => m_slaves_out(6)(I),
wb_masters_i(7) => m_slaves_out(7)(I),
wb_masters_o(0) => m_slaves_in(0)(i),
wb_masters_o(1) => m_slaves_in(1)(I),
wb_masters_o(2) => m_slaves_in(2)(I),
wb_masters_o(3) => m_slaves_in(3)(I),
wb_masters_o(4) => m_slaves_in(4)(I),
wb_masters_o(5) => m_slaves_in(5)(I),
wb_masters_o(6) => m_slaves_in(6)(I),
wb_masters_o(7) => m_slaves_in(7)(I)
);
end generate gen_slave_ifs;
s15_wb_masters_i(0) <= m_slaves_out(0)(15);
s15_wb_masters_i(1) <= m_slaves_out(1)(15);
s15_wb_masters_i(2) <= m_slaves_out(2)(15);
s15_wb_masters_i(3) <= m_slaves_out(3)(15);
s15_wb_masters_i(4) <= m_slaves_out(4)(15);
s15_wb_masters_i(5) <= m_slaves_out(5)(15);
s15_wb_masters_i(6) <= m_slaves_out(6)(15);
s15_wb_masters_i(7) <= m_slaves_out(7)(15);
m_slaves_in(0)(15) <= s15_wb_masters_o(0);
m_slaves_in(1)(15) <= s15_wb_masters_o(1);
m_slaves_in(2)(15) <= s15_wb_masters_o(2);
m_slaves_in(3)(15) <= s15_wb_masters_o(3);
m_slaves_in(4)(15) <= s15_wb_masters_o(4);
m_slaves_in(5)(15) <= s15_wb_masters_o(5);
m_slaves_in(6)(15) <= s15_wb_masters_o(6);
m_slaves_in(7)(15) <= s15_wb_masters_o(7);
U_Slave15 : wb_conmax_slave_if
generic map(
g_pri_sel => g_priority_sel(15)
)
port map(
clk_i => clk_sys_i,
rst_i => rst,
conf_i => s_conf(15),
--Slave interface
wb_slave_i => intwb_s15_i,
wb_slave_o => intwb_s15_o,
--Interfaces to masters
wb_masters_i => s15_wb_masters_i,
wb_masters_o => s15_wb_masters_o
);
U_Reg_File : wb_conmax_rf
generic map(
g_rf_addr => g_rf_addr,
g_adr_width => g_adr_width,
g_sel_width => g_sel_width,
g_dat_width => g_dat_width
)
port map(
clk_i => clk_sys_i,
rst_i => rst,
int_wb_i => intwb_s15_o,
int_wb_o => intwb_s15_i,
ext_wb_i => master_i_int(15),
ext_wb_o => master_o_int(15),
conf_o => s_conf
);
gen_slaves_ports : for i in 0 to g_num_masters-1 generate
master_o(i) <= master_o_int(i);
master_i_int(i) <= master_i(i);
end generate gen_slaves_ports;
gen_unused_slave_ports : for i in g_num_masters to 15 generate
master_i_int(i).ack <= '0';
master_i_int(i).err <= '0';
master_i_int(i).rty <= '0';
end generate gen_unused_slave_ports;
end rtl;
-------------------------------------------------------------------------------
-- Title : An Wishbone delay buffer
-- Project : General Cores Library (gencores)
-------------------------------------------------------------------------------
-- File : xwb_crossbar.vhd
-- Author : Wesley W. Terpstra
-- Company : GSI
-- Created : 2013-12-16
-- Last update: 2013-12-16
-- Platform : FPGA-generic
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description:
--
-- Adds a register between two wishbone interfaces.
-- Useful to improve timing closure when placed between crossbars.
-- Be warned: it reduces the available bandwidth by a half.
--
-------------------------------------------------------------------------------
-- Copyright (c) 2011 GSI / Wesley W. Terpstra
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2013-12-16 1.0 wterpstra V1
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.wishbone_pkg.all;
entity xwb_register_link is
port(
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
slave_i : in t_wishbone_slave_in;
slave_o : out t_wishbone_slave_out;
master_i : in t_wishbone_master_in;
master_o : out t_wishbone_master_out);
end xwb_register_link;
architecture rtl of xwb_register_link is
signal r_slave : t_wishbone_slave_out;
signal r_master : t_wishbone_master_out;
begin
slave_o <= r_slave;
master_o <= r_master;
main : process(clk_sys_i, rst_n_i) is
begin
if rst_n_i = '0' then
r_slave <= cc_dummy_slave_out;
r_master <= cc_dummy_master_out;
r_slave.stall <= '0';
elsif rising_edge(clk_sys_i) then
-- no flow control on ack/err
r_slave <= master_i;
-- either we are accepting data (stb=0) or pushing data (stb=1)
if r_master.stb = '0' then
r_master <= slave_i;
r_master.stb <= slave_i.cyc and slave_i.stb;
r_slave.stall <= slave_i.cyc and slave_i.stb;
else
r_master.stb <= r_master.stb and master_i.stall;
r_slave.stall <= r_master.stb and master_i.stall;
end if;
end if;
end process;
end rtl;
files = [ "xwb_dma.vhd", "xwb_streamer.vhd" ];
-- Register map:
-- 0x00 = read issue address
-- 0x04 = write issue address
-- 0x08 = read stride
-- 0x0C = write stride
-- 0x10 = transfer count
--
-- Behaviour:
-- While (transfer count > 0) {
-- mem[write_issue_address] = mem[read_issue_address]
-- read_issue_address += read_stride
-- write_issue_address += write_stride
-- }
-- interrupt = (transfer_count == 0) && (transfer_count_was == 1)
--
-- Usage:
-- 1. Fill in the issue and stride registers
-- 2. Write non-zero to the counter to initiate transfer
--
-- Status information:
-- All registers can be inspected during the transfer
-- Interrupt line is raised upon completion
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wishbone_pkg.all;
use work.genram_pkg.all;
-- Assumption: wishbone_data_width >= wishbone_address_Width
entity xwb_dma is
generic(
-- Value 0 cannot stream
-- Value 1 only slaves with async ACK can stream
-- Value 2 only slaves with combined latency = 2 can stream
-- Value 3 only slaves with combined latency = 6 can stream
-- Value 4 only slaves with combined latency = 14 can stream
-- ....
logRingLen : integer := 4
);
port(
-- Common wishbone signals
clk_i : in std_logic;
rst_n_i : in std_logic;
-- Slave control port
slave_i : in t_wishbone_slave_in;
slave_o : out t_wishbone_slave_out;
-- Master reader port
r_master_i : in t_wishbone_master_in;
r_master_o : out t_wishbone_master_out;
-- Master writer port
w_master_i : in t_wishbone_master_in;
w_master_o : out t_wishbone_master_out;
-- Pulsed high completion signal
interrupt_o : out std_logic
);
end xwb_dma;
architecture rtl of xwb_dma is
-- State registers (pointer into the ring)
-- Invariant: read_issue_offset >= read_result_offset >= write_issue_offset >= write_result_offset
-- read_issue_offset - write_result_offset <= ringLen (*NOT* strict '<')
signal read_issue_offset : unsigned(logRingLen downto 0);
signal read_result_offset : unsigned(logRingLen downto 0);
signal write_issue_offset : unsigned(logRingLen downto 0);
signal write_result_offset : unsigned(logRingLen downto 0);
-- DMA control registers
signal read_issue_address : t_wishbone_address;
signal write_issue_address : t_wishbone_address;
signal read_stride : t_wishbone_address;
signal write_stride : t_wishbone_address;
signal transfer_count : t_wishbone_address;
-- result status: fail/ok ?
-- Registered wishbone control signals
signal r_master_o_CYC : std_logic;
signal w_master_o_CYC : std_logic;
signal r_master_o_STB : std_logic;
signal w_master_o_STB : std_logic;
signal slave_o_ACK : std_logic;
signal slave_o_DAT : t_wishbone_data;
signal read_issue_progress : boolean;
signal read_result_progress : boolean;
signal write_issue_progress : boolean;
signal write_result_progress : boolean;
signal new_read_issue_offset : unsigned(logRingLen downto 0);
signal new_read_result_offset : unsigned(logRingLen downto 0);
signal new_write_issue_offset : unsigned(logRingLen downto 0);
signal new_write_result_offset : unsigned(logRingLen downto 0);
signal new_transfer_count : t_wishbone_address;
signal ring_boundary : boolean;
signal ring_high : boolean;
signal ring_full : boolean;
signal ring_empty : boolean;
signal done_transfer : boolean;
function active_high(x : boolean)
return std_logic is
begin
if (x) then
return '1';
else
return '0';
end if;
end active_high;
function index(x : unsigned(logRingLen downto 0))
return std_logic_vector is
begin
return std_logic_vector(x(logRingLen-1 downto 0));
end index;
procedure update(signal o : out t_wishbone_address) is
begin
for i in (c_wishbone_data_width/8)-1 downto 0 loop
if slave_i.SEL(i) = '1' then
o(i*8+7 downto i*8) <= slave_i.DAT(i*8+7 downto i*8);
end if;
end loop;
end update;
begin
-- Hard-wired slave pins
slave_o.ACK <= slave_o_ACK;
slave_o.ERR <= '0';
slave_o.RTY <= '0';
slave_o.STALL <= '0';
slave_o.DAT <= slave_o_DAT;
-- Hard-wired master pins
r_master_o.CYC <= r_master_o_CYC;
w_master_o.CYC <= w_master_o_CYC;
r_master_o.STB <= r_master_o_STB;
w_master_o.STB <= w_master_o_STB;
r_master_o.ADR <= read_issue_address;
w_master_o.ADR <= write_issue_address;
r_master_o.SEL <= (others => '1');
w_master_o.SEL <= (others => '1');
r_master_o.WE <= '0';
w_master_o.WE <= '1';
r_master_o.DAT <= (others => '0');
-- Detect bus progress
read_issue_progress <= r_master_o_STB = '1' and r_master_i.STALL = '0';
write_issue_progress <= w_master_o_STB = '1' and w_master_i.STALL = '0';
read_result_progress <= r_master_o_CYC = '1' and (r_master_i.ACK = '1' or r_master_i.ERR = '1' or r_master_i.RTY = '1');
write_result_progress <= w_master_o_CYC = '1' and (w_master_i.ACK = '1' or w_master_i.ERR = '1' or w_master_i.RTY = '1');
-- Advance read pointers
new_read_issue_offset <= (read_issue_offset + 1) when read_issue_progress else read_issue_offset;
new_read_result_offset <= (read_result_offset + 1) when read_result_progress else read_result_offset;
-- Advance write pointers
new_write_issue_offset <= (write_issue_offset + 1) when write_issue_progress else write_issue_offset;
new_write_result_offset <= (write_result_offset + 1) when write_result_progress else write_result_offset;
new_transfer_count <= std_logic_vector(unsigned(transfer_count) - 1) when read_issue_progress else transfer_count;
ring_boundary <= index(new_read_issue_offset) = index(new_write_result_offset);
ring_high <= new_read_issue_offset(logRingLen) /= new_write_result_offset(logRingLen);
ring_full <= ring_boundary and ring_high;
ring_empty <= ring_boundary and not ring_high;
-- Shorten the critical path by comparing to the undecremented value
--done_transfer := unsigned(new_transfer_count) = 0;
done_transfer <= unsigned(transfer_count(c_wishbone_address_width-1 downto 1)) = 0
and (read_issue_progress or transfer_count(0) = '0');
ring : generic_simple_dpram
generic map(
g_data_width => 32,
g_size => 2**logRingLen,
g_dual_clock => false)
port map(
rst_n_i => rst_n_i,
clka_i => clk_i,
wea_i => active_high(read_result_progress),
aa_i => index(read_result_offset),
da_i => r_master_i.DAT,
clkb_i => clk_i,
ab_i => index(new_write_issue_offset),
qb_o => w_master_o.DAT);
main : process(clk_i)
begin
if (rising_edge(clk_i)) then
if (rst_n_i = '0') then
read_issue_offset <= (others => '0');
read_result_offset <= (others => '0');
write_issue_offset <= (others => '0');
write_result_offset <= (others => '0');
read_issue_address <= (others => '0');
write_issue_address <= (others => '0');
read_stride <= (others => '0');
write_stride <= (others => '0');
transfer_count <= (others => '0');
r_master_o_CYC <= '0';
w_master_o_CYC <= '0';
r_master_o_STB <= '0';
w_master_o_STB <= '0';
slave_o_ACK <= '0';
slave_o_DAT <= (others => '0');
interrupt_o <= '0';
else
-- Output any read the user requests
case to_integer(unsigned(slave_i.ADR(4 downto 2))) is
when 0 => slave_o_DAT <= read_issue_address;
when 1 => slave_o_DAT <= write_issue_address;
when 2 => slave_o_DAT <= read_stride;
when 3 => slave_o_DAT <= write_stride;
when 4 => slave_o_DAT <= transfer_count;
when others => slave_o_DAT <= (others => '0');
end case;
if read_issue_progress then
read_issue_address <= std_logic_vector(unsigned(read_issue_address) + unsigned(read_stride));
end if;
if write_issue_progress then
write_issue_address <= std_logic_vector(unsigned(write_issue_address) + unsigned(write_stride));
end if;
r_master_o_STB <= active_high (not ring_full and not done_transfer);
r_master_o_CYC <= active_high((not ring_full and not done_transfer) or
(new_read_result_offset /= new_read_issue_offset));
w_master_o_STB <= active_high (new_write_issue_offset /= read_result_offset); -- avoid read during write
w_master_o_CYC <= active_high (new_write_result_offset /= read_result_offset);
interrupt_o <= active_high (write_result_progress and done_transfer and ring_empty);
transfer_count <= new_transfer_count;
read_issue_offset <= new_read_issue_offset;
read_result_offset <= new_read_result_offset;
write_issue_offset <= new_write_issue_offset;
write_result_offset <= new_write_result_offset;
-- Control logic
if (slave_i.CYC = '1' and slave_i.STB = '1' and slave_i.WE = '1') then
case to_integer(unsigned(slave_i.ADR(4 downto 2))) is
when 0 => update(read_issue_address);
when 1 => update(write_issue_address);
when 2 => update(read_stride);
when 3 => update(write_stride);
when 4 => update(transfer_count);
when others => null;
end case;
end if;
slave_o_ACK <= slave_i.CYC and slave_i.STB;
end if;
end if;
end process;
end rtl;
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.wishbone_pkg.all;
entity xwb_streamer is
generic(
-- Value 0 cannot stream
-- Value 1 only slaves with async ACK can stream
-- Value 2 only slaves with combined latency = 2 can stream
-- Value 3 only slaves with combined latency = 6 can stream
-- Value 4 only slaves with combined latency = 14 can stream
-- ....
logRingLen : integer := 4
);
port(
-- Common wishbone signals
clk_i : in std_logic;
rst_n_i : in std_logic;
-- Master reader port
r_master_i : in t_wishbone_master_in;
r_master_o : out t_wishbone_master_out;
-- Master writer port
w_master_i : in t_wishbone_master_in;
w_master_o : out t_wishbone_master_out);
end xwb_streamer;
architecture rtl of xwb_streamer is
signal slave : t_wishbone_slave_in;
begin
slave.cyc <= '1';
slave.stb <= '1';
slave.we <= '1';
slave.adr <= x"00000010"; -- transfer count
slave.sel <= (others => '1');
slave.dat <= (others => '1');
dma: xwb_dma
generic map(
logRingLen => logRingLen)
port map(
clk_i => clk_i,
rst_n_i => rst_n_i,
slave_i => slave,
slave_o => open,
r_master_i => r_master_i,
r_master_o => r_master_o,
w_master_i => w_master_i,
w_master_o => w_master_o,
interrupt_o => open);
end rtl;
files = ["xwb_dpram.vhd"]
\ No newline at end of file
-------------------------------------------------------------------------------
-- Title : Dual-port RAM for WR core
-- Project : WhiteRabbit
-------------------------------------------------------------------------------
-- File : wrc_dpram.vhd
-- Author : Grzegorz Daniluk
-- Company : Elproma
-- Created : 2011-02-15
-- Last update: 2011-09-26
-- Platform : FPGA-generics
-- Standard : VHDL '93
-------------------------------------------------------------------------------
-- Description:
--
-- Dual port RAM with wishbone interface
-------------------------------------------------------------------------------
-- Copyright (c) 2011 Grzegorz Daniluk
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-02-15 1.0 greg.d Created
-- 2011-06-09 1.01 twlostow Removed unnecessary generics
-- 2011-21-09 1.02 twlostow Struct-ized version
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.genram_pkg.all;
use work.wishbone_pkg.all;
entity xwb_dpram is
generic(
g_size : natural := 16384;
g_init_file : string := "";
g_must_have_init_file : boolean := true;
g_slave1_interface_mode : t_wishbone_interface_mode;
g_slave2_interface_mode : t_wishbone_interface_mode;
g_slave1_granularity : t_wishbone_address_granularity;
g_slave2_granularity : t_wishbone_address_granularity
);
port(
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
slave1_i : in t_wishbone_slave_in;
slave1_o : out t_wishbone_slave_out;
slave2_i : in t_wishbone_slave_in;
slave2_o : out t_wishbone_slave_out
);
end xwb_dpram;
architecture struct of xwb_dpram is
function f_zeros(size : integer)
return std_logic_vector is
begin
return std_logic_vector(to_unsigned(0, size));
end f_zeros;
signal s_wea : std_logic;
signal s_web : std_logic;
signal s_bwea : std_logic_vector(3 downto 0);
signal s_bweb : std_logic_vector(3 downto 0);
signal slave1_in : t_wishbone_slave_in;
signal slave1_out : t_wishbone_slave_out;
signal slave2_in : t_wishbone_slave_in;
signal slave2_out : t_wishbone_slave_out;
begin
U_Adapter1 : wb_slave_adapter
generic map (
g_master_use_struct => true,
g_master_mode => g_slave1_interface_mode,
g_master_granularity => WORD,
g_slave_use_struct => true,
g_slave_mode => g_slave1_interface_mode,
g_slave_granularity => g_slave1_granularity)
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
slave_i => slave1_i,
slave_o => slave1_o,
master_i => slave1_out,
master_o => slave1_in);
U_Adapter2 : wb_slave_adapter
generic map (
g_master_use_struct => true,
g_master_mode => g_slave2_interface_mode,
g_master_granularity => WORD,
g_slave_use_struct => true,
g_slave_mode => g_slave2_interface_mode,
g_slave_granularity => g_slave2_granularity)
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
slave_i => slave2_i,
slave_o => slave2_o,
master_i => slave2_out,
master_o => slave2_in);
U_DPRAM : generic_dpram
generic map(
-- standard parameters
g_data_width => 32,
g_size => g_size,
g_with_byte_enable => true,
g_addr_conflict_resolution => "dont_care",
g_init_file => g_init_file,
g_dual_clock => false
)
port map(
rst_n_i => rst_n_i,
-- Port A
clka_i => clk_sys_i,
bwea_i => s_bwea,
wea_i => s_wea,
aa_i => slave1_in.adr(f_log2_size(g_size)-1 downto 0),
da_i => slave1_in.dat,
qa_o => slave1_out.dat,
-- Port B
clkb_i => clk_sys_i,
bweb_i => s_bweb,
web_i => s_web,
ab_i => slave2_in.adr(f_log2_size(g_size)-1 downto 0),
db_i => slave2_in.dat,
qb_o => slave2_out.dat
);
-- I know this looks weird, but otherwise ISE generates distributed RAM instead of block
-- RAM
s_bwea <= slave1_in.sel when s_wea = '1' else f_zeros(c_wishbone_data_width/8);
s_bweb <= slave2_in.sel when s_web = '1' else f_zeros(c_wishbone_data_width/8);
s_wea <= slave1_in.we and slave1_in.stb and slave1_in.cyc;
s_web <= slave2_in.we and slave2_in.stb and slave2_in.cyc;
process(clk_sys_i)
begin
if(rising_edge(clk_sys_i)) then
if(rst_n_i = '0') then
slave1_out.ack <= '0';
slave2_out.ack <= '0';
else
if(slave1_out.ack = '1' and g_slave1_interface_mode = CLASSIC) then
slave1_out.ack <= '0';
else
slave1_out.ack <= slave1_in.cyc and slave1_in.stb;
end if;
if(slave2_out.ack = '1' and g_slave2_interface_mode = CLASSIC) then
slave2_out.ack <= '0';
else
slave2_out.ack <= slave2_in.cyc and slave2_in.stb;
end if;
end if;
end if;
end process;
slave1_out.stall <= '0';
slave2_out.stall <= '0';
slave1_out.err <= '0';
slave2_out.err <= '0';
slave1_out.rty <= '0';
slave2_out.rty <= '0';
end struct;
files = ["wb_gpio_port.vhd",
"xwb_gpio_port.vhd"];
\ No newline at end of file
------------------------------------------------------------------------------
-- Title : Wishbone GPIO port
-- Project : General Core Collection (gencores) Library
------------------------------------------------------------------------------
-- Author : Tomasz Wlostowski
-- Company : CERN BE-Co-HT
-- Created : 2010-05-18
-- Last update: 2011-10-05
-- Platform : FPGA-generic
-- Standard : VHDL'87
-------------------------------------------------------------------------------
-- Description: Bidirectional GPIO port of configurable width (1 to 256 bits).
-------------------------------------------------------------------------------
-- Copyright (c) 2010, 2011 CERN
--
--
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2010-05-18 1.0 twlostow Created
-- 2010-10-04 1.1 twlostow Added WB slave adapter
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wishbone_pkg.all;
use work.gencores_pkg.all;
entity wb_gpio_port is
generic(
g_interface_mode : t_wishbone_interface_mode := CLASSIC;
g_address_granularity : t_wishbone_address_granularity := WORD;
g_num_pins : natural range 1 to 256 := 32;
g_with_builtin_tristates : boolean := false
);
port(
-- System reset, active low
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
wb_sel_i : in std_logic_vector(c_wishbone_data_width/8-1 downto 0);
wb_cyc_i : in std_logic;
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_adr_i : in std_logic_vector(7 downto 0);
wb_dat_i : in std_logic_vector(c_wishbone_data_width-1 downto 0);
wb_dat_o : out std_logic_vector(c_wishbone_data_width-1 downto 0);
wb_ack_o : out std_logic;
wb_stall_o : out std_logic;
gpio_b : inout std_logic_vector(g_num_pins-1 downto 0);
gpio_out_o : out std_logic_vector(g_num_pins-1 downto 0);
gpio_in_i : in std_logic_vector(g_num_pins-1 downto 0);
gpio_oen_o : out std_logic_vector(g_num_pins-1 downto 0)
);
end wb_gpio_port;
architecture behavioral of wb_gpio_port is
constant c_GPIO_REG_CODR : std_logic_vector(2 downto 0) := "000"; -- *reg* clear output register
constant c_GPIO_REG_SODR : std_logic_vector(2 downto 0) := "001"; -- *reg* set output register
constant c_GPIO_REG_DDR : std_logic_vector(2 downto 0) := "010"; -- *reg* data direction register
constant c_GPIO_REG_PSR : std_logic_vector(2 downto 0) := "011"; -- *reg* pin state register
constant c_NUM_BANKS : integer := (g_num_pins+31) / 32;
signal out_reg, in_reg, dir_reg : std_logic_vector(32*c_NUM_BANKS-1 downto 0);
signal gpio_in : std_logic_vector(32*c_NUM_BANKS-1 downto 0);
signal gpio_in_synced : std_logic_vector(32*c_NUM_BANKS-1 downto 0);
signal ack_int : std_logic;
signal sor_wr : std_logic_vector(c_NUM_BANKS-1 downto 0);
signal cor_wr : std_logic_vector(c_NUM_BANKS-1 downto 0);
signal ddr_wr : std_logic_vector(c_NUM_BANKS-1 downto 0);
signal write_mask : std_logic_vector(7 downto 0);
signal wb_in : t_wishbone_slave_in;
signal wb_out : t_wishbone_slave_out;
signal sel : std_logic;
signal resized_addr : std_logic_vector(c_wishbone_address_width-1 downto 0);
begin
resized_addr(7 downto 0) <= wb_adr_i;
resized_addr(c_wishbone_address_width-1 downto 8) <= (others => '0');
U_Adapter : wb_slave_adapter
generic map (
g_master_use_struct => true,
g_master_mode => CLASSIC,
g_master_granularity => WORD,
g_slave_use_struct => false,
g_slave_mode => g_interface_mode,
g_slave_granularity => g_address_granularity)
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
master_i => wb_out,
master_o => wb_in,
sl_adr_i => resized_addr,
sl_dat_i => wb_dat_i,
sl_sel_i => wb_sel_i,
sl_cyc_i => wb_cyc_i,
sl_stb_i => wb_stb_i,
sl_we_i => wb_we_i,
sl_dat_o => wb_dat_o,
sl_ack_o => wb_ack_o,
sl_stall_o => wb_stall_o);
sel <= '1' when (unsigned(not wb_in.sel) = 0) else '0';
GEN_SYNC_FFS : for i in 0 to g_num_pins-1 generate
INPUT_SYNC : gc_sync_ffs
generic map (
g_sync_edge => "positive")
port map (
rst_n_i => rst_n_i,
clk_i => clk_sys_i,
data_i => gpio_in(i),
synced_o => gpio_in_synced(i),
npulse_o => open
);
end generate GEN_SYNC_FFS;
p_gen_write_mask : process(wb_in.adr)
begin
case wb_in.adr(5 downto 3) is
when "000" => write_mask <= x"01";
when "001" => write_mask <= x"02";
when "010" => write_mask <= x"04";
when "011" => write_mask <= x"08";
when "100" => write_mask <= x"10";
when "101" => write_mask <= x"20";
when "110" => write_mask <= x"40";
when "111" => write_mask <= x"80";
when others => write_mask <= x"00";
end case;
end process;
p_gen_write_strobes : process(write_mask, wb_in.adr, wb_in.we, wb_in.cyc, wb_in.stb, sel)
begin
if(wb_in.we = '1' and wb_in.cyc = '1' and wb_in.stb = '1' and sel = '1') then
case wb_in.adr(2 downto 0) is
when c_GPIO_REG_CODR =>
cor_wr <= write_mask(c_NUM_BANKS-1 downto 0);
sor_wr <= (others => '0');
ddr_wr <= (others => '0');
when c_GPIO_REG_SODR =>
cor_wr <= (others => '0');
sor_wr <= write_mask(c_NUM_BANKS-1 downto 0);
ddr_wr <= (others => '0');
when c_GPIO_REG_DDR =>
sor_wr <= (others => '0');
cor_wr <= (others => '0');
ddr_wr <= write_mask(c_NUM_BANKS-1 downto 0);
when others =>
sor_wr <= (others => '0');
cor_wr <= (others => '0');
ddr_wr <= (others => '0');
end case;
else
sor_wr <= (others => '0');
cor_wr <= (others => '0');
ddr_wr <= (others => '0');
end if;
end process;
gen_banks_wr : for i in 0 to c_NUM_BANKS-1 generate
process (clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
dir_reg(32 * i + 31 downto 32 * i) <= (others => '0');
out_reg(32 * i + 31 downto 32 * i) <= (others => '0');
else
if(sor_wr(i) = '1') then
out_reg(i * 32 + 31 downto i * 32) <= out_reg(i * 32 + 31 downto i * 32) or wb_in.dat;
end if;
if(cor_wr(i) = '1') then
out_reg(i * 32 + 31 downto i * 32) <= out_reg(i * 32 + 31 downto i * 32) and (not wb_in.dat);
end if;
if(ddr_wr(i) = '1') then
dir_reg(i * 32 + 31 downto i * 32) <= wb_in.dat;
end if;
end if;
end if;
end process;
end generate gen_banks_wr;
p_wb_reads : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
wb_out.dat <= (others => '0');
else
wb_out.dat <= (others => 'X');
case wb_in.adr(2 downto 0) is
when c_GPIO_REG_DDR =>
for i in 0 to c_NUM_BANKS-1 loop
if(to_integer(unsigned(wb_in.adr(5 downto 3))) = i) then
wb_out.dat <= dir_reg(32 * i + 31 downto 32 * i);
end if;
end loop; -- i
when c_GPIO_REG_PSR =>
for i in 0 to c_NUM_BANKS-1 loop
if(to_integer(unsigned(wb_in.adr(5 downto 3))) = i) then
wb_out.dat <= gpio_in_synced(32 * i + 31 downto 32 * i);
end if;
end loop; -- i
when others => null;
end case;
end if;
end if;
end process;
p_gen_ack : process (clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
ack_int <= '0';
else
if(ack_int = '1') then
ack_int <= '0';
elsif(wb_in.cyc = '1') and (wb_in.stb = '1') then
ack_int <= '1';
end if;
end if;
end if;
end process;
gen_with_tristates : if(g_with_builtin_tristates) generate
gpio_out_tristate : process (out_reg, dir_reg)
begin
for i in 0 to g_num_pins-1 loop
if(dir_reg(i) = '1') then
gpio_b(i) <= out_reg(i);
else
gpio_b(i) <= 'Z';
end if;
end loop;
end process gpio_out_tristate;
gpio_in <= gpio_b;
end generate gen_with_tristates;
gen_without_tristates : if (not g_with_builtin_tristates) generate
gpio_out_o <= out_reg(g_num_pins-1 downto 0);
gpio_in(g_num_pins-1 downto 0) <= gpio_in_i;
gpio_oen_o <= dir_reg(g_num_pins-1 downto 0);
end generate gen_without_tristates;
wb_out.ack <= ack_int;
wb_out.stall <= '0';
wb_out.err <= '0';
wb_out.int <= '0';
wb_out.rty <='0';
end behavioral;
library ieee;
use ieee.std_logic_1164.all;
use work.wishbone_pkg.all;
entity xwb_gpio_port is
generic(
g_interface_mode : t_wishbone_interface_mode := CLASSIC;
g_address_granularity : t_wishbone_address_granularity := WORD;
g_num_pins : natural range 1 to 256 := 32;
g_with_builtin_tristates : boolean := false
);
port(
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
-- Wishbone
slave_i : in t_wishbone_slave_in;
slave_o : out t_wishbone_slave_out;
desc_o : out t_wishbone_device_descriptor;
gpio_b : inout std_logic_vector(g_num_pins-1 downto 0);
gpio_out_o : out std_logic_vector(g_num_pins-1 downto 0);
gpio_in_i : in std_logic_vector(g_num_pins-1 downto 0);
gpio_oen_o : out std_logic_vector(g_num_pins-1 downto 0)
);
end xwb_gpio_port;
architecture rtl of xwb_gpio_port is
component wb_gpio_port
generic (
g_interface_mode : t_wishbone_interface_mode;
g_address_granularity : t_wishbone_address_granularity;
g_num_pins : natural range 1 to 256;
g_with_builtin_tristates : boolean);
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
wb_sel_i : in std_logic_vector(c_wishbone_data_width/8-1 downto 0);
wb_cyc_i : in std_logic;
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_adr_i : in std_logic_vector(7 downto 0);
wb_dat_i : in std_logic_vector(c_wishbone_data_width-1 downto 0);
wb_dat_o : out std_logic_vector(c_wishbone_data_width-1 downto 0);
wb_ack_o : out std_logic;
wb_stall_o : out std_logic;
gpio_b : inout std_logic_vector(g_num_pins-1 downto 0);
gpio_out_o : out std_logic_vector(g_num_pins-1 downto 0);
gpio_in_i : in std_logic_vector(g_num_pins-1 downto 0);
gpio_oen_o : out std_logic_vector(g_num_pins-1 downto 0));
end component;
begin -- rtl
Wrapped_GPIO : wb_gpio_port
generic map (
g_num_pins => g_num_pins,
g_with_builtin_tristates => g_with_builtin_tristates,
g_interface_mode => g_interface_mode,
g_address_granularity => g_address_granularity)
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
wb_sel_i => slave_i.sel,
wb_cyc_i => slave_i.cyc,
wb_stb_i => slave_i.stb,
wb_we_i => slave_i.we,
wb_adr_i => slave_i.adr(7 downto 0),
wb_dat_i => slave_i.dat(31 downto 0),
wb_dat_o => slave_o.dat(31 downto 0),
wb_ack_o => slave_o.ack,
wb_stall_o => slave_o.stall,
gpio_b => gpio_b,
gpio_out_o => gpio_out_o,
gpio_in_i => gpio_in_i,
gpio_oen_o => gpio_oen_o);
slave_o.err <= '0';
slave_o.int <= '0';
slave_o.rty <= '0';
end rtl;
--==============================================================================
-- CERN (BE-CO-HT)
-- VME Board Control Protocol (VBCP) to Wishbone bridge for VME64x crates
--==============================================================================
--
-- author: Theodor Stana (t.stana@cern.ch)
--
-- date of creation: 2013-02-15
--
-- version: 1.0
--
-- description:
-- This module implements an I2C to Wishbone bridge for VME64x crates,
-- following the protocol defined in [1]. It uses a low-level I2C slave module
-- reacting to transfers initiated by an I2C master, in this case, a VME64x
-- system monitor (SysMon) [2].
--
-- The I2C slave module sets its done_p_o pin high when the I2C address received
-- from the SysMon corresponds to the slave address and every time a byte has
-- been received or sent correctly. The done_p_o pin of the slave module is
-- de-asserted when the slave performs a transfer.
--
-- The bridge module employs a state machine that checks for low-to-high
-- transitions in the slave done_p_o pin and shifts bytes in and out over I2C
-- to implement the protocol defined in [1].
--
-- dependencies:
-- none.
--
-- references:
-- [1] ELMA SNMP Specification
-- http://www.ohwr.org/documents/227
-- [2] System Monitor's Users Manual
-- http://www.ohwr.org/documents/226
--
--==============================================================================
-- GNU LESSER GENERAL PUBLIC LICENSE
--==============================================================================
-- This source file is free software; you can redistribute it and/or modify it
-- under the terms of the GNU Lesser General Public License as published by the
-- Free Software Foundation; either version 2.1 of the License, or (at your
-- option) any later version. This source 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 Lesser General Public License for more details. You should have
-- received a copy of the GNU Lesser General Public License along with this
-- source; if not, download it from http://www.gnu.org/licenses/lgpl-2.1.html
--==============================================================================
-- last changes:
-- 2013-02-28 Theodor Stana t.stana@cern.ch File created
--==============================================================================
-- TODO: -
--==============================================================================
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.gencores_pkg.all;
entity wb_i2c_bridge is
port
(
-- Clock, reset
clk_i : in std_logic;
rst_n_i : in std_logic;
-- I2C lines
scl_i : in std_logic;
scl_o : out std_logic;
scl_en_o : out std_logic;
sda_i : in std_logic;
sda_o : out std_logic;
sda_en_o : out std_logic;
-- I2C address
i2c_addr_i : in std_logic_vector(6 downto 0);
-- Status outputs
-- TIP : Transfer In Progress
-- '1' when the I2C slave detects a matching I2C address, thus a
-- transfer is in progress
-- '0' when idle
-- ERR : Error
-- '1' when the SysMon attempts to access an invalid WB slave
-- '0' when idle
-- WDTO : Watchdog timeout (single clock cycle pulse)
-- '1' -- timeout of watchdog occured
-- '0' -- when idle
tip_o : out std_logic;
err_p_o : out std_logic;
wdto_p_o : out std_logic;
-- Wishbone master signals
wbm_stb_o : out std_logic;
wbm_cyc_o : out std_logic;
wbm_sel_o : out std_logic_vector(3 downto 0);
wbm_we_o : out std_logic;
wbm_dat_i : in std_logic_vector(31 downto 0);
wbm_dat_o : out std_logic_vector(31 downto 0);
wbm_adr_o : out std_logic_vector(31 downto 0);
wbm_ack_i : in std_logic;
wbm_rty_i : in std_logic;
wbm_err_i : in std_logic
);
end entity wb_i2c_bridge;
architecture behav of wb_i2c_bridge is
--============================================================================
-- Type declarations
--============================================================================
type t_state is
(
IDLE, -- idle state
SYSMON_WB_ADR, -- get the WB register address
SIM_WB_TRANSF, -- simulate a WB transfer with the received address
OPER, -- operation to perform on the WB register
SYSMON_RD_WB, -- perform a WB read transfer, for sending word to the SysMon
SYSMON_RD, -- send the word to the SysMon during read transfer
SYSMON_WR, -- read the word sent by the SysMon during write transfer
SYSMON_WR_WB -- perform a WB write transfer, storing the received word
);
--============================================================================
-- Signal declarations
--============================================================================
-- Slave component signals
signal slv_ack : std_logic;
signal op : std_logic;
signal tx_byte : std_logic_vector(7 downto 0);
signal rx_byte : std_logic_vector(7 downto 0);
signal slv_sta_p : std_logic;
signal slv_sto_p : std_logic;
signal slv_addr_good_p : std_logic;
signal slv_r_done_p : std_logic;
signal slv_w_done_p : std_logic;
-- Wishbone temporary signals
signal wb_dat_out : std_logic_vector(31 downto 0);
signal wb_dat_in : std_logic_vector(31 downto 0);
signal wb_adr : std_logic_vector(15 downto 0);
signal wb_cyc : std_logic;
signal wb_stb : std_logic;
signal wb_we : std_logic;
signal wb_ack : std_logic;
signal wb_err : std_logic;
signal wb_rty : std_logic;
-- FSM control signals
signal state : t_state;
signal dat_byte_cnt : unsigned(1 downto 0);
signal adr_byte_cnt : unsigned(0 downto 0);
-- FSM watchdog signals
signal wdt_rst : std_logic;
signal rst_fr_wdt : std_logic;
signal rst_fr_wdt_d0 : std_logic;
--==============================================================================
-- architecture begin
--==============================================================================
begin
--============================================================================
-- Slave component instantiation and connection
--============================================================================
cmp_i2c_slave: gc_i2c_slave
generic map
(
g_gf_len => 8
)
port map
(
clk_i => clk_i,
rst_n_i => rst_n_i,
scl_i => scl_i,
scl_o => scl_o,
scl_en_o => scl_en_o,
sda_i => sda_i,
sda_o => sda_o,
sda_en_o => sda_en_o,
i2c_addr_i => i2c_addr_i,
ack_i => slv_ack,
tx_byte_i => tx_byte,
rx_byte_o => rx_byte,
i2c_sta_p_o => open,
i2c_sto_p_o => slv_sto_p,
addr_good_p_o => slv_addr_good_p,
r_done_p_o => slv_r_done_p,
w_done_p_o => slv_w_done_p,
op_o => op
);
--============================================================================
-- I2C to Wishbone bridge FSM logic
--============================================================================
-- First, assign Wishbone outputs
wbm_dat_o <= wb_dat_out;
wbm_adr_o <= x"0000" & wb_adr;
wbm_cyc_o <= wb_cyc;
wbm_stb_o <= wb_stb;
wbm_we_o <= wb_we;
wbm_sel_o <= (others => '1');
-- Next, assign some Wishbone inputs to internal signals
wb_ack <= wbm_ack_i;
wb_err <= wbm_err_i;
wb_rty <= wbm_rty_i;
-- Then, assign the I2C byte to TX to the first byte of the internal WB input
-- data signal; shifting is handled inside the FSM.
tx_byte <= wb_dat_in(7 downto 0);
-- Finally, the FSM logic
p_fsm: process (clk_i) is
begin
if rising_edge(clk_i) then
if (rst_n_i = '0') or (rst_fr_wdt = '1') then
state <= IDLE;
wb_adr <= (others => '0');
wb_dat_out <= (others => '0');
wb_dat_in <= (others => '0');
wb_cyc <= '0';
wb_stb <= '0';
wb_we <= '0';
slv_ack <= '0';
tip_o <= '0';
err_p_o <= '0';
wdt_rst <= '1';
adr_byte_cnt <= (others => '0');
dat_byte_cnt <= (others => '0');
elsif (slv_sto_p = '1') then
state <= IDLE;
else
case state is
---------------------------------------------------------------------
-- IDLE
---------------------------------------------------------------------
-- After the slave acknowledges its I2C address, the register address
-- bytes have to be shifted in. The start-of-transfer operation
-- is also stored here, to check versus the third I2C transfer in
-- the protocol. At this point, since the SysMon writes the WB
-- address, start_op will be '0' (write).
---------------------------------------------------------------------
when IDLE =>
err_p_o <= '0';
tip_o <= '0';
slv_ack <= '0';
adr_byte_cnt <= (others => '0');
dat_byte_cnt <= (others => '0');
wdt_rst <= '1';
if (slv_addr_good_p = '1') then
tip_o <= '1';
slv_ack <= '1';
wdt_rst <= '0';
state <= SYSMON_WB_ADR;
end if;
---------------------------------------------------------------------
-- SYSMON_WB_ADR
---------------------------------------------------------------------
-- Shift in the two address bytes sent by the SysMon and ACK each of
-- them. The second byte's ACK is also controlled by the next state.
---------------------------------------------------------------------
when SYSMON_WB_ADR =>
if (slv_r_done_p = '1') then
wb_adr <= wb_adr(7 downto 0) & rx_byte;
slv_ack <= '1';
adr_byte_cnt <= adr_byte_cnt + 1;
if (adr_byte_cnt = 1) then
state <= SIM_WB_TRANSF;
end if;
end if;
---------------------------------------------------------------------
-- SIM_WB_TRANSF
---------------------------------------------------------------------
-- Simulate a Wishbone transfer with the received address and go
-- to operation state if we get a WB ACK, or back to idle if we get
-- a WB error. In the latter case, an NACK is sent to the SysMon.
---------------------------------------------------------------------
when SIM_WB_TRANSF =>
wb_cyc <= '1';
wb_stb <= '1';
if (wb_ack = '1') then
slv_ack <= '1';
wb_cyc <= '0';
wb_stb <= '0';
state <= OPER;
elsif (wb_err = '1') then
err_p_o <= '1';
slv_ack <= '0';
wb_cyc <= '0';
wb_stb <= '0';
state <= IDLE;
end if;
---------------------------------------------------------------------
-- OPER
---------------------------------------------------------------------
-- This is the third I2C transfer occuring in the protocol. At this
-- point, the first byte of a SysMon write transfer is sent, or a
-- restart, I2C slave address and read bit are sent to signal a
-- SysMon read transfer.
--
-- So, here we shift in the received byte in case of a SysMon write
-- transfer and then check the OP signal. This is set by the slave
-- while it is in its I2C address read state and will be different
-- from the starting case if a read transfer ('1') occurs.
--
-- If a read transfer follows, the data byte counter and WB data
-- output are cleared to avoid conflicts with future transfers.
---------------------------------------------------------------------
when OPER =>
if (slv_r_done_p = '1') then
wb_dat_out <= rx_byte & wb_dat_out(31 downto 8);
dat_byte_cnt <= dat_byte_cnt + 1;
slv_ack <= '1';
state <= SYSMON_WR;
elsif (slv_addr_good_p = '1') and (op = '1') then
slv_ack <= '1';
state <= SYSMON_RD_WB;
end if;
---------------------------------------------------------------------
-- SYSMON_WR
---------------------------------------------------------------------
-- During write transfers, each byte is shifted in, until all bytes
-- in the transfer have been sent. When this has occured, a Wishbone
-- write transfer is initiated in the next state.
---------------------------------------------------------------------
when SYSMON_WR =>
if (slv_r_done_p = '1') then
wb_dat_out <= rx_byte & wb_dat_out(31 downto 8);
dat_byte_cnt <= dat_byte_cnt + 1;
slv_ack <= '1';
if (dat_byte_cnt = 3) then
state <= SYSMON_WR_WB;
end if;
-- elsif (slv_sto_p = '1') then
-- state <= IDLE;
end if;
---------------------------------------------------------------------
-- SYSMON_WR_WB
---------------------------------------------------------------------
-- Perform a write transfer over Wishbone bus with the received
-- data word.
---------------------------------------------------------------------
when SYSMON_WR_WB =>
wb_cyc <= '1';
wb_stb <= '1';
wb_we <= '1';
if (wb_ack = '1') then -- or (wb_err = '1') then
wb_cyc <= '0';
wb_stb <= '0';
wb_we <= '0';
state <= SYSMON_WR; --IDLE;
elsif (wb_err = '1') then
err_p_o <= '1';
state <= IDLE;
end if;
---------------------------------------------------------------------
-- SYSMON_RD_WB
---------------------------------------------------------------------
-- This state is reached from the operation state; here, we perform
-- a read transfer on the Wishbone bus to prepare the data that
-- should be sent to the SysMon. If the WB address is incorrect, we
-- go back to the IDLE state.
---------------------------------------------------------------------
when SYSMON_RD_WB =>
wb_cyc <= '1';
wb_stb <= '1';
if (wb_ack = '1') then
wb_dat_in <= wbm_dat_i;
wb_cyc <= '0';
wb_stb <= '0';
state <= SYSMON_RD;
elsif (wb_err = '1') then
err_p_o <= '1';
wb_cyc <= '0';
wb_stb <= '0';
state <= IDLE;
end if;
---------------------------------------------------------------------
-- SYSMON_RD
---------------------------------------------------------------------
-- Shift out the bytes over I2C and go back to IDLE state.
---------------------------------------------------------------------
when SYSMON_RD =>
if (slv_w_done_p = '1') then
wb_dat_in <= x"00" & wb_dat_in(31 downto 8);
dat_byte_cnt <= dat_byte_cnt + 1;
slv_ack <= '1';
if (dat_byte_cnt = 3) then
state <= IDLE;
end if;
-- elsif (slv_sto_p = '1') then
-- state <= IDLE;
end if;
---------------------------------------------------------------------
-- Any other state: go back to idle.
---------------------------------------------------------------------
when others =>
state <= IDLE;
end case;
end if;
end if;
end process p_fsm;
--============================================================================
-- FSM watchdog timer
--============================================================================
-- * in the case of writemregs command, a maximum of 35 bytes can be written
-- - 1 I2C address byte
-- - 2 register (Wishbone) address bytes
-- - 8*4 Wishbone register values
-- * we will therefore set the watchdog max. value to allow for 36 bytes to
-- be sent, considering a maximum clk_i frequency of 20 MHz (period = 50 ns)
-- and an SCL frequency of 100 kHz
-- * 100 us / 50 ns = 2000 clock cycles to send one byte
-- * 2000 * 36 bytes = 72000 clock cycles to send 36 bytes
-- * g_wdt_max = 72000
cmp_watchdog : gc_fsm_watchdog
generic map
(
g_wdt_max => 72000
)
port map
(
clk_i => clk_i,
rst_n_i => rst_n_i,
wdt_rst_i => wdt_rst,
fsm_rst_o => rst_fr_wdt
);
-- Process to set the timeout status pulse
p_wdto_outp : process (clk_i)
begin
if rising_edge(clk_i) then
if (rst_n_i = '0') then
rst_fr_wdt_d0 <= '0';
wdto_p_o <= '0';
else
rst_fr_wdt_d0 <= rst_fr_wdt;
wdto_p_o <= rst_fr_wdt and (not rst_fr_wdt_d0);
end if;
end if;
end process p_wdto_outp;
end behav;
--==============================================================================
-- architecture end
--==============================================================================
files = ["wb_irq_timer.vhd",
"irqm_core.vhd",
"wb_irq_lm32.vhd",
"wb_irq_slave.vhd",
"wb_irq_master.vhd",
"wb_irq_pkg.vhd"]
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wishbone_pkg.all;
use work.genram_pkg.all;
use work.wb_irq_pkg.all;
entity irqm_core is
generic( g_channels : natural := 32; -- number of interrupt lines
g_round_rb : boolean := true; -- scheduler true: round robin, false: prioritised
g_det_edge : boolean := true -- edge detection. true: trigger on rising edge of irq lines, false: trigger on high level
);
port (clk_i : in std_logic; -- clock
rst_n_i : in std_logic; -- reset, active LO
--msi if
irq_master_o : out t_wishbone_master_out; -- Wishbone msi irq interface
irq_master_i : in t_wishbone_master_in;
--config
msi_dst_array : in t_wishbone_address_array(g_channels-1 downto 0); -- MSI Destination address for each channel
msi_msg_array : in t_wishbone_data_array(g_channels-1 downto 0); -- MSI Message for each channel
--irq lines
en_i : in std_logic;
mask_i : in std_logic_vector(g_channels-1 downto 0); -- interrupt mask
irq_i : in std_logic_vector(g_channels-1 downto 0) -- interrupt lines
);
end entity;
architecture behavioral of irqm_core is
signal s_msg : t_wishbone_data_array(g_channels-1 downto 0);
signal s_dst : t_wishbone_address_array(g_channels-1 downto 0);
signal s_irq_edge : std_logic_vector(g_channels-1 downto 0);
signal r_irq : std_logic_vector(g_channels-1 downto 0);
signal r_pending : std_logic_vector(g_channels-1 downto 0);
signal s_wb_send : std_logic;
signal r_wb_sending : std_logic;
signal idx : natural range 0 to g_channels-1;
signal idx_robin : natural range 0 to g_channels-1;
signal idx_prio : natural range 0 to g_channels-1;
signal s_en : std_logic_vector(g_channels-1 downto 0);
signal r_cyc : std_logic;
signal r_stb : std_logic;
begin
--shorter names
s_msg <= msi_msg_array;
s_dst <= msi_dst_array;
-- always full words, always write
irq_master_o.cyc <= r_cyc;
irq_master_o.stb <= r_stb;
irq_master_o.sel <= (others => '1');
irq_master_o.we <= '1';
s_en <= (others => en_i);
-------------------------------------------------------------------------
-- registering and counters
-------------------------------------------------------------------------
process(clk_i)
begin
if rising_edge(clk_i) then
if(rst_n_i = '0') then
else
r_irq <= irq_i and mask_i and s_en;
end if;
end if;
end process;
G_Edge_1 : if(g_det_edge) generate
begin
s_irq_edge <= (irq_i and mask_i) and not r_irq;
end generate;
G_Edge_2 : if(not g_det_edge) generate
begin
s_irq_edge <= r_irq;
end generate;
-- round robin
idx_round_robin : process(clk_i)
begin
if rising_edge(clk_i) then
if(rst_n_i = '0') then
idx_robin <= 0;
else
if((not r_wb_sending and r_pending(idx_robin)) = '0') then
if(idx_robin = g_channels-1) then
idx_robin <= 0;
else
idx_robin <= idx_robin +1;
end if;
end if;
end if;
end if;
end process;
-- priority
with f_hot_to_bin(r_pending) select
idx_prio <= 0 when 0,
f_hot_to_bin(r_pending)-1 when others;
idx <= idx_robin when g_round_rb else idx_prio;
-------------------------------------------------------------------------
--******************************************************************************************
-- WB IRQ Interface Arbitration
--------------------------------------------------------------------------------------------
s_wb_send <= r_pending(idx);
-- keep track of what needs sending
queue_mux : process(clk_i)
variable v_set_pending, v_clr_pending : std_logic_vector(r_pending'length-1 downto 0);
begin
if rising_edge(clk_i) then
if((rst_n_i) = '0') then
r_pending <= (others => '0');
else
v_clr_pending := (others => '1');
v_clr_pending(idx) := not r_wb_sending;
v_set_pending := s_irq_edge;
r_pending <= (r_pending or v_set_pending) and v_clr_pending;
end if;
end if;
end process queue_mux;
-------------------------------------------------------------------------
-- WB master generating IRQ msgs
-------------------------------------------------------------------------
-- send pending MSI IRQs over WB
wb_irq_master : process(clk_i)
begin
if rising_edge(clk_i) then
if(rst_n_i = '0') then
r_cyc <= '0';
r_stb <= '0';
r_wb_sending <= '0';
else
r_wb_sending <= '0';
if r_cyc = '1' then
if irq_master_i.stall = '0' then
r_stb <= '0';
end if;
if (irq_master_i.ack or irq_master_i.err or irq_master_i.rty) = '1' then
r_cyc <= '0';
end if;
else
r_cyc <= s_wb_send;
r_stb <= s_wb_send;
r_wb_sending <= s_wb_send;
irq_master_o.adr <= s_dst(idx);
irq_master_o.dat <= s_msg(idx);
end if;
end if;
end if;
end process;
end architecture;
#include "display.h"
const unsigned int REG_MODE = 0x00000000;
const unsigned int REG_RST = 0x00000004;
const unsigned int REG_UART = 0x00010000;
const unsigned int REG_CHAR = 0x00020000;
const unsigned int REG_RAW = 0x00030000;
const char MODE_RAW = 0x03;
const char MODE_UART = 0x01;
const char MODE_CHAR = 0x02;
const char MODE_IDLE = 0x00;
const char ROW_LEN = 11;
void disp_reset()
{
*(display + REG_RST) = 1;
}
void disp_put_c(char ascii)
{
*(display + (REG_MODE>>2)) = (unsigned int)MODE_UART;
*(display + (REG_UART>>2)) = (unsigned int)ascii;
}
void disp_put_str(const char *sPtr)
{
*(display + (REG_MODE>>2)) = (unsigned int)MODE_UART;
while(*sPtr != '\0') *(display + (REG_UART>>2)) = (unsigned int)*sPtr++;
}
void disp_put_line(const char *sPtr, unsigned char row)
{
unsigned char col, outp, pad;
pad = 0;
for(col=0; col<ROW_LEN; col++)
{
if(*(sPtr+col) == '\0') pad = 1;
if(pad) outp = ' ';
else outp = (unsigned int)*(sPtr+col);
disp_loc_c(outp, row, col);
}
}
void disp_loc_c(char ascii, unsigned char row, unsigned char col)
{
unsigned int rowcol;
*(display + (REG_MODE>>2)) = (unsigned int)MODE_CHAR;
rowcol = ((0x07 & (unsigned int)row)<<6) + ((0x0f & (unsigned int)col)<<2);
*(display + ((REG_CHAR + rowcol)>>2)) = (unsigned int)ascii;
}
void disp_put_raw(char pixcol, unsigned int address, char color)
{
char oldpixcol;
*(display + (REG_MODE>>2)) = (unsigned int)MODE_RAW;
oldpixcol = *(display + (REG_RAW>>2) + ((address & 0x3FFF))); //read out old memcontent
if(color) // 1 -> White
*(display + (REG_RAW>>2) + ((address & 0x3FFF))) = (unsigned int)(pixcol | oldpixcol);
else
*(display + (REG_RAW>>2) + ((address & 0x3FFF))) = (unsigned int)(~pixcol & oldpixcol);
*(display + (REG_MODE>>2)) = (unsigned int)MODE_IDLE;
}
unsigned int get_pixcol_addr(unsigned char x_in, unsigned char y_in)
{
unsigned int addr_base = 0x230;
unsigned int x, y;
x = x_in & 0x3F;
if(y_in < 48) y = ((y_in>>3)<<8); //determine row. Shift by 8bit
else y = (5<<8); //outside visible area, take start of bottom row instead
return addr_base + y + x;
}
unsigned int get_pixcol_val(unsigned char y_in)
{
return 1<<(y_in & 0x07);
}
#ifndef _DISPLAY_H_
#define _DISPLAY_H_
extern volatile unsigned int* display;
extern const unsigned int MODE_REG;
extern const unsigned int RST_REG;
extern const unsigned int UART_REG;
extern const unsigned int CHAR_REG;
extern const unsigned int RAW_REG;
extern const char MODE_RAW;
extern const char MODE_UART;
extern const char MODE_CHAR;
extern const char MODE_IDLE;
extern const char ROW_LEN;
void disp_put_c(char ascii);
void disp_put_int(int number);
void disp_put_str(const char *sPtr);
void disp_put_line(const char *sPtr, unsigned char row);
void disp_reset();
void disp_loc_c(char ascii, unsigned char row, unsigned char col);
void disp_put_raw(char pixcol, unsigned int address, char color);
unsigned int get_pixcol_addr(unsigned char x_in, unsigned char y_in);
unsigned int get_pixcol_val(unsigned char y_in);
#endif
dev=$1
eca-ctl $dev enable
eca-ctl $dev -c 1 activate
eca-table $dev flush
eca-table $dev add 0xDEADBE00/64 +0 1 0x000
eca-table $dev add 0xDEADBE01/64 +0 1 0x004
eca-table $dev add 0xDEADBE10/64 +0 1 0x100
eca-table $dev add 0xDEADBE11/64 +0 1 0x104
eca-table $dev flip-active
eca-ctl $dev send -s0 0xDEADBE11 +0 0xAB
eb-read $dev 0x2000d08/4
/** @file irq.c
* @brief MSI capable IRQ handler for the LM32
*
* Copyright (C) 2011-2012 GSI Helmholtz Centre for Heavy Ion Research GmbH
*
* @author Mathias Kreider <m.kreider@gsi.de>
*
* @bug None!
*
*******************************************************************************
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library. If not, see <http://www.gnu.org/licenses/>.
*******************************************************************************
*/
#include "irq.h"
//#include "display.h" DEBUG
extern unsigned int* irq_slave;
const unsigned int IRQ_REG_RST = 0x00000000;
const unsigned int IRQ_REG_STAT = 0x00000004;
const unsigned int IRQ_REG_POP = 0x00000008;
const unsigned int IRQ_OFFS_MSG = 0x00000000;
const unsigned int IRQ_OFFS_SRC = 0x00000004;
const unsigned int IRQ_OFFS_SEL = 0x00000008;
inline void irq_pop_msi( unsigned int irq_no)
{
unsigned int* msg_queue = (unsigned int*)(irq_slave + ((irq_no +1)<<2));
global_msi.msg = *(msg_queue+(IRQ_OFFS_MSG>>2));
global_msi.src = *(msg_queue+(IRQ_OFFS_SRC>>2));
global_msi.sel = *(msg_queue+(IRQ_OFFS_SEL>>2));
*(irq_slave + (IRQ_REG_POP>>2)) = 1<<irq_no;
return;
}
inline void isr_table_clr(void)
{
//set all ISR table entries to Null
unsigned int i;
for(i=0;i<32;i++) isr_ptr_table[i] = 0;
}
inline unsigned int irq_get_mask(void)
{
//read IRQ mask
unsigned int im;
asm volatile ( "rcsr %0, im": "=&r" (im));
return im;
}
inline void irq_set_mask( unsigned int im)
{
//write IRQ mask
asm volatile ( "wcsr im, %0": "=&r" (im));
return;
}
inline void irq_disable(void)
{
//globally disable interrupts
unsigned int ie;
asm volatile ( "rcsr %0, IE\n" \
"andi %0, %0, 0xFFFE\n" \
"wcsr IE, %0" : "=&r" (ie));
return;
}
inline void irq_enable(void)
{
//globally enable interrupts
unsigned int ie;
asm volatile ( "rcsr %0, IE\n" \
"ori %0, %0, 1\n" \
"wcsr IE, %0" : "=&r" (ie));
return;
}
inline void irq_clear( unsigned int mask)
{
//clear pending interrupt flag(s)
unsigned int ip;
asm volatile ( "rcsr %0, ip\n" \
"and %0, %0, %1\n" \
"wcsr ip, %0" : "=&r" (ip): "r" (mask) );
return;
}
inline void irq_process(void)
{
char buffer[12];
unsigned int ip;
unsigned char irq_no = 0;
//get pending flags
asm volatile ("rcsr %0, ip": "=r"(ip));
while(ip) //irqs pending ?
{
if(ip & 0x01) //check if irq with lowest number is pending
{
irq_pop_msi(irq_no); //pop msg from msi queue into global_msi variable
irq_clear(1<<irq_no); //clear pending bit
if((unsigned int)isr_ptr_table[irq_no]) isr_ptr_table[irq_no](); //execute isr
//else disp_put_str("No ISR\nptr found!\n"); DEBUG
}
irq_no++;
ip = ip >> 1; //process next irq
}
return;
}
/** @file irq.h
* @brief Header file for MSI capable IRQ handler for the LM32
*
* Copyright (C) 2011-2012 GSI Helmholtz Centre for Heavy Ion Research GmbH
*
* Usage:
*
* void <an ISR>(void) { <evaluate global_msi and do something useful> }
* ...
* void _irq_entry(void) {irq_process();}
* ...
* void main(void) {
*
* isr_table_clr();
* isr_ptr_table[0]= <an ISR>;
* isr_ptr_table[1]= ...
* ...
* irq_set_mask(0x03); //Enable used IRQs ...
* irq_enable();
* ...
* }
*
* @author Mathias Kreider <m.kreider@gsi.de>
*
* @bug None!
*
*******************************************************************************
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library. If not, see <http://www.gnu.org/licenses/>.
*******************************************************************************
*/
#ifndef __IRQ_H_
#define __IRQ_H_
typedef struct
{
unsigned int msg;
unsigned int src;
unsigned int sel;
} msi;
//ISR function pointer table
typedef void (*isr_ptr_t)(void);
isr_ptr_t isr_ptr_table[32];
//Global containing last processed MSI message
volatile msi global_msi;
inline void irq_pop_msi( unsigned int irq_no);
inline unsigned int irq_get_mask(void);
inline void irq_set_mask( unsigned int im);
inline void irq_disable(void);
inline void irq_enable(void);
inline void irq_clear( unsigned int mask);
inline void isr_table_clr(void);
inline void irq_process(void);
#endif
/*
* Simulator Link script for Lattice Mico32.
* Contributed by Jon Beniston <jon@beniston.com>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*/
OUTPUT_FORMAT("elf32-lm32")
ENTRY(_start)
/*INPUT() */
GROUP(-lgcc -lc)
MEMORY
{
ram : ORIGIN = 0x00000000, LENGTH = 0x10000 /* 64K */
}
SECTIONS
{
.boot : { *(.boot) } > ram
/* Code */
.text :
{
. = ALIGN(4);
_ftext = .;
_ftext_rom = LOADADDR(.text);
*(.text .stub .text.* .gnu.linkonce.t.*)
*(.gnu.warning)
KEEP (*(.init))
KEEP (*(.fini))
/* Constructors and destructors */
KEEP (*crtbegin*.o(.ctors))
KEEP (*(EXCLUDE_FILE (*crtend*.o ) .ctors))
KEEP (*(SORT(.ctors.*)))
KEEP (*(.ctors))
KEEP (*crtbegin*.o(.dtors))
KEEP (*(EXCLUDE_FILE (*crtend*.o ) .dtors))
KEEP (*(SORT(.dtors.*)))
KEEP (*(.dtors))
KEEP (*(.jcr))
_etext = .;
} > ram =0
/* Exception handlers */
.eh_frame_hdr : { *(.eh_frame_hdr) } > ram
.eh_frame : { KEEP (*(.eh_frame)) } > ram
.gcc_except_table : { *(.gcc_except_table) *(.gcc_except_table.*) } > ram
/* Read-only data */
.rodata :
{
. = ALIGN(4);
_frodata = .;
_frodata_rom = LOADADDR(.rodata);
*(.rodata .rodata.* .gnu.linkonce.r.*)
*(.rodata1)
_erodata = .;
} > ram
/* Data */
.data :
{
. = ALIGN(4);
_fdata = .;
_fdata_rom = LOADADDR(.data);
*(.data .data.* .gnu.linkonce.d.*)
*(.data1)
SORT(CONSTRUCTORS)
_gp = ALIGN(16) + 0x7ff0;
*(.sdata .sdata.* .gnu.linkonce.s.*)
_edata = .;
} > ram
/* BSS */
.bss :
{
. = ALIGN(4);
_fbss = .;
*(.dynsbss)
*(.sbss .sbss.* .gnu.linkonce.sb.*)
*(.scommon)
*(.dynbss)
*(.bss .bss.* .gnu.linkonce.b.*)
*(COMMON)
. = ALIGN(4);
_ebss = .;
_end = .;
PROVIDE (end = .);
} > ram
/* First location in stack is highest address in RAM */
PROVIDE(_fstack = ORIGIN(ram) + LENGTH(ram) - 4);
/* Stabs debugging sections. */
.stab 0 : { *(.stab) }
.stabstr 0 : { *(.stabstr) }
.stab.excl 0 : { *(.stab.excl) }
.stab.exclstr 0 : { *(.stab.exclstr) }
.stab.index 0 : { *(.stab.index) }
.stab.indexstr 0 : { *(.stab.indexstr) }
.comment 0 : { *(.comment) }
/* DWARF debug sections.
Symbols in the DWARF debugging sections are relative to the beginning
of the section so we begin them at 0. */
/* DWARF 1 */
.debug 0 : { *(.debug) }
.line 0 : { *(.line) }
/* GNU DWARF 1 extensions */
.debug_srcinfo 0 : { *(.debug_srcinfo) }
.debug_sfnames 0 : { *(.debug_sfnames) }
/* DWARF 1.1 and DWARF 2 */
.debug_aranges 0 : { *(.debug_aranges) }
.debug_pubnames 0 : { *(.debug_pubnames) }
/* DWARF 2 */
.debug_info 0 : { *(.debug_info .gnu.linkonce.wi.*) }
.debug_abbrev 0 : { *(.debug_abbrev) }
.debug_line 0 : { *(.debug_line) }
.debug_frame 0 : { *(.debug_frame) }
.debug_str 0 : { *(.debug_str) }
.debug_loc 0 : { *(.debug_loc) }
.debug_macinfo 0 : { *(.debug_macinfo) }
/* SGI/MIPS DWARF 2 extensions */
.debug_weaknames 0 : { *(.debug_weaknames) }
.debug_funcnames 0 : { *(.debug_funcnames) }
.debug_typenames 0 : { *(.debug_typenames) }
.debug_varnames 0 : { *(.debug_varnames) }
}
#include <stdio.h>
#include "display.h"
#include "irq.h"
volatile unsigned int* display = (unsigned int*)0x02900000;
volatile unsigned int* irq_slave = (unsigned int*)0x02000d00;
char* mat_sprinthex(char* buffer, unsigned long val)
{
unsigned char i,ascii;
const unsigned long mask = 0x0000000F;
for(i=0; i<8;i++)
{
ascii= (val>>(i<<2)) & mask;
if(ascii > 9) ascii = ascii - 10 + 'A';
else ascii = ascii + '0';
buffer[7-i] = ascii;
}
buffer[8] = 0x00;
return buffer;
}
void show_msi()
{
char buffer[12];
mat_sprinthex(buffer, global_msi.msg);
disp_put_str("D ");
disp_put_str(buffer);
disp_put_c('\n');
mat_sprinthex(buffer, global_msi.src);
disp_put_str("A ");
disp_put_str(buffer);
disp_put_c('\n');
mat_sprinthex(buffer, (unsigned long)global_msi.sel);
disp_put_str("S ");
disp_put_str(buffer);
disp_put_c('\n');
}
void isr0()
{
unsigned int j;
disp_put_str("ISR0\n");
show_msi();
for (j = 0; j < 125000000; ++j) {
asm("# noop"); /* no-op the compiler can't optimize away */
}
disp_put_c('\f');
}
void isr1()
{
unsigned int j;
disp_put_str("ISR1\n");
show_msi();
for (j = 0; j < 125000000; ++j) {
asm("# noop"); /* no-op the compiler can't optimize away */
}
disp_put_c('\f');
}
void _irq_entry(void) {
disp_put_c('\f');
disp_put_str("IRQ_ENTRY\n");
irq_process();
}
const char mytext[] = "Hallo Welt!...\n\n";
void main(void) {
isr_table_clr();
isr_ptr_table[0]= isr0;
isr_ptr_table[1]= isr1;
irq_set_mask(0x03);
irq_enable();
int j, xinc, yinc, x, y;
unsigned int time = 0;
unsigned int addr_raw_off;
char color = 0xFF;
disp_reset();
disp_put_c('\f');
disp_put_str(mytext);
x = 0;
y = 9;
yinc = -1;
xinc = 1;
addr_raw_off = 0;
while (1) {
/* Rotate the LEDs */
disp_put_raw( get_pixcol_val((unsigned char)y), get_pixcol_addr((unsigned char)x, (unsigned char)y), color);
if(x == 63) xinc = -1;
if(x == 0) xinc = 1;
if(y == 47) yinc = -1;
if(y == 0) yinc = 1;
x += xinc;
y += yinc;
/* Each loop iteration takes 4 cycles.
* It runs at 125MHz.
* Sleep 0.2 second.
*/
for (j = 0; j < 125000000/160; ++j) {
asm("# noop"); /* no-op the compiler can't optimize away */
}
if(time++ > 500) {time = 0; color = ~color; }
}
}
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.wishbone_pkg.all;
use work.wb_irq_pkg.all;
entity wb_irq_lm32 is
generic(g_msi_queues: natural := 3;
g_profile: string);
port(
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
dwb_o : out t_wishbone_master_out;
dwb_i : in t_wishbone_master_in;
iwb_o : out t_wishbone_master_out;
iwb_i : in t_wishbone_master_in;
irq_slave_o : out t_wishbone_slave_out_array(g_msi_queues-1 downto 0); -- wb msi interface
irq_slave_i : in t_wishbone_slave_in_array(g_msi_queues-1 downto 0);
ctrl_slave_o : out t_wishbone_slave_out; -- ctrl interface for LM32 irq processing
ctrl_slave_i : in t_wishbone_slave_in
);
end wb_irq_lm32;
architecture rtl of wb_irq_lm32 is
signal s_irq : std_logic_vector(31 downto 0);
begin
s_irq(31 downto g_msi_queues) <= (others => '0');
msi_irq: wb_irq_slave
GENERIC MAP( g_queues => g_msi_queues,
g_depth => 8)
PORT MAP (
clk_i => clk_sys_i,
rst_n_i => rst_n_i,
irq_slave_o => irq_slave_o,
irq_slave_i => irq_slave_i,
irq_o => s_irq(g_msi_queues-1 downto 0),
ctrl_slave_o => ctrl_slave_o,
ctrl_slave_i => ctrl_slave_i
);
LM32_CORE : xwb_lm32
generic map(g_profile => g_profile)
port map(
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
irq_i => s_irq,
dwb_o => dwb_o,
dwb_i => dwb_i,
iwb_o => iwb_o,
iwb_i => iwb_i
);
end rtl;
------------------------------------------------------------------------------
-- Title : WB Timer Interrupt
-- Project : Wishbone
------------------------------------------------------------------------------
-- File : wb_irq_timer.vhd
-- Author : Mathias Kreider
-- Company : GSI
-- Created : 2013-08-10
-- Last update: 2013-08-10
-- Platform : FPGA-generic
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Programmable Timer interrupt module (MSI)
-------------------------------------------------------------------------------
-- Copyright (c) 2013 GSI
-------------------------------------------------------------------------------
--
--
--
-- 31 16 6 0
-- Dst.............SrcID....ChID...
-- *************************
--
-- Revisions :
-- Date Version Author Description
-- 2013-08-10 1.0 mkreider Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wishbone_pkg.all;
use work.genram_pkg.all;
use work.wb_irq_pkg.all;
entity wb_irq_master is
generic( g_channels : natural := 32; -- number of interrupt lines
g_round_rb : boolean := true; -- scheduler true: round robin, false: prioritised
g_det_edge : boolean := true; -- edge detection. true: trigger on rising edge of irq lines, false: trigger on high level
g_has_dev_id : boolean := false; -- if set, dst adr bits 11..7 hold g_dev_id as device identifier
g_dev_id : std_logic_vector(4 downto 0) := (others => '0'); -- device identifier
g_has_ch_id : boolean := false; -- if set, dst adr bits 6..2 hold g_ch_id as device identifier
g_default_msg : boolean := true -- initialises msgs to a default value in order to detect uninitialised irq master
);
port (clk_i : std_logic; -- clock
rst_n_i : std_logic; -- reset, active LO
--msi if
irq_master_o : out t_wishbone_master_out; -- Wishbone msi irq interface
irq_master_i : in t_wishbone_master_in;
-- ctrl interface
ctrl_slave_o : out t_wishbone_slave_out;
ctrl_slave_i : in t_wishbone_slave_in;
--irq lines
irq_i : std_logic_vector(g_channels-1 downto 0) -- irq lines
);
end entity;
architecture behavioral of wb_irq_master is
function f_wb_wr(pval : std_logic_vector; ival : std_logic_vector; sel : std_logic_vector; mode : string := "owr") return std_logic_vector is
variable n_sel : std_logic_vector(pval'range);
variable n_val : std_logic_vector(pval'range);
variable result : std_logic_vector(pval'range);
begin
for i in pval'range loop
n_sel(i) := sel(i / 8);
n_val(i) := ival(i);
end loop;
if(mode = "set") then
result := pval or (n_val and n_sel);
elsif (mode = "clr") then
result := pval and not (n_val and n_sel);
else
result := (pval and not n_sel) or (n_val and n_sel);
end if;
return result;
end f_wb_wr;
function f_oob(a : std_logic_vector; limit : natural) return boolean is
begin
if(to_integer(unsigned(a)) > limit-1) then
return true;
else
return false;
end if;
end f_oob;
--******************************************************************************************
-- WB ctrl interface definitions and constants
--------------------------------------------------------------------------------------------
signal s_c_en, s_c_we, r_c_ack, r_c_err : std_logic;
signal s_c_adr : natural range 255 downto 0;
signal s_c_dati, r_c_dato : t_wishbone_data;
signal s_c_sel : t_wishbone_byte_select;
signal r_rst_n : std_logic;
constant c_RST : natural := 0; --0x00, wo, Reset Active Low
constant c_MSK_GET : natural := c_RST +4; --0x04, ro, get irq mask. to only use irq mask vector (mask_i), set all bits to HI
constant c_MSK_SET : natural := c_MSK_GET +4; --0x08, wo, set irq mask bits.
constant c_MSK_CLR : natural := c_MSK_SET +4; --0x0C, wo, clr irq mask bits,
constant c_SW_IRQ : natural := c_MSK_CLR +4; --0x10, wo, software irq
constant c_CHANNEL_INFO : natural := c_SW_IRQ +4; --0x14 ro number of channels in this device
constant c_CHANNEL_SEL : natural := 16#20#; --0x20, rw, channel select. !!!CAUTION!!! content of all c_CH_... regs depends on this
constant c_CH_MSG : natural := c_CHANNEL_SEL +4; --0x24, rw, MSI msg to be sent on MSI when deadline is hit
constant c_CH_DST : natural := c_CH_MSG +4; --0x28, rw, MSI adr to send the msg to when deadline is hit
signal r_csl : t_wishbone_data; -- channel select
signal r_msk, s_msk : std_logic_vector(g_channels-1 downto 0); -- mask
signal r_msg : t_wishbone_data_array(g_channels-1 downto 0); -- messages
signal r_dst : t_wishbone_address_array(g_channels-1 downto 0); -- destinations
signal s_irq, r_swirq : std_logic_vector(g_channels-1 downto 0); --
--v_dst(6 downto 2) := std_logic_vector(to_unsigned(idx, 5));
--if(g_has_dev_id) then
-- v_dst(12 downto 8) := g_dev_id;
-- v_dst(31 downto 16) := s_dst(idx)(31 downto 16);
--else
-- v_dst(31 downto 7) := s_dst(idx)(31 downto 7);
--end if;
begin
--combine mask register and mask lines
s_msk <= r_msk;
s_irq <= irq_i or r_swirq;
--******************************************************************************************
-- WB ctrl interface implementation
--------------------------------------------------------------------------------------------
s_c_en <= ctrL_slave_i.cyc and ctrl_slave_i.stb;
s_c_adr <= to_integer(unsigned(ctrl_slave_i.adr(7 downto 2)) & "00");
s_c_we <= ctrl_slave_i.we;
s_c_dati <= ctrl_slave_i.dat;
s_c_sel <= ctrl_slave_i.sel;
ctrl_slave_o.int <= '0';
ctrl_slave_o.rty <= '0';
ctrl_slave_o.stall <= '0';
ctrl_slave_o.ack <= r_c_ack;
ctrl_slave_o.err <= r_c_err;
ctrl_slave_o.dat <= r_c_dato;
process(clk_i)
variable v_ch_sl : natural range g_channels-1 downto 0;
begin
if rising_edge(clk_i) then
if(rst_n_i = '0' or r_rst_n <= '0') then
r_c_ack <= '0';
r_c_err <= '0';
r_rst_n <= '1';
r_csl <= (others => '0'); -- channel select
r_msk <= (others => '0'); -- irq mask
r_swirq <= (others => '0'); -- software irq
--init code for messages
if(g_default_msg) then
for i in irq_i'range loop
r_dst(i) <= x"CAFEBABE";
end loop;
end if;
else
-- Fire and Forget Registers
r_c_ack <= '0';
r_c_err <= '0';
r_c_dato <= (others => '0');
if(s_c_en = '1') then
v_ch_sl := to_integer(unsigned(r_csl));
r_c_ack <= '1';
if(s_c_we = '1') then
case s_c_adr is
when c_RST => r_rst_n <= '0';
when c_MSK_SET => r_msk <= f_wb_wr(r_msk, s_c_dati, s_c_sel, "set");
when c_MSK_CLR => r_msk <= f_wb_wr(r_msk, s_c_dati, s_c_sel, "clr");
when c_SW_IRQ => r_swirq <= f_wb_wr(r_swirq, s_c_dati, s_c_sel, "owr");
when c_CHANNEL_SEL => if(f_oob(s_c_dati, g_channels)) then -- owr with limit check
r_c_ack <= '0'; r_c_err <= '1';
else
r_csl <= f_wb_wr(r_csl, s_c_dati, s_c_sel, "owr");
end if;
when c_CH_MSG => r_msg(v_ch_sl) <= f_wb_wr(r_msg(v_ch_sl), s_c_dati, s_c_sel, "owr");
when c_CH_DST => r_dst(v_ch_sl) <= f_wb_wr(r_dst(v_ch_sl), s_c_dati, s_c_sel, "owr");
when others => r_c_ack <= '0'; r_c_err <= '1';
end case;
else
case s_c_adr is
when c_MSK_GET => r_c_dato(r_msk'range) <= r_msk;
when c_CHANNEL_INFO => r_c_dato <= std_logic_vector(to_unsigned(g_channels,32));
when c_CHANNEL_SEL => r_c_dato(r_csl'range) <= r_csl;
when c_CH_MSG => r_c_dato(r_msg(v_ch_sl)'range) <= r_msg(v_ch_sl);
when c_CH_DST => r_c_dato(r_dst(v_ch_sl)'range) <= r_dst(v_ch_sl);
when others => r_c_ack <= '0'; r_c_err <= '1';
end case;
end if; -- s_c_we
end if; -- s_c_en
end if; -- rst
end if; -- clk edge
end process;
msi : irqm_core
generic map(g_channels => g_channels,
g_round_rb => g_round_rb,
g_det_edge => g_det_edge
)
port map( clk_i => clk_i,
rst_n_i => rst_n_i,
--msi if
irq_master_o => irq_master_o,
irq_master_i => irq_master_i,
--config
msi_dst_array => r_dst,
msi_msg_array => r_msg,
--irq lines
en_i => '1',
mask_i => s_msk,
irq_i => s_irq
);
end architecture;
--! @file wb_irq_pkg.vhd
--! @brief Wishbone IRQ Master
--!
--! Copyright (C) 2011-2012 GSI Helmholtz Centre for Heavy Ion Research GmbH
--!
--! Important details about its implementation
--! should go in these comments.
--!
--! @author Mathias Kreider <m.kreider@gsi.de>
--!
--------------------------------------------------------------------------------
--! This library is free software; you can redistribute it and/or
--! modify it under the terms of the GNU Lesser General Public
--! License as published by the Free Software Foundation; either
--! version 3 of the License, or (at your option) any later version.
--!
--! This library 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
--! Lesser General Public License for more details.
--!
--! You should have received a copy of the GNU Lesser General Public
--! License along with this library. If not, see <http://www.gnu.org/licenses/>.
---------------------------------------------------------------------------------
library IEEE;
use IEEE.std_logic_1164.all;
use IEEE.numeric_std.all;
library work;
use work.wishbone_pkg.all;
package wb_irq_pkg is
type t_ivec_array_d is array(natural range <>) of t_wishbone_data;
type t_ivec_ad is record
address : t_wishbone_address;
value : t_wishbone_data;
end record t_ivec_ad;
type t_ivec_array_ad is array(natural range <>) of t_ivec_ad;
function f_bin_to_hot(x : natural; len : natural) return std_logic_vector;
function or_all(slv_in : std_logic_vector) return std_logic;
constant c_irq_hostbridge_ep_sdb : t_sdb_device := (
abi_class => x"0000", -- undocumented device
abi_ver_major => x"01",
abi_ver_minor => x"01",
wbd_endian => c_sdb_endian_big,
wbd_width => x"7", -- 8/16/32-bit port granularity
sdb_component => (
addr_first => x"0000000000000000",
addr_last => x"00000000000000ff",
product => (
vendor_id => x"0000000000000651", -- GSI
device_id => x"10050081",
version => x"00000001",
date => x"20120308",
name => "IRQ_HOSTBRIDGE_EP ")));
constant c_irq_ep_sdb : t_sdb_device := (
abi_class => x"0000", -- undocumented device
abi_ver_major => x"01",
abi_ver_minor => x"01",
wbd_endian => c_sdb_endian_big,
wbd_width => x"7", -- 8/16/32-bit port granularity
sdb_component => (
addr_first => x"0000000000000000",
addr_last => x"00000000000000ff",
product => (
vendor_id => x"0000000000000651", -- GSI
device_id => x"10050082",
version => x"00000001",
date => x"20120308",
name => "IRQ_ENDPOINT ")));
constant c_irq_ctrl_sdb : t_sdb_device := (
abi_class => x"0000", -- undocumented device
abi_ver_major => x"01",
abi_ver_minor => x"01",
wbd_endian => c_sdb_endian_big,
wbd_width => x"7", -- 8/16/32-bit port granularity
sdb_component => (
addr_first => x"0000000000000000",
addr_last => x"00000000000000ff",
product => (
vendor_id => x"0000000000000651", -- GSI
device_id => x"10040083",
version => x"00000001",
date => x"20120308",
name => "IRQ_CTRL ")));
component wb_irq_master is
generic( g_channels : natural := 32; -- number of interrupt lines
g_round_rb : boolean := true; -- scheduler true: round robin, false: prioritised
g_det_edge : boolean := true -- edge detection. true: trigger on rising edge of irq lines, false: trigger on high level
);
port (clk_i : std_logic; -- clock
rst_n_i : std_logic; -- reset, active LO
--msi if
irq_master_o : out t_wishbone_master_out; -- Wishbone msi irq interface
irq_master_i : in t_wishbone_master_in;
-- ctrl interface
ctrl_slave_o : out t_wishbone_slave_out;
ctrl_slave_i : in t_wishbone_slave_in;
--irq lines
irq_i : std_logic_vector(g_channels-1 downto 0) -- irq lines
);
end component;
component wb_irq_slave is
generic ( g_queues : natural := 4;
g_depth : natural := 8;
g_datbits : natural := 32;
g_adrbits : natural := 32;
g_selbits : natural := 4
);
port (clk_i : std_logic;
rst_n_i : std_logic;
irq_slave_o : out t_wishbone_slave_out_array(g_queues-1 downto 0);
irq_slave_i : in t_wishbone_slave_in_array(g_queues-1 downto 0);
irq_o : out std_logic_vector(g_queues-1 downto 0);
ctrl_slave_o : out t_wishbone_slave_out;
ctrl_slave_i : in t_wishbone_slave_in
);
end component;
constant c_irq_timer_sdb : t_sdb_device := (
abi_class => x"0000", -- undocumented device
abi_ver_major => x"01",
abi_ver_minor => x"01",
wbd_endian => c_sdb_endian_big,
wbd_width => x"7", -- 8/16/32-bit port granularity
sdb_component => (
addr_first => x"0000000000000000",
addr_last => x"00000000000000ff",
product => (
vendor_id => x"0000000000000651", -- GSI
device_id => x"10040088",
version => x"00000001",
date => x"20120308",
name => "IRQ_TIMER ")));
component wb_irq_timer is
generic ( g_timers : natural := 4);
port (clk_sys_i : in std_logic;
rst_sys_n_i : in std_logic;
tm_tai8ns_i : in std_logic_vector(63 downto 0);
ctrl_slave_o : out t_wishbone_slave_out; -- ctrl interface for LM32 irq processing
ctrl_slave_i : in t_wishbone_slave_in;
irq_master_o : out t_wishbone_master_out; -- wb msi interface
irq_master_i : in t_wishbone_master_in
);
end component;
component irqm_core is
generic( g_channels : natural := 32; -- number of interrupt lines
g_round_rb : boolean := true; -- scheduler true: round robin, false: prioritised
g_det_edge : boolean := true -- edge detection. true: trigger on rising edge of irq lines, false: trigger on high level
);
port (clk_i : in std_logic; -- clock
rst_n_i : in std_logic; -- reset, active LO
--msi if
irq_master_o : out t_wishbone_master_out; -- Wishbone msi irq interface
irq_master_i : in t_wishbone_master_in;
--config
msi_dst_array : in t_wishbone_address_array(g_channels-1 downto 0); -- MSI Destination address for each channel
msi_msg_array : in t_wishbone_data_array(g_channels-1 downto 0); -- MSI Message for each channel
--irq lines
en_i : in std_logic; -- global interrupt enable
mask_i : in std_logic_vector(g_channels-1 downto 0); -- interrupt mask
irq_i : in std_logic_vector(g_channels-1 downto 0) -- interrupt lines
);
end component;
component wb_irq_lm32 is
generic(g_msi_queues: natural := 3;
g_profile: string);
port(
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
dwb_o : out t_wishbone_master_out;
dwb_i : in t_wishbone_master_in;
iwb_o : out t_wishbone_master_out;
iwb_i : in t_wishbone_master_in;
irq_slave_o : out t_wishbone_slave_out_array(g_msi_queues-1 downto 0); -- wb msi interface
irq_slave_i : in t_wishbone_slave_in_array(g_msi_queues-1 downto 0);
ctrl_slave_o : out t_wishbone_slave_out; -- ctrl interface for LM32 irq processing
ctrl_slave_i : in t_wishbone_slave_in
);
end component;
end package;
package body wb_irq_pkg is
function f_big_ripple(a, b : std_logic_vector; c : std_logic) return std_logic_vector is
constant len : natural := a'length;
variable aw, bw, rw : std_logic_vector(len+1 downto 0);
variable x : std_logic_vector(len downto 0);
begin
aw := "0" & a & c;
bw := "0" & b & c;
rw := std_logic_vector(unsigned(aw) + unsigned(bw));
x := rw(len+1 downto 1);
return x;
end f_big_ripple;
function f_bin_to_hot(x : natural; len : natural
) return std_logic_vector is
variable ret : std_logic_vector(len-1 downto 0);
begin
ret := (others => '0');
ret(x) := '1';
return ret;
end function;
function or_all(slv_in : std_logic_vector)
return std_logic is
variable I : natural;
variable ret : std_logic;
begin
ret := '0';
for I in 0 to slv_in'left loop
ret := ret or slv_in(I);
end loop;
return ret;
end function or_all;
end package body;
------------------------------------------------------------------------------
-- Title : WB MSI Core
-- Project : Wishbone
------------------------------------------------------------------------------
-- File : wb_irq_slave.vhd
-- Author : Mathias Kreider
-- Company : GSI
-- Created : 2013-08-10
-- Last update: 2013-08-10
-- Platform : FPGA-generic
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Provide prioritized Message Signaled Interrupt queues for an LM32
-------------------------------------------------------------------------------
-- Copyright (c) 2013 GSI
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2013-08-10 1.0 mkreider Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wishbone_pkg.all;
use work.genram_pkg.all;
use work.wb_irq_pkg.all;
entity wb_irq_slave is
generic ( g_queues : natural := 4; -- number of int lines & queues
g_depth : natural := 8; -- queues depth
g_datbits : natural := 32; -- data bits from irq wb to store
g_adrbits : natural := 32; -- addr bits from irq wb to store
g_selbits : natural := 4 -- sel bits from irq wb to store
);
port (clk_i : std_logic;
rst_n_i : std_logic;
irq_slave_o : out t_wishbone_slave_out_array(g_queues-1 downto 0); -- wb msi interface
irq_slave_i : in t_wishbone_slave_in_array(g_queues-1 downto 0);
irq_o : out std_logic_vector(g_queues-1 downto 0); -- pending irq flags of queues
ctrl_slave_o : out t_wishbone_slave_out; -- ctrl interface for LM32 irq processing
ctrl_slave_i : in t_wishbone_slave_in
);
end entity;
architecture behavioral of wb_irq_slave is
-------------------------------------------------------------------------
--memory map for ctrl wb
-------------------------------------------------------------------------
constant c_RST : natural := 0; --wo
constant c_STATUS : natural := c_RST+4; --ro, 1 bit per queue
constant c_POP : natural := c_STATUS+4; --wo, 1 bit per queue pop
--pages for queues
--queue I is found at: c_QUEUES + I * c_N_QUEUE
constant c_QUEUES : natural := 16;
constant c_N_QUEUE : natural := 16;
constant c_OFFS_DATA : natural := 0; --ro wb data, msi message
constant c_OFFS_ADDR : natural := c_OFFS_DATA+4; --ro wb addr, msi adr low bits ID caller device
constant c_OFFS_SEL : natural := c_OFFS_ADDR+4; --ro wb sel,
-------------------------------------------------------------------------
--ctrl wb signals
signal r_rst_n, s_rst_n : std_logic;
signal r_status, r_pop : std_logic_vector(31 downto 0);
signal queue_offs : natural;
signal word_offs : natural;
signal adr : unsigned(7 downto 0);
--queue signals
type t_queue_dat is array(natural range <>) of std_logic_vector(g_datbits+g_adrbits+g_selbits-1 downto 0);
signal irq_push, irq_pop, irq_full, irq_empty : std_logic_vector(g_queues-1 downto 0);
signal irq_d, irq_q : t_queue_dat(g_queues-1 downto 0);
signal ctrl_en : std_logic;
begin
-------------------------------------------------------------------------
--irq wb and queues
-------------------------------------------------------------------------
irq_o <= r_status(g_queues-1 downto 0); -- LM32 IRQs are active low!
s_rst_n <= rst_n_i and r_rst_n;
G1: for I in 0 to g_queues-1 generate
irq_d(I) <= irq_slave_i(I).sel & irq_slave_i(I).adr & irq_slave_i(I).dat;
irq_push(I) <= irq_slave_i(I).cyc and irq_slave_i(I).stb and not irq_full(I);
irq_slave_o(I).stall <= irq_full(I);
irq_pop(I) <= r_pop(I) and r_status(I);
r_status(I) <= not irq_empty(I);
irq_slave_o(I).int <= '0'; --will be obsolete soon
irq_slave_o(I).rty <= '0';
irq_slave_o(I).err <= '0';
irq_slave_o(I).dat <= (others => '0');
p_ack : process(clk_i) -- ack all
begin
if rising_edge(clk_i) then
irq_slave_o(I).ack <= irq_push(I);
end if;
end process;
irqfifo : generic_sync_fifo
generic map (
g_data_width => g_datbits + g_adrbits + g_selbits,
g_size => g_depth,
g_show_ahead => true,
g_with_empty => true,
g_with_full => true)
port map (
rst_n_i => s_rst_n,
clk_i => clk_i,
d_i => irq_d(I),
we_i => irq_push(I),
q_o => irq_q(I),
rd_i => irq_pop(I),
empty_o => irq_empty(I),
full_o => irq_full(I),
almost_empty_o => open,
almost_full_o => open,
count_o => open);
end generate;
-------------------------------------------------------------------------
-------------------------------------------------------------------------
-- ctrl wb and output
-------------------------------------------------------------------------
r_status(31 downto g_queues) <= (others => '0');
ctrl_en <= ctrL_slave_i.cyc and ctrl_slave_i.stb;
adr <= unsigned(ctrl_slave_i.adr(7 downto 2)) & "00";
queue_offs <= to_integer(adr(adr'left downto 4)-1);
word_offs <= to_integer(adr(3 downto 0));
ctrl_slave_o.int <= '0';
ctrl_slave_o.rty <= '0';
ctrl_slave_o.stall <= '0';
process(clk_i)
variable v_dat : std_logic_vector(g_datbits-1 downto 0);
variable v_adr : std_logic_vector(g_adrbits-1 downto 0);
variable v_sel : std_logic_vector(g_selbits-1 downto 0);
begin
if rising_edge(clk_i) then
if(rst_n_i = '0' or r_rst_n = '0') then
r_rst_n <= '1';
r_pop <= (others => '0');
else
ctrl_slave_o.ack <= '0';
ctrl_slave_o.err <= '0';
r_pop <= (others => '0');
ctrl_slave_o.dat <= (others => '0');
if(ctrl_en = '1') then
if(to_integer(adr) < c_QUEUES) then
case to_integer(adr) is -- control registers
when c_RST => if(ctrl_slave_i.we = '1') then
r_rst_n <= '0';
end if;
ctrl_slave_o.dat <= (others => '0'); ctrl_slave_o.ack <= '1';
when c_STATUS => ctrl_slave_o.dat <= r_status; ctrl_slave_o.ack <= '1';
when c_POP => if(ctrl_slave_i.we = '1') then
r_pop <= ctrl_slave_i.dat;
end if;
ctrl_slave_o.dat <= (others => '0'); ctrl_slave_o.ack <= '1';
when others => null; ctrl_slave_o.err <= '1';
end case;
else -- queues, one mem page per queue
if(adr < c_QUEUES + c_N_QUEUE * g_queues and ctrl_slave_i.we = '0') then
v_dat := irq_q(queue_offs)(g_datbits-1 downto 0);
v_adr := irq_q(queue_offs)(g_adrbits+g_datbits-1 downto g_datbits);
v_sel := irq_q(queue_offs)(g_selbits + g_adrbits + g_datbits-1 downto g_adrbits+g_datbits);
case word_offs is
when c_OFFS_DATA => ctrl_slave_o.dat <= std_logic_vector(to_unsigned(0, 32-g_datbits)) & v_dat; ctrl_slave_o.ack <= '1';
when c_OFFS_ADDR => ctrl_slave_o.dat <= std_logic_vector(to_unsigned(0, 32-g_adrbits)) & v_adr; ctrl_slave_o.ack <= '1';
when c_OFFS_SEL => ctrl_slave_o.dat <= std_logic_vector(to_unsigned(0, 32-g_selbits)) & v_sel; ctrl_slave_o.ack <= '1';
when others => ctrl_slave_o.err <= '1';
end case;
else
ctrl_slave_o.err <= '1';
end if;
end if;
end if;
end if; -- rst
end if; -- clk edge
end process;
-------------------------------------------------------------------------
end architecture;
------------------------------------------------------------------------------
-- Title : WB Timer Interrupt
-- Project : Wishbone
------------------------------------------------------------------------------
-- File : wb_irq_timer.vhd
-- Author : Mathias Kreider
-- Company : GSI
-- Created : 2013-08-10
-- Last update: 2013-08-10
-- Platform : FPGA-generic
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Programmable Timer interrupt module (MSI)
-------------------------------------------------------------------------------
-- Copyright (c) 2013 GSI
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2013-08-10 1.0 mkreider Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wishbone_pkg.all;
use work.genram_pkg.all;
use work.wb_irq_pkg.all;
use work.gencores_pkg.all;
entity wb_irq_timer is
generic ( g_timers : natural := 4); -- number of instantiated timers
port (clk_sys_i : in std_logic; -- system clock
rst_sys_n_i : in std_logic; -- system reset (act LO)
tm_tai8ns_i : in std_logic_vector(63 downto 0); -- system time
ctrl_slave_o : out t_wishbone_slave_out; -- timer ctrl interface
ctrl_slave_i : in t_wishbone_slave_in;
irq_master_o : out t_wishbone_master_out; -- msi irq src
irq_master_i : in t_wishbone_master_in
);
end entity;
architecture behavioral of wb_irq_timer is
function f_wb_wr(pval : std_logic_vector; ival : std_logic_vector; sel : std_logic_vector; mode : string := "owr") return std_logic_vector is
variable n_sel : std_logic_vector(pval'range);
variable n_val : std_logic_vector(pval'range);
variable result : std_logic_vector(pval'range);
begin
for i in pval'range loop
n_sel(i) := sel(i / 8);
n_val(i) := ival(i);
end loop;
if(mode = "set") then
result := pval or (n_val and n_sel);
elsif (mode = "clr") then
result := pval and not (n_val and n_sel);
else
result := (pval and not n_sel) or (n_val and n_sel);
end if;
return result;
end f_wb_wr;
function f_or_vec(a : std_logic_vector) return std_logic is
variable result : std_logic := '0';
begin
for i in 0 to a'left loop
result := result or a(i);
end loop;
return result;
end f_or_vec;
function f_oob(a : std_logic_vector; limit : natural) return boolean is
begin
if(to_integer(unsigned(a)) > limit-1) then
return true;
else
return false;
end if;
end f_oob;
--******************************************************************************************
-- WB ctrl interface definitions and constants
--------------------------------------------------------------------------------------------
signal s_c_en, s_c_we, r_c_ack, r_c_err : std_logic;
signal s_c_adr : natural range 255 downto 0;
signal s_c_dati, r_c_dato : t_wishbone_data;
signal s_c_sel : t_wishbone_byte_select;
signal r_rst_n : std_logic;
constant c_RST : natural := 0; --0x00, wo, Reset Active Low
constant c_ARM_STAT : natural := c_RST +4; --0x04, ro, Shows armed timers, (1 armed, 0 disarmed), 1 bit per timer
constant c_ARM_SET : natural := c_ARM_STAT +4; --0x08, wo, arm timers,
constant c_ARM_CLR : natural := c_ARM_SET +4; --0x0C, wo, disarm timers,
constant c_SRC_STAT : natural := c_ARM_CLR +4; --0x10, ro, shows timer sources, (1 diff time, 0 abs time), 1 bit per timer
constant c_SRC_SET : natural := c_SRC_STAT +4; --0x14, wo, select diff time as source
constant c_SRC_CLR : natural := c_SRC_SET +4; --0x18, wo, select abs time as source
constant c_D_MODE_STAT : natural := c_SRC_CLR +4; --0x1C, ro, shows diff time modes, (1 periodic, 0 1time), 1 bit per timer
constant c_D_MODE_SET : natural := c_D_MODE_STAT +4; --0x20, wo, select periodic mode
constant c_D_MODE_CLR : natural := c_D_MODE_SET +4; --0x24, wo, select 1time mode
constant c_CSC_STAT : natural := c_D_MODE_CLR +4; --0x28, ro, shows cascaded start, (1 cascaded, 0 normal), 1 bit per timer
constant c_CSC_SET : natural := c_CSC_STAT +4; --0x2C, wo, set cascaded start
constant c_CSC_CLR : natural := c_CSC_SET +4; --0x30, wo, select normal start
constant c_DBG : natural := c_CSC_CLR +4; --0x34, wo, reset counters, 1 bit per timer
constant c_BASE_TIMERS : natural := 64; --0x40
constant c_TIMER_SEL : natural := c_BASE_TIMERS +0; --0x40, rw, timer select. !!!CAUTION!!! content of all c_TM_... regs depends on this
constant c_TM_TIME_HI : natural := c_TIMER_SEL +4; --0x44, rw, deadline HI word
constant c_TM_TIME_LO : natural := c_TM_TIME_HI +4; --0x48, rw, deadline LO word
constant c_TM_MSG : natural := c_TM_TIME_LO +4; --0x4C, rw, MSI msg to be sent on MSI when deadline7D5D7F53 is hit
constant c_TM_DST_ADR : natural := c_TM_MSG +4; --0x50, rw, MSI adr to send the msg to when deadline is hit
constant c_CSC_SEL : natural := c_TM_DST_ADR +4; --0x54, rw, select comparator output for cascaded start
subtype t_msk_cnt is unsigned(3 downto 0);
subtype t_time is std_logic_vector(63 downto 0);
type t_msk_cnt_array is array(natural range <>) of t_msk_cnt;
type t_time_array is array(natural range <>) of t_time;
type t_state is (st_IDLE, st_SEND);
signal r_tsl : std_logic_vector(g_timers-1 downto 0); --timer select
signal r_arm, r_arm_1, s_arm_edge : std_logic_vector(g_timers-1 downto 0); --comps armed
signal r_src : std_logic_vector(g_timers-1 downto 0); --comps sources
signal r_mod : std_logic_vector(g_timers-1 downto 0); --counters mode
signal r_csc : std_logic_vector(g_timers-1 downto 0); --cascade starts
signal r_msg : t_wishbone_data_array(g_timers-1 downto 0);
signal r_dst : t_wishbone_address_array(g_timers-1 downto 0);
signal r_csl : t_wishbone_data_array(g_timers-1 downto 0); --cascade selects
signal r_ddl_hi : t_wishbone_data_array(g_timers-1 downto 0);
signal r_ddl_lo : t_wishbone_data_array(g_timers-1 downto 0);
signal s_comp_mask : std_logic_vector(g_timers-1 downto 0);
signal r_msk_cnt : t_msk_cnt_array(g_timers-1 downto 0);
signal r_time : t_time;
signal s_deadline : t_time_array(g_timers-1 downto 0);
signal s_deadline_abs : t_time_array(g_timers-1 downto 0);
signal r_deadline_offs : t_time_array(g_timers-1 downto 0);
signal s_x, s_y, s_sy : t_time_array(g_timers-1 downto 0);
signal s_ovf1, s_ovf2 : std_logic_vector(g_timers-1 downto 0);
signal s_csc_arm, s_csc_arm_edge : std_logic_vector(g_timers-1 downto 0); --cascade starts
--------------------------------------------------------------------------------------------
signal r_comp_1st, comp, s_comp_edge, r_comp : std_logic_vector(g_timers-1 downto 0);
begin
--******************************************************************************************
-- WB ctrl interface implementation
--------------------------------------------------------------------------------------------
s_c_en <= ctrL_slave_i.cyc and ctrl_slave_i.stb;
s_c_adr <= to_integer(unsigned(ctrl_slave_i.adr(7 downto 2)) & "00");
s_c_we <= ctrl_slave_i.we;
s_c_dati <= ctrl_slave_i.dat;
s_c_sel <= ctrl_slave_i.sel;
ctrl_slave_o.int <= '0';
ctrl_slave_o.rty <= '0';
ctrl_slave_o.stall <= '0';
ctrl_slave_o.ack <= r_c_ack;
ctrl_slave_o.err <= r_c_err;
ctrl_slave_o.dat <= r_c_dato;
process(clk_sys_i)
variable v_tm_sl : natural range g_timers-1 downto 0;
begin
if rising_edge(clk_sys_i) then
if(rst_sys_n_i = '0' or r_rst_n <= '0') then
r_c_ack <= '0';
r_c_err <= '0';
r_rst_n <= '1';
r_tsl <= (others => '0'); -- timer select
r_arm <= (others => '0'); -- armed timers
r_src <= (others => '0'); -- src selects
r_mod <= (others => '0'); -- counter modes
r_csc <= (others => '0'); -- counter cascade start
else
-- Fire and Forget Registers
r_c_ack <= '0';
r_c_err <= '0';
r_c_dato <= (others => '0');
if(s_c_en = '1') then
v_tm_sl := to_integer(unsigned(r_tsl));
r_c_ack <= '1';
if(s_c_we = '1') then
case s_c_adr is
when c_RST => r_rst_n <= '0';
when c_ARM_SET => r_arm <= f_wb_wr(r_arm, s_c_dati, s_c_sel, "set");
when c_ARM_CLR => r_arm <= f_wb_wr(r_arm, s_c_dati, s_c_sel, "clr");
when c_SRC_SET => r_src <= f_wb_wr(r_src, s_c_dati, s_c_sel, "set");
when c_SRC_CLR => r_src <= f_wb_wr(r_src, s_c_dati, s_c_sel, "clr");
when c_D_MODE_SET => r_mod <= f_wb_wr(r_mod, s_c_dati, s_c_sel, "set");
when c_D_MODE_CLR => r_mod <= f_wb_wr(r_mod, s_c_dati, s_c_sel, "clr");
when c_CSC_SET => r_csc <= f_wb_wr(r_csc, s_c_dati, s_c_sel, "set");
when c_CSC_CLR => r_csc <= f_wb_wr(r_csc, s_c_dati, s_c_sel, "clr");
when c_TIMER_SEL => if(f_oob(s_c_dati, g_timers)) then -- owr with limit check
r_c_ack <= '0'; r_c_err <= '1';
else
r_tsl <= f_wb_wr(r_tsl, s_c_dati, s_c_sel, "owr");
end if;
when c_TM_TIME_HI => r_ddl_hi(v_tm_sl) <= f_wb_wr(r_ddl_hi(v_tm_sl), s_c_dati, s_c_sel, "owr");
when c_TM_TIME_LO => r_ddl_lo(v_tm_sl) <= f_wb_wr(r_ddl_lo(v_tm_sl), s_c_dati, s_c_sel, "owr");
when c_TM_MSG => r_msg(v_tm_sl) <= f_wb_wr(r_msg(v_tm_sl), s_c_dati, s_c_sel, "owr");
when c_TM_DST_ADR => r_dst(v_tm_sl) <= f_wb_wr(r_dst(v_tm_sl), s_c_dati, s_c_sel, "owr");
when c_CSC_SEL => if(f_oob(s_c_dati, g_timers)) then -- owr with limit check
r_c_ack <= '0'; r_c_err <= '1';
else
r_csl(v_tm_sl) <= f_wb_wr(r_csl(v_tm_sl), s_c_dati, s_c_sel, "owr");
end if;
when others => r_c_ack <= '0'; r_c_err <= '1';
end case;
else
case s_c_adr is
when c_ARM_STAT => r_c_dato(r_arm'range) <= r_arm;
when c_SRC_STAT => r_c_dato(r_src'range) <= r_src;
when c_D_MODE_STAT => r_c_dato(r_mod'range) <= r_mod;
when c_CSC_STAT => r_c_dato(r_csc'range) <= r_csc;
when c_TIMER_SEL => r_c_dato(r_tsl'range) <= r_tsl;
when c_TM_TIME_HI => r_c_dato(r_ddl_hi(v_tm_sl)'range) <= r_ddl_hi(v_tm_sl);
when c_TM_TIME_LO => r_c_dato(r_ddl_lo(v_tm_sl)'range) <= r_ddl_lo(v_tm_sl);
when c_TM_MSG => r_c_dato(r_msg(v_tm_sl)'range) <= r_msg(v_tm_sl);
when c_TM_DST_ADR => r_c_dato(r_dst(v_tm_sl)'range) <= r_dst(v_tm_sl);
when c_CSC_SEL => r_c_dato(r_csl(v_tm_sl)'range) <= r_csl(v_tm_sl);
when others => r_c_ack <= '0'; r_c_err <= '1';
end case;
end if; -- s_c_we
end if; -- s_c_en
end if; -- rst
end if; -- clk edge
end process;
-- register system time to reduce fan out (1 cycle, must be taken into account for delay compensation in time clock crossing core!)
-- register comparator output
-- register start (arm)
reggies : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
r_time <= tm_tai8ns_i;
r_comp <= comp;
r_arm_1 <= r_arm;
end if;
end process reggies;
-- rising edge detectors for comparators and arm regs
s_comp_edge <= (not r_comp and comp);
s_arm_edge <= (not r_arm_1 and r_arm);
--******************************************************************************************
-- counters, src muxes and comparators
--------------------------------------------------------------------------------------------
G1: for I in 0 to g_timers-1 generate
s_csc_arm_edge(I) <= r_arm(I) and r_csc(I) and s_comp_edge(to_integer(unsigned(r_csl(I))));
s_csc_arm(I) <= r_csc(I) and r_comp_1st(to_integer(unsigned(r_csl(I))));
s_deadline_abs(I) <= (r_ddl_hi(I) & r_ddl_lo(I));
s_comp_mask(I) <= std_logic(r_msk_cnt(I)(r_msk_cnt(I)'left)) and r_arm(I) and (not r_csc(I) or s_csc_arm(I) or s_csc_arm_edge(I));
-- offset for deadline. set to current time if ...
deadline_offs : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if(rst_sys_n_i = '0') then
r_msk_cnt(I) <= to_unsigned(5, r_msk_cnt(I)'length);
r_deadline_offs(I) <= (others => '0');
else
-- its in diff mode and ...
-- start or periodic overflow or cascaded start occured
if( (s_arm_edge(I) or (r_mod(I) and s_comp_edge(I) and std_logic(r_msk_cnt(I)(r_msk_cnt(I)'left))) or s_csc_arm_edge(I)) = '1' ) then
-- start countdown for comp mask
r_msk_cnt(I) <= to_unsigned(5, r_msk_cnt(I)'length);
if( r_src(I) = '1') then
r_deadline_offs(I) <= r_time;
else
r_deadline_offs(I) <= (others => '0');
end if;
else
-- countdown until comp output mask is 1
r_msk_cnt(I) <= r_msk_cnt(I) - (to_unsigned(0, r_msk_cnt(I)'length-1) & not r_msk_cnt(I)(r_msk_cnt(I)'left));
end if;
end if;
end if;
end process deadline_offs;
--create comparators
-- carry-save
s_x(I) <= s_deadline_abs(I) xor r_deadline_offs(I) xor not r_time;
s_y(I) <= ((s_deadline_abs(I) and r_deadline_offs(I))
or (r_deadline_offs(I) and not r_time)
or (s_deadline_abs(I) and not r_time));
s_ovf1(I) <= s_y(I)(63);
s_sy(I)(63 downto 1) <= s_y(I)(62 downto 0);
s_sy(I)(0) <= '0';
ea : gc_big_adder
port map(
clk_i => clk_sys_i,
stall_i => '0',
a_i => s_x(I),
b_i => s_sy(I),
c_i => '0',
c1_o => s_ovf2(I),
x2_o => open,
c2_o => open);
end generate;
comps : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if(rst_sys_n_i = '0') then
r_comp_1st <= (others => '0');
comp <= (others => '0');
else
r_comp_1st <= (r_comp_1st or s_comp_edge) and not s_arm_edge;
comp <= not (s_ovf1 or s_ovf2);
end if;
end if;
end process comps;
irq : irqm_core
generic map(g_channels => g_timers,
g_round_rb => true,
g_det_edge => false
)
port map( clk_i => clk_sys_i,
rst_n_i => rst_sys_n_i,
--msi if
irq_master_o => irq_master_o,
irq_master_i => irq_master_i,
--config
msi_dst_array => r_dst,
msi_msg_array => r_msg,
en_i => '1',
mask_i => s_comp_mask,
--irq lines
irq_i => s_comp_edge
);
end architecture behavioral;
files = [ "generated/xwb_lm32.vhd",
"generated/lm32_allprofiles.v",
"src/lm32_mc_arithmetic.v",
"src/jtag_cores.v",
"src/lm32_adder.v",
"src/lm32_addsub.v",
"src/lm32_dp_ram.vhd",
"src/lm32_logic_op.v",
"src/lm32_ram.vhd",
"src/lm32_shifter.v"];
if(target == "altera"):
files.extend(["platform/generic/lm32_multiplier.v", "platform/altera/jtag_tap.v"]);
elif (target == "xilinx" and syn_device[0:4].upper()=="XC6S"): # Spartan6
files.extend(["platform/spartan6/lm32_multiplier.v", "platform/spartan6/jtag_tap.v"])
else:
files.extend(["platform/generic/lm32_multiplier.v", "platform/generic/jtag_tap.v"]);
GenCores LM32 VHDL configurable wrapper
---------------------------------------
This is a VHDL-ized distribution of LM32 CPU core with pre-implemented feature sets selectable using a "profile" generic.
Note that this core uses the new (spec rev. B4) pipelined mode, therefore it requires a *pipelined* interconnect.
Currently there are 6 different profiles (all support interrupts)
- minimal - bare minimum (just LM32 core)
- medium - LM32 core with pipelined multiplier, barrel shifter and sign extension unit
- medium_icache - the above + instruction cache (up to 3 times faster execution)
- medium_(icache_)debug - the above 2 versions with full JTAG debugger
- full - full LM32 core (all instructions + I/D caches + bus errors)
- full_debug - full core with debug
The profiles are defined in lm32.profiles file. If you want to add/remove any, re-run gen_lmcores.py script afterwards (requires
Modelsim vlog compiler for preprocessing Verilog sources).
Acknowledgements:
- Lattice Semiconductor, for open-sourcing this excellent CPU
- Sebastien Bordeauducq, for making the LM32 more platform-agnostic
- Wesley W. Terpstra (GSI) - for excellent pipelined Wishbone wrapper (used in auto-generated xwb_lm32.vhd)
and Xilinx/Altera embedded JTAG cores.
#!/usr/bin/python
# LM32 core generator/customizer. Produces a set of LM32 cores preconfigured according to profiles stored
# in 'lm32.profiles' file.
# The outputs are a single Verilog file containing all customized modules and
# a vhdl top-level wrapper with a set of generics allowing to choose the profile from within a Verilog/VHDL design
import os, glob, tokenize, copy
LM32_features = [ "CFG_PL_MULTIPLY_ENABLED",
"CFG_PL_BARREL_SHIFT_ENABLED",
"CFG_SIGN_EXTEND_ENABLED",
"CFG_MC_DIVIDE_ENABLED",
"CFG_FAST_UNCONDITIONAL_BRANCH",
"CFG_ICACHE_ENABLED",
"CFG_DCACHE_ENABLED",
"CFG_WITH_DEBUG",
"CFG_INTERRUPTS_ENABLED",
"CFG_BUS_ERRORS_ENABLED" ];
LM32_files = [
"src/lm32_top.v",
"src/lm32_mc_arithmetic.v",
"src/lm32_cpu.v",
"src/lm32_load_store_unit.v",
"src/lm32_decoder.v",
"src/lm32_icache.v",
"src/lm32_dcache.v",
"src/lm32_debug.v",
"src/lm32_instruction_unit.v",
"src/lm32_jtag.v",
"src/lm32_interrupt.v"];
LM32_mods = ["lm32_cpu",
"lm32_dcache",
"lm32_debug",
"lm32_decoder",
"lm32_icache",
"lm32_instruction_unit",
"lm32_interrupt",
"lm32_jtag",
"lm32_load_store_unit",
"lm32_mc_arithmetic",
"lm32_top"];
def tobin(x, count=16):
s=""
for i in range(count-1, -1, -1):
if(x & (1<<i)):
s=s+"1";
else:
s=s+"0";
return s
def mangle_names(string, profile_name):
for pattern in LM32_mods:
string = string.replace(pattern, pattern + "_"+profile_name)
return string;
def gen_customized_version(profile_name, feats):
print("GenCfg ", profile_name);
tmp_dir = "/tmp/lm32-customizer";
try:
os.mkdir(tmp_dir);
except:
pass
f = open(tmp_dir + "/system_conf.v", "w");
f.write("`ifndef __system_conf_v\n`define __system_conf_v\n");
for feat in feats:
f.write("`define " + feat + "\n");
f.write("`define CFG_EBA_RESET 32'h00000000\n\
`define CFG_DEBA_RESET 32'h10000000\n\
`define CFG_EBR_POSEDGE_REGISTER_FILE\n\
`define CFG_ICACHE_ASSOCIATIVITY 1\n\
`define CFG_ICACHE_SETS 256\n\
`define CFG_ICACHE_BYTES_PER_LINE 16\n\
`define CFG_ICACHE_BASE_ADDRESS 32'h0\n\
`define CFG_ICACHE_LIMIT 32'h7fffffff\n\
`define CFG_DCACHE_ASSOCIATIVITY 1\n\
`define CFG_DCACHE_SETS 256\n\
`define CFG_DCACHE_BYTES_PER_LINE 16\n\
`define CFG_DCACHE_BASE_ADDRESS 32'h0\n\
`define CFG_DCACHE_LIMIT 32'h7fffffff\n\
`ifdef CFG_WITH_DEBUG\n\
`define CFG_JTAG_ENABLED\n\
`define CFG_JTAG_UART_ENABLED\n\
`define CFG_DEBUG_ENABLED\n\
`define CFG_HW_DEBUG_ENABLED\n\
`define CFG_BREAKPOINTS 32'h4\n\
`define CFG_WATCHPOINTS 32'h4\n\
`endif\n");
f.write("`endif\n");
f.close();
file_list = LM32_files;
ftmp = open(tmp_dir + "/tmp.v", "w");
for fname in file_list:
f = open(fname, "r");
contents = f.read();
mangled = mangle_names(contents, profile_name)
ftmp.write(mangled);
f.close();
ftmp.close();
os.system("vlog -quiet -nologo -E " + tmp_dir+"/lm32_"+profile_name+".v " + tmp_dir + "/tmp.v +incdir+" +tmp_dir+" +incdir+src");
os.system("cat "+tmp_dir+"/lm32_*.v | egrep -v '`line' > generated/lm32_allprofiles.v")
def parse_profiles():
f = open("lm32.profiles", "r")
p = map(lambda x: x.rstrip(" \n").rsplit(' '), f.readlines())
f.close()
return list(p)
def gen_vhdl_component(f, profile_name):
f.write("component lm32_top_"+profile_name+" is \n")
f.write("generic ( eba_reset: std_logic_vector(31 downto 0) );\n");
f.write("port (\n");
f.write("""
clk_i : in std_logic;
rst_i : in std_logic;
interrupt : in std_logic_vector(31 downto 0);
I_DAT_I : in std_logic_vector(31 downto 0);
I_ACK_I : in std_logic;
I_ERR_I : in std_logic;
I_RTY_I : in std_logic;
D_DAT_I : in std_logic_vector(31 downto 0);
D_ACK_I : in std_logic;
D_ERR_I : in std_logic;
D_RTY_I : in std_logic;
I_DAT_O : out std_logic_vector(31 downto 0);
I_ADR_O : out std_logic_vector(31 downto 0);
I_CYC_O : out std_logic;
I_SEL_O : out std_logic_vector(3 downto 0);
I_STB_O : out std_logic;
I_WE_O : out std_logic;
I_CTI_O : out std_logic_vector(2 downto 0);
I_LOCK_O : out std_logic;
I_BTE_O : out std_logic_vector(1 downto 0);
D_DAT_O : out std_logic_vector(31 downto 0);
D_ADR_O : out std_logic_vector(31 downto 0);
D_CYC_O : out std_logic;
D_SEL_O : out std_logic_vector(3 downto 0);
D_STB_O : out std_logic;
D_WE_O : out std_logic;
D_CTI_O : out std_logic_vector(2 downto 0);
D_LOCK_O : out std_logic;
D_BTE_O : out std_logic_vector(1 downto 0));
end component;
""");
def gen_burst_eval_func(f, prof, cache):
f.write("function f_eval_"+cache+"_burst_length(profile_name:string) return natural is\nbegin\n");
t = {True: 4, False:1}
for p in prof:
has_cache = any(map(lambda x: cache.upper()+"CACHE_ENABLED" in x, list(p[1:])))
f.write("if profile_name = \""+p[0]+"\" then return "+str(t[has_cache])+"; end if; \n");
f.write("return 0;\nend function;\n");
def gen_vhdl_wrapper(prof):
f=open("generated/xwb_lm32.vhd","w");
f.write("""--auto-generated by gen_lmcores.py. Don't hand-edit please
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.wishbone_pkg.all;
entity xwb_lm32 is
generic(g_profile: string;
g_reset_vector: std_logic_vector(31 downto 0) := x"00000000");
port(
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
irq_i : in std_logic_vector(31 downto 0);
dwb_o : out t_wishbone_master_out;
dwb_i : in t_wishbone_master_in;
iwb_o : out t_wishbone_master_out;
iwb_i : in t_wishbone_master_in);
end xwb_lm32;
architecture rtl of xwb_lm32 is \n""");
gen_burst_eval_func(f, prof, "i");
gen_burst_eval_func(f, prof, "d");
for p in prof:
gen_vhdl_component(f, p[0])
f.write("""
function pick(first : boolean;
a, b : t_wishbone_address)
return t_wishbone_address is
begin
if first then
return a;
else
return b;
end if;
end pick;
function b2l(val : boolean)
return std_logic is
begin
if val then
return '1';
else
return '0';
end if;
end b2l;
function strip_undefined
(x : std_logic_vector) return std_logic_vector is
variable tmp : std_logic_vector(x'left downto 0);
begin
for i in 0 to x'left loop
if(x(i)='X' or x(i)='U' or x(i)='Z') then
tmp(i) := '0';
else
tmp(i) := x(i);
end if;
end loop; -- i
return tmp;
end strip_undefined;
constant dcache_burst_length : natural := f_eval_d_burst_length(g_profile);
constant icache_burst_length : natural := f_eval_i_burst_length(g_profile);
-- Control pins from the LM32
signal I_ADR : t_wishbone_address;
signal D_ADR : t_wishbone_address;
signal I_CYC : std_logic;
signal D_CYC : std_logic;
signal I_CTI : t_wishbone_cycle_type;
signal D_CTI : t_wishbone_cycle_type;
-- We also watch the STALL lines from the v4 slaves
-- Registered logic:
signal inst_was_busy : std_logic;
signal data_was_busy : std_logic;
signal inst_addr_reg : t_wishbone_address;
signal data_addr_reg : t_wishbone_address;
signal inst_remaining : natural range 0 to icache_burst_length;
signal data_remaining : natural range 0 to dcache_burst_length;
-- Asynchronous logic:
signal I_STB_O : std_logic;
signal D_STB_O : std_logic;
signal rst:std_logic;
begin
rst <= not rst_n_i;
""");
for p in prof:
f.write("gen_profile_"+p[0]+": if (g_profile = \"" + p[0]+"\") generate\n");
f.write("U_Wrapped_LM32: lm32_top_"+p[0]+"\n");
f.write("""
generic map (
eba_reset => g_reset_vector)
port map(
clk_i => clk_sys_i,
rst_i => rst,
interrupt => irq_i,
-- Pass slave responses through unmodified
I_DAT_I => strip_undefined(iwb_i.DAT),
I_ACK_I => iwb_i.ACK,
I_ERR_I => iwb_i.ERR,
I_RTY_I => iwb_i.RTY,
D_DAT_I => strip_undefined(dwb_i.DAT),
D_ACK_I => dwb_i.ACK,
D_ERR_I => dwb_i.ERR,
D_RTY_I => dwb_i.RTY,
-- Writes can only happen as a single cycle
I_DAT_O => iwb_o.DAT,
D_DAT_O => dwb_o.DAT,
I_WE_O => iwb_o.WE,
D_WE_O => dwb_o.WE,
-- SEL /= 1111 only for single cycles
I_SEL_O => iwb_o.SEL,
D_SEL_O => dwb_o.SEL,
-- We can ignore BTE as we know it's always linear burst mode
I_BTE_O => open,
D_BTE_O => open,
-- Lock is never flagged by LM32. Besides, WBv4 locks intercon on CYC.
I_LOCK_O => open,
D_LOCK_O => open,
-- The LM32 has STB=CYC always
I_STB_O => open,
D_STB_O => open,
-- We monitor these pins to direct the adapter's logic
I_ADR_O => I_ADR,
I_CYC_O => I_CYC,
I_CTI_O => I_CTI,
D_ADR_O => D_ADR,
D_CYC_O => D_CYC,
D_CTI_O => D_CTI);
""");
f.write("end generate gen_profile_"+p[0]+";\n")
f.write("""
-- Cycle durations always match in our adapter
iwb_o.CYC <= I_CYC;
dwb_o.CYC <= D_CYC;
iwb_o.STB <= I_STB_O;
dwb_o.STB <= D_STB_O;
I_STB_O <= (I_CYC and not inst_was_busy) or b2l(inst_remaining /= 0);
inst : process(clk_sys_i)
variable inst_addr : t_wishbone_address;
variable inst_length : natural;
begin
if rising_edge(clk_sys_i) then
if rst = '1' then
inst_was_busy <= '0';
inst_remaining <= 0;
inst_addr_reg <= (others => '0');
else
inst_was_busy <= I_CYC;
-- Is this the start of a new WB cycle?
if I_CYC = '1' and inst_was_busy = '0' then
inst_addr := I_ADR;
if I_CTI = "010" then
inst_length := icache_burst_length;
else
inst_length := 1;
end if;
else
inst_addr := inst_addr_reg;
inst_length := inst_remaining;
end if;
-- When stalled, we cannot advance the address
if iwb_i.STALL = '0' and I_STB_O = '1' then
inst_addr_reg <= std_logic_vector(unsigned(inst_addr) + 4);
inst_remaining <= inst_length - 1;
else
inst_addr_reg <= inst_addr;
inst_remaining <= inst_length;
end if;
end if;
end if;
end process;
D_STB_O <= (D_CYC and not data_was_busy) or b2l(data_remaining /= 0);
data : process(clk_sys_i)
variable data_addr : t_wishbone_address;
variable data_length : natural;
begin
if rising_edge(clk_sys_i) then
if rst = '1' then
data_was_busy <= '0';
data_remaining <= 0;
data_addr_reg <= (others => '0');
else
data_was_busy <= D_CYC;
-- Is this the start of a new WB cycle?
if D_CYC = '1' and data_was_busy = '0' then
data_addr := D_ADR;
if D_CTI = "010" then
data_length := dcache_burst_length;
else
data_length := 1;
end if;
else
data_addr := data_addr_reg;
data_length := data_remaining;
end if;
-- When stalled, we cannot advance the address
if dwb_i.STALL = '0' and D_STB_O = '1' then
data_addr_reg <= std_logic_vector(unsigned(data_addr) + 4);
data_remaining <= data_length - 1;
else
data_addr_reg <= data_addr;
data_remaining <= data_length;
end if;
end if;
end if;
end process;
-- The first request uses the WBv3 address, thereafter an incrementing one.
dwb_o.ADR <= pick(data_was_busy = '0', D_ADR, data_addr_reg);
iwb_o.ADR <= pick(inst_was_busy = '0', I_ADR, inst_addr_reg);
end rtl;
""");
f.close()
os.system("vlib work")
profiles = parse_profiles()
for p in profiles:
gen_customized_version(p[0], p[1:])
gen_vhdl_wrapper(profiles)
This source diff could not be displayed because it is too large. You can view the blob instead.
--auto-generated by gen_lmcores.py. Don't hand-edit please
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.wishbone_pkg.all;
entity xwb_lm32 is
generic(g_profile: string;
g_reset_vector: std_logic_vector(31 downto 0) := x"00000000");
port(
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
irq_i : in std_logic_vector(31 downto 0);
dwb_o : out t_wishbone_master_out;
dwb_i : in t_wishbone_master_in;
iwb_o : out t_wishbone_master_out;
iwb_i : in t_wishbone_master_in);
end xwb_lm32;
architecture rtl of xwb_lm32 is
function f_eval_i_burst_length(profile_name:string) return natural is
begin
if profile_name = "minimal" then return 1; end if;
if profile_name = "medium" then return 1; end if;
if profile_name = "medium_icache" then return 4; end if;
if profile_name = "medium_debug" then return 4; end if;
if profile_name = "medium_icache_debug" then return 4; end if;
if profile_name = "full" then return 4; end if;
if profile_name = "full_debug" then return 4; end if;
return 0;
end function;
function f_eval_d_burst_length(profile_name:string) return natural is
begin
if profile_name = "minimal" then return 1; end if;
if profile_name = "medium" then return 1; end if;
if profile_name = "medium_icache" then return 1; end if;
if profile_name = "medium_debug" then return 1; end if;
if profile_name = "medium_icache_debug" then return 1; end if;
if profile_name = "full" then return 4; end if;
if profile_name = "full_debug" then return 4; end if;
return 0;
end function;
component lm32_top_minimal is
generic ( eba_reset: std_logic_vector(31 downto 0) );
port (
clk_i : in std_logic;
rst_i : in std_logic;
interrupt : in std_logic_vector(31 downto 0);
I_DAT_I : in std_logic_vector(31 downto 0);
I_ACK_I : in std_logic;
I_ERR_I : in std_logic;
I_RTY_I : in std_logic;
D_DAT_I : in std_logic_vector(31 downto 0);
D_ACK_I : in std_logic;
D_ERR_I : in std_logic;
D_RTY_I : in std_logic;
I_DAT_O : out std_logic_vector(31 downto 0);
I_ADR_O : out std_logic_vector(31 downto 0);
I_CYC_O : out std_logic;
I_SEL_O : out std_logic_vector(3 downto 0);
I_STB_O : out std_logic;
I_WE_O : out std_logic;
I_CTI_O : out std_logic_vector(2 downto 0);
I_LOCK_O : out std_logic;
I_BTE_O : out std_logic_vector(1 downto 0);
D_DAT_O : out std_logic_vector(31 downto 0);
D_ADR_O : out std_logic_vector(31 downto 0);
D_CYC_O : out std_logic;
D_SEL_O : out std_logic_vector(3 downto 0);
D_STB_O : out std_logic;
D_WE_O : out std_logic;
D_CTI_O : out std_logic_vector(2 downto 0);
D_LOCK_O : out std_logic;
D_BTE_O : out std_logic_vector(1 downto 0));
end component;
component lm32_top_medium is
generic ( eba_reset: std_logic_vector(31 downto 0) );
port (
clk_i : in std_logic;
rst_i : in std_logic;
interrupt : in std_logic_vector(31 downto 0);
I_DAT_I : in std_logic_vector(31 downto 0);
I_ACK_I : in std_logic;
I_ERR_I : in std_logic;
I_RTY_I : in std_logic;
D_DAT_I : in std_logic_vector(31 downto 0);
D_ACK_I : in std_logic;
D_ERR_I : in std_logic;
D_RTY_I : in std_logic;
I_DAT_O : out std_logic_vector(31 downto 0);
I_ADR_O : out std_logic_vector(31 downto 0);
I_CYC_O : out std_logic;
I_SEL_O : out std_logic_vector(3 downto 0);
I_STB_O : out std_logic;
I_WE_O : out std_logic;
I_CTI_O : out std_logic_vector(2 downto 0);
I_LOCK_O : out std_logic;
I_BTE_O : out std_logic_vector(1 downto 0);
D_DAT_O : out std_logic_vector(31 downto 0);
D_ADR_O : out std_logic_vector(31 downto 0);
D_CYC_O : out std_logic;
D_SEL_O : out std_logic_vector(3 downto 0);
D_STB_O : out std_logic;
D_WE_O : out std_logic;
D_CTI_O : out std_logic_vector(2 downto 0);
D_LOCK_O : out std_logic;
D_BTE_O : out std_logic_vector(1 downto 0));
end component;
component lm32_top_medium_icache is
generic ( eba_reset: std_logic_vector(31 downto 0) );
port (
clk_i : in std_logic;
rst_i : in std_logic;
interrupt : in std_logic_vector(31 downto 0);
I_DAT_I : in std_logic_vector(31 downto 0);
I_ACK_I : in std_logic;
I_ERR_I : in std_logic;
I_RTY_I : in std_logic;
D_DAT_I : in std_logic_vector(31 downto 0);
D_ACK_I : in std_logic;
D_ERR_I : in std_logic;
D_RTY_I : in std_logic;
I_DAT_O : out std_logic_vector(31 downto 0);
I_ADR_O : out std_logic_vector(31 downto 0);
I_CYC_O : out std_logic;
I_SEL_O : out std_logic_vector(3 downto 0);
I_STB_O : out std_logic;
I_WE_O : out std_logic;
I_CTI_O : out std_logic_vector(2 downto 0);
I_LOCK_O : out std_logic;
I_BTE_O : out std_logic_vector(1 downto 0);
D_DAT_O : out std_logic_vector(31 downto 0);
D_ADR_O : out std_logic_vector(31 downto 0);
D_CYC_O : out std_logic;
D_SEL_O : out std_logic_vector(3 downto 0);
D_STB_O : out std_logic;
D_WE_O : out std_logic;
D_CTI_O : out std_logic_vector(2 downto 0);
D_LOCK_O : out std_logic;
D_BTE_O : out std_logic_vector(1 downto 0));
end component;
component lm32_top_medium_debug is
generic ( eba_reset: std_logic_vector(31 downto 0) );
port (
clk_i : in std_logic;
rst_i : in std_logic;
interrupt : in std_logic_vector(31 downto 0);
I_DAT_I : in std_logic_vector(31 downto 0);
I_ACK_I : in std_logic;
I_ERR_I : in std_logic;
I_RTY_I : in std_logic;
D_DAT_I : in std_logic_vector(31 downto 0);
D_ACK_I : in std_logic;
D_ERR_I : in std_logic;
D_RTY_I : in std_logic;
I_DAT_O : out std_logic_vector(31 downto 0);
I_ADR_O : out std_logic_vector(31 downto 0);
I_CYC_O : out std_logic;
I_SEL_O : out std_logic_vector(3 downto 0);
I_STB_O : out std_logic;
I_WE_O : out std_logic;
I_CTI_O : out std_logic_vector(2 downto 0);
I_LOCK_O : out std_logic;
I_BTE_O : out std_logic_vector(1 downto 0);
D_DAT_O : out std_logic_vector(31 downto 0);
D_ADR_O : out std_logic_vector(31 downto 0);
D_CYC_O : out std_logic;
D_SEL_O : out std_logic_vector(3 downto 0);
D_STB_O : out std_logic;
D_WE_O : out std_logic;
D_CTI_O : out std_logic_vector(2 downto 0);
D_LOCK_O : out std_logic;
D_BTE_O : out std_logic_vector(1 downto 0));
end component;
component lm32_top_medium_icache_debug is
generic ( eba_reset: std_logic_vector(31 downto 0) );
port (
clk_i : in std_logic;
rst_i : in std_logic;
interrupt : in std_logic_vector(31 downto 0);
I_DAT_I : in std_logic_vector(31 downto 0);
I_ACK_I : in std_logic;
I_ERR_I : in std_logic;
I_RTY_I : in std_logic;
D_DAT_I : in std_logic_vector(31 downto 0);
D_ACK_I : in std_logic;
D_ERR_I : in std_logic;
D_RTY_I : in std_logic;
I_DAT_O : out std_logic_vector(31 downto 0);
I_ADR_O : out std_logic_vector(31 downto 0);
I_CYC_O : out std_logic;
I_SEL_O : out std_logic_vector(3 downto 0);
I_STB_O : out std_logic;
I_WE_O : out std_logic;
I_CTI_O : out std_logic_vector(2 downto 0);
I_LOCK_O : out std_logic;
I_BTE_O : out std_logic_vector(1 downto 0);
D_DAT_O : out std_logic_vector(31 downto 0);
D_ADR_O : out std_logic_vector(31 downto 0);
D_CYC_O : out std_logic;
D_SEL_O : out std_logic_vector(3 downto 0);
D_STB_O : out std_logic;
D_WE_O : out std_logic;
D_CTI_O : out std_logic_vector(2 downto 0);
D_LOCK_O : out std_logic;
D_BTE_O : out std_logic_vector(1 downto 0));
end component;
component lm32_top_full is
generic ( eba_reset: std_logic_vector(31 downto 0) );
port (
clk_i : in std_logic;
rst_i : in std_logic;
interrupt : in std_logic_vector(31 downto 0);
I_DAT_I : in std_logic_vector(31 downto 0);
I_ACK_I : in std_logic;
I_ERR_I : in std_logic;
I_RTY_I : in std_logic;
D_DAT_I : in std_logic_vector(31 downto 0);
D_ACK_I : in std_logic;
D_ERR_I : in std_logic;
D_RTY_I : in std_logic;
I_DAT_O : out std_logic_vector(31 downto 0);
I_ADR_O : out std_logic_vector(31 downto 0);
I_CYC_O : out std_logic;
I_SEL_O : out std_logic_vector(3 downto 0);
I_STB_O : out std_logic;
I_WE_O : out std_logic;
I_CTI_O : out std_logic_vector(2 downto 0);
I_LOCK_O : out std_logic;
I_BTE_O : out std_logic_vector(1 downto 0);
D_DAT_O : out std_logic_vector(31 downto 0);
D_ADR_O : out std_logic_vector(31 downto 0);
D_CYC_O : out std_logic;
D_SEL_O : out std_logic_vector(3 downto 0);
D_STB_O : out std_logic;
D_WE_O : out std_logic;
D_CTI_O : out std_logic_vector(2 downto 0);
D_LOCK_O : out std_logic;
D_BTE_O : out std_logic_vector(1 downto 0));
end component;
component lm32_top_full_debug is
generic ( eba_reset: std_logic_vector(31 downto 0) );
port (
clk_i : in std_logic;
rst_i : in std_logic;
interrupt : in std_logic_vector(31 downto 0);
I_DAT_I : in std_logic_vector(31 downto 0);
I_ACK_I : in std_logic;
I_ERR_I : in std_logic;
I_RTY_I : in std_logic;
D_DAT_I : in std_logic_vector(31 downto 0);
D_ACK_I : in std_logic;
D_ERR_I : in std_logic;
D_RTY_I : in std_logic;
I_DAT_O : out std_logic_vector(31 downto 0);
I_ADR_O : out std_logic_vector(31 downto 0);
I_CYC_O : out std_logic;
I_SEL_O : out std_logic_vector(3 downto 0);
I_STB_O : out std_logic;
I_WE_O : out std_logic;
I_CTI_O : out std_logic_vector(2 downto 0);
I_LOCK_O : out std_logic;
I_BTE_O : out std_logic_vector(1 downto 0);
D_DAT_O : out std_logic_vector(31 downto 0);
D_ADR_O : out std_logic_vector(31 downto 0);
D_CYC_O : out std_logic;
D_SEL_O : out std_logic_vector(3 downto 0);
D_STB_O : out std_logic;
D_WE_O : out std_logic;
D_CTI_O : out std_logic_vector(2 downto 0);
D_LOCK_O : out std_logic;
D_BTE_O : out std_logic_vector(1 downto 0));
end component;
function pick(first : boolean;
a, b : t_wishbone_address)
return t_wishbone_address is
begin
if first then
return a;
else
return b;
end if;
end pick;
function b2l(val : boolean)
return std_logic is
begin
if val then
return '1';
else
return '0';
end if;
end b2l;
function strip_undefined
(x : std_logic_vector) return std_logic_vector is
variable tmp : std_logic_vector(x'left downto 0);
begin
for i in 0 to x'left loop
if(x(i)='X' or x(i)='U' or x(i)='Z') then
tmp(i) := '0';
else
tmp(i) := x(i);
end if;
end loop; -- i
return tmp;
end strip_undefined;
constant dcache_burst_length : natural := f_eval_d_burst_length(g_profile);
constant icache_burst_length : natural := f_eval_i_burst_length(g_profile);
-- Control pins from the LM32
signal I_ADR : t_wishbone_address;
signal D_ADR : t_wishbone_address;
signal I_CYC : std_logic;
signal D_CYC : std_logic;
signal I_CTI : t_wishbone_cycle_type;
signal D_CTI : t_wishbone_cycle_type;
-- We also watch the STALL lines from the v4 slaves
-- Registered logic:
signal inst_was_busy : std_logic;
signal data_was_busy : std_logic;
signal inst_addr_reg : t_wishbone_address;
signal data_addr_reg : t_wishbone_address;
signal inst_remaining : natural range 0 to icache_burst_length;
signal data_remaining : natural range 0 to dcache_burst_length;
-- Asynchronous logic:
signal I_STB_O : std_logic;
signal D_STB_O : std_logic;
signal rst:std_logic;
begin
rst <= not rst_n_i;
gen_profile_minimal: if (g_profile = "minimal") generate
U_Wrapped_LM32: lm32_top_minimal
generic map (
eba_reset => g_reset_vector)
port map(
clk_i => clk_sys_i,
rst_i => rst,
interrupt => irq_i,
-- Pass slave responses through unmodified
I_DAT_I => strip_undefined(iwb_i.DAT),
I_ACK_I => iwb_i.ACK,
I_ERR_I => iwb_i.ERR,
I_RTY_I => iwb_i.RTY,
D_DAT_I => strip_undefined(dwb_i.DAT),
D_ACK_I => dwb_i.ACK,
D_ERR_I => dwb_i.ERR,
D_RTY_I => dwb_i.RTY,
-- Writes can only happen as a single cycle
I_DAT_O => iwb_o.DAT,
D_DAT_O => dwb_o.DAT,
I_WE_O => iwb_o.WE,
D_WE_O => dwb_o.WE,
-- SEL /= 1111 only for single cycles
I_SEL_O => iwb_o.SEL,
D_SEL_O => dwb_o.SEL,
-- We can ignore BTE as we know it's always linear burst mode
I_BTE_O => open,
D_BTE_O => open,
-- Lock is never flagged by LM32. Besides, WBv4 locks intercon on CYC.
I_LOCK_O => open,
D_LOCK_O => open,
-- The LM32 has STB=CYC always
I_STB_O => open,
D_STB_O => open,
-- We monitor these pins to direct the adapter's logic
I_ADR_O => I_ADR,
I_CYC_O => I_CYC,
I_CTI_O => I_CTI,
D_ADR_O => D_ADR,
D_CYC_O => D_CYC,
D_CTI_O => D_CTI);
end generate gen_profile_minimal;
gen_profile_medium: if (g_profile = "medium") generate
U_Wrapped_LM32: lm32_top_medium
generic map (
eba_reset => g_reset_vector)
port map(
clk_i => clk_sys_i,
rst_i => rst,
interrupt => irq_i,
-- Pass slave responses through unmodified
I_DAT_I => strip_undefined(iwb_i.DAT),
I_ACK_I => iwb_i.ACK,
I_ERR_I => iwb_i.ERR,
I_RTY_I => iwb_i.RTY,
D_DAT_I => strip_undefined(dwb_i.DAT),
D_ACK_I => dwb_i.ACK,
D_ERR_I => dwb_i.ERR,
D_RTY_I => dwb_i.RTY,
-- Writes can only happen as a single cycle
I_DAT_O => iwb_o.DAT,
D_DAT_O => dwb_o.DAT,
I_WE_O => iwb_o.WE,
D_WE_O => dwb_o.WE,
-- SEL /= 1111 only for single cycles
I_SEL_O => iwb_o.SEL,
D_SEL_O => dwb_o.SEL,
-- We can ignore BTE as we know it's always linear burst mode
I_BTE_O => open,
D_BTE_O => open,
-- Lock is never flagged by LM32. Besides, WBv4 locks intercon on CYC.
I_LOCK_O => open,
D_LOCK_O => open,
-- The LM32 has STB=CYC always
I_STB_O => open,
D_STB_O => open,
-- We monitor these pins to direct the adapter's logic
I_ADR_O => I_ADR,
I_CYC_O => I_CYC,
I_CTI_O => I_CTI,
D_ADR_O => D_ADR,
D_CYC_O => D_CYC,
D_CTI_O => D_CTI);
end generate gen_profile_medium;
gen_profile_medium_icache: if (g_profile = "medium_icache") generate
U_Wrapped_LM32: lm32_top_medium_icache
generic map (
eba_reset => g_reset_vector)
port map(
clk_i => clk_sys_i,
rst_i => rst,
interrupt => irq_i,
-- Pass slave responses through unmodified
I_DAT_I => strip_undefined(iwb_i.DAT),
I_ACK_I => iwb_i.ACK,
I_ERR_I => iwb_i.ERR,
I_RTY_I => iwb_i.RTY,
D_DAT_I => strip_undefined(dwb_i.DAT),
D_ACK_I => dwb_i.ACK,
D_ERR_I => dwb_i.ERR,
D_RTY_I => dwb_i.RTY,
-- Writes can only happen as a single cycle
I_DAT_O => iwb_o.DAT,
D_DAT_O => dwb_o.DAT,
I_WE_O => iwb_o.WE,
D_WE_O => dwb_o.WE,
-- SEL /= 1111 only for single cycles
I_SEL_O => iwb_o.SEL,
D_SEL_O => dwb_o.SEL,
-- We can ignore BTE as we know it's always linear burst mode
I_BTE_O => open,
D_BTE_O => open,
-- Lock is never flagged by LM32. Besides, WBv4 locks intercon on CYC.
I_LOCK_O => open,
D_LOCK_O => open,
-- The LM32 has STB=CYC always
I_STB_O => open,
D_STB_O => open,
-- We monitor these pins to direct the adapter's logic
I_ADR_O => I_ADR,
I_CYC_O => I_CYC,
I_CTI_O => I_CTI,
D_ADR_O => D_ADR,
D_CYC_O => D_CYC,
D_CTI_O => D_CTI);
end generate gen_profile_medium_icache;
gen_profile_medium_debug: if (g_profile = "medium_debug") generate
U_Wrapped_LM32: lm32_top_medium_debug
generic map (
eba_reset => g_reset_vector)
port map(
clk_i => clk_sys_i,
rst_i => rst,
interrupt => irq_i,
-- Pass slave responses through unmodified
I_DAT_I => strip_undefined(iwb_i.DAT),
I_ACK_I => iwb_i.ACK,
I_ERR_I => iwb_i.ERR,
I_RTY_I => iwb_i.RTY,
D_DAT_I => strip_undefined(dwb_i.DAT),
D_ACK_I => dwb_i.ACK,
D_ERR_I => dwb_i.ERR,
D_RTY_I => dwb_i.RTY,
-- Writes can only happen as a single cycle
I_DAT_O => iwb_o.DAT,
D_DAT_O => dwb_o.DAT,
I_WE_O => iwb_o.WE,
D_WE_O => dwb_o.WE,
-- SEL /= 1111 only for single cycles
I_SEL_O => iwb_o.SEL,
D_SEL_O => dwb_o.SEL,
-- We can ignore BTE as we know it's always linear burst mode
I_BTE_O => open,
D_BTE_O => open,
-- Lock is never flagged by LM32. Besides, WBv4 locks intercon on CYC.
I_LOCK_O => open,
D_LOCK_O => open,
-- The LM32 has STB=CYC always
I_STB_O => open,
D_STB_O => open,
-- We monitor these pins to direct the adapter's logic
I_ADR_O => I_ADR,
I_CYC_O => I_CYC,
I_CTI_O => I_CTI,
D_ADR_O => D_ADR,
D_CYC_O => D_CYC,
D_CTI_O => D_CTI);
end generate gen_profile_medium_debug;
gen_profile_medium_icache_debug: if (g_profile = "medium_icache_debug") generate
U_Wrapped_LM32: lm32_top_medium_icache_debug
generic map (
eba_reset => g_reset_vector)
port map(
clk_i => clk_sys_i,
rst_i => rst,
interrupt => irq_i,
-- Pass slave responses through unmodified
I_DAT_I => strip_undefined(iwb_i.DAT),
I_ACK_I => iwb_i.ACK,
I_ERR_I => iwb_i.ERR,
I_RTY_I => iwb_i.RTY,
D_DAT_I => strip_undefined(dwb_i.DAT),
D_ACK_I => dwb_i.ACK,
D_ERR_I => dwb_i.ERR,
D_RTY_I => dwb_i.RTY,
-- Writes can only happen as a single cycle
I_DAT_O => iwb_o.DAT,
D_DAT_O => dwb_o.DAT,
I_WE_O => iwb_o.WE,
D_WE_O => dwb_o.WE,
-- SEL /= 1111 only for single cycles
I_SEL_O => iwb_o.SEL,
D_SEL_O => dwb_o.SEL,
-- We can ignore BTE as we know it's always linear burst mode
I_BTE_O => open,
D_BTE_O => open,
-- Lock is never flagged by LM32. Besides, WBv4 locks intercon on CYC.
I_LOCK_O => open,
D_LOCK_O => open,
-- The LM32 has STB=CYC always
I_STB_O => open,
D_STB_O => open,
-- We monitor these pins to direct the adapter's logic
I_ADR_O => I_ADR,
I_CYC_O => I_CYC,
I_CTI_O => I_CTI,
D_ADR_O => D_ADR,
D_CYC_O => D_CYC,
D_CTI_O => D_CTI);
end generate gen_profile_medium_icache_debug;
gen_profile_full: if (g_profile = "full") generate
U_Wrapped_LM32: lm32_top_full
generic map (
eba_reset => g_reset_vector)
port map(
clk_i => clk_sys_i,
rst_i => rst,
interrupt => irq_i,
-- Pass slave responses through unmodified
I_DAT_I => strip_undefined(iwb_i.DAT),
I_ACK_I => iwb_i.ACK,
I_ERR_I => iwb_i.ERR,
I_RTY_I => iwb_i.RTY,
D_DAT_I => strip_undefined(dwb_i.DAT),
D_ACK_I => dwb_i.ACK,
D_ERR_I => dwb_i.ERR,
D_RTY_I => dwb_i.RTY,
-- Writes can only happen as a single cycle
I_DAT_O => iwb_o.DAT,
D_DAT_O => dwb_o.DAT,
I_WE_O => iwb_o.WE,
D_WE_O => dwb_o.WE,
-- SEL /= 1111 only for single cycles
I_SEL_O => iwb_o.SEL,
D_SEL_O => dwb_o.SEL,
-- We can ignore BTE as we know it's always linear burst mode
I_BTE_O => open,
D_BTE_O => open,
-- Lock is never flagged by LM32. Besides, WBv4 locks intercon on CYC.
I_LOCK_O => open,
D_LOCK_O => open,
-- The LM32 has STB=CYC always
I_STB_O => open,
D_STB_O => open,
-- We monitor these pins to direct the adapter's logic
I_ADR_O => I_ADR,
I_CYC_O => I_CYC,
I_CTI_O => I_CTI,
D_ADR_O => D_ADR,
D_CYC_O => D_CYC,
D_CTI_O => D_CTI);
end generate gen_profile_full;
gen_profile_full_debug: if (g_profile = "full_debug") generate
U_Wrapped_LM32: lm32_top_full_debug
generic map (
eba_reset => g_reset_vector)
port map(
clk_i => clk_sys_i,
rst_i => rst,
interrupt => irq_i,
-- Pass slave responses through unmodified
I_DAT_I => strip_undefined(iwb_i.DAT),
I_ACK_I => iwb_i.ACK,
I_ERR_I => iwb_i.ERR,
I_RTY_I => iwb_i.RTY,
D_DAT_I => strip_undefined(dwb_i.DAT),
D_ACK_I => dwb_i.ACK,
D_ERR_I => dwb_i.ERR,
D_RTY_I => dwb_i.RTY,
-- Writes can only happen as a single cycle
I_DAT_O => iwb_o.DAT,
D_DAT_O => dwb_o.DAT,
I_WE_O => iwb_o.WE,
D_WE_O => dwb_o.WE,
-- SEL /= 1111 only for single cycles
I_SEL_O => iwb_o.SEL,
D_SEL_O => dwb_o.SEL,
-- We can ignore BTE as we know it's always linear burst mode
I_BTE_O => open,
D_BTE_O => open,
-- Lock is never flagged by LM32. Besides, WBv4 locks intercon on CYC.
I_LOCK_O => open,
D_LOCK_O => open,
-- The LM32 has STB=CYC always
I_STB_O => open,
D_STB_O => open,
-- We monitor these pins to direct the adapter's logic
I_ADR_O => I_ADR,
I_CYC_O => I_CYC,
I_CTI_O => I_CTI,
D_ADR_O => D_ADR,
D_CYC_O => D_CYC,
D_CTI_O => D_CTI);
end generate gen_profile_full_debug;
-- Cycle durations always match in our adapter
iwb_o.CYC <= I_CYC;
dwb_o.CYC <= D_CYC;
iwb_o.STB <= I_STB_O;
dwb_o.STB <= D_STB_O;
I_STB_O <= (I_CYC and not inst_was_busy) or b2l(inst_remaining /= 0);
inst : process(clk_sys_i)
variable inst_addr : t_wishbone_address;
variable inst_length : natural;
begin
if rising_edge(clk_sys_i) then
if rst = '1' then
inst_was_busy <= '0';
inst_remaining <= 0;
inst_addr_reg <= (others => '0');
else
inst_was_busy <= I_CYC;
-- Is this the start of a new WB cycle?
if I_CYC = '1' and inst_was_busy = '0' then
inst_addr := I_ADR;
if I_CTI = "010" then
inst_length := icache_burst_length;
else
inst_length := 1;
end if;
else
inst_addr := inst_addr_reg;
inst_length := inst_remaining;
end if;
-- When stalled, we cannot advance the address
if iwb_i.STALL = '0' and I_STB_O = '1' then
inst_addr_reg <= std_logic_vector(unsigned(inst_addr) + 4);
inst_remaining <= inst_length - 1;
else
inst_addr_reg <= inst_addr;
inst_remaining <= inst_length;
end if;
end if;
end if;
end process;
D_STB_O <= (D_CYC and not data_was_busy) or b2l(data_remaining /= 0);
data : process(clk_sys_i)
variable data_addr : t_wishbone_address;
variable data_length : natural;
begin
if rising_edge(clk_sys_i) then
if rst = '1' then
data_was_busy <= '0';
data_remaining <= 0;
data_addr_reg <= (others => '0');
else
data_was_busy <= D_CYC;
-- Is this the start of a new WB cycle?
if D_CYC = '1' and data_was_busy = '0' then
data_addr := D_ADR;
if D_CTI = "010" then
data_length := dcache_burst_length;
else
data_length := 1;
end if;
else
data_addr := data_addr_reg;
data_length := data_remaining;
end if;
-- When stalled, we cannot advance the address
if dwb_i.STALL = '0' and D_STB_O = '1' then
data_addr_reg <= std_logic_vector(unsigned(data_addr) + 4);
data_remaining <= data_length - 1;
else
data_addr_reg <= data_addr;
data_remaining <= data_length;
end if;
end if;
end if;
end process;
-- The first request uses the WBv3 address, thereafter an incrementing one.
dwb_o.ADR <= pick(data_was_busy = '0', D_ADR, data_addr_reg);
iwb_o.ADR <= pick(inst_was_busy = '0', I_ADR, inst_addr_reg);
end rtl;
minimal CFG_INTERRUPTS_ENABLED
medium CFG_PL_MULTIPLY_ENABLED CFG_PL_BARREL_SHIFT_ENABLED CFG_SIGN_EXTEND_ENABLED CFG_INTERRUPTS_ENABLED
medium_icache CFG_PL_MULTIPLY_ENABLED CFG_PL_BARREL_SHIFT_ENABLED CFG_SIGN_EXTEND_ENABLED CFG_ICACHE_ENABLED CFG_INTERRUPTS_ENABLED
medium_debug CFG_PL_MULTIPLY_ENABLED CFG_PL_BARREL_SHIFT_ENABLED CFG_SIGN_EXTEND_ENABLED CFG_ICACHE_ENABLED CFG_WITH_DEBUG CFG_INTERRUPTS_ENABLED
medium_icache_debug CFG_PL_MULTIPLY_ENABLED CFG_PL_BARREL_SHIFT_ENABLED CFG_SIGN_EXTEND_ENABLED CFG_ICACHE_ENABLED CFG_WITH_DEBUG CFG_INTERRUPTS_ENABLED
full CFG_PL_MULTIPLY_ENABLED CFG_PL_BARREL_SHIFT_ENABLED CFG_SIGN_EXTEND_ENABLED CFG_INTERRUPTS_ENABLED CFG_MC_DIVIDE_ENABLED CFG_FAST_UNCONDITIONAL_BRANCH CFG_ICACHE_ENABLED CFG_DCACHE_ENABLED CFG_BUS_ERRORS_ENABLED
full_debug CFG_PL_MULTIPLY_ENABLED CFG_PL_BARREL_SHIFT_ENABLED CFG_SIGN_EXTEND_ENABLED CFG_INTERRUPTS_ENABLED CFG_MC_DIVIDE_ENABLED CFG_FAST_UNCONDITIONAL_BRANCH CFG_ICACHE_ENABLED CFG_DCACHE_ENABLED CFG_BUS_ERRORS_ENABLED CFG_WITH_DEBUG
\ No newline at end of file
module jtag_tap(
output tck,
output tdi,
input tdo,
output capture,
output shift,
output e1dr,
output update,
output reset
);
assign reset = 0;
wire nil1, nil2, nil3, nil4;
sld_virtual_jtag altera_jtag(
.ir_in (),
.ir_out (),
.tck (tck),
.tdo (tdo),
.tdi (tdi),
.virtual_state_cdr (capture), // capture DR -> load shift register
.virtual_state_sdr (shift), // shift -> shift register from TDI to TDO
.virtual_state_e1dr (e1dr), // done shift -> ignore (might shift more yet)
.virtual_state_pdr (nil1), // paused -> ignore
.virtual_state_e2dr (nil2), // done pause -> ignore
.virtual_state_udr (update), // update DR -> load state from shift
.virtual_state_cir (nil3), // capture IR -> write ir_out
.virtual_state_uir (nil4) // update IR -> read ir_in
// synopsys translate_off
,
.jtag_state_cdr (),
.jtag_state_cir (),
.jtag_state_e1dr (),
.jtag_state_e1ir (),
.jtag_state_e2dr (),
.jtag_state_e2ir (),
.jtag_state_pdr (),
.jtag_state_pir (),
.jtag_state_rti (),
.jtag_state_sdr (),
.jtag_state_sdrs (),
.jtag_state_sir (),
.jtag_state_sirs (),
.jtag_state_tlr (), // test logic reset
.jtag_state_udr (),
.jtag_state_uir (),
.tms ()
// synopsys translate_on
);
defparam
altera_jtag.sld_auto_instance_index = "YES",
altera_jtag.sld_instance_index = 0,
altera_jtag.sld_ir_width = 1,
altera_jtag.sld_sim_action = "",
altera_jtag.sld_sim_n_scan = 0,
altera_jtag.sld_sim_total_length = 0;
endmodule
module jtag_tap(
output tck,
output tdi,
input tdo,
output capture,
output shift,
output e1dr,
output update,
output reset
);
assign tck = 1;
assign tdi = 1;
assign capture = 0;
assign shift = 0;
assign e1dr = 0;
assign update = 0;
assign reset = 0;
endmodule
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_multiplier.v
// Title : Pipelined multiplier.
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : No Change
// =============================================================================
`include "../../src/lm32_include.v"
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_multiplier (
// ----- Inputs -----
clk_i,
rst_i,
stall_x,
stall_m,
operand_0,
operand_1,
// ----- Ouputs -----
result
);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
input rst_i; // Reset
input stall_x; // Stall instruction in X stage
input stall_m; // Stall instruction in M stage
input [`LM32_WORD_RNG] operand_0; // Muliplicand
input [`LM32_WORD_RNG] operand_1; // Multiplier
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
output [`LM32_WORD_RNG] result; // Product of multiplication
wire [`LM32_WORD_RNG] result;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
// Divide multiplicands into high and low
`define HALF_WORD_WIDTH (`LM32_WORD_WIDTH/2)
`define HALF_WORD_RNG (`HALF_WORD_WIDTH-1):0
// Result = c+d+e = a*b
reg [`HALF_WORD_RNG] a0, a1, b0, b1;
reg [`HALF_WORD_RNG] c0, c1;
reg [`HALF_WORD_RNG] d1, e1;
reg [`HALF_WORD_RNG] result0, result1;
assign result = {result1, result0};
/////////////////////////////////////////////////////
// Sequential logic
/////////////////////////////////////////////////////
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
a0 <= {`HALF_WORD_WIDTH{1'b0}};
a1 <= {`HALF_WORD_WIDTH{1'b0}};
b0 <= {`HALF_WORD_WIDTH{1'b0}};
b1 <= {`HALF_WORD_WIDTH{1'b0}};
c0 <= {`HALF_WORD_WIDTH{1'b0}};
c1 <= {`HALF_WORD_WIDTH{1'b0}};
d1 <= {`HALF_WORD_WIDTH{1'b0}};
e1 <= {`HALF_WORD_WIDTH{1'b0}};
result0 <= {`HALF_WORD_WIDTH{1'b0}};
result1 <= {`HALF_WORD_WIDTH{1'b0}};
end
else
begin
if (stall_x == `FALSE)
begin
{a1, a0} <= operand_0;
{b1, b0} <= operand_1;
end
if (stall_m == `FALSE)
begin
{c1, c0} <= a0 * b0;
d1 <= a0 * b1;
e1 <= a1 * b0;
end
result0 <= c0;
result1 <= c1 + d1 + e1;
end
end
endmodule
module jtag_tap(
output tck,
output tdi,
input tdo,
output capture,
output shift,
output e1dr,
output update,
output reset
);
// Unfortunately the exit1 state for DR (e1dr) is mising
// We can simulate it by interpretting 'update' as e1dr and delaying 'update'
wire sel;
wire g_capture;
wire g_shift;
wire g_update;
reg update_delay;
assign capture = g_capture & sel;
assign shift = g_shift & sel;
assign e1dr = g_update & sel;
assign update = update_delay;
BSCAN_SPARTAN6 #(
.JTAG_CHAIN(1)
) bscan (
.CAPTURE(g_capture),
.DRCK(tck),
.RESET(reset),
.RUNTEST(),
.SEL(sel),
.SHIFT(g_shift),
.TCK(),
.TDI(tdi),
.TMS(),
.UPDATE(g_update),
.TDO(tdo)
);
always@(posedge tck)
update_delay <= g_update;
endmodule
/*
* Milkymist VJ SoC
* Copyright (C) 2007, 2008, 2009, 2010 Sebastien Bourdeauducq
*
* This program 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, version 3 of the License.
*
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
*/
module lm32_multiplier(
input clk_i,
input rst_i,
input stall_x,
input stall_m,
input [31:0] operand_0,
input [31:0] operand_1,
output [31:0] result
);
// See UG389, esp. p. 29 "Fully Pipelined, 35 x 35 Multiplier Use Model (Large Multiplier)"
wire [17:0] au = {3'd0, operand_0[31:17]};
wire [17:0] al = {1'b0, operand_0[16:0]};
wire [17:0] bu = {3'd0, operand_1[31:17]};
wire [17:0] bl = {1'b0, operand_1[16:0]};
wire [17:0] bl_forward;
wire [35:0] al_bl;
reg [16:0] result_low;
always @(posedge clk_i) begin
if(rst_i)
result_low <= 17'd0;
else
result_low <= al_bl[16:0];
end
assign result[16:0] = result_low;
DSP48A1 #(
.A0REG(1),
.A1REG(0),
.B0REG(1),
.B1REG(0),
.CARRYINREG(0),
.CARRYINSEL("OPMODE5"),
.CARRYOUTREG(0),
.CREG(0),
.DREG(0),
.MREG(1),
.OPMODEREG(0),
.PREG(0),
.RSTTYPE("SYNC")
) D1 (
.BCOUT(bl_forward),
.PCOUT(),
.CARRYOUT(),
.CARRYOUTF(),
.M(al_bl),
.P(),
.PCIN(),
.CLK(clk_i),
.OPMODE(8'd1),
.A(al),
.B(bl),
.C(48'h0),
.CARRYIN(),
.D(18'b0),
.CEA(~stall_x),
.CEB(~stall_x),
.CEC(1'b0),
.CECARRYIN(1'b0),
.CED(1'b0),
.CEM(~stall_m),
.CEOPMODE(1'b0),
.CEP(1'b1),
.RSTA(rst_i),
.RSTB(rst_i),
.RSTC(1'b0),
.RSTCARRYIN(1'b0),
.RSTD(1'b0),
.RSTM(rst_i),
.RSTOPMODE(1'b0),
.RSTP(1'b0)
);
wire [47:0] au_bl_sum;
DSP48A1 #(
.A0REG(1),
.A1REG(0),
.B0REG(0),
.B1REG(0),
.CARRYINREG(0),
.CARRYINSEL("OPMODE5"),
.CARRYOUTREG(0),
.CREG(0),
.DREG(0),
.MREG(1),
.OPMODEREG(0),
.PREG(0),
.RSTTYPE("SYNC")
) D2 (
.BCOUT(),
.PCOUT(au_bl_sum),
.CARRYOUT(),
.CARRYOUTF(),
.M(),
.P(),
.PCIN(),
.CLK(clk_i),
.OPMODE(8'd13),
.A(au),
.B(bl_forward),
.C({31'd0, al_bl[33:17]}),
.CARRYIN(),
.D(18'h0),
.CEA(~stall_x),
.CEB(1'b0),
.CEC(1'b0),
.CECARRYIN(1'b0),
.CED(1'b0),
.CEM(~stall_m),
.CEOPMODE(1'b0),
.CEP(1'b0),
.RSTA(rst_i),
.RSTB(1'b0),
.RSTC(1'b0),
.RSTCARRYIN(1'b0),
.RSTD(1'b0),
.RSTM(rst_i),
.RSTOPMODE(1'b0),
.RSTP(1'b0)
);
wire [47:0] r_full;
assign result[31:17] = r_full[16:0];
DSP48A1 #(
.A0REG(1),
.A1REG(0),
.B0REG(1),
.B1REG(0),
.CARRYINREG(0),
.CARRYINSEL("OPMODE5"),
.CARRYOUTREG(0),
.CREG(0),
.DREG(0),
.MREG(1),
.OPMODEREG(0),
.PREG(1),
.RSTTYPE("SYNC")
) D3 (
.BCOUT(),
.PCOUT(),
.CARRYOUT(),
.CARRYOUTF(),
.M(),
.P(r_full),
.PCIN(au_bl_sum),
.CLK(clk_i),
.OPMODE(8'd5),
.A(bu),
.B(al),
.C(48'b0),
.CARRYIN(),
.D(18'b0),
.CEA(~stall_x),
.CEB(~stall_x),
.CEC(1'b0),
.CECARRYIN(1'b0),
.CED(1'b0),
.CEM(~stall_m),
.CEOPMODE(1'b0),
.CEP(1'b1),
.RSTA(rst_i),
.RSTB(rst_i),
.RSTC(1'b0),
.RSTCARRYIN(1'b0),
.RSTD(1'b0),
.RSTM(rst_i),
.RSTOPMODE(1'b0),
.RSTP(rst_i)
);
endmodule
// Modified by GSI to use simple positive edge clocking and the JTAG capture state
module jtag_cores (
input [7:0] reg_d,
input [2:0] reg_addr_d,
output reg_update,
output [7:0] reg_q,
output [2:0] reg_addr_q,
output jtck,
output jrstn
);
wire tck;
wire tdi;
wire tdo;
wire capture;
wire shift;
wire update;
wire e1dr;
wire reset;
jtag_tap jtag_tap (
.tck(tck),
.tdi(tdi),
.tdo(tdo),
.capture(capture),
.shift(shift),
.e1dr(e1dr),
.update(update),
.reset(reset)
);
reg [10:0] jtag_shift;
reg [10:0] jtag_latched;
always @(posedge tck)
begin
if(reset)
jtag_shift <= 11'b0;
else begin
if (shift)
jtag_shift <= {tdi, jtag_shift[10:1]};
else if (capture)
jtag_shift <= {reg_d, reg_addr_d};
end
end
assign tdo = jtag_shift[0];
always @(posedge tck)
begin
if(reset)
jtag_latched <= 11'b0;
else begin
if (e1dr)
jtag_latched <= jtag_shift;
end
end
assign reg_update = update;
assign reg_q = jtag_latched[10:3];
assign reg_addr_q = jtag_latched[2:0];
assign jtck = tck;
assign jrstn = ~reset;
endmodule
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// ============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_adder.v
// Title : Integer adder / subtractor with comparison flag generation
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : No Change
// =============================================================================
`include "lm32_include.v"
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_adder (
// ----- Inputs -------
adder_op_x,
adder_op_x_n,
operand_0_x,
operand_1_x,
// ----- Outputs -------
adder_result_x,
adder_carry_n_x,
adder_overflow_x
);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input adder_op_x; // Operating to perform, 0 for addition, 1 for subtraction
input adder_op_x_n; // Inverted version of adder_op_x
input [`LM32_WORD_RNG] operand_0_x; // Operand to add, or subtract from
input [`LM32_WORD_RNG] operand_1_x; // Opearnd to add, or subtract by
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
output [`LM32_WORD_RNG] adder_result_x; // Result of addition or subtraction
wire [`LM32_WORD_RNG] adder_result_x;
output adder_carry_n_x; // Inverted carry
wire adder_carry_n_x;
output adder_overflow_x; // Indicates if overflow occured, only valid for subtractions
reg adder_overflow_x;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
wire a_sign; // Sign (i.e. positive or negative) of operand 0
wire b_sign; // Sign of operand 1
wire result_sign; // Sign of result
/////////////////////////////////////////////////////
// Instantiations
/////////////////////////////////////////////////////
lm32_addsub addsub (
// ----- Inputs -----
.DataA (operand_0_x),
.DataB (operand_1_x),
.Cin (adder_op_x),
.Add_Sub (adder_op_x_n),
// ----- Ouputs -----
.Result (adder_result_x),
.Cout (adder_carry_n_x)
);
/////////////////////////////////////////////////////
// Combinational Logic
/////////////////////////////////////////////////////
// Extract signs of operands and result
assign a_sign = operand_0_x[`LM32_WORD_WIDTH-1];
assign b_sign = operand_1_x[`LM32_WORD_WIDTH-1];
assign result_sign = adder_result_x[`LM32_WORD_WIDTH-1];
// Determine whether an overflow occured when performing a subtraction
always @(*)
begin
// +ve - -ve = -ve -> overflow
// -ve - +ve = +ve -> overflow
if ( (!a_sign & b_sign & result_sign)
|| (a_sign & !b_sign & !result_sign)
)
adder_overflow_x = `TRUE;
else
adder_overflow_x = `FALSE;
end
endmodule
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_addsub.v
// Title : PMI adder/subtractor.
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : No Change
// =============================================================================
`include "lm32_include.v"
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_addsub (
// ----- Inputs -------
DataA,
DataB,
Cin,
Add_Sub,
// ----- Outputs -------
Result,
Cout
);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input [31:0] DataA;
input [31:0] DataB;
input Cin;
input Add_Sub;
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
output [31:0] Result;
wire [31:0] Result;
output Cout;
wire Cout;
/////////////////////////////////////////////////////
// Instantiations
/////////////////////////////////////////////////////
// Modified for Milkymist: removed non-portable instantiated block
wire [32:0] tmp_addResult = DataA + DataB + Cin;
wire [32:0] tmp_subResult = DataA - DataB - !Cin;
assign Result = (Add_Sub == 1) ? tmp_addResult[31:0] : tmp_subResult[31:0];
assign Cout = (Add_Sub == 1) ? tmp_addResult[32] : !tmp_subResult[32];
endmodule
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_cpu.v
// Title : Top-level of CPU.
// Dependencies : lm32_include.v
//
// Version 3.4
// 1. Bug Fix: In a tight infinite loop (add, sw, bi) incoming interrupts were
// never serviced.
//
// Version 3.3
// 1. Feature: Support for memory that is tightly coupled to processor core, and
// has a single-cycle access latency (same as caches). Instruction port has
// access to a dedicated physically-mapped memory. Data port has access to
// a dedicated physically-mapped memory. In order to be able to manipulate
// values in both these memories via the debugger, these memories also
// interface with the data port of LM32.
// 2. Feature: Extended Configuration Register
// 3. Bug Fix: Removed port names that conflict with keywords reserved in System-
// Verilog.
//
// Version 3.2
// 1. Bug Fix: Single-stepping a load/store to invalid address causes debugger to
// hang. At the same time CPU fails to register data bus error exception. Bug
// is caused because (a) data bus error exception occurs after load/store has
// passed X stage and next sequential instruction (e.g., brk) is already in X
// stage, and (b) data bus error exception had lower priority than, say, brk
// exception.
// 2. Bug Fix: If a brk (or scall/eret/bret) sequentially follows a load/store to
// invalid location, CPU will fail to register data bus error exception. The
// solution is to stall scall/eret/bret/brk instructions in D pipeline stage
// until load/store has completed.
// 3. Feature: Enable precise identification of load/store that causes seg fault.
// 4. SYNC resets used for register file when implemented in EBRs.
//
// Version 3.1
// 1. Feature: LM32 Register File can now be mapped in to on-chip block RAM (EBR)
// instead of distributed memory by enabling the option in LM32 GUI.
// 2. Feature: LM32 also adds a static branch predictor to improve branch
// performance. All immediate-based forward-pointing branches are predicted
// not-taken. All immediate-based backward-pointing branches are predicted taken.
//
// Version 7.0SP2, 3.0
// No Change
//
// Version 6.1.17
// Initial Release
// =============================================================================
`include "lm32_include.v"
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_cpu (
// ----- Inputs -------
clk_i,
`ifdef CFG_EBR_NEGEDGE_REGISTER_FILE
clk_n_i,
`endif
rst_i,
// From external devices
`ifdef CFG_INTERRUPTS_ENABLED
interrupt,
`endif
// From user logic
`ifdef CFG_USER_ENABLED
user_result,
user_complete,
`endif
`ifdef CFG_JTAG_ENABLED
// From JTAG
jtag_clk,
jtag_update,
jtag_reg_q,
jtag_reg_addr_q,
`endif
`ifdef CFG_IWB_ENABLED
// Instruction Wishbone master
I_DAT_I,
I_ACK_I,
I_ERR_I,
I_RTY_I,
`endif
// Data Wishbone master
D_DAT_I,
D_ACK_I,
D_ERR_I,
D_RTY_I,
// ----- Outputs -------
`ifdef CFG_TRACE_ENABLED
trace_pc,
trace_pc_valid,
trace_exception,
trace_eid,
trace_eret,
`ifdef CFG_DEBUG_ENABLED
trace_bret,
`endif
`endif
`ifdef CFG_JTAG_ENABLED
jtag_reg_d,
jtag_reg_addr_d,
`endif
`ifdef CFG_USER_ENABLED
user_valid,
user_opcode,
user_operand_0,
user_operand_1,
`endif
`ifdef CFG_IWB_ENABLED
// Instruction Wishbone master
I_DAT_O,
I_ADR_O,
I_CYC_O,
I_SEL_O,
I_STB_O,
I_WE_O,
I_CTI_O,
I_LOCK_O,
I_BTE_O,
`endif
// Data Wishbone master
D_DAT_O,
D_ADR_O,
D_CYC_O,
D_SEL_O,
D_STB_O,
D_WE_O,
D_CTI_O,
D_LOCK_O,
D_BTE_O
);
/////////////////////////////////////////////////////
// Parameters
/////////////////////////////////////////////////////
parameter eba_reset = `CFG_EBA_RESET; // Reset value for EBA CSR
`ifdef CFG_DEBUG_ENABLED
parameter deba_reset = `CFG_DEBA_RESET; // Reset value for DEBA CSR
`endif
`ifdef CFG_ICACHE_ENABLED
parameter icache_associativity = `CFG_ICACHE_ASSOCIATIVITY; // Associativity of the cache (Number of ways)
parameter icache_sets = `CFG_ICACHE_SETS; // Number of sets
parameter icache_bytes_per_line = `CFG_ICACHE_BYTES_PER_LINE; // Number of bytes per cache line
parameter icache_base_address = `CFG_ICACHE_BASE_ADDRESS; // Base address of cachable memory
parameter icache_limit = `CFG_ICACHE_LIMIT; // Limit (highest address) of cachable memory
`else
parameter icache_associativity = 1;
parameter icache_sets = 512;
parameter icache_bytes_per_line = 16;
parameter icache_base_address = 0;
parameter icache_limit = 0;
`endif
`ifdef CFG_DCACHE_ENABLED
parameter dcache_associativity = `CFG_DCACHE_ASSOCIATIVITY; // Associativity of the cache (Number of ways)
parameter dcache_sets = `CFG_DCACHE_SETS; // Number of sets
parameter dcache_bytes_per_line = `CFG_DCACHE_BYTES_PER_LINE; // Number of bytes per cache line
parameter dcache_base_address = `CFG_DCACHE_BASE_ADDRESS; // Base address of cachable memory
parameter dcache_limit = `CFG_DCACHE_LIMIT; // Limit (highest address) of cachable memory
`else
parameter dcache_associativity = 1;
parameter dcache_sets = 512;
parameter dcache_bytes_per_line = 16;
parameter dcache_base_address = 0;
parameter dcache_limit = 0;
`endif
`ifdef CFG_DEBUG_ENABLED
parameter watchpoints = `CFG_WATCHPOINTS; // Number of h/w watchpoint CSRs
`else
parameter watchpoints = 0;
`endif
`ifdef CFG_ROM_DEBUG_ENABLED
parameter breakpoints = `CFG_BREAKPOINTS; // Number of h/w breakpoint CSRs
`else
parameter breakpoints = 0;
`endif
`ifdef CFG_INTERRUPTS_ENABLED
parameter interrupts = `CFG_INTERRUPTS; // Number of interrupts
`else
parameter interrupts = 0;
`endif
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
`ifdef CFG_EBR_NEGEDGE_REGISTER_FILE
input clk_n_i; // Inverted clock
`endif
input rst_i; // Reset
`ifdef CFG_INTERRUPTS_ENABLED
input [`LM32_INTERRUPT_RNG] interrupt; // Interrupt pins
`endif
`ifdef CFG_USER_ENABLED
input [`LM32_WORD_RNG] user_result; // User-defined instruction result
input user_complete; // User-defined instruction execution is complete
`endif
`ifdef CFG_JTAG_ENABLED
input jtag_clk; // JTAG clock
input jtag_update; // JTAG state machine is in data register update state
input [`LM32_BYTE_RNG] jtag_reg_q;
input [2:0] jtag_reg_addr_q;
`endif
`ifdef CFG_IWB_ENABLED
input [`LM32_WORD_RNG] I_DAT_I; // Instruction Wishbone interface read data
input I_ACK_I; // Instruction Wishbone interface acknowledgement
input I_ERR_I; // Instruction Wishbone interface error
input I_RTY_I; // Instruction Wishbone interface retry
`endif
input [`LM32_WORD_RNG] D_DAT_I; // Data Wishbone interface read data
input D_ACK_I; // Data Wishbone interface acknowledgement
input D_ERR_I; // Data Wishbone interface error
input D_RTY_I; // Data Wishbone interface retry
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
`ifdef CFG_TRACE_ENABLED
output [`LM32_PC_RNG] trace_pc; // PC to trace
reg [`LM32_PC_RNG] trace_pc;
output trace_pc_valid; // Indicates that a new trace PC is valid
reg trace_pc_valid;
output trace_exception; // Indicates an exception has occured
reg trace_exception;
output [`LM32_EID_RNG] trace_eid; // Indicates what type of exception has occured
reg [`LM32_EID_RNG] trace_eid;
output trace_eret; // Indicates an eret instruction has been executed
reg trace_eret;
`ifdef CFG_DEBUG_ENABLED
output trace_bret; // Indicates a bret instruction has been executed
reg trace_bret;
`endif
`endif
`ifdef CFG_JTAG_ENABLED
output [`LM32_BYTE_RNG] jtag_reg_d;
wire [`LM32_BYTE_RNG] jtag_reg_d;
output [2:0] jtag_reg_addr_d;
wire [2:0] jtag_reg_addr_d;
`endif
`ifdef CFG_USER_ENABLED
output user_valid; // Indicates if user_opcode is valid
wire user_valid;
output [`LM32_USER_OPCODE_RNG] user_opcode; // User-defined instruction opcode
reg [`LM32_USER_OPCODE_RNG] user_opcode;
output [`LM32_WORD_RNG] user_operand_0; // First operand for user-defined instruction
wire [`LM32_WORD_RNG] user_operand_0;
output [`LM32_WORD_RNG] user_operand_1; // Second operand for user-defined instruction
wire [`LM32_WORD_RNG] user_operand_1;
`endif
`ifdef CFG_IWB_ENABLED
output [`LM32_WORD_RNG] I_DAT_O; // Instruction Wishbone interface write data
wire [`LM32_WORD_RNG] I_DAT_O;
output [`LM32_WORD_RNG] I_ADR_O; // Instruction Wishbone interface address
wire [`LM32_WORD_RNG] I_ADR_O;
output I_CYC_O; // Instruction Wishbone interface cycle
wire I_CYC_O;
output [`LM32_BYTE_SELECT_RNG] I_SEL_O; // Instruction Wishbone interface byte select
wire [`LM32_BYTE_SELECT_RNG] I_SEL_O;
output I_STB_O; // Instruction Wishbone interface strobe
wire I_STB_O;
output I_WE_O; // Instruction Wishbone interface write enable
wire I_WE_O;
output [`LM32_CTYPE_RNG] I_CTI_O; // Instruction Wishbone interface cycle type
wire [`LM32_CTYPE_RNG] I_CTI_O;
output I_LOCK_O; // Instruction Wishbone interface lock bus
wire I_LOCK_O;
output [`LM32_BTYPE_RNG] I_BTE_O; // Instruction Wishbone interface burst type
wire [`LM32_BTYPE_RNG] I_BTE_O;
`endif
output [`LM32_WORD_RNG] D_DAT_O; // Data Wishbone interface write data
wire [`LM32_WORD_RNG] D_DAT_O;
output [`LM32_WORD_RNG] D_ADR_O; // Data Wishbone interface address
wire [`LM32_WORD_RNG] D_ADR_O;
output D_CYC_O; // Data Wishbone interface cycle
wire D_CYC_O;
output [`LM32_BYTE_SELECT_RNG] D_SEL_O; // Data Wishbone interface byte select
wire [`LM32_BYTE_SELECT_RNG] D_SEL_O;
output D_STB_O; // Data Wishbone interface strobe
wire D_STB_O;
output D_WE_O; // Data Wishbone interface write enable
wire D_WE_O;
output [`LM32_CTYPE_RNG] D_CTI_O; // Data Wishbone interface cycle type
wire [`LM32_CTYPE_RNG] D_CTI_O;
output D_LOCK_O; // Date Wishbone interface lock bus
wire D_LOCK_O;
output [`LM32_BTYPE_RNG] D_BTE_O; // Data Wishbone interface burst type
wire [`LM32_BTYPE_RNG] D_BTE_O;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
// Pipeline registers
`ifdef LM32_CACHE_ENABLED
reg valid_a; // Instruction in A stage is valid
`endif
reg valid_f; // Instruction in F stage is valid
reg valid_d; // Instruction in D stage is valid
reg valid_x; // Instruction in X stage is valid
reg valid_m; // Instruction in M stage is valid
reg valid_w; // Instruction in W stage is valid
wire q_x;
wire [`LM32_WORD_RNG] immediate_d; // Immediate operand
wire load_d; // Indicates a load instruction
reg load_x;
reg load_m;
wire load_q_x;
wire store_q_x;
wire q_m;
wire load_q_m;
wire store_q_m;
wire store_d; // Indicates a store instruction
reg store_x;
reg store_m;
wire [`LM32_SIZE_RNG] size_d; // Size of load/store (byte, hword, word)
reg [`LM32_SIZE_RNG] size_x;
wire branch_d; // Indicates a branch instruction
wire branch_predict_d; // Indicates a branch is predicted
wire branch_predict_taken_d; // Indicates a branch is predicted taken
wire [`LM32_PC_RNG] branch_predict_address_d; // Address to which predicted branch jumps
wire [`LM32_PC_RNG] branch_target_d;
wire bi_unconditional;
wire bi_conditional;
reg branch_x;
reg branch_predict_x;
reg branch_predict_taken_x;
reg branch_m;
reg branch_predict_m;
reg branch_predict_taken_m;
wire branch_mispredict_taken_m; // Indicates a branch was mispredicted as taken
wire branch_flushX_m; // Indicates that instruction in X stage must be squashed
wire branch_reg_d; // Branch to register or immediate
wire [`LM32_PC_RNG] branch_offset_d; // Branch offset for immediate branches
reg [`LM32_PC_RNG] branch_target_x; // Address to branch to
reg [`LM32_PC_RNG] branch_target_m;
wire [`LM32_D_RESULT_SEL_0_RNG] d_result_sel_0_d; // Which result should be selected in D stage for operand 0
wire [`LM32_D_RESULT_SEL_1_RNG] d_result_sel_1_d; // Which result should be selected in D stage for operand 1
wire x_result_sel_csr_d; // Select X stage result from CSRs
reg x_result_sel_csr_x;
`ifdef LM32_MC_ARITHMETIC_ENABLED
wire q_d;
wire x_result_sel_mc_arith_d; // Select X stage result from multi-cycle arithmetic unit
reg x_result_sel_mc_arith_x;
`endif
`ifdef LM32_NO_BARREL_SHIFT
wire x_result_sel_shift_d; // Select X stage result from shifter
reg x_result_sel_shift_x;
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
wire x_result_sel_sext_d; // Select X stage result from sign-extend logic
reg x_result_sel_sext_x;
`endif
wire x_result_sel_logic_d; // Select X stage result from logic op unit
`ifdef CFG_USER_ENABLED
wire x_result_sel_user_d; // Select X stage result from user-defined logic
reg x_result_sel_user_x;
`endif
wire x_result_sel_add_d; // Select X stage result from adder
reg x_result_sel_add_x;
wire m_result_sel_compare_d; // Select M stage result from comparison logic
reg m_result_sel_compare_x;
reg m_result_sel_compare_m;
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
wire m_result_sel_shift_d; // Select M stage result from shifter
reg m_result_sel_shift_x;
reg m_result_sel_shift_m;
`endif
wire w_result_sel_load_d; // Select W stage result from load/store unit
reg w_result_sel_load_x;
reg w_result_sel_load_m;
reg w_result_sel_load_w;
`ifdef CFG_PL_MULTIPLY_ENABLED
wire w_result_sel_mul_d; // Select W stage result from multiplier
reg w_result_sel_mul_x;
reg w_result_sel_mul_m;
reg w_result_sel_mul_w;
`endif
wire x_bypass_enable_d; // Whether result is bypassable in X stage
reg x_bypass_enable_x;
wire m_bypass_enable_d; // Whether result is bypassable in M stage
reg m_bypass_enable_x;
reg m_bypass_enable_m;
wire sign_extend_d; // Whether to sign-extend or zero-extend
reg sign_extend_x;
wire write_enable_d; // Register file write enable
reg write_enable_x;
wire write_enable_q_x;
reg write_enable_m;
wire write_enable_q_m;
reg write_enable_w;
wire write_enable_q_w;
wire read_enable_0_d; // Register file read enable 0
wire [`LM32_REG_IDX_RNG] read_idx_0_d; // Register file read index 0
wire read_enable_1_d; // Register file read enable 1
wire [`LM32_REG_IDX_RNG] read_idx_1_d; // Register file read index 1
wire [`LM32_REG_IDX_RNG] write_idx_d; // Register file write index
reg [`LM32_REG_IDX_RNG] write_idx_x;
reg [`LM32_REG_IDX_RNG] write_idx_m;
reg [`LM32_REG_IDX_RNG] write_idx_w;
wire [`LM32_CSR_RNG] csr_d; // CSR read/write index
reg [`LM32_CSR_RNG] csr_x;
wire [`LM32_CONDITION_RNG] condition_d; // Branch condition
reg [`LM32_CONDITION_RNG] condition_x;
`ifdef CFG_DEBUG_ENABLED
wire break_d; // Indicates a break instruction
reg break_x;
`endif
wire scall_d; // Indicates a scall instruction
reg scall_x;
wire eret_d; // Indicates an eret instruction
reg eret_x;
wire eret_q_x;
`ifdef CFG_TRACE_ENABLED
reg eret_m;
reg eret_w;
`endif
`ifdef CFG_DEBUG_ENABLED
wire bret_d; // Indicates a bret instruction
reg bret_x;
wire bret_q_x;
`ifdef CFG_TRACE_ENABLED
reg bret_m;
reg bret_w;
`endif
`endif
wire csr_write_enable_d; // CSR write enable
reg csr_write_enable_x;
wire csr_write_enable_q_x;
`ifdef CFG_USER_ENABLED
wire [`LM32_USER_OPCODE_RNG] user_opcode_d; // User-defined instruction opcode
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
wire bus_error_d; // Indicates an bus error occured while fetching the instruction in this pipeline stage
reg bus_error_x;
reg data_bus_error_exception_m;
reg [`LM32_PC_RNG] memop_pc_w;
`endif
reg [`LM32_WORD_RNG] d_result_0; // Result of instruction in D stage (operand 0)
reg [`LM32_WORD_RNG] d_result_1; // Result of instruction in D stage (operand 1)
reg [`LM32_WORD_RNG] x_result; // Result of instruction in X stage
reg [`LM32_WORD_RNG] m_result; // Result of instruction in M stage
reg [`LM32_WORD_RNG] w_result; // Result of instruction in W stage
reg [`LM32_WORD_RNG] operand_0_x; // Operand 0 for X stage instruction
reg [`LM32_WORD_RNG] operand_1_x; // Operand 1 for X stage instruction
reg [`LM32_WORD_RNG] store_operand_x; // Data read from register to store
reg [`LM32_WORD_RNG] operand_m; // Operand for M stage instruction
reg [`LM32_WORD_RNG] operand_w; // Operand for W stage instruction
// To/from register file
`ifdef CFG_EBR_POSEDGE_REGISTER_FILE
reg [`LM32_WORD_RNG] reg_data_live_0;
reg [`LM32_WORD_RNG] reg_data_live_1;
reg use_buf; // Whether to use reg_data_live or reg_data_buf
reg [`LM32_WORD_RNG] reg_data_buf_0;
reg [`LM32_WORD_RNG] reg_data_buf_1;
`endif
`ifdef LM32_EBR_REGISTER_FILE
`else
reg [`LM32_WORD_RNG] registers[0:(1<<`LM32_REG_IDX_WIDTH)-1]; // Register file
`endif
wire [`LM32_WORD_RNG] reg_data_0; // Register file read port 0 data
wire [`LM32_WORD_RNG] reg_data_1; // Register file read port 1 data
reg [`LM32_WORD_RNG] bypass_data_0; // Register value 0 after bypassing
reg [`LM32_WORD_RNG] bypass_data_1; // Register value 1 after bypassing
wire reg_write_enable_q_w;
reg interlock; // Indicates pipeline should be stalled because of a read-after-write hazzard
wire stall_a; // Stall instruction in A pipeline stage
wire stall_f; // Stall instruction in F pipeline stage
wire stall_d; // Stall instruction in D pipeline stage
wire stall_x; // Stall instruction in X pipeline stage
wire stall_m; // Stall instruction in M pipeline stage
// To/from adder
wire adder_op_d; // Whether to add or subtract
reg adder_op_x;
reg adder_op_x_n; // Inverted version of adder_op_x
wire [`LM32_WORD_RNG] adder_result_x; // Result from adder
wire adder_overflow_x; // Whether a signed overflow occured
wire adder_carry_n_x; // Whether a carry was generated
// To/from logical operations unit
wire [`LM32_LOGIC_OP_RNG] logic_op_d; // Which operation to perform
reg [`LM32_LOGIC_OP_RNG] logic_op_x;
wire [`LM32_WORD_RNG] logic_result_x; // Result of logical operation
`ifdef CFG_SIGN_EXTEND_ENABLED
// From sign-extension unit
wire [`LM32_WORD_RNG] sextb_result_x; // Result of byte sign-extension
wire [`LM32_WORD_RNG] sexth_result_x; // Result of half-word sign-extenstion
wire [`LM32_WORD_RNG] sext_result_x; // Result of sign-extension specified by instruction
`endif
// To/from shifter
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
`ifdef CFG_ROTATE_ENABLED
wire rotate_d; // Whether we should rotate or shift
reg rotate_x;
`endif
wire direction_d; // Which direction to shift in
reg direction_x;
wire [`LM32_WORD_RNG] shifter_result_m; // Result of shifter
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
wire shift_left_d; // Indicates whether to perform a left shift or not
wire shift_left_q_d;
wire shift_right_d; // Indicates whether to perform a right shift or not
wire shift_right_q_d;
`endif
`ifdef LM32_NO_BARREL_SHIFT
wire [`LM32_WORD_RNG] shifter_result_x; // Result of single-bit right shifter
`endif
// To/from multiplier
`ifdef LM32_MULTIPLY_ENABLED
wire [`LM32_WORD_RNG] multiplier_result_w; // Result from multiplier
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
wire multiply_d; // Indicates whether to perform a multiply or not
wire multiply_q_d;
`endif
// To/from divider
`ifdef CFG_MC_DIVIDE_ENABLED
wire divide_d; // Indicates whether to perform a divider or not
wire divide_q_d;
wire modulus_d;
wire modulus_q_d;
wire divide_by_zero_x; // Indicates an attempt was made to divide by zero
`endif
// To from multi-cycle arithmetic unit
`ifdef LM32_MC_ARITHMETIC_ENABLED
wire mc_stall_request_x; // Multi-cycle arithmetic unit stall request
wire [`LM32_WORD_RNG] mc_result_x;
`endif
// From CSRs
`ifdef CFG_INTERRUPTS_ENABLED
wire [`LM32_WORD_RNG] interrupt_csr_read_data_x;// Data read from interrupt CSRs
`endif
wire [`LM32_WORD_RNG] cfg; // Configuration CSR
wire [`LM32_WORD_RNG] cfg2; // Extended Configuration CSR
`ifdef CFG_CYCLE_COUNTER_ENABLED
reg [`LM32_WORD_RNG] cc; // Cycle counter CSR
`endif
reg [`LM32_WORD_RNG] csr_read_data_x; // Data read from CSRs
// To/from instruction unit
wire [`LM32_PC_RNG] pc_f; // PC of instruction in F stage
wire [`LM32_PC_RNG] pc_d; // PC of instruction in D stage
wire [`LM32_PC_RNG] pc_x; // PC of instruction in X stage
wire [`LM32_PC_RNG] pc_m; // PC of instruction in M stage
wire [`LM32_PC_RNG] pc_w; // PC of instruction in W stage
`ifdef CFG_TRACE_ENABLED
reg [`LM32_PC_RNG] pc_c; // PC of last commited instruction
`endif
`ifdef CFG_EBR_POSEDGE_REGISTER_FILE
wire [`LM32_INSTRUCTION_RNG] instruction_f; // Instruction in F stage
`endif
//p_ragma attribute instruction_d preserve_signal true
//p_ragma attribute instruction_d preserve_driver true
wire [`LM32_INSTRUCTION_RNG] instruction_d; // Instruction in D stage
`ifdef CFG_ICACHE_ENABLED
wire iflush; // Flush instruction cache
wire icache_stall_request; // Stall pipeline because instruction cache is busy
wire icache_restart_request; // Restart instruction that caused an instruction cache miss
wire icache_refill_request; // Request to refill instruction cache
wire icache_refilling; // Indicates the instruction cache is being refilled
`endif
`ifdef CFG_IROM_ENABLED
wire [`LM32_WORD_RNG] irom_store_data_m; // Store data to instruction ROM
wire [`LM32_WORD_RNG] irom_address_xm; // Address to instruction ROM from load-store unit
wire [`LM32_WORD_RNG] irom_data_m; // Load data from instruction ROM
wire irom_we_xm; // Indicates data needs to be written to instruction ROM
wire irom_stall_request_x; // Indicates D stage needs to be stalled on a store to instruction ROM
`endif
// To/from load/store unit
`ifdef CFG_DCACHE_ENABLED
wire dflush_x; // Flush data cache
reg dflush_m;
wire dcache_stall_request; // Stall pipeline because data cache is busy
wire dcache_restart_request; // Restart instruction that caused a data cache miss
wire dcache_refill_request; // Request to refill data cache
wire dcache_refilling; // Indicates the data cache is being refilled
`endif
wire [`LM32_WORD_RNG] load_data_w; // Result of a load instruction
wire stall_wb_load; // Stall pipeline because of a load via the data Wishbone interface
// To/from JTAG interface
`ifdef CFG_JTAG_ENABLED
`ifdef CFG_JTAG_UART_ENABLED
wire [`LM32_WORD_RNG] jtx_csr_read_data; // Read data for JTX CSR
wire [`LM32_WORD_RNG] jrx_csr_read_data; // Read data for JRX CSR
`endif
`ifdef CFG_HW_DEBUG_ENABLED
wire jtag_csr_write_enable; // Debugger CSR write enable
wire [`LM32_WORD_RNG] jtag_csr_write_data; // Data to write to specified CSR
wire [`LM32_CSR_RNG] jtag_csr; // Which CSR to write
wire jtag_read_enable;
wire [`LM32_BYTE_RNG] jtag_read_data;
wire jtag_write_enable;
wire [`LM32_BYTE_RNG] jtag_write_data;
wire [`LM32_WORD_RNG] jtag_address;
wire jtag_access_complete;
`endif
`ifdef CFG_DEBUG_ENABLED
wire jtag_break; // Request from debugger to raise a breakpoint
`endif
`endif
// Hazzard detection
wire raw_x_0; // RAW hazzard between instruction in X stage and read port 0
wire raw_x_1; // RAW hazzard between instruction in X stage and read port 1
wire raw_m_0; // RAW hazzard between instruction in M stage and read port 0
wire raw_m_1; // RAW hazzard between instruction in M stage and read port 1
wire raw_w_0; // RAW hazzard between instruction in W stage and read port 0
wire raw_w_1; // RAW hazzard between instruction in W stage and read port 1
// Control flow
wire cmp_zero; // Result of comparison is zero
wire cmp_negative; // Result of comparison is negative
wire cmp_overflow; // Comparison produced an overflow
wire cmp_carry_n; // Comparison produced a carry, inverted
reg condition_met_x; // Condition of branch instruction is met
reg condition_met_m;
`ifdef CFG_FAST_UNCONDITIONAL_BRANCH
wire branch_taken_x; // Branch is taken in X stage
`endif
wire branch_taken_m; // Branch is taken in M stage
wire kill_f; // Kill instruction in F stage
wire kill_d; // Kill instruction in D stage
wire kill_x; // Kill instruction in X stage
wire kill_m; // Kill instruction in M stage
wire kill_w; // Kill instruction in W stage
reg [`LM32_PC_WIDTH+2-1:8] eba; // Exception Base Address (EBA) CSR
`ifdef CFG_DEBUG_ENABLED
reg [`LM32_PC_WIDTH+2-1:8] deba; // Debug Exception Base Address (DEBA) CSR
`endif
reg [`LM32_EID_RNG] eid_x; // Exception ID in X stage
`ifdef CFG_TRACE_ENABLED
reg [`LM32_EID_RNG] eid_m; // Exception ID in M stage
reg [`LM32_EID_RNG] eid_w; // Exception ID in W stage
`endif
`ifdef CFG_DEBUG_ENABLED
`ifdef LM32_SINGLE_STEP_ENABLED
wire dc_ss; // Is single-step enabled
`endif
wire dc_re; // Remap all exceptions
wire bp_match;
wire wp_match;
wire exception_x; // An exception occured in the X stage
reg exception_m; // An instruction that caused an exception is in the M stage
wire debug_exception_x; // Indicates if a debug exception has occured
reg debug_exception_m;
reg debug_exception_w;
wire debug_exception_q_w;
wire non_debug_exception_x; // Indicates if a non debug exception has occured
reg non_debug_exception_m;
reg non_debug_exception_w;
wire non_debug_exception_q_w;
`else
wire exception_x; // Indicates if a debug exception has occured
reg exception_m;
reg exception_w;
wire exception_q_w;
`endif
`ifdef CFG_DEBUG_ENABLED
`ifdef CFG_JTAG_ENABLED
wire reset_exception; // Indicates if a reset exception has occured
`endif
`endif
`ifdef CFG_INTERRUPTS_ENABLED
wire interrupt_exception; // Indicates if an interrupt exception has occured
`endif
`ifdef CFG_DEBUG_ENABLED
wire breakpoint_exception; // Indicates if a breakpoint exception has occured
wire watchpoint_exception; // Indicates if a watchpoint exception has occured
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
wire instruction_bus_error_exception; // Indicates if an instruction bus error exception has occured
wire data_bus_error_exception; // Indicates if a data bus error exception has occured
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
wire divide_by_zero_exception; // Indicates if a divide by zero exception has occured
`endif
wire system_call_exception; // Indicates if a system call exception has occured
`ifdef CFG_BUS_ERRORS_ENABLED
reg data_bus_error_seen; // Indicates if a data bus error was seen
`endif
/////////////////////////////////////////////////////
// Functions
/////////////////////////////////////////////////////
`include "lm32_functions.v"
/////////////////////////////////////////////////////
// Instantiations
/////////////////////////////////////////////////////
// Instruction unit
lm32_instruction_unit #(
.eba_reset (eba_reset),
.associativity (icache_associativity),
.sets (icache_sets),
.bytes_per_line (icache_bytes_per_line),
.base_address (icache_base_address),
.limit (icache_limit)
) instruction_unit (
// ----- Inputs -------
.clk_i (clk_i),
.rst_i (rst_i),
// From pipeline
.stall_a (stall_a),
.stall_f (stall_f),
.stall_d (stall_d),
.stall_x (stall_x),
.stall_m (stall_m),
.valid_f (valid_f),
.valid_d (valid_d),
.kill_f (kill_f),
.branch_predict_taken_d (branch_predict_taken_d),
.branch_predict_address_d (branch_predict_address_d),
`ifdef CFG_FAST_UNCONDITIONAL_BRANCH
.branch_taken_x (branch_taken_x),
.branch_target_x (branch_target_x),
`endif
.exception_m (exception_m),
.branch_taken_m (branch_taken_m),
.branch_mispredict_taken_m (branch_mispredict_taken_m),
.branch_target_m (branch_target_m),
`ifdef CFG_ICACHE_ENABLED
.iflush (iflush),
`endif
`ifdef CFG_IROM_ENABLED
.irom_store_data_m (irom_store_data_m),
.irom_address_xm (irom_address_xm),
.irom_we_xm (irom_we_xm),
`endif
`ifdef CFG_DCACHE_ENABLED
.dcache_restart_request (dcache_restart_request),
.dcache_refill_request (dcache_refill_request),
.dcache_refilling (dcache_refilling),
`endif
`ifdef CFG_IWB_ENABLED
// From Wishbone
.i_dat_i (I_DAT_I),
.i_ack_i (I_ACK_I),
.i_err_i (I_ERR_I),
.i_rty_i (I_RTY_I),
`endif
`ifdef CFG_HW_DEBUG_ENABLED
.jtag_read_enable (jtag_read_enable),
.jtag_write_enable (jtag_write_enable),
.jtag_write_data (jtag_write_data),
.jtag_address (jtag_address),
`endif
// ----- Outputs -------
// To pipeline
.pc_f (pc_f),
.pc_d (pc_d),
.pc_x (pc_x),
.pc_m (pc_m),
.pc_w (pc_w),
`ifdef CFG_ICACHE_ENABLED
.icache_stall_request (icache_stall_request),
.icache_restart_request (icache_restart_request),
.icache_refill_request (icache_refill_request),
.icache_refilling (icache_refilling),
`endif
`ifdef CFG_IROM_ENABLED
.irom_data_m (irom_data_m),
`endif
`ifdef CFG_IWB_ENABLED
// To Wishbone
.i_dat_o (I_DAT_O),
.i_adr_o (I_ADR_O),
.i_cyc_o (I_CYC_O),
.i_sel_o (I_SEL_O),
.i_stb_o (I_STB_O),
.i_we_o (I_WE_O),
.i_cti_o (I_CTI_O),
.i_lock_o (I_LOCK_O),
.i_bte_o (I_BTE_O),
`endif
`ifdef CFG_HW_DEBUG_ENABLED
.jtag_read_data (jtag_read_data),
.jtag_access_complete (jtag_access_complete),
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
.bus_error_d (bus_error_d),
`endif
`ifdef CFG_EBR_POSEDGE_REGISTER_FILE
.instruction_f (instruction_f),
`endif
.instruction_d (instruction_d)
);
// Instruction decoder
lm32_decoder decoder (
// ----- Inputs -------
.instruction (instruction_d),
// ----- Outputs -------
.d_result_sel_0 (d_result_sel_0_d),
.d_result_sel_1 (d_result_sel_1_d),
.x_result_sel_csr (x_result_sel_csr_d),
`ifdef LM32_MC_ARITHMETIC_ENABLED
.x_result_sel_mc_arith (x_result_sel_mc_arith_d),
`endif
`ifdef LM32_NO_BARREL_SHIFT
.x_result_sel_shift (x_result_sel_shift_d),
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
.x_result_sel_sext (x_result_sel_sext_d),
`endif
.x_result_sel_logic (x_result_sel_logic_d),
`ifdef CFG_USER_ENABLED
.x_result_sel_user (x_result_sel_user_d),
`endif
.x_result_sel_add (x_result_sel_add_d),
.m_result_sel_compare (m_result_sel_compare_d),
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
.m_result_sel_shift (m_result_sel_shift_d),
`endif
.w_result_sel_load (w_result_sel_load_d),
`ifdef CFG_PL_MULTIPLY_ENABLED
.w_result_sel_mul (w_result_sel_mul_d),
`endif
.x_bypass_enable (x_bypass_enable_d),
.m_bypass_enable (m_bypass_enable_d),
.read_enable_0 (read_enable_0_d),
.read_idx_0 (read_idx_0_d),
.read_enable_1 (read_enable_1_d),
.read_idx_1 (read_idx_1_d),
.write_enable (write_enable_d),
.write_idx (write_idx_d),
.immediate (immediate_d),
.branch_offset (branch_offset_d),
.load (load_d),
.store (store_d),
.size (size_d),
.sign_extend (sign_extend_d),
.adder_op (adder_op_d),
.logic_op (logic_op_d),
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
.direction (direction_d),
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
.shift_left (shift_left_d),
.shift_right (shift_right_d),
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
.multiply (multiply_d),
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
.divide (divide_d),
.modulus (modulus_d),
`endif
.branch (branch_d),
.bi_unconditional (bi_unconditional),
.bi_conditional (bi_conditional),
.branch_reg (branch_reg_d),
.condition (condition_d),
`ifdef CFG_DEBUG_ENABLED
.break_opcode (break_d),
`endif
.scall (scall_d),
.eret (eret_d),
`ifdef CFG_DEBUG_ENABLED
.bret (bret_d),
`endif
`ifdef CFG_USER_ENABLED
.user_opcode (user_opcode_d),
`endif
.csr_write_enable (csr_write_enable_d)
);
// Load/store unit
lm32_load_store_unit #(
.associativity (dcache_associativity),
.sets (dcache_sets),
.bytes_per_line (dcache_bytes_per_line),
.base_address (dcache_base_address),
.limit (dcache_limit)
) load_store_unit (
// ----- Inputs -------
.clk_i (clk_i),
.rst_i (rst_i),
// From pipeline
.stall_a (stall_a),
.stall_x (stall_x),
.stall_m (stall_m),
.kill_x (kill_x),
.kill_m (kill_m),
.exception_m (exception_m),
.store_operand_x (store_operand_x),
.load_store_address_x (adder_result_x),
.load_store_address_m (operand_m),
.load_store_address_w (operand_w[1:0]),
.load_x (load_x),
.store_x (store_x),
.load_q_x (load_q_x),
.store_q_x (store_q_x),
.load_q_m (load_q_m),
.store_q_m (store_q_m),
.sign_extend_x (sign_extend_x),
.size_x (size_x),
`ifdef CFG_DCACHE_ENABLED
.dflush (dflush_m),
`endif
`ifdef CFG_IROM_ENABLED
.irom_data_m (irom_data_m),
`endif
// From Wishbone
.d_dat_i (D_DAT_I),
.d_ack_i (D_ACK_I),
.d_err_i (D_ERR_I),
.d_rty_i (D_RTY_I),
// ----- Outputs -------
// To pipeline
`ifdef CFG_DCACHE_ENABLED
.dcache_refill_request (dcache_refill_request),
.dcache_restart_request (dcache_restart_request),
.dcache_stall_request (dcache_stall_request),
.dcache_refilling (dcache_refilling),
`endif
`ifdef CFG_IROM_ENABLED
.irom_store_data_m (irom_store_data_m),
.irom_address_xm (irom_address_xm),
.irom_we_xm (irom_we_xm),
.irom_stall_request_x (irom_stall_request_x),
`endif
.load_data_w (load_data_w),
.stall_wb_load (stall_wb_load),
// To Wishbone
.d_dat_o (D_DAT_O),
.d_adr_o (D_ADR_O),
.d_cyc_o (D_CYC_O),
.d_sel_o (D_SEL_O),
.d_stb_o (D_STB_O),
.d_we_o (D_WE_O),
.d_cti_o (D_CTI_O),
.d_lock_o (D_LOCK_O),
.d_bte_o (D_BTE_O)
);
// Adder
lm32_adder adder (
// ----- Inputs -------
.adder_op_x (adder_op_x),
.adder_op_x_n (adder_op_x_n),
.operand_0_x (operand_0_x),
.operand_1_x (operand_1_x),
// ----- Outputs -------
.adder_result_x (adder_result_x),
.adder_carry_n_x (adder_carry_n_x),
.adder_overflow_x (adder_overflow_x)
);
// Logic operations
lm32_logic_op logic_op (
// ----- Inputs -------
.logic_op_x (logic_op_x),
.operand_0_x (operand_0_x),
.operand_1_x (operand_1_x),
// ----- Outputs -------
.logic_result_x (logic_result_x)
);
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
// Pipelined barrel-shifter
lm32_shifter shifter (
// ----- Inputs -------
.clk_i (clk_i),
.rst_i (rst_i),
.stall_x (stall_x),
.direction_x (direction_x),
.sign_extend_x (sign_extend_x),
.operand_0_x (operand_0_x),
.operand_1_x (operand_1_x),
// ----- Outputs -------
.shifter_result_m (shifter_result_m)
);
`endif
`ifdef CFG_PL_MULTIPLY_ENABLED
// Pipeline fixed-point multiplier
lm32_multiplier multiplier (
// ----- Inputs -------
.clk_i (clk_i),
.rst_i (rst_i),
.stall_x (stall_x),
.stall_m (stall_m),
.operand_0 (d_result_0),
.operand_1 (d_result_1),
// ----- Outputs -------
.result (multiplier_result_w)
);
`endif
`ifdef LM32_MC_ARITHMETIC_ENABLED
// Multi-cycle arithmetic
lm32_mc_arithmetic mc_arithmetic (
// ----- Inputs -------
.clk_i (clk_i),
.rst_i (rst_i),
.stall_d (stall_d),
.kill_x (kill_x),
`ifdef CFG_MC_DIVIDE_ENABLED
.divide_d (divide_q_d),
.modulus_d (modulus_q_d),
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
.multiply_d (multiply_q_d),
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
.shift_left_d (shift_left_q_d),
.shift_right_d (shift_right_q_d),
.sign_extend_d (sign_extend_d),
`endif
.operand_0_d (d_result_0),
.operand_1_d (d_result_1),
// ----- Outputs -------
.result_x (mc_result_x),
`ifdef CFG_MC_DIVIDE_ENABLED
.divide_by_zero_x (divide_by_zero_x),
`endif
.stall_request_x (mc_stall_request_x)
);
`endif
`ifdef CFG_INTERRUPTS_ENABLED
// Interrupt unit
lm32_interrupt interrupt_unit (
// ----- Inputs -------
.clk_i (clk_i),
.rst_i (rst_i),
// From external devices
.interrupt (interrupt),
// From pipeline
.stall_x (stall_x),
`ifdef CFG_DEBUG_ENABLED
.non_debug_exception (non_debug_exception_q_w),
.debug_exception (debug_exception_q_w),
`else
.exception (exception_q_w),
`endif
.eret_q_x (eret_q_x),
`ifdef CFG_DEBUG_ENABLED
.bret_q_x (bret_q_x),
`endif
.csr (csr_x),
.csr_write_data (operand_1_x),
.csr_write_enable (csr_write_enable_q_x),
// ----- Outputs -------
.interrupt_exception (interrupt_exception),
// To pipeline
.csr_read_data (interrupt_csr_read_data_x)
);
`endif
`ifdef CFG_JTAG_ENABLED
// JTAG interface
lm32_jtag jtag (
// ----- Inputs -------
.clk_i (clk_i),
.rst_i (rst_i),
// From JTAG
.jtag_clk (jtag_clk),
.jtag_update (jtag_update),
.jtag_reg_q (jtag_reg_q),
.jtag_reg_addr_q (jtag_reg_addr_q),
// From pipeline
`ifdef CFG_JTAG_UART_ENABLED
.csr (csr_x),
.csr_write_data (operand_1_x),
.csr_write_enable (csr_write_enable_q_x),
.stall_x (stall_x),
`endif
`ifdef CFG_HW_DEBUG_ENABLED
.jtag_read_data (jtag_read_data),
.jtag_access_complete (jtag_access_complete),
`endif
`ifdef CFG_DEBUG_ENABLED
.exception_q_w (debug_exception_q_w || non_debug_exception_q_w),
`endif
// ----- Outputs -------
// To pipeline
`ifdef CFG_JTAG_UART_ENABLED
.jtx_csr_read_data (jtx_csr_read_data),
.jrx_csr_read_data (jrx_csr_read_data),
`endif
`ifdef CFG_HW_DEBUG_ENABLED
.jtag_csr_write_enable (jtag_csr_write_enable),
.jtag_csr_write_data (jtag_csr_write_data),
.jtag_csr (jtag_csr),
.jtag_read_enable (jtag_read_enable),
.jtag_write_enable (jtag_write_enable),
.jtag_write_data (jtag_write_data),
.jtag_address (jtag_address),
`endif
`ifdef CFG_DEBUG_ENABLED
.jtag_break (jtag_break),
.jtag_reset (reset_exception),
`endif
// To JTAG
.jtag_reg_d (jtag_reg_d),
.jtag_reg_addr_d (jtag_reg_addr_d)
);
`endif
`ifdef CFG_DEBUG_ENABLED
// Debug unit
lm32_debug #(
.breakpoints (breakpoints),
.watchpoints (watchpoints)
) hw_debug (
// ----- Inputs -------
.clk_i (clk_i),
.rst_i (rst_i),
.pc_x (pc_x),
.load_x (load_x),
.store_x (store_x),
.load_store_address_x (adder_result_x),
.csr_write_enable_x (csr_write_enable_q_x),
.csr_write_data (operand_1_x),
.csr_x (csr_x),
`ifdef CFG_HW_DEBUG_ENABLED
.jtag_csr_write_enable (jtag_csr_write_enable),
.jtag_csr_write_data (jtag_csr_write_data),
.jtag_csr (jtag_csr),
`endif
`ifdef LM32_SINGLE_STEP_ENABLED
.eret_q_x (eret_q_x),
.bret_q_x (bret_q_x),
.stall_x (stall_x),
.exception_x (exception_x),
.q_x (q_x),
`ifdef CFG_DCACHE_ENABLED
.dcache_refill_request (dcache_refill_request),
`endif
`endif
// ----- Outputs -------
`ifdef LM32_SINGLE_STEP_ENABLED
.dc_ss (dc_ss),
`endif
.dc_re (dc_re),
.bp_match (bp_match),
.wp_match (wp_match)
);
`endif
// Register file
`ifdef CFG_EBR_POSEDGE_REGISTER_FILE
/*----------------------------------------------------------------------
Register File is implemented using EBRs. There can be three accesses to
the register file in each cycle: two reads and one write. On-chip block
RAM has two read/write ports. To accomodate three accesses, two on-chip
block RAMs are used (each register file "write" is made to both block
RAMs).
One limitation of the on-chip block RAMs is that one cannot perform a
read and write to same location in a cycle (if this is done, then the
data read out is indeterminate).
----------------------------------------------------------------------*/
wire [31:0] regfile_data_0, regfile_data_1;
reg [31:0] w_result_d;
reg regfile_raw_0, regfile_raw_0_nxt;
reg regfile_raw_1, regfile_raw_1_nxt;
/*----------------------------------------------------------------------
Check if read and write is being performed to same register in current
cycle? This is done by comparing the read and write IDXs.
----------------------------------------------------------------------*/
always @(reg_write_enable_q_w or write_idx_w or instruction_f)
begin
if (reg_write_enable_q_w
&& (write_idx_w == instruction_f[25:21]))
regfile_raw_0_nxt = 1'b1;
else
regfile_raw_0_nxt = 1'b0;
if (reg_write_enable_q_w
&& (write_idx_w == instruction_f[20:16]))
regfile_raw_1_nxt = 1'b1;
else
regfile_raw_1_nxt = 1'b0;
end
/*----------------------------------------------------------------------
Select latched (delayed) write value or data from register file. If
read in previous cycle was performed to register written to in same
cycle, then latched (delayed) write value is selected.
----------------------------------------------------------------------*/
always @(regfile_raw_0 or w_result_d or regfile_data_0)
if (regfile_raw_0)
reg_data_live_0 = w_result_d;
else
reg_data_live_0 = regfile_data_0;
/*----------------------------------------------------------------------
Select latched (delayed) write value or data from register file. If
read in previous cycle was performed to register written to in same
cycle, then latched (delayed) write value is selected.
----------------------------------------------------------------------*/
always @(regfile_raw_1 or w_result_d or regfile_data_1)
if (regfile_raw_1)
reg_data_live_1 = w_result_d;
else
reg_data_live_1 = regfile_data_1;
/*----------------------------------------------------------------------
Latch value written to register file
----------------------------------------------------------------------*/
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
if (rst_i == `TRUE)
begin
regfile_raw_0 <= 1'b0;
regfile_raw_1 <= 1'b0;
w_result_d <= 32'b0;
end
else
begin
regfile_raw_0 <= regfile_raw_0_nxt;
regfile_raw_1 <= regfile_raw_1_nxt;
w_result_d <= w_result;
end
/*----------------------------------------------------------------------
Register file instantiation as Pseudo-Dual Port EBRs.
----------------------------------------------------------------------*/
// Modified by GSI: removed non-portable RAM instantiation
lm32_dp_ram
#(
// ----- Parameters -----
.addr_depth(1<<5),
.addr_width(5),
.data_width(32)
)
reg_0
(
// ----- Inputs -----
.clk_i (clk_i),
.rst_i (rst_i),
.we_i (reg_write_enable_q_w),
.wdata_i (w_result),
.waddr_i (write_idx_w),
.raddr_i (instruction_f[25:21]),
// ----- Outputs -----
.rdata_o (regfile_data_0)
);
lm32_dp_ram
#(
.addr_depth(1<<5),
.addr_width(5),
.data_width(32)
)
reg_1
(
// ----- Inputs -----
.clk_i (clk_i),
.rst_i (rst_i),
.we_i (reg_write_enable_q_w),
.wdata_i (w_result),
.waddr_i (write_idx_w),
.raddr_i (instruction_f[20:16]),
// ----- Outputs -----
.rdata_o (regfile_data_1)
);
`endif
`ifdef CFG_EBR_NEGEDGE_REGISTER_FILE
pmi_ram_dp
#(
// ----- Parameters -----
.pmi_wr_addr_depth(1<<5),
.pmi_wr_addr_width(5),
.pmi_wr_data_width(32),
.pmi_rd_addr_depth(1<<5),
.pmi_rd_addr_width(5),
.pmi_rd_data_width(32),
.pmi_regmode("noreg"),
.pmi_gsr("enable"),
.pmi_resetmode("sync"),
.pmi_init_file("none"),
.pmi_init_file_format("binary"),
.pmi_family(`LATTICE_FAMILY),
.module_type("pmi_ram_dp")
)
reg_0
(
// ----- Inputs -----
.Data(w_result),
.WrAddress(write_idx_w),
.RdAddress(read_idx_0_d),
.WrClock(clk_i),
.RdClock(clk_n_i),
.WrClockEn(`TRUE),
.RdClockEn(stall_f == `FALSE),
.WE(reg_write_enable_q_w),
.Reset(rst_i),
// ----- Outputs -----
.Q(reg_data_0)
);
pmi_ram_dp
#(
// ----- Parameters -----
.pmi_wr_addr_depth(1<<5),
.pmi_wr_addr_width(5),
.pmi_wr_data_width(32),
.pmi_rd_addr_depth(1<<5),
.pmi_rd_addr_width(5),
.pmi_rd_data_width(32),
.pmi_regmode("noreg"),
.pmi_gsr("enable"),
.pmi_resetmode("sync"),
.pmi_init_file("none"),
.pmi_init_file_format("binary"),
.pmi_family(`LATTICE_FAMILY),
.module_type("pmi_ram_dp")
)
reg_1
(
// ----- Inputs -----
.Data(w_result),
.WrAddress(write_idx_w),
.RdAddress(read_idx_1_d),
.WrClock(clk_i),
.RdClock(clk_n_i),
.WrClockEn(`TRUE),
.RdClockEn(stall_f == `FALSE),
.WE(reg_write_enable_q_w),
.Reset(rst_i),
// ----- Outputs -----
.Q(reg_data_1)
);
`endif
/////////////////////////////////////////////////////
// Combinational Logic
/////////////////////////////////////////////////////
`ifdef CFG_EBR_POSEDGE_REGISTER_FILE
// Select between buffered and live data from register file
assign reg_data_0 = use_buf ? reg_data_buf_0 : reg_data_live_0;
assign reg_data_1 = use_buf ? reg_data_buf_1 : reg_data_live_1;
`endif
`ifdef LM32_EBR_REGISTER_FILE
`else
// Register file read ports
assign reg_data_0 = registers[read_idx_0_d];
assign reg_data_1 = registers[read_idx_1_d];
`endif
// Detect read-after-write hazzards
assign raw_x_0 = (write_idx_x == read_idx_0_d) && (write_enable_q_x == `TRUE);
assign raw_m_0 = (write_idx_m == read_idx_0_d) && (write_enable_q_m == `TRUE);
assign raw_w_0 = (write_idx_w == read_idx_0_d) && (write_enable_q_w == `TRUE);
assign raw_x_1 = (write_idx_x == read_idx_1_d) && (write_enable_q_x == `TRUE);
assign raw_m_1 = (write_idx_m == read_idx_1_d) && (write_enable_q_m == `TRUE);
assign raw_w_1 = (write_idx_w == read_idx_1_d) && (write_enable_q_w == `TRUE);
// Interlock detection - Raise an interlock for RAW hazzards
always @(*)
begin
if ( ( (x_bypass_enable_x == `FALSE)
&& ( ((read_enable_0_d == `TRUE) && (raw_x_0 == `TRUE))
|| ((read_enable_1_d == `TRUE) && (raw_x_1 == `TRUE))
)
)
|| ( (m_bypass_enable_m == `FALSE)
&& ( ((read_enable_0_d == `TRUE) && (raw_m_0 == `TRUE))
|| ((read_enable_1_d == `TRUE) && (raw_m_1 == `TRUE))
)
)
)
interlock = `TRUE;
else
interlock = `FALSE;
end
// Bypass for reg port 0
always @(*)
begin
if (raw_x_0 == `TRUE)
bypass_data_0 = x_result;
else if (raw_m_0 == `TRUE)
bypass_data_0 = m_result;
else if (raw_w_0 == `TRUE)
bypass_data_0 = w_result;
else
bypass_data_0 = reg_data_0;
end
// Bypass for reg port 1
always @(*)
begin
if (raw_x_1 == `TRUE)
bypass_data_1 = x_result;
else if (raw_m_1 == `TRUE)
bypass_data_1 = m_result;
else if (raw_w_1 == `TRUE)
bypass_data_1 = w_result;
else
bypass_data_1 = reg_data_1;
end
/*----------------------------------------------------------------------
Branch prediction is performed in D stage of pipeline. Only PC-relative
branches are predicted: forward-pointing conditional branches are not-
taken, while backward-pointing conditional branches are taken.
Unconditional branches are always predicted taken!
----------------------------------------------------------------------*/
assign branch_predict_d = bi_unconditional | bi_conditional;
assign branch_predict_taken_d = bi_unconditional ? 1'b1 : (bi_conditional ? instruction_d[15] : 1'b0);
// Compute branch target address: Branch PC PLUS Offset
assign branch_target_d = pc_d + branch_offset_d;
// Compute fetch address. Address of instruction sequentially after the
// branch if branch is not taken. Target address of branch is branch is
// taken
assign branch_predict_address_d = branch_predict_taken_d ? branch_target_d : pc_f;
// D stage result selection
always @(*)
begin
d_result_0 = d_result_sel_0_d[0] ? {pc_f, 2'b00} : bypass_data_0;
case (d_result_sel_1_d)
`LM32_D_RESULT_SEL_1_ZERO: d_result_1 = {`LM32_WORD_WIDTH{1'b0}};
`LM32_D_RESULT_SEL_1_REG_1: d_result_1 = bypass_data_1;
`LM32_D_RESULT_SEL_1_IMMEDIATE: d_result_1 = immediate_d;
default: d_result_1 = {`LM32_WORD_WIDTH{1'bx}};
endcase
end
`ifdef CFG_USER_ENABLED
// Operands for user-defined instructions
assign user_operand_0 = operand_0_x;
assign user_operand_1 = operand_1_x;
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
// Sign-extension
assign sextb_result_x = {{24{operand_0_x[7]}}, operand_0_x[7:0]};
assign sexth_result_x = {{16{operand_0_x[15]}}, operand_0_x[15:0]};
assign sext_result_x = size_x == `LM32_SIZE_BYTE ? sextb_result_x : sexth_result_x;
`endif
`ifdef LM32_NO_BARREL_SHIFT
// Only single bit shift operations are supported when barrel-shifter isn't implemented
assign shifter_result_x = {operand_0_x[`LM32_WORD_WIDTH-1] & sign_extend_x, operand_0_x[`LM32_WORD_WIDTH-1:1]};
`endif
// Condition evaluation
assign cmp_zero = operand_0_x == operand_1_x;
assign cmp_negative = adder_result_x[`LM32_WORD_WIDTH-1];
assign cmp_overflow = adder_overflow_x;
assign cmp_carry_n = adder_carry_n_x;
always @(*)
begin
case (condition_x)
`LM32_CONDITION_U1: condition_met_x = `TRUE;
`LM32_CONDITION_U2: condition_met_x = `TRUE;
`LM32_CONDITION_E: condition_met_x = cmp_zero;
`LM32_CONDITION_NE: condition_met_x = !cmp_zero;
`LM32_CONDITION_G: condition_met_x = !cmp_zero && (cmp_negative == cmp_overflow);
`LM32_CONDITION_GU: condition_met_x = cmp_carry_n && !cmp_zero;
`LM32_CONDITION_GE: condition_met_x = cmp_negative == cmp_overflow;
`LM32_CONDITION_GEU: condition_met_x = cmp_carry_n;
default: condition_met_x = 1'bx;
endcase
end
// X stage result selection
always @(*)
begin
x_result = x_result_sel_add_x ? adder_result_x
: x_result_sel_csr_x ? csr_read_data_x
`ifdef CFG_SIGN_EXTEND_ENABLED
: x_result_sel_sext_x ? sext_result_x
`endif
`ifdef CFG_USER_ENABLED
: x_result_sel_user_x ? user_result
`endif
`ifdef LM32_NO_BARREL_SHIFT
: x_result_sel_shift_x ? shifter_result_x
`endif
`ifdef LM32_MC_ARITHMETIC_ENABLED
: x_result_sel_mc_arith_x ? mc_result_x
`endif
: logic_result_x;
end
// M stage result selection
always @(*)
begin
m_result = m_result_sel_compare_m ? {{`LM32_WORD_WIDTH-1{1'b0}}, condition_met_m}
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
: m_result_sel_shift_m ? shifter_result_m
`endif
: operand_m;
end
// W stage result selection
always @(*)
begin
w_result = w_result_sel_load_w ? load_data_w
`ifdef CFG_PL_MULTIPLY_ENABLED
: w_result_sel_mul_w ? multiplier_result_w
`endif
: operand_w;
end
`ifdef CFG_FAST_UNCONDITIONAL_BRANCH
// Indicate when a branch should be taken in X stage
assign branch_taken_x = (stall_x == `FALSE)
&& ( (branch_x == `TRUE)
&& ((condition_x == `LM32_CONDITION_U1) || (condition_x == `LM32_CONDITION_U2))
&& (valid_x == `TRUE)
&& (branch_predict_x == `FALSE)
);
`endif
// Indicate when a branch should be taken in M stage (exceptions are a type of branch)
assign branch_taken_m = (stall_m == `FALSE)
&& ( ( (branch_m == `TRUE)
&& (valid_m == `TRUE)
&& ( ( (condition_met_m == `TRUE)
&& (branch_predict_taken_m == `FALSE)
)
|| ( (condition_met_m == `FALSE)
&& (branch_predict_m == `TRUE)
&& (branch_predict_taken_m == `TRUE)
)
)
)
|| (exception_m == `TRUE)
);
// Indicate when a branch in M stage is mispredicted as being taken
assign branch_mispredict_taken_m = (condition_met_m == `FALSE)
&& (branch_predict_m == `TRUE)
&& (branch_predict_taken_m == `TRUE);
// Indicate when a branch in M stage will cause flush in X stage
assign branch_flushX_m = (stall_m == `FALSE)
&& ( ( (branch_m == `TRUE)
&& (valid_m == `TRUE)
&& ( (condition_met_m == `TRUE)
|| ( (condition_met_m == `FALSE)
&& (branch_predict_m == `TRUE)
&& (branch_predict_taken_m == `TRUE)
)
)
)
|| (exception_m == `TRUE)
);
// Generate signal that will kill instructions in each pipeline stage when necessary
assign kill_f = ( (valid_d == `TRUE)
&& (branch_predict_taken_d == `TRUE)
)
|| (branch_taken_m == `TRUE)
`ifdef CFG_FAST_UNCONDITIONAL_BRANCH
|| (branch_taken_x == `TRUE)
`endif
`ifdef CFG_ICACHE_ENABLED
|| (icache_refill_request == `TRUE)
`endif
`ifdef CFG_DCACHE_ENABLED
|| (dcache_refill_request == `TRUE)
`endif
;
assign kill_d = (branch_taken_m == `TRUE)
`ifdef CFG_FAST_UNCONDITIONAL_BRANCH
|| (branch_taken_x == `TRUE)
`endif
`ifdef CFG_ICACHE_ENABLED
|| (icache_refill_request == `TRUE)
`endif
`ifdef CFG_DCACHE_ENABLED
|| (dcache_refill_request == `TRUE)
`endif
;
assign kill_x = (branch_flushX_m == `TRUE)
`ifdef CFG_DCACHE_ENABLED
|| (dcache_refill_request == `TRUE)
`endif
;
assign kill_m = `FALSE
`ifdef CFG_DCACHE_ENABLED
|| (dcache_refill_request == `TRUE)
`endif
;
assign kill_w = `FALSE
`ifdef CFG_DCACHE_ENABLED
|| (dcache_refill_request == `TRUE)
`endif
;
// Exceptions
`ifdef CFG_DEBUG_ENABLED
assign breakpoint_exception = ( ( (break_x == `TRUE)
|| (bp_match == `TRUE)
)
&& (valid_x == `TRUE)
)
`ifdef CFG_JTAG_ENABLED
|| (jtag_break == `TRUE)
`endif
;
`endif
`ifdef CFG_DEBUG_ENABLED
assign watchpoint_exception = wp_match == `TRUE;
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
assign instruction_bus_error_exception = ( (bus_error_x == `TRUE)
&& (valid_x == `TRUE)
);
assign data_bus_error_exception = data_bus_error_seen == `TRUE;
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
assign divide_by_zero_exception = divide_by_zero_x == `TRUE;
`endif
assign system_call_exception = ( (scall_x == `TRUE)
`ifdef CFG_BUS_ERRORS_ENABLED
&& (valid_x == `TRUE)
`endif
);
`ifdef CFG_DEBUG_ENABLED
assign debug_exception_x = (breakpoint_exception == `TRUE)
|| (watchpoint_exception == `TRUE)
;
assign non_debug_exception_x = (system_call_exception == `TRUE)
`ifdef CFG_JTAG_ENABLED
|| (reset_exception == `TRUE)
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
|| (instruction_bus_error_exception == `TRUE)
|| (data_bus_error_exception == `TRUE)
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
|| (divide_by_zero_exception == `TRUE)
`endif
`ifdef CFG_INTERRUPTS_ENABLED
|| ( (interrupt_exception == `TRUE)
`ifdef LM32_SINGLE_STEP_ENABLED
&& (dc_ss == `FALSE)
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
&& (store_q_m == `FALSE)
&& (D_CYC_O == `FALSE)
`endif
)
`endif
;
assign exception_x = (debug_exception_x == `TRUE) || (non_debug_exception_x == `TRUE);
`else
assign exception_x = (system_call_exception == `TRUE)
`ifdef CFG_BUS_ERRORS_ENABLED
|| (instruction_bus_error_exception == `TRUE)
|| (data_bus_error_exception == `TRUE)
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
|| (divide_by_zero_exception == `TRUE)
`endif
`ifdef CFG_INTERRUPTS_ENABLED
|| ( (interrupt_exception == `TRUE)
`ifdef LM32_SINGLE_STEP_ENABLED
&& (dc_ss == `FALSE)
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
&& (store_q_m == `FALSE)
&& (D_CYC_O == `FALSE)
`endif
)
`endif
;
`endif
// Exception ID
always @(*)
begin
`ifdef CFG_DEBUG_ENABLED
`ifdef CFG_JTAG_ENABLED
if (reset_exception == `TRUE)
eid_x = `LM32_EID_RESET;
else
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
if (data_bus_error_exception == `TRUE)
eid_x = `LM32_EID_DATA_BUS_ERROR;
else
`endif
if (breakpoint_exception == `TRUE)
eid_x = `LM32_EID_BREAKPOINT;
else
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
if (data_bus_error_exception == `TRUE)
eid_x = `LM32_EID_DATA_BUS_ERROR;
else
if (instruction_bus_error_exception == `TRUE)
eid_x = `LM32_EID_INST_BUS_ERROR;
else
`endif
`ifdef CFG_DEBUG_ENABLED
if (watchpoint_exception == `TRUE)
eid_x = `LM32_EID_WATCHPOINT;
else
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
if (divide_by_zero_exception == `TRUE)
eid_x = `LM32_EID_DIVIDE_BY_ZERO;
else
`endif
`ifdef CFG_INTERRUPTS_ENABLED
if ( (interrupt_exception == `TRUE)
`ifdef LM32_SINGLE_STEP_ENABLED
&& (dc_ss == `FALSE)
`endif
)
eid_x = `LM32_EID_INTERRUPT;
else
`endif
eid_x = `LM32_EID_SCALL;
end
// Stall generation
assign stall_a = (stall_f == `TRUE);
assign stall_f = (stall_d == `TRUE);
assign stall_d = (stall_x == `TRUE)
|| ( (interlock == `TRUE)
&& (kill_d == `FALSE)
)
|| ( ( (eret_d == `TRUE)
|| (scall_d == `TRUE)
`ifdef CFG_BUS_ERRORS_ENABLED
|| (bus_error_d == `TRUE)
`endif
)
&& ( (load_q_x == `TRUE)
|| (load_q_m == `TRUE)
|| (store_q_x == `TRUE)
|| (store_q_m == `TRUE)
|| (D_CYC_O == `TRUE)
)
&& (kill_d == `FALSE)
)
`ifdef CFG_DEBUG_ENABLED
|| ( ( (break_d == `TRUE)
|| (bret_d == `TRUE)
)
&& ( (load_q_x == `TRUE)
|| (store_q_x == `TRUE)
|| (load_q_m == `TRUE)
|| (store_q_m == `TRUE)
|| (D_CYC_O == `TRUE)
)
&& (kill_d == `FALSE)
)
`endif
|| ( (csr_write_enable_d == `TRUE)
&& (load_q_x == `TRUE)
)
;
assign stall_x = (stall_m == `TRUE)
`ifdef LM32_MC_ARITHMETIC_ENABLED
|| ( (mc_stall_request_x == `TRUE)
&& (kill_x == `FALSE)
)
`endif
`ifdef CFG_IROM_ENABLED
// Stall load/store instruction in D stage if there is an ongoing store
// operation to instruction ROM in M stage
|| ( (irom_stall_request_x == `TRUE)
&& ( (load_d == `TRUE)
|| (store_d == `TRUE)
)
)
`endif
;
assign stall_m = (stall_wb_load == `TRUE)
`ifdef CFG_SIZE_OVER_SPEED
|| (D_CYC_O == `TRUE)
`else
|| ( (D_CYC_O == `TRUE)
&& ( (store_m == `TRUE)
/*
Bug: Following loop does not allow interrupts to be services since
either D_CYC_O or store_m is always high during entire duration of
loop.
L1: addi r1, r1, 1
sw (r2,0), r1
bi L1
Introduce a single-cycle stall when a wishbone cycle is in progress
and a new store instruction is in Execute stage and a interrupt
exception has occured. This stall will ensure that D_CYC_O and
store_m will both be low for one cycle.
*/
`ifdef CFG_INTERRUPTS_ENABLED
|| ((store_x == `TRUE) && (interrupt_exception == `TRUE))
`endif
|| (load_m == `TRUE)
|| (load_x == `TRUE)
)
)
`endif
`ifdef CFG_DCACHE_ENABLED
|| (dcache_stall_request == `TRUE) // Need to stall in case a taken branch is in M stage and data cache is only being flush, so wont be restarted
`endif
`ifdef CFG_ICACHE_ENABLED
|| (icache_stall_request == `TRUE) // Pipeline needs to be stalled otherwise branches may be lost
|| ((I_CYC_O == `TRUE) && ((branch_m == `TRUE) || (exception_m == `TRUE)))
`else
`ifdef CFG_IWB_ENABLED
|| (I_CYC_O == `TRUE)
`endif
`endif
`ifdef CFG_USER_ENABLED
|| ( (user_valid == `TRUE) // Stall whole pipeline, rather than just X stage, where the instruction is, so we don't have to worry about exceptions (maybe)
&& (user_complete == `FALSE)
)
`endif
;
// Qualify state changing control signals
`ifdef LM32_MC_ARITHMETIC_ENABLED
assign q_d = (valid_d == `TRUE) && (kill_d == `FALSE);
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
assign shift_left_q_d = (shift_left_d == `TRUE) && (q_d == `TRUE);
assign shift_right_q_d = (shift_right_d == `TRUE) && (q_d == `TRUE);
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
assign multiply_q_d = (multiply_d == `TRUE) && (q_d == `TRUE);
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
assign divide_q_d = (divide_d == `TRUE) && (q_d == `TRUE);
assign modulus_q_d = (modulus_d == `TRUE) && (q_d == `TRUE);
`endif
assign q_x = (valid_x == `TRUE) && (kill_x == `FALSE);
assign csr_write_enable_q_x = (csr_write_enable_x == `TRUE) && (q_x == `TRUE);
assign eret_q_x = (eret_x == `TRUE) && (q_x == `TRUE);
`ifdef CFG_DEBUG_ENABLED
assign bret_q_x = (bret_x == `TRUE) && (q_x == `TRUE);
`endif
assign load_q_x = (load_x == `TRUE)
&& (q_x == `TRUE)
`ifdef CFG_DEBUG_ENABLED
&& (bp_match == `FALSE)
`endif
;
assign store_q_x = (store_x == `TRUE)
&& (q_x == `TRUE)
`ifdef CFG_DEBUG_ENABLED
&& (bp_match == `FALSE)
`endif
;
`ifdef CFG_USER_ENABLED
assign user_valid = (x_result_sel_user_x == `TRUE) && (q_x == `TRUE);
`endif
assign q_m = (valid_m == `TRUE) && (kill_m == `FALSE) && (exception_m == `FALSE);
assign load_q_m = (load_m == `TRUE) && (q_m == `TRUE);
assign store_q_m = (store_m == `TRUE) && (q_m == `TRUE);
`ifdef CFG_DEBUG_ENABLED
assign debug_exception_q_w = ((debug_exception_w == `TRUE) && (valid_w == `TRUE));
assign non_debug_exception_q_w = ((non_debug_exception_w == `TRUE) && (valid_w == `TRUE));
`else
assign exception_q_w = ((exception_w == `TRUE) && (valid_w == `TRUE));
`endif
// Don't qualify register write enables with kill, as the signal is needed early, and it doesn't matter if the instruction is killed (except for the actual write - but that is handled separately)
assign write_enable_q_x = (write_enable_x == `TRUE) && (valid_x == `TRUE) && (branch_flushX_m == `FALSE);
assign write_enable_q_m = (write_enable_m == `TRUE) && (valid_m == `TRUE);
assign write_enable_q_w = (write_enable_w == `TRUE) && (valid_w == `TRUE);
// The enable that actually does write the registers needs to be qualified with kill
assign reg_write_enable_q_w = (write_enable_w == `TRUE) && (kill_w == `FALSE) && (valid_w == `TRUE);
// Configuration (CFG) CSR
assign cfg = {
`LM32_REVISION,
watchpoints[3:0],
breakpoints[3:0],
interrupts[5:0],
`ifdef CFG_JTAG_UART_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef CFG_ROM_DEBUG_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef CFG_HW_DEBUG_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef CFG_DEBUG_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef CFG_ICACHE_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef CFG_DCACHE_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef CFG_CYCLE_COUNTER_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef CFG_USER_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef LM32_BARREL_SHIFT_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef LM32_MULTIPLY_ENABLED
`TRUE
`else
`FALSE
`endif
};
assign cfg2 = {
30'b0,
`ifdef CFG_IROM_ENABLED
`TRUE,
`else
`FALSE,
`endif
`ifdef CFG_DRAM_ENABLED
`TRUE
`else
`FALSE
`endif
};
// Cache flush
`ifdef CFG_ICACHE_ENABLED
assign iflush = ( (csr_write_enable_d == `TRUE)
&& (csr_d == `LM32_CSR_ICC)
&& (stall_d == `FALSE)
&& (kill_d == `FALSE)
&& (valid_d == `TRUE))
// Added by GSI: needed to flush cache after loading firmware per JTAG
`ifdef CFG_HW_DEBUG_ENABLED
||
( (jtag_csr_write_enable == `TRUE)
&& (jtag_csr == `LM32_CSR_ICC))
`endif
;
`endif
`ifdef CFG_DCACHE_ENABLED
assign dflush_x = ( (csr_write_enable_q_x == `TRUE)
&& (csr_x == `LM32_CSR_DCC))
// Added by GSI: needed to flush cache after loading firmware per JTAG
`ifdef CFG_HW_DEBUG_ENABLED
||
( (jtag_csr_write_enable == `TRUE)
&& (jtag_csr == `LM32_CSR_DCC))
`endif
;
`endif
// Extract CSR index
assign csr_d = read_idx_0_d[`LM32_CSR_RNG];
// CSR reads
always @(*)
begin
case (csr_x)
`ifdef CFG_INTERRUPTS_ENABLED
`LM32_CSR_IE,
`LM32_CSR_IM,
`LM32_CSR_IP: csr_read_data_x = interrupt_csr_read_data_x;
`endif
`ifdef CFG_CYCLE_COUNTER_ENABLED
`LM32_CSR_CC: csr_read_data_x = cc;
`endif
`LM32_CSR_CFG: csr_read_data_x = cfg;
`LM32_CSR_EBA: csr_read_data_x = {eba, 8'h00};
`ifdef CFG_DEBUG_ENABLED
`LM32_CSR_DEBA: csr_read_data_x = {deba, 8'h00};
`endif
`ifdef CFG_JTAG_UART_ENABLED
`LM32_CSR_JTX: csr_read_data_x = jtx_csr_read_data;
`LM32_CSR_JRX: csr_read_data_x = jrx_csr_read_data;
`endif
`LM32_CSR_CFG2: csr_read_data_x = cfg2;
default: csr_read_data_x = {`LM32_WORD_WIDTH{1'bx}};
endcase
end
/////////////////////////////////////////////////////
// Sequential Logic
/////////////////////////////////////////////////////
// Exception Base Address (EBA) CSR
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
eba <= eba_reset[`LM32_PC_WIDTH+2-1:8];
else
begin
if ((csr_write_enable_q_x == `TRUE) && (csr_x == `LM32_CSR_EBA) && (stall_x == `FALSE))
eba <= operand_1_x[`LM32_PC_WIDTH+2-1:8];
`ifdef CFG_HW_DEBUG_ENABLED
if ((jtag_csr_write_enable == `TRUE) && (jtag_csr == `LM32_CSR_EBA))
eba <= jtag_csr_write_data[`LM32_PC_WIDTH+2-1:8];
`endif
end
end
`ifdef CFG_DEBUG_ENABLED
// Debug Exception Base Address (DEBA) CSR
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
deba <= deba_reset[`LM32_PC_WIDTH+2-1:8];
else
begin
if ((csr_write_enable_q_x == `TRUE) && (csr_x == `LM32_CSR_DEBA) && (stall_x == `FALSE))
deba <= operand_1_x[`LM32_PC_WIDTH+2-1:8];
`ifdef CFG_HW_DEBUG_ENABLED
if ((jtag_csr_write_enable == `TRUE) && (jtag_csr == `LM32_CSR_DEBA))
deba <= jtag_csr_write_data[`LM32_PC_WIDTH+2-1:8];
`endif
end
end
`endif
// Cycle Counter (CC) CSR
`ifdef CFG_CYCLE_COUNTER_ENABLED
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
cc <= {`LM32_WORD_WIDTH{1'b0}};
else
cc <= cc + 1'b1;
end
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
// Watch for data bus errors
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
data_bus_error_seen <= `FALSE;
else
begin
// Set flag when bus error is detected
if ((D_ERR_I == `TRUE) && (D_CYC_O == `TRUE))
data_bus_error_seen <= `TRUE;
// Clear flag when exception is taken
if ((exception_m == `TRUE) && (kill_m == `FALSE))
data_bus_error_seen <= `FALSE;
end
end
`endif
// Valid bits to indicate whether an instruction in a partcular pipeline stage is valid or not
`ifdef CFG_ICACHE_ENABLED
`ifdef CFG_DCACHE_ENABLED
always @(*)
begin
if ( (icache_refill_request == `TRUE)
|| (dcache_refill_request == `TRUE)
)
valid_a = `FALSE;
else if ( (icache_restart_request == `TRUE)
|| (dcache_restart_request == `TRUE)
)
valid_a = `TRUE;
else
valid_a = !icache_refilling && !dcache_refilling;
end
`else
always @(*)
begin
if (icache_refill_request == `TRUE)
valid_a = `FALSE;
else if (icache_restart_request == `TRUE)
valid_a = `TRUE;
else
valid_a = !icache_refilling;
end
`endif
`else
`ifdef CFG_DCACHE_ENABLED
always @(*)
begin
if (dcache_refill_request == `TRUE)
valid_a = `FALSE;
else if (dcache_restart_request == `TRUE)
valid_a = `TRUE;
else
valid_a = !dcache_refilling;
end
`endif
`endif
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
valid_f <= `FALSE;
valid_d <= `FALSE;
valid_x <= `FALSE;
valid_m <= `FALSE;
valid_w <= `FALSE;
end
else
begin
if ((kill_f == `TRUE) || (stall_a == `FALSE))
`ifdef LM32_CACHE_ENABLED
valid_f <= valid_a;
`else
valid_f <= `TRUE;
`endif
else if (stall_f == `FALSE)
valid_f <= `FALSE;
if (kill_d == `TRUE)
valid_d <= `FALSE;
else if (stall_f == `FALSE)
valid_d <= valid_f & !kill_f;
else if (stall_d == `FALSE)
valid_d <= `FALSE;
if (stall_d == `FALSE)
valid_x <= valid_d & !kill_d;
else if (kill_x == `TRUE)
valid_x <= `FALSE;
else if (stall_x == `FALSE)
valid_x <= `FALSE;
if (kill_m == `TRUE)
valid_m <= `FALSE;
else if (stall_x == `FALSE)
valid_m <= valid_x & !kill_x;
else if (stall_m == `FALSE)
valid_m <= `FALSE;
if (stall_m == `FALSE)
valid_w <= valid_m & !kill_m;
else
valid_w <= `FALSE;
end
end
// Microcode pipeline registers
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
`ifdef CFG_USER_ENABLED
user_opcode <= {`LM32_USER_OPCODE_WIDTH{1'b0}};
`endif
operand_0_x <= {`LM32_WORD_WIDTH{1'b0}};
operand_1_x <= {`LM32_WORD_WIDTH{1'b0}};
store_operand_x <= {`LM32_WORD_WIDTH{1'b0}};
branch_target_x <= {`LM32_PC_WIDTH{1'b0}};
x_result_sel_csr_x <= `FALSE;
`ifdef LM32_MC_ARITHMETIC_ENABLED
x_result_sel_mc_arith_x <= `FALSE;
`endif
`ifdef LM32_NO_BARREL_SHIFT
x_result_sel_shift_x <= `FALSE;
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
x_result_sel_sext_x <= `FALSE;
`endif
`ifdef CFG_USER_ENABLED
x_result_sel_user_x <= `FALSE;
`endif
x_result_sel_add_x <= `FALSE;
m_result_sel_compare_x <= `FALSE;
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
m_result_sel_shift_x <= `FALSE;
`endif
w_result_sel_load_x <= `FALSE;
`ifdef CFG_PL_MULTIPLY_ENABLED
w_result_sel_mul_x <= `FALSE;
`endif
x_bypass_enable_x <= `FALSE;
m_bypass_enable_x <= `FALSE;
write_enable_x <= `FALSE;
write_idx_x <= {`LM32_REG_IDX_WIDTH{1'b0}};
csr_x <= {`LM32_CSR_WIDTH{1'b0}};
load_x <= `FALSE;
store_x <= `FALSE;
size_x <= {`LM32_SIZE_WIDTH{1'b0}};
sign_extend_x <= `FALSE;
adder_op_x <= `FALSE;
adder_op_x_n <= `FALSE;
logic_op_x <= 4'h0;
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
direction_x <= `FALSE;
`endif
`ifdef CFG_ROTATE_ENABLED
rotate_x <= `FALSE;
`endif
branch_x <= `FALSE;
branch_predict_x <= `FALSE;
branch_predict_taken_x <= `FALSE;
condition_x <= `LM32_CONDITION_U1;
`ifdef CFG_DEBUG_ENABLED
break_x <= `FALSE;
`endif
scall_x <= `FALSE;
eret_x <= `FALSE;
`ifdef CFG_DEBUG_ENABLED
bret_x <= `FALSE;
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
bus_error_x <= `FALSE;
data_bus_error_exception_m <= `FALSE;
`endif
csr_write_enable_x <= `FALSE;
operand_m <= {`LM32_WORD_WIDTH{1'b0}};
branch_target_m <= {`LM32_PC_WIDTH{1'b0}};
m_result_sel_compare_m <= `FALSE;
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
m_result_sel_shift_m <= `FALSE;
`endif
w_result_sel_load_m <= `FALSE;
`ifdef CFG_PL_MULTIPLY_ENABLED
w_result_sel_mul_m <= `FALSE;
`endif
m_bypass_enable_m <= `FALSE;
branch_m <= `FALSE;
branch_predict_m <= `FALSE;
branch_predict_taken_m <= `FALSE;
exception_m <= `FALSE;
load_m <= `FALSE;
store_m <= `FALSE;
write_enable_m <= `FALSE;
write_idx_m <= {`LM32_REG_IDX_WIDTH{1'b0}};
condition_met_m <= `FALSE;
`ifdef CFG_DCACHE_ENABLED
dflush_m <= `FALSE;
`endif
`ifdef CFG_DEBUG_ENABLED
debug_exception_m <= `FALSE;
non_debug_exception_m <= `FALSE;
`endif
operand_w <= {`LM32_WORD_WIDTH{1'b0}};
w_result_sel_load_w <= `FALSE;
`ifdef CFG_PL_MULTIPLY_ENABLED
w_result_sel_mul_w <= `FALSE;
`endif
write_idx_w <= {`LM32_REG_IDX_WIDTH{1'b0}};
write_enable_w <= `FALSE;
`ifdef CFG_DEBUG_ENABLED
debug_exception_w <= `FALSE;
non_debug_exception_w <= `FALSE;
`else
exception_w <= `FALSE;
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
memop_pc_w <= {`LM32_PC_WIDTH{1'b0}};
`endif
end
else
begin
// D/X stage registers
if (stall_x == `FALSE)
begin
`ifdef CFG_USER_ENABLED
user_opcode <= user_opcode_d;
`endif
operand_0_x <= d_result_0;
operand_1_x <= d_result_1;
store_operand_x <= bypass_data_1;
branch_target_x <= branch_reg_d == `TRUE ? bypass_data_0[`LM32_PC_RNG] : branch_target_d;
x_result_sel_csr_x <= x_result_sel_csr_d;
`ifdef LM32_MC_ARITHMETIC_ENABLED
x_result_sel_mc_arith_x <= x_result_sel_mc_arith_d;
`endif
`ifdef LM32_NO_BARREL_SHIFT
x_result_sel_shift_x <= x_result_sel_shift_d;
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
x_result_sel_sext_x <= x_result_sel_sext_d;
`endif
`ifdef CFG_USER_ENABLED
x_result_sel_user_x <= x_result_sel_user_d;
`endif
x_result_sel_add_x <= x_result_sel_add_d;
m_result_sel_compare_x <= m_result_sel_compare_d;
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
m_result_sel_shift_x <= m_result_sel_shift_d;
`endif
w_result_sel_load_x <= w_result_sel_load_d;
`ifdef CFG_PL_MULTIPLY_ENABLED
w_result_sel_mul_x <= w_result_sel_mul_d;
`endif
x_bypass_enable_x <= x_bypass_enable_d;
m_bypass_enable_x <= m_bypass_enable_d;
load_x <= load_d;
store_x <= store_d;
branch_x <= branch_d;
branch_predict_x <= branch_predict_d;
branch_predict_taken_x <= branch_predict_taken_d;
write_idx_x <= write_idx_d;
csr_x <= csr_d;
size_x <= size_d;
sign_extend_x <= sign_extend_d;
adder_op_x <= adder_op_d;
adder_op_x_n <= ~adder_op_d;
logic_op_x <= logic_op_d;
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
direction_x <= direction_d;
`endif
`ifdef CFG_ROTATE_ENABLED
rotate_x <= rotate_d;
`endif
condition_x <= condition_d;
csr_write_enable_x <= csr_write_enable_d;
`ifdef CFG_DEBUG_ENABLED
break_x <= break_d;
`endif
scall_x <= scall_d;
`ifdef CFG_BUS_ERRORS_ENABLED
bus_error_x <= bus_error_d;
`endif
eret_x <= eret_d;
`ifdef CFG_DEBUG_ENABLED
bret_x <= bret_d;
`endif
write_enable_x <= write_enable_d;
end
// X/M stage registers
if (stall_m == `FALSE)
begin
operand_m <= x_result;
m_result_sel_compare_m <= m_result_sel_compare_x;
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
m_result_sel_shift_m <= m_result_sel_shift_x;
`endif
if (exception_x == `TRUE)
begin
w_result_sel_load_m <= `FALSE;
`ifdef CFG_PL_MULTIPLY_ENABLED
w_result_sel_mul_m <= `FALSE;
`endif
end
else
begin
w_result_sel_load_m <= w_result_sel_load_x;
`ifdef CFG_PL_MULTIPLY_ENABLED
w_result_sel_mul_m <= w_result_sel_mul_x;
`endif
end
m_bypass_enable_m <= m_bypass_enable_x;
load_m <= load_x;
store_m <= store_x;
`ifdef CFG_FAST_UNCONDITIONAL_BRANCH
branch_m <= branch_x && !branch_taken_x;
`else
branch_m <= branch_x;
branch_predict_m <= branch_predict_x;
branch_predict_taken_m <= branch_predict_taken_x;
`endif
`ifdef CFG_DEBUG_ENABLED
// Data bus errors are generated by the wishbone and are
// made known to the processor only in next cycle (as a
// non-debug exception). A break instruction can be seen
// in same cycle (causing a debug exception). Handle non
// -debug exception first!
if (non_debug_exception_x == `TRUE)
write_idx_m <= `LM32_EA_REG;
else if (debug_exception_x == `TRUE)
write_idx_m <= `LM32_BA_REG;
else
write_idx_m <= write_idx_x;
`else
if (exception_x == `TRUE)
write_idx_m <= `LM32_EA_REG;
else
write_idx_m <= write_idx_x;
`endif
condition_met_m <= condition_met_x;
`ifdef CFG_DEBUG_ENABLED
if (exception_x == `TRUE)
if ((dc_re == `TRUE)
|| ((debug_exception_x == `TRUE)
&& (non_debug_exception_x == `FALSE)))
branch_target_m <= {deba, eid_x, {3{1'b0}}};
else
branch_target_m <= {eba, eid_x, {3{1'b0}}};
else
branch_target_m <= branch_target_x;
`else
branch_target_m <= exception_x == `TRUE ? {eba, eid_x, {3{1'b0}}} : branch_target_x;
`endif
`ifdef CFG_TRACE_ENABLED
eid_m <= eid_x;
eret_m <= eret_q_x;
`endif
`ifdef CFG_DCACHE_ENABLED
dflush_m <= dflush_x;
`endif
`ifdef CFG_TRACE_ENABLED
`ifdef CFG_DEBUG_ENABLED
bret_m <= bret_q_x;
`endif
`endif
write_enable_m <= exception_x == `TRUE ? `TRUE : write_enable_x;
`ifdef CFG_DEBUG_ENABLED
debug_exception_m <= debug_exception_x;
non_debug_exception_m <= non_debug_exception_x;
`endif
end
// State changing regs
if (stall_m == `FALSE)
begin
if ((exception_x == `TRUE) && (q_x == `TRUE) && (stall_x == `FALSE))
exception_m <= `TRUE;
else
exception_m <= `FALSE;
`ifdef CFG_BUS_ERRORS_ENABLED
data_bus_error_exception_m <= (data_bus_error_exception == `TRUE)
`ifdef CFG_DEBUG_ENABLED
&& (reset_exception == `FALSE)
`endif
;
`endif
end
// M/W stage registers
`ifdef CFG_BUS_ERRORS_ENABLED
operand_w <= exception_m == `TRUE ? (data_bus_error_exception_m ? {memop_pc_w, 2'b00} : {pc_m, 2'b00}) : m_result;
`else
operand_w <= exception_m == `TRUE ? {pc_m, 2'b00} : m_result;
`endif
w_result_sel_load_w <= w_result_sel_load_m;
`ifdef CFG_PL_MULTIPLY_ENABLED
w_result_sel_mul_w <= w_result_sel_mul_m;
`endif
write_idx_w <= write_idx_m;
`ifdef CFG_TRACE_ENABLED
eid_w <= eid_m;
eret_w <= eret_m;
`ifdef CFG_DEBUG_ENABLED
bret_w <= bret_m;
`endif
`endif
write_enable_w <= write_enable_m;
`ifdef CFG_DEBUG_ENABLED
debug_exception_w <= debug_exception_m;
non_debug_exception_w <= non_debug_exception_m;
`else
exception_w <= exception_m;
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
if ( (stall_m == `FALSE)
&& ( (load_q_m == `TRUE)
|| (store_q_m == `TRUE)
)
)
memop_pc_w <= pc_m;
`endif
end
end
`ifdef CFG_EBR_POSEDGE_REGISTER_FILE
// Buffer data read from register file, in case a stall occurs, and watch for
// any writes to the modified registers
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
use_buf <= `FALSE;
reg_data_buf_0 <= {`LM32_WORD_WIDTH{1'b0}};
reg_data_buf_1 <= {`LM32_WORD_WIDTH{1'b0}};
end
else
begin
if (stall_d == `FALSE)
use_buf <= `FALSE;
else if (use_buf == `FALSE)
begin
reg_data_buf_0 <= reg_data_live_0;
reg_data_buf_1 <= reg_data_live_1;
use_buf <= `TRUE;
end
if (reg_write_enable_q_w == `TRUE)
begin
if (write_idx_w == read_idx_0_d)
reg_data_buf_0 <= w_result;
if (write_idx_w == read_idx_1_d)
reg_data_buf_1 <= w_result;
end
end
end
`endif
`ifdef LM32_EBR_REGISTER_FILE
`else
// Register file write port
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE) begin
registers[0] <= {`LM32_WORD_WIDTH{1'b0}};
registers[1] <= {`LM32_WORD_WIDTH{1'b0}};
registers[2] <= {`LM32_WORD_WIDTH{1'b0}};
registers[3] <= {`LM32_WORD_WIDTH{1'b0}};
registers[4] <= {`LM32_WORD_WIDTH{1'b0}};
registers[5] <= {`LM32_WORD_WIDTH{1'b0}};
registers[6] <= {`LM32_WORD_WIDTH{1'b0}};
registers[7] <= {`LM32_WORD_WIDTH{1'b0}};
registers[8] <= {`LM32_WORD_WIDTH{1'b0}};
registers[9] <= {`LM32_WORD_WIDTH{1'b0}};
registers[10] <= {`LM32_WORD_WIDTH{1'b0}};
registers[11] <= {`LM32_WORD_WIDTH{1'b0}};
registers[12] <= {`LM32_WORD_WIDTH{1'b0}};
registers[13] <= {`LM32_WORD_WIDTH{1'b0}};
registers[14] <= {`LM32_WORD_WIDTH{1'b0}};
registers[15] <= {`LM32_WORD_WIDTH{1'b0}};
registers[16] <= {`LM32_WORD_WIDTH{1'b0}};
registers[17] <= {`LM32_WORD_WIDTH{1'b0}};
registers[18] <= {`LM32_WORD_WIDTH{1'b0}};
registers[19] <= {`LM32_WORD_WIDTH{1'b0}};
registers[20] <= {`LM32_WORD_WIDTH{1'b0}};
registers[21] <= {`LM32_WORD_WIDTH{1'b0}};
registers[22] <= {`LM32_WORD_WIDTH{1'b0}};
registers[23] <= {`LM32_WORD_WIDTH{1'b0}};
registers[24] <= {`LM32_WORD_WIDTH{1'b0}};
registers[25] <= {`LM32_WORD_WIDTH{1'b0}};
registers[26] <= {`LM32_WORD_WIDTH{1'b0}};
registers[27] <= {`LM32_WORD_WIDTH{1'b0}};
registers[28] <= {`LM32_WORD_WIDTH{1'b0}};
registers[29] <= {`LM32_WORD_WIDTH{1'b0}};
registers[30] <= {`LM32_WORD_WIDTH{1'b0}};
registers[31] <= {`LM32_WORD_WIDTH{1'b0}};
end
else begin
if (reg_write_enable_q_w == `TRUE)
registers[write_idx_w] <= w_result;
end
end
`endif
`ifdef CFG_TRACE_ENABLED
// PC tracing logic
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
trace_pc_valid <= `FALSE;
trace_pc <= {`LM32_PC_WIDTH{1'b0}};
trace_exception <= `FALSE;
trace_eid <= `LM32_EID_RESET;
trace_eret <= `FALSE;
`ifdef CFG_DEBUG_ENABLED
trace_bret <= `FALSE;
`endif
pc_c <= eba_reset/4;
end
else
begin
trace_pc_valid <= `FALSE;
// Has an exception occured
`ifdef CFG_DEBUG_ENABLED
if ((debug_exception_q_w == `TRUE) || (non_debug_exception_q_w == `TRUE))
`else
if (exception_q_w == `TRUE)
`endif
begin
trace_exception <= `TRUE;
trace_pc_valid <= `TRUE;
trace_pc <= pc_w;
trace_eid <= eid_w;
end
else
trace_exception <= `FALSE;
if ((valid_w == `TRUE) && (!kill_w))
begin
// An instruction is commiting. Determine if it is non-sequential
if (pc_c + 1'b1 != pc_w)
begin
// Non-sequential instruction
trace_pc_valid <= `TRUE;
trace_pc <= pc_w;
end
// Record PC so we can determine if next instruction is sequential or not
pc_c <= pc_w;
// Indicate if it was an eret/bret instruction
trace_eret <= eret_w;
`ifdef CFG_DEBUG_ENABLED
trace_bret <= bret_w;
`endif
end
else
begin
trace_eret <= `FALSE;
`ifdef CFG_DEBUG_ENABLED
trace_bret <= `FALSE;
`endif
end
end
end
`endif
/////////////////////////////////////////////////////
// Behavioural Logic
/////////////////////////////////////////////////////
// synthesis translate_off
// Reset register 0. Only needed for simulation.
initial
begin
`ifdef LM32_EBR_REGISTER_FILE
`else
registers[0] = {`LM32_WORD_WIDTH{1'b0}};
`endif
end
// synthesis translate_on
endmodule
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_dcache.v
// Title : Data cache
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : Support for user-selected resource usage when implementing
// : cache memory. Additional parameters must be defined when
// : invoking lm32_ram.v
// =============================================================================
`include "lm32_include.v"
`ifdef CFG_DCACHE_ENABLED
`define LM32_DC_ADDR_OFFSET_RNG addr_offset_msb:addr_offset_lsb
`define LM32_DC_ADDR_SET_RNG addr_set_msb:addr_set_lsb
`define LM32_DC_ADDR_TAG_RNG addr_tag_msb:addr_tag_lsb
`define LM32_DC_ADDR_IDX_RNG addr_set_msb:addr_offset_lsb
`define LM32_DC_TMEM_ADDR_WIDTH addr_set_width
`define LM32_DC_TMEM_ADDR_RNG (`LM32_DC_TMEM_ADDR_WIDTH-1):0
`define LM32_DC_DMEM_ADDR_WIDTH (addr_offset_width+addr_set_width)
`define LM32_DC_DMEM_ADDR_RNG (`LM32_DC_DMEM_ADDR_WIDTH-1):0
`define LM32_DC_TAGS_WIDTH (addr_tag_width+1)
`define LM32_DC_TAGS_RNG (`LM32_DC_TAGS_WIDTH-1):0
`define LM32_DC_TAGS_TAG_RNG (`LM32_DC_TAGS_WIDTH-1):1
`define LM32_DC_TAGS_VALID_RNG 0
`define LM32_DC_STATE_RNG 2:0
`define LM32_DC_STATE_FLUSH 3'b001
`define LM32_DC_STATE_CHECK 3'b010
`define LM32_DC_STATE_REFILL 3'b100
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_dcache (
// ----- Inputs -----
clk_i,
rst_i,
stall_a,
stall_x,
stall_m,
address_x,
address_m,
load_q_m,
store_q_m,
store_data,
store_byte_select,
refill_ready,
refill_data,
dflush,
// ----- Outputs -----
stall_request,
restart_request,
refill_request,
refill_address,
refilling,
load_data
);
/////////////////////////////////////////////////////
// Parameters
/////////////////////////////////////////////////////
parameter associativity = 1; // Associativity of the cache (Number of ways)
parameter sets = 512; // Number of sets
parameter bytes_per_line = 16; // Number of bytes per cache line
parameter base_address = 0; // Base address of cachable memory
parameter limit = 0; // Limit (highest address) of cachable memory
localparam addr_offset_width = clogb2(bytes_per_line)-1-2;
localparam addr_set_width = clogb2(sets)-1;
localparam addr_offset_lsb = 2;
localparam addr_offset_msb = (addr_offset_lsb+addr_offset_width-1);
localparam addr_set_lsb = (addr_offset_msb+1);
localparam addr_set_msb = (addr_set_lsb+addr_set_width-1);
localparam addr_tag_lsb = (addr_set_msb+1);
localparam addr_tag_msb = clogb2(`CFG_DCACHE_LIMIT-`CFG_DCACHE_BASE_ADDRESS)-1;
localparam addr_tag_width = (addr_tag_msb-addr_tag_lsb+1);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
input rst_i; // Reset
input stall_a; // Stall A stage
input stall_x; // Stall X stage
input stall_m; // Stall M stage
input [`LM32_WORD_RNG] address_x; // X stage load/store address
input [`LM32_WORD_RNG] address_m; // M stage load/store address
input load_q_m; // Load instruction in M stage
input store_q_m; // Store instruction in M stage
input [`LM32_WORD_RNG] store_data; // Data to store
input [`LM32_BYTE_SELECT_RNG] store_byte_select; // Which bytes in store data should be modified
input refill_ready; // Indicates next word of refill data is ready
input [`LM32_WORD_RNG] refill_data; // Refill data
input dflush; // Indicates cache should be flushed
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
output stall_request; // Request pipeline be stalled because cache is busy
wire stall_request;
output restart_request; // Request to restart instruction that caused the cache miss
reg restart_request;
output refill_request; // Request a refill
reg refill_request;
output [`LM32_WORD_RNG] refill_address; // Address to refill from
reg [`LM32_WORD_RNG] refill_address;
output refilling; // Indicates if the cache is currently refilling
reg refilling;
output [`LM32_WORD_RNG] load_data; // Data read from cache
wire [`LM32_WORD_RNG] load_data;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
wire read_port_enable; // Cache memory read port clock enable
wire write_port_enable; // Cache memory write port clock enable
wire [0:associativity-1] way_tmem_we; // Tag memory write enable
wire [0:associativity-1] way_dmem_we; // Data memory write enable
wire [`LM32_WORD_RNG] way_data[0:associativity-1]; // Data read from data memory
wire [`LM32_DC_TAGS_TAG_RNG] way_tag[0:associativity-1];// Tag read from tag memory
wire [0:associativity-1] way_valid; // Indicates which ways are valid
wire [0:associativity-1] way_match; // Indicates which ways matched
wire miss; // Indicates no ways matched
wire [`LM32_DC_TMEM_ADDR_RNG] tmem_read_address; // Tag memory read address
wire [`LM32_DC_TMEM_ADDR_RNG] tmem_write_address; // Tag memory write address
wire [`LM32_DC_DMEM_ADDR_RNG] dmem_read_address; // Data memory read address
wire [`LM32_DC_DMEM_ADDR_RNG] dmem_write_address; // Data memory write address
wire [`LM32_DC_TAGS_RNG] tmem_write_data; // Tag memory write data
reg [`LM32_WORD_RNG] dmem_write_data; // Data memory write data
reg [`LM32_DC_STATE_RNG] state; // Current state of FSM
wire flushing; // Indicates if cache is currently flushing
wire check; // Indicates if cache is currently checking for hits/misses
wire refill; // Indicates if cache is currently refilling
wire valid_store; // Indicates if there is a valid store instruction
reg [associativity-1:0] refill_way_select; // Which way should be refilled
reg [`LM32_DC_ADDR_OFFSET_RNG] refill_offset; // Which word in cache line should be refilled
wire last_refill; // Indicates when on last cycle of cache refill
reg [`LM32_DC_TMEM_ADDR_RNG] flush_set; // Which set is currently being flushed
genvar i, j;
/////////////////////////////////////////////////////
// Functions
/////////////////////////////////////////////////////
`include "lm32_functions.v"
/////////////////////////////////////////////////////
// Instantiations
/////////////////////////////////////////////////////
generate
for (i = 0; i < associativity; i = i + 1)
begin : memories
// Way data
if (`LM32_DC_DMEM_ADDR_WIDTH < 11)
begin : data_memories
lm32_ram
#(
// ----- Parameters -------
.data_width (32),
.address_width (`LM32_DC_DMEM_ADDR_WIDTH)
// Modified for Milkymist: removed non-portable RAM parameters
) way_0_data_ram
(
// ----- Inputs -------
.read_clk (clk_i),
.write_clk (clk_i),
.reset (rst_i),
.read_address (dmem_read_address),
.enable_read (read_port_enable),
.write_address (dmem_write_address),
.enable_write (write_port_enable),
.write_enable (way_dmem_we[i]),
.write_data (dmem_write_data),
// ----- Outputs -------
.read_data (way_data[i])
);
end
else
begin
for (j = 0; j < 4; j = j + 1)
begin : byte_memories
lm32_ram
#(
// ----- Parameters -------
.data_width (8),
.address_width (`LM32_DC_DMEM_ADDR_WIDTH)
// Modified for Milkymist: removed non-portable RAM parameters
) way_0_data_ram
(
// ----- Inputs -------
.read_clk (clk_i),
.write_clk (clk_i),
.reset (rst_i),
.read_address (dmem_read_address),
.enable_read (read_port_enable),
.write_address (dmem_write_address),
.enable_write (write_port_enable),
.write_enable (way_dmem_we[i] & (store_byte_select[j] | refill)),
.write_data (dmem_write_data[(j+1)*8-1:j*8]),
// ----- Outputs -------
.read_data (way_data[i][(j+1)*8-1:j*8])
);
end
end
// Way tags
lm32_ram
#(
// ----- Parameters -------
.data_width (`LM32_DC_TAGS_WIDTH),
.address_width (`LM32_DC_TMEM_ADDR_WIDTH)
// Modified for Milkymist: removed non-portable RAM parameters
) way_0_tag_ram
(
// ----- Inputs -------
.read_clk (clk_i),
.write_clk (clk_i),
.reset (rst_i),
.read_address (tmem_read_address),
.enable_read (read_port_enable),
.write_address (tmem_write_address),
.enable_write (`TRUE),
.write_enable (way_tmem_we[i]),
.write_data (tmem_write_data),
// ----- Outputs -------
.read_data ({way_tag[i], way_valid[i]})
);
end
endgenerate
/////////////////////////////////////////////////////
// Combinational logic
/////////////////////////////////////////////////////
// Compute which ways in the cache match the address being read
generate
for (i = 0; i < associativity; i = i + 1)
begin : match
assign way_match[i] = ({way_tag[i], way_valid[i]} == {address_m[`LM32_DC_ADDR_TAG_RNG], `TRUE});
end
endgenerate
// Select data from way that matched the address being read
generate
if (associativity == 1)
begin : data_1
assign load_data = way_data[0];
end
else if (associativity == 2)
begin : data_2
assign load_data = way_match[0] ? way_data[0] : way_data[1];
end
endgenerate
generate
if (`LM32_DC_DMEM_ADDR_WIDTH < 11)
begin
// Select data to write to data memories
always @(*)
begin
if (refill == `TRUE)
dmem_write_data = refill_data;
else
begin
dmem_write_data[`LM32_BYTE_0_RNG] = store_byte_select[0] ? store_data[`LM32_BYTE_0_RNG] : load_data[`LM32_BYTE_0_RNG];
dmem_write_data[`LM32_BYTE_1_RNG] = store_byte_select[1] ? store_data[`LM32_BYTE_1_RNG] : load_data[`LM32_BYTE_1_RNG];
dmem_write_data[`LM32_BYTE_2_RNG] = store_byte_select[2] ? store_data[`LM32_BYTE_2_RNG] : load_data[`LM32_BYTE_2_RNG];
dmem_write_data[`LM32_BYTE_3_RNG] = store_byte_select[3] ? store_data[`LM32_BYTE_3_RNG] : load_data[`LM32_BYTE_3_RNG];
end
end
end
else
begin
// Select data to write to data memories - FIXME: Should use different write ports on dual port RAMs, but they don't work
always @(*)
begin
if (refill == `TRUE)
dmem_write_data = refill_data;
else
dmem_write_data = store_data;
end
end
endgenerate
// Compute address to use to index into the data memories
generate
if (bytes_per_line > 4)
assign dmem_write_address = (refill == `TRUE)
? {refill_address[`LM32_DC_ADDR_SET_RNG], refill_offset}
: address_m[`LM32_DC_ADDR_IDX_RNG];
else
assign dmem_write_address = (refill == `TRUE)
? refill_address[`LM32_DC_ADDR_SET_RNG]
: address_m[`LM32_DC_ADDR_IDX_RNG];
endgenerate
assign dmem_read_address = address_x[`LM32_DC_ADDR_IDX_RNG];
// Compute address to use to index into the tag memories
assign tmem_write_address = (flushing == `TRUE)
? flush_set
: refill_address[`LM32_DC_ADDR_SET_RNG];
assign tmem_read_address = address_x[`LM32_DC_ADDR_SET_RNG];
// Compute signal to indicate when we are on the last refill accesses
generate
if (bytes_per_line > 4)
assign last_refill = refill_offset == {addr_offset_width{1'b1}};
else
assign last_refill = `TRUE;
endgenerate
// Compute data and tag memory access enable
assign read_port_enable = (stall_x == `FALSE);
assign write_port_enable = (refill_ready == `TRUE) || !stall_m;
// Determine when we have a valid store
assign valid_store = (store_q_m == `TRUE) && (check == `TRUE);
// Compute data and tag memory write enables
generate
if (associativity == 1)
begin : we_1
assign way_dmem_we[0] = (refill_ready == `TRUE) || ((valid_store == `TRUE) && (way_match[0] == `TRUE));
assign way_tmem_we[0] = (refill_ready == `TRUE) || (flushing == `TRUE);
end
else
begin : we_2
assign way_dmem_we[0] = ((refill_ready == `TRUE) && (refill_way_select[0] == `TRUE)) || ((valid_store == `TRUE) && (way_match[0] == `TRUE));
assign way_dmem_we[1] = ((refill_ready == `TRUE) && (refill_way_select[1] == `TRUE)) || ((valid_store == `TRUE) && (way_match[1] == `TRUE));
assign way_tmem_we[0] = ((refill_ready == `TRUE) && (refill_way_select[0] == `TRUE)) || (flushing == `TRUE);
assign way_tmem_we[1] = ((refill_ready == `TRUE) && (refill_way_select[1] == `TRUE)) || (flushing == `TRUE);
end
endgenerate
// On the last refill cycle set the valid bit, for all other writes it should be cleared
assign tmem_write_data[`LM32_DC_TAGS_VALID_RNG] = ((last_refill == `TRUE) || (valid_store == `TRUE)) && (flushing == `FALSE);
assign tmem_write_data[`LM32_DC_TAGS_TAG_RNG] = refill_address[`LM32_DC_ADDR_TAG_RNG];
// Signals that indicate which state we are in
assign flushing = state[0];
assign check = state[1];
assign refill = state[2];
assign miss = (~(|way_match)) && (load_q_m == `TRUE) && (stall_m == `FALSE);
assign stall_request = (check == `FALSE);
/////////////////////////////////////////////////////
// Sequential logic
/////////////////////////////////////////////////////
// Record way selected for replacement on a cache miss
generate
if (associativity >= 2)
begin : way_select
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
refill_way_select <= {{associativity-1{1'b0}}, 1'b1};
else
begin
if (refill_request == `TRUE)
refill_way_select <= {refill_way_select[0], refill_way_select[1]};
end
end
end
endgenerate
// Record whether we are currently refilling
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
refilling <= `FALSE;
else
refilling <= refill;
end
// Instruction cache control FSM
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
state <= `LM32_DC_STATE_FLUSH;
flush_set <= {`LM32_DC_TMEM_ADDR_WIDTH{1'b1}};
refill_request <= `FALSE;
refill_address <= {`LM32_WORD_WIDTH{1'bx}};
restart_request <= `FALSE;
end
else
begin
case (state)
// Flush the cache
`LM32_DC_STATE_FLUSH:
begin
if (flush_set == {`LM32_DC_TMEM_ADDR_WIDTH{1'b0}})
state <= `LM32_DC_STATE_CHECK;
flush_set <= flush_set - 1'b1;
end
// Check for cache misses
`LM32_DC_STATE_CHECK:
begin
if (stall_a == `FALSE)
restart_request <= `FALSE;
if (miss == `TRUE)
begin
refill_request <= `TRUE;
refill_address <= address_m;
state <= `LM32_DC_STATE_REFILL;
end
else if (dflush == `TRUE)
state <= `LM32_DC_STATE_FLUSH;
end
// Refill a cache line
`LM32_DC_STATE_REFILL:
begin
refill_request <= `FALSE;
if (refill_ready == `TRUE)
begin
if (last_refill == `TRUE)
begin
restart_request <= `TRUE;
state <= `LM32_DC_STATE_CHECK;
end
end
end
endcase
end
end
generate
if (bytes_per_line > 4)
begin
// Refill offset
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
refill_offset <= {addr_offset_width{1'b0}};
else
begin
case (state)
// Check for cache misses
`LM32_DC_STATE_CHECK:
begin
if (miss == `TRUE)
refill_offset <= {addr_offset_width{1'b0}};
end
// Refill a cache line
`LM32_DC_STATE_REFILL:
begin
if (refill_ready == `TRUE)
refill_offset <= refill_offset + 1'b1;
end
endcase
end
end
end
endgenerate
endmodule
`endif
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_debug.v
// Title : Hardware debug registers and associated logic.
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : No Change
// Version : 3.2
// : Fixed simulation bug which flares up when number of
// : watchpoints is zero.
// =============================================================================
`include "lm32_include.v"
`ifdef CFG_DEBUG_ENABLED
// States for single-step FSM
`define LM32_DEBUG_SS_STATE_RNG 2:0
`define LM32_DEBUG_SS_STATE_IDLE 3'b000
`define LM32_DEBUG_SS_STATE_WAIT_FOR_RET 3'b001
`define LM32_DEBUG_SS_STATE_EXECUTE_ONE_INSN 3'b010
`define LM32_DEBUG_SS_STATE_RAISE_BREAKPOINT 3'b011
`define LM32_DEBUG_SS_STATE_RESTART 3'b100
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_debug (
// ----- Inputs -------
clk_i,
rst_i,
pc_x,
load_x,
store_x,
load_store_address_x,
csr_write_enable_x,
csr_write_data,
csr_x,
`ifdef CFG_HW_DEBUG_ENABLED
jtag_csr_write_enable,
jtag_csr_write_data,
jtag_csr,
`endif
`ifdef LM32_SINGLE_STEP_ENABLED
eret_q_x,
bret_q_x,
stall_x,
exception_x,
q_x,
`ifdef CFG_DCACHE_ENABLED
dcache_refill_request,
`endif
`endif
// ----- Outputs -------
`ifdef LM32_SINGLE_STEP_ENABLED
dc_ss,
`endif
dc_re,
bp_match,
wp_match
);
/////////////////////////////////////////////////////
// Parameters
/////////////////////////////////////////////////////
parameter breakpoints = 0; // Number of breakpoint CSRs
parameter watchpoints = 0; // Number of watchpoint CSRs
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
input rst_i; // Reset
input [`LM32_PC_RNG] pc_x; // X stage PC
input load_x; // Load instruction in X stage
input store_x; // Store instruction in X stage
input [`LM32_WORD_RNG] load_store_address_x; // Load or store effective address
input csr_write_enable_x; // wcsr instruction in X stage
input [`LM32_WORD_RNG] csr_write_data; // Data to write to CSR
input [`LM32_CSR_RNG] csr_x; // Which CSR to write
`ifdef CFG_HW_DEBUG_ENABLED
input jtag_csr_write_enable; // JTAG interface CSR write enable
input [`LM32_WORD_RNG] jtag_csr_write_data; // Data to write to CSR
input [`LM32_CSR_RNG] jtag_csr; // Which CSR to write
`endif
`ifdef LM32_SINGLE_STEP_ENABLED
input eret_q_x; // eret instruction in X stage
input bret_q_x; // bret instruction in X stage
input stall_x; // Instruction in X stage is stalled
input exception_x; // An exception has occured in X stage
input q_x; // Indicates the instruction in the X stage is qualified
`ifdef CFG_DCACHE_ENABLED
input dcache_refill_request; // Indicates data cache wants to be refilled
`endif
`endif
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
`ifdef LM32_SINGLE_STEP_ENABLED
output dc_ss; // Single-step enable
reg dc_ss;
`endif
output dc_re; // Remap exceptions
reg dc_re;
output bp_match; // Indicates a breakpoint has matched
wire bp_match;
output wp_match; // Indicates a watchpoint has matched
wire wp_match;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
genvar i; // Loop index for generate statements
// Debug CSRs
reg [`LM32_PC_RNG] bp_a[0:breakpoints-1]; // Instruction breakpoint address
reg bp_e[0:breakpoints-1]; // Instruction breakpoint enable
wire [0:breakpoints-1]bp_match_n; // Indicates if a h/w instruction breakpoint matched
reg [`LM32_WPC_C_RNG] wpc_c[0:watchpoints-1]; // Watchpoint enable
reg [`LM32_WORD_RNG] wp[0:watchpoints-1]; // Watchpoint address
wire [0:watchpoints-1]wp_match_n; // Indicates if a h/w data watchpoint matched
wire debug_csr_write_enable; // Debug CSR write enable (from either a wcsr instruction of external debugger)
wire [`LM32_WORD_RNG] debug_csr_write_data; // Data to write to debug CSR
wire [`LM32_CSR_RNG] debug_csr; // Debug CSR to write to
`ifdef LM32_SINGLE_STEP_ENABLED
// FIXME: Declaring this as a reg causes ModelSim 6.1.15b to crash, so use integer for now
reg [`LM32_DEBUG_SS_STATE_RNG] state; // State of single-step FSM
//integer state; // State of single-step FSM
`endif
/////////////////////////////////////////////////////
// Functions
/////////////////////////////////////////////////////
`include "lm32_functions.v"
/////////////////////////////////////////////////////
// Combinational Logic
/////////////////////////////////////////////////////
// Check for breakpoints
generate
for (i = 0; i < breakpoints; i = i + 1)
begin : bp_comb
assign bp_match_n[i] = ((bp_a[i] == pc_x) && (bp_e[i] == `TRUE));
end
endgenerate
generate
`ifdef LM32_SINGLE_STEP_ENABLED
if (breakpoints > 0)
assign bp_match = (|bp_match_n) || (state == `LM32_DEBUG_SS_STATE_RAISE_BREAKPOINT);
else
assign bp_match = state == `LM32_DEBUG_SS_STATE_RAISE_BREAKPOINT;
`else
if (breakpoints > 0)
assign bp_match = |bp_match_n;
else
assign bp_match = `FALSE;
`endif
endgenerate
// Check for watchpoints
generate
for (i = 0; i < watchpoints; i = i + 1)
begin : wp_comb
assign wp_match_n[i] = (wp[i] == load_store_address_x) && ((load_x & wpc_c[i][0]) | (store_x & wpc_c[i][1]));
end
endgenerate
generate
if (watchpoints > 0)
assign wp_match = |wp_match_n;
else
assign wp_match = `FALSE;
endgenerate
`ifdef CFG_HW_DEBUG_ENABLED
// Multiplex between wcsr instruction writes and debugger writes to the debug CSRs
assign debug_csr_write_enable = (csr_write_enable_x == `TRUE) || (jtag_csr_write_enable == `TRUE);
assign debug_csr_write_data = jtag_csr_write_enable == `TRUE ? jtag_csr_write_data : csr_write_data;
assign debug_csr = jtag_csr_write_enable == `TRUE ? jtag_csr : csr_x;
`else
assign debug_csr_write_enable = csr_write_enable_x;
assign debug_csr_write_data = csr_write_data;
assign debug_csr = csr_x;
`endif
/////////////////////////////////////////////////////
// Sequential Logic
/////////////////////////////////////////////////////
// Breakpoint address and enable CSRs
generate
for (i = 0; i < breakpoints; i = i + 1)
begin : bp_seq
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
bp_a[i] <= {`LM32_PC_WIDTH{1'bx}};
bp_e[i] <= `FALSE;
end
else
begin
if ((debug_csr_write_enable == `TRUE) && (debug_csr == `LM32_CSR_BP0 + i))
begin
bp_a[i] <= debug_csr_write_data[`LM32_PC_RNG];
bp_e[i] <= debug_csr_write_data[0];
end
end
end
end
endgenerate
// Watchpoint address and control flags CSRs
generate
for (i = 0; i < watchpoints; i = i + 1)
begin : wp_seq
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
wp[i] <= {`LM32_WORD_WIDTH{1'bx}};
wpc_c[i] <= `LM32_WPC_C_DISABLED;
end
else
begin
if (debug_csr_write_enable == `TRUE)
begin
if (debug_csr == `LM32_CSR_DC)
wpc_c[i] <= debug_csr_write_data[3+i*2:2+i*2];
if (debug_csr == `LM32_CSR_WP0 + i)
wp[i] <= debug_csr_write_data;
end
end
end
end
endgenerate
// Remap exceptions control bit
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
dc_re <= `FALSE;
else
begin
if ((debug_csr_write_enable == `TRUE) && (debug_csr == `LM32_CSR_DC))
dc_re <= debug_csr_write_data[1];
end
end
`ifdef LM32_SINGLE_STEP_ENABLED
// Single-step control flag
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
state <= `LM32_DEBUG_SS_STATE_IDLE;
dc_ss <= `FALSE;
end
else
begin
if ((debug_csr_write_enable == `TRUE) && (debug_csr == `LM32_CSR_DC))
begin
dc_ss <= debug_csr_write_data[0];
if (debug_csr_write_data[0] == `FALSE)
state <= `LM32_DEBUG_SS_STATE_IDLE;
else
state <= `LM32_DEBUG_SS_STATE_WAIT_FOR_RET;
end
case (state)
`LM32_DEBUG_SS_STATE_WAIT_FOR_RET:
begin
// Wait for eret or bret instruction to be executed
if ( ( (eret_q_x == `TRUE)
|| (bret_q_x == `TRUE)
)
&& (stall_x == `FALSE)
)
state <= `LM32_DEBUG_SS_STATE_EXECUTE_ONE_INSN;
end
`LM32_DEBUG_SS_STATE_EXECUTE_ONE_INSN:
begin
// Wait for an instruction to be executed
if ((q_x == `TRUE) && (stall_x == `FALSE))
state <= `LM32_DEBUG_SS_STATE_RAISE_BREAKPOINT;
end
`LM32_DEBUG_SS_STATE_RAISE_BREAKPOINT:
begin
// Wait for exception to be raised
`ifdef CFG_DCACHE_ENABLED
if (dcache_refill_request == `TRUE)
state <= `LM32_DEBUG_SS_STATE_EXECUTE_ONE_INSN;
else
`endif
if ((exception_x == `TRUE) && (q_x == `TRUE) && (stall_x == `FALSE))
begin
dc_ss <= `FALSE;
state <= `LM32_DEBUG_SS_STATE_RESTART;
end
end
`LM32_DEBUG_SS_STATE_RESTART:
begin
// Watch to see if stepped instruction is restarted due to a cache miss
`ifdef CFG_DCACHE_ENABLED
if (dcache_refill_request == `TRUE)
state <= `LM32_DEBUG_SS_STATE_EXECUTE_ONE_INSN;
else
`endif
state <= `LM32_DEBUG_SS_STATE_IDLE;
end
endcase
end
end
`endif
endmodule
`endif
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_decoder.v
// Title : Instruction decoder
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : Support for static branch prediction. Information about
// : branch type is generated and passed on to the predictor.
// Version : 3.2
// : No change
// Version : 3.3
// : Renamed port names that conflict with keywords reserved
// : in System-Verilog.
// =============================================================================
`include "lm32_include.v"
// Index of opcode field in an instruction
`define LM32_OPCODE_RNG 31:26
`define LM32_OP_RNG 30:26
// Opcodes - Some are only listed as 5 bits as their MSB is a don't care
`define LM32_OPCODE_ADD 5'b01101
`define LM32_OPCODE_AND 5'b01000
`define LM32_OPCODE_ANDHI 6'b011000
`define LM32_OPCODE_B 6'b110000
`define LM32_OPCODE_BI 6'b111000
`define LM32_OPCODE_BE 6'b010001
`define LM32_OPCODE_BG 6'b010010
`define LM32_OPCODE_BGE 6'b010011
`define LM32_OPCODE_BGEU 6'b010100
`define LM32_OPCODE_BGU 6'b010101
`define LM32_OPCODE_BNE 6'b010111
`define LM32_OPCODE_CALL 6'b110110
`define LM32_OPCODE_CALLI 6'b111110
`define LM32_OPCODE_CMPE 5'b11001
`define LM32_OPCODE_CMPG 5'b11010
`define LM32_OPCODE_CMPGE 5'b11011
`define LM32_OPCODE_CMPGEU 5'b11100
`define LM32_OPCODE_CMPGU 5'b11101
`define LM32_OPCODE_CMPNE 5'b11111
`define LM32_OPCODE_DIVU 6'b100011
`define LM32_OPCODE_LB 6'b000100
`define LM32_OPCODE_LBU 6'b010000
`define LM32_OPCODE_LH 6'b000111
`define LM32_OPCODE_LHU 6'b001011
`define LM32_OPCODE_LW 6'b001010
`define LM32_OPCODE_MODU 6'b110001
`define LM32_OPCODE_MUL 5'b00010
`define LM32_OPCODE_NOR 5'b00001
`define LM32_OPCODE_OR 5'b01110
`define LM32_OPCODE_ORHI 6'b011110
`define LM32_OPCODE_RAISE 6'b101011
`define LM32_OPCODE_RCSR 6'b100100
`define LM32_OPCODE_SB 6'b001100
`define LM32_OPCODE_SEXTB 6'b101100
`define LM32_OPCODE_SEXTH 6'b110111
`define LM32_OPCODE_SH 6'b000011
`define LM32_OPCODE_SL 5'b01111
`define LM32_OPCODE_SR 5'b00101
`define LM32_OPCODE_SRU 5'b00000
`define LM32_OPCODE_SUB 6'b110010
`define LM32_OPCODE_SW 6'b010110
`define LM32_OPCODE_USER 6'b110011
`define LM32_OPCODE_WCSR 6'b110100
`define LM32_OPCODE_XNOR 5'b01001
`define LM32_OPCODE_XOR 5'b00110
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_decoder (
// ----- Inputs -------
instruction,
// ----- Outputs -------
d_result_sel_0,
d_result_sel_1,
x_result_sel_csr,
`ifdef LM32_MC_ARITHMETIC_ENABLED
x_result_sel_mc_arith,
`endif
`ifdef LM32_NO_BARREL_SHIFT
x_result_sel_shift,
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
x_result_sel_sext,
`endif
x_result_sel_logic,
`ifdef CFG_USER_ENABLED
x_result_sel_user,
`endif
x_result_sel_add,
m_result_sel_compare,
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
m_result_sel_shift,
`endif
w_result_sel_load,
`ifdef CFG_PL_MULTIPLY_ENABLED
w_result_sel_mul,
`endif
x_bypass_enable,
m_bypass_enable,
read_enable_0,
read_idx_0,
read_enable_1,
read_idx_1,
write_enable,
write_idx,
immediate,
branch_offset,
load,
store,
size,
sign_extend,
adder_op,
logic_op,
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
direction,
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
shift_left,
shift_right,
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
multiply,
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
divide,
modulus,
`endif
branch,
branch_reg,
condition,
bi_conditional,
bi_unconditional,
`ifdef CFG_DEBUG_ENABLED
break_opcode,
`endif
scall,
eret,
`ifdef CFG_DEBUG_ENABLED
bret,
`endif
`ifdef CFG_USER_ENABLED
user_opcode,
`endif
csr_write_enable
);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input [`LM32_INSTRUCTION_RNG] instruction; // Instruction to decode
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
output [`LM32_D_RESULT_SEL_0_RNG] d_result_sel_0;
reg [`LM32_D_RESULT_SEL_0_RNG] d_result_sel_0;
output [`LM32_D_RESULT_SEL_1_RNG] d_result_sel_1;
reg [`LM32_D_RESULT_SEL_1_RNG] d_result_sel_1;
output x_result_sel_csr;
reg x_result_sel_csr;
`ifdef LM32_MC_ARITHMETIC_ENABLED
output x_result_sel_mc_arith;
reg x_result_sel_mc_arith;
`endif
`ifdef LM32_NO_BARREL_SHIFT
output x_result_sel_shift;
reg x_result_sel_shift;
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
output x_result_sel_sext;
reg x_result_sel_sext;
`endif
output x_result_sel_logic;
reg x_result_sel_logic;
`ifdef CFG_USER_ENABLED
output x_result_sel_user;
reg x_result_sel_user;
`endif
output x_result_sel_add;
reg x_result_sel_add;
output m_result_sel_compare;
reg m_result_sel_compare;
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
output m_result_sel_shift;
reg m_result_sel_shift;
`endif
output w_result_sel_load;
reg w_result_sel_load;
`ifdef CFG_PL_MULTIPLY_ENABLED
output w_result_sel_mul;
reg w_result_sel_mul;
`endif
output x_bypass_enable;
wire x_bypass_enable;
output m_bypass_enable;
wire m_bypass_enable;
output read_enable_0;
wire read_enable_0;
output [`LM32_REG_IDX_RNG] read_idx_0;
wire [`LM32_REG_IDX_RNG] read_idx_0;
output read_enable_1;
wire read_enable_1;
output [`LM32_REG_IDX_RNG] read_idx_1;
wire [`LM32_REG_IDX_RNG] read_idx_1;
output write_enable;
wire write_enable;
output [`LM32_REG_IDX_RNG] write_idx;
wire [`LM32_REG_IDX_RNG] write_idx;
output [`LM32_WORD_RNG] immediate;
wire [`LM32_WORD_RNG] immediate;
output [`LM32_PC_RNG] branch_offset;
wire [`LM32_PC_RNG] branch_offset;
output load;
wire load;
output store;
wire store;
output [`LM32_SIZE_RNG] size;
wire [`LM32_SIZE_RNG] size;
output sign_extend;
wire sign_extend;
output adder_op;
wire adder_op;
output [`LM32_LOGIC_OP_RNG] logic_op;
wire [`LM32_LOGIC_OP_RNG] logic_op;
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
output direction;
wire direction;
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
output shift_left;
wire shift_left;
output shift_right;
wire shift_right;
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
output multiply;
wire multiply;
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
output divide;
wire divide;
output modulus;
wire modulus;
`endif
output branch;
wire branch;
output branch_reg;
wire branch_reg;
output [`LM32_CONDITION_RNG] condition;
wire [`LM32_CONDITION_RNG] condition;
output bi_conditional;
wire bi_conditional;
output bi_unconditional;
wire bi_unconditional;
`ifdef CFG_DEBUG_ENABLED
output break_opcode;
wire break_opcode;
`endif
output scall;
wire scall;
output eret;
wire eret;
`ifdef CFG_DEBUG_ENABLED
output bret;
wire bret;
`endif
`ifdef CFG_USER_ENABLED
output [`LM32_USER_OPCODE_RNG] user_opcode;
wire [`LM32_USER_OPCODE_RNG] user_opcode;
`endif
output csr_write_enable;
wire csr_write_enable;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
wire [`LM32_WORD_RNG] extended_immediate; // Zero or sign extended immediate
wire [`LM32_WORD_RNG] high_immediate; // Immediate as high 16 bits
wire [`LM32_WORD_RNG] call_immediate; // Call immediate
wire [`LM32_WORD_RNG] branch_immediate; // Conditional branch immediate
wire sign_extend_immediate; // Whether the immediate should be sign extended (`TRUE) or zero extended (`FALSE)
wire select_high_immediate; // Whether to select the high immediate
wire select_call_immediate; // Whether to select the call immediate
wire op_add;
wire op_and;
wire op_andhi;
wire op_b;
wire op_bi;
wire op_be;
wire op_bg;
wire op_bge;
wire op_bgeu;
wire op_bgu;
wire op_bne;
wire op_call;
wire op_calli;
wire op_cmpe;
wire op_cmpg;
wire op_cmpge;
wire op_cmpgeu;
wire op_cmpgu;
wire op_cmpne;
`ifdef CFG_MC_DIVIDE_ENABLED
wire op_divu;
`endif
wire op_lb;
wire op_lbu;
wire op_lh;
wire op_lhu;
wire op_lw;
`ifdef CFG_MC_DIVIDE_ENABLED
wire op_modu;
`endif
`ifdef LM32_MULTIPLY_ENABLED
wire op_mul;
`endif
wire op_nor;
wire op_or;
wire op_orhi;
wire op_raise;
wire op_rcsr;
wire op_sb;
`ifdef CFG_SIGN_EXTEND_ENABLED
wire op_sextb;
wire op_sexth;
`endif
wire op_sh;
`ifdef LM32_BARREL_SHIFT_ENABLED
wire op_sl;
`endif
wire op_sr;
wire op_sru;
wire op_sub;
wire op_sw;
`ifdef CFG_USER_ENABLED
wire op_user;
`endif
wire op_wcsr;
wire op_xnor;
wire op_xor;
wire arith;
wire logical;
wire cmp;
wire bra;
wire call;
`ifdef LM32_BARREL_SHIFT_ENABLED
wire shift;
`endif
`ifdef LM32_NO_BARREL_SHIFT
wire shift;
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
wire sext;
`endif
/////////////////////////////////////////////////////
// Functions
/////////////////////////////////////////////////////
`include "lm32_functions.v"
/////////////////////////////////////////////////////
// Combinational logic
/////////////////////////////////////////////////////
// Determine opcode
assign op_add = instruction[`LM32_OP_RNG] == `LM32_OPCODE_ADD;
assign op_and = instruction[`LM32_OP_RNG] == `LM32_OPCODE_AND;
assign op_andhi = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_ANDHI;
assign op_b = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_B;
assign op_bi = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_BI;
assign op_be = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_BE;
assign op_bg = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_BG;
assign op_bge = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_BGE;
assign op_bgeu = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_BGEU;
assign op_bgu = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_BGU;
assign op_bne = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_BNE;
assign op_call = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_CALL;
assign op_calli = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_CALLI;
assign op_cmpe = instruction[`LM32_OP_RNG] == `LM32_OPCODE_CMPE;
assign op_cmpg = instruction[`LM32_OP_RNG] == `LM32_OPCODE_CMPG;
assign op_cmpge = instruction[`LM32_OP_RNG] == `LM32_OPCODE_CMPGE;
assign op_cmpgeu = instruction[`LM32_OP_RNG] == `LM32_OPCODE_CMPGEU;
assign op_cmpgu = instruction[`LM32_OP_RNG] == `LM32_OPCODE_CMPGU;
assign op_cmpne = instruction[`LM32_OP_RNG] == `LM32_OPCODE_CMPNE;
`ifdef CFG_MC_DIVIDE_ENABLED
assign op_divu = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_DIVU;
`endif
assign op_lb = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_LB;
assign op_lbu = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_LBU;
assign op_lh = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_LH;
assign op_lhu = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_LHU;
assign op_lw = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_LW;
`ifdef CFG_MC_DIVIDE_ENABLED
assign op_modu = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_MODU;
`endif
`ifdef LM32_MULTIPLY_ENABLED
assign op_mul = instruction[`LM32_OP_RNG] == `LM32_OPCODE_MUL;
`endif
assign op_nor = instruction[`LM32_OP_RNG] == `LM32_OPCODE_NOR;
assign op_or = instruction[`LM32_OP_RNG] == `LM32_OPCODE_OR;
assign op_orhi = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_ORHI;
assign op_raise = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_RAISE;
assign op_rcsr = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_RCSR;
assign op_sb = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_SB;
`ifdef CFG_SIGN_EXTEND_ENABLED
assign op_sextb = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_SEXTB;
assign op_sexth = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_SEXTH;
`endif
assign op_sh = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_SH;
`ifdef LM32_BARREL_SHIFT_ENABLED
assign op_sl = instruction[`LM32_OP_RNG] == `LM32_OPCODE_SL;
`endif
assign op_sr = instruction[`LM32_OP_RNG] == `LM32_OPCODE_SR;
assign op_sru = instruction[`LM32_OP_RNG] == `LM32_OPCODE_SRU;
assign op_sub = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_SUB;
assign op_sw = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_SW;
`ifdef CFG_USER_ENABLED
assign op_user = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_USER;
`endif
assign op_wcsr = instruction[`LM32_OPCODE_RNG] == `LM32_OPCODE_WCSR;
assign op_xnor = instruction[`LM32_OP_RNG] == `LM32_OPCODE_XNOR;
assign op_xor = instruction[`LM32_OP_RNG] == `LM32_OPCODE_XOR;
// Group opcodes by function
assign arith = op_add | op_sub;
assign logical = op_and | op_andhi | op_nor | op_or | op_orhi | op_xor | op_xnor;
assign cmp = op_cmpe | op_cmpg | op_cmpge | op_cmpgeu | op_cmpgu | op_cmpne;
assign bi_conditional = op_be | op_bg | op_bge | op_bgeu | op_bgu | op_bne;
assign bi_unconditional = op_bi;
assign bra = op_b | bi_unconditional | bi_conditional;
assign call = op_call | op_calli;
`ifdef LM32_BARREL_SHIFT_ENABLED
assign shift = op_sl | op_sr | op_sru;
`endif
`ifdef LM32_NO_BARREL_SHIFT
assign shift = op_sr | op_sru;
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
assign shift_left = op_sl;
assign shift_right = op_sr | op_sru;
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
assign sext = op_sextb | op_sexth;
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
assign multiply = op_mul;
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
assign divide = op_divu;
assign modulus = op_modu;
`endif
assign load = op_lb | op_lbu | op_lh | op_lhu | op_lw;
assign store = op_sb | op_sh | op_sw;
// Select pipeline multiplexor controls
always @(*)
begin
// D stage
if (call)
d_result_sel_0 = `LM32_D_RESULT_SEL_0_NEXT_PC;
else
d_result_sel_0 = `LM32_D_RESULT_SEL_0_REG_0;
if (call)
d_result_sel_1 = `LM32_D_RESULT_SEL_1_ZERO;
else if ((instruction[31] == 1'b0) && !bra)
d_result_sel_1 = `LM32_D_RESULT_SEL_1_IMMEDIATE;
else
d_result_sel_1 = `LM32_D_RESULT_SEL_1_REG_1;
// X stage
x_result_sel_csr = `FALSE;
`ifdef LM32_MC_ARITHMETIC_ENABLED
x_result_sel_mc_arith = `FALSE;
`endif
`ifdef LM32_NO_BARREL_SHIFT
x_result_sel_shift = `FALSE;
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
x_result_sel_sext = `FALSE;
`endif
x_result_sel_logic = `FALSE;
`ifdef CFG_USER_ENABLED
x_result_sel_user = `FALSE;
`endif
x_result_sel_add = `FALSE;
if (op_rcsr)
x_result_sel_csr = `TRUE;
`ifdef LM32_MC_ARITHMETIC_ENABLED
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
else if (shift_left | shift_right)
x_result_sel_mc_arith = `TRUE;
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
else if (divide | modulus)
x_result_sel_mc_arith = `TRUE;
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
else if (multiply)
x_result_sel_mc_arith = `TRUE;
`endif
`endif
`ifdef LM32_NO_BARREL_SHIFT
else if (shift)
x_result_sel_shift = `TRUE;
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
else if (sext)
x_result_sel_sext = `TRUE;
`endif
else if (logical)
x_result_sel_logic = `TRUE;
`ifdef CFG_USER_ENABLED
else if (op_user)
x_result_sel_user = `TRUE;
`endif
else
x_result_sel_add = `TRUE;
// M stage
m_result_sel_compare = cmp;
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
m_result_sel_shift = shift;
`endif
// W stage
w_result_sel_load = load;
`ifdef CFG_PL_MULTIPLY_ENABLED
w_result_sel_mul = op_mul;
`endif
end
// Set if result is valid at end of X stage
assign x_bypass_enable = arith
| logical
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
| shift_left
| shift_right
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
| multiply
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
| divide
| modulus
`endif
`ifdef LM32_NO_BARREL_SHIFT
| shift
`endif
`ifdef CFG_SIGN_EXTEND_ENABLED
| sext
`endif
`ifdef CFG_USER_ENABLED
| op_user
`endif
| op_rcsr
;
// Set if result is valid at end of M stage
assign m_bypass_enable = x_bypass_enable
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
| shift
`endif
| cmp
;
// Register file read port 0
assign read_enable_0 = ~(op_bi | op_calli);
assign read_idx_0 = instruction[25:21];
// Register file read port 1
assign read_enable_1 = ~(op_bi | op_calli | load);
assign read_idx_1 = instruction[20:16];
// Register file write port
assign write_enable = ~(bra | op_raise | store | op_wcsr);
assign write_idx = call
? 5'd29
: instruction[31] == 1'b0
? instruction[20:16]
: instruction[15:11];
// Size of load/stores
assign size = instruction[27:26];
// Whether to sign or zero extend
assign sign_extend = instruction[28];
// Set adder_op to 1 to perform a subtraction
assign adder_op = op_sub | op_cmpe | op_cmpg | op_cmpge | op_cmpgeu | op_cmpgu | op_cmpne | bra;
// Logic operation (and, or, etc)
assign logic_op = instruction[29:26];
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
// Shift direction
assign direction = instruction[29];
`endif
// Control flow microcodes
assign branch = bra | call;
assign branch_reg = op_call | op_b;
assign condition = instruction[28:26];
`ifdef CFG_DEBUG_ENABLED
assign break_opcode = op_raise & ~instruction[2];
`endif
assign scall = op_raise & instruction[2];
assign eret = op_b & (instruction[25:21] == 5'd30);
`ifdef CFG_DEBUG_ENABLED
assign bret = op_b & (instruction[25:21] == 5'd31);
`endif
`ifdef CFG_USER_ENABLED
// Extract user opcode
assign user_opcode = instruction[10:0];
`endif
// CSR read/write
assign csr_write_enable = op_wcsr;
// Extract immediate from instruction
assign sign_extend_immediate = ~(op_and | op_cmpgeu | op_cmpgu | op_nor | op_or | op_xnor | op_xor);
assign select_high_immediate = op_andhi | op_orhi;
assign select_call_immediate = instruction[31];
assign high_immediate = {instruction[15:0], 16'h0000};
assign extended_immediate = {{16{sign_extend_immediate & instruction[15]}}, instruction[15:0]};
assign call_immediate = {{6{instruction[25]}}, instruction[25:0]};
assign branch_immediate = {{16{instruction[15]}}, instruction[15:0]};
assign immediate = select_high_immediate == `TRUE
? high_immediate
: extended_immediate;
assign branch_offset = select_call_immediate == `TRUE
? (call_immediate[`LM32_PC_WIDTH-1:0])
: (branch_immediate[`LM32_PC_WIDTH-1:0]);
endmodule
module lm32_dp_ram(
clk_i,
rst_i,
we_i,
waddr_i,
wdata_i,
raddr_i,
rdata_o);
parameter addr_width = 32;
parameter addr_depth = 1024;
parameter data_width = 8;
input clk_i;
input rst_i;
input we_i;
input [addr_width-1:0] waddr_i;
input [data_width-1:0] wdata_i;
input [addr_width-1:0] raddr_i;
output [data_width-1:0] rdata_o;
reg [data_width-1:0] ram[addr_depth-1:0];
reg [addr_width-1:0] raddr_r;
assign rdata_o = ram[raddr_r];
integer i;
initial begin
for (i=0;i<=addr_depth-1;i=i+1)
ram[i] = 0;
end
always @ (posedge clk_i)
begin
if (we_i)
ram[waddr_i] <= wdata_i;
raddr_r <= raddr_i;
end
endmodule
-- Work-alike to lm32_dp_ram.v, but using generic_simple_dpram
library ieee;
use ieee.std_logic_1164.all;
library work;
use work.genram_pkg.all;
entity lm32_dp_ram is
generic(
addr_width : natural := 32;
addr_depth : natural := 1024;
data_width : natural := 8);
port(
clk_i : in std_logic;
rst_i : in std_logic;
we_i : in std_logic;
waddr_i : in std_logic_vector(addr_width-1 downto 0);
wdata_i : in std_logic_vector(data_width-1 downto 0);
raddr_i : in std_logic_vector(addr_width-1 downto 0);
rdata_o : out std_logic_vector(data_width-1 downto 0));
end lm32_dp_ram;
architecture syn of lm32_dp_ram is
constant c_addr_width : natural := f_log2_size(addr_depth);
begin
ram : generic_simple_dpram
generic map(
g_data_width => data_width,
g_size => addr_depth,
g_with_byte_enable => false,
g_addr_conflict_resolution => "write_first",
g_dual_clock => false)
port map(
clka_i => clk_i,
wea_i => we_i,
aa_i => waddr_i(c_addr_width-1 downto 0),
da_i => wdata_i,
clkb_i => clk_i,
ab_i => raddr_i(c_addr_width-1 downto 0),
qb_o => rdata_o);
end syn;
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_functions.v
// Title : Common functions
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.5
// : Added function to generate log-of-two that rounds-up to
// : power-of-two
// =============================================================================
function integer clogb2;
input [31:0] value;
begin
for (clogb2 = 0; value > 0; clogb2 = clogb2 + 1)
value = value >> 1;
end
endfunction
function integer clogb2_v1;
input [31:0] value;
reg [31:0] i;
reg [31:0] temp;
begin
temp = 0;
i = 0;
for (i = 0; temp < value; i = i + 1)
temp = 1<<i;
clogb2_v1 = i-1;
end
endfunction
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_icache.v
// Title : Instruction cache
// Dependencies : lm32_include.v
//
// Version 3.5
// 1. Bug Fix: Instruction cache flushes issued from Instruction Inline Memory
// cause segmentation fault due to incorrect fetches.
//
// Version 3.1
// 1. Feature: Support for user-selected resource usage when implementing
// cache memory. Additional parameters must be defined when invoking module
// lm32_ram. Instruction cache miss mechanism is dependent on branch
// prediction being performed in D stage of pipeline.
//
// Version 7.0SP2, 3.0
// No change
// =============================================================================
`include "lm32_include.v"
`ifdef CFG_ICACHE_ENABLED
`define LM32_IC_ADDR_OFFSET_RNG addr_offset_msb:addr_offset_lsb
`define LM32_IC_ADDR_SET_RNG addr_set_msb:addr_set_lsb
`define LM32_IC_ADDR_TAG_RNG addr_tag_msb:addr_tag_lsb
`define LM32_IC_ADDR_IDX_RNG addr_set_msb:addr_offset_lsb
`define LM32_IC_TMEM_ADDR_WIDTH addr_set_width
`define LM32_IC_TMEM_ADDR_RNG (`LM32_IC_TMEM_ADDR_WIDTH-1):0
`define LM32_IC_DMEM_ADDR_WIDTH (addr_offset_width+addr_set_width)
`define LM32_IC_DMEM_ADDR_RNG (`LM32_IC_DMEM_ADDR_WIDTH-1):0
`define LM32_IC_TAGS_WIDTH (addr_tag_width+1)
`define LM32_IC_TAGS_RNG (`LM32_IC_TAGS_WIDTH-1):0
`define LM32_IC_TAGS_TAG_RNG (`LM32_IC_TAGS_WIDTH-1):1
`define LM32_IC_TAGS_VALID_RNG 0
`define LM32_IC_STATE_RNG 3:0
`define LM32_IC_STATE_FLUSH_INIT 4'b0001
`define LM32_IC_STATE_FLUSH 4'b0010
`define LM32_IC_STATE_CHECK 4'b0100
`define LM32_IC_STATE_REFILL 4'b1000
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_icache (
// ----- Inputs -----
clk_i,
rst_i,
stall_a,
stall_f,
address_a,
address_f,
read_enable_f,
refill_ready,
refill_data,
iflush,
`ifdef CFG_IROM_ENABLED
select_f,
`endif
valid_d,
branch_predict_taken_d,
// ----- Outputs -----
stall_request,
restart_request,
refill_request,
refill_address,
refilling,
inst
);
/////////////////////////////////////////////////////
// Parameters
/////////////////////////////////////////////////////
parameter associativity = 1; // Associativity of the cache (Number of ways)
parameter sets = 512; // Number of sets
parameter bytes_per_line = 16; // Number of bytes per cache line
parameter base_address = 0; // Base address of cachable memory
parameter limit = 0; // Limit (highest address) of cachable memory
localparam addr_offset_width = clogb2(bytes_per_line)-1-2;
localparam addr_set_width = clogb2(sets)-1;
localparam addr_offset_lsb = 2;
localparam addr_offset_msb = (addr_offset_lsb+addr_offset_width-1);
localparam addr_set_lsb = (addr_offset_msb+1);
localparam addr_set_msb = (addr_set_lsb+addr_set_width-1);
localparam addr_tag_lsb = (addr_set_msb+1);
localparam addr_tag_msb = clogb2(`CFG_ICACHE_LIMIT-`CFG_ICACHE_BASE_ADDRESS)-1;
localparam addr_tag_width = (addr_tag_msb-addr_tag_lsb+1);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
input rst_i; // Reset
input stall_a; // Stall instruction in A stage
input stall_f; // Stall instruction in F stage
input valid_d; // Valid instruction in D stage
input branch_predict_taken_d; // Instruction in D stage is a branch and is predicted taken
input [`LM32_PC_RNG] address_a; // Address of instruction in A stage
input [`LM32_PC_RNG] address_f; // Address of instruction in F stage
input read_enable_f; // Indicates if cache access is valid
input refill_ready; // Next word of refill data is ready
input [`LM32_INSTRUCTION_RNG] refill_data; // Data to refill the cache with
input iflush; // Flush the cache
`ifdef CFG_IROM_ENABLED
input select_f; // Instruction in F stage is mapped through instruction cache
`endif
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
output stall_request; // Request to stall the pipeline
wire stall_request;
output restart_request; // Request to restart instruction that caused the cache miss
reg restart_request;
output refill_request; // Request to refill a cache line
wire refill_request;
output [`LM32_PC_RNG] refill_address; // Base address of cache refill
reg [`LM32_PC_RNG] refill_address;
output refilling; // Indicates the instruction cache is currently refilling
reg refilling;
output [`LM32_INSTRUCTION_RNG] inst; // Instruction read from cache
wire [`LM32_INSTRUCTION_RNG] inst;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
wire enable;
wire [0:associativity-1] way_mem_we;
wire [`LM32_INSTRUCTION_RNG] way_data[0:associativity-1];
wire [`LM32_IC_TAGS_TAG_RNG] way_tag[0:associativity-1];
wire [0:associativity-1] way_valid;
wire [0:associativity-1] way_match;
wire miss;
wire [`LM32_IC_TMEM_ADDR_RNG] tmem_read_address;
wire [`LM32_IC_TMEM_ADDR_RNG] tmem_write_address;
wire [`LM32_IC_DMEM_ADDR_RNG] dmem_read_address;
wire [`LM32_IC_DMEM_ADDR_RNG] dmem_write_address;
wire [`LM32_IC_TAGS_RNG] tmem_write_data;
reg [`LM32_IC_STATE_RNG] state;
wire flushing;
wire check;
wire refill;
reg [associativity-1:0] refill_way_select;
reg [`LM32_IC_ADDR_OFFSET_RNG] refill_offset;
wire last_refill;
reg [`LM32_IC_TMEM_ADDR_RNG] flush_set;
genvar i;
/////////////////////////////////////////////////////
// Functions
/////////////////////////////////////////////////////
`include "lm32_functions.v"
/////////////////////////////////////////////////////
// Instantiations
/////////////////////////////////////////////////////
generate
for (i = 0; i < associativity; i = i + 1)
begin : memories
lm32_ram
#(
// ----- Parameters -------
.data_width (32),
.address_width (`LM32_IC_DMEM_ADDR_WIDTH)
// Modified for Milkymist: removed non-portable RAM parameters
)
way_0_data_ram
(
// ----- Inputs -------
.read_clk (clk_i),
.write_clk (clk_i),
.reset (rst_i),
.read_address (dmem_read_address),
.enable_read (enable),
.write_address (dmem_write_address),
.enable_write (`TRUE),
.write_enable (way_mem_we[i]),
.write_data (refill_data),
// ----- Outputs -------
.read_data (way_data[i])
);
lm32_ram
#(
// ----- Parameters -------
.data_width (`LM32_IC_TAGS_WIDTH),
.address_width (`LM32_IC_TMEM_ADDR_WIDTH)
// Modified for Milkymist: removed non-portable RAM parameters
)
way_0_tag_ram
(
// ----- Inputs -------
.read_clk (clk_i),
.write_clk (clk_i),
.reset (rst_i),
.read_address (tmem_read_address),
.enable_read (enable),
.write_address (tmem_write_address),
.enable_write (`TRUE),
.write_enable (way_mem_we[i] | flushing),
.write_data (tmem_write_data),
// ----- Outputs -------
.read_data ({way_tag[i], way_valid[i]})
);
end
endgenerate
/////////////////////////////////////////////////////
// Combinational logic
/////////////////////////////////////////////////////
// Compute which ways in the cache match the address address being read
generate
for (i = 0; i < associativity; i = i + 1)
begin : match
assign way_match[i] = ({way_tag[i], way_valid[i]} == {address_f[`LM32_IC_ADDR_TAG_RNG], `TRUE});
end
endgenerate
// Select data from way that matched the address being read
generate
if (associativity == 1)
begin : inst_1
assign inst = way_match[0] ? way_data[0] : 32'b0;
end
else if (associativity == 2)
begin : inst_2
assign inst = way_match[0] ? way_data[0] : (way_match[1] ? way_data[1] : 32'b0);
end
endgenerate
// Compute address to use to index into the data memories
generate
if (bytes_per_line > 4)
assign dmem_write_address = {refill_address[`LM32_IC_ADDR_SET_RNG], refill_offset};
else
assign dmem_write_address = refill_address[`LM32_IC_ADDR_SET_RNG];
endgenerate
assign dmem_read_address = address_a[`LM32_IC_ADDR_IDX_RNG];
// Compute address to use to index into the tag memories
assign tmem_read_address = address_a[`LM32_IC_ADDR_SET_RNG];
assign tmem_write_address = flushing
? flush_set
: refill_address[`LM32_IC_ADDR_SET_RNG];
// Compute signal to indicate when we are on the last refill accesses
generate
if (bytes_per_line > 4)
assign last_refill = refill_offset == {addr_offset_width{1'b1}};
else
assign last_refill = `TRUE;
endgenerate
// Compute data and tag memory access enable
assign enable = (stall_a == `FALSE);
// Compute data and tag memory write enables
generate
if (associativity == 1)
begin : we_1
assign way_mem_we[0] = (refill_ready == `TRUE);
end
else
begin : we_2
assign way_mem_we[0] = (refill_ready == `TRUE) && (refill_way_select[0] == `TRUE);
assign way_mem_we[1] = (refill_ready == `TRUE) && (refill_way_select[1] == `TRUE);
end
endgenerate
// On the last refill cycle set the valid bit, for all other writes it should be cleared
assign tmem_write_data[`LM32_IC_TAGS_VALID_RNG] = last_refill & !flushing;
assign tmem_write_data[`LM32_IC_TAGS_TAG_RNG] = refill_address[`LM32_IC_ADDR_TAG_RNG];
// Signals that indicate which state we are in
assign flushing = |state[1:0];
assign check = state[2];
assign refill = state[3];
assign miss = (~(|way_match)) && (read_enable_f == `TRUE) && (stall_f == `FALSE) && !(valid_d && branch_predict_taken_d);
assign stall_request = (check == `FALSE);
assign refill_request = (refill == `TRUE);
/////////////////////////////////////////////////////
// Sequential logic
/////////////////////////////////////////////////////
// Record way selected for replacement on a cache miss
generate
if (associativity >= 2)
begin : way_select
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
refill_way_select <= {{associativity-1{1'b0}}, 1'b1};
else
begin
if (miss == `TRUE)
refill_way_select <= {refill_way_select[0], refill_way_select[1]};
end
end
end
endgenerate
// Record whether we are refilling
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
refilling <= `FALSE;
else
refilling <= refill;
end
// Instruction cache control FSM
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
state <= `LM32_IC_STATE_FLUSH_INIT;
flush_set <= {`LM32_IC_TMEM_ADDR_WIDTH{1'b1}};
refill_address <= {`LM32_PC_WIDTH{1'bx}};
restart_request <= `FALSE;
end
else
begin
case (state)
// Flush the cache for the first time after reset
`LM32_IC_STATE_FLUSH_INIT:
begin
if (flush_set == {`LM32_IC_TMEM_ADDR_WIDTH{1'b0}})
state <= `LM32_IC_STATE_CHECK;
flush_set <= flush_set - 1'b1;
end
// Flush the cache in response to an write to the ICC CSR
`LM32_IC_STATE_FLUSH:
begin
if (flush_set == {`LM32_IC_TMEM_ADDR_WIDTH{1'b0}})
`ifdef CFG_IROM_ENABLED
if (select_f)
state <= `LM32_IC_STATE_REFILL;
else
`endif
state <= `LM32_IC_STATE_CHECK;
flush_set <= flush_set - 1'b1;
end
// Check for cache misses
`LM32_IC_STATE_CHECK:
begin
if (stall_a == `FALSE)
restart_request <= `FALSE;
if (iflush == `TRUE)
begin
refill_address <= address_f;
state <= `LM32_IC_STATE_FLUSH;
end
else if (miss == `TRUE)
begin
refill_address <= address_f;
state <= `LM32_IC_STATE_REFILL;
end
end
// Refill a cache line
`LM32_IC_STATE_REFILL:
begin
if (refill_ready == `TRUE)
begin
if (last_refill == `TRUE)
begin
restart_request <= `TRUE;
state <= `LM32_IC_STATE_CHECK;
end
end
end
endcase
end
end
generate
if (bytes_per_line > 4)
begin
// Refill offset
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
refill_offset <= {addr_offset_width{1'b0}};
else
begin
case (state)
// Check for cache misses
`LM32_IC_STATE_CHECK:
begin
if (iflush == `TRUE)
refill_offset <= {addr_offset_width{1'b0}};
else if (miss == `TRUE)
refill_offset <= {addr_offset_width{1'b0}};
end
// Refill a cache line
`LM32_IC_STATE_REFILL:
begin
if (refill_ready == `TRUE)
refill_offset <= refill_offset + 1'b1;
end
endcase
end
end
end
endgenerate
endmodule
`endif
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_include.v
// Title : CPU global macros
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : No Change
// Version : 3.2
// : No Change
// Version : 3.3
// : Support for extended configuration register
// =============================================================================
`ifdef LM32_INCLUDE_V
`else
`define LM32_INCLUDE_V
// Configuration options
//`include "system_conf.v"
`ifdef TRUE
`else
`define TRUE 1'b1
`define FALSE 1'b0
`define TRUE_N 1'b0
`define FALSE_N 1'b1
`endif
// Wishbone configuration
`define CFG_IWB_ENABLED
`define CFG_DWB_ENABLED
// Data-path width
`define LM32_WORD_WIDTH 32
`define LM32_WORD_RNG (`LM32_WORD_WIDTH-1):0
`define LM32_SHIFT_WIDTH 5
`define LM32_SHIFT_RNG (`LM32_SHIFT_WIDTH-1):0
`define LM32_BYTE_SELECT_WIDTH 4
`define LM32_BYTE_SELECT_RNG (`LM32_BYTE_SELECT_WIDTH-1):0
// Register file size
`define LM32_REGISTERS 32
`define LM32_REG_IDX_WIDTH 5
`define LM32_REG_IDX_RNG (`LM32_REG_IDX_WIDTH-1):0
// Standard register numbers
`define LM32_RA_REG `LM32_REG_IDX_WIDTH'd29
`define LM32_EA_REG `LM32_REG_IDX_WIDTH'd30
`define LM32_BA_REG `LM32_REG_IDX_WIDTH'd31
// Range of Program Counter. Two LSBs are always 0.
`ifdef CFG_ICACHE_ENABLED
`define LM32_PC_WIDTH (clogb2(`CFG_ICACHE_LIMIT-`CFG_ICACHE_BASE_ADDRESS)-2)
`else
`ifdef CFG_IWB_ENABLED
`define LM32_PC_WIDTH (`LM32_WORD_WIDTH-2)
`else
`define LM32_PC_WIDTH `LM32_IROM_ADDRESS_WIDTH
`endif
`endif
`define LM32_PC_RNG (`LM32_PC_WIDTH+2-1):2
// Range of an instruction
`define LM32_INSTRUCTION_WIDTH 32
`define LM32_INSTRUCTION_RNG (`LM32_INSTRUCTION_WIDTH-1):0
// Adder operation
`define LM32_ADDER_OP_ADD 1'b0
`define LM32_ADDER_OP_SUBTRACT 1'b1
// Shift direction
`define LM32_SHIFT_OP_RIGHT 1'b0
`define LM32_SHIFT_OP_LEFT 1'b1
// Derive macro that indicates whether we have single-stepping or not
`ifdef CFG_ROM_DEBUG_ENABLED
`define LM32_SINGLE_STEP_ENABLED
`else
`ifdef CFG_HW_DEBUG_ENABLED
`define LM32_SINGLE_STEP_ENABLED
`endif
`endif
// Derive macro that indicates whether JTAG interface is required
`ifdef CFG_JTAG_UART_ENABLED
`define LM32_JTAG_ENABLED
`else
`ifdef CFG_DEBUG_ENABLED
`define LM32_JTAG_ENABLED
`else
`endif
`endif
// Derive macro that indicates whether we have a barrel-shifter or not
`ifdef CFG_PL_BARREL_SHIFT_ENABLED
`define LM32_BARREL_SHIFT_ENABLED
`else // CFG_PL_BARREL_SHIFT_ENABLED
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
`define LM32_BARREL_SHIFT_ENABLED
`else
`define LM32_NO_BARREL_SHIFT
`endif
`endif // CFG_PL_BARREL_SHIFT_ENABLED
// Derive macro that indicates whether we have a multiplier or not
`ifdef CFG_PL_MULTIPLY_ENABLED
`define LM32_MULTIPLY_ENABLED
`else
`ifdef CFG_MC_MULTIPLY_ENABLED
`define LM32_MULTIPLY_ENABLED
`endif
`endif
// Derive a macro that indicates whether or not the multi-cycle arithmetic unit is required
`ifdef CFG_MC_DIVIDE_ENABLED
`define LM32_MC_ARITHMETIC_ENABLED
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
`define LM32_MC_ARITHMETIC_ENABLED
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
`define LM32_MC_ARITHMETIC_ENABLED
`endif
// Derive macro that indicates if we are using an EBR register file
`ifdef CFG_EBR_POSEDGE_REGISTER_FILE
`define LM32_EBR_REGISTER_FILE
`endif
`ifdef CFG_EBR_NEGEDGE_REGISTER_FILE
`define LM32_EBR_REGISTER_FILE
`endif
// Revision number
`define LM32_REVISION 6'h02
// Logical operations - Function encoded directly in instruction
`define LM32_LOGIC_OP_RNG 3:0
// Conditions for conditional branches
`define LM32_CONDITION_WIDTH 3
`define LM32_CONDITION_RNG (`LM32_CONDITION_WIDTH-1):0
`define LM32_CONDITION_E 3'b001
`define LM32_CONDITION_G 3'b010
`define LM32_CONDITION_GE 3'b011
`define LM32_CONDITION_GEU 3'b100
`define LM32_CONDITION_GU 3'b101
`define LM32_CONDITION_NE 3'b111
`define LM32_CONDITION_U1 3'b000
`define LM32_CONDITION_U2 3'b110
// Size of load or store instruction - Encoding corresponds to opcode
`define LM32_SIZE_WIDTH 2
`define LM32_SIZE_RNG 1:0
`define LM32_SIZE_BYTE 2'b00
`define LM32_SIZE_HWORD 2'b11
`define LM32_SIZE_WORD 2'b10
`define LM32_ADDRESS_LSBS_WIDTH 2
// Width and range of a CSR index
`ifdef CFG_DEBUG_ENABLED
`define LM32_CSR_WIDTH 5
`define LM32_CSR_RNG (`LM32_CSR_WIDTH-1):0
`else
`ifdef CFG_JTAG_ENABLED
`define LM32_CSR_WIDTH 4
`define LM32_CSR_RNG (`LM32_CSR_WIDTH-1):0
`else
`define LM32_CSR_WIDTH 4 // CFG2 is "a"
`define LM32_CSR_RNG (`LM32_CSR_WIDTH-1):0
`endif
`endif
// CSR indices
`define LM32_CSR_IE `LM32_CSR_WIDTH'h0
`define LM32_CSR_IM `LM32_CSR_WIDTH'h1
`define LM32_CSR_IP `LM32_CSR_WIDTH'h2
`define LM32_CSR_ICC `LM32_CSR_WIDTH'h3
`define LM32_CSR_DCC `LM32_CSR_WIDTH'h4
`define LM32_CSR_CC `LM32_CSR_WIDTH'h5
`define LM32_CSR_CFG `LM32_CSR_WIDTH'h6
`define LM32_CSR_EBA `LM32_CSR_WIDTH'h7
`ifdef CFG_DEBUG_ENABLED
`define LM32_CSR_DC `LM32_CSR_WIDTH'h8
`define LM32_CSR_DEBA `LM32_CSR_WIDTH'h9
`endif
`define LM32_CSR_CFG2 `LM32_CSR_WIDTH'ha
`ifdef CFG_JTAG_ENABLED
`define LM32_CSR_JTX `LM32_CSR_WIDTH'he
`define LM32_CSR_JRX `LM32_CSR_WIDTH'hf
`endif
`ifdef CFG_DEBUG_ENABLED
`define LM32_CSR_BP0 `LM32_CSR_WIDTH'h10
`define LM32_CSR_BP1 `LM32_CSR_WIDTH'h11
`define LM32_CSR_BP2 `LM32_CSR_WIDTH'h12
`define LM32_CSR_BP3 `LM32_CSR_WIDTH'h13
`define LM32_CSR_WP0 `LM32_CSR_WIDTH'h18
`define LM32_CSR_WP1 `LM32_CSR_WIDTH'h19
`define LM32_CSR_WP2 `LM32_CSR_WIDTH'h1a
`define LM32_CSR_WP3 `LM32_CSR_WIDTH'h1b
`endif
// Values for WPC CSR
`define LM32_WPC_C_RNG 1:0
`define LM32_WPC_C_DISABLED 2'b00
`define LM32_WPC_C_READ 2'b01
`define LM32_WPC_C_WRITE 2'b10
`define LM32_WPC_C_READ_WRITE 2'b11
// Exception IDs
`define LM32_EID_WIDTH 3
`define LM32_EID_RNG (`LM32_EID_WIDTH-1):0
`define LM32_EID_RESET 3'h0
`define LM32_EID_BREAKPOINT 3'd1
`define LM32_EID_INST_BUS_ERROR 3'h2
`define LM32_EID_WATCHPOINT 3'd3
`define LM32_EID_DATA_BUS_ERROR 3'h4
`define LM32_EID_DIVIDE_BY_ZERO 3'h5
`define LM32_EID_INTERRUPT 3'h6
`define LM32_EID_SCALL 3'h7
// Pipeline result selection mux controls
`define LM32_D_RESULT_SEL_0_RNG 0:0
`define LM32_D_RESULT_SEL_0_REG_0 1'b0
`define LM32_D_RESULT_SEL_0_NEXT_PC 1'b1
`define LM32_D_RESULT_SEL_1_RNG 1:0
`define LM32_D_RESULT_SEL_1_ZERO 2'b00
`define LM32_D_RESULT_SEL_1_REG_1 2'b01
`define LM32_D_RESULT_SEL_1_IMMEDIATE 2'b10
`define LM32_USER_OPCODE_WIDTH 11
`define LM32_USER_OPCODE_RNG (`LM32_USER_OPCODE_WIDTH-1):0
// Derive a macro to indicate if either of the caches are implemented
`ifdef CFG_ICACHE_ENABLED
`define LM32_CACHE_ENABLED
`else
`ifdef CFG_DCACHE_ENABLED
`define LM32_CACHE_ENABLED
`endif
`endif
/////////////////////////////////////////////////////
// Interrupts
/////////////////////////////////////////////////////
// Currently this is fixed to 32 and should not be changed
`define CFG_INTERRUPTS 32
`define LM32_INTERRUPT_WIDTH `CFG_INTERRUPTS
`define LM32_INTERRUPT_RNG (`LM32_INTERRUPT_WIDTH-1):0
/////////////////////////////////////////////////////
// General
/////////////////////////////////////////////////////
// Sub-word range types
`define LM32_BYTE_WIDTH 8
`define LM32_BYTE_RNG 7:0
`define LM32_HWORD_WIDTH 16
`define LM32_HWORD_RNG 15:0
// Word sub-byte indicies
`define LM32_BYTE_0_RNG 7:0
`define LM32_BYTE_1_RNG 15:8
`define LM32_BYTE_2_RNG 23:16
`define LM32_BYTE_3_RNG 31:24
// Word sub-halfword indices
`define LM32_HWORD_0_RNG 15:0
`define LM32_HWORD_1_RNG 31:16
// Use a synchronous reset
`define CFG_RESET_SENSITIVITY
// Wishbone defines
// Refer to Wishbone System-on-Chip Interconnection Architecture
// These should probably be moved to a Wishbone common file
// Wishbone cycle types
`define LM32_CTYPE_WIDTH 3
`define LM32_CTYPE_RNG (`LM32_CTYPE_WIDTH-1):0
`define LM32_CTYPE_CLASSIC 3'b000
`define LM32_CTYPE_CONSTANT 3'b001
`define LM32_CTYPE_INCREMENTING 3'b010
`define LM32_CTYPE_END 3'b111
// Wishbone burst types
`define LM32_BTYPE_WIDTH 2
`define LM32_BTYPE_RNG (`LM32_BTYPE_WIDTH-1):0
`define LM32_BTYPE_LINEAR 2'b00
`define LM32_BTYPE_4_BEAT 2'b01
`define LM32_BTYPE_8_BEAT 2'b10
`define LM32_BTYPE_16_BEAT 2'b11
`endif
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_instruction_unit.v
// Title : Instruction unit
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : Support for static branch prediction is added. Fetching of
// : instructions can also be altered by branches predicted in D
// : stage of pipeline, and mispredicted branches in the X and M
// : stages of the pipeline.
// Version : 3.2
// : EBRs use SYNC resets instead of ASYNC resets.
// Version : 3.3
// : Support for a non-cacheable Instruction Memory that has a
// : single-cycle access latency. This memory can be accessed by
// : data port of LM32 (so that debugger has access to it).
// Version : 3.4
// : No change
// Version : 3.5
// : Bug fix: Inline memory is correctly generated if it is not a
// : power-of-two.
// : Bug fix: Fixed a bug that caused LM32 (configured without
// : instruction cache) to lock up in to an infinite loop due to a
// : instruction bus error when EBA was set to instruction inline
// : memory.
// =============================================================================
`include "lm32_include.v"
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_instruction_unit (
// ----- Inputs -------
clk_i,
rst_i,
// From pipeline
stall_a,
stall_f,
stall_d,
stall_x,
stall_m,
valid_f,
valid_d,
kill_f,
branch_predict_taken_d,
branch_predict_address_d,
`ifdef CFG_FAST_UNCONDITIONAL_BRANCH
branch_taken_x,
branch_target_x,
`endif
exception_m,
branch_taken_m,
branch_mispredict_taken_m,
branch_target_m,
`ifdef CFG_ICACHE_ENABLED
iflush,
`endif
`ifdef CFG_DCACHE_ENABLED
dcache_restart_request,
dcache_refill_request,
dcache_refilling,
`endif
`ifdef CFG_IROM_ENABLED
irom_store_data_m,
irom_address_xm,
irom_we_xm,
`endif
`ifdef CFG_IWB_ENABLED
// From Wishbone
i_dat_i,
i_ack_i,
i_err_i,
i_rty_i,
`endif
`ifdef CFG_HW_DEBUG_ENABLED
jtag_read_enable,
jtag_write_enable,
jtag_write_data,
jtag_address,
`endif
// ----- Outputs -------
// To pipeline
pc_f,
pc_d,
pc_x,
pc_m,
pc_w,
`ifdef CFG_ICACHE_ENABLED
icache_stall_request,
icache_restart_request,
icache_refill_request,
icache_refilling,
`endif
`ifdef CFG_IROM_ENABLED
irom_data_m,
`endif
`ifdef CFG_IWB_ENABLED
// To Wishbone
i_dat_o,
i_adr_o,
i_cyc_o,
i_sel_o,
i_stb_o,
i_we_o,
i_cti_o,
i_lock_o,
i_bte_o,
`endif
`ifdef CFG_HW_DEBUG_ENABLED
jtag_read_data,
jtag_access_complete,
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
bus_error_d,
`endif
`ifdef CFG_EBR_POSEDGE_REGISTER_FILE
instruction_f,
`endif
instruction_d
);
/////////////////////////////////////////////////////
// Parameters
/////////////////////////////////////////////////////
parameter eba_reset = `CFG_EBA_RESET; // Reset value for EBA CSR
parameter associativity = 1; // Associativity of the cache (Number of ways)
parameter sets = 512; // Number of sets
parameter bytes_per_line = 16; // Number of bytes per cache line
parameter base_address = 0; // Base address of cachable memory
parameter limit = 0; // Limit (highest address) of cachable memory
// For bytes_per_line == 4, we set 1 so part-select range isn't reversed, even though not really used
localparam eba_reset_minus_4 = eba_reset - 4;
localparam addr_offset_width = bytes_per_line == 4 ? 1 : clogb2(bytes_per_line)-1-2;
localparam addr_offset_lsb = 2;
localparam addr_offset_msb = (addr_offset_lsb+addr_offset_width-1);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
input rst_i; // Reset
input stall_a; // Stall A stage instruction
input stall_f; // Stall F stage instruction
input stall_d; // Stall D stage instruction
input stall_x; // Stall X stage instruction
input stall_m; // Stall M stage instruction
input valid_f; // Instruction in F stage is valid
input valid_d; // Instruction in D stage is valid
input kill_f; // Kill instruction in F stage
input branch_predict_taken_d; // Branch is predicted taken in D stage
input [`LM32_PC_RNG] branch_predict_address_d; // Branch target address
`ifdef CFG_FAST_UNCONDITIONAL_BRANCH
input branch_taken_x; // Branch instruction in X stage is taken
input [`LM32_PC_RNG] branch_target_x; // Target PC of X stage branch instruction
`endif
input exception_m;
input branch_taken_m; // Branch instruction in M stage is taken
input branch_mispredict_taken_m; // Branch instruction in M stage is mispredicted as taken
input [`LM32_PC_RNG] branch_target_m; // Target PC of M stage branch instruction
`ifdef CFG_ICACHE_ENABLED
input iflush; // Flush instruction cache
`endif
`ifdef CFG_DCACHE_ENABLED
input dcache_restart_request; // Restart instruction that caused a data cache miss
input dcache_refill_request; // Request to refill data cache
input dcache_refilling;
`endif
`ifdef CFG_IROM_ENABLED
input [`LM32_WORD_RNG] irom_store_data_m; // Data from load-store unit
input [`LM32_WORD_RNG] irom_address_xm; // Address from load-store unit
input irom_we_xm; // Indicates if memory operation is load or store
`endif
`ifdef CFG_IWB_ENABLED
input [`LM32_WORD_RNG] i_dat_i; // Instruction Wishbone interface read data
input i_ack_i; // Instruction Wishbone interface acknowledgement
input i_err_i; // Instruction Wishbone interface error
input i_rty_i; // Instruction Wishbone interface retry
`endif
`ifdef CFG_HW_DEBUG_ENABLED
input jtag_read_enable; // JTAG read memory request
input jtag_write_enable; // JTAG write memory request
input [`LM32_BYTE_RNG] jtag_write_data; // JTAG wrirte data
input [`LM32_WORD_RNG] jtag_address; // JTAG read/write address
`endif
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
output [`LM32_PC_RNG] pc_f; // F stage PC
reg [`LM32_PC_RNG] pc_f;
output [`LM32_PC_RNG] pc_d; // D stage PC
reg [`LM32_PC_RNG] pc_d;
output [`LM32_PC_RNG] pc_x; // X stage PC
reg [`LM32_PC_RNG] pc_x;
output [`LM32_PC_RNG] pc_m; // M stage PC
reg [`LM32_PC_RNG] pc_m;
output [`LM32_PC_RNG] pc_w; // W stage PC
reg [`LM32_PC_RNG] pc_w;
`ifdef CFG_ICACHE_ENABLED
output icache_stall_request; // Instruction cache stall request
wire icache_stall_request;
output icache_restart_request; // Request to restart instruction that cached instruction cache miss
wire icache_restart_request;
output icache_refill_request; // Instruction cache refill request
wire icache_refill_request;
output icache_refilling; // Indicates the icache is refilling
wire icache_refilling;
`endif
`ifdef CFG_IROM_ENABLED
output [`LM32_WORD_RNG] irom_data_m; // Data to load-store unit on load
wire [`LM32_WORD_RNG] irom_data_m;
`endif
`ifdef CFG_IWB_ENABLED
output [`LM32_WORD_RNG] i_dat_o; // Instruction Wishbone interface write data
`ifdef CFG_HW_DEBUG_ENABLED
reg [`LM32_WORD_RNG] i_dat_o;
`else
wire [`LM32_WORD_RNG] i_dat_o;
`endif
output [`LM32_WORD_RNG] i_adr_o; // Instruction Wishbone interface address
reg [`LM32_WORD_RNG] i_adr_o;
output i_cyc_o; // Instruction Wishbone interface cycle
reg i_cyc_o;
output [`LM32_BYTE_SELECT_RNG] i_sel_o; // Instruction Wishbone interface byte select
`ifdef CFG_HW_DEBUG_ENABLED
reg [`LM32_BYTE_SELECT_RNG] i_sel_o;
`else
wire [`LM32_BYTE_SELECT_RNG] i_sel_o;
`endif
output i_stb_o; // Instruction Wishbone interface strobe
reg i_stb_o;
output i_we_o; // Instruction Wishbone interface write enable
`ifdef CFG_HW_DEBUG_ENABLED
reg i_we_o;
`else
wire i_we_o;
`endif
output [`LM32_CTYPE_RNG] i_cti_o; // Instruction Wishbone interface cycle type
reg [`LM32_CTYPE_RNG] i_cti_o;
output i_lock_o; // Instruction Wishbone interface lock bus
reg i_lock_o;
output [`LM32_BTYPE_RNG] i_bte_o; // Instruction Wishbone interface burst type
wire [`LM32_BTYPE_RNG] i_bte_o;
`endif
`ifdef CFG_HW_DEBUG_ENABLED
output [`LM32_BYTE_RNG] jtag_read_data; // Data read for JTAG interface
reg [`LM32_BYTE_RNG] jtag_read_data;
output jtag_access_complete; // Requested memory access by JTAG interface is complete
wire jtag_access_complete;
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
output bus_error_d; // Indicates a bus error occured while fetching the instruction
reg bus_error_d;
`endif
`ifdef CFG_EBR_POSEDGE_REGISTER_FILE
output [`LM32_INSTRUCTION_RNG] instruction_f; // F stage instruction (only to have register indices extracted from)
wire [`LM32_INSTRUCTION_RNG] instruction_f;
`endif
output [`LM32_INSTRUCTION_RNG] instruction_d; // D stage instruction to be decoded
reg [`LM32_INSTRUCTION_RNG] instruction_d;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
reg [`LM32_PC_RNG] pc_a; // A stage PC
`ifdef LM32_CACHE_ENABLED
reg [`LM32_PC_RNG] restart_address; // Address to restart from after a cache miss
`endif
`ifdef CFG_ICACHE_ENABLED
wire icache_read_enable_f; // Indicates if instruction cache miss is valid
wire [`LM32_PC_RNG] icache_refill_address; // Address that caused cache miss
reg icache_refill_ready; // Indicates when next word of refill data is ready to be written to cache
reg [`LM32_INSTRUCTION_RNG] icache_refill_data; // Next word of refill data, fetched from Wishbone
wire [`LM32_INSTRUCTION_RNG] icache_data_f; // Instruction fetched from instruction cache
wire [`LM32_CTYPE_RNG] first_cycle_type; // First Wishbone cycle type
wire [`LM32_CTYPE_RNG] next_cycle_type; // Next Wishbone cycle type
wire last_word; // Indicates if this is the last word in the cache line
wire [`LM32_PC_RNG] first_address; // First cache refill address
`else
`ifdef CFG_IWB_ENABLED
reg [`LM32_INSTRUCTION_RNG] wb_data_f; // Instruction fetched from Wishbone
`endif
`endif
`ifdef CFG_IROM_ENABLED
wire irom_select_a; // Indicates if A stage PC maps to a ROM address
reg irom_select_f; // Indicates if F stage PC maps to a ROM address
wire [`LM32_INSTRUCTION_RNG] irom_data_f; // Instruction fetched from ROM
`endif
`ifdef CFG_EBR_POSEDGE_REGISTER_FILE
`else
wire [`LM32_INSTRUCTION_RNG] instruction_f; // F stage instruction
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
reg bus_error_f; // Indicates if a bus error occured while fetching the instruction in the F stage
`endif
`ifdef CFG_HW_DEBUG_ENABLED
reg jtag_access; // Indicates if a JTAG WB access is in progress
`endif
/////////////////////////////////////////////////////
// Functions
/////////////////////////////////////////////////////
`include "lm32_functions.v"
/////////////////////////////////////////////////////
// Instantiations
/////////////////////////////////////////////////////
// Instruction ROM
`ifdef CFG_IROM_ENABLED
pmi_ram_dp_true
#(
// ----- Parameters -------
.pmi_family (`LATTICE_FAMILY),
//.pmi_addr_depth_a (1 << (clogb2(`CFG_IROM_LIMIT/4-`CFG_IROM_BASE_ADDRESS/4+1)-1)),
//.pmi_addr_width_a ((clogb2(`CFG_IROM_LIMIT/4-`CFG_IROM_BASE_ADDRESS/4+1)-1)),
//.pmi_data_width_a (`LM32_WORD_WIDTH),
//.pmi_addr_depth_b (1 << (clogb2(`CFG_IROM_LIMIT/4-`CFG_IROM_BASE_ADDRESS/4+1)-1)),
//.pmi_addr_width_b ((clogb2(`CFG_IROM_LIMIT/4-`CFG_IROM_BASE_ADDRESS/4+1)-1)),
//.pmi_data_width_b (`LM32_WORD_WIDTH),
.pmi_addr_depth_a (`CFG_IROM_LIMIT/4-`CFG_IROM_BASE_ADDRESS/4+1),
.pmi_addr_width_a (clogb2_v1(`CFG_IROM_LIMIT/4-`CFG_IROM_BASE_ADDRESS/4+1)),
.pmi_data_width_a (`LM32_WORD_WIDTH),
.pmi_addr_depth_b (`CFG_IROM_LIMIT/4-`CFG_IROM_BASE_ADDRESS/4+1),
.pmi_addr_width_b (clogb2_v1(`CFG_IROM_LIMIT/4-`CFG_IROM_BASE_ADDRESS/4+1)),
.pmi_data_width_b (`LM32_WORD_WIDTH),
.pmi_regmode_a ("noreg"),
.pmi_regmode_b ("noreg"),
.pmi_gsr ("enable"),
.pmi_resetmode ("sync"),
.pmi_init_file (`CFG_IROM_INIT_FILE),
.pmi_init_file_format (`CFG_IROM_INIT_FILE_FORMAT),
.module_type ("pmi_ram_dp_true")
)
ram (
// ----- Inputs -------
.ClockA (clk_i),
.ClockB (clk_i),
.ResetA (rst_i),
.ResetB (rst_i),
.DataInA ({32{1'b0}}),
.DataInB (irom_store_data_m),
.AddressA (pc_a[(clogb2(`CFG_IROM_LIMIT/4-`CFG_IROM_BASE_ADDRESS/4+1)-1)+2-1:2]),
.AddressB (irom_address_xm[(clogb2(`CFG_IROM_LIMIT/4-`CFG_IROM_BASE_ADDRESS/4+1)-1)+2-1:2]),
.ClockEnA (!stall_a),
.ClockEnB (!stall_x || !stall_m),
.WrA (`FALSE),
.WrB (irom_we_xm),
// ----- Outputs -------
.QA (irom_data_f),
.QB (irom_data_m)
);
`endif
`ifdef CFG_ICACHE_ENABLED
// Instruction cache
lm32_icache #(
.associativity (associativity),
.sets (sets),
.bytes_per_line (bytes_per_line),
.base_address (base_address),
.limit (limit)
) icache (
// ----- Inputs -----
.clk_i (clk_i),
.rst_i (rst_i),
.stall_a (stall_a),
.stall_f (stall_f),
.branch_predict_taken_d (branch_predict_taken_d),
.valid_d (valid_d),
.address_a (pc_a),
.address_f (pc_f),
.read_enable_f (icache_read_enable_f),
.refill_ready (icache_refill_ready),
.refill_data (icache_refill_data),
.iflush (iflush),
// ----- Outputs -----
.stall_request (icache_stall_request),
.restart_request (icache_restart_request),
.refill_request (icache_refill_request),
.refill_address (icache_refill_address),
.refilling (icache_refilling),
.inst (icache_data_f)
);
`endif
/////////////////////////////////////////////////////
// Combinational Logic
/////////////////////////////////////////////////////
`ifdef CFG_ICACHE_ENABLED
// Generate signal that indicates when instruction cache misses are valid
assign icache_read_enable_f = (valid_f == `TRUE)
&& (kill_f == `FALSE)
`ifdef CFG_DCACHE_ENABLED
&& (dcache_restart_request == `FALSE)
`endif
`ifdef CFG_IROM_ENABLED
&& (irom_select_f == `FALSE)
`endif
;
`endif
// Compute address of next instruction to fetch
always @(*)
begin
// The request from the latest pipeline stage must take priority
`ifdef CFG_DCACHE_ENABLED
if (dcache_restart_request == `TRUE)
pc_a = restart_address;
else
`endif
if (branch_taken_m == `TRUE)
if ((branch_mispredict_taken_m == `TRUE) && (exception_m == `FALSE))
pc_a = pc_x;
else
pc_a = branch_target_m;
`ifdef CFG_FAST_UNCONDITIONAL_BRANCH
else if (branch_taken_x == `TRUE)
pc_a = branch_target_x;
`endif
else
if ( (valid_d == `TRUE) && (branch_predict_taken_d == `TRUE) )
pc_a = branch_predict_address_d;
else
`ifdef CFG_ICACHE_ENABLED
if (icache_restart_request == `TRUE)
pc_a = restart_address;
else
`endif
pc_a = pc_f + 1'b1;
end
// Select where instruction should be fetched from
`ifdef CFG_IROM_ENABLED
assign irom_select_a = ({pc_a, 2'b00} >= `CFG_IROM_BASE_ADDRESS) && ({pc_a, 2'b00} <= `CFG_IROM_LIMIT);
`endif
// Select instruction from selected source
`ifdef CFG_ICACHE_ENABLED
`ifdef CFG_IROM_ENABLED
assign instruction_f = irom_select_f == `TRUE ? irom_data_f : icache_data_f;
`else
assign instruction_f = icache_data_f;
`endif
`else
`ifdef CFG_IROM_ENABLED
`ifdef CFG_IWB_ENABLED
assign instruction_f = irom_select_f == `TRUE ? irom_data_f : wb_data_f;
`else
assign instruction_f = irom_data_f;
`endif
`else
assign instruction_f = wb_data_f;
`endif
`endif
// Unused/constant Wishbone signals
`ifdef CFG_IWB_ENABLED
`ifdef CFG_HW_DEBUG_ENABLED
`else
assign i_dat_o = 32'd0;
assign i_we_o = `FALSE;
assign i_sel_o = 4'b1111;
`endif
assign i_bte_o = `LM32_BTYPE_LINEAR;
`endif
`ifdef CFG_ICACHE_ENABLED
// Determine parameters for next cache refill Wishbone access
generate
case (bytes_per_line)
4:
begin
assign first_cycle_type = `LM32_CTYPE_END;
assign next_cycle_type = `LM32_CTYPE_END;
assign last_word = `TRUE;
assign first_address = icache_refill_address;
end
8:
begin
assign first_cycle_type = `LM32_CTYPE_INCREMENTING;
assign next_cycle_type = `LM32_CTYPE_END;
assign last_word = i_adr_o[addr_offset_msb:addr_offset_lsb] == 1'b1;
assign first_address = {icache_refill_address[`LM32_PC_WIDTH+2-1:addr_offset_msb+1], {addr_offset_width{1'b0}}};
end
16:
begin
assign first_cycle_type = `LM32_CTYPE_INCREMENTING;
assign next_cycle_type = i_adr_o[addr_offset_msb] == 1'b1 ? `LM32_CTYPE_END : `LM32_CTYPE_INCREMENTING;
assign last_word = i_adr_o[addr_offset_msb:addr_offset_lsb] == 2'b11;
assign first_address = {icache_refill_address[`LM32_PC_WIDTH+2-1:addr_offset_msb+1], {addr_offset_width{1'b0}}};
end
endcase
endgenerate
`endif
/////////////////////////////////////////////////////
// Sequential Logic
/////////////////////////////////////////////////////
// PC
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
pc_f <= eba_reset_minus_4[`LM32_PC_RNG];
pc_d <= {`LM32_PC_WIDTH{1'b0}};
pc_x <= {`LM32_PC_WIDTH{1'b0}};
pc_m <= {`LM32_PC_WIDTH{1'b0}};
pc_w <= {`LM32_PC_WIDTH{1'b0}};
end
else
begin
if (stall_f == `FALSE)
pc_f <= pc_a;
if (stall_d == `FALSE)
pc_d <= pc_f;
if (stall_x == `FALSE)
pc_x <= pc_d;
if (stall_m == `FALSE)
pc_m <= pc_x;
pc_w <= pc_m;
end
end
`ifdef LM32_CACHE_ENABLED
// Address to restart from after a cache miss has been handled
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
restart_address <= {`LM32_PC_WIDTH{1'b0}};
else
begin
`ifdef CFG_DCACHE_ENABLED
`ifdef CFG_ICACHE_ENABLED
// D-cache restart address must take priority, otherwise instructions will be lost
if (dcache_refill_request == `TRUE)
restart_address <= pc_w;
else if ((icache_refill_request == `TRUE) && (!dcache_refilling) && (!dcache_restart_request))
restart_address <= icache_refill_address;
`else
if (dcache_refill_request == `TRUE)
restart_address <= pc_w;
`endif
`else
`ifdef CFG_ICACHE_ENABLED
if (icache_refill_request == `TRUE)
restart_address <= icache_refill_address;
`endif
`endif
end
end
`endif
// Record where instruction was fetched from
`ifdef CFG_IROM_ENABLED
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
irom_select_f <= `FALSE;
else
begin
if (stall_f == `FALSE)
irom_select_f <= irom_select_a;
end
end
`endif
`ifdef CFG_HW_DEBUG_ENABLED
assign jtag_access_complete = (i_cyc_o == `TRUE) && ((i_ack_i == `TRUE) || (i_err_i == `TRUE)) && (jtag_access == `TRUE);
always @(*)
begin
case (jtag_address[1:0])
2'b00: jtag_read_data = i_dat_i[`LM32_BYTE_3_RNG];
2'b01: jtag_read_data = i_dat_i[`LM32_BYTE_2_RNG];
2'b10: jtag_read_data = i_dat_i[`LM32_BYTE_1_RNG];
2'b11: jtag_read_data = i_dat_i[`LM32_BYTE_0_RNG];
endcase
end
`endif
`ifdef CFG_IWB_ENABLED
// Instruction Wishbone interface
`ifdef CFG_ICACHE_ENABLED
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
i_cyc_o <= `FALSE;
i_stb_o <= `FALSE;
i_adr_o <= {`LM32_WORD_WIDTH{1'b0}};
i_cti_o <= `LM32_CTYPE_END;
i_lock_o <= `FALSE;
icache_refill_data <= {`LM32_INSTRUCTION_WIDTH{1'b0}};
icache_refill_ready <= `FALSE;
`ifdef CFG_BUS_ERRORS_ENABLED
bus_error_f <= `FALSE;
`endif
`ifdef CFG_HW_DEBUG_ENABLED
i_we_o <= `FALSE;
i_sel_o <= 4'b1111;
jtag_access <= `FALSE;
`endif
end
else
begin
icache_refill_ready <= `FALSE;
// Is a cycle in progress?
if (i_cyc_o == `TRUE)
begin
// Has cycle completed?
if ((i_ack_i == `TRUE) || (i_err_i == `TRUE))
begin
`ifdef CFG_HW_DEBUG_ENABLED
if (jtag_access == `TRUE)
begin
i_cyc_o <= `FALSE;
i_stb_o <= `FALSE;
i_we_o <= `FALSE;
jtag_access <= `FALSE;
end
else
`endif
begin
if (last_word == `TRUE)
begin
// Cache line fill complete
i_cyc_o <= `FALSE;
i_stb_o <= `FALSE;
i_lock_o <= `FALSE;
end
// Fetch next word in cache line
i_adr_o[addr_offset_msb:addr_offset_lsb] <= i_adr_o[addr_offset_msb:addr_offset_lsb] + 1'b1;
i_cti_o <= next_cycle_type;
// Write fetched data into instruction cache
icache_refill_ready <= `TRUE;
icache_refill_data <= i_dat_i;
end
end
`ifdef CFG_BUS_ERRORS_ENABLED
if (i_err_i == `TRUE)
begin
bus_error_f <= `TRUE;
$display ("Instruction bus error. Address: %x", i_adr_o);
end
`endif
end
else
begin
if ((icache_refill_request == `TRUE) && (icache_refill_ready == `FALSE))
begin
// Read first word of cache line
`ifdef CFG_HW_DEBUG_ENABLED
i_sel_o <= 4'b1111;
`endif
i_adr_o <= {first_address, 2'b00};
i_cyc_o <= `TRUE;
i_stb_o <= `TRUE;
i_cti_o <= first_cycle_type;
//i_lock_o <= `TRUE;
`ifdef CFG_BUS_ERRORS_ENABLED
bus_error_f <= `FALSE;
`endif
end
`ifdef CFG_HW_DEBUG_ENABLED
else
begin
if ((jtag_read_enable == `TRUE) || (jtag_write_enable == `TRUE))
begin
case (jtag_address[1:0])
2'b00: i_sel_o <= 4'b1000;
2'b01: i_sel_o <= 4'b0100;
2'b10: i_sel_o <= 4'b0010;
2'b11: i_sel_o <= 4'b0001;
endcase
i_adr_o <= jtag_address;
i_dat_o <= {4{jtag_write_data}};
i_cyc_o <= `TRUE;
i_stb_o <= `TRUE;
i_we_o <= jtag_write_enable;
i_cti_o <= `LM32_CTYPE_END;
jtag_access <= `TRUE;
end
end
`endif
`ifdef CFG_BUS_ERRORS_ENABLED
// Clear bus error when exception taken, otherwise they would be
// continually generated if exception handler is cached
`ifdef CFG_FAST_UNCONDITIONAL_BRANCH
if (branch_taken_x == `TRUE)
bus_error_f <= `FALSE;
`endif
if (branch_taken_m == `TRUE)
bus_error_f <= `FALSE;
`endif
end
end
end
`else
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
i_cyc_o <= `FALSE;
i_stb_o <= `FALSE;
i_adr_o <= {`LM32_WORD_WIDTH{1'b0}};
i_cti_o <= `LM32_CTYPE_END;
i_lock_o <= `FALSE;
wb_data_f <= {`LM32_INSTRUCTION_WIDTH{1'b0}};
`ifdef CFG_BUS_ERRORS_ENABLED
bus_error_f <= `FALSE;
`endif
end
else
begin
// Is a cycle in progress?
if (i_cyc_o == `TRUE)
begin
// Has cycle completed?
if((i_ack_i == `TRUE) || (i_err_i == `TRUE))
begin
// Cycle complete
i_cyc_o <= `FALSE;
i_stb_o <= `FALSE;
// Register fetched instruction
wb_data_f <= i_dat_i;
end
`ifdef CFG_BUS_ERRORS_ENABLED
if (i_err_i == `TRUE)
begin
bus_error_f <= `TRUE;
$display ("Instruction bus error. Address: %x", i_adr_o);
end
`endif
end
else
begin
// Wait for an instruction fetch from an external address
if ( (stall_a == `FALSE)
`ifdef CFG_IROM_ENABLED
&& (irom_select_a == `FALSE)
`endif
)
begin
// Fetch instruction
`ifdef CFG_HW_DEBUG_ENABLED
i_sel_o <= 4'b1111;
`endif
i_adr_o <= {pc_a, 2'b00};
i_cyc_o <= `TRUE;
i_stb_o <= `TRUE;
`ifdef CFG_BUS_ERRORS_ENABLED
bus_error_f <= `FALSE;
`endif
end
else
begin
if ( (stall_a == `FALSE)
`ifdef CFG_IROM_ENABLED
&& (irom_select_a == `TRUE)
`endif
)
begin
`ifdef CFG_BUS_ERRORS_ENABLED
bus_error_f <= `FALSE;
`endif
end
end
end
end
end
`endif
`endif
// Instruction register
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
instruction_d <= {`LM32_INSTRUCTION_WIDTH{1'b0}};
`ifdef CFG_BUS_ERRORS_ENABLED
bus_error_d <= `FALSE;
`endif
end
else
begin
if (stall_d == `FALSE)
begin
instruction_d <= instruction_f;
`ifdef CFG_BUS_ERRORS_ENABLED
bus_error_d <= bus_error_f;
`endif
end
end
end
endmodule
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_interrupt.v
// Title : Interrupt logic
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : No Change
// =============================================================================
`include "lm32_include.v"
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_interrupt (
// ----- Inputs -------
clk_i,
rst_i,
// From external devices
interrupt,
// From pipeline
stall_x,
`ifdef CFG_DEBUG_ENABLED
non_debug_exception,
debug_exception,
`else
exception,
`endif
eret_q_x,
`ifdef CFG_DEBUG_ENABLED
bret_q_x,
`endif
csr,
csr_write_data,
csr_write_enable,
// ----- Outputs -------
interrupt_exception,
// To pipeline
csr_read_data
);
/////////////////////////////////////////////////////
// Parameters
/////////////////////////////////////////////////////
parameter interrupts = `CFG_INTERRUPTS; // Number of interrupts
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
input rst_i; // Reset
input [interrupts-1:0] interrupt; // Interrupt pins, active-low
input stall_x; // Stall X pipeline stage
`ifdef CFG_DEBUG_ENABLED
input non_debug_exception; // Non-debug related exception has been raised
input debug_exception; // Debug-related exception has been raised
`else
input exception; // Exception has been raised
`endif
input eret_q_x; // Return from exception
`ifdef CFG_DEBUG_ENABLED
input bret_q_x; // Return from breakpoint
`endif
input [`LM32_CSR_RNG] csr; // CSR read/write index
input [`LM32_WORD_RNG] csr_write_data; // Data to write to specified CSR
input csr_write_enable; // CSR write enable
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
output interrupt_exception; // Request to raide an interrupt exception
wire interrupt_exception;
output [`LM32_WORD_RNG] csr_read_data; // Data read from CSR
reg [`LM32_WORD_RNG] csr_read_data;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
wire [interrupts-1:0] asserted; // Which interrupts are currently being asserted
//p_ragma attribute asserted preserve_signal true
wire [interrupts-1:0] interrupt_n_exception;
// Interrupt CSRs
reg ie; // Interrupt enable
reg eie; // Exception interrupt enable
`ifdef CFG_DEBUG_ENABLED
reg bie; // Breakpoint interrupt enable
`endif
reg [interrupts-1:0] ip; // Interrupt pending
reg [interrupts-1:0] im; // Interrupt mask
/////////////////////////////////////////////////////
// Combinational Logic
/////////////////////////////////////////////////////
// Determine which interrupts have occured and are unmasked
assign interrupt_n_exception = ip & im;
// Determine if any unmasked interrupts have occured
assign interrupt_exception = (|interrupt_n_exception) & ie;
// Determine which interrupts are currently being asserted (active-low) or are already pending
assign asserted = ip | interrupt;
generate
if (interrupts > 1)
begin
// CSR read
always @(*)
begin
case (csr)
`LM32_CSR_IE: csr_read_data = {{`LM32_WORD_WIDTH-3{1'b0}},
`ifdef CFG_DEBUG_ENABLED
bie,
`else
1'b0,
`endif
eie,
ie
};
`LM32_CSR_IP: csr_read_data = ip;
`LM32_CSR_IM: csr_read_data = im;
default: csr_read_data = {`LM32_WORD_WIDTH{1'bx}};
endcase
end
end
else
begin
// CSR read
always @(*)
begin
case (csr)
`LM32_CSR_IE: csr_read_data = {{`LM32_WORD_WIDTH-3{1'b0}},
`ifdef CFG_DEBUG_ENABLED
bie,
`else
1'b0,
`endif
eie,
ie
};
`LM32_CSR_IP: csr_read_data = ip;
default: csr_read_data = {`LM32_WORD_WIDTH{1'bx}};
endcase
end
end
endgenerate
/////////////////////////////////////////////////////
// Sequential Logic
/////////////////////////////////////////////////////
`define IE_DELAY 10
reg [`IE_DELAY:0] eie_delay = 0;
generate
if (interrupts > 1)
begin
// IE, IM, IP - Interrupt Enable, Interrupt Mask and Interrupt Pending CSRs
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
ie <= `FALSE;
eie <= `FALSE;
`ifdef CFG_DEBUG_ENABLED
bie <= `FALSE;
`endif
im <= {interrupts{1'b0}};
ip <= {interrupts{1'b0}};
eie_delay <= 0;
end
else
begin
// Set IP bit when interrupt line is asserted
ip <= asserted;
`ifdef CFG_DEBUG_ENABLED
if (non_debug_exception == `TRUE)
begin
// Save and then clear interrupt enable
eie <= ie;
ie <= `FALSE;
end
else if (debug_exception == `TRUE)
begin
// Save and then clear interrupt enable
bie <= ie;
ie <= `FALSE;
end
`else
if (exception == `TRUE)
begin
// Save and then clear interrupt enable
eie <= ie;
ie <= `FALSE;
end
`endif
else if (stall_x == `FALSE)
begin
if(eie_delay[0])
ie <= eie;
eie_delay <= {1'b0, eie_delay[`IE_DELAY:1]};
if (eret_q_x == `TRUE) begin
// Restore interrupt enable
eie_delay[`IE_DELAY] <= `TRUE;
eie_delay[`IE_DELAY-1:0] <= 0;
end
`ifdef CFG_DEBUG_ENABLED
else if (bret_q_x == `TRUE)
// Restore interrupt enable
ie <= bie;
`endif
else if (csr_write_enable == `TRUE)
begin
// Handle wcsr write
if (csr == `LM32_CSR_IE)
begin
ie <= csr_write_data[0];
eie <= csr_write_data[1];
`ifdef CFG_DEBUG_ENABLED
bie <= csr_write_data[2];
`endif
end
if (csr == `LM32_CSR_IM)
im <= csr_write_data[interrupts-1:0];
if (csr == `LM32_CSR_IP)
ip <= asserted & ~csr_write_data[interrupts-1:0];
end
end
end
end
end
else
begin
// IE, IM, IP - Interrupt Enable, Interrupt Mask and Interrupt Pending CSRs
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
ie <= `FALSE;
eie <= `FALSE;
`ifdef CFG_DEBUG_ENABLED
bie <= `FALSE;
`endif
ip <= {interrupts{1'b0}};
eie_delay <= 0;
end
else
begin
// Set IP bit when interrupt line is asserted
ip <= asserted;
`ifdef CFG_DEBUG_ENABLED
if (non_debug_exception == `TRUE)
begin
// Save and then clear interrupt enable
eie <= ie;
ie <= `FALSE;
end
else if (debug_exception == `TRUE)
begin
// Save and then clear interrupt enable
bie <= ie;
ie <= `FALSE;
end
`else
if (exception == `TRUE)
begin
// Save and then clear interrupt enable
eie <= ie;
ie <= `FALSE;
end
`endif
else if (stall_x == `FALSE)
begin
if(eie_delay[0])
ie <= eie;
eie_delay <= {1'b0, eie_delay[`IE_DELAY:1]};
if (eret_q_x == `TRUE) begin
// Restore interrupt enable
eie_delay[`IE_DELAY] <= `TRUE;
eie_delay[`IE_DELAY-1:0] <= 0;
end
`ifdef CFG_DEBUG_ENABLED
else if (bret_q_x == `TRUE)
// Restore interrupt enable
ie <= bie;
`endif
else if (csr_write_enable == `TRUE)
begin
// Handle wcsr write
if (csr == `LM32_CSR_IE)
begin
ie <= csr_write_data[0];
eie <= csr_write_data[1];
`ifdef CFG_DEBUG_ENABLED
bie <= csr_write_data[2];
`endif
end
if (csr == `LM32_CSR_IP)
ip <= asserted & ~csr_write_data[interrupts-1:0];
end
end
end
end
end
endgenerate
endmodule
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_jtag.v
// Title : JTAG interface
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : No Change
// =============================================================================
`include "lm32_include.v"
`ifdef CFG_JTAG_ENABLED
`define LM32_DP 3'b000
`define LM32_TX 3'b001
`define LM32_RX 3'b010
// LM32 Debug Protocol commands IDs
`define LM32_DP_RNG 3:0
`define LM32_DP_READ_MEMORY 4'b0001
`define LM32_DP_WRITE_MEMORY 4'b0010
`define LM32_DP_READ_SEQUENTIAL 4'b0011
`define LM32_DP_WRITE_SEQUENTIAL 4'b0100
`define LM32_DP_WRITE_CSR 4'b0101
`define LM32_DP_BREAK 4'b0110
`define LM32_DP_RESET 4'b0111
// States for FSM
`define LM32_JTAG_STATE_RNG 3:0
`define LM32_JTAG_STATE_READ_COMMAND 4'h0
`define LM32_JTAG_STATE_READ_BYTE_0 4'h1
`define LM32_JTAG_STATE_READ_BYTE_1 4'h2
`define LM32_JTAG_STATE_READ_BYTE_2 4'h3
`define LM32_JTAG_STATE_READ_BYTE_3 4'h4
`define LM32_JTAG_STATE_READ_BYTE_4 4'h5
`define LM32_JTAG_STATE_PROCESS_COMMAND 4'h6
`define LM32_JTAG_STATE_WAIT_FOR_MEMORY 4'h7
`define LM32_JTAG_STATE_WAIT_FOR_CSR 4'h8
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_jtag (
// ----- Inputs -------
clk_i,
rst_i,
jtag_clk,
jtag_update,
jtag_reg_q,
jtag_reg_addr_q,
`ifdef CFG_JTAG_UART_ENABLED
csr,
csr_write_enable,
csr_write_data,
stall_x,
`endif
`ifdef CFG_HW_DEBUG_ENABLED
jtag_read_data,
jtag_access_complete,
`endif
`ifdef CFG_DEBUG_ENABLED
exception_q_w,
`endif
// ----- Outputs -------
`ifdef CFG_JTAG_UART_ENABLED
jtx_csr_read_data,
jrx_csr_read_data,
`endif
`ifdef CFG_HW_DEBUG_ENABLED
jtag_csr_write_enable,
jtag_csr_write_data,
jtag_csr,
jtag_read_enable,
jtag_write_enable,
jtag_write_data,
jtag_address,
`endif
`ifdef CFG_DEBUG_ENABLED
jtag_break,
jtag_reset,
`endif
jtag_reg_d,
jtag_reg_addr_d
);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
input rst_i; // Reset
input jtag_clk; // JTAG clock
input jtag_update; // JTAG data register has been updated
input [`LM32_BYTE_RNG] jtag_reg_q; // JTAG data register
input [2:0] jtag_reg_addr_q; // JTAG data register
`ifdef CFG_JTAG_UART_ENABLED
input [`LM32_CSR_RNG] csr; // CSR to write
input csr_write_enable; // CSR write enable
input [`LM32_WORD_RNG] csr_write_data; // Data to write to specified CSR
input stall_x; // Stall instruction in X stage
`endif
`ifdef CFG_HW_DEBUG_ENABLED
input [`LM32_BYTE_RNG] jtag_read_data; // Data read from requested address
input jtag_access_complete; // Memory access if complete
`endif
`ifdef CFG_DEBUG_ENABLED
input exception_q_w; // Indicates an exception has occured in W stage
`endif
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
`ifdef CFG_JTAG_UART_ENABLED
output [`LM32_WORD_RNG] jtx_csr_read_data; // Value of JTX CSR for rcsr instructions
wire [`LM32_WORD_RNG] jtx_csr_read_data;
output [`LM32_WORD_RNG] jrx_csr_read_data; // Value of JRX CSR for rcsr instructions
wire [`LM32_WORD_RNG] jrx_csr_read_data;
`endif
`ifdef CFG_HW_DEBUG_ENABLED
output jtag_csr_write_enable; // CSR write enable
reg jtag_csr_write_enable;
output [`LM32_WORD_RNG] jtag_csr_write_data; // Data to write to specified CSR
wire [`LM32_WORD_RNG] jtag_csr_write_data;
output [`LM32_CSR_RNG] jtag_csr; // CSR to write
wire [`LM32_CSR_RNG] jtag_csr;
output jtag_read_enable; // Memory read enable
reg jtag_read_enable;
output jtag_write_enable; // Memory write enable
reg jtag_write_enable;
output [`LM32_BYTE_RNG] jtag_write_data; // Data to write to specified address
wire [`LM32_BYTE_RNG] jtag_write_data;
output [`LM32_WORD_RNG] jtag_address; // Memory read/write address
wire [`LM32_WORD_RNG] jtag_address;
`endif
`ifdef CFG_DEBUG_ENABLED
output jtag_break; // Request to raise a breakpoint exception
reg jtag_break;
output jtag_reset; // Request to raise a reset exception
reg jtag_reset;
`endif
output [`LM32_BYTE_RNG] jtag_reg_d;
reg [`LM32_BYTE_RNG] jtag_reg_d;
output [2:0] jtag_reg_addr_d;
wire [2:0] jtag_reg_addr_d;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
reg rx_update; // Clock-domain crossing registers
reg rx_update_r; // Registered version of rx_update
reg rx_update_r_r; // Registered version of rx_update_r
reg rx_update_r_r_r; // Registered version of rx_update_r_r
// These wires come from the JTAG clock domain.
// They have been held unchanged for an entire JTAG clock cycle before the jtag_update toggle flips
wire [`LM32_BYTE_RNG] rx_byte;
wire [2:0] rx_addr;
`ifdef CFG_JTAG_UART_ENABLED
reg [`LM32_BYTE_RNG] uart_tx_byte; // UART TX data
reg uart_tx_valid; // TX data is valid
reg [`LM32_BYTE_RNG] uart_rx_byte; // UART RX data
reg uart_rx_valid; // RX data is valid
`endif
reg [`LM32_DP_RNG] command; // The last received command
`ifdef CFG_HW_DEBUG_ENABLED
reg [`LM32_BYTE_RNG] jtag_byte_0; // Registers to hold command paramaters
reg [`LM32_BYTE_RNG] jtag_byte_1;
reg [`LM32_BYTE_RNG] jtag_byte_2;
reg [`LM32_BYTE_RNG] jtag_byte_3;
reg [`LM32_BYTE_RNG] jtag_byte_4;
reg processing; // Indicates if we're still processing a memory read/write
`endif
reg [`LM32_JTAG_STATE_RNG] state; // Current state of FSM
/////////////////////////////////////////////////////
// Combinational Logic
/////////////////////////////////////////////////////
`ifdef CFG_HW_DEBUG_ENABLED
assign jtag_csr_write_data = {jtag_byte_0, jtag_byte_1, jtag_byte_2, jtag_byte_3};
assign jtag_csr = jtag_byte_4[`LM32_CSR_RNG];
assign jtag_address = {jtag_byte_0, jtag_byte_1, jtag_byte_2, jtag_byte_3};
assign jtag_write_data = jtag_byte_4;
`endif
// Generate status flags for reading via the JTAG interface
`ifdef CFG_JTAG_UART_ENABLED
assign jtag_reg_addr_d[1:0] = {uart_rx_valid, uart_tx_valid};
`else
assign jtag_reg_addr_d[1:0] = 2'b00;
`endif
`ifdef CFG_HW_DEBUG_ENABLED
assign jtag_reg_addr_d[2] = processing;
`else
assign jtag_reg_addr_d[2] = 1'b0;
`endif
`ifdef CFG_JTAG_UART_ENABLED
assign jtx_csr_read_data = {{`LM32_WORD_WIDTH-9{1'b0}}, uart_tx_valid, 8'h00};
assign jrx_csr_read_data = {{`LM32_WORD_WIDTH-9{1'b0}}, uart_rx_valid, uart_rx_byte};
`endif
/////////////////////////////////////////////////////
// Sequential Logic
/////////////////////////////////////////////////////
assign rx_byte = jtag_reg_q;
assign rx_addr = jtag_reg_addr_q;
// The JTAG latched jtag_reg[_addr]_q at least one JTCK before jtag_update is raised
// Thus, they are stable (and safe to sample) when jtag_update is high
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
rx_update <= 1'b0;
rx_update_r <= 1'b0;
rx_update_r_r <= 1'b0;
rx_update_r_r_r <= 1'b0;
end
else
begin
rx_update <= jtag_update;
rx_update_r <= rx_update;
rx_update_r_r <= rx_update_r;
rx_update_r_r_r <= rx_update_r_r;
end
end
// LM32 debug protocol state machine
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
state <= `LM32_JTAG_STATE_READ_COMMAND;
command <= 4'b0000;
jtag_reg_d <= 8'h00;
`ifdef CFG_HW_DEBUG_ENABLED
processing <= `FALSE;
jtag_csr_write_enable <= `FALSE;
jtag_read_enable <= `FALSE;
jtag_write_enable <= `FALSE;
`endif
`ifdef CFG_DEBUG_ENABLED
jtag_break <= `FALSE;
jtag_reset <= `FALSE;
`endif
`ifdef CFG_JTAG_UART_ENABLED
uart_tx_byte <= 8'h00;
uart_tx_valid <= `FALSE;
uart_rx_byte <= 8'h00;
uart_rx_valid <= `FALSE;
`endif
end
else
begin
`ifdef CFG_JTAG_UART_ENABLED
if ((csr_write_enable == `TRUE) && (stall_x == `FALSE))
begin
case (csr)
`LM32_CSR_JTX:
begin
// Set flag indicating data is available
uart_tx_byte <= csr_write_data[`LM32_BYTE_0_RNG];
uart_tx_valid <= `TRUE;
end
`LM32_CSR_JRX:
begin
// Clear flag indidicating data has been received
uart_rx_valid <= `FALSE;
end
endcase
end
`endif
`ifdef CFG_DEBUG_ENABLED
// When an exception has occured, clear the requests
if (exception_q_w == `TRUE)
begin
jtag_break <= `FALSE;
jtag_reset <= `FALSE;
end
`endif
case (state)
`LM32_JTAG_STATE_READ_COMMAND:
begin
// Wait for rx register to toggle which indicates new data is available
if ((~rx_update_r_r_r & rx_update_r_r) == `TRUE)
begin
command <= rx_byte[7:4];
case (rx_addr)
`ifdef CFG_DEBUG_ENABLED
`LM32_DP:
begin
case (rx_byte[7:4])
`ifdef CFG_HW_DEBUG_ENABLED
`LM32_DP_READ_MEMORY:
state <= `LM32_JTAG_STATE_READ_BYTE_0;
`LM32_DP_READ_SEQUENTIAL:
begin
{jtag_byte_2, jtag_byte_3} <= {jtag_byte_2, jtag_byte_3} + 1'b1;
state <= `LM32_JTAG_STATE_PROCESS_COMMAND;
end
`LM32_DP_WRITE_MEMORY:
state <= `LM32_JTAG_STATE_READ_BYTE_0;
`LM32_DP_WRITE_SEQUENTIAL:
begin
{jtag_byte_2, jtag_byte_3} <= {jtag_byte_2, jtag_byte_3} + 1'b1;
state <= 5;
end
`LM32_DP_WRITE_CSR:
state <= `LM32_JTAG_STATE_READ_BYTE_0;
`endif
`LM32_DP_BREAK:
begin
`ifdef CFG_JTAG_UART_ENABLED
uart_rx_valid <= `FALSE;
uart_tx_valid <= `FALSE;
`endif
jtag_break <= `TRUE;
end
`LM32_DP_RESET:
begin
`ifdef CFG_JTAG_UART_ENABLED
uart_rx_valid <= `FALSE;
uart_tx_valid <= `FALSE;
`endif
jtag_reset <= `TRUE;
end
endcase
end
`endif
`ifdef CFG_JTAG_UART_ENABLED
`LM32_TX:
begin
uart_rx_byte <= rx_byte;
uart_rx_valid <= `TRUE;
end
`LM32_RX:
begin
jtag_reg_d <= uart_tx_byte;
uart_tx_valid <= `FALSE;
end
`endif
default:
;
endcase
end
end
`ifdef CFG_HW_DEBUG_ENABLED
`LM32_JTAG_STATE_READ_BYTE_0:
begin
if ((~rx_update_r_r_r & rx_update_r_r) == `TRUE)
begin
jtag_byte_0 <= rx_byte;
state <= `LM32_JTAG_STATE_READ_BYTE_1;
end
end
`LM32_JTAG_STATE_READ_BYTE_1:
begin
if ((~rx_update_r_r_r & rx_update_r_r) == `TRUE)
begin
jtag_byte_1 <= rx_byte;
state <= `LM32_JTAG_STATE_READ_BYTE_2;
end
end
`LM32_JTAG_STATE_READ_BYTE_2:
begin
if ((~rx_update_r_r_r & rx_update_r_r) == `TRUE)
begin
jtag_byte_2 <= rx_byte;
state <= `LM32_JTAG_STATE_READ_BYTE_3;
end
end
`LM32_JTAG_STATE_READ_BYTE_3:
begin
if ((~rx_update_r_r_r & rx_update_r_r) == `TRUE)
begin
jtag_byte_3 <= rx_byte;
if (command == `LM32_DP_READ_MEMORY)
state <= `LM32_JTAG_STATE_PROCESS_COMMAND;
else
state <= `LM32_JTAG_STATE_READ_BYTE_4;
end
end
`LM32_JTAG_STATE_READ_BYTE_4:
begin
if ((~rx_update_r_r_r & rx_update_r_r) == `TRUE)
begin
jtag_byte_4 <= rx_byte;
state <= `LM32_JTAG_STATE_PROCESS_COMMAND;
end
end
`LM32_JTAG_STATE_PROCESS_COMMAND:
begin
case (command)
`LM32_DP_READ_MEMORY,
`LM32_DP_READ_SEQUENTIAL:
begin
jtag_read_enable <= `TRUE;
processing <= `TRUE;
state <= `LM32_JTAG_STATE_WAIT_FOR_MEMORY;
end
`LM32_DP_WRITE_MEMORY,
`LM32_DP_WRITE_SEQUENTIAL:
begin
jtag_write_enable <= `TRUE;
processing <= `TRUE;
state <= `LM32_JTAG_STATE_WAIT_FOR_MEMORY;
end
`LM32_DP_WRITE_CSR:
begin
jtag_csr_write_enable <= `TRUE;
processing <= `TRUE;
state <= `LM32_JTAG_STATE_WAIT_FOR_CSR;
end
endcase
end
`LM32_JTAG_STATE_WAIT_FOR_MEMORY:
begin
if (jtag_access_complete == `TRUE)
begin
jtag_read_enable <= `FALSE;
jtag_reg_d <= jtag_read_data;
jtag_write_enable <= `FALSE;
processing <= `FALSE;
state <= `LM32_JTAG_STATE_READ_COMMAND;
end
end
`LM32_JTAG_STATE_WAIT_FOR_CSR:
begin
jtag_csr_write_enable <= `FALSE;
processing <= `FALSE;
state <= `LM32_JTAG_STATE_READ_COMMAND;
end
`endif
endcase
end
end
endmodule
`endif
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_load_store_unit.v
// Title : Load and store unit
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : Instead of disallowing an instruction cache miss on a data cache
// : miss, both can now occur at the same time. If both occur at same
// : time, then restart address is the address of instruction that
// : caused data cache miss.
// Version : 3.2
// : EBRs use SYNC resets instead of ASYNC resets.
// Version : 3.3
// : Support for new non-cacheable Data Memory that is accessible by
// : the data port and has a one cycle access latency.
// Version : 3.4
// : No change
// Version : 3.5
// : Bug fix: Inline memory is correctly generated if it is not a
// : power-of-two
// =============================================================================
`include "lm32_include.v"
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_load_store_unit (
// ----- Inputs -------
clk_i,
rst_i,
// From pipeline
stall_a,
stall_x,
stall_m,
kill_x,
kill_m,
exception_m,
store_operand_x,
load_store_address_x,
load_store_address_m,
load_store_address_w,
load_x,
store_x,
load_q_x,
store_q_x,
load_q_m,
store_q_m,
sign_extend_x,
size_x,
`ifdef CFG_DCACHE_ENABLED
dflush,
`endif
`ifdef CFG_IROM_ENABLED
irom_data_m,
`endif
// From Wishbone
d_dat_i,
d_ack_i,
d_err_i,
d_rty_i,
// ----- Outputs -------
// To pipeline
`ifdef CFG_DCACHE_ENABLED
dcache_refill_request,
dcache_restart_request,
dcache_stall_request,
dcache_refilling,
`endif
`ifdef CFG_IROM_ENABLED
irom_store_data_m,
irom_address_xm,
irom_we_xm,
irom_stall_request_x,
`endif
load_data_w,
stall_wb_load,
// To Wishbone
d_dat_o,
d_adr_o,
d_cyc_o,
d_sel_o,
d_stb_o,
d_we_o,
d_cti_o,
d_lock_o,
d_bte_o
);
/////////////////////////////////////////////////////
// Parameters
/////////////////////////////////////////////////////
parameter associativity = 1; // Associativity of the cache (Number of ways)
parameter sets = 512; // Number of sets
parameter bytes_per_line = 16; // Number of bytes per cache line
parameter base_address = 0; // Base address of cachable memory
parameter limit = 0; // Limit (highest address) of cachable memory
// For bytes_per_line == 4, we set 1 so part-select range isn't reversed, even though not really used
localparam addr_offset_width = bytes_per_line == 4 ? 1 : clogb2(bytes_per_line)-1-2;
localparam addr_offset_lsb = 2;
localparam addr_offset_msb = (addr_offset_lsb+addr_offset_width-1);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
input rst_i; // Reset
input stall_a; // A stage stall
input stall_x; // X stage stall
input stall_m; // M stage stall
input kill_x; // Kill instruction in X stage
input kill_m; // Kill instruction in M stage
input exception_m; // An exception occured in the M stage
input [`LM32_WORD_RNG] store_operand_x; // Data read from register to store
input [`LM32_WORD_RNG] load_store_address_x; // X stage load/store address
input [`LM32_WORD_RNG] load_store_address_m; // M stage load/store address
input [1:0] load_store_address_w; // W stage load/store address (only least two significant bits are needed)
input load_x; // Load instruction in X stage
input store_x; // Store instruction in X stage
input load_q_x; // Load instruction in X stage
input store_q_x; // Store instruction in X stage
input load_q_m; // Load instruction in M stage
input store_q_m; // Store instruction in M stage
input sign_extend_x; // Whether load instruction in X stage should sign extend or zero extend
input [`LM32_SIZE_RNG] size_x; // Size of load or store (byte, hword, word)
`ifdef CFG_DCACHE_ENABLED
input dflush; // Flush the data cache
`endif
`ifdef CFG_IROM_ENABLED
input [`LM32_WORD_RNG] irom_data_m; // Data from Instruction-ROM
`endif
input [`LM32_WORD_RNG] d_dat_i; // Data Wishbone interface read data
input d_ack_i; // Data Wishbone interface acknowledgement
input d_err_i; // Data Wishbone interface error
input d_rty_i; // Data Wishbone interface retry
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
`ifdef CFG_DCACHE_ENABLED
output dcache_refill_request; // Request to refill data cache
wire dcache_refill_request;
output dcache_restart_request; // Request to restart the instruction that caused a data cache miss
wire dcache_restart_request;
output dcache_stall_request; // Data cache stall request
wire dcache_stall_request;
output dcache_refilling;
wire dcache_refilling;
`endif
`ifdef CFG_IROM_ENABLED
output irom_store_data_m; // Store data to Instruction ROM
wire [`LM32_WORD_RNG] irom_store_data_m;
output [`LM32_WORD_RNG] irom_address_xm; // Load/store address to Instruction ROM
wire [`LM32_WORD_RNG] irom_address_xm;
output irom_we_xm; // Write-enable of 2nd port of Instruction ROM
wire irom_we_xm;
output irom_stall_request_x; // Stall instruction in D stage
wire irom_stall_request_x;
`endif
output [`LM32_WORD_RNG] load_data_w; // Result of a load instruction
reg [`LM32_WORD_RNG] load_data_w;
output stall_wb_load; // Request to stall pipeline due to a load from the Wishbone interface
reg stall_wb_load;
output [`LM32_WORD_RNG] d_dat_o; // Data Wishbone interface write data
reg [`LM32_WORD_RNG] d_dat_o;
output [`LM32_WORD_RNG] d_adr_o; // Data Wishbone interface address
reg [`LM32_WORD_RNG] d_adr_o;
output d_cyc_o; // Data Wishbone interface cycle
reg d_cyc_o;
output [`LM32_BYTE_SELECT_RNG] d_sel_o; // Data Wishbone interface byte select
reg [`LM32_BYTE_SELECT_RNG] d_sel_o;
output d_stb_o; // Data Wishbone interface strobe
reg d_stb_o;
output d_we_o; // Data Wishbone interface write enable
reg d_we_o;
output [`LM32_CTYPE_RNG] d_cti_o; // Data Wishbone interface cycle type
reg [`LM32_CTYPE_RNG] d_cti_o;
output d_lock_o; // Date Wishbone interface lock bus
reg d_lock_o;
output [`LM32_BTYPE_RNG] d_bte_o; // Data Wishbone interface burst type
wire [`LM32_BTYPE_RNG] d_bte_o;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
// Microcode pipeline registers - See inputs for description
reg [`LM32_SIZE_RNG] size_m;
reg [`LM32_SIZE_RNG] size_w;
reg sign_extend_m;
reg sign_extend_w;
reg [`LM32_WORD_RNG] store_data_x;
reg [`LM32_WORD_RNG] store_data_m;
reg [`LM32_BYTE_SELECT_RNG] byte_enable_x;
reg [`LM32_BYTE_SELECT_RNG] byte_enable_m;
wire [`LM32_WORD_RNG] data_m;
reg [`LM32_WORD_RNG] data_w;
`ifdef CFG_DCACHE_ENABLED
wire dcache_select_x; // Select data cache to load from / store to
reg dcache_select_m;
wire [`LM32_WORD_RNG] dcache_data_m; // Data read from cache
wire [`LM32_WORD_RNG] dcache_refill_address; // Address to refill data cache from
reg dcache_refill_ready; // Indicates the next word of refill data is ready
wire [`LM32_CTYPE_RNG] first_cycle_type; // First Wishbone cycle type
wire [`LM32_CTYPE_RNG] next_cycle_type; // Next Wishbone cycle type
wire last_word; // Indicates if this is the last word in the cache line
wire [`LM32_WORD_RNG] first_address; // First cache refill address
`endif
`ifdef CFG_DRAM_ENABLED
wire dram_select_x; // Select data RAM to load from / store to
reg dram_select_m;
reg dram_bypass_en; // RAW in data RAM; read latched (bypass) value rather than value from memory
reg [`LM32_WORD_RNG] dram_bypass_data; // Latched value of store'd data to data RAM
wire [`LM32_WORD_RNG] dram_data_out; // Data read from data RAM
wire [`LM32_WORD_RNG] dram_data_m; // Data read from data RAM: bypass value or value from memory
wire [`LM32_WORD_RNG] dram_store_data_m; // Data to write to RAM
`endif
wire wb_select_x; // Select Wishbone to load from / store to
`ifdef CFG_IROM_ENABLED
wire irom_select_x; // Select instruction ROM to load from / store to
reg irom_select_m;
`endif
reg wb_select_m;
reg [`LM32_WORD_RNG] wb_data_m; // Data read from Wishbone
reg wb_load_complete; // Indicates when a Wishbone load is complete
/////////////////////////////////////////////////////
// Functions
/////////////////////////////////////////////////////
`include "lm32_functions.v"
/////////////////////////////////////////////////////
// Instantiations
/////////////////////////////////////////////////////
`ifdef CFG_DRAM_ENABLED
// Data RAM
pmi_ram_dp_true
#(
// ----- Parameters -------
.pmi_family (`LATTICE_FAMILY),
//.pmi_addr_depth_a (1 << (clogb2(`CFG_DRAM_LIMIT/4-`CFG_DRAM_BASE_ADDRESS/4+1)-1)),
//.pmi_addr_width_a ((clogb2(`CFG_DRAM_LIMIT/4-`CFG_DRAM_BASE_ADDRESS/4+1)-1)),
//.pmi_data_width_a (`LM32_WORD_WIDTH),
//.pmi_addr_depth_b (1 << (clogb2(`CFG_DRAM_LIMIT/4-`CFG_DRAM_BASE_ADDRESS/4+1)-1)),
//.pmi_addr_width_b ((clogb2(`CFG_DRAM_LIMIT/4-`CFG_DRAM_BASE_ADDRESS/4+1)-1)),
//.pmi_data_width_b (`LM32_WORD_WIDTH),
.pmi_addr_depth_a (`CFG_DRAM_LIMIT/4-`CFG_DRAM_BASE_ADDRESS/4+1),
.pmi_addr_width_a (clogb2_v1(`CFG_DRAM_LIMIT/4-`CFG_DRAM_BASE_ADDRESS/4+1)),
.pmi_data_width_a (`LM32_WORD_WIDTH),
.pmi_addr_depth_b (`CFG_DRAM_LIMIT/4-`CFG_DRAM_BASE_ADDRESS/4+1),
.pmi_addr_width_b (clogb2_v1(`CFG_DRAM_LIMIT/4-`CFG_DRAM_BASE_ADDRESS/4+1)),
.pmi_data_width_b (`LM32_WORD_WIDTH),
.pmi_regmode_a ("noreg"),
.pmi_regmode_b ("noreg"),
.pmi_gsr ("enable"),
.pmi_resetmode ("sync"),
.pmi_init_file (`CFG_DRAM_INIT_FILE),
.pmi_init_file_format (`CFG_DRAM_INIT_FILE_FORMAT),
.module_type ("pmi_ram_dp_true")
)
ram (
// ----- Inputs -------
.ClockA (clk_i),
.ClockB (clk_i),
.ResetA (rst_i),
.ResetB (rst_i),
.DataInA ({32{1'b0}}),
.DataInB (dram_store_data_m),
.AddressA (load_store_address_x[(clogb2(`CFG_DRAM_LIMIT/4-`CFG_DRAM_BASE_ADDRESS/4+1)-1)+2-1:2]),
.AddressB (load_store_address_m[(clogb2(`CFG_DRAM_LIMIT/4-`CFG_DRAM_BASE_ADDRESS/4+1)-1)+2-1:2]),
// .ClockEnA (!stall_x & (load_x | store_x)),
.ClockEnA (!stall_x),
.ClockEnB (!stall_m),
.WrA (`FALSE),
.WrB (store_q_m & dram_select_m),
// ----- Outputs -------
.QA (dram_data_out),
.QB ()
);
/*----------------------------------------------------------------------
EBRs cannot perform reads from location 'written to' on the same clock
edge. Therefore bypass logic is required to latch the store'd value
and use it for the load (instead of value from memory).
----------------------------------------------------------------------*/
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
if (rst_i == `TRUE)
begin
dram_bypass_en <= `FALSE;
dram_bypass_data <= 0;
end
else
begin
if (stall_x == `FALSE)
dram_bypass_data <= dram_store_data_m;
if ( (stall_m == `FALSE)
&& (stall_x == `FALSE)
&& (store_q_m == `TRUE)
&& ( (load_x == `TRUE)
|| (store_x == `TRUE)
)
&& (load_store_address_x[(`LM32_WORD_WIDTH-1):2] == load_store_address_m[(`LM32_WORD_WIDTH-1):2])
)
dram_bypass_en <= `TRUE;
else
if ( (dram_bypass_en == `TRUE)
&& (stall_x == `FALSE)
)
dram_bypass_en <= `FALSE;
end
assign dram_data_m = dram_bypass_en ? dram_bypass_data : dram_data_out;
`endif
`ifdef CFG_DCACHE_ENABLED
// Data cache
lm32_dcache #(
.associativity (associativity),
.sets (sets),
.bytes_per_line (bytes_per_line),
.base_address (base_address),
.limit (limit)
) dcache (
// ----- Inputs -----
.clk_i (clk_i),
.rst_i (rst_i),
.stall_a (stall_a),
.stall_x (stall_x),
.stall_m (stall_m),
.address_x (load_store_address_x),
.address_m (load_store_address_m),
.load_q_m (load_q_m & dcache_select_m),
.store_q_m (store_q_m & dcache_select_m),
.store_data (store_data_m),
.store_byte_select (byte_enable_m & {4{dcache_select_m}}),
.refill_ready (dcache_refill_ready),
.refill_data (wb_data_m),
.dflush (dflush),
// ----- Outputs -----
.stall_request (dcache_stall_request),
.restart_request (dcache_restart_request),
.refill_request (dcache_refill_request),
.refill_address (dcache_refill_address),
.refilling (dcache_refilling),
.load_data (dcache_data_m)
);
`endif
/////////////////////////////////////////////////////
// Combinational Logic
/////////////////////////////////////////////////////
// Select where data should be loaded from / stored to
`ifdef CFG_DRAM_ENABLED
assign dram_select_x = (load_store_address_x >= `CFG_DRAM_BASE_ADDRESS)
&& (load_store_address_x <= `CFG_DRAM_LIMIT);
`endif
`ifdef CFG_IROM_ENABLED
assign irom_select_x = (load_store_address_x >= `CFG_IROM_BASE_ADDRESS)
&& (load_store_address_x <= `CFG_IROM_LIMIT);
`endif
`ifdef CFG_DCACHE_ENABLED
assign dcache_select_x = (load_store_address_x >= `CFG_DCACHE_BASE_ADDRESS)
&& (load_store_address_x <= `CFG_DCACHE_LIMIT)
`ifdef CFG_DRAM_ENABLED
&& (dram_select_x == `FALSE)
`endif
`ifdef CFG_IROM_ENABLED
&& (irom_select_x == `FALSE)
`endif
;
`endif
assign wb_select_x = `TRUE
`ifdef CFG_DCACHE_ENABLED
&& !dcache_select_x
`endif
`ifdef CFG_DRAM_ENABLED
&& !dram_select_x
`endif
`ifdef CFG_IROM_ENABLED
&& !irom_select_x
`endif
;
// Make sure data to store is in correct byte lane
always @(*)
begin
case (size_x)
`LM32_SIZE_BYTE: store_data_x = {4{store_operand_x[7:0]}};
`LM32_SIZE_HWORD: store_data_x = {2{store_operand_x[15:0]}};
`LM32_SIZE_WORD: store_data_x = store_operand_x;
default: store_data_x = {`LM32_WORD_WIDTH{1'bx}};
endcase
end
// Generate byte enable accoring to size of load or store and address being accessed
always @(*)
begin
casez ({size_x, load_store_address_x[1:0]})
{`LM32_SIZE_BYTE, 2'b11}: byte_enable_x = 4'b0001;
{`LM32_SIZE_BYTE, 2'b10}: byte_enable_x = 4'b0010;
{`LM32_SIZE_BYTE, 2'b01}: byte_enable_x = 4'b0100;
{`LM32_SIZE_BYTE, 2'b00}: byte_enable_x = 4'b1000;
{`LM32_SIZE_HWORD, 2'b1?}: byte_enable_x = 4'b0011;
{`LM32_SIZE_HWORD, 2'b0?}: byte_enable_x = 4'b1100;
{`LM32_SIZE_WORD, 2'b??}: byte_enable_x = 4'b1111;
default: byte_enable_x = 4'bxxxx;
endcase
end
`ifdef CFG_DRAM_ENABLED
// Only replace selected bytes
assign dram_store_data_m[`LM32_BYTE_0_RNG] = byte_enable_m[0] ? store_data_m[`LM32_BYTE_0_RNG] : dram_data_m[`LM32_BYTE_0_RNG];
assign dram_store_data_m[`LM32_BYTE_1_RNG] = byte_enable_m[1] ? store_data_m[`LM32_BYTE_1_RNG] : dram_data_m[`LM32_BYTE_1_RNG];
assign dram_store_data_m[`LM32_BYTE_2_RNG] = byte_enable_m[2] ? store_data_m[`LM32_BYTE_2_RNG] : dram_data_m[`LM32_BYTE_2_RNG];
assign dram_store_data_m[`LM32_BYTE_3_RNG] = byte_enable_m[3] ? store_data_m[`LM32_BYTE_3_RNG] : dram_data_m[`LM32_BYTE_3_RNG];
`endif
`ifdef CFG_IROM_ENABLED
// Only replace selected bytes
assign irom_store_data_m[`LM32_BYTE_0_RNG] = byte_enable_m[0] ? store_data_m[`LM32_BYTE_0_RNG] : irom_data_m[`LM32_BYTE_0_RNG];
assign irom_store_data_m[`LM32_BYTE_1_RNG] = byte_enable_m[1] ? store_data_m[`LM32_BYTE_1_RNG] : irom_data_m[`LM32_BYTE_1_RNG];
assign irom_store_data_m[`LM32_BYTE_2_RNG] = byte_enable_m[2] ? store_data_m[`LM32_BYTE_2_RNG] : irom_data_m[`LM32_BYTE_2_RNG];
assign irom_store_data_m[`LM32_BYTE_3_RNG] = byte_enable_m[3] ? store_data_m[`LM32_BYTE_3_RNG] : irom_data_m[`LM32_BYTE_3_RNG];
`endif
`ifdef CFG_IROM_ENABLED
// Instead of implementing a byte-addressable instruction ROM (for store byte instruction),
// a load-and-store architecture is used wherein a 32-bit value is loaded, the requisite
// byte is replaced, and the whole 32-bit value is written back
assign irom_address_xm = ((irom_select_m == `TRUE) && (store_q_m == `TRUE))
? load_store_address_m
: load_store_address_x;
// All store instructions perform a write operation in the M stage
assign irom_we_xm = (irom_select_m == `TRUE)
&& (store_q_m == `TRUE);
// A single port in instruction ROM is available to load-store unit for doing loads/stores.
// Since every store requires a load (in X stage) and then a store (in M stage), we cannot
// allow load (or store) instructions sequentially after the store instructions to proceed
// until the store instruction has vacated M stage (i.e., completed the store operation)
assign irom_stall_request_x = (irom_select_x == `TRUE)
&& (store_q_x == `TRUE);
`endif
`ifdef CFG_DCACHE_ENABLED
`ifdef CFG_DRAM_ENABLED
`ifdef CFG_IROM_ENABLED
// WB + DC + DRAM + IROM
assign data_m = wb_select_m == `TRUE
? wb_data_m
: dram_select_m == `TRUE
? dram_data_m
: irom_select_m == `TRUE
? irom_data_m
: dcache_data_m;
`else
// WB + DC + DRAM
assign data_m = wb_select_m == `TRUE
? wb_data_m
: dram_select_m == `TRUE
? dram_data_m
: dcache_data_m;
`endif
`else
`ifdef CFG_IROM_ENABLED
// WB + DC + IROM
assign data_m = wb_select_m == `TRUE
? wb_data_m
: irom_select_m == `TRUE
? irom_data_m
: dcache_data_m;
`else
// WB + DC
assign data_m = wb_select_m == `TRUE
? wb_data_m
: dcache_data_m;
`endif
`endif
`else
`ifdef CFG_DRAM_ENABLED
`ifdef CFG_IROM_ENABLED
// WB + DRAM + IROM
assign data_m = wb_select_m == `TRUE
? wb_data_m
: dram_select_m == `TRUE
? dram_data_m
: irom_data_m;
`else
// WB + DRAM
assign data_m = wb_select_m == `TRUE
? wb_data_m
: dram_data_m;
`endif
`else
`ifdef CFG_IROM_ENABLED
// WB + IROM
assign data_m = wb_select_m == `TRUE
? wb_data_m
: irom_data_m;
`else
// WB
assign data_m = wb_data_m;
`endif
`endif
`endif
// Sub-word selection and sign/zero-extension for loads
always @(*)
begin
casez ({size_w, load_store_address_w[1:0]})
{`LM32_SIZE_BYTE, 2'b11}: load_data_w = {{24{sign_extend_w & data_w[7]}}, data_w[7:0]};
{`LM32_SIZE_BYTE, 2'b10}: load_data_w = {{24{sign_extend_w & data_w[15]}}, data_w[15:8]};
{`LM32_SIZE_BYTE, 2'b01}: load_data_w = {{24{sign_extend_w & data_w[23]}}, data_w[23:16]};
{`LM32_SIZE_BYTE, 2'b00}: load_data_w = {{24{sign_extend_w & data_w[31]}}, data_w[31:24]};
{`LM32_SIZE_HWORD, 2'b1?}: load_data_w = {{16{sign_extend_w & data_w[15]}}, data_w[15:0]};
{`LM32_SIZE_HWORD, 2'b0?}: load_data_w = {{16{sign_extend_w & data_w[31]}}, data_w[31:16]};
{`LM32_SIZE_WORD, 2'b??}: load_data_w = data_w;
default: load_data_w = {`LM32_WORD_WIDTH{1'bx}};
endcase
end
// Unused/constant Wishbone signals
assign d_bte_o = `LM32_BTYPE_LINEAR;
`ifdef CFG_DCACHE_ENABLED
// Generate signal to indicate last word in cache line
generate
case (bytes_per_line)
4:
begin
assign first_cycle_type = `LM32_CTYPE_END;
assign next_cycle_type = `LM32_CTYPE_END;
assign last_word = `TRUE;
assign first_address = {dcache_refill_address[`LM32_WORD_WIDTH-1:2], 2'b00};
end
8:
begin
assign first_cycle_type = `LM32_CTYPE_INCREMENTING;
assign next_cycle_type = `LM32_CTYPE_END;
assign last_word = (&d_adr_o[addr_offset_msb:addr_offset_lsb]) == 1'b1;
assign first_address = {dcache_refill_address[`LM32_WORD_WIDTH-1:addr_offset_msb+1], {addr_offset_width{1'b0}}, 2'b00};
end
16:
begin
assign first_cycle_type = `LM32_CTYPE_INCREMENTING;
assign next_cycle_type = d_adr_o[addr_offset_msb] == 1'b1 ? `LM32_CTYPE_END : `LM32_CTYPE_INCREMENTING;
assign last_word = (&d_adr_o[addr_offset_msb:addr_offset_lsb]) == 1'b1;
assign first_address = {dcache_refill_address[`LM32_WORD_WIDTH-1:addr_offset_msb+1], {addr_offset_width{1'b0}}, 2'b00};
end
endcase
endgenerate
`endif
/////////////////////////////////////////////////////
// Sequential Logic
/////////////////////////////////////////////////////
// Data Wishbone interface
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
d_cyc_o <= `FALSE;
d_stb_o <= `FALSE;
d_dat_o <= {`LM32_WORD_WIDTH{1'b0}};
d_adr_o <= {`LM32_WORD_WIDTH{1'b0}};
d_sel_o <= {`LM32_BYTE_SELECT_WIDTH{`FALSE}};
d_we_o <= `FALSE;
d_cti_o <= `LM32_CTYPE_END;
d_lock_o <= `FALSE;
wb_data_m <= {`LM32_WORD_WIDTH{1'b0}};
wb_load_complete <= `FALSE;
stall_wb_load <= `FALSE;
`ifdef CFG_DCACHE_ENABLED
dcache_refill_ready <= `FALSE;
`endif
end
else
begin
`ifdef CFG_DCACHE_ENABLED
// Refill ready should only be asserted for a single cycle
dcache_refill_ready <= `FALSE;
`endif
// Is a Wishbone cycle already in progress?
if (d_cyc_o == `TRUE)
begin
// Is the cycle complete?
if ((d_ack_i == `TRUE) || (d_err_i == `TRUE))
begin
`ifdef CFG_DCACHE_ENABLED
if ((dcache_refilling == `TRUE) && (!last_word))
begin
// Fetch next word of cache line
d_adr_o[addr_offset_msb:addr_offset_lsb] <= d_adr_o[addr_offset_msb:addr_offset_lsb] + 1'b1;
end
else
`endif
begin
// Refill/access complete
d_cyc_o <= `FALSE;
d_stb_o <= `FALSE;
d_lock_o <= `FALSE;
end
`ifdef CFG_DCACHE_ENABLED
d_cti_o <= next_cycle_type;
// If we are performing a refill, indicate to cache next word of data is ready
dcache_refill_ready <= dcache_refilling;
`endif
// Register data read from Wishbone interface
wb_data_m <= d_dat_i;
// Don't set when stores complete - otherwise we'll deadlock if load in m stage
wb_load_complete <= !d_we_o;
end
// synthesis translate_off
if (d_err_i == `TRUE)
$display ("Data bus error. Address: %x", d_adr_o);
// synthesis translate_on
end
else
begin
`ifdef CFG_DCACHE_ENABLED
if (dcache_refill_request == `TRUE)
begin
// Start cache refill
d_adr_o <= first_address;
d_cyc_o <= `TRUE;
d_sel_o <= {`LM32_WORD_WIDTH/8{`TRUE}};
d_stb_o <= `TRUE;
d_we_o <= `FALSE;
d_cti_o <= first_cycle_type;
//d_lock_o <= `TRUE;
end
else
`endif
if ( (store_q_m == `TRUE)
&& (stall_m == `FALSE)
`ifdef CFG_DRAM_ENABLED
&& (dram_select_m == `FALSE)
`endif
`ifdef CFG_IROM_ENABLED
&& (irom_select_m == `FALSE)
`endif
)
begin
// Data cache is write through, so all stores go to memory
d_dat_o <= store_data_m;
d_adr_o <= load_store_address_m;
d_cyc_o <= `TRUE;
d_sel_o <= byte_enable_m;
d_stb_o <= `TRUE;
d_we_o <= `TRUE;
d_cti_o <= `LM32_CTYPE_END;
end
else if ( (load_q_m == `TRUE)
&& (wb_select_m == `TRUE)
&& (wb_load_complete == `FALSE)
// stall_m will be TRUE, because stall_wb_load will be TRUE
)
begin
// Read requested address
stall_wb_load <= `FALSE;
d_adr_o <= load_store_address_m;
d_cyc_o <= `TRUE;
d_sel_o <= byte_enable_m;
d_stb_o <= `TRUE;
d_we_o <= `FALSE;
d_cti_o <= `LM32_CTYPE_END;
end
end
// Clear load/store complete flag when instruction leaves M stage
if (stall_m == `FALSE)
wb_load_complete <= `FALSE;
// When a Wishbone load first enters the M stage, we need to stall it
if ((load_q_x == `TRUE) && (wb_select_x == `TRUE) && (stall_x == `FALSE))
stall_wb_load <= `TRUE;
// Clear stall request if load instruction is killed
if ((kill_m == `TRUE) || (exception_m == `TRUE))
stall_wb_load <= `FALSE;
end
end
// Pipeline registers
// X/M stage pipeline registers
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
sign_extend_m <= `FALSE;
size_m <= 2'b00;
byte_enable_m <= `FALSE;
store_data_m <= {`LM32_WORD_WIDTH{1'b0}};
`ifdef CFG_DCACHE_ENABLED
dcache_select_m <= `FALSE;
`endif
`ifdef CFG_DRAM_ENABLED
dram_select_m <= `FALSE;
`endif
`ifdef CFG_IROM_ENABLED
irom_select_m <= `FALSE;
`endif
wb_select_m <= `FALSE;
end
else
begin
if (stall_m == `FALSE)
begin
sign_extend_m <= sign_extend_x;
size_m <= size_x;
byte_enable_m <= byte_enable_x;
store_data_m <= store_data_x;
`ifdef CFG_DCACHE_ENABLED
dcache_select_m <= dcache_select_x;
`endif
`ifdef CFG_DRAM_ENABLED
dram_select_m <= dram_select_x;
`endif
`ifdef CFG_IROM_ENABLED
irom_select_m <= irom_select_x;
`endif
wb_select_m <= wb_select_x;
end
end
end
// M/W stage pipeline registers
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
size_w <= 2'b00;
data_w <= {`LM32_WORD_WIDTH{1'b0}};
sign_extend_w <= `FALSE;
end
else
begin
size_w <= size_m;
data_w <= data_m;
sign_extend_w <= sign_extend_m;
end
end
/////////////////////////////////////////////////////
// Behavioural Logic
/////////////////////////////////////////////////////
// synthesis translate_off
// Check for non-aligned loads or stores
always @(posedge clk_i)
begin
if (((load_q_m == `TRUE) || (store_q_m == `TRUE)) && (stall_m == `FALSE))
begin
if ((size_m === `LM32_SIZE_HWORD) && (load_store_address_m[0] !== 1'b0))
$display ("Warning: Non-aligned halfword access. Address: 0x%0x Time: %0t.", load_store_address_m, $time);
if ((size_m === `LM32_SIZE_WORD) && (load_store_address_m[1:0] !== 2'b00))
$display ("Warning: Non-aligned word access. Address: 0x%0x Time: %0t.", load_store_address_m, $time);
end
end
// synthesis translate_on
endmodule
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_logic_op.v
// Title : Logic operations (and / or / not etc)
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : No Change
// =============================================================================
`include "lm32_include.v"
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_logic_op (
// ----- Inputs -------
logic_op_x,
operand_0_x,
operand_1_x,
// ----- Outputs -------
logic_result_x
);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input [`LM32_LOGIC_OP_RNG] logic_op_x;
input [`LM32_WORD_RNG] operand_0_x;
input [`LM32_WORD_RNG] operand_1_x;
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
output [`LM32_WORD_RNG] logic_result_x;
reg [`LM32_WORD_RNG] logic_result_x;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
integer logic_idx;
/////////////////////////////////////////////////////
// Combinational Logic
/////////////////////////////////////////////////////
always @(*)
begin
for(logic_idx = 0; logic_idx < `LM32_WORD_WIDTH; logic_idx = logic_idx + 1)
logic_result_x[logic_idx] = logic_op_x[{operand_1_x[logic_idx], operand_0_x[logic_idx]}];
end
endmodule
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm_mc_arithmetic.v
// Title : Multi-cycle arithmetic unit.
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : No Change
// =============================================================================
`include "lm32_include.v"
`define LM32_MC_STATE_RNG 2:0
`define LM32_MC_STATE_IDLE 3'b000
`define LM32_MC_STATE_MULTIPLY 3'b001
`define LM32_MC_STATE_MODULUS 3'b010
`define LM32_MC_STATE_DIVIDE 3'b011
`define LM32_MC_STATE_SHIFT_LEFT 3'b100
`define LM32_MC_STATE_SHIFT_RIGHT 3'b101
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_mc_arithmetic (
// ----- Inputs -----
clk_i,
rst_i,
stall_d,
kill_x,
`ifdef CFG_MC_DIVIDE_ENABLED
divide_d,
modulus_d,
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
multiply_d,
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
shift_left_d,
shift_right_d,
sign_extend_d,
`endif
operand_0_d,
operand_1_d,
// ----- Ouputs -----
result_x,
`ifdef CFG_MC_DIVIDE_ENABLED
divide_by_zero_x,
`endif
stall_request_x
);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
input rst_i; // Reset
input stall_d; // Stall instruction in D stage
input kill_x; // Kill instruction in X stage
`ifdef CFG_MC_DIVIDE_ENABLED
input divide_d; // Perform divide
input modulus_d; // Perform modulus
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
input multiply_d; // Perform multiply
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
input shift_left_d; // Perform left shift
input shift_right_d; // Perform right shift
input sign_extend_d; // Whether to sign-extend (arithmetic) or zero-extend (logical)
`endif
input [`LM32_WORD_RNG] operand_0_d;
input [`LM32_WORD_RNG] operand_1_d;
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
output [`LM32_WORD_RNG] result_x; // Result of operation
reg [`LM32_WORD_RNG] result_x;
`ifdef CFG_MC_DIVIDE_ENABLED
output divide_by_zero_x; // A divide by zero was attempted
reg divide_by_zero_x;
`endif
output stall_request_x; // Request to stall pipeline from X stage back
wire stall_request_x;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
reg [`LM32_WORD_RNG] p; // Temporary registers
reg [`LM32_WORD_RNG] a;
reg [`LM32_WORD_RNG] b;
`ifdef CFG_MC_DIVIDE_ENABLED
wire [32:0] t;
`endif
reg [`LM32_MC_STATE_RNG] state; // Current state of FSM
reg [5:0] cycles; // Number of cycles remaining in the operation
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
reg sign_extend_x; // Whether to sign extend of zero extend right shifts
wire fill_value; // Value to fill with for right barrel-shifts
`endif
/////////////////////////////////////////////////////
// Combinational logic
/////////////////////////////////////////////////////
// Stall pipeline while any operation is being performed
assign stall_request_x = state != `LM32_MC_STATE_IDLE;
`ifdef CFG_MC_DIVIDE_ENABLED
// Subtraction
assign t = {p[`LM32_WORD_WIDTH-2:0], a[`LM32_WORD_WIDTH-1]} - b;
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
// Determine fill value for right shift - Sign bit for arithmetic shift, or zero for logical shift
assign fill_value = (sign_extend_x == `TRUE) & b[`LM32_WORD_WIDTH-1];
`endif
/////////////////////////////////////////////////////
// Sequential logic
/////////////////////////////////////////////////////
// Perform right shift
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
cycles <= {6{1'b0}};
p <= {`LM32_WORD_WIDTH{1'b0}};
a <= {`LM32_WORD_WIDTH{1'b0}};
b <= {`LM32_WORD_WIDTH{1'b0}};
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
sign_extend_x <= 1'b0;
`endif
`ifdef CFG_MC_DIVIDE_ENABLED
divide_by_zero_x <= `FALSE;
`endif
result_x <= {`LM32_WORD_WIDTH{1'b0}};
state <= `LM32_MC_STATE_IDLE;
end
else
begin
`ifdef CFG_MC_DIVIDE_ENABLED
divide_by_zero_x <= `FALSE;
`endif
case (state)
`LM32_MC_STATE_IDLE:
begin
if (stall_d == `FALSE)
begin
cycles <= `LM32_WORD_WIDTH;
p <= 32'b0;
a <= operand_0_d;
b <= operand_1_d;
`ifdef CFG_MC_DIVIDE_ENABLED
if (divide_d == `TRUE)
state <= `LM32_MC_STATE_DIVIDE;
if (modulus_d == `TRUE)
state <= `LM32_MC_STATE_MODULUS;
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
if (multiply_d == `TRUE)
state <= `LM32_MC_STATE_MULTIPLY;
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
if (shift_left_d == `TRUE)
begin
state <= `LM32_MC_STATE_SHIFT_LEFT;
sign_extend_x <= sign_extend_d;
cycles <= operand_1_d[4:0];
a <= operand_0_d;
b <= operand_0_d;
end
if (shift_right_d == `TRUE)
begin
state <= `LM32_MC_STATE_SHIFT_RIGHT;
sign_extend_x <= sign_extend_d;
cycles <= operand_1_d[4:0];
a <= operand_0_d;
b <= operand_0_d;
end
`endif
end
end
`ifdef CFG_MC_DIVIDE_ENABLED
`LM32_MC_STATE_DIVIDE:
begin
if (t[32] == 1'b0)
begin
p <= t[31:0];
a <= {a[`LM32_WORD_WIDTH-2:0], 1'b1};
end
else
begin
p <= {p[`LM32_WORD_WIDTH-2:0], a[`LM32_WORD_WIDTH-1]};
a <= {a[`LM32_WORD_WIDTH-2:0], 1'b0};
end
result_x <= a;
if ((cycles == `LM32_WORD_WIDTH'd0) || (kill_x == `TRUE))
begin
// Check for divide by zero
divide_by_zero_x <= b == {`LM32_WORD_WIDTH{1'b0}};
state <= `LM32_MC_STATE_IDLE;
end
cycles <= cycles - 1'b1;
end
`LM32_MC_STATE_MODULUS:
begin
if (t[32] == 1'b0)
begin
p <= t[31:0];
a <= {a[`LM32_WORD_WIDTH-2:0], 1'b1};
end
else
begin
p <= {p[`LM32_WORD_WIDTH-2:0], a[`LM32_WORD_WIDTH-1]};
a <= {a[`LM32_WORD_WIDTH-2:0], 1'b0};
end
result_x <= p;
if ((cycles == `LM32_WORD_WIDTH'd0) || (kill_x == `TRUE))
begin
// Check for divide by zero
divide_by_zero_x <= b == {`LM32_WORD_WIDTH{1'b0}};
state <= `LM32_MC_STATE_IDLE;
end
cycles <= cycles - 1'b1;
end
`endif
`ifdef CFG_MC_MULTIPLY_ENABLED
`LM32_MC_STATE_MULTIPLY:
begin
if (b[0] == 1'b1)
p <= p + a;
b <= {1'b0, b[`LM32_WORD_WIDTH-1:1]};
a <= {a[`LM32_WORD_WIDTH-2:0], 1'b0};
result_x <= p;
if ((cycles == `LM32_WORD_WIDTH'd0) || (kill_x == `TRUE))
state <= `LM32_MC_STATE_IDLE;
cycles <= cycles - 1'b1;
end
`endif
`ifdef CFG_MC_BARREL_SHIFT_ENABLED
`LM32_MC_STATE_SHIFT_LEFT:
begin
a <= {a[`LM32_WORD_WIDTH-2:0], 1'b0};
result_x <= a;
if ((cycles == `LM32_WORD_WIDTH'd0) || (kill_x == `TRUE))
state <= `LM32_MC_STATE_IDLE;
cycles <= cycles - 1'b1;
end
`LM32_MC_STATE_SHIFT_RIGHT:
begin
b <= {fill_value, b[`LM32_WORD_WIDTH-1:1]};
result_x <= b;
if ((cycles == `LM32_WORD_WIDTH'd0) || (kill_x == `TRUE))
state <= `LM32_MC_STATE_IDLE;
cycles <= cycles - 1'b1;
end
`endif
endcase
end
end
endmodule
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_ram.v
// Title : Pseudo dual-port RAM.
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : Options added to select EBRs (True-DP, Psuedo-DP, DQ, or
// : Distributed RAM).
// Version : 3.2
// : EBRs use SYNC resets instead of ASYNC resets.
// Version : 3.5
// : Added read-after-write hazard resolution when using true
// : dual-port EBRs
// =============================================================================
`include "lm32_include.v"
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_ram
(
// ----- Inputs -------
read_clk,
write_clk,
reset,
enable_read,
read_address,
enable_write,
write_address,
write_data,
write_enable,
// ----- Outputs -------
read_data
);
/*----------------------------------------------------------------------
Parameters
----------------------------------------------------------------------*/
parameter data_width = 1; // Width of the data ports
parameter address_width = 1; // Width of the address ports
/*----------------------------------------------------------------------
Inputs
----------------------------------------------------------------------*/
input read_clk; // Read clock
input write_clk; // Write clock
input reset; // Reset
input enable_read; // Access enable
input [address_width-1:0] read_address; // Read/write address
input enable_write; // Access enable
input [address_width-1:0] write_address;// Read/write address
input [data_width-1:0] write_data; // Data to write to specified address
input write_enable; // Write enable
/*----------------------------------------------------------------------
Outputs
----------------------------------------------------------------------*/
output [data_width-1:0] read_data; // Data read from specified addess
wire [data_width-1:0] read_data;
/*----------------------------------------------------------------------
Internal nets and registers
----------------------------------------------------------------------*/
reg [data_width-1:0] mem[0:(1<<address_width)-1]; // The RAM
reg [address_width-1:0] ra; // Registered read address
/*----------------------------------------------------------------------
Combinational Logic
----------------------------------------------------------------------*/
// Read port
assign read_data = mem[ra];
/*----------------------------------------------------------------------
Sequential Logic
----------------------------------------------------------------------*/
// Write port
integer i;
initial begin
for(i=0;i<=(1<<address_width)-1;i=i+1)
mem[i] = 0;
end
always @(posedge write_clk)
if ((write_enable == `TRUE) && (enable_write == `TRUE))
mem[write_address] <= write_data;
// Register read address for use on next cycle
always @(posedge read_clk)
if (enable_read)
ra <= read_address;
endmodule
-- Work-alike to lm32_ram.v, but using generic_simple_dpram
library ieee;
use ieee.std_logic_1164.all;
library work;
use work.genram_pkg.all;
entity lm32_ram is
generic(
data_width : natural := 1;
address_width : natural := 1);
port(
read_clk : in std_logic;
write_clk : in std_logic;
reset : in std_logic;
enable_read : in std_logic;
read_address : in std_logic_vector(address_width-1 downto 0);
enable_write : in std_logic;
write_address : in std_logic_vector(address_width-1 downto 0);
write_data : in std_logic_vector(data_width -1 downto 0);
write_enable : in std_logic;
read_data : out std_logic_vector(data_width -1 downto 0));
end lm32_ram;
architecture syn of lm32_ram is
signal wea : std_logic;
signal reb : std_logic;
-- Emulate read-enable using another bypass
signal old_data : std_logic_vector(data_width-1 downto 0);
signal new_data : std_logic_vector(data_width-1 downto 0);
signal data : std_logic_vector(data_width-1 downto 0);
begin
wea <= enable_write and write_enable;
ram : generic_simple_dpram
generic map(
g_data_width => data_width,
g_size => 2**address_width,
g_with_byte_enable => false,
g_addr_conflict_resolution => "write_first",
g_dual_clock => false) -- read_clk always = write_clk in LM32
port map(
clka_i => read_clk,
wea_i => wea,
aa_i => write_address,
da_i => write_data,
clkb_i => write_clk,
ab_i => read_address,
qb_o => new_data);
data <= old_data when reb='0' else new_data;
read_data <= data;
main : process(read_clk) is
begin
if rising_edge(read_clk) then
old_data <= data;
reb <= enable_read;
end if;
end process;
end syn;
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_shifter.v
// Title : Barrel shifter
// Dependencies : lm32_include.v
// Version : 6.1.17
// : Initial Release
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : No Change
// =============================================================================
`include "lm32_include.v"
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_shifter (
// ----- Inputs -------
clk_i,
rst_i,
stall_x,
direction_x,
sign_extend_x,
operand_0_x,
operand_1_x,
// ----- Outputs -------
shifter_result_m
);
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
input rst_i; // Reset
input stall_x; // Stall instruction in X stage
input direction_x; // Direction to shift
input sign_extend_x; // Whether shift is arithmetic (1'b1) or logical (1'b0)
input [`LM32_WORD_RNG] operand_0_x; // Operand to shift
input [`LM32_WORD_RNG] operand_1_x; // Operand that specifies how many bits to shift by
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
output [`LM32_WORD_RNG] shifter_result_m; // Result of shift
wire [`LM32_WORD_RNG] shifter_result_m;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
reg direction_m;
wire [`LM32_WORD_WIDTH*2-1:0] right_shift_result_x;
reg [`LM32_WORD_RNG] left_shift_result;
reg [`LM32_WORD_RNG] right_shift_result_m;
reg [`LM32_WORD_RNG] left_shift_operand;
wire [`LM32_WORD_RNG] right_shift_operand;
wire fill_value;
wire [`LM32_WORD_RNG] right_shift_in;
integer shift_idx_0;
integer shift_idx_1;
/////////////////////////////////////////////////////
// Combinational Logic
/////////////////////////////////////////////////////
// Select operands - To perform a left shift, we reverse the bits and perform a right shift
always @(*)
begin
for (shift_idx_0 = 0; shift_idx_0 < `LM32_WORD_WIDTH; shift_idx_0 = shift_idx_0 + 1)
left_shift_operand[`LM32_WORD_WIDTH-1-shift_idx_0] = operand_0_x[shift_idx_0];
end
assign right_shift_operand = direction_x == `LM32_SHIFT_OP_LEFT ? left_shift_operand : operand_0_x;
// Determine fill value for right shift - Sign bit for arithmetic shift, or zero for logical shift
assign fill_value = (sign_extend_x == `TRUE) && (direction_x == `LM32_SHIFT_OP_RIGHT)
? operand_0_x[`LM32_WORD_WIDTH-1]
: 1'b0;
// Determine bits to shift in for right shift or rotate
assign right_shift_in = {`LM32_WORD_WIDTH{fill_value}};
// Determine the result of the shifter
assign right_shift_result_x = {right_shift_in, right_shift_operand} >> operand_1_x[`LM32_SHIFT_RNG];
// Reverse bits to get left shift result
always @(*)
begin
for (shift_idx_1 = 0; shift_idx_1 < `LM32_WORD_WIDTH; shift_idx_1 = shift_idx_1 + 1)
left_shift_result[`LM32_WORD_WIDTH-1-shift_idx_1] = right_shift_result_m[shift_idx_1];
end
// Select result
assign shifter_result_m = direction_m == `LM32_SHIFT_OP_LEFT ? left_shift_result : right_shift_result_m;
/////////////////////////////////////////////////////
// Sequential Logic
/////////////////////////////////////////////////////
// Perform right shift
always @(posedge clk_i `CFG_RESET_SENSITIVITY)
begin
if (rst_i == `TRUE)
begin
right_shift_result_m <= {`LM32_WORD_WIDTH{1'b0}};
direction_m <= `FALSE;
end
else
begin
if (stall_x == `FALSE)
begin
right_shift_result_m <= right_shift_result_x[`LM32_WORD_RNG];
direction_m <= direction_x;
end
end
end
endmodule
// =============================================================================
// COPYRIGHT NOTICE
// Copyright 2006 (c) Lattice Semiconductor Corporation
// ALL RIGHTS RESERVED
// This confidential and proprietary software may be used only as authorised by
// a licensing agreement from Lattice Semiconductor Corporation.
// The entire notice above must be reproduced on all authorized copies and
// copies may only be made to the extent permitted by a licensing agreement from
// Lattice Semiconductor Corporation.
//
// Lattice Semiconductor Corporation TEL : 1-800-Lattice (USA and Canada)
// 5555 NE Moore Court 408-826-6000 (other locations)
// Hillsboro, OR 97124 web : http://www.latticesemi.com/
// U.S.A email: techsupport@latticesemi.com
// =============================================================================/
// FILE DETAILS
// Project : LatticeMico32
// File : lm32_top.v
// Title : Top-level of CPU.
// Dependencies : lm32_include.v
// Version : 6.1.17
// : removed SPI - 04/12/07
// Version : 7.0SP2, 3.0
// : No Change
// Version : 3.1
// : No Change
// =============================================================================
`include "system_conf.v"
`include "lm32_include.v"
/////////////////////////////////////////////////////
// Module interface
/////////////////////////////////////////////////////
module lm32_top (
// ----- Inputs -------
clk_i,
rst_i,
// From external devices
//`ifdef CFG_INTERRUPTS_ENABLED
interrupt,
//`endif
// From user logic
`ifdef CFG_USER_ENABLED
user_result,
user_complete,
`endif
`ifdef CFG_IWB_ENABLED
// Instruction Wishbone master
I_DAT_I,
I_ACK_I,
I_ERR_I,
I_RTY_I,
`endif
// Data Wishbone master
D_DAT_I,
D_ACK_I,
D_ERR_I,
D_RTY_I,
// ----- Outputs -------
`ifdef CFG_USER_ENABLED
user_valid,
user_opcode,
user_operand_0,
user_operand_1,
`endif
`ifdef CFG_IWB_ENABLED
// Instruction Wishbone master
I_DAT_O,
I_ADR_O,
I_CYC_O,
I_SEL_O,
I_STB_O,
I_WE_O,
I_CTI_O,
I_LOCK_O,
I_BTE_O,
`endif
// Data Wishbone master
D_DAT_O,
D_ADR_O,
D_CYC_O,
D_SEL_O,
D_STB_O,
D_WE_O,
D_CTI_O,
D_LOCK_O,
D_BTE_O
);
parameter eba_reset = 32'h00000000;
/////////////////////////////////////////////////////
// Inputs
/////////////////////////////////////////////////////
input clk_i; // Clock
input rst_i; // Reset
//`ifdef CFG_INTERRUPTS_ENABLED
input [`LM32_INTERRUPT_RNG] interrupt; // Interrupt pins
//`endif
`ifdef CFG_USER_ENABLED
input [`LM32_WORD_RNG] user_result; // User-defined instruction result
input user_complete; // Indicates the user-defined instruction result is valid
`endif
`ifdef CFG_IWB_ENABLED
input [`LM32_WORD_RNG] I_DAT_I; // Instruction Wishbone interface read data
input I_ACK_I; // Instruction Wishbone interface acknowledgement
input I_ERR_I; // Instruction Wishbone interface error
input I_RTY_I; // Instruction Wishbone interface retry
`endif
input [`LM32_WORD_RNG] D_DAT_I; // Data Wishbone interface read data
input D_ACK_I; // Data Wishbone interface acknowledgement
input D_ERR_I; // Data Wishbone interface error
input D_RTY_I; // Data Wishbone interface retry
/////////////////////////////////////////////////////
// Outputs
/////////////////////////////////////////////////////
`ifdef CFG_USER_ENABLED
output user_valid; // Indicates that user_opcode and user_operand_* are valid
wire user_valid;
output [`LM32_USER_OPCODE_RNG] user_opcode; // User-defined instruction opcode
reg [`LM32_USER_OPCODE_RNG] user_opcode;
output [`LM32_WORD_RNG] user_operand_0; // First operand for user-defined instruction
wire [`LM32_WORD_RNG] user_operand_0;
output [`LM32_WORD_RNG] user_operand_1; // Second operand for user-defined instruction
wire [`LM32_WORD_RNG] user_operand_1;
`endif
`ifdef CFG_IWB_ENABLED
output [`LM32_WORD_RNG] I_DAT_O; // Instruction Wishbone interface write data
wire [`LM32_WORD_RNG] I_DAT_O;
output [`LM32_WORD_RNG] I_ADR_O; // Instruction Wishbone interface address
wire [`LM32_WORD_RNG] I_ADR_O;
output I_CYC_O; // Instruction Wishbone interface cycle
wire I_CYC_O;
output [`LM32_BYTE_SELECT_RNG] I_SEL_O; // Instruction Wishbone interface byte select
wire [`LM32_BYTE_SELECT_RNG] I_SEL_O;
output I_STB_O; // Instruction Wishbone interface strobe
wire I_STB_O;
output I_WE_O; // Instruction Wishbone interface write enable
wire I_WE_O;
output [`LM32_CTYPE_RNG] I_CTI_O; // Instruction Wishbone interface cycle type
wire [`LM32_CTYPE_RNG] I_CTI_O;
output I_LOCK_O; // Instruction Wishbone interface lock bus
wire I_LOCK_O;
output [`LM32_BTYPE_RNG] I_BTE_O; // Instruction Wishbone interface burst type
wire [`LM32_BTYPE_RNG] I_BTE_O;
`endif
output [`LM32_WORD_RNG] D_DAT_O; // Data Wishbone interface write data
wire [`LM32_WORD_RNG] D_DAT_O;
output [`LM32_WORD_RNG] D_ADR_O; // Data Wishbone interface address
wire [`LM32_WORD_RNG] D_ADR_O;
output D_CYC_O; // Data Wishbone interface cycle
wire D_CYC_O;
output [`LM32_BYTE_SELECT_RNG] D_SEL_O; // Data Wishbone interface byte select
wire [`LM32_BYTE_SELECT_RNG] D_SEL_O;
output D_STB_O; // Data Wishbone interface strobe
wire D_STB_O;
output D_WE_O; // Data Wishbone interface write enable
wire D_WE_O;
output [`LM32_CTYPE_RNG] D_CTI_O; // Data Wishbone interface cycle type
wire [`LM32_CTYPE_RNG] D_CTI_O;
output D_LOCK_O; // Date Wishbone interface lock bus
wire D_LOCK_O;
output [`LM32_BTYPE_RNG] D_BTE_O; // Data Wishbone interface burst type
wire [`LM32_BTYPE_RNG] D_BTE_O;
/////////////////////////////////////////////////////
// Internal nets and registers
/////////////////////////////////////////////////////
`ifdef CFG_JTAG_ENABLED
// Signals between JTAG interface and CPU
wire [`LM32_BYTE_RNG] jtag_reg_d;
wire [`LM32_BYTE_RNG] jtag_reg_q;
wire jtag_update;
wire [2:0] jtag_reg_addr_d;
wire [2:0] jtag_reg_addr_q;
wire jtck;
wire jrstn;
`endif
// TODO: get the trace signals out
`ifdef CFG_TRACE_ENABLED
// PC trace signals
wire [`LM32_PC_RNG] trace_pc; // PC to trace (address of next non-sequential instruction)
wire trace_pc_valid; // Indicates that a new trace PC is valid
wire trace_exception; // Indicates an exception has occured
wire [`LM32_EID_RNG] trace_eid; // Indicates what type of exception has occured
wire trace_eret; // Indicates an eret instruction has been executed
`ifdef CFG_DEBUG_ENABLED
wire trace_bret; // Indicates a bret instruction has been executed
`endif
`endif
/////////////////////////////////////////////////////
// Functions
/////////////////////////////////////////////////////
`include "lm32_functions.v"
/////////////////////////////////////////////////////
// Instantiations
/////////////////////////////////////////////////////
// LM32 CPU
lm32_cpu
#(
.eba_reset(eba_reset)
) cpu (
// ----- Inputs -------
.clk_i (clk_i),
`ifdef CFG_EBR_NEGEDGE_REGISTER_FILE
.clk_n_i (clk_n),
`endif
.rst_i (rst_i),
// From external devices
`ifdef CFG_INTERRUPTS_ENABLED
.interrupt (interrupt),
`endif
// From user logic
`ifdef CFG_USER_ENABLED
.user_result (user_result),
.user_complete (user_complete),
`endif
`ifdef CFG_JTAG_ENABLED
// From JTAG
.jtag_clk (jtck),
.jtag_update (jtag_update),
.jtag_reg_q (jtag_reg_q),
.jtag_reg_addr_q (jtag_reg_addr_q),
`endif
`ifdef CFG_IWB_ENABLED
// Instruction Wishbone master
.I_DAT_I (I_DAT_I),
.I_ACK_I (I_ACK_I),
.I_ERR_I (I_ERR_I),
.I_RTY_I (I_RTY_I),
`endif
// Data Wishbone master
.D_DAT_I (D_DAT_I),
.D_ACK_I (D_ACK_I),
.D_ERR_I (D_ERR_I),
.D_RTY_I (D_RTY_I),
// ----- Outputs -------
`ifdef CFG_TRACE_ENABLED
.trace_pc (trace_pc),
.trace_pc_valid (trace_pc_valid),
.trace_exception (trace_exception),
.trace_eid (trace_eid),
.trace_eret (trace_eret),
`ifdef CFG_DEBUG_ENABLED
.trace_bret (trace_bret),
`endif
`endif
`ifdef CFG_JTAG_ENABLED
.jtag_reg_d (jtag_reg_d),
.jtag_reg_addr_d (jtag_reg_addr_d),
`endif
`ifdef CFG_USER_ENABLED
.user_valid (user_valid),
.user_opcode (user_opcode),
.user_operand_0 (user_operand_0),
.user_operand_1 (user_operand_1),
`endif
`ifdef CFG_IWB_ENABLED
// Instruction Wishbone master
.I_DAT_O (I_DAT_O),
.I_ADR_O (I_ADR_O),
.I_CYC_O (I_CYC_O),
.I_SEL_O (I_SEL_O),
.I_STB_O (I_STB_O),
.I_WE_O (I_WE_O),
.I_CTI_O (I_CTI_O),
.I_LOCK_O (I_LOCK_O),
.I_BTE_O (I_BTE_O),
`endif
// Data Wishbone master
.D_DAT_O (D_DAT_O),
.D_ADR_O (D_ADR_O),
.D_CYC_O (D_CYC_O),
.D_SEL_O (D_SEL_O),
.D_STB_O (D_STB_O),
.D_WE_O (D_WE_O),
.D_CTI_O (D_CTI_O),
.D_LOCK_O (D_LOCK_O),
.D_BTE_O (D_BTE_O)
);
`ifdef CFG_JTAG_ENABLED
// JTAG cores
jtag_cores jtag_cores (
// ----- Inputs -----
.reg_d (jtag_reg_d),
.reg_addr_d (jtag_reg_addr_d),
// ----- Outputs -----
.reg_update (jtag_update),
.reg_q (jtag_reg_q),
.reg_addr_q (jtag_reg_addr_q),
.jtck (jtck),
.jrstn (jrstn)
);
`endif
endmodule
------------------------------------------------------------------------------
-- Title : Wishbone Serial LCD controller
-- Project : General Cores
------------------------------------------------------------------------------
-- File : wb_serial_lcd.vhd
-- Author : Wesley W. Terpstra
-- Company : GSI
-- Created : 2013-02-22
-- Last update: 2013-02-22
-- Platform : FPGA-generic
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Frame-buffer for driving an ISO01RGB display
-------------------------------------------------------------------------------
-- Copyright (c) 2013 GSI
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2013-02-22 1.0 terpstra Created
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wishbone_pkg.all;
use work.genram_pkg.all;
entity wb_serial_lcd is
generic(
g_cols : natural := 40;
g_rows : natural := 24;
g_hold : natural := 15; -- How many times to repeat a line (for sharpness)
g_wait : natural := 1); -- How many cycles per state change (for 20MHz timing)
port(
slave_clk_i : in std_logic;
slave_rstn_i : in std_logic;
slave_i : in t_wishbone_slave_in;
slave_o : out t_wishbone_slave_out;
di_clk_i : in std_logic;
di_scp_o : out std_logic;
di_lp_o : out std_logic;
di_flm_o : out std_logic;
di_dat_o : out std_logic);
end entity;
architecture rtl of wb_serial_lcd is
type t_state is (CLK_HIGH, FLM_HIGH, LP_HIGH, SLEEP1, SLEEP2,
CLK_LOW, SLEEP3, LP_LOW, FLM_LOW, SET_DATA);
constant c_lo : natural := f_ceil_log2((g_cols+31)/32);
constant c_hi : natural := f_ceil_log2(g_rows);
constant c_bits : natural := c_lo+c_hi;
constant c_len : natural := g_cols*g_hold;
signal r_state : t_state := SET_DATA;
signal r_row : integer range 0 to g_rows-1 := 0;
signal r_col : integer range 0 to c_len -1 := 0;
signal r_wait : integer range 0 to g_wait-1 := 0;
signal r_lp : std_logic := '0';
signal r_flm : std_logic := '0';
signal r_ack : std_logic := '0';
signal s_wea : std_logic;
signal s_qa : std_logic_vector(31 downto 0);
signal s_qb : std_logic_vector(31 downto 0);
signal s_ab : std_logic_vector(c_bits-1 downto 0);
begin
slave_o.rty <= '0';
slave_o.err <= '0';
slave_o.stall <= '0';
s_wea <= slave_i.cyc and slave_i.stb and slave_i.we;
s_ab(c_bits-1 downto c_lo) <= std_logic_vector(to_unsigned(r_row, c_hi));
s_ab(c_lo -1 downto 0) <= std_logic_vector(to_unsigned(r_col, c_lo+5)(c_lo+4 downto 5));
mem : generic_dpram
generic map(
g_data_width => 32,
g_size => 2**c_bits,
g_with_byte_enable => true,
g_dual_clock => true)
port map(
clka_i => slave_clk_i,
bwea_i => slave_i.sel,
wea_i => s_wea,
aa_i => slave_i.adr(c_bits+1 downto 2),
da_i => slave_i.dat,
qa_o => s_qa,
clkb_i => di_clk_i,
bweb_i => (others => '0'),
web_i => '0',
ab_i => s_ab,
db_i => (others => '0'),
qb_o => s_qb);
-- Provide WB access to the frame buffer
wb : process(slave_clk_i) is
begin
if rising_edge(slave_clk_i) then
r_ack <= slave_i.cyc and slave_i.stb;
slave_o.ack <= r_ack;
slave_o.dat <= s_qa;
end if;
end process;
-- Draw the frame buffer to the display
main : process(di_clk_i) is
begin
if rising_edge(di_clk_i) then
if r_wait /= g_wait-1 then
r_wait <= r_wait + 1;
else
r_wait <= 0;
case r_state is
when CLK_HIGH =>
di_scp_o <= '1';
r_state <= FLM_HIGH;
when FLM_HIGH =>
di_flm_o <= r_flm;
r_state <= LP_HIGH;
when LP_HIGH =>
di_lp_o <= r_lp;
r_state <= SLEEP1;
when SLEEP1 =>
r_state <= SLEEP2;
when SLEEP2 =>
r_state <= CLK_LOW;
when CLK_LOW =>
di_scp_o <= '0';
r_state <= SLEEP3;
when SLEEP3 =>
r_state <= LP_LOW;
when LP_LOW =>
di_lp_o <= '0';
r_state <= FLM_LOW;
when FLM_LOW =>
di_flm_o <= '0';
r_state <= SET_DATA;
when SET_DATA =>
di_dat_o <= s_qb(to_integer(31 - to_unsigned(r_col, 5)));
r_state <= CLK_HIGH;
if r_col /= 0 then
r_lp <= '0';
r_flm <= '0';
r_col <= r_col-1;
r_row <= r_row;
elsif r_row = 0 then
r_lp <= '1';
r_flm <= '1';
r_col <= c_len-1;
r_row <= r_row+1;
elsif r_row /= g_rows-1 then
r_lp <= '1';
r_flm <= '0';
r_col <= c_len-1;
r_row <= r_row+1;
else
r_lp <= '1';
r_flm <= '0';
r_col <= c_len-1;
r_row <= 0;
end if;
end case;
end if;
end if;
end process;
end rtl;
files = ["simple_pwm_wbgen2_pkg.vhd",
"simple_pwm_wb.vhd",
"wb_simple_pwm.vhd",
"xwb_simple_pwm.vhd"
];
---------------------------------------------------------------------------------------
-- Title : Wishbone slave core for Simple Pulse Width Modulation Controller
---------------------------------------------------------------------------------------
-- File : simple_pwm_wb.vhd
-- Author : auto-generated by wbgen2 from simple_pwm_wb.wb
-- Created : Mon May 20 15:36:44 2013
-- Standard : VHDL'87
---------------------------------------------------------------------------------------
-- THIS FILE WAS GENERATED BY wbgen2 FROM SOURCE FILE simple_pwm_wb.wb
-- DO NOT HAND-EDIT UNLESS IT'S ABSOLUTELY NECESSARY!
---------------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.spwm_wbgen2_pkg.all;
entity simple_pwm_wb is
port (
rst_n_i : in std_logic;
clk_sys_i : in std_logic;
wb_adr_i : in std_logic_vector(3 downto 0);
wb_dat_i : in std_logic_vector(31 downto 0);
wb_dat_o : out std_logic_vector(31 downto 0);
wb_cyc_i : in std_logic;
wb_sel_i : in std_logic_vector(3 downto 0);
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_ack_o : out std_logic;
wb_stall_o : out std_logic;
regs_i : in t_spwm_in_registers;
regs_o : out t_spwm_out_registers
);
end simple_pwm_wb;
architecture syn of simple_pwm_wb is
signal ack_sreg : std_logic_vector(9 downto 0);
signal rddata_reg : std_logic_vector(31 downto 0);
signal wrdata_reg : std_logic_vector(31 downto 0);
signal bwsel_reg : std_logic_vector(3 downto 0);
signal rwaddr_reg : std_logic_vector(3 downto 0);
signal ack_in_progress : std_logic ;
signal wr_int : std_logic ;
signal rd_int : std_logic ;
signal allones : std_logic_vector(31 downto 0);
signal allzeros : std_logic_vector(31 downto 0);
begin
-- Some internal signals assignments. For (foreseen) compatibility with other bus standards.
wrdata_reg <= wb_dat_i;
bwsel_reg <= wb_sel_i;
rd_int <= wb_cyc_i and (wb_stb_i and (not wb_we_i));
wr_int <= wb_cyc_i and (wb_stb_i and wb_we_i);
allones <= (others => '1');
allzeros <= (others => '0');
--
-- Main register bank access process.
process (clk_sys_i, rst_n_i)
begin
if (rst_n_i = '0') then
ack_sreg <= "0000000000";
ack_in_progress <= '0';
rddata_reg <= "00000000000000000000000000000000";
regs_o.cr_presc_load_o <= '0';
regs_o.cr_period_load_o <= '0';
regs_o.dr0_load_o <= '0';
regs_o.dr1_load_o <= '0';
regs_o.dr2_load_o <= '0';
regs_o.dr3_load_o <= '0';
regs_o.dr4_load_o <= '0';
regs_o.dr5_load_o <= '0';
regs_o.dr6_load_o <= '0';
regs_o.dr7_load_o <= '0';
elsif rising_edge(clk_sys_i) then
-- advance the ACK generator shift register
ack_sreg(8 downto 0) <= ack_sreg(9 downto 1);
ack_sreg(9) <= '0';
if (ack_in_progress = '1') then
if (ack_sreg(0) = '1') then
regs_o.cr_presc_load_o <= '0';
regs_o.cr_period_load_o <= '0';
regs_o.dr0_load_o <= '0';
regs_o.dr1_load_o <= '0';
regs_o.dr2_load_o <= '0';
regs_o.dr3_load_o <= '0';
regs_o.dr4_load_o <= '0';
regs_o.dr5_load_o <= '0';
regs_o.dr6_load_o <= '0';
regs_o.dr7_load_o <= '0';
ack_in_progress <= '0';
else
regs_o.cr_presc_load_o <= '0';
regs_o.cr_period_load_o <= '0';
regs_o.dr0_load_o <= '0';
regs_o.dr1_load_o <= '0';
regs_o.dr2_load_o <= '0';
regs_o.dr3_load_o <= '0';
regs_o.dr4_load_o <= '0';
regs_o.dr5_load_o <= '0';
regs_o.dr6_load_o <= '0';
regs_o.dr7_load_o <= '0';
end if;
else
if ((wb_cyc_i = '1') and (wb_stb_i = '1')) then
case rwaddr_reg(3 downto 0) is
when "0000" =>
if (wb_we_i = '1') then
regs_o.cr_presc_load_o <= '1';
regs_o.cr_period_load_o <= '1';
end if;
rddata_reg(15 downto 0) <= regs_i.cr_presc_i;
rddata_reg(31 downto 16) <= regs_i.cr_period_i;
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "0001" =>
if (wb_we_i = '1') then
end if;
rddata_reg(3 downto 0) <= regs_i.sr_n_channels_i;
rddata_reg(4) <= 'X';
rddata_reg(5) <= 'X';
rddata_reg(6) <= 'X';
rddata_reg(7) <= 'X';
rddata_reg(8) <= 'X';
rddata_reg(9) <= 'X';
rddata_reg(10) <= 'X';
rddata_reg(11) <= 'X';
rddata_reg(12) <= 'X';
rddata_reg(13) <= 'X';
rddata_reg(14) <= 'X';
rddata_reg(15) <= 'X';
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "0010" =>
if (wb_we_i = '1') then
regs_o.dr0_load_o <= '1';
end if;
rddata_reg(15 downto 0) <= regs_i.dr0_i;
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "0011" =>
if (wb_we_i = '1') then
regs_o.dr1_load_o <= '1';
end if;
rddata_reg(15 downto 0) <= regs_i.dr1_i;
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "0100" =>
if (wb_we_i = '1') then
regs_o.dr2_load_o <= '1';
end if;
rddata_reg(15 downto 0) <= regs_i.dr2_i;
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "0101" =>
if (wb_we_i = '1') then
regs_o.dr3_load_o <= '1';
end if;
rddata_reg(15 downto 0) <= regs_i.dr3_i;
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "0110" =>
if (wb_we_i = '1') then
regs_o.dr4_load_o <= '1';
end if;
rddata_reg(15 downto 0) <= regs_i.dr4_i;
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "0111" =>
if (wb_we_i = '1') then
regs_o.dr5_load_o <= '1';
end if;
rddata_reg(15 downto 0) <= regs_i.dr5_i;
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "1000" =>
if (wb_we_i = '1') then
regs_o.dr6_load_o <= '1';
end if;
rddata_reg(15 downto 0) <= regs_i.dr6_i;
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "1001" =>
if (wb_we_i = '1') then
regs_o.dr7_load_o <= '1';
end if;
rddata_reg(15 downto 0) <= regs_i.dr7_i;
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when others =>
-- prevent the slave from hanging the bus on invalid address
ack_in_progress <= '1';
ack_sreg(0) <= '1';
end case;
end if;
end if;
end if;
end process;
-- Drive the data output bus
wb_dat_o <= rddata_reg;
-- Prescaler Ratio
regs_o.cr_presc_o <= wrdata_reg(15 downto 0);
-- Period
regs_o.cr_period_o <= wrdata_reg(31 downto 16);
-- Channel count
-- Value
regs_o.dr0_o <= wrdata_reg(15 downto 0);
-- Value
regs_o.dr1_o <= wrdata_reg(15 downto 0);
-- Value
regs_o.dr2_o <= wrdata_reg(15 downto 0);
-- Value
regs_o.dr3_o <= wrdata_reg(15 downto 0);
-- Value
regs_o.dr4_o <= wrdata_reg(15 downto 0);
-- Value
regs_o.dr5_o <= wrdata_reg(15 downto 0);
-- Value
regs_o.dr6_o <= wrdata_reg(15 downto 0);
-- Value
regs_o.dr7_o <= wrdata_reg(15 downto 0);
rwaddr_reg <= wb_adr_i;
wb_stall_o <= (not ack_sreg(0)) and (wb_stb_i and wb_cyc_i);
-- ACK signal generation. Just pass the LSB of ACK counter.
wb_ack_o <= ack_sreg(0);
end syn;
-- -*- Mode: LUA; tab-width: 2 -*-
-------------------------------------------------------------------------------
-- Title : Simple PWM controller
-- Project : General Cores Collection
-------------------------------------------------------------------------------
-- File : simple_pwm_wb.wb
-- Author : Tomasz Włostowski
-- Company : CERN BE-CO-HT
-- Created : 2012-12-10
-- Last update: 2013-01-09
-------------------------------------------------------------------------------
-- Description: A simple, multichannel PWM controller (register layout)
-------------------------------------------------------------------------------
--
-- Copyright (c) 2013 CERN / BE-CO-HT
--
-- This source file is free software; you can redistribute it
-- and/or modify it under the terms of the GNU Lesser General
-- Public License as published by the Free Software Foundation;
-- either version 2.1 of the License, or (at your option) any
-- later version.
--
-- This source 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 Lesser General Public License for more
-- details.
--
-- You should have received a copy of the GNU Lesser General
-- Public License along with this source; if not, download it
-- from http://www.gnu.org/licenses/lgpl-2.1l.html
--
-------------------------------------------------------------------------------
peripheral {
name = "Simple Pulse Width Modulation Controller";
description = "A very simple multichannel PWM controller.";
prefix = "spwm";
hdl_entity = "simple_pwm_wb";
reg {
name = "Control Register";
prefix = "CR";
field {
name = "Prescaler Ratio";
description = "PWM Base clock prescaler. Divides the system clock to obtain the PWM counter clock. The division ratio is (PRESC + 1).";
type = SLV;
size = 16;
prefix = "PRESC";
access_bus = READ_WRITE;
access_dev = READ_WRITE;
load = LOAD_EXT;
};
field {
name = "Period";
description = "PWM Cycle Period. The real-time period value <code>PERIOD * (PRESC + 1) * t_clk_sys.</code>. Acceptable values: 0..65534.";
type = SLV;
size = 16;
prefix = "PERIOD";
access_bus = READ_WRITE;
access_dev = READ_WRITE;
load = LOAD_EXT;
};
};
reg {
name = "Status Register";
prefix = "SR";
field {
name = "Channel count";
description = "Number of channels supported by this particular implementation, from 1 to 8. ";
type = SLV;
size = 4;
prefix = "N_CHANNELS";
access_bus = READ_ONLY;
access_dev = WRITE_ONLY;
};
};
};
drive_reg_template =
{
reg {
name = "Channel %d Drive Register";
description = "Current PWM duty cycle for channel %d.";
prefix = "DR%d";
field {
name = "Value";
type = SLV;
size = 16;
access_bus = READ_WRITE;
access_dev = READ_WRITE;
load = LOAD_EXT;
};
};
};
function generate_dregs(n)
local i;
for i=0,n-1 do
local T=deepcopy(drive_reg_template);
foreach_reg({TYPE_REG}, function(r)
r.name = string.format(r.name, i);
r.prefix = string.format(r.prefix, i);
print(r.name)
end, T);
table_join(periph, T);
end
end
generate_dregs(8);
---------------------------------------------------------------------------------------
-- Title : Wishbone slave core for Simple Pulse Width Modulation Controller
---------------------------------------------------------------------------------------
-- File : simple_pwm_wbgen2_pkg.vhd
-- Author : auto-generated by wbgen2 from simple_pwm_wb.wb
-- Created : Mon May 20 15:36:44 2013
-- Standard : VHDL'87
---------------------------------------------------------------------------------------
-- THIS FILE WAS GENERATED BY wbgen2 FROM SOURCE FILE simple_pwm_wb.wb
-- DO NOT HAND-EDIT UNLESS IT'S ABSOLUTELY NECESSARY!
---------------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
package spwm_wbgen2_pkg is
-- Input registers (user design -> WB slave)
type t_spwm_in_registers is record
cr_presc_i : std_logic_vector(15 downto 0);
cr_period_i : std_logic_vector(15 downto 0);
sr_n_channels_i : std_logic_vector(3 downto 0);
dr0_i : std_logic_vector(15 downto 0);
dr1_i : std_logic_vector(15 downto 0);
dr2_i : std_logic_vector(15 downto 0);
dr3_i : std_logic_vector(15 downto 0);
dr4_i : std_logic_vector(15 downto 0);
dr5_i : std_logic_vector(15 downto 0);
dr6_i : std_logic_vector(15 downto 0);
dr7_i : std_logic_vector(15 downto 0);
end record;
constant c_spwm_in_registers_init_value: t_spwm_in_registers := (
cr_presc_i => (others => '0'),
cr_period_i => (others => '0'),
sr_n_channels_i => (others => '0'),
dr0_i => (others => '0'),
dr1_i => (others => '0'),
dr2_i => (others => '0'),
dr3_i => (others => '0'),
dr4_i => (others => '0'),
dr5_i => (others => '0'),
dr6_i => (others => '0'),
dr7_i => (others => '0')
);
-- Output registers (WB slave -> user design)
type t_spwm_out_registers is record
cr_presc_o : std_logic_vector(15 downto 0);
cr_presc_load_o : std_logic;
cr_period_o : std_logic_vector(15 downto 0);
cr_period_load_o : std_logic;
dr0_o : std_logic_vector(15 downto 0);
dr0_load_o : std_logic;
dr1_o : std_logic_vector(15 downto 0);
dr1_load_o : std_logic;
dr2_o : std_logic_vector(15 downto 0);
dr2_load_o : std_logic;
dr3_o : std_logic_vector(15 downto 0);
dr3_load_o : std_logic;
dr4_o : std_logic_vector(15 downto 0);
dr4_load_o : std_logic;
dr5_o : std_logic_vector(15 downto 0);
dr5_load_o : std_logic;
dr6_o : std_logic_vector(15 downto 0);
dr6_load_o : std_logic;
dr7_o : std_logic_vector(15 downto 0);
dr7_load_o : std_logic;
end record;
constant c_spwm_out_registers_init_value: t_spwm_out_registers := (
cr_presc_o => (others => '0'),
cr_presc_load_o => '0',
cr_period_o => (others => '0'),
cr_period_load_o => '0',
dr0_o => (others => '0'),
dr0_load_o => '0',
dr1_o => (others => '0'),
dr1_load_o => '0',
dr2_o => (others => '0'),
dr2_load_o => '0',
dr3_o => (others => '0'),
dr3_load_o => '0',
dr4_o => (others => '0'),
dr4_load_o => '0',
dr5_o => (others => '0'),
dr5_load_o => '0',
dr6_o => (others => '0'),
dr6_load_o => '0',
dr7_o => (others => '0'),
dr7_load_o => '0'
);
function "or" (left, right: t_spwm_in_registers) return t_spwm_in_registers;
function f_x_to_zero (x:std_logic) return std_logic;
function f_x_to_zero (x:std_logic_vector) return std_logic_vector;
end package;
package body spwm_wbgen2_pkg is
function f_x_to_zero (x:std_logic) return std_logic is
begin
if(x = 'X' or x = 'U') then
return '0';
else
return x;
end if;
end function;
function f_x_to_zero (x:std_logic_vector) return std_logic_vector is
variable tmp: std_logic_vector(x'length-1 downto 0);
begin
for i in 0 to x'length-1 loop
if(x(i) = 'X' or x(i) = 'U') then
tmp(i):= '0';
else
tmp(i):=x(i);
end if;
end loop;
return tmp;
end function;
function "or" (left, right: t_spwm_in_registers) return t_spwm_in_registers is
variable tmp: t_spwm_in_registers;
begin
tmp.cr_presc_i := f_x_to_zero(left.cr_presc_i) or f_x_to_zero(right.cr_presc_i);
tmp.cr_period_i := f_x_to_zero(left.cr_period_i) or f_x_to_zero(right.cr_period_i);
tmp.sr_n_channels_i := f_x_to_zero(left.sr_n_channels_i) or f_x_to_zero(right.sr_n_channels_i);
tmp.dr0_i := f_x_to_zero(left.dr0_i) or f_x_to_zero(right.dr0_i);
tmp.dr1_i := f_x_to_zero(left.dr1_i) or f_x_to_zero(right.dr1_i);
tmp.dr2_i := f_x_to_zero(left.dr2_i) or f_x_to_zero(right.dr2_i);
tmp.dr3_i := f_x_to_zero(left.dr3_i) or f_x_to_zero(right.dr3_i);
tmp.dr4_i := f_x_to_zero(left.dr4_i) or f_x_to_zero(right.dr4_i);
tmp.dr5_i := f_x_to_zero(left.dr5_i) or f_x_to_zero(right.dr5_i);
tmp.dr6_i := f_x_to_zero(left.dr6_i) or f_x_to_zero(right.dr6_i);
tmp.dr7_i := f_x_to_zero(left.dr7_i) or f_x_to_zero(right.dr7_i);
return tmp;
end function;
end package body;
-------------------------------------------------------------------------------
-- Title : Simple Wishbone PWM Controller
-- Project : General Cores Collection (gencores) library
-------------------------------------------------------------------------------
-- File : wb_simple_pwm.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-Co-HT
-- Created : 2012-12-18
-- Last update: 2012-12-20
-- Platform : FPGA-generic
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: A simple PWM controller, supporting up to 8 channels. Aside from
-- duty cycle control, all channels share period and base frequency settings,
-- contrillable via Wishbone
-------------------------------------------------------------------------------
--
-- This source file is free software; you can redistribute it
-- and/or modify it under the terms of the GNU Lesser General
-- Public License as published by the Free Software Foundation;
-- either version 2.1 of the License, or (at your option) any
-- later version.
--
-- This source 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 Lesser General Public License for more
-- details.
--
-- You should have received a copy of the GNU Lesser General
-- Public License along with this source; if not, download it
-- from http://www.gnu.org/licenses/lgpl-2.1.html
--
-------------------------------------------------------------------------------
library ieee;
use ieee.STD_LOGIC_1164.all;
use ieee.NUMERIC_STD.all;
use work.wishbone_pkg.all;
use work.spwm_wbgen2_pkg.all;
entity wb_simple_pwm is
generic(
g_num_channels : integer range 1 to 8;
g_default_period : integer range 0 to 255 := 0;
g_default_presc : integer range 0 to 255 := 0;
g_default_val : integer range 0 to 255 := 0;
g_interface_mode : t_wishbone_interface_mode := PIPELINED;
g_address_granularity : t_wishbone_address_granularity := BYTE
);
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
wb_adr_i : in std_logic_vector(5 downto 0);
wb_dat_i : in std_logic_vector(31 downto 0);
wb_dat_o : out std_logic_vector(31 downto 0);
wb_cyc_i : in std_logic;
wb_sel_i : in std_logic_vector(3 downto 0);
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_ack_o : out std_logic;
wb_stall_o : out std_logic;
pwm_o : out std_logic_vector(g_num_channels-1 downto 0)
);
end wb_simple_pwm;
architecture behavioral of wb_simple_pwm is
type t_drive_array is array(0 to g_num_channels-1) of std_logic_vector(15 downto 0);
procedure f_handle_dr_writes(index : integer; data : std_logic_vector; load : std_logic;
signal d : out t_drive_array) is
begin
if(index <= g_num_channels and load = '1') then
d(index-1) <= data;
end if;
end f_handle_dr_writes;
component simple_pwm_wb
port (
rst_n_i : in std_logic;
clk_sys_i : in std_logic;
wb_adr_i : in std_logic_vector(3 downto 0);
wb_dat_i : in std_logic_vector(31 downto 0);
wb_dat_o : out std_logic_vector(31 downto 0);
wb_cyc_i : in std_logic;
wb_sel_i : in std_logic_vector(3 downto 0);
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_ack_o : out std_logic;
wb_stall_o : out std_logic;
regs_i : in t_spwm_in_registers;
regs_o : out t_spwm_out_registers);
end component;
signal drive : t_drive_array;
signal regs_in : t_spwm_in_registers;
signal regs_out : t_spwm_out_registers;
signal tick : std_logic;
signal cntr_pre, cntr_main : unsigned(15 downto 0);
signal presc_val : std_logic_vector(15 downto 0);
signal period_val : std_logic_vector(15 downto 0);
begin -- behavioral
U_WB_Slave : simple_pwm_wb
port map (
rst_n_i => rst_n_i,
clk_sys_i => clk_sys_i,
wb_adr_i => wb_adr_i(5 downto 2),
wb_dat_i => wb_dat_i,
wb_dat_o => wb_dat_o,
wb_cyc_i => wb_cyc_i,
wb_sel_i => wb_sel_i,
wb_stb_i => wb_stb_i,
wb_we_i => wb_we_i,
wb_ack_o => wb_ack_o,
wb_stall_o => wb_stall_o,
regs_i => regs_in,
regs_o => regs_out);
-------------------------------------------------------------------------------
-- FIXME: this is ugly. Add register array support in wbgen2!
-------------------------------------------------------------------------------
p_drive_readback : process(drive)
begin
regs_in.dr0_i <= drive(0);
if(g_num_channels >= 2) then
regs_in.dr1_i <= drive(1);
end if;
if(g_num_channels >= 3) then
regs_in.dr2_i <= drive(2);
end if;
if(g_num_channels >= 4) then
regs_in.dr3_i <= drive(3);
end if;
if(g_num_channels >= 5) then
regs_in.dr4_i <= drive(4);
end if;
if(g_num_channels >= 6) then
regs_in.dr5_i <= drive(5);
end if;
if(g_num_channels >= 7) then
regs_in.dr6_i <= drive(6);
end if;
if(g_num_channels >= 8) then
regs_in.dr7_i <= drive(7);
end if;
end process;
p_drive_write : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
for i in 0 to g_num_channels-1 loop
drive(i) <= std_logic_vector(to_unsigned(g_default_val, 16));
end loop; -- i
else
f_handle_dr_writes(1, regs_out.dr0_o, regs_out.dr0_load_o, drive);
f_handle_dr_writes(2, regs_out.dr1_o, regs_out.dr1_load_o, drive);
f_handle_dr_writes(3, regs_out.dr2_o, regs_out.dr2_load_o, drive);
f_handle_dr_writes(4, regs_out.dr3_o, regs_out.dr3_load_o, drive);
f_handle_dr_writes(5, regs_out.dr4_o, regs_out.dr4_load_o, drive);
f_handle_dr_writes(6, regs_out.dr5_o, regs_out.dr5_load_o, drive);
f_handle_dr_writes(7, regs_out.dr6_o, regs_out.dr6_load_o, drive);
f_handle_dr_writes(8, regs_out.dr7_o, regs_out.dr7_load_o, drive);
end if;
end if;
end process;
regs_in.cr_presc_i <= presc_val;
regs_in.cr_period_i <= period_val;
load_cr: process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if(rst_n_i = '0') then
presc_val <= std_logic_vector(to_unsigned(g_default_presc, 16));
elsif(regs_out.cr_presc_load_o = '1') then
presc_val <= regs_out.cr_presc_o;
end if;
if(rst_n_i = '0') then
period_val <= std_logic_vector(to_unsigned(g_default_period, 16));
elsif(regs_out.cr_period_load_o = '1') then
period_val <= regs_out.cr_period_o;
end if;
end if;
end process;
p_prescaler : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' or std_logic_vector(cntr_pre) = presc_val then
cntr_pre <= (others => '0');
tick <= '1';
else
cntr_pre <= cntr_pre + 1;
tick <= '0';
end if;
end if;
end process;
p_main_counter : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if (tick = '1' and std_logic_vector(cntr_main) = period_val) or rst_n_i = '0' then
cntr_main <= (others => '0');
elsif(tick = '1') then
cntr_main <= cntr_main + 1;
end if;
end if;
end process;
p_comparators : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
pwm_o <= (others => '0');
else
for i in 0 to g_num_channels-1 loop
if(cntr_main < unsigned(drive(i))) then
pwm_o(i) <= '1';
else
pwm_o(i) <= '0';
end if;
end loop; -- i
end if;
end if;
end process;
end behavioral;
-------------------------------------------------------------------------------
-- Title : Simple Wishbone PWM Controller
-- Project : General Cores Collection (gencores) library
-------------------------------------------------------------------------------
-- File : xwb_simple_pwm.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-Co-HT
-- Created : 2012-12-18
-- Last update: 2012-12-20
-- Platform : FPGA-generic
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: A simple PWM controller, supporting up to 8 channels. Aside from
-- duty cycle control, all channels share period and base frequency settings,
-- contrillable via Wishbone
-------------------------------------------------------------------------------
--
-- This source file is free software; you can redistribute it
-- and/or modify it under the terms of the GNU Lesser General
-- Public License as published by the Free Software Foundation;
-- either version 2.1 of the License, or (at your option) any
-- later version.
--
-- This source 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 Lesser General Public License for more
-- details.
--
-- You should have received a copy of the GNU Lesser General
-- Public License along with this source; if not, download it
-- from http://www.gnu.org/licenses/lgpl-2.1.html
--
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
library work;
use work.wishbone_pkg.all;
entity xwb_simple_pwm is
generic (
g_num_channels : integer range 1 to 8;
g_default_period : integer range 0 to 255 := 0;
g_default_presc : integer range 0 to 255 := 0;
g_default_val : integer range 0 to 255 := 0;
g_interface_mode : t_wishbone_interface_mode := PIPELINED;
g_address_granularity : t_wishbone_address_granularity := BYTE);
port(
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
-- Wishbone
slave_i : in t_wishbone_slave_in;
slave_o : out t_wishbone_slave_out;
pwm_o : out std_logic_vector(g_num_channels-1 downto 0)
);
end xwb_simple_pwm;
architecture wrapper of xwb_simple_pwm is
component wb_simple_pwm
generic (
g_num_channels : integer range 1 to 8;
g_default_period : integer range 0 to 255;
g_default_presc : integer range 0 to 255;
g_default_val : integer range 0 to 255;
g_interface_mode : t_wishbone_interface_mode;
g_address_granularity : t_wishbone_address_granularity);
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
wb_adr_i : in std_logic_vector(5 downto 0);
wb_dat_i : in std_logic_vector(31 downto 0);
wb_dat_o : out std_logic_vector(31 downto 0);
wb_cyc_i : in std_logic;
wb_sel_i : in std_logic_vector(3 downto 0);
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_ack_o : out std_logic;
wb_stall_o : out std_logic;
pwm_o : out std_logic_vector(g_num_channels-1 downto 0));
end component;
begin -- rtl
U_Wrapped_PWM : wb_simple_pwm
generic map (
g_num_channels => g_num_channels,
g_default_period => g_default_period,
g_default_presc => g_default_presc,
g_default_val => g_default_val,
g_interface_mode => g_interface_mode,
g_address_granularity => g_address_granularity)
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
wb_adr_i => slave_i.adr(5 downto 0),
wb_dat_i => slave_i.dat,
wb_dat_o => slave_o.dat,
wb_cyc_i => slave_i.cyc,
wb_sel_i => slave_i.sel,
wb_stb_i => slave_i.stb,
wb_we_i => slave_i.we,
wb_ack_o => slave_o.ack,
wb_stall_o => slave_o.stall,
pwm_o => pwm_o);
slave_o.err <= '0';
slave_o.rty <= '0';
slave_o.int <= '0';
end wrapper;
files = ["wb_tics.vhd", "xwb_tics.vhd"];
\ No newline at end of file
-------------------------------------------------------------------------------
-- Title : WhiteRabbit PTP Core tics
-- Project : WhiteRabbit
-------------------------------------------------------------------------------
-- File : wb_tics.vhd
-- Author : Grzegorz Daniluk
-- Company : Elproma
-- Created : 2011-04-03
-- Last update: 2011-10-04
-- Platform : FPGA-generics
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description:
-- WB_TICS is a simple counter with wishbone interface. Each step of a counter
-- takes 1 usec. It is used by ptp-noposix as a replace of gettimeofday()
-- function.
-------------------------------------------------------------------------------
-- Copyright (c) 2011 Grzegorz Daniluk
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-04-03 1.0 greg.d Created
-- 2011-10-04 1.1 twlostow added wishbone adapter
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wishbone_pkg.all;
entity wb_tics is
generic (
g_interface_mode : t_wishbone_interface_mode := CLASSIC;
g_address_granularity : t_wishbone_address_granularity := WORD;
g_period : integer);
port(
rst_n_i : in std_logic;
clk_sys_i : in std_logic;
wb_adr_i : in std_logic_vector(3 downto 0);
wb_dat_i : in std_logic_vector(c_wishbone_data_width-1 downto 0);
wb_dat_o : out std_logic_vector(c_wishbone_data_width-1 downto 0);
wb_cyc_i : in std_logic;
wb_sel_i : in std_logic_vector(c_wishbone_data_width/8-1 downto 0);
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_ack_o : out std_logic;
wb_stall_o: out std_logic
);
end wb_tics;
architecture behaviour of wb_tics is
constant c_TICS_REG : std_logic_vector(1 downto 0) := "00";
signal cntr_div : unsigned(23 downto 0);
signal cntr_tics : unsigned(31 downto 0);
signal cntr_overflow : std_logic;
signal wb_in : t_wishbone_slave_in;
signal wb_out : t_wishbone_slave_out;
signal resized_addr : std_logic_vector(c_wishbone_address_width-1 downto 0);
begin
resized_addr(3 downto 0) <= wb_adr_i;
resized_addr(c_wishbone_address_width-1 downto 4) <= (others => '0');
U_Adapter : wb_slave_adapter
generic map (
g_master_use_struct => true,
g_master_mode => CLASSIC,
g_master_granularity => WORD,
g_slave_use_struct => false,
g_slave_mode => g_interface_mode,
g_slave_granularity => g_address_granularity)
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
master_i => wb_out,
master_o => wb_in,
sl_adr_i => resized_addr,
sl_dat_i => wb_dat_i,
sl_sel_i => wb_sel_i,
sl_cyc_i => wb_cyc_i,
sl_stb_i => wb_stb_i,
sl_we_i => wb_we_i,
sl_dat_o => wb_dat_o,
sl_ack_o => wb_ack_o,
sl_stall_o => wb_stall_o);
process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if(rst_n_i = '0') then
cntr_div <= (others => '0');
cntr_overflow <= '0';
else
if(cntr_div = g_period-1) then
cntr_div <= (others => '0');
cntr_overflow <= '1';
else
cntr_div <= cntr_div + 1;
cntr_overflow <= '0';
end if;
end if;
end if;
end process;
--usec counter
process(clk_sys_i)
begin
if(rising_edge(clk_sys_i)) then
if(rst_n_i = '0') then
cntr_tics <= (others => '0');
elsif(cntr_overflow = '1') then
cntr_tics <= cntr_tics + 1;
end if;
end if;
end process;
--Wishbone interface
process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if(rst_n_i = '0') then
wb_out.ack <= '0';
wb_out.dat <= (others => '0');
else
if(wb_in.stb = '1' and wb_in.cyc = '1') then
if(wb_in.we = '0') then
case wb_in.adr(1 downto 0) is
when c_TICS_REG =>
wb_out.dat <= std_logic_vector(cntr_tics);
when others =>
wb_out.dat <= (others => '0');
end case;
end if;
wb_out.ack <= '1';
else
wb_out.dat <= (others => '0');
wb_out.ack <= '0';
end if;
end if;
end if;
end process;
end behaviour;
-- todo: configurable interrupt, output compare, PWM?
library ieee;
use ieee.std_logic_1164.all;
use work.wishbone_pkg.all;
entity xwb_tics is
generic(
g_interface_mode : t_wishbone_interface_mode := CLASSIC;
g_address_granularity : t_wishbone_address_granularity := WORD;
g_period : integer
);
port(
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
-- Wishbone
slave_i : in t_wishbone_slave_in;
slave_o : out t_wishbone_slave_out;
desc_o : out t_wishbone_device_descriptor
);
end xwb_tics;
architecture rtl of xwb_tics is
component wb_tics
generic (
g_interface_mode : t_wishbone_interface_mode := CLASSIC;
g_address_granularity : t_wishbone_address_granularity := WORD;
g_period : integer);
port (
rst_n_i : in std_logic;
clk_sys_i : in std_logic;
wb_adr_i : in std_logic_vector(3 downto 0);
wb_dat_i : in std_logic_vector(c_wishbone_data_width-1 downto 0);
wb_dat_o : out std_logic_vector(c_wishbone_data_width-1 downto 0);
wb_cyc_i : in std_logic;
wb_sel_i : in std_logic_vector(c_wishbone_data_width/8-1 downto 0);
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_ack_o : out std_logic;
wb_stall_o : out std_logic);
end component;
begin
U_Tics : wb_tics
generic map (
g_interface_mode => g_interface_mode,
g_address_granularity => g_address_granularity,
g_period => g_period)
port map (
rst_n_i => rst_n_i,
clk_sys_i => clk_sys_i,
wb_adr_i => slave_i.adr(3 downto 0),
wb_dat_i => slave_i.Dat,
wb_dat_o => slave_o.dat,
wb_cyc_i => slave_i.cyc,
wb_sel_i => slave_i.sel,
wb_stb_i => slave_i.stb,
wb_we_i => slave_i.we,
wb_ack_o => slave_o.ack,
wb_stall_o => slave_o.stall);
slave_o.err <= '0';
slave_o.int <= '0';
slave_o.rty <= '0';
end rtl;
------------------------------------------------------------------------------
-- Title : Wishbone memory-mapper SPI flash
-- Project : General Cores
------------------------------------------------------------------------------
-- File : wb_spi_flash.vhd
-- Author : Wesley W. Terpstra
-- Company : GSI
-- Created : 2013-04-15
-- Last update: 2013-04-15
-- Platform : FPGA-generic
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: Maps an entire flash device to wishbone memory
-------------------------------------------------------------------------------
-- Copyright (c) 2013 GSI
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2013-04-15 1.0 terpstra Created
-- 2013-08-28 2.0 terpstra Quad-lane support
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
library work;
use work.wishbone_pkg.all;
use work.genram_pkg.all;
use work.gencores_pkg.all;
-- Memory mapped flash controller
entity wb_spi_flash is
generic(
g_port_width : natural := 1; -- 1 for EPCS, 4 for EPCQ
g_addr_width : natural := 24; -- size dependent (EPCQ256=25, EPCS128=24, ...)
g_idle_time : natural := 3;
g_dummy_time : natural := 8;
g_config : boolean := false;
-- leave these at defaults if you have:
-- a) slow clock, b) valid constraints, or c) registered in/outputs
g_input_latch_edge : std_logic := '1'; -- rising
g_output_latch_edge : std_logic := '0'; -- falling
g_input_to_output_cycles : natural := 1); -- between 1 and 8
port(
clk_i : in std_logic;
rstn_i : in std_logic;
slave_i : in t_wishbone_slave_in;
slave_o : out t_wishbone_slave_out;
-- For properly constrained designs, set clk_out_i = clk_in_i.
clk_out_i : in std_logic;
clk_in_i : in std_logic;
ncs_o : out std_logic;
oe_o : out std_logic_vector(g_port_width-1 downto 0);
asdi_o : out std_logic_vector(g_port_width-1 downto 0);
data_i : in std_logic_vector(g_port_width-1 downto 0);
external_request_i : in std_logic; -- JTAG wants to use SPI?
external_granted_o : out std_logic);
end entity;
architecture rtl of wb_spi_flash is
subtype t_word is std_logic_vector(31 downto 0);
subtype t_byte is std_logic_vector( 7 downto 0);
subtype t_address is unsigned(g_addr_width-1 downto 2);
subtype t_status is std_logic_vector(g_port_width-1 downto 0);
subtype t_count is unsigned(f_ceil_log2(t_word'length)-1 downto 0);
subtype t_ack_delay is std_logic_vector(g_input_to_output_cycles-1 downto 0);
constant c_read_status : t_byte := "00000101"; -- datain
constant c_write_enable : t_byte := "00000110"; --
constant c_fast_read : t_byte := "00001011"; -- address, dummy, datain
constant c_fast_read2 : t_byte := "10111011"; -- address, dummy, datain
constant c_fast_read4 : t_byte := "11101011"; -- address, dummy, datain
constant c_write_bytes : t_byte := "00000010"; -- address, dataout
constant c_write_bytes2 : t_byte := "11010010"; -- address, dataout
constant c_write_bytes4 : t_byte := "00010010"; -- address, dataout
constant c_erase_sector : t_byte := "11011000"; -- address
constant c_4addr : t_byte := "10110111"; --
constant c_3addr : t_byte := "11101001"; --
constant c_write_vstatus: t_byte := "10000001"; --
constant c_vstatus_data : t_byte := std_logic_vector(to_unsigned(g_dummy_time, 4)) & "1111"; -- no XIP (1), res (1), sequential read (11)
function f_addr_bits(x : natural) return natural is begin
if x <= 24 then return 24; else return 32; end if;
end f_addr_bits;
constant c_addr_bits : natural := f_addr_bits(g_addr_width);
constant c_low_time : t_count := to_unsigned(g_idle_time-1, t_count'length);
constant c_cmd_time : t_count := to_unsigned(t_byte'length-1, t_count'length);
constant c_status_time : t_count := to_unsigned(8*((g_input_to_output_cycles+14)/8)-1, t_count'length);
constant c_addr_time : t_count := to_unsigned((c_addr_bits/g_port_width)-1, t_count'length);
constant c_data_time : t_count := to_unsigned((t_wishbone_data'length/g_port_width)-1, t_count'length);
constant c_whatever : std_logic_vector(g_port_width-1 downto 0) := (others => '-');
constant c_magic_reg : t_address := (others => '1');
type t_state is (
S_ERROR, S_WAIT, S_INIT, S_DISPATCH, S_JTAG,
S_READ, S_READ_ADDR, S_READ_DUMMY, S_READ_DATA, S_LOWER_CS_IDLE,
S_ENABLE_WRITE, S_LOWER_CS_WRITE, S_WRITE, S_WRITE_ADDR, S_WRITE_DATA,
S_ENABLE_ERASE, S_LOWER_CS_ERASE, S_ERASE,
S_ERASE_ADDR3, S_ERASE_ADDR2, S_ERASE_ADDR1, S_ERASE_ADDR0,
S_ENABLE_VSTATUS, S_LOWER_CS_VSTATUS, S_VSTATUS, S_VSTATUS_DATA, S_LOWER2_CS_VSTATUS,
S_ENABLE_XADDR, S_LOWER_CS_XADDR, S_XADDR,
S_LOWER_CS_WAIT, S_READ_STATUS, S_LOAD_STATUS, S_WAIT_READY);
-- Format a command for output
function f_stripe(cmd : t_byte) return t_word is
variable result : t_word := (others => '-');
begin
for i in t_byte'range loop
result(i*g_port_width + t_word'length-g_port_width*8) := cmd(i);
if g_port_width >= 4 then
result(i*g_port_width + t_word'length-g_port_width*8 + 2) := '1';
result(i*g_port_width + t_word'length-g_port_width*8 + 3) := '1';
end if;
end loop;
return result;
end f_stripe;
-- Format data for output
function f_data(data : t_wishbone_data; sel : t_wishbone_byte_select) return t_wishbone_data is
variable result : t_wishbone_data := (others => '1');
begin
for i in t_wishbone_byte_select'range loop
if sel(i) = '1' then -- leave unselected bytes high
result(8*i+7 downto 8*i) := data(8*i+7 downto 8*i);
end if;
end loop;
return result;
end f_data;
-- Format an address for output
function f_address(address : t_address) return t_word is
variable result : t_word := (others => '0');
begin
result((t_word'length-c_addr_bits)+t_address'left downto
(t_word'length-c_addr_bits)+t_address'right) :=
std_logic_vector(address);
return result;
end f_address;
-- Addresses wrap within a page
constant c_page_size : natural := 256;
constant c_page_width : natural := f_ceil_log2(c_page_size);
function f_increment(address : t_address) return t_address is
variable result : t_address := address;
begin
result(c_page_width-1 downto 2) := result(c_page_width-1 downto 2) + 1;
return result;
end f_increment;
constant c_full_oe : std_logic_vector(3 downto 0) := "1101";
constant c_idle : t_word := f_stripe("--------");
constant c_oe_default : t_status := c_full_oe(t_status'range);
signal r_state : t_state := S_INIT;
signal r_state_n : t_state := S_INIT;
signal r_count : t_count := (others => '-');
signal r_stall : std_logic := '0';
signal r_stall_n : std_logic := '0';
signal r_ack : t_ack_delay := (others => '0');
signal r_ack_n : std_logic := '0';
signal r_err : std_logic := '0';
signal r_dat : t_wishbone_data := (others => '-');
signal r_adr : t_address := (others => '-');
signal r_ncs : std_logic := '1';
signal r_oe : t_status := (others => '0');
signal r_shift_o : t_word := (others => '-');
signal r_shift_i : t_word := (others => '-');
-- Clock crossing signals
signal master_i : t_wishbone_master_in;
signal master_o : t_wishbone_master_out;
signal clk_out_rstn : std_logic;
signal s_wip : std_logic; -- write in progress
begin
assert (g_port_width = 1 or g_port_width = 2 or g_port_width = 4)
report "g_port_width must be 1, 2, or 4, not " & integer'image(g_port_width)
severity error;
assert (g_input_to_output_cycles >= 1 and g_input_to_output_cycles <= 8)
report "g_input_to_output_cycles must be between 1 and 8, not " & integer'image(g_input_to_output_cycles)
severity error;
crossing : xwb_clock_crossing
port map(
slave_clk_i => clk_i,
slave_rst_n_i => rstn_i,
slave_i => slave_i,
slave_o => slave_o,
master_clk_i => clk_out_i,
master_rst_n_i => clk_out_rstn,
master_i => master_i,
master_o => master_o);
sync_reset : gc_sync_ffs
generic map(
g_sync_edge => "positive")
port map(
clk_i => clk_out_i,
rst_n_i => '1',
data_i => rstn_i,
synced_o => clk_out_rstn,
npulse_o => open,
ppulse_o => open);
master_i.ack <= r_ack(r_ack'left);
master_i.err <= r_err;
master_i.rty <= '0';
master_i.int <= '0';
master_i.dat <= r_shift_i;
master_i.stall <= r_stall;
-- input is prepared by SPI of falling edge => latch it on rising edge
input : process(clk_in_i) is
begin
if clk_in_i'event and clk_in_i = g_input_latch_edge then
r_shift_i <= r_shift_i(31-g_port_width downto 0) & data_i;
end if;
end process;
asdi_o <= r_shift_o(31 downto 32-g_port_width);
ncs_o <= r_ncs;
oe_o <= r_oe;
-- output is latched by SPI on rising edge => prepare it on falling edge
output : process(clk_out_i, clk_out_rstn) is
begin
if clk_out_rstn = '0' then
r_shift_o <= (others => '-');
r_ncs <= '1';
r_oe <= (others => '0');
elsif clk_out_i'event and clk_out_i = g_output_latch_edge then
case r_state is
when S_ERROR =>
r_shift_o <= c_idle;
r_ncs <= '1';
r_oe <= c_oe_default;
when S_WAIT =>
r_shift_o <= r_shift_o(31-g_port_width downto 0) & c_whatever;
r_ncs <= r_ncs;
r_oe <= r_oe;
when S_INIT =>
r_shift_o <= c_idle;
r_ncs <= '1';
r_oe <= c_oe_default;
when S_DISPATCH =>
r_shift_o <= c_idle;
r_ncs <= '1';
r_oe <= c_oe_default;
when S_JTAG =>
r_shift_o <= (others => '-');
r_ncs <= '1';
r_oe <= (others => '0');
when S_READ =>
case g_port_width is
when 1 => r_shift_o <= f_stripe(c_fast_read);
when 2 => r_shift_o <= f_stripe(c_fast_read2);
when 4 => r_shift_o <= f_stripe(c_fast_read4);
when others => null;
end case;
r_ncs <= '0';
r_oe <= c_oe_default;
when S_READ_ADDR =>
r_shift_o <= f_address(r_adr);
r_ncs <= '0';
r_oe <= (others => '1');
when S_READ_DUMMY =>
r_shift_o <= (others => '0');
r_ncs <= '0';
r_oe <= (others => '1');
when S_READ_DATA =>
r_shift_o <= (others => '-');
r_ncs <= '0';
r_oe <= (others => '0');
when S_LOWER_CS_IDLE =>
r_shift_o <= c_idle;
r_ncs <= '1';
r_oe <= c_oe_default;
when S_ENABLE_WRITE =>
r_shift_o <= f_stripe(c_write_enable);
r_ncs <= '0';
r_oe <= c_oe_default;
when S_LOWER_CS_WRITE =>
r_shift_o <= c_idle;
r_ncs <= '1';
r_oe <= c_oe_default;
when S_WRITE =>
case g_port_width is
when 1 => r_shift_o <= f_stripe(c_write_bytes);
when 2 => r_shift_o <= f_stripe(c_write_bytes2);
when 4 => r_shift_o <= f_stripe(c_write_bytes4);
when others => null;
end case;
r_ncs <= '0';
r_oe <= c_oe_default;
when S_WRITE_ADDR =>
r_shift_o <= f_address(r_adr);
r_ncs <= '0';
r_oe <= (others => '1');
when S_WRITE_DATA =>
r_shift_o <= r_dat;
r_ncs <= '0';
r_oe <= (others => '1');
when S_ENABLE_ERASE =>
r_shift_o <= f_stripe(c_write_enable);
r_ncs <= '0';
r_oe <= c_oe_default;
when S_LOWER_CS_ERASE =>
r_shift_o <= c_idle;
r_ncs <= '1';
r_oe <= c_oe_default;
when S_ERASE =>
r_shift_o <= f_stripe(c_erase_sector);
r_ncs <= '0';
r_oe <= c_oe_default;
when S_ERASE_ADDR3 =>
r_shift_o <= f_stripe(f_address(r_adr)(31 downto 24));
r_ncs <= '0';
r_oe <= c_oe_default;
when S_ERASE_ADDR2 =>
r_shift_o <= f_stripe(f_address(r_adr)(23 downto 16));
r_ncs <= '0';
r_oe <= c_oe_default;
when S_ERASE_ADDR1 =>
r_shift_o <= f_stripe(f_address(r_adr)(15 downto 8));
r_ncs <= '0';
r_oe <= c_oe_default;
when S_ERASE_ADDR0 =>
r_shift_o <= f_stripe(f_address(r_adr)(7 downto 0));
r_ncs <= '0';
r_oe <= c_oe_default;
when S_ENABLE_VSTATUS =>
r_shift_o <= f_stripe(c_write_enable);
r_ncs <= '0';
r_oe <= c_oe_default;
when S_LOWER_CS_VSTATUS =>
r_shift_o <= c_idle;
r_ncs <= '1';
r_oe <= c_oe_default;
when S_VSTATUS =>
r_shift_o <= f_stripe(c_write_vstatus);
r_ncs <= '0';
r_oe <= c_oe_default;
when S_VSTATUS_DATA =>
r_shift_o <= f_stripe(c_vstatus_data);
r_ncs <= '0';
r_oe <= c_oe_default;
when S_LOWER2_CS_VSTATUS =>
r_shift_o <= c_idle;
r_ncs <= '1';
r_oe <= c_oe_default;
when S_ENABLE_XADDR =>
r_shift_o <= f_stripe(c_write_enable);
r_ncs <= '0';
r_oe <= c_oe_default;
when S_LOWER_CS_XADDR =>
r_shift_o <= c_idle;
r_ncs <= '1';
r_oe <= c_oe_default;
when S_XADDR =>
if g_addr_width > 24 then
r_shift_o <= f_stripe(c_4addr);
else
r_shift_o <= f_stripe(c_3addr);
end if;
r_ncs <= '0';
r_oe <= c_oe_default;
when S_LOWER_CS_WAIT =>
r_shift_o <= c_idle;
r_ncs <= '1';
r_oe <= c_oe_default;
when S_READ_STATUS =>
r_shift_o <= f_stripe(c_read_status);
r_ncs <= '0';
r_oe <= c_oe_default;
when S_LOAD_STATUS =>
r_shift_o <= c_idle;
r_ncs <= '0';
r_oe <= c_oe_default;
when S_WAIT_READY =>
if s_wip = '0' then -- not busy
r_shift_o <= c_idle;
r_ncs <= '1';
r_oe <= c_oe_default;
else
r_shift_o <= c_idle;
r_ncs <= '0';
r_oe <= c_oe_default;
end if;
end case;
end if;
end process;
wip1 : if g_port_width = 1 generate
s_wip <= r_shift_i((9-g_input_to_output_cycles) mod 8);
end generate;
wipx : if g_port_width /= 1 generate
s_wip <= r_shift_i(((9-g_input_to_output_cycles) mod 8) * g_port_width + 1);
end generate;
main : process(clk_out_i, clk_out_rstn) is
begin
if clk_out_rstn = '0' then
r_state <= S_INIT;
r_state_n <= S_WAIT;
r_count <= (others => '-');
r_stall <= '0';
r_stall_n <= '0';
r_ack <= (others => '0');
r_ack_n <= '0';
r_err <= '0';
r_dat <= (others => '-');
r_adr <= (others => '-');
external_granted_o <= '0';
elsif rising_edge(clk_out_i) then
-- Default transition rules
r_state <= S_WAIT;
r_stall <= '1';
r_ack(0) <= '0';
r_err <= '0';
if g_input_to_output_cycles > 1 then
r_ack(g_input_to_output_cycles-1 downto 1) <=
r_ack(g_input_to_output_cycles-2 downto 0);
end if;
case r_state is
when S_ERROR =>
-- trap bad state machine behaviour
r_count <= (others => '-');
r_state <= S_ERROR;
r_state_n <= S_ERROR;
when S_WAIT =>
r_count <= r_count - 1;
if r_count = 1 then -- is set to 0?
r_state <= r_state_n;
r_stall <= r_stall_n;
r_ack(0) <= r_ack_n;
r_state_n <= S_ERROR;
r_stall_n <= '1';
r_ack_n <= '0';
end if;
when S_INIT =>
r_count <= (others => '1');
if g_config then
r_state_n <= S_ENABLE_VSTATUS;
else
r_state_n <= S_LOWER_CS_WAIT;
end if;
when S_DISPATCH =>
r_count <= (others => '-');
r_state_n <= S_ERROR;
r_dat <= f_data(master_o.dat, master_o.sel);
r_adr <= unsigned(master_o.adr(t_address'range));
r_stall <= master_o.cyc and master_o.stb;
r_state <= S_DISPATCH;
if master_o.cyc = '1' and master_o.stb = '1' then
if master_o.we = '0' then
r_state <= S_READ;
else
if unsigned(master_o.adr(t_address'range)) = c_magic_reg then
r_adr <= unsigned(master_o.dat(t_address'range));
r_state <= S_ENABLE_ERASE;
else
r_state <= S_ENABLE_WRITE;
end if;
end if;
elsif external_request_i = '1' then
external_granted_o <= '1';
r_state <= S_JTAG;
end if;
when S_JTAG =>
r_count <= (others => '-');
r_state_n <= S_ERROR;
if external_request_i = '1' then
r_state <= S_JTAG;
else
r_state <= S_LOWER_CS_WAIT;
external_granted_o <= '0';
end if;
when S_READ =>
r_count <= c_cmd_time;
r_state_n <= S_READ_ADDR;
when S_READ_ADDR =>
r_count <= c_addr_time;
r_state_n <= S_READ_DUMMY;
r_adr <= f_increment(r_adr);
when S_READ_DUMMY =>
r_count <= to_unsigned(g_dummy_time-1, t_count'length);
r_state_n <= S_READ_DATA;
when S_READ_DATA =>
r_count <= c_data_time;
r_ack_n <= '1';
r_adr <= f_increment(r_adr);
-- exploit the fact that clock_crossing doesn't change a stalled strobe
if master_o.cyc = '1' and master_o.stb = '1' and master_o.we = '0' and
master_o.adr(t_address'range) = std_logic_vector(r_adr) then
r_state_n <= S_READ_DATA;
r_stall <= '0';
else
r_state_n <= S_LOWER_CS_IDLE;
end if;
when S_LOWER_CS_IDLE =>
r_count <= c_low_time;
r_state_n <= S_DISPATCH;
r_stall_n <= '0';
when S_ENABLE_WRITE =>
r_count <= c_cmd_time;
r_state_n <= S_LOWER_CS_WRITE;
when S_LOWER_CS_WRITE =>
r_count <= c_low_time;
r_state_n <= S_WRITE;
when S_WRITE =>
r_count <= c_cmd_time;
r_state_n <= S_WRITE_ADDR;
when S_WRITE_ADDR =>
r_count <= c_addr_time;
r_state_n <= S_WRITE_DATA;
r_adr <= f_increment(r_adr);
when S_WRITE_DATA =>
r_count <= c_data_time;
r_ack_n <= '1';
r_adr <= f_increment(r_adr);
-- exploit the fact that clock_crossing doesn't change a stalled strobe
if master_o.cyc = '1' and master_o.stb = '1' and master_o.we = '1' and
master_o.adr(t_address'range) = std_logic_vector(r_adr) then
r_state_n <= S_WRITE_DATA;
r_dat <= f_data(master_o.dat, master_o.sel);
r_stall <= '0';
else
r_state_n <= S_LOWER_CS_WAIT;
end if;
when S_ENABLE_ERASE =>
r_count <= c_cmd_time;
r_state_n <= S_LOWER_CS_ERASE;
when S_LOWER_CS_ERASE =>
r_count <= c_low_time;
r_state_n <= S_ERASE;
when S_ERASE =>
r_count <= c_cmd_time;
r_state_n <= S_ERASE_ADDR3;
r_ack_n <= '1';
when S_ERASE_ADDR3 =>
r_count <= c_cmd_time;
r_state_n <= S_ERASE_ADDR2;
when S_ERASE_ADDR2 =>
r_count <= c_cmd_time;
r_state_n <= S_ERASE_ADDR1;
when S_ERASE_ADDR1 =>
r_count <= c_cmd_time;
if g_addr_width > 24 then
r_state_n <= S_ERASE_ADDR0;
else
r_state_n <= S_LOWER_CS_WAIT;
end if;
when S_ERASE_ADDR0 =>
r_count <= c_cmd_time;
r_state_n <= S_LOWER_CS_WAIT;
when S_ENABLE_VSTATUS =>
r_count <= c_cmd_time;
r_state_n <= S_LOWER_CS_VSTATUS;
when S_LOWER_CS_VSTATUS =>
r_count <= c_low_time;
r_state_n <= S_VSTATUS;
when S_VSTATUS =>
r_count <= c_cmd_time;
r_state_n <= S_VSTATUS_DATA;
when S_VSTATUS_DATA =>
r_count <= c_cmd_time;
r_state_n <= S_LOWER2_CS_VSTATUS;
when S_LOWER2_CS_VSTATUS =>
r_count <= c_low_time;
r_state_n <= S_ENABLE_XADDR;
when S_ENABLE_XADDR =>
r_count <= c_cmd_time;
r_state_n <= S_LOWER_CS_XADDR;
when S_LOWER_CS_XADDR =>
r_count <= c_low_time;
r_state_n <= S_XADDR;
when S_XADDR =>
r_count <= c_cmd_time;
r_state_n <= S_LOWER_CS_WAIT;
when S_LOWER_CS_WAIT =>
r_count <= c_low_time;
r_state_n <= S_READ_STATUS;
when S_READ_STATUS =>
r_count <= c_cmd_time;
r_state_n <= S_LOAD_STATUS;
when S_LOAD_STATUS =>
r_count <= c_status_time;
r_state_n <= S_WAIT_READY;
when S_WAIT_READY =>
-- Allow polling the magic register to detect busy
if master_o.cyc = '1' and master_o.stb = '1' and master_o.we = '0' and
unsigned(master_o.adr(t_address'range)) = c_magic_reg then
r_dat <= (others => '0');
r_stall <= '0';
r_err <= '1';
end if;
if s_wip = '0' then -- not busy
r_count <= c_low_time;
r_state_n <= S_DISPATCH;
r_stall_n <= '0';
else
r_count <= c_cmd_time;
r_state_n <= S_WAIT_READY;
end if;
end case;
end if;
end process;
end rtl;
files = [ "uart_async_rx.vhd",
"uart_async_tx.vhd",
"uart_baud_gen.vhd",
"simple_uart_wb.vhd",
"simple_uart_pkg.vhd",
"wb_simple_uart.vhd",
"xwb_simple_uart.vhd"];
#!/bin/bash
mkdir -p doc
wbgen2 -D ./doc/wb_simple_uart.html -V simple_uart_wb.vhd -p simple_uart_pkg.vhd --cstyle struct -C wb_uart.h --hstyle record --lang vhdl simple_uart_wb.wb
---------------------------------------------------------------------------------------
-- Title : Wishbone slave core for Simple Wishbone UART
---------------------------------------------------------------------------------------
-- File : simple_uart_pkg.vhd
-- Author : auto-generated by wbgen2 from simple_uart_wb.wb
-- Created : Thu Feb 14 10:36:11 2013
-- Standard : VHDL'87
---------------------------------------------------------------------------------------
-- THIS FILE WAS GENERATED BY wbgen2 FROM SOURCE FILE simple_uart_wb.wb
-- DO NOT HAND-EDIT UNLESS IT'S ABSOLUTELY NECESSARY!
---------------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
package uart_wbgen2_pkg is
-- Input registers (user design -> WB slave)
type t_uart_in_registers is record
sr_tx_busy_i : std_logic;
sr_rx_rdy_i : std_logic;
rdr_rx_data_i : std_logic_vector(7 downto 0);
host_tdr_rdy_i : std_logic;
host_rdr_data_i : std_logic_vector(7 downto 0);
host_rdr_rdy_i : std_logic;
host_rdr_count_i : std_logic_vector(15 downto 0);
end record;
constant c_uart_in_registers_init_value: t_uart_in_registers := (
sr_tx_busy_i => '0',
sr_rx_rdy_i => '0',
rdr_rx_data_i => (others => '0'),
host_tdr_rdy_i => '0',
host_rdr_data_i => (others => '0'),
host_rdr_rdy_i => '0',
host_rdr_count_i => (others => '0')
);
-- Output registers (WB slave -> user design)
type t_uart_out_registers is record
bcr_o : std_logic_vector(31 downto 0);
bcr_wr_o : std_logic;
tdr_tx_data_o : std_logic_vector(7 downto 0);
tdr_tx_data_wr_o : std_logic;
host_tdr_data_o : std_logic_vector(7 downto 0);
host_tdr_data_wr_o : std_logic;
end record;
constant c_uart_out_registers_init_value: t_uart_out_registers := (
bcr_o => (others => '0'),
bcr_wr_o => '0',
tdr_tx_data_o => (others => '0'),
tdr_tx_data_wr_o => '0',
host_tdr_data_o => (others => '0'),
host_tdr_data_wr_o => '0'
);
function "or" (left, right: t_uart_in_registers) return t_uart_in_registers;
function f_x_to_zero (x:std_logic) return std_logic;
function f_x_to_zero (x:std_logic_vector) return std_logic_vector;
end package;
package body uart_wbgen2_pkg is
function f_x_to_zero (x:std_logic) return std_logic is
begin
if(x = 'X' or x = 'U') then
return '0';
else
return x;
end if;
end function;
function f_x_to_zero (x:std_logic_vector) return std_logic_vector is
variable tmp: std_logic_vector(x'length-1 downto 0);
begin
for i in 0 to x'length-1 loop
if(x(i) = 'X' or x(i) = 'U') then
tmp(i):= '0';
else
tmp(i):=x(i);
end if;
end loop;
return tmp;
end function;
function "or" (left, right: t_uart_in_registers) return t_uart_in_registers is
variable tmp: t_uart_in_registers;
begin
tmp.sr_tx_busy_i := f_x_to_zero(left.sr_tx_busy_i) or f_x_to_zero(right.sr_tx_busy_i);
tmp.sr_rx_rdy_i := f_x_to_zero(left.sr_rx_rdy_i) or f_x_to_zero(right.sr_rx_rdy_i);
tmp.rdr_rx_data_i := f_x_to_zero(left.rdr_rx_data_i) or f_x_to_zero(right.rdr_rx_data_i);
tmp.host_tdr_rdy_i := f_x_to_zero(left.host_tdr_rdy_i) or f_x_to_zero(right.host_tdr_rdy_i);
tmp.host_rdr_data_i := f_x_to_zero(left.host_rdr_data_i) or f_x_to_zero(right.host_rdr_data_i);
tmp.host_rdr_rdy_i := f_x_to_zero(left.host_rdr_rdy_i) or f_x_to_zero(right.host_rdr_rdy_i);
tmp.host_rdr_count_i := f_x_to_zero(left.host_rdr_count_i) or f_x_to_zero(right.host_rdr_count_i);
return tmp;
end function;
end package body;
---------------------------------------------------------------------------------------
-- Title : Wishbone slave core for Simple Wishbone UART
---------------------------------------------------------------------------------------
-- File : simple_uart_wb.vhd
-- Author : auto-generated by wbgen2 from simple_uart_wb.wb
-- Created : Thu Feb 14 10:36:11 2013
-- Standard : VHDL'87
---------------------------------------------------------------------------------------
-- THIS FILE WAS GENERATED BY wbgen2 FROM SOURCE FILE simple_uart_wb.wb
-- DO NOT HAND-EDIT UNLESS IT'S ABSOLUTELY NECESSARY!
---------------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
use work.uart_wbgen2_pkg.all;
entity simple_uart_wb is
port (
rst_n_i : in std_logic;
clk_sys_i : in std_logic;
wb_adr_i : in std_logic_vector(2 downto 0);
wb_dat_i : in std_logic_vector(31 downto 0);
wb_dat_o : out std_logic_vector(31 downto 0);
wb_cyc_i : in std_logic;
wb_sel_i : in std_logic_vector(3 downto 0);
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_ack_o : out std_logic;
wb_stall_o : out std_logic;
rdr_rack_o : out std_logic;
host_rack_o : out std_logic;
regs_i : in t_uart_in_registers;
regs_o : out t_uart_out_registers
);
end simple_uart_wb;
architecture syn of simple_uart_wb is
signal ack_sreg : std_logic_vector(9 downto 0);
signal rddata_reg : std_logic_vector(31 downto 0);
signal wrdata_reg : std_logic_vector(31 downto 0);
signal bwsel_reg : std_logic_vector(3 downto 0);
signal rwaddr_reg : std_logic_vector(2 downto 0);
signal ack_in_progress : std_logic ;
signal wr_int : std_logic ;
signal rd_int : std_logic ;
signal allones : std_logic_vector(31 downto 0);
signal allzeros : std_logic_vector(31 downto 0);
begin
-- Some internal signals assignments. For (foreseen) compatibility with other bus standards.
wrdata_reg <= wb_dat_i;
bwsel_reg <= wb_sel_i;
rd_int <= wb_cyc_i and (wb_stb_i and (not wb_we_i));
wr_int <= wb_cyc_i and (wb_stb_i and wb_we_i);
allones <= (others => '1');
allzeros <= (others => '0');
--
-- Main register bank access process.
process (clk_sys_i, rst_n_i)
begin
if (rst_n_i = '0') then
ack_sreg <= "0000000000";
ack_in_progress <= '0';
rddata_reg <= "00000000000000000000000000000000";
regs_o.bcr_wr_o <= '0';
regs_o.tdr_tx_data_wr_o <= '0';
rdr_rack_o <= '0';
regs_o.host_tdr_data_wr_o <= '0';
host_rack_o <= '0';
elsif rising_edge(clk_sys_i) then
-- advance the ACK generator shift register
ack_sreg(8 downto 0) <= ack_sreg(9 downto 1);
ack_sreg(9) <= '0';
if (ack_in_progress = '1') then
if (ack_sreg(0) = '1') then
regs_o.bcr_wr_o <= '0';
regs_o.tdr_tx_data_wr_o <= '0';
rdr_rack_o <= '0';
regs_o.host_tdr_data_wr_o <= '0';
host_rack_o <= '0';
ack_in_progress <= '0';
else
regs_o.bcr_wr_o <= '0';
regs_o.tdr_tx_data_wr_o <= '0';
regs_o.host_tdr_data_wr_o <= '0';
end if;
else
if ((wb_cyc_i = '1') and (wb_stb_i = '1')) then
case rwaddr_reg(2 downto 0) is
when "000" =>
if (wb_we_i = '1') then
end if;
rddata_reg(0) <= regs_i.sr_tx_busy_i;
rddata_reg(1) <= regs_i.sr_rx_rdy_i;
rddata_reg(2) <= 'X';
rddata_reg(3) <= 'X';
rddata_reg(4) <= 'X';
rddata_reg(5) <= 'X';
rddata_reg(6) <= 'X';
rddata_reg(7) <= 'X';
rddata_reg(8) <= 'X';
rddata_reg(9) <= 'X';
rddata_reg(10) <= 'X';
rddata_reg(11) <= 'X';
rddata_reg(12) <= 'X';
rddata_reg(13) <= 'X';
rddata_reg(14) <= 'X';
rddata_reg(15) <= 'X';
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "001" =>
if (wb_we_i = '1') then
regs_o.bcr_wr_o <= '1';
end if;
rddata_reg(0) <= 'X';
rddata_reg(1) <= 'X';
rddata_reg(2) <= 'X';
rddata_reg(3) <= 'X';
rddata_reg(4) <= 'X';
rddata_reg(5) <= 'X';
rddata_reg(6) <= 'X';
rddata_reg(7) <= 'X';
rddata_reg(8) <= 'X';
rddata_reg(9) <= 'X';
rddata_reg(10) <= 'X';
rddata_reg(11) <= 'X';
rddata_reg(12) <= 'X';
rddata_reg(13) <= 'X';
rddata_reg(14) <= 'X';
rddata_reg(15) <= 'X';
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "010" =>
if (wb_we_i = '1') then
regs_o.tdr_tx_data_wr_o <= '1';
end if;
rddata_reg(0) <= 'X';
rddata_reg(1) <= 'X';
rddata_reg(2) <= 'X';
rddata_reg(3) <= 'X';
rddata_reg(4) <= 'X';
rddata_reg(5) <= 'X';
rddata_reg(6) <= 'X';
rddata_reg(7) <= 'X';
rddata_reg(8) <= 'X';
rddata_reg(9) <= 'X';
rddata_reg(10) <= 'X';
rddata_reg(11) <= 'X';
rddata_reg(12) <= 'X';
rddata_reg(13) <= 'X';
rddata_reg(14) <= 'X';
rddata_reg(15) <= 'X';
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "011" =>
if (wb_we_i = '1') then
end if;
rddata_reg(7 downto 0) <= regs_i.rdr_rx_data_i;
rdr_rack_o <= '1';
rddata_reg(8) <= 'X';
rddata_reg(9) <= 'X';
rddata_reg(10) <= 'X';
rddata_reg(11) <= 'X';
rddata_reg(12) <= 'X';
rddata_reg(13) <= 'X';
rddata_reg(14) <= 'X';
rddata_reg(15) <= 'X';
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "100" =>
if (wb_we_i = '1') then
regs_o.host_tdr_data_wr_o <= '1';
end if;
rddata_reg(8) <= regs_i.host_tdr_rdy_i;
rddata_reg(0) <= 'X';
rddata_reg(1) <= 'X';
rddata_reg(2) <= 'X';
rddata_reg(3) <= 'X';
rddata_reg(4) <= 'X';
rddata_reg(5) <= 'X';
rddata_reg(6) <= 'X';
rddata_reg(7) <= 'X';
rddata_reg(9) <= 'X';
rddata_reg(10) <= 'X';
rddata_reg(11) <= 'X';
rddata_reg(12) <= 'X';
rddata_reg(13) <= 'X';
rddata_reg(14) <= 'X';
rddata_reg(15) <= 'X';
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "101" =>
if (wb_we_i = '1') then
end if;
rddata_reg(7 downto 0) <= regs_i.host_rdr_data_i;
host_rack_o <= '1';
rddata_reg(8) <= regs_i.host_rdr_rdy_i;
rddata_reg(24 downto 9) <= regs_i.host_rdr_count_i;
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when others =>
-- prevent the slave from hanging the bus on invalid address
ack_in_progress <= '1';
ack_sreg(0) <= '1';
end case;
end if;
end if;
end if;
end process;
-- Drive the data output bus
wb_dat_o <= rddata_reg;
-- TX busy
-- RX ready
-- Baudrate divider setting
-- pass-through field: Baudrate divider setting in register: Baudrate control register
regs_o.bcr_o <= wrdata_reg(31 downto 0);
-- Transmit data
-- pass-through field: Transmit data in register: Transmit data regsiter
regs_o.tdr_tx_data_o <= wrdata_reg(7 downto 0);
-- Received data
-- TX Data
-- pass-through field: TX Data in register: Host VUART Tx register
regs_o.host_tdr_data_o <= wrdata_reg(7 downto 0);
-- TX Ready
-- RX Data
-- RX Ready
-- RX FIFO Count
rwaddr_reg <= wb_adr_i;
wb_stall_o <= (not ack_sreg(0)) and (wb_stb_i and wb_cyc_i);
-- ACK signal generation. Just pass the LSB of ACK counter.
wb_ack_o <= ack_sreg(0);
end syn;
-- -*- Mode: LUA; tab-width: 2 -*-
peripheral {
name = "Simple Wishbone UART";
description = "A simple Wishbone UART (8N1 mode) with programmable baud rate. ";
prefix = "uart";
hdl_entity = "simple_uart_wb";
reg {
name = "Status Register";
prefix = "SR";
field {
name = "TX busy";
description = "1: UART is busy transmitting a byte\n0: UART is idle and ready to transmit next byte";
prefix = "TX_BUSY";
type = BIT;
access_bus = READ_ONLY;
access_dev = WRITE_ONLY;
};
field {
name = "RX ready";
description = "1: UART received a byte and its in RXD register\n0: no data in RXD register";
prefix = "RX_RDY";
type = BIT;
access_bus = READ_ONLY;
access_dev = WRITE_ONLY;
};
};
reg {
name = "Baudrate control register";
description = "Register controlling the UART baudrate";
prefix = "BCR";
field {
name = "Baudrate divider setting";
description = "Baudrate setting. The value can be calculated using the following equation:\
BRATE = ((Baudrate * 8) << 9 + (ClockFreq >> 8)) / (ClockFreq >> 7)";
size = 32;
type = PASS_THROUGH;
};
};
reg {
name = "Transmit data regsiter";
prefix = "TDR";
field {
name = "Transmit data";
prefix = "TX_DATA";
size = 8;
type = PASS_THROUGH;
};
};
reg {
name = "Receive data regsiter";
prefix = "RDR";
field {
ack_read = "rdr_rack_o";
name = "Received data";
prefix = "RX_DATA";
size = 8;
type = SLV;
access_bus = READ_ONLY;
access_dev = WRITE_ONLY;
};
};
reg {
name = "Host VUART Tx register";
prefix = "HOST_TDR";
field {
name = "TX Data";
prefix = "DATA";
type = PASS_THROUGH;
size = 8;
};
field {
name = "TX Ready";
prefix = "RDY";
type= BIT;
access_dev= WRITE_ONLY;
access_bus=READ_ONLY;
};
};
reg {
name = "Host VUART Rx register";
prefix = "HOST_RDR";
field {
ack_read = "host_rack_o";
name = "RX Data";
prefix = "DATA";
type = SLV;
size = 8;
access_dev= WRITE_ONLY;
access_bus=READ_ONLY;
};
field {
name = "RX Ready";
prefix = "RDY";
type= BIT;
access_dev= WRITE_ONLY;
access_bus=READ_ONLY;
};
field {
name = "RX FIFO Count";
prefix = "COUNT";
type = SLV;
size = 16;
access_dev= WRITE_ONLY;
access_bus=READ_ONLY;
};
};
};
\ No newline at end of file
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
entity uart_async_rx is
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
baud8_tick_i: in std_logic;
rxd_i : in std_logic;
rx_ready_o : out std_logic;
rx_error_o : out std_logic;
rx_data_o : out std_logic_vector(7 downto 0)
);
end uart_async_rx;
architecture behavioral of uart_async_rx is
signal Baud8Tick : std_logic;
signal RxD_sync_inv : std_logic_vector(1 downto 0);
signal RxD_cnt_inv : unsigned(1 downto 0);
signal RxD_bit_inv : std_logic;
signal state : std_logic_vector(3 downto 0);
signal bit_spacing : std_logic_vector(3 downto 0);
signal next_bit : std_logic;
signal RxD_data : std_logic_vector(7 downto 0);
signal RxD_data_ready : std_logic;
signal RxD_data_error : std_logic;
begin -- behavioral
Baud8Tick <= baud8_tick_i;
process(clk_sys_i, rst_n_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
RxD_sync_inv <= (others => '0');
else
if(Baud8Tick = '1') then
RxD_sync_inv <= RxD_sync_inv(0) & (not rxd_i);
end if;
end if;
end if;
end process;
process(clk_sys_i, rst_n_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
RxD_bit_inv <= '0';
RxD_cnt_inv <= (others => '0');
else
if(Baud8Tick = '1') then
if(RxD_sync_inv(1) = '1' and RxD_cnt_inv /= "11") then
RxD_cnt_inv <= RxD_cnt_inv + 1;
elsif (RxD_sync_inv(1) = '0' and RxD_cnt_inv /= "00") then
RxD_cnt_inv <= RxD_cnt_inv - 1;
end if;
if(RxD_cnt_inv = "00") then
RxD_bit_inv <= '0';
elsif(RxD_cnt_inv = "11") then
RxD_bit_inv <= '1';
end if;
end if;
end if;
end if;
end process;
next_bit <= '1' when (bit_spacing = x"a") else '0';
process(clk_sys_i, rst_n_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
bit_spacing <= (others => '0');
else
if(state = x"0") then
bit_spacing <= "0000";
elsif(Baud8Tick = '1') then
-- bit_spacing <= std_logic_vector(resize((unsigned(bit_spacing(2 downto 0)) + 1), 4))
bit_spacing <= std_logic_vector(unsigned('0' & bit_spacing(2 downto 0)) + 1)
or (bit_spacing(3) & "000");
end if;
end if;
end if;
end process;
process(clk_sys_i, rst_n_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
state <= (others => '0');
else
if(Baud8Tick = '1') then
case state is
when "0000" =>
if(RxD_bit_inv = '1') then -- start bit
state <= "1000";
end if;
when "1000" =>
if(next_bit = '1') then
state <= "1001"; -- bit 0
end if;
when "1001" =>
if(next_bit = '1') then
state <= "1010"; -- bit 1
end if;
when "1010" =>
if(next_bit = '1') then
state <= "1011"; -- bit 2
end if;
when "1011" =>
if(next_bit = '1') then
state <= "1100"; -- bit 3
end if;
when "1100" =>
if(next_bit = '1') then
state <= "1101"; -- bit 4
end if;
when "1101" =>
if(next_bit = '1') then
state <= "1110"; -- bit 5
end if;
when "1110" =>
if(next_bit = '1') then
state <= "1111"; -- bit 6
end if;
when "1111" =>
if(next_bit = '1') then
state <= "0001"; -- bit 7
end if;
when "0001" =>
if(next_bit = '1') then
state <= "0000"; -- bit stop
end if;
when others => state <= "0000";
end case;
end if;
end if;
end if;
end process;
process(clk_sys_i, rst_n_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
RxD_data <= (others => '0');
else
if(Baud8Tick = '1' and next_bit = '1' and state(3) = '1') then
RxD_data <= (not RxD_bit_inv) & RxD_data(7 downto 1);
end if;
end if;
end if;
end process;
process(clk_sys_i, rst_n_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
RxD_data_error <= '0';
RxD_data_ready <= '0';
else
if(Baud8Tick = '1' and next_bit = '1' and state = "0001" and RxD_bit_inv = '0') then
RxD_data_ready <= '1';
else
RxD_data_ready <= '0';
end if;
if(Baud8Tick = '1' and next_bit = '1' and state = "0001" and RxD_bit_inv = '1') then
RxD_data_error <= '1';
else
RxD_data_error <= '0';
end if;
end if;
end if;
end process;
rx_data_o <= RxD_data;
rx_ready_o <= RxD_data_ready;
rx_error_o <= RxD_data_error;
end behavioral;
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
entity uart_async_tx is
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
baud_tick_i: in std_logic;
txd_o : out std_logic;
tx_start_p_i : in std_logic;
tx_data_i : in std_logic_vector(7 downto 0);
tx_busy_o : out std_logic
);
end uart_async_tx;
architecture behavioral of uart_async_tx is
signal BaudTick : std_logic;
signal TxD_busy : std_logic;
signal TxD_ready : std_logic;
signal state : std_logic_vector(3 downto 0);
signal TxD_dataReg : std_logic_vector(7 downto 0);
signal TxD_dataD : std_logic_vector(7 downto 0);
signal muxbit : std_logic;
signal TxD : std_logic;
begin -- behavioral
TxD_ready <= '1' when state = "0000" else '0';
TxD_busy <= not TxD_ready;
BaudTick <= baud_tick_i;
process(clk_sys_i, rst_n_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
TxD_dataReg <= (others => '0');
elsif TxD_ready = '1' and tx_start_p_i = '1' then
TxD_dataReg <= tx_data_i;
end if;
end if;
end process;
TxD_dataD <= TxD_dataReg;
process(clk_sys_i, rst_n_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
state <= "0000";
else
case state is
when "0000" =>
if (tx_start_p_i = '1') then
state <= "0001";
end if;
when "0001" =>
if (BaudTick = '1') then
state <= "0100";
end if;
when "0100" =>
if (BaudTick = '1') then
state <= "1000";
end if;
when "1000" =>
if (BaudTick = '1') then
state <= "1001";
end if;
when "1001" =>
if (BaudTick = '1') then
state <= "1010";
end if;
when "1010" =>
if (BaudTick = '1') then
state <= "1011";
end if;
when "1011" =>
if (BaudTick = '1') then
state <= "1100";
end if;
when "1100" =>
if (BaudTick = '1') then
state <= "1101";
end if;
when "1101" =>
if (BaudTick = '1') then
state <= "1110";
end if;
when "1110" =>
if (BaudTick = '1') then
state <= "1111";
end if;
when "1111" =>
if (BaudTick = '1') then
state <= "0010";
end if;
when "0010" =>
if (BaudTick = '1') then
state <= "0011";
end if;
when "0011" =>
if (BaudTick = '1') then
state <= "0000";
end if;
when others =>
state <= "0000";
end case;
end if;
end if;
end process;
process(TxD_dataD, state)
begin
case state(2 downto 0) is
when "000" => muxbit <= TxD_dataD(0);
when "001" => muxbit <= TxD_dataD(1);
when "010" => muxbit <= TxD_dataD(2);
when "011" => muxbit <= TxD_dataD(3);
when "100" => muxbit <= TxD_dataD(4);
when "101" => muxbit <= TxD_dataD(5);
when "110" => muxbit <= TxD_dataD(6);
when "111" => muxbit <= TxD_dataD(7);
when others => null;
end case;
end process;
process(clk_sys_i, rst_n_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
TxD <= '0';
else
if(unsigned(state) < to_unsigned(4, state'length) or (state(3) = '1' and muxbit = '1')) then
TxD <= '1';
else
TxD <= '0';
end if;
end if;
end if;
end process;
txd_o <= TxD;
tx_busy_o <= TxD_busy;
end behavioral;
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
entity uart_baud_gen is
generic (
g_baud_acc_width : integer := 16);
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
baudrate_i : in std_logic_vector(g_baud_acc_width downto 0);
baud_tick_o : out std_logic;
baud8_tick_o : out std_logic);
end uart_baud_gen;
architecture behavioral of uart_baud_gen is
signal Baud8GeneratorInc : unsigned(g_baud_acc_width downto 0);
signal Baud8GeneratorAcc : unsigned(g_baud_acc_width downto 0);
signal Baud8Tick : std_logic;
signal Baud_sreg : std_logic_vector(7 downto 0) := "10000000";
begin -- behavioral
Baud8GeneratorInc <= unsigned(baudrate_i);
process(clk_sys_i, rst_n_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
Baud8GeneratorAcc <= (others => '0');
else
Baud8GeneratorAcc <= ('0' & Baud8GeneratorAcc(Baud8GeneratorAcc'high-1 downto 0)) + Baud8GeneratorInc;
end if;
end if;
end process;
Baud8Tick <= std_logic(Baud8GeneratorAcc(g_baud_acc_width));
process(clk_sys_i, rst_n_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
Baud_sreg <= "10000000";
elsif Baud8Tick = '1' then
Baud_sreg <= Baud_sreg(0) & Baud_sreg(7 downto 1);
end if;
end if;
end process;
baud_tick_o <= Baud_sreg(0) and Baud8Tick;
baud8_tick_o <= Baud8Tick;
end behavioral;
---------------------------------------------------------------------------------------
-- Title : Wishbone slave core for Simple Wishbone UART
---------------------------------------------------------------------------------------
-- File : uart_wb_slave.vhd
-- Author : auto-generated by wbgen2 from uart.wb
-- Created : Mon Feb 21 10:04:29 2011
-- Standard : VHDL'87
---------------------------------------------------------------------------------------
-- THIS FILE WAS GENERATED BY wbgen2 FROM SOURCE FILE uart.wb
-- DO NOT HAND-EDIT UNLESS IT'S ABSOLUTELY NECESSARY!
---------------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;
entity uart_wb_slave is
port (
rst_n_i : in std_logic;
wb_clk_i : in std_logic;
wb_addr_i : in std_logic_vector(1 downto 0);
wb_data_i : in std_logic_vector(31 downto 0);
wb_data_o : out std_logic_vector(31 downto 0);
wb_cyc_i : in std_logic;
wb_sel_i : in std_logic_vector(3 downto 0);
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_ack_o : out std_logic;
-- Port for BIT field: 'TX busy' in reg: 'Status Register'
uart_sr_tx_busy_i : in std_logic;
-- Port for BIT field: 'RX ready' in reg: 'Status Register'
uart_sr_rx_rdy_i : in std_logic;
-- Port for std_logic_vector field: 'Baudrate divider setting' in reg: 'Baudrate control register'
uart_bcr_o : out std_logic_vector(31 downto 0);
-- Port for std_logic_vector field: 'Transmit data' in reg: 'Transmit data regsiter'
uart_tdr_tx_data_o : out std_logic_vector(7 downto 0);
uart_tdr_tx_data_i : in std_logic_vector(7 downto 0);
uart_tdr_tx_data_load_o : out std_logic;
-- Port for std_logic_vector field: 'Received data' in reg: 'Receive data regsiter'
uart_rdr_rx_data_i : in std_logic_vector(7 downto 0);
rdr_rack_o : out std_logic
);
end uart_wb_slave;
architecture syn of uart_wb_slave is
signal uart_bcr_int : std_logic_vector(31 downto 0);
signal ack_sreg : std_logic_vector(9 downto 0);
signal rddata_reg : std_logic_vector(31 downto 0);
signal wrdata_reg : std_logic_vector(31 downto 0);
signal bwsel_reg : std_logic_vector(3 downto 0);
signal rwaddr_reg : std_logic_vector(1 downto 0);
signal ack_in_progress : std_logic ;
signal wr_int : std_logic ;
signal rd_int : std_logic ;
signal bus_clock_int : std_logic ;
signal allones : std_logic_vector(31 downto 0);
signal allzeros : std_logic_vector(31 downto 0);
begin
-- Some internal signals assignments. For (foreseen) compatibility with other bus standards.
wrdata_reg <= wb_data_i;
bwsel_reg <= wb_sel_i;
bus_clock_int <= wb_clk_i;
rd_int <= wb_cyc_i and (wb_stb_i and (not wb_we_i));
wr_int <= wb_cyc_i and (wb_stb_i and wb_we_i);
allones <= (others => '1');
allzeros <= (others => '0');
--
-- Main register bank access process.
process (bus_clock_int, rst_n_i)
begin
if (rst_n_i = '0') then
ack_sreg <= "0000000000";
ack_in_progress <= '0';
rddata_reg <= "00000000000000000000000000000000";
uart_bcr_int <= "00000000000000000000000000000000";
uart_tdr_tx_data_load_o <= '0';
rdr_rack_o <= '0';
elsif rising_edge(bus_clock_int) then
-- advance the ACK generator shift register
ack_sreg(8 downto 0) <= ack_sreg(9 downto 1);
ack_sreg(9) <= '0';
if (ack_in_progress = '1') then
if (ack_sreg(0) = '1') then
uart_tdr_tx_data_load_o <= '0';
rdr_rack_o <= '0';
ack_in_progress <= '0';
else
uart_tdr_tx_data_load_o <= '0';
end if;
else
if ((wb_cyc_i = '1') and (wb_stb_i = '1')) then
case rwaddr_reg(1 downto 0) is
when "00" =>
if (wb_we_i = '1') then
else
rddata_reg(0) <= uart_sr_tx_busy_i;
rddata_reg(1) <= uart_sr_rx_rdy_i;
rddata_reg(2) <= 'X';
rddata_reg(3) <= 'X';
rddata_reg(4) <= 'X';
rddata_reg(5) <= 'X';
rddata_reg(6) <= 'X';
rddata_reg(7) <= 'X';
rddata_reg(8) <= 'X';
rddata_reg(9) <= 'X';
rddata_reg(10) <= 'X';
rddata_reg(11) <= 'X';
rddata_reg(12) <= 'X';
rddata_reg(13) <= 'X';
rddata_reg(14) <= 'X';
rddata_reg(15) <= 'X';
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
end if;
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "01" =>
if (wb_we_i = '1') then
uart_bcr_int <= wrdata_reg(31 downto 0);
else
rddata_reg(31 downto 0) <= uart_bcr_int;
end if;
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "10" =>
if (wb_we_i = '1') then
uart_tdr_tx_data_load_o <= '1';
else
rddata_reg(7 downto 0) <= uart_tdr_tx_data_i;
rddata_reg(8) <= 'X';
rddata_reg(9) <= 'X';
rddata_reg(10) <= 'X';
rddata_reg(11) <= 'X';
rddata_reg(12) <= 'X';
rddata_reg(13) <= 'X';
rddata_reg(14) <= 'X';
rddata_reg(15) <= 'X';
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
end if;
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when "11" =>
if (wb_we_i = '1') then
else
rddata_reg(7 downto 0) <= uart_rdr_rx_data_i;
rdr_rack_o <= '1';
rddata_reg(8) <= 'X';
rddata_reg(9) <= 'X';
rddata_reg(10) <= 'X';
rddata_reg(11) <= 'X';
rddata_reg(12) <= 'X';
rddata_reg(13) <= 'X';
rddata_reg(14) <= 'X';
rddata_reg(15) <= 'X';
rddata_reg(16) <= 'X';
rddata_reg(17) <= 'X';
rddata_reg(18) <= 'X';
rddata_reg(19) <= 'X';
rddata_reg(20) <= 'X';
rddata_reg(21) <= 'X';
rddata_reg(22) <= 'X';
rddata_reg(23) <= 'X';
rddata_reg(24) <= 'X';
rddata_reg(25) <= 'X';
rddata_reg(26) <= 'X';
rddata_reg(27) <= 'X';
rddata_reg(28) <= 'X';
rddata_reg(29) <= 'X';
rddata_reg(30) <= 'X';
rddata_reg(31) <= 'X';
end if;
ack_sreg(0) <= '1';
ack_in_progress <= '1';
when others =>
-- prevent the slave from hanging the bus on invalid address
ack_in_progress <= '1';
ack_sreg(0) <= '1';
end case;
end if;
end if;
end if;
end process;
-- Drive the data output bus
wb_data_o <= rddata_reg;
-- TX busy
-- RX ready
-- Baudrate divider setting
uart_bcr_o <= uart_bcr_int;
-- Transmit data
uart_tdr_tx_data_o <= wrdata_reg(7 downto 0);
-- Received data
rwaddr_reg <= wb_addr_i;
-- ACK signal generation. Just pass the LSB of ACK counter.
wb_ack_o <= ack_sreg(0);
end syn;
-------------------------------------------------------------------------------
-- Title : Simple Wishbone UART
-- Project : General Cores Collection (gencores) library
-------------------------------------------------------------------------------
-- File : wb_simple_uart.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-Co-HT
-- Created : 2011-02-21
-- Last update: 2011-10-04
-- Platform : FPGA-generics
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: A simple UART controller, providing two modes of operation
-- (both can be used simultenously):
-- - physical UART (encoding fixed to 8 data bits, no parity and one stop bit)
-- - virtual UART: TXed data is passed via a FIFO to the Wishbone host (and
-- vice versa).
-------------------------------------------------------------------------------
-- Copyright (c) 2011 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2011-02-21 1.0 twlostow Created
-- 2011-10-04 1.1 twlostow merged with VUART, added adapter
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
use work.genram_pkg.all;
use work.wishbone_pkg.all;
use work.UART_wbgen2_pkg.all;
entity wb_simple_uart is
generic(
g_with_virtual_uart : boolean;
g_with_physical_uart : boolean;
g_interface_mode : t_wishbone_interface_mode := CLASSIC;
g_address_granularity : t_wishbone_address_granularity := WORD
);
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
wb_adr_i : in std_logic_vector(4 downto 0);
wb_dat_i : in std_logic_vector(31 downto 0);
wb_dat_o : out std_logic_vector(31 downto 0);
wb_cyc_i : in std_logic;
wb_sel_i : in std_logic_vector(3 downto 0);
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_ack_o : out std_logic;
wb_stall_o : out std_logic;
uart_rxd_i : in std_logic;
uart_txd_o : out std_logic
);
end wb_simple_uart;
architecture syn of wb_simple_uart is
constant c_baud_acc_width : integer := 16;
constant c_vuart_fifo_size : integer := 1024;
component uart_baud_gen
generic (
g_baud_acc_width : integer);
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
baudrate_i : in std_logic_vector(g_baud_acc_width downto 0);
baud_tick_o : out std_logic;
baud8_tick_o : out std_logic);
end component;
component uart_async_rx
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
baud8_tick_i : in std_logic;
rxd_i : in std_logic;
rx_ready_o : out std_logic;
rx_error_o : out std_logic;
rx_data_o : out std_logic_vector(7 downto 0));
end component;
component uart_async_tx
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
baud_tick_i : in std_logic;
txd_o : out std_logic;
tx_start_p_i : in std_logic;
tx_data_i : in std_logic_vector(7 downto 0);
tx_busy_o : out std_logic);
end component;
component simple_uart_wb
port (
rst_n_i : in std_logic;
clk_sys_i : in std_logic;
wb_adr_i : in std_logic_vector(2 downto 0);
wb_dat_i : in std_logic_vector(31 downto 0);
wb_dat_o : out std_logic_vector(31 downto 0);
wb_cyc_i : in std_logic;
wb_sel_i : in std_logic_vector(3 downto 0);
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_ack_o : out std_logic;
wb_stall_o : out std_logic;
rdr_rack_o : out std_logic;
host_rack_o : out std_logic;
regs_i : in t_uart_in_registers;
regs_o : out t_uart_out_registers
);
end component;
signal rx_ready_reg : std_logic;
signal rx_ready : std_logic;
signal uart_bcr : std_logic_vector(31 downto 0);
signal rdr_rack : std_logic;
signal host_rack : std_logic;
signal baud_tick : std_logic;
signal baud_tick8 : std_logic;
signal resized_addr : std_logic_vector(c_wishbone_address_width-1 downto 0);
signal wb_in : t_wishbone_slave_in;
signal wb_out : t_wishbone_slave_out;
signal regs_in : t_UART_in_registers;
signal regs_out : t_UART_out_registers;
signal fifo_empty, fifo_full, fifo_rd, fifo_wr : std_logic;
signal fifo_count : std_logic_vector(f_log2_size(c_vuart_fifo_size)-1 downto 0);
signal phys_rx_ready, phys_tx_busy : std_logic;
signal phys_rx_data : std_logic_vector(7 downto 0);
begin -- syn
gen_check_generics : if(not g_with_physical_uart and not g_with_virtual_uart) generate
assert false report "wb_simple_uart: dummy configuration (use virtual, physical or both uarts)" severity failure;
end generate gen_check_generics;
resized_addr(4 downto 0) <= wb_adr_i;
resized_addr(c_wishbone_address_width-1 downto 5) <= (others => '0');
U_Adapter : wb_slave_adapter
generic map (
g_master_use_struct => true,
g_master_mode => CLASSIC,
g_master_granularity => WORD,
g_slave_use_struct => false,
g_slave_mode => g_interface_mode,
g_slave_granularity => g_address_granularity)
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
master_i => wb_out,
master_o => wb_in,
sl_adr_i => resized_addr,
sl_dat_i => wb_dat_i,
sl_sel_i => wb_sel_i,
sl_cyc_i => wb_cyc_i,
sl_stb_i => wb_stb_i,
sl_we_i => wb_we_i,
sl_dat_o => wb_dat_o,
sl_ack_o => wb_ack_o,
sl_stall_o => wb_stall_o);
U_WB_SLAVE : simple_uart_wb
port map (
rst_n_i => rst_n_i,
clk_sys_i => clk_sys_i,
wb_adr_i => wb_in.adr(2 downto 0),
wb_dat_i => wb_in.dat,
wb_dat_o => wb_out.dat,
wb_cyc_i => wb_in.cyc,
wb_sel_i => wb_in.sel,
wb_stb_i => wb_in.stb,
wb_we_i => wb_in.we,
wb_ack_o => wb_out.ack,
wb_stall_o => wb_out.stall,
rdr_rack_o => rdr_rack,
host_rack_o => host_rack,
regs_o => regs_out,
regs_i => regs_in);
gen_phys_uart : if(g_with_physical_uart) generate
p_bcr_reg : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
uart_bcr <= (others => '0');
elsif(regs_out.bcr_wr_o = '1')then
uart_bcr <= regs_out.bcr_o;
end if;
end if;
end process;
U_BAUD_GEN : uart_baud_gen
generic map (
g_baud_acc_width => c_baud_acc_width)
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
baudrate_i => uart_bcr(c_baud_acc_width downto 0),
baud_tick_o => baud_tick,
baud8_tick_o => baud_tick8);
U_TX : uart_async_tx
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
baud_tick_i => baud_tick,
txd_o => uart_txd_o,
tx_start_p_i => regs_out.tdr_tx_data_wr_o,
tx_data_i => regs_out.tdr_tx_data_o,
tx_busy_o => phys_tx_busy);
U_RX : uart_async_rx
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
baud8_tick_i => baud_tick8,
rxd_i => uart_rxd_i,
rx_ready_o => phys_rx_ready,
rx_error_o => open,
rx_data_o => phys_rx_data);
end generate gen_phys_uart;
gen_vuart : if(g_with_virtual_uart) generate
fifo_wr <= not fifo_full and regs_out.tdr_tx_data_wr_o;
fifo_rd <= not fifo_empty and not regs_in.host_rdr_rdy_i;
U_VUART_FIFO : generic_sync_fifo
generic map (
g_data_width => 8,
g_size => c_vuart_fifo_size,
g_with_count => true)
port map (
rst_n_i => rst_n_i,
clk_i => clk_sys_i,
d_i => regs_out.tdr_tx_data_o,
we_i => fifo_wr,
q_o => regs_in.host_rdr_data_i,
rd_i => fifo_rd,
empty_o => fifo_empty,
full_o => fifo_full,
count_o => fifo_count);
regs_in.host_rdr_count_i(fifo_count'left downto 0) <= fifo_count;
regs_in.host_rdr_count_i(15 downto fifo_count'length) <= (others => '0');
p_vuart_rx_ready : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
regs_in.host_rdr_rdy_i <= '0';
elsif(fifo_rd = '1') then
regs_in.host_rdr_rdy_i <= '1';
elsif(host_rack = '1') then
regs_in.host_rdr_rdy_i <= '0';
end if;
end if;
end process;
end generate gen_vuart;
p_drive_rx_ready : process(clk_sys_i)
begin
if rising_edge(clk_sys_i) then
if rst_n_i = '0' then
regs_in.sr_rx_rdy_i <= '0';
regs_in.rdr_rx_data_i <= (others => '0');
else
if(rdr_rack = '1' and phys_rx_ready = '0' and regs_out.host_tdr_data_wr_o = '0') then
regs_in.sr_rx_rdy_i <= '0';
elsif(phys_rx_ready = '1' and g_with_physical_uart) then
regs_in.sr_rx_rdy_i <= '1';
regs_in.rdr_rx_data_i <= phys_rx_data;
elsif(regs_out.host_tdr_data_wr_o = '1' and g_with_virtual_uart) then
regs_in.sr_rx_rdy_i <= '1';
regs_in.rdr_rx_data_i <= regs_out.host_tdr_data_o;
end if;
end if;
end if;
end process;
regs_in.sr_tx_busy_i <= phys_tx_busy when (g_with_physical_uart) else '0';
end syn;
------------------------------------------------------------------------------
-- Title : Simple Wishbone UART
-- Project : General Cores Collection (gencores) library
------------------------------------------------------------------------------
-- File : xwb_simple_uart.vhd
-- Author : Tomasz Wlostowski
-- Company : CERN BE-Co-HT
-- Created : 2010-05-18
-- Last update: 2011-11-02
-- Platform : FPGA-generic
-- Standard : VHDL'93
-------------------------------------------------------------------------------
-- Description: A simple UART controller, providing two modes of operation
-- (both can be used simultenously):
-- - physical UART (encoding fixed to 8 data bits, no parity and one stop bit)
-- - virtual UART: TXed data is passed via a FIFO to the Wishbone host (and
-- vice versa).
-------------------------------------------------------------------------------
-- Copyright (c) 2010 CERN
-------------------------------------------------------------------------------
-- Revisions :
-- Date Version Author Description
-- 2010-05-18 1.0 twlostow Created
-- 2011-10-04 1.1 twlostow xwb module
-------------------------------------------------------------------------------
library ieee;
use ieee.std_logic_1164.all;
library work;
use work.wishbone_pkg.all;
entity xwb_simple_uart is
generic(
g_with_virtual_uart : boolean := true;
g_with_physical_uart : boolean := true;
g_interface_mode : t_wishbone_interface_mode := CLASSIC;
g_address_granularity : t_wishbone_address_granularity := WORD
);
port(
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
-- Wishbone
slave_i : in t_wishbone_slave_in;
slave_o : out t_wishbone_slave_out;
desc_o : out t_wishbone_device_descriptor;
uart_rxd_i: in std_logic;
uart_txd_o: out std_logic
);
end xwb_simple_uart;
architecture rtl of xwb_simple_uart is
component wb_simple_uart
generic (
g_with_virtual_uart : boolean;
g_with_physical_uart : boolean;
g_interface_mode : t_wishbone_interface_mode;
g_address_granularity : t_wishbone_address_granularity);
port (
clk_sys_i : in std_logic;
rst_n_i : in std_logic;
wb_adr_i : in std_logic_vector(4 downto 0);
wb_dat_i : in std_logic_vector(31 downto 0);
wb_dat_o : out std_logic_vector(31 downto 0);
wb_cyc_i : in std_logic;
wb_sel_i : in std_logic_vector(3 downto 0);
wb_stb_i : in std_logic;
wb_we_i : in std_logic;
wb_ack_o : out std_logic;
wb_stall_o : out std_logic;
uart_rxd_i : in std_logic;
uart_txd_o : out std_logic);
end component;
begin -- rtl
U_Wrapped_UART: wb_simple_uart
generic map (
g_with_virtual_uart => g_with_virtual_uart,
g_with_physical_uart => g_with_physical_uart,
g_interface_mode => g_interface_mode,
g_address_granularity => g_address_granularity)
port map (
clk_sys_i => clk_sys_i,
rst_n_i => rst_n_i,
wb_adr_i => slave_i.adr(4 downto 0),
wb_dat_i => slave_i.dat,
wb_dat_o => slave_o.dat,
wb_cyc_i => slave_i.cyc,
wb_sel_i => slave_i.sel,
wb_stb_i => slave_i.stb,
wb_we_i => slave_i.we,
wb_ack_o => slave_o.ack,
wb_stall_o => slave_o.stall,
uart_rxd_i => uart_rxd_i,
uart_txd_o => uart_txd_o);
slave_o.err <= '0';
slave_o.rty <= '0';
slave_o.int <='0';
end rtl;
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment