ardupilot/libraries/AP_HAL_F4Light/wirish/exc.S

193 lines
6.6 KiB
ArmAsm

/* *****************************************************************************
* The MIT License
*
exception handling and task switcher, (C) 2017 night_ghost@ykoctpa.ru
adedd some useful info to __error handler and debugger, dual stacks support and task switching code
based on:
* Copyright (c) 2010 Perry Hung.
*
* 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.
* ****************************************************************************/
# On an exception, push a fake stack thread mode stack frame and redirect
# thread execution to a thread mode error handler
# From RM008:
# The SP is decremented by eight words by the completion of the stack push.
# Figure 5-1 shows the contents of the stack after an exception pre-empts the
# current program flow.
#
# Old SP--> <previous>
# xPSR 28
# PC 24
# LR 20
# r12 16
# r3 12
# r2 8
# r1 4
# SP--> r0 0
.syntax unified
.cpu cortex-m4
.text
.globl HardFault_Handler
.globl NMI_Handler
.globl MemManage_Handler
.globl BusFault_Handler
.globl UsageFault_Handler
.globl __default_exc
.globl __do_context_switch
.code 16
.thumb_func
HardFault_Handler:
mov r0, #2
b __default_exc
.thumb_func
MemManage_Handler:
mov r0, #3
b __default_exc
.thumb_func
BusFault_Handler:
mov r0, #4
b __default_exc
.thumb_func
UsageFault_Handler:
mov r0, #5
b __default_exc
.thumb_func
FLASH_IRQHandler:
mov r0, #6
b __default_exc
.thumb_func
__default_exc:
tst lr, #4
ite eq
mrseq r1, msp
mrsne r1, psp
ldr r3, [r1, #24] @ PC of exception - if access to wrong address
ldr r1, [r1, #20] @ LR of exception - if call to wrong address
ldr r2, BFAR @ for debug, to see what happens
ldr r2, [r2]
ldr r2, CFSR
ldr r2, [r2]
ldr r2, HFSR
ldr r2, [r2]
ldr r2, DFSR
ldr r2, [r2]
ldr r2, AFSR
ldr r2, [r2]
ldr r2, SHCSR
ldr r2, [r2]
ldr r2, MMFAR
ldr r2, [r2]
mov r12, r1
ldr r2, NVIC_CCR @ Enable returning to thread mode even if there are
mov r1 ,#1 @ pending exceptions. See flag NONEBASETHRDENA - http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0337e/Cihcbadd.html
str r1, [r2]
cpsid i @ Disable global interrupts
mov r1, #0
ldr r2, SYSTICK_CSR @ Disable systick handler
str r1, [r2]
ldr r2, MPU_CTRL @ disable MPU
str r1, [r2]
ldr r1, CPSR_MASK @ Set default CPSR
mov r2, r12
push {r1} @ SP+4
ldr r1, TARGET_PC @ Set target pc
push {r1} @ SP+8
push {r1} @ target LR - the same SP+12
ldr r1, [sp, #12] @ R0 of interrupted program
push {r1} @ will be in R12 SP+16
ldr r1, [sp, #28] @ R3 of interrupted program (12+16)
push {r1} @ will be in R3
push {r2} @ R2 - LR
push {r3} @ R1 - PC of exception
push {r0} @ R0 - exception code
tst lr, #4
ite eq
ldreq r1, EXC_RETURN @ Return to thread mode with PSP
ldrne r1, EXC_RETURN @ Return to thread mode with MSP
mov lr, r1
bx lr @ Exception exit
.thumb_func
__do_context_switch: @ we already in interrupt so all interrupts with higher priority will use MSP - so dont need to disable interrupts
MRS R0, PSP @ PSP is process stack pointer
TST LR, #0x10 @ exc_return[4]=0? (it means that current process
IT EQ @ has active floating point context)
VSTMDBEQ R0!, {S16-S31} @ if so - save it.
STMDB R0!, {R4-R11, LR} @ save remaining regs r4-11 and LR (EXC_RETURN) on process stack
@ At this point, entire context of process has been saved
LDR R2, px_running @ address of s_running
LDR R1, [R2] @ value of s_running - address of old task_t
STR R0, [R1] @ store stack pointer
LDR R3, px_nextTask @ address of next_task
LDR R1, [R3] @ value of next_task - address of new task_t
STR R1, [R2] @ save to s_running
LDR R0, [R1] @ R0 is new process SP
@load context of new process
LDMIA R0!, {R4-R11, LR} @ Restore r4-11 and LR from new process stack
TST LR, #0x10 @ exc_return[4]=0? (it means that new process
IT EQ @ has active floating point context)
VLDMIAEQ R0!, {S16-S31} @ if so - restore it.
MSR PSP, R0 @ Load PSP with new process SP
BX LR @ Return to saved exc_return. Exception return will restore remaining context
.align 4
CPSR_MASK: .word 0x61000000
EXC_RETURN: .word 0xFFFFFFF9
EXC_RETURN_PSP: .word 0xFFFFFFFD
TARGET_PC: .word __error
NVIC_CCR: .word 0xE000ED14 @ NVIC configuration control register
SYSTICK_CSR: .word 0xE000E010 @ Systick control register
MPU_CTRL: .word 0xE000ED94 @ MPU Control register
BFAR: .word 0xE000ED38
CFSR: .word 0xE000ED28
HFSR: .word 0xE000ED2C
DFSR: .word 0xE000ED30
AFSR: .word 0xE000ED3C
SHCSR: .word 0xE000ED24
MMFAR: .word 0xE000ED34 @ MemManage Fault Address register
px_running: .word s_running
px_nextTask: .word next_task