Merge branch 'master' of git://git.denx.de/u-boot-uniphier

13 files changed, 537 insertions, 176 deletions

diff --git a/arch/arm/mach-uniphier/Kconfig b/arch/arm/mach-uniphier/Kconfig
index e39ced674f..a8a0b90c39 100644
--- a/arch/arm/mach-uniphier/Kconfig
+++ b/arch/arm/mach-uniphier/Kconfig
@@ -6,6 +6,8 @@ config SYS_CONFIG_NAME
 config ARCH_UNIPHIER_32BIT
 	bool
 	select CPU_V7
+	select CPU_V7_HAS_NONSEC
+	select ARMV7_NONSEC
 
 config ARCH_UNIPHIER_64BIT
 	bool
@@ -70,6 +72,13 @@ config ARCH_UNIPHIER_LD6B
 	depends on ARCH_UNIPHIER_PRO5_PXS2_LD6B
 	default y
 
+config CACHE_UNIPHIER
+	bool "Enable the UniPhier L2 cache controller"
+	depends on ARCH_UNIPHIER_32BIT
+	default y
+	help
+	  This option allows to use the UniPhier System Cache as L2 cache.
+
 config MICRO_SUPPORT_CARD
 	bool "Use Micro Support Card"
 	help
diff --git a/arch/arm/mach-uniphier/arm32/Makefile b/arch/arm/mach-uniphier/arm32/Makefile
index 5074ebda97..6f05d727e0 100644
--- a/arch/arm/mach-uniphier/arm32/Makefile
+++ b/arch/arm/mach-uniphier/arm32/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_DEBUG_LL) += debug_ll.o
 else
 obj-y += late_lowlevel_init.o
 obj-y += cache-uniphier.o
+obj-$(CONFIG_ARMV7_PSCI) += psci.o psci_smp.o
 endif
 
 obj-y += timer.o
diff --git a/arch/arm/mach-uniphier/arm32/arm-mpcore.h b/arch/arm/mach-uniphier/arm32/arm-mpcore.h
index cf7cd46c10..1a856641de 100644
--- a/arch/arm/mach-uniphier/arm32/arm-mpcore.h
+++ b/arch/arm/mach-uniphier/arm32/arm-mpcore.h
@@ -12,6 +12,9 @@
 
 /* SCU Control Register */
 #define SCU_CTRL		0x00
+#define SCU_ENABLE		(1 << 0)
+#define SCU_STANDBY_ENABLE	(1 << 5)
+
 /* SCU Configuration Register */
 #define SCU_CONF		0x04
 /* SCU CPU Power Status Register */
diff --git a/arch/arm/mach-uniphier/arm32/cache-uniphier.c b/arch/arm/mach-uniphier/arm32/cache-uniphier.c
index 76fe5ebe09..658969b049 100644
--- a/arch/arm/mach-uniphier/arm32/cache-uniphier.c
+++ b/arch/arm/mach-uniphier/arm32/cache-uniphier.c
@@ -8,11 +8,74 @@
 
 #include <common.h>
 #include <linux/io.h>
+#include <linux/kernel.h>
 #include <asm/armv7.h>
