1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (c) 2021, Bootlin 4 */ 5 6 #include <assert.h> 7 #include <drivers/clk.h> 8 #include <drivers/clk_dt.h> 9 #include <initcall.h> 10 #include <kernel/boot.h> 11 #include <kernel/dt_driver.h> 12 #include <kernel/panic.h> 13 #include <libfdt.h> 14 #include <stddef.h> 15 16 static int fdt_clock_cells(const void *fdt, int nodeoffset) 17 { 18 const fdt32_t *c = NULL; 19 int len = 0; 20 21 c = fdt_getprop(fdt, nodeoffset, "#clock-cells", &len); 22 if (!c) 23 return len; 24 25 if (len != sizeof(*c)) 26 return -FDT_ERR_BADNCELLS; 27 28 return (int)fdt32_to_cpu(*c); 29 } 30 31 TEE_Result clk_dt_register_clk_provider(const void *fdt, int nodeoffset, 32 clk_dt_get_func get_dt_clk, void *data) 33 { 34 struct dt_driver_provider *prv = NULL; 35 int clock_cells = 0; 36 37 prv = calloc(1, sizeof(*prv)); 38 if (!prv) 39 return TEE_ERROR_OUT_OF_MEMORY; 40 41 prv->get_of_device = (get_of_device_func)get_dt_clk; 42 prv->priv_data = data; 43 prv->nodeoffset = nodeoffset; 44 clock_cells = fdt_clock_cells(fdt, nodeoffset); 45 if (clock_cells < 0) { 46 free(prv); 47 return TEE_ERROR_GENERIC; 48 } 49 prv->provider_cells = clock_cells; 50 prv->phandle = fdt_get_phandle(fdt, nodeoffset); 51 52 SLIST_INSERT_HEAD(&dt_driver_provider_list, prv, link); 53 54 return TEE_SUCCESS; 55 } 56 57 static TEE_Result clk_dt_release_provider(void) 58 { 59 struct dt_driver_provider *prv = NULL; 60 61 while (!SLIST_EMPTY(&dt_driver_provider_list)) { 62 prv = SLIST_FIRST(&dt_driver_provider_list); 63 SLIST_REMOVE_HEAD(&dt_driver_provider_list, link); 64 free(prv); 65 } 66 67 return TEE_SUCCESS; 68 } 69 70 driver_init_late(clk_dt_release_provider); 71 72 static struct dt_driver_provider *clk_get_provider_by_node(int nodeoffset) 73 { 74 struct dt_driver_provider *prv = NULL; 75 76 SLIST_FOREACH(prv, &dt_driver_provider_list, link) 77 if (prv->nodeoffset == nodeoffset) 78 return prv; 79 80 return NULL; 81 } 82 83 static struct dt_driver_provider *clk_get_provider_by_phandle(uint32_t phandle) 84 { 85 struct dt_driver_provider *prv = NULL; 86 87 SLIST_FOREACH(prv, &dt_driver_provider_list, link) 88 if (prv->phandle == phandle) 89 return prv; 90 91 return NULL; 92 } 93 94 static struct clk *clk_dt_get_from_provider(struct dt_driver_provider *prv, 95 unsigned int clock_cells, 96 const uint32_t *prop) 97 { 98 unsigned int arg = 0; 99 struct clk *clk = NULL; 100 struct dt_driver_phandle_args *pargs = NULL; 101 102 pargs = calloc(1, clock_cells * sizeof(uint32_t *) + 103 sizeof(*pargs)); 104 if (!pargs) 105 return NULL; 106 107 pargs->args_count = clock_cells; 108 for (arg = 0; arg < clock_cells; arg++) 109 pargs->args[arg] = fdt32_to_cpu(prop[arg + 1]); 110 111 clk = prv->get_of_device(pargs, prv->priv_data); 112 113 free(pargs); 114 115 return clk; 116 } 117 118 struct clk *clk_dt_get_by_name(const void *fdt, int nodeoffset, 119 const char *name) 120 { 121 int clk_id = 0; 122 123 clk_id = fdt_stringlist_search(fdt, nodeoffset, "clock-names", name); 124 if (clk_id < 0) 125 return NULL; 126 127 return clk_dt_get_by_idx(fdt, nodeoffset, clk_id); 128 } 129 130 static struct clk *clk_dt_get_by_idx_prop(const char *prop_name, 131 const void *fdt, int nodeoffset, 132 unsigned int clk_idx) 133 { 134 int len = 0; 135 int idx = 0; 136 int idx32 = 0; 137 int clock_cells = 0; 138 uint32_t phandle = 0; 139 const uint32_t *prop = NULL; 140 struct dt_driver_provider *prv = NULL; 141 142 prop = fdt_getprop(fdt, nodeoffset, prop_name, &len); 143 if (!prop) 144 return NULL; 145 146 while (idx < len) { 147 idx32 = idx / sizeof(uint32_t); 148 phandle = fdt32_to_cpu(prop[idx32]); 149 150 prv = clk_get_provider_by_phandle(phandle); 151 if (!prv) 152 return NULL; 153 154 clock_cells = prv->provider_cells; 155 if (clk_idx) { 156 clk_idx--; 157 idx += sizeof(phandle) + clock_cells * sizeof(uint32_t); 158 continue; 159 } 160 161 return clk_dt_get_from_provider(prv, clock_cells, &prop[idx32]); 162 } 163 164 return NULL; 165 } 166 167 struct clk *clk_dt_get_by_idx(const void *fdt, int nodeoffset, 168 unsigned int clk_idx) 169 { 170 return clk_dt_get_by_idx_prop("clocks", fdt, nodeoffset, clk_idx); 171 } 172 173 static const struct clk_driver * 174 clk_get_compatible_driver(const char *compat, 175 const struct dt_device_match **out_dm) 176 { 177 const struct dt_driver *drv = NULL; 178 const struct dt_device_match *dm = NULL; 179 const struct clk_driver *clk_drv = NULL; 180 181 for_each_dt_driver(drv) { 182 if (drv->type != DT_DRIVER_CLK) 183 continue; 184 185 clk_drv = (const struct clk_driver *)drv->driver; 186 for (dm = drv->match_table; dm && dm->compatible; dm++) { 187 if (strcmp(dm->compatible, compat) == 0) { 188 if (out_dm) 189 *out_dm = dm; 190 191 return clk_drv; 192 } 193 } 194 } 195 196 return NULL; 197 } 198 199 /* Recursively called from parse_clock_property() */ 200 static TEE_Result clk_probe_clock_provider_node(const void *fdt, int node); 201 202 static TEE_Result parse_clock_property(const void *fdt, int node) 203 { 204 int len = 0; 205 int idx = 0; 206 int parent_node = 0; 207 int clock_cells = 0; 208 uint32_t phandle = 0; 209 const uint32_t *prop = NULL; 210 TEE_Result res = TEE_ERROR_GENERIC; 211 212 prop = fdt_getprop(fdt, node, "clocks", &len); 213 if (!prop) 214 return TEE_SUCCESS; 215 216 len /= sizeof(uint32_t); 217 while (idx < len) { 218 phandle = fdt32_to_cpu(prop[idx]); 219 220 parent_node = fdt_node_offset_by_phandle(fdt, phandle); 221 if (parent_node < 0) 222 return TEE_ERROR_GENERIC; 223 224 /* Parent probe should not fail or clock won't be available */ 225 res = clk_probe_clock_provider_node(fdt, parent_node); 226 if (res) 227 panic("Failed to probe parent clock"); 228 229 clock_cells = fdt_clock_cells(fdt, parent_node); 230 if (clock_cells < 0) 231 return TEE_ERROR_GENERIC; 232 233 idx += 1 + clock_cells; 234 } 235 236 return TEE_SUCCESS; 237 } 238 239 static TEE_Result clk_dt_node_clock_probe_driver(const void *fdt, int node) 240 { 241 int idx = 0; 242 int len = 0; 243 int count = 0; 244 const char *compat = NULL; 245 TEE_Result res = TEE_ERROR_GENERIC; 246 const struct clk_driver *clk_drv = NULL; 247 const struct dt_device_match *dm = NULL; 248 249 count = fdt_stringlist_count(fdt, node, "compatible"); 250 if (count < 0) 251 return TEE_ERROR_GENERIC; 252 253 for (idx = 0; idx < count; idx++) { 254 compat = fdt_stringlist_get(fdt, node, "compatible", idx, &len); 255 if (!compat) 256 return TEE_ERROR_GENERIC; 257 258 clk_drv = clk_get_compatible_driver(compat, &dm); 259 if (!clk_drv) 260 continue; 261 262 res = clk_drv->probe(fdt, node, dm->compat_data); 263 if (res != TEE_SUCCESS) { 264 EMSG("Failed to probe clock driver for compatible %s", 265 compat); 266 panic(); 267 } else { 268 return TEE_SUCCESS; 269 } 270 } 271 272 return TEE_ERROR_GENERIC; 273 } 274 275 static TEE_Result clk_probe_clock_provider_node(const void *fdt, int node) 276 { 277 int len = 0; 278 int status = 0; 279 TEE_Result res = TEE_ERROR_GENERIC; 280 281 status = _fdt_get_status(fdt, node); 282 if (!(status & DT_STATUS_OK_SEC)) 283 return TEE_ERROR_ITEM_NOT_FOUND; 284 285 /* Check if the node is a clock provider */ 286 if (!fdt_getprop(fdt, node, "#clock-cells", &len)) 287 return TEE_ERROR_ITEM_NOT_FOUND; 288 289 /* Check if node has already been probed */ 290 if (clk_get_provider_by_node(node)) 291 return TEE_SUCCESS; 292 293 /* Check if the node has a clock property first to probe parent */ 294 res = parse_clock_property(fdt, node); 295 if (res) 296 return res; 297 298 return clk_dt_node_clock_probe_driver(fdt, node); 299 } 300 301 static void clk_probe_node(const void *fdt, int parent_node) 302 { 303 int child = 0; 304 int status = 0; 305 __maybe_unused TEE_Result res = TEE_ERROR_GENERIC; 306 307 fdt_for_each_subnode(child, fdt, parent_node) { 308 status = _fdt_get_status(fdt, child); 309 if (status == DT_STATUS_DISABLED) 310 continue; 311 312 res = clk_probe_clock_provider_node(fdt, child); 313 assert(res == TEE_SUCCESS || res == TEE_ERROR_ITEM_NOT_FOUND); 314 315 clk_probe_node(fdt, child); 316 } 317 } 318 319 static void parse_assigned_clock(const void *fdt, int nodeoffset) 320 { 321 int rate_len = 0; 322 int clock_idx = 0; 323 struct clk *clk = NULL; 324 unsigned long rate = 0; 325 struct clk *parent = NULL; 326 const uint32_t *rate_prop = NULL; 327 328 rate_prop = fdt_getprop(fdt, nodeoffset, "assigned-clock-rates", 329 &rate_len); 330 rate_len /= sizeof(uint32_t); 331 332 while (1) { 333 clk = clk_dt_get_by_idx_prop("assigned-clocks", fdt, nodeoffset, 334 clock_idx); 335 if (!clk) 336 return; 337 338 parent = clk_dt_get_by_idx_prop("assigned-clock-parents", fdt, 339 nodeoffset, clock_idx); 340 if (parent) { 341 if (clk_set_parent(clk, parent)) { 342 EMSG("Could not set clk %s parent to clock %s", 343 clk->name, parent->name); 344 panic(); 345 } 346 } 347 348 if (rate_prop && clock_idx <= rate_len) { 349 rate = fdt32_to_cpu(rate_prop[clock_idx]); 350 if (rate && clk_set_rate(clk, rate) != TEE_SUCCESS) 351 panic(); 352 } 353 354 clock_idx++; 355 } 356 } 357 358 static void clk_probe_assigned(const void *fdt, int parent_node) 359 { 360 int len = 0; 361 int child = 0; 362 int status = 0; 363 364 fdt_for_each_subnode(child, fdt, parent_node) { 365 clk_probe_assigned(fdt, child); 366 367 status = _fdt_get_status(fdt, child); 368 if (status == DT_STATUS_DISABLED) 369 continue; 370 371 if (fdt_getprop(fdt, child, "assigned-clocks", &len)) 372 parse_assigned_clock(fdt, child); 373 } 374 } 375 376 static TEE_Result clk_dt_probe(void) 377 { 378 const void *fdt = get_embedded_dt(); 379 380 DMSG("Probing clocks from devicetree"); 381 if (!fdt) 382 panic(); 383 384 clk_probe_node(fdt, -1); 385 386 clk_probe_assigned(fdt, -1); 387 388 return TEE_SUCCESS; 389 } 390 early_init(clk_dt_probe); 391