diff --git a/arch/arm/mach-imx/mx7/psci-mx7.c b/arch/arm/mach-imx/mx7/psci-mx7.c
index 95f8cb4..8b839d3 100644
--- a/arch/arm/mach-imx/mx7/psci-mx7.c
+++ b/arch/arm/mach-imx/mx7/psci-mx7.c
@@ -8,6 +8,7 @@
 #include <asm/psci.h>
 #include <asm/secure.h>
 #include <asm/arch/imx-regs.h>
+#include <linux/bitops.h>
 #include <common.h>
 #include <fsl_wdog.h>
 
@@ -34,6 +35,24 @@
 #define CCM_ROOT_WDOG		0xbb80
 #define CCM_CCGR_WDOG1		0x49c0
 
+#define MPIDR_AFF0		GENMASK(7, 0)
+
+#define IMX7D_PSCI_NR_CPUS	2
+#if IMX7D_PSCI_NR_CPUS > CONFIG_ARMV7_PSCI_NR_CPUS
+#error "invalid value for CONFIG_ARMV7_PSCI_NR_CPUS"
+#endif
+
+u8 psci_state[IMX7D_PSCI_NR_CPUS] __secure_data = {
+	 PSCI_AFFINITY_LEVEL_ON,
+	 PSCI_AFFINITY_LEVEL_OFF};
+
+static inline void psci_set_state(int cpu, u8 state)
+{
+	psci_state[cpu] = state;
+	dsb();
+	isb();
+}
+
 static inline void imx_gpcv2_set_m_core_pgc(bool enable, u32 offset)
 {
 	writel(enable, GPC_IPS_BASE_ADDR + offset);
@@ -67,25 +86,51 @@ __secure void imx_enable_cpu_ca7(int cpu, bool enable)
 	writel(val, SRC_BASE_ADDR + SRC_A7RCR1);
 }
 
+__secure void psci_arch_cpu_entry(void)
+{
+	u32 cpu = psci_get_cpu_id();
+
+	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON);
+}
+
 __secure s32 psci_cpu_on(u32 __always_unused function_id, u32 mpidr, u32 ep,
 			 u32 context_id)
 {
-	u32 cpu = (mpidr & 0x1);
+	u32 cpu = mpidr & MPIDR_AFF0;
+
+	if (mpidr & ~MPIDR_AFF0)
+		return ARM_PSCI_RET_INVAL;
+
+	if (cpu >= IMX7D_PSCI_NR_CPUS)
+		return ARM_PSCI_RET_INVAL;
+
+	if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON)
+		return ARM_PSCI_RET_ALREADY_ON;
+
+	if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON_PENDING)
+		return ARM_PSCI_RET_ON_PENDING;
 
 	psci_save(cpu, ep, context_id);
 
 	writel((u32)psci_cpu_entry, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D);
+
+	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON_PENDING);
+
 	imx_gpcv2_set_core1_power(true);
 	imx_enable_cpu_ca7(cpu, true);
-	return 0;
+
+	return ARM_PSCI_RET_SUCCESS;
 }
 
 __secure s32 psci_cpu_off(void)
 {
 	int cpu;
 
-	psci_cpu_off_common();
 	cpu = psci_get_cpu_id();
+
+	psci_cpu_off_common();
+	psci_set_state(cpu, PSCI_AFFINITY_LEVEL_OFF);
+
 	imx_enable_cpu_ca7(cpu, false);
 	imx_gpcv2_set_core1_power(false);
 	writel(0, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4);
@@ -121,3 +166,48 @@ __secure void psci_system_off(void)
 	while (1)
 		wfi();
 }
+
+__secure u32 psci_version(void)
+{
+	return ARM_PSCI_VER_1_0;
+}
+
+__secure s32 psci_cpu_suspend(u32 __always_unused function_id, u32 power_state,
+			      u32 entry_point_address,
+			      u32 context_id)
+{
+	return ARM_PSCI_RET_INVAL;
+}
+
+__secure s32 psci_affinity_info(u32 __always_unused function_id,
+				u32 target_affinity,
+				u32 lowest_affinity_level)
+{
+	u32 cpu = target_affinity & MPIDR_AFF0;
+
+	if (lowest_affinity_level > 0)
+		return ARM_PSCI_RET_INVAL;
+
+	if (target_affinity & ~MPIDR_AFF0)
+		return ARM_PSCI_RET_INVAL;
+
+	if (cpu >= IMX7D_PSCI_NR_CPUS)
+		return ARM_PSCI_RET_INVAL;
+
+	return psci_state[cpu];
+}
+
+__secure s32 psci_features(u32 __always_unused function_id, u32 psci_fid)
+{
+	switch (psci_fid) {
+	case ARM_PSCI_0_2_FN_PSCI_VERSION:
+	case ARM_PSCI_0_2_FN_CPU_OFF:
+	case ARM_PSCI_0_2_FN_CPU_ON:
+	case ARM_PSCI_0_2_FN_AFFINITY_INFO:
+	case ARM_PSCI_0_2_FN_SYSTEM_OFF:
+	case ARM_PSCI_0_2_FN_SYSTEM_RESET:
+	case ARM_PSCI_1_0_FN_PSCI_FEATURES:
+		return 0x0;
+	}
+	return ARM_PSCI_RET_NI;
+}