Add platform support for QorIQ SoCs.

This includes the following changes:
* SMP kickoff for QorIQ (tested on P5020)
* Errata fixes for some silicon revisions
* Enables L2 (and L3 if available) caches
Obtained from:	Semihalf
Sponsored by:	Alex Perez/Inertial Computing

6 files changed, 432 insertions, 59 deletions

diff --git a/sys/powerpc/booke/booke_machdep.c b/sys/powerpc/booke/booke_machdep.c
index c98e301..f102677 100644
--- a/sys/powerpc/booke/booke_machdep.c
+++ b/sys/powerpc/booke/booke_machdep.c
@@ -316,8 +316,20 @@ booke_init(uint32_t arg1, uint32_t arg2)
 
 	ret = powerpc_init(dtbp, 0, 0, mdp);
 
-	/* Enable L1 caches */
+	/* Default to 32 byte cache line size. */
+	switch ((mfpvr()) >> 16) {
+	case FSL_E500mc:
+	case FSL_E5500:
+	case FSL_E6500:
+		cacheline_size = 64;
+		break;
+	}
+
+	/* Enable caches */
 	booke_enable_l1_cache();
+	booke_enable_l2_cache();
+
+	booke_enable_bpred();
 
 	return (ret);
 }
diff --git a/sys/powerpc/booke/locore.S b/sys/powerpc/booke/locore.S
index 817cc4f..2d6c66d 100644
--- a/sys/powerpc/booke/locore.S
+++ b/sys/powerpc/booke/locore.S
@@ -301,7 +301,7 @@ bp_ntlb1s:
 
 	.globl	bp_tlb1
 bp_tlb1:
-	.space	4 * 3 * 16
+	.space	4 * 3 * 64
 
 	.globl	bp_tlb1_end
 bp_tlb1_end:
@@ -731,6 +731,113 @@ ENTRY(icache_enable)
 	blr
 
 /*
+ * L2 cache disable/enable/inval sequences for E500mc.
+ */
+
+ENTRY(l2cache_inval)
+	mfspr	%r3, SPR_L2CSR0
+	oris	%r3, %r3, (L2CSR0_L2FI | L2CSR0_L2LFC)@h
+	ori	%r3, %r3, (L2CSR0_L2FI | L2CSR0_L2LFC)@l
+	isync
+	mtspr	SPR_L2CSR0, %r3
+	isync
+1:	mfspr   %r3, SPR_L2CSR0
+	andis.	%r3, %r3, L2CSR0_L2FI@h
+	bne	1b
+	blr
+
+ENTRY(l2cache_enable)
+	mfspr	%r3, SPR_L2CSR0
+	oris	%r3, %r3, (L2CSR0_L2E | L2CSR0_L2PE)@h
+	isync
+	mtspr	SPR_L2CSR0, %r3
+	isync
+	blr
+
+/*
+ * Branch predictor setup.
+ */
+ENTRY(bpred_enable)
+	mfspr	%r3, SPR_BUCSR
+	ori	%r3, %r3, BUCSR_BBFI
+	isync
+	mtspr	SPR_BUCSR, %r3
+	isync
+	ori	%r3, %r3, BUCSR_BPEN
+	isync
+	mtspr	SPR_BUCSR, %r3
+	isync
+	blr
+
+ENTRY(dataloss_erratum_access)
+	/* Lock two cache lines into I-Cache */
+	sync
+	mfspr	%r11, SPR_L1CSR1
+	rlwinm	%r11, %r11, 0, ~L1CSR1_ICUL
+	sync
+	isync
+	mtspr	SPR_L1CSR1, %r11
+	isync
+
+	mflr	%r9
+	bl	1f
+	.long 2f-.
+1:
+	mflr	%r5
+	lwz	%r8, 0(%r5)
+	mtlr	%r9
+	add	%r8, %r8, %r5
+	icbtls	0, 0, %r8
+	addi	%r9, %r8, 64
+
+	sync
+	mfspr	%r11, SPR_L1CSR1
+3:	andi.	%r11, %r11, L1CSR1_ICUL
+	bne	3b
+
+	icbtls	0, 0, %r9
+
+	sync
+	mfspr	%r11, SPR_L1CSR1
+3:	andi.	%r11, %r11, L1CSR1_ICUL
+	bne	3b
+
+	b	2f
+	.align	6
+	/* Inside a locked cacheline, wait a while, write, then wait a while */
+2:	sync
+
+	mfspr	%r5, TBR_TBL
+4:	addis	%r11, %r5, 0x100000@h	/* wait around one million timebase ticks */
+	mfspr	%r5, TBR_TBL
+	subf.	%r5, %r5, %r11
+	bgt	4b
+
+	stw	%r4, 0(%r3)
+
+	mfspr	%r5, TBR_TBL
+4:	addis	%r11, %r5, 0x100000@h	/* wait around one million timebase ticks */
+	mfspr	%r5, TBR_TBL
+	subf.	%r5, %r5, %r11
+	bgt	4b
+
+	sync
+
+	/*
+	 * Fill out the rest of this cache line and the next with nops,
+	 * to ensure that nothing outside the locked area will be
+	 * fetched due to a branch.
+	 */
+	.rept 19
+	nop
+	.endr
+
+	icblc	0, 0, %r8
+	icblc	0, 0, %r9
+
+	blr
+
+/*
  * int setfault()
  *
  * Similar to setjmp to setup for handling faults on accesses to user memory.
diff --git a/sys/powerpc/booke/machdep_e500.c b/sys/powerpc/booke/machdep_e500.c
index ab47f62..e0e6095 100644
--- a/sys/powerpc/booke/machdep_e500.c
+++ b/sys/powerpc/booke/machdep_e500.c
@@ -27,9 +27,15 @@
 #include <sys/cdefs.h>
 __FBSDID("$FreeBSD$");
 
+#include <sys/cdefs.h>
 #include <sys/types.h>
+#include <sys/param.h>
+#include <sys/proc.h>
 #include <sys/reboot.h>
 
+#include <vm/vm.h>
+#include <vm/pmap.h>
+
 #include <machine/machdep.h>
 
 #include <dev/fdt/fdt_common.h>
@@ -42,6 +48,7 @@ extern void icache_enable(void);
 extern void icache_inval(void);
 extern void l2cache_enable(void);
 extern void l2cache_inval(void);
+extern void bpred_enable(void);
 
 void
 booke_init_tlb(vm_paddr_t fdt_immr_pa)
@@ -79,7 +86,6 @@ booke_enable_l1_cache(void)
 		    (csr & L1CSR1_ICE) ? "en" : "dis");
 }
 
-#if 0
 void
 booke_enable_l2_cache(void)
 {
@@ -102,55 +108,18 @@ booke_enable_l2_cache(void)
 }
 
 void
-booke_enable_l3_cache(void)
+booke_enable_bpred(void)
 {
-	uint32_t csr, size, ver;
-
-	/* Enable L3 CoreNet Platform Cache (CPC) */
-	ver = SVR_VER(mfspr(SPR_SVR));
-	if (ver == SVR_P2041 || ver == SVR_P2041E || ver == SVR_P3041 ||
-	    ver == SVR_P3041E || ver == SVR_P5020 || ver == SVR_P5020E) {
-		csr = ccsr_read4(OCP85XX_CPC_CSR0);
-		if ((csr & OCP85XX_CPC_CSR0_CE) == 0) {
-			l3cache_inval();
-			l3cache_enable();
-		}
+	uint32_t csr;
 
-		csr = ccsr_read4(OCP85XX_CPC_CSR0);
-		if ((boothowto & RB_VERBOSE) != 0 ||
-		    (csr & OCP85XX_CPC_CSR0_CE) == 0) {
-			size = OCP85XX_CPC_CFG0_SZ_K(ccsr_read4(OCP85XX_CPC_CFG0));
-			printf("L3 Corenet Platform Cache: %d KB %sabled\n",
-			    size, (csr & OCP85XX_CPC_CSR0_CE) == 0 ?
-			    "dis" : "en");
-		}
-	}
+	bpred_enable();
+	csr = mfspr(SPR_BUCSR);
+	if ((boothowto & RB_VERBOSE) != 0 || (csr & BUCSR_BPEN) == 0)
+		printf("Branch Predictor %sabled\n",
+		    (csr & BUCSR_BPEN) ? "en" : "dis");
 }
 
 void
 booke_disable_l2_cache(void)
 {
 }
