sys/arm64/arm64/mp_machdep.c

   1 /*-
   2  * Copyright (c) 2015 The FreeBSD Foundation
   3  * All rights reserved.
   4  *
   5  * This software was developed by Andrew Turner under
   6  * sponsorship from the FreeBSD Foundation.
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  * 1. Redistributions of source code must retain the above copyright
  12  *    notice, this list of conditions and the following disclaimer.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in the
  15  *    documentation and/or other materials provided with the distribution.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  18  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  21  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  22  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  23  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  24  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  25  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  26  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  27  * SUCH DAMAGE.
  28  *
  29  */
  30
  31 #include "opt_kstack_pages.h"
  32 #include "opt_platform.h"
  33
  34 #include <sys/cdefs.h>
  35 __FBSDID("$FreeBSD$");
  36
  37 #include <sys/param.h>
  38 #include <sys/systm.h>
  39 #include <sys/bus.h>
  40 #include <sys/cpu.h>
  41 #include <sys/kernel.h>
  42 #include <sys/malloc.h>
  43 #include <sys/module.h>
  44 #include <sys/mutex.h>
  45 #include <sys/proc.h>
  46 #include <sys/sched.h>
  47 #include <sys/smp.h>
  48
  49 #include <vm/vm.h>
  50 #include <vm/pmap.h>
  51 #include <vm/vm_extern.h>
  52 #include <vm/vm_kern.h>
  53
  54 #include <machine/debug_monitor.h>
  55 #include <machine/intr.h>
  56 #include <machine/smp.h>
  57 #ifdef VFP
  58 #include <machine/vfp.h>
  59 #endif
  60
  61 #ifdef FDT
  62 #include <dev/ofw/openfirm.h>
  63 #include <dev/ofw/ofw_cpu.h>
  64 #endif
  65
  66 #include <dev/psci/psci.h>
  67
  68 boolean_t ofw_cpu_reg(phandle_t node, u_int, cell_t *);
  69
  70 extern struct pcpu __pcpu[];
  71
  72 static enum {
  73         CPUS_UNKNOWN,
  74 #ifdef FDT
  75         CPUS_FDT,
  76 #endif
  77 } cpu_enum_method;
  78
  79 static device_identify_t arm64_cpu_identify;
  80 static device_probe_t arm64_cpu_probe;
  81 static device_attach_t arm64_cpu_attach;
  82
  83 static void ipi_ast(void *);
  84 static void ipi_hardclock(void *);
  85 static void ipi_preempt(void *);
  86 static void ipi_rendezvous(void *);
  87 static void ipi_stop(void *);
  88
  89 static int ipi_handler(void *arg);
  90
  91 struct mtx ap_boot_mtx;
  92 struct pcb stoppcbs[MAXCPU];
  93
  94 #ifdef INVARIANTS
  95 static uint32_t cpu_reg[MAXCPU][2];
  96 #endif
  97 static device_t cpu_list[MAXCPU];
  98
  99 void mpentry(unsigned long cpuid);
 100 void init_secondary(uint64_t);
 101
 102 uint8_t secondary_stacks[MAXCPU - 1][PAGE_SIZE * KSTACK_PAGES] __aligned(16);
 103
 104 /* Set to 1 once we're ready to let the APs out of the pen. */
 105 volatile int aps_ready = 0;
 106
 107 /* Temporary variables for init_secondary()  */
 108 void *dpcpu[MAXCPU - 1];
 109
 110 static device_method_t arm64_cpu_methods[] = {
 111         /* Device interface */
 112         DEVMETHOD(device_identify,      arm64_cpu_identify),
 113         DEVMETHOD(device_probe,         arm64_cpu_probe),
 114         DEVMETHOD(device_attach,        arm64_cpu_attach),
 115
 116         DEVMETHOD_END
 117 };
 118
 119 static devclass_t arm64_cpu_devclass;
 120 static driver_t arm64_cpu_driver = {
 121         "arm64_cpu",
 122         arm64_cpu_methods,
 123         0
 124 };
 125
 126 DRIVER_MODULE(arm64_cpu, cpu, arm64_cpu_driver, arm64_cpu_devclass, 0, 0);
 127
 128 static void
 129 arm64_cpu_identify(driver_t *driver, device_t parent)
 130 {
 131
 132         if (device_find_child(parent, "arm64_cpu", -1) != NULL)
 133                 return;
 134         if (BUS_ADD_CHILD(parent, 0, "arm64_cpu", -1) == NULL)
 135                 device_printf(parent, "add child failed\n");
 136 }
 137
 138 static int
 139 arm64_cpu_probe(device_t dev)
 140 {
 141         u_int cpuid;
 142
 143         cpuid = device_get_unit(dev);
 144         if (cpuid >= MAXCPU || cpuid > mp_maxid)
 145                 return (EINVAL);
 146
 147         device_quiet(dev);
 148         return (0);
 149 }
 150
 151 static int
 152 arm64_cpu_attach(device_t dev)
 153 {
 154         const uint32_t *reg;
 155         size_t reg_size;
 156         u_int cpuid;
 157         int i;
 158
 159         cpuid = device_get_unit(dev);
 160
 161         if (cpuid >= MAXCPU || cpuid > mp_maxid)
 162                 return (EINVAL);
 163         KASSERT(cpu_list[cpuid] == NULL, ("Already have cpu %u", cpuid));
 164
 165         reg = cpu_get_cpuid(dev, &reg_size);
 166         if (reg == NULL)
 167                 return (EINVAL);
 168
 169         if (bootverbose) {
 170                 device_printf(dev, "register <");
 171                 for (i = 0; i < reg_size; i++)
 172                         printf("%s%x", (i == 0) ? "" : " ", reg[i]);
 173                 printf(">\n");
 174         }
 175
 176         /* Set the device to start it later */
 177         cpu_list[cpuid] = dev;
 178
 179         return (0);
 180 }
 181
 182 static void
 183 release_aps(void *dummy __unused)
 184 {
 185         int cpu, i;
 186
 187         /* Setup the IPI handler */
 188         for (i = 0; i < INTR_IPI_COUNT; i++)
 189                 arm_setup_ipihandler(ipi_handler, i);
 190
 191         atomic_store_rel_int(&aps_ready, 1);
 192         /* Wake up the other CPUs */
 193         __asm __volatile("sev");
 194
 195         printf("Release APs\n");
 196
 197         for (i = 0; i < 2000; i++) {
 198                 if (smp_started) {
 199                         for (cpu = 0; cpu <= mp_maxid; cpu++) {
 200                                 if (CPU_ABSENT(cpu))
 201                                         continue;
 202                                 print_cpu_features(cpu);
 203                         }
 204                         return;
 205                 }
 206                 DELAY(1000);
 207         }
 208
 209         printf("APs not started\n");
 210 }
 211 SYSINIT(start_aps, SI_SUB_SMP, SI_ORDER_FIRST, release_aps, NULL);
 212
 213 void
 214 init_secondary(uint64_t cpu)
 215 {
 216         struct pcpu *pcpup;
 217         int i;
 218
 219         pcpup = &__pcpu[cpu];
 220         /*
 221          * Set the pcpu pointer with a backup in tpidr_el1 to be
 222          * loaded when entering the kernel from userland.
 223          */
 224         __asm __volatile(
 225             "mov x18, %0 \n"
 226             "msr tpidr_el1, %0" :: "r"(pcpup));
 227
 228         /* Spin until the BSP releases the APs */
 229         while (!aps_ready)
 230                 __asm __volatile("wfe");
 231
 232         /* Initialize curthread */
 233         KASSERT(PCPU_GET(idlethread) != NULL, ("no idle thread"));
 234         pcpup->pc_curthread = pcpup->pc_idlethread;
 235         pcpup->pc_curpcb = pcpup->pc_idlethread->td_pcb;
 236
 237         /*
 238          * Identify current CPU. This is necessary to setup
 239          * affinity registers and to provide support for
 240          * runtime chip identification.
 241          */
 242         identify_cpu();
 243
 244         /* Configure the interrupt controller */
 245         arm_init_secondary();
 246
 247         for (i = 0; i < INTR_IPI_COUNT; i++)
 248                 arm_unmask_ipi(i);
 249
 250         /* Start per-CPU event timers. */
 251         cpu_initclocks_ap();
 252
 253 #ifdef VFP
 254         vfp_init();
 255 #endif
 256
 257         dbg_monitor_init();
 258
 259         /* Enable interrupts */
 260         intr_enable();
 261
 262         mtx_lock_spin(&ap_boot_mtx);
 263
 264         atomic_add_rel_32(&smp_cpus, 1);
 265
 266         if (smp_cpus == mp_ncpus) {
 267                 /* enable IPI's, tlb shootdown, freezes etc */
 268                 atomic_store_rel_int(&smp_started, 1);
 269         }
 270
 271         mtx_unlock_spin(&ap_boot_mtx);
 272
 273         /* Enter the scheduler */
 274         sched_throw(NULL);
 275
 276         panic("scheduler returned us to init_secondary");
 277         /* NOTREACHED */
 278 }
 279
 280 static void
 281 ipi_ast(void *dummy __unused)
 282 {
 283
 284         CTR0(KTR_SMP, "IPI_AST");
 285 }
 286
 287 static void
 288 ipi_hardclock(void *dummy __unused)
 289 {
 290
 291         CTR1(KTR_SMP, "%s: IPI_HARDCLOCK", __func__);
 292         hardclockintr();
 293 }
 294
 295 static void
 296 ipi_preempt(void *dummy __unused)
 297 {
 298         CTR1(KTR_SMP, "%s: IPI_PREEMPT", __func__);
 299         sched_preempt(curthread);
 300 }
 301
 302 static void
 303 ipi_rendezvous(void *dummy __unused)
 304 {
 305
 306         CTR0(KTR_SMP, "IPI_RENDEZVOUS");
 307         smp_rendezvous_action();
 308 }
 309
 310 static void
 311 ipi_stop(void *dummy __unused)
 312 {
 313         u_int cpu;
 314
 315         CTR0(KTR_SMP, "IPI_STOP");
 316
 317         cpu = PCPU_GET(cpuid);
 318         savectx(&stoppcbs[cpu]);
 319
 320         /* Indicate we are stopped */
 321         CPU_SET_ATOMIC(cpu, &stopped_cpus);
 322
 323         /* Wait for restart */
 324         while (!CPU_ISSET(cpu, &started_cpus))
 325                 cpu_spinwait();
 326
 327         CPU_CLR_ATOMIC(cpu, &started_cpus);
 328         CPU_CLR_ATOMIC(cpu, &stopped_cpus);
 329         CTR0(KTR_SMP, "IPI_STOP (restart)");
 330 }
 331
 332 static int
 333 ipi_handler(void *arg)
 334 {
 335         u_int cpu, ipi;
 336
 337         arg = (void *)((uintptr_t)arg & ~(1 << 16));
 338         KASSERT((uintptr_t)arg < INTR_IPI_COUNT,
 339             ("Invalid IPI %ju", (uintptr_t)arg));
 340
 341         cpu = PCPU_GET(cpuid);
 342         ipi = (uintptr_t)arg;
 343
 344         switch(ipi) {
 345         case IPI_AST:
 346                 ipi_ast(NULL);
 347                 break;
 348         case IPI_PREEMPT:
 349                 ipi_preempt(NULL);
 350                 break;
 351         case IPI_RENDEZVOUS:
 352                 ipi_rendezvous(NULL);
 353                 break;
 354         case IPI_STOP:
 355         case IPI_STOP_HARD:
 356                 ipi_stop(NULL);
 357                 break;
 358         case IPI_HARDCLOCK:
 359                 ipi_hardclock(NULL);
 360                 break;
 361         default:
 362                 panic("Unknown IPI %#0x on cpu %d", ipi, curcpu);
 363         }
 364
 365         return (FILTER_HANDLED);
 366 }
 367
 368 struct cpu_group *
 369 cpu_topo(void)
 370 {
 371
 372         return (smp_topo_none());
 373 }
 374
 375 /* Determine if we running MP machine */
 376 int
 377 cpu_mp_probe(void)
 378 {
 379
 380         /* ARM64TODO: Read the u bit of mpidr_el1 to determine this */
 381         return (1);
 382 }
 383
 384 #ifdef FDT
 385 static boolean_t
 386 cpu_init_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
 387 {
 388         uint64_t target_cpu;
 389         struct pcpu *pcpup;
 390         vm_paddr_t pa;
 391         int err;
 392
 393         /* Check we are able to start this cpu */
 394         if (id > mp_maxid)
 395                 return (0);
 396
 397         KASSERT(id < MAXCPU, ("Too mant CPUs"));
 398
 399         KASSERT(addr_size == 1 || addr_size == 2, ("Invalid register size"));
 400 #ifdef INVARIANTS
 401         cpu_reg[id][0] = reg[0];
 402         if (addr_size == 2)
 403                 cpu_reg[id][1] = reg[1];
 404 #endif
 405
 406         /* We are already running on cpu 0 */
 407         if (id == 0)
 408                 return (1);
 409
 410
 411         pcpup = &__pcpu[id];
 412         pcpu_init(pcpup, id, sizeof(struct pcpu));
 413
 414         dpcpu[id - 1] = (void *)kmem_malloc(kernel_arena, DPCPU_SIZE,
 415             M_WAITOK | M_ZERO);
 416         dpcpu_init(dpcpu[id - 1], id);
 417
 418         target_cpu = reg[0];
 419         if (addr_size == 2) {
 420                 target_cpu <<= 32;
 421                 target_cpu |= reg[1];
 422         }
 423
 424         printf("Starting CPU %u (%lx)\n", id, target_cpu);
 425         pa = pmap_extract(kernel_pmap, (vm_offset_t)mpentry);
 426
 427         err = psci_cpu_on(target_cpu, pa, id);
 428         if (err != PSCI_RETVAL_SUCCESS) {
 429                 /* Panic here if INVARIANTS are enabled */
 430                 KASSERT(0, ("Failed to start CPU %u (%lx)\n", id, target_cpu));
 431
 432                 pcpu_destroy(pcpup);
 433                 kmem_free(kernel_arena, (vm_offset_t)dpcpu[id - 1], DPCPU_SIZE);
 434                 dpcpu[id - 1] = NULL;
 435                 /* Notify the user that the CPU failed to start */
 436                 printf("Failed to start CPU %u (%lx)\n", id, target_cpu);
 437         } else
 438                 CPU_SET(id, &all_cpus);
 439
 440         return (1);
 441 }
 442 #endif
 443
 444 /* Initialize and fire up non-boot processors */
 445 void
 446 cpu_mp_start(void)
 447 {
 448
 449         mtx_init(&ap_boot_mtx, "ap boot", NULL, MTX_SPIN);
 450
 451         CPU_SET(0, &all_cpus);
 452
 453         switch(cpu_enum_method) {
 454 #ifdef FDT
 455         case CPUS_FDT:
 456                 ofw_cpu_early_foreach(cpu_init_fdt, true);
 457                 break;
 458 #endif
 459         case CPUS_UNKNOWN:
 460                 break;
 461         }
 462 }
 463
 464 /* Introduce rest of cores to the world */
 465 void
 466 cpu_mp_announce(void)
 467 {
 468 }
 469
 470 void
 471 cpu_mp_setmaxid(void)
 472 {
 473 #ifdef FDT
 474         int cores;
 475
 476         cores = ofw_cpu_early_foreach(NULL, false);
 477         if (cores > 0) {
 478                 cores = MIN(cores, MAXCPU);
 479                 if (bootverbose)
 480                         printf("Found %d CPUs in the device tree\n", cores);
 481                 mp_ncpus = cores;
 482                 mp_maxid = cores - 1;
 483                 cpu_enum_method = CPUS_FDT;
 484                 return;
 485         }
 486 #endif
 487
 488         if (bootverbose)
 489                 printf("No CPU data, limiting to 1 core\n");
 490         mp_ncpus = 1;
 491         mp_maxid = 0;
 492 }