core: arm: psci: add suspend resume common functions

Add cpu suspend/resume common functions.

Platform psci suspend functions need to call
sm_pm_cpu_suspend(arg, platform_suspend) to runs into suspend.

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

Signed-off-by: Peng Fan <peng.fan@nxp.com>
Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>
Reviewed-by: Etienne Carriere <etienne.carriere@linaro.org>

core/arch/arm/include/arm32.h

@@ -33,6 +33,12 @@
 #include <stdint.h>
 #include <util.h>
 
+#define CORTEX_A7_PART_NUM		0xC07
+#define CORTEX_A9_PART_NUM		0xC09
+
+#define MIDR_PRIMARY_PART_NUM_SHIFT	4
+#define MIDR_PRIMARY_PART_NUM_WIDTH	12
+
 #define CPSR_MODE_MASK	ARM32_CPSR_MODE_MASK
 #define CPSR_MODE_USR	ARM32_CPSR_MODE_USR
 #define CPSR_MODE_FIQ	ARM32_CPSR_MODE_FIQ

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/kernel/thread.h

@@ -62,6 +62,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/include/sm/pm.h

@@ -0,0 +1,50 @@
+/*
+ * 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 sm_pm_cpu_suspend_save(struct sm_pm_ctx *ptr, uint32_t sp);
+void sm_pm_cpu_do_suspend(uint32_t *ptr);
+void sm_pm_cpu_do_resume(void);
+
+/*
+ * Exported to platform suspend, arg will be passed to fn as r0
+ * Return value: 0  - cpu resumed from suspended state.
+ *               -1 - cpu not suspended.
+ */
+int sm_pm_cpu_suspend(uint32_t arg, int (*fn)(uint32_t));
+#endif

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

@@ -134,4 +134,6 @@ static inline bool sm_platform_handler(__unused struct sm_ctx *ctx)
 bool sm_platform_handler(struct sm_ctx *ctx);
 #endif
 
+void sm_save_modes_regs(struct sm_mode_regs *regs);
+void sm_restore_modes_regs(struct sm_mode_regs *regs);
 #endif /*SM_SM_H*/

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,11 @@ DEFINES
 
 	/* struct thread_core_local */
 	DEFINE(THREAD_CORE_LOCAL_R0, offsetof(struct thread_core_local, r[0]));
+	DEFINE(THREAD_CORE_LOCAL_SM_PM_CTX_PHYS,
+	       offsetof(struct thread_core_local, sm_pm_ctx_phys));
+	DEFINE(THREAD_CORE_LOCAL_SIZE, sizeof(struct thread_core_local));
+
+	DEFINE(SM_PM_CTX_SIZE, sizeof(struct sm_pm_ctx));
 #endif /*ARM32*/
 
 #ifdef ARM64

core/arch/arm/sm/pm.c

@@ -0,0 +1,72 @@
+/*
+ * 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/thread.h>
+#include <kernel/tlb_helpers.h>
+#include <kernel/tz_ssvce_pl310.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <platform_config.h>
+#include <sm/optee_smc.h>
+#include <sm/pm.h>
+#include <sm/psci.h>
+#include <sm/sm.h>
+#include <stdint.h>
+
+#if CFG_TEE_CORE_NB_CORE > 4
+#error "Max support 4 cores in one cluster now"
+#endif
+
+void sm_pm_cpu_suspend_save(struct sm_pm_ctx *ctx, uint32_t sp)
+{
+	struct thread_core_local *p = thread_get_core_local();
+
+	p->sm_pm_ctx_phys = virt_to_phys((void *)ctx);
+
+	/* The content will be passed to sm_pm_cpu_do_resume as register sp */
+	ctx->sp = sp;
+	ctx->cpu_resume_addr =
+		virt_to_phys((void *)(vaddr_t)sm_pm_cpu_do_resume);
+
+	sm_pm_cpu_do_suspend(ctx->suspend_regs);
+
+	dcache_op_level1(DCACHE_OP_CLEAN_INV);
+
+#ifdef CFG_PL310
+	arm_cl2_cleanbyway(core_mmu_get_va(PL310_BASE, MEM_AREA_IO_SEC));
+#endif
+}