Sync from master (75a3f3dd3a930eab1dc58fa0258a9d1605a0ac11)

6 files changed, 143 insertions, 0 deletions

diff --git a/libcpu/arm/cortex-a/cache.c b/libcpu/arm/cortex-a/cache.c
index 6d28d9e9f9..30af86baa1 100644
--- a/libcpu/arm/cortex-a/cache.c
+++ b/libcpu/arm/cortex-a/cache.c
@@ -30,6 +30,7 @@ void rt_hw_cpu_icache_invalidate(void *addr, int size)
     rt_uint32_t start_addr = (rt_uint32_t)addr;
     rt_uint32_t end_addr = (rt_uint32_t) addr + size + line_size - 1;
 
+    asm volatile ("dmb":::"memory");
     start_addr &= ~(line_size-1);
     end_addr &= ~(line_size-1);
     while (start_addr < end_addr)
@@ -37,6 +38,7 @@ void rt_hw_cpu_icache_invalidate(void *addr, int size)
         asm volatile ("mcr p15, 0, %0, c7, c5, 1" :: "r"(start_addr));  /* icimvau */
         start_addr += line_size;
     }
+    asm volatile ("dsb\n\tisb":::"memory");
 }
 
 void rt_hw_cpu_dcache_invalidate(void *addr, int size)
@@ -45,6 +47,7 @@ void rt_hw_cpu_dcache_invalidate(void *addr, int size)
     rt_uint32_t start_addr = (rt_uint32_t)addr;
     rt_uint32_t end_addr = (rt_uint32_t) addr + size + line_size - 1;
 
+    asm volatile ("dmb":::"memory");
     start_addr &= ~(line_size-1);
     end_addr &= ~(line_size-1);
     while (start_addr < end_addr)
@@ -52,6 +55,7 @@ void rt_hw_cpu_dcache_invalidate(void *addr, int size)
         asm volatile ("mcr p15, 0, %0, c7, c6, 1" :: "r"(start_addr));  /* dcimvac */
         start_addr += line_size;
     }
+    asm volatile ("dsb":::"memory");
 }
 
 void rt_hw_cpu_dcache_clean(void *addr, int size)
@@ -60,6 +64,7 @@ void rt_hw_cpu_dcache_clean(void *addr, int size)
     rt_uint32_t start_addr = (rt_uint32_t)addr;
     rt_uint32_t end_addr = (rt_uint32_t) addr + size + line_size - 1;
 
+    asm volatile ("dmb":::"memory");
     start_addr &= ~(line_size-1);
     end_addr &= ~(line_size-1);
     while (start_addr < end_addr)
@@ -67,6 +72,7 @@ void rt_hw_cpu_dcache_clean(void *addr, int size)
         asm volatile ("mcr p15, 0, %0, c7, c10, 1" :: "r"(start_addr)); /* dccmvac */
         start_addr += line_size;
     }
+    asm volatile ("dsb":::"memory");
 }
 
 void rt_hw_cpu_icache_ops(int ops, void *addr, int size)
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/cpu.c b/libcpu/arm/cortex-a/cpu.c
index 6b129f89ee..7c6bf58a4b 100644
--- a/libcpu/arm/cortex-a/cpu.c
+++ b/libcpu/arm/cortex-a/cpu.c
@@ -14,6 +14,7 @@
 #include <board.h>
 
 #ifdef RT_USING_SMP
+
 int rt_hw_cpu_id(void)
 {
     int cpu_id;
@@ -25,6 +26,11 @@ int rt_hw_cpu_id(void)
     return cpu_id;
 };
 
+void rt_hw_spin_lock_init(rt_hw_spinlock_t *lock)
+{
+    lock->slock = 0;
+}
+
 void rt_hw_spin_lock(rt_hw_spinlock_t *lock)
 {
     unsigned long tmp;
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..268cc7ef56 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
@@ -37,6 +41,7 @@
 .globl stack_start
 .globl stack_top
 
+.align 3
 stack_start:
 .rept ISR_Stack_Size
 .byte 0
@@ -50,6 +55,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 +186,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 +257,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 +278,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 +326,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 +370,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 +385,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 +415,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