Implement debug hasel support (#287)

* Implement hasel/hawindow support.

This should allow simultaneous resume and halt to work.

* Fix anyrunning/anyhalted bits.

* Add --without-hasel argument for testing.

* Make halt/resume times more equal.

Switching threads after every instruction executed in debug mode leads
to a lot of extra instructions being executed on the "other" thread when
both are really supposed to halt/resume near-simultaneously. Fixed that
by adding wfi to debug_rom.S, and implementing it to switch to the other
hart as well as check for JTAG input.

When resuming, write the hart ID to the debug ROM so that the DM knows
which hart actually resumed. (Before simultaneous resume it just assumed
the current one.)

Also got rid of resume symbol in debug_rom.S since it had no purpose.

* Preserve Debug ROM entry points.

* Make sure minstret is correct when wfi happens.

9 files changed, 157 insertions, 83 deletions

diff --git a/debug_rom/debug_rom.S b/debug_rom/debug_rom.S
index 28c7076..03df533 100755
--- a/debug_rom/debug_rom.S
+++ b/debug_rom/debug_rom.S
@@ -14,6 +14,7 @@
 entry:
        jal zero, _entry
 resume:
+       // Not used.
        jal zero, _resume
 exception:
        jal zero, _exception
@@ -37,7 +38,8 @@ entry_loop:
         csrr s0, CSR_MHARTID
         lbu  s0, DEBUG_ROM_FLAGS(s0) // multiple harts can resume  here
         andi s0, s0, (1 << DEBUG_ROM_FLAG_RESUME)
-        bnez s0, resume
+        bnez s0, _resume
+        wfi
         jal  zero, entry_loop
 
 _exception:
@@ -45,8 +47,9 @@ _exception:
         ebreak
 
 going:
+        csrr s0, CSR_MHARTID
+        sw s0, DEBUG_ROM_GOING(zero)     // When debug module sees this write, the GO flag is reset.
         csrr s0, CSR_DSCRATCH            // Restore s0 here
-        sw zero, DEBUG_ROM_GOING(zero)   // When debug module sees this write, the GO flag is reset.
         fence
         fence.i
         jalr zero, zero, %lo(whereto)    // Debug module will put different instructions and data in the RAM, 
diff --git a/debug_rom/debug_rom.h b/debug_rom/debug_rom.h
index d21e166..3fa018a 100644
--- a/debug_rom/debug_rom.h
+++ b/debug_rom/debug_rom.h
@@ -1,12 +1,13 @@
 static const unsigned char debug_rom_raw[] = {
-  0x6f, 0x00, 0xc0, 0x00, 0x6f, 0x00, 0x40, 0x05, 0x6f, 0x00, 0x40, 0x03,
+  0x6f, 0x00, 0xc0, 0x00, 0x6f, 0x00, 0xc0, 0x05, 0x6f, 0x00, 0x80, 0x03,
   0x0f, 0x00, 0xf0, 0x0f, 0x73, 0x10, 0x24, 0x7b, 0x73, 0x24, 0x40, 0xf1,
   0x23, 0x20, 0x80, 0x10, 0x03, 0x44, 0x04, 0x40, 0x13, 0x74, 0x14, 0x00,
-  0x63, 0x10, 0x04, 0x02, 0x73, 0x24, 0x40, 0xf1, 0x03, 0x44, 0x04, 0x40,
-  0x13, 0x74, 0x24, 0x00, 0xe3, 0x18, 0x04, 0xfc, 0x6f, 0xf0, 0xdf, 0xfd,
-  0x23, 0x26, 0x00, 0x10, 0x73, 0x00, 0x10, 0x00, 0x73, 0x24, 0x20, 0x7b,
-  0x23, 0x22, 0x00, 0x10, 0x0f, 0x00, 0xf0, 0x0f, 0x0f, 0x10, 0x00, 0x00,
-  0x67, 0x00, 0x00, 0x30, 0x73, 0x24, 0x40, 0xf1, 0x23, 0x24, 0x80, 0x10,
-  0x73, 0x24, 0x20, 0x7b, 0x73, 0x00, 0x20, 0x7b
+  0x63, 0x12, 0x04, 0x02, 0x73, 0x24, 0x40, 0xf1, 0x03, 0x44, 0x04, 0x40,
+  0x13, 0x74, 0x24, 0x00, 0x63, 0x16, 0x04, 0x02, 0x73, 0x00, 0x50, 0x10,
+  0x6f, 0xf0, 0x9f, 0xfd, 0x23, 0x26, 0x00, 0x10, 0x73, 0x00, 0x10, 0x00,
+  0x73, 0x24, 0x40, 0xf1, 0x23, 0x22, 0x80, 0x10, 0x73, 0x24, 0x20, 0x7b,
+  0x0f, 0x00, 0xf0, 0x0f, 0x0f, 0x10, 0x00, 0x00, 0x67, 0x00, 0x00, 0x30,
+  0x73, 0x24, 0x40, 0xf1, 0x23, 0x24, 0x80, 0x10, 0x73, 0x24, 0x20, 0x7b,
+  0x73, 0x00, 0x20, 0x7b
 };
-static const unsigned int debug_rom_raw_len = 104;
+static const unsigned int debug_rom_raw_len = 112;
diff --git a/riscv/debug_module.cc b/riscv/debug_module.cc
index f746a72..0c3e927 100644
--- a/riscv/debug_module.cc
+++ b/riscv/debug_module.cc
@@ -32,7 +32,7 @@ static unsigned field_width(unsigned n)
 ///////////////////////// debug_module_t
 
 debug_module_t::debug_module_t(sim_t *sim, unsigned progbufsize, unsigned max_bus_master_bits,
-    bool require_authentication, unsigned abstract_rti) :
+    bool require_authentication, unsigned abstract_rti, bool support_hasel) :
   nprocs(sim->nprocs()),
   progbufsize(progbufsize),
   program_buffer_bytes(4 + 4*progbufsize),
