1 files changed, 75 insertions, 65 deletions

diff --git a/arch/mips/netlogic/common/smp.c b/arch/mips/netlogic/common/smp.c
index db17f49886c..4fde7ac76cc 100644
--- a/arch/mips/netlogic/common/smp.c
+++ b/arch/mips/netlogic/common/smp.c
@@ -59,12 +59,17 @@
 
 void nlm_send_ipi_single(int logical_cpu, unsigned int action)
 {
-	int cpu = cpu_logical_map(logical_cpu);
+	int cpu, node;
+	uint64_t picbase;
+
+	cpu = cpu_logical_map(logical_cpu);
+	node = nlm_cpuid_to_node(cpu);
+	picbase = nlm_get_node(node)->picbase;
 
 	if (action & SMP_CALL_FUNCTION)
-		nlm_pic_send_ipi(nlm_pic_base, cpu, IRQ_IPI_SMP_FUNCTION, 0);
+		nlm_pic_send_ipi(picbase, cpu, IRQ_IPI_SMP_FUNCTION, 0);
 	if (action & SMP_RESCHEDULE_YOURSELF)
-		nlm_pic_send_ipi(nlm_pic_base, cpu, IRQ_IPI_SMP_RESCHEDULE, 0);
+		nlm_pic_send_ipi(picbase, cpu, IRQ_IPI_SMP_RESCHEDULE, 0);
 }
 
 void nlm_send_ipi_mask(const struct cpumask *mask, unsigned int action)
@@ -79,15 +84,19 @@ void nlm_send_ipi_mask(const struct cpumask *mask, unsigned int action)
 /* IRQ_IPI_SMP_FUNCTION Handler */
 void nlm_smp_function_ipi_handler(unsigned int irq, struct irq_desc *desc)
 {
-	write_c0_eirr(1ull << irq);
+	clear_c0_eimr(irq);
+	ack_c0_eirr(irq);
 	smp_call_function_interrupt();
+	set_c0_eimr(irq);
 }
 
 /* IRQ_IPI_SMP_RESCHEDULE  handler */
 void nlm_smp_resched_ipi_handler(unsigned int irq, struct irq_desc *desc)
 {
-	write_c0_eirr(1ull << irq);
+	clear_c0_eimr(irq);
+	ack_c0_eirr(irq);
 	scheduler_ipi();
+	set_c0_eimr(irq);
 }
 
 /*
@@ -96,20 +105,23 @@ void nlm_smp_resched_ipi_handler(unsigned int irq, struct irq_desc *desc)
 void nlm_early_init_secondary(int cpu)
 {
 	change_c0_config(CONF_CM_CMASK, 0x3);
-	write_c0_ebase((uint32_t)nlm_common_ebase);
 #ifdef CONFIG_CPU_XLP
-	if (hard_smp_processor_id() % 4 == 0)
-		xlp_mmu_init();
+	xlp_mmu_init();
 #endif
+	write_c0_ebase(nlm_current_node()->ebase);
 }
 
 /*
  * Code to run on secondary just after probing the CPU
  */
-static void __cpuinit nlm_init_secondary(void)
+static void nlm_init_secondary(void)
 {
-	current_cpu_data.core = hard_smp_processor_id() / 4;
-	nlm_smp_irq_init();
+	int hwtid;
+
+	hwtid = hard_smp_processor_id();
+	current_cpu_data.core = hwtid / NLM_THREADS_PER_CORE;
+	nlm_percpu_init(hwtid);
+	nlm_smp_irq_init(hwtid);
 }
 
 void nlm_prepare_cpus(unsigned int max_cpus)
@@ -120,82 +132,89 @@ void nlm_prepare_cpus(unsigned int max_cpus)
 
 void nlm_smp_finish(void)
 {
-#ifdef notyet
-	nlm_common_msgring_cpu_init();
-#endif
 	local_irq_enable();
 }
 
-void nlm_cpus_done(void)
-{
-}
-
 /*
  * Boot all other cpus in the system, initialize them, and bring them into
  * the boot function
  */
