diff --git a/lib/CPUs/MIPS/src/core/dcache.vhd b/lib/CPUs/MIPS/src/core/dcache.vhd
index 85d1639..1747133 100644
--- a/lib/CPUs/MIPS/src/core/dcache.vhd
+++ b/lib/CPUs/MIPS/src/core/dcache.vhd
@@ -26,33 +26,52 @@ 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
-		line_size	: natural := 8		-- words
+		CACHE_SIZE			: natural := 1024;	-- words
+		LINE_SIZE			: natural := 8;		-- words
+		WRITE_FIFO_SIZE		: natural := 4
 	);
 	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
+		rst					: in  STD_LOGIC;
+		clk					: in  STD_LOGIC;
+		
+		en					: in  STD_LOGIC;
+		we					: in  STD_LOGIC;
+		be					: in  unsigned(3 downto 0);
+		addr				: in  word_t;
+		din					: in  word_t;
+		dout				: out word_t;
+		rdy					: out STD_LOGIC;
+		
+		-- Cache control
+		ctrl_in				: in  cache_ctrl_t;
+
+		tlb_query_out		: out tlb_query_in_t;
+		tlb_query_in		: in  tlb_query_out_t;
+
+		-- busmaster data
+		bus_din				: in word_t;
+		bus_din_rdy			: out std_logic;
+		bus_din_vld			: in 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 +102,26 @@ COMPONENT dpram_2w2r2c_ra is
 END COMPONENT;
 
 	constant addr_width			: natural := 32;
-	constant word_index_width	: natural := lg2(line_size);
-	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_parity_width	: natural := 3;
-	constant tag_ram_data_width	: natural := 1 + tag_parity_width + tag_width;
+	constant word_index_width	: natural := lg2(LINE_SIZE);
+	constant cache_index_width	: natural := lg2(CACHE_SIZE) - word_index_width;
+	constant tag_width			: natural := addr_width - lg2(CACHE_SIZE) - 2;
+	constant tag_ram_data_width	: natural := 1 + tag_width + lg2(TLB_NUM_ENTRIES)+1;
 	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;
-		tv_p	: unsigned(tag_parity_width-1 downto 0);
-		tag		: unsigned(tag_width-1 downto 0);
+		valid		: std_logic;
+		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+3 downto 4);
+		result.tag		:= x(tag_width downto 1);
+		result.idx		:= x(lg2(TLB_NUM_ENTRIES)+tag_width+1 downto tag_width+1);
 		
 		return result;
 	end to_dcache_entry;
@@ -111,153 +129,311 @@ 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(3 downto 1)								:= x.tv_p;
-		result(tag_width+3 downto 4)					:= x.tag;
+		result(0)													:= x.valid;
+		result(tag_width downto 1)									:= 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, flush, mem_request, mem_access, mem_data, rd_cache);
-	signal s, sn : cache_state_t;
+	function to_word_index(x : word_t) return unsigned is
+	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		: dcache_entry_t;
-	signal	cache_entry_out		: dcache_entry_t;
-	signal	cache_entry_out_inv	: dcache_entry_t;
-	signal  cpu_data_ram_addr	: unsigned(lg2(cache_size)-1 downto 0);
-	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;
+	function to_cache_index(x : word_t) return unsigned is
+	variable result : unsigned(cache_index_width-1 downto 0);
+	begin
+		result := x(cache_index_width+word_index_width+1 downto word_index_width+2);
+		
+		return result;
+		
+	end to_cache_index;
 
-	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	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);
+	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;
 
-	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);
-	alias req_cache_index is request_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
-	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_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, 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	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);
-	tag_inv			<= ctrl.inv_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
+	-- Print info
+	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					<= addr;
+	tlb_query_out.idx					<= cache_entry_out.idx;
+	tlb_query_out.write					<= we;
 	
