This adds the call and support functions necessary to add Exception Manager support to AArch64. --- .../cpu/aarch64/aarch64-exception-default.S | 81 +-------- .../cpu/aarch64/aarch64-exception-default.c | 62 +++++++ .../cpu/aarch64/aarch64-exception-interrupt.S | 165 ++++++++++++++++++ spec/build/cpukit/optexceptionmanager.yml | 4 + 4 files changed, 238 insertions(+), 74 deletions(-)
diff --git a/cpukit/score/cpu/aarch64/aarch64-exception-default.S b/cpukit/score/cpu/aarch64/aarch64-exception-default.S index 2a4ddbcc61..ef95619fb1 100644 --- a/cpukit/score/cpu/aarch64/aarch64-exception-default.S +++ b/cpukit/score/cpu/aarch64/aarch64-exception-default.S @@ -203,14 +203,13 @@ curr_el_spx_sync_get_pc: /* The current PC is now in LR */ /* Store the vector */ str lr, [sp, #AARCH64_EXCEPTION_FRAME_REGISTER_VECTOR_OFFSET] mov x0, sp - blr x1 -/* bl to CEF restore routine (doesn't restore lr) */ - bl .pop_exception_context - ldr lr, [sp, #AARCH64_EXCEPTION_FRAME_REGISTER_LR_OFFSET] /* get lr from CEF */ -/* drop space reserved for CEF and clear exclusive */ - add sp, sp, #AARCH64_EXCEPTION_FRAME_SIZE - msr spsel, #1 /* switch to thread stack */ - eret /* exception return */ +/* Not expected to return */ + br x1 + nop + nop + nop + nop + nop nop nop nop @@ -475,69 +474,3 @@ twiddle: stp q30, q31, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x1e0)] /* Done, return to exception handler */ ret - -/* - * Apply the exception frame to the current register status, SP points to the EF - */ -.pop_exception_context: -/* Pop daif and spsr */ - ldp x2, x3, [sp, #AARCH64_EXCEPTION_FRAME_REGISTER_DAIF_OFFSET] -/* Restore daif and spsr */ - msr DAIF, x2 - msr SPSR_EL1, x3 -/* Pop FAR and ESR */ - ldp x2, x3, [sp, #AARCH64_EXCEPTION_FRAME_REGISTER_SYNDROME_OFFSET] -/* Restore ESR and FAR */ - msr ESR_EL1, x2 - msr FAR_EL1, x3 -/* Pop fpcr and fpsr */ - ldp x2, x3, [sp, #AARCH64_EXCEPTION_FRAME_REGISTER_FPSR_OFFSET] -/* Restore fpcr and fpsr */ - msr FPSR, x2 - msr FPCR, x3 -/* Pop VFP registers */ - ldp q0, q1, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x000)] - ldp q2, q3, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x020)] - ldp q4, q5, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x040)] - ldp q6, q7, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x060)] - ldp q8, q9, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x080)] - ldp q10, q11, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x0a0)] - ldp q12, q13, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x0c0)] - ldp q14, q15, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x0e0)] - ldp q16, q17, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x100)] - ldp q18, q19, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x120)] - ldp q20, q21, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x140)] - ldp q22, q23, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x160)] - ldp q24, q25, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x180)] - ldp q26, q27, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x1a0)] - ldp q28, q29, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x1c0)] - ldp q30, q31, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x1e0)] -/* Pop x0-x29(fp) */ - ldp x2, x3, [sp, #0x10] - ldp x4, x5, [sp, #0x20] - ldp x6, x7, [sp, #0x30] - ldp x8, x9, [sp, #0x40] - ldp x10, x11, [sp, #0x50] - ldp x12, x13, [sp, #0x60] - ldp x14, x15, [sp, #0x70] - ldp x16, x17, [sp, #0x80] - ldp x18, x19, [sp, #0x90] - ldp x20, x21, [sp, #0xa0] - ldp x22, x23, [sp, #0xb0] - ldp x24, x25, [sp, #0xc0] - ldp x26, x27, [sp, #0xd0] - ldp x28, x29, [sp, #0xe0] -/* Pop sp and ELR */ - ldp x0, x1, [sp, #AARCH64_EXCEPTION_FRAME_REGISTER_SP_OFFSET] -/* Restore thread SP */ - msr spsel, #1 - mov sp, x0 - msr spsel, #0 -/* Restore exception LR */ - msr ELR_EL1, x1 - ldp x0, x1, [sp, #0x00] - -/* We must clear reservations to ensure consistency with atomic operations */ - clrex - - ret diff --git a/cpukit/score/cpu/aarch64/aarch64-exception-default.c b/cpukit/score/cpu/aarch64/aarch64-exception-default.c index 2ebb3dee9f..4e7484f718 100644 --- a/cpukit/score/cpu/aarch64/aarch64-exception-default.c +++ b/cpukit/score/cpu/aarch64/aarch64-exception-default.c @@ -43,8 +43,70 @@ #include <rtems/score/cpu.h> #include <rtems/fatal.h> +#include <rtems/exception.h> void _AArch64_Exception_default( CPU_Exception_frame *frame ) { rtems_fatal( RTEMS_FATAL_SOURCE_EXCEPTION, (rtems_fatal_code) frame ); } + +void Exception_Manager_Copy_CPU_Exception_frame( + CPU_Exception_frame *new_ef, + CPU_Exception_frame *old_ef +) +{ + *new_ef = *old_ef; +} + +#define ESR_EC_GET(reg) ( ( reg >> 26 ) & 0x3f ) +#define ESR_ISS_GET(reg) ( reg & 0x1ffffff ) +#define WnR ( 1 << 6 ) + +Exception_Class Exception_Manager_Get_class( CPU_Exception_frame *ef ) +{ + uint64_t EC = ESR_EC_GET( ef->register_syndrome ); + uint64_t ISS = ESR_ISS_GET( ef->register_syndrome ); + + switch ( EC ) { + case 0x1: /* WFI */ + case 0x7: /* SVE/SIMD/FP */ + case 0xa: /* LD64B/ST64B* */ + case 0x18: /* MSR/MRS/system instruction */ + case 0x19: /* SVE */ + return EXCEPTION_TRAPPED_INSTRUCTION; + case 0x15: + return EXCEPTION_SUPERVISOR; + case 0x21: + return EXCEPTION_INSTRUCTION_ABORT; + case 0x22: + return EXCEPTION_PC_ALIGNMENT; + case 0x25: + return ( ISS & WnR ) ? EXCEPTION_DATA_ABORT_WRITE : EXCEPTION_DATA_ABORT_READ; + case 0x26: + return EXCEPTION_SP_ALIGNMENT; + case 0x2c: + return EXCEPTION_FPU; + case 0x31: + return EXCEPTION_BREAKPOINT; + case 0x33: + return EXCEPTION_STEP; + case 0x35: + return EXCEPTION_WATCHPOINT; + case 0x3c: + return EXCEPTION_BREAK_INSTRUCTION; + /* AArch32-specific, from-EL0, etc. */ + default: + return EXCEPTION_UNKNOWN; + } +} + +void Exception_Manager_Set_resume( CPU_Exception_frame *ef, void *address ) +{ + ef->register_pc = address; +} + +#define AARCH64_INSTRUCTION_SIZE 4 +void Exception_Manager_Set_resume_next_instruction( CPU_Exception_frame *ef ) +{ + ef->register_pc += AARCH64_INSTRUCTION_SIZE; +} diff --git a/cpukit/score/cpu/aarch64/aarch64-exception-interrupt.S b/cpukit/score/cpu/aarch64/aarch64-exception-interrupt.S index b206f5764b..4ba05a17e7 100644 --- a/cpukit/score/cpu/aarch64/aarch64-exception-interrupt.S +++ b/cpukit/score/cpu/aarch64/aarch64-exception-interrupt.S @@ -45,6 +45,8 @@ .globl _AArch64_Exception_interrupt_no_nest .globl _AArch64_Exception_interrupt_nest +.globl Exception_Manager_dispatch_and_resume +.globl Exception_Manager_resume #ifdef AARCH64_MULTILIB_ARCH_V8_ILP32 #ifdef RTEMS_SMP @@ -324,3 +326,166 @@ Return to embedded exception vector code pop_interrupt_context /* Return to vector for final cleanup */ ret + +/* + * This function is expected to resume execution using the CPU_Exception_frame + * provided in x0. This function does not adhere to the AAPCS64 calling + * convention because all necessary state is contained within the exception + * frame. + */ +Exception_Manager_resume: +/* Reset stack pointer */ + mov sp, x0 + +/* call CEF restore routine (doesn't restore lr) */ + bl .pop_exception_context + +/* get lr from CEF */ + ldr lr, [sp, #AARCH64_EXCEPTION_FRAME_REGISTER_LR_OFFSET] + +/* drop space reserved for CEF */ + add sp, sp, #AARCH64_EXCEPTION_FRAME_SIZE + +/* switch to thread stack */ + msr spsel, #1 + eret + +/* + * This function is expected to undo dispatch disabling, perform dispatch, and + * resume execution using the CPU_Exception_frame provided in x0. This function + * does not adhere to the AAPCS64 calling convention because all necessary + * state is contained within the exception frame. + */ +Exception_Manager_dispatch_and_resume: +/* Get per-CPU control of current processor */ + GET_SELF_CPU_CONTROL SELF_CPU_CONTROL_GET_REG + +/* Reset stack pointer */ + mov sp, x0 + +/* Check dispatch disable and perform dispatch if necessary */ +/* Load some per-CPU variables */ + ldr w0, [SELF_CPU_CONTROL, #PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL] + ldrb w1, [SELF_CPU_CONTROL, #PER_CPU_DISPATCH_NEEDED] + ldr w2, [SELF_CPU_CONTROL, #PER_CPU_ISR_DISPATCH_DISABLE] + ldr w3, [SELF_CPU_CONTROL, #PER_CPU_ISR_NEST_LEVEL] + +/* Decrement levels and determine thread dispatch state */ + eor w1, w1, w0 + sub w0, w0, #1 + orr w1, w1, w0 + orr w1, w1, w2 + sub w3, w3, #1 + +/* Store thread dispatch disable and ISR nest levels */ + str w0, [SELF_CPU_CONTROL, #PER_CPU_THREAD_DISPATCH_DISABLE_LEVEL] + str w3, [SELF_CPU_CONTROL, #PER_CPU_ISR_NEST_LEVEL] + +/* store should_skip_thread_dispatch in x22 */ + mov x22, x1 + +/* + * It is now safe to assume that the source of the exception has been resolved. + * Copy the exception frame to the thread stack to be compatible with thread + * dispatch. This may arbitrarily clobber corruptible registers since all + * important state is contained in the exception frame. + * + * No need to save current LR since this will never return to the caller. + */ + bl .move_exception_frame_and_switch_to_thread_stack + +/* + * Check thread dispatch necessary, ISR dispatch disable and thread dispatch + * disable level. + */ + cmp x22, #0 + bne .Lno_need_thread_dispatch_resume + bl .AArch64_Perform_Thread_Dispatch +.Lno_need_thread_dispatch_resume: +/* call CEF restore routine (doesn't restore lr) */ + bl .pop_exception_context + +/* get lr from CEF */ + ldr lr, [sp, #AARCH64_EXCEPTION_FRAME_REGISTER_LR_OFFSET] + +/* drop space reserved for CEF */ + add sp, sp, #AARCH64_EXCEPTION_FRAME_SIZE + eret + +/* Assumes sp currently points to the EF on the exception stack and SPSel is 0 */ +.move_exception_frame_and_switch_to_thread_stack: + mov x1, sp /* Set x1 to the current exception frame */ + msr spsel, #1 /* switch to thread stack */ + ldr x0, [x1, #AARCH64_EXCEPTION_FRAME_REGISTER_SP_OFFSET] /* Get thread SP from exception frame since it may have been updated */ + mov sp, x0 + sub sp, sp, #AARCH64_EXCEPTION_FRAME_SIZE /* reserve space for CEF */ + mov x0, sp /* Set x0 to the new exception frame */ + mov x20, lr /* Save LR */ + bl Exception_Manager_Copy_CPU_Exception_frame /* Copy exception frame to reserved thread stack space */ + mov lr, x20 /* Restore LR */ + msr spsel, #0 /* switch to exception stack */ + add sp, sp, #AARCH64_EXCEPTION_FRAME_SIZE /* release space for CEF on exception stack */ + msr spsel, #1 /* switch to thread stack */ + ret + +/* + * Apply the exception frame to the current register status, SP points to the EF + */ +.pop_exception_context: +/* Pop daif and spsr */ + ldp x2, x3, [sp, #AARCH64_EXCEPTION_FRAME_REGISTER_DAIF_OFFSET] +/* Restore daif and spsr */ + msr DAIF, x2 + msr SPSR_EL1, x3 +/* Pop FAR and ESR */ + ldp x2, x3, [sp, #AARCH64_EXCEPTION_FRAME_REGISTER_SYNDROME_OFFSET] +/* Restore ESR and FAR */ + msr ESR_EL1, x2 + msr FAR_EL1, x3 +/* Pop fpcr and fpsr */ + ldp x2, x3, [sp, #AARCH64_EXCEPTION_FRAME_REGISTER_FPSR_OFFSET] +/* Restore fpcr and fpsr */ + msr FPSR, x2 + msr FPCR, x3 +/* Pop VFP registers */ + ldp q0, q1, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x000)] + ldp q2, q3, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x020)] + ldp q4, q5, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x040)] + ldp q6, q7, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x060)] + ldp q8, q9, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x080)] + ldp q10, q11, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x0a0)] + ldp q12, q13, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x0c0)] + ldp q14, q15, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x0e0)] + ldp q16, q17, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x100)] + ldp q18, q19, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x120)] + ldp q20, q21, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x140)] + ldp q22, q23, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x160)] + ldp q24, q25, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x180)] + ldp q26, q27, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x1a0)] + ldp q28, q29, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x1c0)] + ldp q30, q31, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_Q0_OFFSET + 0x1e0)] +/* Pop x0-x29(fp) */ + ldp x2, x3, [sp, #0x10] + ldp x4, x5, [sp, #0x20] + ldp x6, x7, [sp, #0x30] + ldp x8, x9, [sp, #0x40] + ldp x10, x11, [sp, #0x50] + ldp x12, x13, [sp, #0x60] + ldp x14, x15, [sp, #0x70] + ldp x16, x17, [sp, #0x80] + ldp x18, x19, [sp, #0x90] + ldp x20, x21, [sp, #0xa0] + ldp x22, x23, [sp, #0xb0] + ldp x24, x25, [sp, #0xc0] + ldp x26, x27, [sp, #0xd0] + ldp x28, x29, [sp, #0xe0] +/* Pop ELR, SP already popped */ + ldr x1, [sp, #(AARCH64_EXCEPTION_FRAME_REGISTER_SP_OFFSET + 0x8)] +/* Restore exception LR */ + msr ELR_EL1, x1 + ldp x0, x1, [sp, #0x00] + +/* We must clear reservations to ensure consistency with atomic operations */ + clrex + + ret diff --git a/spec/build/cpukit/optexceptionmanager.yml b/spec/build/cpukit/optexceptionmanager.yml index 2188e16cf1..4d4e09c86a 100644 --- a/spec/build/cpukit/optexceptionmanager.yml +++ b/spec/build/cpukit/optexceptionmanager.yml @@ -12,6 +12,10 @@ default-by-variant: [] description: | Enable the Exception Manager enabled-by: +- aarch64/xilinx_zynqmp_ilp32_qemu +- aarch64/xilinx_zynqmp_ilp32_zu3eg +- aarch64/xilinx_zynqmp_lp64_qemu +- aarch64/xilinx_zynqmp_lp64_zu3eg links: [] name: RTEMS_EXCEPTION_MANAGER type: build -- 2.30.2 _______________________________________________ devel mailing list devel@rtems.org http://lists.rtems.org/mailman/listinfo/devel