1 /*
2 * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 /*
8 * Contains generic routines to fix up the device tree blob passed on to
9 * payloads like BL32 and BL33 (and further down the boot chain).
10 * This allows to easily add PSCI nodes, when the original DT does not have
11 * it or advertises another method.
12 * Also it supports to add reserved memory nodes to describe memory that
13 * is used by the secure world, so that non-secure software avoids using
14 * that.
15 */
16
17 #include <errno.h>
18 #include <stdio.h>
19 #include <string.h>
20
21 #include <libfdt.h>
22
23 #include <arch.h>
24 #include <common/debug.h>
25 #include <common/fdt_fixup.h>
26 #include <common/fdt_wrappers.h>
27 #include <drivers/console.h>
28 #include <lib/psci/psci.h>
29 #include <lib/utils_def.h>
30 #include <plat/common/platform.h>
31
32
append_psci_compatible(void * fdt,int offs,const char * str)33 static int append_psci_compatible(void *fdt, int offs, const char *str)
34 {
35 return fdt_appendprop(fdt, offs, "compatible", str, strlen(str) + 1);
36 }
37
38 /*
39 * Those defines are for PSCI v0.1 legacy clients, which we expect to use
40 * the same execution state (AArch32/AArch64) as TF-A.
41 * Kernels running in AArch32 on an AArch64 TF-A should use PSCI v0.2.
42 */
43 #ifdef __aarch64__
44 #define PSCI_CPU_SUSPEND_FNID PSCI_CPU_SUSPEND_AARCH64
45 #define PSCI_CPU_ON_FNID PSCI_CPU_ON_AARCH64
46 #else
47 #define PSCI_CPU_SUSPEND_FNID PSCI_CPU_SUSPEND_AARCH32
48 #define PSCI_CPU_ON_FNID PSCI_CPU_ON_AARCH32
49 #endif
50
51 /*******************************************************************************
52 * dt_add_psci_node() - Add a PSCI node into an existing device tree
53 * @fdt: pointer to the device tree blob in memory
54 *
55 * Add a device tree node describing PSCI into the root level of an existing
56 * device tree blob in memory.
57 * This will add v0.1, v0.2 and v1.0 compatible strings and the standard
58 * function IDs for v0.1 compatibility.
59 * An existing PSCI node will not be touched, the function will return success
60 * in this case. This function will not touch the /cpus enable methods, use
61 * dt_add_psci_cpu_enable_methods() for that.
62 *
63 * Return: 0 on success, -1 otherwise.
64 ******************************************************************************/
dt_add_psci_node(void * fdt)65 int dt_add_psci_node(void *fdt)
66 {
67 int offs;
68
69 if (fdt_path_offset(fdt, "/psci") >= 0) {
70 WARN("PSCI Device Tree node already exists!\n");
71 return 0;
72 }
73
74 offs = fdt_path_offset(fdt, "/");
75 if (offs < 0) {
76 return -1;
77 }
78 offs = fdt_add_subnode(fdt, offs, "psci");
79 if (offs < 0) {
80 return -1;
81 }
82 if (append_psci_compatible(fdt, offs, "arm,psci-1.0") != 0) {
83 return -1;
84 }
85 if (append_psci_compatible(fdt, offs, "arm,psci-0.2") != 0) {
86 return -1;
87 }
88 if (append_psci_compatible(fdt, offs, "arm,psci") != 0) {
89 return -1;
90 }
91 if (fdt_setprop_string(fdt, offs, "method", "smc") != 0) {
92 return -1;
93 }
94 if (fdt_setprop_u32(fdt, offs, "cpu_suspend", PSCI_CPU_SUSPEND_FNID) != 0) {
95 return -1;
96 }
97 if (fdt_setprop_u32(fdt, offs, "cpu_off", PSCI_CPU_OFF) != 0) {
98 return -1;
99 }
100 if (fdt_setprop_u32(fdt, offs, "cpu_on", PSCI_CPU_ON_FNID) != 0) {
101 return -1;
102 }
103 return 0;
104 }
105
106 /*
107 * Find the first subnode that has a "device_type" property with the value
108 * "cpu" and which's enable-method is not "psci" (yet).
109 * Returns 0 if no such subnode is found, so all have already been patched
110 * or none have to be patched in the first place.
111 * Returns 1 if *one* such subnode has been found and successfully changed
112 * to "psci".
113 * Returns negative values on error.
114 *
115 * Call in a loop until it returns 0. Recalculate the node offset after
116 * it has returned 1.
117 */
dt_update_one_cpu_node(void * fdt,int offset)118 static int dt_update_one_cpu_node(void *fdt, int offset)
119 {
120 int offs;
121
122 /* Iterate over all subnodes to find those with device_type = "cpu". */
123 for (offs = fdt_first_subnode(fdt, offset); offs >= 0;
124 offs = fdt_next_subnode(fdt, offs)) {
125 const char *prop;
126 int len;
127 int ret;
128
129 prop = fdt_getprop(fdt, offs, "device_type", &len);
130 if (prop == NULL)
131 continue;
132 if ((strcmp(prop, "cpu") != 0) || (len != 4))
133 continue;
134
135 /* Ignore any nodes which already use "psci". */
136 prop = fdt_getprop(fdt, offs, "enable-method", &len);
137 if ((prop != NULL) &&
138 (strcmp(prop, "psci") == 0) && (len == 5))
139 continue;
140
141 ret = fdt_setprop_string(fdt, offs, "enable-method", "psci");
142 if (ret < 0)
143 return ret;
144 /*
145 * Subnode found and patched.
146 * Restart to accommodate potentially changed offsets.
147 */
148 return 1;
149 }
150
151 if (offs == -FDT_ERR_NOTFOUND)
152 return 0;
153
154 return offs;
155 }
156
157 /*******************************************************************************
158 * dt_add_psci_cpu_enable_methods() - switch CPU nodes in DT to use PSCI
159 * @fdt: pointer to the device tree blob in memory
160 *
161 * Iterate over all CPU device tree nodes (/cpus/cpu@x) in memory to change
162 * the enable-method to PSCI. This will add the enable-method properties, if
163 * required, or will change existing properties to read "psci".
164 *
165 * Return: 0 on success, or a negative error value otherwise.
166 ******************************************************************************/
167
dt_add_psci_cpu_enable_methods(void * fdt)168 int dt_add_psci_cpu_enable_methods(void *fdt)
169 {
170 int offs, ret;
171
172 do {
173 offs = fdt_path_offset(fdt, "/cpus");
174 if (offs < 0) {
175 return offs;
176 }
177
178 ret = dt_update_one_cpu_node(fdt, offs);
179 } while (ret > 0);
180
181 return ret;
182 }
183
184 #define HIGH_BITS_U32(x) ((sizeof(x) > 4U) ? (uint32_t)((x) >> 32) : (uint32_t)0)
185
186 /*******************************************************************************
187 * fdt_add_reserved_memory() - reserve (secure) memory regions in DT
188 * @dtb: pointer to the device tree blob in memory
189 * @node_name: name of the subnode to be used
190 * @base: physical base address of the reserved region
191 * @size: size of the reserved region
192 *
193 * Add a region of memory to the /reserved-memory node in a device tree in
194 * memory, creating that node if required. Each region goes into a subnode
195 * of that node and has a @node_name, a @base address and a @size.
196 * This will prevent any device tree consumer from using that memory. It
197 * can be used to announce secure memory regions, as it adds the "no-map"
198 * property to prevent mapping and speculative operations on that region.
199 *
200 * See reserved-memory/reserved-memory.txt in the (Linux kernel) DT binding
201 * documentation for details.
202 * According to this binding, the address-cells and size-cells must match
203 * those of the root node.
204 *
205 * Return: 0 on success, a negative error value otherwise.
206 ******************************************************************************/
fdt_add_reserved_memory(void * dtb,const char * node_name,uintptr_t base,size_t size)207 int fdt_add_reserved_memory(void *dtb, const char *node_name,
208 uintptr_t base, size_t size)
209 {
210 int offs = fdt_path_offset(dtb, "/reserved-memory");
211 int node;
212 uint32_t addresses[4];
213 int ac, sc;
214 unsigned int idx = 0;
215
216 ac = fdt_address_cells(dtb, 0);
217 sc = fdt_size_cells(dtb, 0);
218 if (ac < 0 || sc < 0) {
219 return -EINVAL;
220 }
221 if (offs < 0) { /* create if not existing yet */
222 offs = fdt_add_subnode(dtb, 0, "reserved-memory");
223 if (offs < 0) {
224 return offs;
225 }
226 fdt_setprop_u32(dtb, offs, "#address-cells", (uint32_t)ac);
227 fdt_setprop_u32(dtb, offs, "#size-cells", (uint32_t)sc);
228 fdt_setprop(dtb, offs, "ranges", NULL, 0);
229 }
230
231 /* Check for existing regions */
232 fdt_for_each_subnode(node, dtb, offs) {
233 uintptr_t c_base;
234 size_t c_size;
235 int ret;
236
237 ret = fdt_get_reg_props_by_index(dtb, node, 0, &c_base, &c_size);
238 /* Ignore illegal subnodes */
239 if (ret != 0) {
240 continue;
241 }
242
243 /* existing region entirely contains the new region */
244 if (base >= c_base && (base + size) <= (c_base + c_size)) {
245 return 0;
246 }
247 }
248
249 if (ac > 1) {
250 addresses[idx] = cpu_to_fdt32(HIGH_BITS_U32(base));
251 idx++;
252 }
253 addresses[idx] = cpu_to_fdt32(LO(base));
254 idx++;
255 if (sc > 1) {
256 addresses[idx] = cpu_to_fdt32(HIGH_BITS_U32(size));
257 idx++;
258 }
259 addresses[idx] = cpu_to_fdt32(LO(size));
260 idx++;
261 offs = fdt_add_subnode(dtb, offs, node_name);
262 fdt_setprop(dtb, offs, "no-map", NULL, 0);
263 fdt_setprop(dtb, offs, "reg", addresses, idx * sizeof(uint32_t));
264
265 return 0;
266 }
267
268 /*******************************************************************************
269 * fdt_add_cpu() Add a new CPU node to the DT
270 * @dtb: Pointer to the device tree blob in memory
271 * @parent: Offset of the parent node
272 * @mpidr: MPIDR for the current CPU
273 *
274 * Create and add a new cpu node to a DTB.
275 *
276 * Return the offset of the new node or a negative value in case of error
277 ******************************************************************************/
278
fdt_add_cpu(void * dtb,int parent,u_register_t mpidr)279 static int fdt_add_cpu(void *dtb, int parent, u_register_t mpidr)
280 {
281 int cpu_offs;
282 int err;
283 char snode_name[15];
284 uint64_t reg_prop;
285
286 reg_prop = mpidr & MPID_MASK & ~MPIDR_MT_MASK;
287
288 snprintf(snode_name, sizeof(snode_name), "cpu@%x",
289 (unsigned int)reg_prop);
290
291 cpu_offs = fdt_add_subnode(dtb, parent, snode_name);
292 if (cpu_offs < 0) {
293 ERROR ("FDT: add subnode \"%s\" failed: %i\n",
294 snode_name, cpu_offs);
295 return cpu_offs;
296 }
297
298 err = fdt_setprop_string(dtb, cpu_offs, "compatible", "arm,armv8");
299 if (err < 0) {
300 ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
301 "compatible", cpu_offs);
302 return err;
303 }
304
305 err = fdt_setprop_u64(dtb, cpu_offs, "reg", reg_prop);
306 if (err < 0) {
307 ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
308 "reg", cpu_offs);
309 return err;
310 }
311
312 err = fdt_setprop_string(dtb, cpu_offs, "device_type", "cpu");
313 if (err < 0) {
314 ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
315 "device_type", cpu_offs);
316 return err;
317 }
318
319 err = fdt_setprop_string(dtb, cpu_offs, "enable-method", "psci");
320 if (err < 0) {
321 ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
322 "enable-method", cpu_offs);
323 return err;
324 }
325
326 return cpu_offs;
327 }
328
329 /******************************************************************************
330 * fdt_add_cpus_node() - Add the cpus node to the DTB
331 * @dtb: pointer to the device tree blob in memory
332 * @afflv0: Maximum number of threads per core (affinity level 0).
333 * @afflv1: Maximum number of CPUs per cluster (affinity level 1).
334 * @afflv2: Maximum number of clusters (affinity level 2).
335 *
336 * Iterate over all the possible MPIDs given the maximum affinity levels and
337 * add a cpus node to the DTB with all the valid CPUs on the system.
338 * If there is already a /cpus node, exit gracefully
339 *
340 * A system with two CPUs would generate a node equivalent or similar to:
341 *
342 * cpus {
343 * #address-cells = <2>;
344 * #size-cells = <0>;
345 *
346 * cpu0: cpu@0 {
347 * compatible = "arm,armv8";
348 * reg = <0x0 0x0>;
349 * device_type = "cpu";
350 * enable-method = "psci";
351 * };
352 * cpu1: cpu@10000 {
353 * compatible = "arm,armv8";
354 * reg = <0x0 0x100>;
355 * device_type = "cpu";
356 * enable-method = "psci";
357 * };
358 * };
359 *
360 * Full documentation about the CPU bindings can be found at:
361 * https://www.kernel.org/doc/Documentation/devicetree/bindings/arm/cpus.txt
362 *
363 * Return the offset of the node or a negative value on error.
364 ******************************************************************************/
365
fdt_add_cpus_node(void * dtb,unsigned int afflv0,unsigned int afflv1,unsigned int afflv2)366 int fdt_add_cpus_node(void *dtb, unsigned int afflv0,
367 unsigned int afflv1, unsigned int afflv2)
368 {
369 int offs;
370 int err;
371 unsigned int i, j, k;
372 u_register_t mpidr;
373 int cpuid;
374
375 if (fdt_path_offset(dtb, "/cpus") >= 0) {
376 return -EEXIST;
377 }
378
379 offs = fdt_add_subnode(dtb, 0, "cpus");
380 if (offs < 0) {
381 ERROR ("FDT: add subnode \"cpus\" node to parent node failed");
382 return offs;
383 }
384
385 err = fdt_setprop_u32(dtb, offs, "#address-cells", 2);
386 if (err < 0) {
387 ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
388 "#address-cells", offs);
389 return err;
390 }
391
392 err = fdt_setprop_u32(dtb, offs, "#size-cells", 0);
393 if (err < 0) {
394 ERROR ("FDT: write to \"%s\" property of node at offset %i failed\n",
395 "#size-cells", offs);
396 return err;
397 }
398
399 /*
400 * Populate the node with the CPUs.
401 * As libfdt prepends subnodes within a node, reverse the index count
402 * so the CPU nodes would be better ordered.
403 */
404 for (i = afflv2; i > 0U; i--) {
405 for (j = afflv1; j > 0U; j--) {
406 for (k = afflv0; k > 0U; k--) {
407 mpidr = ((i - 1) << MPIDR_AFF2_SHIFT) |
408 ((j - 1) << MPIDR_AFF1_SHIFT) |
409 ((k - 1) << MPIDR_AFF0_SHIFT) |
410 (read_mpidr_el1() & MPIDR_MT_MASK);
411
412 cpuid = plat_core_pos_by_mpidr(mpidr);
413 if (cpuid >= 0) {
414 /* Valid MPID found */
415 err = fdt_add_cpu(dtb, offs, mpidr);
416 if (err < 0) {
417 ERROR ("FDT: %s 0x%08x\n",
418 "error adding CPU",
419 (uint32_t)mpidr);
420 return err;
421 }
422 }
423 }
424 }
425 }
426
427 return offs;
428 }
429
430 /*******************************************************************************
431 * fdt_add_cpu_idle_states() - add PSCI CPU idle states to cpu nodes in the DT
432 * @dtb: pointer to the device tree blob in memory
433 * @states: array of idle state descriptions, ending with empty element
434 *
435 * Add information about CPU idle states to the devicetree. This function
436 * assumes that CPU idle states are not already present in the devicetree, and
437 * that all CPU states are equally applicable to all CPUs.
438 *
439 * See arm/idle-states.yaml and arm/psci.yaml in the (Linux kernel) DT binding
440 * documentation for more details.
441 *
442 * Return: 0 on success, a negative error value otherwise.
443 ******************************************************************************/
fdt_add_cpu_idle_states(void * dtb,const struct psci_cpu_idle_state * state)444 int fdt_add_cpu_idle_states(void *dtb, const struct psci_cpu_idle_state *state)
445 {
446 int cpu_node, cpus_node, idle_states_node, ret;
447 uint32_t count, phandle;
448
449 ret = fdt_find_max_phandle(dtb, &phandle);
450 phandle++;
451 if (ret < 0) {
452 return ret;
453 }
454
455 cpus_node = fdt_path_offset(dtb, "/cpus");
456 if (cpus_node < 0) {
457 return cpus_node;
458 }
459
460 /* Create the idle-states node and its child nodes. */
461 idle_states_node = fdt_add_subnode(dtb, cpus_node, "idle-states");
462 if (idle_states_node < 0) {
463 return idle_states_node;
464 }
465
466 ret = fdt_setprop_string(dtb, idle_states_node, "entry-method", "psci");
467 if (ret < 0) {
468 return ret;
469 }
470
471 for (count = 0U; state->name != NULL; count++, phandle++, state++) {
472 int idle_state_node;
473
474 idle_state_node = fdt_add_subnode(dtb, idle_states_node,
475 state->name);
476 if (idle_state_node < 0) {
477 return idle_state_node;
478 }
479
480 fdt_setprop_string(dtb, idle_state_node, "compatible",
481 "arm,idle-state");
482 fdt_setprop_u32(dtb, idle_state_node, "arm,psci-suspend-param",
483 state->power_state);
484 if (state->local_timer_stop) {
485 fdt_setprop_empty(dtb, idle_state_node,
486 "local-timer-stop");
487 }
488 fdt_setprop_u32(dtb, idle_state_node, "entry-latency-us",
489 state->entry_latency_us);
490 fdt_setprop_u32(dtb, idle_state_node, "exit-latency-us",
491 state->exit_latency_us);
492 fdt_setprop_u32(dtb, idle_state_node, "min-residency-us",
493 state->min_residency_us);
494 if (state->wakeup_latency_us) {
495 fdt_setprop_u32(dtb, idle_state_node,
496 "wakeup-latency-us",
497 state->wakeup_latency_us);
498 }
499 fdt_setprop_u32(dtb, idle_state_node, "phandle", phandle);
500 }
501
502 if (count == 0U) {
503 return 0;
504 }
505
506 /* Link each cpu node to the idle state nodes. */
507 fdt_for_each_subnode(cpu_node, dtb, cpus_node) {
508 const char *device_type;
509 fdt32_t *value;
510
511 /* Only process child nodes with device_type = "cpu". */
512 device_type = fdt_getprop(dtb, cpu_node, "device_type", NULL);
513 if (device_type == NULL || strcmp(device_type, "cpu") != 0) {
514 continue;
515 }
516
517 /* Allocate space for the list of phandles. */
518 ret = fdt_setprop_placeholder(dtb, cpu_node, "cpu-idle-states",
519 count * sizeof(phandle),
520 (void **)&value);
521 if (ret < 0) {
522 return ret;
523 }
524
525 /* Fill in the phandles of the idle state nodes. */
526 for (uint32_t i = 0U; i < count; ++i) {
527 value[i] = cpu_to_fdt32(phandle - count + i);
528 }
529 }
530
531 return 0;
532 }
533
534 /**
535 * fdt_adjust_gic_redist() - Adjust GICv3 redistributor size
536 * @dtb: Pointer to the DT blob in memory
537 * @nr_cores: Number of CPU cores on this system.
538 * @gicr_base: Base address of the first GICR frame, or ~0 if unchanged
539 * @gicr_frame_size: Size of the GICR frame per core
540 *
541 * On a GICv3 compatible interrupt controller, the redistributor provides
542 * a number of 64k pages per each supported core. So with a dynamic topology,
543 * this size cannot be known upfront and thus can't be hardcoded into the DTB.
544 *
545 * Find the DT node describing the GICv3 interrupt controller, and adjust
546 * the size of the redistributor to match the number of actual cores on
547 * this system.
548 * A GICv4 compatible redistributor uses four 64K pages per core, whereas GICs
549 * without support for direct injection of virtual interrupts use two 64K pages.
550 * The @gicr_frame_size parameter should be 262144 and 131072, respectively.
551 * Also optionally allow adjusting the GICR frame base address, when this is
552 * different due to ITS frames between distributor and redistributor.
553 *
554 * Return: 0 on success, negative error value otherwise.
555 */
fdt_adjust_gic_redist(void * dtb,unsigned int nr_cores,uintptr_t gicr_base,unsigned int gicr_frame_size)556 int fdt_adjust_gic_redist(void *dtb, unsigned int nr_cores,
557 uintptr_t gicr_base, unsigned int gicr_frame_size)
558 {
559 int offset = fdt_node_offset_by_compatible(dtb, 0, "arm,gic-v3");
560 uint64_t reg_64;
561 uint32_t reg_32;
562 void *val;
563 int parent, ret;
564 int ac, sc;
565
566 if (offset < 0) {
567 return offset;
568 }
569
570 parent = fdt_parent_offset(dtb, offset);
571 if (parent < 0) {
572 return parent;
573 }
574 ac = fdt_address_cells(dtb, parent);
575 sc = fdt_size_cells(dtb, parent);
576 if (ac < 0 || sc < 0) {
577 return -EINVAL;
578 }
579
580 if (gicr_base != INVALID_BASE_ADDR) {
581 if (ac == 1) {
582 reg_32 = cpu_to_fdt32(gicr_base);
583 val = ®_32;
584 } else {
585 reg_64 = cpu_to_fdt64(gicr_base);
586 val = ®_64;
587 }
588 /*
589 * The redistributor base address is the second address in
590 * the "reg" entry, so we have to skip one address and one
591 * size cell.
592 */
593 ret = fdt_setprop_inplace_namelen_partial(dtb, offset,
594 "reg", 3,
595 (ac + sc) * 4,
596 val, ac * 4);
597 if (ret < 0) {
598 return ret;
599 }
600 }
601
602 if (sc == 1) {
603 reg_32 = cpu_to_fdt32(nr_cores * gicr_frame_size);
604 val = ®_32;
605 } else {
606 reg_64 = cpu_to_fdt64(nr_cores * (uint64_t)gicr_frame_size);
607 val = ®_64;
608 }
609
610 /*
611 * The redistributor is described in the second "reg" entry.
612 * So we have to skip one address and one size cell, then another
613 * address cell to get to the second size cell.
614 */
615 return fdt_setprop_inplace_namelen_partial(dtb, offset, "reg", 3,
616 (ac + sc + ac) * 4,
617 val, sc * 4);
618 }
619 /**
620 * fdt_set_mac_address () - store MAC address in device tree
621 * @dtb: pointer to the device tree blob in memory
622 * @eth_idx: number of Ethernet interface in /aliases node
623 * @mac_addr: pointer to 6 byte MAC address to store
624 *
625 * Use the generic local-mac-address property in a network device DT node
626 * to define the MAC address this device should be using. Many platform
627 * network devices lack device-specific non-volatile storage to hold this
628 * address, and leave it up to firmware to find and store a unique MAC
629 * address in the DT.
630 * The MAC address could be read from some board or firmware defined storage,
631 * or could be derived from some other unique property like a serial number.
632 *
633 * Return: 0 on success, a negative libfdt error value otherwise.
634 */
fdt_set_mac_address(void * dtb,unsigned int ethernet_idx,const uint8_t * mac_addr)635 int fdt_set_mac_address(void *dtb, unsigned int ethernet_idx,
636 const uint8_t *mac_addr)
637 {
638 char eth_alias[12];
639 const char *path;
640 int node;
641
642 if (ethernet_idx > 9U) {
643 return -FDT_ERR_BADVALUE;
644 }
645 snprintf(eth_alias, sizeof(eth_alias), "ethernet%d", ethernet_idx);
646
647 path = fdt_get_alias(dtb, eth_alias);
648 if (path == NULL) {
649 return -FDT_ERR_NOTFOUND;
650 }
651
652 node = fdt_path_offset(dtb, path);
653 if (node < 0) {
654 ERROR("Path \"%s\" not found in DT: %d\n", path, node);
655 return node;
656 }
657
658 return fdt_setprop(dtb, node, "local-mac-address", mac_addr, 6);
659 }
660