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_fn 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_fn 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 *clk_get_compatible_driver(const char *compat) 183 { 184 const struct dt_driver *drv = NULL; 185 const struct dt_device_match *dm = NULL; 186 const struct clk_driver *clk_drv = NULL; 187 188 for_each_dt_driver(drv) { 189 if (drv->type != DT_DRIVER_CLK) 190 continue; 191 192 clk_drv = (const struct clk_driver *)drv->driver; 193 for (dm = drv->match_table; dm && dm->compatible; dm++) { 194 if (strcmp(dm->compatible, compat) == 0) 195 return clk_drv; 196 } 197 } 198 199 return NULL; 200 } 201 202 /* Recursively called from parse_clock_property() */ 203 static TEE_Result clk_probe_clock_provider_node(const void *fdt, int node); 204 205 static TEE_Result parse_clock_property(const void *fdt, int node) 206 { 207 int len = 0; 208 int idx = 0; 209 int parent_node = 0; 210 int clock_cells = 0; 211 uint32_t phandle = 0; 212 const uint32_t *prop = NULL; 213 TEE_Result res = TEE_ERROR_GENERIC; 214 215 prop = fdt_getprop(fdt, node, "clocks", &len); 216 if (!prop) 217 return TEE_SUCCESS; 218 219 len /= sizeof(uint32_t); 220 while (idx < len) { 221 phandle = fdt32_to_cpu(prop[idx]); 222 223 parent_node = fdt_node_offset_by_phandle(fdt, phandle); 224 if (parent_node < 0) 225 return TEE_ERROR_GENERIC; 226 227 /* Parent setup should not fail or clock won't be available */ 228 res = clk_probe_clock_provider_node(fdt, parent_node); 229 if (res) 230 panic("Failed to setup parent clock"); 231 232 clock_cells = fdt_clock_cells(fdt, parent_node); 233 if (clock_cells < 0) 234 return TEE_ERROR_GENERIC; 235 236 idx += 1 + clock_cells; 237 } 238 239 return TEE_SUCCESS; 240 } 241 242 static TEE_Result clk_dt_node_clock_setup_driver(const void *fdt, int node) 243 { 244 int idx = 0; 245 int len = 0; 246 int count = 0; 247 const char *compat = NULL; 248 TEE_Result res = TEE_ERROR_GENERIC; 249 const struct clk_driver *clk_drv = NULL; 250 251 count = fdt_stringlist_count(fdt, node, "compatible"); 252 if (count < 0) 253 return TEE_ERROR_GENERIC; 254 255 for (idx = 0; idx < count; idx++) { 256 compat = fdt_stringlist_get(fdt, node, "compatible", idx, &len); 257 if (!compat) 258 return TEE_ERROR_GENERIC; 259 260 clk_drv = clk_get_compatible_driver(compat); 261 if (!clk_drv) 262 continue; 263 264 res = clk_drv->probe(fdt, node); 265 if (res != TEE_SUCCESS) { 266 EMSG("Failed to probe clock driver for compatible %s", 267 compat); 268 panic(); 269 } else { 270 return TEE_SUCCESS; 271 } 272 } 273 274 return TEE_ERROR_GENERIC; 275 } 276 277 static TEE_Result clk_probe_clock_provider_node(const void *fdt, int node) 278 { 279 int len = 0; 280 int status = 0; 281 TEE_Result res = TEE_ERROR_GENERIC; 282 283 status = _fdt_get_status(fdt, node); 284 if (!(status & DT_STATUS_OK_SEC)) 285 return TEE_ERROR_ITEM_NOT_FOUND; 286 287 /* Check if the node is a clock provider */ 288 if (!fdt_getprop(fdt, node, "#clock-cells", &len)) 289 return TEE_ERROR_ITEM_NOT_FOUND; 290 291 /* Check if node has already been probed */ 292 if (clk_get_provider_by_node(node)) 293 return TEE_SUCCESS; 294 295 /* Check if the node has a clock property first to setup parent */ 296 res = parse_clock_property(fdt, node); 297 if (res) 298 return res; 299 300 return clk_dt_node_clock_setup_driver(fdt, node); 301 } 302 303 static void clk_probe_node(const void *fdt, int parent_node) 304 { 305 int child = 0; 306 int status = 0; 307 __maybe_unused TEE_Result res = TEE_ERROR_GENERIC; 308 309 fdt_for_each_subnode(child, fdt, parent_node) { 310 status = _fdt_get_status(fdt, child); 311 if (status == DT_STATUS_DISABLED) 312 continue; 313 314 res = clk_probe_clock_provider_node(fdt, child); 315 assert(res == TEE_SUCCESS || res == TEE_ERROR_ITEM_NOT_FOUND); 316 317 clk_probe_node(fdt, child); 318 } 319 } 320 321 static void parse_assigned_clock(const void *fdt, int nodeoffset) 322 { 323 int rate_len = 0; 324 int clock_idx = 0; 325 struct clk *clk = NULL; 326 unsigned long rate = 0; 327 struct clk *parent = NULL; 328 const uint32_t *rate_prop = NULL; 329 330 rate_prop = fdt_getprop(fdt, nodeoffset, "assigned-clock-rates", 331 &rate_len); 332 rate_len /= sizeof(uint32_t); 333 334 while (1) { 335 clk = clk_dt_get_by_idx_prop("assigned-clocks", fdt, nodeoffset, 336 clock_idx); 337 if (!clk) 338 return; 339 340 parent = clk_dt_get_by_idx_prop("assigned-clock-parents", fdt, 341 nodeoffset, clock_idx); 342 if (parent) { 343 if (clk_set_parent(clk, parent)) { 344 EMSG("Could not set clk %s parent to clock %s", 345 clk->name, parent->name); 346 panic(); 347 } 348 } 349 350 if (rate_prop && clock_idx <= rate_len) { 351 rate = fdt32_to_cpu(rate_prop[clock_idx]); 352 if (rate && clk_set_rate(clk, rate) != TEE_SUCCESS) 353 panic(); 354 } 355 356 clock_idx++; 357 } 358 } 359 360 static void clk_probe_assigned(const void *fdt, int parent_node) 361 { 362 int len = 0; 363 int child = 0; 364 int status = 0; 365 366 fdt_for_each_subnode(child, fdt, parent_node) { 367 clk_probe_assigned(fdt, child); 368 369 status = _fdt_get_status(fdt, child); 370 if (status == DT_STATUS_DISABLED) 371 continue; 372 373 if (fdt_getprop(fdt, child, "assigned-clocks", &len)) 374 parse_assigned_clock(fdt, child); 375 } 376 } 377 378 static TEE_Result clk_dt_probe(void) 379 { 380 const void *fdt = get_embedded_dt(); 381 382 DMSG("Probing clocks from devicetree"); 383 if (!fdt) 384 panic(); 385 386 clk_probe_node(fdt, -1); 387 388 clk_probe_assigned(fdt, -1); 389 390 return TEE_SUCCESS; 391 } 392 early_init(clk_dt_probe); 393