add cortex-a fpu support
This commit is contained in:
parent
91f6abb765
commit
043611b98a
|
@ -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 */
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue