1/* SPDX-License-Identifier: BSD-2-Clause */
2/*
3 * Copyright (C) 2017 Texas Instruments Incorporated - http://www.ti.com/
4 *	Andrew Davis <afd@ti.com>
5 */
6
7/*
8 * Entry points for the A9 init.
9 * It is assumed no stack is available when these routines are called.
10 * It is assumed each routine is called with return address in LR
11 * and with ARM registers R0, R1, R2, R3 being scratchable.
12 */
13
14#include <asm.S>
15#include <platform_config.h>
16#include <sm/optee_smc.h>
17#include <sm/teesmc_opteed.h>
18#include <sm/teesmc_opteed_macros.h>
19
20.arch_extension sec
21
22.section .text
23.balign 4
24.code 32
25
26booted:
27	.word	0
28
29/*
30 * Cortex A9 check for resume
31 *
32 * Use scratables registers R0-R3.
33 * No stack usage.
34 * LR store return address.
35 * Trap CPU in case of error.
36 */
37FUNC plat_cpu_reset_early , :
38	/* Check if we are resuming */
39	ldr	r3, =booted
40	ldr	r2, [r3]
41	cmp	r2, #0
42	/* Cold boot, mark our boot flag and return to normal boot */
43	moveq	r2, #1
44	streq	r2, [r3]
45	bxeq	lr
46	/* Otherwise we are resuming */
47	b	resume_springboard
48END_FUNC plat_cpu_reset_early
49
50LOCAL_FUNC resume_springboard , :
51UNWIND(	.cantunwind)
52	/* Setup tmp stack */
53	bl	__get_core_pos
54	cmp	r0, #CFG_TEE_CORE_NB_CORE
55	/* Unsupported CPU, park it before it breaks something */
56unhandled_cpu:
57	wfige
58	bge	unhandled_cpu
59	add	r0, r0, #1
60	ldr	r1, =stack_tmp_stride
61	ldr	r1, [r1]
62	mul	r1, r0, r1
63	ldr	r0, =stack_tmp
64#if (STACK_TMP_GUARD != 0)
65	mov_imm	r2, STACK_TMP_GUARD
66	sub	r0, r0, r2
67#endif
68	add	sp, r1, r0
69
70	/* Push our return on the stack as sm_pm_cpu_do_resume expects */
71	adr	lr, after_resume
72	push	{r4 - r12, lr}
73
74	/* Assumes suspend_regs is flat-mapped */
75	ldr	r0, =suspend_regs
76	bl	sm_pm_cpu_do_resume
77
78after_resume:
79	bl	thread_init_per_cpu
80
81	/* r5 contains the non-secure entry address (ARMv7 bootarg #0) */
82	mov	r0, r5
83	bl	init_sec_mon
84
85	bl	main_init_gic
86
87	mov	r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE
88	mov	r1, #0
89	mov	r2, #0
90	mov	r3, #0
91	mov	r4, #0
92	smc	#0
93	b	.	/* SMC should not return */
94END_FUNC resume_springboard
95