@@ -46,7 +46,9 @@ debug_module_t::debug_module_t(sim_t *sim, unsigned progbufsize, unsigned max_bu
   sim(sim),
   // The spec lets a debugger select nonexistent harts. Create hart_state for
   // them because I'm too lazy to add the code to just ignore accesses.
-  hart_state(1 << field_width(sim->nprocs()))
+  hart_state(1 << field_width(sim->nprocs())),
+  hart_array_mask(sim->nprocs()),
+  support_hasel(support_hasel)
 {
   D(fprintf(stderr, "debug_data_start=0x%x\n", debug_data_start));
   D(fprintf(stderr, "debug_progbuf_start=0x%x\n", debug_progbuf_start));
@@ -223,7 +225,8 @@ bool debug_module_t::store(reg_t addr, size_t len, const uint8_t* bytes)
   }
 
   if (addr == DEBUG_ROM_GOING) {
-    debug_rom_flags[dmcontrol.hartsel] &= ~(1 << DEBUG_ROM_FLAG_GO);
+    assert(len == 4);
+    debug_rom_flags[id] &= ~(1 << DEBUG_ROM_FLAG_GO);
     return true;
   }
 
@@ -266,16 +269,25 @@ uint32_t debug_module_t::read32(uint8_t *memory, unsigned int index)
   return value;
 }
 
-processor_t *debug_module_t::current_proc() const
+processor_t *debug_module_t::processor(unsigned hartid) const
 {
   processor_t *proc = NULL;
   try {
-    proc = sim->get_core(dmcontrol.hartsel);
+    proc = sim->get_core(hartid);
   } catch (const std::out_of_range&) {
   }
   return proc;
 }
 
