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