s390/smp: fix physical to logical CPU map for SMT

[sagit-ice-cold/kernel_xiaomi_msm8998.git] / arch / s390 / kernel / smp.c

diff --git a/arch/s390/kernel/smp.c b/arch/s390/kernel/smp.c
index 9062df5..f113fcd 100644 (file)
--- a/arch/s390/kernel/smp.c
+++ b/arch/s390/kernel/smp.c
@@ -200,6 +200,7 @@ static int pcpu_alloc_lowcore(struct pcpu *pcpu, int cpu)
        lc->panic_stack = panic_stack + PANIC_FRAME_OFFSET;
        lc->cpu_nr = cpu;
        lc->spinlock_lockval = arch_spin_lockval(cpu);
+       lc->br_r1_trampoline = 0x07f1;  /* br %r1 */
        if (MACHINE_HAS_VX)
                lc->vector_save_area_addr =
                        (unsigned long) &lc->vector_save_area;
@@ -250,7 +251,9 @@ static void pcpu_prepare_secondary(struct pcpu *pcpu, int cpu)
        __ctl_store(lc->cregs_save_area, 0, 15);
        save_access_regs((unsigned int *) lc->access_regs_save_area);
        memcpy(lc->stfle_fac_list, S390_lowcore.stfle_fac_list,
-              MAX_FACILITY_BIT/8);
+              sizeof(lc->stfle_fac_list));
+       memcpy(lc->alt_stfle_fac_list, S390_lowcore.alt_stfle_fac_list,
+              sizeof(lc->alt_stfle_fac_list));
 }
 
 static void pcpu_attach_task(struct pcpu *pcpu, struct task_struct *tsk)
