Peng Fan | 9f779fa | 2019-08-26 08:11:56 +0000 | [diff] [blame] | 1 | // SPDX-License-Identifier: GPL-2.0+ |
| 2 | /* |
| 3 | * Copyright 2019 NXP |
| 4 | */ |
| 5 | |
| 6 | #include <common.h> |
| 7 | #include <asm/arch/sci/sci.h> |
Peng Fan | 01cacf9 | 2019-08-26 08:12:06 +0000 | [diff] [blame] | 8 | #include <asm/arch/sys_proto.h> |
Peng Fan | 9f779fa | 2019-08-26 08:11:56 +0000 | [diff] [blame] | 9 | #include <dm/ofnode.h> |
| 10 | #include <fdt_support.h> |
Peng Fan | f44afd5 | 2020-05-05 20:28:37 +0800 | [diff] [blame] | 11 | #include <linux/libfdt.h> |
Peng Fan | 9f779fa | 2019-08-26 08:11:56 +0000 | [diff] [blame] | 12 | |
| 13 | DECLARE_GLOBAL_DATA_PTR; |
| 14 | |
| 15 | static bool check_owned_resource(sc_rsrc_t rsrc_id) |
| 16 | { |
| 17 | bool owned; |
| 18 | |
| 19 | owned = sc_rm_is_resource_owned(-1, rsrc_id); |
| 20 | |
| 21 | return owned; |
| 22 | } |
| 23 | |
| 24 | static int disable_fdt_node(void *blob, int nodeoffset) |
| 25 | { |
| 26 | int rc, ret; |
| 27 | const char *status = "disabled"; |
| 28 | |
| 29 | do { |
| 30 | rc = fdt_setprop(blob, nodeoffset, "status", status, |
| 31 | strlen(status) + 1); |
| 32 | if (rc) { |
| 33 | if (rc == -FDT_ERR_NOSPACE) { |
| 34 | ret = fdt_increase_size(blob, 512); |
| 35 | if (ret) |
| 36 | return ret; |
| 37 | } |
| 38 | } |
| 39 | } while (rc == -FDT_ERR_NOSPACE); |
| 40 | |
| 41 | return rc; |
| 42 | } |
| 43 | |
| 44 | static void update_fdt_with_owned_resources(void *blob) |
| 45 | { |
| 46 | /* |
| 47 | * Traverses the fdt nodes, check its power domain and use |
| 48 | * the resource id in the power domain for checking whether |
| 49 | * it is owned by current partition |
| 50 | */ |
| 51 | struct fdtdec_phandle_args args; |
| 52 | int offset = 0, depth = 0; |
| 53 | u32 rsrc_id; |
| 54 | int rc, i; |
| 55 | |
| 56 | for (offset = fdt_next_node(blob, offset, &depth); offset > 0; |
| 57 | offset = fdt_next_node(blob, offset, &depth)) { |
| 58 | debug("Node name: %s, depth %d\n", |
| 59 | fdt_get_name(blob, offset, NULL), depth); |
| 60 | |
| 61 | if (!fdt_get_property(blob, offset, "power-domains", NULL)) { |
| 62 | debug(" - ignoring node %s\n", |
| 63 | fdt_get_name(blob, offset, NULL)); |
| 64 | continue; |
| 65 | } |
| 66 | |
| 67 | if (!fdtdec_get_is_enabled(blob, offset)) { |
| 68 | debug(" - ignoring node %s\n", |
| 69 | fdt_get_name(blob, offset, NULL)); |
| 70 | continue; |
| 71 | } |
| 72 | |
| 73 | i = 0; |
| 74 | while (true) { |
| 75 | rc = fdtdec_parse_phandle_with_args(blob, offset, |
| 76 | "power-domains", |
| 77 | "#power-domain-cells", |
| 78 | 0, i++, &args); |
| 79 | if (rc == -ENOENT) { |
| 80 | break; |
| 81 | } else if (rc) { |
| 82 | printf("Parse power-domains of %s wrong: %d\n", |
| 83 | fdt_get_name(blob, offset, NULL), rc); |
| 84 | continue; |
| 85 | } |
| 86 | |
| 87 | rsrc_id = args.args[0]; |
| 88 | |
| 89 | if (!check_owned_resource(rsrc_id)) { |
| 90 | rc = disable_fdt_node(blob, offset); |
| 91 | if (!rc) { |
| 92 | printf("Disable %s rsrc %u not owned\n", |
| 93 | fdt_get_name(blob, offset, NULL), |
| 94 | rsrc_id); |
| 95 | } else { |
| 96 | printf("Unable to disable %s, err=%s\n", |
| 97 | fdt_get_name(blob, offset, NULL), |
| 98 | fdt_strerror(rc)); |
| 99 | } |
| 100 | } |
| 101 | } |
| 102 | } |
| 103 | } |
| 104 | |
Peng Fan | 01cacf9 | 2019-08-26 08:12:06 +0000 | [diff] [blame] | 105 | static int config_smmu_resource_sid(int rsrc, int sid) |
| 106 | { |
| 107 | int err; |
| 108 | |
| 109 | if (!check_owned_resource(rsrc)) { |
| 110 | printf("%s rsrc[%d] not owned\n", __func__, rsrc); |
| 111 | return -1; |
| 112 | } |
| 113 | err = sc_rm_set_master_sid(-1, rsrc, sid); |
| 114 | debug("set_master_sid rsrc=%d sid=0x%x err=%d\n", rsrc, sid, err); |
| 115 | if (err != SC_ERR_NONE) { |
| 116 | pr_err("fail set_master_sid rsrc=%d sid=0x%x err=%d\n", rsrc, sid, err); |
| 117 | return -EINVAL; |
| 118 | } |
| 119 | |
| 120 | return 0; |
| 121 | } |
| 122 | |
| 123 | static int config_smmu_fdt_device_sid(void *blob, int device_offset, int sid) |
| 124 | { |
| 125 | const char *name = fdt_get_name(blob, device_offset, NULL); |
| 126 | struct fdtdec_phandle_args args; |
| 127 | int rsrc, ret; |
| 128 | int proplen; |
| 129 | const fdt32_t *prop; |
| 130 | int i; |
| 131 | |
| 132 | prop = fdt_getprop(blob, device_offset, "fsl,sc_rsrc_id", &proplen); |
| 133 | if (prop) { |
| 134 | int i; |
| 135 | |
| 136 | debug("configure node %s sid 0x%x for %d resources\n", |
| 137 | name, sid, (int)(proplen / sizeof(fdt32_t))); |
| 138 | for (i = 0; i < proplen / sizeof(fdt32_t); ++i) { |
| 139 | ret = config_smmu_resource_sid(fdt32_to_cpu(prop[i]), |
| 140 | sid); |
| 141 | if (ret) |
| 142 | return ret; |
| 143 | } |
| 144 | |
| 145 | return 0; |
| 146 | } |
| 147 | |
| 148 | i = 0; |
| 149 | while (true) { |
| 150 | ret = fdtdec_parse_phandle_with_args(blob, device_offset, |
| 151 | "power-domains", |
| 152 | "#power-domain-cells", |
| 153 | 0, i++, &args); |
| 154 | if (ret == -ENOENT) { |
| 155 | break; |
| 156 | } else if (ret) { |
| 157 | printf("Parse power-domains of node %s wrong: %d\n", |
| 158 | fdt_get_name(blob, device_offset, NULL), ret); |
| 159 | continue; |
| 160 | } |
| 161 | |
| 162 | debug("configure node %s sid 0x%x rsrc=%d\n", |
| 163 | name, sid, rsrc); |
| 164 | rsrc = args.args[0]; |
| 165 | |
| 166 | ret = config_smmu_resource_sid(rsrc, sid); |
| 167 | if (ret) |
| 168 | break; |
| 169 | } |
| 170 | |
| 171 | return ret; |
| 172 | } |
| 173 | |
| 174 | static int config_smmu_fdt(void *blob) |
| 175 | { |
| 176 | int offset, proplen, i, ret; |
| 177 | const fdt32_t *prop; |
| 178 | const char *name; |
| 179 | |
| 180 | /* Legacy smmu bindings, still used by xen. */ |
| 181 | offset = fdt_node_offset_by_compatible(blob, 0, "arm,mmu-500"); |
| 182 | prop = fdt_getprop(blob, offset, "mmu-masters", &proplen); |
| 183 | if (offset > 0 && prop) { |
| 184 | debug("found legacy mmu-masters property\n"); |
| 185 | |
| 186 | for (i = 0; i < proplen / 8; ++i) { |
| 187 | u32 phandle = fdt32_to_cpu(prop[2 * i]); |
| 188 | int sid = fdt32_to_cpu(prop[2 * i + 1]); |
| 189 | int device_offset; |
| 190 | |
| 191 | device_offset = fdt_node_offset_by_phandle(blob, |
| 192 | phandle); |
| 193 | if (device_offset < 0) { |
| 194 | pr_err("Not find device from mmu_masters: %d", |
| 195 | device_offset); |
| 196 | continue; |
| 197 | } |
| 198 | ret = config_smmu_fdt_device_sid(blob, device_offset, |
| 199 | sid); |
| 200 | if (ret) |
| 201 | return ret; |
| 202 | } |
| 203 | |
| 204 | /* Ignore new bindings if old bindings found, just like linux. */ |
| 205 | return 0; |
| 206 | } |
| 207 | |
| 208 | /* Generic smmu bindings */ |
| 209 | offset = 0; |
| 210 | while ((offset = fdt_next_node(blob, offset, NULL)) > 0) { |
| 211 | name = fdt_get_name(blob, offset, NULL); |
| 212 | prop = fdt_getprop(blob, offset, "iommus", &proplen); |
| 213 | if (!prop) |
| 214 | continue; |
| 215 | debug("node %s iommus proplen %d\n", name, proplen); |
| 216 | |
| 217 | if (proplen == 12) { |
| 218 | int sid = fdt32_to_cpu(prop[1]); |
| 219 | |
| 220 | config_smmu_fdt_device_sid(blob, offset, sid); |
| 221 | } else if (proplen != 4) { |
| 222 | debug("node %s ignore unexpected iommus proplen=%d\n", |
| 223 | name, proplen); |
| 224 | } |
| 225 | } |
| 226 | |
| 227 | return 0; |
| 228 | } |
| 229 | |
Peng Fan | 94e4d02 | 2019-08-26 08:12:13 +0000 | [diff] [blame] | 230 | static int ft_add_optee_node(void *fdt, bd_t *bd) |
| 231 | { |
| 232 | const char *path, *subpath; |
| 233 | int offs; |
| 234 | |
| 235 | /* |
| 236 | * No TEE space allocated indicating no TEE running, so no |
| 237 | * need to add optee node in dts |
| 238 | */ |
| 239 | if (!boot_pointer[1]) |
| 240 | return 0; |
| 241 | |
| 242 | offs = fdt_increase_size(fdt, 512); |
| 243 | if (offs) { |
| 244 | printf("No Space for dtb\n"); |
| 245 | return 1; |
| 246 | } |
| 247 | |
| 248 | path = "/firmware"; |
| 249 | offs = fdt_path_offset(fdt, path); |
| 250 | if (offs < 0) { |
| 251 | path = "/"; |
| 252 | offs = fdt_path_offset(fdt, path); |
| 253 | |
| 254 | if (offs < 0) { |
| 255 | printf("Could not find root node.\n"); |
| 256 | return offs; |
| 257 | } |
| 258 | |
| 259 | subpath = "firmware"; |
| 260 | offs = fdt_add_subnode(fdt, offs, subpath); |
| 261 | if (offs < 0) { |
| 262 | printf("Could not create %s node.\n", subpath); |
| 263 | return offs; |
| 264 | } |
| 265 | } |
| 266 | |
| 267 | subpath = "optee"; |
| 268 | offs = fdt_add_subnode(fdt, offs, subpath); |
| 269 | if (offs < 0) { |
| 270 | printf("Could not create %s node.\n", subpath); |
| 271 | return offs; |
| 272 | } |
| 273 | |
| 274 | fdt_setprop_string(fdt, offs, "compatible", "linaro,optee-tz"); |
| 275 | fdt_setprop_string(fdt, offs, "method", "smc"); |
| 276 | |
| 277 | return 0; |
| 278 | } |
| 279 | |
Peng Fan | 9f779fa | 2019-08-26 08:11:56 +0000 | [diff] [blame] | 280 | int ft_system_setup(void *blob, bd_t *bd) |
| 281 | { |
Peng Fan | 01cacf9 | 2019-08-26 08:12:06 +0000 | [diff] [blame] | 282 | int ret; |
Peng Fan | f44afd5 | 2020-05-05 20:28:37 +0800 | [diff] [blame] | 283 | int off; |
| 284 | |
| 285 | if (CONFIG_BOOTAUX_RESERVED_MEM_BASE) { |
| 286 | off = fdt_add_mem_rsv(blob, CONFIG_BOOTAUX_RESERVED_MEM_BASE, |
| 287 | CONFIG_BOOTAUX_RESERVED_MEM_SIZE); |
| 288 | if (off < 0) |
| 289 | printf("Failed to reserve memory for bootaux: %s\n", |
| 290 | fdt_strerror(off)); |
| 291 | } |
Peng Fan | 01cacf9 | 2019-08-26 08:12:06 +0000 | [diff] [blame] | 292 | |
Peng Fan | 9f779fa | 2019-08-26 08:11:56 +0000 | [diff] [blame] | 293 | update_fdt_with_owned_resources(blob); |
| 294 | |
Peng Fan | 01cacf9 | 2019-08-26 08:12:06 +0000 | [diff] [blame] | 295 | if (is_imx8qm()) { |
| 296 | ret = config_smmu_fdt(blob); |
| 297 | if (ret) |
| 298 | return ret; |
| 299 | } |
| 300 | |
Peng Fan | 94e4d02 | 2019-08-26 08:12:13 +0000 | [diff] [blame] | 301 | return ft_add_optee_node(blob, bd); |
Peng Fan | 9f779fa | 2019-08-26 08:11:56 +0000 | [diff] [blame] | 302 | } |