5 files changed, 48 insertions, 22 deletions

diff --git a/arch/mips/netlogic/common/irq.c b/arch/mips/netlogic/common/irq.c
index 5afc4b7fce0f..c100b9afa0ab 100644
--- a/arch/mips/netlogic/common/irq.c
+++ b/arch/mips/netlogic/common/irq.c
@@ -203,6 +203,8 @@ void nlm_set_pic_extra_ack(int node, int irq, void (*xack)(struct irq_data *))
 
         xirq = nlm_irq_to_xirq(node, irq);
         pic_data = irq_get_handler_data(xirq);
+        if (WARN_ON(!pic_data))
+                return;
         pic_data->extra_ack = xack;
 }
 
diff --git a/arch/mips/netlogic/common/reset.S b/arch/mips/netlogic/common/reset.S
index b231fe1e7a09..701c4bcb9e47 100644
--- a/arch/mips/netlogic/common/reset.S
+++ b/arch/mips/netlogic/common/reset.S
@@ -35,6 +35,7 @@
 
 #include <asm/asm.h>
 #include <asm/asm-offsets.h>
+#include <asm/cpu.h>
 #include <asm/cacheops.h>
 #include <asm/regdef.h>
 #include <asm/mipsregs.h>
@@ -74,13 +75,25 @@
 .endm
 
 /*
+ * Allow access to physical mem >64G by enabling ELPA in PAGEGRAIN
+ * register. This is needed before going to C code since the SP can
+ * in this region. Called from all HW threads.
+ */
+.macro xlp_early_mmu_init
+        mfc0    t0, CP0_PAGEMASK, 1
+        li      t1, (1 << 29)           /* ELPA bit */
+        or      t0, t1
+        mtc0    t0, CP0_PAGEMASK, 1
+.endm
+
+/*
  * L1D cache has to be flushed before enabling threads in XLP.
  * On XLP8xx/XLP3xx, we do a low level flush using processor control
  * registers. On XLPII CPUs, usual cache instructions work.
  */
 .macro  xlp_flush_l1_dcache
         mfc0    t0, CP0_EBASE, 0
-        andi    t0, t0, 0xff00
+        andi    t0, t0, PRID_IMP_MASK
         slt     t1, t0, 0x1200
         beqz    t1, 15f
         nop
@@ -159,11 +172,15 @@ FEXPORT(nlm_reset_entry)
 
 1:      /* Entry point on core wakeup */
         mfc0    t0, CP0_EBASE, 0        /* processor ID */
-        andi    t0, 0xff00
+        andi    t0, PRID_IMP_MASK
         li      t1, 0x1500              /* XLP 9xx */
         beq     t0, t1, 2f              /* does not need to set coherent */
         nop
 
+        li      t1, 0x1300              /* XLP 5xx */
+        beq     t0, t1, 2f              /* does not need to set coherent */
+        nop
+
         /* set bit in SYS coherent register for the core */
         mfc0    t0, CP0_EBASE, 1
         mfc0    t1, CP0_EBASE, 1
@@ -197,6 +214,9 @@ FEXPORT(nlm_reset_entry)
 EXPORT(nlm_boot_siblings)
         /* core L1D flush before enable threads */
         xlp_flush_l1_dcache
+        /* save ra and sp, will be used later (only for boot cpu) */
+        dmtc0   ra, $22, 6
+        dmtc0   sp, $22, 7
         /* Enable hw threads by writing to MAP_THREADMODE of the core */
         li      t0, CKSEG1ADDR(RESET_DATA_PHYS)
         lw      t1, BOOT_THREAD_MODE(t0)        /* t1 <- thread mode */
@@ -225,6 +245,8 @@ EXPORT(nlm_boot_siblings)
 #endif
         mtc0    t1, CP0_STATUS
 
+        xlp_early_mmu_init
+
         /* mark CPU ready */
         li      t3, CKSEG1ADDR(RESET_DATA_PHYS)
         ADDIU   t1, t3, BOOT_CPU_READY
@@ -238,14 +260,12 @@ EXPORT(nlm_boot_siblings)
         nop
 
         /*
-         * For the boot CPU, we have to restore registers and
-         * return
+         * For the boot CPU, we have to restore ra and sp and return, rest
+         * of the registers will be restored by the caller
          */
-4:      dmfc0   t0, $4, 2       /* restore SP from UserLocal */
-        li      t1, 0xfadebeef
-        dmtc0   t1, $4, 2       /* restore SP from UserLocal */
-        PTR_SUBU sp, t0, PT_SIZE
-        RESTORE_ALL
+4:
+        dmfc0   ra, $22, 6
+        dmfc0   sp, $22, 7
         jr      ra
         nop
 EXPORT(nlm_reset_entry_end)
