From 504e7cf208acdcbb275a0cd6a951491ff303509b Mon Sep 17 00:00:00 2001
From: Luke Kenneth Casson Leighton <lkcl@lkcl.net>
Date: Fri, 9 Apr 2021 00:24:42 +0100
Subject: [PATCH] sort out pc reset when DMI interface requests reset

---
 src/soc/debug/firmware_upload.py | 12 ++++++++---
 src/soc/simple/issuer.py         | 37 ++++++++++++++++++++------------
 2 files changed, 32 insertions(+), 17 deletions(-)

diff --git a/src/soc/debug/firmware_upload.py b/src/soc/debug/firmware_upload.py
index b16309cc..7480e8d4 100644
--- a/src/soc/debug/firmware_upload.py
+++ b/src/soc/debug/firmware_upload.py
@@ -29,6 +29,10 @@ def test_pinset():
             'test': ['io0-', 'io1+', 'io2>', 'io3*'],
            }
 
+def brev(n, width):
+    b = '{:0{width}b}'.format(n, width=width)
+    return int(b[::-1], 2)
+
 
 # JTAG-ircodes for accessing DMI
 DMI_ADDR = 8
@@ -83,7 +87,9 @@ def jtag_sim(dut, firmware):
     print ("dmi ctrl status", bin(status))
 
     # write DMI CTRL register - STOP and RESET
-    status = yield from writeread_dmi_addr(dut, DBGCore.CTRL, 0b011)
+    status = yield from writeread_dmi_addr(dut, DBGCore.CTRL,
+                        (1<<DBGCtrl.STOP) |
+                        (1<<DBGCtrl.RESET))
     print ("dmi ctrl status", hex(status))
     assert status == 0 # returned old value (nice! cool feature!)
 
@@ -102,7 +108,7 @@ def jtag_sim(dut, firmware):
     # write/read wishbone data
     for val in firmware:
         data = yield from jtag_read_write_reg(dut, WB_WRRD, 32, val)
-        print ("wb write", val, hex(data))
+        print ("wb write", hex(val), hex(data))
 
     # write Wishbone address
     yield from jtag_read_write_reg(dut, WB_ADDR, 30, 0)
@@ -110,7 +116,7 @@ def jtag_sim(dut, firmware):
     # confirm data written
     for val in firmware:
         data = yield from jtag_read_write_reg(dut, WB_READ, 32, val)
-        print ("wb read", val, hex(data))
+        print ("wb read", hex(val), hex(data))
 
     ####### JTAG to DMI Setup (IC-Reset, start) ######
 
diff --git a/src/soc/simple/issuer.py b/src/soc/simple/issuer.py
index 374d3a0d..92f85784 100644
--- a/src/soc/simple/issuer.py
+++ b/src/soc/simple/issuer.py
@@ -56,22 +56,23 @@ def get_insn(f_instr_o, pc):
         return f_instr_o.word_select(pc[2], 32)
 
 # gets state input or reads from state regfile
-def state_get(m, state_i, name, regfile, regnum):
+def state_get(m, core_rst, state_i, name, regfile, regnum):
     comb = m.d.comb
     sync = m.d.sync
     # read the PC
     res = Signal(64, reset_less=True, name=name)
     res_ok_delay = Signal(name="%s_ok_delay" % name)
-    sync += res_ok_delay.eq(~state_i.ok)
-    with m.If(state_i.ok):
-        # incoming override (start from pc_i)
-        comb += res.eq(state_i.data)
-    with m.Else():
-        # otherwise read StateRegs regfile for PC...
-        comb += regfile.ren.eq(1<<regnum)
-    # ... but on a 1-clock delay
-    with m.If(res_ok_delay):
-        comb += res.eq(regfile.data_o)
+    with m.If(~core_rst):
+        sync += res_ok_delay.eq(~state_i.ok)
+        with m.If(state_i.ok):
+            # incoming override (start from pc_i)
+            comb += res.eq(state_i.data)
+        with m.Else():
+            # otherwise read StateRegs regfile for PC...
+            comb += regfile.ren.eq(1<<regnum)
+        # ... but on a 1-clock delay
+        with m.If(res_ok_delay):
+            comb += res.eq(regfile.data_o)
     return res
 
 def get_predint(m, mask, name):
@@ -879,6 +880,10 @@ class TestIssuerInternal(Elaboratable):
         # set up peripherals and core
         core_rst = self.setup_peripherals(m)
 
+        # reset current state if core reset requested
+        with m.If(core_rst):
+            m.d.sync += self.cur_state.eq(0)
+
         # PC and instruction from I-Memory
         comb += self.pc_o.eq(cur_state.pc)
         pc_changed = Signal() # note write to PC
@@ -886,9 +891,11 @@ class TestIssuerInternal(Elaboratable):
 
         # read state either from incoming override or from regfile
         # TODO: really should be doing MSR in the same way
-        pc = state_get(m, self.pc_i, "pc",                  # read PC
+        pc = state_get(m, core_rst, self.pc_i,
+                            "pc",                  # read PC
                             self.state_r_pc, StateRegs.PC)
-        svstate = state_get(m, self.svstate_i, "svstate",   # read SVSTATE
+        svstate = state_get(m, core_rst, self.svstate_i,
+                            "svstate",   # read SVSTATE
                             self.state_r_sv, StateRegs.SVSTATE)
 
         # don't write pc every cycle
@@ -900,7 +907,9 @@ class TestIssuerInternal(Elaboratable):
 
         # address of the next instruction, in the absence of a branch
         # depends on the instruction size
-        nia = Signal(64, reset_less=True)
+        nia = Signal(64)
+        with m.If(core_rst):
+            sync += nia.eq(0)
 
         # connect up debug signals
         # TODO comb += core.icache_rst_i.eq(dbg.icache_rst_o)
-- 
2.30.2