-
-static void
-l3cache_inval(void)
-{
-
-	/* Flash invalidate the CPC and clear all the locks */
-	ccsr_write4(OCP85XX_CPC_CSR0, OCP85XX_CPC_CSR0_FI |
-	    OCP85XX_CPC_CSR0_LFC);
-	while (ccsr_read4(OCP85XX_CPC_CSR0) & (OCP85XX_CPC_CSR0_FI |
-	    OCP85XX_CPC_CSR0_LFC))
-		;
-}
-
-static void
-l3cache_enable(void)
-{
-
-	ccsr_write4(OCP85XX_CPC_CSR0, OCP85XX_CPC_CSR0_CE |
-	    OCP85XX_CPC_CSR0_PE);
-	/* Read back to sync write */
-	ccsr_read4(OCP85XX_CPC_CSR0);
-}
-#endif
diff --git a/sys/powerpc/mpc85xx/mpc85xx.c b/sys/powerpc/mpc85xx/mpc85xx.c
index 6608b4d..717e98a 100644
--- a/sys/powerpc/mpc85xx/mpc85xx.c
+++ b/sys/powerpc/mpc85xx/mpc85xx.c
@@ -32,18 +32,26 @@ __FBSDID("$FreeBSD$");
 #include <sys/systm.h>
 #include <sys/lock.h>
 #include <sys/mutex.h>
+#include <sys/reboot.h>
 #include <sys/rman.h>
 
 #include <vm/vm.h>
 #include <vm/vm_param.h>
+#include <vm/pmap.h>
 
 #include <machine/cpu.h>
 #include <machine/cpufunc.h>
+#include <machine/machdep.h>
 #include <machine/pio.h>
 #include <machine/spr.h>
 
 #include <dev/fdt/fdt_common.h>
 
+#include <dev/fdt/fdt_common.h>
+#include <dev/ofw/ofw_bus.h>
+#include <dev/ofw/ofw_bus_subr.h>
+#include <dev/ofw/openfirm.h>
+
 #include <powerpc/mpc85xx/mpc85xx.h>
 
 
@@ -249,3 +257,166 @@ law_pci_target(struct resource *res, int *trgt_mem, int *trgt_io)
 	return (rv);
 }
 