-int nlm_cpu_ready[NR_CPUS];
 unsigned long nlm_next_gp;
 unsigned long nlm_next_sp;
-
-cpumask_t phys_cpu_present_map;
+static cpumask_t phys_cpu_present_mask;
 
 void nlm_boot_secondary(int logical_cpu, struct task_struct *idle)
 {
-	unsigned long gp = (unsigned long)task_thread_info(idle);
-	unsigned long sp = (unsigned long)__KSTK_TOS(idle);
-	int cpu = cpu_logical_map(logical_cpu);
+	int cpu, node;
 
-	nlm_next_sp = sp;
-	nlm_next_gp = gp;
+	cpu = cpu_logical_map(logical_cpu);
+	node = nlm_cpuid_to_node(logical_cpu);
+	nlm_next_sp = (unsigned long)__KSTK_TOS(idle);
+	nlm_next_gp = (unsigned long)task_thread_info(idle);
 
-	/* barrier */
+	/* barrier for sp/gp store above */
 	__sync();
-	nlm_pic_send_ipi(nlm_pic_base, cpu, 1, 1);
+	nlm_pic_send_ipi(nlm_get_node(node)->picbase, cpu, 1, 1);  /* NMI */
 }
 
 void __init nlm_smp_setup(void)
 {
 	unsigned int boot_cpu;
-	int num_cpus, i;
+	int num_cpus, i, ncore, node;
+	volatile u32 *cpu_ready = nlm_get_boot_data(BOOT_CPU_READY);
+	char buf[64];
 
 	boot_cpu = hard_smp_processor_id();
-	cpus_clear(phys_cpu_present_map);
+	cpumask_clear(&phys_cpu_present_mask);
 
-	cpu_set(boot_cpu, phys_cpu_present_map);
+	cpumask_set_cpu(boot_cpu, &phys_cpu_present_mask);
 	__cpu_number_map[boot_cpu] = 0;
 	__cpu_logical_map[0] = boot_cpu;
-	cpu_set(0, cpu_possible_map);
+	set_cpu_possible(0, true);
 
 	num_cpus = 1;
 	for (i = 0; i < NR_CPUS; i++) {
 		/*
-		 * nlm_cpu_ready array is not set for the boot_cpu,
+		 * cpu_ready array is not set for the boot_cpu,
 		 * it is only set for ASPs (see smpboot.S)
 		 */
-		if (nlm_cpu_ready[i]) {
-			cpu_set(i, phys_cpu_present_map);
+		if (cpu_ready[i]) {
+			cpumask_set_cpu(i, &phys_cpu_present_mask);
 			__cpu_number_map[i] = num_cpus;
 			__cpu_logical_map[num_cpus] = i;
-			cpu_set(num_cpus, cpu_possible_map);
+			set_cpu_possible(num_cpus, true);
+			node = nlm_cpuid_to_node(i);
+			cpumask_set_cpu(num_cpus, &nlm_get_node(node)->cpumask);
 			++num_cpus;
 		}
 	}
 
-	pr_info("Phys CPU present map: %lx, possible map %lx\n",
-		(unsigned long)phys_cpu_present_map.bits[0],
-		(unsigned long)cpu_possible_map.bits[0]);
+	cpumask_scnprintf(buf, ARRAY_SIZE(buf), &phys_cpu_present_mask);
+	pr_info("Physical CPU mask: %s\n", buf);
+	cpumask_scnprintf(buf, ARRAY_SIZE(buf), cpu_possible_mask);
+	pr_info("Possible CPU mask: %s\n", buf);
+
+	/* check with the cores we have woken up */
+	for (ncore = 0, i = 0; i < NLM_NR_NODES; i++)
+		ncore += hweight32(nlm_get_node(i)->coremask);
+
+	pr_info("Detected (%dc%dt) %d Slave CPU(s)\n", ncore,
+		nlm_threads_per_core, num_cpus);
 
-	pr_info("Detected %i Slave CPU(s)\n", num_cpus);
+	/* switch NMI handler to boot CPUs */
 	nlm_set_nmi_handler(nlm_boot_secondary_cpus);
 }
 
-static int nlm_parse_cpumask(u32 cpu_mask)
+static int nlm_parse_cpumask(cpumask_t *wakeup_mask)
 {
 	uint32_t core0_thr_mask, core_thr_mask;
-	int threadmode, i;
+	int threadmode, i, j;
+	char buf[64];
 
-	core0_thr_mask = cpu_mask & 0xf;
+	core0_thr_mask = 0;
+	for (i = 0; i < NLM_THREADS_PER_CORE; i++)
+		if (cpumask_test_cpu(i, wakeup_mask))
+			core0_thr_mask |= (1 << i);
 	switch (core0_thr_mask) {
 	case 1:
 		nlm_threads_per_core = 1;
@@ -214,41 +233,33 @@ static int nlm_parse_cpumask(u32 cpu_mask)
 	}
 
 	/* Verify other cores CPU masks */
-	nlm_coremask = 1;
-	nlm_cpumask = core0_thr_mask;
-	for (i = 1; i < 8; i++) {
-		core_thr_mask = (cpu_mask >> (i * 4)) & 0xf;
-		if (core_thr_mask) {
-			if (core_thr_mask != core0_thr_mask)
+	for (i = 0; i < NR_CPUS; i += NLM_THREADS_PER_CORE) {
+		core_thr_mask = 0;
+		for (j = 0; j < NLM_THREADS_PER_CORE; j++)
+			if (cpumask_test_cpu(i + j, wakeup_mask))
+				core_thr_mask |= (1 << j);
+		if (core_thr_mask != 0 && core_thr_mask != core0_thr_mask)
 				goto unsupp;
-			nlm_coremask |= 1 << i;
-			nlm_cpumask |= core0_thr_mask << (4 * i);
-		}
 	}
 	return threadmode;
 
 unsupp:
-	panic("Unsupported CPU mask %x\n", cpu_mask);
+	cpumask_scnprintf(buf, ARRAY_SIZE(buf), wakeup_mask);
+	panic("Unsupported CPU mask %s", buf);
 	return 0;
 }
 
-int __cpuinit nlm_wakeup_secondary_cpus(u32 wakeup_mask)
+int nlm_wakeup_secondary_cpus(void)
 {
-	unsigned long reset_vec;
-	char *reset_data;
+	u32 *reset_data;
 	int threadmode;
 
-	/* Update reset entry point with CPU init code */
-	reset_vec = CKSEG1ADDR(RESET_VEC_PHYS);
-	memcpy((void *)reset_vec, (void *)nlm_reset_entry,
-			(nlm_reset_entry_end - nlm_reset_entry));
-
 	/* verify the mask and setup core config variables */
-	threadmode = nlm_parse_cpumask(wakeup_mask);
+	threadmode = nlm_parse_cpumask(&nlm_cpumask);
 
 	/* Setup CPU init parameters */
-	reset_data = (char *)CKSEG1ADDR(RESET_DATA_PHYS);
-	*(int *)(reset_data + BOOT_THREAD_MODE) = threadmode;
+	reset_data = nlm_get_boot_data(BOOT_THREAD_MODE);
+	*reset_data = threadmode;
 
 #ifdef CONFIG_CPU_XLP
 	xlp_wakeup_secondary_cpus();
@@ -263,7 +274,6 @@ struct plat_smp_ops nlm_smp_ops = {
 	.send_ipi_mask		= nlm_send_ipi_mask,
 	.init_secondary		= nlm_init_secondary,
 	.smp_finish		= nlm_smp_finish,
-	.cpus_done		= nlm_cpus_done,
 	.boot_secondary		= nlm_boot_secondary,
 	.smp_setup		= nlm_smp_setup,
 	.prepare_cpus		= nlm_prepare_cpus,