riscv: Use semihosting to set breakpoint and watchpoint when running under debugger

components/app_trace/port/riscv/port.c

@@ -27,8 +27,6 @@ typedef struct {
     esp_apptrace_mem_block_t *  mem_blocks;
 } esp_apptrace_riscv_ctrl_block_t;
 
-#define RISCV_APPTRACE_SYSNR    0x64
-
 #define ESP_APPTRACE_RISCV_BLOCK_LEN_MSK         0x7FFFUL
 #define ESP_APPTRACE_RISCV_BLOCK_LEN(_l_)        ((_l_) & ESP_APPTRACE_RISCV_BLOCK_LEN_MSK)
 #define ESP_APPTRACE_RISCV_BLOCK_LEN_GET(_v_)    ((_v_) & ESP_APPTRACE_RISCV_BLOCK_LEN_MSK)
@@ -104,7 +102,7 @@ __attribute__((weak)) int esp_apptrace_advertise_ctrl_block(void *ctrl_block_add
     if (!esp_cpu_in_ocd_debug_mode()) {
         return 0;
     }
-    return (int) semihosting_call_noerrno(RISCV_APPTRACE_SYSNR, (long*)ctrl_block_addr);
+    return (int) semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_APPTRACE_INIT, (long*)ctrl_block_addr);
 }
 
 /* Returns up buffers config.

components/esp_system/port/arch/riscv/debug_stubs.c

@@ -14,7 +14,6 @@
 
 const static char *TAG = "esp_dbg_stubs";
 
-#define RISCV_DBG_STUBS_SYSNR    0x65
 
 /* Advertises apptrace control block address to host */
 static int esp_dbg_stubs_advertise_table(void *stub_table_addr)
@@ -22,7 +21,7 @@ static int esp_dbg_stubs_advertise_table(void *stub_table_addr)
     if (!esp_cpu_in_ocd_debug_mode()) {
         return 0;
     }
-    return (int) semihosting_call_noerrno(RISCV_DBG_STUBS_SYSNR, (long*)stub_table_addr);
+    return (int) semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_DBG_STUBS_INIT, (long*)stub_table_addr);
 }
 
 void esp_dbg_stubs_ll_init(void *stub_table_addr)

components/freertos/FreeRTOS-Kernel/portable/riscv/port.c

@@ -134,6 +134,9 @@ __attribute__((naked)) static void prvTaskExitError(void)
                 ".option norvc\n" \
                 "nop\n" \
                 ".option pop");
+    /* Task entry's RA will point here. Shifting RA into prvTaskExitError is necessary
+       to make GDB backtrace ending inside that function.
+       Otherwise backtrace will end in the function laying just before prvTaskExitError in address space. */
     _prvTaskExitError();
 }
 
@@ -201,7 +204,7 @@ StackType_t *pxPortInitialiseStack(StackType_t *pxTopOfStack, TaskFunction_t pxC
     memset(frame, 0, sizeof(*frame));
     /* Shifting RA into prvTaskExitError is necessary to make GDB backtrace ending inside that function.
        Otherwise backtrace will end in the function laying just before prvTaskExitError in address space. */
-    frame->ra = (UBaseType_t)prvTaskExitError + 4/*nop size*/;
+    frame->ra = (UBaseType_t)prvTaskExitError + 4/*size of the nop insruction at the beginning of prvTaskExitError*/;
     frame->mepc = (UBaseType_t)pxCode;
     frame->a0 = (UBaseType_t)pvParameters;
     frame->gp = (UBaseType_t)&__global_pointer$;

components/hal/esp32c2/include/hal/cpu_ll.h

@@ -14,6 +14,7 @@
 #include "soc/assist_debug_reg.h"
 #include "esp_attr.h"
 #include "riscv/csr.h"
+#include "riscv/semihosting.h"
 
 /*performance counter*/
 #define CSR_PCER_MACHINE    0x7e0
@@ -71,8 +72,29 @@ static inline void cpu_ll_init_hwloop(void)
     // Nothing needed here for ESP32-C3
 }
 
+FORCE_INLINE_ATTR bool cpu_ll_is_debugger_attached(void)
+{
+    return REG_GET_BIT(ASSIST_DEBUG_CORE_0_DEBUG_MODE_REG, ASSIST_DEBUG_CORE_0_DEBUG_MODULE_ACTIVE);
+}
+
 static inline void cpu_ll_set_breakpoint(int id, uint32_t pc)
 {
+    if (cpu_ll_is_debugger_attached()) {
+        /* If we want to set breakpoint which when hit transfers control to debugger
+         * we need to set `action` in `mcontrol` to 1 (Enter Debug Mode).
+         * That `action` value is supported only when `dmode` of `tdata1` is set.
+         * But `dmode` can be modified by debugger only (from Debug Mode).
+         *
+         * So when debugger is connected we use special syscall to ask it to set breakpoint for us.
+         */
+        long args[] = {true, id, (long)pc};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_BREAKPOINT_SET, args);
+        if (ret == 0) {
+            return;
+        }
+    }
+    /* The code bellow sets breakpoint which will trigger `Breakpoint` exception
+     * instead transfering control to debugger. */
     RV_WRITE_CSR(tselect,id);
     RV_SET_CSR(CSR_TCONTROL,TCONTROL_MTE);
     RV_SET_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE|TDATA1_EXECUTE);
