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 { 156 struct dt_driver_phandle_args *pargs = NULL; 157 unsigned int n = 0; 158 void *device = NULL; 159 160 pargs = calloc(1, prv->provider_cells * sizeof(uint32_t *) + 161 sizeof(*pargs)); 162 if (!pargs) 163 return NULL; 164 165 pargs->args_count = prv->provider_cells; 166 for (n = 0; n < prv->provider_cells; n++) 167 pargs->args[n] = fdt32_to_cpu(prop[n + 1]); 168 169 device = prv->get_of_device(pargs, prv->priv_data); 170 171 free(pargs); 172 173 return device; 174 } 175 176 void *dt_driver_device_from_node_idx_prop(const char *prop_name, 177 const void *fdt, int nodeoffset, 178 unsigned int prop_idx) 179 { 180 int len = 0; 181 int idx = 0; 182 int idx32 = 0; 183 int prv_cells = 0; 184 uint32_t phandle = 0; 185 const uint32_t *prop = NULL; 186 struct dt_driver_provider *prv = NULL; 187 188 prop = fdt_getprop(fdt, nodeoffset, prop_name, &len); 189 if (!prop) 190 return NULL; 191 192 while (idx < len) { 193 idx32 = idx / sizeof(uint32_t); 194 phandle = fdt32_to_cpu(prop[idx32]); 195 196 prv = dt_driver_get_provider_by_phandle(phandle); 197 if (!prv) 198 return NULL; 199 200 prv_cells = dt_driver_provider_cells(prv); 201 if (prop_idx) { 202 prop_idx--; 203 idx += sizeof(phandle) + prv_cells * sizeof(uint32_t); 204 continue; 205 } 206 207 return device_from_provider_prop(prv, prop + idx32); 208 } 209 210 return NULL; 211 } 212 213 /* Lookup a compatible driver, possibly of a specific @type, for the FDT node */ 214 static TEE_Result probe_device_by_compat(const void *fdt, int node, 215 const char *compat, 216 enum dt_driver_type type) 217 { 218 const struct dt_driver *drv = NULL; 219 const struct dt_device_match *dm = NULL; 220 221 for_each_dt_driver(drv) { 222 if (drv->type != type) 223 continue; 224 225 for (dm = drv->match_table; dm && dm->compatible; dm++) 226 if (strcmp(dm->compatible, compat) == 0) 227 return drv->probe(fdt, node, dm->compat_data); 228 } 229 230 return TEE_ERROR_ITEM_NOT_FOUND; 231 } 232 233 TEE_Result dt_driver_probe_device_by_node(const void *fdt, int nodeoffset, 234 enum dt_driver_type type) 235 { 236 int idx = 0; 237 int len = 0; 238 int count = 0; 239 const char *compat = NULL; 240 TEE_Result res = TEE_ERROR_GENERIC; 241 242 count = fdt_stringlist_count(fdt, nodeoffset, "compatible"); 243 if (count < 0) 244 return TEE_ERROR_ITEM_NOT_FOUND; 245 246 for (idx = 0; idx < count; idx++) { 247 compat = fdt_stringlist_get(fdt, nodeoffset, "compatible", 248 idx, &len); 249 if (!compat) 250 return TEE_ERROR_GENERIC; 251 252 res = probe_device_by_compat(fdt, nodeoffset, compat, type); 253 254 if (res != TEE_ERROR_ITEM_NOT_FOUND) 255 return res; 256 } 257 258 return TEE_ERROR_ITEM_NOT_FOUND; 259 } 260