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