diff --git a/src/system/kernel/arch/ppc/arch_asm.S b/src/system/kernel/arch/ppc/arch_asm.S index c254faaefb..de81e2c47b 100644 --- a/src/system/kernel/arch/ppc/arch_asm.S +++ b/src/system/kernel/arch/ppc/arch_asm.S @@ -192,9 +192,30 @@ FUNCTION(setl2cr): // void ppc_context_switch(addr_t *old_sp, addr_t new_sp); FUNCTION(ppc_context_switch): - // regs to push on the stack: r13-r31, cr, r2, lr + // regs to push on the stack: f13-f31, r13-r31, cr, r2, lr // push the old regs we need to save on the stack + // f31-13 + stfdu %f31, -8(%r1) + stfdu %f30, -8(%r1) + stfdu %f29, -8(%r1) + stfdu %f28, -8(%r1) + stfdu %f27, -8(%r1) + stfdu %f26, -8(%r1) + stfdu %f25, -8(%r1) + stfdu %f24, -8(%r1) + stfdu %f23, -8(%r1) + stfdu %f22, -8(%r1) + stfdu %f21, -8(%r1) + stfdu %f20, -8(%r1) + stfdu %f19, -8(%r1) + stfdu %f18, -8(%r1) + stfdu %f17, -8(%r1) + stfdu %f16, -8(%r1) + stfdu %f15, -8(%r1) + stfdu %f14, -8(%r1) + stfdu %f13, -8(%r1) + // r31-13, r2 stwu %r31, -4(%r1) stwu %r30, -4(%r1) @@ -258,7 +279,28 @@ FUNCTION(ppc_context_switch): lwzu %r30, 4(%r1) lwzu %r31, 4(%r1) - addi %r1, %r1, 4 + // f13-31 + lfdu %f13, 4(%r1) + lfdu %f14, 8(%r1) + lfdu %f15, 8(%r1) + lfdu %f16, 8(%r1) + lfdu %f17, 8(%r1) + lfdu %f18, 8(%r1) + lfdu %f19, 8(%r1) + lfdu %f20, 8(%r1) + lfdu %f21, 8(%r1) + lfdu %f22, 8(%r1) + lfdu %f23, 8(%r1) + lfdu %f24, 8(%r1) + lfdu %f25, 8(%r1) + lfdu %f26, 8(%r1) + lfdu %f27, 8(%r1) + lfdu %f28, 8(%r1) + lfdu %f29, 8(%r1) + lfdu %f30, 8(%r1) + lfdu %f31, 8(%r1) + + addi %r1, %r1, 8 blr diff --git a/src/system/kernel/arch/ppc/arch_cpu.cpp b/src/system/kernel/arch/ppc/arch_cpu.cpp index aabe3c55fb..7c69454a5f 100644 --- a/src/system/kernel/arch/ppc/arch_cpu.cpp +++ b/src/system/kernel/arch/ppc/arch_cpu.cpp @@ -19,6 +19,9 @@ static bool sHasTlbia; status_t arch_cpu_preboot_init(kernel_args *args) { + // enable FPU + set_msr(get_msr() | MSR_FP_AVAILABLE); + // The current thread must be NULL for all CPUs till we have threads. // Some boot code relies on this. arch_thread_set_current_thread(NULL); diff --git a/src/system/kernel/arch/ppc/arch_exceptions.S b/src/system/kernel/arch/ppc/arch_exceptions.S index c73770db2b..45027ab326 100644 --- a/src/system/kernel/arch/ppc/arch_exceptions.S +++ b/src/system/kernel/arch/ppc/arch_exceptions.S @@ -119,6 +119,8 @@ exception_vector_common: rlwinm %r0, %r0, 28, 30, 31 /* extract mmu bits */ mfmsr %r3 /* load the current msr */ rlwimi %r3, %r0, 4, 26, 27 /* merge the mmu bits with the current msr */ + li %r0, 1 + rlwimi %r3, %r0, 13, 18, 18 /* turn on FPU, too */ mtmsr %r3 /* load new msr (turning the mmu back on) */ isync @@ -194,6 +196,10 @@ FUNCTION(ppc_exception_tail): /* restore the CR, it was messed up in the previous compare */ mtcrf 0xff, %r0 + /* align r1 to 8 bytes, so the iframe will be aligned too */ + li %r0, 0xfffffff8 + and %r1, %r1, %r0 + /* save the registers */ bl __save_regs @@ -231,6 +237,46 @@ FUNCTION(ppc_exception_tail): * and ready to be saved */ __save_regs: + /* Note: The iframe must be 8 byte aligned. The stack pointer we are passed + in r1 is aligned. So we store the floating point registers first and + need to take care that an even number of 4 byte registers is stored, + or insert padding respectively. */ + + /* push f0-f31 */ + stfdu %f0, -8(%r1) + stfdu %f1, -8(%r1) + stfdu %f2, -8(%r1) + stfdu %f3, -8(%r1) + stfdu %f4, -8(%r1) + stfdu %f5, -8(%r1) + stfdu %f6, -8(%r1) + stfdu %f7, -8(%r1) + stfdu %f8, -8(%r1) + stfdu %f9, -8(%r1) + stfdu %f10, -8(%r1) + stfdu %f11, -8(%r1) + stfdu %f12, -8(%r1) + stfdu %f13, -8(%r1) + stfdu %f14, -8(%r1) + stfdu %f15, -8(%r1) + stfdu %f16, -8(%r1) + stfdu %f17, -8(%r1) + stfdu %f18, -8(%r1) + stfdu %f19, -8(%r1) + stfdu %f20, -8(%r1) + stfdu %f21, -8(%r1) + stfdu %f22, -8(%r1) + stfdu %f23, -8(%r1) + stfdu %f24, -8(%r1) + stfdu %f25, -8(%r1) + stfdu %f26, -8(%r1) + stfdu %f27, -8(%r1) + stfdu %f28, -8(%r1) + stfdu %f29, -8(%r1) + stfdu %f30, -8(%r1) + stfdu %f31, -8(%r1) + + /* push r0-r3 */ lwz %r0, 16(%r2) /* original r0 */ stwu %r0, -4(%r1) /* push r0 */ mfsprg1 %r0 /* original r1 */ @@ -271,6 +317,8 @@ __save_regs: stwu %r31, -4(%r1) /* save some of the other regs */ + mffs %f0 + stfs %f0, -4(%r1) /* push FPSCR */ mfctr %r0 stwu %r0, -4(%r1) /* push CTR */ mfxer %r0 @@ -313,6 +361,8 @@ __restore_regs_and_rfi: mtxer %r0 lwzu %r0, 4(%r1) /* CTR */ mtctr %r0 + lfs %f0, 4(%r1) /* FPSCR */ + mtfsf 0xff, %f0 lwzu %r31, 4(%r1) lwzu %r30, 4(%r1) @@ -343,6 +393,46 @@ __restore_regs_and_rfi: lwzu %r5, 4(%r1) lwzu %r4, 4(%r1) lwzu %r3, 4(%r1) + + /* Stop here, before we overwrite r1, and continue with the floating point + registers first. */ + addi %r2, %r1, 16 /* skip r3-r0 */ + + /* f31-f0 */ + lfd %f31, 0(%r2) + lfdu %f30, 8(%r2) + lfdu %f29, 8(%r2) + lfdu %f28, 8(%r2) + lfdu %f27, 8(%r2) + lfdu %f26, 8(%r2) + lfdu %f25, 8(%r2) + lfdu %f24, 8(%r2) + lfdu %f23, 8(%r2) + lfdu %f22, 8(%r2) + lfdu %f21, 8(%r2) + lfdu %f20, 8(%r2) + lfdu %f19, 8(%r2) + lfdu %f18, 8(%r2) + lfdu %f17, 8(%r2) + lfdu %f16, 8(%r2) + lfdu %f15, 8(%r2) + lfdu %f14, 8(%r2) + lfdu %f13, 8(%r2) + lfdu %f12, 8(%r2) + lfdu %f11, 8(%r2) + lfdu %f10, 8(%r2) + lfdu %f9, 8(%r2) + lfdu %f8, 8(%r2) + lfdu %f7, 8(%r2) + lfdu %f6, 8(%r2) + lfdu %f5, 8(%r2) + lfdu %f4, 8(%r2) + lfdu %f3, 8(%r2) + lfdu %f2, 8(%r2) + lfdu %f1, 8(%r2) + lfd %f0, 8(%r2) + + /* r2-r0 */ lwzu %r2, 4(%r1) lwz %r0, 8(%r1) lwz %r1, 4(%r1) diff --git a/src/system/kernel/arch/ppc/arch_thread.c b/src/system/kernel/arch/ppc/arch_thread.c index d84aab3cc5..8c82fe240b 100644 --- a/src/system/kernel/arch/ppc/arch_thread.c +++ b/src/system/kernel/arch/ppc/arch_thread.c @@ -136,8 +136,8 @@ arch_thread_init_kthread_stack(struct thread *t, int (*start_func)(void), kstackTop -= 2; kstackTop = (addr_t*)((addr_t)kstackTop & ~0xf); - // LR, CR, r2, r13-r31, as pushed by ppc_context_switch() - kstackTop -= 22; + // LR, CR, r2, r13-r31, f13-f31, as pushed by ppc_context_switch() + kstackTop -= 22 + 2 * 19; // let LR point to ppc_kernel_thread_root() kstackTop[0] = (addr_t)&ppc_kernel_thread_root;