]> CyberLeo.Net >> Repos - FreeBSD/FreeBSD.git/blob - sys/arm64/arm64/mp_machdep.c
MFC r312432:
[FreeBSD/FreeBSD.git] / sys / arm64 / arm64 / mp_machdep.c
1 /*-
2  * Copyright (c) 2015-2016 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 #include "pic_if.h"
69
70 typedef void intr_ipi_send_t(void *, cpuset_t, u_int);
71 typedef void intr_ipi_handler_t(void *);
72
73 #define INTR_IPI_NAMELEN        (MAXCOMLEN + 1)
74 struct intr_ipi {
75         intr_ipi_handler_t *    ii_handler;
76         void *                  ii_handler_arg;
77         intr_ipi_send_t *       ii_send;
78         void *                  ii_send_arg;
79         char                    ii_name[INTR_IPI_NAMELEN];
80         u_long *                ii_count;
81 };
82
83 static struct intr_ipi ipi_sources[INTR_IPI_COUNT];
84
85 static struct intr_ipi *intr_ipi_lookup(u_int);
86 static void intr_pic_ipi_setup(u_int, const char *, intr_ipi_handler_t *,
87     void *);
88
89 boolean_t ofw_cpu_reg(phandle_t node, u_int, cell_t *);
90
91 extern struct pcpu __pcpu[];
92
93 static enum {
94         CPUS_UNKNOWN,
95 #ifdef FDT
96         CPUS_FDT,
97 #endif
98 } cpu_enum_method;
99
100 static device_identify_t arm64_cpu_identify;
101 static device_probe_t arm64_cpu_probe;
102 static device_attach_t arm64_cpu_attach;
103
104 static void ipi_ast(void *);
105 static void ipi_hardclock(void *);
106 static void ipi_preempt(void *);
107 static void ipi_rendezvous(void *);
108 static void ipi_stop(void *);
109
110 static int ipi_handler(void *arg);
111
112 struct mtx ap_boot_mtx;
113 struct pcb stoppcbs[MAXCPU];
114
115 static device_t cpu_list[MAXCPU];
116
117 /*
118  * Not all systems boot from the first CPU in the device tree. To work around
119  * this we need to find which CPU we have booted from so when we later
120  * enable the secondary CPUs we skip this one.
121  */
122 static int cpu0 = -1;
123
124 void mpentry(unsigned long cpuid);
125 void init_secondary(uint64_t);
126
127 uint8_t secondary_stacks[MAXCPU - 1][PAGE_SIZE * KSTACK_PAGES] __aligned(16);
128
129 /* Set to 1 once we're ready to let the APs out of the pen. */
130 volatile int aps_ready = 0;
131
132 /* Temporary variables for init_secondary()  */
133 void *dpcpu[MAXCPU - 1];
134
135 static device_method_t arm64_cpu_methods[] = {
136         /* Device interface */
137         DEVMETHOD(device_identify,      arm64_cpu_identify),
138         DEVMETHOD(device_probe,         arm64_cpu_probe),
139         DEVMETHOD(device_attach,        arm64_cpu_attach),
140
141         DEVMETHOD_END
142 };
143
144 static devclass_t arm64_cpu_devclass;
145 static driver_t arm64_cpu_driver = {
146         "arm64_cpu",
147         arm64_cpu_methods,
148         0
149 };
150
151 DRIVER_MODULE(arm64_cpu, cpu, arm64_cpu_driver, arm64_cpu_devclass, 0, 0);
152
153 static void
154 arm64_cpu_identify(driver_t *driver, device_t parent)
155 {
156
157         if (device_find_child(parent, "arm64_cpu", -1) != NULL)
158                 return;
159         if (BUS_ADD_CHILD(parent, 0, "arm64_cpu", -1) == NULL)
160                 device_printf(parent, "add child failed\n");
161 }
162
163 static int
164 arm64_cpu_probe(device_t dev)
165 {
166         u_int cpuid;
167
168         cpuid = device_get_unit(dev);
169         if (cpuid >= MAXCPU || cpuid > mp_maxid)
170                 return (EINVAL);
171
172         device_quiet(dev);
173         return (0);
174 }
175
176 static int
177 arm64_cpu_attach(device_t dev)
178 {
179         const uint32_t *reg;
180         size_t reg_size;
181         u_int cpuid;
182         int i;
183
184         cpuid = device_get_unit(dev);
185
186         if (cpuid >= MAXCPU || cpuid > mp_maxid)
187                 return (EINVAL);
188         KASSERT(cpu_list[cpuid] == NULL, ("Already have cpu %u", cpuid));
189
190         reg = cpu_get_cpuid(dev, &reg_size);
191         if (reg == NULL)
192                 return (EINVAL);
193
194         if (bootverbose) {
195                 device_printf(dev, "register <");
196                 for (i = 0; i < reg_size; i++)
197                         printf("%s%x", (i == 0) ? "" : " ", reg[i]);
198                 printf(">\n");
199         }
200
201         /* Set the device to start it later */
202         cpu_list[cpuid] = dev;
203
204         return (0);
205 }
206
207 static void
208 release_aps(void *dummy __unused)
209 {
210         int cpu, i;
211
212         intr_pic_ipi_setup(IPI_AST, "ast", ipi_ast, NULL);
213         intr_pic_ipi_setup(IPI_PREEMPT, "preempt", ipi_preempt, NULL);
214         intr_pic_ipi_setup(IPI_RENDEZVOUS, "rendezvous", ipi_rendezvous, NULL);
215         intr_pic_ipi_setup(IPI_STOP, "stop", ipi_stop, NULL);
216         intr_pic_ipi_setup(IPI_STOP_HARD, "stop hard", ipi_stop, NULL);
217         intr_pic_ipi_setup(IPI_HARDCLOCK, "hardclock", ipi_hardclock, NULL);
218
219         atomic_store_rel_int(&aps_ready, 1);
220         /* Wake up the other CPUs */
221         __asm __volatile("sev");
222
223         printf("Release APs\n");
224
225         for (i = 0; i < 2000; i++) {
226                 if (smp_started) {
227                         for (cpu = 0; cpu <= mp_maxid; cpu++) {
228                                 if (CPU_ABSENT(cpu))
229                                         continue;
230                                 print_cpu_features(cpu);
231                         }
232                         return;
233                 }
234                 DELAY(1000);
235         }
236
237         printf("APs not started\n");
238 }
239 SYSINIT(start_aps, SI_SUB_SMP, SI_ORDER_FIRST, release_aps, NULL);
240
241 void
242 init_secondary(uint64_t cpu)
243 {
244         struct pcpu *pcpup;
245
246         pcpup = &__pcpu[cpu];
247         /*
248          * Set the pcpu pointer with a backup in tpidr_el1 to be
249          * loaded when entering the kernel from userland.
250          */
251         __asm __volatile(
252             "mov x18, %0 \n"
253             "msr tpidr_el1, %0" :: "r"(pcpup));
254
255         /* Spin until the BSP releases the APs */
256         while (!aps_ready)
257                 __asm __volatile("wfe");
258
259         /* Initialize curthread */
260         KASSERT(PCPU_GET(idlethread) != NULL, ("no idle thread"));
261         pcpup->pc_curthread = pcpup->pc_idlethread;
262         pcpup->pc_curpcb = pcpup->pc_idlethread->td_pcb;
263
264         /*
265          * Identify current CPU. This is necessary to setup
266          * affinity registers and to provide support for
267          * runtime chip identification.
268          */
269         identify_cpu();
270
271         intr_pic_init_secondary();
272
273         /* Start per-CPU event timers. */
274         cpu_initclocks_ap();
275
276 #ifdef VFP
277         vfp_init();
278 #endif
279
280         dbg_monitor_init();
281
282         /* Enable interrupts */
283         intr_enable();
284
285         mtx_lock_spin(&ap_boot_mtx);
286
287         atomic_add_rel_32(&smp_cpus, 1);
288
289         if (smp_cpus == mp_ncpus) {
290                 /* enable IPI's, tlb shootdown, freezes etc */
291                 atomic_store_rel_int(&smp_started, 1);
292         }
293
294         mtx_unlock_spin(&ap_boot_mtx);
295
296         /* Enter the scheduler */
297         sched_throw(NULL);
298
299         panic("scheduler returned us to init_secondary");
300         /* NOTREACHED */
301 }
302
303 /*
304  *  Send IPI thru interrupt controller.
305  */
306 static void
307 pic_ipi_send(void *arg, cpuset_t cpus, u_int ipi)
308 {
309
310         KASSERT(intr_irq_root_dev != NULL, ("%s: no root attached", __func__));
311         PIC_IPI_SEND(intr_irq_root_dev, arg, cpus, ipi);
312 }
313
314 /*
315  *  Setup IPI handler on interrupt controller.
316  *
317  *  Not SMP coherent.
318  */
319 static void
320 intr_pic_ipi_setup(u_int ipi, const char *name, intr_ipi_handler_t *hand,
321     void *arg)
322 {
323         struct intr_irqsrc *isrc;
324         struct intr_ipi *ii;
325         int error;
326
327         KASSERT(intr_irq_root_dev != NULL, ("%s: no root attached", __func__));
328         KASSERT(hand != NULL, ("%s: ipi %u no handler", __func__, ipi));
329
330         error = PIC_IPI_SETUP(intr_irq_root_dev, ipi, &isrc);
331         if (error != 0)
332                 return;
333
334         isrc->isrc_handlers++;
335
336         ii = intr_ipi_lookup(ipi);
337         KASSERT(ii->ii_count == NULL, ("%s: ipi %u reused", __func__, ipi));
338
339         ii->ii_handler = hand;
340         ii->ii_handler_arg = arg;
341         ii->ii_send = pic_ipi_send;
342         ii->ii_send_arg = isrc;
343         strlcpy(ii->ii_name, name, INTR_IPI_NAMELEN);
344         ii->ii_count = intr_ipi_setup_counters(name);
345 }
346
347 static void
348 intr_ipi_send(cpuset_t cpus, u_int ipi)
349 {
350         struct intr_ipi *ii;
351
352         ii = intr_ipi_lookup(ipi);
353         if (ii->ii_count == NULL)
354                 panic("%s: not setup IPI %u", __func__, ipi);
355
356         ii->ii_send(ii->ii_send_arg, cpus, ipi);
357 }
358
359 static void
360 ipi_ast(void *dummy __unused)
361 {
362
363         CTR0(KTR_SMP, "IPI_AST");
364 }
365
366 static void
367 ipi_hardclock(void *dummy __unused)
368 {
369
370         CTR1(KTR_SMP, "%s: IPI_HARDCLOCK", __func__);
371         hardclockintr();
372 }
373
374 static void
375 ipi_preempt(void *dummy __unused)
376 {
377         CTR1(KTR_SMP, "%s: IPI_PREEMPT", __func__);
378         sched_preempt(curthread);
379 }
380
381 static void
382 ipi_rendezvous(void *dummy __unused)
383 {
384
385         CTR0(KTR_SMP, "IPI_RENDEZVOUS");
386         smp_rendezvous_action();
387 }
388
389 static void
390 ipi_stop(void *dummy __unused)
391 {
392         u_int cpu;
393
394         CTR0(KTR_SMP, "IPI_STOP");
395
396         cpu = PCPU_GET(cpuid);
397         savectx(&stoppcbs[cpu]);
398
399         /* Indicate we are stopped */
400         CPU_SET_ATOMIC(cpu, &stopped_cpus);
401
402         /* Wait for restart */
403         while (!CPU_ISSET(cpu, &started_cpus))
404                 cpu_spinwait();
405
406         CPU_CLR_ATOMIC(cpu, &started_cpus);
407         CPU_CLR_ATOMIC(cpu, &stopped_cpus);
408         CTR0(KTR_SMP, "IPI_STOP (restart)");
409 }
410
411 struct cpu_group *
412 cpu_topo(void)
413 {
414
415         return (smp_topo_none());
416 }
417
418 /* Determine if we running MP machine */
419 int
420 cpu_mp_probe(void)
421 {
422
423         /* ARM64TODO: Read the u bit of mpidr_el1 to determine this */
424         return (1);
425 }
426
427 #ifdef FDT
428 static boolean_t
429 cpu_init_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
430 {
431         uint64_t target_cpu;
432         struct pcpu *pcpup;
433         vm_paddr_t pa;
434         u_int cpuid;
435         int err;
436
437         /* Check we are able to start this cpu */
438         if (id > mp_maxid)
439                 return (0);
440
441         KASSERT(id < MAXCPU, ("Too many CPUs"));
442
443         /* We are already running on cpu 0 */
444         if (id == cpu0)
445                 return (1);
446
447         /*
448          * Rotate the CPU IDs to put the boot CPU as CPU 0. We keep the other
449          * CPUs ordered as the are likely grouped into clusters so it can be
450          * useful to keep that property, e.g. for the GICv3 driver to send
451          * an IPI to all CPUs in the cluster.
452          */
453         cpuid = id;
454         if (cpuid < cpu0)
455                 cpuid += mp_maxid + 1;
456         cpuid -= cpu0;
457
458         pcpup = &__pcpu[cpuid];
459         pcpu_init(pcpup, cpuid, sizeof(struct pcpu));
460
461         dpcpu[cpuid - 1] = (void *)kmem_malloc(kernel_arena, DPCPU_SIZE,
462             M_WAITOK | M_ZERO);
463         dpcpu_init(dpcpu[cpuid - 1], cpuid);
464
465         target_cpu = reg[0];
466         if (addr_size == 2) {
467                 target_cpu <<= 32;
468                 target_cpu |= reg[1];
469         }
470
471         printf("Starting CPU %u (%lx)\n", cpuid, target_cpu);
472         pa = pmap_extract(kernel_pmap, (vm_offset_t)mpentry);
473
474         err = psci_cpu_on(target_cpu, pa, cpuid);
475         if (err != PSCI_RETVAL_SUCCESS) {
476                 /* Panic here if INVARIANTS are enabled */
477                 KASSERT(0, ("Failed to start CPU %u (%lx)\n", id,
478                     target_cpu));
479
480                 pcpu_destroy(pcpup);
481                 kmem_free(kernel_arena, (vm_offset_t)dpcpu[cpuid - 1],
482                     DPCPU_SIZE);
483                 dpcpu[cpuid - 1] = NULL;
484                 /* Notify the user that the CPU failed to start */
485                 printf("Failed to start CPU %u (%lx)\n", id, target_cpu);
486         } else
487                 CPU_SET(cpuid, &all_cpus);
488
489         return (1);
490 }
491 #endif
492
493 /* Initialize and fire up non-boot processors */
494 void
495 cpu_mp_start(void)
496 {
497
498         mtx_init(&ap_boot_mtx, "ap boot", NULL, MTX_SPIN);
499
500         CPU_SET(0, &all_cpus);
501
502         switch(cpu_enum_method) {
503 #ifdef FDT
504         case CPUS_FDT:
505                 KASSERT(cpu0 >= 0, ("Current CPU was not found"));
506                 ofw_cpu_early_foreach(cpu_init_fdt, true);
507                 break;
508 #endif
509         case CPUS_UNKNOWN:
510                 break;
511         }
512 }
513
514 /* Introduce rest of cores to the world */
515 void
516 cpu_mp_announce(void)
517 {
518 }
519
520 static boolean_t
521 cpu_find_cpu0_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
522 {
523         uint64_t mpidr_fdt, mpidr_reg;
524
525         if (cpu0 < 0) {
526                 mpidr_fdt = reg[0];
527                 if (addr_size == 2) {
528                         mpidr_fdt <<= 32;
529                         mpidr_fdt |= reg[1];
530                 }
531
532                 mpidr_reg = READ_SPECIALREG(mpidr_el1);
533
534                 if ((mpidr_reg & 0xff00fffffful) == mpidr_fdt)
535                         cpu0 = id;
536         }
537
538         return (TRUE);
539 }
540
541 void
542 cpu_mp_setmaxid(void)
543 {
544 #ifdef FDT
545         int cores;
546
547         cores = ofw_cpu_early_foreach(cpu_find_cpu0_fdt, false);
548         if (cores > 0) {
549                 cores = MIN(cores, MAXCPU);
550                 if (bootverbose)
551                         printf("Found %d CPUs in the device tree\n", cores);
552                 mp_ncpus = cores;
553                 mp_maxid = cores - 1;
554                 cpu_enum_method = CPUS_FDT;
555                 return;
556         }
557 #endif
558
559         if (bootverbose)
560                 printf("No CPU data, limiting to 1 core\n");
561         mp_ncpus = 1;
562         mp_maxid = 0;
563 }
564
565 /*
566  *  Lookup IPI source.
567  */
568 static struct intr_ipi *
569 intr_ipi_lookup(u_int ipi)
570 {
571
572         if (ipi >= INTR_IPI_COUNT)
573                 panic("%s: no such IPI %u", __func__, ipi);
574
575         return (&ipi_sources[ipi]);
576 }
577
578 /*
579  *  interrupt controller dispatch function for IPIs. It should
580  *  be called straight from the interrupt controller, when associated
581  *  interrupt source is learned. Or from anybody who has an interrupt
582  *  source mapped.
583  */
584 void
585 intr_ipi_dispatch(u_int ipi, struct trapframe *tf)
586 {
587         void *arg;
588         struct intr_ipi *ii;
589
590         ii = intr_ipi_lookup(ipi);
591         if (ii->ii_count == NULL)
592                 panic("%s: not setup IPI %u", __func__, ipi);
593
594         intr_ipi_increment_count(ii->ii_count, PCPU_GET(cpuid));
595
596         /*
597          * Supply ipi filter with trapframe argument
598          * if none is registered.
599          */
600         arg = ii->ii_handler_arg != NULL ? ii->ii_handler_arg : tf;
601         ii->ii_handler(arg);
602 }
603
604 #ifdef notyet
605 /*
606  *  Map IPI into interrupt controller.
607  *
608  *  Not SMP coherent.
609  */
610 static int
611 ipi_map(struct intr_irqsrc *isrc, u_int ipi)
612 {
613         boolean_t is_percpu;
614         int error;
615
616         if (ipi >= INTR_IPI_COUNT)
617                 panic("%s: no such IPI %u", __func__, ipi);
618
619         KASSERT(intr_irq_root_dev != NULL, ("%s: no root attached", __func__));
620
621         isrc->isrc_type = INTR_ISRCT_NAMESPACE;
622         isrc->isrc_nspc_type = INTR_IRQ_NSPC_IPI;
623         isrc->isrc_nspc_num = ipi_next_num;
624
625         error = PIC_REGISTER(intr_irq_root_dev, isrc, &is_percpu);
626         if (error == 0) {
627                 isrc->isrc_dev = intr_irq_root_dev;
628                 ipi_next_num++;
629         }
630         return (error);
631 }
632
633 /*
634  *  Setup IPI handler to interrupt source.
635  *
636  *  Note that there could be more ways how to send and receive IPIs
637  *  on a platform like fast interrupts for example. In that case,
638  *  one can call this function with ASIF_NOALLOC flag set and then
639  *  call intr_ipi_dispatch() when appropriate.
640  *
641  *  Not SMP coherent.
642  */
643 int
644 intr_ipi_set_handler(u_int ipi, const char *name, intr_ipi_filter_t *filter,
645     void *arg, u_int flags)
646 {
647         struct intr_irqsrc *isrc;
648         int error;
649
650         if (filter == NULL)
651                 return(EINVAL);
652
653         isrc = intr_ipi_lookup(ipi);
654         if (isrc->isrc_ipifilter != NULL)
655                 return (EEXIST);
656
657         if ((flags & AISHF_NOALLOC) == 0) {
658                 error = ipi_map(isrc, ipi);
659                 if (error != 0)
660                         return (error);
661         }
662
663         isrc->isrc_ipifilter = filter;
664         isrc->isrc_arg = arg;
665         isrc->isrc_handlers = 1;
666         isrc->isrc_count = intr_ipi_setup_counters(name);
667         isrc->isrc_index = 0; /* it should not be used in IPI case */
668
669         if (isrc->isrc_dev != NULL) {
670                 PIC_ENABLE_INTR(isrc->isrc_dev, isrc);
671                 PIC_ENABLE_SOURCE(isrc->isrc_dev, isrc);
672         }
673         return (0);
674 }
675 #endif
676
677 /* Sending IPI */
678 void
679 ipi_all_but_self(u_int ipi)
680 {
681         cpuset_t cpus;
682
683         cpus = all_cpus;
684         CPU_CLR(PCPU_GET(cpuid), &cpus);
685         CTR2(KTR_SMP, "%s: ipi: %x", __func__, ipi);
686         intr_ipi_send(cpus, ipi);
687 }
688
689 void
690 ipi_cpu(int cpu, u_int ipi)
691 {
692         cpuset_t cpus;
693
694         CPU_ZERO(&cpus);
695         CPU_SET(cpu, &cpus);
696
697         CTR3(KTR_SMP, "%s: cpu: %d, ipi: %x", __func__, cpu, ipi);
698         intr_ipi_send(cpus, ipi);
699 }
700
701 void
702 ipi_selected(cpuset_t cpus, u_int ipi)
703 {
704
705         CTR2(KTR_SMP, "%s: ipi: %x", __func__, ipi);
706         intr_ipi_send(cpus, ipi);
707 }