arm: imx: add i.MX7D support

Add i.MX7D support.
 - Add register definition
 - Add gpcv2 to powerup and powerdown cpu
 - Introduce soc runtime detection, the final goal is to support i.MX
   family using one image, but still far from it. Now using the runtime
   detection, we could remove the CFG_MX[6,7][x] to simplify the code,
   such as in imx psci cpu on/off using one function to support 6Q/7D
   without CFG_[X].
 - Add PSCI cpu/off/affinity

The scripts to build 7dsdb image.
make PLATFORM=imx-mx7dsabresd \
mkimage -A arm -O linux -C none -a 0xbdffffe4 -e 0xbe000000 \
 -d out/arm-plat-imx/core/tee.bin uTee-7d

Signed-off-by: Peng Fan <peng.fan@nxp.com>
Acked-by: Jerome Forissier <jerome.forissier@linaro.org>
Acked-by: Jens Wiklander <jens.wiklander@linaro.org>

14 files changed, 539 insertions, 31 deletions

diff --git a/.travis.yml b/.travis.yml
index d24aeace..30fe2ead 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -195,6 +195,9 @@ script:
   - $make PLATFORM=imx-mx6qsabresd
   - $make PLATFORM=imx-mx6dlsabresd
 
+  # i.MX7Dual SABRE
+  - $make PLATFORM=imx-mx7dsabresd
+
   # Texas Instruments DRA7xx
   - $make PLATFORM=ti-dra7xx
 
diff --git a/MAINTAINERS.md b/MAINTAINERS.md
index f8ff35be..1a881d8a 100644
--- a/MAINTAINERS.md
+++ b/MAINTAINERS.md
@@ -14,6 +14,7 @@ for these platforms.
 | FSL i.MX6 Quad SABRE SD Board		|`Yan Yan <yan.yan@windriver.com>`,`Feng Yu <Yu.Feng@windriver.com>`|
 | FSL i.MX6 UltraLite EVK Board		|`Peng Fan <peng.fan@nxp.com>`|
 | NXP i.MX6 ULL EVK Board		|`Peng Fan <peng.fan@nxp.com>`|
+| NXP i.MX7 Dual SabreSD Board		|`Peng Fan <peng.fan@nxp.com>`|
 | ARM Foundation FVP			|`Linaro <op-tee@linaro.org>`|
 | HiKey Board (HiSilicon Kirin 620)	|`Linaro <op-tee@linaro.org>`|
 | HiSilicon D02				|`Linaro <op-tee@linaro.org>`|
diff --git a/README.md b/README.md
index 6d8fab89..8aa6e945 100644
--- a/README.md
+++ b/README.md
@@ -50,6 +50,7 @@ platforms have different sub-maintainers, please refer to the file
 | [FSL i.MX6 Quad SABRE Lite Board](https://boundarydevices.com/product/sabre-lite-imx6-sbc/) |`PLATFORM=imx`| Yes |
 | [FSL i.MX6 Quad SABRE SD Board](http://www.nxp.com/products/software-and-tools/hardware-development-tools/sabre-development-system/sabre-board-for-smart-devices-based-on-the-i.mx-6quad-applications-processors:RD-IMX6Q-SABRE) |`PLATFORM=imx`| Yes |
 | [FSL i.MX6 UltraLite EVK Board](http://www.freescale.com/products/arm-processors/i.mx-applications-processors-based-on-arm-cores/i.mx-6-processors/i.mx6qp/i.mx6ultralite-evaluation-kit:MCIMX6UL-EVK) |`PLATFORM=imx`| Yes |
+| [NXP i.MX7Dual SabreSD Board](http://www.nxp.com/products/software-and-tools/hardware-development-tools/sabre-development-system/sabre-board-for-smart-devices-based-on-the-i.mx-7dual-applications-processors:MCIMX7SABRE) |`PLATFORM=imx-mx7dsabresd`| Yes |
 | [ARM Foundation FVP](https://developer.arm.com/products/system-design/fixed-virtual-platforms) |`PLATFORM=vexpress-fvp`| Yes |
 | [HiSilicon D02](http://open-estuary.org/d02-2)|`PLATFORM=d02`| No |
 | [HiKey Board (HiSilicon Kirin 620)](https://www.96boards.org/products/hikey)|`PLATFORM=hikey`| Yes |
diff --git a/core/arch/arm/plat-imx/conf.mk b/core/arch/arm/plat-imx/conf.mk
index 93d60451..76ef4494 100644
--- a/core/arch/arm/plat-imx/conf.mk
+++ b/core/arch/arm/plat-imx/conf.mk
@@ -7,6 +7,7 @@ mx6q-flavorlist = mx6qsabrelite mx6qsabresd
 mx6d-flavorlist =
 mx6dl-flavorlist = mx6dlsabresd
 mx6s-flavorlist =
+mx7-flavorlist = mx7dsabresd
 
 ifneq (,$(filter $(PLATFORM_FLAVOR),$(mx6ul-flavorlist)))
 $(call force,CFG_MX6UL,y)
@@ -20,10 +21,19 @@ else ifneq (,$(filter $(PLATFORM_FLAVOR),$(mx6dl-flavorlist)))
 $(call force,CFG_MX6DL,y)
 else ifneq (,$(filter $(PLATFORM_FLAVOR),$(mx6s-flavorlist)))
 $(call force,CFG_MX6S,y)
+else ifneq (,$(filter $(PLATFORM_FLAVOR),$(mx7-flavorlist)))
+$(call force,CFG_MX7,y)
 else
 $(error Unsupported PLATFORM_FLAVOR "$(PLATFORM_FLAVOR)")
 endif
 
+ifneq (,$(filter $(PLATFORM_FLAVOR),mx7dsabresd))
+CFG_DDR_SIZE ?= 0x40000000
+CFG_DT ?= y
+CFG_NS_ENTRY_ADDR ?= 0x80800000
+CFG_PSCI_ARM32 ?= y
+CFG_TEE_CORE_NB_CORE ?= 2
+endif
 
 # Common i.MX6 config
 core_arm32-platform-aflags	+= -mfpu=neon
@@ -60,6 +70,11 @@ CFG_BOOT_SYNC_CPU ?= y
 CFG_BOOT_SECONDARY_REQUEST ?= y
 endif
 
+ifeq ($(filter y, $(CFG_MX7)), y)
+include core/arch/arm/cpu/cortex-a7.mk
 
-ta-targets = ta_arm32
+$(call force,CFG_SECURE_TIME_SOURCE_REE,y)
+CFG_BOOT_SECONDARY_REQUEST ?= y
+endif
 
+ta-targets = ta_arm32
diff --git a/core/arch/arm/plat-imx/config/config_imx7.h b/core/arch/arm/plat-imx/config/config_imx7.h
new file mode 100644
index 00000000..4066c678
--- /dev/null
+++ b/core/arch/arm/plat-imx/config/config_imx7.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright 2017 NXP
+ * All rights reserved.
+ *
+ * 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.
+ */
+
+#ifndef CFG_UART_BASE
+#define CFG_UART_BASE	(UART1_BASE)
+#endif
+
+#ifndef CFG_DDR_SIZE
+#error "CFG_DDR_SIZE not defined"
+#endif
+
+#define DRAM0_BASE		0x80000000
+#define DRAM0_SIZE		CFG_DDR_SIZE
+
+/* Location of trusted dram */
+#define TZDRAM_BASE		(DRAM0_BASE + CFG_DDR_SIZE - 32 * 1024 * 1024)
+#define TZDRAM_SIZE		(30 * 1024 * 1024)
+
+/* Full GlobalPlatform test suite requires CFG_SHMEM_SIZE to be at least 2MB */
+#define CFG_SHMEM_START		(TZDRAM_BASE + TZDRAM_SIZE)
+#define CFG_SHMEM_SIZE		0x200000
+
+#define CFG_TEE_RAM_VA_SIZE	(1024 * 1024)
+
+/*
+ * Everything is in TZDRAM.
+ * +------------------+
+ * |        | TEE_RAM |
+ * + TZDRAM +---------+
+ * |        | TA_RAM  |
+ * +--------+---------+
+ */
+#define CFG_TEE_RAM_PH_SIZE     CFG_TEE_RAM_VA_SIZE
+#define CFG_TEE_RAM_START	TZDRAM_BASE
+#define CFG_TA_RAM_START	ROUNDUP((TZDRAM_BASE + CFG_TEE_RAM_VA_SIZE), \
+					CORE_MMU_DEVICE_SIZE)
+#define CFG_TA_RAM_SIZE		ROUNDDOWN((TZDRAM_SIZE - CFG_TEE_RAM_VA_SIZE), \
+					  CORE_MMU_DEVICE_SIZE)
+
+#ifndef CFG_TEE_LOAD_ADDR
+#define CFG_TEE_LOAD_ADDR	CFG_TEE_RAM_START
+#endif
+
+#define CONSOLE_UART_BASE	(CFG_UART_BASE)
diff --git a/core/arch/arm/plat-imx/gpcv2.c b/core/arch/arm/plat-imx/gpcv2.c
new file mode 100644
index 00000000..ec3a5303
--- /dev/null
+++ b/core/arch/arm/plat-imx/gpcv2.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) 2017 NXP
+ * All rights reserved.
+ *
+ * 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 <imx.h>
+#include <io.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <stdint.h>
+
+static vaddr_t gpc_base(void)
+{
+	return core_mmu_get_va(GPC_BASE, MEM_AREA_IO_SEC);
+}
+
+void imx_gpcv2_set_core_pgc(bool enable, uint32_t offset)
+{
+	uint32_t val = read32(gpc_base() + offset) & (~GPC_PGC_PCG_MASK);
+
+	if (enable)
+		val |= GPC_PGC_PCG_MASK;
+
+	write32(val, gpc_base() + offset);
+}
+
+void imx_gpcv2_set_core1_pdn_by_software(void)
+{
+	uint32_t val = read32(gpc_base() + GPC_CPU_PGC_SW_PDN_REQ);
+
+	imx_gpcv2_set_core_pgc(true, GPC_PGC_C1);
+
+	val |= GPC_PGC_SW_PDN_PUP_REQ_CORE1_MASK;
+
+	write32(val, gpc_base() + GPC_CPU_PGC_SW_PDN_REQ);
+
+	while ((read32(gpc_base() + GPC_CPU_PGC_SW_PDN_REQ) &
+	       GPC_PGC_SW_PDN_PUP_REQ_CORE1_MASK) != 0)
+		;
+
+	imx_gpcv2_set_core_pgc(false, GPC_PGC_C1);
+}
+
+void imx_gpcv2_set_core1_pup_by_software(void)
+{
+	uint32_t val = read32(gpc_base() + GPC_CPU_PGC_SW_PUP_REQ);
+
+	imx_gpcv2_set_core_pgc(true, GPC_PGC_C1);
+
+	val |= GPC_PGC_SW_PDN_PUP_REQ_CORE1_MASK;
+
+	write32(val, gpc_base() + GPC_CPU_PGC_SW_PUP_REQ);
+
+	while ((read32(gpc_base() + GPC_CPU_PGC_SW_PUP_REQ) &
+	       GPC_PGC_SW_PDN_PUP_REQ_CORE1_MASK) != 0)
+		;
+
+	imx_gpcv2_set_core_pgc(false, GPC_PGC_C1);
+}
diff --git a/core/arch/arm/plat-imx/imx-common.c b/core/arch/arm/plat-imx/imx-common.c
index 953d7d1a..3e584e84 100644
--- a/core/arch/arm/plat-imx/imx-common.c
+++ b/core/arch/arm/plat-imx/imx-common.c
@@ -35,16 +35,111 @@
 #include <mm/core_memprot.h>
 #include <platform_config.h>
 
+static uint32_t imx_digproc(void)
+{
+	static uint32_t reg;
+	vaddr_t anatop_addr;
+
+	if (!reg) {
+		anatop_addr = core_mmu_get_va(ANATOP_BASE, MEM_AREA_IO_SEC);
+
+		/* TODO: Handle SL here */
+#ifdef CFG_MX7
+		reg = read32(anatop_addr + OFFSET_DIGPROG_IMX7D);
+#else
+		reg = read32(anatop_addr + OFFSET_DIGPROG);
+#endif
+	}
+
+	return reg;
+}
+
+static uint32_t imx_soc_rev_major(void)
+{
+	return ((imx_digproc() & 0xff00) >> 8) + 1;
+}
+
+uint32_t imx_soc_type(void)
+{
+	return (imx_digproc() >> 16) & 0xff;
+}
+
+bool soc_is_imx6ul(void)
+{
+	return imx_soc_type() == SOC_MX6UL;
+}
+
+bool soc_is_imx6ull(void)
+{
+	return imx_soc_type() == SOC_MX6ULL;
+}
+
+bool soc_is_imx6sdl(void)
+{
+	return imx_soc_type() == SOC_MX6DL;
+}
+
+bool soc_is_imx6dq(void)
+{
+	return (imx_soc_type() == SOC_MX6Q) && (imx_soc_rev_major() == 1);
+}
+
+bool soc_is_imx6dqp(void)
+{
+	return (imx_soc_type() == SOC_MX6Q) && (imx_soc_rev_major() == 2);
+}
+
+bool soc_is_imx7s(void)
+{
+	vaddr_t addr = core_mmu_get_va(OCOTP_BASE + 0x450, MEM_AREA_IO_SEC);
+	uint32_t val = read32(addr);
+
+	if (soc_is_imx7ds()) {
+		if (val & 1)
+			return true;
+		else
+			return false;
+	}
+
+	return false;
+}
+
+bool soc_is_imx7d(void)
+{
+	vaddr_t addr = core_mmu_get_va(OCOTP_BASE + 0x450, MEM_AREA_IO_SEC);
+	uint32_t val = read32(addr);
+
+	if (soc_is_imx7ds()) {
+		if (val & 1)
+			return false;
+		else
+			return true;
+	}
+
+	return false;
+}
+
+bool soc_is_imx7ds(void)
+{
+	return imx_soc_type() == SOC_MX7D;
+}
+
 uint32_t imx_get_src_gpr(int cpu)
 {
 	vaddr_t va = core_mmu_get_va(SRC_BASE, MEM_AREA_IO_SEC);
 
-	return read32(va + SRC_GPR1 + cpu * 8 + 4);
+	if (soc_is_imx7d())
+		return read32(va + SRC_GPR1_MX7 + cpu * 8 + 4);
+	else
+		return read32(va + SRC_GPR1 + cpu * 8 + 4);
 }
 
 void imx_set_src_gpr(int cpu, uint32_t val)
 {
 	vaddr_t va = core_mmu_get_va(SRC_BASE, MEM_AREA_IO_SEC);
 
-	write32(val, va + SRC_GPR1 + cpu * 8 + 4);
+	if (soc_is_imx7d())
+		write32(val, va + SRC_GPR1_MX7 + cpu * 8 + 4);
+	else
+		write32(val, va + SRC_GPR1 + cpu * 8 + 4);
 }
diff --git a/core/arch/arm/plat-imx/imx-regs.h b/core/arch/arm/plat-imx/imx-regs.h
index 77a02818..860c7adf 100644
--- a/core/arch/arm/plat-imx/imx-regs.h
+++ b/core/arch/arm/plat-imx/imx-regs.h
@@ -132,32 +132,114 @@
 #define IOMUXC_GPR10_OCRAM_TZ_ADDR_LOCK_OFFSET_6UL	(27)
 #define IOMUXC_GPR10_OCRAM_TZ_ADDR_LOCK_MASK_6UL	GENMASK_32(31, 27)
 
-/* Central Security Unit register values */
-#define CSU_BASE			0x021C0000
-#define CSU_CSL_START			0x0
-#define CSU_CSL_END			0xA0
-#define CSU_CSL5			0x14
-#define CSU_CSL16			0x40
-#define CSU_ACCESS_ALL			0x00FF00FF
-#define CSU_SETTING_LOCK		0x01000100
-
 #if defined(CFG_MX6UL) || defined(CFG_MX6ULL)
 #define DRAM0_BASE			0x80000000
 #else
 #define DRAM0_BASE			0x10000000
 #endif
 
-#define SRC_SCR				0x000
-#define SRC_GPR1			0x020
-#define SRC_SCR_CPU_ENABLE_ALL		SHIFT_U32(0x7, 22)
-#define SRC_SCR_CORE1_RST_OFFSET	14
-#define SRC_SCR_CORE1_ENABLE_OFFSET	22
+#elif defined(CFG_MX7)
+#define GIC_BASE		0x31000000
+#define GIC_SIZE		0x8000
+#define GICC_OFFSET		0x2000
+#define GICD_OFFSET		0x1000
+
+#define CAAM_BASE		0x30900000
+#define UART1_BASE		0x30860000
+#define UART2_BASE		0x30890000
+#define UART3_BASE		0x30880000
+
+#define AIPS1_BASE		0x30000000
+#define AIPS1_SIZE		0x400000
+#define AIPS2_BASE		0x30400000
+#define AIPS2_SIZE		0x400000
+#define AIPS3_BASE		0x30800000
+#define AIPS3_SIZE		0x400000
+
+#define WDOG_BASE		0x30280000
+#define LPSR_BASE		0x30270000
+#define IOMUXC_BASE		0x30330000
+#define IOMUXC_GPR_BASE		0x30340000
+#define OCOTP_BASE		0x30350000
+#define ANATOP_BASE		0x30360000
+#define SNVS_BASE		0x30370000
+#define CCM_BASE		0x30380000
+#define SRC_BASE		0x30390000
+#define GPC_BASE		0x303A0000
+#define TZASC_BASE		0x30780000
+#define DDRC_PHY_BASE		0x30790000
+#define MMDC_P0_BASE		0x307A0000
+#define DDRC_BASE		0x307A0000
+#define IRAM_BASE		0x00900000
+#define IRAM_S_BASE		0x00180000
+
+#define CSU_CSL_START		0x303E0000
+#define CSU_CSL_END		0x303E0100
+#define CSU_CSL_59		(0x303E0000 + 59 * 4)
+#define CSU_CSL_28		(0x303E0000 + 28 * 4)
+#define CSU_CSL_15		(0x303E0000 + 15 * 4)
+#define CSU_CSL_12		(0x303E0000 + 12 * 4)
+#define	CSU_ACCESS_ALL		0x00FF00FF
+#define CSU_SETTING_LOCK	0x01000100
+
+#define TRUSTZONE_OCRAM_START	0x180000
+
+#define IOMUXC_GPR9_OFFSET				0x24
+#define IOMUXC_GPR9_TZASC1_MUX_CONTROL_OFFSET		0
+
+#define IOMUXC_GPR11_OFFSET				0x2C
+#define IOMUXC_GPR11_OCRAM_S_TZ_ADDR_OFFSET		11
+#define IOMUXC_GPR11_OCRAM_S_TZ_ADDR_MASK		GENMASK_32(13, 11)
+
+#define IOMUXC_GPR11_OCRAM_S_TZ_EN_OFFSET		10
+#define IOMUXC_GPR11_OCRAM_S_TZ_EN_MASK			GENMASK_32(10, 10)
+
+#define IOMUXC_GPR11_OCRAM_S_TZ_EN_LOCK_OFFSET		26
+#define IOMUXC_GPR11_OCRAM_S_TZ_EN_LOCK_MASK		GENMASK_32(26, 26)
+#define IOMUXC_GPR11_OCRAM_S_TZ_ADDR_LOCK_OFFSET	GENMASK_32(29, 27)
 #else
-#error "CFG_MX6 not defined"
+#error "CFG_MX6/7 not defined"
 #endif
 
 #define IOMUXC_GPR4_OFFSET	0x10
 #define IOMUXC_GPR5_OFFSET	0x14
 #define ARM_WFI_STAT_MASK(n)	BIT(n)
 
+#define ARM_WFI_STAT_MASK_7D(n)	BIT(25 + ((n) & 1))
+
+#define SRC_SCR				0x000
+#define SRC_GPR1			0x020
+#define SRC_GPR2			0x024
+#define SRC_SCR_CORE1_RST_OFFSET	14
+#define SRC_SCR_CORE1_ENABLE_OFFSET	22
+#define SRC_SCR_CPU_ENABLE_ALL		SHIFT_U32(0x7, 22)
+
+#define SRC_GPR1_MX7			0x074
+#define SRC_A7RCR0			0x004
+#define SRC_A7RCR1			0x008
+#define SRC_A7RCR0_A7_CORE_RESET0_OFFSET	0
+#define SRC_A7RCR1_A7_CORE1_ENABLE_OFFSET	1
+
+#define SNVS_LPCR_OFF			0x38
+#define SNVS_LPCR_TOP_MASK		BIT(6)
+#define SNVS_LPCR_DP_EN_MASK		BIT(5)
+#define SNVS_LPCR_SRTC_ENV_MASK		1
+
+#define WCR_OFF				0
+
+#define OFFSET_DIGPROG			0x260
+#define OFFSET_DIGPROG_IMX6SL		0x280
+#define OFFSET_DIGPROG_IMX7D		0x800
+
+/* GPC V2 */
+#define GPC_PGC_C1			0x840
+#define GPC_PGC_C1_PUPSCR		0x844
+
+#define GPC_PGC_PCG_MASK		BIT(0)
+
+#define GPC_CPU_PGC_SW_PUP_REQ		0xf0
+#define GPC_PU_PGC_SW_PUP_REQ		0xf8
+#define GPC_CPU_PGC_SW_PDN_REQ		0xfc
+#define GPC_PU_PGC_SW_PDN_REQ		0x104
+#define GPC_PGC_SW_PDN_PUP_REQ_CORE1_MASK BIT(1)
 #endif
diff --git a/core/arch/arm/plat-imx/imx.h b/core/arch/arm/plat-imx/imx.h
index 144ac1d7..953a3b56 100644
--- a/core/arch/arm/plat-imx/imx.h
+++ b/core/arch/arm/plat-imx/imx.h
@@ -30,7 +30,31 @@
 #define PLAT_IMX_IMX_H
 
 #include <stdint.h>
+#include <stdbool.h>
+
+#define SOC_MX6SL	0x60
+#define SOC_MX6DL	0x61
+#define SOC_MX6SX	0x62
+#define SOC_MX6Q	0x63
+#define SOC_MX6UL	0x64
+#define SOC_MX6ULL	0x65
+#define SOC_MX6SLL	0x67
+#define SOC_MX6D	0x6A
+#define SOC_MX7D	0x72
 
 uint32_t imx_get_src_gpr(int cpu);
 void imx_set_src_gpr(int cpu, uint32_t val);
+
+bool soc_is_imx6ul(void);
+bool soc_is_imx6ull(void);
+bool soc_is_imx6sdl(void);
+bool soc_is_imx6dq(void);
+bool soc_is_imx6dqp(void);
+bool soc_is_imx7ds(void);
+bool soc_is_imx7d(void);
+bool soc_is_imx7s(void);
+uint32_t imx_soc_type(void);
+void imx_gpcv2_set_core1_pdn_by_software(void);
+void imx_gpcv2_set_core1_pup_by_software(void);
+void imx_gpcv2_set_core_pgc(bool enable, uint32_t offset);
 #endif
diff --git a/core/arch/arm/plat-imx/imx7.c b/core/arch/arm/plat-imx/imx7.c
new file mode 100644
index 00000000..96132bd3
--- /dev/null
+++ b/core/arch/arm/plat-imx/imx7.c
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2017 NXP
+ * All rights reserved.
+ *
+ * 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 <drivers/tzc380.h>
+#include <io.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <kernel/misc.h>
+#include <kernel/pm_stubs.h>
+#include <mm/core_mmu.h>
+#include <mm/core_memprot.h>
+#include <platform_config.h>
+#include <imx.h>
+#include <stdint.h>
+#include <sm/optee_smc.h>
+#include <sm/psci.h>
+#include <tee/entry_std.h>
+#include <tee/entry_fast.h>
+#include <util.h>
+
+register_phys_mem(MEM_AREA_IO_SEC, SRC_BASE, CORE_MMU_DEVICE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, IOMUXC_BASE, CORE_MMU_DEVICE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, CCM_BASE, CORE_MMU_DEVICE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, ANATOP_BASE, CORE_MMU_DEVICE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, GPC_BASE, CORE_MMU_DEVICE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, DDRC_BASE, CORE_MMU_DEVICE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, AIPS1_BASE, AIPS1_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, AIPS2_BASE, AIPS2_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, AIPS3_BASE, AIPS3_SIZE);
+
+void plat_cpu_reset_late(void)
+{
+	uintptr_t addr;
+	uint32_t val;
+
+	if (get_core_pos() != 0)
+		return;
+
+	/*
+	 * Configure imx7 CSU, first grant all peripherals
+	 * TODO: fine tune the permissions
+	 */
+	for (addr = CSU_CSL_START; addr != CSU_CSL_END; addr += 4)
+		write32(CSU_ACCESS_ALL, core_mmu_get_va(addr, MEM_AREA_IO_SEC));
+
+	dsb();
+	/* Protect OCRAM_S */
+	write32(0x003300FF, core_mmu_get_va(CSU_CSL_59, MEM_AREA_IO_SEC));
+	/* Proect TZASC */
+	write32(0x00FF0033, core_mmu_get_va(CSU_CSL_28, MEM_AREA_IO_SEC));
+	/*
+	 * Proect CSU
+	 * Note: Ater this settings, CSU seems still can be read,
+	 * in non-secure world but can not be written.
+	 */
+	write32(0x00FF0033, core_mmu_get_va(CSU_CSL_15, MEM_AREA_IO_SEC));
+	/*
+	 * Proect SRC
+	 * write32(0x003300FF, get_base(CSU_CSL_12, MEM_AREA_IO_SEC));
+	 */
+	dsb();
+
+	/* lock the settings */
+	for (addr = CSU_CSL_START; addr != CSU_CSL_END; addr += 4) {
+		val = read32(core_mmu_get_va(addr, MEM_AREA_IO_SEC));
+		write32(val | CSU_SETTING_LOCK,
+			core_mmu_get_va(addr, MEM_AREA_IO_SEC));
+	}
+}
diff --git a/core/arch/arm/plat-imx/main.c b/core/arch/arm/plat-imx/main.c
index f02ba47e..ea43d4b3 100644
--- a/core/arch/arm/plat-imx/main.c
+++ b/core/arch/arm/plat-imx/main.c
@@ -64,6 +64,7 @@ static struct imx_uart_data console_data;
 
 register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, CORE_MMU_DEVICE_SIZE);
 register_phys_mem(MEM_AREA_IO_SEC, GIC_BASE, CORE_MMU_DEVICE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, ANATOP_BASE, CORE_MMU_DEVICE_SIZE);
 
 const struct thread_handlers *generic_boot_get_handlers(void)
 {
@@ -97,7 +98,8 @@ void main_init_gic(void)
 	itr_init(&gic_data.chip);
 }
 
-#if defined(CFG_MX6Q) || defined(CFG_MX6D) || defined(CFG_MX6DL)
+#if defined(CFG_MX6Q) || defined(CFG_MX6D) || defined(CFG_MX6DL) || \
+	defined(CFG_MX7)
 void main_secondary_init_gic(void)
 {
 	gic_cpu_init(&gic_data);
diff --git a/core/arch/arm/plat-imx/platform_config.h b/core/arch/arm/plat-imx/platform_config.h
index 4a71b42f..28c5ff8f 100644
--- a/core/arch/arm/plat-imx/platform_config.h
+++ b/core/arch/arm/plat-imx/platform_config.h
@@ -30,11 +30,16 @@
 #ifndef PLATFORM_CONFIG_H
 #define PLATFORM_CONFIG_H
 
+#include <imx-regs.h>
+
 #define STACK_ALIGNMENT			64
 
-/* For i.MX 6UltraLite and 6ULL EVK board */
+/* For i.MX7D/S platforms */
+#if defined(CFG_MX7)
+#include <config/config_imx7.h>
 
-#if defined(CFG_MX6UL) || defined(CFG_MX6ULL)
+/* For i.MX 6UltraLite and 6ULL EVK board */
+#elif defined(CFG_MX6UL) || defined(CFG_MX6ULL)
 #include <imx-regs.h>
 
 #ifdef CFG_WITH_PAGER
diff --git a/core/arch/arm/plat-imx/psci.c b/core/arch/arm/plat-imx/psci.c
index a0a70e0b..3f182d2a 100644
--- a/core/arch/arm/plat-imx/psci.c
+++ b/core/arch/arm/plat-imx/psci.c
@@ -60,6 +60,21 @@ int psci_cpu_on(uint32_t core_idx, uint32_t entry,
 	/* set secondary cores' NS entry addresses */
 	ns_entry_addrs[core_idx] = entry;
 
+	if (soc_is_imx7ds()) {
+		write32((uint32_t)CFG_TEE_LOAD_ADDR,
+			va + SRC_GPR1_MX7 + core_idx * 8);
+
+		imx_gpcv2_set_core1_pup_by_software();
+
+		/* release secondary core */
+		val = read32(va + SRC_A7RCR1);
+		val |=  BIT32(SRC_A7RCR1_A7_CORE1_ENABLE_OFFSET +
+			      (core_idx - 1));
+		write32(val, va + SRC_A7RCR1);
+
+		return PSCI_RET_SUCCESS;
+	}
+
 	/* boot secondary cores from OP-TEE load address */
 	write32((uint32_t)CFG_TEE_LOAD_ADDR, va + SRC_GPR1 + core_idx * 8);
 
@@ -103,7 +118,10 @@ int psci_affinity_info(uint32_t affinity,
 
 	cpu = affinity;
 
-	wfi = read32(gpr5) & ARM_WFI_STAT_MASK(cpu);
+	if (soc_is_imx7d())
+		wfi = true;
+	else
+		wfi = read32(gpr5) & ARM_WFI_STAT_MASK(cpu);
 
 	if ((imx_get_src_gpr(cpu) == 0) || !wfi)
 		return PSCI_AFFINITY_LEVEL_ON;
@@ -113,14 +131,23 @@ int psci_affinity_info(uint32_t affinity,
 	 * Wait secondary cpus ready to be killed
 	 * TODO: Change to non dead loop
 	 */
-	while (read32(va + SRC_GPR1 + cpu * 8 + 4) != UINT32_MAX)
-		;
-
-	/* Kill cpu */
-	val = read32(va + SRC_SCR);
-	val &= ~BIT32(SRC_SCR_CORE1_ENABLE_OFFSET + cpu - 1);
-	val |=  BIT32(SRC_SCR_CORE1_RST_OFFSET + cpu - 1);
-	write32(val, va + SRC_SCR);
+	if (soc_is_imx7d()) {
+		while (read32(va + SRC_GPR1_MX7 + cpu * 8 + 4) != UINT_MAX)
+			;
+
+		val = read32(va + SRC_A7RCR1);
+		val &=  ~BIT32(SRC_A7RCR1_A7_CORE1_ENABLE_OFFSET + (cpu - 1));
+		write32(val, va + SRC_A7RCR1);
+	} else {
+		while (read32(va + SRC_GPR1 + cpu * 8 + 4) != UINT32_MAX)
+			;
+
+		/* Kill cpu */
+		val = read32(va + SRC_SCR);
+		val &= ~BIT32(SRC_SCR_CORE1_ENABLE_OFFSET + cpu - 1);
+		val |=  BIT32(SRC_SCR_CORE1_RST_OFFSET + cpu - 1);
+		write32(val, va + SRC_SCR);
+	}
 
 	/* Clean arg */
 	imx_set_src_gpr(cpu, 0);
diff --git a/core/arch/arm/plat-imx/sub.mk b/core/arch/arm/plat-imx/sub.mk
index e42213ca..90df1233 100644
--- a/core/arch/arm/plat-imx/sub.mk
+++ b/core/arch/arm/plat-imx/sub.mk
@@ -2,7 +2,7 @@ global-incdirs-y += .
 srcs-y += main.c imx-common.c
 
 srcs-$(CFG_PL310) += imx_pl310.c
-srcs-$(CFG_PSCI_ARM32) += psci.c
+srcs-$(CFG_PSCI_ARM32) += psci.c gpcv2.c
 cflags-psci.c-y += -Wno-suggest-attribute=noreturn
 
 ifneq (,$(filter y, $(CFG_MX6Q) $(CFG_MX6D) $(CFG_MX6DL)))
@@ -13,3 +13,5 @@ ifneq (,$(filter y, $(CFG_MX6UL) $(CFG_MX6ULL)))
 srcs-y += a7_plat_init.S
 srcs-y += imx6ul.c
 endif
+
+srcs-$(CFG_MX7) += imx7.c a7_plat_init.S