@@ -82,6 +104,14 @@ static inline void cpu_ll_set_breakpoint(int id, uint32_t pc)
 
 static inline void cpu_ll_clear_breakpoint(int id)
 {
+    if (cpu_ll_is_debugger_attached()) {
+        /* see description in cpu_ll_set_breakpoint()  */
+        long args[] = {false, id};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_BREAKPOINT_SET, args);
+        if (ret == 0){
+            return;
+        }
+    }
     RV_WRITE_CSR(tselect,id);
     RV_CLEAR_CSR(CSR_TCONTROL,TCONTROL_MTE);
     RV_CLEAR_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE|TDATA1_EXECUTE);
@@ -105,6 +135,17 @@ static inline void cpu_ll_set_watchpoint(int id,
                                         bool on_write)
 {
     uint32_t addr_napot;
+
+    if (cpu_ll_is_debugger_attached()) {
+        /* see description in cpu_ll_set_breakpoint()  */
+        long args[] = {true, id, (long)addr, (long)size,
+            (long)((on_read ? ESP_SEMIHOSTING_WP_FLG_RD : 0) | (on_write ? ESP_SEMIHOSTING_WP_FLG_WR : 0))};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_WATCHPOINT_SET, args);
+        if (ret == 0) {
+            return;
+        }
+    }
+
     RV_WRITE_CSR(tselect,id);
     RV_SET_CSR(CSR_TCONTROL, TCONTROL_MPTE | TCONTROL_MTE);
     RV_SET_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE);
@@ -123,6 +164,14 @@ static inline void cpu_ll_set_watchpoint(int id,
 
 static inline void cpu_ll_clear_watchpoint(int id)
 {
+    if (cpu_ll_is_debugger_attached()) {
+        /* see description in cpu_ll_set_breakpoint()  */
+        long args[] = {false, id};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_WATCHPOINT_SET, args);
+        if (ret == 0){
+            return;
+        }
+    }
     RV_WRITE_CSR(tselect,id);
     RV_CLEAR_CSR(CSR_TCONTROL,TCONTROL_MTE);
     RV_CLEAR_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE);
@@ -132,11 +181,6 @@ static inline void cpu_ll_clear_watchpoint(int id)
     return;
 }
 
-FORCE_INLINE_ATTR bool cpu_ll_is_debugger_attached(void)
-{
-    return REG_GET_BIT(ASSIST_DEBUG_CORE_0_DEBUG_MODE_REG, ASSIST_DEBUG_CORE_0_DEBUG_MODULE_ACTIVE);
-}
-
 static inline void cpu_ll_break(void)
 {
     asm volatile("ebreak\n");

components/hal/esp32c3/include/hal/cpu_ll.h

@@ -14,6 +14,7 @@
 #include "soc/assist_debug_reg.h"
 #include "esp_attr.h"
 #include "riscv/csr.h"
+#include "riscv/semihosting.h"
 
 /*performance counter*/
 #define CSR_PCER_MACHINE    0x7e0
@@ -71,8 +72,29 @@ static inline void cpu_ll_init_hwloop(void)
     // Nothing needed here for ESP32-C3
 }
 
+FORCE_INLINE_ATTR bool cpu_ll_is_debugger_attached(void)
+{
+    return REG_GET_BIT(ASSIST_DEBUG_CORE_0_DEBUG_MODE_REG, ASSIST_DEBUG_CORE_0_DEBUG_MODULE_ACTIVE);
+}
+
 static inline void cpu_ll_set_breakpoint(int id, uint32_t pc)
 {
+    if (cpu_ll_is_debugger_attached()) {
+        /* If we want to set breakpoint which when hit transfers control to debugger
+         * we need to set `action` in `mcontrol` to 1 (Enter Debug Mode).
+         * That `action` value is supported only when `dmode` of `tdata1` is set.
+         * But `dmode` can be modified by debugger only (from Debug Mode).
+         *
+         * So when debugger is connected we use special syscall to ask it to set breakpoint for us.
+         */
+        long args[] = {true, id, (long)pc};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_BREAKPOINT_SET, args);
+        if (ret == 0) {
+            return;
+        }
+    }
+    /* The code bellow sets breakpoint which will trigger `Breakpoint` exception
+     * instead transfering control to debugger. */
     RV_WRITE_CSR(tselect,id);
     RV_SET_CSR(CSR_TCONTROL,TCONTROL_MTE);
     RV_SET_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE|TDATA1_EXECUTE);