+static void
+l3cache_inval(void)
+{
+
+	/* Flash invalidate the CPC and clear all the locks */
+	ccsr_write4(OCP85XX_CPC_CSR0, OCP85XX_CPC_CSR0_FI |
+	    OCP85XX_CPC_CSR0_LFC);
+	while (ccsr_read4(OCP85XX_CPC_CSR0) & (OCP85XX_CPC_CSR0_FI |
+	    OCP85XX_CPC_CSR0_LFC))
+		;
+}
+
+static void
+l3cache_enable(void)
+{
+
+	ccsr_write4(OCP85XX_CPC_CSR0, OCP85XX_CPC_CSR0_CE |
+	    OCP85XX_CPC_CSR0_PE);
+	/* Read back to sync write */
+	ccsr_read4(OCP85XX_CPC_CSR0);
+}
+
+void
+mpc85xx_enable_l3_cache(void)
+{
+	uint32_t csr, size, ver;
+
+	/* Enable L3 CoreNet Platform Cache (CPC) */
+	ver = SVR_VER(mfspr(SPR_SVR));
+	if (ver == SVR_P2041 || ver == SVR_P2041E || ver == SVR_P3041 ||
+	    ver == SVR_P3041E || ver == SVR_P5020 || ver == SVR_P5020E) {
+		csr = ccsr_read4(OCP85XX_CPC_CSR0);
+		if ((csr & OCP85XX_CPC_CSR0_CE) == 0) {
+			l3cache_inval();
+			l3cache_enable();
+		}
+
+		csr = ccsr_read4(OCP85XX_CPC_CSR0);
+		if ((boothowto & RB_VERBOSE) != 0 ||
+		    (csr & OCP85XX_CPC_CSR0_CE) == 0) {
+			size = OCP85XX_CPC_CFG0_SZ_K(ccsr_read4(OCP85XX_CPC_CFG0));
+			printf("L3 Corenet Platform Cache: %d KB %sabled\n",
+			    size, (csr & OCP85XX_CPC_CSR0_CE) == 0 ?
+			    "dis" : "en");
+		}
+	}
+}
+
+static void
+mpc85xx_dataloss_erratum_spr976(void)
+{
+	uint32_t svr = SVR_VER(mfspr(SPR_SVR));
+
+	/* Ignore whether it's the E variant */
+	svr &= ~0x8;
+
+	if (svr != SVR_P3041 && svr != SVR_P4040 &&
+	    svr != SVR_P4080 && svr != SVR_P5020)
+		return;
+
+	mb();
+	isync();
+	mtspr(976, (mfspr(976) & ~0x1f8) | 0x48);
+	isync();
+}
+
+static vm_offset_t
+mpc85xx_map_dcsr(void)
+{
+	phandle_t node;
+	u_long b, s;
+	int err;
+
+	/*
+	 * Try to access the dcsr node directly i.e. through /aliases/.
+	 */
+	if ((node = OF_finddevice("dcsr")) != -1)
+		if (fdt_is_compatible_strict(node, "fsl,dcsr"))
+			goto moveon;
+	/*
+	 * Find the node the long way.
+	 */
+	if ((node = OF_finddevice("/")) == -1)
+		return (ENXIO);
+
+	if ((node = ofw_bus_find_compatible(node, "fsl,dcsr")) == 0)
+		return (ENXIO);
+
+moveon:
+	err = fdt_get_range(node, 0, &b, &s);
+
+	if (err != 0)
+		return (err);
+
+#ifdef QORIQ_DPAA
+	law_enable(OCP85XX_TGTIF_DCSR, b, 0x400000);
+#endif
+	return pmap_early_io_map(b, 0x400000);
+}
+
+
+
+void
+mpc85xx_fix_errata(vm_offset_t va_ccsr)
+{
+	uint32_t svr = SVR_VER(mfspr(SPR_SVR));
+	vm_offset_t va_dcsr;
+
+	/* Ignore whether it's the E variant */
+	svr &= ~0x8;
+
+	if (svr != SVR_P3041 && svr != SVR_P4040 &&
+	    svr != SVR_P4080 && svr != SVR_P5020)
+		return;
+
+	if (mfmsr() & PSL_EE)
+		return;
+
+	/*
+	 * dcsr region need to be mapped thus patch can refer to.
+	 * Align dcsr right after ccsbar.
+	 */
+	va_dcsr = mpc85xx_map_dcsr();
+	if (va_dcsr == 0)
+		goto err;
+
+	/*
+	 * As A004510 errata specify, special purpose register 976
+	 * SPR976[56:60] = 6'b001001 must be set. e500mc core reference manual
+	 * does not document SPR976 register.
+	 */
+	mpc85xx_dataloss_erratum_spr976();
+
+	/*
+	 * Specific settings in the CCF and core platform cache (CPC)
+	 * are required to reconfigure the CoreNet coherency fabric.
+	 * The register settings that should be updated are described
+	 * in errata and relay on base address, offset and updated value.
+	 * Special conditions must be used to update these registers correctly.
+	 */
+	dataloss_erratum_access(va_dcsr + 0xb0e08, 0xe0201800);
+	dataloss_erratum_access(va_dcsr + 0xb0e18, 0xe0201800);
+	dataloss_erratum_access(va_dcsr + 0xb0e38, 0xe0400000);
+	dataloss_erratum_access(va_dcsr + 0xb0008, 0x00900000);
+	dataloss_erratum_access(va_dcsr + 0xb0e40, 0xe00a0000);
+
+	switch (svr) {
+	case SVR_P5020:
+		dataloss_erratum_access(va_ccsr + 0x18600, 0xc0000000);
+		break;
+	case SVR_P4040:
+	case SVR_P4080:
+		dataloss_erratum_access(va_ccsr + 0x18600, 0xff000000);
+		break;
+	case SVR_P3041:
+		dataloss_erratum_access(va_ccsr + 0x18600, 0xf0000000);
+	}
+	dataloss_erratum_access(va_ccsr + 0x10f00, 0x415e5000);
+	dataloss_erratum_access(va_ccsr + 0x11f00, 0x415e5000);
+
+err:
+	return;
+}
diff --git a/sys/powerpc/mpc85xx/mpc85xx.h b/sys/powerpc/mpc85xx/mpc85xx.h
index 31adc58..ac8b8ff 100644
--- a/sys/powerpc/mpc85xx/mpc85xx.h
+++ b/sys/powerpc/mpc85xx/mpc85xx.h
@@ -40,6 +40,25 @@ extern vm_offset_t		ccsrbar_va;
 #define	OCP85XX_CCSRBAR		(CCSRBAR_VA + 0x0)
 #define	OCP85XX_BPTR		(CCSRBAR_VA + 0x20)
 
