plat-ti: Add Suspend/Resume support for AM43xx

When the non-secure world is attempting to suspend it will call into the
secure side using a platform service call. We implement this here in
OP-TEE by saving the needed secure side registers.

On resume the ROM will restore the secure side to its original
configuration and OP-TEE will be re-entered from its boot reset vector.
Add a check for the resume case and restore the secure registers if we
are resuming.

Signed-off-by: Andrew F. Davis <afd@ti.com>
Acked-by: Jerome Forissier <jerome.forissier@linaro.org>
Acked-by: Etienne Carriere <etienne.carriere@linaro.org>

5 files changed, 144 insertions, 1 deletions

diff --git a/core/arch/arm/plat-ti/a9_plat_init.S b/core/arch/arm/plat-ti/a9_plat_init.S
new file mode 100644
index 00000000..45879c1b
--- /dev/null
+++ b/core/arch/arm/plat-ti/a9_plat_init.S
@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) 2017, Texas Instruments
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES// LOSS OF USE, DATA, OR PROFITS// OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*
+ * Entry points for the A9 init.
+ * It is assumed no stack is available when these routines are called.
+ * It is assumed each routine is called with return address in LR
+ * and with ARM registers R0, R1, R2, R3 being scratchable.
+ */
+
+#include <asm.S>
+#include <kernel/unwind.h>
+#include <platform_config.h>
+#include <sm/optee_smc.h>
+#include <sm/teesmc_opteed.h>
+#include <sm/teesmc_opteed_macros.h>
+
+.section .text
+.balign 4
+.code 32
+
+booted:
+	.word	0
+
+/*
+ * Cortex A9 check for resume
+ *
+ * Use scratables registers R0-R3.
+ * No stack usage.
+ * LR store return address.
+ * Trap CPU in case of error.
+ */
+FUNC plat_cpu_reset_early , :
+UNWIND(	.fnstart)
+	/* Check if we are resuming */
+	ldr	r3, =booted
+	ldr	r2, [r3]
+	cmp	r2, #0
+	/* Cold boot, mark our boot flag and return to normal boot */
+	moveq	r2, #1
+	streq	r2, [r3]
+	bxeq	lr
+	/* Otherwise we are resuming */
+	b	resume_springboard
+UNWIND(	.fnend)
+END_FUNC plat_cpu_reset_early
+
+LOCAL_FUNC resume_springboard , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	mov	r3, r5
+
+	/* Setup tmp stack */
+	bl	get_core_pos
+	cmp	r0, #CFG_TEE_CORE_NB_CORE
+	/* Unsupported CPU, park it before it breaks something */
+unhandled_cpu:
+	wfige
+	bge	unhandled_cpu
+	ldr	r1, =stack_tmp_stride
+	ldr	r1, [r1]
+	mul	r1, r0, r1
+	ldr	r0, =stack_tmp_export
+	ldr	r0, [r0]
+	add	sp, r1, r0
+
+	/* Push our return on the stack as sm_pm_cpu_do_resume expects */
+	adr	lr, after_resume
+	push	{r4 - r12, lr}
+
+	/* Assumes suspend_regs is flat-mapped */
+	ldr	r0, =suspend_regs
+	bl	sm_pm_cpu_do_resume
+
+after_resume:
+	mov	r4, r3
+	bl	thread_init_per_cpu
+
+	mov	r0, r4
+	bl	init_sec_mon
+
+	mov	r0, #TEESMC_OPTEED_RETURN_ENTRY_DONE
+	mov	r1, #0
+	mov	r2, #0
+	mov	r3, #0
+	mov	r4, #0
+	smc	#0
+	b	.	/* SMC should not return */
+UNWIND(	.fnend)
+END_FUNC resume_springboard
diff --git a/core/arch/arm/plat-ti/api_monitor_index_a9.h b/core/arch/arm/plat-ti/api_monitor_index_a9.h
index 5c9f3418..7cbf1743 100644
--- a/core/arch/arm/plat-ti/api_monitor_index_a9.h
+++ b/core/arch/arm/plat-ti/api_monitor_index_a9.h
@@ -31,6 +31,12 @@
 #define API_HAL_RET_VALUE_OK 0x00000000
 #define API_HAL_RET_VALUE_SERVICE_UNKNWON 0xFFFFFFFF
 
