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