+#define	OCP85XX_BSTRH		(CCSRBAR_VA + 0x20)
+#define	OCP85XX_BSTRL		(CCSRBAR_VA + 0x24)
+#define	OCP85XX_BSTAR		(CCSRBAR_VA + 0x28)
+
+#define	OCP85XX_COREDISR	(CCSRBAR_VA + 0xE0094)
+#define	OCP85XX_BRR		(CCSRBAR_VA + 0xE00E4)
+
+/*
+ * Run Control and Power Management registers
+ */
+#define CCSR_CTBENR		(CCSRBAR_VA + 0xE2084)
+#define CCSR_CTBCKSELR		(CCSRBAR_VA + 0xE208C)
+#define CCSR_CTBCHLTCR		(CCSRBAR_VA + 0xE2094)
+
+/*
+ * DDR Memory controller.
+ */
+#define	OCP85XX_DDR1_CS0_CONFIG		(CCSRBAR_VA + 0x8080)
+
 /*
  * E500 Coherency Module registers
  */
@@ -68,6 +87,7 @@ extern vm_offset_t		ccsrbar_va;
 #define	OCP85XX_TGTIF_RAM1	0x10
 #define	OCP85XX_TGTIF_RAM2	0x11
 #define	OCP85XX_TGTIF_BMAN	0x18
+#define	OCP85XX_TGTIF_DCSR	0x1D
 #define	OCP85XX_TGTIF_QMAN	0x3C
 #define	OCP85XX_TRGT_SHIFT	20
 #else
@@ -84,6 +104,20 @@ extern vm_offset_t		ccsrbar_va;
 #define OCP85XX_L2CTL		(CCSRBAR_VA + 0x20000)
 
 /*
+ * L3 CoreNet platform cache (CPC) registers
+ */
+#define	OCP85XX_CPC_CSR0		(CCSRBAR_VA + 0x10000)
+#define	  OCP85XX_CPC_CSR0_CE		  0x80000000
+#define	  OCP85XX_CPC_CSR0_PE		  0x40000000
+#define	  OCP85XX_CPC_CSR0_FI		  0x00200000
+#define	  OCP85XX_CPC_CSR0_WT		  0x00080000
+#define	  OCP85XX_CPC_CSR0_FL		  0x00000800
+#define	  OCP85XX_CPC_CSR0_LFC		  0x00000400
+#define	OCP85XX_CPC_CFG0		(CCSRBAR_VA + 0x10008)
+#define	  OCP85XX_CPC_CFG_SZ_MASK	  0x00003fff
+#define	  OCP85XX_CPC_CFG0_SZ_K(x)	  (((x) & OCP85XX_CPC_CFG_SZ_MASK) << 6)
+
+/*
  * Power-On Reset configuration
  */
 #define	OCP85XX_PORDEVSR	(CCSRBAR_VA + 0xe000c)
@@ -110,4 +144,8 @@ int law_pci_target(struct resource *, int *, int *);
 DECLARE_CLASS(mpc85xx_platform);
 int mpc85xx_attach(platform_t);
 
