--------------------------------------------------------------------------
-- Project: JIPS, a portable 32-bit RISC CPU written in VHDL
-- This file: The arithmetic logic unit
--
-- Copyright (C) 2008 J. Ahrensfeld
--
-- This program is free software: you can redistribute it and/or modify
-- it under the terms of the GNU General Public License as published by
-- the Free Software Foundation, either version 3 of the License, or
-- (at your option) any later version.
--
-- This program is distributed in the hope that it will be useful,
-- but WITHOUT ANY WARRANTY; without even the implied warranty of
-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-- GNU General Public License for more details.
--
-- You should have received a copy of the GNU General Public License
-- along with this program.  If not, see <http://www.gnu.org/licenses/>.
--
-- For questions and ideas, please contact the author at jens@jayfield.org
--
--------------------------------------------------------------------------

library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.numeric_std.ALL;

use work.mips_types.all;
use work.mips_util_pkg.all;
use work.mips_instr.all;

entity cop is
	Generic
	(
		icache_size	: natural := 2048;	-- words
		icache_line	: natural := 8;		-- words
		dcache_size	: natural := 2048;	-- words
		dcache_line	: natural := 8		-- words
	);
	Port 
	(
		rst			: in  STD_LOGIC;
		clk			: in  STD_LOGIC;
		eb			: in  STD_LOGIC;
		int			: in  unsigned(5 downto 0);
		nmi			: in  STD_LOGIC;
		ir_en		: in  STD_LOGIC;
		ir			: in  word_t;
		ctrl_in		: in  cop0_ctrl_in_t;
		ctrl_out	: out cop0_ctrl_out_t;
		din			: in  word_t;
		dout		: out word_t
	);
end cop;

architecture Behavioral of cop is

	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 tlb_sel			: STD_LOGIC;
	signal epc_reg_we		: STD_LOGIC;
	signal code_reg_we		: STD_LOGIC;
	signal bd				: STD_LOGIC;
	signal sw_int			: unsigned(1 downto 0);
	signal ip				: unsigned(7 downto 0);
	signal im				: unsigned(7 downto 0);
	signal status_save		: STD_LOGIC;
	signal status_rest		: STD_LOGIC;
	signal eflags			: exc_flags_t;
	signal EB_reg			: STD_LOGIC;
	signal icache_info		: unsigned(7 downto 0);
	signal dcache_info		: unsigned(7 downto 0);
	signal reg_rptr			: reg_ptr_t;
	signal reg_wptr			: reg_ptr_t;
	signal inject_exc		: STD_LOGIC;
	signal latch_vect_en	: STD_LOGIC;
	signal int_nmi			: STD_LOGIC;
	signal nmi_asserted		: std_logic;
	
	type cop_pipe_t is record
		opc			: opcode_t;
		RFE			: std_logic;
		CO			: std_logic;
		reg_ptr		: reg_ptr_t;
		rs			: reg_ptr_t;
		rd			: reg_ptr_t;
		rt			: reg_ptr_t;
		func		: func_t;
		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;
	signal cop_pipe_EX : cop_pipe_t;
	
	function eval_int(ip : unsigned) return STD_LOGIC is
	variable result : STD_LOGIC;
	begin
			result := '0';
			for i in ip'range loop
				result := result or ip(i);
			end loop;

			return result;
	end eval_int;

	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

	-- 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);
	cop_pipe_ID.rd					<= extract_rd(ir);
	cop_pipe_ID.rt					<= extract_rt(ir);
	cop_pipe_ID.func				<= extract_func(ir);
	cop_pipe_ID.CO					<= ir(25);
	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';
	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;
	
	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;


-- NMI/RST transition detector
proc_nmi_transition:
	process(clk)
	begin
		if rising_edge(clk) then
			if rst = '1' then
				int_nmi			<= '0';
				nmi_asserted	<= '0';
			elsif latch_vect_en = '1' then
				int_nmi	<= '0';
			elsif nmi_asserted = '1' then
				if nmi = '0' then
					nmi_asserted	<= '0';
					int_nmi			<= '1';
				end if;	
			elsif nmi = '1' then
				nmi_asserted	<= '1';
			end if;	
		end if;
	end process;

-- Endian switch sampled at hard reset
sample_endian:
	process(clk)
	begin
		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
		if rising_edge(clk) then
			if rst = '1' then
				exc_state <= exc_idle;
			else
				exc_state <= exc_staten;
			end if;	
		end if;
	end process;

-- Main exception FSM
proc_exc_state:
	process(exc_state, ctrl_in, cop_pipe_ID)
	begin
	
		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';

		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:
	process(clk)
	begin
		if rising_edge(clk) then
			if rst = '1' then
				bd			<= '0';
				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;

-- BadVaddr and exception code write
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');
				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";
					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
				elsif eflags.Bp = '1'	then
					exc_code		<= "01001";
					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
				elsif eflags.RI = '1'	then
					exc_code		<= "01010";
					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
				elsif eflags.IAdEL = '1'	then
					exc_code		<= "00100";
					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
				elsif eflags.IAdEK = '1'	then
					exc_code		<= "00100";
					regs.badvaddr	<= ctrl_in.pipe.imem_addr;
				elsif eflags.DAdEL = '1'	then
					exc_code		<= "00100";
					regs.badvaddr	<= ctrl_in.pipe.dmem_addr;
				elsif eflags.DAdES = '1'	then
					exc_code		<= "00101";
					regs.badvaddr	<= ctrl_in.pipe.dmem_addr;
				elsif eflags.Int = '1' then
					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;

-- 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:
	process(clk)
	begin
		if rising_edge(clk) then
			ctrl_out.icache.inv_at	<= '0';
			ctrl_out.icache.inv_all	<= '0';
			ctrl_out.dcache.inv_at	<= '0';
			ctrl_out.dcache.inv_all	<= '0';
			if cop_pipe_ID.cs = '1' and cop_pipe_ID.CO = '1' then
				case cop_pipe_ID.func is
					when "100000" =>
						ctrl_out.icache.inv_all	<= '1';
					when "100001" =>
						ctrl_out.icache.inv_at	<= '1';
					when "100010" =>
						ctrl_out.dcache.inv_all	<= '1';
					when "100011" =>
						ctrl_out.dcache.inv_at	<= '1';

					when others => null;
				end case;
			end if;	
		end if;
	end process;

-- 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);
	begin
		if rising_edge(clk) then
			if rst = '1' then
				sw_int	<= (others => '0');
			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.pipe.int	<= (eval_int(ipm_v) and regs.status(0));
			ctrl_out.pipe.NMI	<= int_nmi;
		end if;
	end process;

		
-- WIRED: Wired Register (RO)
cop_wired_register:
	process(clk)
	begin
		if rising_edge(clk) then
			if rst = '1' then
				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;

-- CONFIG: COP Config Register
cop_config_register:
	process(clk)
	begin
		if rising_edge(clk) then
			if rst = '1' then
				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;

-- STATUS: COP Status Register
cop_status_register:
	process(clk)
	begin
		if rising_edge(clk) then
			if rst = '1' then
				regs.status	<= X"00600002";			-- at reset: set kernel mode enabled and interrupts disabled
			elsif status_save = '1' then
				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	
					regs.status(22)	<= '1';
				end if;
				regs.status(1 downto 0)	<= "10";	-- at exception: set kernel mode enabled and interrupts disabled
			elsif status_rest = '1' then
				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;	
					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;
	end process;
	
--------------------------------------------------------------------------
end Behavioral;
