MIPS: merged from BRANCH_MIPS_TLB

lib/CPUs/MIPS/ghdl/mips_core.inc

@@ -2,6 +2,7 @@
 include $(VHDL_MAKE_HOME)/defs.mk
 include $(LIB_PATH)/FIFO/ghdl/fifo_sync.inc
 include $(LIB_PATH)/FIFO/ghdl/fifo_async.inc
+include $(LIB_PATH)/JBUS/busmaster/ghdl/busmaster_async.inc
 
 # -----------------------------------------------------------
 PKG_NAME := MIPS_CORE
@@ -10,20 +11,24 @@ PKG_NAME := MIPS_CORE
 MOD_PATH := $(LIB_PATH)/CPUs/MIPS/src/core
 $(PKG_NAME)_SRCS += $(FIFO_SYNC_SRCS)
 $(PKG_NAME)_SRCS += $(FIFO_ASYNC_SRCS)
-
+$(PKG_NAME)_SRCS += $(BUSMASTER_ASYNC_SRCS)
 $(PKG_NAME)_SRCS += $(LIB_PATH)/rams/sim/src/dpram_2w2r2c_ra.vhd
 $(PKG_NAME)_SRCS += $(LIB_PATH)/rams/sim/src/dpram_1w1r2c_ra.vhd
+$(PKG_NAME)_SRCS += $(MOD_PATH)/mips_util_pkg.vhd
 $(PKG_NAME)_SRCS += $(MOD_PATH)/mips_types.vhd
+$(PKG_NAME)_SRCS += $(MOD_PATH)/mips_func_pkg.vhd
 $(PKG_NAME)_SRCS += $(MOD_PATH)/mips_instr.vhd
 $(PKG_NAME)_SRCS += $(MOD_PATH)/mips_alu.vhd
 $(PKG_NAME)_SRCS += $(MOD_PATH)/mips_bcu.vhd
-$(PKG_NAME)_SRCS += $(MOD_PATH)/mips_biu.vhd
+$(PKG_NAME)_SRCS += $(MOD_PATH)/mips_reg.vhd
-$(PKG_NAME)_SRCS += $(MOD_PATH)/mips_cop.vhd
+$(PKG_NAME)_SRCS += $(MOD_PATH)/mips_cam.vhd
+$(PKG_NAME)_SRCS += $(MOD_PATH)/mips_tlb.vhd
 $(PKG_NAME)_SRCS += $(MOD_PATH)/mips_dcache.vhd
 $(PKG_NAME)_SRCS += $(MOD_PATH)/mips_icache.vhd
+$(PKG_NAME)_SRCS += $(MOD_PATH)/mips_biu.vhd
+$(PKG_NAME)_SRCS += $(MOD_PATH)/mips_cop.vhd
 $(PKG_NAME)_SRCS += $(MOD_PATH)/mips_muldiv.vhd
 $(PKG_NAME)_SRCS += $(MOD_PATH)/mips_pipeline.vhd
-$(PKG_NAME)_SRCS += $(MOD_PATH)/mips_reg.vhd
 $(PKG_NAME)_SRCS += $(MOD_PATH)/mips_shifter.vhd
 $(PKG_NAME)_SRCS += $(MOD_PATH)/mips_top.vhd
 

lib/CPUs/MIPS/src/core/mips_biu.vhd

@@ -27,426 +27,282 @@ use IEEE.numeric_std.ALL;
 
 library work;
 use work.mips_types.all;
+use work.mips_util_pkg.all;
+use work.busmaster_types.all;
 
 entity biu is
 	Generic
 	(
-		icache_size	: natural := 2048;	-- words
+		ICACHE_SIZE			: natural := 2048;	-- words
-		icache_line	: natural := 8;		-- words
+		ICACHE_LINE			: natural := 8;		-- words
-		dcache_size	: natural := 2048;	-- words
+		DCACHE_SIZE			: natural := 2048;	-- words
-		dcache_line	: natural := 8		-- words
+		DCACHE_LINE			: natural := 8;		-- words
+		WRITE_FIFO_SIZE		: natural := 4;		-- words
+		WITH_TLB			: boolean := true;
+		TRANSLATE_KSEG0_1	: boolean := true
 	);
 	Port
 	(
+		clk				: in  STD_LOGIC;
+		busy			: out STD_LOGIC;
+
+		imem_err		: out STD_LOGIC;
+		dmem_err		: out STD_LOGIC;
+
 		RST_I			: in  STD_LOGIC;
 		CLK_I			: in  STD_LOGIC;
 		ACK_I			: in  STD_LOGIC;
 		SRDY_I			: in  STD_LOGIC;
 		ADDR_O			: out unsigned(31 downto 0);
-		DAT_I			: in  unsigned(31 downto 0);
+		MDAT_I			: in  unsigned(31 downto 0);
-		DAT_O			: out unsigned(31 downto 0);
+		MDAT_O			: out unsigned(31 downto 0);
 		WE_O			: out STD_LOGIC;
 		SEL_O			: out unsigned(3 downto 0);
 		CYC_O			: out STD_LOGIC;
 		STB_O			: out STD_LOGIC;
 		MRDY_O			: out STD_LOGIC;
-		cop0_ctrl_in	: in cop0_ctrl_out_t;
+		
-		cpu_clk			: in  STD_LOGIC;
+		ctrl_in			: in  biu_ctrl_in_t;
-		cpu_imem_err	: out STD_LOGIC;
+		ctrl_out		: out biu_ctrl_out_t
-		cpu_imem_rdy	: out STD_LOGIC;
+		
-		cpu_imem_en		: in  STD_LOGIC;
-		cpu_imem_addr	: in  word_t;
-		cpu_imem_din	: out word_t;
-		cpu_dmem_err	: out STD_LOGIC;
-		cpu_dmem_rdy	: out STD_LOGIC;
-		cpu_dmem_en		: in  STD_LOGIC;
-		cpu_dmem_we		: in  STD_LOGIC;
-		cpu_dmem_be		: in  unsigned(3 downto 0);
-		cpu_dmem_dout	: in  word_t;
-		cpu_dmem_din	: out word_t;
-		cpu_dmem_addr	: in  word_t
 	);
 end biu;
 
 architecture behavior of biu is
 
-	COMPONENT icache
-	GENERIC
-	(
-		cache_size	: natural;	-- words
-		line_size	: natural	-- words
-	);
-	PORT
-	(
-		RST_I		: in  STD_LOGIC;
-		CLK_I		: in  STD_LOGIC;
-		ACK_I		: in  STD_LOGIC;
-		SRDY_I		: in  STD_LOGIC;
-		MRDY_O		: out STD_LOGIC;
-		ADDR_O		: out word_t;
-		DAT_I		: in  word_t;
-		STB_O		: out STD_LOGIC;
-		CYC_O		: out STD_LOGIC;
-		ctrl		: in  cache_ctrl_t;
-		cpu_en		: in  STD_LOGIC;
-		cpu_addr	: in  word_t;
-		cpu_dout	: out word_t;
-		cpu_busy	: out STD_LOGIC
-	);
-	END COMPONENT;
-
-	COMPONENT dcache
-	GENERIC
-	(
-		cache_size	: natural;	-- words
-		line_size	: natural	-- words
-	);
-	PORT
-	(
-		RST_I		: in  STD_LOGIC;
-		CLK_I		: in  STD_LOGIC;
-		ACK_I		: in  STD_LOGIC;
-		SRDY_I		: in  STD_LOGIC;
-		MRDY_O		: out STD_LOGIC;
-		ADDR_O		: out word_t;
-		DAT_I		: in  word_t;
-		STB_O		: out STD_LOGIC;
-		CYC_O		: out STD_LOGIC;
-		ctrl		: in  cache_ctrl_t;
-		cpu_en		: in  STD_LOGIC;
-		cpu_we		: in  STD_LOGIC;
-		cpu_be		: in  unsigned(3 downto 0);
-		cpu_addr	: in  word_t;
-		cpu_din		: in  word_t;
-		cpu_dout	: out word_t;
-		cpu_busy	: out STD_LOGIC
-	);
-	END COMPONENT;
-
 	type bus_state_t is (init, ready, icache_bus_access, dcache_bus_access, write_bus, read_bus, read_finish);
 	
 	signal s, sn 			: bus_state_t;
 	signal bus_idle			: std_logic;
-	signal busy				: std_logic;
-	signal dmem_be			: unsigned(3 downto 0);
-	signal dcache_dout		: word_t;
-	signal dcache_mem_gnt	: std_logic;
-	signal icache_mem_gnt 	: std_logic;
-	signal dmem_mem_wr_gnt	: std_logic;
-	signal dmem_mem_rd_gnt	: std_logic;
-	signal dcache_busy		: std_logic;
-	signal icache_busy 		: std_logic;
-	signal CYC_O_icache		: std_logic;
-	signal CYC_O_dcache		: std_logic;
-	signal CYC_O_dmem_rd	: std_logic;
-	signal CYC_O_dmem_wr	: std_logic;
-	signal SRDY_I_icache	: std_logic;
-	signal SRDY_I_dcache	: std_logic;
-	signal MRDY_O_icache	: std_logic;
-	signal MRDY_O_dcache	: std_logic;
-	signal ADDR_O_icache	: word_t;
-	signal ADDR_O_dcache	: word_t;
-	signal ADDR_O_dmem_rd	: word_t;
-	signal ADDR_O_dmem_wr	: word_t;
-	signal STB_O_icache		: std_logic;
-	signal STB_O_dcache		: std_logic;
-	signal STB_O_dmem_rd	: std_logic;
-	signal STB_O_dmem_wr	: std_logic;
-	signal DAT_I_dmem_rd	: word_t;
-	signal DAT_O_dmem_wr	: word_t;
-	signal SEL_O_dmem_wr	: unsigned(3 downto 0);
-	signal SEL_O_dmem_rd	: unsigned(3 downto 0);
-	signal dcached			: std_logic;
-	signal dcache_en2		: std_logic;
-	signal dcache_en		: std_logic;
-	signal uncached_access	: std_logic;
-	signal ACK				: std_logic;
-	signal DAT				: unsigned(31 downto 0);
 
 	type timeout_cnt_t is range 0 to 1E5-1;	
 	signal bus_timeout_cnt	: timeout_cnt_t;
 	signal bus_timeout		: std_logic;
 	
-	signal bout_fifo_din	: unsigned(68 downto 0);
+	-------------------------------------------------
-	signal bout_fifo_dout	: unsigned(68 downto 0);
+	signal RST				: std_logic;
-	signal bout_fifo_re		: std_logic;
+	signal CPU_CLK			: std_logic;
-	signal bout_fifo_we		: std_logic;
-	signal bout_fifo_full	: std_logic;
-	signal bout_fifo_empty	: std_logic;
-	signal bout_rdy			: std_logic;
 
-	signal bin_fifo_din		: unsigned(31 downto 0);
+	-- busmaster signals
-	signal bin_fifo_dout	: unsigned(31 downto 0);
+	signal bus_din						: word_t;
-	signal bin_fifo_re		: std_logic;
+	signal bus_din_rdy					: std_logic;
-	signal bin_fifo_we		: std_logic;
+	signal bus_din_we					: std_logic;
-	signal bin_fifo_full	: std_logic;
-	signal bin_fifo_empty	: std_logic;
 
-	alias bout_fifo_addr_in is bout_fifo_din(31 downto 0);
+	signal bus_dout						: word_t;
-	alias bout_fifo_data_in is bout_fifo_din(63 downto 32);
+	signal bus_dout_vld					: std_logic;
-	alias bout_fifo_sel_in is bout_fifo_din(67 downto 64);
+	signal bus_dout_re					: std_logic;
-	alias bout_fifo_we_in is bout_fifo_din(68);
-	alias bout_fifo_addr_out is bout_fifo_dout(31 downto 0);
-	alias bout_fifo_data_out is bout_fifo_dout(63 downto 32);
-	alias bout_fifo_sel_out is bout_fifo_dout(67 downto 64);
-	alias bout_fifo_we_out is bout_fifo_dout(68);
 
-	signal write_fifo_din	: unsigned(67 downto 0);
+	signal bus_cmd_rdy					: std_logic;
-	signal write_fifo_dout	: unsigned(67 downto 0);
+	signal bus_cmd_we					: std_logic;
-	signal write_fifo_re	: std_logic;
+	signal bus_cmd_cycle_en				: std_logic;
-	signal write_fifo_we	: std_logic;
+	signal bus_cmd						: bm_cmd_t;
-	signal write_fifo_full	: std_logic;
+	signal bus_cyc_complete				: std_logic;
-	signal write_fifo_empty	: std_logic;
 
-	alias write_fifo_addr_in is write_fifo_din(31 downto 0);
+	signal i_bus_din_rdy				: std_logic;
-	alias write_fifo_data_in is write_fifo_din(63 downto 32);
+	signal i_bus_cmd_rdy				: std_logic;
-	alias write_fifo_sel_in is write_fifo_din(67 downto 64);
+	signal i_bus_cmd_we					: std_logic;
-	alias write_fifo_addr_out is write_fifo_dout(31 downto 0);
+	signal i_bus_cmd_cycle_en			: std_logic;
-	alias write_fifo_data_out is write_fifo_dout(63 downto 32);
+	signal i_bus_cmd					: bm_cmd_t;
-	alias write_fifo_sel_out is write_fifo_dout(67 downto 64);
 
-	signal read_cycle	: std_logic;
+	signal i_tlb_rdy					: std_logic;
+	signal i_cache_rdy					: std_logic;
+
+	signal d_bus_din_rdy				: std_logic;
+	signal d_bus_cmd_rdy				: std_logic;
+	signal d_bus_cmd_we					: std_logic;
+	signal d_bus_cmd_cycle_en			: std_logic;
+	signal d_bus_cmd					: bm_cmd_t;
+
+	signal d_tlb_rdy					: std_logic;
+	signal d_cache_rdy					: std_logic;
+
+	signal icache_bus_gnt				: std_logic;
+	signal dcache_bus_gnt				: std_logic;
 
 begin
 
-read_cyc_register:
+	CPU_CLK					<= clk;
-	process(cpu_clk)
+	RST						<= RST_I;
+
+biu_busy_state:
+	process(clk)
 	begin
-		if rising_edge(cpu_clk) then
+		if rising_edge(clk) then
 			if RST_I = '1' then
-				read_cycle		<= '0';
+				busy	<= '1';
-			else
+			elsif (i_tlb_rdy and i_cache_rdy and d_cache_rdy and d_tlb_rdy) = '1' then
-				read_cycle 		<= (dmem_mem_rd_gnt and CYC_O_dmem_rd) 
+				busy	<= '0';
-								or (dcache_mem_gnt and CYC_O_dcache) 
-								or (icache_mem_gnt and CYC_O_icache);
 			end if;
 		end if;
 	end process;	
-					
-	MRDY_O			<= not bin_fifo_full;
-	CYC_O			<= not bout_fifo_empty or read_cycle;
-	STB_O			<= not bout_fifo_empty;
-	ADDR_O			<= bout_fifo_addr_out;
-	DAT_O			<= bout_fifo_data_out;
-	SEL_O			<= bout_fifo_sel_out;
-	WE_O			<= bout_fifo_we_out;
 
-	cpu_imem_rdy	<= not icache_busy;
+inst_busmaster : entity work.busmaster_async
-	busy			<= CYC_O_dmem_rd or dcache_busy or (write_fifo_full);
-	cpu_dmem_rdy	<= not busy;
-	
-inst_icache : icache
 	GENERIC MAP
 	(
-		cache_size	=> icache_size,	-- words
+		DATA_WIDTH			=> 32,
-		line_size	=> icache_line	-- words
+		FIFO_DEPTH			=> 16
-	)	
-	PORT MAP
-	(
-		CLK_I		=> cpu_clk,
-		RST_I		=> RST_I,
-		STB_O		=> STB_O_icache,
-		CYC_O		=> CYC_O_icache,
-		ADDR_O		=> ADDR_O_icache,
-		MRDY_O		=> MRDY_O_icache,
-		DAT_I		=> DAT,
-		ACK_I		=> ACK,
-		SRDY_I		=> SRDY_I_icache,
-		ctrl		=> cop0_ctrl_in.icache,
-		cpu_en		=> cpu_imem_en,
-		cpu_addr	=> cpu_imem_addr,
-		cpu_dout	=> cpu_imem_din,
-		cpu_busy	=> icache_busy
-	);
-
-	SRDY_I_icache	<= bout_rdy and icache_mem_gnt;
-
-inst_dcache : dcache
-	GENERIC MAP
-	(
-		cache_size	=> dcache_size,	-- words
-		line_size	=> dcache_line	-- words
 	)
 	PORT MAP
 	(
-		CLK_I		=> cpu_clk,
+		-- System signals
-		RST_I		=> RST_I,
+		rst					=> RST,
-		CYC_O		=> CYC_O_dcache,
+		clk					=> CPU_CLK,
-		STB_O		=> STB_O_dcache,
+
-		ADDR_O		=> ADDR_O_dcache,
+		cmd_cycle_finished	=> bus_cyc_complete,
-		MRDY_O		=> MRDY_O_dcache,
+		cmd_cycle_en		=> bus_cmd_cycle_en,
-		DAT_I		=> DAT,
+		
-		ACK_I		=> ACK,
+		cmd_rdy				=> bus_cmd_rdy,
-		SRDY_I		=> SRDY_I_dcache,
+		cmd_we				=> bus_cmd_we,
-		ctrl		=> cop0_ctrl_in.dcache,
+		cmd_in				=> bus_cmd,
-		cpu_en		=> dcache_en,
+		
-		cpu_we		=> cpu_dmem_we,
+		din_rdy				=> bus_din_rdy,
-		cpu_be		=> cpu_dmem_be,
+		din_we				=> bus_din_we,
-		cpu_addr	=> cpu_dmem_addr,
+		din					=> bus_din,
-		cpu_din		=> cpu_dmem_dout,
+
-		cpu_dout	=> dcache_dout,
+		dout				=> bus_dout,
-		cpu_busy	=> dcache_busy
+		dout_vld			=> bus_dout_vld,
+		dout_re				=> bus_dout_re,
+
+		-- JBUS Master signals
+		RST_I				=> RST_I,
+		CLK_I				=> CLK_I,
+		CYC_O				=> CYC_O,
+		STB_O				=> STB_O,
+		WE_O				=> WE_O,
+		SEL_O				=> SEL_O,
+		ACK_I				=> ACK_I,
+		SRDY_I				=> SRDY_I,
+		MRDY_O				=> MRDY_O,
+		ADDR_O				=> ADDR_O,
+		MDAT_I				=> MDAT_I,
+		MDAT_O				=> MDAT_O
 	);
 
-	SRDY_I_dcache	<= bout_rdy and dcache_mem_gnt;
+inst_i_tlb : entity work.tlb
-
-	dcached			<= '1' when cpu_dmem_addr(31 downto 29) /= "101" else '0';
-	cpu_dmem_din	<= dcache_dout when uncached_access = '0' else DAT_I_dmem_rd;
-	dcache_en		<= dcached and cpu_dmem_en and dcache_en2; -- or write_fifo_full);
-	
--- Instantiate synchronous FIFO
-inst_bin_fifo: entity work.fifo_async
 	GENERIC MAP
 	(
-		addr_width			=> 4,
+		NUM_ENTRIES			=> TLB_NUM_ENTRIES,
-		data_width			=> 32,
+		CACHE_KSEG1			=> false,
-		do_last_read_update	=> true
+		TRANSLATE_KSEG0_1	=> TRANSLATE_KSEG0_1,
+		USE_TLB				=> WITH_TLB
 	)
 	PORT MAP
 	(
-		rst			=>	RST_I,
+		-- System signals
-		clk_w		=>	CLK_I,
+		rst					=> RST,
-		clk_r		=>	cpu_clk,
+		clk					=> CPU_CLK,
-		we			=>	bin_fifo_we,
+		ce					=> '1',
-		re			=>	bin_fifo_re,
+
-		fifo_full	=>	bin_fifo_full,
+		rdy					=> i_tlb_rdy,
-		fifo_empty	=>	bin_fifo_empty,
+
-		fifo_afull	=>	open,
+		ctrl_in				=> ctrl_in.itlb_ctrl,
-		fifo_aempty	=>	open,
+		ctrl_out			=> ctrl_out.itlb_ctrl,
-		data_w		=>  bin_fifo_din,
+
-		data_r		=>  bin_fifo_dout
+		query_in			=> ctrl_in.itlb_qry,
+		query_out			=> ctrl_out.itlb_qry
+
 	);
-	
+
-	bin_fifo_din	<= DAT_I;
+inst_icache: entity work.icache
-	DAT				<= bin_fifo_dout;
-	ACK				<= not bin_fifo_empty;
-	bin_fifo_we		<= ACK_I;
-	bin_fifo_re		<= dmem_mem_rd_gnt or MRDY_O_icache or MRDY_O_dcache;
-	
--- Instantiate synchronous FIFO
-inst_bout_fifo: entity work.fifo_async
 	GENERIC MAP
 	(
-		addr_width			=> 4,
+		CACHE_SIZE			=>	ICACHE_SIZE,
-		data_width			=> 69,
+		LINE_SIZE			=>	ICACHE_LINE
-		do_last_read_update	=> false,
-		add_extra_read_register => true
 	)
 	PORT MAP
 	(
-		rst			=>	RST_I,
+		rst					=>	RST,
-		clk_w		=>	cpu_clk,
+		clk					=>	CPU_CLK,
-		clk_r		=>	CLK_I,
+		rdy					=> 	i_cache_rdy,
-		we			=>	bout_fifo_we,
+
-		re			=>	bout_fifo_re,
+		-- CPU control/data
-		fifo_full	=>	bout_fifo_full,
+		query_in			=>	ctrl_in.icache_qry,
-		fifo_empty	=>	bout_fifo_empty,
+		query_out			=>	ctrl_out.icache_qry,
-		fifo_afull	=>	open,
+
-		fifo_aempty	=>	open,
+		-- Cache control
-		data_w		=>  bout_fifo_din,
+		ctrl_in				=>	ctrl_in.icache,
-		data_r		=>  bout_fifo_dout
+		
+		-- busmaster data
+		bus_din				=>	bus_dout,
+		bus_din_rdy			=>	i_bus_din_rdy,
+		bus_din_vld			=>	bus_dout_vld,
+
+		-- busmaster command
+		bus_cmd_rdy			=>	i_bus_cmd_rdy,
+		bus_cmd_we			=>	i_bus_cmd_we,
+		bus_cmd_cycle_en	=>	i_bus_cmd_cycle_en,
+		bus_cmd_out			=>	i_bus_cmd,
+		bus_cyc_complete	=>	bus_cyc_complete
 	);
-	bout_rdy			<= not bout_fifo_full;
+
-	bout_fifo_re		<= SRDY_I and not bin_fifo_full;
 	
-	bout_fifo_we		<= STB_O_dmem_wr or STB_O_dmem_rd or STB_O_dcache or STB_O_icache;
+inst_d_tlb : entity work.tlb
-
-	bout_fifo_data_in	<= DAT_O_dmem_wr when dmem_mem_wr_gnt = '1' else (others => '-');
-	
-	bout_fifo_addr_in	<= ADDR_O_dmem_wr when dmem_mem_wr_gnt = '1' else
-						   ADDR_O_dmem_rd when dmem_mem_rd_gnt = '1' else
-						   ADDR_O_dcache when dcache_mem_gnt = '1' else
-						   ADDR_O_icache when icache_mem_gnt = '1' else (others => '-');
-
-	bout_fifo_sel_in	<= SEL_O_dmem_wr when dmem_mem_wr_gnt = '1' else
-						   SEL_O_dmem_rd when dmem_mem_rd_gnt = '1' else (others => '1');
-
-	bout_fifo_we_in		<= '1' when dmem_mem_wr_gnt = '1' else '0';
-
--- Instantiate synchronous FIFO
-inst_write_fifo: entity work.fifo_sync
 	GENERIC MAP
 	(
-		addr_width			=> 4,
+		NUM_ENTRIES			=> TLB_NUM_ENTRIES,
-		data_width			=> 68,
+		CACHE_KSEG1			=> false,
-		do_last_read_update	=> true
+		TRANSLATE_KSEG0_1	=> TRANSLATE_KSEG0_1,
+		USE_TLB				=> WITH_TLB
 	)
 	PORT MAP
 	(
-		rst			=>	RST_I,
+		-- System signals
-		clk			=>	cpu_clk,
+		rst					=> RST,
-		we			=>	write_fifo_we,
+		clk					=> CPU_CLK,
-		re			=>	write_fifo_re,
+		ce					=> '1',
-		fifo_full	=>	write_fifo_full,
+
-		fifo_empty	=>	write_fifo_empty,
+		rdy					=> d_tlb_rdy,
-		fifo_afull	=>	open,
+
-		fifo_aempty	=>	open,
+		ctrl_in				=> ctrl_in.dtlb_ctrl,
-		data_w		=>  write_fifo_din,
+		ctrl_out			=> ctrl_out.dtlb_ctrl,
-		data_r		=>  write_fifo_dout
+
+		query_in			=> ctrl_in.dtlb_qry,
+		query_out			=> ctrl_out.dtlb_qry
+
+	);
+
+inst_dcache: entity work.dcache
+	GENERIC MAP
+	(
+		CACHE_SIZE			=>	DCACHE_SIZE,
+		LINE_SIZE			=>	DCACHE_LINE,
+		WRITE_FIFO_SIZE		=>	WRITE_FIFO_SIZE
+	)
+	PORT MAP
+	(
+		rst					=>	RST,
+		clk					=>	CPU_CLK,
+		rdy					=>	d_cache_rdy,
+
+		-- CPU control/data
+		query_in			=>	ctrl_in.dcache_qry,
+		query_out			=>	ctrl_out.dcache_qry,
+
+		-- Cache control
+		ctrl_in				=>	ctrl_in.dcache,
+
+		-- busmaster data
+		bus_din				=>	bus_dout,
+		bus_din_rdy			=>	d_bus_din_rdy,
+		bus_din_vld			=>	bus_dout_vld,
+
+		bus_dout			=>	bus_din,
+		bus_dout_vld		=>	bus_din_we,
+		bus_dout_rdy		=>	bus_din_rdy,
+
+		-- busmaster command
+		bus_cmd_rdy			=>	d_bus_cmd_rdy,
+		bus_cmd_we			=>	d_bus_cmd_we,
+		bus_cmd_cycle_en	=>	d_bus_cmd_cycle_en,
+		bus_cmd_out			=>	d_bus_cmd,
+		bus_cyc_complete	=>	bus_cyc_complete
 	);
 
-	CYC_O_dmem_wr	<= not write_fifo_empty;
-	DAT_O_dmem_wr	<= write_fifo_data_out;
-	ADDR_O_dmem_wr	<= write_fifo_addr_out;
-	SEL_O_dmem_wr	<= write_fifo_sel_out;
 
-	write_fifo_data_in	<= cpu_dmem_dout;
-	write_fifo_addr_in	<= cpu_dmem_addr;
-	write_fifo_sel_in	<= cpu_dmem_be;
-	write_fifo_re		<= STB_O_dmem_wr;
-	write_fifo_we		<= cpu_dmem_en and not busy and cpu_dmem_we;
-	
-dmem_rd_flags:
-	process(cpu_clk)
-	begin
-		if rising_edge(cpu_clk) then
-			uncached_access	<= '0';
-			if RST_I = '1' then
-				CYC_O_dmem_rd		<= '0';
-				dcache_en2			<= '1';
-			else	
-				if ACK = '1' and dmem_mem_rd_gnt = '1' then
-					uncached_access		<= '1';
-					CYC_O_dmem_rd		<= '0';
-					dcache_en2			<= '1';
-				end if;	
-				if cpu_dmem_en = '1' and busy = '0' and cpu_dmem_we = '0' then
-					if dcached = '0' then
-						CYC_O_dmem_rd	<= '1';
-						dcache_en2		<= '0';
-					end if;	
-				end if;
-			end if;
-		end if;
-	end process;	
-	
-dmem_rd_data:
-	process(cpu_clk)
-	begin
-		if rising_edge(cpu_clk) then
-			if RST_I = '1' then
-				DAT_I_dmem_rd		<= (others => '0');
-			elsif ACK = '1' and CYC_O_dmem_rd = '1' then
-				DAT_I_dmem_rd		<= DAT;
-			end if;
-		end if;
-	end process;	
-	
-dmem_rd_regs:
-	process(cpu_clk)
-	begin
-		if rising_edge(cpu_clk) then
-			if cpu_dmem_en = '1' and busy = '0' then
-				ADDR_O_dmem_rd		<= cpu_dmem_addr;
-				SEL_O_dmem_rd		<= cpu_dmem_be;
-			end if;
-		end if;
-	end process;	
-	
 bus_state_next:
-	process(cpu_clk)
+	process(clk)
 	begin
-		if rising_edge(cpu_clk) then
+		if rising_edge(clk) then
 			if RST_I = '1' then
 				s	<= init;
 			else
@@ -456,61 +312,51 @@ bus_state_next:
 	end process;	
 	
 bus_state:
-	process(s, CYC_O_icache, CYC_O_dcache, CYC_O_dmem_wr, CYC_O_dmem_rd, bout_rdy, ACK)
+	process(s, i_bus_cmd_cycle_en, d_bus_cmd_cycle_en, i_bus_cmd_we, i_bus_cmd, d_bus_cmd_we, d_bus_cmd, bus_cyc_complete, i_bus_din_rdy, d_bus_din_rdy, bus_cmd_rdy)
 	begin
 		
-		icache_mem_gnt		<= '0';
+		icache_bus_gnt		<= '0';
-		dcache_mem_gnt		<= '0';
+		dcache_bus_gnt		<= '0';
-		dmem_mem_rd_gnt		<= '0';	
+
-		dmem_mem_wr_gnt		<= '0';	
+		i_bus_cmd_rdy		<= '0';
-		STB_O_dmem_rd		<= '0';
+		d_bus_cmd_rdy		<= '0';
-		STB_O_dmem_wr		<= '0';
+		
+		bus_cmd_cycle_en	<= '0';
+		bus_cmd_we			<= '0';	
+		bus_cmd				<= i_bus_cmd;
+		bus_dout_re			<= '0';
 		bus_idle			<= '0';
+		
 		sn					<= s;
 		case s is
 		when init =>
 			sn		<= ready;
 		when ready =>
 			bus_idle	<= '1';
-			if CYC_O_dmem_wr = '1' then
+			if i_bus_cmd_cycle_en = '1' then
-				sn			<= write_bus;
-			elsif CYC_O_dmem_rd = '1' then
-				sn			<= read_bus;
-			elsif CYC_O_icache = '1' then
 				sn			<= icache_bus_access;
-			elsif CYC_O_dcache = '1' then
+			elsif d_bus_cmd_cycle_en = '1' then
 				sn			<= dcache_bus_access;
 			end if;
 		when icache_bus_access =>
-			icache_mem_gnt	<= '1';
+			icache_bus_gnt		<= '1';
-			if CYC_O_icache = '0' then
+			i_bus_cmd_rdy		<= bus_cmd_rdy;
+			bus_cmd_cycle_en	<= i_bus_cmd_cycle_en;
+			bus_cmd_we			<= i_bus_cmd_we;
+			bus_dout_re			<= i_bus_din_rdy;
+			if i_bus_cmd_cycle_en = '0' then
 				sn			<= ready;
 			end if;	
 		when dcache_bus_access =>
-			dcache_mem_gnt	<= '1';
+			dcache_bus_gnt		<= '1';
-			if CYC_O_dcache = '0' then
+			d_bus_cmd_rdy		<= bus_cmd_rdy;
+			bus_cmd_cycle_en	<= d_bus_cmd_cycle_en;
+			bus_cmd				<= d_bus_cmd;	
+			bus_cmd_we			<= d_bus_cmd_we;
+			bus_dout_re			<= d_bus_din_rdy;
+			if d_bus_cmd_cycle_en = '0' then
 				sn			<= ready;
 			end if;	
-		when write_bus =>
-			dmem_mem_wr_gnt	<= '1';	
-			if CYC_O_dmem_wr = '1' then
-				if bout_rdy = '1' then
-					STB_O_dmem_wr	<= '1';
-				end if;
-			else
-				sn			<= ready;
-			end if;	
-		when read_bus =>
-			dmem_mem_rd_gnt	<= '1';	
-			if bout_rdy = '1' then
-				STB_O_dmem_rd	<= '1';
-				sn				<= read_finish;
-			end if;	
-		when read_finish =>
-			dmem_mem_rd_gnt	<= '1';	
-			if ACK = '1' then
-				sn			<= ready;
-			end if;
 		when others =>
 			sn	<= ready;
 		end case;		
@@ -518,9 +364,9 @@ bus_state:
 	end process;	
 
 bus_timeout_counter:
-	process(cpu_clk)
+	process(clk)
 	begin
-		if rising_edge(cpu_clk) then
+		if rising_edge(clk) then
 			if bus_idle = '0' then
 				if bus_timeout_cnt /= 0 then
 					bus_timeout_cnt	<= bus_timeout_cnt - 1;