-
-#include "ssc-regs.h"
-
-#ifdef CONFIG_UNIPHIER_L2CACHE_ON
+#include <asm/processor.h>
+
+#include "cache-uniphier.h"
+
+/* control registers */
+#define UNIPHIER_SSCC		0x500c0000	/* Control Register */
+#define    UNIPHIER_SSCC_BST			(0x1 << 20)	/* UCWG burst read */
+#define    UNIPHIER_SSCC_ACT			(0x1 << 19)	/* Inst-Data separate */
+#define    UNIPHIER_SSCC_WTG			(0x1 << 18)	/* WT gathering on */
+#define    UNIPHIER_SSCC_PRD			(0x1 << 17)	/* enable pre-fetch */
+#define    UNIPHIER_SSCC_ON			(0x1 <<  0)	/* enable cache */
+#define UNIPHIER_SSCLPDAWCR	0x500c0030	/* Unified/Data Active Way Control */
+#define UNIPHIER_SSCLPIAWCR	0x500c0034	/* Instruction Active Way Control */
+
+/* revision registers */
+#define UNIPHIER_SSCID		0x503c0100	/* ID Register */
+
+/* operation registers */
+#define UNIPHIER_SSCOPE		0x506c0244	/* Cache Operation Primitive Entry */
+#define    UNIPHIER_SSCOPE_CM_INV		0x0	/* invalidate */
+#define    UNIPHIER_SSCOPE_CM_CLEAN		0x1	/* clean */
+#define    UNIPHIER_SSCOPE_CM_FLUSH		0x2	/* flush */
+#define    UNIPHIER_SSCOPE_CM_SYNC		0x8	/* sync (drain bufs) */
+#define    UNIPHIER_SSCOPE_CM_FLUSH_PREFETCH	0x9	/* flush p-fetch buf */
+#define UNIPHIER_SSCOQM		0x506c0248
+#define    UNIPHIER_SSCOQM_TID_MASK		(0x3 << 21)
+#define    UNIPHIER_SSCOQM_TID_LRU_DATA		(0x0 << 21)
+#define    UNIPHIER_SSCOQM_TID_LRU_INST		(0x1 << 21)
+#define    UNIPHIER_SSCOQM_TID_WAY		(0x2 << 21)
+#define    UNIPHIER_SSCOQM_S_MASK		(0x3 << 17)
+#define    UNIPHIER_SSCOQM_S_RANGE		(0x0 << 17)
+#define    UNIPHIER_SSCOQM_S_ALL		(0x1 << 17)
+#define    UNIPHIER_SSCOQM_S_WAY		(0x2 << 17)
+#define    UNIPHIER_SSCOQM_CE			(0x1 << 15)	/* notify completion */
+#define    UNIPHIER_SSCOQM_CW			(0x1 << 14)
+#define    UNIPHIER_SSCOQM_CM_MASK		(0x7)
+#define    UNIPHIER_SSCOQM_CM_INV		0x0	/* invalidate */
+#define    UNIPHIER_SSCOQM_CM_CLEAN		0x1	/* clean */
+#define    UNIPHIER_SSCOQM_CM_FLUSH		0x2	/* flush */
+#define    UNIPHIER_SSCOQM_CM_PREFETCH		0x3	/* prefetch to cache */
+#define    UNIPHIER_SSCOQM_CM_PREFETCH_BUF	0x4	/* prefetch to pf-buf */
+#define    UNIPHIER_SSCOQM_CM_TOUCH		0x5	/* touch */
+#define    UNIPHIER_SSCOQM_CM_TOUCH_ZERO	0x6	/* touch to zero */
+#define    UNIPHIER_SSCOQM_CM_TOUCH_DIRTY	0x7	/* touch with dirty */
+#define UNIPHIER_SSCOQAD	0x506c024c	/* Cache Operation Queue Address */
+#define UNIPHIER_SSCOQSZ	0x506c0250	/* Cache Operation Queue Size */
+#define UNIPHIER_SSCOQMASK	0x506c0254	/* Cache Operation Queue Address Mask */
+#define UNIPHIER_SSCOQWN	0x506c0258	/* Cache Operation Queue Way Number */
+#define UNIPHIER_SSCOPPQSEF	0x506c025c	/* Cache Operation Queue Set Complete */
+#define    UNIPHIER_SSCOPPQSEF_FE		(0x1 << 1)
+#define    UNIPHIER_SSCOPPQSEF_OE		(0x1 << 0)
+#define UNIPHIER_SSCOLPQS	0x506c0260	/* Cache Operation Queue Status */
+#define    UNIPHIER_SSCOLPQS_EF			(0x1 << 2)
+#define    UNIPHIER_SSCOLPQS_EST		(0x1 << 1)
+#define    UNIPHIER_SSCOLPQS_QST		(0x1 << 0)
+
+#define UNIPHIER_SSC_LINE_SIZE		128
+#define UNIPHIER_SSC_RANGE_OP_MAX_SIZE	(0x00400000 - (UNIPHIER_SSC_LINE_SIZE))
+
+#define UNIPHIER_SSCOQAD_IS_NEEDED(op) \
+		((op & UNIPHIER_SSCOQM_S_MASK) == UNIPHIER_SSCOQM_S_RANGE)
+#define UNIPHIER_SSCOQWM_IS_NEEDED(op) \
+		(((op & UNIPHIER_SSCOQM_S_MASK) == UNIPHIER_SSCOQM_S_WAY) || \
+		 ((op & UNIPHIER_SSCOQM_TID_MASK) == UNIPHIER_SSCOQM_TID_WAY))
+
+/* uniphier_cache_sync - perform a sync point for a particular cache level */
 static void uniphier_cache_sync(void)
 {
 	/* drain internal buffers */
@@ -21,62 +84,56 @@ static void uniphier_cache_sync(void)
 	readl(UNIPHIER_SSCOPE);
 }
 
-static void uniphier_cache_maint_all(u32 operation)
+/**
+ * uniphier_cache_maint_common - run a queue operation
+ *
+ * @start: start address of range operation (don't care for "all" operation)
+ * @size: data size of range operation (don't care for "all" operation)
+ * @ways: target ways (don't care for operations other than pre-fetch, touch
+ * @operation: flags to specify the desired cache operation
+ */
+static void uniphier_cache_maint_common(u32 start, u32 size, u32 ways,
+					u32 operation)
 {
 	/* clear the complete notification flag */
 	writel(UNIPHIER_SSCOLPQS_EF, UNIPHIER_SSCOLPQS);
 
-	/* try until the command is successfully set */
 	do {
-		writel(UNIPHIER_SSCOQM_S_ALL | UNIPHIER_SSCOQM_CE | operation,
-		       UNIPHIER_SSCOQM);
-	} while (readl(UNIPHIER_SSCOPPQSEF) &
-		 (UNIPHIER_SSCOPPQSEF_FE | UNIPHIER_SSCOPPQSEF_OE));
+		/* set cache operation */
+		writel(UNIPHIER_SSCOQM_CE | operation, UNIPHIER_SSCOQM);
 
-	/* wait until the operation is completed */
-	while (readl(UNIPHIER_SSCOLPQS) != UNIPHIER_SSCOLPQS_EF)
-		;
+		/* set address range if needed */
+		if (likely(UNIPHIER_SSCOQAD_IS_NEEDED(operation))) {
+			writel(start, UNIPHIER_SSCOQAD);
+			writel(size, UNIPHIER_SSCOQSZ);
+		}
 
-	uniphier_cache_sync();
-}
+		/* set target ways if needed */
+		if (unlikely(UNIPHIER_SSCOQWM_IS_NEEDED(operation)))
+			writel(ways, UNIPHIER_SSCOQWN);
+	} while (unlikely(readl(UNIPHIER_SSCOPPQSEF) &
+			  (UNIPHIER_SSCOPPQSEF_FE | UNIPHIER_SSCOPPQSEF_OE)));
 
