OmniNX/Atmosphere
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

kern: implement new thread context/fpu semantics

Browse Source
This commit is contained in:
Michael Scire
2022-03-22 19:59:39 -07:00
committed by SciresM
parent 79afa3b64c
commit 6e17317d5d
21 changed files with 1291 additions and 668 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
mesosphere/kernel/source/arch/arm64/exception_vectors.s
View File

@@ -125,7 +125,7 @@ vector_entry synch_a64
vector_entry irq_a64
clrex
b _ZN3ams4kern4arch5arm6422EL0IrqExceptionHandlerEv
b _ZN3ams4kern4arch5arm6425EL0A64IrqExceptionHandlerEv
check_vector_size irq_a64
vector_entry fiq_a64
@@ -148,7 +148,7 @@ vector_entry synch_a32
vector_entry irq_a32
clrex
b _ZN3ams4kern4arch5arm6422EL0IrqExceptionHandlerEv
b _ZN3ams4kern4arch5arm6425EL0A32IrqExceptionHandlerEv
check_vector_size irq_a32
vector_entry fiq_a32

208
mesosphere/kernel/source/arch/arm64/kern_exception_handlers_asm.s
View File

@@ -13,7 +13,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <mesosphere/kern_select_assembly_offsets.h>
#include <mesosphere/kern_select_assembly_macros.h>
/* ams::kern::arch::arm64::EL1IrqExceptionHandler() */
.section .text._ZN3ams4kern4arch5arm6422EL1IrqExceptionHandlerEv, "ax", %progbits
@@ -66,11 +66,11 @@ _ZN3ams4kern4arch5arm6422EL1IrqExceptionHandlerEv:
/* Return from the exception. */
eret
/* ams::kern::arch::arm64::EL0IrqExceptionHandler() */
.section .text._ZN3ams4kern4arch5arm6422EL0IrqExceptionHandlerEv, "ax", %progbits
.global _ZN3ams4kern4arch5arm6422EL0IrqExceptionHandlerEv
.type _ZN3ams4kern4arch5arm6422EL0IrqExceptionHandlerEv, %function
_ZN3ams4kern4arch5arm6422EL0IrqExceptionHandlerEv:
/* ams::kern::arch::arm64::EL0A64IrqExceptionHandler() */
.section .text._ZN3ams4kern4arch5arm6425EL0A64IrqExceptionHandlerEv, "ax", %progbits
.global _ZN3ams4kern4arch5arm6425EL0A64IrqExceptionHandlerEv
.type _ZN3ams4kern4arch5arm6425EL0A64IrqExceptionHandlerEv, %function
_ZN3ams4kern4arch5arm6425EL0A64IrqExceptionHandlerEv:
/* Save registers that need saving. */
sub sp, sp, #(EXCEPTION_CONTEXT_SIZE)
@@ -105,7 +105,18 @@ _ZN3ams4kern4arch5arm6422EL0IrqExceptionHandlerEv:
mov x0, #1
bl _ZN3ams4kern4arch5arm6417KInterruptManager15HandleInterruptEb
/* Restore state from the context. */
/* If we don't need to restore the fpu, skip restoring it. */
ldrb w1, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_EXCEPTION_FLAGS)]
tbz w1, #(THREAD_EXCEPTION_FLAG_BIT_INDEX_IS_FPU_CONTEXT_RESTORE_NEEDED), 1f
/* Clear the needs-fpu-restore flag. */
and w1, w1, #(~THREAD_EXCEPTION_FLAG_IS_FPU_CONTEXT_RESTORE_NEEDED)
strb w1, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_EXCEPTION_FLAGS)]
/* Perform a full fpu restore. */
ENABLE_AND_RESTORE_FPU64(x2, x0, x1, w0, w1)
1: /* Restore state from the context. */
ldp x30, x20, [sp, #(EXCEPTION_CONTEXT_X30_SP)]
ldp x21, x22, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
ldr x23, [sp, #(EXCEPTION_CONTEXT_TPIDR)]
@@ -141,6 +152,74 @@ _ZN3ams4kern4arch5arm6422EL0IrqExceptionHandlerEv:
/* Return from the exception. */
eret
/* ams::kern::arch::arm64::EL0A32IrqExceptionHandler() */
.section .text._ZN3ams4kern4arch5arm6425EL0A32IrqExceptionHandlerEv, "ax", %progbits
.global _ZN3ams4kern4arch5arm6425EL0A32IrqExceptionHandlerEv
.type _ZN3ams4kern4arch5arm6425EL0A32IrqExceptionHandlerEv, %function
_ZN3ams4kern4arch5arm6425EL0A32IrqExceptionHandlerEv:
/* Save registers that need saving. */
sub sp, sp, #(EXCEPTION_CONTEXT_SIZE)
stp x0, x1, [sp, #(EXCEPTION_CONTEXT_X0_X1)]
stp x2, x3, [sp, #(EXCEPTION_CONTEXT_X2_X3)]
stp x4, x5, [sp, #(EXCEPTION_CONTEXT_X4_X5)]
stp x6, x7, [sp, #(EXCEPTION_CONTEXT_X6_X7)]
stp x8, x9, [sp, #(EXCEPTION_CONTEXT_X8_X9)]
stp x10, x11, [sp, #(EXCEPTION_CONTEXT_X10_X11)]
stp x12, x13, [sp, #(EXCEPTION_CONTEXT_X12_X13)]
stp x14, x15, [sp, #(EXCEPTION_CONTEXT_X14_X15)]
mrs x21, elr_el1
mrs x22, spsr_el1
mrs x23, tpidr_el0
mov w22, w22
stp x21, x22, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
str x23, [sp, #(EXCEPTION_CONTEXT_TPIDR)]
/* Invoke KInterruptManager::HandleInterrupt(bool user_mode). */
ldr x18, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_CUR_THREAD)]
mov x0, #1
bl _ZN3ams4kern4arch5arm6417KInterruptManager15HandleInterruptEb
/* If we don't need to restore the fpu, skip restoring it. */
ldrb w1, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_EXCEPTION_FLAGS)]
tbz w1, #(THREAD_EXCEPTION_FLAG_BIT_INDEX_IS_FPU_CONTEXT_RESTORE_NEEDED), 1f
/* Clear the needs-fpu-restore flag. */
and w1, w1, #(~THREAD_EXCEPTION_FLAG_IS_FPU_CONTEXT_RESTORE_NEEDED)
strb w1, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_EXCEPTION_FLAGS)]
/* Perform a full fpu restore. */
ENABLE_AND_RESTORE_FPU32(x2, x0, x1, w0, w1)
1: /* Restore state from the context. */
ldp x21, x22, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
ldr x23, [sp, #(EXCEPTION_CONTEXT_TPIDR)]
#if defined(MESOSPHERE_ENABLE_HARDWARE_SINGLE_STEP)
/* Since we're returning from an exception, set SPSR.SS so that we advance an instruction if single-stepping. */
orr x22, x22, #(1 << 21)
#endif
msr elr_el1, x21
msr spsr_el1, x22
msr tpidr_el0, x23
ldp x0, x1, [sp, #(EXCEPTION_CONTEXT_X0_X1)]
ldp x2, x3, [sp, #(EXCEPTION_CONTEXT_X2_X3)]
ldp x4, x5, [sp, #(EXCEPTION_CONTEXT_X4_X5)]
ldp x6, x7, [sp, #(EXCEPTION_CONTEXT_X6_X7)]
ldp x8, x9, [sp, #(EXCEPTION_CONTEXT_X8_X9)]
ldp x10, x11, [sp, #(EXCEPTION_CONTEXT_X10_X11)]
ldp x12, x13, [sp, #(EXCEPTION_CONTEXT_X12_X13)]
ldp x14, x15, [sp, #(EXCEPTION_CONTEXT_X14_X15)]
add sp, sp, #(EXCEPTION_CONTEXT_SIZE)
/* Return from the exception. */
eret
/* ams::kern::arch::arm64::EL0SynchronousExceptionHandler() */
.section .text._ZN3ams4kern4arch5arm6430EL0SynchronousExceptionHandlerEv, "ax", %progbits
.global _ZN3ams4kern4arch5arm6430EL0SynchronousExceptionHandlerEv
@@ -155,23 +234,23 @@ _ZN3ams4kern4arch5arm6430EL0SynchronousExceptionHandlerEv:
/* Is this an aarch32 SVC? */
cmp x17, #0x11
b.eq 2f
b.eq 4f
/* Is this an aarch64 SVC? */
cmp x17, #0x15
b.eq 3f
b.eq 5f
/* Is this an FPU error? */
cmp x17, #0x7
b.eq 4f
b.eq 6f
/* Is this a data abort? */
cmp x17, #0x24
b.eq 5f
b.eq 7f
/* Is this an instruction abort? */
cmp x17, #0x20
b.eq 5f
b.eq 7f
1: /* The exception is not a data abort or instruction abort caused by a TLB conflict. */
/* It is also not an SVC or an FPU exception. Handle it generically! */
@@ -212,6 +291,17 @@ _ZN3ams4kern4arch5arm6430EL0SynchronousExceptionHandlerEv:
mov x0, sp
bl _ZN3ams4kern4arch5arm6415HandleExceptionEPNS2_17KExceptionContextE
/* If we don't need to restore the fpu, skip restoring it. */
ldrb w1, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_EXCEPTION_FLAGS)]
tbz w1, #(THREAD_EXCEPTION_FLAG_BIT_INDEX_IS_FPU_CONTEXT_RESTORE_NEEDED), 3f
/* Clear the needs-fpu-restore flag. */
and w1, w1, #(~THREAD_EXCEPTION_FLAG_IS_FPU_CONTEXT_RESTORE_NEEDED)
strb w1, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_EXCEPTION_FLAGS)]
/* Enable and restore the fpu. */
ENABLE_AND_RESTORE_FPU(x2, x0, x1, w0, w1, 2, 3)
/* Restore state from the context. */
ldp x30, x20, [sp, #(EXCEPTION_CONTEXT_X30_SP)]
ldp x21, x22, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
@@ -243,19 +333,19 @@ _ZN3ams4kern4arch5arm6430EL0SynchronousExceptionHandlerEv:
/* Return from the exception. */
eret
2: /* SVC from aarch32. */
4: /* SVC from aarch32. */
ldp x16, x17, [sp], 16
b _ZN3ams4kern4arch5arm6412SvcHandler32Ev
3: /* SVC from aarch64. */
5: /* SVC from aarch64. */
ldp x16, x17, [sp], 16
b _ZN3ams4kern4arch5arm6412SvcHandler64Ev
4: /* FPU exception. */
6: /* FPU exception. */
ldp x16, x17, [sp], 16
b _ZN3ams4kern4arch5arm6425FpuAccessExceptionHandlerEv
5: /* Check if there's a TLB conflict that caused the abort. */
7: /* Check if there's a TLB conflict that caused the abort. */
and x17, x16, #0x3F
cmp x17, #0x30
b.ne 1b
@@ -265,20 +355,20 @@ _ZN3ams4kern4arch5arm6430EL0SynchronousExceptionHandlerEv:
and x17, x17, #(0xFFFF << 48)
/* Check if FAR is valid by examining the FnV bit. */
tbnz x16, #10, 6f
tbnz x16, #10, 8f
/* FAR is valid, so we can invalidate the address it holds. */
mrs x16, far_el1
lsr x16, x16, #12
orr x17, x16, x17
tlbi vae1, x17
b 7f
b 9f
6: /* There's a TLB conflict and FAR isn't valid. */
8: /* There's a TLB conflict and FAR isn't valid. */
/* Invalidate the entire TLB. */
tlbi aside1, x17
7: /* Return from a TLB conflict. */
9: /* Return from a TLB conflict. */
/* Ensure instruction consistency. */
dsb ish
isb
@@ -304,11 +394,11 @@ _ZN3ams4kern4arch5arm6430EL1SynchronousExceptionHandlerEv:
/* Is this an instruction abort? */
cmp x0, #0x21
b.eq 5f
b.eq 4f
/* Is this a data abort? */
cmp x0, #0x25
b.eq 5f
b.eq 4f
1: /* The exception is not a data abort or instruction abort caused by a TLB conflict. */
/* Load the exception stack top from otherwise "unused" virtual timer compare value. */
@@ -331,16 +421,16 @@ _ZN3ams4kern4arch5arm6430EL1SynchronousExceptionHandlerEv:
mrs x0, esr_el1
lsr x1, x0, #0x1a
cmp x1, #0x25
b.ne 3f
b.ne 2f
/* Data abort. Check if it was from trying to access userspace memory. */
mrs x1, elr_el1
adr x0, _ZN3ams4kern4arch5arm6432UserspaceAccessFunctionAreaBeginEv
cmp x1, x0
b.lo 3f
b.lo 2f
adr x0, _ZN3ams4kern4arch5arm6430UserspaceAccessFunctionAreaEndEv
cmp x1, x0
b.hs 3f
b.hs 2f
/* We aborted trying to access userspace memory. */
/* All functions that access user memory return a boolean for whether they succeeded. */
@@ -353,7 +443,7 @@ _ZN3ams4kern4arch5arm6430EL1SynchronousExceptionHandlerEv:
msr elr_el1, x30
eret
3: /* The exception wasn't an triggered by copying memory from userspace. */
2: /* The exception wasn't an triggered by copying memory from userspace. */
ldr x0, [sp, #8]
ldr x1, [sp, #16]
@@ -390,10 +480,10 @@ _ZN3ams4kern4arch5arm6430EL1SynchronousExceptionHandlerEv:
mov x0, sp
bl _ZN3ams4kern4arch5arm6415HandleExceptionEPNS2_17KExceptionContextE
4: /* HandleException should never return. The best we can do is infinite loop. */
b 4b
3: /* HandleException should never return. The best we can do is infinite loop. */
b 3b
5: /* Check if there's a TLB conflict that caused the abort. */
4: /* Check if there's a TLB conflict that caused the abort. */
/* NOTE: There is a Nintendo bug in this code that we correct. */
/* Nintendo compares the low 6 bits of x0 without restoring the value. */
/* They intend to check the DFSC/IFSC bits of esr_el1, but because they */
@@ -408,19 +498,19 @@ _ZN3ams4kern4arch5arm6430EL1SynchronousExceptionHandlerEv:
/* They do not refresh the value of x0, and again compare with */
/* the relevant bit already masked out of x0. */
mrs x0, esr_el1
tbnz x0, #10, 6f
tbnz x0, #10, 5f
/* FAR is valid, so we can invalidate the address it holds. */
mrs x0, far_el1
lsr x0, x0, #12
tlbi vaae1, x0
b 7f
b 6f
6: /* There's a TLB conflict and FAR isn't valid. */
5: /* There's a TLB conflict and FAR isn't valid. */
/* Invalidate the entire TLB. */
tlbi vmalle1
7: /* Return from a TLB conflict. */
6: /* Return from a TLB conflict. */
/* Ensure instruction consistency. */
dsb ish
isb
@@ -437,52 +527,17 @@ _ZN3ams4kern4arch5arm6430EL1SynchronousExceptionHandlerEv:
.global _ZN3ams4kern4arch5arm6425FpuAccessExceptionHandlerEv
.type _ZN3ams4kern4arch5arm6425FpuAccessExceptionHandlerEv, %function
_ZN3ams4kern4arch5arm6425FpuAccessExceptionHandlerEv:
/* Save registers that need saving. */
/* Save registers. */
sub sp, sp, #(EXCEPTION_CONTEXT_SIZE)
stp x0, x1, [sp, #(EXCEPTION_CONTEXT_X0_X1)]
stp x2, x3, [sp, #(EXCEPTION_CONTEXT_X2_X3)]
stp x4, x5, [sp, #(EXCEPTION_CONTEXT_X4_X5)]
stp x6, x7, [sp, #(EXCEPTION_CONTEXT_X6_X7)]
stp x8, x9, [sp, #(EXCEPTION_CONTEXT_X8_X9)]
stp x10, x11, [sp, #(EXCEPTION_CONTEXT_X10_X11)]
stp x12, x13, [sp, #(EXCEPTION_CONTEXT_X12_X13)]
stp x14, x15, [sp, #(EXCEPTION_CONTEXT_X14_X15)]
stp x16, x17, [sp, #(EXCEPTION_CONTEXT_X16_X17)]
stp x18, x19, [sp, #(EXCEPTION_CONTEXT_X18_X19)]
stp x20, x21, [sp, #(EXCEPTION_CONTEXT_X20_X21)]
mrs x19, sp_el0
mrs x20, elr_el1
mrs x21, spsr_el1
mov w21, w21
stp x30, x19, [sp, #(EXCEPTION_CONTEXT_X30_SP)]
stp x20, x21, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
/* Invoke the FPU context switch handler. */
ldr x18, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_CUR_THREAD)]
bl _ZN3ams4kern4arch5arm6423FpuContextSwitchHandlerEv
/* Restore registers that we saved. */
ldp x30, x19, [sp, #(EXCEPTION_CONTEXT_X30_SP)]
ldp x20, x21, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]
msr sp_el0, x19
msr elr_el1, x20
msr spsr_el1, x21
ENABLE_AND_RESTORE_FPU(x2, x0, x1, w0, w1, 1, 2)
/* Restore registers. */
ldp x0, x1, [sp, #(EXCEPTION_CONTEXT_X0_X1)]
ldp x2, x3, [sp, #(EXCEPTION_CONTEXT_X2_X3)]
ldp x4, x5, [sp, #(EXCEPTION_CONTEXT_X4_X5)]
ldp x6, x7, [sp, #(EXCEPTION_CONTEXT_X6_X7)]
ldp x8, x9, [sp, #(EXCEPTION_CONTEXT_X8_X9)]
ldp x10, x11, [sp, #(EXCEPTION_CONTEXT_X10_X11)]
ldp x12, x13, [sp, #(EXCEPTION_CONTEXT_X12_X13)]
ldp x14, x15, [sp, #(EXCEPTION_CONTEXT_X14_X15)]
ldp x16, x17, [sp, #(EXCEPTION_CONTEXT_X16_X17)]
ldp x18, x19, [sp, #(EXCEPTION_CONTEXT_X18_X19)]
ldp x20, x21, [sp, #(EXCEPTION_CONTEXT_X20_X21)]
add sp, sp, #(EXCEPTION_CONTEXT_SIZE)
@@ -585,6 +640,17 @@ _ZN3ams4kern4arch5arm6421EL0SystemErrorHandlerEv:
mov x0, sp
bl _ZN3ams4kern4arch5arm6415HandleExceptionEPNS2_17KExceptionContextE
/* If we don't need to restore the fpu, skip restoring it. */
ldrb w1, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_EXCEPTION_FLAGS)]
tbz w1, #(THREAD_EXCEPTION_FLAG_BIT_INDEX_IS_FPU_CONTEXT_RESTORE_NEEDED), 2f
/* Clear the needs-fpu-restore flag. */
and w1, w1, #(~THREAD_EXCEPTION_FLAG_IS_FPU_CONTEXT_RESTORE_NEEDED)
strb w1, [sp, #(EXCEPTION_CONTEXT_SIZE + THREAD_STACK_PARAMETERS_EXCEPTION_FLAGS)]
/* Enable and restore the fpu. */
ENABLE_AND_RESTORE_FPU(x2, x0, x1, w0, w1, 1, 2)
/* Restore state from the context. */
ldp x30, x20, [sp, #(EXCEPTION_CONTEXT_X30_SP)]
ldp x21, x22, [sp, #(EXCEPTION_CONTEXT_PC_PSR)]

254
mesosphere/kernel/source/arch/arm64/kern_k_scheduler_asm.s
View File

@@ -13,91 +13,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <mesosphere/kern_select_assembly_offsets.h>
#define SAVE_THREAD_CONTEXT(ctx, tmp0, tmp1, done_label) \
/* Save the callee save registers + SP and cpacr. */ \
mov tmp0, sp; \
mrs tmp1, cpacr_el1; \
stp x19, x20, [ctx, #(THREAD_CONTEXT_X19_X20)]; \
stp x21, x22, [ctx, #(THREAD_CONTEXT_X21_X22)]; \
stp x23, x24, [ctx, #(THREAD_CONTEXT_X23_X24)]; \
stp x25, x26, [ctx, #(THREAD_CONTEXT_X25_X26)]; \
stp x27, x28, [ctx, #(THREAD_CONTEXT_X27_X28)]; \
stp x29, x30, [ctx, #(THREAD_CONTEXT_X29_X30)]; \
\
stp tmp0, tmp1, [ctx, #(THREAD_CONTEXT_SP_CPACR)]; \
\
/* Check whether the FPU is enabled. */ \
/* If it isn't, skip saving FPU state. */ \
and tmp1, tmp1, #0x300000; \
cbz tmp1, done_label; \
\
/* Save fpcr and fpsr. */ \
mrs tmp0, fpcr; \
mrs tmp1, fpsr; \
stp tmp0, tmp1, [ctx, #(THREAD_CONTEXT_FPCR_FPSR)]; \
\
/* Save the FPU registers. */ \
stp q0, q1, [ctx, #(16 * 0 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q2, q3, [ctx, #(16 * 2 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q4, q5, [ctx, #(16 * 4 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q6, q7, [ctx, #(16 * 6 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q8, q9, [ctx, #(16 * 8 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q10, q11, [ctx, #(16 * 10 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q12, q13, [ctx, #(16 * 12 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q14, q15, [ctx, #(16 * 14 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q16, q17, [ctx, #(16 * 16 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q18, q19, [ctx, #(16 * 18 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q20, q21, [ctx, #(16 * 20 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q22, q23, [ctx, #(16 * 22 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q24, q25, [ctx, #(16 * 24 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q26, q27, [ctx, #(16 * 26 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q28, q29, [ctx, #(16 * 28 + THREAD_CONTEXT_FPU_REGISTERS)]; \
stp q30, q31, [ctx, #(16 * 30 + THREAD_CONTEXT_FPU_REGISTERS)];
#define RESTORE_THREAD_CONTEXT(ctx, tmp0, tmp1, done_label) \
/* Restore the callee save registers + SP and cpacr. */ \
ldp tmp0, tmp1, [ctx, #(THREAD_CONTEXT_SP_CPACR)]; \
mov sp, tmp0; \
ldp x19, x20, [ctx, #(THREAD_CONTEXT_X19_X20)]; \
ldp x21, x22, [ctx, #(THREAD_CONTEXT_X21_X22)]; \
ldp x23, x24, [ctx, #(THREAD_CONTEXT_X23_X24)]; \
ldp x25, x26, [ctx, #(THREAD_CONTEXT_X25_X26)]; \
ldp x27, x28, [ctx, #(THREAD_CONTEXT_X27_X28)]; \
ldp x29, x30, [ctx, #(THREAD_CONTEXT_X29_X30)]; \
\
msr cpacr_el1, tmp1; \
isb; \
\
/* Check whether the FPU is enabled. */ \
/* If it isn't, skip saving FPU state. */ \
and tmp1, tmp1, #0x300000; \
cbz tmp1, done_label; \
\
/* Save fpcr and fpsr. */ \
ldp tmp0, tmp1, [ctx, #(THREAD_CONTEXT_FPCR_FPSR)]; \
msr fpcr, tmp0; \
msr fpsr, tmp1; \
\
/* Save the FPU registers. */ \
ldp q0, q1, [ctx, #(16 * 0 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q2, q3, [ctx, #(16 * 2 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q4, q5, [ctx, #(16 * 4 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q6, q7, [ctx, #(16 * 6 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q8, q9, [ctx, #(16 * 8 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q10, q11, [ctx, #(16 * 10 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q12, q13, [ctx, #(16 * 12 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q14, q15, [ctx, #(16 * 14 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q16, q17, [ctx, #(16 * 16 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q18, q19, [ctx, #(16 * 18 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q20, q21, [ctx, #(16 * 20 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q22, q23, [ctx, #(16 * 22 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q24, q25, [ctx, #(16 * 24 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q26, q27, [ctx, #(16 * 26 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q28, q29, [ctx, #(16 * 28 + THREAD_CONTEXT_FPU_REGISTERS)]; \
ldp q30, q31, [ctx, #(16 * 30 + THREAD_CONTEXT_FPU_REGISTERS)];
#include <mesosphere/kern_select_assembly_macros.h>
/* ams::kern::KScheduler::ScheduleImpl() */
.section .text._ZN3ams4kern10KScheduler12ScheduleImplEv, "ax", %progbits
@@ -138,21 +54,107 @@ _ZN3ams4kern10KScheduler12ScheduleImplEv:
/* Get a reference to the current thread's stack parameters. */
add x2, sp, #0x1000
and x2, x2, #~(0x1000-1)
sub x2, x2, #(THREAD_STACK_PARAMETERS_SIZE)
/* Check if the thread has terminated. We can do this by checking the DPC flags for DpcFlag_Terminated. */
ldurb w3, [x2, #-(THREAD_STACK_PARAMETERS_SIZE - THREAD_STACK_PARAMETERS_DPC_FLAGS)]
tbnz w3, #1, 3f
/* Get a reference to the current thread's context. */
add x3, x2, #(THREAD_STACK_PARAMETERS_THREAD_CONTEXT)
/* The current thread hasn't terminated, so we want to save its context. */
ldur x2, [x2, #-(THREAD_STACK_PARAMETERS_SIZE - THREAD_STACK_PARAMETERS_CONTEXT)]
SAVE_THREAD_CONTEXT(x2, x4, x5, 2f)
/* Save the callee-save registers + SP. */
stp x19, x20, [x3, #(THREAD_CONTEXT_X19_X20)]
stp x21, x22, [x3, #(THREAD_CONTEXT_X21_X22)]
stp x23, x24, [x3, #(THREAD_CONTEXT_X23_X24)]
stp x25, x26, [x3, #(THREAD_CONTEXT_X25_X26)]
stp x27, x28, [x3, #(THREAD_CONTEXT_X27_X28)]
stp x29, x30, [x3, #(THREAD_CONTEXT_X29_X30)]
2: /* We're done saving this thread's context, so we need to unlock it. */
/* We can just do an atomic write to the relevant KThreadContext member. */
add x2, x2, #(THREAD_CONTEXT_LOCKED)
stlrb wzr, [x2]
mov x4, sp
str x4, [x3, #(THREAD_CONTEXT_SP)]
3: /* The current thread's context has been entirely taken care of. */
/* Check if the fpu is enabled; if it is, we need to save it. */
mrs x5, cpacr_el1
and x4, x5, #0x300000
cbz x4, 8f
/* We need to save the fpu state; save fpsr/fpcr. */
mrs x4, fpcr
mrs x6, fpsr
stp w4, w6, [x3, #(THREAD_CONTEXT_FPCR_FPSR)]
/* Set fpu-restore-needed in our exception flags. */
ldrb w4, [x2, #(THREAD_STACK_PARAMETERS_EXCEPTION_FLAGS)]
orr w4, w4, #(THREAD_EXCEPTION_FLAG_IS_FPU_CONTEXT_RESTORE_NEEDED)
strb w4, [x2, #(THREAD_STACK_PARAMETERS_EXCEPTION_FLAGS)]
/* We need to save fpu state based on whether we're a 64-bit or 32-bit thread. */
tbz w4, #(THREAD_EXCEPTION_FLAG_BIT_INDEX_IS_FPU_64_BIT), 4f
/* We have a 64-bit fpu. */
/* If we're in a usermode exception, we need to save the caller-save fpu registers. */
tbz w4, #(THREAD_EXCEPTION_FLAG_BIT_INDEX_IS_IN_USERMODE_EXCEPTION_HANDLER), 2f
/* If we're in an SVC (and not a usermode exception), we only need to save the callee-save fpu registers. */
tbz w4, #(THREAD_EXCEPTION_FLAG_BIT_INDEX_IS_CALLING_SVC), 3f
2: /* Save the 64-bit caller-save fpu registers. */
ldr x6, [x2, #(THREAD_STACK_PARAMETERS_CALLER_SAVE_FPU_REGISTERS)]
stp q0, q1, [x6, #(THREAD_FPU64_CONTEXT_Q0_Q1)]
stp q2, q3, [x6, #(THREAD_FPU64_CONTEXT_Q2_Q3)]
stp q4, q5, [x6, #(THREAD_FPU64_CONTEXT_Q4_Q5)]
stp q6, q7, [x6, #(THREAD_FPU64_CONTEXT_Q6_Q7)]
stp q16, q17, [x6, #(THREAD_FPU64_CONTEXT_Q16_Q17)]
stp q18, q19, [x6, #(THREAD_FPU64_CONTEXT_Q18_Q19)]
stp q20, q21, [x6, #(THREAD_FPU64_CONTEXT_Q20_Q21)]
stp q22, q23, [x6, #(THREAD_FPU64_CONTEXT_Q22_Q23)]
stp q24, q25, [x6, #(THREAD_FPU64_CONTEXT_Q24_Q25)]
stp q26, q27, [x6, #(THREAD_FPU64_CONTEXT_Q26_Q27)]
stp q28, q29, [x6, #(THREAD_FPU64_CONTEXT_Q28_Q29)]
stp q30, q31, [x6, #(THREAD_FPU64_CONTEXT_Q30_Q31)]
3: /* Save the 64-bit callee-save fpu registers. */
stp q8, q9, [x3, #(THREAD_CONTEXT_FPU64_Q8_Q9)]
stp q10, q11, [x3, #(THREAD_CONTEXT_FPU64_Q10_Q11)]
stp q12, q13, [x3, #(THREAD_CONTEXT_FPU64_Q12_Q13)]
stp q14, q15, [x3, #(THREAD_CONTEXT_FPU64_Q14_Q15)]
b 7f
4: /* We have a 32-bit fpu. */
/* If we're in a usermode exception, we need to save the caller-save fpu registers. */
tbz w4, #(THREAD_EXCEPTION_FLAG_BIT_INDEX_IS_IN_USERMODE_EXCEPTION_HANDLER), 5f
/* If we're in an SVC (and not a usermode exception), we only need to save the callee-save fpu registers. */
tbz w4, #(THREAD_EXCEPTION_FLAG_BIT_INDEX_IS_CALLING_SVC), 6f
5: /* Save the 32-bit caller-save fpu registers. */
ldr x6, [x2, #(THREAD_STACK_PARAMETERS_CALLER_SAVE_FPU_REGISTERS)]
stp q0, q1, [x6, #(THREAD_FPU32_CONTEXT_Q0_Q1)]
stp q2, q3, [x6, #(THREAD_FPU32_CONTEXT_Q2_Q3)]
stp q8, q9, [x6, #(THREAD_FPU32_CONTEXT_Q8_Q9)]
stp q10, q11, [x6, #(THREAD_FPU32_CONTEXT_Q10_Q11)]
stp q12, q13, [x6, #(THREAD_FPU32_CONTEXT_Q12_Q13)]
stp q14, q15, [x6, #(THREAD_FPU32_CONTEXT_Q14_Q15)]
6: /* Save the 32-bit callee-save fpu registers. */
stp q4, q5, [x3, #(THREAD_CONTEXT_FPU32_Q4_Q5)]
stp q6, q7, [x3, #(THREAD_CONTEXT_FPU32_Q6_Q7)]
7: /* With the fpu state saved, disable the fpu. */
and x5, x5, #(~0x300000)
msr cpacr_el1, x5
8: /* We're done saving this thread's context. */
/* Check if the thread is terminated by checking the DPC flags for DpcFlag_Terminated. */
ldrb w4, [x2, #(THREAD_STACK_PARAMETERS_DPC_FLAGS)]
tbnz w4, #1, 9f
/* The thread isn't terminated, so we want to unlock it. */
/* Write atomically to the context's locked member. */
add x3, x3, #(THREAD_CONTEXT_LOCKED)
stlrb wzr, [x3]
9: /* The current thread's context has been entirely taken care of. */
/* Now we want to loop until we successfully switch the thread context. */
/* Start by saving all the values we care about in callee-save registers. */
mov x19, x0 /* this */
@@ -162,54 +164,54 @@ _ZN3ams4kern10KScheduler12ScheduleImplEv:
/* Set our stack to the idle thread stack. */
ldr x3, [x20, #(KSCHEDULER_IDLE_THREAD_STACK)]
mov sp, x3
b 5f
b 11f
4: /* We failed to successfully do the context switch, and need to retry. */
10: /* We failed to successfully do the context switch, and need to retry. */
/* Clear the exclusive monitor. */
clrex
/* Clear the need's scheduling bool (and dmb ish after, as it's an atomic). */
/* TODO: Should this be a stlrb? Nintendo does not do one. */
strb wzr, [x20]
dmb ish
/* Refresh the highest priority thread. */
ldr x21, [x20, #(KSCHEDULER_HIGHEST_PRIORITY_THREAD)]
5: /* We're starting to try to do the context switch. */
11: /* We're starting to try to do the context switch. */
/* Check if the highest priority thread if null. */
/* If it is, we want to branch to a special idle thread loop. */
cbz x21, 11f
cbz x21, 16f
/* Get the highest priority thread's context, and save it. */
/* ams::kern::KThread::GetContextForSchedulerLoop() */
add x22, x21, #(THREAD_THREAD_CONTEXT)
ldr x22, [x21, #(THREAD_KERNEL_STACK_TOP)]
sub x22, x22, #(THREAD_STACK_PARAMETERS_SIZE - THREAD_STACK_PARAMETERS_THREAD_CONTEXT)
/* Prepare to try to acquire the context lock. */
add x1, x22, #(THREAD_CONTEXT_LOCKED)
mov w2, #1
6: /* We want to try to lock the highest priority thread's context. */
12: /* We want to try to lock the highest priority thread's context. */
/* Check if the lock is already held. */
ldaxrb w3, [x1]
cbnz w3, 7f
cbnz w3, 13f
/* If it's not, try to take it. */
stxrb w3, w2, [x1]
cbnz w3, 6b
cbnz w3, 12b
/* We hold the lock, so we can now switch the thread. */
b 8f
b 14f
7: /* The highest priority thread's context is already locked. */
13: /* The highest priority thread's context is already locked. */
/* Check if we need scheduling. If we don't, we can retry directly. */
ldarb w3, [x20] // ldarb w3, [x20, #(KSCHEDULER_NEEDS_SCHEDULING)]
cbz w3, 6b
cbz w3, 12b
/* If we do, another core is interfering, and we must start from the top. */
b 4b
b 10b
8: /* It's time to switch the thread. */
14: /* It's time to switch the thread. */
/* Switch to the highest priority thread. */
mov x0, x19
mov x1, x21
@@ -219,22 +221,30 @@ _ZN3ams4kern10KScheduler12ScheduleImplEv:
/* Check if we need scheduling. If we don't, then we can't complete the switch and should retry. */
ldarb w1, [x20] // ldarb w1, [x20, #(KSCHEDULER_NEEDS_SCHEDULING)]
cbnz w1, 10f
cbnz w1, 15f
/* Restore the thread context. */
mov x0, x22
RESTORE_THREAD_CONTEXT(x0, x1, x2, 9f)
ldp x19, x20, [x0, #(THREAD_CONTEXT_X19_X20)]
ldp x21, x22, [x0, #(THREAD_CONTEXT_X21_X22)]
ldp x23, x24, [x0, #(THREAD_CONTEXT_X23_X24)]
ldp x25, x26, [x0, #(THREAD_CONTEXT_X25_X26)]
ldp x27, x28, [x0, #(THREAD_CONTEXT_X27_X28)]
ldp x29, x30, [x0, #(THREAD_CONTEXT_X29_X30)]
9: /* We're done restoring the thread context, and can return safely. */
ldr x1, [x0, #(THREAD_CONTEXT_SP)]
mov sp, x1
/* Return. */
ret
10: /* Our switch failed. */
15: /* Our switch failed. */
/* We should unlock the thread context, and then retry. */
add x1, x22, #(THREAD_CONTEXT_LOCKED)
stlrb wzr, [x1]
b 4b
b 10b
11: /* The next thread is nullptr! */
16: /* The next thread is nullptr! */
/* Switch to nullptr. This will actually switch to the idle thread. */
mov x0, x19
mov x1, #0
@@ -242,14 +252,14 @@ _ZN3ams4kern10KScheduler12ScheduleImplEv:
/* Call ams::kern::KScheduler::SwitchThread(ams::kern::KThread *) */
bl _ZN3ams4kern10KScheduler12SwitchThreadEPNS0_7KThreadE
12: /* We've switched to the idle thread, so we want to process interrupt tasks until we schedule a non-idle thread. */
17: /* We've switched to the idle thread, so we want to process interrupt tasks until we schedule a non-idle thread. */
/* Check whether there are runnable interrupt tasks. */
ldrb w3, [x20, #(KSCHEDULER_INTERRUPT_TASK_RUNNABLE)]
cbnz w3, 13f
cbnz w3, 18f
/* Check if we need scheduling. */
ldarb w3, [x20] // ldarb w3, [x20, #(KSCHEDULER_NEEDS_SCHEDULING)]
cbnz w3, 4b
cbnz w3, 10b
/* Clear the previous thread. */
str xzr, [x20, #(KSCHEDULER_PREVIOUS_THREAD)]
@@ -260,9 +270,9 @@ _ZN3ams4kern10KScheduler12ScheduleImplEv:
msr daifclr, #2
msr daifset, #2
b 12b
b 17b
13: /* We have interrupt tasks to execute! */
18: /* We have interrupt tasks to execute! */
/* Execute any pending interrupt tasks. */
ldr x0, [x20, #(KSCHEDULER_INTERRUPT_TASK_MANAGER)]
bl _ZN3ams4kern21KInterruptTaskManager7DoTasksEv
@@ -271,4 +281,4 @@ _ZN3ams4kern10KScheduler12ScheduleImplEv:
strb wzr, [x20, #(KSCHEDULER_INTERRUPT_TASK_RUNNABLE)]
/* Retry the scheduling loop. */
b 4b
b 10b

55
mesosphere/kernel/source/arch/arm64/kern_k_thread_context_asm.s
View File

@@ -87,58 +87,3 @@ _ZN3ams4kern4arch5arm6427SupervisorModeThreadStarterEv:
/* This should never execute, but Nintendo includes an ERET here. */
eret
/* ams::kern::arch::arm64::KThreadContext::RestoreFpuRegisters64(const KThreadContext &) */
.section .text._ZN3ams4kern4arch5arm6414KThreadContext21RestoreFpuRegisters64ERKS3_, "ax", %progbits
.global _ZN3ams4kern4arch5arm6414KThreadContext21RestoreFpuRegisters64ERKS3_
.type _ZN3ams4kern4arch5arm6414KThreadContext21RestoreFpuRegisters64ERKS3_, %function
_ZN3ams4kern4arch5arm6414KThreadContext21RestoreFpuRegisters64ERKS3_:
/* Load and restore FPCR and FPSR from the context. */
ldr x1, [x0, #(THREAD_CONTEXT_FPCR)]
msr fpcr, x1
ldr x1, [x0, #(THREAD_CONTEXT_FPSR)]
msr fpsr, x1
/* Restore the FPU registers. */
ldp q0, q1, [x0, #(16 * 0 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q2, q3, [x0, #(16 * 2 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q4, q5, [x0, #(16 * 4 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q6, q7, [x0, #(16 * 6 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q8, q9, [x0, #(16 * 8 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q10, q11, [x0, #(16 * 10 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q12, q13, [x0, #(16 * 12 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q14, q15, [x0, #(16 * 14 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q16, q17, [x0, #(16 * 16 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q18, q19, [x0, #(16 * 18 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q20, q21, [x0, #(16 * 20 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q22, q23, [x0, #(16 * 22 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q24, q25, [x0, #(16 * 24 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q26, q27, [x0, #(16 * 26 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q28, q29, [x0, #(16 * 28 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q30, q31, [x0, #(16 * 30 + THREAD_CONTEXT_FPU_REGISTERS)]
ret
/* ams::kern::arch::arm64::KThreadContext::RestoreFpuRegisters32(const KThreadContext &) */
.section .text._ZN3ams4kern4arch5arm6414KThreadContext21RestoreFpuRegisters32ERKS3_, "ax", %progbits
.global _ZN3ams4kern4arch5arm6414KThreadContext21RestoreFpuRegisters32ERKS3_
.type _ZN3ams4kern4arch5arm6414KThreadContext21RestoreFpuRegisters32ERKS3_, %function
_ZN3ams4kern4arch5arm6414KThreadContext21RestoreFpuRegisters32ERKS3_:
/* Load and restore FPCR and FPSR from the context. */
ldr x1, [x0, #(THREAD_CONTEXT_FPCR)]
msr fpcr, x1
ldr x1, [x0, #(THREAD_CONTEXT_FPSR)]
msr fpsr, x1
/* Restore the FPU registers. */
ldp q0, q1, [x0, #(16 * 0 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q2, q3, [x0, #(16 * 2 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q4, q5, [x0, #(16 * 4 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q6, q7, [x0, #(16 * 6 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q8, q9, [x0, #(16 * 8 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q10, q11, [x0, #(16 * 10 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q12, q13, [x0, #(16 * 12 + THREAD_CONTEXT_FPU_REGISTERS)]
ldp q14, q15, [x0, #(16 * 14 + THREAD_CONTEXT_FPU_REGISTERS)]
ret