Properly initialize Armada XP MP subsystem.

- correct setting of Auxiliary Control Register for MP mode
- correct setting of Auxiliarty Debug registers
- cleanup management of memory contains bootup code
- early initialization of Coherency Fabric (MP and not-MP mode)
- enable Snoop Filtering

Obtained from:	Semihalf

3 files changed, 117 insertions, 85 deletions

diff --git a/sys/arm/mv/armadaxp/armadaxp.c b/sys/arm/mv/armadaxp/armadaxp.c
index 60a9cc0..3bbeadd 100644
--- a/sys/arm/mv/armadaxp/armadaxp.c
+++ b/sys/arm/mv/armadaxp/armadaxp.c
@@ -51,6 +51,9 @@ __FBSDID("$FreeBSD$");
 				    (0x0F & (sar >> 24)))
 
 static uint32_t count_l2clk(void);
+void armadaxp_l2_init(void);
+void armadaxp_init_coher_fabric(void);
+int platform_get_ncpus(void);
 
 #define ARMADAXP_L2_BASE		(MV_BASE + 0x8000)
 #define ARMADAXP_L2_CTRL		0x100
@@ -77,7 +80,11 @@ static uint32_t count_l2clk(void);
 #define ARMADAXP_L2_FLUSH_PHYS		0x7F0
 #define ARMADAXP_L2_FLUSH_WAY		0x7FC
 
-#define COHER_FABRIC_CFU		0x228
+#define MV_COHERENCY_FABRIC_BASE	(MV_MBUS_BRIDGE_BASE + 0x200)
+#define COHER_FABRIC_CTRL		0x00
+#define COHER_FABRIC_CONF		0x04
+#define COHER_FABRIC_CFU		0x28
+#define COHER_FABRIC_CIB_CTRL		0x80
 
 /* XXX Make gpio driver optional and remove it */
 struct resource_spec mv_gpio_res[] = {
@@ -188,8 +195,46 @@ get_l2clk(void)
 	return (l2clk_freq);
 }
 
-void armadaxp_l2_init(void);
-void armadaxp_l2_idcache_inv_all(void);
+static uint32_t
+read_coher_fabric(uint32_t reg)
+{
+
+	return (bus_space_read_4(fdtbus_bs_tag, MV_COHERENCY_FABRIC_BASE, reg));
+}
+
+static void
+write_coher_fabric(uint32_t reg, uint32_t val)
+{
+
+	bus_space_write_4(fdtbus_bs_tag, MV_COHERENCY_FABRIC_BASE, reg, val);
+}
+
+int
+platform_get_ncpus(void)
+{
+#if !defined(SMP)
+	return (1);
+#else
+	return ((read_coher_fabric(COHER_FABRIC_CONF) & 0xf) + 1);
+#endif
+}
+
+void
+armadaxp_init_coher_fabric(void)
+{
+	uint32_t val, cpus, mask;
+
+	cpus = platform_get_ncpus();
+	mask = (1 << cpus) - 1;
+	val = read_coher_fabric(COHER_FABRIC_CTRL);
+	val |= (mask << 24);
+	write_coher_fabric(COHER_FABRIC_CTRL, val);
+
+	val = read_coher_fabric(COHER_FABRIC_CONF);
+	val |= (mask << 24);
+	val |= (1 << 15);
+	write_coher_fabric(COHER_FABRIC_CONF, val);
+}
 
 #define ALL_WAYS	0xffffffff
 
@@ -208,7 +253,7 @@ write_l2_cache(uint32_t reg, uint32_t val)
 	bus_space_write_4(fdtbus_bs_tag, ARMADAXP_L2_BASE, reg, val);
 }
 
