basic pm framework

Signed-off-by: Silvano di Ninno <silvano.dininno@nxp.com>

9 files changed, 343 insertions, 86 deletions

diff --git a/core/arch/arm/plat-imx/imx.h b/core/arch/arm/plat-imx/imx.h
index 5ec90115..8fd7a5b7 100644
--- a/core/arch/arm/plat-imx/imx.h
+++ b/core/arch/arm/plat-imx/imx.h
@@ -38,6 +38,9 @@ bool soc_is_imx7ds(void);
 bool soc_is_imx7ulp(void);
 void imx_gpcv2_set_core1_pdn_by_software(void);
 void imx_gpcv2_set_core1_pup_by_software(void);
+
+void plat_cpu_wakeup_late(void);
+
 #ifdef CFG_TZC380
 TEE_Result tzasc_init(void);
 #endif
diff --git a/core/arch/arm/plat-imx/imx_pm.h b/core/arch/arm/plat-imx/imx_pm.h
index e19cfc8e..2cdd260f 100644
--- a/core/arch/arm/plat-imx/imx_pm.h
+++ b/core/arch/arm/plat-imx/imx_pm.h
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: BSD-2-Clause */
 /*
- * Copyright 2017 NXP
+ * Copyright 2017-2018 NXP
  */
 
 #ifndef __IMX_PM_H
@@ -64,14 +64,6 @@ struct imx7_pm_info {
 	uint32_t	ddrc_phy_val[MX7_DDRC_NUM][2];
 } __aligned(8);
 
-struct suspend_save_regs {
-	uint32_t irq[3];
-	uint32_t fiq[3];
-	uint32_t und[3];
-	uint32_t abt[3];
-	uint32_t mon[3];
-} __aligned(8);
-
 struct imx7_pm_data {
 	uint32_t ddr_type;
 	uint32_t ddrc_num;
@@ -80,18 +72,44 @@ struct imx7_pm_data {
 	uint32_t (*ddrc_phy_offset)[2];
 };
 
+/* IMX6 Power initialization functions */
+int imx6_suspend_init(void);
+int imx6sx_cpuidle_init(void);
+int imx6ul_cpuidle_init(void);
+int imx6sl_cpuidle_init(void);
+int imx6sll_cpuidle_init(void);
+
+void v7_cpu_resume(void);
+
+int imx6ul_lowpower_idle(uint32_t power_state, uintptr_t entry,
+			 uint32_t context_id, struct sm_nsec_ctx *nsec);
+int imx6sx_lowpower_idle(uint32_t power_state, uintptr_t entry,
+			 uint32_t context_id, struct sm_nsec_ctx *nsec);
+int imx6sll_lowpower_idle(uint32_t power_state, uintptr_t entry,
+			 uint32_t context_id, struct sm_nsec_ctx *nsec);
+int imx6sl_lowpower_idle(uint32_t power_state, uintptr_t entry,
+			 uint32_t context_id, struct sm_nsec_ctx *nsec);
+int imx6_cpu_suspend(uint32_t power_state, uintptr_t entry,
+		     uint32_t context_id, struct sm_nsec_ctx *nsec);
+
+/* IMX7 Power Initialization functions */
+int imx7_suspend_init(void);
+int imx7ulp_suspend_init(void);
+int imx7d_cpuidle_init(void);
+
 void imx7_suspend(struct imx7_pm_info *info);
 void imx7_resume(void);
 void ca7_cpu_resume(void);
-int imx7_suspend_init(void);
-int pm_imx7_iram_tbl_init(void);
+void imx7ulp_cpu_resume(void);
+
 int imx7_cpu_suspend(uint32_t power_state, uintptr_t entry,
 		     uint32_t context_id, struct sm_nsec_ctx *nsec);
 int imx7d_lowpower_idle(uint32_t power_state, uintptr_t entry,
 			uint32_t context_id, struct sm_nsec_ctx *nsec);
 void imx7d_low_power_idle(struct imx7_pm_info *info);
-int imx7d_cpuidle_init(void);
-void v7_cpu_resume(void);
+
+int imx7ulp_cpu_suspend(uint32_t power_state, uintptr_t entry,
+			uint32_t context_id, struct sm_nsec_ctx *nsec);
 #endif
 
 #endif
diff --git a/core/arch/arm/plat-imx/main.c b/core/arch/arm/plat-imx/main.c
index cec9342e..e05a3baf 100644
--- a/core/arch/arm/plat-imx/main.c
+++ b/core/arch/arm/plat-imx/main.c
@@ -16,6 +16,7 @@
 #endif
 #include <io.h>
 #include <imx.h>
+#include <kernel/cache_helpers.h>
 #include <kernel/generic_boot.h>
 #include <kernel/misc.h>
 #include <kernel/panic.h>
@@ -208,3 +209,21 @@ void plat_cpu_reset_late(void)
 #endif
 	}
 }
