From 98f09946980328693db0e2e2039217957cc09fe6 Mon Sep 17 00:00:00 2001
From: Benjamin Herrenschmidt <benh@kernel.crashing.org>
Date: Tue, 10 Sep 2019 17:43:52 +0100
Subject: [PATCH] Add core debug module

This module adds some simple core controls:

  reset, stop, start, step

along with icache clear and reading the NIA and core
status bits

Signed-off-by: Benjamin Herrenschmidt <benh@kernel.crashing.org
---
 Makefile        |  10 ++--
 common.vhdl     |   9 ++-
 core.vhdl       |  70 ++++++++++++++++++----
 core_debug.vhdl | 152 ++++++++++++++++++++++++++++++++++++++++++++++++
 core_tb.vhdl    |  33 ++++++-----
 decode1.vhdl    |   1 +
 decode2.vhdl    |  11 +++-
 fetch1.vhdl     |   2 +
 fetch2.vhdl     |   7 ++-
 soc.vhdl        |  54 +++++++----------
 10 files changed, 281 insertions(+), 68 deletions(-)
 create mode 100644 core_debug.vhdl

diff --git a/Makefile b/Makefile
index b675a8f..2bd0074 100644
--- a/Makefile
+++ b/Makefile
@@ -13,8 +13,10 @@ all: $(all)
 	$(GHDL) -a $(GHDLFLAGS) $<
 
 common.o: decode_types.o
-core_tb.o: common.o core.o soc.o
-core.o: common.o wishbone_types.o fetch1.o fetch2.o icache.o decode1.o decode2.o register_file.o cr_file.o execute1.o execute2.o loadstore1.o loadstore2.o multiply.o writeback.o
+sim_jtag.o: sim_jtag_socket.o
+core_tb.o: common.o core.o soc.o sim_jtag.o
+core.o: common.o wishbone_types.o fetch1.o fetch2.o icache.o decode1.o decode2.o register_file.o cr_file.o execute1.o execute2.o loadstore1.o loadstore2.o multiply.o writeback.o core_debug.o
+core_debug.o:
 cr_file.o: common.o
 crhelpers.o: common.o
 decode1.o: common.o decode_types.o
@@ -59,8 +61,8 @@ fpga/soc_reset_tb.o: fpga/soc_reset.o
 soc_reset_tb: fpga/soc_reset_tb.o fpga/soc_reset.o
 	$(GHDL) -e $(GHDLFLAGS) soc_reset_tb
 
-core_tb: core_tb.o simple_ram_behavioural_helpers_c.o sim_console_c.o
-	$(GHDL) -e $(GHDLFLAGS) -Wl,simple_ram_behavioural_helpers_c.o -Wl,sim_console_c.o $@
+core_tb: core_tb.o simple_ram_behavioural_helpers_c.o sim_console_c.o sim_jtag_socket_c.o
+	$(GHDL) -e $(GHDLFLAGS) -Wl,simple_ram_behavioural_helpers_c.o -Wl,sim_console_c.o -Wl,sim_jtag_socket_c.o $@
 
 fetch_tb: fetch_tb.o
 	$(GHDL) -e $(GHDLFLAGS) $@
diff --git a/common.vhdl b/common.vhdl
index bf383ca..fe6738e 100644
--- a/common.vhdl
+++ b/common.vhdl
@@ -13,23 +13,26 @@ package common is
 	end record;
 
 	type Fetch1ToFetch2Type is record
-		nia: std_ulogic_vector(63 downto 0);
+	    nia: std_ulogic_vector(63 downto 0);
+	    pipe_stop : std_ulogic;
 	end record;
 
 	type Fetch2ToDecode1Type is record
 		valid: std_ulogic;
+		stop_mark : std_ulogic;
 		nia: std_ulogic_vector(63 downto 0);
 		insn: std_ulogic_vector(31 downto 0);
 	end record;
