plat-stm: beautify source code

Move plat_cpu_reset_late() to C-source.
Move arm_cl2_config() to C-source.
Beautify main.c and rng_support.c.

Signed-off-by: Etienne Carriere <etienne.carriere@linaro.org>
Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>
Reviewed-by: Jerome Forissier <jerome.forissier@linaro.org>

6 files changed, 195 insertions, 317 deletions

diff --git a/core/arch/arm/include/arm32.h b/core/arch/arm/include/arm32.h
index 9b8ee63d..69d50c3e 100644
--- a/core/arch/arm/include/arm32.h
+++ b/core/arch/arm/include/arm32.h
@@ -351,6 +351,13 @@ static inline uint32_t read_ifsr(void)
 	return ifsr;
 }
 
+static inline void write_scr(uint32_t scr)
+{
+	asm volatile ("mcr	p15, 0, %[scr], c1, c1, 0"
+			: : [scr] "r" (scr)
+	);
+}
+
 static inline void isb(void)
 {
 	asm volatile ("isb");
diff --git a/core/arch/arm/include/kernel/generic_boot.h b/core/arch/arm/include/kernel/generic_boot.h
index c9e6516d..002bf3a7 100644
--- a/core/arch/arm/include/kernel/generic_boot.h
+++ b/core/arch/arm/include/kernel/generic_boot.h
@@ -47,6 +47,12 @@ void init_sec_mon(unsigned long nsec_entry);
 
 const struct thread_handlers *generic_boot_get_handlers(void);
 
+/* weak routines eventually overridden by platform */
+void plat_cpu_reset_early(void);
+void plat_cpu_reset_late(void);
+void arm_cl2_config(vaddr_t pl310);
+void arm_cl2_enable(vaddr_t pl310);
+
 #ifdef CFG_BOOT_SECONDARY_REQUEST
 extern uint32_t ns_entry_addrs[];
 #endif
diff --git a/core/arch/arm/plat-stm/main.c b/core/arch/arm/plat-stm/main.c
index 47d47b39..d41cd1b0 100644
--- a/core/arch/arm/plat-stm/main.c
+++ b/core/arch/arm/plat-stm/main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, STMicroelectronics International N.V.
+ * Copyright (c) 2014-2016, STMicroelectronics International N.V.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -25,9 +25,13 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <arm32.h>
+#include <asc.h>
 #include <console.h>
 #include <drivers/pl011.h>
+#include <io.h>
 #include <kernel/generic_boot.h>
+#include <kernel/misc.h>
 #include <kernel/panic.h>
 #include <kernel/pm_stubs.h>
 #include <kernel/tz_ssvce_pl310.h>
@@ -37,15 +41,41 @@
 #include <stdint.h>
 #include <tee/entry_std.h>
 #include <tee/entry_fast.h>
-#include <asc.h>
+#include <trace.h>
+
+register_phys_mem(MEM_AREA_IO_SEC, CPU_IOMEM_BASE, CORE_MMU_DEVICE_SIZE);
+register_phys_mem(MEM_AREA_IO_SEC, RNG_BASE, CORE_MMU_DEVICE_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, UART_CONSOLE_BASE, CORE_MMU_DEVICE_SIZE);
+
+#if defined(PLATFORM_FLAVOR_b2260)
+#define stm_tee_entry_std	tee_entry_std
+static bool ns_resources_ready(void)
+{
+	return true;
+}
+#else
+/* some nonsecure resource might not be ready (uart) */
+static int boot_is_completed __early_bss;
+static bool ns_resources_ready(void)
+{
+	return !!boot_is_completed;
+}
+static void stm_tee_entry_std(struct thread_smc_args *smc_args)
+{
+	boot_is_completed = 1;
+	tee_entry_std(smc_args);
+}
+#endif
 
-static void main_fiq(void);
-static void stm_tee_entry_std(struct thread_smc_args *smc_args);
+static void stm_fiq(void)
+{
+	panic();
+}
 
 static const struct thread_handlers handlers = {
 	.std_smc = stm_tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.fiq = stm_fiq,
 	.cpu_on = pm_panic,
 	.cpu_off = pm_panic,
 	.cpu_suspend = pm_panic,
@@ -59,22 +89,8 @@ const struct thread_handlers *generic_boot_get_handlers(void)
 	return &handlers;
 }
 
-static int boot_is_completed;
-static void stm_tee_entry_std(struct thread_smc_args *smc_args)
-{
-	boot_is_completed = 1;
-	tee_entry_std(smc_args);
-}
-
-static void main_fiq(void)
-{
-	panic();
-}
-
-
 static vaddr_t console_base(void)
 {
-	/* in case it's used before .bss is cleared */
 	static void *va __early_bss;
 
 	if (cpu_mmu_enabled()) {
@@ -91,21 +107,21 @@ void console_init(void)
 
 void console_putc(int ch)
 {
-	if (!boot_is_completed)
-		return;
-	__asc_xmit_char((char)ch, console_base());
+	if (ns_resources_ready()) {
+		if (ch == '\n')
+			__asc_xmit_char('\r', console_base());
+		__asc_xmit_char((char)ch, console_base());
+	}
 }
 
 void console_flush(void)
 {
-	if (!boot_is_completed)
-		return;
-	__asc_flush(console_base());
+	if (ns_resources_ready())
+		__asc_flush(console_base());
 }
 
 vaddr_t pl310_base(void)
 {
-	/* in case it's used before .bss is cleared */
 	static void *va __early_bss;
 
 	if (cpu_mmu_enabled()) {
@@ -115,3 +131,116 @@ vaddr_t pl310_base(void)
 	}
 	return PL310_BASE;
 }
+
+void arm_cl2_config(vaddr_t pl310)
+{
+	uint32_t v;
+	/* pl310 off */
+	write32(0, pl310 + PL310_CTRL);
+
+	/*
+	 * TAG RAM Control Register
+	 *
+	 * bit[10:8]:1 - 2 cycle of write accesses latency
+	 * bit[6:4]:1 - 2 cycle of read accesses latency
+	 * bit[2:0]:1 - 2 cycle of setup latency
+	 */
+	v = read32(pl310 + PL310_TAG_RAM_CTRL);
+	v &= 0xFFFFF888;
+	v |= 0xFFFFF999;
+	write32(v, pl310 + PL310_TAG_RAM_CTRL);
+
+	/*
+	 * DATA RAM Control Register
+	 *
+	 * bit[10:8]:2 - 3 cycle of write accesses latency
+	 * bit[6:4]:2 - 3 cycle of read accesses latency
+	 * bit[2:0]:2 - 3 cycle of setup latency
+	 */
+	v = read32(pl310 + PL310_DATA_RAM_CTRL);
+	v &= 0xFFFFF888;
+	v |= 0xFFFFFAAA;
+	write32(v, pl310 + PL310_DATA_RAM_CTRL);
+
+	/*
+	 * Auxiliary Control Register
+	 *
+	 * I/Dcache prefetch enabled (bit29:28=2b11)
+	 * NS can access interrupts (bit27=1)
+	 * NS can lockown cache lines (bit26=1)
+	 * Pseudo-random replacement policy (bit25=0)
+	 * Force write allocated (default)
+	 * Shared attribute internally ignored (bit22=1, bit13=0)
+	 * Parity disabled (bit21=0)
+	 * Event monitor disabled (bit20=0)
+	 * Set or preserved way config: size (bit19:17), ass (bit16)
+	 * Store buffer device limitation enabled (bit11=1)
+	 * Cacheable accesses have high prio (bit10=0)
+	 * Full Line Zero (FLZ) disabled (bit0=0)
+	 */
+	v = PL310_AUX_CTRL_INIT;
+	write32(v, pl310 + PL310_AUX_CTRL);
+
+	/*
+	 * Prefetch Control Register
+	 *
+	 * Double linefill disabled (bit30=0)
+	 * I/D prefetch enabled (bit29:28=2b11)
+	 * Prefetch drop enabled (bit24=1)
+	 * Incr double linefill disable (bit23=0)
+	 * Prefetch offset = 7 (bit4:0)
+	 */
+	write32(0x31000007, pl310 + PL310_PREFETCH_CTRL);
+
+	/*
+	 * Power Register
+	 *
+	 * Dynamic clock gating enabled
+	 * Standby mode enabled
+	 */
+	write32(0x00000003, pl310 + PL310_POWER_CTRL);
+
+	/* invalidate all pl310 cache ways */
+	arm_cl2_invbyway(pl310);
+}
+
+void plat_cpu_reset_late(void)
+{
+	int i;
+
+	assert(!cpu_mmu_enabled());
+
+	/* Allow NSec to manage FIQ/Imprecise abort (SCR[FW]=1, SCR[AW]=1) */
+	write_scr(SCR_AW | SCR_FW);
+
+	if (get_core_pos())
+		return;
+
+	/* both secure CPU access SCU */
+	write32(3, SCU_BASE + SCU_SAC);
+
+	/* both nonsec cpu access SCU, private and global timer */
+	write32(0x333, SCU_BASE + SCU_NSAC);
+
+	/* SCU Filtering End Address register */
+	write32(CPU_PORT_FILT_END, SCU_BASE + SCU_FILT_EA);
+	write32(CPU_PORT_FILT_START, SCU_BASE + SCU_FILT_SA);
+
+	/*
+	 * SCU Control Register : CTRL = 0x00000065
+	 * - ic stanby enable=1
+	 * - scu standby enable=1
+	 * - scu enable=1
+	 */
+	write32(0x0065, SCU_BASE + SCU_CTRL);
+
+	/*
+	 * - All external interrupts are NonSecure.
+	 */
+	for (i = 0; i < (31 * 4); i += 4)
+		write32(0xFFFFFFFF, GIC_DIST_BASE + GIC_DIST_ISR1 + i);
+
+	/* PL310 Memory Controller port filtering */
+	write32(CPU_PORT_FILT_END, pl310_base() + PL310_ADDR_FILT_END);
+	write32(CPU_PORT_FILT_START | 1, pl310_base() + PL310_ADDR_FILT_START);
+}
diff --git a/core/arch/arm/plat-stm/platform_config.h b/core/arch/arm/plat-stm/platform_config.h
index 71f6061d..2f6650a6 100644
--- a/core/arch/arm/plat-stm/platform_config.h
+++ b/core/arch/arm/plat-stm/platform_config.h
@@ -90,6 +90,8 @@
 #define UART_CONSOLE_BASE	ST_ASC21_REGS_BASE
 #define RNG_BASE		0x08A89000
 
+#define PL310_AUX_CTRL_INIT	0x3C480800
+
 #elif defined(PLATFORM_FLAVOR_cannes)
 
 #define DRAM0_BASE		0x40000000
@@ -103,6 +105,8 @@
 #define UART_CONSOLE_BASE	ST_ASC20_REGS_BASE
 #define RNG_BASE		0x08A89000
 
+#define PL310_AUX_CTRL_INIT	0x3C480800
+
 #elif defined(PLATFORM_FLAVOR_orly2)
 
 #define DRAM0_BASE		0x40000000
@@ -118,6 +122,8 @@
 #define UART_CONSOLE_BASE	ST_ASC21_REGS_BASE
 #define RNG_BASE		0xFEE80000
 
+#define PL310_AUX_CTRL_INIT	0x3C480800
+
 #else
 
 #error "Unknown platform flavor"
@@ -130,7 +136,6 @@
 #define CFG_TEE_LOAD_ADDR	CFG_TEE_RAM_START
 #endif
 
-
 #define PL310_BASE		(CPU_IOMEM_BASE + 0x2000)
 #define GIC_DIST_BASE		(CPU_IOMEM_BASE + 0x1000)
 #define SCU_BASE		(CPU_IOMEM_BASE + 0x0000)
diff --git a/core/arch/arm/plat-stm/rng_support.c b/core/arch/arm/plat-stm/rng_support.c
index 4a0e239c..25b38931 100644
--- a/core/arch/arm/plat-stm/rng_support.c
+++ b/core/arch/arm/plat-stm/rng_support.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, STMicroelectronics International N.V.
+ * Copyright (c) 2014-2016, STMicroelectronics International N.V.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -34,20 +34,14 @@
 
 #include "rng_support.h"
 
-#define USE_SW_DELAY         0
-
-/*
- * if a HW issue is detected, infinite loop is started until valid data are
- * available.
- * - User-side timeout is expected to detect the issue.
- * else error is logged and 0x00 is returned
- */
-#define USE_USER_TIMEOUT     1
-
 /* Address of the register to read in the RNG IP */
 #define RNG_VAL_OFFSET             0x24
 #define RNG_STATUS_OFFSET          0x20
 
+#define RNG_STATUS_ERR0		BIT32(0)
+#define RNG_STATUS_ERR1		BIT32(1)
+#define RNG_STATUS_FULL		BIT32(5)
+
 static vaddr_t rng_base(void)
 {
 	static void *va __early_bss;
@@ -62,27 +56,16 @@ static vaddr_t rng_base(void)
 
 static inline int hwrng_waithost_fifo_full(void)
 {
-	int res = 0;
-	volatile uint32_t status;
+	uint32_t status;
 
-	/* Wait HOST FIFO FULL (see rng_fspec_revG_120720.pdf) */
 	do {
 		status = read32(rng_base() + RNG_STATUS_OFFSET);
-	} while ((status & 0x20) != 0x20);
+	} while (!(status & RNG_STATUS_FULL));
 
-	/* Check STATUS (see rng_fspec_revG_120720.pdf) */
-	if ((status & 0x3) != 0) {
-		EMSG("generated HW random data are not valid");
-		res = -1;
-	}
-
-#if (USE_USER_TIMEOUT == 1)
-	if (res != 0)
-		while (1)
-			;
-#endif
+	if (status & (RNG_STATUS_ERR0 | RNG_STATUS_ERR1))
+		return 1;
 
-	return res;
+	return 0;
 }
 
 uint8_t hw_get_random_byte(void)
@@ -131,7 +114,6 @@ uint8_t hw_get_random_byte(void)
 	volatile uint32_t tmpval[_LOCAL_FIFO_SIZE/2];
 	uint8_t value;
 	int i;
-	int res;
 
 	nbcall++;
 
@@ -144,21 +126,13 @@ uint8_t hw_get_random_byte(void)
 		return value;
 	}
 
-	/* Wait HOST FIFO full */
-	res = hwrng_waithost_fifo_full();
-	if (res < 0)
-		return 0x00;
+	if (hwrng_waithost_fifo_full())
+		return 0;
 
 	/* Read the FIFO according the number of expected element */
-	for (i = 0; i < _LOCAL_FIFO_SIZE / 2; i++) {
+	for (i = 0; i < _LOCAL_FIFO_SIZE / 2; i++)
 		tmpval[i] = read32(rng_base() + RNG_VAL_OFFSET) & 0xFFFF;
-#if (USE_SW_DELAY == 1)
-		/* Wait 0.667 us (fcpu = 600Mhz -> 400 cycles) @see doc */
-		volatile int ll = 200;
-		while (ll--)
-			;
-#endif
-	}
+
 	/* Update the local SW fifo for next request */
 	pos = 0;
 	for (i = 0; i < _LOCAL_FIFO_SIZE / 2; i++) {
diff --git a/core/arch/arm/plat-stm/tz_a9init.S b/core/arch/arm/plat-stm/tz_a9init.S
index 4fe245e6..50fb861e 100644
--- a/core/arch/arm/plat-stm/tz_a9init.S
+++ b/core/arch/arm/plat-stm/tz_a9init.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, STMicroelectronics International N.V.
+ * Copyright (c) 2014-2016, STMicroelectronics International N.V.
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -25,12 +25,6 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-/*
- * Entry points for the A9 inits, A9 revision specific or not.
- * It is assume no stack is available when these routines are called.
- * It is assume each routine is called with return address in LR
- * and with ARM registers R0, R1, R2, R3 being scratchable.
- */
 #include <arm32.h>
 #include <arm32_macros.S>
 #include <asm.S>
@@ -38,119 +32,11 @@
 #include <kernel/unwind.h>
 #include <platform_config.h>
 
-#define CPUID_A9_R3P0_H 0x413f
-#define CPUID_A9_R3P0_L 0xc090
-
 .section .text
 .balign 4
 .code 32
 
 /*
- * void arm_cl2_config(vaddr_t pl310_base) - Memory Cache Level2 Configuration
- *
- * Use scratables registers R0-R3.
- * No stack usage.
- * LR store return address.
- * Trap CPU in case of error.
- */
-FUNC arm_cl2_config , :
-UNWIND(	.fnstart)
-
-	mrc  p15, 0, r2, c0, c0, 0  /* read A9 ID */
-	movw r1, #CPUID_A9_R3P0_L
-	movt r1, #CPUID_A9_R3P0_H
-	cmp  r2, r1
-	beq  _config_l2cc_r3p0
-	b . /* TODO: unknown id: reset? log? */
-
-_config_l2cc_r3p0:
-	/*
-	 * TAG RAM Control Register
-	 *
-	 * bit[10:8]:1 - 2 cycle of write accesses latency
-	 * bit[6:4]:1 - 2 cycle of read accesses latency
-	 * bit[2:0]:1 - 2 cycle of setup latency
-	 */
-	ldr  r2, [r0, #PL310_TAG_RAM_CTRL]
-	movw r1, #0xf888
-	movt r1, #0xffff
-	and  r2,r2,r1
-	movw r1, #0xf999
-	movt r1, #0xffff
-	orr  r2,r2,r1
-	str  r2, [r0, #PL310_TAG_RAM_CTRL]
-
-	/*
-	 * DATA RAM Control Register
-	 *
-	 * bit[10:8]:2 - 3 cycle of write accesses latency
-	 * bit[6:4]:2 - 3 cycle of read accesses latency
-	 * bit[2:0]:2 - 3 cycle of setup latency
-	 */
-	ldr  r2, [r0, #PL310_DATA_RAM_CTRL]
-	movw r1, #0xf888
-	movt r1, #0xffff
-	and  r2,r2,r1
-	movw r1, #0xfaaa
-	movt r1, #0xffff
-	orr  r2,r2,r1
-	str  r2, [r0, #PL310_DATA_RAM_CTRL]
-
-	/*
-	 * Auxiliary Control Register = 0x3C480800
-	 *
-	 * I/Dcache prefetch enabled (bit29:28=2b11)
-	 * NS can access interrupts (bit27=1)
-	 * NS can lockown cache lines (bit26=1)
-	 * Pseudo-random replacement policy (bit25=0)
-	 * Force write allocated (default)
-	 * Shared attribute internally ignored (bit22=1, bit13=0)
-	 * Parity disabled (bit21=0)
-	 * Event monitor disabled (bit20=0)
-	 * 128kB ways, 8-way associativity (bit19:17=3b100 bit16=0)
-	 * Store buffer device limitation enabled (bit11=1)
-	 * Cacheable accesses have high prio (bit10=0)
-	 * Full Line Zero (FLZ) disabled (bit0=0)
-	 */
-	movw r1, #0x0800
-	movt r1, #0x3C48
-	str  r1, [r0, #PL310_AUX_CTRL]
-
-	/*
-	 * Prefetch Control Register = 0x31000007
-	 *
-	 * Double linefill disabled (bit30=0)
-	 * I/D prefetch enabled (bit29:28=2b11)
-	 * Prefetch drop enabled (bit24=1)
-	 * Incr double linefill disable (bit23=0)
-	 * Prefetch offset = 7 (bit4:0)
-	 */
-	movw r1, #0x0007
-	movt r1, #0x3100
-	str  r1, [r0, #PL310_PREFETCH_CTRL]
-
-	/*
-	 * Power Register = 0x00000003
-	 *
-	 * Dynamic clock gating enabled
-	 * Standby mode enabled
-	 */
-	movw r1, #0x0003
-	movt r1, #0x0000
-	str  r1, [r0, #PL310_POWER_CTRL]
-
-	/* invalidate all cache ways */
-	movw r1, #0x00FF
-	movt r1, #0x0000
-	str  r1, [r0, #PL310_INV_BY_WAY]
-
-	mov pc, lr
-UNWIND(	.fnend)
-END_FUNC arm_cl2_config
-/* End of arm_cl2_config */
-
-
-/*
  * void arm_cl2_enable(vaddr_t pl310_base) - Memory Cache Level2 Enable Function
  *
  * If PL310 supports FZLW, enable also FZL in A9 core
@@ -164,7 +50,6 @@ END_FUNC arm_cl2_config
 FUNC arm_cl2_enable , :
 UNWIND(	.fnstart)
 
-
 	/* Enable PL310 ctrl -> only set lsb bit */
 	mov  r1, #0x1
 	str  r1, [r0, #PL310_CTRL]
@@ -172,15 +57,16 @@ UNWIND(	.fnstart)
 	/* if L2 FLZW enable, enable in L1 */
 	ldr  r1, [r0, #PL310_AUX_CTRL]
 	tst  r1, #(1 << 0) /* test AUX_CTRL[FLZ] */
-	mrc  p15, 0, r0, c1, c0, 1
+	read_actlr r0
 	orrne r0, r0, #(1 << 3) /* enable ACTLR[FLZW] */
-	mcr  p15, 0, r0, c1, c0, 1
+	write_actlr r0
 
 	mov pc, lr
+
 UNWIND(	.fnend)
 END_FUNC arm_cl2_enable
 
-/* 
+/*
  * Cortex A9 configuration early configuration
  *
  * Use scratables registers R0-R3.
@@ -191,15 +77,6 @@ END_FUNC arm_cl2_enable
 FUNC plat_cpu_reset_early , :
 UNWIND(	.fnstart)
 
-	/* only r3p0 is supported */
-	mrc  p15, 0, r0, c0, c0, 0  /* read A9 ID */
-	movw r1, #CPUID_A9_R3P0_L
-	movt r1, #CPUID_A9_R3P0_H
-	cmp  r0, r1
-	beq  _early_a9_r3p0
-	b . /* TODO: unknown id: reset? log? */
-
-_early_a9_r3p0:
 	/*
 	 * Mandated HW config loaded
 	 *
@@ -253,128 +130,8 @@ _early_a9_r3p0:
 	mov  r1, #0xFFFFFFFF
 	str  r1, [r0, #CORE_ICC_ICCPMR]
 
-	mov pc, lr /* back to tzinit */
-UNWIND(	.fnend)
-END_FUNC plat_cpu_reset_early
-
-/*
- * A9 secured config, needed only from a single core
- *
- * Use scratables registers R0-R3.
- * No stack usage.
- * LR store return address.
- * Trap CPU in case of error.
- *
- * TODO: size optim in code
- */
-FUNC plat_cpu_reset_late , :
-UNWIND(	.fnstart)
-
-	/* Allow NSec to manage FIQ/Imprecise abort (SCR[FW]=1, SCR[AW]=1) */
-	read_scr r0
-	orr r0, r0, #SCR_AW
-	orr r0, r0, #SCR_FW
-	write_scr r0
-
-	mrc p15, 0, r0, c0, c0, 5
-	ands r0, #3
-	beq _boot_late_primary_cpu
-
-_boot_late_secondary_cpu:
 	mov pc, lr
 
-_boot_late_primary_cpu:
-	/*
-	 * Snoop Control Unit configuration
-	 *
-	 * SCU is enabled with filtering off.
-	 * Both Secure/Unsecure can access SCU and timers
-	 *
-	 * 0x00 SCUControl = 0x00000060 !!!  should be 0x5 !   A NETTOYER !!!!!!!!!!!!!!!!!!!!!!!!!
-	 * 0x04 SCUConfiguration =  ???                        A NETTOYER !!!!!!!!!!!!!!!!!!!!!!!!!
-	 * 0x0C SCUInvalidateAll (Secure cfg)
-	 * 0x40 FilteringStartAddress = 0x40000000
-	 * 0x44 FilteeringEndAddress - 0x80000000
-	 * 0x50 SCUAccessControl
-	 * 0x54 SCUSecureAccessControl
-	 */
-
-	/*
-	 * SCU Access Register : SAC = 0x00000003
-	 * - both secure CPU access SCU
-	 */
-	ldr  r0, =SCU_BASE
-	movw r1, #0x0003
-	movt r1, #0x0000
-	str  r1, [r0, #SCU_SAC]
-
-	/*
-	 * SCU NonSecure Access Register : SNSAC : 0x00000333
-	 * - both nonsec cpu access SCU, private and global timer
-	 */
-	movw r1, #0x0333
-	movt r1, #0x0000
-	str  r1, [r0, #SCU_NSAC]
-
-	/*
-	 * SCU Filtering End Address register: SFEA
-	 */
-	movw r1, #(CPU_PORT_FILT_END & 0xFFFF)
-	movt r1, #(CPU_PORT_FILT_END >> 16)
-	str  r1, [r0, #SCU_FILT_EA]
-
-	/*
-	 * SCU Filtering Start Address register: SFSA
-	 */
-	movw r1, #(CPU_PORT_FILT_START & 0xFFFF)
-	movt r1, #(CPU_PORT_FILT_START >> 16)
-	str  r1, [r0, #SCU_FILT_SA]
-
-	/*
-	 * SCU Control Register : CTRL = 0x00000065
-	 * - ic stanby enable=1
-	 * - scu standby enable=1
-	 * - scu enable=1
-	 */
-	movw r1, #0x0065
-	movt r1, #0x0000
-	str	 r1, [r0, #SCU_CTRL]
-
-	/*- GIC secure configuration ---*/
-
-	/*
-	 * Register ICDISR[1-31] = 0xFFFFFFFF
-	 * - All external interrupts are NonSecure.
-	 */
-	ldr  r0, =(GIC_DIST_BASE + GIC_DIST_ISR1)
-	mov  r2, #0xFFFFFFFF
-	mov  r1, #31 /* Nb of loop rounds */
-loop_1:
-	str r2, [r0]
-	add r0, #4
-	sub r1, r1, #1
-	cmp r1, #0
-	bne loop_1
-
-
-	/*- PL310 Memory Controller (Note: should be done with NS=1) ---*/
-	/* Note: we're in secure mode with mmu disabled => NS=0 */
-
-	/*
-	 * reg12_addr_filtering_end
-	 */
-	ldr  r0, =PL310_BASE
-	movw r1, #(CPU_PORT_FILT_END & 0xFFFF)
-	movt r1, #(CPU_PORT_FILT_END >> 16)
-	str  r1, [r0, #PL310_ADDR_FILT_END]
-
-	/*
-	 * reg12_addr_filtering_start
-	 */
-	movw r1, #((CPU_PORT_FILT_START & 0xFFFF) | 1)
-	movt r1, #(CPU_PORT_FILT_START >> 16)
-	str  r1, [r0, #PL310_ADDR_FILT_START]
-
-	mov pc, lr
 UNWIND(	.fnend)
-END_FUNC plat_cpu_reset_late
+END_FUNC plat_cpu_reset_early
+