From 5d702282e7992b4266ed66e6ace348acd1a6d4ef Mon Sep 17 00:00:00 2001
From: Sumit Garg <sumit.garg@nxp.com>
Date: Wed, 4 May 2016 14:05:01 -0400
Subject: [PATCH 4/4] ARMv7: plat-ls: Implement PSCI funtionality

Implement PSCI functionality over-riding generic framework under
CFG_ARMV7_PSCI macro. Current implementation supports following:

1. PSCI_VERSION
2. PSCI_CPU_ON
3. PSCI_CPU_OFF
4. PSCI_AFFINITY_INFO
5. PSCI_SYSTEM_OFF
6. PSCI_SYSTEM_RESET

Signed-off-by: Sumit Garg <sumit.garg@nxp.com>
---
 core/arch/arm/include/arm32_macros.S     |  15 +++
 core/arch/arm/kernel/generic_entry_a32.S |  11 +-
 core/arch/arm/plat-ls/conf.mk            |  11 ++
 core/arch/arm/plat-ls/platform_config.h  |   2 +
 core/arch/arm/plat-ls/psci.c             | 193 +++++++++++++++++++++++++++
 core/arch/arm/plat-ls/psci_helper.S      | 215 +++++++++++++++++++++++++++++++
 core/arch/arm/plat-ls/psci_helper.h      |  49 +++++++
 core/arch/arm/plat-ls/sub.mk             |   2 +
 8 files changed, 494 insertions(+), 4 deletions(-)
 create mode 100644 core/arch/arm/plat-ls/psci.c
 create mode 100644 core/arch/arm/plat-ls/psci_helper.S
 create mode 100644 core/arch/arm/plat-ls/psci_helper.h

diff --git a/core/arch/arm/include/arm32_macros.S b/core/arch/arm/include/arm32_macros.S
index 20a5991..ba6a270 100644
--- a/core/arch/arm/include/arm32_macros.S
+++ b/core/arch/arm/include/arm32_macros.S
@@ -119,3 +119,18 @@
 	mrc	p15, 0, \reg, c1, c1, 2
 	.endm
 
+	.macro timer_wait  reg, ticks
+	movw	\reg, #(\ticks & 0xffff)
+	movt	\reg, #(\ticks >> 16)
+	mcr	p15, 0, \reg, c14, c2, 0
+	isb
+	mov	\reg, #3
+	mcr	p15, 0, \reg, c14, c2, 1
+1 :	isb
+	mrc	p15, 0, \reg, c14, c2, 1
+	ands	\reg, \reg, #4
+	bne	1b
+	mov	\reg, #0
+	mcr	p15, 0, \reg, c14, c2, 1
+	isb
+	.endm
diff --git a/core/arch/arm/kernel/generic_entry_a32.S b/core/arch/arm/kernel/generic_entry_a32.S
index 98e9bf8..7f91fff 100644
--- a/core/arch/arm/kernel/generic_entry_a32.S
+++ b/core/arch/arm/kernel/generic_entry_a32.S
@@ -186,7 +186,7 @@ UNWIND(	.cantunwind)
 	ldr	r0, =_start
 	write_vbar r0
 
-#if defined(CFG_WITH_ARM_TRUSTED_FW)
+#if defined(CFG_WITH_ARM_TRUSTED_FW) || defined(CFG_ARMV7_PSCI)
 	b	reset_primary
 #else
 	bl	get_core_pos
@@ -364,7 +364,7 @@ UNWIND(	.fnstart)
 UNWIND(	.fnend)
 END_FUNC unhandled_cpu
 
-#if defined(CFG_WITH_ARM_TRUSTED_FW)
+#if defined(CFG_WITH_ARM_TRUSTED_FW) || defined(CFG_ARMV7_PSCI)
 FUNC cpu_on_handler , :
 UNWIND(	.fnstart)
 UNWIND(	.cantunwind)
@@ -394,13 +394,16 @@ UNWIND(	.cantunwind)
 
 	mov	r0, r4
 	mov	r1, r5
+#if defined(CFG_ARMV7_PSCI)
+	bl      generic_boot_init_secondary
+#else
 	bl	generic_boot_cpu_on_handler
