In order to resume CPU from suspend via trusted Foundations firmware,
the LP1/LP2 boot vectors shall be specified using the firmware calls.
Signed-off-by: Dmitry Osipenko <digetx@xxxxxxxxx>
---
arch/arm/mach-tegra/pm.c | 14 ++++++++++++++
arch/arm/mach-tegra/reset-handler.S | 26 ++++++++++++++++++++++++++
2 files changed, 40 insertions(+)
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index 66c8cd63dd86..97bdfffc244e 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -33,6 +33,7 @@
#include <soc/tegra/pmc.h>
#include <asm/cacheflush.h>
+#include <asm/firmware.h>
#include <asm/idmap.h>
#include <asm/proc-fns.h>
#include <asm/smp_plat.h>
@@ -207,6 +208,8 @@ void tegra_idle_lp2_last(void)
if (trusted_foundations_registered())
outer_disable();
+ call_firmware_op(prepare_idle, TF_PM_MODE_LP2);
+
cpu_suspend(PHYS_OFFSET - PAGE_OFFSET, &tegra_sleep_cpu);
/*
@@ -368,6 +371,17 @@ static int tegra_suspend_enter(suspend_state_t state)
if (trusted_foundations_registered())
outer_disable();
+ switch (mode) {
+ case TEGRA_SUSPEND_LP1:
+ call_firmware_op(prepare_idle, TF_PM_MODE_LP1);
+ break;
+ case TEGRA_SUSPEND_LP2:
+ call_firmware_op(prepare_idle, TF_PM_MODE_LP2);
+ break;
+ default:
+ break;
+ }
+
cpu_suspend(PHYS_OFFSET - PAGE_OFFSET, tegra_sleep_func);
/*
diff --git a/arch/arm/mach-tegra/reset-handler.S b/arch/arm/mach-tegra/reset-handler.S
index c827b185d281..24fb375e11a9 100644
--- a/arch/arm/mach-tegra/reset-handler.S
+++ b/arch/arm/mach-tegra/reset-handler.S
@@ -20,6 +20,7 @@
#include <soc/tegra/flowctrl.h>
#include <soc/tegra/fuse.h>
+#include <asm/assembler.h>
#include <asm/asm-offsets.h>
#include <asm/cache.h>
@@ -76,6 +77,7 @@ ENTRY(tegra_resume)
orr r1, r1, #1
str r1, [r0]
#endif
+ bl tegra_resume_trusted_foundations
#ifdef CONFIG_CACHE_L2X0
/* L2 cache resume & re-enable */
@@ -88,6 +90,30 @@ end_ca9_scu_l2_resume:
b cpu_resume
ENDPROC(tegra_resume)
+
+/*
+ * tegra_resume_trusted_foundations
+ *
+ * Trusted Foundations firmware initialization.
+ *
+ * Doesn't return if firmware presents.
+ * Corrupted registers: r1, r2
+ */
+ENTRY(tegra_resume_trusted_foundations)
+ /* Check whether Trusted Foundations firmware presents. */
+ mov32 r2, TEGRA_IRAM_BASE + TEGRA_IRAM_RESET_HANDLER_OFFSET
+ ldr r1, =__tegra_cpu_reset_handler_data_offset + \
+ RESET_DATA(TF_PRESENT)
+ ldr r1, [r2, r1]
+ cmp r1, #0
+ reteq lr
+
+ .arch_extension sec
+ /* First call after suspend wakes firmware. No arguments required. */
+ smc #0
+
+ b cpu_resume
+ENDPROC(tegra_resume_trusted_foundations)
#endif
.align L1_CACHE_SHIFT
--
2.20.1
