1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (c) 2021, Linaro Limited 4 * Copyright (c) 2021, Bootlin 5 */ 6 7 #include <initcall.h> 8 #include <kernel/dt.h> 9 #include <kernel/dt_driver.h> 10 #include <libfdt.h> 11 #include <malloc.h> 12 #include <sys/queue.h> 13 #include <tee_api_types.h> 14 15 /* 16 * struct dt_driver_provider - DT related info on probed device 17 * 18 * Saves information on the probed device so that device 19 * drivers can get resources from DT phandle and related arguments. 20 * 21 * @nodeoffset: Node offset of device referenced in the FDT 22 * @type: One of DT_DRIVER_* or DT_DRIVER_NOTYPE. 23 * @provider_cells: Cells count in the FDT used by the driver's references 24 * @get_of_device: Function to get driver's device ref from phandle data 25 * @priv_data: Driver private data passed as @get_of_device argument 26 * @link: Reference in DT driver providers list 27 */ 28 struct dt_driver_provider { 29 int nodeoffset; 30 enum dt_driver_type type; 31 unsigned int provider_cells; 32 uint32_t phandle; 33 get_of_device_func get_of_device; 34 void *priv_data; 35 SLIST_ENTRY(dt_driver_provider) link; 36 }; 37 38 static SLIST_HEAD(, dt_driver_provider) dt_driver_provider_list = 39 SLIST_HEAD_INITIALIZER(dt_driver_provider_list); 40 41 /* 42 * Driver provider registering API functions 43 */ 44 45 TEE_Result dt_driver_register_provider(const void *fdt, int nodeoffset, 46 get_of_device_func get_of_device, 47 void *priv, enum dt_driver_type type) 48 { 49 struct dt_driver_provider *prv = NULL; 50 int provider_cells = 0; 51 uint32_t phandle = 0; 52 53 provider_cells = fdt_get_dt_driver_cells(fdt, nodeoffset, type); 54 if (provider_cells < 0) { 55 DMSG("Failed to find provider cells: %d", provider_cells); 56 return TEE_ERROR_GENERIC; 57 } 58 59 phandle = fdt_get_phandle(fdt, nodeoffset); 60 if (!phandle || phandle == (uint32_t)-1) { 61 DMSG("Failed to find provide phandle"); 62 return TEE_ERROR_GENERIC; 63 } 64 65 prv = calloc(1, sizeof(*prv)); 66 if (!prv) 67 return TEE_ERROR_OUT_OF_MEMORY; 68 69 prv->nodeoffset = nodeoffset; 70 prv->type = type; 71 prv->provider_cells = provider_cells; 72 prv->phandle = phandle; 73 prv->get_of_device = get_of_device; 74 prv->priv_data = priv; 75 76 SLIST_INSERT_HEAD(&dt_driver_provider_list, prv, link); 77 78 return TEE_SUCCESS; 79 } 80 81 /* Release driver provider references once all dt_drivers are initialized */ 82 static TEE_Result dt_driver_release_provider(void) 83 { 84 struct dt_driver_provider *prv = NULL; 85 86 while (!SLIST_EMPTY(&dt_driver_provider_list)) { 87 prv = SLIST_FIRST(&dt_driver_provider_list); 88 SLIST_REMOVE_HEAD(&dt_driver_provider_list, link); 89 free(prv); 90 } 91 92 return TEE_SUCCESS; 93 } 94 95 driver_init_late(dt_driver_release_provider); 96 97 /* 98 * Helper functions for dt_drivers querying driver provider information 99 */ 100 101 int fdt_get_dt_driver_cells(const void *fdt, int nodeoffset, 102 enum dt_driver_type type) 103 { 104 const char *cells_name = NULL; 105 const fdt32_t *c = NULL; 106 int len = 0; 107 108 switch (type) { 109 case DT_DRIVER_CLK: 110 cells_name = "#clock-cells"; 111 break; 112 default: 113 panic(); 114 } 115 116 c = fdt_getprop(fdt, nodeoffset, cells_name, &len); 117 if (!c) 118 return len; 119 120 if (len != sizeof(*c)) 121 return -FDT_ERR_BADNCELLS; 122 123 return fdt32_to_cpu(*c); 124 } 125 126 unsigned int dt_driver_provider_cells(struct dt_driver_provider *prv) 127 { 128 return prv->provider_cells; 129 } 130 131 struct dt_driver_provider *dt_driver_get_provider_by_node(int nodeoffset) 132 { 133 struct dt_driver_provider *prv = NULL; 134 135 SLIST_FOREACH(prv, &dt_driver_provider_list, link) 136 if (prv->nodeoffset == nodeoffset) 137 return prv; 138 139 return NULL; 140 } 141 142 struct dt_driver_provider *dt_driver_get_provider_by_phandle(uint32_t phandle) 143 { 144 struct dt_driver_provider *prv = NULL; 145 146 SLIST_FOREACH(prv, &dt_driver_provider_list, link) 147 if (prv->phandle == phandle) 148 return prv; 149 150 return NULL; 151 } 152 153 static void *device_from_provider_prop(struct dt_driver_provider *prv, 154 const uint32_t *prop, 155 TEE_Result *res) 156 { 157 struct dt_driver_phandle_args *pargs = NULL; 158 unsigned int n = 0; 159 void *device = NULL; 160 161 pargs = calloc(1, prv->provider_cells * sizeof(uint32_t *) + 162 sizeof(*pargs)); 163 if (!pargs) { 164 *res = TEE_ERROR_OUT_OF_MEMORY; 165 return NULL; 166 } 167 168 pargs->args_count = prv->provider_cells; 169 for (n = 0; n < prv->provider_cells; n++) 170 pargs->args[n] = fdt32_to_cpu(prop[n + 1]); 171 172 device = prv->get_of_device(pargs, prv->priv_data, res); 173 174 free(pargs); 175 176 return device; 177 } 178 179 void *dt_driver_device_from_node_idx_prop(const char *prop_name, 180 const void *fdt, int nodeoffset, 181 unsigned int prop_idx, 182 TEE_Result *res) 183 { 184 int len = 0; 185 int idx = 0; 186 int idx32 = 0; 187 int prv_cells = 0; 188 uint32_t phandle = 0; 189 const uint32_t *prop = NULL; 190 struct dt_driver_provider *prv = NULL; 191 192 prop = fdt_getprop(fdt, nodeoffset, prop_name, &len); 193 if (!prop) { 194 *res = TEE_ERROR_GENERIC; 195 return NULL; 196 } 197 198 while (idx < len) { 199 idx32 = idx / sizeof(uint32_t); 200 phandle = fdt32_to_cpu(prop[idx32]); 201 202 prv = dt_driver_get_provider_by_phandle(phandle); 203 if (!prv) { 204 *res = TEE_ERROR_GENERIC; 205 return NULL; 206 } 207 208 prv_cells = dt_driver_provider_cells(prv); 209 if (prop_idx) { 210 prop_idx--; 211 idx += sizeof(phandle) + prv_cells * sizeof(uint32_t); 212 continue; 213 } 214 215 return device_from_provider_prop(prv, prop + idx32, res); 216 } 217 218 *res = TEE_ERROR_GENERIC; 219 return NULL; 220 } 221 222 /* Lookup a compatible driver, possibly of a specific @type, for the FDT node */ 223 static TEE_Result probe_device_by_compat(const void *fdt, int node, 224 const char *compat, 225 enum dt_driver_type type) 226 { 227 const struct dt_driver *drv = NULL; 228 const struct dt_device_match *dm = NULL; 229 230 for_each_dt_driver(drv) { 231 if (drv->type != type) 232 continue; 233 234 for (dm = drv->match_table; dm && dm->compatible; dm++) 235 if (strcmp(dm->compatible, compat) == 0) 236 return drv->probe(fdt, node, dm->compat_data); 237 } 238 239 return TEE_ERROR_ITEM_NOT_FOUND; 240 } 241 242 TEE_Result dt_driver_probe_device_by_node(const void *fdt, int nodeoffset, 243 enum dt_driver_type type) 244 { 245 int idx = 0; 246 int len = 0; 247 int count = 0; 248 const char *compat = NULL; 249 TEE_Result res = TEE_ERROR_GENERIC; 250 251 count = fdt_stringlist_count(fdt, nodeoffset, "compatible"); 252 if (count < 0) 253 return TEE_ERROR_ITEM_NOT_FOUND; 254 255 for (idx = 0; idx < count; idx++) { 256 compat = fdt_stringlist_get(fdt, nodeoffset, "compatible", 257 idx, &len); 258 if (!compat) 259 return TEE_ERROR_GENERIC; 260 261 res = probe_device_by_compat(fdt, nodeoffset, compat, type); 262 263 if (res != TEE_ERROR_ITEM_NOT_FOUND) 264 return res; 265 } 266 267 return TEE_ERROR_ITEM_NOT_FOUND; 268 } 269