-	constant Fetch2ToDecode1Init : Fetch2ToDecode1Type := (valid => '0', others => (others => '0'));
+	constant Fetch2ToDecode1Init : Fetch2ToDecode1Type := (valid => '0', stop_mark => '0', others => (others => '0'));
 
 	type Decode1ToDecode2Type is record
 		valid: std_ulogic;
+		stop_mark : std_ulogic;
 		nia: std_ulogic_vector(63 downto 0);
 		insn: std_ulogic_vector(31 downto 0);
 		decode: decode_rom_t;
 	end record;
-	constant Decode1ToDecode2Init : Decode1ToDecode2Type := (valid => '0', decode => decode_rom_init, others => (others => '0'));
+	constant Decode1ToDecode2Init : Decode1ToDecode2Type := (valid => '0', stop_mark => '0', decode => decode_rom_init, others => (others => '0'));
 
 	type Fetch2ToIcacheType is record
 		req: std_ulogic;
diff --git a/core.vhdl b/core.vhdl
index d34bf71..d0bd5c5 100644
--- a/core.vhdl
+++ b/core.vhdl
@@ -20,9 +20,14 @@ entity core is
         wishbone_data_in  : in wishbone_slave_out;
         wishbone_data_out : out wishbone_master_out;
 
-        -- Added for debug, ghdl doesn't support external names unfortunately
-        registers     : out regfile;
-        terminate_out : out std_ulogic
+	dmi_addr	: in std_ulogic_vector(3 downto 0);
+	dmi_din	        : in std_ulogic_vector(63 downto 0);
+	dmi_dout	: out std_ulogic_vector(63 downto 0);
+	dmi_req	        : in std_ulogic;
+	dmi_wr		: in std_ulogic;
+	dmi_ack	        : out std_ulogic;
+
+	terminated_out   : out std_logic
         );
 end core;
 
@@ -73,11 +78,23 @@ architecture behave of core is
     signal flush: std_ulogic;
 
     signal complete: std_ulogic;
-
     signal terminate: std_ulogic;
+    signal core_rst: std_ulogic;
+
+    -- Debug actions
+    signal dbg_core_stop: std_ulogic;
+    signal dbg_core_rst: std_ulogic;
+    signal dbg_icache_rst: std_ulogic;
+
+    -- Debug status
+    signal dbg_core_is_stopped: std_ulogic;
+
+    -- For sim
+    signal registers: regfile;
+
 begin
 
