21 files changed, 363 insertions, 259 deletions

diff --git a/arch/arm/cpu/armv7/Kconfig b/arch/arm/cpu/armv7/Kconfig
index afeaac8..bd6108e 100644
--- a/arch/arm/cpu/armv7/Kconfig
+++ b/arch/arm/cpu/armv7/Kconfig
@@ -21,7 +21,7 @@ config ARMV7_BOOT_SEC_DEFAULT
 	Say Y here to boot in secure mode by default even if non-secure mode
 	is supported. This option is useful to boot kernels which do not
 	suppport booting in non-secure mode. Only set this if you need it.
-	This can be overriden at run-time by setting the bootm_boot_mode env.
+	This can be overridden at run-time by setting the bootm_boot_mode env.
 	variable to "sec" or "nonsec".
 
 config ARMV7_VIRT
diff --git a/arch/arm/cpu/armv7/Makefile b/arch/arm/cpu/armv7/Makefile
index ddd8d12..0d4bfbc 100644
--- a/arch/arm/cpu/armv7/Makefile
+++ b/arch/arm/cpu/armv7/Makefile
@@ -19,7 +19,7 @@ endif
 endif
 
 obj-$(CONFIG_ARMV7_NONSEC)	+= nonsec_virt.o virt-v7.o virt-dt.o
-obj-$(CONFIG_ARMV7_PSCI)	+= psci.o
+obj-$(CONFIG_ARMV7_PSCI)	+= psci.o psci-common.o
 
 obj-$(CONFIG_IPROC) += iproc-common/
 obj-$(CONFIG_KONA) += kona-common/
diff --git a/arch/arm/cpu/armv7/am33xx/config.mk b/arch/arm/cpu/armv7/am33xx/config.mk
index 6d95d32..ab94708 100644
--- a/arch/arm/cpu/armv7/am33xx/config.mk
+++ b/arch/arm/cpu/armv7/am33xx/config.mk
@@ -26,6 +26,7 @@ endif
 else
 ifeq ($(CONFIG_TI_SECURE_DEVICE),y)
 ALL-$(CONFIG_QSPI_BOOT) += u-boot_HS_XIP_X-LOADER
+ALL-y	+= u-boot_HS.img
 endif
 ALL-y	+= u-boot.img
 endif
diff --git a/arch/arm/cpu/armv7/cache_v7.c b/arch/arm/cpu/armv7/cache_v7.c
index dc309da..52f1856 100644
--- a/arch/arm/cpu/armv7/cache_v7.c
+++ b/arch/arm/cpu/armv7/cache_v7.c
@@ -19,23 +19,6 @@
 void v7_flush_dcache_all(void);
 void v7_invalidate_dcache_all(void);
 
