| /* |
| * Copyright (c) 2012-2015 Travis Geiselbrecht |
| * |
| * Permission is hereby granted, free of charge, to any person obtaining |
| * a copy of this software and associated documentation files |
| * (the "Software"), to deal in the Software without restriction, |
| * including without limitation the rights to use, copy, modify, merge, |
| * publish, distribute, sublicense, and/or sell copies of the Software, |
| * and to permit persons to whom the Software is furnished to do so, |
| * subject to the following conditions: |
| * |
| * The above copyright notice and this permission notice shall be |
| * included in all copies or substantial portions of the Software. |
| * |
| * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
| * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF |
| * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. |
| * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY |
| * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, |
| * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE |
| * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
| */ |
| #include <sys/types.h> |
| #include <string.h> |
| #include <stdlib.h> |
| #include <debug.h> |
| #include <trace.h> |
| #include <assert.h> |
| #include <kernel/thread.h> |
| #include <arch/arm.h> |
| #include <arch/arm/cm.h> |
| |
| #define LOCAL_TRACE 0 |
| |
| struct arm_cm_context_switch_frame { |
| #if (__CORTEX_M >= 0x03) |
| uint32_t r4; |
| uint32_t r5; |
| uint32_t r6; |
| uint32_t r7; |
| uint32_t r8; |
| uint32_t r9; |
| uint32_t r10; |
| uint32_t r11; |
| uint32_t lr; |
| #else |
| /* frame format is slightly different due to ordering of push/pops */ |
| uint32_t r8; |
| uint32_t r9; |
| uint32_t r10; |
| uint32_t r11; |
| uint32_t r4; |
| uint32_t r5; |
| uint32_t r6; |
| uint32_t r7; |
| uint32_t lr; |
| #endif |
| }; |
| |
| /* macros for saving and restoring a context switch frame, depending on what version of |
| * the architecture you are */ |
| #if (__CORTEX_M >= 0x03) |
| |
| /* cortex-m3 and above (armv7-m) */ |
| #define SAVE_REGS "push { r4-r11, lr };" |
| #define RESTORE_REGS "pop { r4-r11, lr };" |
| #define RESTORE_REGS_PC "pop { r4-r11, pc };" |
| #define SAVE_SP(basereg, tempreg, offset) "str sp, [" #basereg "," #offset "];" |
| #define LOAD_SP(basereg, tempreg, offset) "ldr sp, [" #basereg "," #offset "];" |
| #define CLREX "clrex;" |
| |
| #else |
| |
| /* cortex-m0 and cortex-m0+ (armv6-m) */ |
| #define SAVE_REGS \ |
| "push { r4-r7, lr };" \ |
| "mov r4, r8;" \ |
| "mov r5, r9;" \ |
| "mov r6, r10;" \ |
| "mov r7, r11;" \ |
| "push { r4-r7 };" |
| #define RESTORE_REGS \ |
| "pop { r4-r7 };" \ |
| "mov r8 , r4;" \ |
| "mov r9 , r5;" \ |
| "mov r10, r6;" \ |
| "mov r11, r7;" \ |
| "pop { r4-r7 };" \ |
| "pop { r0 };" \ |
| "mov lr, r0;" /* NOTE: trashes r0 */ |
| #define RESTORE_REGS_PC \ |
| "pop { r4-r7 };" \ |
| "mov r8 , r4;" \ |
| "mov r9 , r5;" \ |
| "mov r10, r6;" \ |
| "mov r11, r7;" \ |
| "pop { r4-r7, pc };" |
| #define SAVE_SP(basereg, tempreg, offset) \ |
| "mov " #tempreg ", sp;" \ |
| "str " #tempreg ", [" #basereg "," #offset "];" |
| #define LOAD_SP(basereg, tempreg, offset) \ |
| "ldr " #tempreg ", [" #basereg "," #offset "];" \ |
| "mov sp, " #tempreg ";" |
| |
| /* there is no clrex on armv6m devices */ |
| #define CLREX "" |
| |
| #endif |
| |
| /* since we're implicitly uniprocessor, store a pointer to the current thread here */ |
| thread_t *_current_thread; |
| |
| static void initial_thread_func(void) __NO_RETURN; |
| static void initial_thread_func(void) |
| { |
| int ret; |
| |
| LTRACEF("thread %p calling %p with arg %p\n", _current_thread, _current_thread->entry, _current_thread->arg); |
| #if LOCAL_TRACE |
| dump_thread(_current_thread); |
| #endif |
| |
| /* release the thread lock that was implicitly held across the reschedule */ |
| thread_unlock_ints_disabled(); |
| arch_enable_ints(); |
| |
| ret = _current_thread->entry(_current_thread->arg); |
| |
| LTRACEF("thread %p exiting with %d\n", _current_thread, ret); |
| |
| thread_exit(ret); |
| } |
| |
| void arch_thread_initialize(struct thread *t) |
| { |
| LTRACEF("thread %p, stack %p\n", t, t->stack); |
| |
| /* find the top of the stack and align it on an 8 byte boundary */ |
| uint32_t *sp = (void *)round_down((vaddr_t)t->stack + t->stack_size, 8); |
| |
| struct arm_cm_context_switch_frame *frame = (void *)sp; |
| frame--; |
| |
| /* arrange for lr to point to our starting routine */ |
| frame->lr = (uint32_t)&initial_thread_func; |
| |
| t->arch.sp = (addr_t)frame; |
| t->arch.was_preempted = false; |
| |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| /* zero the fpu register state */ |
| memset(t->arch.fpregs, 0, sizeof(t->arch.fpregs)); |
| t->arch.fpused = false; |
| #endif |
| } |
| |
| static volatile struct arm_cm_exception_frame_long *preempt_frame; |
| |
| static void pendsv(struct arm_cm_exception_frame_long *frame) |
| { |
| arch_disable_ints(); |
| |
| LTRACEF("preempting thread %p (%s)\n", _current_thread, _current_thread->name); |
| |
| /* save the iframe the pendsv fired on and hit the preemption code */ |
| preempt_frame = frame; |
| thread_preempt(); |
| |
| LTRACEF("fell through\n"); |
| |
| /* if we got here, there wasn't anything to switch to, so just fall through and exit */ |
| preempt_frame = NULL; |
| |
| arch_enable_ints(); |
| } |
| |
| /* |
| * raw pendsv exception handler, triggered by interrupt glue to schedule |
| * a preemption check. |
| */ |
| __NAKED void _pendsv(void) |
| { |
| __asm__ volatile( |
| SAVE_REGS |
| "mov r0, sp;" |
| "bl %c0;" |
| RESTORE_REGS_PC |
| :: "i" (pendsv) |
| ); |
| __UNREACHABLE; |
| } |
| /* |
| * svc handler, used to hard switch the cpu into exception mode to return |
| * to preempted thread. |
| */ |
| __NAKED void _svc(void) |
| { |
| __asm__ volatile( |
| /* load the pointer to the original exception frame we want to restore */ |
| "mov sp, r4;" |
| RESTORE_REGS_PC |
| ); |
| } |
| |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| __NAKED static void _half_save_and_svc(struct thread *oldthread, struct thread *newthread, bool fpu_save, bool restore_fpu) |
| #else |
| __NAKED static void _half_save_and_svc(struct thread *oldthread, struct thread *newthread) |
| #endif |
| { |
| __asm__ volatile( |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| /* see if we need to save fpu context */ |
| "tst r2, #1;" |
| "beq 0f;" |
| |
| /* save part of the fpu context on the stack */ |
| "vmrs r2, fpscr;" |
| "push { r2 };" |
| "vpush { s0-s15 };" |
| |
| /* save the top regs into the thread struct */ |
| "add r2, r0, %[fp_off];" |
| "vstm r2, { s16-s31 };" |
| |
| "0:" |
| #endif |
| |
| /* save regular context */ |
| SAVE_REGS |
| SAVE_SP(r0, r2, %[sp_off]) |
| |
| /* restore the new thread's stack pointer, but not the integer state (yet) */ |
| LOAD_SP(r1, r2, %[sp_off]) |
| |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| /* see if we need to restore fpu context */ |
| "tst r3, #1;" |
| "beq 0f;" |
| |
| /* restore the top part of the fpu context */ |
| "add r3, r1, %[fp_off];" |
| "vldm r3, { s16-s31 };" |
| |
| /* restore the bottom part of the context, stored up the frame a little bit */ |
| "add r3, sp, %[fp_exc_off];" |
| "vldm r3!, { s0-s15 };" |
| "ldr r3, [r3];" |
| "vmsr fpscr, r3;" |
| "b 1f;" |
| |
| /* disable fpu context if we're not restoring anything */ |
| "0:" |
| "mrs r3, CONTROL;" |
| "bic r3, #(1<<2);" /* unset FPCA */ |
| "msr CONTROL, r3;" |
| "isb;" |
| |
| "1:" |
| #endif |
| |
| CLREX |
| "cpsie i;" |
| |
| /* make a svc call to get us into handler mode. |
| * use r4 as an arg, since r0 is saved on the stack for the svc */ |
| "mov r4, sp;" |
| "svc #0;" |
| :: [sp_off] "i"(offsetof(thread_t, arch.sp)) |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| ,[fp_off] "i"(offsetof(thread_t, arch.fpregs)) |
| ,[fp_exc_off] "i"(sizeof(struct arm_cm_exception_frame_long)) |
| #endif |
| ); |
| } |
| |
| /* simple scenario where the to and from thread yielded */ |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| __NAKED static void _arch_non_preempt_context_switch(struct thread *oldthread, struct thread *newthread, bool save_fpu, bool restore_fpu) |
| #else |
| __NAKED static void _arch_non_preempt_context_switch(struct thread *oldthread, struct thread *newthread) |
| #endif |
| { |
| __asm__ volatile( |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| /* see if we need to save fpu context */ |
| "tst r2, #1;" |
| "beq 0f;" |
| |
| /* save part of the fpu context on the stack */ |
| "vmrs r2, fpscr;" |
| "push { r2 };" |
| "vpush { s0-s15 };" |
| |
| /* save the top regs into the thread struct */ |
| "add r2, r0, %[fp_off];" |
| "vstm r2, { s16-s31 };" |
| |
| "0:" |
| #endif |
| |
| /* save regular context */ |
| SAVE_REGS |
| SAVE_SP(r0, r2, %[sp_off]) |
| |
| /* restore new context */ |
| LOAD_SP(r1, r2, %[sp_off]) |
| RESTORE_REGS |
| |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| /* see if we need to restore fpu context */ |
| "tst r3, #1;" |
| "beq 0f;" |
| |
| /* restore fpu context */ |
| "add r3, r1, %[fp_off];" |
| "vldm r3, { s16-s31 };" |
| |
| "vpop { s0-s15 };" |
| "pop { r3 };" |
| "vmsr fpscr, r3;" |
| "b 1f;" |
| |
| /* disable fpu context if we're not restoring anything */ |
| "0:" |
| "mrs r3, CONTROL;" |
| "bic r3, #(1<<2);" /* unset FPCA */ |
| "msr CONTROL, r3;" |
| "isb;" |
| |
| "1:" |
| #endif |
| |
| CLREX |
| "bx lr;" |
| :: [sp_off] "i"(offsetof(thread_t, arch.sp)) |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| , [fp_off] "i"(offsetof(thread_t, arch.fpregs)) |
| #endif |
| ); |
| } |
| |
| __NAKED static void _thread_mode_bounce(bool fpused) |
| { |
| __asm__ volatile( |
| /* restore main context */ |
| RESTORE_REGS |
| |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| /* see if we need to restore fpu context */ |
| "tst r0, #1;" |
| "beq 0f;" |
| |
| /* restore fpu context */ |
| "vpop { s0-s15 };" |
| "pop { r0 };" |
| "vmsr fpscr, r0;" |
| "b 1f;" |
| |
| /* disable fpu context if we're not restoring anything */ |
| "0:" |
| "mrs r3, CONTROL;" |
| "bic r3, #(1<<2);" /* unset FPCA */ |
| "msr CONTROL, r3;" |
| "isb;" |
| |
| "1:" |
| #endif |
| |
| "bx lr;" |
| ); |
| __UNREACHABLE; |
| } |
| |
| /* |
| * The raw context switch routine. Called by the scheduler when it decides to switch. |
| * Called either in the context of a thread yielding or blocking (interrupts disabled, |
| * on the system stack), or inside the pendsv handler on a thread that is being preempted |
| * (interrupts disabled, in handler mode). If preempt_frame is set the thread |
| * is being preempted. |
| */ |
| void arch_context_switch(struct thread *oldthread, struct thread *newthread) |
| { |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| LTRACEF("FPCCR.LSPACT %lu, FPCAR 0x%x, CONTROL.FPCA %lu\n", |
| FPU->FPCCR & FPU_FPCCR_LSPACT_Msk, FPU->FPCAR, __get_CONTROL() & CONTROL_FPCA_Msk); |
| #endif |
| |
| /* if preempt_frame is set, we are being preempted */ |
| if (preempt_frame) { |
| LTRACEF("we're preempted, old frame %p, old lr 0x%x, pc 0x%x, new preempted bool %d\n", |
| preempt_frame, preempt_frame->lr, preempt_frame->pc, newthread->arch.was_preempted); |
| |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| /* see if extended fpu frame was pushed */ |
| if ((preempt_frame->lr & (1<<4)) == 0) { |
| LTRACEF("thread %s pushed fpu frame\n", oldthread->name); |
| |
| /* save the top part of the context */ |
| /* note this should also trigger a lazy fpu save if it hasn't already done so */ |
| asm volatile("vstm %0, { s16-s31 }" :: "r" (&oldthread->arch.fpregs[0])); |
| oldthread->arch.fpused = true; |
| |
| /* verify that FPCCR.LSPACT was cleared and CONTROL.FPCA was set */ |
| DEBUG_ASSERT((FPU->FPCCR & FPU_FPCCR_LSPACT_Msk) == 0); |
| DEBUG_ASSERT(__get_CONTROL() & CONTROL_FPCA_Msk); |
| } else { |
| DEBUG_ASSERT(oldthread->arch.fpused == false); |
| } |
| #endif |
| |
| oldthread->arch.was_preempted = true; |
| oldthread->arch.sp = (addr_t)preempt_frame; |
| preempt_frame = NULL; |
| |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| /* if new thread has saved fpu state, restore it */ |
| if (newthread->arch.fpused) { |
| LTRACEF("newthread FPCCR.LSPACT %lu, FPCAR 0x%x, CONTROL.FPCA %lu\n", |
| FPU->FPCCR & FPU_FPCCR_LSPACT_Msk, FPU->FPCAR, __get_CONTROL() & CONTROL_FPCA_Msk); |
| |
| /* enable the fpu manually */ |
| __set_CONTROL(__get_CONTROL() | CONTROL_FPCA_Msk); |
| asm volatile("isb"); |
| |
| DEBUG_ASSERT((FPU->FPCCR & FPU_FPCCR_LSPACT_Msk) == 0); |
| DEBUG_ASSERT(__get_CONTROL() & CONTROL_FPCA_Msk); |
| |
| /* restore the top of the fpu state, the rest will happen below */ |
| asm volatile("vldm %0, { s16-s31 }" :: "r" (&newthread->arch.fpregs[0])); |
| } |
| #endif |
| |
| if (newthread->arch.was_preempted) { |
| /* return directly to the preempted thread's iframe */ |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| LTRACEF("newthread2 FPCCR.LSPACT %lu, FPCAR 0x%x, CONTROL.FPCA %lu\n", |
| FPU->FPCCR & FPU_FPCCR_LSPACT_Msk, FPU->FPCAR, __get_CONTROL() & CONTROL_FPCA_Msk); |
| #endif |
| __asm__ volatile( |
| "mov sp, %0;" |
| "cpsie i;" |
| CLREX |
| RESTORE_REGS_PC |
| :: "r"(newthread->arch.sp) |
| ); |
| __UNREACHABLE; |
| } else { |
| /* we're inside a pendsv, switching to a user mode thread */ |
| /* set up a fake frame to exception return to */ |
| struct arm_cm_exception_frame_short *frame = (void *)newthread->arch.sp; |
| frame--; |
| |
| frame->pc = (uint32_t)&_thread_mode_bounce; |
| frame->psr = (1 << 24); /* thread bit set, IPSR 0 */ |
| frame->r0 = frame->r1 = frame->r2 = frame->r3 = frame->r12 = frame->lr = 0; |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| /* pass the fpused bool to _thread_mode_bounce */ |
| frame->r0 = newthread->arch.fpused; |
| #endif |
| |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| LTRACEF("iretting to user space, fpused %u\n", newthread->arch.fpused); |
| #else |
| LTRACEF("iretting to user space\n"); |
| #endif |
| |
| __asm__ volatile( |
| CLREX |
| "mov sp, %0;" |
| "bx %1;" |
| :: "r"(frame), "r"(0xfffffff9) |
| ); |
| __UNREACHABLE; |
| } |
| } else { |
| oldthread->arch.was_preempted = false; |
| |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| /* see if we have fpu state we need to save */ |
| if (!oldthread->arch.fpused && __get_CONTROL() & CONTROL_FPCA_Msk) { |
| /* mark this thread as using float */ |
| LTRACEF("thread %s uses float\n", oldthread->name); |
| oldthread->arch.fpused = true; |
| } |
| #endif |
| |
| if (newthread->arch.was_preempted) { |
| LTRACEF("not being preempted, but switching to preempted thread\n"); |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| _half_save_and_svc(oldthread, newthread, oldthread->arch.fpused, newthread->arch.fpused); |
| #else |
| _half_save_and_svc(oldthread, newthread); |
| #endif |
| } else { |
| /* fast path, both sides did not preempt */ |
| LTRACEF("both sides are not preempted newsp 0x%lx\n", newthread->arch.sp); |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| _arch_non_preempt_context_switch(oldthread, newthread, oldthread->arch.fpused, newthread->arch.fpused); |
| #else |
| _arch_non_preempt_context_switch(oldthread, newthread); |
| #endif |
| } |
| } |
| |
| } |
| |
| void arch_dump_thread(thread_t *t) |
| { |
| if (t->state != THREAD_RUNNING) { |
| dprintf(INFO, "\tarch: "); |
| dprintf(INFO, "sp 0x%lx, was preempted %u", t->arch.sp, t->arch.was_preempted); |
| #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) |
| dprintf(INFO, ", fpused %u", t->arch.fpused); |
| #endif |
| dprintf(INFO, "\n"); |
| } |
| } |
| |
| |
