!441 aarch64增加用户态gdb调试功能

Merge pull request !441 from jesven/aarch64-gdb

components/lwp/arch/aarch64/cortex-a/lwp_arch.h

@@ -36,6 +36,11 @@ int arch_expand_user_stack(void *addr);
 
 unsigned long ffz(unsigned long x);
 
+rt_inline void icache_invalid_all(void)
+{
+    asm volatile ("ic ialluis\n\tisb sy":::"memory");
+}
+
 #ifdef __cplusplus
 }
 #endif

components/lwp/arch/aarch64/cortex-a/lwp_gcc.S

@@ -214,6 +214,12 @@ SVC_Handler:
     and x0, x8, #0xf000
     cmp x0, #0xe000
     beq lwp_signal_quit
+
+#ifdef RT_USING_GDBSERVER
+    cmp x0, #0xf000
+    beq ret_from_user
+#endif
+
     uxtb x0, w8
     bl lwp_get_sys_api
     cmp x0, xzr
@@ -258,11 +264,35 @@ ret_to_user:
     stp x4, x5, [sp, #-0x10]!
     stp x6, x7, [sp, #-0x10]!
     stp x8, x9, [sp, #-0x10]!
+    stp x10, x11, [sp, #-0x10]!
+    stp x12, x13, [sp, #-0x10]!
+    stp x14, x15, [sp, #-0x10]!
+    stp x16, x17, [sp, #-0x10]!
+    stp x18, x19, [sp, #-0x10]!
+    stp x20, x21, [sp, #-0x10]!
+    stp x22, x23, [sp, #-0x10]!
+    stp x24, x25, [sp, #-0x10]!
+    stp x26, x27, [sp, #-0x10]!
+    stp x28, x29, [sp, #-0x10]!
+
     mrs x0, fpcr
     mrs x1, fpsr
     stp x0, x1, [sp, #-0x10]!
     stp x29, x30, [sp, #-0x10]!
 
+#ifdef RT_USING_GDBSERVER
+    bl thread_is_in_debug
+    mov x1, #(1 << 21)
+    mrs x2, spsr_el1
+    cbz w0, 1f
+    orr x2, x2, x1
+    b 2f
+1:
+    bic x2, x2, x1
+2:
+    msr spsr_el1, x2
+#endif
+
     bl lwp_signal_check
     cmp x0, xzr
 
@@ -270,6 +300,17 @@ ret_to_user:
     ldp x0, x1, [sp], #0x10
     msr fpcr, x0
     msr fpsr, x1
+
+    ldp x28, x29, [sp], #0x10
+    ldp x26, x27, [sp], #0x10
+    ldp x24, x25, [sp], #0x10
+    ldp x22, x23, [sp], #0x10
+    ldp x20, x21, [sp], #0x10
+    ldp x18, x19, [sp], #0x10
+    ldp x16, x17, [sp], #0x10
+    ldp x14, x15, [sp], #0x10
+    ldp x12, x13, [sp], #0x10
+    ldp x10, x11, [sp], #0x10
     ldp x8, x9, [sp], #0x10
     ldp x6, x7, [sp], #0x10
     ldp x4, x5, [sp], #0x10
@@ -278,6 +319,51 @@ ret_to_user:
     RESTORE_FPU sp
 
     bne user_do_signal
+
+#ifdef RT_USING_GDBSERVER
+    SAVE_FPU sp
+    stp x0, x1, [sp, #-0x10]!
+    stp x2, x3, [sp, #-0x10]!
+    stp x4, x5, [sp, #-0x10]!
+    stp x6, x7, [sp, #-0x10]!
+    stp x8, x9, [sp, #-0x10]!
+    stp x10, x11, [sp, #-0x10]!
+    stp x12, x13, [sp, #-0x10]!
+    stp x14, x15, [sp, #-0x10]!
+    stp x16, x17, [sp, #-0x10]!
+    stp x18, x19, [sp, #-0x10]!
+    stp x20, x21, [sp, #-0x10]!
+    stp x22, x23, [sp, #-0x10]!
+    stp x24, x25, [sp, #-0x10]!
+    stp x26, x27, [sp, #-0x10]!
+    stp x28, x29, [sp, #-0x10]!
+    mrs x0, fpcr
+    mrs x1, fpsr
+    stp x0, x1, [sp, #-0x10]!
+    stp x29, x30, [sp, #-0x10]!
+    mrs x0, elr_el1
+    bl lwp_check_debug_attach_req
+    ldp x29, x30, [sp], #0x10
+    ldp x0, x1, [sp], #0x10
+    msr fpcr, x0
+    msr fpsr, x1
+    ldp x28, x29, [sp], #0x10
+    ldp x26, x27, [sp], #0x10
+    ldp x24, x25, [sp], #0x10
+    ldp x22, x23, [sp], #0x10
+    ldp x20, x21, [sp], #0x10
+    ldp x18, x19, [sp], #0x10
+    ldp x16, x17, [sp], #0x10
+    ldp x14, x15, [sp], #0x10
+    ldp x12, x13, [sp], #0x10
+    ldp x10, x11, [sp], #0x10
+    ldp x8, x9, [sp], #0x10
+    ldp x6, x7, [sp], #0x10
+    ldp x4, x5, [sp], #0x10
+    ldp x2, x3, [sp], #0x10
+    ldp x0, x1, [sp], #0x10
+    RESTORE_FPU sp
+#endif
     eret
 
 .global lwp_check_exit
@@ -287,6 +373,16 @@ lwp_check_exit:
     stp x2, x3, [sp, #-0x10]!
     stp x4, x5, [sp, #-0x10]!
     stp x6, x7, [sp, #-0x10]!
+    stp x10, x11, [sp, #-0x10]!
+    stp x12, x13, [sp, #-0x10]!
+    stp x14, x15, [sp, #-0x10]!
+    stp x16, x17, [sp, #-0x10]!
+    stp x18, x19, [sp, #-0x10]!
+    stp x20, x21, [sp, #-0x10]!
+    stp x22, x23, [sp, #-0x10]!
+    stp x24, x25, [sp, #-0x10]!
+    stp x26, x27, [sp, #-0x10]!
+    stp x28, x29, [sp, #-0x10]!
     mrs x0, fpcr
     mrs x1, fpsr
     stp x0, x1, [sp, #-0x10]!
@@ -299,6 +395,16 @@ lwp_check_exit:
     ldp x0, x1, [sp], #0x10
     msr fpcr, x0
     msr fpsr, x1
+    ldp x28, x29, [sp], #0x10
+    ldp x26, x27, [sp], #0x10
+    ldp x24, x25, [sp], #0x10
+    ldp x22, x23, [sp], #0x10
+    ldp x20, x21, [sp], #0x10
+    ldp x18, x19, [sp], #0x10
+    ldp x16, x17, [sp], #0x10
+    ldp x14, x15, [sp], #0x10
+    ldp x12, x13, [sp], #0x10
+    ldp x10, x11, [sp], #0x10
     ldp x6, x7, [sp], #0x10
     ldp x4, x5, [sp], #0x10
     ldp x2, x3, [sp], #0x10
@@ -310,6 +416,138 @@ lwp_check_exit:
     mov x0, xzr
     b sys_exit
 
+/*
+struct rt_hw_exp_stack
+{
+    unsigned long pc;     0
+    unsigned long cpsr;
+    unsigned long sp_el0; 0x10
+    unsigned long x30;
+    unsigned long fpcr;   0x20
+    unsigned long fpsr;
+    unsigned long x28;    0x30
+    unsigned long x29;
+    unsigned long x26;    0x40
+    unsigned long x27;
+    unsigned long x24;    0x50
+    unsigned long x25;
+    unsigned long x22;    0x60
+    unsigned long x23;
+    unsigned long x20;    0x70
+    unsigned long x21;
+    unsigned long x18;    0x80
+    unsigned long x19;
+    unsigned long x16;    0x90
+    unsigned long x17;
+    unsigned long x14;    0xa0
+    unsigned long x15;
+    unsigned long x12;    0xb0
+    unsigned long x13;
+    unsigned long x10;    0xc0
+    unsigned long x11;
+    unsigned long x8;     0xd0
+    unsigned long x9;
+    unsigned long x6;     0xe0
+    unsigned long x7;
+    unsigned long x4;     0xf0
+    unsigned long x5;
+    unsigned long x2;     0x100
+    unsigned long x3;
+    unsigned long x0;     0x110
+    unsigned long x1;
+
+    unsigned long long fpu[16]; 0x120
+                                0x220 = 0x120 + 0x10 * 0x10
+};
+*/
+#ifdef RT_USING_GDBSERVER
+.global lwp_check_debug
+lwp_check_debug:
+    SAVE_FPU sp
+    stp x0, x1, [sp, #-0x10]!
+    stp x2, x3, [sp, #-0x10]!
+    stp x4, x5, [sp, #-0x10]!
+    stp x6, x7, [sp, #-0x10]!
+    stp x10, x11, [sp, #-0x10]!
+    stp x12, x13, [sp, #-0x10]!
+    stp x14, x15, [sp, #-0x10]!
+    stp x16, x17, [sp, #-0x10]!
+    stp x18, x19, [sp, #-0x10]!
+    stp x20, x21, [sp, #-0x10]!
+    stp x22, x23, [sp, #-0x10]!
+    stp x24, x25, [sp, #-0x10]!
+    stp x26, x27, [sp, #-0x10]!
+    stp x28, x29, [sp, #-0x10]!
+
+    mrs x0, fpcr
+    mrs x1, fpsr
+    stp x0, x1, [sp, #-0x10]!
+    stp x29, x30, [sp, #-0x10]!
+
+    bl lwp_check_debug_suspend
+    cmp w0, wzr
+    beq lwp_check_debug_quit
+
+    mrs x2, sp_el0
+    sub x2, x2, #8
+    mov x3, x2
+    msr sp_el0, x2
+    ldr x0, =lwp_debugreturn
+    ldr w1, [x0]
+    str w1, [x2]
+    ldr w1, [x0, #4]
+    str w1, [x2, #4]
+
+    dc cvau, x2
+    add x2, x2, #4
+    dc cvau, x2
+
+    dsb sy
+    isb sy
+
+    ic ialluis
+    isb sy
+
+    msr elr_el1, x3 /* lwp_debugreturn */
+    mrs x1, spsr_el1
+    stp x0, x1, [sp, #-0x10]!
+    mov x1, #(SPSR_Mode(0) | SPSR_A64)
+    msr spsr_el1, x1
+    eret
+ret_from_user:
+    /* sp_el0 += 8 for drop ins lwp_debugreturn */
+    mrs x0, sp_el0
+    add x0, x0, #8
+    msr sp_el0, x0
+    /* now is el1, sp is pos(empty) - sizeof(context) */
+    mov x0, sp
+    add x0, x0, #0x220 /* sizeof(context) */
+    mov sp, x0
+    ldp x0, x1, [sp], #0x10  /* x0 is origin spsr_el1 */
+    msr spsr_el1, x0
+lwp_check_debug_quit:
+    ldp x29, x30, [sp], #0x10
+    ldp x0, x1, [sp], #0x10
+    msr fpcr, x0
+    msr fpsr, x1
+    ldp x28, x29, [sp], #0x10
+    ldp x26, x27, [sp], #0x10
+    ldp x24, x25, [sp], #0x10
+    ldp x22, x23, [sp], #0x10
+    ldp x20, x21, [sp], #0x10
+    ldp x18, x19, [sp], #0x10
+    ldp x16, x17, [sp], #0x10
+    ldp x14, x15, [sp], #0x10
+    ldp x12, x13, [sp], #0x10
+    ldp x10, x11, [sp], #0x10
+    ldp x6, x7, [sp], #0x10
+    ldp x4, x5, [sp], #0x10
+    ldp x2, x3, [sp], #0x10
+    ldp x0, x1, [sp], #0x10
+    RESTORE_FPU sp
+    ret
+#endif
+
 lwp_signal_quit:
     msr daifset, #3
 /*
@@ -383,7 +621,7 @@ user_do_signal:
     ldr w1, [x0, #4]
     str w1, [sp, #4]
 
-    mov x9, sp /* lwp_sigreturn */
+    mov x20, sp /* lwp_sigreturn */
     mov x0, sp
 
     dc cvau, x0
@@ -397,17 +635,23 @@ user_do_signal:
     mrs x2, spsr_el1
     bl lwp_signal_backup
     /* x0 is signal */
-    mov x8, x0
+    mov x19, x0
     bl lwp_sighandler_get
     adds x1, x0, xzr
-    mov x0, x8
+    mov x0, x19
     bne 1f
-    mov x1, x9
+    mov x1, x20
 1:
     msr elr_el1, x1
-    mov x30, x9
+    mov x30, x20
     eret
 
+#ifdef RT_USING_GDBSERVER
+lwp_debugreturn:
+    mov x9, 0xf000
+    svc #0
+#endif
+
 lwp_sigreturn:
     mov x8, #0xe000
     svc #0

components/lwp/lwp.c

@@ -1300,7 +1300,14 @@ void lwp_user_setting_restore(rt_thread_t thread)
     {
         struct rt_lwp *l = (struct rt_lwp *)thread->lwp;
 
-        set_process_id((uint32_t)(size_t)l);
+        if (l != 0)
+        {
+            set_process_id((size_t)l->pid);
+        }
+        else
+        {
+            set_process_id(0);
+        }
         if (l && l->debug)
         {
             uint32_t step_type = 0;

libcpu/aarch64/common/armv8.h

@@ -15,7 +15,7 @@
 struct rt_hw_exp_stack
 {
 	unsigned long pc;
-	unsigned long spsr;
+	unsigned long cpsr;
 	unsigned long sp_el0;
 	unsigned long x30;
     unsigned long fpcr;

libcpu/aarch64/common/context_gcc.S

@@ -30,6 +30,14 @@ rt_hw_cpu_id:
    mrs x0, tpidr_el1           /* MPIDR_EL1: Multi-Processor Affinity Register */
    ret
 
+/*
+void set_process_id(size_t id)
+*/
+.global set_process_id
+set_process_id:
+    msr CONTEXTIDR_EL1, x0
+    ret
+
   /*
  *enable gtimer
  */
@@ -136,6 +144,10 @@ rt_hw_get_gtimer_frq:
     /* Set the SP to point to the stack of the task being restored. */
     MOV     SP, X0
 
+#ifdef RT_USING_GDBSERVER
+    bl lwp_check_debug
+#endif
+
     BL      lwp_check_exit
 
     LDP     X2, X3, [SP], #0x10  /* SPSR and ELR. */
@@ -175,6 +187,10 @@ rt_hw_get_gtimer_frq:
     /* Set the SP to point to the stack of the task being restored. */
     MOV     SP, X0
 
+#ifdef RT_USING_LWP
+    bl      lwp_check_exit
+#endif
+
     BL      lwp_check_exit
 
     BL      rt_thread_self

libcpu/aarch64/common/trap.c

@@ -31,10 +31,103 @@ extern long list_thread(void);
 #include <lwp_core_dump.h>
 #endif
 
+#ifdef RT_USING_GDBSERVER
+#include <lwp_gdbserver.h>
+#include <hw_breakpoint.h>
+
+gdb_thread_info wp_thread_info = {
+    RT_NULL,
+    GDB_NOTIFIY_NR,
+    0, RT_NULL, 0
+};
+
+static int check_debug_event(struct rt_hw_exp_stack *regs, unsigned long esr)
+{
+    uint32_t elx = regs->cpsr & 0x1fUL;
+    unsigned char ec;
+
+    ec = (unsigned char)((esr >> 26) & 0x3fU);
+    if (elx == 0x00) /* is EL0 */
+    {
+        struct rt_channel_msg msg;
+        gdb_thread_info thread_info;
+        int ret;
+
+        if (ec == 0x3c || ec == 0x30 /* breakpoint event */
+                || ec == 0x32) /* step event */
+        {
+            if (wp_thread_info.notify_type == GDB_NOTIFIY_WATCHPOINT)
+            {
+                thread_info.watch_addr = (uint32_t *)regs->pc;
+                thread_info.rw = wp_thread_info.rw;
+                thread_info.notify_type = GDB_NOTIFIY_WATCHPOINT;
+                wp_thread_info.notify_type = GDB_NOTIFIY_NR;
+                ret = 2;
+            }
+            else
+            {
+                /* is breakpoint event */
+                do {
+                    struct rt_lwp *gdb_lwp = gdb_get_dbg_lwp();
+                    struct rt_lwp *lwp;
+
+                    if (!gdb_lwp)
+                    {
+                        break;
+                    }
+                    lwp = lwp_self();
+                    if (lwp == gdb_lwp)
+                    {
+                        break;
+                    }
+                    *(uint32_t *)regs->pc = lwp->bak_first_ins;
+                    rt_hw_cpu_dcache_ops(RT_HW_CACHE_FLUSH, (void *)regs->pc, 4);
+                    icache_invalid_all();
+                    lwp->debug = 0;
+                    return 1;
+                } while (0);
+
+                thread_info.notify_type = GDB_NOTIFIY_BREAKPOINT;
+                thread_info.abt_ins = *(uint32_t *)regs->pc;
+                ret = 1;
+            }
+        }
+        else if (ec == 0x35 || ec == 0x34) /* watchpoint event */
+        {
+            /* is watchpoint event */
+            arch_deactivate_breakpoints();
+            arch_activate_step();
+            wp_thread_info.rw = (esr >> 6) & 1UL;
+            wp_thread_info.notify_type = GDB_NOTIFIY_WATCHPOINT;
+            return 3;
+        }
+        else
+        {
+            return 0; /* not support */
+        }
+        thread_info.thread = rt_thread_self();
+        thread_info.thread->regs = regs;
+        msg.u.d = (void *)&thread_info;
+        rt_hw_dmb();
+        thread_info.thread->debug_suspend = 1;
+        rt_hw_dsb();
+        rt_thread_suspend_with_flag(thread_info.thread, RT_UNINTERRUPTIBLE);
+        rt_raw_channel_send(gdb_get_server_channel(), &msg);
+        rt_schedule();
+        while (thread_info.thread->debug_suspend)
+        {
+            rt_thread_suspend_with_flag(thread_info.thread, RT_UNINTERRUPTIBLE);
+            rt_schedule();
+        }
+        return ret;
+    }
+    return 0;
+}
+#endif
 void sys_exit(int value);
 void check_user_fault(struct rt_hw_exp_stack *regs, uint32_t pc_adj, char *info)
 {
-    uint32_t mode = regs->spsr;
+    uint32_t mode = regs->cpsr;
 
     if ((mode & 0x1f) == 0x00)
     {
@@ -88,38 +181,10 @@ void rt_hw_show_register(struct rt_hw_exp_stack *regs)
     rt_kprintf("X24:0x%16.16p X25:0x%16.16p X26:0x%16.16p X27:0x%16.16p\n", (void *)regs->x24, (void *)regs->x25, (void *)regs->x26, (void *)regs->x27);
     rt_kprintf("X28:0x%16.16p X29:0x%16.16p X30:0x%16.16p\n", (void *)regs->x28, (void *)regs->x29, (void *)regs->x30);
     rt_kprintf("SP_EL0:0x%16.16p\n", (void *)regs->sp_el0);
-    rt_kprintf("SPSR  :0x%16.16p\n", (void *)regs->spsr);
+    rt_kprintf("SPSR  :0x%16.16p\n", (void *)regs->cpsr);
     rt_kprintf("EPC   :0x%16.16p\n", (void *)regs->pc);
 }
 
-/**
- * An abort indicates that the current memory access cannot be completed,
- * which occurs during an instruction prefetch.
- *
- * @param regs system registers
- *
- * @note never invoke this function in application
- */
-void rt_hw_trap_pabt(struct rt_hw_exp_stack *regs)
-{
-#ifdef RT_USING_LWP
-#ifdef RT_USING_GDBSERVER
-    if (check_debug_event(regs, 4))
-    {
-        return;
-    }
-#endif
-    check_user_fault(regs, 4, "User prefetch abort");
-#endif
-    rt_unwind(regs, 4);
-    rt_kprintf("prefetch abort:\n");
-    rt_hw_show_register(regs);
-#ifdef RT_USING_FINSH
-    list_thread();
-#endif
-    rt_hw_cpu_shutdown();
-}
-
 void rt_hw_trap_irq(void)
 {
 #ifdef SOC_BCM283x
@@ -140,9 +205,9 @@ void rt_hw_trap_irq(void)
             if (int_source & 0x08)
             {
                 isr_func = isr_table[IRQ_ARM_TIMER].handler;
-                #ifdef RT_USING_INTERRUPT_INFO
-                            isr_table[IRQ_ARM_TIMER].counter++;
-                #endif
+#ifdef RT_USING_INTERRUPT_INFO
+                isr_table[IRQ_ARM_TIMER].counter++;
+#endif
                 if (isr_func)
                 {
                     param = isr_table[IRQ_ARM_TIMER].param;
@@ -252,6 +317,15 @@ void rt_hw_trap_exception(struct rt_hw_exp_stack *regs)
     asm volatile("mrs %0, esr_el1":"=r"(esr));
     ec = (unsigned char)((esr >> 26) & 0x3fU);
 
+#ifdef RT_USING_LWP
+#ifdef RT_USING_GDBSERVER
+    if (check_debug_event(regs, esr))
+    {
+        return;
+    }
+    else
+#endif
+#endif
     if (ec == 0x15) /* is 64bit syscall ? */
     {
         SVC_Handler(regs);

libcpu/arm/cortex-a/start_gcc.S

@@ -281,6 +281,7 @@ enable_mmu:
 
 .global set_process_id
 set_process_id:
+    LSL r0, r0, #8
     MCR p15, 0, r0, c13, c0, 1
     mov pc, lr