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 struct dt_driver_prov_list dt_driver_provider_list = 16 SLIST_HEAD_INITIALIZER(dt_driver_provider_list); 17 18 /* 19 * Driver provider registering API functions 20 */ 21 22 TEE_Result dt_driver_register_provider(const void *fdt, int nodeoffset, 23 get_of_device_func get_of_device, 24 void *priv, enum dt_driver_type type) 25 { 26 struct dt_driver_provider *prv = NULL; 27 int provider_cells = 0; 28 uint32_t phandle = 0; 29 30 provider_cells = fdt_get_dt_driver_cells(fdt, nodeoffset, type); 31 if (provider_cells < 0) { 32 DMSG("Failed to find provider cells: %d", provider_cells); 33 return TEE_ERROR_GENERIC; 34 } 35 36 phandle = fdt_get_phandle(fdt, nodeoffset); 37 if (!phandle || phandle == (uint32_t)-1) { 38 DMSG("Failed to find provide phandle"); 39 return TEE_ERROR_GENERIC; 40 } 41 42 prv = calloc(1, sizeof(*prv)); 43 if (!prv) 44 return TEE_ERROR_OUT_OF_MEMORY; 45 46 prv->nodeoffset = nodeoffset; 47 prv->type = type; 48 prv->provider_cells = provider_cells; 49 prv->phandle = phandle; 50 prv->get_of_device = get_of_device; 51 prv->priv_data = priv; 52 53 SLIST_INSERT_HEAD(&dt_driver_provider_list, prv, link); 54 55 return TEE_SUCCESS; 56 } 57 58 /* Release driver provider references once all dt_drivers are initialized */ 59 static TEE_Result dt_driver_release_provider(void) 60 { 61 struct dt_driver_provider *prv = NULL; 62 63 while (!SLIST_EMPTY(&dt_driver_provider_list)) { 64 prv = SLIST_FIRST(&dt_driver_provider_list); 65 SLIST_REMOVE_HEAD(&dt_driver_provider_list, link); 66 free(prv); 67 } 68 69 return TEE_SUCCESS; 70 } 71 72 driver_init_late(dt_driver_release_provider); 73 74 /* 75 * Helper functions for dt_drivers querying driver provider information 76 */ 77 78 int fdt_get_dt_driver_cells(const void *fdt, int nodeoffset, 79 enum dt_driver_type type) 80 { 81 const char *cells_name = NULL; 82 const fdt32_t *c = NULL; 83 int len = 0; 84 85 switch (type) { 86 case DT_DRIVER_CLK: 87 cells_name = "#clock-cells"; 88 break; 89 default: 90 panic(); 91 } 92 93 c = fdt_getprop(fdt, nodeoffset, cells_name, &len); 94 if (!c) 95 return len; 96 97 if (len != sizeof(*c)) 98 return -FDT_ERR_BADNCELLS; 99 100 return fdt32_to_cpu(*c); 101 } 102 103 unsigned int dt_driver_provider_cells(struct dt_driver_provider *prv) 104 { 105 return prv->provider_cells; 106 } 107 108 struct dt_driver_provider *dt_driver_get_provider_by_node(int nodeoffset) 109 { 110 struct dt_driver_provider *prv = NULL; 111 112 SLIST_FOREACH(prv, &dt_driver_provider_list, link) 113 if (prv->nodeoffset == nodeoffset) 114 return prv; 115 116 return NULL; 117 } 118 119 struct dt_driver_provider *dt_driver_get_provider_by_phandle(uint32_t phandle) 120 { 121 struct dt_driver_provider *prv = NULL; 122 123 SLIST_FOREACH(prv, &dt_driver_provider_list, link) 124 if (prv->phandle == phandle) 125 return prv; 126 127 return NULL; 128 } 129 130 static void *device_from_provider_prop(struct dt_driver_provider *prv, 131 const uint32_t *prop) 132 { 133 struct dt_driver_phandle_args *pargs = NULL; 134 unsigned int n = 0; 135 void *device = NULL; 136 137 pargs = calloc(1, prv->provider_cells * sizeof(uint32_t *) + 138 sizeof(*pargs)); 139 if (!pargs) 140 return NULL; 141 142 pargs->args_count = prv->provider_cells; 143 for (n = 0; n < prv->provider_cells; n++) 144 pargs->args[n] = fdt32_to_cpu(prop[n + 1]); 145 146 device = prv->get_of_device(pargs, prv->priv_data); 147 148 free(pargs); 149 150 return device; 151 } 152 153 void *dt_driver_device_from_node_idx_prop(const char *prop_name, 154 const void *fdt, int nodeoffset, 155 unsigned int prop_idx) 156 { 157 int len = 0; 158 int idx = 0; 159 int idx32 = 0; 160 int prv_cells = 0; 161 uint32_t phandle = 0; 162 const uint32_t *prop = NULL; 163 struct dt_driver_provider *prv = NULL; 164 165 prop = fdt_getprop(fdt, nodeoffset, prop_name, &len); 166 if (!prop) 167 return NULL; 168 169 while (idx < len) { 170 idx32 = idx / sizeof(uint32_t); 171 phandle = fdt32_to_cpu(prop[idx32]); 172 173 prv = dt_driver_get_provider_by_phandle(phandle); 174 if (!prv) 175 return NULL; 176 177 prv_cells = dt_driver_provider_cells(prv); 178 if (prop_idx) { 179 prop_idx--; 180 idx += sizeof(phandle) + prv_cells * sizeof(uint32_t); 181 continue; 182 } 183 184 return device_from_provider_prop(prv, prop + idx32); 185 } 186 187 return NULL; 188 } 189 190 /* Lookup a compatible driver, possibly of a specific @type, for the FDT node */ 191 static TEE_Result probe_device_by_compat(const void *fdt, int node, 192 const char *compat, 193 enum dt_driver_type type) 194 { 195 const struct dt_driver *drv = NULL; 196 const struct dt_device_match *dm = NULL; 197 198 for_each_dt_driver(drv) { 199 if (drv->type != type) 200 continue; 201 202 for (dm = drv->match_table; dm && dm->compatible; dm++) 203 if (strcmp(dm->compatible, compat) == 0) 204 return drv->probe(fdt, node, dm->compat_data); 205 } 206 207 return TEE_ERROR_ITEM_NOT_FOUND; 208 } 209 210 TEE_Result dt_driver_probe_device_by_node(const void *fdt, int nodeoffset, 211 enum dt_driver_type type) 212 { 213 int idx = 0; 214 int len = 0; 215 int count = 0; 216 const char *compat = NULL; 217 TEE_Result res = TEE_ERROR_GENERIC; 218 219 count = fdt_stringlist_count(fdt, nodeoffset, "compatible"); 220 if (count < 0) 221 return TEE_ERROR_ITEM_NOT_FOUND; 222 223 for (idx = 0; idx < count; idx++) { 224 compat = fdt_stringlist_get(fdt, nodeoffset, "compatible", 225 idx, &len); 226 if (!compat) 227 return TEE_ERROR_GENERIC; 228 229 res = probe_device_by_compat(fdt, nodeoffset, compat, type); 230 231 if (res != TEE_ERROR_ITEM_NOT_FOUND) 232 return res; 233 } 234 235 return TEE_ERROR_ITEM_NOT_FOUND; 236 } 237