+void mpc85xx_enable_l3_cache(void);
+void mpc85xx_fix_errata(vm_offset_t);
+void dataloss_erratum_access(vm_offset_t, uint32_t);
+
 #endif /* _MPC85XX_H_ */
diff --git a/sys/powerpc/mpc85xx/platform_mpc85xx.c b/sys/powerpc/mpc85xx/platform_mpc85xx.c
index eb9577f..c84561d 100644
--- a/sys/powerpc/mpc85xx/platform_mpc85xx.c
+++ b/sys/powerpc/mpc85xx/platform_mpc85xx.c
@@ -24,6 +24,7 @@
  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include "opt_platform.h"
 #include <sys/cdefs.h>
 __FBSDID("$FreeBSD$");
 
@@ -38,6 +39,7 @@ __FBSDID("$FreeBSD$");
 #include <machine/bus.h>
 #include <machine/cpu.h>
 #include <machine/hid.h>
+#include <machine/machdep.h>
 #include <machine/platform.h>
 #include <machine/platformvar.h>
 #include <machine/smp.h>
@@ -175,6 +177,9 @@ mpc85xx_attach(platform_t plat)
 	}
 	ccsrbar_va = pmap_early_io_map(ccsrbar, ccsrsize);
 
+	mpc85xx_fix_errata(ccsrbar_va);
+	mpc85xx_enable_l3_cache();
+
 	/*
 	 * Clear local access windows. Skip DRAM entries, so we don't shoot
 	 * ourselves in the foot.
@@ -182,14 +187,14 @@ mpc85xx_attach(platform_t plat)
 	law_max = law_getmax();
 	for (i = 0; i < law_max; i++) {
 		sr = ccsr_read4(OCP85XX_LAWSR(i));
-		if ((sr & 0x80000000) == 0)
+		if ((sr & OCP85XX_ENA_MASK) == 0)
 			continue;
 		tgt = (sr & 0x01f00000) >> 20;
 		if (tgt == OCP85XX_TGTIF_RAM1 || tgt == OCP85XX_TGTIF_RAM2 ||
 		    tgt == OCP85XX_TGTIF_RAM_INTL)
 			continue;
 
-		ccsr_write4(OCP85XX_LAWSR(i), sr & 0x7fffffff);
+		ccsr_write4(OCP85XX_LAWSR(i), sr & OCP85XX_DIS_MASK);
 	}
 
 	return (0);
@@ -256,7 +261,11 @@ mpc85xx_timebase_freq(platform_t plat, struct cpuref *cpuref)
 	 * HID0[SEL_TBCLK] = 0
 	 */
 	if (freq != 0)
+#ifdef QORIQ_DPAA
+		ticks = freq / 32;
+#else
 		ticks = freq / 8;
+#endif
 
 out:
 	if (ticks <= 0)
