From d3da497c747a1d37aa1b2b495287bb3921aaf9d4 Mon Sep 17 00:00:00 2001
From: Alexey Lapshin <alexey.lapshin@espressif.com>
Date: Wed, 19 Apr 2023 20:18:11 +0800
Subject: [PATCH] riscv: fix & refactor triggers add/delete

---
 components/riscv/include/riscv/csr.h      |  1 +
 components/riscv/include/riscv/rv_utils.h | 83 ++++++++++++++---------
 2 files changed, 51 insertions(+), 33 deletions(-)

diff --git a/components/riscv/include/riscv/csr.h b/components/riscv/include/riscv/csr.h
index 82b0ac033d..a26a169748 100644
--- a/components/riscv/include/riscv/csr.h
+++ b/components/riscv/include/riscv/csr.h
@@ -106,6 +106,7 @@ extern "C" {
 #define TDATA1_MATCH     (1<<7)
 #define TDATA1_MATCH_V   (0xF)   /*R/W,Address match type :0 : Exact byte match  1 : NAPOT range match */
 #define TDATA1_MATCH_S   (7)
+#define TDATA1_HIT_S     (20)
 
 
 /* RISC-V CSR macros
diff --git a/components/riscv/include/riscv/rv_utils.h b/components/riscv/include/riscv/rv_utils.h
index e7349ef082..b44fb15374 100644
--- a/components/riscv/include/riscv/rv_utils.h
+++ b/components/riscv/include/riscv/rv_utils.h
@@ -132,49 +132,66 @@ FORCE_INLINE_ATTR void rv_utils_set_breakpoint(int bp_num, uint32_t bp_addr)
     /* The code bellow sets breakpoint which will trigger `Breakpoint` exception
      * instead transfering control to debugger. */
     RV_WRITE_CSR(tselect, bp_num);
-    RV_SET_CSR(CSR_TCONTROL, TCONTROL_MTE);
-    RV_SET_CSR(CSR_TDATA1, TDATA1_USER | TDATA1_MACHINE | TDATA1_EXECUTE);
+    RV_WRITE_CSR(CSR_TCONTROL, TCONTROL_MTE);
+    RV_WRITE_CSR(CSR_TDATA1, TDATA1_USER | TDATA1_MACHINE | TDATA1_EXECUTE);
     RV_WRITE_CSR(tdata2, bp_addr);
 }
 
+FORCE_INLINE_ATTR void rv_utils_set_watchpoint(int wp_num,
+                                               uint32_t wp_addr,
+                                               size_t size,
+                                               bool on_read,
+                                               bool on_write)
+{
+    RV_WRITE_CSR(tselect, wp_num);
+    RV_WRITE_CSR(CSR_TCONTROL, TCONTROL_MPTE | TCONTROL_MTE);
+    RV_WRITE_CSR(CSR_TDATA1, TDATA1_USER               |
+                         TDATA1_MACHINE                |
+                         TDATA1_MATCH                  |
+                         (on_read  ? TDATA1_LOAD  : 0) |
+                         (on_write ? TDATA1_STORE : 0));
+    /* From RISC-V Debug Specification:
+     * NAPOT (Naturally Aligned Power-Of-Two):
+     * Matches when the top M bits of any compare value match the top M bits of tdata2.
+     * M is XLEN − 1 minus the index of the least-significant bit containing 0 in tdata2.
+     *
+     * Note: Expectng that size is number power of 2
+     *
+     * Examples for understanding how to calculate NAPOT:
+     *
+     * nnnn...nnnn0 2-byte  NAPOT range
+     * nnnn...nnn01 4-byte  NAPOT range
+     * nnnn...nn011 8-byte  NAPOT range
+     * nnnn...n0111 16-byte NAPOT range
+     * nnnn...01111 32-byte NAPOT range
+     *  * where n are bits from original address
+     */
+    const uint32_t half_size = size >> 1;
+    uint32_t napot = wp_addr;
+    napot &= ~half_size;      /* set the least-significant bit with zero */
+    napot |= half_size - 1;   /* fill all bits with ones after least-significant bit */
+    RV_WRITE_CSR(tdata2, napot);
+}
+
 FORCE_INLINE_ATTR void rv_utils_clear_breakpoint(int bp_num)
 {
     RV_WRITE_CSR(tselect, bp_num);
-    RV_CLEAR_CSR(CSR_TCONTROL, TCONTROL_MTE);
-    RV_CLEAR_CSR(CSR_TDATA1, TDATA1_USER | TDATA1_MACHINE | TDATA1_EXECUTE);
-}
-
-FORCE_INLINE_ATTR void rv_utils_set_watchpoint(int wp_num,
-                                           uint32_t wp_addr,
-                                           size_t size,
-                                           bool on_read,
-                                           bool on_write)
-{
-    RV_WRITE_CSR(tselect, wp_num);
-    RV_SET_CSR(CSR_TCONTROL, TCONTROL_MPTE | TCONTROL_MTE);
-    RV_SET_CSR(CSR_TDATA1, TDATA1_USER | TDATA1_MACHINE);
-    RV_SET_CSR_FIELD(CSR_TDATA1, (long unsigned int) TDATA1_MATCH, 1);
-
-    // add 0 in napot encoding
-    uint32_t addr_napot;
-    addr_napot = ((uint32_t) wp_addr) | ((size >> 1) - 1);
-    if (on_read) {
-        RV_SET_CSR(CSR_TDATA1, TDATA1_LOAD);
-    }
-    if (on_write) {
-        RV_SET_CSR(CSR_TDATA1, TDATA1_STORE);
-    }
-    RV_WRITE_CSR(tdata2, addr_napot);
+    /* tdata1 is a WARL(write any read legal) register
+     * We can just write 0 to it
+     */
+    RV_WRITE_CSR(CSR_TDATA1, 0);
 }
 
 FORCE_INLINE_ATTR void rv_utils_clear_watchpoint(int wp_num)
 {
-    RV_WRITE_CSR(tselect, wp_num);
-    RV_CLEAR_CSR(CSR_TCONTROL, TCONTROL_MTE);
-    RV_CLEAR_CSR(CSR_TDATA1, TDATA1_USER | TDATA1_MACHINE);
-    RV_CLEAR_CSR_FIELD(CSR_TDATA1, (long unsigned int) TDATA1_MATCH);
-    RV_CLEAR_CSR(CSR_TDATA1, TDATA1_MACHINE);
-    RV_CLEAR_CSR(CSR_TDATA1, TDATA1_LOAD | TDATA1_STORE | TDATA1_EXECUTE);
+    /* riscv have the same registers for breakpoints and watchpoints */
+    rv_utils_clear_breakpoint(wp_num);
+}
+
+FORCE_INLINE_ATTR bool rv_utils_is_trigger_fired(int id)
+{
+    RV_WRITE_CSR(tselect, id);
+    return (RV_READ_CSR(tdata1) >> TDATA1_HIT_S) & 1;
 }
 
 // ---------------------- Debugger -------------------------