-    terminate_out <= terminate;
+    core_rst <= dbg_core_rst or rst;
 
     fetch1_0: entity work.fetch1
         generic map (
@@ -85,7 +102,7 @@ begin
             )
         port map (
             clk => clk,
-            rst => rst,
+            rst => core_rst,
             stall_in => fetch1_stall_in,
             flush_in => flush,
             e_in => execute1_to_fetch1,
@@ -97,12 +114,13 @@ begin
     fetch2_0: entity work.fetch2
         port map (
             clk => clk,
-            rst => rst,
+            rst => core_rst,
             stall_in => fetch2_stall_in,
             stall_out => fetch2_stall_out,
             flush_in => flush,
             i_in => icache_to_fetch2,
             i_out => fetch2_to_icache,
+	    stop_in => dbg_core_stop,
             f_in => fetch1_to_fetch2,
             f_out => fetch2_to_decode1
             );
@@ -116,7 +134,7 @@ begin
             )
         port map(
             clk => clk,
-            rst => rst,
+            rst => rst or dbg_icache_rst,
             i_in => fetch2_to_icache,
             i_out => icache_to_fetch2,
             wishbone_out => wishbone_insn_out,
@@ -126,7 +144,7 @@ begin
     decode1_0: entity work.decode1
         port map (
             clk => clk,
-            rst => rst,
+            rst => core_rst,
             stall_in => decode1_stall_in,
             flush_in => flush,
             f_in => fetch2_to_decode1,
@@ -138,10 +156,11 @@ begin
     decode2_0: entity work.decode2
         port map (
             clk => clk,
-            rst => rst,
+            rst => core_rst,
             stall_out => decode2_stall_out,
             flush_in => flush,
             complete_in => complete,
+	    stopped_out => dbg_core_is_stopped,
             d_in => decode1_to_decode2,
             e_out => decode2_to_execute1,
             l_out => decode2_to_loadstore1,
@@ -222,4 +241,35 @@ begin
             complete_out => complete
             );
 
+    debug_0: entity work.core_debug
+	port map (
+	    clk => clk,
+	    rst => rst,
+	    dmi_addr => dmi_addr,
+	    dmi_din => dmi_din,
+	    dmi_dout => dmi_dout,
+	    dmi_req => dmi_req,
+	    dmi_wr => dmi_wr,
+	    dmi_ack => dmi_ack,
+	    core_stop => dbg_core_stop,
+	    core_rst => dbg_core_rst,
+	    icache_rst => dbg_icache_rst,
+	    terminate => terminate,
+	    core_stopped => dbg_core_is_stopped,
+	    nia => fetch1_to_fetch2.nia,
+	    terminated_out => terminated_out
+	    );
+
+    -- Dump registers if core terminates
+    sim_terminate_test: if SIM generate
+	dump_registers: process(all)
+	begin
+	    if terminate = '1' then
+		loop_0: for i in 0 to 31 loop
+		    report "REG " & to_hstring(registers(i));
+		end loop loop_0;
+	    end if;
+	end process;
+    end generate;
+
 end behave;
diff --git a/core_debug.vhdl b/core_debug.vhdl
new file mode 100644
index 0000000..c93c70d
--- /dev/null
+++ b/core_debug.vhdl
@@ -0,0 +1,152 @@
+library ieee;
+use ieee.std_logic_1164.all;
+use ieee.numeric_std.all;
+
+library work;
+use work.common.all;
+
+entity core_debug is
+    port (
+        clk          : in std_logic;
+        rst          : in std_logic;
+
+	dmi_addr	: in std_ulogic_vector(3 downto 0);
+	dmi_din	        : in std_ulogic_vector(63 downto 0);
+	dmi_dout	: out std_ulogic_vector(63 downto 0);
+	dmi_req	        : in std_ulogic;
+	dmi_wr		: in std_ulogic;
+	dmi_ack	        : out std_ulogic;
+
+	-- Debug actions
+	core_stop       : out std_ulogic;
+	core_rst        : out std_ulogic;
+	icache_rst      : out std_ulogic;
+
+	-- Core status inputs
+	terminate       : in std_ulogic;
+	core_stopped    : in std_ulogic;
+	nia             : in std_ulogic_vector(63 downto 0);
+
+	-- Misc
+	terminated_out  : out std_ulogic
+        );
+end core_debug;
+
+architecture behave of core_debug is
+        -- DMI needs fixing... make a one clock pulse
+    signal dmi_req_1: std_ulogic;
+
+    -- CTRL register (direct actions, write 1 to act, read back 0)
+    -- bit     0 : Core stop
+    -- bit     1 : Core reset (doesn't clear stop)
+    -- bit     2 : Icache reset
+    -- bit     3 : Single step
+    -- bit     4 : Core start
+    constant DBG_CORE_CTRL	   : std_ulogic_vector(3 downto 0) := "0000";
+    constant DBG_CORE_CTRL_STOP    : integer := 0;
+    constant DBG_CORE_CTRL_RESET   : integer := 1;
+    constant DBG_CORE_CTRL_ICRESET : integer := 2;
+    constant DBG_CORE_CTRL_STEP    : integer := 3;
+    constant DBG_CORE_CTRL_START   : integer := 4;
+
+    -- STAT register (read only)
+    -- bit    0 : Core stopping (wait til bit 1 set)
+    -- bit    1 : Core stopped
+    -- bit    2 : Core terminated (clears with start or reset)
+    constant DBG_CORE_STAT	   : std_ulogic_vector(3 downto 0) := "0001";
+    constant DBG_CORE_STAT_STOPPING  : integer := 0;
+    constant DBG_CORE_STAT_STOPPED   : integer := 1;
+    constant DBG_CORE_STAT_TERM      : integer := 2;
+
+    -- NIA register (read only for now)
+    constant DBG_CORE_NIA	     : std_ulogic_vector(3 downto 0) := "0010";
+
+    -- Some internal wires
+    signal stat_reg : std_ulogic_vector(63 downto 0);
+
+    -- Some internal latches
+    signal stopping     : std_ulogic;
+    signal do_step      : std_ulogic;
+    signal do_reset     : std_ulogic;
+    signal do_icreset   : std_ulogic;
+    signal terminated   : std_ulogic;
+
+begin
+       -- Single cycle register accesses on DMI
+    dmi_ack <= dmi_req;
+
+    -- Status register read composition
+    stat_reg <= (2 => terminated,
+		 1 => core_stopped,
+		 0 => stopping,
+		 others => '0');
+
+    -- DMI read data mux
+    with dmi_addr select dmi_dout <=
+	stat_reg        when DBG_CORE_STAT,
+	nia             when DBG_CORE_NIA,
+	(others => '0') when others;
+
+    -- DMI writes
+    reg_write: process(clk)
+    begin
+	if rising_edge(clk) then
+	    if (rst) then
+		stopping <= '0';
+		terminated <= '0';
+	    else
+		-- Reset the 1-cycle "do" signals
+		do_step <= '0';
+		do_reset <= '0';
+		do_icreset <= '0';
+
+		-- Edge detect on dmi_req for 1-shot pulses
+		dmi_req_1 <= dmi_req;
+		if dmi_req = '1' and dmi_req_1 = '0' then
+		    if dmi_wr = '1' then
+			report("DMI write to " & to_hstring(dmi_addr));
+
+			-- Control register actions
+			if dmi_addr = DBG_CORE_CTRL then
+			    if dmi_din(DBG_CORE_CTRL_RESET) = '1' then
+				do_reset <= '1';
+				terminated <= '0';
+			    end if;
+			    if dmi_din(DBG_CORE_CTRL_STOP) = '1' then
+				stopping <= '1';
+			    end if;
+			    if dmi_din(DBG_CORE_CTRL_STEP) = '1' then
+				do_step <= '1';
+				terminated <= '0';
+			    end if;
+			    if dmi_din(DBG_CORE_CTRL_ICRESET) = '1' then
+				do_icreset <= '1';
+			    end if;
+			    if dmi_din(DBG_CORE_CTRL_START) = '1' then
+				stopping <= '0';
+				terminated <= '0';
+			    end if;
+			end if;
+		    else
+			report("DMI read from " & to_string(dmi_addr));
+		    end if;
+		end if;
+
+		-- Set core stop on terminate. We'll be stopping some time *after*
+		-- the offending instruction, at least until we can do back flushes
+		-- that preserve NIA which we can't just yet.
+		if terminate = '1' then
+		    stopping <= '1';
+		    terminated <= '1';
+		end if;
+	    end if;
+	end if;
+    end process;
+
+    -- Core control signals generated by the debug module
+    core_stop <= stopping and not do_step;
+    core_rst <= do_reset;
+    icache_rst <= do_icreset;
+    terminated_out <= terminated;
+end behave;
+
diff --git a/core_tb.vhdl b/core_tb.vhdl
index 4522da4..672b424 100644
--- a/core_tb.vhdl
+++ b/core_tb.vhdl
@@ -1,5 +1,6 @@
 library ieee;
 use ieee.std_logic_1164.all;
+use ieee.numeric_std.all;
 
 library work;
 use work.common.all;
@@ -29,19 +30,21 @@ begin
 	    uart0_txd => open
 	    );
 
-	clk_process: process
-	begin
-		clk <= '0';
-		wait for clk_period/2;
-		clk <= '1';
-		wait for clk_period/2;
-	end process;
-
-	rst_process: process
-	begin
-		rst <= '1';
-		wait for 10*clk_period;
-		rst <= '0';
-		wait;
-	end process;
+    clk_process: process
+    begin
+	clk <= '0';
+	wait for clk_period/2;
+	clk <= '1';
+	wait for clk_period/2;
+    end process;
+
+    rst_process: process
+    begin
+	rst <= '1';
+	wait for 10*clk_period;
+	rst <= '0';
+	wait;
+    end process;
+
+    jtag: entity work.sim_jtag;
 end;
diff --git a/decode1.vhdl b/decode1.vhdl
index 6e8a521..3e2026d 100644
--- a/decode1.vhdl
+++ b/decode1.vhdl
@@ -248,6 +248,7 @@ begin
 		v.valid := f_in.valid;
 		v.nia  := f_in.nia;
 		v.insn := f_in.insn;
+		v.stop_mark := f_in.stop_mark;
 
 		ppc_insn := PPC_ILLEGAL;
 
diff --git a/decode2.vhdl b/decode2.vhdl
index 15dae5d..482b91c 100644
--- a/decode2.vhdl
+++ b/decode2.vhdl
@@ -16,6 +16,8 @@ entity decode2 is
 		complete_in : in std_ulogic;
 		stall_out : out std_ulogic;
 
+		stopped_out : out std_ulogic;
+
 		flush_in: in std_ulogic;
 
 		d_in  : in Decode1ToDecode2Type;
@@ -330,9 +332,16 @@ begin
 		-- through the pipeline.
 		stall_out <= '0';
 		is_valid := d_in.valid;
+
+		-- Handle debugger stop
+		stopped_out <= '0';
+		if d_in.stop_mark = '1' and v_int.outstanding = 0 then
+		    stopped_out <= '1';
+		end if;
+
 		case v_int.state is
 		when IDLE =>
-			if (flush_in = '0') and (d_in.valid = '1') and (d_in.decode.sgl_pipe = '1') then
+			if (flush_in = '0') and (is_valid = '1') and (d_in.decode.sgl_pipe = '1') then
 				if v_int.outstanding /= 0 then
 					v_int.state := WAIT_FOR_PREV_TO_COMPLETE;
 					stall_out <= '1';
diff --git a/fetch1.vhdl b/fetch1.vhdl
index ff7d64a..8e8c5a5 100644
--- a/fetch1.vhdl
+++ b/fetch1.vhdl
@@ -68,6 +68,8 @@ begin
 
 		-- Update outputs
 		f_out <= r;
+
+		report "fetch1 R:" & std_ulogic'image(e_in.redirect) & " v.nia:" & to_hstring(v.nia) & " f_out.nia:" & to_hstring(f_out.nia);
 	end process;
 
 end architecture behaviour;
diff --git a/fetch2.vhdl b/fetch2.vhdl
index 37cb66c..9573761 100644
--- a/fetch2.vhdl
+++ b/fetch2.vhdl
@@ -15,6 +15,7 @@ entity fetch2 is
 		stall_out    : out std_ulogic;
 
 		flush_in     : in std_ulogic;
+		stop_in      : in std_ulogic;
 
 		i_in         : in IcacheToFetch2Type;
 		i_out        : out Fetch2ToIcacheType;
@@ -49,12 +50,12 @@ begin
 		v.valid := i_in.ack;
 		v.nia := f_in.nia;
 		v.insn := i_in.insn;
-		stall_out <= not i_in.ack;
+		stall_out <= stop_in or not i_in.ack;
 
-
-		if flush_in = '1' then
+		if flush_in = '1' or stop_in = '1' then
 			v.valid := '0';
 		end if;
+		v.stop_mark := stop_in;
 
 		-- Update registers
 		rin <= v;
diff --git a/soc.vhdl b/soc.vhdl
index 4b02807..39d72a9 100644
--- a/soc.vhdl
+++ b/soc.vhdl
@@ -1,8 +1,9 @@
 library ieee;
 use ieee.std_logic_1164.all;
+use ieee.numeric_std.all;
 use ieee.math_real.all;
-
 use std.textio.all;
+use std.env.stop;
 
 library work;
 use work.common.all;
@@ -24,7 +25,10 @@ entity soc is
 
 	-- UART0 signals:
 	uart0_txd    : out std_ulogic;
-	uart0_rxd    : in  std_ulogic
+	uart0_rxd    : in  std_ulogic;
+
+	-- Misc (to use for things like LEDs)
+	core_terminated : out std_ulogic
 	);
 end entity soc;
 
@@ -52,10 +56,6 @@ architecture behaviour of soc is
     signal wb_bram_out    : wishbone_slave_out;
     constant mem_adr_bits : positive := positive(ceil(log2(real(MEMORY_SIZE))));
 
-    -- Core debug signals (used in SIM only)
-    signal registers     : regfile;
-    signal terminate     : std_ulogic;
-
     -- DMI debug bus signals
     signal dmi_addr	: std_ulogic_vector(7 downto 0);
     signal dmi_din	: std_ulogic_vector(63 downto 0);
@@ -85,8 +85,12 @@ begin
 	    wishbone_insn_out => wishbone_icore_out,
 	    wishbone_data_in => wishbone_dcore_in,
 	    wishbone_data_out => wishbone_dcore_out,
-	    registers => registers,
-	    terminate_out => terminate
+	    dmi_addr => dmi_addr(3 downto 0),
+	    dmi_dout => dmi_core_dout,
+	    dmi_din => dmi_dout,
+	    dmi_wr => dmi_wr,
+	    dmi_ack => dmi_core_ack,
+	    dmi_req => dmi_core_req
 	    );
 
     -- Wishbone bus master arbiter & mux