@@ -309,17 +318,37 @@ mpc85xx_smp_start_cpu(platform_t plat, struct pcpu *pc)
 {
 #ifdef SMP
 	uint32_t *tlb1;
-	uint32_t bptr, eebpcr;
+	vm_paddr_t bptr;
+	uint32_t reg;
 	int i, timeout;
+	uintptr_t brr;
+	int cpuid;
+
+#ifdef QORIQ_DPAA
+	uint32_t tgt;
 
-	eebpcr = ccsr_read4(OCP85XX_EEBPCR);
-	if ((eebpcr & (1 << (pc->pc_cpuid + 24))) != 0) {
+	reg = ccsr_read4(OCP85XX_COREDISR);
+	cpuid = pc->pc_cpuid;
+
+	if ((reg & cpuid) != 0) {
+		printf("%s: CPU %d is disabled!\n", __func__, pc->pc_cpuid);
+		return (-1);
+	}
+
+	brr = OCP85XX_BRR;
+#else /* QORIQ_DPAA */
+	brr = OCP85XX_EEBPCR;
+	cpuid = pc->pc_cpuid + 24;
+#endif
+	reg = ccsr_read4(brr);
+	if ((reg & (1 << cpuid)) != 0) {
 		printf("SMP: CPU %d already out of hold-off state!\n",
 		    pc->pc_cpuid);
 		return (ENXIO);
 	}
 
 	ap_pcpu = pc;
+	__asm __volatile("msync; isync");
 
 	i = 0;
 	tlb1 = bp_tlb1;
@@ -335,24 +364,67 @@ mpc85xx_smp_start_cpu(platform_t plat, struct pcpu *pc)
 	if (i < bp_ntlb1s)
 		bp_ntlb1s = i;
 
+	/* Flush caches to have our changes hit DRAM. */
+	cpu_flush_dcache(__boot_page, 4096);
+
+	bptr = ((vm_paddr_t)(uintptr_t)__boot_page - KERNBASE) + kernload;
+	KASSERT((bptr & 0xfff) == 0,
+	    ("%s: boot page is not aligned (%#jx)", __func__, (uintmax_t)bptr));
+#ifdef QORIQ_DPAA
+
+	/*
+	 * Read DDR controller configuration to select proper BPTR target ID.
+	 *
+	 * On P5020 bit 29 of DDR1_CS0_CONFIG enables DDR controllers
+	 * interleaving. If this bit is set, we have to use
+	 * OCP85XX_TGTIF_RAM_INTL as BPTR target ID. On other QorIQ DPAA SoCs,
+	 * this bit is reserved and always 0.
+	 */
+
+	reg = ccsr_read4(OCP85XX_DDR1_CS0_CONFIG);
+	if (reg & (1 << 29))
+		tgt = OCP85XX_TGTIF_RAM_INTL;
+	else
+		tgt = OCP85XX_TGTIF_RAM1;
+
+	/*
+	 * Set BSTR to the physical address of the boot page
+	 */
+	ccsr_write4(OCP85XX_BSTRH, bptr >> 32);
+	ccsr_write4(OCP85XX_BSTRL, bptr);
+	ccsr_write4(OCP85XX_BSTAR, OCP85XX_ENA_MASK |
+	    (tgt << OCP85XX_TRGT_SHIFT) | (ffsl(PAGE_SIZE) - 2));
+
+	/* Read back OCP85XX_BSTAR to synchronize write */
+	ccsr_read4(OCP85XX_BSTAR);
+
+	/*
+	 * Enable and configure time base on new CPU.
+	 */
+
+	/* Set TB clock source to platform clock / 32 */
+	reg = ccsr_read4(CCSR_CTBCKSELR);
+	ccsr_write4(CCSR_CTBCKSELR, reg & ~(1 << pc->pc_cpuid));
+
+	/* Enable TB */
+	reg = ccsr_read4(CCSR_CTBENR);
+	ccsr_write4(CCSR_CTBENR, reg | (1 << pc->pc_cpuid));
+#else
+
 	/*
 	 * Set BPTR to the physical address of the boot page
 	 */
-	bptr = ((uint32_t)__boot_page - KERNBASE) + kernload;
-	KASSERT((bptr & 0xfff) == 0,
-	    ("%s: boot page is not aligned (%#x)", __func__, bptr));
 	bptr = (bptr >> 12) | 0x80000000u;
 	ccsr_write4(OCP85XX_BPTR, bptr);
 	__asm __volatile("isync; msync");
 
-	/* Flush caches to have our changes hit DRAM. */
-	cpu_flush_dcache(__boot_page, 4096);
+#endif /* QORIQ_DPAA */
 
 	/*
 	 * Release AP from hold-off state
 	 */
-	eebpcr |= (1 << (pc->pc_cpuid + 24));
-	ccsr_write4(OCP85XX_EEBPCR, eebpcr);
+	reg = ccsr_read4(brr);
+	ccsr_write4(brr, reg | (1 << cpuid));
 	__asm __volatile("isync; msync");
 
 	timeout = 500;
@@ -364,7 +436,11 @@ mpc85xx_smp_start_cpu(platform_t plat, struct pcpu *pc)
 	 * address (= 0xfffff000) isn't permanently remapped and thus not
 	 * usable otherwise.
 	 */
+#ifdef QORIQ_DPAA
+	ccsr_write4(OCP85XX_BSTAR, 0);
+#else
 	ccsr_write4(OCP85XX_BPTR, 0);
+#endif
 	__asm __volatile("isync; msync");
 
 	if (!pc->pc_awake)