188 lines
8.1 KiB
ArmAsm
188 lines
8.1 KiB
ArmAsm
/*
|
|
* Threads-asm.S - Library for threading on the Teensy.
|
|
*
|
|
*******************
|
|
*
|
|
* Copyright 2017 by Fernando Trias.
|
|
*
|
|
* 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.
|
|
*
|
|
*******************
|
|
*
|
|
* context_switch() changes the context to a new thread. It follows this strategy:
|
|
*
|
|
* 1. Abort if called from within an interrupt (unless using PIT)
|
|
* 2. Save registers r4-r11 to the current thread state (s0-s31 for FPU)
|
|
* 3. If not running on MSP, save PSP to the current thread state
|
|
* 4. Get the next running thread state
|
|
* 5. Restore r4-r11 from thread state (s0-s31 for FPU)
|
|
* 6. Set MSP or PSP depending on state
|
|
* 7. Switch MSP/PSP on return
|
|
*
|
|
* Notes:
|
|
* - Cortex-M has two stack pointers, MSP and PSP, which we alternate. See the
|
|
* reference manual under the Exception Model section.
|
|
* - I tried coding this in asm embedded in Threads.cpp but the compiler
|
|
* optimizations kept changing my code and removing lines so I have to use
|
|
* a separate assembly file. But if you try it, make sure to declare the
|
|
* function "naked" so the stack pointer SP is not modified when called.
|
|
* This means you can't use local variables, which are stored in stack.
|
|
* Try to turn optimizations off using optimize("O0") (which doesn't really
|
|
* turn off all optimizations).
|
|
* - Function can be called from systick_isr() or from the PIT timer (implemented
|
|
* by IntervalTimer)
|
|
* - If using systick, we override the default systick_isr() in order
|
|
* to preserve the stack and LR. If using PIT, we override the pitX_isr() for
|
|
* the same reason.
|
|
* - Since Systick can be called from within another interrupt, for simplicity, we
|
|
* check for this and abort.
|
|
* - Teensy uses MSP for it's main thread; we preserve that. Alternatively, we
|
|
* could have used PSP for all threads, including main, and reserve MSP for
|
|
* interrupts only. This would simplify the code slightly, but could introduce
|
|
* incompatabilities.
|
|
* - If this interrupt is nested within another interrupt, all kinds of bad
|
|
* things can happen. This is especially true if usb_isr() is active. In theory
|
|
* we should be able to do a switch even within an interrupt, but in my
|
|
* tests, it would not work reliably.
|
|
* - If using the PIT interrupt, it's priority is set to 255 (the lowest) so it
|
|
* cannot interrupt an interrupt.
|
|
*/
|
|
|
|
.syntax unified
|
|
.align 2
|
|
.thumb
|
|
|
|
.global context_switch_direct
|
|
.thumb_func
|
|
context_switch_direct:
|
|
CPSID I
|
|
// Call here to force a context switch, so we skip checking the tick counter.
|
|
B call_direct
|
|
|
|
.global context_switch_direct_active
|
|
.thumb_func
|
|
context_switch_direct_active:
|
|
CPSID I
|
|
// Call here to force a context switch, so we skip checking the tick counter.
|
|
B call_direct_active
|
|
|
|
.global context_switch_pit_isr
|
|
.thumb_func
|
|
context_switch_pit_isr:
|
|
CPSID I
|
|
LDR r0, =context_timer_flag // acknowledge the interrupt by
|
|
LDR r0, [r0] // getting the pointer to the pointer
|
|
MOVS r1, #1 //
|
|
STR r1, [r0] // and setting to 1
|
|
B context_switch_check // now go do the context switch
|
|
|
|
.global context_switch
|
|
.thumb_func
|
|
context_switch:
|
|
|
|
// Disable all interrupts; if we get interrupted during a context switch this
|
|
// could corrupt the system.
|
|
CPSID I
|
|
|
|
// Did we interrupt another interrupt? If so, don't switch. Switching would
|
|
// wreck the system. In theory, we could reschedule the switch until the
|
|
// other interrupt is done. Or we could do a more sophisticated switch, but the
|
|
// easiest thing is to just ignore this condition.
|
|
CMP lr, #0xFFFFFFF1 // this means we interrupted an interrupt
|
|
BEQ to_exit // so don't do anything until next time
|
|
CMP lr, #0xFFFFFFE1 // this means we interrupted an interrupt with FPU
|
|
BEQ to_exit // so don't do anything until next time
|
|
|
|
context_switch_check:
|
|
|
|
// Count down number of ticks we should stay in thread
|
|
LDR r0, =currentCount // get the tick count (address to variable)
|
|
LDR r1, [r0] // get the value from the address
|
|
CMP r1, #0 // is it 0?
|
|
BEQ call_direct // if so, thread is done, so switch
|
|
SUB r1, #1 // otherwise, subtract 1 tick
|
|
STR r1, [r0] // and put it back
|
|
B to_exit // and quit until next context_switch
|
|
|
|
call_direct:
|
|
|
|
// Just do the context-switch (even if it's not time)
|
|
LDR r0, =currentActive // If the thread isn't active, skip it
|
|
LDR r0, [r0]
|
|
CMP r0, #1
|
|
BNE to_exit
|
|
|
|
call_direct_active:
|
|
|
|
// Save the r4-r11 registers; (r0-r3,r12 are saved by the interrupt handler).
|
|
// Most thread libraries save this to the thread stack. I don't for simplicity
|
|
// and to make debugging easier. Since the Teensy doesn't have a debugging port,
|
|
// it's hard to examine the stack so this is easier.
|
|
LDR r0, =currentSave // get the address of the pointer
|
|
LDR r0, [r0] // get the pointer itself
|
|
STMIA r0!, {r4-r11,lr} // save r4-r11 to buffer
|
|
|
|
#ifdef __ARM_PCS_VFP // compile if using FPU
|
|
VSTMIA r0!, {s0-s31} // save all FPU registers
|
|
VMRS r1, FPSCR // and FPU app status register
|
|
STMIA r0!, {r1}
|
|
#endif
|
|
|
|
// Are we running on thread 0, which is MSP?
|
|
// It so, there is no need to save the stack pointer because MSP is never changed.
|
|
// If not, save the stack pointer.
|
|
LDR r0, =currentMSP // get the address of the variable
|
|
LDR r0, [r0] // get value from address
|
|
CMP r0, #0 // it is 0? This means it's PSP
|
|
BNE current_is_msp // not 0, so MSP, we can skip saving SP
|
|
MRS r0, psp // get the PSP value
|
|
LDR r1, =currentSP // get the address of our save variable
|
|
STR r0, [r1] // and store the PSP value there
|
|
current_is_msp:
|
|
|
|
BL loadNextThread; // set the state to next running thread
|
|
|
|
// Restore the r4-r11 registers from the saved thread
|
|
LDR r0, =currentSave // get address of pointer save buffer
|
|
LDR r0, [r0] // get the actual pointer
|
|
LDMIA r0!, {r4-r11,lr} // and restore r4-r11 & lr from save buffer
|
|
|
|
#ifdef __ARM_PCS_VFP // compile if using FPU
|
|
VLDMIA r0!, {s0-s31} // restore all FPU registers
|
|
LDMIA r0!, {r1} // and the FP app status register
|
|
VMSR FPSCR, r1
|
|
#endif
|
|
|
|
// Setting LR causes the handler to switch MSP/PSP when returning.
|
|
// Switching to MSP? no need to restore MSP.
|
|
AND lr, lr, #0x10 // return stack with FP bit?
|
|
ORR lr, lr, #0xFFFFFFE9 // add basic LR bits
|
|
LDR r0, =currentMSP // get address of the variable
|
|
LDR r0, [r0] // get the actual value
|
|
CMP r0, #0 // is it 0? Then it's PSP
|
|
BNE to_exit // it's not 0, so it's MSP, all done
|
|
// if it's PSP, we need to switch PSP
|
|
LDR r0, =currentSP // get address of stack pointer
|
|
LDR r0, [r0] // get the actual value
|
|
MSR psp, r0 // save it to PSP
|
|
ORR lr, lr, #0b100 // set the PSP context switch
|
|
|
|
to_exit:
|
|
// Re-enable interrupts
|
|
CPSIE I
|
|
// Return. The CPU will change MSP/PSP as needed based on LR
|
|
BX lr
|