+
+#ifdef CFG_PSCI_ARM32
+/*
+ * Platform Wakeup late function executed with MMU
+ * ON after suspend.
+ */
+void plat_cpu_wakeup_late(void)
+{
+#ifdef CFG_SCU
+	scu_init();
+	dcache_op_all(DCACHE_OP_CLEAN_INV);
+#endif
+
+#ifdef CFG_TZC380
+	tzasc_init();
+#endif
+}
+#endif
diff --git a/core/arch/arm/plat-imx/pm/cpuidle-imx7d.c b/core/arch/arm/plat-imx/pm/cpuidle-imx7d.c
index 34b3598f..52f74c32 100644
--- a/core/arch/arm/plat-imx/pm/cpuidle-imx7d.c
+++ b/core/arch/arm/plat-imx/pm/cpuidle-imx7d.c
@@ -33,20 +33,23 @@
 #include <tee/entry_fast.h>
 #include <util.h>
 
+/*
+ * cpuidle and suspend use the same one,
+ * because lowpower idle and suspend can not reach at the same time
+ */
+
 int imx7d_cpuidle_init(void)
 {
-	uint32_t lpm_idle_ocram_base =
+	uint32_t lowpower_idle_ocram_base =
 		core_mmu_get_va(TRUSTZONE_OCRAM_START +
 				LOWPOWER_IDLE_OCRAM_OFFSET,
 				MEM_AREA_TEE_COHERENT);
 	struct imx7_pm_info *p =
-		(struct imx7_pm_info *)lpm_idle_ocram_base;
-
-	pm_imx7_iram_tbl_init();
+		(struct imx7_pm_info *)lowpower_idle_ocram_base;
 
 	dcache_op_level1(DCACHE_OP_CLEAN_INV);
 
-	p->va_base = lpm_idle_ocram_base;
+	p->va_base = lowpower_idle_ocram_base;
 	p->pa_base = TRUSTZONE_OCRAM_START + LOWPOWER_IDLE_OCRAM_OFFSET;
 	p->tee_resume = (paddr_t)virt_to_phys((void *)(vaddr_t)v7_cpu_resume);
 	p->pm_info_size = sizeof(*p);
@@ -59,7 +62,7 @@ int imx7d_cpuidle_init(void)
 	p->src_va_base = core_mmu_get_va(SRC_BASE, MEM_AREA_IO_SEC);
 	p->src_pa_base = SRC_BASE;
 	p->iomuxc_gpr_va_base = core_mmu_get_va(IOMUXC_GPR_BASE,
-						MEM_AREA_IO_SEC);
+					MEM_AREA_IO_SEC);
 	p->iomuxc_gpr_pa_base = IOMUXC_GPR_BASE;
 	p->gpc_va_base = core_mmu_get_va(GPC_BASE, MEM_AREA_IO_SEC);
 	p->gpc_pa_base = GPC_BASE;
@@ -69,11 +72,11 @@ int imx7d_cpuidle_init(void)
 	p->num_lpi_cpus = 0;
 	p->num_online_cpus = -1;
 
