diff --git a/trunk/bootrom/build/libraries/os/common/os_context.c b/trunk/bootrom/build/libraries/os/common/os_context.c index faf7101..f368671 100644 --- a/trunk/bootrom/build/libraries/os/common/os_context.c +++ b/trunk/bootrom/build/libraries/os/common/os_context.c @@ -86,6 +86,31 @@ asm void osInitContext( #if defined(SDK_TCM_APPLY) && defined(SDK_ARM9) #include #endif + +#ifdef SDK_CONTEXT_HAS_VFP +/*---------------------------------------------------------------------------* + Name: i_osSaveContextVFP + + Description: Save current context into specified memory + + Arguments: context pointer to the memory to be stored the current context + + Returns: None + *---------------------------------------------------------------------------*/ +asm BOOL i_osSaveContextVFP( OSContext* context ) +{ + add r1, r0, #__cpp(offsetof(OSContext,fpexc)) + fmrx r2, fpexc // VFPイネーブルレジスタ保存 + stmia r1!, {r2} + tst r2, #HW_FPEXC_VFP_ENABLE + fmrxne r2, fpscr // VFPコントロールレジスタ保存（VFPイネーブル時） + stmiane r1!, {r2} + fstmfdsne r1!, {f0-f31} // VFPレジスタの保存（VFPイネーブル時） + bx lr +} +#endif // SDK_CONTEXT_HAS_VFP + + /*---------------------------------------------------------------------------* Name: osSaveContext @@ -96,13 +121,11 @@ asm void osInitContext( Returns: 0 saving a context (normal) 1 if context are reloaded via osLoadContext *---------------------------------------------------------------------------*/ -asm BOOL osSaveContext( register OSContext* context ) +asm BOOL osSaveContext( OSContext* context ) { -#if 0 // defined(SDK_ARM9) +#ifdef SDK_CONTEXT_HAS_VFP stmfd sp!, { lr, r0 } - add r0, r0, #OS_CONTEXT_CP_CONTEXT - ldr r1, =CP_SaveContext - blx r1 + bl i_osSaveContextVFP ldmfd sp!, { lr, r0 } #endif @@ -133,6 +156,32 @@ asm BOOL osSaveContext( register OSContext* context ) #define OFFSETOF(x,y) (int)(&(((x*)0)->y)) +#ifdef SDK_CONTEXT_HAS_VFP +/*---------------------------------------------------------------------------* + Name: i_osLoadContextVFP + + Description: Reload specified context as current context + + Arguments: context pointer to the memory to switch to the context + + Returns: None + *---------------------------------------------------------------------------*/ +asm void i_osLoadContextVFP( OSContext* context ) +{ + mov r2, #HW_FPEXC_VFP_ENABLE + fmxr fpexc, r2 // VFPを一時的に強制イネーブル + ldr r2, [r0,#__cpp(offsetof(OSContext,fpscr))] // VFPコントロールレジスタ復帰 + fmxr fpscr, r2 + add r1, r0, #__cpp(offsetof(OSContext,fpexc)) // VFPイネーブルレジスタ復帰 + ldmia r1!, {r2} + fmxr fpexc, r2 + tst r2, #HW_FPEXC_VFP_ENABLE + fldmfdsne r1!, {f0-f31} + bx lr +} +#endif // SDK_CONTEXT_HAS_VFP + + /*---------------------------------------------------------------------------* Name: osLoadContext @@ -142,23 +191,14 @@ asm BOOL osSaveContext( register OSContext* context ) Returns: None *---------------------------------------------------------------------------*/ -asm void osLoadContext( register OSContext* context ) +asm void osLoadContext( OSContext* context ) { -#if OS_CONTEXT_CPSR != 0 -#pragma message(has changed!!!) - add r0, r0, #OS_CONTEXT_CPSR +#ifdef SDK_CONTEXT_HAS_VFP + stmfd sp!, { lr, r0 } + bl i_osLoadContextVFP + ldmfd sp!, { lr, r0 } #endif - -#if 0 // defined(SDK_ARM9) - // call CPi_RestoreContext - stmfd sp!, { lr, r0 } - add r0, r0, #OS_CONTEXT_CP_CONTEXT - ldr r1, =CPi_RestoreContext - blx r1 - ldmfd sp!, { lr, r0 } -#endif - //---- モードを svc に mrs r1, cpsr bic r1, r1, #HW_PSR_CPU_MODE_MASK @@ -178,19 +218,6 @@ asm void osLoadContext( register OSContext* context ) ldr lr, [ r0, #OS_CONTEXT_PC_PLUS4 - OS_CONTEXT_R0 ] ldmia r0, { r0 - r14 }^ nop - -#if defined(SDK_ARM9) -#if 0 // don't need, because still spend more than 34 cycle for divider. - //---- CP_WaitDiv - stmfd sp!, { r0-r1 } - ldr r0, =REG_DIVCNT_ADDR -@00: - ldr r1, [r0] - and r1, r1, #REG_CP_DIVCNT_BUSY_MASK - bne @00 - ldmfd sp!, { r0-r1 } -#endif -#endif //---- Switch cpsr and Jump to (context->pc_plus4 - 4) subs pc, lr, #4 diff --git a/trunk/bootrom/build/libraries/os/common/os_irqHandler.c b/trunk/bootrom/build/libraries/os/common/os_irqHandler.c index a02d018..c2549e8 100644 --- a/trunk/bootrom/build/libraries/os/common/os_irqHandler.c +++ b/trunk/bootrom/build/libraries/os/common/os_irqHandler.c @@ -128,6 +128,8 @@ asm void osIrqHandler_ThreadSwitch(void) { INASM_EXTERN( i_osIrqThreadQueue ) INASM_EXTERN( i_osThreadInfo ) + INASM_EXTERN( i_osSaveContextVFP ) + INASM_EXTERN( i_osLoadContextVFP ) //-------------------------------------------------- // wakeup threads in i_osIrqThreadQueue @@ -241,6 +243,14 @@ LSYM(13) mrs r2, SPSR str r2, [ r0, #OS_THREAD_OFFSET_CONTEXT ]! // *r0=context:CPSR +#ifdef SDK_CONTEXT_HAS_VFP + // first, save CP context + stmfd sp!, { r0, r1 } + add r0, r0, #OS_THREAD_OFFSET_CONTEXT + bl i_osSaveContextVFP + ldmfd sp!, { r0, r1 } +#endif + ldmib sp!, { r2,r3 } // Get R0,R1 // *sp=stack:R1 stmib r0!, { r2,r3 } // Put R0,R1 // *r0=context:R1 @@ -254,6 +264,14 @@ LSYM(13) #endif // ---- OS_LoadContext +#ifdef SDK_CONTEXT_HAS_VFP + // first, load CP context + stmfd sp!, { r1 } + add r0, r1, #OS_THREAD_OFFSET_CONTEXT + bl i_osLoadContextVFP + ldmfd sp!, { r1 } +#endif + #ifdef SDK_CONTEXT_HAS_SP_SVC ldr sp, [ r1, #OS_THREAD_OFFSET_CONTEXT+OS_CONTEXT_SP_SVC ] mov r3, #HW_PSR_IRQ_MODE|HW_PSR_FIQ_DISABLE|HW_PSR_IRQ_DISABLE|HW_PSR_ARM_STATE diff --git a/trunk/bootrom/include/brom/os/common/context.h b/trunk/bootrom/include/brom/os/common/context.h index 2bf16c1..22288bf 100644 --- a/trunk/bootrom/include/brom/os/common/context.h +++ b/trunk/bootrom/include/brom/os/common/context.h @@ -28,6 +28,9 @@ extern "C" { //---------------------------------------------------------------------------- #define SDK_CONTEXT_HAS_SP_SVC +#ifdef SDK_ARM11 +#define SDK_CONTEXT_HAS_VFP +#endif // SDK_ARM11 //---------------------------------------------------------------------------- @@ -57,15 +60,6 @@ extern "C" { #define OS_CONTEXT_SP_SVC OS_CONTEXT_R13_SVC #endif -#ifdef SDK_ARM9 -#ifdef SDK_CONTEXT_HAS_SP_SVC -#define OS_CONTEXT_CP_CONTEXT 72 -#else -#define OS_CONTEXT_CP_CONTEXT 68 -#endif -#endif - - typedef struct OSContext { u32 cpsr; @@ -76,6 +70,11 @@ typedef struct OSContext #ifdef SDK_CONTEXT_HAS_SP_SVC u32 sp_svc; #endif +#ifdef SDK_CONTEXT_HAS_VFP + u32 fpexc; + u32 fpscr; + u32 f[32]; +#endif } OSContext;