-static int check_cache_range(unsigned long start, unsigned long stop)
-{
-	int ok = 1;
-
-	if (start & (CONFIG_SYS_CACHELINE_SIZE - 1))
-		ok = 0;
-
-	if (stop & (CONFIG_SYS_CACHELINE_SIZE - 1))
-		ok = 0;
-
-	if (!ok)
-		debug("CACHE: Misaligned operation at range [%08lx, %08lx]\n",
-			start, stop);
-
-	return ok;
-}
-
 static u32 get_ccsidr(void)
 {
 	u32 ccsidr;
@@ -61,27 +44,8 @@ static void v7_dcache_inval_range(u32 start, u32 stop, u32 line_len)
 {
 	u32 mva;
 
-	/*
-	 * If start address is not aligned to cache-line do not
-	 * invalidate the first cache-line
-	 */
-	if (start & (line_len - 1)) {
-		printf("ERROR: %s - start address is not aligned - 0x%08x\n",
-			__func__, start);
-		/* move to next cache line */
-		start = (start + line_len - 1) & ~(line_len - 1);
-	}
-
-	/*
-	 * If stop address is not aligned to cache-line do not
-	 * invalidate the last cache-line
-	 */
-	if (stop & (line_len - 1)) {
-		printf("ERROR: %s - stop address is not aligned - 0x%08x\n",
-			__func__, stop);
-		/* align to the beginning of this cache line */
-		stop &= ~(line_len - 1);
-	}
+	if (!check_cache_range(start, stop))
+		return;
 
 	for (mva = start; mva < stop; mva = mva + line_len) {
 		/* DCIMVAC - Invalidate data cache by MVA to PoC */
@@ -195,6 +159,14 @@ void flush_dcache_all(void)
 {
 }
 
+void invalidate_dcache_range(unsigned long start, unsigned long stop)
+{
+}
+
+void flush_dcache_range(unsigned long start, unsigned long stop)
+{
+}
+
 void arm_init_before_mmu(void)
 {
 }
diff --git a/arch/arm/cpu/armv7/ls102xa/psci.S b/arch/arm/cpu/armv7/ls102xa/psci.S
index cf5cd48..f9b26b4 100644
--- a/arch/arm/cpu/armv7/ls102xa/psci.S
+++ b/arch/arm/cpu/armv7/ls102xa/psci.S
@@ -29,16 +29,16 @@
 	@ r2 = target PC
 .globl	psci_cpu_on
 psci_cpu_on:
-	push	{lr}
+	push	{r4, r5, r6, lr}
 
 	@ Clear and Get the correct CPU number
 	@ r1 = 0xf01
-	and	r1, r1, #0xff
+	and	r4, r1, #0xff
 
-	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	str	r2, [r0]
-	dsb
+	mov	r0, r4
+	mov	r1, r2
+	bl	psci_save_target_pc
+	mov	r1, r4
 
 	@ Get DCFG base address
 	movw	r4, #(CONFIG_SYS_FSL_GUTS_ADDR & 0xffff)
@@ -101,7 +101,7 @@ holdoff_release:
 	@ Return
 	mov	r0, #ARM_PSCI_RET_SUCCESS
 
-	pop	{lr}
+	pop	{r4, r5, r6, lr}
 	bx	lr
 
 .globl	psci_cpu_off
@@ -111,16 +111,4 @@ psci_cpu_off:
 1:	wfi
 	b	1b
 
-.globl	psci_arch_init
-psci_arch_init:
-	mov	r6, lr
-
-	bl	psci_get_cpu_id
-	bl	psci_get_cpu_stack_top
-	mov	sp, r0
-
-	bx	r6
-
-	.globl psci_text_end
-psci_text_end:
 	.popsection
diff --git a/arch/arm/cpu/armv7/mx7/psci-mx7.c b/arch/arm/cpu/armv7/mx7/psci-mx7.c
index 9a33047..502552d 100644
--- a/arch/arm/cpu/armv7/mx7/psci-mx7.c
+++ b/arch/arm/cpu/armv7/mx7/psci-mx7.c
@@ -1,9 +1,9 @@
 #include <asm/io.h>
 #include <asm/psci.h>
+#include <asm/secure.h>
 #include <asm/arch/imx-regs.h>
 #include <common.h>
 
-#define __secure __attribute__((section("._secure.text")))
 
 #define GPC_CPU_PGC_SW_PDN_REQ	0xfc
 #define GPC_CPU_PGC_SW_PUP_REQ	0xf0
diff --git a/arch/arm/cpu/armv7/mx7/psci.S b/arch/arm/cpu/armv7/mx7/psci.S
index 34c6ab3..96e88d6 100644
--- a/arch/arm/cpu/armv7/mx7/psci.S
+++ b/arch/arm/cpu/armv7/mx7/psci.S
@@ -9,35 +9,22 @@
 
 	.arch_extension sec
 
-	@ r1 = target CPU
-	@ r2 = target PC
-
-.globl	psci_arch_init
-psci_arch_init:
-	mov	r6, lr
-
-	bl	psci_get_cpu_id
-	bl	psci_get_cpu_stack_top
-	mov	sp, r0
-
-	bx	r6
-
-	@ r1 = target CPU
-	@ r2 = target PC
-
 .globl psci_cpu_on
 psci_cpu_on:
-	push	{lr}
+	push	{r4, r5, lr}
 
+	mov	r4, r0
+	mov	r5, r1
 	mov	r0, r1
-	bl	psci_get_cpu_stack_top
-	str	r2, [r0]
-	dsb
+	mov	r1, r2
+	bl	psci_save_target_pc
 
+	mov	r0, r4
+	mov	r1, r5
 	ldr	r2, =psci_cpu_entry
 	bl	imx_cpu_on
 
-	pop	{pc}
+	pop	{r4, r5, pc}
 
 .globl psci_cpu_off
 psci_cpu_off:
@@ -49,6 +36,4 @@ psci_cpu_off:
 1: 	wfi
 	b 1b
 
-	.globl psci_text_end
-psci_text_end:
 	.popsection
diff --git a/arch/arm/cpu/armv7/nonsec_virt.S b/arch/arm/cpu/armv7/nonsec_virt.S
index b7563ed..95ce938 100644
--- a/arch/arm/cpu/armv7/nonsec_virt.S
+++ b/arch/arm/cpu/armv7/nonsec_virt.S
@@ -49,8 +49,13 @@ _secure_monitor:
 	mcr	p15, 0, r5, c12, c0, 1
 	isb
 
-	@ Obtain a secure stack, and configure the PSCI backend
+	@ Obtain a secure stack
+	bl	psci_stack_setup
+
+	@ Configure the PSCI backend
+	push	{r0, r1, r2, ip}
 	bl	psci_arch_init
+	pop	{r0, r1, r2, ip}
 #endif
 
 #ifdef CONFIG_ARM_ERRATA_773022
diff --git a/arch/arm/cpu/armv7/omap-common/Makefile b/arch/arm/cpu/armv7/omap-common/Makefile
index 87a7ac0..3172bae 100644
--- a/arch/arm/cpu/armv7/omap-common/Makefile
+++ b/arch/arm/cpu/armv7/omap-common/Makefile
@@ -36,3 +36,5 @@ obj-y	+= boot-common.o
 obj-y	+= lowlevel_init.o
 
 obj-y	+= mem-common.o
+
+obj-$(CONFIG_TI_SECURE_DEVICE) += sec-common.o
diff --git a/arch/arm/cpu/armv7/omap-common/config_secure.mk b/arch/arm/cpu/armv7/omap-common/config_secure.mk
index c7bb101..1122439 100644
--- a/arch/arm/cpu/armv7/omap-common/config_secure.mk
+++ b/arch/arm/cpu/armv7/omap-common/config_secure.mk
@@ -12,8 +12,8 @@ cmd_mkomapsecimg = $(TI_SECURE_DEV_PKG)/scripts/create-boot-image.sh \
 	$(if $(KBUILD_VERBOSE:1=), >/dev/null)
 else
 cmd_mkomapsecimg = $(TI_SECURE_DEV_PKG)/scripts/create-boot-image.sh \
-    $(patsubst u-boot_HS_%,%,$(@F)) $< $@ $(CONFIG_ISW_ENTRY_ADDR) \
-    $(if $(KBUILD_VERBOSE:1=), >/dev/null)
+	$(patsubst u-boot_HS_%,%,$(@F)) $< $@ $(CONFIG_ISW_ENTRY_ADDR) \
+	$(if $(KBUILD_VERBOSE:1=), >/dev/null)
 endif
 else
 cmd_mkomapsecimg = echo "WARNING:" \
@@ -25,14 +25,33 @@ cmd_mkomapsecimg = echo "WARNING: TI_SECURE_DEV_PKG environment" \
 	"variable must be defined for TI secure devices. $@ was NOT created!"
 endif
 
+ifdef CONFIG_SPL_LOAD_FIT
+quiet_cmd_omapsecureimg = SECURE  $@
+ifneq ($(TI_SECURE_DEV_PKG),)
+ifneq ($(wildcard $(TI_SECURE_DEV_PKG)/scripts/secure-binary-image.sh),)
+cmd_omapsecureimg = $(TI_SECURE_DEV_PKG)/scripts/secure-binary-image.sh \
+	$< $@ \
+	$(if $(KBUILD_VERBOSE:1=), >/dev/null)
+else
+cmd_omapsecureimg = echo "WARNING:" \
+	"$(TI_SECURE_DEV_PKG)/scripts/secure-binary-image.sh not found." \
+	"$@ was NOT created!"; cp $< $@
+endif
+else
+cmd_omapsecureimg = echo "WARNING: TI_SECURE_DEV_PKG environment" \
+	"variable must be defined for TI secure devices." \
+	"$@ was NOT created!"; cp $< $@
+endif
+endif
+
+
 # Standard X-LOADER target (QPSI, NOR flash)
 u-boot-spl_HS_X-LOADER: $(obj)/u-boot-spl.bin
 	$(call if_changed,mkomapsecimg)
 
-# For MLO targets (SD card boot) the final file name
-# that is copied to the SD card fAT partition must
-# be MLO, so we make a copy of the output file to a
-# new file with that name
+# For MLO targets (SD card boot) the final file name that is copied to the SD
+# card FAT partition must be MLO, so we make a copy of the output file to a new
+# file with that name
 u-boot-spl_HS_MLO: $(obj)/u-boot-spl.bin
 	$(call if_changed,mkomapsecimg)
 	@if [ -f $@ ]; then \
@@ -51,16 +70,44 @@ u-boot-spl_HS_ULO: $(obj)/u-boot-spl.bin
 u-boot-spl_HS_ISSW: $(obj)/u-boot-spl.bin
 	$(call if_changed,mkomapsecimg)
 
-# For SPI flash on AM335x and AM43xx, these
-# require special byte swap handling so we use
-# the SPI_X-LOADER target instead of X-LOADER
-# and let the create-boot-image.sh script handle
-# that
+# For SPI flash on AM335x and AM43xx, these require special byte swap handling
+# so we use the SPI_X-LOADER target instead of X-LOADER and let the
+# create-boot-image.sh script handle that
 u-boot-spl_HS_SPI_X-LOADER: $(obj)/u-boot-spl.bin
 	$(call if_changed,mkomapsecimg)
 
-# For supporting single stage XiP QSPI on AM43xx, the
-# image is a full u-boot file, not an SPL. In this case
-# the mkomapsecimg command looks for a u-boot-HS_* prefix
+# For supporting single stage XiP QSPI on AM43xx, the image is a full u-boot
+# file, not an SPL. In this case the mkomapsecimg command looks for a
+# u-boot-HS_* prefix
 u-boot_HS_XIP_X-LOADER: $(obj)/u-boot.bin
 	$(call if_changed,mkomapsecimg)
+
+# For supporting the SPL loading and interpreting of FIT images whose
+# components are pre-processed before being integrated into the FIT image in
+# order to secure them in some way
+ifdef CONFIG_SPL_LOAD_FIT
+
+MKIMAGEFLAGS_u-boot_HS.img = -f auto -A $(ARCH) -T firmware -C none -O u-boot \
+	-a $(CONFIG_SYS_TEXT_BASE) -e $(CONFIG_SYS_UBOOT_START) \
+	-n "U-Boot $(UBOOTRELEASE) for $(BOARD) board" -E \
+	$(patsubst %,-b arch/$(ARCH)/dts/%.dtb,$(subst ",,$(CONFIG_OF_LIST)))
+
+OF_LIST_TARGETS = $(patsubst %,arch/$(ARCH)/dts/%.dtb,$(subst ",,$(CONFIG_OF_LIST)))
+$(OF_LIST_TARGETS): dtbs
+
+%_HS.dtb: %.dtb
+	$(call if_changed,omapsecureimg)
+	$(Q)if [ -f $@ ]; then \
+		cp -f $@ $<; \
+	fi
+
+u-boot-nodtb_HS.bin: u-boot-nodtb.bin
+	$(call if_changed,omapsecureimg)
+
+u-boot_HS.img: u-boot-nodtb_HS.bin u-boot.img $(patsubst %.dtb,%_HS.dtb,$(OF_LIST_TARGETS))
+	$(call if_changed,mkimage)
+	$(Q)if [ -f $@ ]; then \
+		cp -f $@ u-boot.img; \
+	fi
+
+endif
diff --git a/arch/arm/cpu/armv7/omap-common/emif-common.c b/arch/arm/cpu/armv7/omap-common/emif-common.c
index 9a9c764..2b79010 100644
--- a/arch/arm/cpu/armv7/omap-common/emif-common.c
+++ b/arch/arm/cpu/armv7/omap-common/emif-common.c
@@ -37,7 +37,8 @@ void set_lpmode_selfrefresh(u32 base)
 void force_emif_self_refresh()
 {
 	set_lpmode_selfrefresh(EMIF1_BASE);
-	set_lpmode_selfrefresh(EMIF2_BASE);
+	if (!is_dra72x())
+		set_lpmode_selfrefresh(EMIF2_BASE);
 }
 
 inline u32 emif_num(u32 base)
diff --git a/arch/arm/cpu/armv7/omap-common/hwinit-common.c b/arch/arm/cpu/armv7/omap-common/hwinit-common.c
index 2f9693f..f317293 100644
--- a/arch/arm/cpu/armv7/omap-common/hwinit-common.c
+++ b/arch/arm/cpu/armv7/omap-common/hwinit-common.c
@@ -147,8 +147,7 @@ void early_system_init(void)
 	hw_data_init();
 
 #ifdef CONFIG_SPL_BUILD
-	if (warm_reset() &&
-	    (is_omap44xx() || (omap_revision() == OMAP5430_ES1_0)))
+	if (warm_reset())
 		force_emif_self_refresh();
 #endif
 	watchdog_init();
diff --git a/arch/arm/cpu/armv7/omap-common/lowlevel_init.S b/arch/arm/cpu/armv7/omap-common/lowlevel_init.S
index 5283135..66a3b3d 100644
--- a/arch/arm/cpu/armv7/omap-common/lowlevel_init.S
+++ b/arch/arm/cpu/armv7/omap-common/lowlevel_init.S
@@ -16,9 +16,10 @@
 #include <asm/arch/spl.h>
 #include <linux/linkage.h>
 
+.arch_extension sec
+
 #ifdef CONFIG_SPL
 ENTRY(save_boot_params)
-
 	ldr	r1, =OMAP_SRAM_SCRATCH_BOOT_PARAMS
 	str	r0, [r1]
 	b	save_boot_params_ret
@@ -26,14 +27,40 @@ ENDPROC(save_boot_params)
 #endif
 
 ENTRY(omap_smc1)
-	PUSH	{r4-r12, lr}	@ save registers - ROM code may pollute
+	push	{r4-r12, lr}	@ save registers - ROM code may pollute
 				@ our registers
-	MOV	r12, r0		@ Service
-	MOV	r0, r1		@ Argument
-	DSB
-	DMB
-	.word	0xe1600070	@ SMC #0 - hand assembled for GCC versions
-				@ call ROM Code API for the service requested
+	mov	r12, r0		@ Service
+	mov	r0, r1		@ Argument
 
-	POP	{r4-r12, pc}
+	dsb
+	dmb
+	smc	0		@ SMC #0 to enter monitor mode
+				@ call ROM Code API for the service requested
+	pop	{r4-r12, pc}
 ENDPROC(omap_smc1)
+
+ENTRY(omap_smc_sec)
+	push	{r4-r12, lr}	@ save registers - ROM code may pollute
+				@ our registers
+	mov	r6, #0xFF	@ Indicate new Task call
+	mov	r12, #0x00	@ Secure Service ID in R12
+
+	dsb
+	dmb
+	smc	0		@ SMC #0 to enter monitor mode
+
+	b	omap_smc_sec_end @ exit at end of the service execution
+	nop
+
+	@ In case of IRQ happening in Secure, then ARM will branch here.
+	@ At that moment, IRQ will be pending and ARM will jump to Non Secure
+	@ IRQ handler
+	mov	r12, #0xFE
+
+	dsb
+	dmb
+	smc	0		@ SMC #0 to enter monitor mode
+
+omap_smc_sec_end:
+	pop	{r4-r12, pc}
+ENDPROC(omap_smc_sec)
diff --git a/arch/arm/cpu/armv7/omap-common/sec-common.c b/arch/arm/cpu/armv7/omap-common/sec-common.c
new file mode 100644
index 0000000..246a239
--- /dev/null
+++ b/arch/arm/cpu/armv7/omap-common/sec-common.c
@@ -0,0 +1,139 @@
+/*
+ *
+ * Common security related functions for OMAP devices
+ *
+ * (C) Copyright 2016
+ * Texas Instruments, <www.ti.com>
+ *
+ * Daniel Allred <d-allred@ti.com>
+ * Andreas Dannenberg <dannenberg@ti.com>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+
+#include <common.h>
+#include <stdarg.h>
+
+#include <asm/arch/sys_proto.h>
+#include <asm/omap_common.h>
+#include <asm/omap_sec_common.h>
+#include <asm/spl.h>
+#include <spl.h>
+
+/* Index for signature verify ROM API */
+#define API_HAL_KM_VERIFYCERTIFICATESIGNATURE_INDEX	(0x0000000E)
+
+static uint32_t secure_rom_call_args[5] __aligned(ARCH_DMA_MINALIGN);
+
+u32 secure_rom_call(u32 service, u32 proc_id, u32 flag, ...)
+{
+	int i;
+	u32 num_args;
+	va_list ap;
+
+	va_start(ap, flag);
+
+	num_args = va_arg(ap, u32);
+
+	if (num_args > 4)
+		return 1;
+
+	/* Copy args to aligned args structure */
+	for (i = 0; i < num_args; i++)
+		secure_rom_call_args[i + 1] = va_arg(ap, u32);
+
+	secure_rom_call_args[0] = num_args;
+
+	va_end(ap);
+
+	/* if data cache is enabled, flush the aligned args structure */
+	flush_dcache_range(
+		(unsigned int)&secure_rom_call_args[0],
+		(unsigned int)&secure_rom_call_args[0] +
+		roundup(sizeof(secure_rom_call_args), ARCH_DMA_MINALIGN));
+
+	return omap_smc_sec(service, proc_id, flag, secure_rom_call_args);
+}
+
+static u32 find_sig_start(char *image, size_t size)
+{
+	char *image_end = image + size;
+	char *sig_start_magic = "CERT_";
+	int magic_str_len = strlen(sig_start_magic);
+	char *ch;
+
+	while (--image_end > image) {
+		if (*image_end == '_') {
+			ch = image_end - magic_str_len + 1;
+			if (!strncmp(ch, sig_start_magic, magic_str_len))
+				return (u32)ch;
+		}
+	}
+	return 0;
+}
+
+int secure_boot_verify_image(void **image, size_t *size)
+{
+	int result = 1;
+	u32 cert_addr, sig_addr;
+	size_t cert_size;
+
+	/* Perform cache writeback on input buffer */
+	flush_dcache_range(
+		(u32)*image,
+		(u32)*image + roundup(*size, ARCH_DMA_MINALIGN));
+
+	cert_addr = (uint32_t)*image;
+	sig_addr = find_sig_start((char *)*image, *size);
+
+	if (sig_addr == 0) {
+		printf("No signature found in image!\n");
+		result = 1;
+		goto auth_exit;
+	}
+
+	*size = sig_addr - cert_addr;	/* Subtract out the signature size */
+	cert_size = *size;
+
+	/* Check if image load address is 32-bit aligned */
+	if (!IS_ALIGNED(cert_addr, 4)) {
+		printf("Image is not 4-byte aligned!\n");
+		result = 1;
+		goto auth_exit;
+	}
+
+	/* Image size also should be multiple of 4 */
+	if (!IS_ALIGNED(cert_size, 4)) {
+		printf("Image size is not 4-byte aligned!\n");
+		result = 1;
+		goto auth_exit;
+	}
+
+	/* Call ROM HAL API to verify certificate signature */
+	debug("%s: load_addr = %x, size = %x, sig_addr = %x\n", __func__,
+	      cert_addr, cert_size, sig_addr);
+
+	result = secure_rom_call(
+		API_HAL_KM_VERIFYCERTIFICATESIGNATURE_INDEX, 0, 0,
+		4, cert_addr, cert_size, sig_addr, 0xFFFFFFFF);
+auth_exit:
+	if (result != 0) {
+		printf("Authentication failed!\n");
+		printf("Return Value = %08X\n", result);
+		hang();
+	}
+
+	/*
+	 * Output notification of successful authentication as well the name of
+	 * the signing certificate used to re-assure the user that the secure
+	 * code is being processed as expected. However suppress any such log
+	 * output in case of building for SPL and booting via YMODEM. This is
+	 * done to avoid disturbing the YMODEM serial protocol transactions.
+	 */
+	if (!(IS_ENABLED(CONFIG_SPL_BUILD) &&
+	      IS_ENABLED(CONFIG_SPL_YMODEM_SUPPORT) &&
+	      spl_boot_device() == BOOT_DEVICE_UART))
+		printf("Authentication passed: %s\n", (char *)sig_addr);
+
+	return result;
+}
diff --git a/arch/arm/cpu/armv7/omap5/config.mk b/arch/arm/cpu/armv7/omap5/config.mk
index a7e55a5..d245572 100644
--- a/arch/arm/cpu/armv7/omap5/config.mk
+++ b/arch/arm/cpu/armv7/omap5/config.mk
@@ -15,5 +15,8 @@ else
 ALL-y	+= MLO
 endif
 else
+ifeq ($(CONFIG_TI_SECURE_DEVICE),y)
+ALL-y   += u-boot_HS.img
+endif
 ALL-y	+= u-boot.img
 endif
diff --git a/arch/arm/cpu/armv7/psci-common.c b/arch/arm/cpu/armv7/psci-common.c
new file mode 100644
index 0000000..d14b693
--- /dev/null
+++ b/arch/arm/cpu/armv7/psci-common.c
@@ -0,0 +1,39 @@
+/*
+ * Common PSCI functions
+ *
+ * Copyright (C) 2016 Chen-Yu Tsai
+ * Author: Chen-Yu Tsai <wens@csie.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <config.h>
+#include <asm/armv7.h>
+#include <asm/macro.h>
+#include <asm/psci.h>
+#include <asm/secure.h>
+#include <linux/linkage.h>
+
+static u32 psci_target_pc[CONFIG_ARMV7_PSCI_NR_CPUS] __secure_data = { 0 };
+
+void __secure psci_save_target_pc(int cpu, u32 pc)
+{
+	psci_target_pc[cpu] = pc;
+	DSB;
+}
+
+u32 __secure psci_get_target_pc(int cpu)
+{
+	return psci_target_pc[cpu];
+}
+
diff --git a/arch/arm/cpu/armv7/psci.S b/arch/arm/cpu/armv7/psci.S
index ab40837..350b75c 100644
--- a/arch/arm/cpu/armv7/psci.S
+++ b/arch/arm/cpu/armv7/psci.S
@@ -196,29 +196,56 @@ ENTRY(psci_cpu_off_common)
 	bx	lr
 ENDPROC(psci_cpu_off_common)
 
-@ expects CPU ID in r0 and returns stack top in r0
-ENTRY(psci_get_cpu_stack_top)
-	mov	r3, #0x400			@ 1kB of stack per CPU
-	mul	r0, r0, r3
-
-	ldr	r3, =psci_text_end		@ end of monitor text
-	add	r3, r3, #0x2000			@ Skip two pages
-	lsr	r3, r3, #12			@ Align to start of page
-	lsl	r3, r3, #12
-	sub	r3, r3, #4			@ reserve 1 word for target PC
-	sub	r0, r3, r0			@ here's our stack!
-
+@ The stacks are allocated in reverse order, i.e.
+@ the stack for CPU0 has the highest memory address.
+@
+@ --------------------  __secure_stack_end
+@ |  CPU0 target PC  |
+@ |------------------|
+@ |                  |
+@ |    CPU0 stack    |
+@ |                  |
+@ |------------------|  __secure_stack_end - 1KB
+@ |        .         |
+@ |        .         |
+@ |        .         |
+@ |        .         |
+@ --------------------  __secure_stack_start
+@
+@ This expects CPU ID in r0 and returns stack top in r0
+LENTRY(psci_get_cpu_stack_top)
+	@ stack top = __secure_stack_end - (cpuid << ARM_PSCI_STACK_SHIFT)
+	ldr	r3, =__secure_stack_end
+	sub	r0, r3, r0, LSL #ARM_PSCI_STACK_SHIFT
+	sub	r0, r0, #4		@ Save space for target PC
 	bx	lr
 ENDPROC(psci_get_cpu_stack_top)
 
+@ {r0, r1, r2, ip} from _do_nonsec_entry(kernel_entry, 0, machid, r2) in
+@ arch/arm/lib/bootm.c:boot_jump_linux() must remain unchanged across
+@ this function.
+ENTRY(psci_stack_setup)
+	mov	r6, lr
+	mov	r7, r0
+	bl	psci_get_cpu_id		@ CPU ID => r0
+	bl	psci_get_cpu_stack_top	@ stack top => r0
+	mov	sp, r0
+	mov	r0, r7
+	bx	r6
+ENDPROC(psci_stack_setup)
+
+ENTRY(psci_arch_init)
+	mov	pc, lr
+ENDPROC(psci_arch_init)
+.weak psci_arch_init
+
 ENTRY(psci_cpu_entry)
 	bl	psci_enable_smp
 
 	bl	_nonsec_init
 
 	bl	psci_get_cpu_id			@ CPU ID => r0
-	bl	psci_get_cpu_stack_top		@ stack top => r0
-	ldr	r0, [r0]			@ target PC at stack top
+	bl	psci_get_target_pc		@ target PC => r0
 	b	_do_nonsec_entry
 ENDPROC(psci_cpu_entry)
 
diff --git a/arch/arm/cpu/armv7/sunxi/Makefile b/arch/arm/cpu/armv7/sunxi/Makefile
index c208510..b35b9df 100644
--- a/arch/arm/cpu/armv7/sunxi/Makefile
+++ b/arch/arm/cpu/armv7/sunxi/Makefile
@@ -14,7 +14,6 @@ obj-$(CONFIG_MACH_SUN8I_H3)	+= tzpc.o
 
 ifndef CONFIG_SPL_BUILD
 obj-$(CONFIG_ARMV7_PSCI)	+= psci.o
-obj-$(CONFIG_ARMV7_PSCI)	+= psci_head.o
 endif
 
 ifdef CONFIG_SPL_BUILD
diff --git a/arch/arm/cpu/armv7/sunxi/psci.c b/arch/arm/cpu/armv7/sunxi/psci.c
index a118e9d..7ac8406 100644
--- a/arch/arm/cpu/armv7/sunxi/psci.c
+++ b/arch/arm/cpu/armv7/sunxi/psci.c
@@ -17,11 +17,11 @@
 #include <asm/gic.h>
 #include <asm/io.h>
 #include <asm/psci.h>
+#include <asm/secure.h>
 #include <asm/system.h>
 
 #include <linux/bitops.h>
 
-#define __secure	__attribute__ ((section ("._secure.text")))
 #define __irq		__attribute__ ((interrupt ("IRQ")))
 
 #define	GICD_BASE	(SUNXI_GIC400_BASE + GIC_DIST_OFFSET)
@@ -209,9 +209,8 @@ int __secure psci_cpu_on(u32 __always_unused unused, u32 mpidr, u32 pc)
 		(struct sunxi_cpucfg_reg *)SUNXI_CPUCFG_BASE;
 	u32 cpu = (mpidr & 0x3);
 
-	/* store target PC at target CPU stack top */
-	writel(pc, psci_get_cpu_stack_top(cpu));
-	DSB;
+	/* store target PC */
+	psci_save_target_pc(cpu, pc);
 
 	/* Set secondary core power on PC */
 	writel((u32)&psci_cpu_entry, &cpucfg->priv0);
@@ -250,7 +249,7 @@ void __secure psci_cpu_off(void)
 		wfi();
 }
 
-void __secure sunxi_gic_init(void)
+void __secure psci_arch_init(void)
 {
 	u32 reg;
 
diff --git a/arch/arm/cpu/armv7/sunxi/psci_head.S b/arch/arm/cpu/armv7/sunxi/psci_head.S
deleted file mode 100644
index 8fa823d..0000000
--- a/arch/arm/cpu/armv7/sunxi/psci_head.S
+++ /dev/null
@@ -1,66 +0,0 @@
-/*
- * Copyright (C) 2013 - ARM Ltd
- * Author: Marc Zyngier <marc.zyngier@arm.com>
- *
- * Based on code by Carl van Schaik <carl@ok-labs.com>.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <config.h>
-#include <linux/linkage.h>
-
-#include <asm/arch-armv7/generictimer.h>
-#include <asm/gic.h>
-#include <asm/macro.h>
-#include <asm/psci.h>
-#include <asm/arch/cpu.h>
-
-/*
- * Memory layout:
- *
- * SECURE_RAM to text_end :
- *	._secure_text section
- * text_end to ALIGN_PAGE(text_end):
- *	nothing
- * ALIGN_PAGE(text_end) to ALIGN_PAGE(text_end) + 0x1000)
- *	1kB of stack per CPU (4 CPUs max).
- */
-
-	.pushsection ._secure.text, "ax"
-
-	.arch_extension sec
-
-#define	GICD_BASE		(SUNXI_GIC400_BASE +  0x1000)
-#define	GICC_BASE		(SUNXI_GIC400_BASE +  0x2000)
-
-@ {r0, r1, r2, ip} from _do_nonsec_entry(kernel_entry, 0, machid, r2) in
-@ arch/arm/lib/bootm.c:boot_jump_linux() must remain unchanged across
-@ this function.
-ENTRY(psci_arch_init)
-	mov	r6, lr
-	mov	r7, r0
-	bl	psci_get_cpu_id		@ CPU ID => r0
-	bl	psci_get_cpu_stack_top	@ stack top => r0
-	sub	r0, r0, #4		@ Save space for target PC
-	mov	sp, r0
-	mov	r0, r7
-	mov	lr, r6
-
-	push	{r0, r1, r2, ip, lr}
-	bl	sunxi_gic_init
-	pop	{r0, r1, r2, ip, pc}
-ENDPROC(psci_arch_init)
-
-ENTRY(psci_text_end)
-	.popsection
diff --git a/arch/arm/cpu/armv7/virt-dt.c b/arch/arm/cpu/armv7/virt-dt.c
index 32c368f..707dad4 100644
--- a/arch/arm/cpu/armv7/virt-dt.c
+++ b/arch/arm/cpu/armv7/virt-dt.c
@@ -26,69 +26,6 @@
 #include <asm/armv7.h>
 #include <asm/psci.h>
 
-static int fdt_psci(void *fdt)
-{
-#ifdef CONFIG_ARMV7_PSCI
-	int nodeoff;
-	int tmp;
-
-	nodeoff = fdt_path_offset(fdt, "/cpus");
-	if (nodeoff < 0) {
-		printf("couldn't find /cpus\n");
-		return nodeoff;
-	}
-
-	/* add 'enable-method = "psci"' to each cpu node */
-	for (tmp = fdt_first_subnode(fdt, nodeoff);
-	     tmp >= 0;
-	     tmp = fdt_next_subnode(fdt, tmp)) {
-		const struct fdt_property *prop;
-		int len;
-
-		prop = fdt_get_property(fdt, tmp, "device_type", &len);
-		if (!prop)
-			continue;
-		if (len < 4)
-			continue;
-		if (strcmp(prop->data, "cpu"))
-			continue;
-
-		fdt_setprop_string(fdt, tmp, "enable-method", "psci");
-	}
-
-	nodeoff = fdt_path_offset(fdt, "/psci");
-	if (nodeoff < 0) {
-		nodeoff = fdt_path_offset(fdt, "/");
-		if (nodeoff < 0)
-			return nodeoff;
-
-		nodeoff = fdt_add_subnode(fdt, nodeoff, "psci");
-		if (nodeoff < 0)
-			return nodeoff;
-	}
-
-	tmp = fdt_setprop_string(fdt, nodeoff, "compatible", "arm,psci");
-	if (tmp)
-		return tmp;
-	tmp = fdt_setprop_string(fdt, nodeoff, "method", "smc");
-	if (tmp)
-		return tmp;
-	tmp = fdt_setprop_u32(fdt, nodeoff, "cpu_suspend", ARM_PSCI_FN_CPU_SUSPEND);
-	if (tmp)
-		return tmp;
-	tmp = fdt_setprop_u32(fdt, nodeoff, "cpu_off", ARM_PSCI_FN_CPU_OFF);
-	if (tmp)
-		return tmp;
-	tmp = fdt_setprop_u32(fdt, nodeoff, "cpu_on", ARM_PSCI_FN_CPU_ON);
-	if (tmp)
-		return tmp;
-	tmp = fdt_setprop_u32(fdt, nodeoff, "migrate", ARM_PSCI_FN_MIGRATE);
-	if (tmp)
-		return tmp;
-#endif
-	return 0;
-}
-
 int armv7_apply_memory_carveout(u64 *start, u64 *size)
 {
 #ifdef CONFIG_ARMV7_SECURE_RESERVE_SIZE