+bool debug_module_t::hart_selected(unsigned hartid) const
+{
+  if (dmcontrol.hasel) {
+    return hartid == dmcontrol.hartsel || hart_array_mask[hartid];
+  } else {
+    return hartid == dmcontrol.hartsel;
+  }
+}
+
 unsigned debug_module_t::sb_access_bits()
 {
   return 8 << sbcs.sbaccess;
@@ -375,14 +387,11 @@ bool debug_module_t::dmi_read(unsigned address, uint32_t *value)
     switch (address) {
       case DMI_DMCONTROL:
         {
-          processor_t *proc = current_proc();
-          if (proc)
-            dmcontrol.haltreq = proc->halt_request;
-
           result = set_field(result, DMI_DMCONTROL_HALTREQ, dmcontrol.haltreq);
           result = set_field(result, DMI_DMCONTROL_RESUMEREQ, dmcontrol.resumereq);
           result = set_field(result, DMI_DMCONTROL_HARTSELHI,
               dmcontrol.hartsel >> DMI_DMCONTROL_HARTSELLO_LENGTH);
+          result = set_field(result, DMI_DMCONTROL_HASEL, dmcontrol.hasel);
           result = set_field(result, DMI_DMCONTROL_HARTSELLO, dmcontrol.hartsel);
           result = set_field(result, DMI_DMCONTROL_HARTRESET, dmcontrol.hartreset);
 	  result = set_field(result, DMI_DMCONTROL_NDMRESET, dmcontrol.ndmreset);
@@ -391,36 +400,39 @@ bool debug_module_t::dmi_read(unsigned address, uint32_t *value)
         break;
       case DMI_DMSTATUS:
         {
-          processor_t *proc = current_proc();
-
-	  dmstatus.allnonexistant = false;
-	  dmstatus.allunavail = false;
-	  dmstatus.allrunning = false;
-	  dmstatus.allhalted = false;
-          dmstatus.allresumeack = false;
-          if (proc) {
-            if (hart_state[dmcontrol.hartsel].halted) {
-              dmstatus.allhalted = true;
-            } else {
-              dmstatus.allrunning = true;
+	  dmstatus.allhalted = true;
+          dmstatus.anyhalted = false;
+	  dmstatus.allrunning = true;
+          dmstatus.anyrunning = false;
+          dmstatus.allnonexistant = true;
+          dmstatus.allresumeack = true;
+          dmstatus.anyresumeack = false;
+          for (unsigned i = 0; i < nprocs; i++) {
+            if (hart_selected(i)) {
+              dmstatus.allnonexistant = false;
+              if (hart_state[i].resumeack) {
+                dmstatus.anyresumeack = true;
+              } else {
+                dmstatus.allresumeack = false;
+              }
+              if (hart_state[i].halted) {
+                dmstatus.allrunning = false;
+                dmstatus.anyhalted = true;
+              } else {
+                dmstatus.allhalted = false;
+                dmstatus.anyrunning = true;
+              }
             }
-          } else {
-	    dmstatus.allnonexistant = true;
-          }
-	  dmstatus.anynonexistant = dmstatus.allnonexistant;
-	  dmstatus.anyunavail = dmstatus.allunavail;
-	  dmstatus.anyrunning = dmstatus.allrunning;
-	  dmstatus.anyhalted = dmstatus.allhalted;
-          if (proc) {
-            if (hart_state[dmcontrol.hartsel].resumeack) {
-              dmstatus.allresumeack = true;
-            } else {
-              dmstatus.allresumeack = false;
-            }
-          } else {
-            dmstatus.allresumeack = false;
           }
 
+          // We don't allow selecting non-existant harts through
+          // hart_array_mask, so the only way it's possible is by writing a
+          // non-existant hartsel.
+          dmstatus.anynonexistant = (dmcontrol.hartsel >= nprocs);
+
+	  dmstatus.allunavail = false;
+	  dmstatus.anyunavail = false;
+
           result = set_field(result, DMI_DMSTATUS_IMPEBREAK,
               dmstatus.impebreak);
           result = set_field(result, DMI_DMSTATUS_ALLHAVERESET,
@@ -462,6 +474,20 @@ bool debug_module_t::dmi_read(unsigned address, uint32_t *value)
         result = set_field(result, DMI_HARTINFO_DATASIZE, abstractcs.datacount);
         result = set_field(result, DMI_HARTINFO_DATAADDR, debug_data_start);
         break;
+      case DMI_HAWINDOWSEL:
+        result = hawindowsel;
+        break;
+      case DMI_HAWINDOW:
+        {
+          unsigned base = hawindowsel * 32;
+          for (unsigned i = 0; i < 32; i++) {
+            unsigned n = base + i;
+            if (n < nprocs && hart_array_mask[n]) {
+              result |= 1 << i;
+            }
+          }
+        }
+        break;
       case DMI_SBCS:
         result = set_field(result, DMI_SBCS_SBVERSION, sbcs.version);
         result = set_field(result, DMI_SBCS_SBREADONADDR, sbcs.readonaddr);
@@ -727,35 +753,47 @@ bool debug_module_t::dmi_write(unsigned address, uint32_t value)
           if (!dmcontrol.dmactive && get_field(value, DMI_DMCONTROL_DMACTIVE))
             reset();
           dmcontrol.dmactive = get_field(value, DMI_DMCONTROL_DMACTIVE);
-          if (!dmstatus.authenticated)
+          if (!dmstatus.authenticated || !dmcontrol.dmactive)
             return true;
-          if (dmcontrol.dmactive) {
-            dmcontrol.haltreq = get_field(value, DMI_DMCONTROL_HALTREQ);
-            dmcontrol.resumereq = get_field(value, DMI_DMCONTROL_RESUMEREQ);
-            dmcontrol.hartreset = get_field(value, DMI_DMCONTROL_HARTRESET);
-            dmcontrol.ndmreset = get_field(value, DMI_DMCONTROL_NDMRESET);
-            dmcontrol.hartsel = get_field(value, DMI_DMCONTROL_HARTSELHI) <<
-              DMI_DMCONTROL_HARTSELLO_LENGTH;
-            dmcontrol.hartsel |= get_field(value, DMI_DMCONTROL_HARTSELLO);
-            dmcontrol.hartsel &= (1L<<hartsellen) - 1;
-            if (get_field(value, DMI_DMCONTROL_ACKHAVERESET)) {
-              hart_state[dmcontrol.hartsel].havereset = false;
-            }
-          }
-          processor_t *proc = current_proc();
-          if (proc) {
-            proc->halt_request = dmcontrol.haltreq;
-            if (dmcontrol.resumereq) {
-              debug_rom_flags[dmcontrol.hartsel] |= (1 << DEBUG_ROM_FLAG_RESUME);
-              hart_state[dmcontrol.hartsel].resumeack = false;
+
+          dmcontrol.haltreq = get_field(value, DMI_DMCONTROL_HALTREQ);
+          dmcontrol.resumereq = get_field(value, DMI_DMCONTROL_RESUMEREQ);
+          dmcontrol.hartreset = get_field(value, DMI_DMCONTROL_HARTRESET);
+          dmcontrol.ndmreset = get_field(value, DMI_DMCONTROL_NDMRESET);
+          if (support_hasel)
+            dmcontrol.hasel = get_field(value, DMI_DMCONTROL_HASEL);
+          else
+            dmcontrol.hasel = 0;
+          dmcontrol.hartsel = get_field(value, DMI_DMCONTROL_HARTSELHI) <<
+            DMI_DMCONTROL_HARTSELLO_LENGTH;
+          dmcontrol.hartsel |= get_field(value, DMI_DMCONTROL_HARTSELLO);
+          dmcontrol.hartsel &= (1L<<hartsellen) - 1;
+          for (unsigned i = 0; i < nprocs; i++) {
+            if (hart_selected(i)) {
+              if (get_field(value, DMI_DMCONTROL_ACKHAVERESET)) {
+                hart_state[i].havereset = false;
+              }
+              processor_t *proc = processor(i);
+              if (proc) {
+                proc->halt_request = dmcontrol.haltreq;
+                if (dmcontrol.haltreq) {
+                  D(fprintf(stderr, "halt hart %d\n", i));
+                }
+                if (dmcontrol.resumereq) {
+                  D(fprintf(stderr, "resume hart %d\n", i));
+                  debug_rom_flags[i] |= (1 << DEBUG_ROM_FLAG_RESUME);
+                  hart_state[i].resumeack = false;
+                }
+                if (dmcontrol.hartreset) {
+                  proc->reset();
+                }
+              }
             }
-	    if (dmcontrol.hartreset) {
-	      proc->reset();
-	    }
           }
+
           if (dmcontrol.ndmreset) {
             for (size_t i = 0; i < sim->nprocs(); i++) {
-              proc = sim->get_core(i);
+              processor_t *proc = sim->get_core(i);
               proc->reset();
             }
           }
@@ -766,6 +804,22 @@ bool debug_module_t::dmi_write(unsigned address, uint32_t value)
         command = value;
         return perform_abstract_command();
 
+      case DMI_HAWINDOWSEL:
+        hawindowsel = value & ((1U<<field_width(nprocs))-1);
+        return true;
+
+      case DMI_HAWINDOW:
+        {
+          unsigned base = hawindowsel * 32;
+          for (unsigned i = 0; i < 32; i++) {
+            unsigned n = base + i;
+            if (n < nprocs) {
+              hart_array_mask[n] = (value >> i) & 1;
+            }
+          }
+        }
+        return true;
+
       case DMI_ABSTRACTCS:
         abstractcs.cmderr = (cmderr_t) (((uint32_t) (abstractcs.cmderr)) & (~(uint32_t)(get_field(value, DMI_ABSTRACTCS_CMDERR))));
         return true;
diff --git a/riscv/debug_module.h b/riscv/debug_module.h
index 5f9ba85..5bd9e66 100644
--- a/riscv/debug_module.h
+++ b/riscv/debug_module.h
@@ -11,6 +11,7 @@ class sim_t;
 typedef struct {
   bool haltreq;
   bool resumereq;
+  bool hasel;
   unsigned hartsel;
   bool hartreset;
   bool dmactive;
@@ -94,7 +95,7 @@ class debug_module_t : public abstract_device_t
      */
     debug_module_t(sim_t *sim, unsigned progbufsize,
         unsigned max_bus_master_bits, bool require_authentication,
-        unsigned abstract_rti);
+        unsigned abstract_rti, bool support_hasel);
     ~debug_module_t();
 
     void add_device(bus_t *bus);
@@ -162,6 +163,8 @@ class debug_module_t : public abstract_device_t
     abstractcs_t abstractcs;
     abstractauto_t abstractauto;
     uint32_t command;
+    uint16_t hawindowsel;
+    std::vector<bool> hart_array_mask;
 
     sbcs_t sbcs;
     uint32_t sbaddress[4];
@@ -170,12 +173,14 @@ class debug_module_t : public abstract_device_t
     uint32_t challenge;
     const uint32_t secret = 1;
 
-    processor_t *current_proc() const;
+    processor_t *processor(unsigned hartid) const;
+    bool hart_selected(unsigned hartid) const;
     void reset();
     bool perform_abstract_command();
 
     bool abstract_command_completed;
     unsigned rti_remaining;
+    bool support_hasel;
 };
 
 #endif
diff --git a/riscv/decode.h b/riscv/decode.h
index f9e3b6f..3fa78c1 100644
--- a/riscv/decode.h
+++ b/riscv/decode.h
@@ -212,9 +212,12 @@ private:
        STATE.pc = __npc; \
      } while(0)
 
+class wait_for_interrupt_t {};
+
 #define wfi() \
   do { set_pc_and_serialize(npc); \
        npc = PC_SERIALIZE_WFI; \
+       throw wait_for_interrupt_t(); \
      } while(0)
 
 #define serialize() set_pc_and_serialize(npc)
diff --git a/riscv/execute.cc b/riscv/execute.cc
index e639e90..86e6e3e 100644
--- a/riscv/execute.cc
+++ b/riscv/execute.cc
@@ -147,13 +147,6 @@ void processor_t::step(size_t n)
           pc = execute_insn(this, pc, fetch);
 
           advance_pc();
-
-          if (unlikely(state.pc >= DEBUG_ROM_ENTRY &&
-                       state.pc < DEBUG_END)) {
-            // We're waiting for the debugger to tell us something.
-            return;
-          }
-
         }
       }
       else while (instret < n)
@@ -244,6 +237,16 @@ void processor_t::step(size_t n)
           abort();
       }
     }
+    catch (wait_for_interrupt_t &t)
+    {
+      // Return to the outer simulation loop, which gives other devices/harts a
+      // chance to generate interrupts.
+      //
+      // In the debug ROM this prevents us from wasting time looping, but also
+      // allows us to switch to other threads only once per idle loop in case
+      // there is activity.
+      n = instret;
+    }
 
     state.minstret += instret;
     n -= instret;
diff --git a/riscv/sim.cc b/riscv/sim.cc
index ddc9bce..db68930 100644
--- a/riscv/sim.cc
+++ b/riscv/sim.cc
@@ -29,12 +29,12 @@ sim_t::sim_t(const char* isa, size_t nprocs, bool halted, reg_t start_pc,
              const std::vector<std::string>& args,
              std::vector<int> const hartids, unsigned progsize,
              unsigned max_bus_master_bits, bool require_authentication,
-             suseconds_t abstract_delay_usec)
+             suseconds_t abstract_delay_usec, bool support_hasel)
   : htif_t(args), mems(mems), procs(std::max(nprocs, size_t(1))),
     start_pc(start_pc), current_step(0), current_proc(0), debug(false),
     histogram_enabled(false), dtb_enabled(true), remote_bitbang(NULL),
     debug_module(this, progsize, max_bus_master_bits, require_authentication,
-        abstract_delay_usec)
+        abstract_delay_usec, support_hasel)
 {
   signal(SIGINT, &handle_signal);
 
diff --git a/riscv/sim.h b/riscv/sim.h
index 1bb28cb..ef3e780 100644
--- a/riscv/sim.h
+++ b/riscv/sim.h
@@ -25,7 +25,8 @@ public:
         std::vector<std::pair<reg_t, mem_t*>> mems,
         const std::vector<std::string>& args, const std::vector<int> hartids,
         unsigned progsize, unsigned max_bus_master_bits,
-        bool require_authentication, suseconds_t abstract_delay_usec);
+        bool require_authentication, suseconds_t abstract_delay_usec,
+        bool support_hasel);
   ~sim_t();
 
   // run the simulation to completion