@@ -253,6 +273,7 @@ EXPORT(nlm_reset_entry_end)
 LEAF(nlm_init_boot_cpu)
 #ifdef CONFIG_CPU_XLP
         xlp_config_lsu
+        xlp_early_mmu_init
 #endif
         jr      ra
         nop
diff --git a/arch/mips/netlogic/common/smp.c b/arch/mips/netlogic/common/smp.c
index 6baae15cc7b1..4fde7ac76cc9 100644
--- a/arch/mips/netlogic/common/smp.c
+++ b/arch/mips/netlogic/common/smp.c
@@ -135,10 +135,6 @@ void nlm_smp_finish(void)
         local_irq_enable();
 }
 
-void nlm_cpus_done(void)
-{
-}
-
 /*
  * Boot all other cpus in the system, initialize them, and bring them into
  * the boot function
@@ -198,7 +194,7 @@ void __init nlm_smp_setup(void)
         cpumask_scnprintf(buf, ARRAY_SIZE(buf), cpu_possible_mask);
         pr_info("Possible CPU mask: %s\n", buf);
 
-        /* check with the cores we have worken up */
+        /* 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);
 
@@ -213,6 +209,7 @@ static int nlm_parse_cpumask(cpumask_t *wakeup_mask)
 {
         uint32_t core0_thr_mask, core_thr_mask;
         int threadmode, i, j;
+        char buf[64];
 
         core0_thr_mask = 0;
         for (i = 0; i < NLM_THREADS_PER_CORE; i++)
@@ -247,8 +244,8 @@ static int nlm_parse_cpumask(cpumask_t *wakeup_mask)
         return threadmode;
 
 unsupp:
-        panic("Unsupported CPU mask %lx",
-                (unsigned long)cpumask_bits(wakeup_mask)[0]);
+        cpumask_scnprintf(buf, ARRAY_SIZE(buf), wakeup_mask);
+        panic("Unsupported CPU mask %s", buf);
         return 0;
 }
 
@@ -277,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,
diff --git a/arch/mips/netlogic/common/smpboot.S b/arch/mips/netlogic/common/smpboot.S
index 8597657c27fc..805355b0bd05 100644
--- a/arch/mips/netlogic/common/smpboot.S
+++ b/arch/mips/netlogic/common/smpboot.S
@@ -54,8 +54,9 @@
         .set    noat
         .set    arch=xlr                /* for mfcr/mtcr, XLR is sufficient */
 
-FEXPORT(xlp_boot_core0_siblings)        /* "Master" cpu starts from here */
-        dmtc0   sp, $4, 2               /* SP saved in UserLocal */
+/* Called by the boot cpu to wake up its sibling threads */
+NESTED(xlp_boot_core0_siblings, PT_SIZE, sp)
+        /* CPU register contents lost when enabling threads, save them first */
         SAVE_ALL
         sync
         /* find the location to which nlm_boot_siblings was relocated */
@@ -65,9 +66,12 @@ FEXPORT(xlp_boot_core0_siblings)	/* "Master" cpu starts from here */
         dsubu   t2, t1
         daddu   t2, t0
         /* call it */
-        jr      t2
+        jalr    t2
         nop
-        /* not reached */
+        RESTORE_ALL
+        jr      ra
+        nop
+END(xlp_boot_core0_siblings)
 
 NESTED(nlm_boot_secondary_cpus, 16, sp)
         /* Initialize CP0 Status */
diff --git a/arch/mips/netlogic/common/time.c b/arch/mips/netlogic/common/time.c
index 13391b8a6031..0c0a1a606f73 100644
--- a/arch/mips/netlogic/common/time.c
+++ b/arch/mips/netlogic/common/time.c
@@ -82,6 +82,7 @@ static struct clocksource csrc_pic = {
 static void nlm_init_pic_timer(void)
 {
         uint64_t picbase = nlm_get_node(0)->picbase;
+        u32 picfreq;
 
         nlm_pic_set_timer(picbase, PIC_CLOCK_TIMER, ~0ULL, 0, 0);
         if (current_cpu_data.cputype == CPU_XLR) {
@@ -92,7 +93,9 @@ static void nlm_init_pic_timer(void)
                 csrc_pic.read   = nlm_get_pic_timer;
         }
         csrc_pic.rating = 1000;
-        clocksource_register_hz(&csrc_pic, pic_timer_freq());
+        picfreq = pic_timer_freq();
+        clocksource_register_hz(&csrc_pic, picfreq);
+        pr_info("PIC clock source added, frequency %d\n", picfreq);
 }
 
 void __init plat_time_init(void)