@@ -136,20 +140,6 @@ begin
     end process slave_intercon;
 
     -- Simulated memory and UART
-    sim_terminate_test: if SIM generate
-
-	-- Dump registers if core terminates
-	dump_registers: process(all)
-	begin
-	    if terminate = '1' then
-		loop_0: for i in 0 to 31 loop
-		    report "REG " & to_hstring(registers(i));
-		end loop loop_0;
-		assert false report "end of test" severity failure;
-	    end if;
-	end process;
-
-    end generate;
 
     -- UART0 wishbone slave
     -- XXX FIXME: Need a proper wb64->wb8 adapter that
@@ -207,8 +197,8 @@ begin
 
     -- DMI interconnect
     dmi_intercon: process(dmi_addr, dmi_req,
-		     dmi_wb_ack, dmi_wb_dout,
-		     dmi_core_ack, dmi_core_dout)
+			  dmi_wb_ack, dmi_wb_dout,
+			  dmi_core_ack, dmi_core_dout)
 
 	-- DMI address map (each address is a full 64-bit register)
 	--
@@ -222,12 +212,11 @@ begin
 	variable slave : slave_type;
     begin
 	-- Simple address decoder
-	if dmi_addr(7 downto 0) = "000000--" then
+	slave := SLAVE_NONE;
+	if std_match(dmi_addr, "000000--") then
 	    slave := SLAVE_WB;
-	elsif dmi_addr(7 downto 0) = "0001----" then
+	elsif std_match(dmi_addr, "0001----") then
 	    slave := SLAVE_CORE;
-	else
-	    slave := SLAVE_NONE;
 	end if;
 
 	-- DMI muxing
@@ -246,11 +235,12 @@ begin
 	    dmi_ack <= dmi_req;
 	    dmi_din <= (others => '1');
 	end case;
-    end process;
 
-    -- Core dummy
-    dmi_core_ack <= dmi_core_req;
-    dmi_core_dout <= x"0000000000000000";
+	-- SIM magic exit
+	if SIM and dmi_req = '1' and dmi_addr = "11111111" and dmi_wr = '1' then
+	    stop;
+	end if;
+    end process;
 
     -- Wishbone debug master (TODO: Add a DMI address decoder)
     wishbone_debug: entity work.wishbone_debug_master