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