Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 1 | /* |
| 2 | * Copyright (C) 2013 Imagination Technologies |
| 3 | * Author: Paul Burton <paul.burton@imgtec.com> |
| 4 | * |
| 5 | * This program is free software; you can redistribute it and/or modify it |
| 6 | * under the terms of the GNU General Public License as published by the |
| 7 | * Free Software Foundation; either version 2 of the License, or (at your |
| 8 | * option) any later version. |
| 9 | */ |
| 10 | |
Paul Burton | a8c2061 | 2015-09-22 11:12:14 -0700 | [diff] [blame] | 11 | #include <linux/delay.h> |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 12 | #include <linux/io.h> |
Andrew Bresticker | 4060bbe | 2014-10-20 12:03:53 -0700 | [diff] [blame] | 13 | #include <linux/irqchip/mips-gic.h> |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 14 | #include <linux/sched.h> |
| 15 | #include <linux/slab.h> |
| 16 | #include <linux/smp.h> |
| 17 | #include <linux/types.h> |
| 18 | |
Paul Burton | 0fc0708 | 2014-07-09 12:51:05 +0100 | [diff] [blame] | 19 | #include <asm/bcache.h> |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 20 | #include <asm/mips-cm.h> |
| 21 | #include <asm/mips-cpc.h> |
| 22 | #include <asm/mips_mt.h> |
| 23 | #include <asm/mipsregs.h> |
Paul Burton | 1d8f1f5 | 2014-04-14 14:13:57 +0100 | [diff] [blame] | 24 | #include <asm/pm-cps.h> |
Paul Burton | 0fc0708 | 2014-07-09 12:51:05 +0100 | [diff] [blame] | 25 | #include <asm/r4kcache.h> |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 26 | #include <asm/smp-cps.h> |
| 27 | #include <asm/time.h> |
| 28 | #include <asm/uasm.h> |
| 29 | |
Paul Burton | 6422a91 | 2016-02-03 03:15:34 +0000 | [diff] [blame] | 30 | static bool threads_disabled; |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 31 | static DECLARE_BITMAP(core_power, NR_CPUS); |
| 32 | |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 33 | struct core_boot_config *mips_cps_core_bootcfg; |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 34 | |
Paul Burton | 6422a91 | 2016-02-03 03:15:34 +0000 | [diff] [blame] | 35 | static int __init setup_nothreads(char *s) |
| 36 | { |
| 37 | threads_disabled = true; |
| 38 | return 0; |
| 39 | } |
| 40 | early_param("nothreads", setup_nothreads); |
| 41 | |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 42 | static unsigned core_vpe_count(unsigned core) |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 43 | { |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 44 | unsigned cfg; |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 45 | |
Paul Burton | 6422a91 | 2016-02-03 03:15:34 +0000 | [diff] [blame] | 46 | if (threads_disabled) |
| 47 | return 1; |
| 48 | |
Paul Burton | 5a3e7c0 | 2016-02-03 03:15:33 +0000 | [diff] [blame] | 49 | if ((!config_enabled(CONFIG_MIPS_MT_SMP) || !cpu_has_mipsmt) |
| 50 | && (!config_enabled(CONFIG_CPU_MIPSR6) || !cpu_has_vp)) |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 51 | return 1; |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 52 | |
Paul Burton | 4ede316 | 2015-09-22 11:12:17 -0700 | [diff] [blame] | 53 | mips_cm_lock_other(core, 0); |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 54 | cfg = read_gcr_co_config() & CM_GCR_Cx_CONFIG_PVPE_MSK; |
Paul Burton | 4ede316 | 2015-09-22 11:12:17 -0700 | [diff] [blame] | 55 | mips_cm_unlock_other(); |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 56 | return (cfg >> CM_GCR_Cx_CONFIG_PVPE_SHF) + 1; |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 57 | } |
| 58 | |
| 59 | static void __init cps_smp_setup(void) |
| 60 | { |
| 61 | unsigned int ncores, nvpes, core_vpes; |
Paul Burton | 5a3e7c0 | 2016-02-03 03:15:33 +0000 | [diff] [blame] | 62 | unsigned long core_entry; |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 63 | int c, v; |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 64 | |
| 65 | /* Detect & record VPE topology */ |
| 66 | ncores = mips_cm_numcores(); |
Paul Burton | 5a3e7c0 | 2016-02-03 03:15:33 +0000 | [diff] [blame] | 67 | pr_info("%s topology ", cpu_has_mips_r6 ? "VP" : "VPE"); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 68 | for (c = nvpes = 0; c < ncores; c++) { |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 69 | core_vpes = core_vpe_count(c); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 70 | pr_cont("%c%u", c ? ',' : '{', core_vpes); |
| 71 | |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 72 | /* Use the number of VPEs in core 0 for smp_num_siblings */ |
| 73 | if (!c) |
| 74 | smp_num_siblings = core_vpes; |
| 75 | |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 76 | for (v = 0; v < min_t(int, core_vpes, NR_CPUS - nvpes); v++) { |
| 77 | cpu_data[nvpes + v].core = c; |
Paul Burton | 5a3e7c0 | 2016-02-03 03:15:33 +0000 | [diff] [blame] | 78 | #if defined(CONFIG_MIPS_MT_SMP) || defined(CONFIG_CPU_MIPSR6) |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 79 | cpu_data[nvpes + v].vpe_id = v; |
| 80 | #endif |
| 81 | } |
| 82 | |
| 83 | nvpes += core_vpes; |
| 84 | } |
| 85 | pr_cont("} total %u\n", nvpes); |
| 86 | |
| 87 | /* Indicate present CPUs (CPU being synonymous with VPE) */ |
| 88 | for (v = 0; v < min_t(unsigned, nvpes, NR_CPUS); v++) { |
| 89 | set_cpu_possible(v, true); |
| 90 | set_cpu_present(v, true); |
| 91 | __cpu_number_map[v] = v; |
| 92 | __cpu_logical_map[v] = v; |
| 93 | } |
| 94 | |
Paul Burton | 33b6866 | 2014-04-14 15:58:45 +0100 | [diff] [blame] | 95 | /* Set a coherent default CCA (CWB) */ |
| 96 | change_c0_config(CONF_CM_CMASK, 0x5); |
| 97 | |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 98 | /* Core 0 is powered up (we're running on it) */ |
| 99 | bitmap_set(core_power, 0, 1); |
| 100 | |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 101 | /* Initialise core 0 */ |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 102 | mips_cps_core_init(); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 103 | |
| 104 | /* Make core 0 coherent with everything */ |
| 105 | write_gcr_cl_coherence(0xff); |
Niklas Cassel | 90db024 | 2015-01-15 16:41:13 +0100 | [diff] [blame] | 106 | |
Paul Burton | 5a3e7c0 | 2016-02-03 03:15:33 +0000 | [diff] [blame] | 107 | if (mips_cm_revision() >= CM_REV_CM3) { |
| 108 | core_entry = CKSEG1ADDR((unsigned long)mips_cps_core_entry); |
| 109 | write_gcr_bev_base(core_entry); |
| 110 | } |
| 111 | |
Niklas Cassel | 90db024 | 2015-01-15 16:41:13 +0100 | [diff] [blame] | 112 | #ifdef CONFIG_MIPS_MT_FPAFF |
| 113 | /* If we have an FPU, enroll ourselves in the FPU-full mask */ |
| 114 | if (cpu_has_fpu) |
Ezequiel Garcia | 7363cb7 | 2015-04-28 18:34:23 -0300 | [diff] [blame] | 115 | cpumask_set_cpu(0, &mt_fpu_cpumask); |
Niklas Cassel | 90db024 | 2015-01-15 16:41:13 +0100 | [diff] [blame] | 116 | #endif /* CONFIG_MIPS_MT_FPAFF */ |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 117 | } |
| 118 | |
| 119 | static void __init cps_prepare_cpus(unsigned int max_cpus) |
| 120 | { |
Paul Burton | 5c399f6 | 2014-04-14 15:21:25 +0100 | [diff] [blame] | 121 | unsigned ncores, core_vpes, c, cca; |
| 122 | bool cca_unsuitable; |
Paul Burton | 0f4d3d1 | 2014-04-14 12:21:49 +0100 | [diff] [blame] | 123 | u32 *entry_code; |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 124 | |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 125 | mips_mt_set_cpuoptions(); |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 126 | |
Paul Burton | 5c399f6 | 2014-04-14 15:21:25 +0100 | [diff] [blame] | 127 | /* Detect whether the CCA is unsuited to multi-core SMP */ |
| 128 | cca = read_c0_config() & CONF_CM_CMASK; |
| 129 | switch (cca) { |
| 130 | case 0x4: /* CWBE */ |
| 131 | case 0x5: /* CWB */ |
| 132 | /* The CCA is coherent, multi-core is fine */ |
| 133 | cca_unsuitable = false; |
| 134 | break; |
| 135 | |
| 136 | default: |
| 137 | /* CCA is not coherent, multi-core is not usable */ |
| 138 | cca_unsuitable = true; |
| 139 | } |
| 140 | |
| 141 | /* Warn the user if the CCA prevents multi-core */ |
| 142 | ncores = mips_cm_numcores(); |
| 143 | if (cca_unsuitable && ncores > 1) { |
| 144 | pr_warn("Using only one core due to unsuitable CCA 0x%x\n", |
| 145 | cca); |
| 146 | |
| 147 | for_each_present_cpu(c) { |
| 148 | if (cpu_data[c].core) |
| 149 | set_cpu_present(c, false); |
| 150 | } |
| 151 | } |
| 152 | |
Paul Burton | 0155a06 | 2014-04-16 11:10:57 +0100 | [diff] [blame] | 153 | /* |
| 154 | * Patch the start of mips_cps_core_entry to provide: |
| 155 | * |
Paul Burton | 0155a06 | 2014-04-16 11:10:57 +0100 | [diff] [blame] | 156 | * s0 = kseg0 CCA |
| 157 | */ |
Paul Burton | 0f4d3d1 | 2014-04-14 12:21:49 +0100 | [diff] [blame] | 158 | entry_code = (u32 *)&mips_cps_core_entry; |
Paul Burton | 0155a06 | 2014-04-16 11:10:57 +0100 | [diff] [blame] | 159 | uasm_i_addiu(&entry_code, 16, 0, cca); |
Paul Burton | 0fc0708 | 2014-07-09 12:51:05 +0100 | [diff] [blame] | 160 | blast_dcache_range((unsigned long)&mips_cps_core_entry, |
| 161 | (unsigned long)entry_code); |
| 162 | bc_wback_inv((unsigned long)&mips_cps_core_entry, |
| 163 | (void *)entry_code - (void *)&mips_cps_core_entry); |
| 164 | __sync(); |
Paul Burton | 0f4d3d1 | 2014-04-14 12:21:49 +0100 | [diff] [blame] | 165 | |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 166 | /* Allocate core boot configuration structs */ |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 167 | mips_cps_core_bootcfg = kcalloc(ncores, sizeof(*mips_cps_core_bootcfg), |
| 168 | GFP_KERNEL); |
| 169 | if (!mips_cps_core_bootcfg) { |
| 170 | pr_err("Failed to allocate boot config for %u cores\n", ncores); |
| 171 | goto err_out; |
| 172 | } |
| 173 | |
| 174 | /* Allocate VPE boot configuration structs */ |
| 175 | for (c = 0; c < ncores; c++) { |
| 176 | core_vpes = core_vpe_count(c); |
| 177 | mips_cps_core_bootcfg[c].vpe_config = kcalloc(core_vpes, |
| 178 | sizeof(*mips_cps_core_bootcfg[c].vpe_config), |
| 179 | GFP_KERNEL); |
| 180 | if (!mips_cps_core_bootcfg[c].vpe_config) { |
| 181 | pr_err("Failed to allocate %u VPE boot configs\n", |
| 182 | core_vpes); |
| 183 | goto err_out; |
| 184 | } |
| 185 | } |
| 186 | |
| 187 | /* Mark this CPU as booted */ |
| 188 | atomic_set(&mips_cps_core_bootcfg[current_cpu_data.core].vpe_mask, |
| 189 | 1 << cpu_vpe_id(¤t_cpu_data)); |
| 190 | |
| 191 | return; |
| 192 | err_out: |
| 193 | /* Clean up allocations */ |
| 194 | if (mips_cps_core_bootcfg) { |
| 195 | for (c = 0; c < ncores; c++) |
| 196 | kfree(mips_cps_core_bootcfg[c].vpe_config); |
| 197 | kfree(mips_cps_core_bootcfg); |
| 198 | mips_cps_core_bootcfg = NULL; |
| 199 | } |
| 200 | |
| 201 | /* Effectively disable SMP by declaring CPUs not present */ |
| 202 | for_each_possible_cpu(c) { |
| 203 | if (c == 0) |
| 204 | continue; |
| 205 | set_cpu_present(c, false); |
| 206 | } |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 207 | } |
| 208 | |
Matt Redfearn | 9736c61 | 2016-07-07 08:50:38 +0100 | [diff] [blame^] | 209 | static void boot_core(unsigned int core, unsigned int vpe_id) |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 210 | { |
Paul Burton | a8c2061 | 2015-09-22 11:12:14 -0700 | [diff] [blame] | 211 | u32 access, stat, seq_state; |
| 212 | unsigned timeout; |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 213 | |
| 214 | /* Select the appropriate core */ |
Paul Burton | 4ede316 | 2015-09-22 11:12:17 -0700 | [diff] [blame] | 215 | mips_cm_lock_other(core, 0); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 216 | |
| 217 | /* Set its reset vector */ |
| 218 | write_gcr_co_reset_base(CKSEG1ADDR((unsigned long)mips_cps_core_entry)); |
| 219 | |
| 220 | /* Ensure its coherency is disabled */ |
| 221 | write_gcr_co_coherence(0); |
| 222 | |
Matt Redfearn | 497e803e | 2015-12-18 12:47:00 +0000 | [diff] [blame] | 223 | /* Start it with the legacy memory map and exception base */ |
| 224 | write_gcr_co_reset_ext_base(CM_GCR_RESET_EXT_BASE_UEB); |
| 225 | |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 226 | /* Ensure the core can access the GCRs */ |
| 227 | access = read_gcr_access(); |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 228 | access |= 1 << (CM_GCR_ACCESS_ACCESSEN_SHF + core); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 229 | write_gcr_access(access); |
| 230 | |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 231 | if (mips_cpc_present()) { |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 232 | /* Reset the core */ |
Paul Burton | dd9233d | 2014-03-07 10:42:52 +0000 | [diff] [blame] | 233 | mips_cpc_lock_other(core); |
Paul Burton | 5a3e7c0 | 2016-02-03 03:15:33 +0000 | [diff] [blame] | 234 | |
| 235 | if (mips_cm_revision() >= CM_REV_CM3) { |
Matt Redfearn | 9736c61 | 2016-07-07 08:50:38 +0100 | [diff] [blame^] | 236 | /* Run only the requested VP following the reset */ |
| 237 | write_cpc_co_vp_stop(0xf); |
| 238 | write_cpc_co_vp_run(1 << vpe_id); |
Paul Burton | 5a3e7c0 | 2016-02-03 03:15:33 +0000 | [diff] [blame] | 239 | |
| 240 | /* |
| 241 | * Ensure that the VP_RUN register is written before the |
| 242 | * core leaves reset. |
| 243 | */ |
| 244 | wmb(); |
| 245 | } |
| 246 | |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 247 | write_cpc_co_cmd(CPC_Cx_CMD_RESET); |
Paul Burton | a8c2061 | 2015-09-22 11:12:14 -0700 | [diff] [blame] | 248 | |
| 249 | timeout = 100; |
| 250 | while (true) { |
| 251 | stat = read_cpc_co_stat_conf(); |
| 252 | seq_state = stat & CPC_Cx_STAT_CONF_SEQSTATE_MSK; |
| 253 | |
| 254 | /* U6 == coherent execution, ie. the core is up */ |
| 255 | if (seq_state == CPC_Cx_STAT_CONF_SEQSTATE_U6) |
| 256 | break; |
| 257 | |
| 258 | /* Delay a little while before we start warning */ |
| 259 | if (timeout) { |
| 260 | timeout--; |
| 261 | mdelay(10); |
| 262 | continue; |
| 263 | } |
| 264 | |
| 265 | pr_warn("Waiting for core %u to start... STAT_CONF=0x%x\n", |
| 266 | core, stat); |
| 267 | mdelay(1000); |
| 268 | } |
| 269 | |
Paul Burton | dd9233d | 2014-03-07 10:42:52 +0000 | [diff] [blame] | 270 | mips_cpc_unlock_other(); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 271 | } else { |
| 272 | /* Take the core out of reset */ |
| 273 | write_gcr_co_reset_release(0); |
| 274 | } |
| 275 | |
Paul Burton | 4ede316 | 2015-09-22 11:12:17 -0700 | [diff] [blame] | 276 | mips_cm_unlock_other(); |
| 277 | |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 278 | /* The core is now powered up */ |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 279 | bitmap_set(core_power, core, 1); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 280 | } |
| 281 | |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 282 | static void remote_vpe_boot(void *dummy) |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 283 | { |
Paul Burton | f12401d | 2016-02-03 03:15:31 +0000 | [diff] [blame] | 284 | unsigned core = current_cpu_data.core; |
| 285 | struct core_boot_config *core_cfg = &mips_cps_core_bootcfg[core]; |
| 286 | |
| 287 | mips_cps_boot_vpes(core_cfg, cpu_vpe_id(¤t_cpu_data)); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 288 | } |
| 289 | |
| 290 | static void cps_boot_secondary(int cpu, struct task_struct *idle) |
| 291 | { |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 292 | unsigned core = cpu_data[cpu].core; |
| 293 | unsigned vpe_id = cpu_vpe_id(&cpu_data[cpu]); |
| 294 | struct core_boot_config *core_cfg = &mips_cps_core_bootcfg[core]; |
| 295 | struct vpe_boot_config *vpe_cfg = &core_cfg->vpe_config[vpe_id]; |
Paul Burton | 5a3e7c0 | 2016-02-03 03:15:33 +0000 | [diff] [blame] | 296 | unsigned long core_entry; |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 297 | unsigned int remote; |
| 298 | int err; |
| 299 | |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 300 | vpe_cfg->pc = (unsigned long)&smp_bootstrap; |
| 301 | vpe_cfg->sp = __KSTK_TOS(idle); |
| 302 | vpe_cfg->gp = (unsigned long)task_thread_info(idle); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 303 | |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 304 | atomic_or(1 << cpu_vpe_id(&cpu_data[cpu]), &core_cfg->vpe_mask); |
| 305 | |
Paul Burton | 1d8f1f5 | 2014-04-14 14:13:57 +0100 | [diff] [blame] | 306 | preempt_disable(); |
| 307 | |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 308 | if (!test_bit(core, core_power)) { |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 309 | /* Boot a VPE on a powered down core */ |
Matt Redfearn | 9736c61 | 2016-07-07 08:50:38 +0100 | [diff] [blame^] | 310 | boot_core(core, vpe_id); |
Paul Burton | 1d8f1f5 | 2014-04-14 14:13:57 +0100 | [diff] [blame] | 311 | goto out; |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 312 | } |
| 313 | |
Paul Burton | 5a3e7c0 | 2016-02-03 03:15:33 +0000 | [diff] [blame] | 314 | if (cpu_has_vp) { |
| 315 | mips_cm_lock_other(core, vpe_id); |
| 316 | core_entry = CKSEG1ADDR((unsigned long)mips_cps_core_entry); |
| 317 | write_gcr_co_reset_base(core_entry); |
| 318 | mips_cm_unlock_other(); |
| 319 | } |
| 320 | |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 321 | if (core != current_cpu_data.core) { |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 322 | /* Boot a VPE on another powered up core */ |
| 323 | for (remote = 0; remote < NR_CPUS; remote++) { |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 324 | if (cpu_data[remote].core != core) |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 325 | continue; |
| 326 | if (cpu_online(remote)) |
| 327 | break; |
| 328 | } |
| 329 | BUG_ON(remote >= NR_CPUS); |
| 330 | |
Paul Burton | 245a786 | 2014-04-14 12:04:27 +0100 | [diff] [blame] | 331 | err = smp_call_function_single(remote, remote_vpe_boot, |
| 332 | NULL, 1); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 333 | if (err) |
| 334 | panic("Failed to call remote CPU\n"); |
Paul Burton | 1d8f1f5 | 2014-04-14 14:13:57 +0100 | [diff] [blame] | 335 | goto out; |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 336 | } |
| 337 | |
Paul Burton | 5a3e7c0 | 2016-02-03 03:15:33 +0000 | [diff] [blame] | 338 | BUG_ON(!cpu_has_mipsmt && !cpu_has_vp); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 339 | |
| 340 | /* Boot a VPE on this core */ |
Paul Burton | f12401d | 2016-02-03 03:15:31 +0000 | [diff] [blame] | 341 | mips_cps_boot_vpes(core_cfg, vpe_id); |
Paul Burton | 1d8f1f5 | 2014-04-14 14:13:57 +0100 | [diff] [blame] | 342 | out: |
| 343 | preempt_enable(); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 344 | } |
| 345 | |
| 346 | static void cps_init_secondary(void) |
| 347 | { |
| 348 | /* Disable MT - we only want to run 1 TC per VPE */ |
| 349 | if (cpu_has_mipsmt) |
| 350 | dmt(); |
| 351 | |
Paul Burton | ba1c0a4 | 2016-02-03 03:15:29 +0000 | [diff] [blame] | 352 | if (mips_cm_revision() >= CM_REV_CM3) { |
| 353 | unsigned ident = gic_read_local_vp_id(); |
| 354 | |
| 355 | /* |
| 356 | * Ensure that our calculation of the VP ID matches up with |
| 357 | * what the GIC reports, otherwise we'll have configured |
| 358 | * interrupts incorrectly. |
| 359 | */ |
| 360 | BUG_ON(ident != mips_cm_vp_id(smp_processor_id())); |
| 361 | } |
| 362 | |
Paul Burton | d642e4e | 2016-05-17 15:31:05 +0100 | [diff] [blame] | 363 | if (cpu_has_veic) |
| 364 | clear_c0_status(ST0_IM); |
| 365 | else |
| 366 | change_c0_status(ST0_IM, STATUSF_IP2 | STATUSF_IP3 | |
| 367 | STATUSF_IP4 | STATUSF_IP5 | |
| 368 | STATUSF_IP6 | STATUSF_IP7); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 369 | } |
| 370 | |
| 371 | static void cps_smp_finish(void) |
| 372 | { |
| 373 | write_c0_compare(read_c0_count() + (8 * mips_hpt_frequency / HZ)); |
| 374 | |
| 375 | #ifdef CONFIG_MIPS_MT_FPAFF |
| 376 | /* If we have an FPU, enroll ourselves in the FPU-full mask */ |
| 377 | if (cpu_has_fpu) |
Rusty Russell | 8dd9289 | 2015-03-05 10:49:17 +1030 | [diff] [blame] | 378 | cpumask_set_cpu(smp_processor_id(), &mt_fpu_cpumask); |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 379 | #endif /* CONFIG_MIPS_MT_FPAFF */ |
| 380 | |
| 381 | local_irq_enable(); |
| 382 | } |
| 383 | |
Paul Burton | 1d8f1f5 | 2014-04-14 14:13:57 +0100 | [diff] [blame] | 384 | #ifdef CONFIG_HOTPLUG_CPU |
| 385 | |
| 386 | static int cps_cpu_disable(void) |
| 387 | { |
| 388 | unsigned cpu = smp_processor_id(); |
| 389 | struct core_boot_config *core_cfg; |
| 390 | |
| 391 | if (!cpu) |
| 392 | return -EBUSY; |
| 393 | |
| 394 | if (!cps_pm_support_state(CPS_PM_POWER_GATED)) |
| 395 | return -EINVAL; |
| 396 | |
| 397 | core_cfg = &mips_cps_core_bootcfg[current_cpu_data.core]; |
| 398 | atomic_sub(1 << cpu_vpe_id(¤t_cpu_data), &core_cfg->vpe_mask); |
Paul Burton | e114ba2 | 2014-06-11 11:00:56 +0100 | [diff] [blame] | 399 | smp_mb__after_atomic(); |
Paul Burton | 1d8f1f5 | 2014-04-14 14:13:57 +0100 | [diff] [blame] | 400 | set_cpu_online(cpu, false); |
Rusty Russell | 8dd9289 | 2015-03-05 10:49:17 +1030 | [diff] [blame] | 401 | cpumask_clear_cpu(cpu, &cpu_callin_map); |
Paul Burton | 1d8f1f5 | 2014-04-14 14:13:57 +0100 | [diff] [blame] | 402 | |
| 403 | return 0; |
| 404 | } |
| 405 | |
| 406 | static DECLARE_COMPLETION(cpu_death_chosen); |
| 407 | static unsigned cpu_death_sibling; |
| 408 | static enum { |
| 409 | CPU_DEATH_HALT, |
| 410 | CPU_DEATH_POWER, |
| 411 | } cpu_death; |
| 412 | |
| 413 | void play_dead(void) |
| 414 | { |
| 415 | unsigned cpu, core; |
| 416 | |
| 417 | local_irq_disable(); |
| 418 | idle_task_exit(); |
| 419 | cpu = smp_processor_id(); |
| 420 | cpu_death = CPU_DEATH_POWER; |
| 421 | |
| 422 | if (cpu_has_mipsmt) { |
| 423 | core = cpu_data[cpu].core; |
| 424 | |
| 425 | /* Look for another online VPE within the core */ |
| 426 | for_each_online_cpu(cpu_death_sibling) { |
| 427 | if (cpu_data[cpu_death_sibling].core != core) |
| 428 | continue; |
| 429 | |
| 430 | /* |
| 431 | * There is an online VPE within the core. Just halt |
| 432 | * this TC and leave the core alone. |
| 433 | */ |
| 434 | cpu_death = CPU_DEATH_HALT; |
| 435 | break; |
| 436 | } |
| 437 | } |
| 438 | |
| 439 | /* This CPU has chosen its way out */ |
| 440 | complete(&cpu_death_chosen); |
| 441 | |
| 442 | if (cpu_death == CPU_DEATH_HALT) { |
| 443 | /* Halt this TC */ |
| 444 | write_c0_tchalt(TCHALT_H); |
| 445 | instruction_hazard(); |
| 446 | } else { |
| 447 | /* Power down the core */ |
| 448 | cps_pm_enter_state(CPS_PM_POWER_GATED); |
| 449 | } |
| 450 | |
| 451 | /* This should never be reached */ |
| 452 | panic("Failed to offline CPU %u", cpu); |
| 453 | } |
| 454 | |
| 455 | static void wait_for_sibling_halt(void *ptr_cpu) |
| 456 | { |
Markos Chandras | fd5ed30 | 2015-07-01 09:13:28 +0100 | [diff] [blame] | 457 | unsigned cpu = (unsigned long)ptr_cpu; |
Paul Burton | c90e49f | 2014-07-09 12:48:21 +0100 | [diff] [blame] | 458 | unsigned vpe_id = cpu_vpe_id(&cpu_data[cpu]); |
Paul Burton | 1d8f1f5 | 2014-04-14 14:13:57 +0100 | [diff] [blame] | 459 | unsigned halted; |
| 460 | unsigned long flags; |
| 461 | |
| 462 | do { |
| 463 | local_irq_save(flags); |
| 464 | settc(vpe_id); |
| 465 | halted = read_tc_c0_tchalt(); |
| 466 | local_irq_restore(flags); |
| 467 | } while (!(halted & TCHALT_H)); |
| 468 | } |
| 469 | |
| 470 | static void cps_cpu_die(unsigned int cpu) |
| 471 | { |
| 472 | unsigned core = cpu_data[cpu].core; |
| 473 | unsigned stat; |
| 474 | int err; |
| 475 | |
| 476 | /* Wait for the cpu to choose its way out */ |
| 477 | if (!wait_for_completion_timeout(&cpu_death_chosen, |
| 478 | msecs_to_jiffies(5000))) { |
| 479 | pr_err("CPU%u: didn't offline\n", cpu); |
| 480 | return; |
| 481 | } |
| 482 | |
| 483 | /* |
| 484 | * Now wait for the CPU to actually offline. Without doing this that |
| 485 | * offlining may race with one or more of: |
| 486 | * |
| 487 | * - Onlining the CPU again. |
| 488 | * - Powering down the core if another VPE within it is offlined. |
| 489 | * - A sibling VPE entering a non-coherent state. |
| 490 | * |
| 491 | * In the non-MT halt case (ie. infinite loop) the CPU is doing nothing |
| 492 | * with which we could race, so do nothing. |
| 493 | */ |
| 494 | if (cpu_death == CPU_DEATH_POWER) { |
| 495 | /* |
| 496 | * Wait for the core to enter a powered down or clock gated |
| 497 | * state, the latter happening when a JTAG probe is connected |
| 498 | * in which case the CPC will refuse to power down the core. |
| 499 | */ |
| 500 | do { |
| 501 | mips_cpc_lock_other(core); |
| 502 | stat = read_cpc_co_stat_conf(); |
| 503 | stat &= CPC_Cx_STAT_CONF_SEQSTATE_MSK; |
| 504 | mips_cpc_unlock_other(); |
| 505 | } while (stat != CPC_Cx_STAT_CONF_SEQSTATE_D0 && |
| 506 | stat != CPC_Cx_STAT_CONF_SEQSTATE_D2 && |
| 507 | stat != CPC_Cx_STAT_CONF_SEQSTATE_U2); |
| 508 | |
| 509 | /* Indicate the core is powered off */ |
| 510 | bitmap_clear(core_power, core, 1); |
| 511 | } else if (cpu_has_mipsmt) { |
| 512 | /* |
| 513 | * Have a CPU with access to the offlined CPUs registers wait |
| 514 | * for its TC to halt. |
| 515 | */ |
| 516 | err = smp_call_function_single(cpu_death_sibling, |
| 517 | wait_for_sibling_halt, |
Markos Chandras | fd5ed30 | 2015-07-01 09:13:28 +0100 | [diff] [blame] | 518 | (void *)(unsigned long)cpu, 1); |
Paul Burton | 1d8f1f5 | 2014-04-14 14:13:57 +0100 | [diff] [blame] | 519 | if (err) |
| 520 | panic("Failed to call remote sibling CPU\n"); |
| 521 | } |
| 522 | } |
| 523 | |
| 524 | #endif /* CONFIG_HOTPLUG_CPU */ |
| 525 | |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 526 | static struct plat_smp_ops cps_smp_ops = { |
| 527 | .smp_setup = cps_smp_setup, |
| 528 | .prepare_cpus = cps_prepare_cpus, |
| 529 | .boot_secondary = cps_boot_secondary, |
| 530 | .init_secondary = cps_init_secondary, |
| 531 | .smp_finish = cps_smp_finish, |
Qais Yousef | bb11cff | 2015-12-08 13:20:28 +0000 | [diff] [blame] | 532 | .send_ipi_single = mips_smp_send_ipi_single, |
| 533 | .send_ipi_mask = mips_smp_send_ipi_mask, |
Paul Burton | 1d8f1f5 | 2014-04-14 14:13:57 +0100 | [diff] [blame] | 534 | #ifdef CONFIG_HOTPLUG_CPU |
| 535 | .cpu_disable = cps_cpu_disable, |
| 536 | .cpu_die = cps_cpu_die, |
| 537 | #endif |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 538 | }; |
| 539 | |
Paul Burton | 68c1232 | 2014-03-14 16:06:16 +0000 | [diff] [blame] | 540 | bool mips_cps_smp_in_use(void) |
| 541 | { |
| 542 | extern struct plat_smp_ops *mp_ops; |
| 543 | return mp_ops == &cps_smp_ops; |
| 544 | } |
| 545 | |
Paul Burton | 0ee958e | 2014-01-15 10:31:53 +0000 | [diff] [blame] | 546 | int register_cps_smp_ops(void) |
| 547 | { |
| 548 | if (!mips_cm_present()) { |
| 549 | pr_warn("MIPS CPS SMP unable to proceed without a CM\n"); |
| 550 | return -ENODEV; |
| 551 | } |
| 552 | |
| 553 | /* check we have a GIC - we need one for IPIs */ |
| 554 | if (!(read_gcr_gic_status() & CM_GCR_GIC_STATUS_EX_MSK)) { |
| 555 | pr_warn("MIPS CPS SMP unable to proceed without a GIC\n"); |
| 556 | return -ENODEV; |
| 557 | } |
| 558 | |
| 559 | register_smp_ops(&cps_smp_ops); |
| 560 | return 0; |
| 561 | } |