/* SPDX-License-Identifier: BSD-2-Clause */
/*
 * Copyright (c) 2016, Linaro Limited
 * Copyright (c) 2014, STMicroelectronics International N.V.
 */

#include <arm32_macros.S>
#include <arm.h>
#include <asm.S>
#include <generated/asm-defines.h>
#include <keep.h>
#include <kernel/unwind.h>
#include <sm/optee_smc.h>
#include <sm/sm.h>
#include <sm/teesmc_opteed.h>
#include <sm/teesmc_opteed_macros.h>
#include <util.h>

#define SM_CTX_SEC_END	(SM_CTX_SEC + SM_CTX_SEC_SIZE)

	.macro save_regs mode
	cps	\mode
	mrs	r2, spsr
	str	r2, [r0], #4
	str	sp, [r0], #4
	str	lr, [r0], #4
	.endm

FUNC sm_save_unbanked_regs , :
UNWIND(	.fnstart)
UNWIND(	.cantunwind)
	/* User mode registers has to be saved from system mode */
	cps	#CPSR_MODE_SYS
	str	sp, [r0], #4
	str	lr, [r0], #4

	save_regs	#CPSR_MODE_IRQ
	save_regs	#CPSR_MODE_FIQ
	save_regs	#CPSR_MODE_SVC
	save_regs	#CPSR_MODE_ABT
	save_regs	#CPSR_MODE_UND

#ifdef CFG_SM_NO_CYCLE_COUNTING
	read_pmcr r2
	stm	r0!, {r2}
#endif

#ifdef CFG_FTRACE_SUPPORT
	read_cntkctl r2
	stm	r0!, {r2}
#endif
	cps	#CPSR_MODE_MON
	bx	lr
UNWIND(	.fnend)
END_FUNC sm_save_unbanked_regs

	.macro restore_regs mode
	cps	\mode
	ldr	r2, [r0], #4
	ldr	sp, [r0], #4
	ldr	lr, [r0], #4
	msr	spsr_fsxc, r2
	.endm

/* Restores the mode specific registers */
FUNC sm_restore_unbanked_regs , :
UNWIND(	.fnstart)
UNWIND(	.cantunwind)
	/* User mode registers has to be saved from system mode */
	cps	#CPSR_MODE_SYS
	ldr	sp, [r0], #4
	ldr	lr, [r0], #4

	restore_regs	#CPSR_MODE_IRQ
	restore_regs	#CPSR_MODE_FIQ
	restore_regs	#CPSR_MODE_SVC
	restore_regs	#CPSR_MODE_ABT
	restore_regs	#CPSR_MODE_UND

#ifdef CFG_SM_NO_CYCLE_COUNTING
	ldm	r0!, {r2}
	write_pmcr r2
#endif

#ifdef CFG_FTRACE_SUPPORT
	ldm	r0!, {r2}
	write_cntkctl r2
#endif
	cps	#CPSR_MODE_MON
	bx	lr
UNWIND(	.fnend)
END_FUNC sm_restore_unbanked_regs

/*
 * stack_tmp is used as stack, the top of the stack is reserved to hold
 * struct sm_ctx, everything below is for normal stack usage. As several
 * different CPU modes are using the same stack it's important that switch
 * of CPU mode isn't done until one mode is done. This means FIQ, IRQ and
 * Async abort has to be masked while using stack_tmp.
 */
