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