Merge pull request #2731 from jesven/a9-fpu

add cortex-a fpu support

4 files changed, 130 insertions, 0 deletions

diff --git a/libcpu/arm/cortex-a/context_gcc.S b/libcpu/arm/cortex-a/context_gcc.S
index dff7a1de68..768c5138e9 100644
--- a/libcpu/arm/cortex-a/context_gcc.S
+++ b/libcpu/arm/cortex-a/context_gcc.S
@@ -76,6 +76,18 @@ rt_hw_context_switch:
     stmfd sp, {r13, r14}^   @ push usr_sp usr_lr
     sub sp, #8
 #endif
+#ifdef RT_USING_FPU
+    /* fpu context */
+    vmrs r6, fpexc
+    tst  r6, #(1<<30)
+    beq 1f
+    vstmdb sp!, {d0-d15}
+    vstmdb sp!, {d16-d31}
+    vmrs r5, fpscr
+    stmfd sp!, {r5}
+1:
+    stmfd sp!, {r6}
+#endif
 
     str sp, [r0]            @ store sp in preempted tasks TCB
     ldr sp, [r1]            @ get new task stack pointer
@@ -147,6 +159,18 @@ rt_hw_context_switch_exit:
     mov     sp, r0
 #endif
 #endif
+#ifdef RT_USING_FPU
+/* fpu context */
+    ldmfd sp!, {r6}
+    vmsr fpexc, r6
+    tst  r6, #(1<<30)
+    beq 1f
+    ldmfd sp!, {r5}
+    vmsr fpscr, r5
+    vldmia sp!, {d16-d31}
+    vldmia sp!, {d0-d15}
+1:
+#endif
 
 #ifdef RT_USING_LWP
     ldmfd   sp, {r13, r14}^ /* usr_sp, usr_lr */
diff --git a/libcpu/arm/cortex-a/stack.c b/libcpu/arm/cortex-a/stack.c
index d76b8a428a..07d795ab5b 100644
--- a/libcpu/arm/cortex-a/stack.c
+++ b/libcpu/arm/cortex-a/stack.c
@@ -60,6 +60,9 @@ rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter,
     *(--stk) = 0;       /* user lr */
     *(--stk) = 0;       /* user sp*/
 #endif
+#ifdef RT_USING_FPU
+    *(--stk) = 0;       /* not use fpu*/
+#endif
 
     /* return task's current stack address */
     return (rt_uint8_t *)stk;
diff --git a/libcpu/arm/cortex-a/start_gcc.S b/libcpu/arm/cortex-a/start_gcc.S
index 014846f3aa..91faa0e3ff 100644
--- a/libcpu/arm/cortex-a/start_gcc.S
+++ b/libcpu/arm/cortex-a/start_gcc.S
@@ -22,7 +22,11 @@
 .equ I_Bit,           0x80            @ when I bit is set, IRQ is disabled
 .equ F_Bit,           0x40            @ when F bit is set, FIQ is disabled
 
+#ifdef RT_USING_FPU
+.equ UND_Stack_Size,     0x00000400
+#else
 .equ UND_Stack_Size,     0x00000000
+#endif
 .equ SVC_Stack_Size,     0x00000400
 .equ ABT_Stack_Size,     0x00000000
 .equ RT_FIQ_STACK_PGSZ,  0x00000000
@@ -50,6 +54,11 @@ _reset:
     /* set the cpu to SVC32 mode and disable interrupt */
     cps #Mode_SVC
 
+#ifdef RT_USING_FPU
+    mov r4, #0xfffffff
+    mcr p15, 0, r4, c1, c0, 2
+#endif
+
     /* disable the data alignment check */
     mrc p15, 0, r1, c1, c0, 0
     bic r1, #(1<<1)
@@ -176,6 +185,19 @@ vector_irq:
     stmfd   r0, {r13, r14}^ /* usr_sp, usr_lr */
     sub   r0, #8
 #endif
+#ifdef RT_USING_FPU
+    /* fpu context */
+    vmrs r6, fpexc
+    tst  r6, #(1<<30)
+    beq 1f
+    vstmdb r0!, {d0-d15}
+    vstmdb r0!, {d16-d31}
+    vmrs r5, fpscr
+    stmfd r0!, {r5}
+1:
+    stmfd r0!, {r6}
+#endif
+
     /* now irq stack is clean */
     /* r0 is task svc_sp */
     /* backup r0 -> r8 */
