1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2019 Michal Meloun <mmel@FreeBSD.org>
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions
8 * are met:
9 * 1. Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * 2. Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 *
15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
25 * SUCH DAMAGE.
26 */
27
28 #include <sys/param.h>
29 #include <sys/systm.h>
30 #include <sys/bus.h>
31 #include <sys/kernel.h>
32 #include <sys/lock.h>
33 #include <sys/mutex.h>
34 #include <sys/smp.h>
35
36 #include <vm/vm.h>
37 #include <vm/pmap.h>
38
39 #include <machine/cpu.h>
40 #include <machine/fdt.h>
41 #include <machine/smp.h>
42 #include <machine/platformvar.h>
43
44 #include <dev/ofw/openfirm.h>
45 #include <dev/ofw/ofw_cpu.h>
46 #include <dev/ofw/ofw_bus_subr.h>
47 #include <dev/psci/psci.h>
48
49 #include <arm/rockchip/rk32xx_mp.h>
50
51 #define IMEM_PHYSBASE 0xFF700000
52 #define IMEM_SIZE 0x00018000
53
54 #define PMU_PHYSBASE 0xFF730000
55 #define PMU_SIZE 0x00010000
56 #define PMU_PWRDN_CON 0x08
57
58 static int running_cpus;
59 static uint32_t psci_mask, pmu_mask;
60 void
rk32xx_mp_setmaxid(platform_t plat)61 rk32xx_mp_setmaxid(platform_t plat)
62 {
63 int ncpu;
64
65 /* If we've already set the global vars don't bother to do it again. */
66 if (mp_ncpus != 0)
67 return;
68
69 /* Read current CP15 Cache Size ID Register */
70 ncpu = cp15_l2ctlr_get();
71 ncpu = CPUV7_L2CTLR_NPROC(ncpu);
72
73 mp_ncpus = ncpu;
74 mp_maxid = ncpu - 1;
75 }
76
77 static void
rk32xx_mp_start_pmu(uint32_t mask)78 rk32xx_mp_start_pmu(uint32_t mask)
79 {
80 bus_space_handle_t imem;
81 bus_space_handle_t pmu;
82 uint32_t val;
83 int i, rv;
84
85 rv = bus_space_map(fdtbus_bs_tag, IMEM_PHYSBASE, IMEM_SIZE, 0, &imem);
86 if (rv != 0)
87 panic("Couldn't map the IMEM\n");
88 rv = bus_space_map(fdtbus_bs_tag, PMU_PHYSBASE, PMU_SIZE, 0, &pmu);
89 if (rv != 0)
90 panic("Couldn't map the PMU\n");
91
92 /* Power off all secondary cores first */
93 val = bus_space_read_4(fdtbus_bs_tag, pmu, PMU_PWRDN_CON);
94 for (i = 1; i < mp_ncpus; i++)
95 val |= 1 << i;
96 bus_space_write_4(fdtbus_bs_tag, pmu, PMU_PWRDN_CON, val);
97 DELAY(5000);
98
99 /* Power up all secondary cores */
100 val = bus_space_read_4(fdtbus_bs_tag, pmu, PMU_PWRDN_CON);
101 for (i = 1; i < mp_ncpus; i++)
102 val &= ~(1 << i);
103 bus_space_write_4(fdtbus_bs_tag, pmu, PMU_PWRDN_CON, val);
104 DELAY(5000);
105
106 /* Copy mpentry address then magic to sram */
107 val = pmap_kextract((vm_offset_t)mpentry);
108 bus_space_write_4(fdtbus_bs_tag, imem, 8, val);
109 dsb();
110 bus_space_write_4(fdtbus_bs_tag, imem, 4, 0xDEADBEAF);
111 dsb();
112
113 sev();
114
115 bus_space_unmap(fdtbus_bs_tag, imem, IMEM_SIZE);
116 bus_space_unmap(fdtbus_bs_tag, pmu, PMU_SIZE);
117 }
118
119 static bool
rk32xx_start_ap(u_int id,phandle_t node,u_int addr_cells,pcell_t * reg)120 rk32xx_start_ap(u_int id, phandle_t node, u_int addr_cells, pcell_t *reg)
121 {
122 int rv;
123 char method[16];
124 uint32_t mask;
125
126 if (!ofw_bus_node_status_okay(node))
127 return(false);
128
129 /* Skip boot CPU. */
130 if (id == 0)
131 return (true);
132
133 if (running_cpus >= mp_ncpus)
134 return (false);
135 running_cpus++;
136
137 mask = 1 << (*reg & 0x0f);
138
139 #ifdef INVARIANTS
140 if ((mask & pmu_mask) || (mask & psci_mask))
141 printf("CPU: Duplicated register value: 0x%X for CPU(%d)\n",
142 *reg, id);
143 #endif
144 rv = OF_getprop(node, "enable-method", method, sizeof(method));
145 if (rv > 0 && strcmp(method, "psci") == 0) {
146 psci_mask |= mask;
147 rv = psci_cpu_on(*reg, pmap_kextract((vm_offset_t)mpentry), id);
148 if (rv != PSCI_RETVAL_SUCCESS) {
149 printf("Failed to start CPU(%d)\n", id);
150 return (false);
151 }
152 return (true);
153 }
154
155 pmu_mask |= mask;
156 return (true);
157 }
158
159 void
rk32xx_mp_start_ap(platform_t plat)160 rk32xx_mp_start_ap(platform_t plat)
161 {
162
163 ofw_cpu_early_foreach(rk32xx_start_ap, true);
164 if (pmu_mask != 0 && psci_mask != 0) {
165 printf("Inconsistent CPUs startup methods detected.\n");
166 printf("Only PSCI enabled cores will be started.\n");
167 return;
168 }
169 if (pmu_mask != 0)
170 rk32xx_mp_start_pmu(pmu_mask);
171 }
172