-	memcpy((void *)(lpm_idle_ocram_base + sizeof(*p)),
-	       (void *)(vaddr_t)imx7d_low_power_idle,
+	memcpy((void *)(lowpower_idle_ocram_base + sizeof(*p)),
+		(void *)(vaddr_t)imx7d_low_power_idle,
 	       LOWPOWER_IDLE_OCRAM_SIZE - sizeof(*p));
 
-	dcache_clean_range((void *)lpm_idle_ocram_base,
+	dcache_clean_range((void *)lowpower_idle_ocram_base,
 			   LOWPOWER_IDLE_OCRAM_SIZE);
 	/*
 	 * Note that IRAM IOSEC map, if changed to MEM map,
@@ -137,7 +140,7 @@ static void imx_pen_unlock(int cpu)
 static uint32_t get_online_cpus(void)
 {
 	vaddr_t src_a7rcr1 = core_mmu_get_va(SRC_BASE + SRC_A7RCR1,
-					     MEM_AREA_IO_SEC);
+					MEM_AREA_IO_SEC);
 	uint32_t val = read32(src_a7rcr1);
 
 	return (val & (1 << BP_SRC_A7RCR1_A7_CORE1_ENABLE)) ? 2 : 1;
@@ -148,23 +151,17 @@ int imx7d_lowpower_idle(uint32_t power_state __unused,
 			uint32_t context_id __unused,
 			struct sm_nsec_ctx *nsec)
 {
-	struct imx7_pm_info *p;
-	uint32_t cpuidle_ocram_base;
-	static uint32_t gic_inited;
 	int ret;
-
-	uint32_t cpu_id __maybe_unused = get_core_pos();
-	uint32_t type = (power_state & PSCI_POWER_STATE_TYPE_MASK) >>
-		PSCI_POWER_STATE_TYPE_SHIFT;
-	uint32_t cpu = get_core_pos();
-
-	cpuidle_ocram_base = core_mmu_get_va(TRUSTZONE_OCRAM_START +
+	uint32_t cpuidle_ocram_base = core_mmu_get_va(TRUSTZONE_OCRAM_START +
 					     LOWPOWER_IDLE_OCRAM_OFFSET,
 					     MEM_AREA_TEE_COHERENT);
-	p = (struct imx7_pm_info *)cpuidle_ocram_base;
-
-	imx_pen_lock(cpu);
-
+	struct imx7_pm_info *p =
+			(struct imx7_pm_info *)cpuidle_ocram_base;
+	uint32_t type = (power_state & PSCI_POWER_STATE_TYPE_MASK) >>
+		PSCI_POWER_STATE_TYPE_SHIFT;
+	static uint32_t gic_inited = 0;
+	uint32_t cpu_id = get_core_pos();
+	imx_pen_lock(cpu_id);
 	if (!lowpoweridle_init) {
 		imx7d_cpuidle_init();
 		lowpoweridle_init = 1;
@@ -188,28 +185,25 @@ int imx7d_lowpower_idle(uint32_t power_state __unused,
 	 */
 	if (ret < 0) {
 		p->num_lpi_cpus--;
-		imx_pen_unlock(cpu);
+		imx_pen_unlock(cpu_id);
 		DMSG("=== Not suspended, GPC IRQ Pending === %d\n", cpu_id);
 		return 0;
 	}
 
-	/*
-	 * Restore register of different mode in secure world
-	 * When cpu powers up, after ROM init, cpu in secure SVC
-	 * mode, we first need to restore monitor regs.
-	 */
+	/* Restore register of different mode in secure world */
 	sm_restore_modes_regs(&nsec->mode_regs);
 
 	p->num_lpi_cpus--;
-	/* Back to Linux */
+
+	/* Set entry for back to Linux */
 	nsec->mon_lr = (uint32_t)entry;
 
 	if (gic_inited == 0) {
 		/*
-		 * TODO: Call the Wakeup Late function to restore some
+		 * Call the Wakeup Late function to restore some
 		 * HW configuration (e.g. TZASC)
 		 */
-		plat_cpu_reset_late();
+		plat_cpu_wakeup_late();
 
 		main_init_gic();
 		gic_inited = 1;
@@ -219,7 +213,7 @@ int imx7d_lowpower_idle(uint32_t power_state __unused,
 		gic_inited = 0;
 	}
 
-	imx_pen_unlock(cpu);
+	imx_pen_unlock(cpu_id);
 
 	return 0;
 }
diff --git a/core/arch/arm/plat-imx/pm/imx7_suspend.c b/core/arch/arm/plat-imx/pm/imx7_suspend.c
index 0dc955c1..e75af0e9 100644
--- a/core/arch/arm/plat-imx/pm/imx7_suspend.c
+++ b/core/arch/arm/plat-imx/pm/imx7_suspend.c
@@ -1,12 +1,11 @@
 // SPDX-License-Identifier: BSD-2-Clause
 /*
- * Copyright (C) 2017 NXP
+ * Copyright 2017-2018 NXP
  *
  * Peng Fan <peng.fan@nxp.com>
  */
 
 #include <arm.h>
-#include <arm32.h>
 #include <console.h>
 #include <drivers/imx_uart.h>
 #include <io.h>
@@ -23,8 +22,6 @@
 #include <sm/psci.h>
 #include <stdint.h>
 
-static int suspended_init;
-
 int imx7_cpu_suspend(uint32_t power_state __unused, uintptr_t entry,
 		     uint32_t context_id __unused, struct sm_nsec_ctx *nsec)
 {
@@ -34,11 +31,6 @@ int imx7_cpu_suspend(uint32_t power_state __unused, uintptr_t entry,
 	struct imx7_pm_info *p = (struct imx7_pm_info *)suspend_ocram_base;
 	int ret;
 
-	if (!suspended_init) {
-		imx7_suspend_init();
-		suspended_init = 1;
-	}
-
 	/* Store non-sec ctx regs */
 	sm_save_modes_regs(&nsec->mode_regs);
 
@@ -53,11 +45,15 @@ int imx7_cpu_suspend(uint32_t power_state __unused, uintptr_t entry,
 		return 0;
 	}
 
-	plat_cpu_reset_late();
-
 	/* Restore register of different mode in secure world */
 	sm_restore_modes_regs(&nsec->mode_regs);
 
+	/*
+	 * Call the Wakeup Late function to restore some
+	 * HW configuration (e.g. TZASC)
+	 */
+	plat_cpu_wakeup_late();
+
 	/* Set entry for back to Linux */
 	nsec->mon_lr = (uint32_t)entry;
 
diff --git a/core/arch/arm/plat-imx/pm/pm-imx7.c b/core/arch/arm/plat-imx/pm/pm-imx7.c
index 04cbfd54..3a96cd89 100644
--- a/core/arch/arm/plat-imx/pm/pm-imx7.c
+++ b/core/arch/arm/plat-imx/pm/pm-imx7.c
@@ -1,31 +1,81 @@
 // SPDX-License-Identifier: BSD-2-Clause
 /*
- * Copyright 2017 NXP
+ * Copyright 2017-2018 NXP
  */
 
 #include <arm.h>
-#include <arm32.h>
-#include <console.h>
-#include <io.h>
 #include <imx.h>
 #include <imx_pm.h>
 #include <kernel/panic.h>
-#include <kernel/pm_stubs.h>
+#include <io.h>
 #include <kernel/cache_helpers.h>
-#include <mm/core_mmu.h>
 #include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
 #include <mmdc.h>
-#include <platform_config.h>
-#include <sm/pm.h>
-#include <sm/psci.h>
-#include <sm/sm.h>
 #include <string.h>
 
-paddr_t iram_tbl_phys_addr = -1UL;
-void *iram_tbl_virt_addr;
+paddr_t iram_tlb_phys_addr = -1UL;
+void *iram_tlb_virt_addr;
 
 #define READ_DATA_FROM_HARDWARE		0
 
+static uint32_t imx7d_ddrc_lpddr3_setting[][2] = {
+	{ 0x0, READ_DATA_FROM_HARDWARE },
+	{ 0x1a0, READ_DATA_FROM_HARDWARE },
+	{ 0x1a4, READ_DATA_FROM_HARDWARE },
+	{ 0x1a8, READ_DATA_FROM_HARDWARE },
+	{ 0x64, READ_DATA_FROM_HARDWARE },
+	{ 0xd0, READ_DATA_FROM_HARDWARE },
+	{ 0xdc, READ_DATA_FROM_HARDWARE },
+	{ 0xe0, READ_DATA_FROM_HARDWARE },
+	{ 0xe4, READ_DATA_FROM_HARDWARE },
+	{ 0xf4, READ_DATA_FROM_HARDWARE },
+	{ 0x100, READ_DATA_FROM_HARDWARE },
+	{ 0x104, READ_DATA_FROM_HARDWARE },
+	{ 0x108, READ_DATA_FROM_HARDWARE },
+	{ 0x10c, READ_DATA_FROM_HARDWARE },
+	{ 0x110, READ_DATA_FROM_HARDWARE },
+	{ 0x114, READ_DATA_FROM_HARDWARE },
+	{ 0x118, READ_DATA_FROM_HARDWARE },
+	{ 0x120, READ_DATA_FROM_HARDWARE },
+	{ 0x11c, READ_DATA_FROM_HARDWARE },
+	{ 0x180, READ_DATA_FROM_HARDWARE },
+	{ 0x184, READ_DATA_FROM_HARDWARE },
+	{ 0x190, READ_DATA_FROM_HARDWARE },
+	{ 0x194, READ_DATA_FROM_HARDWARE },
+	{ 0x200, READ_DATA_FROM_HARDWARE },
+	{ 0x204, READ_DATA_FROM_HARDWARE },
+	{ 0x210, READ_DATA_FROM_HARDWARE },
+	{ 0x214, READ_DATA_FROM_HARDWARE },
+	{ 0x218, READ_DATA_FROM_HARDWARE },
+	{ 0x240, READ_DATA_FROM_HARDWARE },
+	{ 0x244, READ_DATA_FROM_HARDWARE },
+};
+
+static uint32_t imx7d_ddrc_phy_lpddr3_setting[][2] = {
+	{ 0x0, READ_DATA_FROM_HARDWARE },
+	{ 0x4, READ_DATA_FROM_HARDWARE },
+	{ 0x8, READ_DATA_FROM_HARDWARE },
+	{ 0x10, READ_DATA_FROM_HARDWARE },
+	{ 0xb0, READ_DATA_FROM_HARDWARE },
+	{ 0x1c, READ_DATA_FROM_HARDWARE },
+	{ 0x9c, READ_DATA_FROM_HARDWARE },
+	{ 0x7c, READ_DATA_FROM_HARDWARE },
+	{ 0x80, READ_DATA_FROM_HARDWARE },
+	{ 0x84, READ_DATA_FROM_HARDWARE },
+	{ 0x88, READ_DATA_FROM_HARDWARE },
+	{ 0x6c, READ_DATA_FROM_HARDWARE },
+	{ 0x20, READ_DATA_FROM_HARDWARE },
+	{ 0x30, READ_DATA_FROM_HARDWARE },
+	{ 0x50, 0x01000008 },
+	{ 0x50, 0x00000008 },
+	{ 0xc0, 0x0e407304 },
+	{ 0xc0, 0x0e447304 },
+	{ 0xc0, 0x0e447306 },
+	{ 0xc0, 0x0e4c7304 },
+	{ 0xc0, 0x0e487306 },
+};
+
 static uint32_t imx7d_ddrc_ddr3_setting[][2] = {
 	{ 0x0, READ_DATA_FROM_HARDWARE },
 	{ 0x1a0, READ_DATA_FROM_HARDWARE },
@@ -79,6 +129,13 @@ static uint32_t imx7d_ddrc_phy_ddr3_setting[][2] = {
 	{ 0xc0, 0x0e407306 },
 };
 
+static struct imx7_pm_data imx7d_pm_data_lpddr3 = {
+	.ddrc_num = ARRAY_SIZE(imx7d_ddrc_lpddr3_setting),
+	.ddrc_offset = imx7d_ddrc_lpddr3_setting,
+	.ddrc_phy_num = ARRAY_SIZE(imx7d_ddrc_phy_lpddr3_setting),
+	.ddrc_phy_offset = imx7d_ddrc_phy_lpddr3_setting,
+};
+
 static struct imx7_pm_data imx7d_pm_data_ddr3 = {
 	.ddrc_num = ARRAY_SIZE(imx7d_ddrc_ddr3_setting),
 	.ddrc_offset = imx7d_ddrc_ddr3_setting,
@@ -90,21 +147,21 @@ paddr_t phys_addr[] = {
 	AIPS1_BASE, AIPS2_BASE, AIPS3_BASE
 };
 
-int pm_imx7_iram_tbl_init(void)
+static int pm_imx7_iram_tlb_init(void)
 {
 	uint32_t i;
 	struct tee_mmap_region map;
 
 	/* iram mmu translation table already initialized */
-	if (iram_tbl_phys_addr != (-1UL))
+	if (iram_tlb_phys_addr != (-1UL))
 		return 0;
 
-	iram_tbl_phys_addr = TRUSTZONE_OCRAM_START + 16 * 1024;
-	iram_tbl_virt_addr = phys_to_virt(iram_tbl_phys_addr,
+	iram_tlb_phys_addr = TRUSTZONE_OCRAM_START + 16 * 1024;
+	iram_tlb_virt_addr = phys_to_virt(iram_tlb_phys_addr,
 					  MEM_AREA_TEE_COHERENT);
 
 	/* 16KB */
-	memset(iram_tbl_virt_addr, 0, 16 * 1024);
+	memset(iram_tlb_virt_addr, 0, 16 * 1024);
 
 	for (i = 0; i < ARRAY_SIZE(phys_addr); i++) {
 		map.pa = phys_addr[i];
@@ -115,7 +172,7 @@ int pm_imx7_iram_tbl_init(void)
 		map.attr = TEE_MATTR_VALID_BLOCK | TEE_MATTR_PRW |
 			   TEE_MATTR_SECURE |
 			   (TEE_MATTR_CACHE_NONCACHE << TEE_MATTR_CACHE_SHIFT);
-		map_memarea_sections(&map, (uint32_t *)iram_tbl_virt_addr);
+		map_memarea_sections(&map, (uint32_t *)iram_tlb_virt_addr);
 	}
 
 	/* Note IRAM_S_BASE is not 1M aligned, so take care */
@@ -125,7 +182,7 @@ int pm_imx7_iram_tbl_init(void)
 	map.size = CORE_MMU_DEVICE_SIZE;
 	map.type = MEM_AREA_TEE_COHERENT;
 	map.attr = TEE_MATTR_VALID_BLOCK | TEE_MATTR_PRWX | TEE_MATTR_SECURE;
-	map_memarea_sections(&map, (uint32_t *)iram_tbl_virt_addr);
+	map_memarea_sections(&map, (uint32_t *)iram_tlb_virt_addr);
 
 	map.pa = GIC_BASE;
 	map.va = (vaddr_t)phys_to_virt((paddr_t)GIC_BASE, MEM_AREA_IO_SEC);
@@ -133,11 +190,13 @@ int pm_imx7_iram_tbl_init(void)
 	map.size = CORE_MMU_DEVICE_SIZE;
 	map.type = MEM_AREA_TEE_COHERENT;
 	map.attr = TEE_MATTR_VALID_BLOCK | TEE_MATTR_PRW | TEE_MATTR_SECURE;
-	map_memarea_sections(&map, (uint32_t *)iram_tbl_virt_addr);
+	map_memarea_sections(&map, (uint32_t *)iram_tlb_virt_addr);
 
 	return 0;
 }
 
+struct imx7_pm_info *pm_info;
+
 int imx7_suspend_init(void)
 {
 	uint32_t i;
@@ -149,10 +208,13 @@ int imx7_suspend_init(void)
 	struct imx7_pm_info *p = (struct imx7_pm_info *)suspend_ocram_base;
 	struct imx7_pm_data *pm_data;
 
-	pm_imx7_iram_tbl_init();
+	pm_imx7_iram_tlb_init();
+	pm_info = p;
 
 	dcache_op_level1(DCACHE_OP_CLEAN_INV);
 
+	DMSG("%x %x\n", suspend_ocram_base, sizeof(*p));
+
 	p->pa_base = TRUSTZONE_OCRAM_START + SUSPEND_OCRAM_OFFSET;
 	p->tee_resume = virt_to_phys((void *)(vaddr_t)ca7_cpu_resume);
 	p->pm_info_size = sizeof(*p);
@@ -186,6 +248,9 @@ int imx7_suspend_init(void)
 	case IMX_DDR_TYPE_DDR3:
 		pm_data = &imx7d_pm_data_ddr3;
 		break;
+	case IMX_DDR_TYPE_LPDDR3:
+		pm_data = &imx7d_pm_data_lpddr3;
+		break;
 	default:
 		panic("Not supported ddr type\n");
 		break;
diff --git a/core/arch/arm/plat-imx/pm/psci-cpuidle-imx7.S b/core/arch/arm/plat-imx/pm/psci-cpuidle-imx7.S
index 13021fe3..508f422a 100644
--- a/core/arch/arm/plat-imx/pm/psci-cpuidle-imx7.S
+++ b/core/arch/arm/plat-imx/pm/psci-cpuidle-imx7.S
@@ -145,7 +145,7 @@
 	/* Flush the BTAC. */
 	write_bpiallis
 
-	ldr	r6, =iram_tbl_phys_addr
+	ldr	r6, =iram_tlb_phys_addr
 	ldr	r7, [r6]
 
 	dsb
diff --git a/core/arch/arm/plat-imx/pm/psci-suspend-imx7.S b/core/arch/arm/plat-imx/pm/psci-suspend-imx7.S
index 6eb4d747..b3ddd146 100644
--- a/core/arch/arm/plat-imx/pm/psci-suspend-imx7.S
+++ b/core/arch/arm/plat-imx/pm/psci-suspend-imx7.S
@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: BSD-2-Clause */
 /*
- * Copyright 2017 NXP
+ * Copyright 2017-2018 NXP
  */
 
 #include <arm.h>
@@ -89,7 +89,7 @@
 	/* Flush the BTAC. */
 	write_bpiallis
 
-	ldr	r6, =iram_tbl_phys_addr
+	ldr	r6, =iram_tlb_phys_addr
 	ldr	r6, [r6]
 	dsb
 	isb
diff --git a/core/arch/arm/plat-imx/pm/psci.c b/core/arch/arm/plat-imx/pm/psci.c
index 9cbe860a..056a5711 100644
--- a/core/arch/arm/plat-imx/pm/psci.c
+++ b/core/arch/arm/plat-imx/pm/psci.c
@@ -1,6 +1,7 @@
 // SPDX-License-Identifier: BSD-2-Clause
 /*
  * Copyright (C) 2016 Freescale Semiconductor, Inc.
+ * Copyright 2018 NXP
  *
  * Peng Fan <peng.fan@nxp.com>
  */
@@ -152,17 +153,59 @@ int psci_affinity_info(uint32_t affinity,
 
 void __noreturn psci_system_off(void)
 {
+#ifndef CFG_MX7ULP
 	vaddr_t snvs_base = core_mmu_get_va(SNVS_BASE, MEM_AREA_IO_SEC);
 
 	write32(SNVS_LPCR_TOP_MASK |
 		SNVS_LPCR_DP_EN_MASK |
 		SNVS_LPCR_SRTC_ENV_MASK, snvs_base + SNVS_LPCR_OFF);
 	dsb();
+#endif
 
 	while (1)
 		;
 }
 
+__weak int imx6ul_lowpower_idle(uint32_t power_state __unused,
+				uintptr_t entry __unused,
+				uint32_t context_id __unused,
+				struct sm_nsec_ctx *nsec __unused)
+{
+	return 0;
+}
+
+__weak int imx6sx_lowpower_idle(uint32_t power_state __unused,
+				uintptr_t entry __unused,
+				uint32_t context_id __unused,
+				struct sm_nsec_ctx *nsec __unused)
+{
+	return 0;
+}
+
+__weak int imx6sl_lowpower_idle(uint32_t power_state __unused,
+				uintptr_t entry __unused,
+				uint32_t context_id __unused,
+				struct sm_nsec_ctx *nsec __unused)
+{
+	return 0;
+}
+
+__weak int imx6sll_lowpower_idle(uint32_t power_state __unused,
+				uintptr_t entry __unused,
+				uint32_t context_id __unused,
+				struct sm_nsec_ctx *nsec __unused)
+{
+	return 0;
+}
+
+__weak int imx6_cpu_suspend(uint32_t power_state __unused,
+			    uintptr_t entry __unused,
+			    uint32_t context_id __unused,
+			    struct sm_nsec_ctx *nsec __unused)
+{
+	return 0;
+}
+
 __weak int imx7d_lowpower_idle(uint32_t power_state __unused,
 			uintptr_t entry __unused,
 			uint32_t context_id __unused,
@@ -179,6 +222,14 @@ __weak int imx7_cpu_suspend(uint32_t power_state __unused,
 	return 0;
 }
 
+__weak int imx7ulp_cpu_suspend(uint32_t power_state __unused,
+			    uintptr_t entry __unused,
+			    uint32_t context_id __unused,
+			    struct sm_nsec_ctx *nsec __unused)
+{
+	return 0;
+}
+
 int psci_cpu_suspend(uint32_t power_state,
 		     uintptr_t entry, uint32_t context_id __unused,
 		     struct sm_nsec_ctx *nsec)
@@ -202,17 +253,50 @@ int psci_cpu_suspend(uint32_t power_state,
 	 * TODO: follow PSCI StateID sample encoding.
 	 */
 	DMSG("ID = %d\n", id);
+
+	/*
+	 * For i.MX6SL, the cpuidle need the state of LDO 2P5 and
+	 * the busfreq mode. these info is packed in the power_state,
+	 * when doing 'id' check, the LDO 2P5 and busfreq mode info need
+	 * to be removed from 'id'.
+	 */
+	if (soc_is_imx6sl())
+		id &= ~(0x6);
+
 	if (id == 1) {
-		if (soc_is_imx7ds())
+		if (soc_is_imx6ul() || soc_is_imx6ull())
+			return imx6ul_lowpower_idle(power_state, entry,
+						    context_id, nsec);
+		else if (soc_is_imx7ds())
 			return imx7d_lowpower_idle(power_state, entry,
 						   context_id, nsec);
-		return ret;
+		else if (soc_is_imx6sx())
+			return imx6sx_lowpower_idle(power_state, entry,
+						    context_id, nsec);
+		else if (soc_is_imx6sl())
+			return imx6sl_lowpower_idle(power_state, entry,
+						    context_id, nsec);
+		else if (soc_is_imx6sll())
+			return imx6sll_lowpower_idle(power_state, entry,
+						    context_id, nsec);
+		else {
+			EMSG("Not supported now\n");
+			return ret;
+		}
 	} else if (id == 0) {
-		if (soc_is_imx7ds()) {
+		if (soc_is_imx7ds())
 			return imx7_cpu_suspend(power_state, entry,
 						context_id, nsec);
+		else if (soc_is_imx6()) 
+			return imx6_cpu_suspend(power_state, entry,
+						context_id, nsec);
+		else if (soc_is_imx7ulp())
+			return imx7ulp_cpu_suspend(power_state, entry,
+						context_id, nsec);
+		else {
+			EMSG("Not supported now\n");
+			return ret;
 		}
-		return ret;
 	}
 
 	DMSG("ID %d not supported\n", id);
@@ -220,7 +304,85 @@ int psci_cpu_suspend(uint32_t power_state,
 	return ret;
 }
 
+/* Weak functions because files are not all built */
+__weak int imx6ul_cpuidle_init(void)
+{
+	return 0;
+}
+
+__weak int imx6sx_cpuidle_init(void)
+{
+	return 0;
+}
+
+__weak int imx6sll_cpuidle_init(void)
+{
+	return 0;
+}
+
+__weak int imx6_suspend_init(void)
+{
+	return 0;
+}
+
+__weak int imx7d_cpuidle_init(void)
+{
+	return 0;
+}
+
+__weak int imx7_suspend_init(void)
+{
+	return 0;
+}
+
+__weak int imx7ulp_suspend_init(void)
+{
+	return 0;
+}
+
+static TEE_Result init_psci(void)
+{
+	TEE_Result ret = TEE_SUCCESS;
+	int err = 0;
+
+	/*
+	 * Initialize the power management data.
+	 * It must be done after the OCRAM initialization.
+	 */
+#ifdef CFG_MX7ULP
+	err = imx7ulp_suspend_init();
+#else
+	if (soc_is_imx6ul() || soc_is_imx6ull()) {
+		err = imx6ul_cpuidle_init();
+	} else if (soc_is_imx6sx()) {
+		err = imx6sx_cpuidle_init();
+	} else if (soc_is_imx6sll()) {
+		err = imx6sll_cpuidle_init();
+	} else if (soc_is_imx7ds()) {
+		err = imx7d_cpuidle_init();
+	}
+
+	if (!err) {
+		if (soc_is_imx6()) {
+			err = imx6_suspend_init();
+		} else {
+			if (soc_is_imx7ds()) {
+				err = imx7_suspend_init();
+			}
+		}
+	}
+#endif
+
+	if (err) {
+		ret = TEE_ERROR_GENERIC;
+	}
+
+	return ret;
+}
+
 void psci_system_reset(void)
 {
 	imx_wdog_restart();
 }
+
+service_init(init_psci);