1 /* 2 * Copyright (c) 2016-2019, 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 */ 13 14 #include <string.h> 15 16 #include <libfdt.h> 17 18 #include <common/debug.h> 19 #include <drivers/console.h> 20 #include <lib/psci/psci.h> 21 22 #include <common/fdt_fixup.h> 23 24 static int append_psci_compatible(void *fdt, int offs, const char *str) 25 { 26 return fdt_appendprop(fdt, offs, "compatible", str, strlen(str) + 1); 27 } 28 29 int dt_add_psci_node(void *fdt) 30 { 31 int offs; 32 33 if (fdt_path_offset(fdt, "/psci") >= 0) { 34 WARN("PSCI Device Tree node already exists!\n"); 35 return 0; 36 } 37 38 offs = fdt_path_offset(fdt, "/"); 39 if (offs < 0) 40 return -1; 41 offs = fdt_add_subnode(fdt, offs, "psci"); 42 if (offs < 0) 43 return -1; 44 if (append_psci_compatible(fdt, offs, "arm,psci-1.0")) 45 return -1; 46 if (append_psci_compatible(fdt, offs, "arm,psci-0.2")) 47 return -1; 48 if (append_psci_compatible(fdt, offs, "arm,psci")) 49 return -1; 50 if (fdt_setprop_string(fdt, offs, "method", "smc")) 51 return -1; 52 if (fdt_setprop_u32(fdt, offs, "cpu_suspend", PSCI_CPU_SUSPEND_AARCH64)) 53 return -1; 54 if (fdt_setprop_u32(fdt, offs, "cpu_off", PSCI_CPU_OFF)) 55 return -1; 56 if (fdt_setprop_u32(fdt, offs, "cpu_on", PSCI_CPU_ON_AARCH64)) 57 return -1; 58 if (fdt_setprop_u32(fdt, offs, "sys_poweroff", PSCI_SYSTEM_OFF)) 59 return -1; 60 if (fdt_setprop_u32(fdt, offs, "sys_reset", PSCI_SYSTEM_RESET)) 61 return -1; 62 return 0; 63 } 64 65 /* 66 * Find the first subnode that has a "device_type" property with the value 67 * "cpu" and which's enable-method is not "psci" (yet). 68 * Returns 0 if no such subnode is found, so all have already been patched 69 * or none have to be patched in the first place. 70 * Returns 1 if *one* such subnode has been found and successfully changed 71 * to "psci". 72 * Returns -1 on error. 73 * 74 * Call in a loop until it returns 0. Recalculate the node offset after 75 * it has returned 1. 76 */ 77 static int dt_update_one_cpu_node(void *fdt, int offset) 78 { 79 int offs; 80 81 /* Iterate over all subnodes to find those with device_type = "cpu". */ 82 for (offs = fdt_first_subnode(fdt, offset); offs >= 0; 83 offs = fdt_next_subnode(fdt, offs)) { 84 const char *prop; 85 int len; 86 87 prop = fdt_getprop(fdt, offs, "device_type", &len); 88 if (!prop) 89 continue; 90 if (memcmp(prop, "cpu", 4) != 0 || len != 4) 91 continue; 92 93 /* Ignore any nodes which already use "psci". */ 94 prop = fdt_getprop(fdt, offs, "enable-method", &len); 95 if (prop && memcmp(prop, "psci", 5) == 0 && len == 5) 96 continue; 97 98 if (fdt_setprop_string(fdt, offs, "enable-method", "psci")) 99 return -1; 100 /* 101 * Subnode found and patched. 102 * Restart to accommodate potentially changed offsets. 103 */ 104 return 1; 105 } 106 107 if (offs == -FDT_ERR_NOTFOUND) 108 return 0; 109 110 return offs; 111 } 112 113 int dt_add_psci_cpu_enable_methods(void *fdt) 114 { 115 int offs, ret; 116 117 do { 118 offs = fdt_path_offset(fdt, "/cpus"); 119 if (offs < 0) 120 return offs; 121 122 ret = dt_update_one_cpu_node(fdt, offs); 123 } while (ret > 0); 124 125 return ret; 126 } 127