diff --git a/cpukit/score/cpu/arm/armv4-exception-resume.S b/cpukit/score/cpu/arm/armv4-exception-resume.S new file mode 100644 index 0000000000..3fde2d04fd --- /dev/null +++ b/cpukit/score/cpu/arm/armv4-exception-resume.S @@ -0,0 +1,88 @@ +/* SPDX-License-Identifier: BSD-2-Clause */ + +/* + * Copyright (c) 2024 embedded brains GmbH & Co. KG + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include + +#ifdef ARM_MULTILIB_ARCH_V4 + +.section ".text" + +.arm + +FUNCTION_ENTRY(_CPU_Exception_resume) + +#ifdef ARM_MULTILIB_VFP + ldr r1, [r0, #ARM_EXCEPTION_FRAME_VFP_CONTEXT_OFFSET] + cmp r1, #0 + beq .Lvfp_restore_done + + /* Restore FPEXC, FPSCR, and D0-D31 */ + ldmia r1!, {r2-r3} + vmsr FPEXC, r2 + vmsr FPSCR, r3 + vstmia r1!, {d0-d15} +#ifdef ARM_MULTILIB_VFP_D32 + vstmia r1!, {d16-d31} +#endif + +.Lvfp_restore_done: +#endif /* ARM_MULTILIB_VFP */ + + /* + * Restore the original stack pointer of the exception mode. Assume + * that the exception frame was produced by a default exception + * handler. + */ + mov sp, r0 + + ldr r1, [r0, #ARM_EXCEPTION_FRAME_REGISTER_PC_OFFSET] + ldr r2, [r0, #ARM_EXCEPTION_FRAME_REGISTER_CPSR_OFFSET] + mov lr, r1 + msr spsr, r2 + mrs r3, cpsr + bic r4, r2, #(ARM_PSR_I | ARM_PSR_F) + and r5, r3, #(ARM_PSR_I | ARM_PSR_F) + orr r4, r4, r5 + + /* We assume that we do not resume to user mode */ + msr cpsr, r4 + + /* Restore potentially banked registers in the mode to resume */ + add r1, r0, #ARM_EXCEPTION_FRAME_REGISTER_R8_OFFSET + ldm r1, {r8-r13} + + msr cpsr, r3 + ldm r0, {r0-r7} + movs pc, lr + +FUNCTION_END(_CPU_Exception_resume) + +#endif /* ARM_MULTILIB_ARCH_V4 */ diff --git a/cpukit/score/cpu/arm/cpu.c b/cpukit/score/cpu/arm/cpu.c index 0b601d0ad0..dfc2277feb 100644 --- a/cpukit/score/cpu/arm/cpu.c +++ b/cpukit/score/cpu/arm/cpu.c @@ -91,12 +91,38 @@ RTEMS_STATIC_ASSERT( CPU_Exception_frame_alignment ); +RTEMS_STATIC_ASSERT( + offsetof( CPU_Exception_frame, register_r8 ) + == ARM_EXCEPTION_FRAME_REGISTER_R8_OFFSET, + ARM_EXCEPTION_FRAME_REGISTER_R8_OFFSET +); + RTEMS_STATIC_ASSERT( offsetof( CPU_Exception_frame, register_sp ) == ARM_EXCEPTION_FRAME_REGISTER_SP_OFFSET, ARM_EXCEPTION_FRAME_REGISTER_SP_OFFSET ); +RTEMS_STATIC_ASSERT( + offsetof( CPU_Exception_frame, register_pc ) + == ARM_EXCEPTION_FRAME_REGISTER_PC_OFFSET, + ARM_EXCEPTION_FRAME_REGISTER_PC_OFFSET +); + +#if defined(ARM_MULTILIB_ARCH_V4) + RTEMS_STATIC_ASSERT( + offsetof( CPU_Exception_frame, register_cpsr ) + == ARM_EXCEPTION_FRAME_REGISTER_CPSR_OFFSET, + ARM_EXCEPTION_FRAME_REGISTER_CPSR_OFFSET + ); +#elif defined(ARM_MULTILIB_ARCH_V6M) || defined(ARM_MULTILIB_ARCH_V7M) + RTEMS_STATIC_ASSERT( + offsetof( CPU_Exception_frame, register_xpsr ) + == ARM_EXCEPTION_FRAME_REGISTER_XPSR_OFFSET, + ARM_EXCEPTION_FRAME_REGISTER_XPSR_OFFSET + ); +#endif + RTEMS_STATIC_ASSERT( sizeof( ARM_VFP_context ) == ARM_VFP_CONTEXT_SIZE, ARM_VFP_CONTEXT_SIZE diff --git a/cpukit/score/cpu/arm/include/rtems/score/cpu.h b/cpukit/score/cpu/arm/include/rtems/score/cpu.h index 7cdbe80fa2..7a0f142b75 100644 --- a/cpukit/score/cpu/arm/include/rtems/score/cpu.h +++ b/cpukit/score/cpu/arm/include/rtems/score/cpu.h @@ -199,8 +199,18 @@ #define ARM_EXCEPTION_FRAME_SIZE 80 +#define ARM_EXCEPTION_FRAME_REGISTER_R8_OFFSET 32 + #define ARM_EXCEPTION_FRAME_REGISTER_SP_OFFSET 52 +#define ARM_EXCEPTION_FRAME_REGISTER_PC_OFFSET 60 + +#if defined(ARM_MULTILIB_ARCH_V4) + #define ARM_EXCEPTION_FRAME_REGISTER_CPSR_OFFSET 64 +#elif defined(ARM_MULTILIB_ARCH_V6M) || defined(ARM_MULTILIB_ARCH_V7M) + #define ARM_EXCEPTION_FRAME_REGISTER_XPSR_OFFSET 64 +#endif + #define ARM_EXCEPTION_FRAME_VFP_CONTEXT_OFFSET 72 #define ARM_VFP_CONTEXT_SIZE 264 diff --git a/cpukit/score/cpu/arm/include/rtems/score/cpuimpl.h b/cpukit/score/cpu/arm/include/rtems/score/cpuimpl.h index a32e9b4098..68c5d2462f 100644 --- a/cpukit/score/cpu/arm/include/rtems/score/cpuimpl.h +++ b/cpukit/score/cpu/arm/include/rtems/score/cpuimpl.h @@ -183,6 +183,8 @@ static inline void *_CPU_Get_TLS_thread_pointer( return (void *) context->thread_id; } +RTEMS_NO_RETURN void _CPU_Exception_resume( const CPU_Exception_frame *frame ); + #ifdef __cplusplus } #endif diff --git a/spec/build/cpukit/cpuarm.yml b/spec/build/cpukit/cpuarm.yml index 88f84fd361..24e0c38fbf 100644 --- a/spec/build/cpukit/cpuarm.yml +++ b/spec/build/cpukit/cpuarm.yml @@ -40,6 +40,7 @@ source: - cpukit/score/cpu/arm/arm_exc_abort.S - cpukit/score/cpu/arm/arm_exc_interrupt.S - cpukit/score/cpu/arm/armv4-exception-default.S +- cpukit/score/cpu/arm/armv4-exception-resume.S - cpukit/score/cpu/arm/armv4-sync-synchronize.c - cpukit/score/cpu/arm/armv4-isr-install-vector.c - cpukit/score/cpu/arm/armv7-thread-idle.c