@@ -535,15 +381,15 @@ bus_timeout_counter:
 	end process;	
 
 bus_err:
-	process(cpu_clk)
+	process(clk)
 	begin
-		if rising_edge(cpu_clk) then
+		if rising_edge(clk) then
 			if RST_I = '1' then
-				cpu_imem_err	<= '0';
+				imem_err	<= '0';
-				cpu_dmem_err	<= '0';
+				dmem_err	<= '0';
 			elsif bus_timeout = '1' then
-				cpu_imem_err	<= icache_mem_gnt;
+				imem_err	<= icache_bus_gnt;
-				cpu_dmem_err	<= dcache_mem_gnt or dmem_mem_wr_gnt or dmem_mem_rd_gnt;
+				dmem_err	<= dcache_bus_gnt;
 			end if;
 		end if;
 	end process;	

lib/CPUs/MIPS/src/core/mips_cam.vhd

@@ -0,0 +1,340 @@
+--------------------------------------------------------------------------
+-- Project: JIPS, a portable 32-bit RISC CPU written in VHDL
+-- This file: JIPS top file 
+--
+-- Copyright (C) 2008 J. Ahrensfeld
+--
+-- 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, either version 3 of the License, or
+-- (at your option) any later version.
+--
+-- 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/>.
+--
+-- For questions and ideas, please contact the author at jens@jayfield.org
+--
+--------------------------------------------------------------------------
+
+LIBRARY IEEE;
+USE IEEE.MATH_REAL.ALL;
+USE IEEE.STD_LOGIC_1164.ALL;
+USE IEEE.NUMERIC_STD.ALL;
+
+library work;
+use work.mips_types.all;
+use work.mips_util_pkg.all;
+
+ENTITY cam IS
+	Generic
+	(
+		NUM_ENTRIES			: natural := 32;
+		DATA_WIDTH			: natural := 20;
+		CAM_RAM_MAX_WIDTH	: natural := 15
+	);
+	Port
+	(
+		rst				: in  STD_LOGIC;
+		clk				: in  STD_LOGIC;
+		clk_rd			: in  STD_LOGIC;
+		
+		rdy				: out STD_LOGIC;
+
+		re				: in  STD_LOGIC;
+		we				: in  STD_LOGIC;
+		vld				: in  STD_LOGIC;
+		addr			: in  unsigned (lg2(NUM_ENTRIES)-1 downto 0);
+		tag_wr			: in  unsigned (DATA_WIDTH-1 downto 0);
+
+		tag_rd			: in  unsigned (DATA_WIDTH-1 downto 0);
+		hit				: out unsigned (NUM_ENTRIES-1 downto 0);
+		hit_vld			: out STD_LOGIC
+
+	);
+END cam;
+
+ARCHITECTURE behavior OF cam IS
+
+	type debug_t is record
+		num_cam_rams			: integer;
+		cam_ram_data_width		: integer;
+		cam_ram_addr_width		: integer;
+	end record;
+	
+	signal debug : debug_t;
+	
+	constant NUM_BLOCKS						: integer := integer(real(DATA_WIDTH)/real(CAM_RAM_MAX_WIDTH-lg2(NUM_ENTRIES)));
+	constant BLOCK_ADDR_WIDTH				: integer := CAM_RAM_MAX_WIDTH - lg2(NUM_ENTRIES);
+	
+	type state_t is (init, ready, clear);
+	signal state							: state_t;
+	signal state_next						: state_t;
+
+	type pipe_stage_t is record
+		re			: std_logic;
+		we			: std_logic;
+		vld			: std_logic;
+		addr		: unsigned (lg2(NUM_ENTRIES)-1 downto 0);
+		data		: unsigned (DATA_WIDTH-1 downto 0);
+		vld_cam		: unsigned (NUM_ENTRIES-1 downto 0);
+	end record;
+	
+	signal pipe0							: pipe_stage_t;
+	signal pipe1							: pipe_stage_t;
+	signal pipe2							: pipe_stage_t;
+	signal pipe3							: pipe_stage_t;
+	signal pipe0_rd							: pipe_stage_t;
+	signal pipe1_rd							: pipe_stage_t;
+	signal pipe2_rd							: pipe_stage_t;
+	signal pipe3_rd							: pipe_stage_t;
+	signal  busy							: std_logic;
+	signal	vld_cam							: unsigned (NUM_ENTRIES-1 downto 0);
+	
+	signal  cam_ram_re						: std_logic;
+	signal  cam_ram_we						: std_logic;
+	
+	type cam_ram_array_t is array (0 to NUM_BLOCKS-1) of unsigned (NUM_ENTRIES-1 downto 0);
+	signal	cam_ram_din						: cam_ram_array_t;
+	signal	cam_ram_dout					: cam_ram_array_t;
+	signal	cam_ram_result					: unsigned (NUM_ENTRIES-1 downto 0);
+	
+	signal	cam_ram_addr_wr					: unsigned (DATA_WIDTH-1 downto 0);
+	signal	cam_ram_addr_rd					: unsigned (DATA_WIDTH-1 downto 0);
+
+	signal  track_ram_re					: std_logic;
+	signal  track_ram_we					: std_logic;
+	signal	track_ram_addr_wr				: unsigned (lg2(NUM_ENTRIES)-1 downto 0);
+	signal	track_ram_addr_rd				: unsigned (lg2(NUM_ENTRIES)-1 downto 0);
+	signal	track_ram_din					: unsigned (DATA_WIDTH downto 0);
+	signal	track_ram_dout					: unsigned (DATA_WIDTH downto 0);
+
+	signal clear_data						: unsigned (DATA_WIDTH-1 downto 0);
+	signal clear_addr						: unsigned (BLOCK_ADDR_WIDTH-1 downto 0);
+	signal clear_count_en					: std_logic;
+	signal clear_count_rdy					: std_logic;
+	signal over_write						: std_logic;
+	
+	
+begin
+	
+	over_write					<= track_ram_dout(DATA_WIDTH) and pipe1.we;
+	rdy							<= not busy;
+	hit_vld						<= pipe1.re;
+	
+	debug.num_cam_rams			<= NUM_BLOCKS;
+	debug.cam_ram_addr_width	<= BLOCK_ADDR_WIDTH;
+	debug.cam_ram_data_width	<= NUM_ENTRIES;
+
+	track_ram_re				<= '1';
+	track_ram_we				<= clear_count_en or pipe1.we;
+	track_ram_addr_rd			<= pipe0.addr;
+	track_ram_addr_wr			<= clear_data(lg2(NUM_ENTRIES)-1 downto 0) when clear_count_en = '1' else pipe1.addr;
+	track_ram_din				<= (others => '0') when clear_count_en = '1' else (pipe1.vld & pipe1.data);
+
+	cam_ram_re					<= re when pipe1_rd.we = '0' else not busy;
+	cam_ram_we					<= clear_count_en or (pipe2.we and pipe2.vld) or over_write;
+	cam_ram_addr_rd				<= tag_rd when pipe1_rd.we = '0' else pipe1_rd.data;	-- clk_rd same edge of clk
+--	cam_ram_addr_rd				<= tag_rd when pipe2_rd.we = '0' else pipe2_rd.data;	-- clk_rd opposite edge of clk
+	cam_ram_addr_wr				<= clear_data when clear_count_en = '1' else track_ram_dout(DATA_WIDTH-1 downto 0) when over_write = '1' else pipe2.data;
+
+combine_cam_ram_din:
+	process(clear_count_en, over_write, pipe2, cam_ram_dout)
+	begin
+		for i in 0 to NUM_BLOCKS-1 loop
+			if clear_count_en = '1' or over_write = '1' then
+				cam_ram_din(i) <= (others => '0');
+			else
+				cam_ram_din(i) <= (((NUM_ENTRIES-1 downto 0 => pipe2.vld) and pipe2.vld_cam) xor cam_ram_dout(i));	
+			end if;
+		end loop;
+	end process;
+
+combine_clear_addr:
+	process(clear_addr)
+	begin
+		for i in 0 to NUM_BLOCKS-1 loop
+			clear_data((i+1)*BLOCK_ADDR_WIDTH-1 downto i*BLOCK_ADDR_WIDTH) <= clear_addr;
+		end loop;
+	end process;
+
+combine_cam_ram_result:
+	process(cam_ram_dout)
+	variable result : unsigned (NUM_ENTRIES-1 downto 0);
+	begin
+		result := (others => '0');
+		
+		for i in 0 to NUM_BLOCKS-1 loop
+			result := result or cam_ram_dout(i);
+		end loop;
+		
+		cam_ram_result		<= result;
+	end process;
+	
+	pipe0.re		<= re and not busy;
+	pipe0.vld		<= vld;
+	pipe0.we		<= we and not busy;
+	pipe0.addr		<= addr;
+	pipe0.data		<= tag_wr;
+	pipe0.vld_cam	<= vld_cam;
+
+pipe_line_wr:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				pipe3.we	<= '0';
+				pipe2.we	<= '0';
+				pipe1.we	<= '0';
+			else
+				pipe3	<= pipe2;
+				pipe2	<= pipe1;
+				pipe1	<= pipe0;
+			end if;
+		end if;
+	end process;	
+
+	pipe0_rd	<= pipe0;
+pipe_line_rd:
+	process(clk_rd)
+	begin
+		if rising_edge(clk_rd) then
+			if rst = '1' then
+				pipe3_rd.we	<= '0';
+				pipe2_rd.we	<= '0';
+				pipe1_rd.we	<= '0';
+			else
+				pipe3_rd	<= pipe2_rd;
+				pipe2_rd	<= pipe1_rd;
+				pipe1_rd	<= pipe0_rd;
+			end if;
+		end if;
+	end process;	
+
+combine_cam_hit:
+	process(cam_ram_dout, clear_count_en)
+	variable result : unsigned (NUM_ENTRIES-1 downto 0);
+	begin
+		result := (others => not clear_count_en);
+		
+		for i in 0 to NUM_BLOCKS-1 loop
+			result := result and cam_ram_dout(i);
+		end loop;
+		
+		hit			<= result;
+	end process;
+	
+vld_decode:
+process(vld, addr)
+	begin
+		vld_cam							<= (others => '0');
+		vld_cam(to_integer(addr))		<= vld;
+	end process;	
+
+
+inst_track_ram: entity work.dpram_1w1r2c_ra
+	GENERIC MAP 
+	(
+		addr_width	=>	lg2(NUM_ENTRIES),
+		data_width	=>	DATA_WIDTH+1
+	)
+	PORT MAP
+	(
+		clk_a	=>	clk,
+		clk_b	=>	clk,
+		we_a	=>	track_ram_we,
+		re_b	=>	'1',
+		addr_a	=>	track_ram_addr_wr,
+		addr_b	=>	track_ram_addr_rd,
+		din_a	=>	track_ram_din,
+		dout_b	=>	track_ram_dout
+	);
+
+gen_cam_ram:
+for i in 0 to NUM_BLOCKS-1 generate
+	
+	inst_cam_ram: entity work.dpram_1w1r2c_ra
+		GENERIC MAP 
+		(
+			addr_width	=>	BLOCK_ADDR_WIDTH,
+			data_width	=>	NUM_ENTRIES
+		)
+		PORT MAP
+		(
+			clk_a	=>	clk,
+			clk_b	=>	clk_rd,
+			we_a	=>	cam_ram_we,
+			re_b	=>	cam_ram_re,
+			addr_a	=>	cam_ram_addr_wr((i+1)*BLOCK_ADDR_WIDTH-1 downto i*BLOCK_ADDR_WIDTH),
+			addr_b	=>	cam_ram_addr_rd((i+1)*BLOCK_ADDR_WIDTH-1 downto i*BLOCK_ADDR_WIDTH),
+			din_a	=>	cam_ram_din(i),
+			dout_b	=>	cam_ram_dout(i)
+		);
+
+end generate;
+
+cam_state_next:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				state		<= init;
+			else
+				state		<= state_next;
+			end if;
+		end if;
+	end process;	
+
+cam_state:
+	process(state, clear_count_rdy, over_write)
+	begin
+		busy				<= over_write;
+		clear_count_en		<= '0';
+		
+		state_next		<= state;
+		
+		case state is
+
+		when init =>
+			busy		<= '1';
+			state_next <= clear;
+			
+		when clear =>
+			busy		<= '1';
+			clear_count_en	<= '1';
+			if clear_count_rdy = '1' then
+				state_next <= ready;
+			end if;
+
+		when ready =>
+
+		when others => 
+			state_next <= ready;
+				
+		end case;
+		
+	end process;
+
+clear_counter:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if clear_count_en = '0' then
+				clear_count_rdy	<= '0';
+				clear_addr		<= (BLOCK_ADDR_WIDTH-1 downto 0 => '1');
+			else	
+				if clear_addr /= (BLOCK_ADDR_WIDTH-1 downto 0 => '0') then
+					clear_addr 	<= clear_addr - 1;
+				else
+					clear_count_rdy	<= '1';
+				end if;
+			end if;	
+		end if;
+	end process;	
+	
+------------------------------------------------------------------
+end behavior;

lib/CPUs/MIPS/src/core/mips_dcache.vhd

@@ -26,33 +26,46 @@ USE IEEE.STD_LOGIC_1164.ALL;
 USE IEEE.NUMERIC_STD.ALL;
 
 library work;
+use work.mips_util_pkg.all;
 use work.mips_types.all;
+use work.busmaster_types.all;
 
 ENTITY dcache IS
 	Generic
 	(
-		cache_size	: natural := 2048;	-- words
+		CACHE_SIZE			: natural := 1024;	-- words
-		line_size	: natural := 8		-- words
+		LINE_SIZE			: natural := 8;		-- words
+		WRITE_FIFO_SIZE		: natural := 4
 	);
 	Port
 	(
-		RST_I		: in  STD_LOGIC;
+		rst					: in  STD_LOGIC;
-		CLK_I		: in  STD_LOGIC;
+		clk					: in  STD_LOGIC;
-		ACK_I		: in  STD_LOGIC;
+		rdy					: out STD_LOGIC;
-		SRDY_I		: in  STD_LOGIC;
+		
-		MRDY_O		: out STD_LOGIC;
+		-- CPU control/data
-		ADDR_O		: out word_t;
+		query_in			: in  dcache_query_in_t;
-		DAT_I		: in  word_t;
+		query_out			: out dcache_query_out_t;
-		STB_O		: out STD_LOGIC;
+
-		CYC_O		: out STD_LOGIC;
+		-- Cache control
-		ctrl		: in  cache_ctrl_t;
+		ctrl_in				: in  cache_ctrl_t;
-		cpu_en		: in  STD_LOGIC;
+
-		cpu_we		: in  STD_LOGIC;
+		-- busmaster data
-		cpu_be		: in  unsigned(3 downto 0);
+		bus_din				: in word_t;
-		cpu_addr	: in  word_t;
+		bus_din_rdy			: out std_logic;
-		cpu_din		: in  word_t;
+		bus_din_vld			: in std_logic;
-		cpu_dout	: out word_t;
+
-		cpu_busy	: out STD_LOGIC
+		bus_dout			: out word_t;
+		bus_dout_vld		: out std_logic;
+		bus_dout_rdy		: in std_logic;
+
+		-- busmaster command
+		bus_cmd_rdy			: in std_logic;
+		bus_cmd_we			: out std_logic;
+		bus_cmd_cycle_en	: out std_logic;
+		bus_cmd_out			: out bm_cmd_t;
+		bus_cyc_complete	: in std_logic
+		
 	);
 END dcache;
 
@@ -83,27 +96,26 @@ COMPONENT dpram_2w2r2c_ra is
 END COMPONENT;
 
 	constant addr_width			: natural := 32;
-	constant word_index_width	: natural := lg2(line_size);
+	constant word_index_width	: natural := lg2(LINE_SIZE);
-	constant cache_index_width	: natural := lg2(cache_size) - word_index_width;
+	constant cache_index_width	: natural := lg2(CACHE_SIZE) - word_index_width;
-	constant tag_width			: natural := addr_width - word_index_width - cache_index_width - 2;
+	constant tag_width			: natural := addr_width - lg2(CACHE_SIZE) - 2;
-	constant tag_parity_width	: natural := 3;
+	constant tag_ram_data_width	: natural := 1 + tag_width + lg2(TLB_NUM_ENTRIES)+1;
-	constant tag_ram_data_width	: natural := 1 + tag_parity_width + tag_width;
 	constant tag_ram_addr_width	: natural := cache_index_width;
 	
 	subtype	tag_ram_data_t is unsigned (tag_ram_data_width-1 downto 0);
 
 	type dcache_entry_t is record
-		valid	: std_logic;
+		valid		: std_logic;
-		tv_p	: unsigned(tag_parity_width-1 downto 0);
+		tag			: unsigned(tag_width-1 downto 0);
-		tag		: unsigned(tag_width-1 downto 0);
+		idx			: unsigned(lg2(TLB_NUM_ENTRIES) downto 0);
 	end record;
 	
 	function to_dcache_entry(x : tag_ram_data_t) return dcache_entry_t is
 	variable result : dcache_entry_t;
 	begin
 		result.valid	:= x(0);
-		result.tv_p		:= x(3 downto 1);
+		result.tag		:= x(tag_width downto 1);
-		result.tag		:= x(tag_width+3 downto 4);
+		result.idx		:= x(lg2(TLB_NUM_ENTRIES)+tag_width+1 downto tag_width+1);
 		
 		return result;
 	end to_dcache_entry;
@@ -111,170 +123,314 @@ END COMPONENT;
 	function to_tag_ram_data(x : dcache_entry_t) return tag_ram_data_t is
 	variable result : tag_ram_data_t;
 	begin
-		result(0)										:= x.valid;
+		result(0)													:= x.valid;
-		result(3 downto 1)								:= x.tv_p;
+		result(tag_width downto 1)									:= x.tag;
-		result(tag_width+3 downto 4)					:= x.tag;
+		result(lg2(TLB_NUM_ENTRIES)+tag_width+1 downto tag_width+1)	:= x.idx;
 		
 		return result;
 	end to_tag_ram_data;
 
-	type cache_state_t is (init, ready, invalidate_init, invalidate, flush, mem_request, mem_access, mem_data, rd_cache);
+	function to_word_index(x : word_t) return unsigned is
-	signal s, sn : cache_state_t;
+	variable result : unsigned(word_index_width-1 downto 0);
+	begin
+		result := x(word_index_width+1 downto 2);
+		
+		return result;
+		
+	end to_word_index;
 	
-	signal	cache_req			: std_logic;
-	signal	cache_ack			: std_logic;
-	signal	cache_busy			: std_logic;
-	signal	cache_hit			: std_logic;
-	signal	tag_match			: std_logic;
-	signal	cache_hit_inv		: std_logic;
-	signal	tag_match_inv		: std_logic;
-	
-	signal  request_addr		: unsigned(addr_width-1 downto 0);
-	signal  fill_addr			: unsigned(addr_width-1 downto 0);
 
-	signal  cache_index_inv		: unsigned(cache_index_width-1 downto 0);
+	function to_cache_index(x : word_t) return unsigned is
-	signal  tag_inv				: unsigned(tag_width-1 downto 0);
+	variable result : unsigned(cache_index_width-1 downto 0);
-	signal  tag_reg_inv			: unsigned(tag_width-1 downto 0);
+	begin
-	
+		result := x(cache_index_width+word_index_width+1 downto word_index_width+2);
-	signal	cache_entry_in		: dcache_entry_t;
+		
-	signal	cache_entry_out		: dcache_entry_t;
+		return result;
-	signal	cache_entry_out_inv	: dcache_entry_t;
+		
-	signal  cpu_data_ram_addr	: unsigned(lg2(cache_size)-1 downto 0);
+	end to_cache_index;
-	signal  cpu_data_ram_dout	: word_t;
-	signal  cpu_data_reg		: word_t;
-	signal  cpu_be_reg			: unsigned(3 downto 0);
-	signal  cpu_we_reg			: std_logic;
-	signal  ctrl_data_ram_addr	: unsigned(lg2(cache_size)-1 downto 0);
-	signal	ctrl_data_ram_we	: unsigned(3 downto 0);
-	signal	cpu_data_ram_we		: unsigned(3 downto 0);
-	signal	cpu_we2				: std_logic;
 
