-- C16 System On Chip Architecture
-- Copyright (C) 2003 by Cole Design and Development all rights reserved

library ieee;
use ieee.std_logic_1164.ALL;
use ieee.numeric_std.all;

entity core is
	Port (
		clock : in std_logic;									-- clock signal
		reset : in std_logic;									-- reset input
		irq : in std_logic;										-- interrupt
		read : out std_logic;									-- read data
		write : out std_logic;									-- write data
		data_in : in std_logic_vector (15 downto 0);		-- bytes read
		data_out : out std_logic_vector (15 downto 0);	-- bytes written
		heartbeat : out std_logic;								-- system core heartbeat
		address : out std_logic_vector (15 downto 0));	-- address bus
end core;

architecture Behavioral of core is
	signal clock_counter: integer;
	signal clock_phase: integer;
	signal r0 : unsigned (15 downto 0);			-- general purpose register 0
	signal r1 : unsigned (15 downto 0);			-- general purpose register 1
	signal r2 : unsigned (15 downto 0);			-- general purpose register 2
	signal r3 : unsigned (15 downto 0);			-- general purpose register 3
	signal r4 : unsigned (15 downto 0);			-- general purpose register 4
	signal sr : unsigned (15 downto 0);			-- status register
	signal lr : unsigned (15 downto 0);			-- link register
	signal pc : unsigned (15 downto 0);			-- program counter
	signal result : unsigned (15 downto 0);	-- result register
	signal OPCODE_ADD : std_logic;				-- addition
	signal OPCODE_AND : std_logic;				-- logical and
	signal OPCODE_B : std_logic;					-- branch
	signal OPCODE_BF : std_logic;					-- branch if false
	signal OPCODE_BIC : std_logic;				-- bit clear
	signal OPCODE_BIS : std_logic;				-- bit set
	signal OPCODE_BIT : std_logic;				-- bit test
	signal OPCODE_BL : std_logic;					-- branch with link
	signal OPCODE_BLF : std_logic;				-- branch with link if false
	signal OPCODE_BLT : std_logic;				-- branch with link if true
	signal OPCODE_BT : std_logic;					-- branch if true
	signal OPCODE_CE : std_logic;					-- compare equal
	signal OPCODE_CG : std_logic;					-- compare greater then
	signal OPCODE_CL : std_logic;					-- compare less then
	signal OPCODE_MOV : std_logic;				-- move
	signal OPCODE_NOT : std_logic;				-- logical not
	signal OPCODE_OR : std_logic;					-- logical or
	signal OPCODE_ROT : std_logic;				-- bit rotate
	signal OPCODE_SWP : std_logic;				-- swap bytes in word
	signal OPCODE_XOR : std_logic;				-- logical xor
	signal OPCODE_ILLEGAL : std_logic;			-- illegal instruction
	constant INST_MOV : std_logic_vector (1 downto 0)	:= "10";
	constant INST_B : std_logic_vector (5 downto 0) 	:= "000000";
	constant INST_BL : std_logic_vector (5 downto 0) 	:= "000001";
	constant INST_BT : std_logic_vector (5 downto 0) 	:= "000010";
	constant INST_BLT : std_logic_vector (5 downto 0)	:= "000011";
	constant INST_BF : std_logic_vector (5 downto 0) 	:= "000100";
	constant INST_BLF : std_logic_vector (5 downto 0)	:= "000101";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "000110";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "000111";
	constant INST_AND : std_logic_vector (5 downto 0)	:= "001000";
	constant INST_OR : std_logic_vector (5 downto 0) 	:= "001001";
	constant INST_XOR : std_logic_vector (5 downto 0)	:= "001010";
	constant INST_NOT : std_logic_vector (5 downto 0)	:= "001011";
	constant INST_ROT : std_logic_vector (5 downto 0)	:= "001100";
	constant INST_SWP : std_logic_vector (5 downto 0)	:= "001101";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "001110";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "001111";
	constant INST_BIS : std_logic_vector (5 downto 0)	:= "010000";
	constant INST_BIC : std_logic_vector (5 downto 0)	:= "010001";
	constant INST_BIT : std_logic_vector (5 downto 0)	:= "010010";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "010011";
	constant INST_CE : std_logic_vector (5 downto 0)	:= "010100";
	constant INST_CG : std_logic_vector (5 downto 0)	:= "010101";
	constant INST_CL : std_logic_vector (5 downto 0)	:= "010110";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "010111";
	constant INST_ADD : std_logic_vector (5 downto 0)	:= "011000";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "011001";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "011010";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "011011";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "011100";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "011101";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "011110";
