/*
 * 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