-void v7_outer_cache_flush_all(void)
-{
-	uniphier_cache_maint_all(UNIPHIER_SSCOQM_CM_FLUSH);
-}
-
-void v7_outer_cache_inval_all(void)
-{
-	uniphier_cache_maint_all(UNIPHIER_SSCOQM_CM_INV);
+	/* wait until the operation is completed */
+	while (likely(readl(UNIPHIER_SSCOLPQS) != UNIPHIER_SSCOLPQS_EF))
+		cpu_relax();
 }
 
-static void __uniphier_cache_maint_range(u32 start, u32 size, u32 operation)
+static void uniphier_cache_maint_all(u32 operation)
 {
-	/* clear the complete notification flag */
-	writel(UNIPHIER_SSCOLPQS_EF, UNIPHIER_SSCOLPQS);
-
-	/* try until the command is successfully set */
-	do {
-		writel(UNIPHIER_SSCOQM_S_RANGE | UNIPHIER_SSCOQM_CE | operation,
-		       UNIPHIER_SSCOQM);
-		writel(start, UNIPHIER_SSCOQAD);
-		writel(size, UNIPHIER_SSCOQSZ);
+	uniphier_cache_maint_common(0, 0, 0, UNIPHIER_SSCOQM_S_ALL | operation);
 
-	} while (readl(UNIPHIER_SSCOPPQSEF) &
-		 (UNIPHIER_SSCOPPQSEF_FE | UNIPHIER_SSCOPPQSEF_OE));
-
-	/* wait until the operation is completed */
-	while (readl(UNIPHIER_SSCOLPQS) != UNIPHIER_SSCOLPQS_EF)
-		;
+	uniphier_cache_sync();
 }
 
-static void uniphier_cache_maint_range(u32 start, u32 end, u32 operation)
+static void uniphier_cache_maint_range(u32 start, u32 end, u32 ways,
+				       u32 operation)
 {
 	u32 size;
 
 	/*
-	 * If start address is not aligned to cache-line,
-	 * do cache operation for the first cache-line
+	 * If the start address is not aligned,
+	 * perform a cache operation for the first cache-line
 	 */
 	start = start & ~(UNIPHIER_SSC_LINE_SIZE - 1);
 
@@ -89,15 +146,16 @@ static void uniphier_cache_maint_range(u32 start, u32 end, u32 operation)
 	}
 
 	/*
-	 * If end address is not aligned to cache-line,
-	 * do cache operation for the last cache-line
+	 * If the end address is not aligned,
+	 * perform a cache operation for the last cache-line
 	 */
 	size = ALIGN(size, UNIPHIER_SSC_LINE_SIZE);
 
 	while (size) {
-		u32 chunk_size = size > UNIPHIER_SSC_RANGE_OP_MAX_SIZE ?
-					UNIPHIER_SSC_RANGE_OP_MAX_SIZE : size;
-		__uniphier_cache_maint_range(start, chunk_size, operation);
+		u32 chunk_size = min_t(u32, size, UNIPHIER_SSC_RANGE_OP_MAX_SIZE);
+
+		uniphier_cache_maint_common(start, chunk_size, ways,
+					    UNIPHIER_SSCOQM_S_RANGE | operation);
 
 		start += chunk_size;
 		size -= chunk_size;
@@ -106,17 +164,98 @@ static void uniphier_cache_maint_range(u32 start, u32 end, u32 operation)
 	uniphier_cache_sync();
 }
 