-fill_address_register:
-	process(CLK_I)
+	request_pa							<= tlb_query_in.paddr;
+
+	write_hit							<= cache_req_wr and cache_hit and not tlb_query_in.exc;
+	
+	debug.req_strobe	<= req_strobe;
+	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 en;
+
+request_register:
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
-			if fill_count_en = '1' then
-				if ACK_I = '1' then
-					fill_word_index	<= fill_word_index + 1;
-				end if;	
-			elsif cache_busy = '0' then
-				fill_addr	<= cpu_addr(addr_width-1 downto 2) & "00";
+		if rising_edge(clk) then
+			if rst = '1' then
+				cache_req_rd	<= '0';
+				cache_req_wr	<= '0';
+			elsif cache_rdy = '1' then
+				cache_req_rd	<= en and not we;
+				cache_req_wr	<= en and we;
+				cache_req		<= en;
+				if we = '1' then
+					hit_cache_index		<= to_cache_index(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 request_count_en = '1'then
-				if SRDY_I = '1' then
-					req_word_index	<= req_word_index + 1;
-				end if;	
-			elsif cache_busy = '0' then
-				request_addr	<= cpu_addr(addr_width-1 downto 2) & "00";
-			end if;
-		end if;
-	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';
+		if rising_edge(clk) then
+			if rst = '1' then
+				request_va		<= (others => '0');
+			elsif req_strobe = '1' then
+				din_reg			<= din;
+				be_reg			<= be;
+				request_va		<= addr;
 			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
-				instant_raw	<= cpu_hit_we and cpu_en and not cpu_we;
+			if to_word_index(addr) = fill_word_index and to_cache_index(addr) = hit_cache_index then
+				instant_raw	<= write_hit and en and not 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(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(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 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 
 	(
@@ -266,10 +442,10 @@ inst_tag_ram : dpram_2w2r2c_ra
 	)
 	PORT MAP 
 	(
-		clk_a	=> CLK_I,
-		clk_b	=> CLK_I,
+		clk_a	=> clk,
+		clk_b	=> clk,
 		en_a	=> '1',
-		en_b	=> '1',
+		en_b	=> ram_read_en,
 		we_a	=> tag_ram_we,
 		we_b	=> '0',
 		addr_a	=> tag_ram_addr_wr,
@@ -287,35 +463,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_b	=> CLK_I,
+			clk_a	=> clk,
+			clk_b	=> clk,
 			en_a	=> '1',
-			en_b	=> '1',
-			we_a	=> cpu_data_ram_we(i),
+			en_b	=> ram_read_en,
+			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,
-			din_a	=> cpu_data_reg(8*(i+1)-1 downto 8*i),
-			din_b	=> DAT_I(8*(i+1)-1 downto 8*i),
+			addr_b	=> data_ram_addr,
+			din_a	=> din_reg(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 RST_I = '1' or ctrl.inv_all = '1' or ctrl.inv_at = '1' then
+		if rising_edge(clk) 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';
@@ -325,90 +501,143 @@ cache_invalidate_request:
 	end process;	
 
 cache_state_next:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
-			if RST_I = '1' then
-				s	<= init;
+		if rising_edge(clk) then
+			if rst = '1' then
+				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;
-	ADDR_O				<= request_addr;
-	
-	cpu_busy			<= cache_busy;
-	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		<= to_unsigned(flush_count, cache_index_width) 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_rdy						<= read_rdy and not instant_raw and write_rdy;
+	write_rdy						<= not (write_fifo_full and cache_req_wr);
+	rdy								<= cache_rdy;
 
-	cache_entry_out		<= to_dcache_entry(tag_ram_dout);
-	cache_entry_out_inv	<= to_dcache_entry(tag_ram_dout_inv);
+	cache_dout						<= single_read_reg when single_read_en = '1' else to_mips_01(data_ram_dout);
+	dout							<= 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(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						<= en or was_miss or fill_count_en;
+	data_ram_addr					<= (to_cache_index(addr) & to_word_index(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);
-	ctrl_data_ram_addr	<= fill_cache_index & fill_word_index;
-	ctrl_data_ram_we	<= (others => fill_count_en and ACK_I);
-	cpu_data_ram_we		<= cpu_be_reg when (cpu_hit_we = '1') else (others => '0');
-	
-cache_state:
-	process(s, cache_req, instant_raw, cache_hit, cache_hit_inv, flush_count_rdy, fill_count_rdy, request_count_rdy, fill_tag, SRDY_I, cpu_we_reg, invalidate_req, invalidate_all)
+cache_busy_state_fsm:
+	process(cache_busy_state, tlb_query_in, 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 tlb_query_in.flags.N = '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			<= tlb_query_in.flags.N;
+				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, tlb_query_in, 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';
-		STB_O					<= '0';
+		ram_write_en			<= '0';
+		bus_cmd_cycle_en		<= '0';
+		bus_cmd_we				<= '0';
 		was_miss				<= '0';
 		invalidate_ack			<= '0';
-
-		cache_entry_in.tv_p		<= (others => '0');
-		cache_entry_in.tag		<= fill_tag;
-		cache_entry_in.valid	<= '0';
-		sn	<= s;
+		mem_fetch_ack			<= '0';
+		read_write_buffer		<= '0';
 		
-		case s is
+		cache_entry_in.idx		<= tlb_query_in.idx;
+		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;
+				cache_ctrl_state_next 				<= invalidate;
 				invalidate_en	<= '1';
-			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;	
+			elsif mem_fetch_req = '1' or write_data_avail = '1' then
+				cache_ctrl_state_next				<= bus_request;
+--				bus_cmd_cycle_en					<= '1';
+			elsif write_through_en = '1' then
+				cache_ctrl_state_next				<= wait_write_thru;
+--				bus_cmd_cycle_en					<= '1';
 			end if;	
+
 		when invalidate =>
-			sn <= rd_cache;
+			cache_ctrl_state_next <= rd_cache;
 			invalidate_en			<= '1';
 			invalidate_ack			<= '1';
 			if invalidate_all = '1' then
-				sn	<= flush;
+				cache_ctrl_state_next					<= flush;
 				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');
-				sn						<= rd_cache;
+				cache_ctrl_state_next	<= invalidate_post;
 			end if;	
 				
 		when flush =>
@@ -419,45 +648,83 @@ 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';
-			if SRDY_I = '1' then
-				sn <= mem_access;
-			end if;	
-		when mem_access =>
-			fill_count_en		<= '1';
+
+		when invalidate_post =>
+			was_miss	<= '1';
+			cache_ctrl_state_next		<= ready;
+
+		when wait_write_thru =>
+			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 tlb_query_in.flags.N = '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';
-			STB_O				<= '1';
+			ram_write_en		<= '1';
+			fill_count_en		<= '1';
 			if request_count_rdy = '1' then
-				STB_O			<= '0';
-				sn 				<= mem_data;
+				bus_cmd_we		<= '0';
+				cache_ctrl_state_next	<= mem_read_data;
 			end if;	
-		when mem_data =>
-			CYC_O			<= '1';
-			fill_count_en	<= '1';
+
+		when mem_read_single =>
+			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:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
-			if ctrl.inv_at = '1' then
+		if rising_edge(clk) then
+			if ctrl_in.inv_at = '1' then
 				flush_count		<= to_integer(cache_index_inv);
 			elsif flush_count_rst = '1' then
 				flush_count_rdy	<= '0';
@@ -473,14 +740,14 @@ flush_counter:
 	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
@@ -492,14 +759,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 = '1' then
+				if bus_din_vld = '1' then
 					if fill_count /= 0 then
 						fill_count 	<= fill_count - 1;
 					else
diff --git a/lib/CPUs/MIPS/src/core/icache.vhd b/lib/CPUs/MIPS/src/core/icache.vhd
index 259952a..2ae1326 100644
--- a/lib/CPUs/MIPS/src/core/icache.vhd
+++ b/lib/CPUs/MIPS/src/core/icache.vhd
@@ -26,30 +26,44 @@ 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
-		line_size	: natural := 8		-- words
+		CACHE_SIZE	: natural := 1024;	-- words
+		LINE_SIZE	: natural := 8		-- 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
+		rst					: in  STD_LOGIC;
+		clk					: in  STD_LOGIC;
+
+		-- CPU control/data
+		en					: in  STD_LOGIC;
+		addr				: in  word_t;
+		dout				: out word_t;
+		rdy					: out STD_LOGIC;
+
+		-- Cache control
+		ctrl_in				: in  cache_ctrl_t;
+
+		tlb_query_out		: out tlb_query_in_t;
+		tlb_query_in		: in  tlb_query_out_t;
+
+		-- busmaster data
+		bus_din				: in word_t;
+		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;
 
@@ -98,27 +112,26 @@ COMPONENT dpram_2w2r2c_ra is
 END COMPONENT;
 
 	constant addr_width			: natural := 32;
-	constant word_index_width	: natural := lg2(line_size);
-	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_parity_width	: natural := 3;
-	constant tag_ram_data_width	: natural := 1 + tag_parity_width + tag_width;
+	constant word_index_width	: natural := lg2(LINE_SIZE);
+	constant cache_index_width	: natural := lg2(CACHE_SIZE) - word_index_width;
+	constant tag_width			: natural := addr_width - lg2(CACHE_SIZE) - 2;
+	constant tag_ram_data_width	: natural := 1 + tag_width + lg2(TLB_NUM_ENTRIES);
 	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;
-		tv_p	: unsigned(tag_parity_width-1 downto 0);
-		tag		: unsigned(tag_width-1 downto 0);
+		valid		: std_logic;
+		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+3 downto 4);
+		result.tag		:= x(tag_width downto 1);
+		result.tlb_idx	:= x(lg2(TLB_NUM_ENTRIES)+tag_width downto tag_width+1);
 		
 		return result;
 	end to_icache_entry;
@@ -126,133 +139,219 @@ 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(3 downto 1)								:= x.tv_p;
-		result(tag_width+3 downto 4)					:= x.tag;
+		result(0)													:= x.valid;
+		result(tag_width downto 1)									:= 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, flush, mem_request, mem_access, mem_data, rd_cache, upd_cache);
-	signal s, sn : cache_state_t;
+	function to_word_index(x : word_t) return unsigned is
+	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_miss			: std_logic;
-	signal	tag_match			: std_logic;
-	signal	cache_miss_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  tag_ram_addr_rd		: unsigned(cache_index_width-1 downto 0);
-	signal  tag_ram_data_rd		: tag_ram_data_t;
-	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	was_miss			: std_logic;
-	signal	invalidate_all		: std_logic;
-	signal	invalidate_ack		: std_logic;
-	signal  invalidate_en		: std_logic;
-	signal  invalidate_req		: 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);
+	function to_cache_index(x : word_t) return unsigned is
+	variable result : unsigned(cache_index_width-1 downto 0);
+	begin
+		result := x(cache_index_width+word_index_width+1 downto word_index_width+2);
+		
+		return result;
+		
+	end to_cache_index;
 
-	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);
+	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;
 
-	alias req_word_index is request_addr(word_index_width+1 downto 2);
-	alias req_cache_index is request_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
-	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_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;
+	
+	type debug_t is record
+		ready			: std_logic;
+		req_strobe		: std_logic;
+		req_read		: std_logic;
+		req_va			: word_t;
+		req_pa			: word_t;
+		dout_reg		: word_t;
+	end record;
+
+	signal debug		: debug_t;
+
+	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	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			: 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;
+
+	signal  tag_ram_addr_rd			: unsigned(cache_index_width-1 downto 0);
+	signal  tag_ram_data_rd			: tag_ram_data_t;
+	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
 
-	ram_read_en		<= cpu_en or was_miss;
-	cache_index_inv	<= ctrl.inv_addr(cache_index_width+word_index_width+1 downto word_index_width+2);
-	tag_inv			<= ctrl.inv_addr(tag_width+cache_index_width+word_index_width+1 downto cache_index_width+word_index_width+2);
+	-- Print info
+	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					<= addr;
+	tlb_query_out.write					<= '0';
 	
+	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);
+
+	request_pa							<= tlb_query_in.paddr;
+
+	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 en;
+
+ctrl_request_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if cache_rdy = '1' then
+				cache_req_rd	<= en;
+			end if;
+		end if;
+	end process;	
+
+-------------------------------------------------
 fill_address_register:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
-			if fill_count_en = '1' then
-				if ACK_I = '1' then
-					fill_word_index	<= fill_word_index + 1;
-				end if;	
-			elsif cache_busy = '0' then
-				fill_addr	<= cpu_addr(addr_width-1 downto 2) & "00";
+		if rising_edge(clk) then
+			if rst = '1' then
+				request_va		<= (others => '0');
+			elsif req_strobe = '1' then
+				request_va	<= addr;
+				fill_word_index	<= to_word_index(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:
-	process(CLK_I)
+bus_request_address_register:
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
-			if request_count_en = '1'then
-				if SRDY_I = '1' then
-					req_word_index	<= req_word_index + 1;
-				end if;	
-			elsif cache_busy = '0' then
-				request_addr	<= cpu_addr(addr_width-1 downto 2) & "00";
-			end if;
+		if rising_edge(clk) then
+			if req_strobe = '1' then
+				req_word_index	<= to_word_index(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;	
 
-cpu_request_register:
-	process(CLK_I)
+single_read_register:
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
-			if RST_I = '1' then
-				cache_req	<= '0';
-			elsif cpu_en = '1' then
-				if cache_busy = '0' then
-					cache_req			<= '1';
+		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 en = '1' then
+					single_read_en <= '0';
+					single_read_reg_vld <= '0';
 				end if;	
-			elsif cache_ack = '1' then
-				cache_req			<= '0';
+			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;	
 
 cache_invalidate_request:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
-			if RST_I = '1' or ctrl.inv_all = '1' or ctrl.inv_at = '1' then
+		if rising_edge(clk) 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';
@@ -269,8 +368,8 @@ inst_tag_ram : dpram_2w2r2c_ra
 	)
 	PORT MAP
 	(
-		clk_a	=> CLK_I,
-		clk_b	=> CLK_I,
+		clk_a	=> clk,
+		clk_b	=> clk,
 		en_a	=> '1',
 		en_b	=> ram_read_en,
 		we_a	=> tag_ram_we,
@@ -286,13 +385,13 @@ 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_b	=> CLK_I,
+		clk_a	=> clk,
+		clk_b	=> clk,
 		en_a	=> '1',
 		en_b	=> ram_read_en,
 		we_a	=> data_ram_we,
@@ -303,88 +402,137 @@ inst_data_ram : dpram_1w1r2c_ra
 	);
 	
 cache_state_next:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
-			if RST_I = '1' then
-				s	<= init;
+		if rising_edge(clk) then
+			if rst = '1' then
+				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;
-	ADDR_O				<= fill_tag & fill_cache_index & req_word_index & "00";
+	bus_din_rdy						<= fill_count_en or single_read_en;
+	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;
+	rdy								<= cache_rdy;
 	
-	cpu_busy			<= cache_busy;
-	cpu_dout			<= data_ram_data_rd;
-	
-	cache_entry_out		<= to_icache_entry(tag_ram_data_rd);
-	tag_match			<= '1' when fill_tag = cache_entry_out.tag else '0';
-	cache_miss			<= not (tag_match and cache_entry_out.valid);
+	cache_dout						<= single_read_reg when single_read_en = '1' else data_ram_data_rd;
+	dout							<= cache_dout;
 
-	cache_entry_out_inv	<= to_icache_entry(tag_ram_data_rd_inv);
-	tag_match_inv		<= '1' when tag_reg_inv = cache_entry_out_inv.tag else '0';
-	cache_miss_inv		<= not (tag_match_inv and cache_entry_out_inv.valid);
+	cache_entry_out					<= to_icache_entry(to_mips_01(tag_ram_data_rd));
+	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_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 fill_cache_index;
-	tag_ram_addr_rd		<= cpu_cache_index when was_miss = '0' else fill_cache_index;
-	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;
-	data_ram_we			<= fill_count_en and ACK_I;
+	cache_entry_out_inv				<= to_icache_entry(to_mips_01(tag_ram_data_rd_inv));
+	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;
 
-cache_state:
-	process(s, cache_req, cache_miss, cache_miss_inv, flush_count_rdy, fill_count_rdy, request_count_rdy, fill_tag, SRDY_I, invalidate_req, invalidate_all)
+	ram_read_en						<= en or was_miss;
+	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(addr) when was_miss = '0' else to_cache_index(request_va);
+	data_ram_addr_rd				<= (to_cache_index(addr) & to_word_index(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, tlb_query_in, 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 tlb_query_in.flags.N = '1' or cache_hit = '0' then
+--							mem_fetch_req			<= '1';	
+							read_rdy				<= '0';
+							single_read_set			<= tlb_query_in.flags.N;
+							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, tlb_query_in, 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';
-		STB_O					<= '0';
+		ram_write_en			<= '0';
+		bus_cmd_cycle_en		<= '0';
+		bus_cmd_we				<= '0';
 		was_miss				<= '0';
 		invalidate_ack			<= '0';
-
-		cache_entry_in.tv_p		<= (others => '0');
-		cache_entry_in.tag		<= fill_tag;
-		cache_entry_in.valid	<= '0';
-		sn	<= s;
+		mem_fetch_ack			<= '0';
 		
-		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;
+				cache_ctrl_state_next 	<= invalidate;
 				invalidate_en	<= '1';
-			elsif cache_req = '1' then
-				if cache_miss = '1' then
-					sn						<= mem_request;
-					CYC_O					<= '1';
-				else
-					cache_busy 		<= '0';
-					cache_ack 		<= '1';
-				end if;	
-			end if;	
+			elsif mem_fetch_req = '1' then
+				cache_ctrl_state_next	<= bus_request;
+				bus_cmd_cycle_en		<= '1';
+			end if;
+
 		when invalidate =>
-			sn <= rd_cache;
+			cache_ctrl_state_next <= rd_cache;
 			invalidate_en			<= '1';
 			invalidate_ack			<= '1';
 			if invalidate_all = '1' then
-				sn					<= flush;
+				cache_ctrl_state_next					<= flush;
 				flush_count_rst		<= '1';
-			elsif cache_miss_inv = '0' then
+			elsif cache_hit_inv = '1' then
 				tag_ram_we				<= '1';
 				cache_entry_in.valid	<= '0';
 				cache_entry_in.tag		<= (others => '0');
-				sn						<= rd_cache;
+				cache_ctrl_state_next	<= invalidate_post;
 			end if;	
 				
 		when flush =>
@@ -395,44 +543,71 @@ 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';
-			if SRDY_I = '1' then
-				sn <= mem_access;
-			end if;	
-		when mem_access =>
+
+		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 tlb_query_in.flags.N = '1' then
+					cache_ctrl_state_next <= mem_read_single;
+				end if;	
+			end if;
+
+		when mem_read =>
+			bus_cmd_cycle_en	<= '1';
+			bus_cmd_we			<= '1';
 			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';
-				sn				<= mem_data;
+				bus_cmd_we		<= '0';
+				cache_ctrl_state_next	<= mem_read_data;
 			end if;	
-		when mem_data =>
-			CYC_O			<= '1';
-			fill_count_en	<= '1';
+
+		when mem_read_single =>
+			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:
-	process(CLK_I)
+	process(clk)
 	begin
-		if rising_edge(CLK_I) then
-			if ctrl.inv_at = '1' then
+		if rising_edge(clk) then
+			if ctrl_in.inv_at = '1' then
 				flush_count		<= to_integer(cache_index_inv);
 			elsif flush_count_rst = '1' then
 				flush_count_rdy	<= '0';
@@ -448,14 +623,14 @@ flush_counter:
 	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
@@ -467,14 +642,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 = '1' then
+				if bus_din_vld = '1' then
 					if fill_count /= 0 then
 						fill_count 	<= fill_count - 1;
 					else
diff --git a/lib/CPUs/MIPS/src/core/mips_bui.vhd b/lib/CPUs/MIPS/src/core/mips_bui.vhd
index e2aae9f..d0da9ea 100644
--- a/lib/CPUs/MIPS/src/core/mips_bui.vhd
+++ b/lib/CPUs/MIPS/src/core/mips_bui.vhd
@@ -27,425 +27,317 @@ 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_line	: natural := 8;		-- words
-		dcache_size	: natural := 2048;	-- words
-		dcache_line	: natural := 8		-- words
+		ICACHE_SIZE		: natural := 2048;	-- words
+		ICACHE_LINE		: natural := 8;		-- words
+		DCACHE_SIZE		: natural := 2048;	-- words
+		DCACHE_LINE		: natural := 8;		-- words
+		WRITE_FIFO_SIZE	: natural := 4;		-- words
+		WITH_TLB		: boolean
 	);
 	Port
 	(
+		clk				: in  STD_LOGIC;
+		busy			: out STD_LOGIC;
+
+		imem_err		: out STD_LOGIC;
+		imem_rdy		: out STD_LOGIC;
+		imem_en			: in  STD_LOGIC;
+		imem_addr		: in  word_t;
+		imem_dout		: out word_t;
+		dmem_err		: out STD_LOGIC;
+		dmem_rdy		: out STD_LOGIC;
+		dmem_en			: in  STD_LOGIC;
+		dmem_we			: in  STD_LOGIC;
+		dmem_be			: in  unsigned(3 downto 0);
+		dmem_din		: in  word_t;
+		dmem_dout		: out word_t;
+		dmem_addr		: in  word_t;
+
 		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);
-		DAT_O			: out unsigned(31 downto 0);
+		MDAT_I			: in  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;
-		cpu_imem_err	: out STD_LOGIC;
-		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
+		
+		ctrl_in			: in  biu_ctrl_in_t;
+		ctrl_out		: out biu_ctrl_out_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 bout_fifo_re		: 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 RST				: std_logic;
+	signal CPU_CLK			: std_logic;
 
-	signal bin_fifo_din		: unsigned(31 downto 0);
-	signal bin_fifo_dout	: unsigned(31 downto 0);
-	signal bin_fifo_re		: std_logic;
-	signal bin_fifo_we		: std_logic;
-	signal bin_fifo_full	: std_logic;
-	signal bin_fifo_empty	: std_logic;
+	-- busmaster signals
+	signal bus_din						: word_t;
+	signal bus_din_rdy					: std_logic;
+	signal bus_din_we					: std_logic;
 
-	alias bout_fifo_addr_in is bout_fifo_din(31 downto 0);
-	alias bout_fifo_data_in is bout_fifo_din(63 downto 32);
-	alias bout_fifo_sel_in is bout_fifo_din(67 downto 64);
-	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 bus_dout						: word_t;
+	signal bus_dout_vld					: std_logic;
+	signal bus_dout_re					: std_logic;
 
-	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 bus_cmd_rdy					: std_logic;
+	signal bus_cmd_we					: std_logic;
+	signal bus_cmd_cycle_en				: std_logic;
+	signal bus_cmd						: bm_cmd_t;
+	signal bus_cyc_complete				: 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 i_bus_din_rdy				: std_logic;
+	signal i_bus_cmd_rdy				: std_logic;
+	signal i_bus_cmd_we					: std_logic;
+	signal i_bus_cmd_cycle_en			: std_logic;
+	signal i_bus_cmd					: bm_cmd_t;
+
+	signal i_tlb_ctrl_in				: tlb_ctrl_in_t;
+	signal i_tlb_ctrl_out				: tlb_ctrl_out_t;
+	signal i_tlb_qry_in					: tlb_query_in_t;
+	signal i_tlb_qry_out				: tlb_query_out_t;
+	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_ctrl_in				: tlb_ctrl_in_t;
+	signal d_tlb_ctrl_out				: tlb_ctrl_out_t;
+	signal d_tlb_qry_in					: tlb_query_in_t;
+	signal d_tlb_qry_out				: tlb_query_out_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;
 
-	signal read_cycle	: std_logic;
 
 begin
 
-read_cyc_register:
-	process(cpu_clk)
+	CPU_CLK					<= clk;
+	RST						<= RST_I;
+	imem_rdy				<= i_cache_rdy;
+	dmem_rdy				<= d_cache_rdy;
+
+	ctrl_out.itlb			<= i_tlb_ctrl_out;
+	ctrl_out.dtlb			<= d_tlb_ctrl_out;
+	
+	
+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';
-			else
-				read_cycle 		<= (dmem_mem_rd_gnt and CYC_O_dmem_rd) 
-								or (dcache_mem_gnt and CYC_O_dcache) 
-								or (icache_mem_gnt and CYC_O_icache);
+				busy	<= '1';
+			elsif (i_tlb_rdy and i_cache_rdy and d_cache_rdy and d_tlb_rdy) = '1' then
+				busy	<= '0';
 			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 after 4.5 ns;
-	busy			<= CYC_O_dmem_rd or dcache_busy or (write_fifo_full);
-	cpu_dmem_rdy	<= not busy after 4.5 ns;
-	
-inst_icache : icache
+inst_busmaster : entity work.busmaster_async
 	GENERIC MAP
 	(
-		cache_size	=> icache_size,	-- words
-		line_size	=> icache_line	-- words
-	)	
-	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
+		DATA_WIDTH			=> 32,
+		FIFO_DEPTH			=> 16
 	)
 	PORT MAP
 	(
-		CLK_I		=> cpu_clk,
-		RST_I		=> RST_I,
-		CYC_O		=> CYC_O_dcache,
-		STB_O		=> STB_O_dcache,
-		ADDR_O		=> ADDR_O_dcache,
-		MRDY_O		=> MRDY_O_dcache,
-		DAT_I		=> DAT,
-		ACK_I		=> ACK,
-		SRDY_I		=> SRDY_I_dcache,
-		ctrl		=> cop0_ctrl_in.dcache,
-		cpu_en		=> dcache_en,
-		cpu_we		=> cpu_dmem_we,
-		cpu_be		=> cpu_dmem_be,
-		cpu_addr	=> cpu_dmem_addr,
-		cpu_din		=> cpu_dmem_dout,
-		cpu_dout	=> dcache_dout,
-		cpu_busy	=> dcache_busy
+		-- System signals
+		rst					=> RST,
+		clk					=> CPU_CLK,
+
+		cmd_cycle_finished	=> bus_cyc_complete,
+		cmd_cycle_en		=> bus_cmd_cycle_en,
+		
+		cmd_rdy				=> bus_cmd_rdy,
+		cmd_we				=> bus_cmd_we,
+		cmd_in				=> bus_cmd,
+		
+		din_rdy				=> bus_din_rdy,
+		din_we				=> bus_din_we,
+		din					=> bus_din,
+
+		dout				=> bus_dout,
+		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;
-
-	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
+inst_i_tlb : entity work.tlb
 	GENERIC MAP
 	(
-		addr_width			=> 4,
-		data_width			=> 32,
-		do_last_read_update	=> true
+		NUM_ENTRIES			=> TLB_NUM_ENTRIES,
+		CACHE_KSEG1			=>	false,
+		USE_TLB				=>	WITH_TLB
 	)
 	PORT MAP
 	(
-		rst			=>	RST_I,
-		clk_w		=>	CLK_I,
-		clk_r		=>	cpu_clk,
-		we			=>	bin_fifo_we,
-		re			=>	bin_fifo_re,
-		fifo_full	=>	bin_fifo_full,
-		fifo_empty	=>	bin_fifo_empty,
-		fifo_afull	=>	open,
-		fifo_aempty	=>	open,
-		data_w		=>  bin_fifo_din,
-		data_r		=>  bin_fifo_dout
+		-- System signals
+		rst					=> RST,
+		clk					=> CPU_CLK,
+		ce					=> '1',
+
+		rdy					=> i_tlb_rdy,
+
+		ctrl_in				=> i_tlb_ctrl_in,
+		ctrl_out			=> i_tlb_ctrl_out,
+
+		query_in			=> i_tlb_qry_in,
+		query_out			=> i_tlb_qry_out
+
 	);
+
+	i_tlb_ctrl_in			<= ctrl_in.itlb;
 	
-	bin_fifo_din	<= DAT_I;
-	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
+inst_icache: entity work.icache
 	GENERIC MAP
 	(
-		addr_width			=> 4,
-		data_width			=> 69,
-		do_last_read_update	=> false
+		CACHE_SIZE			=>	ICACHE_SIZE,
+		LINE_SIZE			=>	ICACHE_LINE
 	)
 	PORT MAP
 	(
-		rst			=>	RST_I,
-		clk_w		=>	cpu_clk,
-		clk_r		=>	CLK_I,
-		we			=>	bout_fifo_we,
-		re			=>	bout_fifo_re,
-		fifo_full	=>	bout_fifo_full,
-		fifo_empty	=>	bout_fifo_empty,
-		fifo_afull	=>	open,
-		fifo_aempty	=>	open,
-		data_w		=>  bout_fifo_din,
-		data_r		=>  bout_fifo_dout
+		rst					=>	RST,
+		clk					=>	CPU_CLK,
+
+		-- CPU control/data
+		en					=>	imem_en,
+		addr				=>	imem_addr,
+		dout				=>	imem_dout,
+		rdy					=>	i_cache_rdy,
+
+		-- Cache control
+		ctrl_in				=>	ctrl_in.icache,
+		tlb_query_in		=>  i_tlb_qry_out,
+		tlb_query_out		=>  i_tlb_qry_in,
+		
+		-- 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;
-
-	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
+inst_d_tlb : entity work.tlb
 	GENERIC MAP
 	(
-		addr_width			=> 4,
-		data_width			=> 68,
-		do_last_read_update	=> true
+		NUM_ENTRIES			=> TLB_NUM_ENTRIES,
+		CACHE_KSEG1			=>	false,
+		USE_TLB				=>	WITH_TLB
 	)
 	PORT MAP
 	(
-		rst			=>	RST_I,
-		clk			=>	cpu_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
+		-- System signals
+		rst					=> RST,
+		clk					=> CPU_CLK,
+		ce					=> '1',
+
+		rdy					=> d_tlb_rdy,
+
+		ctrl_in				=> d_tlb_ctrl_in,
+		ctrl_out			=> d_tlb_ctrl_out,
+
+		query_in			=> d_tlb_qry_in,
+		query_out			=> d_tlb_qry_out
+
 	);
 
-	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;
+	d_tlb_ctrl_in			<= ctrl_in.dtlb;
+
+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,
+
+		-- CPU control/data
+		en					=>	dmem_en,
+		we					=>	dmem_we,
+		addr				=>	dmem_addr,
+		be					=>	dmem_be,
+		din					=>	dmem_din,
+		dout				=>	dmem_dout,
+		rdy					=>	d_cache_rdy,
+
+		-- Cache control
+		ctrl_in				=>	ctrl_in.dcache,
+		tlb_query_in		=>  d_tlb_qry_out,
+		tlb_query_out		=>  d_tlb_qry_in,
+
+		-- 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
+	);
 
-	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
@@ -455,61 +347,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';
-		dcache_mem_gnt		<= '0';
-		dmem_mem_rd_gnt		<= '0';	
-		dmem_mem_wr_gnt		<= '0';	
-		STB_O_dmem_rd		<= '0';
-		STB_O_dmem_wr		<= '0';
+		icache_bus_gnt		<= '0';
+		dcache_bus_gnt		<= '0';
+
+		i_bus_cmd_rdy		<= '0';
+		d_bus_cmd_rdy		<= '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
-				sn			<= write_bus;
-			elsif CYC_O_dmem_rd = '1' then
-				sn			<= read_bus;
-			elsif CYC_O_icache = '1' then
+			if i_bus_cmd_cycle_en = '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';
-			if CYC_O_icache = '0' then
+			icache_bus_gnt		<= '1';
+			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';
-			if CYC_O_dcache = '0' then
+			dcache_bus_gnt		<= '1';
+			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;		
@@ -517,9 +399,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;
@@ -534,15 +416,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';
-				cpu_dmem_err	<= '0';
+				imem_err	<= '0';
+				dmem_err	<= '0';
 			elsif bus_timeout = '1' then
-				cpu_imem_err	<= icache_mem_gnt;
-				cpu_dmem_err	<= dcache_mem_gnt or dmem_mem_wr_gnt or dmem_mem_rd_gnt;
+				imem_err	<= icache_bus_gnt;
+				dmem_err	<= dcache_bus_gnt;
 			end if;
 		end if;
 	end process;	
diff --git a/lib/CPUs/MIPS/src/core/mips_cop.vhd b/lib/CPUs/MIPS/src/core/mips_cop.vhd
index af898eb..03c5389 100644
--- a/lib/CPUs/MIPS/src/core/mips_cop.vhd
+++ b/lib/CPUs/MIPS/src/core/mips_cop.vhd
@@ -26,6 +26,7 @@ use IEEE.STD_LOGIC_1164.ALL;
 use IEEE.numeric_std.ALL;
 
 use work.mips_types.all;
+use work.mips_util_pkg.all;
 use work.mips_instr.all;
 
 entity cop is
@@ -54,20 +55,73 @@ end cop;
 
 architecture Behavioral of cop is
 
-	signal epc				: word_t;
-	signal cause			: word_t;
-	signal status			: word_t;
-	signal imem_addr		: word_t;
-	signal imem_addr_we		: STD_LOGIC;
-	signal BadVAddr			: word_t;
+	type cp0_regs_t is record
+		index		: word_t;
+		random		: word_t;
+		entry_lo	: word_t;
+		config		: word_t;
+		context		: word_t;
+		pagemask	: word_t;
+		wired		: word_t;
+		badvaddr	: word_t;
+		entry_hi	: word_t;
+		status		: word_t;
+		cause		: word_t;
+		epc			: word_t;
+		prid		: word_t;
+		test		: word_t;
+	end record;
+	
+	signal	regs				: cp0_regs_t;
+	constant COP0_INDEX			: natural :=  0;
+	constant COP0_RANDOM		: natural :=  1;
+	constant COP0_ENTRY_LO		: natural :=  2;
+	constant COP0_CONFIG		: natural :=  3;
+	constant COP0_CONTEXT		: natural :=  4;
+	constant COP0_PAGEMASK		: natural :=  5;
+	constant COP0_WIRED			: natural :=  6;
+	constant COP0_07			: natural :=  7;
+	constant COP0_BADVADDR		: natural :=  8;
+	constant COP0_09			: natural :=  9;
+	constant COP0_ENTRY_HI		: natural := 10;
+	constant COP0_0B			: natural := 11;
+	constant COP0_STATUS		: natural := 12;
+	constant COP0_CAUSE			: natural := 13;
+	constant COP0_EPC			: natural := 14;
+	constant COP0_PRID			: natural := 15;
+	constant COP0_10			: natural := 16;
+	constant COP0_11			: natural := 17;
+	constant COP0_12			: natural := 18;
+	constant COP0_13			: natural := 19;
+	constant COP0_14			: natural := 20;
+	constant COP0_15			: natural := 21;
+	constant COP0_16			: natural := 22;
+	constant COP0_17			: natural := 23;
+	constant COP0_18			: natural := 24;
+	constant COP0_19			: natural := 25;
+	constant COP0_1A			: natural := 26;
+	constant COP0_1B			: natural := 27;
+	constant COP0_1C			: natural := 28;
+	constant COP0_1D			: natural := 29;
+	constant COP0_1E			: natural := 30;
+	constant COP0_1F			: natural := 31;
+
+	signal EX_reg_we		: unsigned (31 downto 0);
+	signal index			: unsigned (lg2(TLB_NUM_ENTRIES)-1 downto 0);
+	signal random			: unsigned (lg2(TLB_NUM_ENTRIES)-1 downto 0);
+	signal PTEBase			: unsigned (10 downto 0);
+	signal itlb_exc			: STD_LOGIC;
+	signal dtlb_exc			: STD_LOGIC;
+	signal itlb_entry_hi	: tlb_entry_hi_t;
+	signal itlb_entry_lo	: tlb_entry_lo_t;
+	signal dtlb_entry_hi	: tlb_entry_hi_t;
+	signal dtlb_entry_lo	: tlb_entry_lo_t;
+	signal entry_hi			: tlb_entry_hi_t;
+	signal entry_lo			: tlb_entry_lo_t;
 	signal exc_code			: unsigned(4 downto 0);
-	signal test_reg			: word_t;
-	signal test_reg_we		: STD_LOGIC;
-	signal stat_reg_we		: STD_LOGIC;
-	signal eflags_reg_we	: STD_LOGIC;
+	signal tlb_sel			: STD_LOGIC;
 	signal epc_reg_we		: STD_LOGIC;
 	signal code_reg_we		: STD_LOGIC;
-	signal ip_reg_we		: STD_LOGIC;
 	signal bd				: STD_LOGIC;
 	signal sw_int			: unsigned(1 downto 0);
 	signal ip				: unsigned(7 downto 0);
@@ -97,6 +151,10 @@ architecture Behavioral of cop is
 		we			: std_logic;
 		re			: std_logic;
 		cs			: std_logic;
+		TLBP		: std_logic;
+		TLBR		: std_logic;
+		TLBWI		: std_logic;
+		TLBWR		: std_logic;
 	end record;
 	
 	signal cop_pipe_ID : cop_pipe_t;
@@ -116,11 +174,87 @@ architecture Behavioral of cop is
 	type exc_state_t is (exc_idle, exc_pipe_flush, exc_commit, exc_inject, exc_enter, exc_leave, exc_left);
 	signal exc_state, exc_staten : exc_state_t;
 	
+	function to_tlb_entry_lo(x : word_t) return tlb_entry_lo_t is
+	variable result : tlb_entry_lo_t;
+	begin
+		result.PFN	:= x(31 downto lg2(TLB_PAGE_SIZE));
+		result.N 	:= x(11);
+		result.D	:= x(10);
+		result.V	:= x(9);
+		result.G 	:= x(8);
+		
+		return result;
+		
+	end to_tlb_entry_lo;
+	
+	function to_tlb_entry_hi(x : word_t) return tlb_entry_hi_t is
+	variable result : tlb_entry_hi_t;
+	begin
+		result.VPN		:= x(31 downto lg2(TLB_PAGE_SIZE));
+		result.ASID 	:= x(lg2(TLB_PAGE_SIZE)-1 downto lg2(TLB_PAGE_SIZE)-lg2(TLB_NUM_ASIDS));
+	
+		return result;
+
+	end to_tlb_entry_hi;
+
+	function to_tlb_entry_hi(x : tlb_entry_hi_t; va : word_t) return tlb_entry_hi_t is
+	variable result : tlb_entry_hi_t;
+	begin
+		result.VPN		:= va(31 downto lg2(TLB_PAGE_SIZE));
+		result.ASID 	:= x.ASID;
+	
+		return result;
+
+	end to_tlb_entry_hi;
+
+	function to_reg(x : tlb_entry_lo_t) return word_t is
+	variable result : word_t;
+	begin
+		result(31 downto lg2(TLB_PAGE_SIZE))	:= x.PFN;
+		result(11)								:= x.N;
+		result(10)								:= x.D;
+		result(9)								:= x.V;
+		result(8)								:= x.G;
+		result(7 downto 0)						:= (others => '0');
+
+		return result;
+
+	end to_reg;
+
+	function to_reg(x : tlb_entry_hi_t) return word_t is
+	variable result : word_t;
+	begin
+		result(31 downto lg2(TLB_PAGE_SIZE))										:= x.VPN;
+		result(lg2(TLB_PAGE_SIZE)-1 downto lg2(TLB_PAGE_SIZE)-lg2(TLB_NUM_ASIDS))	:= x.ASID;
+		result(lg2(TLB_PAGE_SIZE)-lg2(TLB_NUM_ASIDS)-1 downto 0)					:= (others => '0');
+		
+		return result;
+		
+	end to_reg;
+
 --------------------------------------------------------------------------
 begin
 
-	icache_info 					<= "00" & to_unsigned(lg2(icache_line), 3) & to_unsigned(lg2(icache_size)-8, 3);
-	dcache_info 					<= "00" & to_unsigned(lg2(dcache_line), 3) & to_unsigned(lg2(dcache_size)-8, 3);
+	-- Pass to pipeline TLB status
+	ctrl_out.pipe.ITLB				<= ctrl_in.itlb.stat;
+	ctrl_out.pipe.DTLB				<= ctrl_in.dtlb.stat;
+	
+	-- I-TLB stuff
+	ctrl_out.itlb.entry_re			<= '1'; -- cop_pipe_ID.TLBR and not tlb_sel; -- on tlbr
+	ctrl_out.itlb.entry_we			<= (cop_pipe_EX.TLBWI or cop_pipe_EX.TLBWR) and not tlb_sel; -- on tlbwi or tlbwr
+	ctrl_out.itlb.entry_rd_idx		<= index;
+	ctrl_out.itlb.entry_wr_idx		<= random when cop_pipe_EX.TLBWR = '1' else index;
+	ctrl_out.itlb.entry_lo			<= itlb_entry_lo;
+	ctrl_out.itlb.entry_hi			<= itlb_entry_hi;
+	
+	-- D-TLB stuff
+	ctrl_out.dtlb.entry_re			<= '1'; -- cop_pipe_ID.TLBR and tlb_sel; -- on tlbr
+	ctrl_out.dtlb.entry_we			<= (cop_pipe_EX.TLBWI or cop_pipe_EX.TLBWR) and tlb_sel; -- on tlbwi or tlbwr
+	ctrl_out.dtlb.entry_rd_idx		<= index;
+	ctrl_out.dtlb.entry_wr_idx		<= random when cop_pipe_EX.TLBWR = '1' else index;
+	ctrl_out.dtlb.entry_lo			<= dtlb_entry_lo;
+	ctrl_out.dtlb.entry_hi			<= dtlb_entry_hi;
+	
 	cop_pipe_ID.cs					<= ir_en;
 	cop_pipe_ID.opc					<= extract_opc(ir);
 	cop_pipe_ID.rs					<= extract_rs(ir);
@@ -131,23 +265,151 @@ begin
 	cop_pipe_ID.re					<= cop_pipe_ID.cs when (cop_pipe_ID.opc = "111000" or (cop_pipe_ID.opc = "010000" and cop_pipe_ID.rs = "00000")) else '0';
 	cop_pipe_ID.we					<= cop_pipe_ID.cs when (cop_pipe_ID.opc = "110000" or (cop_pipe_ID.opc = "010000" and cop_pipe_ID.rs = "00100")) else '0';
 	cop_pipe_ID.RFE					<= cop_pipe_ID.cs when (cop_pipe_ID.CO = '1' and cop_pipe_ID.func = "010000") else '0';
-	ctrl_out.EB						<= EB_reg xor (status(25) and status(1));
-	ctrl_out.user_mode				<= status(1);
-	ctrl_out.exc_inject				<= inject_exc or rst;
+	cop_pipe_ID.TLBP				<= cop_pipe_ID.cs when (cop_pipe_ID.CO = '1' and cop_pipe_ID.func = "001000") else '0';
+	cop_pipe_ID.TLBR				<= cop_pipe_ID.cs when (cop_pipe_ID.CO = '1' and cop_pipe_ID.func = "000001") else '0';
+	cop_pipe_ID.TLBWI				<= cop_pipe_ID.cs when (cop_pipe_ID.CO = '1' and cop_pipe_ID.func = "000010") else '0';
+	cop_pipe_ID.TLBWR				<= cop_pipe_ID.cs when (cop_pipe_ID.CO = '1' and cop_pipe_ID.func = "000110") else '0';
+	ctrl_out.pipe.EB				<= EB_reg xor (regs.status(25) and not regs.status(1));
+	ctrl_out.pipe.user_mode			<= not regs.status(1);
+	ctrl_out.pipe.exc_inject		<= inject_exc or rst;
 	cop_pipe_ID.reg_ptr				<= cop_pipe_ID.rd when cop_pipe_ID.opc = "010000" else cop_pipe_ID.rt;
 	
-	im				<= status(15 downto 8);
-	cause			<= bd & (30 downto 16 => '0') & ip & '0' & exc_code & "00";
+	eflags.Ov						<= ctrl_in.pipe.events.alu_ovf or ctrl_in.pipe.events.alu_uvf;
+	eflags.DAdEL					<= ctrl_in.pipe.events.data_load_err;
+	eflags.DAdES					<= ctrl_in.pipe.events.data_store_err;
+	eflags.IAdEL					<= ctrl_in.pipe.events.inst_load_err;
+	eflags.IAdEK					<= ctrl_in.pipe.events.inst_priv_addr and not regs.status(1);
+	eflags.Sys						<= ctrl_in.pipe.events.syscall;
+	eflags.Bp						<= ctrl_in.pipe.events.break;
+	eflags.RI						<= ctrl_in.pipe.events.illegal;
+	eflags.Int						<= ctrl_in.pipe.events.int;
+	eflags.ITLB_MOD					<= ctrl_in.pipe.events.ITLB_MOD;
+	eflags.ITLB_LOAD				<= ctrl_in.pipe.events.ITLB_LOAD;
+	eflags.ITLB_STORE				<= ctrl_in.pipe.events.ITLB_STORE;
+	eflags.DTLB_MOD					<= ctrl_in.pipe.events.DTLB_MOD;
+	eflags.DTLB_LOAD				<= ctrl_in.pipe.events.DTLB_LOAD;
+	eflags.DTLB_STORE				<= ctrl_in.pipe.events.DTLB_STORE;
+
+	im								<= regs.status(15 downto 8);
+	entry_lo						<= itlb_entry_lo when tlb_sel = '0' else dtlb_entry_lo;
+	entry_hi						<= itlb_entry_hi when tlb_sel = '0' else dtlb_entry_hi;
+	icache_info 					<= "00" & to_unsigned(lg2(icache_line), 3) & to_unsigned(lg2(icache_size)-8, 3);
+	dcache_info 					<= "00" & to_unsigned(lg2(dcache_line), 3) & to_unsigned(lg2(dcache_size)-8, 3);
+
+	regs.index						<= '0' & '0' & (29 downto 8+lg2(TLB_NUM_ENTRIES) => '0') & index & (7 downto 0 => '0');
+	regs.random						<= (31 downto 8+lg2(TLB_NUM_ENTRIES) => '0') & random & (7 downto 0 => '0');
+	regs.entry_lo					<= to_reg(entry_lo);
+	regs.context					<= PTEBase & regs.badvaddr(31 downto 13) & "00";
+	regs.entry_hi					<= to_reg(entry_hi);
+	regs.cause						<= bd & (30 downto 16 => '0') & ip & '0' & exc_code & "00";
+	regs.prid						<= dcache_info & icache_info & X"02" & to_unsigned(REVISION ,8);
+	
+	tlb_sel							<= regs.config(0);
+	
+-- COP Register read
+cop_register_read:
+	process(reg_rptr, regs)
+	variable reg : word_t;
+	variable reg_num : natural;
+	begin
+	
+		reg_num := to_integer(reg_rptr);
+		
+		case reg_num is
+		
+		when COP0_INDEX =>			-- 00: index
+			reg	:= regs.index;
+
+		when COP0_RANDOM =>			-- 01: random
+			reg	:= regs.random;
+
+		when COP0_ENTRY_LO =>		-- 02: entry_lo
+			reg	:= regs.entry_lo;
+
+		when COP0_CONFIG =>			-- 03: config
+			reg	:= regs.config;
+		
+		when COP0_CONTEXT	=>		-- 04: Context		
+			reg	:= regs.context;
+			
+		-- 05: PageMask		
+
+		when COP0_WIRED =>			-- 06: wired
+			reg	:= regs.wired;
+
+		-- 07: Reserved		
+
+		when COP0_BADVADDR =>		-- 08: BadVAddr
+			reg	:= regs.badvaddr;
+
+		-- 09: Reserved		
+
+		when COP0_ENTRY_HI =>		-- 0A: entry_hi
+			reg	:= regs.entry_hi;
+
+		-- 0B: Reserved		
+		
+		when COP0_STATUS =>			-- 0C: Status
+			reg	:= regs.status;
+
+		when COP0_CAUSE =>			-- 0D: Cause
+			reg	:= regs.cause;
+
+		when COP0_EPC =>			-- 0E: EPC (Exception Program Counter)
+			reg	:= regs.epc;
+
+		when COP0_PRID =>			-- 0F: PRId (Processor Revision Register)
+			reg	:= regs.prid;
+
+		-- 10 .. 1E : reserved
+		
+		when COP0_1F =>				-- 1F: test
+			reg	:= regs.test;
+
+		when others =>
+			reg	:= (others => '-');
+
+		end case;	
+
+		dout	<= reg after 2 ns;
+
+	end process;
+
+-- Cop pipeline
+cop_pipe:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				cop_pipe_EX.cs	<= '0';
+				cop_pipe_EX.we	<= '0';
+				cop_pipe_EX.re	<= '0';
+			elsif ctrl_in.pipe.sdu.stall_all = '0' then
+				cop_pipe_EX		<= cop_pipe_ID;
+				if cop_pipe_ID.we = '1' then
+					reg_wptr	<= cop_pipe_ID.reg_ptr;
+				end if;
+				if cop_pipe_ID.re = '1' then
+					reg_rptr	<= cop_pipe_ID.reg_ptr;
+				end if;
+			end if;
+		end if;
+	end process;
+
+-- COP Register write strobe generation
+cop_we_gen:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+--			if ctrl_in.pipe.sdu.stall_all = '0' then
+				EX_reg_we		<= (others => '0');
+				if cop_pipe_ID.we = '1' then
+					EX_reg_we(to_integer(cop_pipe_ID.reg_ptr)) <= '1';
+				end if;
+--			end if;	
+		end if;	
+	end process;
 
-	eflags.Ov		<= ctrl_in.events.alu_ovf or ctrl_in.events.alu_uvf;
-	eflags.DAdEL	<= ctrl_in.events.data_load_err;
-	eflags.DAdES	<= ctrl_in.events.data_store_err;
-	eflags.IAdEL	<= ctrl_in.events.inst_load_err;
-	eflags.IAdEK	<= ctrl_in.events.inst_priv_addr and status(1);
-	eflags.Sys		<= ctrl_in.events.syscall;
-	eflags.Bp		<= ctrl_in.events.break;
-	eflags.RI		<= ctrl_in.events.illegal;
-	eflags.Int		<= ctrl_in.events.int;
 
 -- NMI/RST transition detector
 proc_nmi_transition:
@@ -170,80 +432,18 @@ proc_nmi_transition:
 		end if;
 	end process;
 
-
--- Main exception FSM
-proc_exc_state:
-	process(exc_state, ctrl_in, cop_pipe_ID)
+-- Endian switch sampled at hard reset
+sample_endian:
+	process(clk)
 	begin
-	
-		ctrl_out.exc_commit		<= '0';
-		ctrl_out.exc_pending	<= '0';
-		ctrl_out.exc_exit 		<= '0';
-		epc_reg_we				<= '0';	
-		code_reg_we				<= '0';	
-		status_save				<= '0';
-		status_rest				<= '0';
-		inject_exc				<= '0';
-		imem_addr_we			<= '0';
-		latch_vect_en			<= '0';
-
-		exc_staten	<= exc_state;
-		
-		case exc_state is
-		
-		when exc_idle =>
-			if cop_pipe_ID.RFE = '1' then
-				exc_staten			<= exc_leave;
-				ctrl_out.exc_exit 	<= '1';
-				ctrl_out.exc_commit	<= '1';
-			elsif ctrl_in.exc_req = '1' then
-				exc_staten			<= exc_pipe_flush;
-				imem_addr_we		<= '1';
-			end if;
-			
-		when exc_pipe_flush =>
-			ctrl_out.exc_pending	<= '1';
-			ctrl_out.exc_commit		<= '1';
-			if ctrl_in.exc_ack = '1' then
-				exc_staten			<= exc_commit;
-			end if;
-			
-		when exc_commit =>
-			latch_vect_en			<= '1';
-			ctrl_out.exc_pending	<= '1';
-			ctrl_out.exc_commit		<= '1';
-			if ctrl_in.sdu.ID_stall = '0' then
-				exc_staten			<= exc_inject;
-			end if;
-
-		when exc_inject =>
-			ctrl_out.exc_pending	<= '1';
-			ctrl_out.exc_commit		<= '1';
-			code_reg_we				<= '1';
-			status_save				<= '1';
-			epc_reg_we				<= '1';
-			inject_exc				<= '1';
-			exc_staten				<= exc_enter;
-
-		when exc_enter =>
-			ctrl_out.exc_pending	<= '1';
-			exc_staten				<= exc_idle;
-
-		when exc_leave =>
-			exc_staten				<= exc_left;
-			status_rest				<= '1';
-			ctrl_out.exc_exit 		<= '1';
-			ctrl_out.exc_commit		<= '1';
-
-		when exc_left =>
-			exc_staten				<= exc_idle;
-
-		when others =>
-			exc_staten <= exc_idle;
-		
-		end case;
-	end process;	
+		if rising_edge(clk) then
+			if rst = '1' then
+				EB_reg <= eb;
+			end if;	
+		end if;
+	end process;
 
+-- Main exception FSM next
 proc_exc_state_next:
 	process(clk)
 	begin
@@ -256,27 +456,76 @@ proc_exc_state_next:
 		end if;
 	end process;
 
--- Endian switch sampled at hard reset
-sample_endian:
-	process(clk)
+-- Main exception FSM
+proc_exc_state:
+	process(exc_state, ctrl_in, cop_pipe_ID)
 	begin
-		if rising_edge(clk) then
-			if rst = '1' then
-				EB_reg <= eb;
-			end if;	
-		end if;
-	end process;
+	
+		ctrl_out.pipe.exc_commit	<= '0';
+		ctrl_out.pipe.exc_pending	<= '0';
+		ctrl_out.pipe.exc_exit 		<= '0';
+		epc_reg_we					<= '0';	
+		code_reg_we					<= '0';	
+		status_save					<= '0';
+		status_rest					<= '0';
+		inject_exc					<= '0';
+		latch_vect_en				<= '0';
 
--- Sample imem_addr for later use
-exception_last_imem_addr:
-	process(clk)
-	begin
-		if rising_edge(clk) then
-			if imem_addr_we = '1' then
-				imem_addr <= ctrl_in.imem_addr;
-			end if;	
-		end if;
-	end process;
+		exc_staten	<= exc_state;
+		
+		case exc_state is
+		
+		when exc_idle =>
+			if cop_pipe_ID.RFE = '1' then
+				exc_staten					<= exc_leave;
+				ctrl_out.pipe.exc_exit 		<= '1';
+				ctrl_out.pipe.exc_commit	<= '1';
+			elsif ctrl_in.pipe.exc_req = '1' then
+				exc_staten					<= exc_pipe_flush;
+			end if;
+			
+		when exc_pipe_flush =>
+			ctrl_out.pipe.exc_pending		<= '1';
+			ctrl_out.pipe.exc_commit		<= '1';
+			if ctrl_in.pipe.exc_ack = '1' then
+				exc_staten					<= exc_commit;
+			end if;
+			
+		when exc_commit =>
+			latch_vect_en					<= '1';
+			ctrl_out.pipe.exc_pending		<= '1';
+			ctrl_out.pipe.exc_commit		<= '1';
+			if ctrl_in.pipe.sdu.ID_stall = '0' then
+				exc_staten					<= exc_inject;
+			end if;
+
+		when exc_inject =>
+			ctrl_out.pipe.exc_pending		<= '1';
+			ctrl_out.pipe.exc_commit		<= '1';
+			code_reg_we						<= '1';
+			status_save						<= '1';
+			epc_reg_we						<= '1';
+			inject_exc						<= '1';
+			exc_staten						<= exc_enter;
+
+		when exc_enter =>
+			ctrl_out.pipe.exc_pending		<= '1';
+			exc_staten						<= exc_idle;
+
+		when exc_leave =>
+			exc_staten						<= exc_left;
+			status_rest						<= '1';
+			ctrl_out.pipe.exc_exit 			<= '1';
+			ctrl_out.pipe.exc_commit		<= '1';
+
+		when exc_left =>
+			exc_staten						<= exc_idle;
+
+		when others =>
+			exc_staten <= exc_idle;
+		
+		end case;
+	end process;	
 
 -- EPC reg and BD-bit write
 cop_exception_epc_write:
@@ -284,15 +533,11 @@ cop_exception_epc_write:
 	begin
 		if rising_edge(clk) then
 			if rst = '1' then
-				bd		<= '0';
-				epc		<= (others => '0');
-			elsif epc_reg_we = '1' then
 				bd			<= '0';
-				epc			<= ctrl_in.epc_mem;
-				if ctrl_in.bd_wb = '1' then
-					bd			<= '1';
-					epc			<= ctrl_in.epc_wb;
-				end if;
+				regs.epc	<= (others => '0');
+			elsif epc_reg_we = '1' then
+				bd			<= ctrl_in.pipe.bd_wb;
+				regs.epc	<= ctrl_in.pipe.epc_wb;
 			end if;
 		end if;
 	end process;
@@ -302,39 +547,90 @@ cop_exception_map:
 	process(clk)
 	begin
 		if rising_edge(clk) then
+			itlb_exc	<= '0';
+			dtlb_exc	<= '0';
 			if rst = '1' then
-				exc_code	<= (others => '0');
-				BadVAddr	<= (others => '0');
+				exc_code		<= (others => '0');
+				regs.badvaddr	<= (others => '0');
 			elsif code_reg_we = '1' then
 				if eflags.Ov = '1'	then
 					exc_code	<= "01100";
+					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
 				elsif eflags.Sys = '1'	then
-					exc_code	<= "01000";
+					exc_code		<= "01000";
+					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
 				elsif eflags.Bp = '1'	then
-					exc_code	<= "01001";
+					exc_code		<= "01001";
+					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
 				elsif eflags.RI = '1'	then
-					exc_code	<= "01010";
+					exc_code		<= "01010";
+					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
 				elsif eflags.IAdEL = '1'	then
-					exc_code	<= "00100";
-					BadVAddr	<= imem_addr;
+					exc_code		<= "00100";
+					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
 				elsif eflags.IAdEK = '1'	then
-					exc_code	<= "00100";
-					BadVAddr	<= imem_addr;
+					exc_code		<= "00100";
+					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
 				elsif eflags.DAdEL = '1'	then
-					exc_code	<= "00100";
-					BadVAddr	<= ctrl_in.dmem_addr;
+					exc_code		<= "00100";
+					regs.badvaddr	<= ctrl_in.pipe.dmem_addr;
 				elsif eflags.DAdES = '1'	then
-					exc_code	<= "00101";
-					BadVAddr	<= ctrl_in.dmem_addr;
+					exc_code		<= "00101";
+					regs.badvaddr	<= ctrl_in.pipe.dmem_addr;
 				elsif eflags.Int = '1' then
-					exc_code	<= "00000";
+					exc_code		<= "00000";
+					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
+				elsif eflags.ITLB_MOD = '1' then
+					exc_code		<= "00001";
+					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
+					itlb_exc		<= '1';
+				elsif eflags.ITLB_LOAD = '1' then
+					exc_code		<= "00010";
+					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
+					itlb_exc		<= '1';
+				elsif eflags.ITLB_STORE = '1' then
+					exc_code		<= "00011";
+					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
+					itlb_exc		<= '1';
+				elsif eflags.DTLB_MOD = '1' then
+					exc_code		<= "00001";
+					regs.badvaddr	<= ctrl_in.pipe.dmem_addr;
+					dtlb_exc		<= '1';
+				elsif eflags.DTLB_LOAD = '1' then
+					exc_code		<= "00010";
+					regs.badvaddr	<= ctrl_in.pipe.dmem_addr;
+					dtlb_exc		<= '1';
+				elsif eflags.DTLB_STORE = '1' then
+					exc_code		<= "00011";
+					regs.badvaddr	<= ctrl_in.pipe.dmem_addr;
+					dtlb_exc		<= '1';
 				end if;
 			end if;
 		end if;
 	end process;
 
-	ctrl_out.icache.inv_addr	<= test_reg;
-	ctrl_out.dcache.inv_addr	<= test_reg;
+-- Exception vector dispatch
+cop_exc_vector:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				ctrl_out.pipe.exc_vec		<= X"BFC00000";
+			elsif latch_vect_en = '1' then
+				if ctrl_in.pipe.events.nmi = '1' then
+					ctrl_out.pipe.exc_vec		<= X"BFC00000";
+				else
+					ctrl_out.pipe.exc_vec		<= X"80000080";
+					if regs.status(22) = '1' then
+						ctrl_out.pipe.exc_vec	<= X"BFC00180";
+					end if;
+				end if;
+			end if;
+		end if;
+	end process;
+
+	ctrl_out.icache.inv_addr	<= regs.test;
+	ctrl_out.dcache.inv_addr	<= regs.test;
 	
 -- Custom I/D-Cache operations:  invalidate all/line
 cop_cache_op:
@@ -362,8 +658,8 @@ cop_cache_op:
 		end if;
 	end process;
 
--- IP register write and signalling Int to pipeline
-cop_ip_reg_write:
+-- CAUSE: IP register write and signalling Int to pipeline
+cop_ip_register:
 	process(clk)
 	variable ip_v  : unsigned(7 downto 0);
 	variable ipm_v : unsigned(7 downto 0);
@@ -371,172 +667,197 @@ cop_ip_reg_write:
 		if rising_edge(clk) then
 			if rst = '1' then
 				sw_int	<= (others => '0');
-			elsif ctrl_in.sdu.WB_nop = '0' then
-				if ip_reg_we = '1' then
+			elsif ctrl_in.pipe.sdu.WB_nop = '0' then
+				if EX_reg_we(COP0_CAUSE) = '1' then
 					sw_int	<= din(9 downto 8);
 				end if;	
 			end if;	
-			ip_v			:= (int & sw_int);
-			ipm_v			:= ip_v and im;
-			ip				<= ip_v;
-			ctrl_out.int	<= (eval_int(ipm_v) and status(0));
-			ctrl_out.NMI	<= int_nmi;
+			ip_v				:= (int & sw_int);
+			ipm_v				:= ip_v and im;
+			ip					<= ip_v;
+			ctrl_out.pipe.int	<= (eval_int(ipm_v) and regs.status(0));
+			ctrl_out.pipe.NMI	<= int_nmi;
 		end if;
 	end process;
 
--- Exception vector dispatch
-cop_exc_vector:
+		
+-- WIRED: Wired Register (RO)
+cop_wired_register:
 	process(clk)
 	begin
 		if rising_edge(clk) then
 			if rst = '1' then
-				ctrl_out.exc_vec		<= X"BFC00000";
-			elsif latch_vect_en = '1' then
-				if ctrl_in.events.nmi = '1' then
-					ctrl_out.exc_vec		<= X"BFC00000";
-				else
-					ctrl_out.exc_vec		<= X"80000080";
-					if status(22) = '1' then
-						ctrl_out.exc_vec	<= X"BFC00180";
-					end if;
+				regs.wired		<= X"0000_0008";
+			end if;
+		end if;
+	end process;
+
+-- RANDOM: Random Register (RO)
+cop_random_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' or EX_reg_we(COP0_WIRED) = '1' then
+				random		<= to_unsigned(TLB_NUM_ENTRIES-1, lg2(TLB_NUM_ENTRIES));
+			elsif random >  regs.wired(lg2(TLB_NUM_ENTRIES)-1 downto 0) then
+				random 		<= random - 1;
+			else
+				random		<= to_unsigned(TLB_NUM_ENTRIES-1, lg2(TLB_NUM_ENTRIES));
+			end if;
+		end if;
+	end process;
+
+-- INDEX: Index Register (RW)
+cop_index_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				index		<= to_unsigned(0, lg2(TLB_NUM_ENTRIES));
+			elsif EX_reg_we(COP0_INDEX) = '1' then
+				index		<= din(8+lg2(TLB_NUM_ENTRIES)-1 downto 8);
+			end if;
+		end if;
+	end process;
+
+-- CONTEXT: PTEBase Register (RW)
+cop_context_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				PTEBase		<= to_unsigned(0, PTEBase'length);
+			elsif EX_reg_we(COP0_CONTEXT) = '1' then
+				PTEBase		<= din(31 downto 21);
+			end if;
+		end if;
+	end process;
+
+-- ITLB EntryLO
+itlb_entrylo_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				itlb_entry_lo	<= to_tlb_entry_lo(to_unsigned(0, 32));
+			elsif cop_pipe_EX.TLBR = '1' then
+				itlb_entry_lo	<= ctrl_in.itlb.entry_lo;
+			elsif EX_reg_we(COP0_ENTRY_LO) = '1' and tlb_sel = '0' then
+				itlb_entry_lo	<= to_tlb_entry_lo(din);
+			end if;
+		end if;
+	end process;
+
+-- ITLB EntryHI
+itlb_entryhi_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				itlb_entry_hi	<= to_tlb_entry_hi(to_unsigned(0, 32));
+			elsif itlb_exc = '1' then
+				itlb_entry_hi	<= to_tlb_entry_hi(itlb_entry_hi, regs.badvaddr);
+			elsif cop_pipe_EX.TLBR = '1' then
+				itlb_entry_hi	<= ctrl_in.itlb.entry_hi;
+			elsif EX_reg_we(COP0_ENTRY_HI) = '1' and tlb_sel = '0' then
+				itlb_entry_hi	<= to_tlb_entry_hi(din);
+			end if;
+		end if;
+	end process;
+
+-- DTLB
+dtlb_entrylo_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				dtlb_entry_lo	<= to_tlb_entry_lo(to_unsigned(0, 32));
+			elsif cop_pipe_EX.TLBR = '1' then
+				dtlb_entry_lo	<= ctrl_in.dtlb.entry_lo;
+			elsif EX_reg_we(COP0_ENTRY_LO) = '1' and tlb_sel = '1' then
+				dtlb_entry_lo	<= to_tlb_entry_lo(din);
+			end if;
+		end if;
+	end process;
+
+-- DTLB EntryHI
+dtlb_entryhi_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				dtlb_entry_hi	<= to_tlb_entry_hi(to_unsigned(0, 32));
+			elsif dtlb_exc = '1' then
+				dtlb_entry_hi	<= to_tlb_entry_hi(dtlb_entry_hi, regs.badvaddr);
+			elsif cop_pipe_EX.TLBR = '1' then
+				dtlb_entry_hi	<= ctrl_in.dtlb.entry_hi;
+			elsif EX_reg_we(COP0_ENTRY_HI) = '1' and tlb_sel = '1' then
+				dtlb_entry_hi	<= to_tlb_entry_hi(din);
+			end if;
+		end if;
+	end process;
+
+-- TEST: COP Test Register
+cop_test_register:
+	process(clk)
+	begin
+		if rising_edge(clk) then
+			if rst = '1' then
+				regs.test	<= (others => '0');
+			elsif ctrl_in.pipe.sdu.WB_nop = '0' then
+				if EX_reg_we(COP0_1F) = '1' then
+					regs.test	<= din;
 				end if;
 			end if;
 		end if;
 	end process;
 
--- Cop pipeline
-cop_pipe:
+-- CONFIG: COP Config Register
+cop_config_register:
 	process(clk)
 	begin
 		if rising_edge(clk) then
 			if rst = '1' then
-				cop_pipe_EX.cs	<= '0';
-				cop_pipe_EX.we	<= '0';
-				cop_pipe_EX.re	<= '0';
-			elsif ctrl_in.sdu.stall_all = '0' then
-				cop_pipe_EX		<= cop_pipe_ID;
-				if cop_pipe_ID.we = '1' then
-					reg_wptr	<= cop_pipe_ID.reg_ptr;
-				end if;
-				if cop_pipe_ID.re = '1' then
-					reg_rptr	<= cop_pipe_ID.reg_ptr;
+				regs.config	<= (others => '0');
+			elsif ctrl_in.pipe.sdu.WB_nop = '0' then
+				if EX_reg_we(COP0_CONFIG) = '1' then
+					regs.config	<= din;
 				end if;
 			end if;
 		end if;
 	end process;
 
--- COP Register read
-cop_register_read:
-	process(reg_rptr, BadVAddr, status, cause, epc, dcache_info, icache_info, test_reg)
-	variable reg : word_t;
-	begin
-	
-		case reg_rptr is
-		when "01000" =>			-- BadVAddr
-			reg	:= BadVAddr;
-
-		when "01100" =>			-- Status
-			reg	:= status;
-
-		when "01101" =>			-- Cause
-			reg	:= cause;
-
-		when "01110" =>			-- EPC (Exception Program Counter)
-			reg	:= epc;
-
-		when "01111" =>			-- PRId (Processor Revision Register)
-			reg	:= dcache_info & icache_info & X"02" & to_unsigned(REVISION ,8);
-
-		when "11111" =>			-- test_reg
-			reg	:= test_reg;
-
-		when others =>
-			reg	:= (others => '-');
-
-		end case;	
-
-		dout	<= reg after 2 ns;
-
-	end process;
-
--- COP Register write strobe generation
-cop_we_gen:
+-- STATUS: COP Status Register
+cop_status_register:
 	process(clk)
 	begin
 		if rising_edge(clk) then
 			if rst = '1' then
-				test_reg_we	<= '0';
-				stat_reg_we	<= '0';
-				ip_reg_we	<= '0';
-			elsif ctrl_in.sdu.stall_all = '0' then
-				test_reg_we	<= '0';
-				stat_reg_we	<= '0';
-				ip_reg_we	<= '0';
-				if cop_pipe_EX.we = '1' then
-
-					case reg_wptr is
-
-					when "01100" =>
-						stat_reg_we	<= '1'; -- not status(1) or status(28);
-					when "01101" =>			
-						ip_reg_we	<= '1'; -- not status(1) or status(28);
-					when "11111" =>
-						test_reg_we	<= '1'; -- not status(1) or status(28);
-					when others => null;
-
-					end case;
-
-				end if;	
-			end if;	
-		end if;	
-	end process;
-
--- COP Test Register
-cop_test_reg_write:
-	process(clk)
-	begin
-		if rising_edge(clk) then
-			if rst = '1' then
-				test_reg	<= (others => '0');
-			elsif ctrl_in.sdu.WB_nop = '0' then
-				if test_reg_we = '1' then
-					test_reg	<= din;
-				end if;
-			end if;
-		end if;
-	end process;
-
--- COP Status Register
-cop_status_reg_write:
-	process(clk)
-	begin
-		if rising_edge(clk) then
-			if rst = '1' then
-				status	<= X"00600000";
+				regs.status	<= X"00600002";			-- at reset: set kernel mode enabled and interrupts disabled
 			elsif status_save = '1' then
-				if ctrl_in.events.nmi = '0' then
-					status(5 downto 4)	<= status(3 downto 2);
-					status(3 downto 2)	<= status(1 downto 0);
+				if ctrl_in.pipe.events.nmi = '0' then
+					regs.status(5 downto 4)	<= regs.status(3 downto 2);
+					regs.status(3 downto 2)	<= regs.status(1 downto 0);
 				else	
-					status(22)	<= '1';
+					regs.status(22)	<= '1';
 				end if;
-				status(1 downto 0)	<= "00";
+				regs.status(1 downto 0)	<= "10";	-- at exception: set kernel mode enabled and interrupts disabled
 			elsif status_rest = '1' then
-				status(1 downto 0)	<= status(3 downto 2);
-				status(3 downto 2)	<= status(5 downto 4);
-			elsif ctrl_in.sdu.WB_nop = '0' then
-				if stat_reg_we = '1' then
-					if status(1) = '0' then
-						status(0)			<= din(0);	-- IEc
-						status(1)			<= din(1);	-- KUc
-						status(2)			<= din(2);	-- IEp
-						status(3)			<= din(3);	-- KUp
-						status(4)			<= din(4);	-- IEo
-						status(5)			<= din(5);	-- KUo
+				regs.status(1 downto 0)		<= regs.status(3 downto 2);
+				regs.status(3 downto 2)		<= regs.status(5 downto 4);
+			elsif ctrl_in.pipe.sdu.WB_nop = '0' then
+				if EX_reg_we(COP0_STATUS) = '1' then
+					if regs.status(1) = '1' then		-- accessible only in kernel mode
+						regs.status(0)			<= din(0);	-- IEc
+						regs.status(1)			<= din(1);	-- KUc
+						regs.status(2)			<= din(2);	-- IEp
+						regs.status(3)			<= din(3);	-- KUp
+						regs.status(4)			<= din(4);	-- IEo
+						regs.status(5)			<= din(5);	-- KUo
 					end if;	
-					status(15 downto 8)		<= din(15 downto 8);	-- IM
-					status(22)				<= din(22);				-- BEV
-					status(25)				<= din(25);				-- RE
+					regs.status(15 downto 8)	<= din(15 downto 8);	-- IM
+					regs.status(22)				<= din(22);				-- BEV
+					regs.status(25)				<= din(25);				-- RE
 				end if;
 			end if;	
 		end if;
diff --git a/lib/CPUs/MIPS/src/core/mips_pipeline.vhd b/lib/CPUs/MIPS/src/core/mips_pipeline.vhd
index 38dd440..07a35e9 100644
--- a/lib/CPUs/MIPS/src/core/mips_pipeline.vhd
+++ b/lib/CPUs/MIPS/src/core/mips_pipeline.vhd
@@ -27,6 +27,7 @@ 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
@@ -51,8 +52,8 @@ entity pipeline is
 		cop_ir_en	: out STD_LOGIC;
 		cop_din		: in  word_t;
 		cop_dout	: out word_t;
-		c0_ctrl_out	: out cop0_ctrl_in_t;
-		c0_ctrl_in	: in  cop0_ctrl_out_t
+		ctrl_out	: out pipe_ctrl_out_t;
+		ctrl_in		: in  pipe_ctrl_in_t
 		
 	);
 end pipeline;
@@ -166,13 +167,14 @@ architecture Behavioral of pipeline is
 	signal bcu_op_a			: word_t;
 	signal bcu_op_b			: word_t;
 	signal bcu_flags		: bcu_flags_t;
-	signal vaddr			: word_t;
+	signal vaddr1			: word_t;
+	signal vaddr2			: word_t;
 	signal dmem_src			: word_t;
 	signal stage_rst		: unsigned(3 downto 0);
 	signal pipe_rst			: STD_LOGIC;
 	
 	signal pc : pc_t;
-	
+
 --------------------------------------------------------------------------
 begin
 	
@@ -181,13 +183,13 @@ begin
 	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;
 	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			<= '0';
@@ -196,23 +198,17 @@ begin
 	sdu.imem_dep			<= not imem_rdy;
 	sdu.dmem_dep			<= not dmem_rdy and MEM_stage.ctrl.dmem_en;
 	---------------------------------------------------------------------------
-	imem_en 				<= cpu_run and not (sdu.mul_dep or sdu.dmem_dep or c0_ctrl_in.exc_commit);
+	imem_en 				<= cpu_run and not (sdu.mul_dep or sdu.dmem_dep or ctrl_in.exc_commit);
 
 --------------------------------------------------------------------------
 -- Coprocessor assignments
 --------------------------------------------------------------------------
-	c0_ctrl_out.bd_wb		<= WB_stage.bd;
-	c0_ctrl_out.epc_mem		<= MEM_stage.pcn;
-	c0_ctrl_out.epc_wb		<= MEM_stage.epc;
-	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;
+	ctrl_out.sdu			<= sdu;
+	ctrl_out.exc_req		<= WB_stage.exc and not WB_stage.exc_last;
+	ctrl_out.exc_ack		<= '1';
 	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; -- dmem_din when MEM_stage.ctrl.dmem_en = '1' else MEM_stage.ex_result;
 
 --------------------------------------------------------------------------
 -- Muldiv
@@ -279,8 +275,8 @@ proc_stage_branch_ce:
 			if pipe_rst = '1' then
 				branch_ce	<= '1';
 			else
-				branch_ce	<= c0_ctrl_in.exc_pending;
-				if sdu.ID_stall = '0' and c0_ctrl_in.exc_inject = '0' then
+				branch_ce	<= ctrl_in.exc_pending;
+				if sdu.ID_stall = '0' and ctrl_in.exc_inject = '0' then
 					branch_ce	<= '1';
 				end if;
 			end if;
@@ -291,15 +287,15 @@ proc_stage_pc_next:
 	process(clk_1)
 	begin
 		if rising_edge(clk_1) then
-			if c0_ctrl_in.exc_inject = '1' then
-				pc.nxt	<= c0_ctrl_in.exc_vec;
+			if ctrl_in.exc_inject = '1' then
+				pc.nxt	<= ctrl_in.exc_vec;
 			elsif sdu.ID_stall = '0' then
 				pc.last	<= pc.curr;
 				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;
-				else
+				elsif cpu_run = '1' then
 					pc.nxt	<= pc.curr + 4;
 				end if;
 			end if;
@@ -346,13 +342,17 @@ proc_stage_branch:
 	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);
 	
 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_priv_addr	<= not c0_ctrl_in.exc_pending and (pc.nxt(word_t'left) and c0_ctrl_in.user_mode);
+		ID_stage.events.inst_load_err	<= not ctrl_in.exc_pending and (pc.nxt(1) or pc.nxt(0));
+		ID_stage.events.inst_priv_addr	<= not ctrl_in.exc_pending and (pc.nxt(word_t'left) and ctrl_in.user_mode);
+		ID_stage.events.ITLB_LOAD		<= ctrl_in.ITLB.exc_vld and ctrl_in.ITLB.exc_miss and not ctrl_in.ITLB.write;
+		ID_stage.events.ITLB_STORE		<= ctrl_in.ITLB.exc_vld and ctrl_in.ITLB.exc_miss and ctrl_in.ITLB.write;
+		ID_stage.events.ITLB_MOD		<= ctrl_in.ITLB.exc_vld and ctrl_in.ITLB.exc_dirty;
 	end process;
 	
 proc_stage_hdu:
@@ -366,12 +366,12 @@ 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_MEM			:= reg_ptr_a = MEM_stage.reg_wptr and MEM_stage.wreg_we = '1';
-		raw_a_WB			:= reg_ptr_a = WB_stage.reg_wptr and WB_stage.wreg_we = '1';
-		raw_b_EX			:= reg_ptr_b = EX_stage.reg_wptr and EX_stage.wreg_we = '1';
-		raw_b_MEM			:= reg_ptr_b = MEM_stage.reg_wptr and MEM_stage.wreg_we = '1';
-		raw_b_WB			:= reg_ptr_b = WB_stage.reg_wptr and WB_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.reg_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.reg_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.reg_we = '1';
 
 		hdu.alu_fwd_a_ex	<= raw_a_EX after 1 ns;
 		hdu.alu_fwd_a_mem	<= raw_a_MEM after 1 ns;
@@ -390,16 +390,16 @@ 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.reg_data;
 		elsif hdu.alu_fwd_a_wb then
-			data 	:= WB_stage.data;
+			data 	:= WB_stage.reg_data;
 		end if;
 		
 		for i in 0 to 31 loop
 			if data(i) = '1' then
-				ID_stage.reg_a(i)	<= '1' after 2 ns;
+				ID_stage.reg_a(i)	<= '1' after 1 ns;
 			else 
-				ID_stage.reg_a(i)	<= '0' after 2 ns;
+				ID_stage.reg_a(i)	<= '0' after 1 ns;
 			end if;
 		end loop;
 
@@ -413,18 +413,11 @@ 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.reg_data;
 		elsif hdu.alu_fwd_b_wb then
-			data	:= WB_stage.data;
+			data	:= WB_stage.reg_data;
 		end if;
-
-		for i in 0 to 31 loop
-			if data(i) = '1' then
-				ID_stage.reg_b(i)	<= '1' after 2 ns;
-			else 
-				ID_stage.reg_b(i)	<= '0' after 2 ns;
-			end if;
-		end loop;
+		ID_stage.reg_b <= to_mips_01(data) after 1 ns;
 
 	end process;
 	
@@ -449,7 +442,7 @@ proc_imm_mux:
 
 		end case;
 
-		ID_stage.imm <= data after 2 ns;
+		ID_stage.imm <= data after 1 ns;
 
 	end process;
 
@@ -463,10 +456,10 @@ inst_reg_dual: reg_dual
 	PORT MAP
 	(
 		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.reg_data,
 		rptr_a		=> ID_stage.reg_a_rptr,
 		rptr_b		=> ID_stage.reg_b_rptr,
 		dout_a		=> reg_a,
@@ -493,18 +486,19 @@ proc_stage_ID_EX_1:
 				EX_stage.pcn					<= (others => '0');
 				EX_stage.events_in				<= events_clr;
 			elsif sdu.EX_stall = '0' then
+				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.nop			<= sdu.EX_nop;
-				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';
@@ -514,7 +508,7 @@ proc_stage_ID_EX_1:
 	end process;
 
 proc_stage_EX_instr_except:
-	process(EX_stage)
+	process(EX_stage, ctrl_in)
 	begin
 		EX_events_instr					<= events_clr;
 		EX_events_instr.illegal			<= EX_stage.ctrl.exc_illegal;
@@ -536,8 +530,6 @@ 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_1)
 	begin
@@ -551,12 +543,12 @@ proc_stage_EX_mem_except:
 					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';
 							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';
@@ -567,29 +559,31 @@ proc_stage_EX_mem_except:
 		end if;
 	end process;
 
+	ctrl_out.dtlb_va <= ID_stage.va;
+	ctrl_out.dtlb_en <= ID_stage.ctrl.dmem_en;
+
 proc_stage_DMEM_ADDR:
 	process(clk_1)
 	begin
 		if rising_edge(clk_1) then
 			if sdu.EX_stall = '0' then
-				EX_stage.va		<= vaddr;
-				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;
-	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) after 1 ns;
-	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) after 1ns;
-	dmem_addr	<= EX_stage.va;
+	dmem_src				<= cop_din when EX_stage.ctrl.cop_instr_en = '1' else EX_stage.reg_b;
+	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) after 1 ns;
+	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) after 1ns;
+	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);
 	
@@ -608,9 +602,9 @@ proc_wptr_mux:
 			reg_wptr					:= extract_rt(EX_stage.IR);
 		end case;
 		
-		EX_stage.wreg_we <= EX_stage.reg_write after 1 ns;
+		EX_stage.reg_we		<= EX_stage.reg_write after 1 ns;
 		if reg_wptr = "00000" then
-			EX_stage.wreg_we <= '0' after 1 ns;
+			EX_stage.reg_we <= '0' after 1 ns;
 		end if;	
 
 		EX_stage.reg_wptr	<= reg_wptr after 1 ns;
@@ -620,7 +614,7 @@ proc_wptr_mux:
 
 			when wptr_src_const =>
 				EX_stage.reg_wptr	<= to_unsigned(31, reg_ptr_t'length) after 1 ns;
-				EX_stage.wreg_we <= '1' after 1 ns;
+				EX_stage.reg_we <= '1' after 1 ns;
 
 			when others => null;
 		end case;
@@ -685,9 +679,9 @@ shifter_sa_mux:
 			end case;
 			data_inv := not data + 1;
 			if ID_stage.ctrl.alu.shift_right = '0' then
-				EX_stage.shift_ctrl.shamt_rnd	<= data_inv after 2 ns;
+				EX_stage.shift_ctrl.shamt_rnd	<= data_inv after 1 ns;
 			else	
-				EX_stage.shift_ctrl.shamt_rnd	<= data after 2 ns;
+				EX_stage.shift_ctrl.shamt_rnd	<= data after 1 ns;
 			end if;
 			EX_stage.shift_ctrl.shamt_nrm	<= data after 1 ns;
 			EX_stage.shift_ctrl.shift_right	<= ID_stage.ctrl.alu.shift_right;
@@ -745,24 +739,26 @@ proc_stage_MEM_n:
 		if rising_edge(clk_1) then
 			if stage_rst(2) = '1' then
 				MEM_stage.op		<= op_nop;
-				MEM_stage.wreg_we	<= '0';
+				MEM_stage.reg_we	<= '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.events_in		<= EX_stage.events;
+				MEM_stage.nop			<= sdu.MEM_nop;
 				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;
 				if EX_stage.ctrl.dmem_en = '1' then
 					MEM_stage.va			<= EX_stage.va;
 				end if;	
 				MEM_stage.pa_off		<= EX_stage.pa_off;
 				MEM_stage.reg_wptr		<= EX_stage.reg_wptr;
-				MEM_stage.nop			<= sdu.MEM_nop;
 				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.ex_result	<= MEM_stage.reg_data;
 					end if;
 				else
 					MEM_stage.ex_result		<= EX_stage.reg_b;
@@ -775,12 +771,8 @@ proc_stage_MEM_n:
 					MEM_stage.ex_result		<= EX_stage.result;
 				end if;	
 				if sdu.MEM_nop = '1' then
-					MEM_stage.op		<= op_nop;
-					MEM_stage.wreg_we	<= '0';
+					MEM_stage.reg_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;
@@ -813,16 +805,19 @@ proc_stage_MEM_mux:
 				data(31 downto 24)	:= temp2(31 downto 24);
 			end if;	
 		end if;
-		MEM_stage.data				<= data after 1 ns;
+		MEM_stage.reg_data			<= 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.NMI			<= c0_ctrl_in.NMI;
+		MEM_stage.events.Int			<= ctrl_in.int;
+		MEM_stage.events.NMI			<= ctrl_in.NMI;
+		MEM_stage.events.DTLB_LOAD		<= ctrl_in.DTLB.exc_vld and ctrl_in.DTLB.exc_miss and not ctrl_in.DTLB.write;
+		MEM_stage.events.DTLB_STORE		<= ctrl_in.DTLB.exc_vld and ctrl_in.DTLB.exc_miss and ctrl_in.DTLB.write;
+		MEM_stage.events.DTLB_MOD		<= ctrl_in.DTLB.exc_vld and ctrl_in.DTLB.exc_dirty;
 	end process;
 
 	MEM_stage.exc	<= event_is_active(MEM_stage.events);
@@ -832,33 +827,48 @@ proc_stage_MEM_except:
 --------------------------------------------------------------------------
 proc_stage_WB_p:
 	process(clk_1)
+	variable branch_delay : std_logic;
+	
 	begin
 		if rising_edge(clk_1) then
 			if stage_rst(3) = '1' then
 				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;
-			elsif sdu.WB_stall = '0' then
-				WB_stage.op				<= MEM_stage.op;
-				WB_stage.wreg_we		<= MEM_stage.wreg_we;
-				WB_stage.reg_wptr		<= MEM_stage.reg_wptr;
-				WB_stage.data			<= MEM_stage.data;
 				WB_stage.exc			<= '0';
+			elsif sdu.WB_stall = '0' then
+				WB_stage.nop			<= sdu.WB_nop;
+				WB_stage.op				<= MEM_stage.op;
+				WB_stage.epc_last		<= WB_stage.epc;
+				WB_stage.epc			<= MEM_stage.epc;
+				branch_delay			:= MEM_stage.ctrl.branch or MEM_stage.ctrl.jump or MEM_stage.ctrl.jump_long; -- Todo: works
+				WB_stage.bd				<= branch_delay;
+				WB_stage.exc_last		<= WB_stage.exc;
+				WB_stage.exc			<= MEM_stage.exc;
+				WB_stage.reg_we			<= MEM_stage.reg_we;
+				WB_stage.reg_wptr		<= MEM_stage.reg_wptr;
+				WB_stage.reg_data		<= MEM_stage.reg_data;
+				WB_stage.events			<= MEM_stage.events;
 				if sdu.stall_all = '0' then
-					WB_stage.exc			<= MEM_stage.exc;
-					if MEM_stage.exc = '1' then
-						WB_stage.events			<= MEM_stage.events;
+					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;	
-				WB_stage.nop			<= sdu.WB_nop;
 				if sdu.WB_nop = '1' then
-					WB_stage.op				<= op_nop;
-					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
+					WB_stage.reg_we		<= '0';
 				end if;	
+				if WB_stage.exc = '0' then
+					WB_stage.va				<= MEM_stage.va;
+				end if;
 			end if;	
 		end if;	
 	end process;
diff --git a/lib/CPUs/MIPS/src/core/mips_top.vhd b/lib/CPUs/MIPS/src/core/mips_top.vhd
index abe90ff..984f6a4 100644
--- a/lib/CPUs/MIPS/src/core/mips_top.vhd
+++ b/lib/CPUs/MIPS/src/core/mips_top.vhd
@@ -34,7 +34,8 @@ entity mips_top is
 		icache_size	: natural := 8192;	-- words
 		icache_line	: natural := 8;		-- words
 		dcache_size	: natural := 4096;	-- words
-		dcache_line	: natural := 8		-- words
+		dcache_line	: natural := 8;		-- words
+		WITH_TLB	: boolean := true
 	);
 	Port
 	(
@@ -87,8 +88,8 @@ architecture rtl of mips_top is
 		cop_ir_en	: out STD_LOGIC;
 		cop_din		: in  word_t;
 		cop_dout	: out word_t;
-		c0_ctrl_out	: out cop0_ctrl_in_t;
-		c0_ctrl_in	: in  cop0_ctrl_out_t
+		ctrl_out	: out pipe_ctrl_out_t;
+		ctrl_in		: in  pipe_ctrl_in_t
 	);
 	END COMPONENT;
 	signal imem_err			: std_logic;
@@ -105,6 +106,7 @@ architecture rtl of mips_top is
 	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;
@@ -113,8 +115,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_c0_ctrl_in	: cop0_ctrl_out_t;
+	signal pipe_ctrl_in		: pipe_ctrl_in_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
@@ -149,53 +157,58 @@ architecture rtl of mips_top is
 	COMPONENT biu
 	GENERIC
 	(
-		icache_size	: natural;
-		icache_line	: natural;
-		dcache_size	: natural;
-		dcache_line	: natural
+		icache_size		: natural;
+		icache_line		: natural;
+		dcache_size		: natural;
+		dcache_line		: natural;
+		WRITE_FIFO_SIZE	: natural;
+		WITH_TLB		: boolean
 	);
 	PORT
 	(
+		clk				: in  STD_LOGIC;
+		busy			: out STD_LOGIC;
+
+		imem_err		: out STD_LOGIC;
+		imem_rdy		: out STD_LOGIC;
+		imem_en			: in  STD_LOGIC;
+		imem_addr		: in  word_t;
+		imem_dout		: out word_t;
+		dmem_err		: out STD_LOGIC;
+		dmem_rdy		: out STD_LOGIC;
+		dmem_en			: in  STD_LOGIC;
+		dmem_we			: in  STD_LOGIC;
+		dmem_be			: in  unsigned(3 downto 0);
+		dmem_din		: in  word_t;
+		dmem_dout		: out word_t;
+		dmem_addr		: in  word_t;
+
 		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);
-		DAT_O			: out unsigned(31 downto 0);
+		MDAT_I			: in  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;
-		cpu_imem_err	: out STD_LOGIC;
-		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
+
+		ctrl_in			: in  biu_ctrl_in_t;
+		ctrl_out		: out biu_ctrl_out_t
+
 	);
 	END COMPONENT;
 
 begin
 
 -------------------------------------------------------------------
-	debug.imem_err		<= imem_err;
-	debug.dmem_err		<= dmem_err;
-	
-	process(CLK_I)
+	process(cpu_clk)
 	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';
@@ -209,7 +222,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;
@@ -239,10 +252,12 @@ inst_pipeline: pipeline
 		cop_ir_en	=> pipe_cop_ir_en,
 		cop_din		=> pipe_cop_din,
 		cop_dout	=> pipe_cop_dout,
-		c0_ctrl_out	=> pipe_c0_ctrl_out,
-		c0_ctrl_in	=> pipe_c0_ctrl_in
+		ctrl_out	=> pipe_ctrl_out,
+		ctrl_in		=> pipe_ctrl_in
 	);
 
+	pipe_ctrl_in		<= cop0_ctrl_out.pipe;
+
 inst_cop: cop
 	GENERIC MAP
 	(
@@ -260,49 +275,122 @@ inst_cop: cop
 		int			=> INT,
 		ir_en		=> pipe_cop_ir_en,
 		ir			=> pipe_cop_ir,
-		ctrl_in		=> pipe_c0_ctrl_out,
-		ctrl_out	=> pipe_c0_ctrl_in,
+		ctrl_in		=> cop0_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	<= biu_ctrl_out.itlb;
+	cop0_ctrl_in.dtlb	<= biu_ctrl_out.dtlb;
+
 inst_biu: biu
 	GENERIC MAP
 	(
-		icache_size	=> icache_size,	-- words
-		icache_line	=> icache_line,	-- words
-		dcache_size	=> dcache_size,	-- words
-		dcache_line	=> dcache_line	-- words
+		icache_size		=> icache_size,	-- words
+		icache_line		=> icache_line,	-- words
+		dcache_size		=> dcache_size,	-- words
+		dcache_line		=> dcache_line,	-- words
+		WRITE_FIFO_SIZE	=> 4,
+		WITH_TLB		=> WITH_TLB
 	)
 	PORT MAP
 	(
+
+		clk				=> cpu_clk,
+		busy			=> biu_busy,
+		
+		imem_err		=> imem_err,
+		imem_rdy		=> imem_rdy,
+		imem_en			=> imem_en,
+		imem_addr		=> imem_addr,
+		imem_dout		=> imem_din,
+		dmem_err		=> dmem_err,
+		dmem_rdy		=> dmem_rdy,
+		dmem_en			=> dmem_en,
+		dmem_we			=> dmem_we,
+		dmem_be			=> dmem_be,
+		dmem_addr		=> dmem_addr,
+		dmem_din		=> dmem_dout,
+		dmem_dout		=> dmem_din,
+
 		RST_I			=> biu_rst,
 		CLK_I			=> CLK_I,
 		ACK_I			=> ACK_I,
 		SRDY_I			=> SRDY_I,
 		ADDR_O			=> ADDR_O,
-		DAT_I			=> DAT_I,
-		DAT_O			=> DAT_O,
+		MDAT_I			=> DAT_I,
+		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,
-		cpu_imem_err	=> imem_err,
-		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
+
+		ctrl_in			=> biu_ctrl_in,
+		ctrl_out		=> biu_ctrl_out
+
 	);
 
+	biu_ctrl_in.dcache	<= cop0_ctrl_out.dcache;
+	biu_ctrl_in.icache	<= cop0_ctrl_out.icache;
+	biu_ctrl_in.dtlb	<= cop0_ctrl_out.dtlb;
+	biu_ctrl_in.itlb	<= cop0_ctrl_out.itlb;
+
+-------------------------------------------
+-- 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 imem_rdy = '1' then
+				if en_r = '1' then
+					debug.imem_addr_reg	<= addr_r;
+					debug.imem_din_reg	<= imem_din;
+					debug.imem_din_vld	<= '1';
+				end if;
+				if imem_en = '1' then
+					addr_r	:= imem_addr;
+				end if;
+				en_r	:= imem_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 dmem_rdy = '1' then
+				if en_r = '1' then
+					debug.dmem_addr_reg	<= addr_r;
+					debug.dmem_din_reg	<= dmem_din;
+					debug.dmem_din_vld	<= not we_r;
+					debug.dmem_dout_reg	<= dout_r;
+					debug.dmem_dout_vld	<= we_r;
+				end if;
+				if dmem_en = '1' then
+					dout_r	:= dmem_dout;
+					addr_r	:= dmem_addr;
+				end if;
+				en_r	:= dmem_en;
+				we_r	:= dmem_we;
+			end if;
+		end if;
+	end process;	
+
 end rtl;
diff --git a/lib/CPUs/MIPS/src/core/mips_types.vhd b/lib/CPUs/MIPS/src/core/mips_types.vhd
index d861c41..86112d4 100644
--- a/lib/CPUs/MIPS/src/core/mips_types.vhd
+++ b/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 WORD_WIDTH : integer := 32;
+	constant REVISION			: integer	:= 13;
+	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,
@@ -227,6 +241,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 +259,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;
@@ -255,19 +281,80 @@ package mips_types is
 		inv_all			: STD_LOGIC;
 	end record;
 	
-	type cop0_ctrl_in_t is record
-		sdu				: sdu_t;
-		events			: event_t;
-		bd_wb			: 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;
+	type tlb_flags_t is record	
+		N				: std_logic;
+		D				: std_logic;
+		V				: 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;
+		idx				: unsigned (lg2(TLB_NUM_ENTRIES) downto 0);
+	end record;
+
+	type tlb_query_out_t is record
+		vld				: std_logic;
+		exc				: STD_LOGIC;
+		hit				: STD_LOGIC;
+		paddr			: word_t;
+		flags			: tlb_flags_t;
+		idx				: unsigned (lg2(TLB_NUM_ENTRIES) downto 0);
+	end record;
+
+	type tlb_status_t is record	
+		exc_vld			: std_logic;
+		exc_miss		: std_logic;
+		exc_dirty		: std_logic;
+		write			: std_logic;
+	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);
+		stat			: tlb_status_t;
+	end record;
+
+	type biu_ctrl_in_t is record
+		itlb			: tlb_ctrl_in_t;
+		dtlb			: tlb_ctrl_in_t;
+		icache			: cache_ctrl_t;
+		dcache			: cache_ctrl_t;
+		dtlb_va			: word_t;
+		dtlb_en			: STD_LOGIC;
+	end record;
+
+	type biu_ctrl_out_t is record
+		itlb			: tlb_ctrl_out_t;
+		dtlb			: tlb_ctrl_out_t;
+	end record;
+
+	type pipe_ctrl_in_t is record
 		EB				: STD_LOGIC;
 		exc_inject		: STD_LOGIC;
 		exc_commit		: STD_LOGIC;
@@ -277,6 +364,33 @@ package mips_types is
 		NMI				: STD_LOGIC;
 		int				: STD_LOGIC;
 		exc_vec			: word_t;
+		ITLB			: tlb_status_t;
+		DTLB			: tlb_status_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;
+		dtlb_va			: word_t;
+		dtlb_en			: STD_LOGIC;
+	end record;
+
+	type cop0_ctrl_in_t is record
+		pipe			: pipe_ctrl_out_t;
+		itlb			: tlb_ctrl_out_t;
+		dtlb			: tlb_ctrl_out_t;
+	end record;
+
+	type cop0_ctrl_out_t is record
+		pipe			: pipe_ctrl_in_t;
+		itlb			: tlb_ctrl_in_t;
+		dtlb			: tlb_ctrl_in_t;
 		icache			: cache_ctrl_t;
 		dcache			: cache_ctrl_t;
 	end record;
@@ -337,15 +451,16 @@ 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;
@@ -357,10 +472,10 @@ package mips_types is
 	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;
 		ctrl		: ctrl_lines_t;
@@ -371,7 +486,7 @@ package mips_types is
 		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;
@@ -385,13 +500,13 @@ package mips_types is
 		nop			: STD_LOGIC;
 		exc			: std_logic;
 		op			: op_t;
-		pcn			: word_t;
 		epc			: word_t;
+		pcn			: word_t;
 		ctrl		: ctrl_lines_t;
 		ex_result	: word_t;
 		reg_wptr	: reg_ptr_t;
-		wreg_we		: STD_LOGIC;
-		data		: word_t;
+		reg_we		: STD_LOGIC;
+		reg_data	: word_t;
 		va			: word_t;
 		pa_off		: unsigned(1 downto 0);
 		events_in	: event_t;
@@ -400,13 +515,17 @@ package mips_types is
 	
 	type WB_t is record
 		nop			: STD_LOGIC;
+		epc			: word_t;
+		epc_last	: 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;
-		data		: word_t;
-		bd			: STD_LOGIC;
+		reg_we		: STD_LOGIC;
+		reg_data	: word_t;
+		va			: word_t;
 	end record;
 
 	
@@ -422,9 +541,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 lg2(x : natural) return natural;
-	function po2(x : natural) return natural;
+	function "or" (a, b : tlb_flags_t) return tlb_flags_t;
 
 end mips_types;
 
@@ -608,6 +725,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 +753,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 +779,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))));
-	end lg2;	
-	
---------------------------------------------------------------------------
-	function po2(x : natural) return natural is
-	begin
-
-		return 2**lg2(x);
-	end po2;	
+		result.N				:= a.N or b.N;
+		result.D				:= a.D or b.D;
+		result.V				:= a.V or b.V;
+		
+		return result;
+		
+	end "or";
 
 --------------------------------------------------------------------------
 end mips_types;