@@ -82,6 +104,14 @@ static inline void cpu_ll_set_breakpoint(int id, uint32_t pc)
 
 static inline void cpu_ll_clear_breakpoint(int id)
 {
+    if (cpu_ll_is_debugger_attached()) {
+        /* see description in cpu_ll_set_breakpoint()  */
+        long args[] = {false, id};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_BREAKPOINT_SET, args);
+        if (ret == 0){
+            return;
+        }
+    }
     RV_WRITE_CSR(tselect,id);
     RV_CLEAR_CSR(CSR_TCONTROL,TCONTROL_MTE);
     RV_CLEAR_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE|TDATA1_EXECUTE);
@@ -105,6 +135,17 @@ static inline void cpu_ll_set_watchpoint(int id,
                                         bool on_write)
 {
     uint32_t addr_napot;
+
+    if (cpu_ll_is_debugger_attached()) {
+        /* see description in cpu_ll_set_breakpoint()  */
+        long args[] = {true, id, (long)addr, (long)size,
+            (long)((on_read ? ESP_SEMIHOSTING_WP_FLG_RD : 0) | (on_write ? ESP_SEMIHOSTING_WP_FLG_WR : 0))};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_WATCHPOINT_SET, args);
+        if (ret == 0) {
+            return;
+        }
+    }
+
     RV_WRITE_CSR(tselect,id);
     RV_SET_CSR(CSR_TCONTROL, TCONTROL_MPTE | TCONTROL_MTE);
     RV_SET_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE);
@@ -123,6 +164,14 @@ static inline void cpu_ll_set_watchpoint(int id,
 
 static inline void cpu_ll_clear_watchpoint(int id)
 {
+    if (cpu_ll_is_debugger_attached()) {
+        /* see description in cpu_ll_set_breakpoint()  */
+        long args[] = {false, id};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_WATCHPOINT_SET, args);
+        if (ret == 0){
+            return;
+        }
+    }
     RV_WRITE_CSR(tselect,id);
     RV_CLEAR_CSR(CSR_TCONTROL,TCONTROL_MTE);
     RV_CLEAR_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE);
@@ -132,11 +181,6 @@ static inline void cpu_ll_clear_watchpoint(int id)
     return;
 }
 
-FORCE_INLINE_ATTR bool cpu_ll_is_debugger_attached(void)
-{
-    return REG_GET_BIT(ASSIST_DEBUG_CORE_0_DEBUG_MODE_REG, ASSIST_DEBUG_CORE_0_DEBUG_MODULE_ACTIVE);
-}
-
 static inline void cpu_ll_break(void)
 {
     asm volatile("ebreak\n");

components/hal/esp32h2/include/hal/cpu_ll.h

@@ -12,6 +12,7 @@
 #include "soc/assist_debug_reg.h"
 #include "esp_attr.h"
 #include "riscv/csr.h"
+#include "riscv/semihosting.h"
 
 /*performance counter*/
 #define CSR_PCER_MACHINE    0x7e0
@@ -69,8 +70,29 @@ static inline void cpu_ll_init_hwloop(void)
     // Nothing needed here for ESP32-H2
 }
 
+FORCE_INLINE_ATTR bool cpu_ll_is_debugger_attached(void)
+{
+    return REG_GET_BIT(ASSIST_DEBUG_CORE_0_DEBUG_MODE_REG, ASSIST_DEBUG_CORE_0_DEBUG_MODULE_ACTIVE);
+}
+
 static inline void cpu_ll_set_breakpoint(int id, uint32_t pc)
 {
+    if (cpu_ll_is_debugger_attached()) {
+        /* If we want to set breakpoint which when hit transfers control to debugger
+         * we need to set `action` in `mcontrol` to 1 (Enter Debug Mode).
+         * That `action` value is supported only when `dmode` of `tdata1` is set.
+         * But `dmode` can be modified by debugger only (from Debug Mode).
+         *
+         * So when debugger is connected we use special syscall to ask it to set breakpoint for us.
+         */
+        long args[] = {true, id, (long)pc};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_BREAKPOINT_SET, args);
+        if (ret == 0) {
+            return;
+        }
+    }
+    /* The code bellow sets breakpoint which will trigger `Breakpoint` exception
+     * instead transfering control to debugger. */
     RV_WRITE_CSR(tselect,id);
     RV_SET_CSR(CSR_TCONTROL,TCONTROL_MTE);
     RV_SET_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE|TDATA1_EXECUTE);
