1/*
2 * Copyright 2020, Data61, CSIRO (ABN 41 687 119 230)
3 *
4 * SPDX-License-Identifier: GPL-2.0-only
5 */
6
7#include <autoconf.h>
8#include <elfloader/gen_config.h>
9#include <devices_gen.h>
10#include <drivers/smp.h>
11
12#include <printf.h>
13#include <cpuid.h>
14#include <abort.h>
15
16#include <elfloader.h>
17#include <armv/smp.h>
18#include <armv/machine.h>
19
20#if CONFIG_MAX_NUM_NODES > 1
21static volatile int non_boot_lock = 0;
22
23void arm_disable_dcaches(void);
24
25extern void *dtb;
26extern uint32_t dtb_size;
27
28/* Entry point for all CPUs other than the initial. */
29void non_boot_main(void)
30{
31#ifndef CONFIG_ARCH_AARCH64
32    arm_disable_dcaches();
33#endif
34    /* Spin until the first CPU has finished initialisation. */
35    while (!non_boot_lock) {
36#ifndef CONFIG_ARCH_AARCH64
37        cpu_idle();
38#endif
39    }
40
41#ifndef CONFIG_ARM_HYPERVISOR_SUPPORT
42    if (is_hyp_mode()) {
43        extern void leave_hyp(void);
44        leave_hyp();
45    }
46#endif
47    /* Enable the MMU, and enter the kernel. */
48    if (is_hyp_mode()) {
49        arm_enable_hyp_mmu();
50    } else {
51        arm_enable_mmu();
52    }
53
54    /* Jump to the kernel. */
55    ((init_arm_kernel_t)kernel_info.virt_entry)(user_info.phys_region_start,
56                                                user_info.phys_region_end, user_info.phys_virt_offset,
57                                                user_info.virt_entry, (paddr_t)dtb, dtb_size);
58
59    printf("AP Kernel returned back to the elf-loader.\n");
60    abort();
61}
62
63/* TODO: convert imx7 to driver model and remove __attribute__((weak)) */
64void __attribute__((weak)) init_cpus(void)
65{
66    /*
67     * first, figure out which CPU we're booting on.
68     */
69    int booting_cpu_index = -1;
70    word_t mpidr = read_cpuid_mpidr();
71    int i;
72
73    for (i = 0; elfloader_cpus[i].compat != NULL; i++) {
74        if (elfloader_cpus[i].cpu_id == mpidr) {
75            booting_cpu_index = i;
76            break;
77        }
78    }
79
80    if (booting_cpu_index == -1) {
81        printf("Could not find cpu entry for boot cpu (mpidr=0x%x)\n", mpidr);
82        abort();
83    }
84
85    printf("Boot cpu id = 0x%x, index=%d\n", mpidr, booting_cpu_index);
86    /*
87     * We want to boot CPUs in the same cluster before we boot CPUs in another cluster.
88     * This is important on systems like TX2, where the system boots on the A57 cluster,
89     * even though the Denver cluster is the "first" cluster according to the mpidr registers.
90     *
91     * There are a couple of assumptions made here:
92     *  1. The elfloader_cpus array is ordered based on the cpuid field (guaranteed by hardware_gen).
93     *  2. The CPU we boot on is the first CPU in a cluster (not necessarily the first cluster).
94     */
95    int start_index = booting_cpu_index;
96
97    int num_cpus = 1;
98    for (i = start_index + 1; num_cpus < CONFIG_MAX_NUM_NODES && i != start_index; i++) {
99        if (i == booting_cpu_index) {
100            continue;
101        }
102        if (elfloader_cpus[i].compat == NULL) {
103            /* move back to start of the array if we started somewhere in the middle */
104            i = -1;
105            continue;
106        }
107        int ret = plat_cpu_on(&elfloader_cpus[i], core_entry, &core_stacks[num_cpus][0]);
108        if (ret != 0) {
109            printf("Failed to boot cpu 0x%x: %d\n", elfloader_cpus[i].cpu_id, ret);
110            abort();
111        }
112
113        while (!is_core_up(num_cpus));
114        printf("Core %d is up with logic id %d\n", elfloader_cpus[i].cpu_id, num_cpus);
115        num_cpus++;
116    }
117
118#ifdef CONFIG_ARCH_AARCH64
119    /* main CPU has thread id == 0 */
120    MSR("tpidr_el1", 0);
121#endif
122}
123
124void smp_boot(void)
125{
126#ifndef CONFIG_ARCH_AARCH64
127    arm_disable_dcaches();
128#endif
129    init_cpus();
130    non_boot_lock = 1;
131}
132#endif /* CONFIG_MAX_NUM_NODES */
133