135 lines
2.2 KiB
ArmAsm
135 lines
2.2 KiB
ArmAsm
/*
|
|
* context_saverestore.S
|
|
*
|
|
* Copyright (c) 2022 Semidrive Semiconductor.
|
|
* All rights reserved.
|
|
*
|
|
* Description:
|
|
*
|
|
* Revision History:
|
|
* -----------------
|
|
*/
|
|
|
|
portMPU_RegisterSave MACRO
|
|
|
|
MOV r0, #0
|
|
mpu_save_loop:
|
|
MCR p15, 0, r0, c6, c2, 0
|
|
MRC p15, 0, r2, c6, c1, 0
|
|
STR r2, [r1], #4
|
|
MRC p15, 0, r2, c6, c1, 4
|
|
STR r2, [r1], #4
|
|
MRC p15, 0, r2, c6, c1, 2
|
|
STR r2, [r1], #4
|
|
ADD r0, r0, #1
|
|
CMP r0, #16
|
|
BNE mpu_save_loop
|
|
MRC p15, 0, r2, c1, c0, 0
|
|
STR r2, [r1], #4
|
|
|
|
ENDM
|
|
|
|
; /**********************************************************************/
|
|
|
|
portMPU_RegisterRestore MACRO
|
|
|
|
MRC p15, 0, r2, c1, c0, 0
|
|
LDR r0, =(SCTLR_BR | SCTLR_M)
|
|
BIC r2, r2, r0
|
|
MCR p15, 0, r2, c1, c0, 0
|
|
|
|
MOV r0, #0
|
|
mpu_restore_loop:
|
|
MCR p15, 0, r0, c6, c2, 0
|
|
LDR r2, [r1], #4
|
|
MCR p15, 0, r2, c6, c1, 0
|
|
LDR r2, [r1], #4
|
|
MCR p15, 0, r2, c6, c1, 4
|
|
LDR r2, [r1], #4
|
|
MCR p15, 0, r2, c6, c1, 2
|
|
ADD r0, r0, #1
|
|
CMP r0, #16
|
|
BNE mpu_restore_loop
|
|
|
|
MRC p15, 0, r0, c1, c0, 0
|
|
LDR r2, =(SCTLR_BR | SCTLR_M)
|
|
ORR r0, r0, r2
|
|
LDR r2, [r1], #4
|
|
TST r2, #SCTLR_M
|
|
MCRNE p15, 0, r0, c1, c0, 0
|
|
|
|
ENDM
|
|
|
|
; /**********************************************************************/
|
|
|
|
portCPU_RegisterSave MACRO
|
|
|
|
PUSH {r0-r3, r12}
|
|
|
|
STMIA r1, {r0-r12}
|
|
STR lr, [r1, #(4*REG_R15)]
|
|
MRS r2, SPSR
|
|
STR r2, [r1, #(4*REG_CPSR)]
|
|
|
|
#if CONFIG_ARCH_WITH_FPU
|
|
ADD r3, r1, #(4*REG_S0)
|
|
VSTMIA r3!, {s0-s31}
|
|
VMRS r2, fpscr
|
|
STR r2, [r3], #4
|
|
#endif
|
|
|
|
CPS #SYS_MODE
|
|
ADD r3, r1, #(4*REG_R13)
|
|
STMIA r3!, {sp, lr}
|
|
|
|
CPS #SVC_MODE
|
|
ADD r1, r1, #XCPTCONTEXT_SIZE
|
|
portMPU_RegisterSave
|
|
|
|
PUSH {lr}
|
|
LDR r0, arch_clean_invalidate_dcache_all_const
|
|
BLX r0
|
|
POP {lr}
|
|
|
|
POP {r0-r3, r12}
|
|
MOVS pc, lr
|
|
|
|
ENDM
|
|
|
|
; /**********************************************************************/
|
|
|
|
portCPU_RegisterRestore MACRO
|
|
|
|
#if CONFIG_ARCH_WITH_FPU
|
|
ADD r3, r1, #(4*REG_S0)
|
|
VLDMIA r3!, {s0-s31}
|
|
LDR r2, [r3], #4
|
|
VMSR fpscr, r2
|
|
#endif
|
|
|
|
ADD r4, r1, #(4*REG_R15)
|
|
LDMIA r4, {r2, r3}
|
|
MOV lr, r2
|
|
MSR SPSR_cxsf, r3
|
|
|
|
CPS #SYS_MODE
|
|
ADD r4, r1, #(4*REG_R13)
|
|
LDMIA r4, {r2, r3}
|
|
MOV sp, r2
|
|
MOV lr, r3
|
|
|
|
PUSH {lr}
|
|
MOV lr, r1
|
|
|
|
ADD r1, r1, #XCPTCONTEXT_SIZE
|
|
portMPU_RegisterRestore
|
|
|
|
LDMIA lr, {r0-r12}
|
|
POP {lr}
|
|
|
|
CPS #SVC_MODE
|
|
MOVS pc, lr
|
|
|
|
ENDM
|
|
|