-
+#endif
 	bx	r6
 UNWIND(	.fnend)
 END_FUNC cpu_on_handler
 
-#else /* defined(CFG_WITH_ARM_TRUSTED_FW) */
+#else /* defined(CFG_WITH_ARM_TRUSTED_FW) || defined(CFG_ARMV7_PSCI) */
 
 LOCAL_FUNC reset_secondary , :
 UNWIND(	.fnstart)
diff --git a/core/arch/arm/plat-ls/conf.mk b/core/arch/arm/plat-ls/conf.mk
index 8cacdce..60223cc 100644
--- a/core/arch/arm/plat-ls/conf.mk
+++ b/core/arch/arm/plat-ls/conf.mk
@@ -11,7 +11,18 @@ $(call force,CFG_ARM32_core,y)
 $(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
 $(call force,CFG_16550_UART,y)
 $(call force,CFG_PM_STUBS,y)
+
+ifeq ($(CFG_ARM32_core),y)
+$(call force,CFG_ARMV7_PSCI,y)
+else
+$(call force,CFG_ARMV7_PSCI,n)
+endif
+
+ifeq ($(CFG_ARMV7_PSCI),y)
+$(call force,CFG_BOOT_SYNC_CPU,n)
+else
 $(call force,CFG_BOOT_SYNC_CPU,y)
+endif
 
 ta-targets = ta_arm32
 
diff --git a/core/arch/arm/plat-ls/platform_config.h b/core/arch/arm/plat-ls/platform_config.h
index 6e0f0e6..8109011 100644
--- a/core/arch/arm/plat-ls/platform_config.h
+++ b/core/arch/arm/plat-ls/platform_config.h
@@ -48,6 +48,8 @@
 /*  LPUART 2 */
 #define UART3_BASE			0x12960000
 
+/*  TIMER CLOCK FREQ */
+#define CFG_TIMER_CLK_FREQ		12500000
 
 /* console uart define */
 #define CONSOLE_UART_BASE		UART0_BASE
diff --git a/core/arch/arm/plat-ls/psci.c b/core/arch/arm/plat-ls/psci.c
new file mode 100644
index 0000000..5c9eb1e
--- /dev/null
+++ b/core/arch/arm/plat-ls/psci.c
@@ -0,0 +1,193 @@
+/*
+ * Copyright (C) 2016 Freescale Semiconductor, Inc.
+ * 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.
+ */
+
+#include <platform_config.h>
+
+#include <trace.h>
+#include <mm/core_mmu.h>
+#include <psci_helper.h>
+#include <sm/psci.h>
+#include <io.h>
+#include <sm/sm.h>
+#include <arm.h>
+
+#define CONFIG_SYS_FSL_RCPM_ADDR		(CCSR_IOMEM_VA_BASE + 0x00ee2000)
+#define CONFIG_SYS_FSL_GUTS_ADDR		(CCSR_IOMEM_VA_BASE + 0x00ee0000)
+#define CONFIG_SYS_FSL_SCFG_ADDR		(CCSR_IOMEM_VA_BASE + 0x00570000)
+#define RCPM_TWAITSR_OFFSET			0x04C
+#define DCFG_CCSR_RSTCR_OFFSET			0x0B0
+#define DCFG_CCSR_BRR_OFFSET			0x0E4
+#define DCFG_CCSR_SCRATCHRW1_OFFSET		0x200
+#define SCFG_COREx_SFT_RST_OFFSET		0x130
+#define SCFG_CORESRENCR_OFFSET			0x204
+#define DCFG_CCSR_RSTCR_RESET_REQ		0x2
+#define MAX_PSCI_FEATURES			16
+#define PSCI_FEATURE_MASK			0
+
+#define BYTE_SWAP_32(word)			((((word) & 0xff000000) >> 24) | \
+						(((word) & 0x00ff0000) >>  8) | \
+						(((word) & 0x0000ff00) <<  8) | \
+						(((word) & 0x000000ff) << 24))
+
+uint32_t psci_supported_table[MAX_PSCI_FEATURES][2] = {
+					  { SMC_PSCI_FN_PSCI_VERSION, PSCI_FEATURE_MASK },
+					  { SMC_PSCI_FN_CPU_OFF, PSCI_FEATURE_MASK },
+					  { SMC_PSCI_FN_CPU_ON, PSCI_FEATURE_MASK },
+					  { SMC_PSCI_FN_AFFINITY_INFO, PSCI_FEATURE_MASK },
+					  { SMC_PSCI_FN_SYSTEM_OFF, PSCI_FEATURE_MASK },
+					  { SMC_PSCI_FN_SYSTEM_RESET, PSCI_FEATURE_MASK },
+					  { 0, SMC_PSCI_RET_NOT_SUPPORTED },
+					};
+
+struct secondary_ctx sec_ctx = {0};
+
+void smc_psci_cpu_on(struct thread_smc_args *args)
+{
+	uint32_t cpu_id, dcfg_brr, psci_cpu_on_entry, coresr_en, core_rst;
+	cpu_id = args->a1 & 0xff;
+	sec_ctx.psci_ker_entry = args->a2;
+	sec_ctx.context_id = args->a3;
+
+	/* Clean and invalidate cache to save data to external memory */
+	cache_maintenance_l1(DCACHE_AREA_CLEAN_INV, (void *)&sec_ctx,
+			     sizeof(struct secondary_ctx));
+
+	dcfg_brr = read32(CONFIG_SYS_FSL_GUTS_ADDR + DCFG_CCSR_BRR_OFFSET);
+	dcfg_brr = BYTE_SWAP_32(dcfg_brr);
+
+	if (dcfg_brr >> cpu_id) {
+		/* Reset target CPU as it has been released from boot hold-off */
+
+		/* Enable core soft reset */
+		coresr_en = BYTE_SWAP_32(0x80000000);
+		write32(coresr_en, CONFIG_SYS_FSL_SCFG_ADDR + SCFG_CORESRENCR_OFFSET);
+
+		/* Do reset on target CPU */
+		core_rst = BYTE_SWAP_32(0x80000000);
+		write32(core_rst,
+			(CONFIG_SYS_FSL_SCFG_ADDR + (cpu_id << 2) + SCFG_COREx_SFT_RST_OFFSET));
+
+		/* Wait for core to reset */
+		reset_wait();
+
+		/* Disable CORE soft reset */
+		coresr_en = BYTE_SWAP_32(0x0);
+		write32(coresr_en, CONFIG_SYS_FSL_SCFG_ADDR + SCFG_CORESRENCR_OFFSET);
+	}
+
+	/* Release target CPU from boot hold-off */
+	dcfg_brr = read32(CONFIG_SYS_FSL_GUTS_ADDR + DCFG_CCSR_BRR_OFFSET);
+	dcfg_brr = BYTE_SWAP_32(dcfg_brr);
+	dcfg_brr |= 0x1 << cpu_id;
+	dcfg_brr = BYTE_SWAP_32(dcfg_brr);
+	write32(dcfg_brr, CONFIG_SYS_FSL_GUTS_ADDR + DCFG_CCSR_BRR_OFFSET);
+
+	/* Set secondary CPU entry in SCRATCH1 register */
+	psci_cpu_on_entry = (uint32_t) psci_entry_secondary;
+	psci_cpu_on_entry = BYTE_SWAP_32(psci_cpu_on_entry);
+	write32(psci_cpu_on_entry, CONFIG_SYS_FSL_GUTS_ADDR + DCFG_CCSR_SCRATCHRW1_OFFSET);
+}
+
+void __noreturn smc_psci_cpu_off(void)
+{
+	/* Disable cache and smp */
+	disable_cache_smp();
+
+	while (1)
+		asm volatile("wfi");
+}
+
+void smc_psci_affinity_info(struct thread_smc_args *args)
+{
+	uint32_t cpu_id, rcpm_twaitsr;
+	if (args->a2 == 0) {
+		cpu_id = args->a1 & 0xff;
+		rcpm_twaitsr = read32(CONFIG_SYS_FSL_RCPM_ADDR + RCPM_TWAITSR_OFFSET);
+		rcpm_twaitsr = BYTE_SWAP_32(rcpm_twaitsr);
+		if (rcpm_twaitsr & (0x1 << cpu_id)) {
+			args->a0 = SMC_PSCI_AFFINITY_LEVEL_OFF;
+		}
+		else {
+			args->a0 = SMC_PSCI_AFFINITY_LEVEL_ON;
+		}
+	}
+	else
+		args->a0 = SMC_PSCI_RET_INVALID_PARAMS;
+}
+
+void __noreturn smc_psci_system_off(void)
+{
+	while (1)
+		asm volatile("wfi");
+}
+
+void __noreturn smc_psci_system_reset(void)
+{
+	uint32_t reset_req = BYTE_SWAP_32(DCFG_CCSR_RSTCR_RESET_REQ);
+	write32(reset_req, CONFIG_SYS_FSL_GUTS_ADDR + DCFG_CCSR_RSTCR_OFFSET);
+
+	while (1)
+		asm volatile("wfi");
+}
+
+void smc_psci_feature_query(struct thread_smc_args *args)
+{
+	uint8_t idx;
+	for (idx = 0; idx < MAX_PSCI_FEATURES; idx++) {
+		if (psci_supported_table[idx][0] == args->a1) {
+			args->a0 = psci_supported_table[idx][1];
+			break;
+		}
+		else if (psci_supported_table[idx][0] == 0) {
+			args->a0 = psci_supported_table[idx][1];
+			break;
+		}
+	}
+}
+
+/* Initialize secondary core non-secure entry */
+void cpu_init_sec_mon(void)
+{
+	struct sm_nsec_ctx *nsec_ctx;
+	uint32_t ctx_id;
+
+	/* Invalidate cache to fetch data from external memory */
+	cache_maintenance_l1(DCACHE_AREA_INVALIDATE, &sec_ctx,
+			     sizeof(struct secondary_ctx));
+
+	/* Initialize secure monitor */
+	nsec_ctx = sm_get_nsec_ctx();
+	nsec_ctx->mon_lr = sec_ctx.psci_ker_entry;
+	ctx_id = sec_ctx.context_id;
+
+#define CPSR_MODE_HYP	0x1a
+	nsec_ctx->mon_spsr = CPSR_MODE_HYP | CPSR_F | CPSR_I | CPSR_A;
+	if(nsec_ctx->mon_lr & 0x1)
+		nsec_ctx->mon_spsr |= CPSR_T;
+
+	asm volatile("mov r1, %[ctx_id]" : : [ctx_id] "r" (ctx_id));
+}
diff --git a/core/arch/arm/plat-ls/psci_helper.S b/core/arch/arm/plat-ls/psci_helper.S
new file mode 100644
index 0000000..da282d3
--- /dev/null
+++ b/core/arch/arm/plat-ls/psci_helper.S
@@ -0,0 +1,215 @@
+/*
+ * Copyright (c) 2016, Freescale Semiconductor, Inc.
+ * 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.
+ */
+
+#include <asm.S>
+#include <arm.h>
+#include <arm32_macros.S>
+#include <platform_config.h>
+#include <sm/optee_smc.h>
+#include <sm/teesmc_opteed_macros.h>
+#include <sm/teesmc_opteed.h>
+
+#define CPUID_ARM_GENTIMER_SHIFT	16
+#define CPUID_ARM_GENTIMER_MASK		(0xF << CPUID_ARM_GENTIMER_SHIFT)
+
+#define	ONE_MS		(CFG_TIMER_CLK_FREQ / 1000)
+#define	RESET_WAIT	(30 * ONE_MS)
+
+FUNC reset_wait , :
+	timer_wait	r0, RESET_WAIT
+	bx	lr
+END_FUNC reset_wait
+
+/* Imported from Linux kernel */
+LOCAL_FUNC v7_flush_dcache_all , :
+	dmb					@ ensure ordering with previous memory accesses
+	mrc	p15, 1, r0, c0, c0, 1		@ read clidr
+	ands	r3, r0, #0x7000000		@ extract loc from clidr
+	mov	r3, r3, lsr #23			@ left align loc bit field
+	beq	finished			@ if loc is 0, then no need to clean
+	mov	r10, #0				@ start clean at cache level 0
+flush_levels:
+	add	r2, r10, r10, lsr #1		@ work out 3x current cache level
+	mov	r1, r0, lsr r2			@ extract cache type bits from clidr
+	and	r1, r1, #7			@ mask of the bits for current cache only
+	cmp	r1, #2				@ see what cache we have at this level
+	blt	skip				@ skip if no cache, or just i-cache
+	mrs	r9, cpsr			@ make cssr&csidr read atomic
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	isb					@ isb to sych the new cssr&csidr
+	mrc	p15, 1, r1, c0, c0, 0		@ read the new csidr
+	msr	cpsr_c, r9
+	and	r2, r1, #7			@ extract the length of the cache lines
+	add	r2, r2, #4			@ add 4 (line length offset)
+	ldr	r4, =0x3ff
+	ands	r4, r4, r1, lsr #3		@ find maximum number on the way size
+	clz	r5, r4				@ find bit position of way size increment
+	ldr	r7, =0x7fff
+	ands	r7, r7, r1, lsr #13		@ extract max number of the index size
+loop1:
+	mov	r9, r7				@ create working copy of max index
+loop2:
+	orr	r11, r10, r4, lsl r5		@ factor way and cache number into r11
+	orr	r11, r11, r9, lsl r2		@ factor index number into r11
+	mcr	p15, 0, r11, c7, c14, 2		@ clean & invalidate by set/way
+	subs	r9, r9, #1			@ decrement the index
+	bge	loop2
+	subs	r4, r4, #1			@ decrement the way
+	bge	loop1
+skip:
+	add	r10, r10, #2			@ increment cache number
+	cmp	r3, r10
+	bgt	flush_levels
+finished:
+	mov	r10, #0				@ swith back to cache level 0
+	mcr	p15, 2, r10, c0, c0, 0		@ select current cache level in cssr
+	dsb	st
+	isb
+	bx	lr
+END_FUNC v7_flush_dcache_all
+
+FUNC disable_cache_smp , :
+	push	{lr}
+
+	mrc	p15, 0, r0, c1, c0, 0		@ SCTLR
+	bic	r0, r0, #(1 << 2)		@ Clear C bit
+	mcr	p15, 0, r0, c1, c0, 0		@ SCTLR
+	isb
+	dsb
+
+	bl	v7_flush_dcache_all
+
+	clrex					@ Clear exclusive monitor
+
+	mrc	p15, 0, r0, c1, c0, 1		@ ACTLR
+	bic	r0, r0, #(1 << 6)		@ Clear SMP bit
+	mcr	p15, 0, r0, c1, c0, 1		@ ACTLR
+	isb
+	dsb
+
+	pop	{pc}
+END_FUNC disable_cache_smp
+
+LOCAL_FUNC nonsec_init , :
+	push	{lr}
+
+	mrc	p15, 4, r2, c15, c0, 0		@ read CBAR
+	bfc	r2, #0, #15			@ clear reserved bits
+
+	add	r0, r2, #GICC_OFFSET		@ GIC ctrl i/f offset
+	add	r1, r2, #GICD_OFFSET		@ GIC dist i/f offset
+
+	mvn	r3, #0
+	str	r3, [r1, #0x80]
+
+	mov	r3, #3				@ Enable both groups
+	str	r3, [r0, #0x0]			@ and clear all other bits
+	mov	r3, #0xff
+	str     r3, [r0, #0x4]
+
+	mrc	p15, 0, r0, c1, c1, 2
+	movw	r1, #0x3fff
+	movt	r1, #0x0004
+	orr	r0, r0, r1
+	mcr	p15, 0, r0, c1, c1, 2		@ NSACR = all copros to non-sec
+
+/* The CNTFRQ register of the generic timer needs to be
+ * programmed in secure state. Some primary bootloaders / firmware
+ * omit this, so if the frequency is provided in the configuration,
+ * we do this here instead.
+ * But first check if we have the generic timer.
+ */
+#ifdef CFG_TIMER_CLK_FREQ
+	mrc	p15, 0, r0, c0, c1, 1			@ read ID_PFR1
+	and	r0, r0, #CPUID_ARM_GENTIMER_MASK	@ mask arch timer bits
+	cmp	r0, #(1 << CPUID_ARM_GENTIMER_SHIFT)
+	ldreq	r1, =CFG_TIMER_CLK_FREQ
+	mcreq	p15, 0, r1, c14, c0, 0			@ write CNTFRQ
+#endif
+	pop	{lr}
+	bx	lr
+END_FUNC nonsec_init
+
+/*
+ * Initialize cntvoff to zero only once during boot time in
+ * secure monitor mode before passing control to secure OS.
+ * Also NS bit should be set while initializing it.
+ * Required for proper linux smp booting.
+ */
+LOCAL_FUNC cntvoff_init , :
+	mrs	r1, cpsr
+	cps	#CPSR_MODE_MON
+
+	mrc	p15, 0, r0, c1, c1, 0			@ read SCR
+	orr	r0, r0, #1				@ Set NS in SCR
+	mcr	p15, 0, r0, c1, c1, 0			@ write SCR (with NS bit set)
+
+	@ Reset CNTVOFF to 0 before leaving monitor mode
+	mrc	p15, 0, r2, c0, c1, 1			@ read ID_PFR1
+	ands	r2, r2, #CPUID_ARM_GENTIMER_MASK	@ test arch timer bits
+	movne	r2,#0x0
+	mcrrne	p15, 4, r2, r2, c14			@ Reset CNTVOFF to zero
+
+	bic	r0, r0, #1				@ Clear NS in SCR
+	mcr	p15, 0, r0, c1, c1, 0			@ write SCR (with NS bit reset)
+
+	msr	cpsr, r1
+	bx	lr
+END_FUNC cntvoff_init
+
+LOCAL_FUNC psci_kernel_entry , :
+	@ Set secondary cpu kernel entry and spsr
+	bl	cpu_init_sec_mon
+
+	mov	r0, #TEESMC_OPTEED_FUNCID_RETURN_ENTRY_DONE
+	mov	r2, #0
+	mov	r3, #0
+	smc	#0
+	b	.	/* SMC should not return */
+END_FUNC psci_kernel_entry
+
+FUNC psci_entry_secondary , :
+	mrc	p15, 0, r0, c1, c0, 1		@ ACTLR
+	orr	r0, r0, #(1 << 6)		@ Set SMP bit
+	mcr	p15, 0, r0, c1, c0, 1		@ ACTLR
+	isb
+
+	bl	get_core_pos
+	lsl	r0, #2
+
+	@ Initialize sp to stack_tmp_top
+	ldr	r1, =stack_tmp_top
+	ldr	sp, [r1, r0]
+
+	bl	nonsec_init
+
+	bl	cntvoff_init			@ Initialize CNTVOFF to zero
+
+	bl	cpu_on_handler
+
+	bl	psci_kernel_entry
+END_FUNC psci_entry_secondary
diff --git a/core/arch/arm/plat-ls/psci_helper.h b/core/arch/arm/plat-ls/psci_helper.h
new file mode 100644
index 0000000..cc2bb88
--- /dev/null
+++ b/core/arch/arm/plat-ls/psci_helper.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2016, Freescale Semiconductor, Inc.
+ * 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 PSCI_HELPER_H
+#define PSCI_HELPER_H
+
+/*
+ * Original from Linux kernel: include/uapi/linux/psci.h
+ */
+/* PSCI v0.2 affinity level state returned by AFFINITY_INFO */
+#define SMC_PSCI_AFFINITY_LEVEL_ON		0
+#define SMC_PSCI_AFFINITY_LEVEL_OFF		1
+#define SMC_PSCI_AFFINITY_LEVEL_ON_PENDING	2
+
+void cpu_init_sec_mon(void);
+void psci_entry_secondary(void);
+void reset_wait(void);
+void disable_cache_smp(void);
+
+struct secondary_ctx {
+	uint32_t psci_ker_entry;
+	uint32_t context_id;
+};
+
+#endif /*PSCI_HELPER_H*/
diff --git a/core/arch/arm/plat-ls/sub.mk b/core/arch/arm/plat-ls/sub.mk
index 82f2bf7..13343ec 100644
--- a/core/arch/arm/plat-ls/sub.mk
+++ b/core/arch/arm/plat-ls/sub.mk
@@ -1,3 +1,5 @@
 global-incdirs-y += .
 srcs-y += main.c
 srcs-y += ls_core_pos.S
+srcs-$(CFG_ARMV7_PSCI) += psci_helper.S
+srcs-$(CFG_ARMV7_PSCI) += psci.c
-- 
1.8.1.4