LOCAL_FUNC sm_smc_entry , :
UNWIND(	.fnstart)
UNWIND(	.cantunwind)
	srsdb	sp!, #CPSR_MODE_MON
	push	{r0-r7}

	clrex		/* Clear the exclusive monitor */

	/* Find out if we're doing an secure or non-secure entry */
	read_scr r1
	tst	r1, #SCR_NS
	bne	.smc_from_nsec

	/*
	 * As we're coming from secure world (NS bit cleared) the stack
	 * pointer points to sm_ctx.sec.r0 at this stage. After the
	 * instruction below the stack pointer points to sm_ctx.
	 */
	sub	sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0)

	/* Save secure context */
	add	r0, sp, #SM_CTX_SEC
	bl	sm_save_unbanked_regs

	/*
	 * On FIQ exit we're restoring the non-secure context unchanged, on
	 * all other exits we're shifting r1-r4 from secure context into
	 * r0-r3 in non-secure context.
	 */
	add	r8, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0)
	ldm	r8, {r0-r4}
	mov_imm	r9, TEESMC_OPTEED_RETURN_FIQ_DONE
	cmp	r0, r9
	addne	r8, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)
	stmne	r8, {r1-r4}

	/* Restore non-secure context */
	add	r0, sp, #SM_CTX_NSEC
	bl	sm_restore_unbanked_regs

.sm_ret_to_nsec:
	/*
	 * Return to non-secure world
	 */
	add     r0, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R8)
	ldm	r0, {r8-r12}

#ifdef CFG_CORE_WORKAROUND_NSITR_CACHE_PRIME
	/*
	 * Prevent leaking information about which code has been executed.
	 * This is required to be used together with
	 * CFG_CORE_WORKAROUND_SPECTRE_BP to protect Cortex A15 CPUs too.
	 *
	 * CFG_CORE_WORKAROUND_SPECTRE_BP also invalidates the branch
	 * predictor on affected CPUs. In the cases where an alternative
	 * vector has been installed the branch predictor is already
	 * invalidated so invalidating here again would be redundant, but
	 * testing for that is more trouble than it's worth.
	 */
	write_bpiall
#endif

	/* Update SCR */
	read_scr r0
	orr	r0, r0, #(SCR_NS | SCR_FIQ) /* Set NS and FIQ bit in SCR */
	write_scr r0
	/*
	 * isb not needed since we're doing an exception return below
	 * without dependency to the changes in SCR before that.
	 */

	add	sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)
	b	.sm_exit

.smc_from_nsec:
	/*
	 * As we're coming from non-secure world (NS bit set) the stack
	 * pointer points to sm_ctx.nsec.r0 at this stage. After the
	 * instruction below the stack pointer points to sm_ctx.
	 */
	sub	sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)

	bic	r1, r1, #(SCR_NS | SCR_FIQ) /* Clear NS and FIQ bit in SCR */
	write_scr r1
	isb

	add	r0, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R8)
	stm	r0, {r8-r12}

	mov	r0, sp
	bl	sm_from_nsec
	cmp	r0, #SM_EXIT_TO_NON_SECURE
	beq	.sm_ret_to_nsec

	/*
	 * Continue into secure world
	 */
	add	sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_R0)

.sm_exit:
	pop	{r0-r7}
	rfefd	sp!
UNWIND(	.fnend)
END_FUNC sm_smc_entry

/*
 * FIQ handling
 *
 * Saves CPU context in the same way as sm_smc_entry() above. The CPU
 * context will later be restored by sm_smc_entry() when handling a return
 * from FIQ.
 */
