From ea6740350fcdd5fe63f299d931b6d2b9ff7797ef Mon Sep 17 00:00:00 2001
From: Tim Newsome <tim@sifive.com>
Date: Mon, 26 Jun 2023 17:33:18 -0700
Subject: Let debugger control hart availability

This change lets me test OpenOCD's behavior when harts become available.
It only affects how things look to the debugger. Harts that are
"unavailable" still execute code as usual.

Control is implemented through the 2 LSBs of the DMCUSTOM register in
the Debug Module.
---
 riscv/debug_module.cc | 45 +++++++++++++++++++++++++++++++++++++--------
 riscv/debug_module.h  |  6 ++++++
 2 files changed, 43 insertions(+), 8 deletions(-)

diff --git a/riscv/debug_module.cc b/riscv/debug_module.cc
index f7163ef..4a7a802 100644
--- a/riscv/debug_module.cc
+++ b/riscv/debug_module.cc
@@ -73,6 +73,9 @@ debug_module_t::debug_module_t(simif_t *sim, const debug_module_config_t &config
           jal(ZERO, debug_abstract_start - DEBUG_ROM_WHERETO));
 
   memset(debug_abstract, 0, sizeof(debug_abstract));
+  for (unsigned i = 0; i < sizeof(hart_available_state) / sizeof(*hart_available_state); i++) {
+    hart_available_state[i] = true;
+  }
 
   reset();
 }
@@ -202,7 +205,8 @@ bool debug_module_t::store(reg_t addr, size_t len, const uint8_t* bytes)
       if (hart_state[id].haltgroup) {
         for (const auto& [hart_id, hart] : sim->get_harts()) {
           if (!hart_state[hart_id].halted &&
-              hart_state[hart_id].haltgroup == hart_state[id].haltgroup) {
+              hart_state[hart_id].haltgroup == hart_state[id].haltgroup &&
+              hart_available(hart_id)) {
             hart->halt_request = hart->HR_GROUP;
             // TODO: What if the debugger comes and writes dmcontrol before the
             // halt occurs?
@@ -337,6 +341,13 @@ void debug_module_t::sb_write()
   }
 }
 
+bool debug_module_t::hart_available(unsigned hart_id) const
+{
+  if (hart_id < sizeof(hart_available_state) / sizeof(*hart_available_state))
+    return hart_available_state[hart_id];
+  return true;
+}
+
 bool debug_module_t::dmi_read(unsigned address, uint32_t *value)
 {
   uint32_t result = 0;
@@ -391,6 +402,8 @@ bool debug_module_t::dmi_read(unsigned address, uint32_t *value)
           dmstatus.allnonexistant = true;
           dmstatus.allresumeack = true;
           dmstatus.anyresumeack = false;
+          dmstatus.allunavail = true;
+          dmstatus.anyunavail = false;
           for (const auto& [hart_id, hart] : sim->get_harts()) {
             if (hart_selected(hart_id)) {
               dmstatus.allnonexistant = false;
@@ -399,12 +412,19 @@ bool debug_module_t::dmi_read(unsigned address, uint32_t *value)
               } else {
                 dmstatus.allresumeack = false;
               }
+              auto hart = sim->get_harts().at(hart_id);
               if (hart_state[hart_id].halted) {
                 dmstatus.allrunning = false;
                 dmstatus.anyhalted = true;
+                dmstatus.allunavail = false;
+              } else if (!hart_available(hart_id)) {
+                dmstatus.allrunning = false;
+                dmstatus.allhalted = false;
+                dmstatus.anyunavail = true;
               } else {
                 dmstatus.allhalted = false;
                 dmstatus.anyrunning = true;
+                dmstatus.allunavail = false;
               }
             }
           }
@@ -414,9 +434,6 @@ bool debug_module_t::dmi_read(unsigned address, uint32_t *value)
           // non-existant hartsel.
           dmstatus.anynonexistant = dmcontrol.hartsel >= sim->get_cfg().nprocs();
 
-          dmstatus.allunavail = false;
-          dmstatus.anyunavail = false;
-
           result = set_field(result, DM_DMSTATUS_IMPEBREAK,
               dmstatus.impebreak);
           result = set_field(result, DM_DMSTATUS_ALLHAVERESET, selected_hart_state().havereset);
@@ -522,6 +539,11 @@ bool debug_module_t::dmi_read(unsigned address, uint32_t *value)
       case DM_DMCS2:
         result = set_field(result, DM_DMCS2_GROUP, selected_hart_state().haltgroup);
         break;
+      case DM_CUSTOM:
+        for (unsigned i = 0; i < sizeof(hart_available_state) / sizeof(*hart_available_state); i++) {
+          result |= hart_available_state[i] << i;
+        }
+        break;
       default:
         result = 0;
         D(fprintf(stderr, "Unexpected. Returning Error."));
@@ -790,16 +812,18 @@ bool debug_module_t::dmi_write(unsigned address, uint32_t value)
               if (get_field(value, DM_DMCONTROL_ACKHAVERESET)) {
                 hart_state[hart_id].havereset = false;
               }
-              hart->halt_request = dmcontrol.haltreq ? hart->HR_REGULAR : hart->HR_NONE;
-              if (dmcontrol.haltreq) {
+              if (dmcontrol.haltreq && hart_available(hart_id)) {
+                hart->halt_request = hart->HR_REGULAR;
                 D(fprintf(stderr, "halt hart %d\n", hart_id));
+              } else {
+                hart->halt_request = hart->HR_NONE;
               }
-              if (dmcontrol.resumereq) {
+              if (dmcontrol.resumereq && hart_available(hart_id)) {
                 D(fprintf(stderr, "resume hart %d\n", hart_id));
                 debug_rom_flags[hart_id] |= (1 << DEBUG_ROM_FLAG_RESUME);
                 hart_state[hart_id].resumeack = false;
               }
-              if (dmcontrol.hartreset) {
+              if (dmcontrol.hartreset && hart_available(hart_id)) {
                 hart->reset();
               }
             }
@@ -903,6 +927,11 @@ bool debug_module_t::dmi_write(unsigned address, uint32_t value)
           selected_hart_state().haltgroup = get_field(value, DM_DMCS2_GROUP);
         }
         return true;
+      case DM_CUSTOM:
+        for (unsigned i = 0; i < sizeof(hart_available_state) / sizeof(*hart_available_state); i++) {
+          hart_available_state[i] = get_field(value, 1<<i);
+        }
+        return true;
     }
   }
   return false;
diff --git a/riscv/debug_module.h b/riscv/debug_module.h
index 518f119..73bb7fa 100644
--- a/riscv/debug_module.h
+++ b/riscv/debug_module.h
@@ -185,6 +185,12 @@ class debug_module_t : public abstract_device_t
 
     size_t selected_hart_id() const;
     hart_debug_state_t& selected_hart_state();
+
+    /* Whether the first 2 harts are available is controllable through DMCUSTOM,
+     * where bit 0 corresponds to hart 0, etc. When a bit is one the hart
+     * available.  Otherwise it is unavailable. */
+    bool hart_available_state[2];
+    bool hart_available(unsigned hart_id) const;
 };
 
 #endif
-- 
cgit v1.1