-void
+static void
 armadaxp_l2_idcache_inv_all(void)
 {
 	write_l2_cache(ARMADAXP_L2_INV_WAY, ALL_WAYS);
@@ -233,11 +278,6 @@ armadaxp_l2_init(void)
 	/* Clear pending L2 interrupts */
 	write_l2_cache(ARMADAXP_L2_INT_CAUSE, 0x1ff);
 
-	/* Enable Cache and TLB maintenance broadcast */
-	__asm__ __volatile__ ("mrc p15, 1, %0, c15, c2, 0" : "=r"(reg));
-	reg |= (1 << 8);
-	__asm__ __volatile__ ("mcr p15, 1, %0, c15, c2, 0" : :"r"(reg));
-
 	/* Enable l2 cache */
 	reg = read_l2_cache(ARMADAXP_L2_CTRL);
 	write_l2_cache(ARMADAXP_L2_CTRL, reg | L2_ENABLE);
@@ -254,10 +294,14 @@ armadaxp_l2_init(void)
 	 * Enable Cache maintenance operation propagation in coherency fabric
 	 * Change point of coherency and point of unification to DRAM.
 	 */
-	reg = bus_space_read_4(fdtbus_bs_tag, MV_MBUS_BRIDGE_BASE,
-	    COHER_FABRIC_CFU);
+	reg = read_coher_fabric(COHER_FABRIC_CFU);
 	reg |= (1 << 17) | (1 << 18);
-	bus_space_write_4(fdtbus_bs_tag, MV_MBUS_BRIDGE_BASE, COHER_FABRIC_CFU,
-	    reg);
+	write_coher_fabric(COHER_FABRIC_CFU, reg);
+
+	/* Coherent IO Bridge initialization */
+	reg = read_coher_fabric(COHER_FABRIC_CIB_CTRL);
+	reg &= ~(7 << 16);
+	reg |= (7 << 16);
+	write_coher_fabric(COHER_FABRIC_CIB_CTRL, reg);
 }
 
diff --git a/sys/arm/mv/armadaxp/armadaxp_mp.c b/sys/arm/mv/armadaxp/armadaxp_mp.c
index ec7efb6..83332ba 100644
--- a/sys/arm/mv/armadaxp/armadaxp_mp.c
+++ b/sys/arm/mv/armadaxp/armadaxp_mp.c
@@ -33,21 +33,21 @@
 #include <sys/mutex.h>
 #include <sys/smp.h>
 
+#include <vm/vm.h>
+#include <vm/vm_kern.h>
+#include <vm/vm_extern.h>
+
 #include <machine/smp.h>
 #include <machine/fdt.h>
+#include <machine/armreg.h>
 
 #include <arm/mv/mvwin.h>
 
-static int platform_get_ncpus(void);
-
 #define MV_AXP_CPU_DIVCLK_BASE		(MV_BASE + 0x18700)
 #define CPU_DIVCLK_CTRL0		0x00
 #define CPU_DIVCLK_CTRL2_RATIO_FULL0	0x08
 #define CPU_DIVCLK_CTRL2_RATIO_FULL1	0x0c
-
-#define MV_COHERENCY_FABRIC_BASE	(MV_MBUS_BRIDGE_BASE + 0x200)
-#define COHER_FABRIC_CTRL		0x00
-#define COHER_FABRIC_CONF		0x04
+#define CPU_DIVCLK_MASK(x)		(~(0xff << (8 * (x))))
 
 #define CPU_PMU(x)			(MV_BASE + 0x22100 + (0x100 * (x)))
 #define CPU_PMU_BOOT			0x24
@@ -57,20 +57,8 @@ static int platform_get_ncpus(void);
 
 #define CPU_RESUME_CONTROL		(0x20988)
 
-/* Coherency Fabric registers */
-static uint32_t
-read_coher_fabric(uint32_t reg)
-{
-
-	return (bus_space_read_4(fdtbus_bs_tag, MV_COHERENCY_FABRIC_BASE, reg));
-}
-
-static void
-write_coher_fabric(uint32_t reg, uint32_t val)
-{
-
-	bus_space_write_4(fdtbus_bs_tag, MV_COHERENCY_FABRIC_BASE, reg, val);
-}
+void armadaxp_init_coher_fabric(void);
+int platform_get_ncpus(void);
 
 /* Coherency Fabric registers */
 static uint32_t
@@ -111,56 +99,58 @@ platform_mp_init_secondary(void)
 void mpentry(void);
 void mptramp(void);
 