@@ -80,6 +102,14 @@ static inline void cpu_ll_set_breakpoint(int id, uint32_t pc)
 
 static inline void cpu_ll_clear_breakpoint(int id)
 {
+    if (cpu_ll_is_debugger_attached()) {
+        /* see description in cpu_ll_set_breakpoint()  */
+        long args[] = {false, id};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_BREAKPOINT_SET, args);
+        if (ret == 0){
+            return;
+        }
+    }
     RV_WRITE_CSR(tselect,id);
     RV_CLEAR_CSR(CSR_TCONTROL,TCONTROL_MTE);
     RV_CLEAR_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE|TDATA1_EXECUTE);
@@ -103,6 +133,17 @@ static inline void cpu_ll_set_watchpoint(int id,
                                         bool on_write)
 {
     uint32_t addr_napot;
+
+    if (cpu_ll_is_debugger_attached()) {
+        /* see description in cpu_ll_set_breakpoint()  */
+        long args[] = {true, id, (long)addr, (long)size,
+            (long)((on_read ? ESP_SEMIHOSTING_WP_FLG_RD : 0) | (on_write ? ESP_SEMIHOSTING_WP_FLG_WR : 0))};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_WATCHPOINT_SET, args);
+        if (ret == 0) {
+            return;
+        }
+    }
+
     RV_WRITE_CSR(tselect,id);
     RV_SET_CSR(CSR_TCONTROL, TCONTROL_MPTE | TCONTROL_MTE);
     RV_SET_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE);
@@ -121,6 +162,14 @@ static inline void cpu_ll_set_watchpoint(int id,
 
 static inline void cpu_ll_clear_watchpoint(int id)
 {
+    if (cpu_ll_is_debugger_attached()) {
+        /* see description in cpu_ll_set_breakpoint()  */
+        long args[] = {false, id};
+        int ret = semihosting_call_noerrno(ESP_SEMIHOSTING_SYS_WATCHPOINT_SET, args);
+        if (ret == 0){
+            return;
+        }
+    }
     RV_WRITE_CSR(tselect,id);
     RV_CLEAR_CSR(CSR_TCONTROL,TCONTROL_MTE);
     RV_CLEAR_CSR(CSR_TDATA1, TDATA1_USER|TDATA1_MACHINE);
@@ -130,11 +179,6 @@ static inline void cpu_ll_clear_watchpoint(int id)
     return;
 }
 
-FORCE_INLINE_ATTR bool cpu_ll_is_debugger_attached(void)
-{
-    return REG_GET_BIT(ASSIST_DEBUG_CORE_0_DEBUG_MODE_REG, ASSIST_DEBUG_CORE_0_DEBUG_MODULE_ACTIVE);
-}
-
 static inline void cpu_ll_break(void)
 {
     asm volatile("ebreak\n");

components/riscv/include/riscv/semihosting.h

@@ -10,6 +10,53 @@
 extern "C" {
 #endif
 
+
+/* ESP custom semihosting calls numbers */
+
+/**
+ * @brief Initialize apptrace data at host side
+ *
+ * @param addr    address of apptrace control data block
+ * @return        return 0 on sucess or non-zero error code
+ */
+#define ESP_SEMIHOSTING_SYS_APPTRACE_INIT    0x64
+
+/**
+ * @brief Initialize debug stubs table at host side
+ *
+ * @param addr    address of debug stubs table
+ * @return        return 0 on sucess or non-zero error code
+ */
+#define ESP_SEMIHOSTING_SYS_DBG_STUBS_INIT   0x65
+
+/**
+ * @brief Set/clear breakpoint
+ *
+ * @param set     if true set breakpoint, otherwise clear it
+ * @param id      breakpoint ID
+ * @param addr    address to set breakpoint at. Ignored if `set` is false.
+ * @return        return 0 on sucess or non-zero error code
+ */
+#define ESP_SEMIHOSTING_SYS_BREAKPOINT_SET   0x66
+
+/**
+ * @brief Set/clear watchpoint
+ *
+ * @param set     if true set watchpoint, otherwise clear it
+ * @param id      watchpoint ID
+ * @param addr    address to set watchpoint at. Ignored if `set` is false.
+ * @param size    size of watchpoint. Ignored if `set` is false.
+ * @param flags   watchpoint flags, see description below. Ignored if `set` is false.
+ * @return        return 0 on sucess or non-zero error code
+ */
+#define ESP_SEMIHOSTING_SYS_WATCHPOINT_SET   0x67
+
+/* bit values for `flags` argument of ESP_SEMIHOSTING_SYS_WATCHPOINT_SET call. Can be ORed. */
+/* watch for 'reads' at `addr` */
+#define ESP_SEMIHOSTING_WP_FLG_RD   (1UL << 0)
+/* watch for 'writes' at `addr` */
+#define ESP_SEMIHOSTING_WP_FLG_WR   (1UL << 1)
+
 /**
  * @brief Perform semihosting call
  *