--	constant INST_XXX : std_logic_vector (5 downto 0)	:= "011111";

	signal clock_phase_0 : std_logic := '1';
	signal clock_phase_1 : std_logic := '0';
	signal clock_phase_2 : std_logic := '0';
	signal clock_phase_3 : std_logic := '0';
	signal clock_phase_4 : std_logic := '0';
	signal clock_phase_5 : std_logic := '0';
	signal clock_phase_6 : std_logic := '0';
	signal clock_phase_change : std_logic;
	constant RESET_VECTOR : unsigned (15 downto 0) := "0000000000000000";
	constant INTERRUPT_VECTOR : unsigned (15 downto 0) := "0000000000000001";
	constant ILLEGAL_INSTRUCTION_VECTOR : unsigned (15 downto 0) := "0000000000000010";
	signal instruction  : std_logic_vector(15 downto 0);
begin

	OPCODE_ADD <= '1' when instruction(15 downto 10) = INST_ADD else '0';
	OPCODE_AND <= '1' when instruction(15 downto 10) = INST_AND else '0';
	OPCODE_B <= '1' when instruction(15 downto 10) = INST_B else '0';
	OPCODE_BF <= '1' when instruction(15 downto 10) = INST_BF else '0';
	OPCODE_BIC <= '1' when instruction(15 downto 10) = INST_BIC else '0';
	OPCODE_BIS <= '1' when instruction(15 downto 10) = INST_BIS else '0';
	OPCODE_BIT <= '1' when instruction(15 downto 10) = INST_BIT else '0';
	OPCODE_BL <= '1' when instruction(15 downto 10) = INST_BL else '0';
	OPCODE_BLF <= '1' when instruction(15 downto 10) = INST_BLF else '0';
	OPCODE_BLT <= '1' when instruction(15 downto 10) = INST_BLT else '0';
	OPCODE_BT <= '1' when instruction(15 downto 10) = INST_BT else '0';
	OPCODE_CE <= '1' when instruction(15 downto 10) = INST_CE else '0';
	OPCODE_CG <= '1' when instruction(15 downto 10) = INST_CG else '0';
	OPCODE_CL <= '1' when instruction(15 downto 10) = INST_CL else '0';
	OPCODE_MOV <= '1' when instruction(15 downto 14) = INST_MOV else '0';
	OPCODE_NOT <= '1' when instruction(15 downto 10) = INST_NOT else '0';
	OPCODE_OR <= '1' when instruction(15 downto 10) = INST_OR else '0';
	OPCODE_ROT <= '1' when instruction(15 downto 10) = INST_ROT else '0';
	OPCODE_SWP <= '1' when instruction(15 downto 10) = INST_SWP else '0';
	OPCODE_XOR <= '1' when instruction(15 downto 10) = INST_XOR else '0';
	OPCODE_ILLEGAL <= '0' when
		OPCODE_ADD = '1' or
		OPCODE_AND = '1' or
		OPCODE_B = '1' or
		OPCODE_BF = '1' or
		OPCODE_BIC = '1' or
		OPCODE_BIS = '1' or
		OPCODE_BIT = '1' or
		OPCODE_BL = '1' or
		OPCODE_BLF = '1' or
		OPCODE_BLT = '1' or
		OPCODE_BT = '1' or
		OPCODE_CE = '1' or
		OPCODE_CG = '1' or
		OPCODE_CL = '1' or
		OPCODE_MOV = '1' or
		OPCODE_NOT = '1' or
		OPCODE_OR = '1' or
		OPCODE_ROT = '1' or
		OPCODE_SWP = '1' or
		OPCODE_XOR = '1' else '1';

	process (clock_phase_change, reset)
	begin
		if reset = '1' then
			clock_phase_0 <= '1';
			clock_phase_1 <= '0';
			clock_phase_2 <= '0';
			clock_phase_3 <= '0';
			clock_phase_4 <= '0';
			clock_phase_5 <= '0';
			clock_phase_6 <= '0';
		elsif rising_edge(clock_phase_change) then
			clock_phase_0 <= clock_phase_6;
			clock_phase_1 <= clock_phase_0;
			clock_phase_2 <= clock_phase_1;
			clock_phase_3 <= clock_phase_2;
			clock_phase_4 <= clock_phase_3;
			clock_phase_5 <= clock_phase_4;
			clock_phase_6 <= clock_phase_5;
		end if;
	end process;

	process(clock, reset)
	begin
		if rising_edge(clock) then
			if reset = '1' then
				clock_counter <= 0;
			else
				if clock_counter < 10 then -- 10_000_000 then
					clock_counter <= clock_counter + 1;
					clock_phase_change <= '0';
				else
					clock_counter <= 0;
					clock_phase_change <= '1';
				end if;
			end if;
		end if;
	end process;

	process (clock_phase_change, reset, clock_phase_0, clock_phase_1, clock_phase_2, clock_phase_3, clock_phase_4, clock_phase_5, clock_phase_6)
	begin
		if reset = '1' then
			pc <= RESET_VECTOR;
			address <= std_logic_vector(RESET_VECTOR);
			heartbeat <= '0';
			read <= '0';
			write <= '0';
		else
			--------------------------------------------------------------------------------
			-- CLOCK PHASE 0: setup address for next instruction fetch
			--------------------------------------------------------------------------------
			if rising_edge(clock_phase_0) then
			end if;
			if clock_phase_0 = '1' then
				address <= std_logic_vector(pc);
				read <= '1';
				write <= '0';
				heartbeat <= '1';
			end if;
			--------------------------------------------------------------------------------
			-- CLOCK PHASE 1: load instruction
			--------------------------------------------------------------------------------
			if rising_edge(clock_phase_1) then
				instruction <= data_in;
			end if;
			if clock_phase_1 = '1' then
				read <= '1';
			end if;
			--------------------------------------------------------------------------------
			-- CLOCK PHASE 2: setup address for source
			--------------------------------------------------------------------------------
			if rising_edge(clock_phase_2) then
			end if;
			if clock_phase_2 = '1' then
				if OPCODE_MOV = '1' and instruction(8) = '1' then
					-- load from memory (register indirect or address)
					if instruction(9) = '0' then -- load from register indirect
						if instruction(2 downto 0) = "000" then
							address <= std_logic_vector(r0);
						elsif instruction(2 downto 0) = "001" then
							address <= std_logic_vector(r1);
						elsif instruction(2 downto 0) = "010" then
							address <= std_logic_vector(r2);
						elsif instruction(2 downto 0) = "011" then
							address <= std_logic_vector(r3);
						elsif instruction(2 downto 0) = "100" then
							address <= std_logic_vector(r4);
						elsif instruction(2 downto 0) = "101" then
							address <= std_logic_vector(sr);
						elsif instruction(2 downto 0) = "110" then
							address <= std_logic_vector(lr);
						else
							address <= std_logic_vector(pc);
						end if;
					else -- load from address
						address <= "00000000" & instruction(7 downto 0);
					end if;
				elsif OPCODE_OR = '1' and instruction(3) = '1' then
					-- register indirect
					if instruction(2 downto 0) = "000" then
						address <= std_logic_vector(r0);
					elsif instruction(2 downto 0) = "001" then
						address <= std_logic_vector(r1);
					elsif instruction(2 downto 0) = "010" then
						address <= std_logic_vector(r2);
					elsif instruction(2 downto 0) = "011" then
						address <= std_logic_vector(r3);
					elsif instruction(2 downto 0) = "100" then
						address <= std_logic_vector(r4);
					elsif instruction(2 downto 0) = "101" then
						address <= std_logic_vector(sr);
					elsif instruction(2 downto 0) = "110" then
						address <= std_logic_vector(lr);
					else
						address <= std_logic_vector(pc);
					end if;
				elsif OPCODE_BIT = '1' and instruction(9) = '1' then
					-- bit test register indirect
					if instruction(8 downto 6) = "000" then
						address <= std_logic_vector(r0);
					elsif instruction(8 downto 6) = "001" then
						address <= std_logic_vector(r1);
					elsif instruction(8 downto 6) = "010" then
						address <= std_logic_vector(r2);
					elsif instruction(8 downto 6) = "011" then
						address <= std_logic_vector(r3);
					elsif instruction(8 downto 6) = "100" then
						address <= std_logic_vector(r4);
					elsif instruction(8 downto 6) = "101" then
						address <= std_logic_vector(sr);
					elsif instruction(8 downto 6) = "110" then
						address <= std_logic_vector(lr);
					else
						address <= std_logic_vector(pc);
					end if;
				else
					address <= std_logic_vector(pc);
				end if;
				read <= '1';
			end if;
			--------------------------------------------------------------------------------
			-- CLOCK PHASE 3: read source (from register, address, or literal)
			--------------------------------------------------------------------------------
			if rising_edge(clock_phase_3) then
				if OPCODE_BIT = '1' then
					if instruction(9) = '0' then
						-- bit test register direct
						if instruction(8 downto 6) = "000" then
							result <= r0;
						elsif instruction(8 downto 6) = "001" then
							result <= r1;
						elsif instruction(8 downto 6) = "010" then
							result <= r2;
						elsif instruction(8 downto 6) = "011" then
							result <= r3;
						elsif instruction(8 downto 6) = "100" then
							result <= r4;
						elsif instruction(8 downto 6) = "101" then
							result <= sr;
						elsif instruction(8 downto 6) = "110" then
							result <= lr;
						else
							result <= pc;
						end if;
					else
						-- bit test register indirect
						result <= unsigned(data_in);
					end if;
				elsif OPCODE_MOV = '1' then
					if instruction(8) = '0' then
						-- load from register direct or literal
						if instruction(9) = '0' then
							-- load from register direct
							if instruction(2 downto 0) = "000" then
								result <= r0;
							elsif instruction(2 downto 0) = "001" then
								result <= r1;
							elsif instruction(2 downto 0) = "010" then
								result <= r2;
							elsif instruction(2 downto 0) = "011" then
								result <= r3;
							elsif instruction(2 downto 0) = "100" then
								result <= r4;
							elsif instruction(2 downto 0) = "101" then
								result <= sr;
							elsif instruction(2 downto 0) = "110" then
								result <= lr;
							else
								result <= pc;
							end if;
						else
							-- load from literal
							result <= "00000000" & unsigned(instruction(7 downto 0));
						end if;
					else
						-- load from memory
						result <= unsigned(data_in);
					end if;
				elsif OPCODE_OR = '1' then
					if instruction(3) = '0' then
						-- register direct
						if instruction(2 downto 0) = "000" then
							result <= r0;
						elsif instruction(2 downto 0) = "001" then
							result <= r1;
						elsif instruction(2 downto 0) = "010" then
							result <= r2;
						elsif instruction(2 downto 0) = "011" then
							result <= r3;
						elsif instruction(2 downto 0) = "100" then
							result <= r4;
						elsif instruction(2 downto 0) = "101" then
							result <= sr;
						elsif instruction(2 downto 0) = "110" then
							result <= lr;
						else
							result <= pc;
						end if;
					else
						-- register indirect
						result <= unsigned(data_in);
					end if;
				elsif OPCODE_SWP = '1' and instruction(9) = '0' then
					-- register direct
					if instruction(2 downto 0) = "000" then
						result <= r0;
					elsif instruction(2 downto 0) = "001" then
						result <= r1;
					elsif instruction(2 downto 0) = "010" then
						result <= r2;
					elsif instruction(2 downto 0) = "011" then
						result <= r3;
					elsif instruction(2 downto 0) = "100" then
						result <= r4;
					elsif instruction(2 downto 0) = "101" then
						result <= sr;
					elsif instruction(2 downto 0) = "110" then
						result <= lr;
					else
						result <= pc;
					end if;
				end if;
			end if;
			if clock_phase_3 = '1' then
				read <= '1';
			end if;
			--------------------------------------------------------------------------------
			-- CLOCK PHASE 4: setup address and data for destination, and manipulate result reg is needed
			--------------------------------------------------------------------------------
			if rising_edge(clock_phase_4) then
			end if;
			if clock_phase_4 = '1' then
				if OPCODE_MOV = '1' and instruction(13) = '1' then
					-- writing register indirect (to an address)
					if instruction(12 downto 10) = "000" then
						address <= std_logic_vector(r0);
					elsif instruction(12 downto 10) = "001" then
						address <= std_logic_vector(r1);
					elsif instruction(12 downto 10) = "010" then
						address <= std_logic_vector(r2);
					elsif instruction(12 downto 10) = "011" then
						address <= std_logic_vector(r3);
					elsif instruction(12 downto 10) = "100" then
						address <= std_logic_vector(r4);
					elsif instruction(12 downto 10) = "101" then
						address <= std_logic_vector(sr);
					elsif instruction(12 downto 10) = "110" then
						address <= std_logic_vector(lr);
					else
						address <= std_logic_vector(pc);
					end if;
				elsif OPCODE_OR = '1' and instruction(9) = '1' then
					-- writing register indirect (to an address)
					if instruction(8 downto 6) = "000" then
						address <= std_logic_vector(r0);
					elsif instruction(8 downto 6) = "001" then
						address <= std_logic_vector(r1);
					elsif instruction(8 downto 6) = "010" then
						address <= std_logic_vector(r2);
					elsif instruction(8 downto 6) = "011" then
						address <= std_logic_vector(r3);
					elsif instruction(8 downto 6) = "100" then
						address <= std_logic_vector(r4);
					elsif instruction(8 downto 6) = "101" then
						address <= std_logic_vector(sr);
					elsif instruction(8 downto 6) = "110" then
						address <= std_logic_vector(lr);
					else
						address <= std_logic_vector(pc);
					end if;
				else
					address <= std_logic_vector(pc);
				end if;
				read <= '0';
			   heartbeat <= '0';
			end if;
			--------------------------------------------------------------------------------
			-- CLOCK PHASE 5: write to destination and setup program counter for next instruction
			--------------------------------------------------------------------------------
			if rising_edge(clock_phase_5) then
				if OPCODE_BIT = '1' then
					if (instruction(3 downto 0) = "0000" and result(0) = '1') or
					(instruction(3 downto 0) = "0001" and result(1) = '1') or
					(instruction(3 downto 0) = "0010" and result(2) = '1') or
					(instruction(3 downto 0) = "0011" and result(3) = '1') or
					(instruction(3 downto 0) = "0100" and result(4) = '1') or
					(instruction(3 downto 0) = "0101" and result(5) = '1') or
					(instruction(3 downto 0) = "0110" and result(6) = '1') or
					(instruction(3 downto 0) = "0111" and result(7) = '1') or
					(instruction(3 downto 0) = "1000" and result(8) = '1') or
					(instruction(3 downto 0) = "1001" and result(9) = '1') or
					(instruction(3 downto 0) = "1010" and result(10) = '1') or
					(instruction(3 downto 0) = "1011" and result(11) = '1') or
					(instruction(3 downto 0) = "1100" and result(12) = '1') or
					(instruction(3 downto 0) = "1101" and result(13) = '1') or
					(instruction(3 downto 0) = "1110" and result(14) = '1') or
					(instruction(3 downto 0) = "1111" and result(15) = '1') then
						sr(0) <= '1';
					else
						sr(0) <= '0';
					end if;
				elsif OPCODE_BL = '1' then
					lr <= pc + "000000000001";
				elsif OPCODE_MOV = '1' then
					if instruction(13) = '0' then
						-- register direct destination
						if instruction(12 downto 10) = "000" then
							r0 <= result;
						elsif instruction(12 downto 10) = "001" then
							r1 <= result;
						elsif instruction(12 downto 10) = "010" then
							r2 <= result;
						elsif instruction(12 downto 10) = "011" then
							r3 <= result;
						elsif instruction(12 downto 10) = "100" then
							r4 <= result;
						elsif instruction(12 downto 10) = "101" then
							sr <= result;
						elsif instruction(12 downto 10) = "110" then
							lr <= result;
						end if;
					else
						-- register indirect destination
						data_out <= std_logic_vector(result);
					end if;
				elsif OPCODE_OR = '1' then
					if instruction(9) = '0' then
						-- register direct destination
						if instruction(8 downto 6) = "000" then
							r0 <= result or r0;
						elsif instruction(8 downto 6) = "001" then
							r1 <= result or r1;
						elsif instruction(8 downto 6) = "010" then
							r2 <= result or r2;
						elsif instruction(8 downto 6) = "011" then
							r3 <= result or r3;
						elsif instruction(8 downto 6) = "100" then
							r4 <= result or r4;
						elsif instruction(8 downto 6) = "101" then
							sr <= result or sr;
						elsif instruction(8 downto 6) = "110" then
							lr <= result or lr;
						end if;
					else
						-- register indirect destination
						data_out <= std_logic_vector(result) or data_in;
					end if;
				elsif OPCODE_SWP = '1' then
					if instruction(9) = '0' then
						-- register direct
						if instruction(2 downto 0) = "000" then
							r0 <= result(7 downto 0) & result(15 downto 8);
						elsif instruction(2 downto 0) = "001" then
							r1 <= result(7 downto 0) & result(15 downto 8);
						elsif instruction(2 downto 0) = "010" then
							r2 <= result(7 downto 0) & result(15 downto 8);
						elsif instruction(2 downto 0) = "011" then
							r3 <= result(7 downto 0) & result(15 downto 8);
						elsif instruction(2 downto 0) = "100" then
							r4 <= result(7 downto 0) & result(15 downto 8);
						elsif instruction(2 downto 0) = "101" then
							sr <= result(7 downto 0) & result(15 downto 8);
						elsif instruction(2 downto 0) = "110" then
							lr <= result(7 downto 0) & result(15 downto 8);
						end if;
					end if;
				end if;
			end if;
			if clock_phase_5 = '1' then
				write <= '1';
			end if;
			--------------------------------------------------------------------------------
			-- CLOCK PHASE 6: setup program counter for next instruction
			--------------------------------------------------------------------------------
			if rising_edge(clock_phase_6) then
				if OPCODE_ILLEGAL = '1' then
					pc <= ILLEGAL_INSTRUCTION_VECTOR;
				elsif (OPCODE_B = '1') or (OPCODE_BL = '1') or (OPCODE_BT = '1' and sr(0) = '1') then
					pc <= "000000" & unsigned(instruction(9 downto 0));
				elsif OPCODE_MOV = '1' and instruction(13) = '0' and instruction(12 downto 10) = "111" then
					pc <= result;
				elsif OPCODE_OR = '1' and instruction(8 downto 6) = "111" then
					pc <= result or pc;
				elsif OPCODE_SWP = '1' and instruction(9) = '0' and instruction(2 downto 0) = "111" then
					pc <= result(7 downto 0) & result(15 downto 8);
				else
					pc <= pc + "000000000001";
				end if;
			end if;
			if clock_phase_6 = '1' then
				write <= '0';
			end if;
		end if;
	end process;
end Behavioral;