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