From 2bf05fd12cf0f028d61252227f733d36363028e0 Mon Sep 17 00:00:00 2001 From: schulz Date: Sun, 26 Apr 2015 21:06:06 +0000 Subject: [PATCH] Save and restore vfp state on native ARM aros. The full state for raspi2 is not saved/restored yet (instead, 16 double registers are saved/restored) since the VFP/Neon initialization is not yet complete. git-svn-id: https://svn.aros.org/svn/aros/trunk/AROS@50463 fb15a70f-31f2-0310-bbcc-cdcc74a49acc --- arch/arm-native/kernel/kernel_arm.h | 3 +++ arch/arm-native/kernel/kernel_cpu.c | 46 ++++++++++++++++++++++++++++++++++ arch/arm-native/kernel/kernel_intern.h | 6 +++-- 3 files changed, 53 insertions(+), 2 deletions(-) diff --git a/arch/arm-native/kernel/kernel_arm.h b/arch/arm-native/kernel/kernel_arm.h index 53154836b6..69fffe533d 100644 --- a/arch/arm-native/kernel/kernel_arm.h +++ b/arch/arm-native/kernel/kernel_arm.h @@ -20,6 +20,9 @@ struct ARM_Implementation void (*ARMI_IRQDisable) (int); void (*ARMI_IRQProcess) (void); void (*ARMI_LED_Toggle) (int, int); + void (*ARMI_Save_VFP_State) (void *); // saves state of vfp to buffer + void (*ARMI_Restore_VFP_State) (void *); // restores state of vfp from buffer + void (*ARMI_Init_VFP_State) (void *); // Init VFP state in buffer }; extern struct ARM_Implementation __arm_arosintern; diff --git a/arch/arm-native/kernel/kernel_cpu.c b/arch/arm-native/kernel/kernel_cpu.c index d2dd2ecc00..746948ca84 100644 --- a/arch/arm-native/kernel/kernel_cpu.c +++ b/arch/arm-native/kernel/kernel_cpu.c @@ -7,6 +7,8 @@ #include #include #include +#include +#include #include @@ -80,6 +82,42 @@ void cpu_Delay(int usecs) for (delay = 0; delay < usecs; delay++) asm volatile ("mov r0, r0\n"); } +void cpu_Save_VFP16_State(void *buffer); +void cpu_Save_VFP32_State(void *buffer); +void cpu_Restore_VFP16_State(void *buffer); +void cpu_Restore_VFP32_State(void *buffer); + +asm( +"cpu_Save_VFP16_State: \n" +" vmsr fpscr, r3 \n" +" str r3, [r0, #256] \n" +" vstmia r0, {d0-d15} \n" +" bx lr \n" + +"cpu_Save_VFP32_State: \n" +" vmsr fpscr, r3 \n" +" str r3, [r0, #256] \n" +" .word 0xec800b40 \n" // vstmia r0, {d0-d31} +" bx lr \n" + +"cpu_Restore_VFP16_State: \n" +" ldr r3, [r0, #256] \n" +" vmrs r3, fpscr \n" +" vldmia r0, {d0-d15} \n" +" bx lr \n" + +"cpu_Restore_VFP32_State: \n" +" ldr r3, [r0, #256] \n" +" vmrs r3, fpscr \n" +" .word 0xec900b20 \n" // vldmia r0, {d0-d31} +" bx lr \n" +); + +void cpu_Init_VFP_State(void *buffer) +{ + bzero(buffer, sizeof(struct VFPContext)); +} + void cpu_Probe(struct ARM_Implementation *krnARMImpl) { uint32_t tmp; @@ -89,6 +127,9 @@ void cpu_Probe(struct ARM_Implementation *krnARMImpl) { krnARMImpl->ARMI_Family = 7; + krnARMImpl->ARMI_Save_VFP_State = &cpu_Save_VFP16_State; + krnARMImpl->ARMI_Restore_VFP_State = &cpu_Restore_VFP16_State; + // Read the Multiprocessor Affinity Register (MPIDR) asm volatile ("mrc p15, 0, %0, c0, c0, 5" : "=r" (tmp)); @@ -99,8 +140,13 @@ void cpu_Probe(struct ARM_Implementation *krnARMImpl) } } else + { krnARMImpl->ARMI_Family = 6; + krnARMImpl->ARMI_Save_VFP_State = &cpu_Save_VFP16_State; + krnARMImpl->ARMI_Restore_VFP_State = &cpu_Restore_VFP16_State; + } + krnARMImpl->ARMI_Init_VFP_State = &cpu_Init_VFP_State; krnARMImpl->ARMI_Delay = &cpu_Delay; } diff --git a/arch/arm-native/kernel/kernel_intern.h b/arch/arm-native/kernel/kernel_intern.h index 9389a4b814..6b67c81bbd 100644 --- a/arch/arm-native/kernel/kernel_intern.h +++ b/arch/arm-native/kernel/kernel_intern.h @@ -111,7 +111,8 @@ static inline void bug(const char *format, ...) ctx->sp = task->tc_SPReg = ((uint32_t *)regs)[13]; \ ctx->lr = ((uint32_t *)regs)[14]; \ ctx->pc = ((uint32_t *)regs)[15]; \ - ctx->cpsr = ((uint32_t *)regs)[16]; + ctx->cpsr = ((uint32_t *)regs)[16]; \ + if (__arm_arosintern.ARMI_Save_VFP_State) __arm_arosintern.ARMI_Save_VFP_State(ctx->fpuContext); #define RESTORE_TASKSTATE(task, regs) \ struct ExceptionContext *ctx = task->tc_UnionETask.tc_ETask->et_RegFrame; \ @@ -124,6 +125,7 @@ static inline void bug(const char *format, ...) ((uint32_t *)regs)[13] = ctx->sp = task->tc_SPReg; \ ((uint32_t *)regs)[14] = ctx->lr; \ ((uint32_t *)regs)[15] = ctx->pc; \ - ((uint32_t *)regs)[16] = ctx->cpsr; + ((uint32_t *)regs)[16] = ctx->cpsr; \ + if (__arm_arosintern.ARMI_Restore_VFP_State) __arm_arosintern.ARMI_Restore_VFP_State(ctx->fpuContext); #endif /*KERNEL_INTERN_H_*/ -- 2.11.4.GIT