@@ -234,6 +256,18 @@ rt_hw_context_switch_interrupt_do:
     stmfd sp, {r13, r14}^  @push usr_sp, usr_lr
     sub sp, #8
 #endif
+#ifdef RT_USING_FPU
+    /* fpu context */
+    vmrs r6, fpexc
+    tst  r6, #(1<<30)
+    beq 1f
+    vstmdb sp!, {d0-d15}
+    vstmdb sp!, {d16-d31}
+    vmrs r5, fpscr
+    stmfd sp!, {r5}
+1:
+    stmfd sp!, {r6}
+#endif
 
     ldr     r4,  =rt_interrupt_from_thread
     ldr     r5,  [r4]
@@ -243,6 +277,19 @@ rt_hw_context_switch_interrupt_do:
     ldr     r6,  [r6]
     ldr     sp,  [r6]       @ get new task's stack pointer
 
+#ifdef RT_USING_FPU
+/* fpu context */
+    ldmfd sp!, {r6}
+    vmsr fpexc, r6
+    tst  r6, #(1<<30)
+    beq 1f
+    ldmfd sp!, {r5}
+    vmsr fpscr, r5
+    vldmia sp!, {d16-d31}
+    vldmia sp!, {d0-d15}
+1:
+#endif
+
 #ifdef RT_USING_LWP
     ldmfd sp, {r13, r14}^  @pop usr_sp, usr_lr
     add sp, #8
@@ -278,7 +325,14 @@ vector_swi:
     .globl  vector_undef
 vector_undef:
     push_svc_reg
+    cps #Mode_UND
     bl      rt_hw_trap_undef
+#ifdef RT_USING_FPU
+    ldr     lr, [sp, #15*4]
+    ldmia   sp, {r0 - r12}
+    add     sp, sp, #17 * 4
+    movs    pc, lr
+#endif
     b       .
 
     .align  5
@@ -315,6 +369,12 @@ set_secondary_cpu_boot_address:
 
 .global secondary_cpu_start
 secondary_cpu_start:
+
+#ifdef RT_USING_FPU
+    mov r4, #0xfffffff
+    mcr p15, 0, r4, c1, c0, 2
+#endif
+
     mrc p15, 0, r1, c1, c0, 1
     mov r0, #(1<<6)
     orr r1, r0
@@ -324,6 +384,11 @@ secondary_cpu_start:
     bic r0, #(1<<13)
     mcr p15, 0, r0, c1, c0, 0
 
+#ifdef RT_USING_FPU
+    cps #Mode_UND
+    ldr sp, =und_stack_2_limit
+#endif
+
     cps #Mode_IRQ
     ldr sp, =irq_stack_2_limit
 
@@ -349,3 +414,8 @@ irq_stack_2:
     .space (1 << 10)
 irq_stack_2_limit:
 
+#ifdef RT_USING_FPU
+und_stack_2:
+    .space (1 << 10)
+und_stack_2_limit:
+#endif
diff --git a/libcpu/arm/cortex-a/trap.c b/libcpu/arm/cortex-a/trap.c
index a25f6ac862..dc232a46ee 100644
--- a/libcpu/arm/cortex-a/trap.c
+++ b/libcpu/arm/cortex-a/trap.c
@@ -45,6 +45,39 @@ void rt_hw_show_register(struct rt_hw_exp_stack *regs)
  */
 void rt_hw_trap_undef(struct rt_hw_exp_stack *regs)
 {
+#ifdef RT_USING_FPU
+    {
+        uint32_t ins;
+        uint32_t addr;
+
+        if (regs->cpsr & (1 << 5))
+        {
+            /* thumb mode */
+            addr = regs->pc - 2;
+            ins = (uint32_t)*(uint16_t*)addr;
+            if ((ins & (3 << 11)) != 0)
+            {
+                /* 32 bit ins */
+                ins <<= 16;
+                ins += *(uint16_t*)(addr + 2);
+            }
+        }
+        else
+        {
+            addr = regs->pc - 4;
+            ins = *(uint32_t*)addr;
+        }
+        if ((ins & 0xe00) == 0xa00)
+        {
+            /* float ins */
+            uint32_t val = (1U << 30);
+
+            asm volatile ("vmsr fpexc, %0"::"r"(val):"memory");
+            regs->pc = addr;
+            return;
+        }
+    }
+#endif
     rt_kprintf("undefined instruction:\n");
     rt_hw_show_register(regs);
 #ifdef RT_USING_FINSH