core: arm: psci: add suspend resume common functions

Add cpu suspend/resume common functions.

platform psci suspend functions need to call
cpu_suspend(arg, platform_suspend) to runs into suspend.

The i.MX flow is:
psci_cpu_suspend->imx6_cpu_suspend->cpu_suspend(arg, func)
The "func" runs in on-chip ram that not losing power when
system runs into suspend or low power state. The "arg" passed
to "func" as "r0".

The suspend/resume flow is similar to linux suspend/resume flow.

Note:
In this patch, max 1 cluster 4 cores are supported.

Signed-off-by: Peng Fan <peng.fan@nxp.com>

core/arch/arm/include/arm32_macros.S

@@ -27,6 +27,10 @@
 
 	/* Please keep them sorted based on the CRn register */
 
+	.macro read_midr reg
+	mrc     p15, 0, \reg, c0, c0, 0
+	.endm
+
 	.macro read_ctr reg
 	mrc	p15, 0, \reg, c0, c0, 1
 	.endm

core/arch/arm/include/sm/pm.h

@@ -0,0 +1,49 @@
+/*
+ * Copyright 2017 NXP
+ * 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.
+ */
+
+#ifndef SM_PM_H
+#define SM_PM_H
+#include <stdint.h>
+#include <types_ext.h>
+
+struct sm_pm_ctx {
+	uint32_t sp;
+	paddr_t cpu_resume_addr;
+	uint32_t suspend_regs[16];
+};
+
+/* suspend/resume core functions */
+void cpu_suspend_init(void);
+void cpu_suspend_save(struct sm_pm_ctx *ptr, uint32_t save_sz, uint32_t sp);
+void cpu_do_suspend(uint32_t *ptr);
+void cpu_do_resume(void);
+
+/*
+ * Exported to platform suspend, arg will be passed to fn as r0
+ */
+int cpu_suspend(uint32_t arg, int (*fn)(uint32_t));
+#endif

core/arch/arm/kernel/asm-defines.c

@@ -26,6 +26,7 @@
  */
 
 #include <kernel/thread.h>
+#include <sm/pm.h>
 #include <sm/sm.h>
 #include <types_ext.h>
 #include "thread_private.h"
@@ -55,6 +56,14 @@ DEFINES
 
 	/* struct thread_core_local */
 	DEFINE(THREAD_CORE_LOCAL_R0, offsetof(struct thread_core_local, r[0]));
+	DEFINE(THREAD_CORE_LOCAL_PM_CTX_PHYS,
+	       offsetof(struct thread_core_local, sm_pm_ctx_phys));
+	DEFINE(THREAD_CORE_LOCAL_SIZE, sizeof(struct thread_core_local));
+
+	DEFINE(PM_CTX_SP, offsetof(struct sm_pm_ctx, sp));
+	DEFINE(PM_CTX_RESUME_ADDR, offsetof(struct sm_pm_ctx, cpu_resume_addr));
+	DEFINE(PM_CTX_SUSPEND_REGS, offsetof(struct sm_pm_ctx, suspend_regs));
+	DEFINE(PM_CTX_SIZE, sizeof(struct sm_pm_ctx));
 #endif /*ARM32*/
 
 #ifdef ARM64

core/arch/arm/kernel/thread.c

@@ -89,7 +89,7 @@
 
 struct thread_ctx threads[CFG_NUM_THREADS];
 
-static struct thread_core_local thread_core_local[CFG_TEE_CORE_NB_CORE];
+struct thread_core_local thread_core_local[CFG_TEE_CORE_NB_CORE];
 
 #ifdef CFG_WITH_STACK_CANARIES
 #ifdef ARM32

core/arch/arm/kernel/thread_private.h

@@ -134,6 +134,7 @@ struct thread_core_local {
 	uint32_t flags;
 	vaddr_t abt_stack_va_end;
 #ifdef ARM32
+	paddr_t sm_pm_ctx_phys;
 	uint32_t r[2];
 #endif
 #ifdef ARM64

core/arch/arm/sm/pm.c

@@ -0,0 +1,86 @@
+/*
+ * Copyright 2017 NXP
+ *
+ * Peng Fan <peng.fan@nxp.com>
+ *
+ * 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.
+ */
+#include <arm32.h>
+#include <console.h>
+#include <drivers/imx_uart.h>
+#include <io.h>
+#include <kernel/cache_helpers.h>
+#include <kernel/generic_boot.h>
+#include <kernel/misc.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <kernel/tlb_helpers.h>
+#include <kernel/tz_ssvce_pl310.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdint.h>
+#include <sm/optee_smc.h>
+#include <sm/psci.h>
+#include <sm/sm.h>
+#include <sm/pm.h>
+
+#include "../kernel/thread_private.h"
+
+#if CFG_TEE_CORE_NB_CORE > 4
+#error "Max support 4 cores in one cluster now"
+#endif
+
+void cpu_suspend_init(void)
+{
+	/* Place holer for now */
+}
+
+void cpu_suspend_save(struct sm_pm_ctx *ctx, uint32_t save_sz __unused,
+		      uint32_t sp)
+{
+	union {
+		void (*resume_fun)(void);
+		void *resume_addr;
+	} u;
+
+	struct thread_core_local *p = thread_get_core_local();
+
+	p->sm_pm_ctx_phys = virt_to_phys((void *)ctx);
+
+	u.resume_fun = cpu_do_resume;
+	/*
+	 * The content will be passed to cpu_do_resume
+	 * as register r1 and sp
+	 */
+	ctx->sp = sp;
+	ctx->cpu_resume_addr = virt_to_phys(u.resume_addr);
+
+	cpu_do_suspend(&ctx->suspend_regs[0]);
+
+	dcache_op_level1(DCACHE_OP_CLEAN_INV);
+
+#ifdef CFG_PL310
+	arm_cl2_cleanbyway(core_mmu_get_va(PL310_BASE, MEM_AREA_IO_SEC));
+#endif
+}

core/arch/arm/sm/sub.mk

@@ -1,3 +1,3 @@
 srcs-y += sm_a32.S
 srcs-y += sm.c
-srcs-$(CFG_PSCI_ARM32) += std_smc.c psci.c psci-helper.S
+srcs-$(CFG_PSCI_ARM32) += std_smc.c psci.c pm.c psci-helper.S suspend.S