From 650c15caf9a482154fe9dbf150b54b49e9849c63 Mon Sep 17 00:00:00 2001
From: Tim Newsome <tim@sifive.com>
Date: Mon, 27 Nov 2023 14:24:45 -0800
Subject: Add SBA write delay.

This is helpful to test OpenOCD behavior when sbbusyerror is set.
---
 riscv/debug_module.cc | 31 +++++++++++++++++++++++++------
 riscv/debug_module.h  |  5 +++--
 2 files changed, 28 insertions(+), 8 deletions(-)

diff --git a/riscv/debug_module.cc b/riscv/debug_module.cc
index 2df7686..07723c5 100644
--- a/riscv/debug_module.cc
+++ b/riscv/debug_module.cc
@@ -44,7 +44,7 @@ debug_module_t::debug_module_t(simif_t *sim, const debug_module_config_t &config
   hart_state(1 << field_width(sim->get_cfg().max_hartid() + 1)),
   hart_array_mask(sim->get_cfg().max_hartid() + 1),
   rti_remaining(0),
-  sb_read_wait(0)
+  sb_read_wait(0), sb_write_wait(0)
 {
   D(fprintf(stderr, "debug_data_start=0x%x\n", debug_data_start));
   D(fprintf(stderr, "debug_progbuf_start=0x%x\n", debug_progbuf_start));
@@ -300,7 +300,7 @@ void debug_module_t::sb_autoincrement()
 
 bool debug_module_t::sb_busy() const
 {
-  return sb_read_wait > 0;
+  return sb_read_wait > 0 || sb_write_wait > 0;
 }
 
 void debug_module_t::sb_read_start()
@@ -339,6 +339,19 @@ void debug_module_t::sb_read()
   }
 }
 
+void debug_module_t::sb_write_start()
+{
+  if (sb_busy() || sbcs.sbbusyerror) {
+    if (!sbcs.sbbusyerror)
+      D(fprintf(stderr, "Set sbbusyerror because write start while busy\n"));
+    sbcs.sbbusyerror = true;
+    return;
+  }
+  /* Insert artificial delay, so debuggers can test how they handle that
+   * sbbusyerror being set. */
+  sb_write_wait = 20;
+}
+
 void debug_module_t::sb_write()
 {
   reg_t address = ((uint64_t) sbaddress[1] << 32) | sbaddress[0];
@@ -602,6 +615,15 @@ void debug_module_t::run_test_idle()
       }
     }
   }
+  if (sb_write_wait > 0) {
+    sb_write_wait--;
+    if (sb_write_wait == 0) {
+      sb_write();
+      if (sbcs.error == 0) {
+        sb_autoincrement();
+      }
+    }
+  }
 }
 
 static bool is_fpu_reg(unsigned regno)
@@ -950,10 +972,7 @@ bool debug_module_t::dmi_write(unsigned address, uint32_t value)
             case DM_SBDATA0:
               sbdata[0] = value;
               if (sbcs.error == 0) {
-                sb_write();
-                if (sbcs.error == 0) {
-                  sb_autoincrement();
-                }
+                sb_write_start();
               }
               return true;
             case DM_SBDATA1:
diff --git a/riscv/debug_module.h b/riscv/debug_module.h
index cf8781d..75109f6 100644
--- a/riscv/debug_module.h
+++ b/riscv/debug_module.h
@@ -159,9 +159,10 @@ class debug_module_t : public abstract_device_t
 
     void sb_autoincrement();
 
-    /* Start a system bus read. (It could be instantaneous, but to help test
+    /* Start a system bus access. (It could be instantaneous, but to help test
      * OpenOCD a delay can be added.) */
     void sb_read_start();
+    void sb_write_start();
 
     /* Actually read/write. */
     void sb_read();
@@ -203,7 +204,7 @@ class debug_module_t : public abstract_device_t
     bool hart_available_state[2];
     bool hart_available(unsigned hart_id) const;
 
-    unsigned sb_read_wait;
+    unsigned sb_read_wait, sb_write_wait;
 };
 
 #endif
-- 
cgit v1.1