+/* Base for power management related services */
+#define SECURE_SVC_PM       0x70
+
+/* Carry out late actions as part of suspend sequence */
+#define SECURE_SVC_PM_LATE_SUSPEND      (SECURE_SVC_PM + 1)
+
 /* Base Index of APIs */
 #define API_MONITOR_BASE_INDEX 0x00000100
 
diff --git a/core/arch/arm/plat-ti/conf.mk b/core/arch/arm/plat-ti/conf.mk
index 6cfbbe30..6be8bbba 100644
--- a/core/arch/arm/plat-ti/conf.mk
+++ b/core/arch/arm/plat-ti/conf.mk
@@ -13,6 +13,7 @@ CFG_WITH_SOFTWARE_PRNG = y
 $(call force,CFG_NO_SMP,y)
 $(call force,CFG_PL310,y)
 $(call force,CFG_PL310_LOCKED,y)
+$(call force,CFG_PM_ARM32,y)
 $(call force,CFG_SECURE_TIME_SOURCE_REE,y)
 include core/arch/arm/cpu/cortex-a9.mk
 else
diff --git a/core/arch/arm/plat-ti/sm_platform_handler_a9.c b/core/arch/arm/plat-ti/sm_platform_handler_a9.c
index dfe245fa..8c88ea82 100644
--- a/core/arch/arm/plat-ti/sm_platform_handler_a9.c
+++ b/core/arch/arm/plat-ti/sm_platform_handler_a9.c
@@ -27,18 +27,40 @@
 
 #include <arm32.h>
 #include <io.h>
+#include <kernel/cache_helpers.h>
 #include <kernel/tz_ssvce_def.h>
 #include <kernel/tz_ssvce_pl310.h>
 #include <platform_config.h>
+#include <sm/pm.h>
 #include <sm/sm.h>
+#include <mm/core_memprot.h>
 #include "api_monitor_index_a9.h"
 
+uint32_t suspend_regs[16];
+
 bool sm_platform_handler(struct sm_ctx *ctx)
 {
 	if (ctx->nsec.r12 == 0x200)
 		return true;
 
 	switch (ctx->nsec.r12) {
+	case 0x0:
+		switch (ctx->nsec.r0) {
+		case SECURE_SVC_PM_LATE_SUSPEND:
+			sm_pm_cpu_do_suspend(suspend_regs);
+			cache_op_inner(DCACHE_AREA_CLEAN,
+				       suspend_regs,
+				       sizeof(suspend_regs));
+			cache_op_outer(DCACHE_AREA_CLEAN,
+				       virt_to_phys(suspend_regs),
+				       sizeof(suspend_regs));
+			ctx->nsec.r0 = API_HAL_RET_VALUE_OK;
+			break;
+		default:
+			ctx->nsec.r0 = API_HAL_RET_VALUE_SERVICE_UNKNWON;
+			break;
+		}
+		break;
 	case API_MONITOR_L2CACHE_SETDEBUG_INDEX:
 		write32(ctx->nsec.r0, pl310_base() + PL310_DEBUG_CTRL);
 		ctx->nsec.r0 = API_HAL_RET_VALUE_OK;
diff --git a/core/arch/arm/plat-ti/sub.mk b/core/arch/arm/plat-ti/sub.mk
index c45e2406..0b4c3851 100644
--- a/core/arch/arm/plat-ti/sub.mk
+++ b/core/arch/arm/plat-ti/sub.mk
@@ -3,4 +3,4 @@ srcs-y += main.c
 srcs-$(CFG_PL310) += ti_pl310.c
 srcs-$(PLATFORM_FLAVOR_dra7xx) += sm_platform_handler_a15.c
 srcs-$(PLATFORM_FLAVOR_am57xx) += sm_platform_handler_a15.c
-srcs-$(PLATFORM_FLAVOR_am43xx) += sm_platform_handler_a9.c
+srcs-$(PLATFORM_FLAVOR_am43xx) += sm_platform_handler_a9.c a9_plat_init.S