-	signal  tag_ram_addr_rd		: unsigned(cache_index_width-1 downto 0);
+	function to_tag(x : word_t) return unsigned is
-	signal  tag_ram_dout		: tag_ram_data_t;
+	variable result : unsigned(tag_width-1 downto 0);
-	signal  tag_ram_dout_inv	: tag_ram_data_t;
+	begin
-	signal  tag_ram_addr_wr		: unsigned(cache_index_width-1 downto 0);
+		result := x(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
-	signal  tag_ram_din			: tag_ram_data_t;
+		
-	signal	tag_ram_we			: std_logic;
+		return result;
-	
+		
-	signal	fill_count			: natural range 0 to 2**word_index_width-1;
+	end to_tag;
-	signal	fill_count_en		: std_logic;
-	signal	fill_count_rdy		: std_logic;
-	signal	flush_load_en		: std_logic;
-	signal  flush_addr_preset	: unsigned(cache_index_width-1 downto 0);
-	signal	flush_count_preset	: natural range 0 to 2**cache_index_width-1;
-	signal  flush_addr			: unsigned(cache_index_width-1 downto 0);
-	signal	flush_count			: natural range 0 to 2**cache_index_width-1;
-	signal	flush_count_en		: std_logic;
-	signal	flush_count_rdy		: std_logic;
-	signal	request_count		: natural range 0 to 2**word_index_width-1;
-	signal	request_count_en	: std_logic;
-	signal	request_count_rdy	: std_logic;
-	signal	was_miss			: std_logic;
-	signal	invalidate_all		: std_logic;
-	signal	invalidate_ack		: std_logic;
-	signal  invalidate_en		: std_logic;
-	signal  invalidate_req		: std_logic;
-	signal	cpu_hit_we			: std_logic;
-	signal	instant_raw			: std_logic;
-	signal  hit_cache_index		: unsigned(cache_index_width-1 downto 0);
 
-	signal  DAT_I_r				: word_t;
-	signal  ACK_I_r				: std_logic;
-	
-	alias cpu_word_index is cpu_addr(word_index_width+1 downto 2);
-	alias cpu_cache_index is cpu_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
-	alias cpu_tag is cpu_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
-	
-	alias fill_word_index is fill_addr(word_index_width+1 downto 2);
-	alias fill_cache_index is fill_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
-	alias fill_tag is fill_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
 
-	alias req_word_index is request_addr(word_index_width+1 downto 2);
+	type cache_busy_state_t is (init, ready, busy);
-	alias req_cache_index is request_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
+	signal cache_busy_state, cache_busy_state_next : cache_busy_state_t;
-	alias req_tag is request_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
+
+	type cache_ctrl_state_t is (init, ready, invalidate, invalidate_post, flush, bus_request, mem_read, mem_read_single, mem_read_data, mem_read_data_single, rd_cache, upd_cache, wait_write_thru);
+	signal cache_ctrl_state, cache_ctrl_state_next : cache_ctrl_state_t;
+	
+	type debug_t is record
+		ready			: std_logic;
+		req_strobe		: std_logic;
+		req_read		: std_logic;
+		req_write		: std_logic;
+		req_va			: word_t;
+		req_pa			: word_t;
+		dout_reg		: word_t;
+		din_reg			: word_t;
+	end record;
+
+	signal debug		: debug_t;
+	
+	signal	req_strobe				: std_logic;
+	signal	cache_req				: std_logic;
+	signal	cache_req_rd			: std_logic;
+	signal	cache_req_wr			: std_logic;
+	signal	cache_rdy				: std_logic;
+	signal	cache_hit				: std_logic;
+	signal	read_rdy				: std_logic;
+	signal	write_rdy				: std_logic;
+	signal  request_va				: word_t;
+	signal  request_pa				: word_t;
+	signal	request_nc				: std_logic;
+
+	signal	tag_match				: std_logic;
+	signal	cache_hit_inv			: std_logic;
+	signal	tag_match_inv			: std_logic;
+
+	signal	mem_fetch_req			: std_logic;
+	signal	mem_fetch_ack			: std_logic;
+	signal	single_read_en			: std_logic;
+	signal	single_read_set			: std_logic;
+	signal	single_read_reg_vld		: std_logic;
+
+	signal  cache_index_inv			: unsigned(cache_index_width-1 downto 0);
+	signal  tag_inv					: unsigned(tag_width-1 downto 0);
+	signal  tag_reg_inv				: unsigned(tag_width-1 downto 0);
+	
+	signal	cache_entry_in			: dcache_entry_t;
+	signal	cache_entry_out			: dcache_entry_t;
+	signal	cache_entry_out_inv		: dcache_entry_t;
+	signal  data_ram_addr			: unsigned(lg2(CACHE_SIZE)-1 downto 0);
+	signal  data_ram_dout			: word_t;
+	signal  din_reg					: word_t;
+	signal  be_reg					: unsigned(3 downto 0);
+	signal  ctrl_data_ram_addr		: unsigned(lg2(CACHE_SIZE)-1 downto 0);
+	signal	ctrl_data_ram_we		: unsigned(3 downto 0);
+	signal	data_ram_we				: unsigned(3 downto 0);
+	signal  single_read_reg			: word_t;
+	signal  cache_dout				: word_t;
+
+	signal  tag_ram_addr_rd			: unsigned(cache_index_width-1 downto 0);
+	signal  tag_ram_dout			: tag_ram_data_t;
+	signal  tag_ram_dout_inv		: tag_ram_data_t;
+	signal  tag_ram_addr_wr			: unsigned(cache_index_width-1 downto 0);
+	signal  tag_ram_din				: tag_ram_data_t;
+	signal	tag_ram_we				: std_logic;
+
+	signal	fill_count				: natural range 0 to 2**word_index_width-1;
+	signal	fill_count_en			: std_logic;
+	signal	fill_count_rdy			: std_logic;
+	signal	flush_count				: natural range 0 to 2**cache_index_width-1;
+	signal	flush_count_rst			: std_logic;
+	signal	flush_count_en			: std_logic;
+	signal	flush_count_rdy			: std_logic;
+	signal	request_count			: natural range 0 to 2**word_index_width-1;
+	signal	request_count_en		: std_logic;
+	signal	request_count_rdy		: std_logic;
+	signal  ram_read_en				: std_logic;
+	signal	was_miss				: std_logic;
+	signal	invalidate_all			: std_logic;
+	signal	invalidate_ack			: std_logic;
+	signal  invalidate_en			: std_logic;
+	signal  invalidate_req			: std_logic;
+	signal	write_hit				: std_logic;
+	signal	instant_raw				: std_logic;
+	signal  ram_write_en			: std_logic;
+	signal	fill_word_index			: unsigned(word_index_width-1 downto 0);
+	signal	req_word_index			: unsigned(word_index_width-1 downto 0);
+	signal  hit_cache_index			: unsigned(cache_index_width-1 downto 0);
+
+	signal write_fifo_din			: unsigned(67 downto 0);
+	signal write_fifo_dout			: unsigned(67 downto 0);
+	signal write_fifo_re			: std_logic;
+	signal write_fifo_we			: std_logic;
+	signal write_fifo_full			: std_logic;
+	signal write_fifo_empty			: std_logic;
+	signal write_data_avail			: std_logic;
+	signal read_write_buffer		: std_logic;
+
+	alias write_fifo_addr_in is write_fifo_din(31 downto 0);
+	alias write_fifo_data_in is write_fifo_din(63 downto 32);
+	alias write_fifo_sel_in is write_fifo_din(67 downto 64);
+	alias write_fifo_addr_out is write_fifo_dout(31 downto 0);
+	alias write_fifo_data_out is write_fifo_dout(63 downto 32);
+	alias write_fifo_sel_out is write_fifo_dout(67 downto 64);
+
+	signal write_through_en			: std_logic;
 
 begin
 
-	cache_index_inv	<= ctrl.inv_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
+	-- Print info
-	tag_inv			<= ctrl.inv_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
+	assert false report "addr_width        = " & natural'image(addr_width) & "." severity note;
+	assert false report "tag_width         = " & natural'image(tag_width) & "." severity note;
+	assert false report "word_index_width  = " & natural'image(word_index_width) & "." severity note;
+	assert false report "cache_index_width = " & natural'image(cache_index_width) & "." severity note;
 
-	cpu_hit_we	<= cpu_we2 and cache_hit;
+	cache_index_inv						<= ctrl_in.inv_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
+	tag_inv								<= ctrl_in.inv_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
+
+	bus_din_rdy							<= fill_count_en or single_read_en;
+	bus_cmd_out.addr					<= write_fifo_addr_out when read_write_buffer = '1' else to_tag(request_pa) & to_cache_index(request_pa) & req_word_index & "00";
+	bus_cmd_out.rw						<= read_write_buffer;
+	bus_cmd_out.sel						<= write_fifo_sel_out when read_write_buffer = '1' else (others => '1');
+	bus_dout							<= write_fifo_data_out;
+	bus_dout_vld						<= read_write_buffer and write_data_avail;
+
+-- 	tlb_query_out.vld					<= cache_req;
+-- 	tlb_query_out.vaddr					<= request_va;
+-- 	tlb_query_out.write					<= cache_req_wr;
+-- 	tlb_query_out.vld					<= req_strobe;
+-- 	tlb_query_out.vaddr					<= query_in.addr;
+-- 	tlb_query_out.idx					<= cache_entry_out.idx;
+-- 	tlb_query_out.write					<= query_in.we;
 	
-data_in_register:
+	request_pa							<= request_va;
-	process(CLK_I)
+
-	begin
+	write_hit							<= cache_req_wr and cache_hit; -- and not tlb_query_in.exc;
-		if rising_edge(CLK_I) then
-			ACK_I_r	<= ACK_I;
-			if ACK_I = '1' then
-				DAT_I_r <= DAT_I;
-			end if;
-		end if;
-	end process;	
 	
-fill_address_register:
+	debug.req_strobe	<= req_strobe;
-	process(CLK_I)
+	debug.req_read		<= cache_req_rd;
+	debug.req_write		<= cache_req_wr;
+	debug.req_va		<= request_va;
+	debug.req_pa		<= request_pa;
+	debug.din_reg		<= din_reg;
+	debug.dout_reg		<= cache_dout;
+	debug.ready			<= cache_rdy;
+
+-------------------------------------------------
+-- ctrl
+-------------------------------------------------
+req_strobe	<= cache_rdy and query_in.en;
+
+request_register:
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			if fill_count_en = '1' then
+			if rst = '1' then
-				if ACK_I_r = '1' then
+				cache_req_rd	<= '0';
-					fill_word_index	<= fill_word_index + 1;
+				cache_req_wr	<= '0';
-				end if;	
+			elsif cache_rdy = '1' then
-			elsif cache_busy = '0' then
+				cache_req_rd	<= query_in.en and not query_in.we;
-				fill_addr	<= cpu_addr(addr_width-1 downto 2) & "00";
+				cache_req_wr	<= query_in.en and query_in.we;
+				cache_req		<= query_in.en;
+				if query_in.we = '1' then
+					hit_cache_index		<= to_cache_index(query_in.addr);
+				end if;
 			end if;
 		end if;
 	end process;	
 
 request_address_register:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			if request_count_en = '1'then
+			if rst = '1' then
-				if SRDY_I = '1' then
+				request_va		<= (others => '0');
-					req_word_index	<= req_word_index + 1;
+				request_nc		<= '0';
-				end if;	
+			elsif req_strobe = '1' then
-			elsif cache_busy = '0' then
+				din_reg			<= query_in.data;
-				request_addr	<= cpu_addr(addr_width-1 downto 2) & "00";
+				be_reg			<= query_in.be;
-			end if;
+				request_va		<= query_in.addr;
-		end if;
+				request_nc		<= query_in.nc;
-	end process;	
-
-cpu_request_register:
-	process(CLK_I)
-	begin
-		if rising_edge(CLK_I) then
-			if RST_I = '1' then
-				cpu_we_reg	<= '0';
-				cache_req	<= '0';
-			elsif cpu_en = '1' then
-				if cache_busy = '0' then
-					cpu_we2				<= cpu_we;
-					cache_req			<= '1';
-					cpu_data_reg		<= cpu_din;
-					cpu_be_reg			<= cpu_be;
-					cpu_we_reg			<= cpu_we;
-					if cpu_we = '1' then
-						hit_cache_index		<= cpu_cache_index;
-					end if;
-				end if;	
-			elsif cache_ack = '1' then
-				cache_req			<= '0';
-				cpu_we2				<= '0';
 			end if;
 		end if;
 	end process;	
 
 instant_raw_logic:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
 			instant_raw <= '0';
-			if cpu_word_index = fill_word_index and cpu_cache_index = hit_cache_index then
+			if to_word_index(query_in.addr) = fill_word_index and to_cache_index(query_in.addr) = hit_cache_index then
-				instant_raw	<= cpu_hit_we and cpu_en and not cpu_we;
+				instant_raw	<= write_hit and query_in.en and not query_in.we;
 			end if;	
 		end if;
 	end process;	
 
+-------------------------------------------------
+-- Instantiate synchronous FIFO
+inst_write_fifo: entity work.fifo_sync
+	GENERIC MAP
+	(
+		addr_width			=> lg2(WRITE_FIFO_SIZE),
+		data_width			=> 68,
+		do_last_read_update	=> true
+	)
+	PORT MAP
+	(
+		rst			=>	rst,
+		clk			=>	clk,
+		we			=>	write_fifo_we,
+		re			=>	write_fifo_re,
+		fifo_full	=>	write_fifo_full,
+		fifo_empty	=>	write_fifo_empty,
+		fifo_afull	=>	open,
+		fifo_aempty	=>	open,
+		data_w		=>  write_fifo_din,
+		data_r		=>  write_fifo_dout
+	);
+
+	write_fifo_data_in	<= din_reg;
+	write_fifo_addr_in	<= request_pa;
+	write_fifo_sel_in	<= be_reg;
+	write_fifo_re		<= read_write_buffer and bus_dout_rdy and bus_cmd_rdy;
+	write_fifo_we		<= write_through_en;
+	write_data_avail	<= not write_fifo_empty;
+
+fill_address_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if req_strobe = '1' then
+				fill_word_index	<= to_word_index(query_in.addr);
+			elsif bus_din_vld = '1' and fill_count_en = '1' then
+				fill_word_index	<= fill_word_index + 1;
+			end if;
+		end if;
+	end process;	
+
+bus_request_address_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if req_strobe = '1' then
+				req_word_index	<= to_word_index(query_in.addr);
+			elsif request_count_en = '1' and bus_cmd_rdy = '1' then
+				req_word_index	<= req_word_index + 1;
+			end if;	
+		end if;
+	end process;	
+
+single_read_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				single_read_en <= '0';
+				single_read_reg_vld <= '0';
+			elsif single_read_en = '0' then
+				if single_read_set = '1' then
+					single_read_en		<= '1';
+				end if;
+			elsif single_read_reg_vld = '1' then
+				if query_in.en = '1' then
+					single_read_en <= '0';
+					single_read_reg_vld <= '0';
+				end if;	
+			elsif bus_din_vld = '1' and single_read_en = '1' then
+				single_read_reg <= bus_din;
+				single_read_reg_vld <= '1';
+			end if;
+		end if;
+	end process;	
+
 inst_tag_ram : dpram_2w2r2c_ra
 	GENERIC MAP 
 	(
@@ -283,10 +439,10 @@ inst_tag_ram : dpram_2w2r2c_ra
 	)
 	PORT MAP 
 	(
-		clk_a	=> CLK_I,
+		clk_a	=> clk,
-		clk_b	=> CLK_I,
+		clk_b	=> clk,
 		re_a	=> '1',
-		re_b	=> '1',
+		re_b	=> ram_read_en,
 		we_a	=> tag_ram_we,
 		we_b	=> '0',
 		addr_a	=> tag_ram_addr_wr,
@@ -304,35 +460,35 @@ gen_data_ram:
 	inst_data_ram : dpram_2w2r2c_ra
 		GENERIC MAP 
 		(
-			addr_width	=> lg2(cache_size),
+			addr_width	=> lg2(CACHE_SIZE),
 			data_width	=> word_t'length/4
 		)
 		PORT MAP 
 		(
-			clk_a	=> CLK_I,
+			clk_a	=> clk,
-			clk_b	=> CLK_I,
+			clk_b	=> clk,
 			re_a	=> '1',
-			re_b	=> '1',
+			re_b	=> ram_read_en,
-			we_a	=> cpu_data_ram_we(i),
+			we_a	=> data_ram_we(i),
 			we_b	=> ctrl_data_ram_we(i),
 			addr_a	=> ctrl_data_ram_addr,
-			addr_b	=> cpu_data_ram_addr,
+			addr_b	=> data_ram_addr,
-			din_a	=> cpu_data_reg(8*(i+1)-1 downto 8*i),
+			din_a	=> din_reg(8*(i+1)-1 downto 8*i),
-			din_b	=> DAT_I_r(8*(i+1)-1 downto 8*i),
+			din_b	=> bus_din(8*(i+1)-1 downto 8*i),
 			dout_a	=> open,
-			dout_b	=> cpu_data_ram_dout(8*(i+1)-1 downto 8*i)
+			dout_b	=> data_ram_dout(8*(i+1)-1 downto 8*i)
 		);
 	end generate;	
 	
 	
 	
 cache_invalidate_request:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			if RST_I = '1' or ctrl.inv_all = '1' or ctrl.inv_at = '1' then
+			if rst = '1' or ctrl_in.inv_all = '1' or ctrl_in.inv_at = '1' then
 				invalidate_req	<= '1';
-				invalidate_all	<= ctrl.inv_all or RST_I;
+				invalidate_all	<= ctrl_in.inv_all or rst;
 				tag_reg_inv	<= tag_inv;
 			elsif invalidate_ack = '1' then
 				invalidate_req	<= '0';
@@ -342,102 +498,144 @@ cache_invalidate_request:
 	end process;	
 
 cache_state_next:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			if RST_I = '1' then
+			if rst = '1' then
-				s	<= init;
+				cache_ctrl_state	<= init;
+				cache_busy_state	<= init;
 			else
-				s	<= sn;
+				cache_ctrl_state	<= cache_ctrl_state_next;
+				cache_busy_state	<= cache_busy_state_next;
 			end if;
 		end if;
 	end process;	
 
-	MRDY_O				<= fill_count_en;
+	cache_rdy						<= read_rdy and not instant_raw and write_rdy;
-	ADDR_O				<= request_addr;
+	write_rdy						<= not (write_fifo_full and cache_req_wr);
-	
+	query_out.rdy					<= cache_rdy;
-	cpu_busy			<= cache_busy;
+	rdy								<= cache_rdy;
-	cpu_dout			<= cpu_data_ram_dout;
-	
-	tag_match			<= '1' when fill_tag = cache_entry_out.tag else '0';
-	cache_hit			<= tag_match and cache_entry_out.valid;
-	tag_match_inv		<= '1' when tag_reg_inv = cache_entry_out_inv.tag else '0';
-	cache_hit_inv		<= tag_match_inv and cache_entry_out_inv.valid;
-	tag_ram_din			<= to_tag_ram_data(cache_entry_in);
-	tag_ram_addr_wr		<= flush_addr when invalidate_en = '1' else fill_cache_index;
-	tag_ram_addr_rd		<= cpu_cache_index when (was_miss = '0' and instant_raw = '0') else fill_cache_index;
 
-	cache_entry_out		<= to_dcache_entry(tag_ram_dout);
+	cache_dout						<= single_read_reg when single_read_en = '1' else to_mips_01(data_ram_dout);
-	cache_entry_out_inv	<= to_dcache_entry(tag_ram_dout_inv);
+	query_out.data					<= cache_dout;
+
+--	tag_match						<= '1' when to_tag(request_pa) = cache_entry_out.tag else '0';
+	tag_match						<= '1' when ((to_tag(request_pa) xor cache_entry_out.tag) = (tag_width-1 downto 0 => '0')) else '0';
+	cache_hit						<= tag_match and cache_entry_out.valid;
+
+	tag_match_inv					<= '1' when tag_reg_inv = cache_entry_out_inv.tag else '0';
+	cache_hit_inv					<= tag_match_inv and cache_entry_out_inv.valid;
+
+	tag_ram_din						<= to_tag_ram_data(cache_entry_in);
+	tag_ram_addr_wr					<= to_unsigned(flush_count, cache_index_width) when invalidate_en = '1' else to_cache_index(request_va);
+	tag_ram_addr_rd					<= to_cache_index(query_in.addr) when (was_miss = '0' and instant_raw = '0') else to_cache_index(request_va);
+
+	cache_entry_out					<= to_dcache_entry(to_mips_01(tag_ram_dout));
+	cache_entry_out_inv				<= to_dcache_entry(to_mips_01(tag_ram_dout_inv));
+
+	ram_read_en						<= query_in.en or was_miss or fill_count_en;
+	data_ram_addr					<= (to_cache_index(query_in.addr) & to_word_index(query_in.addr)) when (was_miss = '0' and instant_raw = '0' and fill_count_en = '0') else (to_cache_index(request_va) & fill_word_index);
+	ctrl_data_ram_addr				<= to_cache_index(request_va) & fill_word_index;
+	ctrl_data_ram_we				<= (others => ram_write_en and bus_din_vld);
+	data_ram_we						<= be_reg when (write_hit = '1') else (others => '0');
+
+	write_through_en				<= cache_rdy and cache_req_wr; -- and tlb_query_in.hit and not tlb_query_in.flags.D;
 	
-	cpu_data_ram_addr	<= (cpu_cache_index & cpu_word_index) when (was_miss = '0' and instant_raw = '0' and fill_count_en = '0') else (fill_cache_index & fill_word_index);
+cache_busy_state_fsm:
-	ctrl_data_ram_addr	<= fill_cache_index & fill_word_index;
+	process(cache_busy_state, request_nc, cache_req_rd, cache_hit, mem_fetch_ack, single_read_en, single_read_reg_vld)
-	ctrl_data_ram_we	<= (others => fill_count_en and ACK_I_r);
+	begin
-	cpu_data_ram_we		<= cpu_be_reg when (cpu_hit_we = '1') else (others => '0');
+
-	
+		read_rdy 				<= '0';
-cache_state:
+		mem_fetch_req			<= '0';
-	process(s, cache_req, instant_raw, cache_hit, cache_hit_inv, cache_index_inv, flush_count_rdy, fill_count_rdy, request_count_rdy, fill_tag, SRDY_I, cpu_we_reg, invalidate_req, invalidate_all)
+		single_read_set			<= '0';
+		cache_busy_state_next	<= cache_busy_state;
+		
+		case cache_busy_state is
+
+		when init =>
+			cache_busy_state_next <= ready;
+			
+		when ready =>
+			if single_read_en = '1' then
+				read_rdy			<= single_read_reg_vld;
+			else
+				read_rdy 				<= '1';
+				if cache_req_rd = '1' then
+--					if tlb_query_in.exc = '0' then
+						if request_nc = '1' or cache_hit = '0' then
+--							mem_fetch_req			<= '1';	
+							read_rdy				<= '0';
+							cache_busy_state_next 	<= busy;
+						end if;
+--					end if;
+				end if;
+			end if;
+
+		when busy =>
+			mem_fetch_req	<= '1';	
+			if mem_fetch_ack = '1' then
+				single_read_set			<= request_nc;
+				cache_busy_state_next <= ready;
+			end if;
+		
+		when others => 
+			cache_busy_state_next <= ready;
+				
+		end case;
+		
+	end process;
+
+cache_ctrl_state_fsm:
+	process(cache_ctrl_state, mem_fetch_req, request_nc, flush_count_rdy, fill_count_rdy, request_count_rdy, request_pa, bus_cmd_rdy, bus_din_vld, invalidate_req, invalidate_all, write_through_en, write_data_avail)
 	begin
-		cache_busy 				<= cache_req;
-		cache_ack 				<= '0';
 		tag_ram_we				<= '0';
+		flush_count_en			<= '0';
+		flush_count_rst			<= '0';
+		invalidate_en			<= '0';
 		request_count_en		<= '0';
 		fill_count_en			<= '0';
-		CYC_O					<= '0';
+		ram_write_en			<= '0';
-		STB_O					<= '0';
+		bus_cmd_cycle_en		<= '0';
+		bus_cmd_we				<= '0';
 		was_miss				<= '0';
-
-		invalidate_en			<= '0';
 		invalidate_ack			<= '0';
-
+		mem_fetch_ack			<= '0';
-		flush_load_en			<= '0';
+		read_write_buffer		<= '0';
-		flush_count_en			<= '0';
-		flush_addr_preset		<= (others=>'0'); -- all
-		flush_count_preset		<= 2**cache_index_width-1;
-
-		cache_entry_in.tv_p		<= (others => '0');
-		cache_entry_in.tag		<= fill_tag;
-		cache_entry_in.valid	<= '0';
-		sn	<= s;
 		
-		case s is
+		cache_entry_in.tag		<= to_tag(request_pa);
+		cache_entry_in.valid	<= '0';
+		cache_ctrl_state_next	<= cache_ctrl_state;
+		
+		case cache_ctrl_state is
+
 		when init =>
-				sn <= ready;
+				cache_ctrl_state_next <= ready;
+
 		when ready =>
 			if invalidate_req = '1' then
-				sn 				<= invalidate_init;
+				cache_ctrl_state_next 				<= invalidate;
-				invalidate_en	<= '1';
+				invalidate_en	<= '1';
-				flush_load_en	<= '1';
+			elsif mem_fetch_req = '1' or write_data_avail = '1' then
-				if invalidate_all = '1' then
+				cache_ctrl_state_next				<= bus_request;
-					flush_addr_preset		<= (others=>'0'); -- all
+--				bus_cmd_cycle_en					<= '1';
-					flush_count_preset		<= 2**cache_index_width-1;
+			elsif write_through_en = '1' then
-				else
+				cache_ctrl_state_next				<= wait_write_thru;
-					flush_addr_preset		<= cache_index_inv; -- at
+--				bus_cmd_cycle_en					<= '1';
-					flush_count_preset		<= 0;
-				end if;	
-			else	
-				if cache_req = '1' then
-					if cache_hit = '0' and cpu_we_reg = '0' then
-						sn						<= mem_request;
-						CYC_O					<= '1';
-					else
-						cache_busy 		<= instant_raw;
-						cache_ack 		<= not instant_raw;
-					end if;	
-				end if;	
 			end if;	
-
+
-		when invalidate_init =>
-			invalidate_en			<= '1';
-			sn	<= invalidate;
-		
 		when invalidate =>
-			sn	<= rd_cache;
+			cache_ctrl_state_next <= rd_cache;
-			invalidate_en			<= '1';
+			invalidate_en			<= '1';
-			invalidate_ack			<= '1';
+			invalidate_ack			<= '1';
-			if cache_hit_inv = '1' or invalidate_all = '1' then
+			if invalidate_all = '1' then
-				sn					<= flush;
+				cache_ctrl_state_next					<= flush;
-			end if;	
+				flush_count_rst		<= '1';
+			elsif cache_hit_inv = '1' then
+				tag_ram_we				<= '1';
+				cache_entry_in.valid	<= '0';
+				cache_entry_in.tag		<= (others => '0');
+				cache_ctrl_state_next	<= invalidate_post;
+			end if;	
 				
 		when flush =>
 			flush_count_en			<= '1';
@@ -447,68 +645,106 @@ cache_state:
 			cache_entry_in.tag		<= (others => '0');
 			if flush_count_rdy = '1' then
 				tag_ram_we			<= '0';
-				sn					<= rd_cache;
+				cache_ctrl_state_next	<= invalidate_post;
 			end if;
-		when mem_request =>
+
-			CYC_O		<= '1';
+		when invalidate_post =>
-			if SRDY_I = '1' then
+			was_miss	<= '1';
-				sn <= mem_access;
+			cache_ctrl_state_next		<= ready;
-			end if;	
+
-		when mem_access =>
+		when wait_write_thru =>
-			fill_count_en		<= '1';
+			cache_ctrl_state_next				<= bus_request;
+			bus_cmd_cycle_en					<= '1';
+		
+		when bus_request =>
+			bus_cmd_cycle_en	<= '1';
+			if bus_cmd_rdy = '1' then
+				if write_data_avail = '1' then
+					read_write_buffer	<= '1';
+					bus_cmd_we			<= '1';
+				elsif mem_fetch_req = '1' then
+					if request_nc = '1' then
+						cache_ctrl_state_next <= mem_read_single;
+					else
+						cache_ctrl_state_next <= mem_read;
+					end if;	
+				else
+					cache_ctrl_state_next		<= ready;
+				end if;
+			end if;
+			
+		when mem_read =>
+			bus_cmd_cycle_en	<= '1';
+			bus_cmd_we			<= '1';
 			request_count_en	<= '1';
-			CYC_O				<= '1';
+			ram_write_en		<= '1';
-			STB_O				<= '1';
+			fill_count_en		<= '1';
 			if request_count_rdy = '1' then
-				STB_O			<= '0';
+				bus_cmd_we		<= '0';
-				sn 				<= mem_data;
+				cache_ctrl_state_next	<= mem_read_data;
 			end if;	
-		when mem_data =>
+
-			CYC_O			<= '1';
+		when mem_read_single =>
-			fill_count_en	<= '1';
+			mem_fetch_ack		<= '1';
+			bus_cmd_cycle_en	<= '1';
+			bus_cmd_we			<= '1';
+			cache_ctrl_state_next <= mem_read_data_single;
+
+		when mem_read_data =>
+			bus_cmd_cycle_en	<= '1';
+			fill_count_en		<= '1';
+			ram_write_en		<= '1';
 			if fill_count_rdy = '1' then
 				tag_ram_we				<= '1';
 				cache_entry_in.valid	<= '1';
-				sn						<= rd_cache;
+				cache_ctrl_state_next	<= rd_cache;
 			end if;	
-		when rd_cache =>
-			was_miss				<= '1';
-			sn	<= ready;
 
+		when mem_read_data_single =>
+			bus_cmd_cycle_en	<= '1';
+			if bus_din_vld = '1' then
+				cache_ctrl_state_next	<= ready;
+			end if;	
+
+		when rd_cache =>
+			mem_fetch_ack	<= '1';
+			was_miss	<= '1';
+			cache_ctrl_state_next		<= ready;
+			
 		when others => 
-			sn	<= ready;
+			cache_ctrl_state_next		<= ready;
 		end case;
 
 	end process;
-
+
-flush_counter:
+flush_counter:
-	process(CLK_I)
+	process(clk)
-	begin
+	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			if flush_load_en = '1' then
+			if ctrl_in.inv_at = '1' then
-				flush_count_rdy	<= '0';
+				flush_count		<= to_integer(cache_index_inv);
-				flush_count		<= flush_count_preset;
+			elsif flush_count_rst = '1' then
-				flush_addr		<= flush_addr_preset;
+				flush_count_rdy	<= '0';
-			elsif flush_count_en = '1' then
+				flush_count		<= 2**cache_index_width-1;
-				if flush_count /= 0 then
+			elsif flush_count_en = '1' then
-					flush_count	<= flush_count - 1;
+				if flush_count /= 0 then
-					flush_addr	<= flush_addr + 1;
+					flush_count	<= flush_count - 1;
-				else
+				else
-					flush_count_rdy	<= '1';
+					flush_count_rdy	<= '1';
-				end if;	
+				end if;	
-			end if;
+			end if;
-		end if;
+		end if;
-	end process;	
+	end process;	
 		
 request_counter:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
 			if request_count_en = '0' then
 				request_count_rdy	<= '0';
 				request_count		<= 2**word_index_width-1;
 			else	
-				if SRDY_I = '1' then
+				if bus_cmd_rdy = '1' then
 					if request_count /= 0 then
 						request_count 	<= request_count - 1;
 					else
@@ -520,14 +756,14 @@ request_counter:
 	end process;	
 
 fill_counter:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
 			if fill_count_en = '0' then
 				fill_count_rdy	<= '0';
 				fill_count		<= 2**word_index_width-1;
 			else	
-				if ACK_I_r = '1' then
+				if bus_din_vld = '1' then
 					if fill_count /= 0 then
 						fill_count 	<= fill_count - 1;
 					else

lib/CPUs/MIPS/src/core/mips_func_pkg.vhd

@@ -0,0 +1,276 @@
+--------------------------------------------------------------------------
+-- Project: JIPS, a portable 32-bit RISC CPU written in VHDL
+-- This file: Types, constants and functions for JIPS
+--
+-- Copyright (C) 2008 J. Ahrensfeld
+--
+-- 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, either version 3 of the License, or
+-- (at your option) any later version.
+--
+-- 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/>.
+--
+-- For questions and ideas, please contact the author at jens@jayfield.org
+--
+--------------------------------------------------------------------------
+
+library IEEE;
+use IEEE.STD_LOGIC_1164.ALL;
+use IEEE.NUMERIC_STD.ALL;
+use IEEE.MATH_REAL.ALL;
+
+library work;
+use work.mips_util_pkg.all;
+use work.mips_types.all;
+
+--------------------------------------------------------------------------
+package mips_func_pkg is
+
+	function store_shift(x : word_t; va, shift_off : unsigned(1 downto 0); bypass : std_logic) return word_t;
+	function store_be(va : unsigned(1 downto 0); be_src, word2_en, word4_en, align_left, bypass : std_logic) return unsigned;
+	function load_shift(x : word_t; va, shift_off : unsigned(1 downto 0); bypass : std_logic) return word_t;
+	function load_be(va : unsigned(1 downto 0); align_left, bypass : std_logic) return unsigned;
+	function load_sign_ext(x : word_t; bypass, signed, word2_en, word4_en : std_logic) return word_t;
+	function events_clr return event_t;
+	function event_is_active(e : event_t) return std_logic;
+	function "or" (a, b : event_t) return event_t;
+
+end mips_func_pkg;
+
+--------------------------------------------------------------------------
+package body mips_func_pkg is
+
+	function store_shift(x : word_t; va, shift_off : unsigned(1 downto 0); bypass : std_logic) return word_t is
+	variable stage1 : word_t;
+	variable result : word_t;
+	variable sa : unsigned(1 downto 0);
+	begin
+		
+		if bypass = '1'	then
+			sa := "00";
+		else
+			sa := va + shift_off;
+		end if;
+		
+		stage1 := x;
+		if sa(0) = '1' then
+			stage1 := x(23 downto 0) & x(31 downto 24);
+		end if;
+		result := stage1;
+		if sa(1) = '1' then
+			result := stage1(15 downto 0) & stage1(31 downto 16);
+		end if;
+		
+		return result;
+	end store_shift;
+	
+--------------------------------------------------------------------------
+	function store_be(va : unsigned(1 downto 0); be_src, word2_en, word4_en, align_left, bypass : std_logic) return unsigned is
+	variable result : unsigned(3 downto 0);
+	begin
+		result := (3 downto 0 => be_src);
+		if bypass = '0' then
+			if word2_en = '1' then
+				case va is
+					when "00" =>
+						result := "00" & be_src & be_src;
+					when "10" =>
+						result := be_src & be_src & "00";
+					when others => null;
+				end case;
+			elsif word4_en = '1' then
+				case va is
+					when "00" =>
+						result := "000" & be_src;
+					when "01" =>
+						result := "00" & be_src & '0';
+					when "10" =>
+						result := '0' & be_src & "00";
+					when "11" =>
+						result := be_src & "000";
+					when others => null;
+				end case;
+			elsif align_left = '1' then
+				case va is
+					when "00" =>
+						result := be_src & be_src & be_src & be_src;
+					when "01" =>
+						result := be_src & be_src & be_src & '0';
+					when "10" =>
+						result := be_src & be_src & "00";
+					when "11" =>
+						result := be_src & "000";
+					when others => null;
+				end case;
+			else
+				case va is
+					when "00" =>
+						result := "000" & be_src;
+					when "01" =>
+						result := "00" & be_src & be_src;
+					when "10" =>
+						result := '0' & be_src & be_src & be_src;
+					when "11" =>
+						result := be_src & be_src & be_src & be_src;
+					when others => null;
+				end case;
+			end if;
+		end if;
+		
+		return result;
+	
+	end store_be;
+	
+--------------------------------------------------------------------------
+	function load_shift(x : word_t; va, shift_off : unsigned(1 downto 0); bypass : std_logic) return word_t is
+	variable stage1 : word_t;
+	variable result : word_t;
+	variable sa : unsigned(1 downto 0);
+	
+	begin
+		if bypass = '1' then
+			sa 		:= "00";
+		else
+			sa		:= va + shift_off;
+		end if;
+		
+		stage1 := x;
+		if sa(0) = '1' then
+			stage1 :=  x(7 downto 0) & x(31 downto 8);
+		end if;
+		result := stage1;
+		if sa(1) = '1' then
+			result := stage1(15 downto 0) & stage1(31 downto 16);
+		end if;
+		
+		return result;
+	end load_shift;
+	
+--------------------------------------------------------------------------
+	function load_be(va : unsigned(1 downto 0); align_left, bypass : std_logic) return unsigned is
+	variable result : unsigned(3 downto 0);
+	begin
+		result := (3 downto 0 => '1');
+		if bypass = '0' then
+			if align_left = '1' then
+				case va is
+					when "00" =>
+						result := "1000";
+					when "01" =>
+						result := "1100";
+					when "10" =>
+						result := "1110";
+					when "11" =>
+						result := "1111";
+					when others => null;
+				end case;
+			else
+				case va is
+					when "00" =>
+						result := "1111";
+					when "01" =>
+						result := "0111";
+					when "10" =>
+						result := "0011";
+					when "11" =>
+						result := "0001";
+					when others => null;
+				end case;
+			end if;
+		end if;
+		
+		return result;
+	
+	end load_be;
+	
+--------------------------------------------------------------------------
+	function load_sign_ext(x : word_t; bypass, signed, word2_en, word4_en : std_logic) return word_t is
+	variable result : word_t;
+	variable sign : std_logic;
+	
+	begin
+		if bypass = '1' then
+			result := x;
+		elsif word2_en = '1' then
+			sign	:= signed and x(15);
+			result := (31 downto 16 => sign) & x(15 downto 0);
+		else
+			sign	:= signed and x(7);
+			result := (31 downto 8 => sign) & x(7 downto 0);
+		end if;
+		
+		return result;
+	end load_sign_ext;
+	
+--------------------------------------------------------------------------
+	function events_clr return event_t is
+	variable result : event_t;
+	begin
+		result.NMI				:= '0';
+		result.Int				:= '0';
+		result.data_load_err	:= '0';
+		result.data_store_err	:= '0';
+		result.inst_load_err	:= '0';
+		result.inst_priv_addr	:= '0';
+		result.alu_ovf			:= '0';
+		result.alu_uvf			:= '0';
+		result.syscall			:= '0';
+		result.break			:= '0';
+		result.illegal			:= '0';
+		
+		return result;
+		
+	end events_clr;
+
+--------------------------------------------------------------------------
+	function event_is_active(e : event_t) return std_logic is
+	variable result : std_logic;
+	begin
+		result := '0';
+		
+		result	:= result or e.NMI;
+		result	:= result or e.Int;
+		result	:= result or e.data_load_err;
+		result	:= result or e.data_store_err;
+		result	:= result or e.inst_load_err;
+		result	:= result or e.inst_priv_addr;
+		result	:= result or e.alu_ovf;
+		result	:= result or e.alu_uvf;
+		result	:= result or e.syscall;
+		result	:= result or e.break;
+		result	:= result or e.illegal;
+
+		return result;
+		
+	end event_is_active;
+
+--------------------------------------------------------------------------
+	function "or" (a, b : event_t) return event_t is
+	variable result : event_t;
+	begin
+		result.NMI				:= a.NMI or b.NMI;
+		result.Int				:= a.Int or b.Int;
+		result.data_load_err	:= a.data_load_err or b.data_load_err;
+		result.data_store_err	:= a.data_store_err or b.data_store_err;
+		result.inst_load_err	:= a.inst_load_err or b.inst_load_err;
+		result.inst_priv_addr	:= a.inst_priv_addr or b.inst_priv_addr;
+		result.alu_ovf			:= a.alu_ovf or b.alu_ovf;
+		result.alu_uvf			:= a.alu_uvf or b.alu_uvf;
+		result.syscall			:= a.syscall or b.syscall;
+		result.break			:= a.break or b.break;
+		result.illegal			:= a.illegal or b.illegal;
+		
+		return result;
+		
+	end "or";
+
+--------------------------------------------------------------------------
+end mips_func_pkg;
+

lib/CPUs/MIPS/src/core/mips_icache.vhd

@@ -26,30 +26,40 @@ USE IEEE.STD_LOGIC_1164.ALL;
 USE IEEE.NUMERIC_STD.ALL;
 
 library work;
+use work.mips_util_pkg.all;
 use work.mips_types.all;
+use work.busmaster_types.all;
 
 ENTITY icache IS
 	Generic
 	(
-		cache_size	: natural := 2048;	-- words
+		CACHE_SIZE	: natural := 1024;	-- words
-		line_size	: natural := 8		-- words
+		LINE_SIZE	: natural := 8		-- words
 	);
 	Port
 	(
-		RST_I		: in  STD_LOGIC;
+		rst					: in  STD_LOGIC;
-		CLK_I		: in  STD_LOGIC;
+		clk					: in  STD_LOGIC;
-		ACK_I		: in  STD_LOGIC;
+		rdy					: out STD_LOGIC;
-		SRDY_I		: in  STD_LOGIC;
+
-		MRDY_O		: out STD_LOGIC;
+		-- CPU control/data
-		ADDR_O		: out word_t;
+		query_in			: in  icache_query_in_t;
-		DAT_I		: in  word_t;
+		query_out			: out icache_query_out_t;
-		STB_O		: out STD_LOGIC;
+
-		CYC_O		: out STD_LOGIC;
+		-- Cache control
-		ctrl		: in  cache_ctrl_t;
+		ctrl_in				: in  cache_ctrl_t;
-		cpu_en		: in  STD_LOGIC;
+
-		cpu_addr	: in  word_t;
+		-- busmaster data
-		cpu_dout	: out word_t;
+		bus_din				: in word_t;
-		cpu_busy	: out STD_LOGIC
+		bus_din_rdy			: out std_logic;
+		bus_din_vld			: in std_logic;
+
+		-- busmaster command
+		bus_cmd_rdy			: in std_logic;
+		bus_cmd_we			: out std_logic;
+		bus_cmd_cycle_en	: out std_logic;
+		bus_cmd_out			: out bm_cmd_t;
+		bus_cyc_complete	: in std_logic
 	);
 END icache;
 
@@ -65,7 +75,7 @@ COMPONENT dpram_1w1r2c_ra
 		clk_a		: in  STD_LOGIC;
 		clk_b		: in  STD_LOGIC;
 		we_a		: in  STD_LOGIC;
-		re_b		: in  STD_LOGIC;
+		re_b		: in  STD_LOGIC;
 		addr_a		: in  unsigned (addr_width-1 downto 0);
 		addr_b		: in  unsigned (addr_width-1 downto 0);
 		din_a		: in  unsigned (data_width-1 downto 0);
@@ -97,27 +107,26 @@ COMPONENT dpram_2w2r2c_ra is
 END COMPONENT;
 
 	constant addr_width			: natural := 32;
-	constant word_index_width	: natural := lg2(line_size);
+	constant word_index_width	: natural := lg2(LINE_SIZE);
-	constant cache_index_width	: natural := lg2(cache_size) - word_index_width;
+	constant cache_index_width	: natural := lg2(CACHE_SIZE) - word_index_width;
-	constant tag_width			: natural := addr_width - word_index_width - cache_index_width - 2;
+	constant tag_width			: natural := addr_width - lg2(CACHE_SIZE) - 2;
-	constant tag_parity_width	: natural := 3;
+	constant tag_ram_data_width	: natural := 1 + tag_width + lg2(TLB_NUM_ENTRIES);
-	constant tag_ram_data_width	: natural := 1 + tag_parity_width + tag_width;
 	constant tag_ram_addr_width	: natural := cache_index_width;
 	
 	subtype	tag_ram_data_t is unsigned (tag_ram_data_width-1 downto 0);
 
 	type icache_entry_t is record
-		valid	: std_logic;
+		valid		: std_logic;
-		tv_p	: unsigned(tag_parity_width-1 downto 0);
+		tag			: unsigned(tag_width-1 downto 0);
-		tag		: unsigned(tag_width-1 downto 0);
+		tlb_idx		: unsigned(lg2(TLB_NUM_ENTRIES)-1 downto 0);
 	end record;
 	
 	function to_icache_entry(x : tag_ram_data_t) return icache_entry_t is
 	variable result : icache_entry_t;
 	begin
 		result.valid	:= x(0);
-		result.tv_p		:= x(3 downto 1);
+		result.tag		:= x(tag_width downto 1);
-		result.tag		:= x(tag_width+3 downto 4);
+		result.tlb_idx	:= x(lg2(TLB_NUM_ENTRIES)+tag_width downto tag_width+1);
 		
 		return result;
 	end to_icache_entry;
@@ -125,150 +134,222 @@ END COMPONENT;
 	function to_tag_ram_data(x : icache_entry_t) return tag_ram_data_t is
 	variable result : tag_ram_data_t;
 	begin
-		result(0)										:= x.valid;
+		result(0)													:= x.valid;
-		result(3 downto 1)								:= x.tv_p;
+		result(tag_width downto 1)									:= x.tag;
-		result(tag_width+3 downto 4)					:= x.tag;
+		result(lg2(TLB_NUM_ENTRIES)+tag_width downto tag_width+1)	:= x.tlb_idx;
 		
 		return result;
 	end to_tag_ram_data;
 
-	type cache_state_t is (init, ready, invalidate_init, invalidate, flush, mem_request, mem_access, mem_data, rd_cache, upd_cache);
+	function to_word_index(x : word_t) return unsigned is
-	signal s, sn : cache_state_t;
+	variable result : unsigned(word_index_width-1 downto 0);
+	begin
+		result := x(word_index_width+1 downto 2);
+		
+		return result;
+		
+	end to_word_index;
 	
-	signal	cache_req			: std_logic;
-	signal	cache_ack			: std_logic;
-	signal	cache_busy			: std_logic;
-	signal	cache_hit			: std_logic;
-	signal	tag_match			: std_logic;
-	signal	cache_hit_inv		: std_logic;
-	signal	tag_match_inv		: std_logic;
-	signal  request_addr		: unsigned(addr_width-1 downto 0);
-	signal  fill_addr			: unsigned(addr_width-1 downto 0);
-	
-	signal  cache_index_inv		: unsigned(cache_index_width-1 downto 0);
-	signal  tag_inv				: unsigned(tag_width-1 downto 0);
-	signal  tag_reg_inv			: unsigned(tag_width-1 downto 0);
-	
-	signal	cache_entry_in		: icache_entry_t;
-	signal	cache_entry_out		: icache_entry_t;
-	signal	cache_entry_out_inv	: icache_entry_t;
-	signal  data_ram_addr_rd	: unsigned(lg2(cache_size)-1 downto 0);
-	signal  data_ram_data_rd	: word_t;
-	signal  data_ram_addr_wr	: unsigned(lg2(cache_size)-1 downto 0);
-	signal  data_ram_data_wr	: word_t;
-	signal  data_ram_we			: std_logic;
-	signal  data_ram_re			: std_logic;
 
-	signal  tag_ram_addr_rd		: unsigned(cache_index_width-1 downto 0);
+	function to_cache_index(x : word_t) return unsigned is
-	signal  tag_ram_data_rd		: tag_ram_data_t;
+	variable result : unsigned(cache_index_width-1 downto 0);
-	signal  tag_ram_data_rd_inv	: tag_ram_data_t;
+	begin
-	signal  tag_ram_addr_wr		: unsigned(cache_index_width-1 downto 0);
+		result := x(cache_index_width+word_index_width+1 downto word_index_width+2);
-	signal  tag_ram_data_wr		: tag_ram_data_t;
+		
-	signal  tag_ram_we			: std_logic;
+		return result;
---	signal  tag_ram_re			: std_logic;
+		
+	end to_cache_index;
+
+	function to_tag(x : word_t) return unsigned is
+	variable result : unsigned(tag_width-1 downto 0);
+	begin
+		result := x(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
+		
+		return result;
+		
+	end to_tag;
+
+	type cache_busy_state_t is (init, ready, busy);
+	signal cache_busy_state, cache_busy_state_next : cache_busy_state_t;
+
+	type cache_ctrl_state_t is (init, ready, invalidate, invalidate_post, flush, bus_request, mem_read, mem_read_single, mem_read_data, mem_read_data_single, rd_cache, upd_cache);
+	signal cache_ctrl_state, cache_ctrl_state_next : cache_ctrl_state_t;
 	
-	signal	fill_count			: natural range 0 to 2**word_index_width-1;
+	type debug_t is record
-	signal	fill_count_en		: std_logic;
+		ready			: std_logic;
-	signal	fill_count_rdy		: std_logic;
+		req_strobe		: std_logic;
-	signal	flush_load_en		: std_logic;
+		req_read		: std_logic;
-	signal  flush_addr_preset	: unsigned(cache_index_width-1 downto 0);
+		req_va			: word_t;
-	signal	flush_count_preset	: natural range 0 to 2**cache_index_width-1;
+		req_pa			: word_t;
-	signal  flush_addr			: unsigned(cache_index_width-1 downto 0);
+		dout_reg		: word_t;
-	signal	flush_count			: natural range 0 to 2**cache_index_width-1;
+	end record;
-	signal	flush_count_en		: std_logic;
-	signal	flush_count_rdy		: std_logic;
-	signal	request_count		: natural range 0 to 2**word_index_width-1;
-	signal	request_count_en	: std_logic;
-	signal	request_count_rdy	: std_logic;
-	signal	was_miss			: std_logic;
-	signal	invalidate_all		: std_logic;
-	signal	invalidate_ack		: std_logic;
-	signal  invalidate_en		: std_logic;
-	signal  invalidate_req		: std_logic;
 
-	signal  DAT_I_r				: word_t;
+	signal debug		: debug_t;
-	signal  ACK_I_r				: std_logic;
+
+	signal	req_strobe				: std_logic;
+	signal	cache_req_rd			: std_logic;
+	signal	read_rdy				: std_logic;
+	signal	cache_rdy				: std_logic;
+	signal	cache_hit				: std_logic;
+	signal  request_va				: word_t;
+	signal  request_pa				: word_t;
+	signal	request_nc				: std_logic;
+
+	signal	tag_match				: std_logic;
+	signal	cache_hit_inv			: std_logic;
+	signal	tag_match_inv			: std_logic;
 	
-	alias cpu_word_index is cpu_addr(word_index_width+1 downto 2);
+	signal	mem_fetch_req			: std_logic;
-	alias cpu_cache_index is cpu_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
+	signal	mem_fetch_ack			: std_logic;
-	alias cpu_tag is cpu_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
+	signal	single_read_en			: std_logic;
+	signal	single_read_set			: std_logic;
+	signal	single_read_reg_vld		: std_logic;
 
-	alias fill_word_index is fill_addr(word_index_width+1 downto 2);
+	signal  cache_index_inv			: unsigned(cache_index_width-1 downto 0);
-	alias fill_cache_index is fill_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
+	signal  tag_inv					: unsigned(tag_width-1 downto 0);
-	alias fill_tag is fill_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
+	signal  tag_reg_inv				: unsigned(tag_width-1 downto 0);
+	
+	signal	cache_entry_in			: icache_entry_t;
+	signal	cache_entry_out			: icache_entry_t;
+	signal	cache_entry_out_inv		: icache_entry_t;
+	signal  data_ram_addr_rd		: unsigned(lg2(CACHE_SIZE)-1 downto 0);
+	signal  data_ram_data_rd		: word_t;
+	signal  data_ram_addr_wr		: unsigned(lg2(CACHE_SIZE)-1 downto 0);
+	signal  data_ram_data_wr		: word_t;
+	signal	data_ram_we				: std_logic;
+	signal  single_read_reg			: word_t;
+	signal  cache_dout				: word_t;
 
-	alias req_word_index is request_addr(word_index_width+1 downto 2);
+	signal  tag_ram_addr_rd			: unsigned(cache_index_width-1 downto 0);
-	alias req_cache_index is request_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
+	signal  tag_ram_data_rd			: tag_ram_data_t;
-	alias req_tag is request_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
+	signal  tag_ram_data_rd_inv		: tag_ram_data_t;
+	signal  tag_ram_addr_wr			: unsigned(cache_index_width-1 downto 0);
+	signal  tag_ram_data_wr			: tag_ram_data_t;
+	signal	tag_ram_we				: std_logic;
+
+	signal	fill_count				: natural range 0 to 2**word_index_width-1;
+	signal	fill_count_en			: std_logic;
+	signal	fill_count_rdy			: std_logic;
+	signal	flush_count				: natural range 0 to 2**cache_index_width-1;
+	signal	flush_count_rst			: std_logic;
+	signal	flush_count_en			: std_logic;
+	signal	flush_count_rdy			: std_logic;
+	signal	request_count			: natural range 0 to 2**word_index_width-1;
+	signal	request_count_en		: std_logic;
+	signal	request_count_rdy		: std_logic;
+	signal  ram_read_en				: std_logic;
+	signal  ram_write_en			: std_logic;
+	signal	was_miss				: std_logic;
+	signal	invalidate_all			: std_logic;
+	signal	invalidate_ack			: std_logic;
+	signal  invalidate_en			: std_logic;
+	signal  invalidate_req			: std_logic;
+
+	signal	fill_word_index			: unsigned(word_index_width-1 downto 0);
+	signal	req_word_index			: unsigned(word_index_width-1 downto 0);
 
 begin
 
-	cache_index_inv	<= ctrl.inv_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
+	-- Print info
-	tag_inv			<= ctrl.inv_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
+	assert false report "addr_width        = " & natural'image(addr_width) & "." severity note;
+	assert false report "tag_width         = " & natural'image(tag_width) & "." severity note;
+	assert false report "word_index_width  = " & natural'image(word_index_width) & "." severity note;
+	assert false report "cache_index_width = " & natural'image(cache_index_width) & "." severity note;
 
+-- 	tlb_query_out.vld					<= cache_req_rd;
+-- 	tlb_query_out.vaddr					<= request_va;
+-- 	tlb_query_out.write					<= '0';
+--	tlb_query_out.vld					<= req_strobe;
+--	tlb_query_out.vaddr					<= query_in.addr;
+--	tlb_query_out.write					<= '0';
 	
-data_in_register:
+	cache_index_inv						<= ctrl_in.inv_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
-	process(CLK_I)
+	tag_inv								<= ctrl_in.inv_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
+
+	request_pa							<= request_va;
+
+	debug.req_strobe	<= req_strobe;
+	debug.req_read		<= cache_req_rd;
+	debug.req_va		<= request_va;
+	debug.req_pa		<= request_pa;
+	debug.dout_reg		<= cache_dout;
+	debug.ready			<= cache_rdy;
+	
+-------------------------------------------------
+-- ctrl
+-------------------------------------------------
+req_strobe	<= cache_rdy and query_in.en;
+
+ctrl_request_register:
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			ACK_I_r	<= ACK_I;
+			if cache_rdy = '1' then
-			if ACK_I = '1' then
+				cache_req_rd	<= query_in.en;
-				DAT_I_r <= DAT_I;
 			end if;
 		end if;
 	end process;	
-	
+
+-------------------------------------------------
 fill_address_register:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			if fill_count_en = '1' then
+			if rst = '1' then
-				if ACK_I_r = '1' then
+				request_va	<= (others => '0');
-					fill_word_index	<= fill_word_index + 1;
+				request_nc	<= '0';
-				end if;	
+			elsif req_strobe = '1' then
-			elsif cache_busy = '0' then
+				request_va	<= query_in.addr;
-				fill_addr	<= cpu_addr(addr_width-1 downto 2) & "00";
+				request_nc	<= query_in.nc;
+				fill_word_index	<= to_word_index(query_in.addr);
+			elsif bus_din_vld = '1' and fill_count_en = '1' then
+				fill_word_index	<= fill_word_index + 1;
 			end if;
 		end if;
 	end process;	
 
-request_address_register:
+bus_request_address_register:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			if request_count_en = '1'then
+			if req_strobe = '1' then
-				if SRDY_I = '1' then
+				req_word_index	<= to_word_index(query_in.addr);
-					req_word_index	<= req_word_index + 1;
+			elsif request_count_en = '1' and bus_cmd_rdy = '1' then
-				end if;	
+				req_word_index	<= req_word_index + 1;
-			elsif cache_busy = '0' then
+			end if;	
-				request_addr	<= cpu_addr(addr_width-1 downto 2) & "00";
-			end if;
 		end if;
 	end process;	
 
-cpu_request_register:
+single_read_register:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			if RST_I = '1' then
+			if rst = '1' then
-				cache_req	<= '0';
+				single_read_en <= '0';
-			elsif cpu_en = '1' then
+				single_read_reg_vld <= '0';
-				if cache_busy = '0' then
+			elsif single_read_en = '0' then
-					cache_req			<= '1';
+				if single_read_set = '1' then
+					single_read_en		<= '1';
+				end if;
+			elsif single_read_reg_vld = '1' then
+				if query_in.en = '1' then
+					single_read_en <= '0';
+					single_read_reg_vld <= '0';
 				end if;	
-			elsif cache_ack = '1' then
+			elsif bus_din_vld = '1' and single_read_en = '1' then
-				cache_req			<= '0';
+				single_read_reg <= bus_din;
+				single_read_reg_vld <= '1';
 			end if;
 		end if;
 	end process;	
 
 cache_invalidate_request:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			if RST_I = '1' or ctrl.inv_all = '1' or ctrl.inv_at = '1' then
+			if rst = '1' or ctrl_in.inv_all = '1' or ctrl_in.inv_at = '1' then
 				invalidate_req	<= '1';
-				invalidate_all	<= ctrl.inv_all or RST_I;
+				invalidate_all	<= ctrl_in.inv_all or rst;
 				tag_reg_inv	<= tag_inv;
 			elsif invalidate_ack = '1' then
 				invalidate_req	<= '0';
@@ -285,12 +366,12 @@ inst_tag_ram : dpram_2w2r2c_ra
 	)
 	PORT MAP
 	(
-		clk_a	=> CLK_I,
+		clk_a	=> clk,
-		clk_b	=> CLK_I,
+		clk_b	=> clk,
 		re_a	=> '1',
-		re_b	=> '1',
+		re_b	=> ram_read_en,
-		we_a	=> tag_ram_we,
+		we_a	=> tag_ram_we,
-		we_b	=> '0',
+		we_b	=> '0',
 		addr_a	=> tag_ram_addr_wr,
 		addr_b	=> tag_ram_addr_rd,
 		din_a	=> tag_ram_data_wr,
@@ -302,15 +383,15 @@ inst_tag_ram : dpram_2w2r2c_ra
 inst_data_ram : dpram_1w1r2c_ra
 	GENERIC MAP
 	(
-		addr_width	=> lg2(cache_size),
+		addr_width	=> lg2(CACHE_SIZE),
 		data_width	=> word_t'length
 	)
 	PORT MAP
 	(
-		clk_a	=> CLK_I,
+		clk_a	=> clk,
-		clk_b	=> CLK_I,
+		clk_b	=> clk,
-		re_b	=> data_ram_re,
 		we_a	=> data_ram_we,
+		re_b	=> ram_read_en,
 		addr_a	=> data_ram_addr_wr,
 		addr_b	=> data_ram_addr_rd,
 		din_a	=> data_ram_data_wr,
@@ -318,172 +399,236 @@ inst_data_ram : dpram_1w1r2c_ra
 	);
 	
 cache_state_next:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			if RST_I = '1' then
+			if rst = '1' then
-				s	<= init;
+				cache_ctrl_state	<= init;
+				cache_busy_state	<= init;
 			else
-				s	<= sn;
+				cache_ctrl_state	<= cache_ctrl_state_next;
+				cache_busy_state	<= cache_busy_state_next;
 			end if;
 		end if;
 	end process;	
 
-	MRDY_O			<= fill_count_en;
+	bus_din_rdy						<= fill_count_en or single_read_en;
-	ADDR_O				<= fill_tag & fill_cache_index & req_word_index & "00";
+	bus_cmd_out.addr				<= to_tag(request_pa) & to_cache_index(request_pa) & req_word_index & "00";
+	bus_cmd_out.rw					<= '0';
+	bus_cmd_out.sel					<= (others => '1');
+
+	cache_rdy						<= read_rdy;
+	query_out.rdy					<= cache_rdy;
+	rdy								<= cache_rdy;
 	
-	cpu_busy			<= cache_busy;
+	cache_dout						<= single_read_reg when single_read_en = '1' else data_ram_data_rd;
-	cpu_dout			<= data_ram_data_rd;
+	query_out.data					<= cache_dout;
-	
-	cache_entry_out		<= to_icache_entry(tag_ram_data_rd);
-	tag_match			<= '1' when fill_tag = cache_entry_out.tag else '0';
-	cache_hit			<= tag_match and cache_entry_out.valid;
 
-	cache_entry_out_inv	<= to_icache_entry(tag_ram_data_rd_inv);
+	cache_entry_out					<= to_icache_entry(to_mips_01(tag_ram_data_rd));
-	tag_match_inv		<= '1' when tag_reg_inv = cache_entry_out_inv.tag else '0';
+	tag_match						<= '1' when ((to_tag(request_pa) xor cache_entry_out.tag) = (tag_width-1 downto 0 => '0')) else '0';
-	cache_hit_inv		<= tag_match_inv and cache_entry_out_inv.valid;
+	cache_hit						<= tag_match and cache_entry_out.valid;
 
-	tag_ram_data_wr		<= to_tag_ram_data(cache_entry_in);
+	cache_entry_out_inv				<= to_icache_entry(to_mips_01(tag_ram_data_rd_inv));
-	tag_ram_addr_wr		<= flush_addr when invalidate_en = '1' else fill_cache_index;
+	tag_match_inv					<= '1' when tag_reg_inv = cache_entry_out_inv.tag else '0';
-	tag_ram_addr_rd		<= cpu_cache_index when was_miss = '0' else fill_cache_index;
+	cache_hit_inv					<= tag_match_inv and cache_entry_out_inv.valid;
-	data_ram_addr_rd	<= (cpu_cache_index & cpu_word_index) when was_miss = '0' else (fill_cache_index & fill_word_index);
-	data_ram_addr_wr	<= fill_cache_index & fill_word_index;
-	data_ram_data_wr	<= DAT_I_r;
-	data_ram_we			<= fill_count_en and ACK_I_r;
-	data_ram_re		<= (cpu_en or was_miss) and not data_ram_we;
---	tag_ram_re		<= (cpu_en or was_miss) and not data_ram_we;
 
-cache_state:
+	ram_read_en						<= query_in.en or was_miss;
-	process(s, cache_req, cache_hit, cache_hit_inv, cache_index_inv, flush_count_rdy, fill_count_rdy, request_count_rdy, fill_tag, SRDY_I, invalidate_req, invalidate_all)
+	tag_ram_data_wr					<= to_tag_ram_data(cache_entry_in);
+	tag_ram_addr_wr					<= to_unsigned(flush_count, cache_index_width) when invalidate_en = '1' else to_cache_index(request_va);
+	tag_ram_addr_rd					<= to_cache_index(query_in.addr) when was_miss = '0' else to_cache_index(request_va);
+	data_ram_addr_rd				<= (to_cache_index(query_in.addr) & to_word_index(query_in.addr)) when was_miss = '0' else (to_cache_index(request_va) & fill_word_index);
+	data_ram_addr_wr				<= to_cache_index(request_va) & fill_word_index;
+	data_ram_data_wr				<= bus_din;
+	data_ram_we						<= ram_write_en and bus_din_vld;
+
+cache_busy_state_fsm:
+	process(cache_busy_state, request_nc, cache_req_rd, cache_hit, mem_fetch_ack, single_read_en, single_read_reg_vld)
+	begin
+
+		read_rdy 				<= '0';
+		mem_fetch_req			<= '0';
+		single_read_set			<= '0';
+		cache_busy_state_next	<= cache_busy_state;
+		
+		case cache_busy_state is
+
+		when init =>
+			cache_busy_state_next <= ready;
+			
+		when ready =>
+			if single_read_en = '1' then
+				read_rdy			<= single_read_reg_vld;
+			else
+				read_rdy 				<= '1';
+				if cache_req_rd = '1' then
+--					if tlb_query_in.exc = '0' then
+						if request_nc = '1' or cache_hit = '0' then
+--							mem_fetch_req			<= '1';	
+							read_rdy				<= '0';
+							single_read_set			<= request_nc;
+							cache_busy_state_next 	<= busy;
+						end if;
+--					end if;
+				end if;
+			end if;
+
+		when busy =>
+			mem_fetch_req	<= '1';	
+			if mem_fetch_ack = '1' then
+				cache_busy_state_next <= ready;
+			end if;
+		
+		when others => 
+			cache_busy_state_next <= ready;
+				
+		end case;
+		
+	end process;
+
+cache_ctrl_state_fsm:
+	process(cache_ctrl_state, mem_fetch_req, request_nc, bus_din_vld, cache_hit_inv, flush_count_rdy, fill_count_rdy, request_count_rdy, request_pa, bus_cmd_rdy, invalidate_req, invalidate_all)
 	begin
-		cache_busy 				<= cache_req;
-		cache_ack 				<= '0';
 		tag_ram_we				<= '0';
+		flush_count_en			<= '0';
+		flush_count_rst			<= '0';
+		invalidate_en			<= '0';
 		request_count_en		<= '0';
 		fill_count_en			<= '0';
-		CYC_O					<= '0';
+		ram_write_en			<= '0';
-		STB_O					<= '0';
+		bus_cmd_cycle_en		<= '0';
+		bus_cmd_we				<= '0';
 		was_miss				<= '0';
-
-		invalidate_en			<= '0';
 		invalidate_ack			<= '0';
-
+		mem_fetch_ack			<= '0';
-		flush_load_en			<= '0';
-		flush_count_en			<= '0';
-		flush_addr_preset		<= (others=>'0'); -- all
-		flush_count_preset		<= 2**cache_index_width-1;
 		
-		cache_entry_in.tv_p		<= (others => '0');
+		cache_entry_in.tag		<= to_tag(request_pa);
-		cache_entry_in.tag		<= fill_tag;
 		cache_entry_in.valid	<= '0';
-		sn	<= s;
+		cache_ctrl_state_next	<= cache_ctrl_state;
 		
-		case s is
+		case cache_ctrl_state is
+
 		when init =>
-			sn <= ready;
+			cache_ctrl_state_next <= ready;
+
 		when ready =>
 			if invalidate_req = '1' then
-				sn 				<= invalidate_init;
+				cache_ctrl_state_next 	<= invalidate;
 				invalidate_en	<= '1';
-				flush_load_en	<= '1';
+			elsif mem_fetch_req = '1' then
-				if invalidate_all = '1' then
+				cache_ctrl_state_next	<= bus_request;
-					flush_addr_preset		<= (others=>'0'); -- all
+				bus_cmd_cycle_en		<= '1';
-					flush_count_preset		<= 2**cache_index_width-1;
+			end if;
-				else
+
-					flush_addr_preset		<= cache_index_inv; -- at
+		when invalidate =>
-					flush_count_preset		<= 0;
+			cache_ctrl_state_next <= rd_cache;
-				end if;	
+			invalidate_en			<= '1';
-			elsif cache_req = '1' then
+			invalidate_ack			<= '1';
-				if cache_hit = '0' then
+			if invalidate_all = '1' then
-					sn						<= mem_request;
+				cache_ctrl_state_next					<= flush;
-					CYC_O					<= '1';
+				flush_count_rst		<= '1';
-				else
+			elsif cache_hit_inv = '1' then
-					cache_busy 		<= '0';
+				tag_ram_we				<= '1';
-					cache_ack 		<= '1';
+				cache_entry_in.valid	<= '0';
+				cache_entry_in.tag		<= (others => '0');
+				cache_ctrl_state_next	<= invalidate_post;
+			end if;	
+				
+		when flush =>
+			flush_count_en			<= '1';
+			invalidate_en			<= '1';
+			tag_ram_we				<= '1';
+			cache_entry_in.valid	<= '0';
+			cache_entry_in.tag		<= (others => '0');
+			if flush_count_rdy = '1' then
+				tag_ram_we			<= '0';
+				cache_ctrl_state_next	<= invalidate_post;
+			end if;
+
+		when invalidate_post =>
+			was_miss	<= '1';
+			cache_ctrl_state_next		<= ready;
+
+		when bus_request =>
+			bus_cmd_cycle_en	<= '1';
+			if bus_cmd_rdy = '1' then
+				cache_ctrl_state_next <= mem_read;
+				if request_nc = '1' then
+					cache_ctrl_state_next <= mem_read_single;
 				end if;	
-			end if;	
+			end if;
-			
+
-		when invalidate_init =>
+		when mem_read =>
-			invalidate_en			<= '1';
+			bus_cmd_cycle_en	<= '1';
-			sn	<= invalidate;
+			bus_cmd_we			<= '1';
-		
-		when invalidate =>
-			sn	<= rd_cache;
-			invalidate_en			<= '1';
-			invalidate_ack			<= '1';
-			if cache_hit_inv = '1' or invalidate_all = '1' then
-				sn					<= flush;
-			end if;	
-				
-		when flush =>
-			flush_count_en			<= '1';
-			invalidate_en			<= '1';
-			tag_ram_we				<= '1';
-			cache_entry_in.valid	<= '0';
-			cache_entry_in.tag		<= (others => '0');
-			if flush_count_rdy = '1' then
-				tag_ram_we			<= '0';
-				sn					<= rd_cache;
-			end if;
-		when mem_request =>
-			CYC_O		<= '1';
-			if SRDY_I = '1' then
-				sn <= mem_access;
-			end if;	
-		when mem_access =>
 			request_count_en	<= '1';
+			ram_write_en		<= '1';
 			fill_count_en		<= '1';
-			CYC_O				<= '1';
-			STB_O				<= '1';
 			if request_count_rdy = '1' then
-				STB_O			<= '0';
+				bus_cmd_we		<= '0';
-				sn				<= mem_data;
+				cache_ctrl_state_next	<= mem_read_data;
 			end if;	
-		when mem_data =>
+
-			CYC_O			<= '1';
+		when mem_read_single =>
-			fill_count_en	<= '1';
+			mem_fetch_ack		<= '1';
+			bus_cmd_cycle_en	<= '1';
+			bus_cmd_we			<= '1';
+			cache_ctrl_state_next <= mem_read_data_single;
+
+		when mem_read_data =>
+			bus_cmd_cycle_en	<= '1';
+			fill_count_en		<= '1';
+			ram_write_en		<= '1';
 			if fill_count_rdy = '1' then
 				tag_ram_we				<= '1';
 				cache_entry_in.valid	<= '1';
-				sn						<= rd_cache;
+				cache_ctrl_state_next	<= rd_cache;
 			end if;	
+
+		when mem_read_data_single =>
+			bus_cmd_cycle_en	<= '1';
+			if bus_din_vld = '1' then
+				cache_ctrl_state_next	<= ready;
+			end if;	
+
 		when rd_cache =>
+			mem_fetch_ack	<= '1';
 			was_miss	<= '1';
-			sn			<= ready;
+			cache_ctrl_state_next		<= ready;
+			
 		when others => 
-			sn	<= ready;
+			cache_ctrl_state_next		<= ready;
 		end case;
 
 	end process;
 
-flush_counter:
+flush_counter:
-	process(CLK_I)
+	process(clk)
-	begin
+	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
-			if flush_load_en = '1' then
+			if ctrl_in.inv_at = '1' then
-				flush_count_rdy	<= '0';
+				flush_count		<= to_integer(cache_index_inv);
-				flush_count		<= flush_count_preset;
+			elsif flush_count_rst = '1' then
-				flush_addr		<= flush_addr_preset;
+				flush_count_rdy	<= '0';
-			elsif flush_count_en = '1' then
+				flush_count		<= 2**cache_index_width-1;
-				if flush_count /= 0 then
+			elsif flush_count_en = '1' then
-					flush_count	<= flush_count - 1;
+				if flush_count /= 0 then
-					flush_addr	<= flush_addr + 1;
+					flush_count	<= flush_count - 1;
-				else
+				else
-					flush_count_rdy	<= '1';
+					flush_count_rdy	<= '1';
-				end if;	
+				end if;	
-			end if;
+			end if;
-		end if;
+		end if;
-	end process;	
+	end process;	
 		
 request_counter:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
 			if request_count_en = '0' then
 				request_count_rdy	<= '0';
 				request_count		<= 2**word_index_width-1;
 			else	
-				if SRDY_I = '1' then
+				if bus_cmd_rdy = '1' then
 					if request_count /= 0 then
 						request_count 	<= request_count - 1;
 					else
@@ -495,14 +640,14 @@ request_counter:
 	end process;	
 
 fill_counter:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(clk) then
 			if fill_count_en = '0' then
 				fill_count_rdy	<= '0';
 				fill_count		<= 2**word_index_width-1;
 			else	
-				if ACK_I_r = '1' then
+				if bus_din_vld = '1' then
 					if fill_count /= 0 then
 						fill_count 	<= fill_count - 1;
 					else

lib/CPUs/MIPS/src/core/mips_pipeline.vhd

@@ -27,30 +27,30 @@ use IEEE.numeric_std.ALL;
 library work;
 use work.mips_types.all;
 use work.mips_instr.all;
+use work.mips_util_pkg.all;
 
 entity pipeline is
 	Port
 	(
-		rst			: in  STD_LOGIC;
+		rst					: in  STD_LOGIC;
-		clk			: in  STD_LOGIC;
+		clk					: in  STD_LOGIC;
-		ce			: in  STD_LOGIC;
+		ce					: in  STD_LOGIC;
-		imem_rdy	: in  STD_LOGIC;
+		imem_err			: in  STD_LOGIC;
-		imem_en		: out STD_LOGIC;
+		dmem_err			: in  STD_LOGIC;
-		imem_addr	: out word_t;
+		cop_ir				: out word_t;
-		imem_data	: in  word_t;
+		cop_ir_en			: out STD_LOGIC;
-		dmem_rdy	: in  STD_LOGIC;
+		cop_din				: in  word_t;
-		dmem_en		: out STD_LOGIC;
+		cop_dout			: out word_t;
-		dmem_we		: out STD_LOGIC;
+		ctrl_out			: out pipe_ctrl_out_t;
-		dmem_be		: out unsigned(3 downto 0);
+		ctrl_in				: in  pipe_ctrl_in_t;
-		dmem_addr	: out word_t;
+		dtlb_qry_out		: out tlb_query_in_t;
-		dmem_din	: in  word_t;
+		dtlb_qry_in			: in  tlb_query_out_t;
-		dmem_dout	: out word_t;
+		dcache_qry_out		: out dcache_query_in_t;
-		cop_ir		: out word_t;
+		dcache_qry_in		: in  dcache_query_out_t;
-		cop_ir_en	: out STD_LOGIC;
+		itlb_qry_out		: out tlb_query_in_t;
-		cop_din		: in  word_t;
+		itlb_qry_in			: in  tlb_query_out_t;
-		cop_dout	: out word_t;
+		icache_qry_out		: out icache_query_in_t;
-		c0_ctrl_out	: out cop0_ctrl_in_t;
+		icache_qry_in		: in  icache_query_out_t
-		c0_ctrl_in	: in  cop0_ctrl_out_t
 		
 	);
 end pipeline;
@@ -146,87 +146,78 @@ architecture Behavioral of pipeline is
 	signal EX_stage			: EX_t;
 	signal MEM_stage		: MEM_t;
 	signal WB_stage			: WB_t;
+	signal clk_2, clk_1		: STD_LOGIC;
 	signal hdu				: hdu_t;
 	signal sdu				: sdu_t;
 	signal reg_a			: word_t;
 	signal reg_b			: word_t;
-	signal branch_taken		: STD_LOGIC;
+	signal branch_ce		: STD_LOGIC;
+	signal cpu_run			: STD_LOGIC;
 	signal alu_result		: word_t;
 	signal mul_result		: word_t;
 	signal mul_busy			: STD_LOGIC;
 	signal EX_events_instr	: event_t;
 	signal EX_events_alu	: event_t;
 	signal EX_events_mem	: event_t;
+	signal EX_events_DTLB	: event_t;
 	signal EX_events		: event_t;
 	signal MEM_events		: event_t;
-	signal bcu_op_a			: word_t;
+	signal stage_rst		: unsigned(3 downto 0);
-	signal bcu_op_b			: word_t;
+	signal pipe_rst			: STD_LOGIC;
-	signal bcu_flags		: bcu_flags_t;
-	signal vaddr			: word_t;
-	signal dmem_src			: word_t;
-	
-	signal pc : pc_t;
-	signal pc_branch		: word_t;
-	signal pc_curr			: word_t;
-	signal pc_last			: word_t;
-	signal pc_nxt			: word_t;
-	signal pc_branch_take	: STD_LOGIC;
-	signal ID_ctrl_branch	: STD_LOGIC;
-	signal EX_ctrl_branch	: STD_LOGIC;
-	signal MEM_ctrl_branch	: STD_LOGIC;
-	signal sdu_ID_stall		: STD_LOGIC;
-	signal sdu_ID_nop		: STD_LOGIC;
-	signal c0_ctrl_in_exc_pending		: STD_LOGIC;
-	signal c0_ctrl_in_exc_inject		: STD_LOGIC;
 	
+	signal pc 					: pc_t;
+
 --------------------------------------------------------------------------
 begin
 	
+	clk_1					<= clk;
+	clk_2					<= not clk;
+	cpu_run					<= ce;
+	
 	-- Stall Detection Unit ---------------------------------------------------
-	sdu.ID_nop				<= sdu.imem_dep or c0_ctrl_in.exc_pending or ID_stage.exc or EX_stage.exc or MEM_stage.exc;
+	sdu.ID_nop				<= sdu.imem_dep or ctrl_in.exc_pending or ID_stage.exc or EX_stage.exc or MEM_stage.exc; -- or pc.branch_not_taken;
 	sdu.EX_nop				<= sdu.mul_dep or ID_stage.nop or ID_stage.exc;
 	sdu.MEM_nop				<= EX_stage.nop or EX_stage.exc;
-	sdu.WB_nop				<= sdu.dmem_dep or MEM_stage.nop;
+	sdu.WB_nop				<= sdu.dmem_dep or MEM_stage.exc or WB_stage.exc or ctrl_in.exc_pending;
 
 	sdu.stall_all			<= sdu.dmem_dep;
-	sdu.ID_stall			<= sdu.stall_all or sdu.imem_dep or sdu.mul_dep or c0_ctrl_in.exc_exit;
+	sdu.ID_stall			<= sdu.stall_all or sdu.imem_dep or sdu.mul_dep or ctrl_in.exc_exit;
 	sdu.EX_stall			<= sdu.stall_all;
 	sdu.MEM_stall			<= sdu.stall_all;
-	sdu.WB_stall			<= sdu.stall_all;
+	sdu.WB_stall			<= '0';
 
 	sdu.mul_dep				<= ID_stage.ctrl.mul_access and (EX_stage.ctrl.mul_start or mul_busy);
-	sdu.imem_dep			<= not imem_rdy;
+	sdu.imem_dep			<= not icache_qry_in.rdy;
-	sdu.dmem_dep			<= not dmem_rdy and MEM_stage.ctrl.dmem_en;
+	sdu.dmem_dep			<= not dcache_qry_in.rdy and MEM_stage.ctrl.dmem_en;
 	---------------------------------------------------------------------------
-	imem_en 				<= ce and not (sdu.mul_dep or sdu.dmem_dep or c0_ctrl_in.exc_commit);
+	itlb_qry_out.vld		<= (not sdu.ID_stall or ctrl_in.exc_inject);
-	pc_branch				<= pc.pc_branch;
+	itlb_qry_out.write		<= '0';
-	pc_curr					<= pc.curr;
+	itlb_qry_out.vaddr		<= pc.nxt;
-	pc_last					<= pc.last;
+
-	pc_nxt					<= pc.nxt;
+	icache_qry_out.addr		<= itlb_qry_in.paddr;
-	pc_branch_take			<= pc.branch_take;
+	icache_qry_out.en		<= cpu_run and not (sdu.mul_dep or sdu.dmem_dep or ctrl_in.exc_commit);
-	ID_ctrl_branch			<= ID_stage.ctrl.branch;
+	icache_qry_out.nc		<= itlb_qry_in.flags.N;
-	EX_ctrl_branch			<= EX_stage.ctrl.branch;
-	MEM_ctrl_branch			<= MEM_stage.ctrl.branch;
-	sdu_ID_stall			<= sdu.ID_stall;
-	sdu_ID_nop				<= sdu.ID_nop;
-	c0_ctrl_in_exc_pending	<= c0_ctrl_in.exc_pending;
-	c0_ctrl_in_exc_inject	<= c0_ctrl_in.exc_inject;
 	
+	dtlb_qry_out.vld		<= ID_stage.ctrl.dmem_en and not sdu.dmem_dep;
+	dtlb_qry_out.write		<= ID_stage.ctrl.dmem_we;
+	dtlb_qry_out.vaddr		<= ID_stage.va;
+
+	dcache_qry_out.be		<= store_be(EX_stage.pa_off, EX_stage.ctrl.dmem_en, EX_stage.ctrl.word2_en, EX_stage.ctrl.word4_en, EX_stage.ctrl.align_left, EX_stage.ctrl.shift_byp);
+	dcache_qry_out.en		<= EX_stage.dmem_en;
+	dcache_qry_out.we		<= EX_stage.ctrl.dmem_we;
+	dcache_qry_out.data		<= store_shift(EX_stage.reg_b, EX_stage.pa_off, EX_stage.ctrl.shift_offset, EX_stage.ctrl.shift_byp);
+	dcache_qry_out.addr		<= dtlb_qry_in.paddr;
+	dcache_qry_out.nc		<= dtlb_qry_in.flags.N;
+
 --------------------------------------------------------------------------
 -- Coprocessor assignments
 --------------------------------------------------------------------------
-	c0_ctrl_out.bd_wb		<= WB_stage.bd;
+	ctrl_out.sdu			<= sdu;
-	c0_ctrl_out.epc_mem		<= MEM_stage.pcn;
+	ctrl_out.exc_req		<= WB_stage.exc and not WB_stage.exc_last;
-	c0_ctrl_out.epc_wb		<= MEM_stage.epc;
+	ctrl_out.exc_ack		<= '1';
-	c0_ctrl_out.imem_addr	<= EX_stage.epc;
-	c0_ctrl_out.dmem_addr	<= MEM_stage.va;
-	c0_ctrl_out.sdu			<= sdu;
-	c0_ctrl_out.events		<= WB_stage.events;
-	c0_ctrl_out.exc_req		<= MEM_stage.exc;
-	c0_ctrl_out.exc_ack		<= WB_stage.exc;
 	cop_ir					<= ID_stage.IR;
 	cop_ir_en				<= ID_stage.ctrl.cop_instr_en;
-	cop_dout				<= dmem_din when MEM_stage.ctrl.dmem_en = '1' else MEM_stage.ex_result;
+	cop_dout				<= EX_stage.reg_b;
 
 --------------------------------------------------------------------------
 -- Muldiv
@@ -250,90 +241,191 @@ inst_muldiv: muldiv
 --------------------------------------------------------------------------
 -- IF stage
 --------------------------------------------------------------------------
-	imem_addr		<= pc.curr;
 
-proc_stage_pc_next:
+	pipe_rst		<= stage_rst(0);
-	process(clk)
+	
+proc_stage_reset:
+	process(clk_1)
 	begin
-		if rising_edge(clk) then
+		if rising_edge(clk_1) then
-			if c0_ctrl_in.exc_inject = '1' then
+			if rst = '1' then
-				pc.nxt	<= c0_ctrl_in.exc_vec;
+				stage_rst <= (others => '1');
-			elsif sdu.ID_stall = '0' then
+			else
-				pc.last	<= pc.curr;
+				stage_rst <= stage_rst(stage_rst'left-1 downto 0) & '0';
-				if ID_stage.ctrl.jump = '1' then
+			end if;	
-					pc.nxt	<= ID_stage.jimm32;
-				elsif ID_stage.ctrl.jump_long = '1' then
-					pc.nxt	<= ID_stage.reg_a;
-				else
-					pc.nxt	<= pc.curr + 4;
-				end if;
-			end if;
 		end if;	
 	end process;
 
-proc_stage_branch:
+--------------------------------------------------------------------------
-	process(EX_stage, bcu_flags)
+-- pc
+--------------------------------------------------------------------------
+inst_bcu_alt_ID: bcu
+	GENERIC MAP
+	(
+		data_width	=> word_t'length
+	)
+	PORT MAP
+	(
+		op1_in		=> ID_stage.reg_a,
+		op2_in		=> ID_stage.reg_b,
+		flags		=> ID_stage.bcu_flags
+	);
+
+inst_bcu_alt_EX: bcu
+	GENERIC MAP
+	(
+		data_width	=> word_t'length
+	)
+	PORT MAP
+	(
+		op1_in		=> EX_stage.reg_a,
+		op2_in		=> EX_stage.reg_b,
+		flags		=> EX_stage.bcu_flags
+	);
+
+
+proc_stage_branch_alt:
+	process(ID_stage)
 	begin
 		pc.branch_take	<= '0';
-		if EX_stage.ctrl.branch = '1' then
+		if ID_stage.ctrl.branch = '1' then
 
-			case EX_stage.ctrl.bc_src is
+			case ID_stage.ctrl.bc_src is
 
 			when bc_eq_ne =>
-				pc.branch_take <= EX_stage.ctrl.bc_not xor bcu_flags.eq;
+				pc.branch_take <= ID_stage.ctrl.bc_not xor ID_stage.bcu_flags.eq;
 
 			when bc_lez_gtz =>
-				pc.branch_take <= EX_stage.ctrl.bc_not xor (bcu_flags.eq or bcu_flags.ltz);
+				pc.branch_take <= ID_stage.ctrl.bc_not xor (ID_stage.bcu_flags.eq or ID_stage.bcu_flags.ltz);
 
 			when bc_ltz_gez =>
-				pc.branch_take <= EX_stage.ctrl.bc_not xor bcu_flags.ltz;
+				pc.branch_take <= ID_stage.ctrl.bc_not xor ID_stage.bcu_flags.ltz;
 
 			when others => null;
 			end case;
 		end if;	
 	end process;
 
-proc_stage_branch_taken:
+proc_stage_branch_not:
-	process(clk)
+	process(EX_stage)
 	begin
-		if rising_edge(clk) then
+		EX_stage.branch_not_taken	<= '0';
-			if branch_taken = '0' then
+
-				if sdu.ID_stall = '1' or c0_ctrl_in.exc_inject = '1' then
+		case EX_stage.ctrl.bc_src is
-					if pc.branch_take = '1' then
+
-						branch_taken	<= '1';
+		when bc_eq_ne =>
-					end if;
+			EX_stage.branch_not_taken <= EX_stage.ctrl.branch and not(EX_stage.ctrl.bc_not xor EX_stage.bcu_flags.eq);
-				end if;
+
-			elsif sdu.ID_stall = '0' then
+		when bc_lez_gtz =>
-				branch_taken	<= '0';
+			EX_stage.branch_not_taken <= EX_stage.ctrl.branch and not(EX_stage.ctrl.bc_not xor (EX_stage.bcu_flags.eq or EX_stage.bcu_flags.ltz));
+
+		when bc_ltz_gez =>
+			EX_stage.branch_not_taken <= EX_stage.ctrl.branch and not(EX_stage.ctrl.bc_not xor EX_stage.bcu_flags.ltz);
+
+		when others => null;
+		end case;
+	end process;
+
+-- proc_stage_pc_next_alt:
+	process(ctrl_in, sdu, pc, ID_stage, EX_stage)
+	begin
+--		pc.nxt	<= ctrl_in.exc_vec;
+--		if ctrl_in.exc_inject = '0' then
+			pc.nxt	<= pc.plus4;
+			if ID_stage.ctrl.jump = '1' then
+				pc.nxt	<= ID_stage.jimm32;
+			elsif ID_stage.ctrl.jump_long = '1' then
+				pc.nxt	<= ID_stage.reg_a;
+			elsif pc.branch_take = '1' then
+				pc.nxt	<= pc.pc_branch;
 			end if;
+--		end if;
+	end process;
+
+-- always branch in ID and revert in EX
+-- proc_stage_pc_next_alt:
+-- 	process(ctrl_in, sdu, ID_stage, EX_stage, pc)
+-- 	begin
+-- 		if EX_stage.branch_not_taken = '1' then
+-- 			pc.nxt	<= pc.pc_branch_revert;
+-- 		else
+-- 			pc.nxt	<= pc.plus4;
+-- 			if ID_stage.ctrl.jump = '1' then
+-- 				pc.nxt	<= ID_stage.jimm32;
+-- 			elsif ID_stage.ctrl.jump_long = '1' then
+-- 				pc.nxt	<= ID_stage.reg_a;
+-- 			elsif ID_stage.ctrl.branch = '1' then	-- always take branch
+-- 				pc.nxt	<= pc.pc_branch;
+-- 			end if;
+-- 		end if;
+-- 	end process;
+
+proc_stage_pc_plus_4_alt:
+	process(clk_2)
+	begin
+		if rising_edge(clk_2) then
+			if rst = '1' then
+				pc.plus4		<= ctrl_in.exc_vec;
+			elsif cpu_run = '1' then
+				if ctrl_in.exc_inject = '1' then
+					pc.plus4		<= ctrl_in.exc_vec;
+				else
+					pc.plus4		<= pc.curr + 4;
+				end if;
+			end if;	
 		end if;	
 	end process;
 
-proc_stage_pc_branch:
+proc_stage_pc_branch_alt:
-	process(clk)
+	process(clk_2)
 	begin
-		if rising_edge(clk) then
+		if rising_edge(clk_2) then
-			if sdu.ID_stall = '0' and ID_stage.ctrl.branch = '1' then
+			if cpu_run = '1' then
 				pc.pc_branch	<= pc.curr + ID_stage.bimm18;
 			end if;	
 		end if;	
 	end process;
 
-proc_stage_pc:
+proc_stage_pc_curr_alt:
-	process(pc, branch_taken)
+	process(clk_1)
 	begin
-		if pc.branch_take = '1' or branch_taken = '1' then
+		if rising_edge(clk_1) then
-			pc.curr	<= pc.pc_branch;
+			if rst = '1' then
-		else
+				pc.curr			<= ctrl_in.exc_vec;
-			pc.curr	<= pc.nxt;
+				pc.last			<= ctrl_in.exc_vec;
+			elsif cpu_run = '1' and (sdu.ID_stall = '0' or ctrl_in.exc_inject = '1') then
+				pc.last			<= pc.curr;
+				pc.curr			<= pc.nxt;
+			end if;	
 		end if;	
 	end process;
 
+proc_stage_pc_revert_alt:
+	process(clk_1)
+	begin
+		if rising_edge(clk_1) then
+			if cpu_run = '1' and (sdu.ID_stall = '0' or ctrl_in.exc_inject = '1') then
+				if ID_stage.ctrl.branch = '1' then
+					pc.pc_branch_revert	<= pc.plus4;
+				end if;
+			end if;	
+		end if;	
+	end process;
+
+proc_stage_branch_not_taken_alt:
+	process(clk_1)
+	begin
+		if rising_edge(clk_1) then
+			if cpu_run = '1' and (sdu.ID_stall = '0' or ctrl_in.exc_inject = '1') then
+				pc.branch_not_taken <= EX_stage.branch_not_taken;
+			end if;	
+		end if;	
+	end process;
 
 --------------------------------------------------------------------------
 -- ID stage
 --------------------------------------------------------------------------
-	ID_stage.IR					<= to_01(imem_data);
+	ID_stage.IR					<= to_01(icache_qry_in.data);
 	ID_stage.pcn				<= pc.curr;
 	ID_stage.op					<= decode_op(ID_stage.IR);
 	ID_stage.jimm32				<= extract_jimm32(ID_stage.IR, ID_stage.pcn);
@@ -346,13 +438,25 @@ proc_stage_pc:
 	ID_stage.epc				<= pc.last;
 	ID_stage.exc				<= event_is_active(ID_stage.events); -- Todo: works
 	ID_stage.nop				<= sdu.ID_nop;
+	ID_stage.va					<= ID_stage.reg_a + extract_simm32(ID_stage.IR);
 	
+ID_stage_tlb_query_results:
+	process(clk_1)
+	begin
+		if rising_edge(clk_1) then
+			ID_stage.itlb_qry	<= itlb_qry_in;
+		end if;	
+	end process;
+
 proc_ID_except:
-	process(ID_stage, c0_ctrl_in, pc)
+	process(ID_stage, ctrl_in, pc)
 	begin	
 		ID_stage.events					<= events_clr;
-		ID_stage.events.inst_load_err	<= not c0_ctrl_in.exc_pending and (pc.nxt(1) or pc.nxt(0));
+		ID_stage.events.inst_load_err	<= not ctrl_in.exc_pending and (ID_stage.pcn(1) or ID_stage.pcn(0));
-		ID_stage.events.inst_priv_addr	<= not c0_ctrl_in.exc_pending and (pc.nxt(word_t'left) and c0_ctrl_in.user_mode);
+		ID_stage.events.inst_priv_addr	<= not ctrl_in.exc_pending and (ID_stage.pcn(word_t'left) and ctrl_in.user_mode);
+		ID_stage.events.ITLB_LOAD		<= not ctrl_in.exc_pending and ID_stage.itlb_qry.vld and ID_stage.itlb_qry.miss and not ID_stage.itlb_qry.write;
+		ID_stage.events.ITLB_STORE		<= not ctrl_in.exc_pending and ID_stage.itlb_qry.vld and ID_stage.itlb_qry.miss and ID_stage.itlb_qry.write;
+		ID_stage.events.ITLB_MOD		<= not ctrl_in.exc_pending and ID_stage.itlb_qry.vld and ID_stage.itlb_qry.dirty;
 	end process;
 	
 proc_stage_hdu:
@@ -366,19 +470,19 @@ proc_stage_hdu:
 		
 		reg_ptr_a			:= ID_stage.reg_a_rptr;
 		reg_ptr_b			:= ID_stage.reg_b_rptr;
-		raw_a_EX			:= reg_ptr_a = EX_stage.reg_wptr and EX_stage.wreg_we = '1';
+		raw_a_EX			:= reg_ptr_a = EX_stage.reg_wptr and EX_stage.reg_we = '1';
-		raw_a_MEM			:= reg_ptr_a = MEM_stage.reg_wptr and MEM_stage.wreg_we = '1';
+		raw_a_MEM			:= reg_ptr_a = MEM_stage.reg_wptr and MEM_stage.reg_we = '1';
-		raw_a_WB			:= reg_ptr_a = WB_stage.reg_wptr and WB_stage.wreg_we = '1';
+		raw_a_WB			:= reg_ptr_a = WB_stage.reg_wptr and WB_stage.reg_we = '1';
-		raw_b_EX			:= reg_ptr_b = EX_stage.reg_wptr and EX_stage.wreg_we = '1';
+		raw_b_EX			:= reg_ptr_b = EX_stage.reg_wptr and EX_stage.reg_we = '1';
-		raw_b_MEM			:= reg_ptr_b = MEM_stage.reg_wptr and MEM_stage.wreg_we = '1';
+		raw_b_MEM			:= reg_ptr_b = MEM_stage.reg_wptr and MEM_stage.reg_we = '1';
-		raw_b_WB			:= reg_ptr_b = WB_stage.reg_wptr and WB_stage.wreg_we = '1';
+		raw_b_WB			:= reg_ptr_b = WB_stage.reg_wptr and WB_stage.reg_we = '1';
 
-		hdu.alu_fwd_a_ex	<= raw_a_EX;
+		hdu.alu_fwd_a_ex	<= raw_a_EX after 1 ns;
-		hdu.alu_fwd_a_mem	<= raw_a_MEM;
+		hdu.alu_fwd_a_mem	<= raw_a_MEM after 1 ns;
-		hdu.alu_fwd_a_wb	<= raw_a_WB;
+		hdu.alu_fwd_a_wb	<= raw_a_WB after 1 ns;
-		hdu.alu_fwd_b_ex	<= raw_b_EX;
+		hdu.alu_fwd_b_ex	<= raw_b_EX after 1 ns;
-		hdu.alu_fwd_b_mem	<= raw_b_MEM;
+		hdu.alu_fwd_b_mem	<= raw_b_MEM after 1 ns;
-		hdu.alu_fwd_b_wb	<= raw_b_WB;
+		hdu.alu_fwd_b_wb	<= raw_b_WB after 1 ns;
 
 	end process;
 	
@@ -390,18 +494,12 @@ proc_stage_fwd_a:
 		if hdu.alu_fwd_a_ex then
 			data	:= EX_stage.result;
 		elsif hdu.alu_fwd_a_mem then
-			data 	:= MEM_stage.data;
+			data 	:= MEM_stage.result;
 		elsif hdu.alu_fwd_a_wb then
-			data 	:= WB_stage.data;
+			data 	:= WB_stage.result;
 		end if;
 		
-		for i in 0 to 31 loop
+		ID_stage.reg_a		<= to_mips_01(data) after 1 ns;
-			if data(i) = '1' then
-				ID_stage.reg_a(i)	<= '1';
-			else 
-				ID_stage.reg_a(i)	<= '0';
-			end if;
-		end loop;
 
 	end process;
 
@@ -413,18 +511,12 @@ proc_stage_fwd_b:
 		if hdu.alu_fwd_b_ex then
 			data	:= EX_stage.result;
 		elsif hdu.alu_fwd_b_mem then
-			data	:= MEM_stage.data;
+			data	:= MEM_stage.result;
 		elsif hdu.alu_fwd_b_wb then
-			data	:= WB_stage.data;
+			data	:= WB_stage.result;
 		end if;
 
-		for i in 0 to 31 loop
+		ID_stage.reg_b		<= to_mips_01(data) after 1 ns;
-			if data(i) = '1' then
-				ID_stage.reg_b(i)	<= '1';
-			else 
-				ID_stage.reg_b(i)	<= '0';
-			end if;
-		end loop;
 
 	end process;
 	
@@ -449,7 +541,7 @@ proc_imm_mux:
 
 		end case;
 
-		ID_stage.imm <= data;
+		ID_stage.imm <= data after 1 ns;
 
 	end process;
 
@@ -462,11 +554,11 @@ inst_reg_dual: reg_dual
 	)
 	PORT MAP
 	(
-		clk_w		=> clk,
+		clk_w		=> clk_1,
-		we			=> WB_stage.wreg_we,
+		we			=> WB_stage.reg_we,
 		en			=> '1',
 		wptr		=> WB_stage.reg_wptr,
-		din			=> WB_stage.data,
+		din			=> WB_stage.result,
 		rptr_a		=> ID_stage.reg_a_rptr,
 		rptr_b		=> ID_stage.reg_b_rptr,
 		dout_a		=> reg_a,
@@ -479,13 +571,13 @@ inst_reg_dual: reg_dual
 	EX_stage.reg_a_rptr		<= extract_rs(EX_stage.IR);
 	EX_stage.reg_b_rptr		<= extract_rt(EX_stage.IR);
 	EX_stage.result			<= alu_result when EX_stage.ctrl.mul_access = '0' else mul_result;
+	EX_stage.dtlb_qry		<= dtlb_qry_in;
 	
 proc_stage_ID_EX_1:
-	process(clk)
+	process(clk_1)
 	begin
-		if rising_edge(clk) then
+		if rising_edge(clk_1) then
-			if rst = '1' then
+			if stage_rst(1) = '1' then
-				EX_stage.nop					<= '1';
 				EX_stage.op						<= op_nop;
 				EX_stage.IR						<= (others => '0');
 				EX_stage.ctrl					<= ctrl_lines_default;
@@ -493,22 +585,20 @@ proc_stage_ID_EX_1:
 				EX_stage.epc					<= (others => '0');
 				EX_stage.pcn					<= (others => '0');
 				EX_stage.events_in				<= events_clr;
-				EX_stage.reg_a			<= (others => '0');
-				EX_stage.reg_b			<= (others => '0');
 			elsif sdu.EX_stall = '0' then
-				EX_stage.va				<= vaddr;
 				EX_stage.nop			<= sdu.EX_nop;
+				EX_stage.op				<= ID_stage.op;
+				EX_stage.IR				<= ID_stage.IR;
+				EX_stage.va				<= ID_stage.va;
+				
 				EX_stage.reg_a			<= ID_stage.reg_a;
 				EX_stage.reg_b			<= ID_stage.reg_b;
 				EX_stage.events_in		<= ID_stage.events;
-				EX_stage.op				<= ID_stage.op;
 				EX_stage.ctrl			<= ID_stage.ctrl;
 				EX_stage.reg_write		<= ID_stage.reg_write;
-				EX_stage.IR				<= ID_stage.IR;
 				EX_stage.pcn			<= ID_stage.pcn;
 				EX_stage.epc			<= ID_stage.epc;
 				if sdu.EX_nop = '1' then
-					EX_stage.op				<= op_nop;
 					EX_stage.IR				<= (others => '0');
 					EX_stage.ctrl			<= ctrl_lines_default;
 					EX_stage.reg_write		<= '0';
@@ -526,6 +616,15 @@ proc_stage_EX_instr_except:
 		EX_events_instr.syscall			<= EX_stage.ctrl.exc_syscall;
 	end process;
 
+proc_stage_EX_DTLB_except:
+	process(EX_stage)
+	begin
+		EX_events_DTLB					<= events_clr;
+		EX_events_DTLB.DTLB_LOAD		<= EX_stage.dtlb_qry.vld and EX_stage.dtlb_qry.miss and not EX_stage.dtlb_qry.write;
+		EX_events_DTLB.DTLB_STORE		<= EX_stage.dtlb_qry.vld and EX_stage.dtlb_qry.miss and EX_stage.dtlb_qry.write;
+		EX_events_DTLB.DTLB_MOD			<= EX_stage.dtlb_qry.vld and EX_stage.dtlb_qry.dirty;
+	end process;
+
 proc_stage_EX_alu_except:
 	process(EX_stage)
 	begin
@@ -540,30 +639,28 @@ proc_stage_EX_alu_except:
 		end if;	
 	end process;
 
-	vaddr			<= ID_stage.reg_a + extract_simm32(ID_stage.IR);
-	
 proc_stage_EX_mem_except:
-	process(clk)
+	process(clk_1)
 	begin
-		if rising_edge(clk) then
+		if rising_edge(clk_1) then
-			if rst = '1' then
+			if pipe_rst = '1' then
-				dmem_en		<= '0';
+				EX_stage.dmem_en		<= '0';
 			elsif sdu.EX_stall = '0' then
 				EX_events_mem	<= events_clr;
-				dmem_en		<= '0';
+				EX_stage.dmem_en		<= '0';
 				if ID_stage.ctrl.dmem_en = '1' then
-					dmem_en		<= '1';
+					EX_stage.dmem_en		<= '1';
 					if ID_stage.ctrl.except_en = '1' then
 						if ID_stage.ctrl.word2_en = '1' then
-							if vaddr(0) = '1' then
+							if ID_stage.va(0) = '1' then
 								EX_events_mem.data_load_err		<= not ID_stage.ctrl.dmem_we;
 								EX_events_mem.data_store_err	<= ID_stage.ctrl.dmem_we;
-								dmem_en							<= '0';
+								EX_stage.dmem_en							<= '0';
 							end if;
-						elsif vaddr(1 downto 0) /= "00" then
+						elsif ID_stage.va(1 downto 0) /= "00" then
 							EX_events_mem.data_load_err		<= not ID_stage.ctrl.dmem_we;
 							EX_events_mem.data_store_err	<= ID_stage.ctrl.dmem_we;
-							dmem_en							<= '0';
+							EX_stage.dmem_en							<= '0';
 						end if;	
 					end if;
 				end if;
@@ -572,28 +669,23 @@ proc_stage_EX_mem_except:
 	end process;
 
 proc_stage_DMEM_ADDR:
-	process(clk)
+	process(clk_1)
 	begin
-		if rising_edge(clk) then
+		if rising_edge(clk_1) then
 			if sdu.EX_stall = '0' then
-				if c0_ctrl_in.EB = '1' then
+				if ctrl_in.EB = '1' then
 					if ID_stage.ctrl.word2_en = '0' then
-						EX_stage.pa_off	<= not vaddr(1 downto 0);
+						EX_stage.pa_off	<= not ID_stage.va(1 downto 0);
 					else	
-						EX_stage.pa_off	<= not vaddr(1) & vaddr(0);
+						EX_stage.pa_off	<= not ID_stage.va(1) & ID_stage.va(0);
 					end if;
 				else	
-					EX_stage.pa_off	<= vaddr(1 downto 0);
+					EX_stage.pa_off	<= ID_stage.va(1 downto 0);
 				end if;
 			end if;
 		end if;
 	end process;
-	dmem_src	<= cop_din when EX_stage.ctrl.cop_instr_en = '1' else EX_stage.reg_b;
+	EX_stage.events			<= EX_events_DTLB or EX_events_instr or EX_events_mem or EX_events_alu or EX_stage.events_in;
-	dmem_be		<= store_be(EX_stage.pa_off, EX_stage.ctrl.dmem_en, EX_stage.ctrl.word2_en, EX_stage.ctrl.word4_en, EX_stage.ctrl.align_left, EX_stage.ctrl.shift_byp);
-	dmem_we		<= EX_stage.ctrl.dmem_we;
-	dmem_dout	<= store_shift(dmem_src, EX_stage.pa_off, EX_stage.ctrl.shift_offset, EX_stage.ctrl.shift_byp);
-	dmem_addr	<= EX_stage.va;
-	EX_stage.events			<= EX_events_instr or EX_events_mem or EX_events_alu or EX_stage.events_in;
 	EX_stage.exc			<= event_is_active(EX_stage.events);
 	
 --------------------------------------------------------------------------
@@ -611,19 +703,19 @@ proc_wptr_mux:
 			reg_wptr					:= extract_rt(EX_stage.IR);
 		end case;
 		
-		EX_stage.wreg_we <= EX_stage.reg_write;
+		EX_stage.reg_we		<= EX_stage.reg_write after 1 ns;
 		if reg_wptr = "00000" then
-			EX_stage.wreg_we <= '0';
+			EX_stage.reg_we <= '0' after 1 ns;
 		end if;	
 
-		EX_stage.reg_wptr	<= reg_wptr;
+		EX_stage.reg_wptr	<= reg_wptr after 1 ns;
 		case EX_stage.ctrl.wptr_srcsel is
 			when wptr_src_imm =>
-				EX_stage.reg_wptr	<= reg_wptr;
+				EX_stage.reg_wptr	<= reg_wptr after 1 ns;
 
 			when wptr_src_const =>
-				EX_stage.reg_wptr	<= to_unsigned(31, reg_ptr_t'length);
+				EX_stage.reg_wptr	<= to_unsigned(31, reg_ptr_t'length) after 1 ns;
-				EX_stage.wreg_we <= '1';
+				EX_stage.reg_we <= '1' after 1 ns;
 
 			when others => null;
 		end case;
@@ -631,76 +723,62 @@ proc_wptr_mux:
 	end process;
 
 --------------------------------------------------------------------------
-proc_stage_bcu_op:
-	process(clk)
-	begin
-		if rising_edge(clk) then
-			if sdu.EX_stall = '0' then
-				bcu_op_a		<= ID_stage.reg_a;
-				bcu_op_b		<= ID_stage.reg_b;
-			end if;
-		end if;
-	end process;
 
 EX_stage.alu_op1 <= EX_stage.reg_a;
 
 alu_op2_mux:
-	process(clk)
+	process(clk_1)
 	variable data : word_t;
 	begin
 
-		if rising_edge(clk) then
+		if rising_edge(clk_1) and sdu.EX_stall = '0' then
-			if sdu.EX_stall = '0' then
+			data := ID_stage.reg_b;
+
+			case ID_stage.ctrl.alu.op2_src is
+
+			when alu_src_reg =>
 				data := ID_stage.reg_b;
 
-				case ID_stage.ctrl.alu.op2_src is
+			when alu_src_imm =>
+				data := ID_stage.imm;
 
-				when alu_src_reg =>
+			when others => null;
-					data := ID_stage.reg_b;
 
-				when alu_src_imm =>
+			end case;
-					data := ID_stage.imm;
 
-				when others => null;
+			EX_stage.alu_op2 <= data;
-
-				end case;
-
-				EX_stage.alu_op2 <= data;
-			end if;
 		end if;
 
 	end process;
 
 shifter_sa_mux:
-	process(clk)
+	process(clk_1)
 	variable data 		: shamt_t;
 	variable data_inv 	: shamt_t;
 
 	begin
-		if rising_edge(clk) then 
+		if rising_edge(clk_1) and sdu.EX_stall = '0' then
-			if sdu.EX_stall = '0' then
+			data 		:= ID_stage.reg_a(4 downto 0);
-				data 		:= ID_stage.reg_a(4 downto 0);
+			case ID_stage.ctrl.shamt2_srcsel is
-				case ID_stage.ctrl.shamt2_srcsel is
 
-					when sa_src_reg =>
+				when sa_src_reg =>
-						data := ID_stage.reg_a(4 downto 0);
+					data := ID_stage.reg_a(4 downto 0);
 
-					when sa_src_imm =>
+				when sa_src_imm =>
-						data := ID_stage.shamt;
+					data := ID_stage.shamt;
 
-					when others => null;
+				when others => null;
 
-				end case;
+			end case;
-				data_inv := not data + 1;
+			data_inv := not data + 1;
-				if ID_stage.ctrl.alu.shift_right = '0' then
+			if ID_stage.ctrl.alu.shift_right = '0' then
-					EX_stage.shift_ctrl.shamt_rnd	<= data_inv;
+				EX_stage.shift_ctrl.shamt_rnd	<= data_inv after 1 ns;
-				else	
+			else	
-					EX_stage.shift_ctrl.shamt_rnd	<= data;
+				EX_stage.shift_ctrl.shamt_rnd	<= data after 1 ns;
-				end if;
-				EX_stage.shift_ctrl.shamt_nrm	<= data;
-				EX_stage.shift_ctrl.shift_right	<= ID_stage.ctrl.alu.shift_right;
-				EX_stage.shift_ctrl.shift_arith	<= ID_stage.ctrl.alu.shift_arith;
 			end if;
+			EX_stage.shift_ctrl.shamt_nrm	<= data after 1 ns;
+			EX_stage.shift_ctrl.shift_right	<= ID_stage.ctrl.alu.shift_right;
+			EX_stage.shift_ctrl.shift_arith	<= ID_stage.ctrl.alu.shift_arith;
 		end if;
 		
 	end process;
@@ -733,43 +811,31 @@ inst_alu: alu
 		flags			=> EX_stage.alu_flags
 	);
 
-inst_bcu: bcu
-	GENERIC MAP
-	(
-		data_width	=> word_t'length
-	)
-	PORT MAP
-	(
-		op1_in		=> bcu_op_a,
-		op2_in		=> bcu_op_b,
-		flags		=> bcu_flags
-	);
-
 --------------------------------------------------------------------------
 -- MEM stage
 --------------------------------------------------------------------------
 proc_stage_MEM_n:
-	process(clk)
+	process(clk_1)
 	begin
-		if rising_edge(clk) then
+		if rising_edge(clk_1) then
-			if rst = '1' then
+			if stage_rst(2) = '1' then
-				MEM_stage.nop			<= '1';
+				MEM_stage.op		<= op_nop;
-				MEM_stage.op			<= op_nop;
+				MEM_stage.reg_we	<= '0';
-				MEM_stage.va			<= (others => '0');
+				MEM_stage.ctrl		<= ctrl_lines_default;
-				MEM_stage.pcn			<= (others => '0');
+				MEM_stage.pa_off	<= (others => '0');
-				MEM_stage.ex_result	<= (others => '0');
-				MEM_stage.epc			<= (others => '0');
-				MEM_stage.wreg_we		<= '0';
-				MEM_stage.reg_wptr	<= (others => '0');
-				MEM_stage.ctrl			<= ctrl_lines_default;
-				MEM_stage.pa_off		<= (others => '0');
 				MEM_stage.events_in	<= events_clr;
 			elsif sdu.MEM_stall = '0' then
 				MEM_stage.nop			<= sdu.MEM_nop;
-				MEM_stage.events_in		<= EX_stage.events;
+				MEM_stage.reg_a			<= EX_stage.reg_a;
+				MEM_stage.reg_b			<= EX_stage.reg_b;
+				MEM_stage.result_in		<= EX_stage.result;
 				MEM_stage.op			<= EX_stage.op;
-				MEM_stage.wreg_we		<= EX_stage.wreg_we;
+				MEM_stage.epc			<= EX_stage.epc;
+				MEM_stage.pcn			<= EX_stage.pcn;
+				MEM_stage.events_in		<= EX_stage.events;
+				MEM_stage.reg_we		<= EX_stage.reg_we;
 				MEM_stage.ctrl			<= EX_stage.ctrl;
+				MEM_stage.branch_not_taken	<= EX_stage.branch_not_taken;
 				if EX_stage.ctrl.dmem_en = '1' then
 					MEM_stage.va			<= EX_stage.va;
 				end if;	
@@ -777,44 +843,40 @@ proc_stage_MEM_n:
 				MEM_stage.reg_wptr		<= EX_stage.reg_wptr;
 				if MEM_stage.reg_wptr = EX_stage.reg_wptr then
 					if (EX_stage.ctrl.dmem_en and MEM_stage.ctrl.dmem_en) = '1' then
-						MEM_stage.ex_result	<= MEM_stage.data;
+						MEM_stage.result_in	<= MEM_stage.result;
 					end if;
 				else
-					MEM_stage.ex_result		<= EX_stage.reg_b;
+					MEM_stage.result_in		<= EX_stage.reg_b;
 				end if;
 				if EX_stage.ctrl.cop_instr_en = '1' then
 					if EX_stage.ctrl.cop_read = '1' then
-						MEM_stage.ex_result	<= cop_din;
+						MEM_stage.result_in	<= cop_din;
 					end if;	
 				elsif EX_stage.ctrl.dmem_en = '0' then
-					MEM_stage.ex_result		<= EX_stage.result;
+					MEM_stage.result_in		<= EX_stage.result;
 				end if;	
 				if sdu.MEM_nop = '1' then
-					MEM_stage.op		<= op_nop;
+					MEM_stage.reg_we	<= '0';
-					MEM_stage.wreg_we	<= '0';
 					MEM_stage.ctrl		<= ctrl_lines_default;
-				else
-					MEM_stage.pcn			<= EX_stage.pcn;
-					MEM_stage.epc			<= EX_stage.epc;
 				end if;
 			end if;	
 		end if;
 	end process;
 
 proc_stage_MEM_mux:
-	process(MEM_stage, dmem_din)
+	process(MEM_stage, dcache_qry_in.data)
 	variable temp1 : word_t;
 	variable temp2 : word_t;
 	variable data : word_t;
 	variable be : unsigned(3 downto 0);
 	begin
-		data	:= MEM_stage.ex_result;
+		data	:= MEM_stage.result_in;
-		temp1		:= load_shift(dmem_din,  MEM_stage.pa_off, MEM_stage.ctrl.shift_offset, MEM_stage.ctrl.shift_byp);
-		temp2		:= load_sign_ext(temp1, MEM_stage.ctrl.sign_ext_byp, MEM_stage.ctrl.load_signed, MEM_stage.ctrl.word2_en, MEM_stage.ctrl.word4_en);
-		be			:= load_be(MEM_stage.pa_off, MEM_stage.ctrl.align_left, MEM_stage.ctrl.byte_en_byp);
 		if MEM_stage.ctrl.reg_link = '1' then
 			data		:= MEM_stage.epc + 8;
 		elsif MEM_stage.ctrl.dmem_en = '1' then
+			temp1		:= load_shift(dcache_qry_in.data,  MEM_stage.pa_off, MEM_stage.ctrl.shift_offset, MEM_stage.ctrl.shift_byp);
+			temp2		:= load_sign_ext(temp1, MEM_stage.ctrl.sign_ext_byp, MEM_stage.ctrl.load_signed, MEM_stage.ctrl.word2_en, MEM_stage.ctrl.word4_en);
+			be			:= load_be(MEM_stage.pa_off, MEM_stage.ctrl.align_left, MEM_stage.ctrl.byte_en_byp);
 			if be(0) = '1' then
 				data(7 downto 0)	:= temp2(7 downto 0);
 			end if;	
@@ -828,16 +890,16 @@ proc_stage_MEM_mux:
 				data(31 downto 24)	:= temp2(31 downto 24);
 			end if;	
 		end if;
-		MEM_stage.data				<= data;
+		MEM_stage.result			<= data after 1 ns;
 		
 	end process;
 
 proc_stage_MEM_except:
-	process(MEM_stage, c0_ctrl_in)
+	process(MEM_stage, ctrl_in)
 	begin
 		MEM_stage.events				<= MEM_stage.events_in;
-		MEM_stage.events.Int			<= c0_ctrl_in.int;
+		MEM_stage.events.Int			<= ctrl_in.int;
-		MEM_stage.events.NMI			<= c0_ctrl_in.NMI;
+		MEM_stage.events.NMI			<= ctrl_in.NMI;
 	end process;
 
 	MEM_stage.exc	<= event_is_active(MEM_stage.events);
@@ -846,34 +908,50 @@ proc_stage_MEM_except:
 -- WB stage
 --------------------------------------------------------------------------
 proc_stage_WB_p:
-	process(clk)
+	process(clk_1)
+	variable branch_delay : std_logic;
+	
 	begin
-		if rising_edge(clk) then
+		if rising_edge(clk_1) then
-			if rst = '1' then
+			if stage_rst(3) = '1' then
-				WB_stage.nop			<= '1';
 				WB_stage.op				<= op_nop;
-				WB_stage.wreg_we		<= '1';
+				WB_stage.reg_we			<= '1';
 				WB_stage.reg_wptr		<= (others => '0');
-				WB_stage.data			<= (others => '0');
+				WB_stage.epc			<= (others => '0');
-				WB_stage.events		<= events_clr;
+				WB_stage.events			<= events_clr;
-				WB_stage.bd				<= '0';
 				WB_stage.exc			<= '0';
 			elsif sdu.WB_stall = '0' then
 				WB_stage.nop			<= sdu.WB_nop;
+				WB_stage.reg_a			<= MEM_stage.reg_a;
+				WB_stage.reg_b			<= MEM_stage.reg_b;
+				WB_stage.result			<= MEM_stage.result;
 				WB_stage.op				<= MEM_stage.op;
-				WB_stage.wreg_we		<= MEM_stage.wreg_we;
+				WB_stage.epc			<= MEM_stage.epc;
-				WB_stage.reg_wptr		<= MEM_stage.reg_wptr;
+				branch_delay			:= MEM_stage.ctrl.branch or MEM_stage.ctrl.jump or MEM_stage.ctrl.jump_long; -- Todo: works
-				WB_stage.data			<= MEM_stage.data;
+				WB_stage.bd				<= branch_delay;
+				WB_stage.exc_last		<= WB_stage.exc;
 				WB_stage.exc			<= MEM_stage.exc;
-				if MEM_stage.exc = '1' then
+				WB_stage.reg_we			<= MEM_stage.reg_we;
-					WB_stage.events			<= MEM_stage.events;
+				WB_stage.reg_wptr		<= MEM_stage.reg_wptr;
+				WB_stage.events			<= MEM_stage.events;
+				if sdu.stall_all = '0' then
+					if MEM_stage.exc = '1' and WB_stage.exc = '0' then
+						ctrl_out.dmem_addr		<= MEM_stage.va;
+						ctrl_out.imem_addr		<= MEM_stage.epc;
+						ctrl_out.epc_wb			<= MEM_stage.epc;
+						ctrl_out.bd_wb			<= WB_stage.bd;
+						ctrl_out.events			<= MEM_stage.events;
+						if WB_stage.bd = '1' then
+							ctrl_out.epc_wb			<= WB_stage.epc;
+						end if;
+					end if;	
 				end if;	
 				if sdu.WB_nop = '1' then
-					WB_stage.op				<= op_nop;
+					WB_stage.reg_we		<= '0';
-					WB_stage.wreg_we		<= '0';
-				else
-					WB_stage.bd				<= MEM_stage.ctrl.branch or MEM_stage.ctrl.jump or MEM_stage.ctrl.jump_long; -- Todo: works
 				end if;	
+				if WB_stage.exc = '0' then
+					WB_stage.va				<= MEM_stage.va;
+				end if;
 			end if;	
 		end if;	
 	end process;

lib/CPUs/MIPS/src/core/mips_shifter.vhd

@@ -30,37 +30,37 @@ use work.mips_types.all;
 entity shifter is
 	Generic 
 	(
-		data_width : integer := 8
+		DATA_WIDTH : integer := 8
 	);
 	Port
 	(
 		shift_ctrl	: in  shift_ctrl_t;
-		din			: in  unsigned (data_width-1 downto 0);
+		din			: in  unsigned (DATA_WIDTH-1 downto 0);
-		dout		: out unsigned (data_width-1 downto 0)
+		dout		: out unsigned (DATA_WIDTH-1 downto 0)
 	);
 end shifter;
 
 architecture Behavioral of shifter is
 
-	subtype word_t is unsigned(data_width-1 downto 0);
+	subtype word_t is unsigned(DATA_WIDTH-1 downto 0);
 	type word_array_t is array (0 to 5) of word_t;
 
 	signal rot_data			: word_array_t;
 	signal sa_rnd			: unsigned (4 downto 0);
 	signal fill	: std_logic;
 	
-	type fill_mask_rom_t is array (0 to 2*data_width-1) of unsigned(data_width-1 downto 0);
+	type fill_mask_rom_t is array (0 to 2*DATA_WIDTH-1) of unsigned(DATA_WIDTH-1 downto 0);
 --------------------------------------------------------------------------
-function gen_fill_mask_rom(data_width_arg : natural) return fill_mask_rom_t is
+function gen_fill_mask_rom(width : natural) return fill_mask_rom_t is
 	variable result : fill_mask_rom_t;
 	begin
-	    result(0) := (data_width_arg-1 downto 0 => '0');
+	    result(0) := (width-1 downto 0 => '0');
-		for i in 1 to data_width_arg-1 loop
+		for i in 1 to width-1 loop
-			result(i) := (data_width_arg-1-i downto 0 => '0') & (i-1 downto 0 => '1');
+			result(i) := (width-1-i downto 0 => '0') & (i-1 downto 0 => '1');
 		end loop;	
-	    result(data_width_arg) := (data_width_arg-1 downto 0 => '0');
+	    result(width) := (width-1 downto 0 => '0');
-		for i in 1 to data_width_arg-1 loop
+		for i in 1 to width-1 loop
-			result(data_width_arg+i) := (i-1 downto 0 => '1') & (data_width_arg-1-i downto 0 => '0');
+			result(width+i) := (i-1 downto 0 => '1') & (width-1-i downto 0 => '0');
 		end loop;	
 	return result;
 	
@@ -79,10 +79,10 @@ function rot_stage(x : unsigned; en : std_logic; stage_num : natural) return uns
 	
 end rot_stage;
 
-	constant fill_mask_rom	: fill_mask_rom_t := gen_fill_mask_rom(data_width);
+	constant fill_mask_rom	: fill_mask_rom_t := gen_fill_mask_rom(DATA_WIDTH);
-	signal fill_mask		: unsigned(data_width-1 downto 0);
+	signal fill_mask		: unsigned(DATA_WIDTH-1 downto 0);
 	signal fill_mask_addr	: unsigned(5 downto 0);
-	signal dout_filled		: unsigned (data_width-1 downto 0);
+	signal dout_filled		: unsigned (DATA_WIDTH-1 downto 0);
 	
 --------------------------------------------------------------------------
 begin
@@ -96,7 +96,7 @@ gen_rotate_right:
 	end generate;
 	
 	sa_rnd			<= shift_ctrl.shamt_rnd;
-	fill			<= shift_ctrl.shift_arith and din(data_width-1);
+	fill			<= shift_ctrl.shift_arith and din(DATA_WIDTH-1);
 	fill_mask_addr	<= shift_ctrl.shift_right & shift_ctrl.shamt_nrm;
 	fill_mask		<= fill_mask_rom(to_integer(fill_mask_addr));
 
@@ -110,7 +110,7 @@ proc_fill_neu:
 		end if;
 	end process;
 	
-	dout	<= dout_filled;
+	dout	<= dout_filled after 5 ns;
 	
 --------------------------------------------------------------------------
 end Behavioral;

lib/CPUs/MIPS/src/core/mips_tlb.vhd

@@ -0,0 +1,340 @@
+--------------------------------------------------------------------------
+-- Project: JIPS, a portable 32-bit RISC CPU written in VHDL
+-- This file: JIPS top file 
+--
+-- Copyright (C) 2008 J. Ahrensfeld
+--
+-- 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, either version 3 of the License, or
+-- (at your option) any later version.
+--
+-- 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/>.
+--
+-- For questions and ideas, please contact the author at jens@jayfield.org
+--
+--------------------------------------------------------------------------
+
+LIBRARY IEEE;
+USE IEEE.STD_LOGIC_1164.ALL;
+USE IEEE.NUMERIC_STD.ALL;
+
+library work;
+use work.mips_types.all;
+use work.mips_util_pkg.all;
+
+ENTITY tlb IS
+	Generic
+	(
+		NUM_ENTRIES			: natural := 8;
+		CACHE_KSEG1			: boolean := false;
+		TRANSLATE_KSEG0_1	: boolean := true;
+		USE_TLB				: boolean := false
+	);
+	Port
+	(
+		rst				: in  STD_LOGIC;
+		clk				: in  STD_LOGIC;
+		ce				: in  STD_LOGIC;
+
+		rdy				: out STD_LOGIC;
+		
+		ctrl_in			: in  tlb_ctrl_in_t;
+		ctrl_out		: out tlb_ctrl_out_t;
+		
+		query_in		: in  tlb_query_in_t;
+		query_out		: out tlb_query_out_t
+
+	);
+END tlb;
+
+ARCHITECTURE behavior OF tlb IS
+
+	signal clk_rd					: STD_LOGIC;
+	signal blk_hit					: unsigned(NUM_ENTRIES-1 downto 0);
+	signal blk_entry_lo				: tlb_entry_lo_t;
+	
+	constant C_ENTRY_LO_DEFAULT		: tlb_entry_lo_t := 
+	(
+		N => '0', D => '0', V => '1', G => '0', PFN => (others => '0')
+	);
+
+	signal kseg1_force_cached		: STD_LOGIC;
+	signal kseg_0_1_pfn				: unsigned (31 downto lg2(TLB_PAGE_SIZE));
+	signal hit_umc					: STD_LOGIC;
+	signal hit_umuc					: STD_LOGIC;
+	signal entry_lo_res				: tlb_entry_lo_t;
+	signal qry_res					: tlb_query_out_t;
+
+	signal pa						: word_t;
+	signal hit						: STD_LOGIC;
+	signal vld_r					: STD_LOGIC;
+
+	signal cam_rdy					: STD_LOGIC;
+	signal cam_hit_vld				: STD_LOGIC;
+
+	signal vaddr_reg				: word_t;
+	signal write_reg				: STD_LOGIC;
+
+	subtype reg_lo_t is unsigned(23 downto 0);
+	subtype reg_hi_t is unsigned(25 downto 0);
+	
+	signal reg_ctrl_entry_lo_din	: reg_lo_t;
+	signal reg_ctrl_entry_lo_dout	: reg_lo_t;
+	signal reg_qry_entry_lo_dout	: reg_lo_t;
+	signal reg_qry_entry_lo_addr	: unsigned(lg2(NUM_ENTRIES)-1 downto 0);
+	signal reg_ctrl_entry_lo_addr	: unsigned(lg2(NUM_ENTRIES)-1 downto 0);
+
+	signal reg_ctrl_entry_hi_din	: reg_hi_t;
+	signal reg_ctrl_entry_hi_dout	: reg_hi_t;
+
+	function to_entry_lo(x : reg_lo_t) return tlb_entry_lo_t is
+	variable result : tlb_entry_lo_t;
+	begin
+		result.PFN	:= x(23 downto 4);
+		result.N 	:= x(3);
+		result.D	:= x(2);
+		result.V	:= x(1);
+		result.G 	:= x(0);
+		
+		return result;
+		
+	end to_entry_lo;
+	
+	function to_entry_hi(x : reg_hi_t) return tlb_entry_hi_t is
+	variable result : tlb_entry_hi_t;
+	begin
+		result.VPN		:= x(25 downto 6);
+		result.ASID 	:= x(5 downto 0);
+	
+		return result;
+
+	end to_entry_hi;
+
+	function to_reg_lo(x : tlb_entry_lo_t) return reg_lo_t is
+	variable result : reg_lo_t;
+	begin
+		result(23 downto 4)						:= x.PFN;
+		result(3)								:= x.N;
+		result(2)								:= x.D;
+		result(1)								:= x.V;
+		result(0)								:= x.G;
+
+		return result;
+
+	end to_reg_lo;
+
+	function to_reg_hi(x : tlb_entry_hi_t) return reg_hi_t is
+	variable result : reg_hi_t;
+	begin
+		result(25 downto 6)						:= x.VPN;
+		result(5 downto 0)						:= x.ASID;
+		
+		return result;
+		
+	end to_reg_hi;
+
+	signal hit_idx		: integer;
+	function decode(x : unsigned; N : integer) return integer is
+	variable result : integer;
+	begin
+		result := 0;
+		for i in 0 to N-1 loop
+			if x(i) = '1' then
+				result := i;
+			end if;
+		end loop;
+	
+		return result;
+		
+	end decode;
+	
+	function decode2(x : unsigned; N : integer) return unsigned is
+	variable result : unsigned (lg2(N)-1 downto 0);
+	begin
+		result := (others => '0');
+		for i in 0 to N-1 loop
+			result := result or ((lg2(N)-1 downto 0 => x(i)) and to_unsigned(i, lg2(N)));
+		end loop;
+	
+		return result;
+		
+	end decode2;
+
+begin
+	kseg1_force_cached				<= '1' when CACHE_KSEG1 else '0';
+	kseg_0_1_pfn					<= "000" & vaddr_reg(28 downto lg2(TLB_PAGE_SIZE)) when TRANSLATE_KSEG0_1 else vaddr_reg(31 downto lg2(TLB_PAGE_SIZE));
+
+	clk_rd							<= clk;
+	rdy								<= cam_rdy;
+	
+	qry_res.vld						<= vld_r;
+	qry_res.miss					<= not hit;
+	qry_res.dirty					<= hit and qry_res.flags.D and write_reg;
+	qry_res.write					<= write_reg;
+	qry_res.flags.N					<= entry_lo_res.N;
+	qry_res.flags.D					<= entry_lo_res.D;
+	qry_res.flags.V					<= entry_lo_res.V;
+	qry_res.paddr					<= pa;
+	
+	query_out						<= qry_res;
+
+	pa								<= entry_lo_res.PFN & vaddr_reg(lg2(TLB_PAGE_SIZE)-1 downto 0);
+
+	hit_idx							<= decode(blk_hit, NUM_ENTRIES);
+	
+	blk_entry_lo					<= to_entry_lo(to_mips_01(reg_qry_entry_lo_dout));
+--	reg_qry_entry_lo_addr			<= to_unsigned(hit_idx, lg2(NUM_ENTRIES));
+	reg_qry_entry_lo_addr			<= decode2(blk_hit, NUM_ENTRIES);
+	reg_ctrl_entry_lo_din			<= to_reg_lo(ctrl_in.entry_lo);
+	
+	ctrl_out.entry_hi				<= to_entry_hi(to_mips_01(reg_ctrl_entry_hi_dout));
+	reg_ctrl_entry_hi_din			<= to_reg_hi(ctrl_in.entry_hi);
+
+registered_ctrl_out:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if query_in.vld = '1' then
+				vaddr_reg		<= query_in.vaddr;
+				write_reg		<= query_in.write;
+			end if;
+			vld_r	<= query_in.vld;
+			if ce = '1' then
+				if ctrl_in.entry_re = '1' then
+
+					ctrl_out.entry_lo		<= to_entry_lo(to_mips_01(reg_ctrl_entry_lo_dout));
+					ctrl_out.entry_vld		<= '1';
+				end if;
+			end if;
+		end if;
+	end process;
+
+inst_reg_entry_lo: entity work.reg_dual
+	GENERIC MAP
+	(
+		addr_width	=>	lg2(NUM_ENTRIES),
+		data_width	=>	reg_lo_t'length
+	)
+	PORT MAP (
+		clk_w	=>	clk,
+		we		=>	ctrl_in.entry_we,
+		en		=>	'1',
+		wptr	=>	ctrl_in.entry_wr_idx(lg2(NUM_ENTRIES)-1 downto 0),
+		din		=>	reg_ctrl_entry_lo_din,
+		rptr_a	=>	ctrl_in.entry_rd_idx(lg2(NUM_ENTRIES)-1 downto 0),
+		rptr_b	=>	reg_qry_entry_lo_addr,
+		dout_a	=>	reg_ctrl_entry_lo_dout,
+		dout_b	=>	reg_qry_entry_lo_dout
+	);
+
+inst_reg_entry_hi: entity work.dpram_1w1r2c_ra
+	GENERIC MAP 
+	(
+		addr_width	=>	lg2(NUM_ENTRIES),
+		data_width	=>	reg_hi_t'length
+	)
+	PORT MAP
+	(
+		clk_a	=>	clk,
+		clk_b	=>	clk,
+		we_a	=>	ctrl_in.entry_we,
+		re_b	=>	ctrl_in.entry_re,
+		addr_a	=>	ctrl_in.entry_wr_idx(lg2(NUM_ENTRIES)-1 downto 0),
+		addr_b	=>	ctrl_in.entry_rd_idx(lg2(NUM_ENTRIES)-1 downto 0),
+		din_a	=>	reg_ctrl_entry_hi_din,
+		dout_b	=>	reg_ctrl_entry_hi_dout
+	);
+
+inst_cam_va: entity work.cam
+	GENERIC MAP
+	(
+		NUM_ENTRIES			=>	NUM_ENTRIES,
+		DATA_WIDTH			=>	20,
+		CAM_RAM_MAX_WIDTH	=>	15
+	)
+	PORT MAP
+	(
+		rst			=>	rst,
+		clk			=>	clk,
+		clk_rd		=>  clk_rd,
+		rdy			=>	cam_rdy,
+
+		re			=>	query_in.vld,
+		we			=>	ctrl_in.entry_we,
+		vld			=>	'1',
+		addr		=>	ctrl_in.entry_wr_idx,
+		tag_wr		=>	ctrl_in.entry_hi.VPN,
+
+		tag_rd		=>	query_in.vaddr(31 downto lg2(TLB_PAGE_SIZE)),
+		hit			=>	blk_hit(NUM_ENTRIES-1 downto 0),
+		hit_vld		=>	cam_hit_vld
+
+	);
+
+	hit_umc			<= '1' when vaddr_reg(31 downto 29) = "100" else '0';
+	hit_umuc		<= '1' when vaddr_reg(31 downto 29) = "101" else '0';
+	
+tlb_entry_lo_result_translate:
+if USE_TLB = true generate
+	tlb_entry_lo_result:
+		process(blk_entry_lo, hit_umc, hit_umuc, kseg_0_1_pfn, kseg1_force_cached)
+		begin
+			entry_lo_res <= blk_entry_lo;
+			if hit_umc = '1' or hit_umuc = '1' then
+				entry_lo_res.N		<= hit_umuc and not kseg1_force_cached;
+				entry_lo_res		<= C_ENTRY_LO_DEFAULT;
+				entry_lo_res.PFN	<= kseg_0_1_pfn;
+			end if;
+
+		end process;
+end generate;
+
+tlb_entry_lo_result_notranslate:
+if USE_TLB = false generate
+	tlb_entry_lo_result:
+		process(blk_entry_lo, vaddr_reg, hit_umc, hit_umuc, kseg_0_1_pfn, kseg1_force_cached)
+		begin
+			entry_lo_res		<= C_ENTRY_LO_DEFAULT;
+			entry_lo_res.PFN	<= vaddr_reg(31 downto lg2(TLB_PAGE_SIZE));
+			if hit_umc = '1' or hit_umuc = '1' then
+				entry_lo_res.N		<= hit_umuc and not kseg1_force_cached;
+				entry_lo_res		<= C_ENTRY_LO_DEFAULT;
+				entry_lo_res.PFN	<= kseg_0_1_pfn;
+			end if;
+		end process;
+end generate;
+
+tlb_hit_combine_translate:
+if USE_TLB = true generate
+	tlb_hit_combine:
+		process(blk_entry_lo, hit_umc, hit_umuc)
+			variable t_v : std_logic;
+		begin
+			t_v := blk_entry_lo.V;
+			t_v := t_v or hit_umc or hit_umuc;
+			hit	<= t_v;
+
+		end process;
+end generate;
+
+tlb_hit_combine_notranslate:
+if USE_TLB = false generate
+	tlb_hit_combine:
+		process(blk_entry_lo, hit_umc, hit_umuc)
+			variable t_v : std_logic;
+		begin
+			t_v := hit_umc or hit_umuc;
+			hit	<= t_v;
+
+		end process;
+end generate;
+
+------------------------------------------------------------------
+end behavior;

lib/CPUs/MIPS/src/core/mips_top.vhd

@@ -31,14 +31,16 @@ use work.mips_types.all;
 entity mips_top is
 	Generic
 	(
-		icache_size	: natural := 1024;	-- words
+		icache_size			: natural := 1024;	-- words
-		icache_line	: natural := 8;		-- words
+		icache_line			: natural := 8;		-- words
-		dcache_size	: natural := 1024;	-- words
+		dcache_size			: natural := 1024;	-- words
-		dcache_line	: natural := 8		-- words
+		dcache_line			: natural := 8;		-- words
+		WITH_TLB			: boolean := true;
+		TRANSLATE_KSEG0_1	: boolean := true
 	);
 	Port
 	(
-		debug		: out unsigned(1 downto 0);
+		debug		: out chip_debug_t;
 		eb			: in  STD_LOGIC;
 		nmi			: in  STD_LOGIC;
 		cpu_clk		: in  STD_LOGIC;
@@ -67,42 +69,32 @@ architecture rtl of mips_top is
 	COMPONENT pipeline is
 	Port 
 	(
-		rst			: in  STD_LOGIC;
+		rst					: in  STD_LOGIC;
-		clk			: in  STD_LOGIC;
+		clk					: in  STD_LOGIC;
-		ce			: in  STD_LOGIC;
+		ce					: in  STD_LOGIC;
-		imem_rdy	: in  STD_LOGIC;
+		imem_err			: in  STD_LOGIC;
-		imem_en		: out STD_LOGIC;
+		dmem_err			: in  STD_LOGIC;
-		imem_addr	: out word_t;
+		cop_ir				: out word_t;
-		imem_data	: in  word_t;
+		cop_ir_en			: out STD_LOGIC;
-		dmem_rdy	: in  STD_LOGIC;
+		cop_din				: in  word_t;
-		dmem_en		: out STD_LOGIC;
+		cop_dout			: out word_t;
-		dmem_we		: out STD_LOGIC;
+		ctrl_out			: out pipe_ctrl_out_t;
-		dmem_be		: out unsigned(3 downto 0);
+		ctrl_in				: in  pipe_ctrl_in_t;
-		dmem_addr	: out word_t;
+		dtlb_qry_out		: out tlb_query_in_t;
-		dmem_din	: in  word_t;
+		dtlb_qry_in			: in  tlb_query_out_t;
-		dmem_dout	: out word_t;
+		dcache_qry_out		: out dcache_query_in_t;
-		cop_ir		: out word_t;
+		dcache_qry_in		: in  dcache_query_out_t;
-		cop_ir_en	: out STD_LOGIC;
+		itlb_qry_out		: out tlb_query_in_t;
-		cop_din		: in  word_t;
+		itlb_qry_in			: in  tlb_query_out_t;
-		cop_dout	: out word_t;
+		icache_qry_out		: out icache_query_in_t;
-		c0_ctrl_out	: out cop0_ctrl_in_t;
+		icache_qry_in		: in  icache_query_out_t
-		c0_ctrl_in	: in  cop0_ctrl_out_t
 	);
 	END COMPONENT;
+	
 	signal imem_err			: std_logic;
-	signal imem_rdy			: std_logic;
-	signal imem_en			: std_logic;
-	signal imem_addr		: word_t;
-	signal imem_din			: word_t;
 	signal dmem_err			: std_logic;
-	signal dmem_rdy			: std_logic;
-	signal dmem_en			: std_logic;
-	signal dmem_we			: std_logic;
-	signal dmem_addr		: word_t;
-	signal dmem_dout		: word_t;
-	signal dmem_din			: word_t;
-	signal dmem_be			: unsigned(3 downto 0);
 
+	signal biu_busy			: STD_LOGIC;
 	signal biu_rst			: STD_LOGIC;
 	signal cpu_rst			: STD_LOGIC;
 	signal cpu_run			: STD_LOGIC;
@@ -111,8 +103,14 @@ architecture rtl of mips_top is
 	signal pipe_cop_ir_en	: STD_LOGIC;
 	signal pipe_cop_din		: word_t;
 	signal pipe_cop_dout	: word_t;
-	signal pipe_c0_ctrl_out	: cop0_ctrl_in_t;
+	signal pipe_ctrl_in		: pipe_ctrl_in_t;
-	signal pipe_c0_ctrl_in	: cop0_ctrl_out_t;
+	signal pipe_ctrl_out	: pipe_ctrl_out_t;
+
+	signal cop0_ctrl_in		: cop0_ctrl_in_t;
+	signal cop0_ctrl_out	: cop0_ctrl_out_t;
+
+	signal biu_ctrl_in		: biu_ctrl_in_t;
+	signal biu_ctrl_out		: biu_ctrl_out_t;
 	
 --------------------------------------------------------------------------
 -- Coprocessor
@@ -147,53 +145,48 @@ architecture rtl of mips_top is
 	COMPONENT biu
 	GENERIC
 	(
-		icache_size	: natural;
+		icache_size			: natural;
-		icache_line	: natural;
+		icache_line			: natural;
-		dcache_size	: natural;
+		dcache_size			: natural;
-		dcache_line	: natural
+		dcache_line			: natural;
+		WRITE_FIFO_SIZE		: natural;
+		WITH_TLB			: boolean;
+		TRANSLATE_KSEG0_1	: boolean
 	);
 	PORT
 	(
+		clk				: in  STD_LOGIC;
+		busy			: out STD_LOGIC;
+
+		imem_err		: out STD_LOGIC;
+		dmem_err		: out STD_LOGIC;
+
 		RST_I			: in  STD_LOGIC;
 		CLK_I			: in  STD_LOGIC;
 		ACK_I			: in  STD_LOGIC;
 		SRDY_I			: in  STD_LOGIC;
 		ADDR_O			: out unsigned(31 downto 0);
-		DAT_I			: in  unsigned(31 downto 0);
+		MDAT_I			: in  unsigned(31 downto 0);
-		DAT_O			: out unsigned(31 downto 0);
+		MDAT_O			: out unsigned(31 downto 0);
 		WE_O			: out STD_LOGIC;
 		SEL_O			: out unsigned(3 downto 0);
 		CYC_O			: out STD_LOGIC;
 		STB_O			: out STD_LOGIC;
 		MRDY_O			: out STD_LOGIC;
-		cop0_ctrl_in	: in cop0_ctrl_out_t;
+
-		cpu_clk			: in  STD_LOGIC;
+		ctrl_in			: in  biu_ctrl_in_t;
-		cpu_imem_err	: out STD_LOGIC;
+		ctrl_out		: out biu_ctrl_out_t
-		cpu_imem_rdy	: out STD_LOGIC;
+
-		cpu_imem_en		: in  STD_LOGIC;
-		cpu_imem_addr	: in  word_t;
-		cpu_imem_din	: out word_t;
-		cpu_dmem_err	: out STD_LOGIC;
-		cpu_dmem_rdy	: out STD_LOGIC;
-		cpu_dmem_en		: in  STD_LOGIC;
-		cpu_dmem_we		: in  STD_LOGIC;
-		cpu_dmem_be		: in  unsigned(3 downto 0);
-		cpu_dmem_dout	: in  word_t;
-		cpu_dmem_din	: out word_t;
-		cpu_dmem_addr	: in  word_t
 	);
 	END COMPONENT;
 
 begin
 
 -------------------------------------------------------------------
-	debug(0)		<= imem_err;
+	process(cpu_clk)
-	debug(1)		<= dmem_err;
-	
-	process(CLK_I)
 	variable reset_delay : unsigned (31 downto 0);
 	begin
-		if rising_edge(CLK_I) then
+		if rising_edge(cpu_clk) then
 			if RST_I = '1' then
 				reset_delay := (others => '0');
 				biu_rst	<= '1';
@@ -207,7 +200,7 @@ begin
 				if reset_delay(31) = '1' then
 					cpu_rst <= '0';
 				end if;	
-				if reset_delay(16) = '1' then
+				if reset_delay(16) = '1' and biu_busy = '0' then
 					cpu_run	<= '1';
 				end if;	
 			end if;
@@ -217,28 +210,29 @@ begin
 inst_pipeline: pipeline
 	PORT MAP
 	(
-		rst			=> cpu_rst,
+		rst					=> cpu_rst,
-		clk			=> cpu_clk,
+		clk					=> cpu_clk,
-		ce			=> cpu_run,
+		ce					=> cpu_run,
-		imem_rdy	=> imem_rdy,
+		imem_err			=> imem_err,
-		imem_en		=> imem_en,
+		dmem_err			=> dmem_err,
-		imem_addr	=> imem_addr,
+		cop_ir				=> pipe_cop_ir,
-		imem_data	=> imem_din,
+		cop_ir_en			=> pipe_cop_ir_en,
-		dmem_rdy	=> dmem_rdy,
+		cop_din				=> pipe_cop_din,
-		dmem_en		=> dmem_en,
+		cop_dout			=> pipe_cop_dout,
-		dmem_we		=> dmem_we,
+		ctrl_out			=> pipe_ctrl_out,
-		dmem_be		=> dmem_be,
+		ctrl_in				=> pipe_ctrl_in,
-		dmem_addr	=> dmem_addr,
+		dtlb_qry_out		=> biu_ctrl_in.dtlb_qry,
-		dmem_din	=> dmem_din,
+		dtlb_qry_in			=> biu_ctrl_out.dtlb_qry,
-		dmem_dout	=> dmem_dout,
+		dcache_qry_out		=> biu_ctrl_in.dcache_qry,
-		cop_ir		=> pipe_cop_ir,
+		dcache_qry_in		=> biu_ctrl_out.dcache_qry,
-		cop_ir_en	=> pipe_cop_ir_en,
+		itlb_qry_out		=> biu_ctrl_in.itlb_qry,
-		cop_din		=> pipe_cop_din,
+		itlb_qry_in			=> biu_ctrl_out.itlb_qry,
-		cop_dout	=> pipe_cop_dout,
+		icache_qry_out		=> biu_ctrl_in.icache_qry,
-		c0_ctrl_out	=> pipe_c0_ctrl_out,
+		icache_qry_in		=> biu_ctrl_out.icache_qry
-		c0_ctrl_in	=> pipe_c0_ctrl_in
 	);
 
+	pipe_ctrl_in		<= cop0_ctrl_out.pipe;
+
 inst_cop: cop
 	GENERIC MAP
 	(
@@ -256,49 +250,114 @@ inst_cop: cop
 		int			=> INT,
 		ir_en		=> pipe_cop_ir_en,
 		ir			=> pipe_cop_ir,
-		ctrl_in		=> pipe_c0_ctrl_out,
+		ctrl_in		=> cop0_ctrl_in,
-		ctrl_out	=> pipe_c0_ctrl_in,
+		ctrl_out	=> cop0_ctrl_out,
 		dout		=> pipe_cop_din,
 		din			=> pipe_cop_dout
 	);
-	
+
+	cop0_ctrl_in.pipe		<= pipe_ctrl_out;
+	cop0_ctrl_in.itlb_ctrl	<= biu_ctrl_out.itlb_ctrl;
+	cop0_ctrl_in.dtlb_ctrl	<= biu_ctrl_out.dtlb_ctrl;
+
 inst_biu: biu
 	GENERIC MAP
 	(
-		icache_size	=> icache_size,	-- words
+		icache_size			=> icache_size,	-- words
-		icache_line	=> icache_line,	-- words
+		icache_line			=> icache_line,	-- words
-		dcache_size	=> dcache_size,	-- words
+		dcache_size			=> dcache_size,	-- words
-		dcache_line	=> dcache_line	-- words
+		dcache_line			=> dcache_line,	-- words
+		WRITE_FIFO_SIZE		=> 4,
+		WITH_TLB			=> WITH_TLB,
+		TRANSLATE_KSEG0_1	=> TRANSLATE_KSEG0_1
 	)
 	PORT MAP
 	(
+
+		clk				=> cpu_clk,
+		busy			=> biu_busy,
+		
+		imem_err		=> imem_err,
+		dmem_err		=> dmem_err,
+
 		RST_I			=> biu_rst,
 		CLK_I			=> CLK_I,
 		ACK_I			=> ACK_I,
 		SRDY_I			=> SRDY_I,
 		ADDR_O			=> ADDR_O,
-		DAT_I			=> DAT_I,
+		MDAT_I			=> DAT_I,
-		DAT_O			=> DAT_O,
+		MDAT_O			=> DAT_O,
 		WE_O			=> WE_O,
 		SEL_O			=> SEL_O,
 		CYC_O			=> CYC_O,
 		STB_O			=> STB_O,
 		MRDY_O			=> MRDY_O,
-		cop0_ctrl_in	=> pipe_c0_ctrl_in,
+
-		cpu_clk			=> cpu_clk,
+		ctrl_in			=> biu_ctrl_in,
-		cpu_imem_err	=> imem_err,
+		ctrl_out		=> biu_ctrl_out
-		cpu_imem_rdy	=> imem_rdy,
+
-		cpu_imem_en		=> imem_en,
-		cpu_imem_addr	=> imem_addr,
-		cpu_imem_din	=> imem_din,
-		cpu_dmem_err	=> dmem_err,
-		cpu_dmem_rdy	=> dmem_rdy,
-		cpu_dmem_en		=> dmem_en,
-		cpu_dmem_we		=> dmem_we,
-		cpu_dmem_be		=> dmem_be,
-		cpu_dmem_addr	=> dmem_addr,
-		cpu_dmem_din	=> dmem_din,
-		cpu_dmem_dout	=> dmem_dout
 	);
 
+	biu_ctrl_in.icache		<= cop0_ctrl_out.icache;
+	biu_ctrl_in.dcache		<= cop0_ctrl_out.dcache;
+	biu_ctrl_in.itlb_ctrl	<= cop0_ctrl_out.itlb_ctrl;
+	biu_ctrl_in.dtlb_ctrl	<= cop0_ctrl_out.dtlb_ctrl;
+--	biu_ctrl_in.itlb_qry	<= pipe_ctrl_out.itlb_qry;
+--	biu_ctrl_in.dtlb_qry	<= pipe_ctrl_out.dtlb_qry;
+
+-------------------------------------------
+-- debug
+-------------------------------------------
+	debug.imem_err	<= imem_err;
+	debug.dmem_err	<= dmem_err;
+
+debug_imem_din_register:
+	process(cpu_clk)
+	variable en_r: std_logic;
+	variable addr_r: word_t;
+	begin
+		if rising_edge(cpu_clk) then
+			debug.imem_din_vld	<= '0';
+			if biu_ctrl_out.icache_qry.rdy = '1' then
+				if en_r = '1' then
+					debug.imem_addr_reg	<= addr_r;
+					debug.imem_din_reg	<= biu_ctrl_out.icache_qry.data;
+					debug.imem_din_vld	<= '1';
+				end if;
+				if biu_ctrl_in.icache_qry.en = '1' then
+					addr_r	:= biu_ctrl_in.icache_qry.addr;
+				end if;
+				en_r	:= biu_ctrl_in.icache_qry.en;
+			end if;
+		end if;
+	end process;	
+
+debug_dmem_din_register:
+	process(cpu_clk)
+	variable en_r: std_logic;
+	variable we_r: std_logic;
+	variable addr_r: word_t;
+	variable dout_r: word_t;
+	begin
+		if rising_edge(cpu_clk) then
+			debug.dmem_din_vld	<= '0';
+			debug.dmem_dout_vld	<= '0';
+			if biu_ctrl_out.dcache_qry.rdy = '1' then
+				if en_r = '1' then
+					debug.dmem_addr_reg	<= addr_r;
+					debug.dmem_din_reg	<= biu_ctrl_out.dcache_qry.data;
+					debug.dmem_din_vld	<= not we_r;
+					debug.dmem_dout_reg	<= dout_r;
+					debug.dmem_dout_vld	<= we_r;
+				end if;
+				if biu_ctrl_in.dcache_qry.en = '1' then
+					dout_r	:= biu_ctrl_in.dcache_qry.data;
+					addr_r	:= biu_ctrl_in.dcache_qry.addr;
+				end if;
+				en_r	:= biu_ctrl_in.dcache_qry.en;
+				we_r	:= biu_ctrl_in.dcache_qry.we;
+			end if;
+		end if;
+	end process;	
+
 end rtl;

lib/CPUs/MIPS/src/core/mips_types.vhd

@@ -26,12 +26,18 @@ use IEEE.STD_LOGIC_1164.ALL;
 use IEEE.NUMERIC_STD.ALL;
 use IEEE.MATH_REAL.ALL;
 
+library work;
+use work.mips_util_pkg.all;
+
 --------------------------------------------------------------------------
 package mips_types is
 
 	-- Revision of the CPU
-	constant REVISION : integer := 13;
+	constant REVISION			: integer	:= 14;
-	constant WORD_WIDTH : integer := 32;
+	constant WORD_WIDTH			: integer	:= 32;
+	constant TLB_PAGE_SIZE		: positive	:= 4096;
+	constant TLB_NUM_ASIDS		: positive	:= 64;
+	constant TLB_NUM_ENTRIES	: positive	:= 32;
 	
 	--Types
 	subtype instr_name_t is string(1 to 12);
@@ -45,9 +51,17 @@ package mips_types is
 
 	type chip_debug_t is record	
 		imem_err			: std_logic;
+		imem_addr_reg		: word_t;
+		imem_din_reg		: word_t;
+		imem_din_vld		: std_logic;
 		dmem_err			: std_logic;
+		dmem_addr_reg		: word_t;
+		dmem_din_reg		: word_t;
+		dmem_din_vld		: std_logic;
+		dmem_dout_reg		: word_t;
+		dmem_dout_vld		: std_logic;
 	end record;
-
+	
 	type op_t is
 	(
 		op_illegal,
@@ -159,11 +173,14 @@ package mips_types is
 	type wptr_src_t is (wptr_src_imm, wptr_src_const);
 	
 	type pc_t is record	
-		curr			: word_t;
+		curr				: word_t;
-		last			: word_t;
+		last				: word_t;
-		nxt				: word_t;
+		nxt					: word_t;
-		pc_branch		: word_t;
+		pc_branch			: word_t;
-		branch_take		: std_logic;
+		pc_branch_revert	: word_t;
+		branch_take			: std_logic;
+		branch_not_taken	: std_logic;
+		plus4				: word_t;
 	end record;
 	
 	type alu_outsel_t is
@@ -227,6 +244,12 @@ package mips_types is
 		syscall			: STD_LOGIC;
 		break			: STD_LOGIC;
 		illegal			: STD_LOGIC;
+		ITLB_MOD		: STD_LOGIC;
+		ITLB_LOAD		: STD_LOGIC;
+		ITLB_STORE		: STD_LOGIC;
+		DTLB_MOD		: STD_LOGIC;
+		DTLB_LOAD		: STD_LOGIC;
+		DTLB_STORE		: STD_LOGIC;
 	end record;
 	
 	type exc_flags_t is record
@@ -239,6 +262,12 @@ package mips_types is
 		Sys				: STD_LOGIC;
 		Bp				: STD_LOGIC;
 		RI				: STD_LOGIC;
+		ITLB_MOD		: STD_LOGIC;
+		ITLB_LOAD		: STD_LOGIC;
+		ITLB_STORE		: STD_LOGIC;
+		DTLB_MOD		: STD_LOGIC;
+		DTLB_LOAD		: STD_LOGIC;
+		DTLB_STORE		: STD_LOGIC;
 		IBE				: STD_LOGIC;
 		DBE				: STD_LOGIC;
 	end record;
@@ -249,25 +278,108 @@ package mips_types is
 		cause			: word_t;
 	end record;
 	
+	type icache_query_in_t is record
+		en				: std_logic;
+		nc				: std_logic;
+		addr			: word_t;
+	end record;
+
+	type icache_query_out_t is record
+		rdy				: std_logic;
+		data			: word_t;
+	end record;
+
+	type dcache_query_in_t is record
+		en				: std_logic;
+		we				: std_logic;
+		be				: unsigned(3 downto 0);
+		nc				: std_logic;
+		addr			: word_t;
+		data			: word_t;
+	end record;
+
+	type dcache_query_out_t is record
+		rdy				: std_logic;
+		data			: word_t;
+	end record;
+
 	type cache_ctrl_t is record
 		inv_addr		: word_t;
 		inv_at			: STD_LOGIC;
 		inv_all			: STD_LOGIC;
 	end record;
 	
-	type cop0_ctrl_in_t is record
+	type tlb_flags_t is record	
-		sdu				: sdu_t;
+		N				: std_logic;
-		events			: event_t;
+		D				: std_logic;
-		bd_wb			: STD_LOGIC;
+		V				: std_logic;
-		epc_mem			: word_t;
-		epc_wb			: word_t;
-		imem_addr		: word_t;
-		dmem_addr		: word_t;
-		exc_req			: STD_LOGIC;
-		exc_ack			: STD_LOGIC;
 	end record;
 
-	type cop0_ctrl_out_t is record
+	type tlb_entry_lo_t is record	
+		PFN				: unsigned (31 downto lg2(TLB_PAGE_SIZE));
+		G				: std_logic;
+		N				: std_logic;
+		D				: std_logic;
+		V				: std_logic;
+	end record;
+	
+	type tlb_entry_hi_t is record	
+		VPN				: unsigned (31 downto lg2(TLB_PAGE_SIZE));
+		ASID			: unsigned (lg2(TLB_NUM_ASIDS)-1 downto 0);
+	end record;
+
+	type tlb_query_in_t is record
+		vld				: std_logic;
+		write			: std_logic;
+		vaddr			: word_t;
+	end record;
+
+	type tlb_query_out_t is record
+		vld				: std_logic;
+		paddr			: word_t;
+		miss			: STD_LOGIC;
+		dirty			: std_logic;
+		write			: std_logic;
+		flags			: tlb_flags_t;
+	end record;
+
+	type tlb_ctrl_in_t is record
+		entry_we		: STD_LOGIC;
+		entry_re		: STD_LOGIC;
+		entry_rd_idx	: unsigned (lg2(TLB_NUM_ENTRIES)-1 downto 0);
+		entry_wr_idx	: unsigned (lg2(TLB_NUM_ENTRIES)-1 downto 0);
+		entry_lo		: tlb_entry_lo_t;
+		entry_hi		: tlb_entry_hi_t;
+	end record;
+
+	type tlb_ctrl_out_t is record
+		entry_vld		: std_logic;
+		entry_lo		: tlb_entry_lo_t;
+		entry_hi		: tlb_entry_hi_t;
+		entry_prb_idx	: unsigned (lg2(TLB_NUM_ENTRIES)-1 downto 0);
+	end record;
+
+	type biu_ctrl_in_t is record
+		itlb_ctrl		: tlb_ctrl_in_t;
+		dtlb_ctrl		: tlb_ctrl_in_t;
+		icache			: cache_ctrl_t;
+		dcache			: cache_ctrl_t;
+		itlb_qry		: tlb_query_in_t;
+		dtlb_qry		: tlb_query_in_t;
+		icache_qry		: icache_query_in_t;
+		dcache_qry		: dcache_query_in_t;
+	end record;
+
+	type biu_ctrl_out_t is record
+		itlb_ctrl		: tlb_ctrl_out_t;
+		dtlb_ctrl		: tlb_ctrl_out_t;
+		itlb_qry		: tlb_query_out_t;
+		dtlb_qry		: tlb_query_out_t;
+		icache_qry		: icache_query_out_t;
+		dcache_qry		: dcache_query_out_t;
+	end record;
+
+	type pipe_ctrl_in_t is record
 		EB				: STD_LOGIC;
 		exc_inject		: STD_LOGIC;
 		exc_commit		: STD_LOGIC;
@@ -277,6 +389,29 @@ package mips_types is
 		NMI				: STD_LOGIC;
 		int				: STD_LOGIC;
 		exc_vec			: word_t;
+	end record;
+
+	type pipe_ctrl_out_t is record
+		sdu				: sdu_t;
+		events			: event_t;
+		bd_wb			: STD_LOGIC;
+		epc_wb			: word_t;
+		imem_addr		: word_t;
+		dmem_addr		: word_t;
+		exc_req			: STD_LOGIC;
+		exc_ack			: STD_LOGIC;
+	end record;
+
+	type cop0_ctrl_in_t is record
+		pipe			: pipe_ctrl_out_t;
+		itlb_ctrl		: tlb_ctrl_out_t;
+		dtlb_ctrl		: tlb_ctrl_out_t;
+	end record;
+
+	type cop0_ctrl_out_t is record
+		pipe			: pipe_ctrl_in_t;
+		itlb_ctrl		: tlb_ctrl_in_t;
+		dtlb_ctrl		: tlb_ctrl_in_t;
 		icache			: cache_ctrl_t;
 		dcache			: cache_ctrl_t;
 	end record;
@@ -337,76 +472,93 @@ package mips_types is
 	type ID_t is record
 		nop			: STD_LOGIC;
 		exc			: std_logic;
-		IR			: word_t;
 		op			: op_t;
-		pcn			: word_t;
+		IR			: word_t;
 		epc			: word_t;
+		pcn			: word_t;
 		jimm32		: word_t;
 		bimm18		: word_t;
 		imm			: word_t;
 		shamt		: shamt_t;
 		ctrl		: ctrl_lines_t;
+		va			: word_t;
 		reg_write	: STD_LOGIC;
 		reg_a_rptr	: reg_ptr_t;
 		reg_b_rptr	: reg_ptr_t;
 		reg_a		: word_t;
 		reg_b		: word_t;
 		events		: event_t;
+		bcu_flags	: bcu_flags_t;
+		itlb_qry	: tlb_query_out_t;
 	end record;
 	
 	type EX_t is record
 		nop			: STD_LOGIC;
 		exc			: std_logic;
-		IR			: word_t;
 		op			: op_t;
-		pcn			: word_t;
+		IR			: word_t;
 		epc			: word_t;
+		pcn			: word_t;
 		reg_a		: word_t;
 		reg_b		: word_t;
+		result		: word_t;
 		ctrl		: ctrl_lines_t;
 		shift_ctrl	: shift_ctrl_t;
 		alu_op1		: word_t;
 		alu_op2		: word_t;
 		alu_op2_s	: word_t;
 		alu_flags	: alu_flags_t;
-		result		: word_t;
 		reg_write	: STD_LOGIC;
-		wreg_we		: STD_LOGIC;
+		reg_we		: STD_LOGIC;
 		reg_wptr	: reg_ptr_t;
 		reg_a_rptr	: reg_ptr_t;
 		reg_b_rptr	: reg_ptr_t;
 		va			: word_t;
+		dmem_en		: STD_LOGIC;
 		pa_off		: unsigned(1 downto 0);
 		events_in	: event_t;
 		events		: event_t;
+		bcu_flags	: bcu_flags_t;
+		branch_not_taken	: std_logic;
+		dtlb_qry	: tlb_query_out_t;
 	end record;
 	
 	type MEM_t is record
 		nop			: STD_LOGIC;
 		exc			: std_logic;
 		op			: op_t;
-		pcn			: word_t;
 		epc			: word_t;
+		pcn			: word_t;
+		reg_a		: word_t;
+		reg_b		: word_t;
+		result		: word_t;
+		result_in	: word_t;
 		ctrl		: ctrl_lines_t;
-		ex_result	: word_t;
 		reg_wptr	: reg_ptr_t;
-		wreg_we		: STD_LOGIC;
+		reg_we		: STD_LOGIC;
-		data		: word_t;
 		va			: word_t;
 		pa_off		: unsigned(1 downto 0);
 		events_in	: event_t;
 		events		: event_t;
+		bcu_flags	: bcu_flags_t;
+		branch_not_taken	: std_logic;
 	end record;
 	
 	type WB_t is record
 		nop			: STD_LOGIC;
+		epc			: word_t;
+		reg_a		: word_t;
+		reg_b		: word_t;
+		result		: word_t;
+		bd			: STD_LOGIC;
 		exc			: std_logic;
+		exc_last	: std_logic;
 		events		: event_t;
 		op			: op_t;
 		reg_wptr	: reg_ptr_t;
-		wreg_we		: STD_LOGIC;
+		reg_we		: STD_LOGIC;
-		data		: word_t;
+		va			: word_t;
-		bd			: STD_LOGIC;
+		bcu_flags	: bcu_flags_t;
 	end record;
 
 	
@@ -422,9 +574,7 @@ package mips_types is
 	function events_clr return event_t;
 	function event_is_active(e : event_t) return std_logic;
 	function "or" (a, b : event_t) return event_t;
-
+	function "or" (a, b : tlb_flags_t) return tlb_flags_t;
-	function lg2(x : natural) return natural;
-	function po2(x : natural) return natural;
 
 end mips_types;
 
@@ -608,6 +758,12 @@ package body mips_types is
 		result.syscall			:= '0';
 		result.break			:= '0';
 		result.illegal			:= '0';
+		result.ITLB_MOD			:= '0';
+		result.ITLB_LOAD		:= '0';
+		result.ITLB_STORE		:= '0';
+		result.DTLB_MOD			:= '0';
+		result.DTLB_LOAD		:= '0';
+		result.DTLB_STORE		:= '0';
 		
 		return result;
 		
@@ -630,6 +786,12 @@ package body mips_types is
 		result	:= result or e.syscall;
 		result	:= result or e.break;
 		result	:= result or e.illegal;
+		result	:= result or e.ITLB_MOD;
+		result	:= result or e.ITLB_LOAD;
+		result	:= result or e.ITLB_STORE;
+		result	:= result or e.DTLB_MOD;
+		result	:= result or e.DTLB_LOAD;
+		result	:= result or e.DTLB_STORE;
 
 		return result;
 		
@@ -650,23 +812,28 @@ package body mips_types is
 		result.syscall			:= a.syscall or b.syscall;
 		result.break			:= a.break or b.break;
 		result.illegal			:= a.illegal or b.illegal;
+		result.ITLB_MOD			:= a.ITLB_MOD or b.ITLB_MOD;
+		result.ITLB_LOAD		:= a.ITLB_LOAD or b.ITLB_LOAD;
+		result.ITLB_STORE		:= a.ITLB_STORE or b.ITLB_STORE;
+		result.DTLB_MOD			:= a.DTLB_MOD or b.DTLB_MOD;
+		result.DTLB_LOAD		:= a.DTLB_LOAD or b.DTLB_LOAD;
+		result.DTLB_STORE		:= a.DTLB_STORE or b.DTLB_STORE;
 		
 		return result;
 		
 	end "or";
 
 --------------------------------------------------------------------------
-	function lg2(x : natural) return natural is
+	function "or" (a, b : tlb_flags_t) return tlb_flags_t is
+	variable result : tlb_flags_t;
 	begin
-		return natural(ceil(log2(real(x))));
+		result.N				:= a.N or b.N;
-	end lg2;	
+		result.D				:= a.D or b.D;
-	
+		result.V				:= a.V or b.V;
---------------------------------------------------------------------------
+		
-	function po2(x : natural) return natural is
+		return result;
-	begin
+		
-
+	end "or";
-		return 2**lg2(x);
-	end po2;	
 
 --------------------------------------------------------------------------
 end mips_types;

lib/CPUs/MIPS/src/core/mips_util_pkg.vhd

@@ -0,0 +1,68 @@
+--------------------------------------------------------------------------
+-- Project: JIPS, a portable 32-bit RISC CPU written in VHDL
+-- This file: Types, constants and functions for JIPS
+--
+-- Copyright (C) 2008 J. Ahrensfeld
+--
+-- 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, either version 3 of the License, or
+-- (at your option) any later version.
+--
+-- 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/>.
+--
+-- For questions and ideas, please contact the author at jens@jayfield.org
+--
+--------------------------------------------------------------------------
+
+library IEEE;
+use IEEE.STD_LOGIC_1164.ALL;
+use IEEE.NUMERIC_STD.ALL;
+use IEEE.MATH_REAL.ALL;
+
+--------------------------------------------------------------------------
+package mips_util_pkg is
+
+
+	function to_mips_01(x : unsigned) return unsigned;
+	function lg2(x : natural) return natural;
+	function po2(x : natural) return natural;
+
+end mips_util_pkg;
+
+--------------------------------------------------------------------------
+package body mips_util_pkg is
+
+	function to_mips_01(x : unsigned) return unsigned is
+	variable result : unsigned (x'left downto x'right);
+	begin
+		for i in x'left downto x'right loop
+			if x(i) = '1' then
+				result(i)	:= '1';
+			else 
+				result(i)	:= '0';
+			end if;
+		end loop;
+		return result;
+	end to_mips_01;
+
+	function lg2(x : natural) return natural is
+	begin
+		return natural(ceil(log2(real(x))));
+	end lg2;	
+	
+	function po2(x : natural) return natural is
+	begin
+
+		return 2**lg2(x);
+	end po2;	
+
+--------------------------------------------------------------------------
+end mips_util_pkg;
+

lib/CPUs/MIPS/src/tb_mips_biu.vhd

@@ -0,0 +1,410 @@
+LIBRARY ieee;
+use IEEE.STD_LOGIC_1164.ALL;
+USE ieee.numeric_std.ALL;
+
+library work;
+use work.mips_types.all;
+use work.mips_util_pkg.all;
+use work.busmaster_types.all;
+
+ENTITY tb_biu IS
+END tb_biu;
+
+ARCHITECTURE behavior OF tb_biu IS 
+
+	constant DATA_WIDTH			: integer := 32;
+	constant CLK_PERIOD			: time := 10 ns;
+
+	signal CLK				: std_logic := '1';
+	signal RST				: std_logic := '1';
+	signal INT_O			: std_logic;
+	signal CYC_O			: std_logic;
+	signal STB_O			: std_logic;
+	signal WE_O				: std_logic;
+	signal SEL_O			: unsigned(3 downto 0) := (others => '1');
+	signal ACK_I			: std_logic := '0';
+	signal MRDY_O			: std_logic := '1';
+	signal SRDY_I			: std_logic := '0';
+	signal ADDR_O			: unsigned(31 downto 0) := (others => '-');
+	signal MDAT_I			: unsigned(31 downto 0) := (others => '-');
+	signal MDAT_O			: unsigned(31 downto 0) := (others => '-');
+
+	signal index_in			: unsigned(lg2(TLB_NUM_ENTRIES)-1 downto 0) := (others => '0');
+	signal entry_we			: STD_LOGIC := '0';
+	signal entry_lo_in		: tlb_entry_lo_t;
+	signal entry_hi_in		: tlb_entry_hi_t;
+	
+	signal index_out		: unsigned(lg2(TLB_NUM_ENTRIES)-1 downto 0) := (others => '0');
+	signal entry_re			: STD_LOGIC := '0';
+	signal entry_lo_out		: tlb_entry_lo_t;
+	signal entry_hi_out		: tlb_entry_hi_t;
+		
+	-- CPU control/data
+	signal cpu_imem_err					: STD_LOGIC := '0';
+	signal cpu_imem_en					: STD_LOGIC := '0';
+	signal cpu_imem_addr				: word_t := (others => '-');
+	signal cpu_imem_dout				: word_t;
+	signal cpu_imem_rdy					: STD_LOGIC;
+
+	signal cpu_dmem_err					: STD_LOGIC := '0';
+	signal cpu_dmem_en					: STD_LOGIC := '0';
+	signal cpu_dmem_we					: STD_LOGIC := '0';
+	signal cpu_dmem_addr				: word_t := (others => '-');
+	signal cpu_dmem_be					: unsigned(3 downto 0) := (others => '1');
+	signal cpu_dmem_din					: word_t := X"BFC0_BABE";
+	signal cpu_dmem_dout				: word_t;
+	signal cpu_dmem_rdy					: STD_LOGIC;
+
+	-- BIU control
+	signal ctrl_in						: biu_ctrl_in_t;
+	signal ctrl_out						: biu_ctrl_out_t;
+
+	type op_descr_t is (op_idle, op_entry_i_write, op_entry_d_write, op_cpu_i_read, op_cpu_d_read, op_cpu_d_write);
+	signal op_descr						: op_descr_t := op_idle;
+
+	signal cpu_imem_din_reg				: word_t;
+	signal cpu_imem_din_vld				: STD_LOGIC;
+	signal cpu_dmem_din_reg				: word_t;
+	signal cpu_dmem_din_vld				: STD_LOGIC;
+
+BEGIN
+
+inst_biu: entity work.biu
+	GENERIC MAP
+	(
+		ICACHE_SIZE		=>	1024,	-- words
+		ICACHE_LINE		=>	8,		-- words
+		DCACHE_SIZE		=>	1024,	-- words
+		DCACHE_LINE		=>	8,		-- words
+		WRITE_FIFO_SIZE	=>	4,		-- words
+		WITH_TLB		=>	true
+	)
+	PORT MAP
+	(
+		clk			=>	CLK,
+
+		imem_err	=>	cpu_imem_err,
+		imem_rdy	=>	cpu_imem_rdy,
+		imem_en		=>	cpu_imem_en,
+		imem_addr	=>	cpu_imem_addr,
+		imem_dout	=>	cpu_imem_dout,
+		dmem_err	=>	cpu_dmem_err,
+		dmem_rdy	=>	cpu_dmem_rdy,
+		dmem_en		=>	cpu_dmem_en,
+		dmem_we		=>	cpu_dmem_we,
+		dmem_be		=>	cpu_dmem_be,
+		dmem_dout	=>	cpu_dmem_dout,
+		dmem_din	=>	cpu_dmem_din,
+		dmem_addr	=>	cpu_dmem_addr,
+
+		RST_I		=>	RST,
+		CLK_I		=>	CLK,
+		ACK_I		=>	ACK_I,
+		SRDY_I		=>	SRDY_I,
+		ADDR_O		=>	ADDR_O,
+		MDAT_I		=>	MDAT_I,
+		MDAT_O		=>	MDAT_O,
+		WE_O		=>	WE_O,
+		SEL_O		=>	SEL_O,
+		CYC_O		=>	CYC_O,
+		STB_O		=>	STB_O,
+		MRDY_O		=>	MRDY_O,
+		
+		ctrl_in		=>	ctrl_in,
+		ctrl_out	=>	ctrl_out
+		
+	);
+
+
+inst_rom_wb: entity work.rom_wb
+--	GENERIC MAP 
+--	(
+--		NUM_WORDS	=>	256
+--	)
+	PORT MAP
+	(
+		CLK_I		=>	CLK,
+		RST_I		=>	RST,
+		CYC_I		=>	CYC_O,
+		STB_I		=>	STB_O,
+--		SEL_I		=>	SEL_O,
+		WE_I		=>	WE_O,
+		ACK_O		=>	ACK_I,
+		SRDY_O		=>	SRDY_I,
+		MRDY_I		=>	MRDY_O,
+		ADDR_I		=>	ADDR_O,
+--		DAT_I		=>	MDAT_O,
+		DAT_O		=>	MDAT_I
+	);
+
+
+CLK_GEN: process
+	begin
+		wait for CLK_PERIOD/2;
+		CLK <= not CLK;
+	end process;
+
+cpu_imem_din_register:
+	process(clk)
+	variable en2: std_logic;
+	begin
+		if rising_edge(clk) then
+			cpu_imem_din_vld	<= '0';
+			if en2 = '1' and cpu_imem_rdy = '1' then
+				cpu_imem_din_reg	<= cpu_imem_dout;
+				cpu_imem_din_vld	<= '1';
+			end if;
+			en2	:= cpu_imem_en;
+		end if;
+	end process;	
+
+cpu_dmem_din_register:
+	process(clk)
+	variable en2: std_logic;
+	begin
+		if rising_edge(clk) then
+			cpu_dmem_din_vld	<= '0';
+			if en2 = '1' and cpu_dmem_rdy = '1' then
+				cpu_dmem_din_reg	<= cpu_dmem_dout;
+				cpu_dmem_din_vld	<= '1';
+			end if;
+			en2	:= cpu_dmem_en;
+		end if;
+	end process;	
+
+
+	ctrl_in.icache.inv_addr			<= X"BEEF_BABE";
+	ctrl_in.icache.inv_at			<= '0';
+	ctrl_in.icache.inv_all			<= '0';
+
+	ctrl_in.dcache.inv_addr			<= X"BEEF_BABE";
+	ctrl_in.dcache.inv_at			<= '0';
+	ctrl_in.dcache.inv_all			<= '0';
+
+STIMULUS: process
+
+	variable result : unsigned(31 downto 0);
+
+	procedure cmd_entry_i_write(index : integer; asid, vpn, pfn : unsigned; g, n, d, v : std_logic) is
+	begin
+	
+		op_descr <= op_entry_i_write;
+		
+		ctrl_in.itlb.entry_wr_idx		<= to_unsigned(index, lg2(TLB_NUM_ENTRIES));
+
+		ctrl_in.itlb.entry_hi.vpn	<= vpn;
+		ctrl_in.itlb.entry_hi.asid	<= asid;
+
+		ctrl_in.itlb.entry_lo.pfn	<= pfn;	
+		ctrl_in.itlb.entry_lo.n		<= n;	
+		ctrl_in.itlb.entry_lo.d		<= d;	
+		ctrl_in.itlb.entry_lo.v		<= v;	
+		ctrl_in.itlb.entry_lo.g		<= g;
+
+		ctrl_in.itlb.entry_we			<= '1';
+		wait until rising_edge(CLK);
+		ctrl_in.itlb.entry_we			<= '0';
+	
+	end procedure cmd_entry_i_write;
+
+	procedure cmd_entry_d_write(index : integer; asid, vpn, pfn : unsigned; g, n, d, v : std_logic) is
+	begin
+	
+		op_descr <= op_entry_d_write;
+		ctrl_in.dtlb.entry_wr_idx		<= to_unsigned(index, lg2(TLB_NUM_ENTRIES));
+
+		ctrl_in.dtlb.entry_hi.vpn	<= vpn;
+		ctrl_in.dtlb.entry_hi.asid	<= asid;
+
+		ctrl_in.dtlb.entry_lo.pfn	<= pfn;	
+		ctrl_in.dtlb.entry_lo.n		<= n;	
+		ctrl_in.dtlb.entry_lo.d		<= d;	
+		ctrl_in.dtlb.entry_lo.v		<= v;	
+		ctrl_in.dtlb.entry_lo.g		<= g;
+
+		ctrl_in.dtlb.entry_we			<= '1';
+		wait until rising_edge(CLK);
+		ctrl_in.dtlb.entry_we			<= '0';
+	
+	end procedure cmd_entry_d_write;
+
+	procedure cpu_i_read(addr : unsigned) is
+	begin
+
+		op_descr <= op_cpu_i_read;
+		cpu_imem_addr	<= addr;
+		cpu_imem_en		<= '1';
+		wait until rising_edge(CLK) and cpu_imem_rdy = '1';
+		cpu_imem_en		<= '0';
+
+	end procedure cpu_i_read;
+
+	procedure cpu_d_read(addr : unsigned) is
+	begin
+
+		op_descr <= op_cpu_d_read;
+		cpu_dmem_addr	<= addr;
+		cpu_dmem_en		<= '1';
+		wait until rising_edge(CLK) and cpu_dmem_rdy = '1';
+		cpu_dmem_en		<= '0';
+
+	end procedure cpu_d_read;
+
+	procedure cpu_d_write(addr, data : unsigned) is
+	begin
+
+		op_descr <= op_cpu_d_write;
+		cpu_dmem_we		<= '1';
+		cpu_dmem_addr	<= addr;
+		cpu_dmem_din	<= data;
+		cpu_dmem_en		<= '1';
+		wait until rising_edge(CLK) and cpu_dmem_rdy = '1';
+		cpu_dmem_en		<= '0';
+
+	end procedure cpu_d_write;
+
+	begin
+
+		wait for 6*CLK_PERIOD;
+		RST <= '0';
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK) and cpu_imem_rdy = '1' and cpu_dmem_rdy = '1';
+
+		cmd_entry_i_write(0, "110000", X"0000_0", X"00DE_C", '0', '0', '0', '1');
+		cmd_entry_i_write(1, "110000", X"0000_1", X"11DE_C", '0', '0', '0', '1');
+		cmd_entry_i_write(2, "110000", X"0000_2", X"22DE_C", '0', '0', '0', '1');
+		cmd_entry_i_write(3, "110000", X"0000_3", X"33DE_C", '0', '0', '0', '1');
+		cmd_entry_i_write(4, "110000", X"0000_4", X"44DE_C", '0', '0', '0', '1');
+		cmd_entry_i_write(5, "110000", X"0000_5", X"55DE_C", '0', '0', '0', '1');
+		cmd_entry_i_write(6, "110000", X"0000_6", X"66DE_C", '0', '0', '0', '1');
+		cmd_entry_i_write(7, "110000", X"0000_7", X"77DE_C", '0', '0', '0', '1');
+		
+		cmd_entry_d_write(0, "110000", X"0000_0", X"00DE_C", '0', '0', '0', '1');
+		cmd_entry_d_write(1, "110000", X"0000_1", X"11DE_C", '0', '0', '0', '1');
+		cmd_entry_d_write(2, "110000", X"0000_2", X"22DE_C", '0', '0', '0', '1');
+		cmd_entry_d_write(3, "110000", X"0000_3", X"33DE_C", '0', '0', '0', '1');
+		cmd_entry_d_write(4, "110000", X"0000_4", X"44DE_C", '0', '0', '0', '1');
+		cmd_entry_d_write(5, "110000", X"0000_5", X"55DE_C", '0', '0', '0', '1');
+		cmd_entry_d_write(6, "110000", X"0000_6", X"66DE_C", '0', '0', '0', '1');
+		cmd_entry_d_write(7, "110000", X"0000_7", X"77DE_C", '0', '0', '0', '1');
+
+		cpu_i_read(X"BFC0_0200");
+		cpu_i_read(X"BFC0_0180");
+		cpu_d_read(X"BFC0_0200");
+
+		cpu_i_read(X"8000_0204");
+		cpu_i_read(X"8000_0300");
+		cpu_i_read(X"8000_0304");
+		cpu_i_read(X"8000_0308");
+		
+		cpu_i_read(X"9000_0208");
+		cpu_i_read(X"9000_030C");
+		cpu_i_read(X"9100_0310");
+		cpu_i_read(X"9200_0314");
+
+		cpu_i_read(X"A000_020C");
+		cpu_i_read(X"A000_0318");
+		cpu_i_read(X"A100_031C");
+		cpu_i_read(X"A200_0320");
+		
+		cpu_i_read(X"B000_0210");
+		cpu_i_read(X"B000_0324");
+		cpu_i_read(X"B000_0328");
+		cpu_i_read(X"B000_032C");
+
+		cpu_i_read(X"0000_0400");
+		cpu_i_read(X"0000_1420");
+		cpu_i_read(X"0000_2440");
+		cpu_i_read(X"0000_3460");
+		cpu_i_read(X"0000_4480");
+		cpu_i_read(X"0000_54A0");
+		cpu_i_read(X"0000_64C0");
+		cpu_i_read(X"0000_74E0");
+		cpu_i_read(X"0000_8500");
+		cpu_i_read(X"0000_9520");
+		
+		cpu_i_read(X"0000_0400");
+		cpu_i_read(X"0000_1420");
+		cpu_i_read(X"0000_2440");
+		cpu_i_read(X"0000_3460");
+		cpu_i_read(X"0000_4480");
+		cpu_i_read(X"0000_54A0");
+		cpu_i_read(X"0000_64C0");
+		cpu_i_read(X"0000_74E0");
+
+
+		cpu_i_read(X"0000_0400");
+
+		cpu_i_read(X"0000_0410");
+		cpu_i_read(X"0000_0404");
+		cpu_i_read(X"0000_0408");
+		cpu_i_read(X"0000_0404");
+		cpu_i_read(X"0000_0434");
+		cpu_i_read(X"0000_0400");
+
+
+		---------------------------------------------
+		cpu_d_read(X"BFC0_0200");
+		cpu_d_read(X"BFC0_0180");
+
+		cpu_d_read(X"8000_0204");
+		cpu_d_read(X"8000_0300");
+		cpu_d_read(X"8000_0304");
+		cpu_d_read(X"8000_0308");
+		
+		cpu_d_read(X"9000_0208");
+		cpu_d_read(X"9000_030C");
+		cpu_d_read(X"9100_0310");
+		cpu_d_read(X"9200_0314");
+
+		cpu_d_read(X"A000_020C");
+		cpu_d_read(X"A000_0318");
+		cpu_d_read(X"A100_031C");
+		cpu_d_read(X"A200_0320");
+		
+		cpu_d_read(X"B000_0210");
+		cpu_d_read(X"B000_0324");
+		cpu_d_read(X"B000_0328");
+		cpu_d_read(X"B000_032C");
+
+		cpu_d_read(X"0000_0400");
+		cpu_d_read(X"0000_1420");
+		cpu_d_read(X"0000_2440");
+		cpu_d_read(X"0000_3460");
+		cpu_d_read(X"0000_4480");
+		cpu_d_read(X"0000_54A0");
+		cpu_d_read(X"0000_64C0");
+		cpu_d_read(X"0000_74E0");
+		cpu_d_read(X"0000_8500");
+		cpu_d_read(X"0000_9520");
+		
+		cpu_d_read(X"0000_0400");
+		cpu_d_read(X"0000_1420");
+		cpu_d_read(X"0000_2440");
+		cpu_d_read(X"0000_3460");
+		cpu_d_read(X"0000_4480");
+		cpu_d_read(X"0000_54A0");
+		cpu_d_read(X"0000_64C0");
+		cpu_d_read(X"0000_74E0");
+		cpu_d_write(X"0000_0400", X"AAAA_AAAA");
+
+		cpu_d_read(X"0000_0400");
+
+		cpu_d_read(X"0000_0410");
+		cpu_d_read(X"0000_0404");
+		cpu_d_read(X"0000_0408");
+		cpu_d_write(X"0000_0404", X"5555_5555");
+		cpu_d_read(X"0000_0404");
+		cpu_d_write(X"0000_0404", X"6666_6666");
+		cpu_d_read(X"0000_0434");
+		cpu_d_write(X"0000_0400", X"1111_1111");
+		cpu_d_write(X"0000_0404", X"2222_2222");
+		cpu_d_write(X"0000_0408", X"3333_3333");
+		cpu_d_write(X"0000_040C", X"4444_4444");
+		cpu_d_read(X"0000_0400");
+
+	wait;
+	
+	end process;
+	
+END;

lib/CPUs/MIPS/src/tb_mips_cam.vhd

@@ -0,0 +1,265 @@
+LIBRARY ieee;
+use IEEE.STD_LOGIC_1164.ALL;
+USE ieee.numeric_std.ALL;
+
+library work;
+use work.mips_types.all;
+use work.mips_util_pkg.all;
+
+ENTITY tb_cam IS
+END tb_cam;
+
+ARCHITECTURE behavior OF tb_cam IS 
+
+	constant CLK_PERIOD			: time := 10 ns;
+	constant DATA_WIDTH			: integer := 20;
+	constant CAM_NUM_ENTRIES	: integer := 32;
+
+	signal CLK_O				: std_logic := '1';
+	signal RST_O				: std_logic := '1';
+	signal cam_we				: std_logic := '0';
+	signal cam_re				: std_logic := '0';
+	signal cam_vld				: std_logic := '0';
+	signal cam_rdy				: std_logic := '-';
+
+	signal cam_addr				: unsigned(lg2(CAM_NUM_ENTRIES)-1 downto 0) := (others => '0');
+	signal cam_rtag				: unsigned(DATA_WIDTH-1 downto 0) := (others => '0');
+	signal cam_wtag				: unsigned(DATA_WIDTH-1 downto 0) := (others => '0');
+
+	signal cam_hit				: unsigned (CAM_NUM_ENTRIES-1 downto 0);
+	signal cam_hit_vld			: std_logic := '-';
+
+
+BEGIN
+
+inst_cam: entity work.cam
+	GENERIC MAP
+	(
+		NUM_ENTRIES			=>	CAM_NUM_ENTRIES,
+		DATA_WIDTH			=>	DATA_WIDTH,
+		CAM_RAM_MAX_WIDTH	=>	15
+	)
+	PORT MAP
+	(
+		rst			=>	RST_O,
+		clk			=>	CLK_O,
+		clk_rd		=>	CLK_O,
+		
+		rdy			=>	cam_rdy,
+		
+		re			=>	cam_re,
+		we			=>	cam_we,
+		vld			=>	cam_vld,
+		addr		=>	cam_addr,
+		tag_rd		=>	cam_rtag,
+		tag_wr		=>	cam_wtag,
+
+		hit_vld		=>	cam_hit_vld,
+		hit			=> cam_hit
+
+	);
+
+CLK_GEN: process
+	begin
+		wait for CLK_PERIOD/2;
+		CLK_O <= not CLK_O;
+	end process;
+
+
+STIMULUS: process
+
+	variable tag				: unsigned(DATA_WIDTH-1 downto 0) := (others => '0');
+
+	procedure cam_entry_write(index : integer; data : unsigned; valid : std_logic) is
+	begin
+	
+	cam_wtag			<= data;
+	cam_addr			<= to_unsigned(index, lg2(CAM_NUM_ENTRIES));
+	cam_vld				<= valid;
+	cam_we				<= '1';
+	wait until rising_edge(CLK_O) and cam_rdy = '1';
+	cam_we				<= '0';
+	
+	end procedure cam_entry_write;
+
+	procedure cam_lookup(data : unsigned) is
+	begin
+	
+	cam_rtag			<= data;
+	cam_re				<= '1';
+	wait until rising_edge(CLK_O) and cam_rdy = '1';
+	cam_re				<= '0';
+
+	end procedure cam_lookup;
+	
+
+	begin
+
+		wait for 6*CLK_PERIOD;
+		RST_O <= '0';
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+
+		-----------------------
+		tag	:= X"CAF0_0";
+		for i in 0 to CAM_NUM_ENTRIES-1 loop
+			cam_entry_write(i, tag, '1');
+			tag	:= tag + X"0100_0";
+		end loop;
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		tag	:= X"CAF0_0";
+		for i in 0 to CAM_NUM_ENTRIES-1 loop
+			cam_lookup(tag);
+			tag	:= tag + X"0100_0";
+		end loop;
+
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+
+		RST_O <= '1';
+		wait for 6*CLK_PERIOD;
+		wait until rising_edge(CLK_O);
+		RST_O <= '0';
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		-----------------------
+		tag	:= X"CAF0_0";
+		for i in 0 to CAM_NUM_ENTRIES-1 loop
+			cam_entry_write(i, tag, '1');
+			tag	:= tag + X"0010_0";
+		end loop;
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		tag	:= X"CAF0_0";
+		for i in 0 to CAM_NUM_ENTRIES-1 loop
+			cam_lookup(tag);
+			tag	:= tag + X"0010_0";
+		end loop;
+
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+
+		RST_O <= '1';
+		wait for 6*CLK_PERIOD;
+		wait until rising_edge(CLK_O);
+		RST_O <= '0';
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		-----------------------
+		tag	:= X"CAF0_0";
+		for i in 0 to CAM_NUM_ENTRIES-1 loop
+			cam_entry_write(i, tag, '1');
+			tag	:= tag + X"0001_0";
+		end loop;
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		tag	:= X"CAF0_0";
+		for i in 0 to CAM_NUM_ENTRIES-1 loop
+			cam_lookup(tag);
+			tag	:= tag + X"0001_0";
+		end loop;
+
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+
+		RST_O <= '1';
+		wait for 6*CLK_PERIOD;
+		wait until rising_edge(CLK_O);
+		RST_O <= '0';
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		-----------------------
+		tag	:= X"CAF0_0";
+		for i in 0 to CAM_NUM_ENTRIES-1 loop
+			cam_entry_write(i, tag, '1');
+			tag	:= tag + X"0000_1";
+		end loop;
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		tag	:= X"CAF0_0";
+		for i in 0 to CAM_NUM_ENTRIES-1 loop
+			cam_lookup(tag);
+			tag	:= tag + X"0000_1";
+		end loop;
+
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+
+		RST_O <= '1';
+		wait for 6*CLK_PERIOD;
+		wait until rising_edge(CLK_O);
+		RST_O <= '0';
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		-----------------------
+		tag	:= X"CAF0_0";
+		for i in 0 to CAM_NUM_ENTRIES-1 loop
+			cam_entry_write(i, tag, '1');
+			tag	:= tag + X"0102_3";
+		end loop;
+		
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		tag	:= X"CAF0_0";
+		for i in 0 to CAM_NUM_ENTRIES-1 loop
+			cam_lookup(tag);
+			tag	:= tag + X"0102_3";
+		end loop;
+
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+
+		RST_O <= '1';
+		wait for 6*CLK_PERIOD;
+		wait until rising_edge(CLK_O);
+		RST_O <= '0';
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		-----------------------
+		-- overwrite
+		cam_entry_write(2, X"BEEF_2", '1');
+		wait for 8*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		
+		-- overwrite entry
+		cam_entry_write(2, X"CAFE_2", '1');
+		-- new entry
+		cam_entry_write(3, X"CAFE_3", '1');
+		cam_entry_write(4, X"CAFE_4", '1');
+
+		wait for 8*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		cam_entry_write(1, X"CAFE_1", '0');
+		cam_entry_write(2, X"CAFE_2", '0');
+		cam_entry_write(3, X"CAFE_3", '0');
+		cam_entry_write(4, X"CAFE_4", '0');
+
+		wait for 8*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		cam_lookup(X"BEEF_2");
+		cam_lookup(X"CAFE_1");
+		cam_lookup(X"CAFE_2");
+		cam_lookup(X"CAFE_3");
+		cam_lookup(X"CAFE_4");
+		
+		wait for 8*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		cam_entry_write(1, X"CAFE_1", '1');
+		cam_entry_write(2, X"CAFE_2", '1');
+		cam_entry_write(3, X"CAFE_3", '1');
+		cam_entry_write(4, X"CAFE_4", '1');
+
+		wait for 8*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		cam_lookup(X"CAFE_1");
+		cam_lookup(X"CAFE_2");
+		cam_lookup(X"CAFE_3");
+		cam_lookup(X"CAFE_4");
+
+		wait for 8*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+		cam_entry_write(7, X"CAFE_7", '0');
+
+		wait for 8*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and cam_rdy = '1';
+	wait;
+	
+	end process;
+	
+END;

lib/CPUs/MIPS/src/tb_mips_tlb.vhd

@@ -0,0 +1,211 @@
+LIBRARY ieee;
+use IEEE.STD_LOGIC_1164.ALL;
+USE ieee.numeric_std.ALL;
+
+library work;
+use work.mips_types.all;
+use work.mips_util_pkg.all;
+
+ENTITY tb_tlb IS
+END tb_tlb;
+
+ARCHITECTURE behavior OF tb_tlb IS 
+
+	constant DATA_WIDTH			: integer := 32;
+	constant CLK_PERIOD			: time := 10 ns;
+
+	signal CLK_O			: std_logic := '1';
+	signal RST_O			: std_logic := '1';
+	signal ce				: std_logic := '1';
+
+	signal tlb_rdy				: std_logic;
+	
+	signal tlb_ctrl_in		: tlb_ctrl_in_t;
+	signal tlb_ctrl_out		: tlb_ctrl_out_t;
+	signal tlb_query_in		: tlb_query_in_t;
+	signal tlb_query_out	: tlb_query_out_t;
+
+
+BEGIN
+
+inst_uut : entity work.tlb
+	GENERIC MAP
+	(
+		NUM_ENTRIES			=> TLB_NUM_ENTRIES,
+		CACHE_KSEG1			=> false,
+		USE_TLB				=> true
+	)
+	PORT MAP
+	(
+		-- System signals
+		rst					=> RST_O,
+		clk					=> CLK_O,
+		ce					=> ce,
+
+		rdy					=> tlb_rdy,
+		
+		ctrl_in				=> tlb_ctrl_in,
+		ctrl_out			=> tlb_ctrl_out,
+
+		query_in			=> tlb_query_in,
+		query_out			=> tlb_query_out
+
+	);
+
+CLK_GEN: process
+	begin
+		wait for CLK_PERIOD/2;
+		CLK_O <= not CLK_O;
+	end process;
+
+STIMULUS: process
+
+	variable result : unsigned(31 downto 0);
+
+	procedure cmd_entry_write(index : integer; asid, vpn, pfn : unsigned; g, n, d, v : std_logic) is
+	begin
+	
+	tlb_ctrl_in.entry_wr_idx	<= to_unsigned(index, lg2(TLB_NUM_ENTRIES));
+	
+	tlb_ctrl_in.entry_hi.vpn	<= vpn;
+	tlb_ctrl_in.entry_hi.asid	<= asid;
+
+	tlb_ctrl_in.entry_lo.pfn	<= pfn;	
+	tlb_ctrl_in.entry_lo.g		<= g;
+	tlb_ctrl_in.entry_lo.n		<= n;	
+	tlb_ctrl_in.entry_lo.d		<= d;	
+	tlb_ctrl_in.entry_lo.v		<= v;	
+	
+	tlb_ctrl_in.entry_we		<= '1';
+	wait until rising_edge(CLK_O) and tlb_rdy = '1';
+	tlb_ctrl_in.entry_we		<= '0';
+	
+	end procedure cmd_entry_write;
+
+	procedure cmd_read_single(addr : unsigned) is
+	begin
+	
+	end procedure cmd_read_single;
+
+	begin
+
+		tlb_ctrl_in.entry_we	<= '0';
+		tlb_ctrl_in.entry_re	<= '0';
+		tlb_query_in.vld		<= '0';
+		tlb_query_in.write		<= '0';
+		tlb_query_in.vaddr		<= X"BFC0_CAFE";
+		
+		wait for 6*CLK_PERIOD;
+		RST_O <= '0';
+		wait for 60*CLK_PERIOD;
+		wait until rising_edge(CLK_O) and tlb_rdy = '1';
+
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '1';
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '0';
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '1';
+		tlb_query_in.vaddr <= X"9FC0_CAFE";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '0';
+
+		cmd_entry_write(0, "110000", X"0000_0", X"00DE_F", '0', '0', '0', '1');
+		cmd_entry_write(1, "110000", X"0000_1", X"01DE_F", '0', '0', '0', '1');
+		cmd_entry_write(2, "110000", X"0000_2", X"02DE_F", '0', '0', '0', '0');
+		cmd_entry_write(3, "110000", X"0000_3", X"43DE_F", '0', '0', '0', '1');
+		cmd_entry_write(4, "100000", X"0000_4", X"44DE_F", '0', '0', '0', '1');
+		cmd_entry_write(5, "110000", X"0000_5", X"65DE_F", '0', '0', '0', '1');
+		cmd_entry_write(6, "100000", X"0000_6", X"66DE_F", '1', '0', '0', '1');
+		cmd_entry_write(7, "110000", X"0000_7", X"67DE_F", '0', '0', '0', '1');
+		
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '1';
+
+		tlb_query_in.vaddr <= X"8000_BABE";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"8000_1234";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"8100_1234";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"8200_1234";
+		
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"9000_BABE";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"9000_1234";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"9100_1234";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"9200_1234";
+
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"A000_BABE";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"A000_1234";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"A100_1234";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"A200_1234";
+		
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"B000_BABE";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"B000_1234";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"B100_1234";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"B200_1234";
+
+
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= X"0000_0CAF";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= tlb_query_in.vaddr + X"1000";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= tlb_query_in.vaddr + X"1000";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= tlb_query_in.vaddr + X"1000";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= tlb_query_in.vaddr + X"1000";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= tlb_query_in.vaddr + X"1000";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= tlb_query_in.vaddr + X"1000";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= tlb_query_in.vaddr + X"1000";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= tlb_query_in.vaddr + X"1000";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vaddr <= tlb_query_in.vaddr + X"1000";
+
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '0';
+
+		cmd_entry_write(0, "110000", X"0000_0", X"40DE_F", '0', '0', '1', '1');
+
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '1';
+		tlb_query_in.vaddr <= X"0000_0CAF";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '0';
+
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '1';
+		tlb_query_in.write	<= '1';
+		tlb_query_in.vaddr <= X"0008_0CAF";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '0';
+
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '1';
+		tlb_query_in.write	<= '1';
+		tlb_query_in.vaddr <= X"0000_0CAF";
+		wait until rising_edge(CLK_O);
+		tlb_query_in.vld	<= '0';
+
+	wait;
+	
+	end process;
+	
+END;

lib/CPUs/MIPS/src/tb_mips_top.vhd

@@ -25,6 +25,7 @@
 LIBRARY ieee;
 use IEEE.STD_LOGIC_1164.ALL;
 USE ieee.numeric_std.ALL;
+USE std.textio.ALL;
 
 library work;
 use work.mips_types.all;
@@ -39,10 +40,12 @@ ARCHITECTURE behavior OF tb_mips_top IS
 
 	constant SYS_FREQ	 			: real := 100.0;
 	constant CLK_PERIOD 			: time := 10 ns;
-	constant SRAM_ADDR_WIDTH		: integer := 18;		-- bits
+	constant SRAM_ADDR_WIDTH		: integer := 17;		-- bits
-	constant FLASH_ADDR_WIDTH		: integer := 18;		-- bits
+	constant FLASH_ADDR_WIDTH		: integer := 17;		-- bits
-	
+	constant WITH_TLB				: boolean := true;
-	signal debug				: unsigned(1 downto 0);
+	constant KSEG_01_TRANSLATED		: boolean := true;
+	signal debug					: chip_debug_t;
+	signal tb_fin					: STD_LOGIC := '0';
 
 	-- Master 
 	signal nmi					: STD_LOGIC := '0';
@@ -115,7 +118,43 @@ ARCHITECTURE behavior OF tb_mips_top IS
 	type word_array_t is array (natural range <>) of unsigned(31 downto 0);
 	signal sram_data : word_array_t(0 to 2**SRAM_ADDR_WIDTH-1);
 	signal flash_data : word_array_t(0 to 2**FLASH_ADDR_WIDTH-1);
+
+	subtype nibble_t is unsigned (3 downto 0);
 	
+	function to_character(x : nibble_t) return character is
+	type cnv_tbl_t is array (0 to 15) of character;
+	variable cnv_tbl : cnv_tbl_t := "0123456789ABCDEF";
+	variable result : character;
+	begin 
+		for i in nibble_t'low to nibble_t'high loop
+			result := '0';
+			if x(i) /= '0' and x(i) /= '1' then
+				result := 'X';
+			end if;			
+		end loop;
+
+		if result = '0' then
+			result := cnv_tbl(to_integer(x));
+		end if;
+
+		return result;
+	end to_character;
+	
+	
+	function to_string(x : word_t ) return string is
+	variable result : string (1 to 8);
+	variable j : integer := 32;
+	variable nibble : nibble_t;
+	begin
+		for i in 1 to 8 loop
+			nibble := x(j-1 downto j-4);
+			j := j - 4;
+			result(i) := to_character(nibble);
+		end loop;
+		return result;
+		
+	end to_string;
+
 BEGIN
 
 ------------------------------------------------------------------
@@ -128,6 +167,46 @@ CLK_GEN: process
 ------------------------------------------------------------------
 -- Memory mux
 ------------------------------------------------------------------
+--  rom_u        : Virtual = 0xBFC00000, Physical = 0x1FC00000
+--  rom_c        : Virtual = 0x9FC00000, Physical = 0x1FC00000
+
+--  ram_u        : Virtual = 0xA0000000, Physical = 0x00000000
+--  ram_c        : Virtual = 0x80000000, Physical = 0x00000000
+--  flash_u      : Virtual = 0xA4000000, Physical = 0x04000000
+--  flash_c      : Virtual = 0x88000000, Physical = 0x08000000
+--  io_u         : Virtual = 0xA8000000, Physical = 0x0C000000
+--  io_c         : Virtual = 0x88000000, Physical = 0x0C000000
+
+gen_mem_mux_translate:
+if KSEG_01_TRANSLATED = true generate
+mem_mux:
+	process(ADDR_O)
+	begin
+		mem_area			<= mem_dead;
+		flash_page_mode_en	<= '0';
+		if ADDR_O(31 downto 28) = X"0" then
+			if    ADDR_O(27 downto 26) = "00" then
+				mem_area	<= mem_sram;
+			elsif ADDR_O(27 downto 26) = "01" then
+				mem_area			<= mem_flash;
+			elsif ADDR_O(27 downto 26) = "10" then
+				mem_area			<= mem_flash;
+				flash_page_mode_en	<= '1';
+			elsif ADDR_O(27 downto 26) =  "11" then
+				if ADDR_O(18 downto 16) = "000" then
+					mem_area	<= mem_gpio;
+				elsif ADDR_O(18 downto 16) = "001" then
+					mem_area	<= mem_uart;
+				end if;	
+			end if;
+		elsif (ADDR_O(31 downto 28) = X"1" and ADDR_O(15) = '0') then
+			mem_area	<= mem_rom;
+		end if;
+	end process;	
+end generate;
+
+gen_mem_mux_non_translate:
+if KSEG_01_TRANSLATED = false generate
 mem_mux:
 	process(ADDR_O)
 	begin
@@ -152,6 +231,7 @@ mem_mux:
 			mem_area	<= mem_sram;
 		end if;
 	end process;	
+end generate;
 
 signal_mux:
 	process(mem_area, CYC_O)
@@ -199,8 +279,10 @@ signal_mux:
 uut: entity work.mips_top
 	GENERIC MAP
 	(
-		icache_size		=> 1024,	-- words
+		icache_size			=> 256,		-- words
-		dcache_size		=> 1024		-- words
+		dcache_size			=> 256,		-- words
+		WITH_TLB			=> WITH_TLB,
+		TRANSLATE_KSEG0_1	=> KSEG_01_TRANSLATED
 	)
 	PORT MAP
 	(
@@ -224,7 +306,6 @@ uut: entity work.mips_top
 	);
 	INT(1)	<= int_uart;
 	INT(5)	<= int_timer;
-	INT(2)	<= '1';
 	
 inst_rom : entity work.rom_wb
 	PORT MAP
@@ -391,12 +472,16 @@ SRAM_WRITE:
 			end if;	
 		end if;	
 	end process;
-
+	
 ------------------------------------------------------------------
 FLASH_READ:
 	process(rst, flash_cs_n, flash_oe_n, flash_a)
 	type file_t is file of integer;
-	file load_flash : file_t open read_mode is "hello.elf.flash.bin";
+--	file load_flash : file_t open read_mode is "test_tlb_sim.elf.flash.bin";
+	file load_flash : file_t open read_mode is "test_exception_sim.elf.flash.bin";
+--	file load_flash : file_t open read_mode is "test_timer.elf.flash.bin";
+--	file load_flash : file_t open read_mode is "test_dcache_sim.elf.flash.bin";
+--	file load_flash : file_t open read_mode is "dhry.flash.bin";
 	variable instr : integer;
 	variable index : natural;
 	variable temp : signed(31 downto 0);
@@ -432,6 +517,62 @@ FLASH_READ:
 		end if;	
 	end process;
 
+------------------------------------------------------------------
+icache_trace_write:
+	process(clk)
+	type file_t is file of string;
+	file f : file_t open write_mode is "icache_trace.dat";
+	variable file_opened : std_logic;
+	variable saddr : string(1 to 8);
+	variable sdata : string(1 to 8);
+	begin
+		
+		if rising_edge(clk) then
+			if rst = '1' then
+				file_opened := '1';
+			elsif file_opened = '1' then
+				if debug.imem_din_vld = '1' then
+					saddr := to_string(debug.imem_addr_reg);
+					sdata := to_string(debug.imem_din_reg);
+					write(f, saddr & " : " & sdata & CR);
+				end if;
+			else
+				file_close(f);
+				file_opened := '0';
+			end if;
+		end if;
+	end process;
+
+------------------------------------------------------------------
+dcache_trace_write:
+	process(clk)
+	type file_t is file of string;
+	file f : file_t open write_mode is "dcache_trace.dat";
+	variable file_opened : std_logic;
+	variable saddr : string(1 to 8);
+	variable sdata : string(1 to 8);
+	begin
+		
+		if rising_edge(clk) then
+			if rst = '1' then
+				file_opened := '1';
+			elsif file_opened = '1' then
+				if debug.dmem_din_vld = '1' then
+					saddr := to_string(debug.dmem_addr_reg);
+					sdata := to_string(debug.dmem_din_reg);
+					write(f, saddr & " : " & sdata & " R" & CR);
+				elsif debug.dmem_dout_vld = '1' then
+					saddr := to_string(debug.dmem_addr_reg);
+					sdata := to_string(debug.dmem_dout_reg);
+					write(f, saddr & " : " & sdata & " W" & CR);
+				end if;
+			else
+				file_close(f);
+				file_opened := '0';
+			end if;
+		end if;
+	end process;
+
 ------------------------------------------------------------------
 STIMULUS: process
 
@@ -441,10 +582,13 @@ STIMULUS: process
 		wait for 3*CLK_PERIOD;
 		wait until rising_edge(clk);
 		rst <= '0';
-		nmi <= '0';
+
+		wait for 1200000*CLK_PERIOD;
+
 		wait until rising_edge(clk);
-		wait for 1500 us;
+		tb_fin	<= '1';
 		wait until rising_edge(clk);
+		tb_fin	<= '0';
 
 		wait;