diff --git a/spike_main/spike.cc b/spike_main/spike.cc
index 6d8d88a..2bf8d28 100644
--- a/spike_main/spike.cc
+++ b/spike_main/spike.cc
@@ -48,6 +48,7 @@ static void help(int exit_code = 1)
       "required for a DMI access [default 0]\n");
   fprintf(stderr, "  --abstract-rti=<n>    Number of Run-Test/Idle cycles "
       "required for an abstract command to execute [default 0]\n");
+  fprintf(stderr, "  --without-hasel       Debug module supports hasel\n");
 
   exit(exit_code);
 }
@@ -113,6 +114,7 @@ int main(int argc, char** argv)
   bool require_authentication = false;
   unsigned dmi_rti = 0;
   unsigned abstract_rti = 0;
+  bool support_hasel = true;
   std::vector<int> hartids;
 
   auto const hartids_parser = [&](const char *s) {
@@ -164,6 +166,8 @@ int main(int argc, char** argv)
       [&](const char* s){dmi_rti = atoi(s);});
   parser.option(0, "abstract-rti", 1,
       [&](const char* s){abstract_rti = atoi(s);});
+  parser.option(0, "without-hasel", 0,
+      [&](const char* s){support_hasel = false;});
 
   auto argv1 = parser.parse(argv);
   std::vector<std::string> htif_args(argv1, (const char*const*)argv + argc);
@@ -175,7 +179,7 @@ int main(int argc, char** argv)
 
   sim_t s(isa, nprocs, halted, start_pc, mems, htif_args, std::move(hartids),
       progsize, max_bus_master_bits, require_authentication,
-      abstract_rti);
+      abstract_rti, support_hasel);
   std::unique_ptr<remote_bitbang_t> remote_bitbang((remote_bitbang_t *) NULL);
   std::unique_ptr<jtag_dtm_t> jtag_dtm(
       new jtag_dtm_t(&s.debug_module, dmi_rti));