imx: mx7: add system suspend/resume support

This patch adds system suspend/resume support,
when linux kernel enters deep sleep mode, SoC will go
into below mode:

 - CA7 platform goes into STOP mode;
 - SoC goes into DSM mode;
 - DDR goes into self-refresh mode;
 - CPU0/SCU will be powered down.

When wake up event arrives:

 - SoC DSM mdoe exits;
 - CA7 platform exit STOP mode, SCU/CPU0 power up;
 - Invalidate L1 cache;
 - DDR exit self-refresh mode;
 - Do secure monitor mode related initialization;
 - Jump to linux kernel resume entry.

Belwo is the log of 1 iteration of system suspend/resume:

[  338.824862] PM: suspend entry (deep)
[  338.828853] PM: Syncing filesystems ... done.
[  338.834433] Freezing user space processes ... (elapsed 0.001 seconds) done.
[  338.842939] OOM killer disabled.
[  338.846182] Freezing remaining freezable tasks ... (elapsed 0.001 seconds) done.
[  338.869717] PM: suspend devices took 0.010 seconds
[  338.877846] Disabling non-boot CPUs ...
[  338.960301] Retrying again to check for CPU kill
[  338.964953] CPU1 killed.
[  338.968104] Enabling non-boot CPUs ...
[  338.973598] CPU1 is up
[  339.267155] mmc1: queuing unknown CIS tuple 0x80 (2 bytes)
[  339.275833] mmc1: queuing unknown CIS tuple 0x80 (7 bytes)
[  339.284158] mmc1: queuing unknown CIS tuple 0x80 (6 bytes)
[  339.385065] PM: resume devices took 0.400 seconds
[  339.389836] OOM killer enabled.
[  339.392986] Restarting tasks ... done.
[  339.398990] PM: suspend exit

The resume entry function has to initialize stack pointer before calling
C code, otherwise there will be an external abort occur, in additional,
invalidate L1 cache must be done in secure section as well, so this
patch also adds assembly code back and keep it as simple as possible.

Signed-off-by: Anson Huang <Anson.Huang@nxp.com>
Acked-by: Stefan Agner <stefan@agner.ch>
Tested-by: Stefan Agner <stefan@agner.ch>

1 files changed, 67 insertions, 0 deletions

diff --git a/arch/arm/mach-imx/mx7/psci-suspend.S b/arch/arm/mach-imx/mx7/psci-suspend.S
new file mode 100644
index 0000000000..a21403f73f
--- /dev/null
+++ b/arch/arm/mach-imx/mx7/psci-suspend.S
@@ -0,0 +1,67 @@
+/* SPDX-License-Identifier: GPL-2.0+ */
+/*
+ * Copyright 2018 NXP
+ */
+
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/psci.h>
+
+	.pushsection ._secure.text, "ax"
+
+	.arch_extension sec
+
+.globl v7_invalidate_l1
+v7_invalidate_l1:
+	mov	r0, #0
+	mcr	p15, 2, r0, c0, c0, 0
+	mrc	p15, 1, r0, c0, c0, 0
+
+	movw	r1, #0x7fff
+	and	r2, r1, r0, lsr #13
+
+	movw	r1, #0x3ff
+
+	and	r3, r1, r0, lsr #3      @ NumWays - 1
+	add	r2, r2, #1              @ NumSets
+
+	and	r0, r0, #0x7
+	add	r0, r0, #4      @ SetShift
+
+	clz	r1, r3          @ WayShift
+	add	r4, r3, #1      @ NumWays
+1:
+	sub	r2, r2, #1      @ NumSets--
+	mov	r3, r4          @ Temp = NumWays
+2:
+	subs	r3, r3, #1      @ Temp--
+	mov	r5, r3, lsl r1
+	mov	r6, r2, lsl r0
+	orr	r5, r5, r6      @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
+	mcr	p15, 0, r5, c7, c6, 2
+	bgt	2b
+	cmp	r2, #0
+	bgt	1b
+	dsb	st
+	isb
+	mov	pc, lr
+
+.globl psci_system_resume
+psci_system_resume:
+	mov	sp, r0
+
+	/* invalidate L1 I-cache first */
+	mov	r6, #0x0
+	mcr	p15, 0, r6, c7, c5, 0
+	mcr	p15, 0, r6, c7, c5, 6
+	/* enable the Icache and branch prediction */
+	mov	r6, #0x1800
+	mcr	p15, 0, r6, c1, c0, 0
+	isb
+
+	bl	v7_invalidate_l1
+	b	imx_system_resume
+
+	.popsection