diff --git a/arch/arm/mach-imx/mx7/psci.S b/arch/arm/mach-imx/mx7/psci.S
index bc2cd8a..729556c 100644
--- a/arch/arm/mach-imx/mx7/psci.S
+++ b/arch/arm/mach-imx/mx7/psci.S
@@ -12,6 +12,15 @@
 #include <asm/arch-armv7/generictimer.h>
 #include <asm/psci.h>

+#define PSCI_FN_PSCI_VERSION_FEATURE_MASK	0x0
+#define PSCI_FN_CPU_SUSPEND_FEATURE_MASK	0x0
+#define PSCI_FN_CPU_OFF_FEATURE_MASK		0x0
+#define PSCI_FN_CPU_ON_FEATURE_MASK		0x0
+#define PSCI_FN_AFFINITY_INFO_FEATURE_MASK	0x0
+#define PSCI_FN_SYSTEM_OFF_FEATURE_MASK		0x0
+#define PSCI_FN_SYSTEM_RESET_FEATURE_MASK	0x0
+#define PSCI_FN_SYSTEM_SUSPEND_FEATURE_MASK	0x0
+
 	.pushsection ._secure.text, "ax"

 	.arch_extension sec
@@ -58,3 +67,74 @@ psci_system_off:
 	b 3b

 	.popsection
+
+.globl	psci_version
+psci_version:
+	movw	r0, #0
+	movt	r0, #1
+
+	bx	lr
+
+_mx7_psci_supported_table:
+	.word	ARM_PSCI_0_2_FN_PSCI_VERSION
+	.word	PSCI_FN_PSCI_VERSION_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_CPU_SUSPEND
+	.word	PSCI_FN_CPU_SUSPEND_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_CPU_OFF
+	.word	PSCI_FN_CPU_OFF_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_CPU_ON
+	.word	PSCI_FN_CPU_ON_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_AFFINITY_INFO
+	.word	PSCI_FN_AFFINITY_INFO_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_SYSTEM_OFF
+	.word	PSCI_FN_SYSTEM_OFF_FEATURE_MASK
+	.word	ARM_PSCI_0_2_FN_SYSTEM_RESET
+	.word	PSCI_FN_SYSTEM_RESET_FEATURE_MASK
+	.word	ARM_PSCI_1_0_FN_SYSTEM_SUSPEND
+	.word	PSCI_FN_SYSTEM_SUSPEND_FEATURE_MASK
+	.word	0
+	.word	ARM_PSCI_RET_NI
+
+.globl	psci_features
+psci_features:
+	adr	r2, _mx7_psci_supported_table
+1:	ldr	r3, [r2]
+	cmp	r3, #0
+	beq	out_psci_features
+	cmp	r1, r3
+	addne	r2, r2, #8
+	bne	1b
+
+out_psci_features:
+	ldr	r0, [r2, #4]
+	bx	lr
+
+@ r0: return value ARM_PSCI_RET_SUCCESS or ARM_PSCI_RET_INVAL
+@ r1: input target CPU ID in MPIDR format, original value in r1 may be dropped
+@ r4: output validated CPU ID if ARM_PSCI_RET_SUCCESS returns, meaningless for
+@ ARM_PSCI_RET_INVAL,suppose caller saves r4 before calling
+LENTRY(psci_check_target_cpu_id)
+	@ Get the real CPU number
+	and	r4, r1, #0xff
+	mov	r0, #ARM_PSCI_RET_INVAL
+
+	@ Bit[31:24], bits must be zero.
+	tst	r1, #0xff000000
+	bxne	lr
+
+	@ Affinity level 2 - Cluster: only one cluster in MX7.
+	tst	r1, #0xff0000
+	bxne	lr
+
+	@ Affinity level 1 - Processors: should be in 0xf00 format.
+	lsr	r1, r1, #8
+	teq	r1, #0xf
+	bxne	lr
+
+	@ Affinity level 0 - CPU: only 0, 1 are valid in MX7.
+	cmp	r4, #2
+	bxge	lr
+
+	mov	r0, #ARM_PSCI_RET_SUCCESS
+	bx	lr
+ENDPROC(psci_check_target_cpu_id)
--
2.7.4