@@ -299,6 +302,7 @@ static void pcpu_delegate(struct pcpu *pcpu, void (*func)(void *),
        mem_assign_absolute(lc->restart_fn, (unsigned long) func);
        mem_assign_absolute(lc->restart_data, (unsigned long) data);
        mem_assign_absolute(lc->restart_source, source_cpu);
+       __bpon();
        asm volatile(
                "0:     sigp    0,%0,%2 # sigp restart to target cpu\n"
                "       brc     2,0b    # busy, try again\n"
@@ -356,9 +360,13 @@ void smp_call_online_cpu(void (*func)(void *), void *data)
  */
 void smp_call_ipl_cpu(void (*func)(void *), void *data)
 {
+       struct _lowcore *lc = pcpu_devices->lowcore;
+
+       if (pcpu_devices[0].address == stap())
+               lc = &S390_lowcore;
+
        pcpu_delegate(&pcpu_devices[0], func, data,
-                     pcpu_devices->lowcore->panic_stack -
-                     PANIC_FRAME_OFFSET + PAGE_SIZE);
+                     lc->panic_stack - PANIC_FRAME_OFFSET + PAGE_SIZE);
 }
 
 int smp_find_processor_id(u16 address)
@@ -694,39 +702,67 @@ static struct sclp_core_info *smp_get_core_info(void)
 
 static int smp_add_present_cpu(int cpu);
 
-static int __smp_rescan_cpus(struct sclp_core_info *info, int sysfs_add)
+static int smp_add_core(struct sclp_core_entry *core, cpumask_t *avail,
+                       bool configured, bool early)
 {
        struct pcpu *pcpu;
-       cpumask_t avail;
-       int cpu, nr, i, j;
+       int cpu, nr, i;
        u16 address;
 
        nr = 0;
-       cpumask_xor(&avail, cpu_possible_mask, cpu_present_mask);
-       cpu = cpumask_first(&avail);
-       for (i = 0; (i < info->combined) && (cpu < nr_cpu_ids); i++) {
-               if (sclp.has_core_type && info->core[i].type != boot_core_type)
+       if (sclp.has_core_type && core->type != boot_core_type)
+               return nr;
+       cpu = cpumask_first(avail);
+       address = core->core_id << smp_cpu_mt_shift;
+       for (i = 0; (i <= smp_cpu_mtid) && (cpu < nr_cpu_ids); i++) {
+               if (pcpu_find_address(cpu_present_mask, address + i))
                        continue;
-               address = info->core[i].core_id << smp_cpu_mt_shift;
-               for (j = 0; j <= smp_cpu_mtid; j++) {
-                       if (pcpu_find_address(cpu_present_mask, address + j))
-                               continue;
-                       pcpu = pcpu_devices + cpu;
-                       pcpu->address = address + j;
-                       pcpu->state =
-                               (cpu >= info->configured*(smp_cpu_mtid + 1)) ?
-                               CPU_STATE_STANDBY : CPU_STATE_CONFIGURED;
-                       smp_cpu_set_polarization(cpu, POLARIZATION_UNKNOWN);
-                       set_cpu_present(cpu, true);
-                       if (sysfs_add && smp_add_present_cpu(cpu) != 0)
-                               set_cpu_present(cpu, false);
-                       else
-                               nr++;
-                       cpu = cpumask_next(cpu, &avail);
-                       if (cpu >= nr_cpu_ids)
+               pcpu = pcpu_devices + cpu;
+               pcpu->address = address + i;
+               if (configured)
+                       pcpu->state = CPU_STATE_CONFIGURED;
+               else
+                       pcpu->state = CPU_STATE_STANDBY;
+               smp_cpu_set_polarization(cpu, POLARIZATION_UNKNOWN);
+               set_cpu_present(cpu, true);
+               if (!early && smp_add_present_cpu(cpu) != 0)
+                       set_cpu_present(cpu, false);
+               else
+                       nr++;
+               cpumask_clear_cpu(cpu, avail);
+               cpu = cpumask_next(cpu, avail);
+       }
+       return nr;
+}
+
+static int __smp_rescan_cpus(struct sclp_core_info *info, bool early)
+{
+       struct sclp_core_entry *core;
+       cpumask_t avail;
+       bool configured;
+       u16 core_id;
+       int nr, i;
+
+       nr = 0;
+       cpumask_xor(&avail, cpu_possible_mask, cpu_present_mask);
+       /*
+        * Add IPL core first (which got logical CPU number 0) to make sure
+        * that all SMT threads get subsequent logical CPU numbers.
+        */
+       if (early) {
+               core_id = pcpu_devices[0].address >> smp_cpu_mt_shift;
+               for (i = 0; i < info->configured; i++) {
+                       core = &info->core[i];
+                       if (core->core_id == core_id) {
+                               nr += smp_add_core(core, &avail, true, early);
                                break;
+                       }
                }
        }
+       for (i = 0; i < info->combined; i++) {
+               configured = i < info->configured;
+               nr += smp_add_core(&info->core[i], &avail, configured, early);
+       }
        return nr;
 }
 
@@ -774,7 +810,7 @@ static void __init smp_detect_cpus(void)
 
        /* Add CPUs present at boot */
        get_online_cpus();
-       __smp_rescan_cpus(info, 0);
+       __smp_rescan_cpus(info, true);
        put_online_cpus();
        kfree(info);
 }
@@ -888,6 +924,7 @@ void __cpu_die(unsigned int cpu)
 void __noreturn cpu_die(void)
 {
        idle_task_exit();
+       __bpon();
        pcpu_sigp_retry(pcpu_devices + smp_processor_id(), SIGP_STOP, 0);
        for (;;) ;
 }
@@ -1131,7 +1168,7 @@ int __ref smp_rescan_cpus(void)
                return -ENOMEM;
        get_online_cpus();
        mutex_lock(&smp_cpu_state_mutex);
-       nr = __smp_rescan_cpus(info, 1);
+       nr = __smp_rescan_cpus(info, false);
        mutex_unlock(&smp_cpu_state_mutex);
        put_online_cpus();
        kfree(info);
@@ -1147,7 +1184,11 @@ static ssize_t __ref rescan_store(struct device *dev,
 {
        int rc;
 
+       rc = lock_device_hotplug_sysfs();
+       if (rc)
+               return rc;
        rc = smp_rescan_cpus();
+       unlock_device_hotplug();
        return rc ? rc : count;
 }
 static DEVICE_ATTR(rescan, 0200, NULL, rescan_store);