+void uniphier_cache_prefetch_range(u32 start, u32 end, u32 ways)
+{
+	uniphier_cache_maint_range(start, end, ways,
+				   UNIPHIER_SSCOQM_TID_WAY |
+				   UNIPHIER_SSCOQM_CM_PREFETCH);
+}
+
+void uniphier_cache_touch_range(u32 start, u32 end, u32 ways)
+{
+	uniphier_cache_maint_range(start, end, ways,
+				   UNIPHIER_SSCOQM_TID_WAY |
+				   UNIPHIER_SSCOQM_CM_TOUCH);
+}
+
+void uniphier_cache_touch_zero_range(u32 start, u32 end, u32 ways)
+{
+	uniphier_cache_maint_range(start, end, ways,
+				   UNIPHIER_SSCOQM_TID_WAY |
+				   UNIPHIER_SSCOQM_CM_TOUCH_ZERO);
+}
+
+void uniphier_cache_inv_way(u32 ways)
+{
+	uniphier_cache_maint_common(0, 0, ways,
+				    UNIPHIER_SSCOQM_S_WAY |
+				    UNIPHIER_SSCOQM_CM_INV);
+}
+
+void uniphier_cache_set_active_ways(int cpu, u32 active_ways)
+{
+	void __iomem *base = (void __iomem *)UNIPHIER_SSCC + 0xc00;
+
+	switch (readl(UNIPHIER_SSCID)) { /* revision */
+	case 0x11:	/* sLD3 */
+		base = (void __iomem *)UNIPHIER_SSCC + 0x870;
+		break;
+	case 0x12:	/* LD4 */
+	case 0x16:	/* sld8 */
+		base = (void __iomem *)UNIPHIER_SSCC + 0x840;
+		break;
+	default:
+		base = (void __iomem *)UNIPHIER_SSCC + 0xc00;
+		break;
+	}
+
+	writel(active_ways, base + 4 * cpu);
+}
+
+static void uniphier_cache_endisable(int enable)
+{
+	u32 tmp;
+
+	tmp = readl(UNIPHIER_SSCC);
+	if (enable)
+		tmp |= UNIPHIER_SSCC_ON;
+	else
+		tmp &= ~UNIPHIER_SSCC_ON;
+	writel(tmp, UNIPHIER_SSCC);
+}
+
+void uniphier_cache_enable(void)
+{
+	uniphier_cache_endisable(1);
+}
+
+void uniphier_cache_disable(void)
+{
+	uniphier_cache_endisable(0);
+}
+
+#ifdef CONFIG_CACHE_UNIPHIER
+void v7_outer_cache_flush_all(void)
+{
+	uniphier_cache_maint_all(UNIPHIER_SSCOQM_CM_FLUSH);
+}
+
+void v7_outer_cache_inval_all(void)
+{
+	uniphier_cache_maint_all(UNIPHIER_SSCOQM_CM_INV);
+}
+
 void v7_outer_cache_flush_range(u32 start, u32 end)
 {
-	uniphier_cache_maint_range(start, end, UNIPHIER_SSCOQM_CM_FLUSH);
+	uniphier_cache_maint_range(start, end, 0, UNIPHIER_SSCOQM_CM_FLUSH);
 }
 
 void v7_outer_cache_inval_range(u32 start, u32 end)
 {
 	if (start & (UNIPHIER_SSC_LINE_SIZE - 1)) {
 		start &= ~(UNIPHIER_SSC_LINE_SIZE - 1);
-		__uniphier_cache_maint_range(start, UNIPHIER_SSC_LINE_SIZE,
-					     UNIPHIER_SSCOQM_CM_FLUSH);
+		uniphier_cache_maint_range(start, UNIPHIER_SSC_LINE_SIZE, 0,
+					   UNIPHIER_SSCOQM_CM_FLUSH);
 		start += UNIPHIER_SSC_LINE_SIZE;
 	}
 
@@ -127,8 +266,8 @@ void v7_outer_cache_inval_range(u32 start, u32 end)
 
 	if (end & (UNIPHIER_SSC_LINE_SIZE - 1)) {
 		end &= ~(UNIPHIER_SSC_LINE_SIZE - 1);
-		__uniphier_cache_maint_range(end, UNIPHIER_SSC_LINE_SIZE,
-					     UNIPHIER_SSCOQM_CM_FLUSH);
+		uniphier_cache_maint_range(end, UNIPHIER_SSC_LINE_SIZE, 0,
+					   UNIPHIER_SSCOQM_CM_FLUSH);
 	}
 
 	if (start >= end) {
@@ -136,28 +275,20 @@ void v7_outer_cache_inval_range(u32 start, u32 end)
 		return;
 	}
 
-	uniphier_cache_maint_range(start, end, UNIPHIER_SSCOQM_CM_INV);
+	uniphier_cache_maint_range(start, end, 0, UNIPHIER_SSCOQM_CM_INV);
 }
 
 void v7_outer_cache_enable(void)
 {
-	u32 tmp;
-
-	writel(U32_MAX, UNIPHIER_SSCLPDAWCR);	/* activate all ways */
-	tmp = readl(UNIPHIER_SSCC);
-	tmp |= UNIPHIER_SSCC_ON;
-	writel(tmp, UNIPHIER_SSCC);
+	uniphier_cache_set_active_ways(0, U32_MAX);	/* activate all ways */
+	uniphier_cache_enable();
 }
-#endif
 
 void v7_outer_cache_disable(void)
 {
-	u32 tmp;
-
-	tmp = readl(UNIPHIER_SSCC);
-	tmp &= ~UNIPHIER_SSCC_ON;
-	writel(tmp, UNIPHIER_SSCC);
+	uniphier_cache_disable();
 }
