Add handling of FPU for Xtensa CPU in gdbstub.

components/esp_gdbstub/src/gdbstub.c

@@ -62,11 +62,11 @@ void esp_gdbstub_panic_handler(void *in_frame)
         esp_gdbstub_send_end();
     } else if (s_scratch.state == GDBSTUB_NOT_STARTED) {
         s_scratch.state = GDBSTUB_STARTED;
-        /* Save the paniced frame and get the list of tasks */
+        /* Save the panicked frame and get the list of tasks */
         memcpy(&s_scratch.paniced_frame, frame, sizeof(*frame));
         init_task_info();
         find_paniced_task_index();
-        /* Current task is the paniced task */
+        /* Current task is the panicked task */
         if (s_scratch.paniced_task_index == GDBSTUB_CUR_TASK_INDEX_UNKNOWN) {
             set_active_task(0);
         } else {
@@ -144,7 +144,7 @@ static inline void disable_all_wdts(void)
     }
 
     #if SOC_TIMER_GROUPS >= 2
-    /* Interupt WDT is the Main Watchdog Timer of Timer Group 1 */
+    /* Interrupt WDT is the Main Watchdog Timer of Timer Group 1 */
     if (true == wdt1_context_enabled) {
         wdt_hal_write_protect_disable(&wdt1_context);
         wdt_hal_disable(&wdt1_context);
@@ -173,7 +173,7 @@ static inline void enable_all_wdts(void)
         wdt_hal_write_protect_enable(&wdt0_context);
     }
     #if SOC_TIMER_GROUPS >= 2
-    /* Interupt WDT is the Main Watchdog Timer of Timer Group 1 */
+    /* Interrupt WDT is the Main Watchdog Timer of Timer Group 1 */
     if (false == wdt1_context_enabled) {
         wdt_hal_write_protect_disable(&wdt1_context);
         wdt_hal_enable(&wdt1_context);
@@ -208,7 +208,7 @@ static bool process_gdb_kill = false;
 static bool gdb_debug_int = false;
 
 /**
- * @breef Handle UART interrupt
+ * @brief Handle UART interrupt
  *
  * Handle UART interrupt for gdbstub. The function disable WDT.
  * If Ctrl+C combination detected (0x03), then application will start to process incoming GDB messages.
@@ -658,7 +658,8 @@ static void handle_C_command(const unsigned char *cmd, int len)
     esp_gdbstub_send_str_packet("OK");
 }
 
-/** Set Register ... */
+
+/* Set Register ... */
 static void handle_P_command(const unsigned char *cmd, int len)
 {
     uint32_t reg_index = 0;
@@ -684,7 +685,7 @@ static void handle_P_command(const unsigned char *cmd, int len)
     p_addr_ptr[0] = addr_ptr[3];
 
     esp_gdbstub_set_register((esp_gdbstub_frame_t *)temp_regs_frame, reg_index, p_address);
-    /* Convert current regioster file to GDB*/
+    /* Convert current register file to GDB*/
     esp_gdbstub_frame_to_regfile((esp_gdbstub_frame_t *)temp_regs_frame, gdb_local_regfile);
     /* Sen OK response*/
     esp_gdbstub_send_str_packet("OK");

components/esp_gdbstub/src/port/xtensa/gdbstub_xtensa.c

@@ -12,6 +12,7 @@
 #include "esp_cpu.h"
 #include "esp_ipc_isr.h"
 #include "esp_private/crosscore_int.h"
+#include <xtensa_context.h>
 
 #if !XCHAL_HAVE_WINDOWED
 #warning "gdbstub_xtensa: revisit the implementation for Call0 ABI"
@@ -38,13 +39,54 @@ static void update_regfile_common(esp_gdbstub_gdb_regfile_t *dst)
     RSR(CONFIGID1, dst->configid1);
 }
 
+#if XCHAL_HAVE_FP
+/** @brief Read FPU registers to memory
+*/
+static void gdbstub_read_fpu_regs(void *data)
+{
+    float *ptr0;
+    void  *ptr1;
+
+    asm volatile ("mov %0, %1" : "=a" (ptr0) : "a" (data));
+
+    asm volatile ("rur.FCR %0" : "=a" (ptr1));
+    asm volatile ("s32i %0, %1, 64" : "=a" (ptr1) : "a" (ptr0));
+    asm volatile ("rur.FSR %0" : "=a" (ptr1));
+    asm volatile ("s32i %0, %1, 68" : "=a" (ptr1) : "a" (ptr0));
+
+    asm volatile ("ssi f0, %0, 0" :: "a" (ptr0)); //*(ptr0 + 0) = f0;
+    asm volatile ("ssi f1, %0, 4" :: "a" (ptr0)); //*(ptr0 + 4) = f1;
+    asm volatile ("ssi f2, %0, 8" :: "a" (ptr0)); //...
+    asm volatile ("ssi f3, %0, 12" :: "a" (ptr0));
+    asm volatile ("ssi f4, %0, 16" :: "a" (ptr0));
+    asm volatile ("ssi f5, %0, 20" :: "a" (ptr0));
+    asm volatile ("ssi f6, %0, 24" :: "a" (ptr0));
+    asm volatile ("ssi f7, %0, 28" :: "a" (ptr0));
+    asm volatile ("ssi f8, %0, 32" :: "a" (ptr0));
+    asm volatile ("ssi f9, %0, 36" :: "a" (ptr0));
+    asm volatile ("ssi f10, %0, 40" :: "a" (ptr0));
+    asm volatile ("ssi f11, %0, 44" :: "a" (ptr0));
+    asm volatile ("ssi f12, %0, 48" :: "a" (ptr0));
+    asm volatile ("ssi f13, %0, 52" :: "a" (ptr0));
+    asm volatile ("ssi f14, %0, 56" :: "a" (ptr0));
+    asm volatile ("ssi f15, %0, 60" :: "a" (ptr0));
+
+}
+#endif // XCHAL_HAVE_FP
+
+
+extern const uint32_t offset_pxEndOfStack;
+extern const uint32_t offset_cpsa;  /* Offset to start of the CPSA area on the stack. See uxInitialiseStackCPSA(). */
+extern uint32_t _xt_coproc_owner_sa[2];
+
 void esp_gdbstub_frame_to_regfile(const esp_gdbstub_frame_t *frame, esp_gdbstub_gdb_regfile_t *dst)
 {
     init_regfile(dst);
     const uint32_t *a_regs = (const uint32_t *) &frame->a0;
+
     if (!(esp_ptr_executable(esp_cpu_pc_to_addr(frame->pc)) && (frame->pc & 0xC0000000U))) {
         /* Xtensa ABI sets the 2 MSBs of the PC according to the windowed call size
-         * Incase the PC is invalid, GDB will fail to translate addresses to function names
+         * In case the PC is invalid, GDB will fail to translate addresses to function names
          * Hence replacing the PC to a placeholder address in case of invalid PC
          */
         dst->pc = (uint32_t)&_invalid_pc_placeholder;
@@ -59,6 +101,36 @@ void esp_gdbstub_frame_to_regfile(const esp_gdbstub_frame_t *frame, esp_gdbstub_
         dst->a[i] = 0xDEADBEEF;
     }
 
+#if XCHAL_HAVE_FP
+
+    extern void *pxCurrentTCBs[2];
+    void *current_tcb_ptr = pxCurrentTCBs[0];
+    uint32_t *current_fpu_ptr = NULL;
+
+#if !CONFIG_FREERTOS_UNICORE
+    current_tcb_ptr = pxCurrentTCBs[esp_cpu_get_core_id()];
+#endif
+    uint32_t cp_enabled;
+    RSR(CPENABLE, cp_enabled);
+
+    // Check if the co-processor is enabled
+    if (cp_enabled) {
+        gdbstub_read_fpu_regs(dst->f);
+    } else {
+        current_tcb_ptr += offset_pxEndOfStack;
+        current_tcb_ptr = *(void **)current_tcb_ptr;
+        current_tcb_ptr -= offset_cpsa;
+        // Operation (&~0xf) required in .macro get_cpsa_from_tcb reg_A reg_B
+        current_tcb_ptr = (void*)((uint32_t)current_tcb_ptr&~0xf);
+        current_fpu_ptr = *(uint32_t **)(current_tcb_ptr + XT_CP_ASA);
+
+        dst->fcr = current_fpu_ptr[0];
+        dst->fsr = current_fpu_ptr[1];
+        for (int i = 0; i < 16; i++) {
+            dst->f[i] = current_fpu_ptr[i + 2];
+        }
+    }
+#endif //XCHAL_HAVE_FP
 #if XCHAL_HAVE_LOOPS
     dst->lbeg = frame->lbeg;
     dst->lend = frame->lend;
@@ -72,6 +144,84 @@ void esp_gdbstub_frame_to_regfile(const esp_gdbstub_frame_t *frame, esp_gdbstub_
 
 #ifdef CONFIG_ESP_GDBSTUB_SUPPORT_TASKS
 
+/* Represents FreeRTOS TCB structure */
+typedef struct {
+    uint8_t *top_of_stack;
+    /* Other members aren't needed */
+} dummy_tcb_t;
+
+void esp_gdbstub_tcb_frame_to_regfile(dummy_tcb_t *tcb, esp_gdbstub_gdb_regfile_t *dst)
+{
+    const XtExcFrame *frame = (XtExcFrame *) tcb->top_of_stack;
+
+    init_regfile(dst);
+    const uint32_t *a_regs = (const uint32_t *) &frame->a0;
+
+    if (!(esp_ptr_executable(esp_cpu_pc_to_addr(frame->pc)) && (frame->pc & 0xC0000000U))) {
+        /* Xtensa ABI sets the 2 MSBs of the PC according to the windowed call size
+         * In case the PC is invalid, GDB will fail to translate addresses to function names
+         * Hence replacing the PC to a placeholder address in case of invalid PC
+         */
+        dst->pc = (uint32_t)&_invalid_pc_placeholder;
+    } else {
+        dst->pc = (uint32_t)esp_cpu_pc_to_addr(frame->pc);
+    }
+
+    for (int i = 0; i < 16; i++) {
+        dst->a[i] = a_regs[i];
+    }
+    for (int i = 16; i < 64; i++) {
+        dst->a[i] = 0xDEADBEEF;
+    }
+
+#if XCHAL_HAVE_FP
+    uint32_t *current_xt_coproc_owner_sa = (uint32_t *)_xt_coproc_owner_sa[0];
+
+#if !CONFIG_FREERTOS_UNICORE
+    current_xt_coproc_owner_sa = (uint32_t *)_xt_coproc_owner_sa[esp_cpu_get_core_id()];
+#endif
+
+    uint32_t cp_enabled;
+    RSR(CPENABLE, cp_enabled);
+
+    void *current_tcb_ptr = tcb;
+    uint32_t *current_fpu_ptr = NULL;
+    {
+        current_tcb_ptr += offset_pxEndOfStack;
+        current_tcb_ptr = *(void **)current_tcb_ptr;
+        current_tcb_ptr -= offset_cpsa;
+        // Operation (&~0xf) required in .macro get_cpsa_from_tcb reg_A reg_B
+        current_tcb_ptr = (void*)((uint32_t)current_tcb_ptr&~0xf);
+        current_fpu_ptr = *(uint32_t **)(current_tcb_ptr + XT_CP_ASA);
+
+        bool use_fpu_regs =  ((false == cp_enabled) &&  (current_xt_coproc_owner_sa[0] == 1) && (current_fpu_ptr == (uint32_t*)current_xt_coproc_owner_sa[2]));
+
+        dst->fcr = current_fpu_ptr[0];
+        dst->fsr = current_fpu_ptr[1];
+        for (int i = 0; i < 16; i++) {
+            dst->f[i] = current_fpu_ptr[i + 2];
+        }
+
+        /* We have situation when FPU is in use, but the context not stored
+          to the memory, and we have to read from CPU registers.
+        */
+        if (use_fpu_regs) {
+            gdbstub_read_fpu_regs(dst->f);
+        }
+    }
+#endif // XCHAL_HAVE_FP
+
+#if XCHAL_HAVE_LOOPS
+    dst->lbeg = frame->lbeg;
+    dst->lend = frame->lend;
+    dst->lcount = frame->lcount;
+#endif
+
+    dst->ps = (frame->ps & PS_UM) ? (frame->ps & ~PS_EXCM) : frame->ps;
+    dst->sar = frame->sar;
+    update_regfile_common(dst);
+}
+
 static void solicited_frame_to_regfile(const XtSolFrame *frame, esp_gdbstub_gdb_regfile_t *dst)
 {
     init_regfile(dst);
@@ -94,20 +244,12 @@ static void solicited_frame_to_regfile(const XtSolFrame *frame, esp_gdbstub_gdb_
     update_regfile_common(dst);
 }
 
-/* Represents FreeRTOS TCB structure */
-typedef struct {
-    uint8_t *top_of_stack;
-    /* Other members aren't needed */
-} dummy_tcb_t;
-
-
 void esp_gdbstub_tcb_to_regfile(TaskHandle_t tcb, esp_gdbstub_gdb_regfile_t *dst)
 {
-    const dummy_tcb_t *dummy_tcb = (const dummy_tcb_t *) tcb;
+    dummy_tcb_t *dummy_tcb = (dummy_tcb_t *) tcb;
-
     const XtExcFrame *frame = (XtExcFrame *) dummy_tcb->top_of_stack;
     if (frame->exit != 0) {
-        esp_gdbstub_frame_to_regfile(frame, dst);
+        esp_gdbstub_tcb_frame_to_regfile(dummy_tcb, dst);
     } else {
         const XtSolFrame *taskFrame = (const XtSolFrame *) dummy_tcb->top_of_stack;
         solicited_frame_to_regfile(taskFrame, dst);
@@ -206,14 +348,80 @@ void esp_gdbstub_trigger_cpu(void)
  * Set register in frame with address to value
  *
  * */
+
 void esp_gdbstub_set_register(esp_gdbstub_frame_t *frame, uint32_t reg_index, uint32_t value)
 {
-    switch (reg_index) {
+    uint32_t temp_fpu_value = value;
-    case 0:
+    float *ptr0;
+
+    asm volatile ("mov %0, %1" : "=a" (ptr0) : "a" (&temp_fpu_value));
+
+    if (reg_index == 0) {
         frame->pc = value;
-        break;
+    } else if (reg_index > 0 && (reg_index <= 27)) {
-    default:
         (&frame->a0)[reg_index - 1] = value;
-        break;
     }
+#if XCHAL_HAVE_FP
+    void  *ptr1;
+    uint32_t cp_enabled;
+    RSR(CPENABLE, cp_enabled);
+    if (cp_enabled != 0) {
+        if (reg_index == 87) {
+            asm volatile ("lsi f0, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 88) {
+            asm volatile ("lsi f1, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 89) {
+            asm volatile ("lsi f2, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 90) {
+            asm volatile ("lsi f3, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 91) {
+            asm volatile ("lsi f4, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 92) {
+            asm volatile ("lsi f5, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 93) {
+            asm volatile ("lsi f6, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 94) {
+            asm volatile ("lsi f7, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 95) {
+            asm volatile ("lsi f8, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 96) {
+            asm volatile ("lsi f9, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 97) {
+            asm volatile ("lsi f10, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 98) {
+            asm volatile ("lsi f11, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 99) {
+            asm volatile ("lsi f12, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 100) {
+            asm volatile ("lsi f13, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 101) {
+            asm volatile ("lsi f14, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 102) {
+            asm volatile ("lsi f15, %0, 0" :: "a" (ptr0));
+        }
+        if (reg_index == 103) {
+            asm volatile ("l32i %0, %1, 0" : "=a" (ptr1) : "a" (ptr0));
+            asm volatile ("wur.FCR %0" : "=a" (ptr1));
+        }
+        if (reg_index == 104) {
+            asm volatile ("l32i %0, %1, 0" : "=a" (ptr1) : "a" (ptr0));
+            asm volatile ("wur.FSR %0" : "=a" (ptr1));
+        }
+    }
+#endif // XCHAL_HAVE_FP
 }