LOCAL_FUNC sm_fiq_entry , :
UNWIND(	.fnstart)
UNWIND(	.cantunwind)
	/* FIQ has a +4 offset for lr compared to preferred return address */
	sub	lr, lr, #4
	/* sp points just past struct sm_sec_ctx */
	srsdb	sp!, #CPSR_MODE_MON
	push	{r0-r7}

	clrex		/* Clear the exclusive monitor */

	/*
	 * As we're coming from non-secure world the stack pointer points
	 * to sm_ctx.nsec.r0 at this stage. After the instruction below the
	 * stack pointer points to sm_ctx.
	 */
	sub	sp, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R0)

	/* Update SCR */
	read_scr r1
	bic	r1, r1, #(SCR_NS | SCR_FIQ) /* Clear NS and FIQ bit in SCR */
	write_scr r1
	isb

	/* Save non-secure context */
	add	r0, sp, #SM_CTX_NSEC
	bl	sm_save_unbanked_regs
	add     r0, sp, #(SM_CTX_NSEC + SM_NSEC_CTX_R8)
	stm	r0!, {r8-r12}

	/* Set FIQ entry */
	ldr	r0, =(thread_vector_table + THREAD_VECTOR_TABLE_FIQ_ENTRY)
	str	r0, [sp, #(SM_CTX_SEC + SM_SEC_CTX_MON_LR)]

	/* Restore secure context */
	add	r0, sp, #SM_CTX_SEC
	bl	sm_restore_unbanked_regs

	add	sp, sp, #(SM_CTX_SEC + SM_SEC_CTX_MON_LR)

	rfefd	sp!
UNWIND(	.fnend)
END_FUNC sm_fiq_entry

	.section .text.sm_vect_table
        .align	5
LOCAL_FUNC sm_vect_table , :
UNWIND(	.fnstart)
UNWIND(	.cantunwind)
	b	.		/* Reset			*/
	b	.		/* Undefined instruction	*/
	b	sm_smc_entry	/* Secure monitor call		*/
	b	.		/* Prefetch abort		*/
	b	.		/* Data abort			*/
	b	.		/* Reserved			*/
	b	.		/* IRQ				*/
	b	sm_fiq_entry	/* FIQ				*/

#ifdef CFG_CORE_WORKAROUND_SPECTRE_BP
	.macro vector_prologue_spectre
		/*
		 * This depends on SP being 8 byte aligned, that is, the
		 * lowest three bits in SP are zero.
		 *
		 * The idea is to form a specific bit pattern in the lowest
		 * three bits of SP depending on which entry in the vector
		 * we enter via.  This is done by adding 1 to SP in each
		 * entry but the last.
		 */
		add	sp, sp, #1	/* 7:111 Reset			*/
		add	sp, sp, #1	/* 6:110 Undefined instruction	*/
		add	sp, sp, #1	/* 5:101 Secure monitor call	*/
		add	sp, sp, #1	/* 4:100 Prefetch abort		*/
		add	sp, sp, #1	/* 3:011 Data abort		*/
		add	sp, sp, #1	/* 2:010 Reserved		*/
		add	sp, sp, #1	/* 1:001 IRQ			*/
		nop			/* 0:000 FIQ			*/
	.endm

	.align 5
sm_vect_table_a15:
	vector_prologue_spectre
	/*
	 * Invalidate the branch predictor for the current processor.
	 * For Cortex-A8 ACTLR[6] has to be set to 1 for BPIALL to be
	 * effective.
	 * Note that the BPIALL instruction is not effective in
	 * invalidating the branch predictor on Cortex-A15. For that CPU,
	 * set ACTLR[0] to 1 during early processor initialisation, and
	 * invalidate the branch predictor by performing an ICIALLU
	 * instruction. See also:
	 * https://github.com/ARM-software/arm-trusted-firmware/wiki/Arm-Trusted-Firmware-Security-Advisory-TFV-6#variant-2-cve-2017-5715
	 */
	write_iciallu
	isb
	b	1f

	.align 5
sm_vect_table_bpiall:
	vector_prologue_spectre
	/* Invalidate the branch predictor for the current processor. */
	write_bpiall
	isb

1:
	/*
	 * Only two exception does normally occur, smc and fiq. With all
	 * other exceptions it's good enough to just spinn, the lowest bits
	 * still tells which exception we're stuck with when attaching a
	 * debugger.
	 */

	/* Test for FIQ, all the lowest bits of SP are supposed to be 0 */
	tst	sp, #(BIT(0) | BIT(1) | BIT(2))
	beq	sm_fiq_entry

	/* Test for SMC, xor the lowest bits of SP to be 0 */
	eor	sp, sp, #(BIT(0) | BIT(2))
	tst	sp, #(BIT(0) | BIT(1) | BIT(2))
	beq	sm_smc_entry

	/* unhandled exception */
	b	.
#endif /*!CFG_CORE_WORKAROUND_SPECTRE_BP*/
UNWIND(	.fnend)
END_FUNC sm_vect_table

/* void sm_init(vaddr_t stack_pointer); */
FUNC sm_init , :
UNWIND(	.fnstart)
	/* Set monitor stack */
	mrs	r1, cpsr
	cps	#CPSR_MODE_MON
	/* Point just beyond sm_ctx.sec */
	sub	sp, r0, #(SM_CTX_SIZE - SM_CTX_SEC_END)

#ifdef CFG_INIT_CNTVOFF
	read_scr r0
	orr	r0, r0, #SCR_NS /* Set NS bit in SCR */
	write_scr r0
	isb

	/*
	 * Accessing CNTVOFF:
	 * If the implementation includes the Virtualization Extensions
	 * this is a RW register, accessible from Hyp mode, and
	 * from Monitor mode when SCR.NS is set to 1.
	 * If the implementation includes the Security Extensions
	 * but not the Virtualization Extensions, an MCRR or MRRC to
	 * the CNTVOFF encoding is UNPREDICTABLE if executed in Monitor
	 * mode, regardless of the value of SCR.NS.
	 */
	read_id_pfr1 r2
	mov	r3, r2
	ands    r3, r3, #IDPFR1_GENTIMER_MASK
	beq	.no_gentimer
	ands    r2, r2, #IDPFR1_VIRT_MASK
	beq	.no_gentimer
	mov	r2, #0
	write_cntvoff r2, r2

.no_gentimer:
	bic	r0, r0, #SCR_NS /* Clr NS bit in SCR */
	write_scr r0
	isb
#endif
#ifdef CFG_SM_NO_CYCLE_COUNTING
	read_pmcr r0
	orr	r0, #PMCR_DP
	write_pmcr r0
#endif
	msr	cpsr, r1

#ifdef CFG_CORE_WORKAROUND_SPECTRE_BP
	/*
	 * For unrecognized CPUs we fall back to the vector used for
	 * unaffected CPUs. Cortex A-15 has special treatment compared to
	 * the other affected Cortex CPUs.
	 */
	read_midr r1
	ubfx	r2, r1, #MIDR_IMPLEMENTER_SHIFT, #MIDR_IMPLEMENTER_WIDTH
	cmp	r2, #MIDR_IMPLEMENTER_ARM
	bne	1f

	ubfx	r2, r1, #MIDR_PRIMARY_PART_NUM_SHIFT, \
			#MIDR_PRIMARY_PART_NUM_WIDTH

	movw	r3, #CORTEX_A8_PART_NUM
	cmp	r2, r3
	movwne	r3, #CORTEX_A9_PART_NUM
	cmpne	r2, r3
	movwne	r3, #CORTEX_A17_PART_NUM
	cmpne	r2, r3
	ldreq	r0, =sm_vect_table_bpiall
	beq	2f

	movw	r3, #CORTEX_A15_PART_NUM
	cmp	r2, r3
	ldreq	r0, =sm_vect_table_a15
	beq	2f
#endif
	/* Set monitor vector (MVBAR) */
1:	ldr	r0, =sm_vect_table
2:	write_mvbar r0

	bx	lr
END_FUNC sm_init
KEEP_PAGER sm_init


/* struct sm_nsec_ctx *sm_get_nsec_ctx(void); */
FUNC sm_get_nsec_ctx , :
	mrs	r1, cpsr
	cps	#CPSR_MODE_MON
	/*
	 * As we're in secure mode mon_sp points just beyond sm_ctx.sec,
	 * which allows us to calculate the address of sm_ctx.nsec.
	 */
	add	r0, sp, #(SM_CTX_NSEC - SM_CTX_SEC_END)
	msr	cpsr, r1

	bx	lr
END_FUNC sm_get_nsec_ctx