+#endif
 
 void enable_caches(void)
 {
diff --git a/arch/arm/mach-uniphier/arm32/cache-uniphier.h b/arch/arm/mach-uniphier/arm32/cache-uniphier.h
new file mode 100644
index 0000000000..493d66c189
--- /dev/null
+++ b/arch/arm/mach-uniphier/arm32/cache-uniphier.h
@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: Masahiro Yamada <yamada.masahiro@socionext.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef __CACHE_UNIPHIER_H
+#define __CACHE_UNIPHIER_H
+
+#include <linux/types.h>
+
+void uniphier_cache_prefetch_range(u32 start, u32 end, u32 ways);
+void uniphier_cache_touch_range(u32 start, u32 end, u32 ways);
+void uniphier_cache_touch_zero_range(u32 start, u32 end, u32 ways);
+void uniphier_cache_inv_way(u32 ways);
+void uniphier_cache_set_active_ways(int cpu, u32 active_ways);
+void uniphier_cache_enable(void);
+void uniphier_cache_disable(void);
+
+#endif /* __CACHE_UNIPHIER_H */
diff --git a/arch/arm/mach-uniphier/arm32/late_lowlevel_init.S b/arch/arm/mach-uniphier/arm32/late_lowlevel_init.S
index 001d732e39..6f40362cc9 100644
--- a/arch/arm/mach-uniphier/arm32/late_lowlevel_init.S
+++ b/arch/arm/mach-uniphier/arm32/late_lowlevel_init.S
@@ -5,14 +5,10 @@
  * SPDX-License-Identifier:	GPL-2.0+
  */
 
+#include <config.h>
 #include <linux/linkage.h>
 
-#include "ssc-regs.h"
-
 ENTRY(lowlevel_init)
-	ldr	r1, = UNIPHIER_SSCC
-	ldr	r0, [r1]
-	bic	r0, r0, #UNIPHIER_SSCC_ON	@ L2 disable
-	str	r0, [r1]
-	mov	pc, lr
+	ldr	sp, = CONFIG_SYS_INIT_SP_ADDR
+	b	uniphier_cache_disable
 ENDPROC(lowlevel_init)
diff --git a/arch/arm/mach-uniphier/arm32/lowlevel_init.S b/arch/arm/mach-uniphier/arm32/lowlevel_init.S
index 8e32b35723..af5ed1c050 100644
--- a/arch/arm/mach-uniphier/arm32/lowlevel_init.S
+++ b/arch/arm/mach-uniphier/arm32/lowlevel_init.S
@@ -11,8 +11,6 @@
 #include <linux/sizes.h>
 #include <asm/system.h>
 
-#include "ssc-regs.h"
-
 ENTRY(lowlevel_init)
 	mov	r8, lr			@ persevere link reg across call
 
@@ -27,10 +25,6 @@ ENTRY(lowlevel_init)
 	orr	r0, r0, #(CR_C | CR_M)	@ enable MMU and Dcache
 	mcr	p15, 0, r0, c1, c0, 0
 
-#ifdef CONFIG_DEBUG_LL
-	bl	debug_ll_init
-#endif
-
 	bl	setup_init_ram		@ RAM area for stack and page table
 
 	/*
@@ -49,6 +43,10 @@ ENTRY(lowlevel_init)
 
 	bl	enable_mmu
 
+#ifdef CONFIG_DEBUG_LL
+	bl	debug_ll_init
+#endif
+
 	mov	lr, r8			@ restore link
 	mov	pc, lr			@ back to my caller
 ENDPROC(lowlevel_init)
@@ -88,39 +86,42 @@ ENDPROC(enable_mmu)
  */
 #define BOOT_RAM_SIZE	(SZ_32K)
 #define BOOT_RAM_BASE	((CONFIG_SPL_STACK) - (BOOT_RAM_SIZE))
-#define BOOT_WAY_BITS	(0x00000100)   /* way 8 */
+#define BOOT_RAM_WAYS	(0x00000100)	@ way 8
+
+#define SSCO_BASE		0x506c0000
+#define SSCOPE			0x244
+#define SSCOQM			0x248
+#define SSCOQAD			0x24c
+#define SSCOQSZ			0x250
+#define SSCOQWN			0x258
+#define SSCOPPQSEF		0x25c
+#define SSCOLPQS		0x260
 
 ENTRY(setup_init_ram)
-	/*
-	 * Touch to zero for the boot way
-	 */
-0:
-	/*
-	 * set UNIPHIER_SSCOQM, UNIPHIER_SSCOQAD, UNIPHIER_SSCOQSZ, UNIPHIER_SSCOQWN in this order
-	 */
-	ldr	r0, = 0x00408006	@ touch to zero with address range
-	ldr	r1, = UNIPHIER_SSCOQM
-	str	r0, [r1]
+	ldr	r1, = SSCO_BASE
+	mrc	p15, 0, r0, c2, c0, 0	@ TTBR0
+	ldr	r0, [r0, #0x400]	@ entry for virtual address 0x100*****
+	bfc	r0, #0, #20
+	cmp	r0, #0x50000000		@ is sLD3 page table?
+	biceq	r1, r1, #0xc0000000	@ sLD3 ROM maps 0x5******* to 0x1*******
+
+	/* Touch to zero for the boot way */
+0:	ldr	r0, = 0x00408006	@ touch to zero with address range
+	str	r0, [r1, #SSCOQM]
 	ldr	r0, = BOOT_RAM_BASE
-	ldr	r1, = UNIPHIER_SSCOQAD
-	str	r0, [r1]
+	str	r0, [r1, #SSCOQAD]
 	ldr	r0, = BOOT_RAM_SIZE
-	ldr	r1, = UNIPHIER_SSCOQSZ
-	str	r0, [r1]
-	ldr	r0, = BOOT_WAY_BITS
-	ldr	r1, = UNIPHIER_SSCOQWN
-	str	r0, [r1]
-	ldr	r1, = UNIPHIER_SSCOPPQSEF
-	ldr	r0, [r1]
+	str	r0, [r1, #SSCOQSZ]
+	ldr	r0, = BOOT_RAM_WAYS
+	str	r0, [r1, #SSCOQWN]
+	ldr	r0, [r1, #SSCOPPQSEF]
 	cmp	r0, #0			@ check if the command is successfully set
 	bne	0b			@ try again if an error occurs
 
-	ldr	r1, = UNIPHIER_SSCOLPQS
-1:
-	ldr	r0, [r1]
+1:	ldr	r0, [r1, #SSCOLPQS]
 	cmp	r0, #0x4
 	bne	1b			@ wait until the operation is completed
-	str	r0, [r1]		@ clear the complete notification flag
+	str	r0, [r1, #SSCOLPQS]	@ clear the complete notification flag
 
 	mov	pc, lr
 ENDPROC(setup_init_ram)
diff --git a/arch/arm/mach-uniphier/arm32/psci.c b/arch/arm/mach-uniphier/arm32/psci.c
new file mode 100644
index 0000000000..633a3e0840
--- /dev/null
+++ b/arch/arm/mach-uniphier/arm32/psci.c
@@ -0,0 +1,153 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: Masahiro Yamada <yamada.masahiro@socionext.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <linux/bitops.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/psci.h>
+#include <linux/sizes.h>
+#include <asm/processor.h>
+#include <asm/psci.h>
+#include <asm/secure.h>
+
+#include "../debug.h"
+#include "../soc-info.h"
+#include "arm-mpcore.h"
+#include "cache-uniphier.h"
+
+#define UNIPHIER_SMPCTRL_ROM_RSV2	0x59801208
+
+void uniphier_smp_trampoline(void);
+void uniphier_smp_trampoline_end(void);
+u32 uniphier_smp_booted[CONFIG_ARMV7_PSCI_NR_CPUS];
+
+static int uniphier_get_nr_cpus(void)
+{
+	switch (uniphier_get_soc_type()) {
+	case SOC_UNIPHIER_SLD3:
+	case SOC_UNIPHIER_PRO4:
+	case SOC_UNIPHIER_PRO5:
+		return 2;
+	case SOC_UNIPHIER_PXS2:
+	case SOC_UNIPHIER_LD6B:
+		return 4;
+	default:
+		return 1;
+	}
+}
+
+static void uniphier_smp_kick_all_cpus(void)
+{
+	const u32 target_ways = BIT(0);
+	size_t trmp_size;
+	u32 trmp_src = (unsigned long)uniphier_smp_trampoline;
+	u32 trmp_src_end = (unsigned long)uniphier_smp_trampoline_end;
+	u32 trmp_dest, trmp_dest_end;
+	int nr_cpus, i;
+	int timeout = 1000;
+
+	nr_cpus = uniphier_get_nr_cpus();
+	if (nr_cpus == 1)
+		return;
+
+	for (i = 0; i < nr_cpus; i++)	/* lock ways for all CPUs */
+		uniphier_cache_set_active_ways(i, 0);
+	uniphier_cache_inv_way(target_ways);
+	uniphier_cache_enable();
+
+	/* copy trampoline code */
+	uniphier_cache_prefetch_range(trmp_src, trmp_src_end, target_ways);
+
+	trmp_size = trmp_src_end - trmp_src;
+
+	trmp_dest = trmp_src & (SZ_64K - 1);
+	trmp_dest += SZ_1M - SZ_64K * 2;
+
+	trmp_dest_end = trmp_dest + trmp_size;
+
+	uniphier_cache_touch_range(trmp_dest, trmp_dest_end, target_ways);
+
+	writel(trmp_dest, UNIPHIER_SMPCTRL_ROM_RSV2);
+
+	asm("dsb	ishst\n" /* Ensure the write to ROM_RSV2 is visible */
+	    "sev"); /* Bring up all secondary CPUs from Boot ROM into U-Boot */
+
+	while (--timeout) {
+		int all_booted = 1;
+
+		for (i = 1; i < nr_cpus; i++)
+			if (!uniphier_smp_booted[i])
+				all_booted = 0;
+		if (all_booted)
+			break;
+		udelay(1);
+
+		/* barrier here because uniphier_smp_booted[] may be updated */
+		cpu_relax();
+	}
+
+	if (!timeout)
+		printf("warning: some of secondary CPUs may not boot\n");
+
+	uniphier_cache_disable();
+}
+
+void psci_board_init(void)
+{
+	unsigned long scu_base;
+	u32 scu_ctrl, tmp;
+
+	asm("mrc p15, 4, %0, c15, c0, 0" : "=r" (scu_base));
+
+	scu_ctrl = readl(scu_base + 0x30);
+	if (!(scu_ctrl & 1))
+		writel(scu_ctrl | 0x1, scu_base + 0x30);
+
+	scu_ctrl = readl(scu_base + SCU_CTRL);
+	scu_ctrl |= SCU_ENABLE | SCU_STANDBY_ENABLE;
+	writel(scu_ctrl, scu_base + SCU_CTRL);
+
+	tmp = readl(scu_base + SCU_SNSAC);
+	tmp |= 0xfff;
+	writel(tmp, scu_base + SCU_SNSAC);
+
+	uniphier_smp_kick_all_cpus();
+}
+
+void psci_arch_init(void)
+{
+	u32 actlr;
+
+	asm("mrc p15, 0, %0, c1, c0, 1" : "=r" (actlr));
+	actlr |= 0x41;		/* set SMP and FW bits */
+	asm("mcr p15, 0, %0, c1, c0, 1" : : "r" (actlr));
+}
+
+u32 uniphier_psci_holding_pen_release __secure_data = 0xffffffff;
+
+int __secure psci_cpu_on(u32 function_id, u32 cpuid, u32 entry_point)
+{
+	u32 cpu = cpuid & 0xff;
+
+	debug_puts("[U-Boot PSCI]  psci_cpu_on: cpuid=");
+	debug_puth(cpuid);
+	debug_puts(", entry_point=");
+	debug_puth(entry_point);
+	debug_puts("\n");
+
+	psci_save_target_pc(cpu, entry_point);
+
+	/* We assume D-cache is off, so do not call flush_dcache() here */
+	uniphier_psci_holding_pen_release = cpu;
+
+	/* Send an event to wake up the secondary CPU. */
+	asm("dsb	ishst\n"
+	    "sev");
+
+	return PSCI_RET_SUCCESS;
+}
diff --git a/arch/arm/mach-uniphier/arm32/psci_smp.S b/arch/arm/mach-uniphier/arm32/psci_smp.S
new file mode 100644
index 0000000000..aa2fa5f3fc
--- /dev/null
+++ b/arch/arm/mach-uniphier/arm32/psci_smp.S
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: Masahiro Yamada <yamada.masahiro@socionext.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <linux/linkage.h>
+#include <asm/system.h>
+
+	.section ._secure.text, "ax"
+
+ENTRY(uniphier_smp_trampoline)
+	ldr	r0, 0f
+	mrc	p15, 0, r1, c1, c0, 0	@ SCTLR (System Control Register)
+	orr	r1, r1, #CR_I		@ Enable ICache
+	bic	r1, r1, #(CR_C | CR_M)	@ Disable MMU and Dcache
+	mcr	p15, 0, r1, c1, c0, 0
+
+	bx	r0
+0:	.word	uniphier_secondary_startup
+	.globl	uniphier_smp_trampoline_end
+uniphier_smp_trampoline_end:
+ENDPROC(uniphier_smp_trampoline)
+
+LENTRY(uniphier_secondary_startup)
+	mrc	p15, 0, r1, c0, c0, 5	@ MPIDR (Multiprocessor Affinity Reg)
+	and	r1, r1, #0xff
+
+	ldr	r2, =uniphier_smp_booted
+	mov	r0, #1
+	str	r0, [r2, r1, lsl #2]
+
+	ldr	r2, =uniphier_psci_holding_pen_release
+pen:	ldr	r0, [r2]
+	cmp	r0, r1
+	beq	psci_cpu_entry
+	wfe
+	b	pen
+ENDPROC(uniphier_secondary_startup)
diff --git a/arch/arm/mach-uniphier/arm32/ssc-regs.h b/arch/arm/mach-uniphier/arm32/ssc-regs.h
deleted file mode 100644
index 8f423e92da..0000000000
--- a/arch/arm/mach-uniphier/arm32/ssc-regs.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
- * UniPhier System Cache (L2 Cache) registers
- *
- * Copyright (C) 2011-2014 Panasonic Corporation
- * Copyright (C) 2016      Socionext Inc.
- *
- * SPDX-License-Identifier:	GPL-2.0+
- */
-
-#ifndef ARCH_SSC_REGS_H
-#define ARCH_SSC_REGS_H
-
-/* control registers */
-#define UNIPHIER_SSCC		0x500c0000	/* Control Register */
-#define    UNIPHIER_SSCC_BST			(0x1 << 20)	/* UCWG burst read */
-#define    UNIPHIER_SSCC_ACT			(0x1 << 19)	/* Inst-Data separate */
-#define    UNIPHIER_SSCC_WTG			(0x1 << 18)	/* WT gathering on */
-#define    UNIPHIER_SSCC_PRD			(0x1 << 17)	/* enable pre-fetch */
-#define    UNIPHIER_SSCC_ON			(0x1 <<  0)	/* enable cache */
-#define UNIPHIER_SSCLPDAWCR	0x500c0030	/* Unified/Data Active Way Control */
-#define UNIPHIER_SSCLPIAWCR	0x500c0034	/* Instruction Active Way Control */
-
-/* revision registers */
-#define UNIPHIER_SSCID		0x503c0100	/* ID Register */
-
-/* operation registers */
-#define UNIPHIER_SSCOPE		0x506c0244	/* Cache Operation Primitive Entry */
-#define    UNIPHIER_SSCOPE_CM_INV		0x0	/* invalidate */
-#define    UNIPHIER_SSCOPE_CM_CLEAN		0x1	/* clean */
-#define    UNIPHIER_SSCOPE_CM_FLUSH		0x2	/* flush */
-#define    UNIPHIER_SSCOPE_CM_SYNC		0x8	/* sync (drain bufs) */
-#define    UNIPHIER_SSCOPE_CM_FLUSH_PREFETCH	0x9	/* flush p-fetch buf */
-#define UNIPHIER_SSCOQM		0x506c0248
-#define    UNIPHIER_SSCOQM_TID_MASK		(0x3 << 21)
-#define    UNIPHIER_SSCOQM_TID_LRU_DATA		(0x0 << 21)
-#define    UNIPHIER_SSCOQM_TID_LRU_INST		(0x1 << 21)
-#define    UNIPHIER_SSCOQM_TID_WAY		(0x2 << 21)
-#define    UNIPHIER_SSCOQM_S_MASK		(0x3 << 17)
-#define    UNIPHIER_SSCOQM_S_RANGE		(0x0 << 17)
-#define    UNIPHIER_SSCOQM_S_ALL		(0x1 << 17)
-#define    UNIPHIER_SSCOQM_S_WAY		(0x2 << 17)
-#define    UNIPHIER_SSCOQM_CE			(0x1 << 15)	/* notify completion */
-#define    UNIPHIER_SSCOQM_CW			(0x1 << 14)
-#define    UNIPHIER_SSCOQM_CM_MASK		(0x7)
-#define    UNIPHIER_SSCOQM_CM_INV		0x0	/* invalidate */
-#define    UNIPHIER_SSCOQM_CM_CLEAN		0x1	/* clean */
-#define    UNIPHIER_SSCOQM_CM_FLUSH		0x2	/* flush */
-#define    UNIPHIER_SSCOQM_CM_PREFETCH		0x3	/* prefetch to cache */
-#define    UNIPHIER_SSCOQM_CM_PREFETCH_BUF	0x4	/* prefetch to pf-buf */
-#define    UNIPHIER_SSCOQM_CM_TOUCH		0x5	/* touch */
-#define    UNIPHIER_SSCOQM_CM_TOUCH_ZERO	0x6	/* touch to zero */
-#define    UNIPHIER_SSCOQM_CM_TOUCH_DIRTY	0x7	/* touch with dirty */
-#define UNIPHIER_SSCOQAD	0x506c024c	/* Cache Operation Queue Address */
-#define UNIPHIER_SSCOQSZ	0x506c0250	/* Cache Operation Queue Size */
-#define UNIPHIER_SSCOQMASK	0x506c0254	/* Cache Operation Queue Address Mask */
-#define UNIPHIER_SSCOQWN	0x506c0258	/* Cache Operation Queue Way Number */
-#define UNIPHIER_SSCOPPQSEF	0x506c025c	/* Cache Operation Queue Set Complete */
-#define    UNIPHIER_SSCOPPQSEF_FE		(0x1 << 1)
-#define    UNIPHIER_SSCOPPQSEF_OE		(0x1 << 0)
-#define UNIPHIER_SSCOLPQS	0x506c0260	/* Cache Operation Queue Status */
-#define    UNIPHIER_SSCOLPQS_EF			(0x1 << 2)
-#define    UNIPHIER_SSCOLPQS_EST		(0x1 << 1)
-#define    UNIPHIER_SSCOLPQS_QST		(0x1 << 0)
-
-#define UNIPHIER_SSC_LINE_SIZE		128
-#define UNIPHIER_SSC_RANGE_OP_MAX_SIZE	(0x00400000 - (UNIPHIER_SSC_LINE_SIZE))
-
-#endif  /* ARCH_SSC_REGS_H */
diff --git a/arch/arm/mach-uniphier/debug.h b/arch/arm/mach-uniphier/debug.h
new file mode 100644
index 0000000000..bc16f77b96
--- /dev/null
+++ b/arch/arm/mach-uniphier/debug.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2016 Socionext Inc.
+ *   Author: Masahiro Yamada <yamada.masahiro@socionext.com>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef __DEBUG_H__
+#define __DEBUG_H__
+
+#include <linux/io.h>
+#include <linux/serial_reg.h>
+
+#define DEBUG_UART_BASE		0x54006800
+#define UART_SHIFT 2
+
+#define UNIPHIER_UART_TX		0
+#define UNIPHIER_UART_LSR		(5 * 4)
+
+/* All functions are inline so that they can be called from .secure section. */
+
+#ifdef DEBUG
+static inline void debug_putc(int c)
+{
+	void __iomem *base = (void __iomem *)DEBUG_UART_BASE;
+
+	while (!(readl(base + UNIPHIER_UART_LSR) & UART_LSR_THRE))
+		;
+
+	writel(c, base + UNIPHIER_UART_TX);
+}
+
+static inline void debug_puts(const char *s)
+{
+	while (*s) {
+		if (*s == '\n')
+			debug_putc('\r');
+
+		debug_putc(*s++);
+	}
+}
+
+static inline void debug_puth(unsigned long val)
+{
+	int i;
+	unsigned char c;
+
+	for (i = 8; i--; ) {
+		c = ((val >> (i * 4)) & 0xf);
+		c += (c >= 10) ? 'a' - 10 : '0';
+		debug_putc(c);
+	}
+}
+#else
+static inline void debug_putc(int c)
+{
+}
+
+static inline void debug_puts(const char *s)
+{
+}
+
+static inline void debug_puth(unsigned long val)
+{
+}
+#endif
+
+#endif /* __DEBUG_H__ */
diff --git a/arch/arm/mach-uniphier/sbc/sbc-regs.h b/arch/arm/mach-uniphier/sbc/sbc-regs.h
index a5dca74a55..673fb75bca 100644
--- a/arch/arm/mach-uniphier/sbc/sbc-regs.h
+++ b/arch/arm/mach-uniphier/sbc/sbc-regs.h
@@ -1,7 +1,8 @@
 /*
  * UniPhier SBC (System Bus Controller) registers
  *
- * Copyright (C) 2011-2015 Masahiro Yamada <yamada.masahiro@socionext.com>
+ * Copyright (C) 2011-2014 Panasonic Corporation
+ * Copyright (C) 2015-2016 Socionext Inc.
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -75,7 +76,6 @@
 #define	SBCTRL74		(SBCTRL_BASE + 0x170)
 
 #define PC0CTRL				0x598000c0
-#define ROM_BOOT_ROMRSV2		0x59801208
 
 #ifndef __ASSEMBLY__
 #include <linux/io.h>
diff --git a/include/configs/uniphier.h b/include/configs/uniphier.h
index e485583f21..950c5353d2 100644
--- a/include/configs/uniphier.h
+++ b/include/configs/uniphier.h
@@ -1,5 +1,7 @@
 /*
- * Copyright (C) 2012-2015 Masahiro Yamada <yamada.masahiro@socionext.com>
+ * Copyright (C) 2012-2015 Panasonic Corporation
+ * Copyright (C) 2015-2016 Socionext Inc.
+ *   Author: Masahiro Yamada <yamada.masahiro@socionext.com>
  *
  * SPDX-License-Identifier:	GPL-2.0+
  */
@@ -9,6 +11,9 @@
 #ifndef __CONFIG_UNIPHIER_COMMON_H__
 #define __CONFIG_UNIPHIER_COMMON_H__
 
+#define CONFIG_ARMV7_PSCI
+#define CONFIG_ARMV7_PSCI_NR_CPUS   4
+
 #define CONFIG_SYS_EEPROM_PAGE_WRITE_DELAY_MS  10
 
 #define CONFIG_SMC911X
@@ -25,10 +30,11 @@
 /* #define CONFIG_SYS_ICACHE_OFF */
 /* #define CONFIG_SYS_DCACHE_OFF */
 
+#ifdef CONFIG_CACHE_UNIPHIER
+#define CONFIG_SYS_CACHELINE_SIZE	128
+#else
 #define CONFIG_SYS_CACHELINE_SIZE	32
-
-/* Comment out the following to disable L2 cache */
-#define CONFIG_UNIPHIER_L2CACHE_ON
+#endif
 
 #define CONFIG_DISPLAY_CPUINFO
 #define CONFIG_DISPLAY_BOARDINFO