-static void
-initialize_coherency_fabric(void)
-{
-	uint32_t val, cpus, mask;
-
-	cpus = platform_get_ncpus();
-	mask = (1 << cpus) - 1;
-	val = read_coher_fabric(COHER_FABRIC_CTRL);
-	val |= (mask << 24);
-	write_coher_fabric(COHER_FABRIC_CTRL, val);
-
-	val = read_coher_fabric(COHER_FABRIC_CONF);
-	val |= (mask << 24);
-	write_coher_fabric(COHER_FABRIC_CONF, val);
-}
 
 
 void
 platform_mp_start_ap(void)
 {
-	uint32_t reg, *ptr, cpu_num;
-
-	/* Copy boot code to SRAM */
-	*((unsigned int*)(0xf1020240)) = 0xffff0101;
-	*((unsigned int*)(0xf1008500)) = 0xffff0003;
-
-	pmap_kenter_nocache(0x880f0000, 0xffff0000);
-	reg = 0x880f0000;
-
-	for (ptr = (uint32_t *)mptramp; ptr < (uint32_t *)mpentry;
-	    ptr++, reg += 4)
-		*((uint32_t *)reg) = *ptr;
-
-	if (mp_ncpus > 1) {
-		reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL0);
-		reg &= 0x00ffffff;
-		reg |= 0x01000000;
-		write_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL0, reg);
-	}
-	if (mp_ncpus > 2) {
-		reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1);
-		reg &= 0xff00ffff;
-		reg |= 0x00010000;
-		write_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1, reg);
+	uint32_t reg, *src, *dst, cpu_num, div_val, cputype;
+	vm_offset_t smp_boot;
+	/*
+	 * Initialization procedure depends on core revision,
+	 * in this step CHIP ID is checked to choose proper procedure
+	 */
+	cputype = cpufunc_id();
+	cputype &= CPU_ID_CPU_MASK;
+
+	smp_boot = kmem_alloc_nofault(kernel_map, PAGE_SIZE);
+	pmap_kenter_nocache(smp_boot, 0xffff0000);
+	dst = (uint32_t *) smp_boot;
+
+	for (src = (uint32_t *)mptramp; src < (uint32_t *)mpentry;
+	    src++, dst++) {
+		*dst = *src;
 	}
-	if (mp_ncpus > 3) {
-		reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1);
-		reg &= 0x00ffffff;
-		reg |= 0x01000000;
-		write_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1, reg);
+	kmem_free(kernel_map, smp_boot, PAGE_SIZE);
+
+	if (cputype == CPU_ID_MV88SV584X_V7) {
+		/* Core rev A0 */
+		div_val = read_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1);
+		div_val &= 0x3f;
+
+		for (cpu_num = 1; cpu_num < mp_ncpus; cpu_num++ ) {
+			reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1);
+			reg &= CPU_DIVCLK_MASK(cpu_num);
+			reg |= div_val << (cpu_num * 8);
+			write_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1, reg);
+		}
+	} else {
+		/* Core rev Z1 */
+		div_val = 0x01;
+
+		if (mp_ncpus > 1) {
+			reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL0);
+			reg &= CPU_DIVCLK_MASK(3);
+			reg |= div_val << 24;
+			write_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL0, reg);
+		}
+
+		for (cpu_num = 2; cpu_num < mp_ncpus; cpu_num++ ) {
+			reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1);
+			reg &= CPU_DIVCLK_MASK(cpu_num);
+			reg |= div_val << (cpu_num * 8);
+			write_cpu_clkdiv(CPU_DIVCLK_CTRL2_RATIO_FULL1, reg);
+		}
 	}
 
 	reg = read_cpu_clkdiv(CPU_DIVCLK_CTRL0);
@@ -190,14 +180,7 @@ platform_mp_start_ap(void)
 	wmb();
 	DELAY(10);
 
-	initialize_coherency_fabric();
-}
-
-static int
-platform_get_ncpus(void)
-{
-
-	return ((read_coher_fabric(COHER_FABRIC_CONF) & 0xf) + 1);
+	armadaxp_init_coher_fabric();
 }
 
 void
diff --git a/sys/arm/mv/mv_machdep.c b/sys/arm/mv/mv_machdep.c
index a487e39..58d183e 100644
--- a/sys/arm/mv/mv_machdep.c
+++ b/sys/arm/mv/mv_machdep.c
@@ -62,6 +62,7 @@ __FBSDID("$FreeBSD$");
 
 static int platform_mpp_init(void);
 #if defined(SOC_MV_ARMADAXP)
+void armadaxp_init_coher_fabric(void);
 void armadaxp_l2_init(void);
 #endif
 
@@ -237,6 +238,10 @@ initarm_late_init(void)
 	write_cpu_ctrl(CPU_TIMERS_BASE + CPU_TIMER_CONTROL, 0);
 #endif
 #if defined(SOC_MV_ARMADAXP)
+#if !defined(SMP)
+	/* For SMP case it should be initialized after APs are booted */
+	armadaxp_init_coher_fabric();
+#endif
 	armadaxp_l2_init();
 #endif
 }