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.h> 12 #include <kernel/dt_driver.h> 13 #include <kernel/panic.h> 14 #include <libfdt.h> 15 #include <stddef.h> 16 17 TEE_Result clk_dt_get_by_name(const void *fdt, int nodeoffset, 18 const char *name, struct clk **clk) 19 { 20 int clk_id = 0; 21 22 clk_id = fdt_stringlist_search(fdt, nodeoffset, "clock-names", name); 23 if (clk_id < 0) { 24 *clk = NULL; 25 return TEE_ERROR_GENERIC; 26 } 27 28 return clk_dt_get_by_index(fdt, nodeoffset, clk_id, clk); 29 } 30 31 static TEE_Result clk_dt_get_by_idx_prop(const char *prop_name, const void *fdt, 32 int nodeoffset, unsigned int clk_idx, 33 struct clk **clk) 34 { 35 TEE_Result res = TEE_ERROR_GENERIC; 36 void *out_clk = NULL; 37 38 res = dt_driver_device_from_node_idx_prop(prop_name, fdt, nodeoffset, 39 clk_idx, DT_DRIVER_CLK, 40 &out_clk); 41 if (!res) 42 *clk = out_clk; 43 44 return res; 45 } 46 47 TEE_Result clk_dt_get_by_index(const void *fdt, int nodeoffset, 48 unsigned int clk_idx, struct clk **clk) 49 { 50 return clk_dt_get_by_idx_prop("clocks", fdt, nodeoffset, clk_idx, clk); 51 } 52 53 #ifdef CFG_DRIVERS_CLK_EARLY_PROBE 54 /* Recursively called from parse_clock_property() */ 55 static TEE_Result clk_probe_clock_provider_node(const void *fdt, int node); 56 57 static TEE_Result parse_clock_property(const void *fdt, int node) 58 { 59 int len = 0; 60 int idx = 0; 61 int parent_node = 0; 62 int clock_cells = 0; 63 uint32_t phandle = 0; 64 const uint32_t *prop = NULL; 65 TEE_Result res = TEE_ERROR_GENERIC; 66 67 prop = fdt_getprop(fdt, node, "clocks", &len); 68 if (!prop) 69 return TEE_SUCCESS; 70 71 len /= sizeof(uint32_t); 72 while (idx < len) { 73 phandle = fdt32_to_cpu(prop[idx]); 74 75 parent_node = fdt_node_offset_by_phandle(fdt, phandle); 76 if (parent_node < 0) 77 return TEE_ERROR_GENERIC; 78 79 /* Parent probe should not fail or clock won't be available */ 80 res = clk_probe_clock_provider_node(fdt, parent_node); 81 if (res) { 82 EMSG("Probe parent clock node %s on node %s: %#"PRIx32, 83 fdt_get_name(fdt, parent_node, NULL), 84 fdt_get_name(fdt, node, NULL), res); 85 panic(); 86 } 87 88 clock_cells = fdt_get_dt_driver_cells(fdt, parent_node, 89 DT_DRIVER_CLK); 90 if (clock_cells < 0) 91 return TEE_ERROR_GENERIC; 92 93 idx += 1 + clock_cells; 94 } 95 96 return TEE_SUCCESS; 97 } 98 99 static TEE_Result clk_probe_clock_provider_node(const void *fdt, int node) 100 { 101 int len = 0; 102 int status = 0; 103 TEE_Result res = TEE_ERROR_GENERIC; 104 105 status = fdt_get_status(fdt, node); 106 if (!(status & DT_STATUS_OK_SEC)) 107 return TEE_ERROR_ITEM_NOT_FOUND; 108 109 /* Check if the node is a clock provider */ 110 if (!fdt_getprop(fdt, node, "#clock-cells", &len)) 111 return TEE_ERROR_ITEM_NOT_FOUND; 112 113 /* Check if node has already been probed */ 114 if (dt_driver_get_provider_by_node(node, DT_DRIVER_CLK)) 115 return TEE_SUCCESS; 116 117 /* Check if the node has a clock property first to probe parent */ 118 res = parse_clock_property(fdt, node); 119 if (res) 120 return res; 121 122 return dt_driver_probe_device_by_node(fdt, node, DT_DRIVER_CLK); 123 } 124 125 static void clk_probe_node(const void *fdt, int parent_node) 126 { 127 int child = 0; 128 int status = 0; 129 __maybe_unused TEE_Result res = TEE_ERROR_GENERIC; 130 131 fdt_for_each_subnode(child, fdt, parent_node) { 132 status = fdt_get_status(fdt, child); 133 if (status == DT_STATUS_DISABLED) 134 continue; 135 136 res = clk_probe_clock_provider_node(fdt, child); 137 assert(res == TEE_SUCCESS || res == TEE_ERROR_ITEM_NOT_FOUND); 138 139 clk_probe_node(fdt, child); 140 } 141 } 142 143 static void parse_assigned_clock(const void *fdt, int nodeoffset) 144 { 145 int rate_len = 0; 146 int clock_idx = 0; 147 struct clk *clk = NULL; 148 unsigned long rate = 0; 149 struct clk *parent = NULL; 150 const uint32_t *rate_prop = NULL; 151 TEE_Result __maybe_unused res = TEE_ERROR_GENERIC; 152 153 rate_prop = fdt_getprop(fdt, nodeoffset, "assigned-clock-rates", 154 &rate_len); 155 rate_len /= sizeof(uint32_t); 156 157 while (true) { 158 res = clk_dt_get_by_idx_prop("assigned-clocks", fdt, nodeoffset, 159 clock_idx, &clk); 160 if (!clk) 161 return; 162 assert(!res); 163 164 res = clk_dt_get_by_idx_prop("assigned-clock-parents", fdt, 165 nodeoffset, clock_idx, &parent); 166 if (parent) { 167 assert(!res); 168 if (clk_set_parent(clk, parent)) { 169 EMSG("Could not set clk %s parent to clock %s", 170 clk->name, parent->name); 171 panic(); 172 } 173 } 174 175 if (rate_prop && clock_idx < rate_len) { 176 rate = fdt32_to_cpu(rate_prop[clock_idx]); 177 if (rate && clk_set_rate(clk, rate) != TEE_SUCCESS) 178 panic(); 179 } 180 181 clock_idx++; 182 } 183 } 184 185 static void clk_probe_assigned(const void *fdt, int parent_node) 186 { 187 int len = 0; 188 int child = 0; 189 int status = 0; 190 191 fdt_for_each_subnode(child, fdt, parent_node) { 192 clk_probe_assigned(fdt, child); 193 194 status = fdt_get_status(fdt, child); 195 if (status == DT_STATUS_DISABLED) 196 continue; 197 198 if (fdt_getprop(fdt, child, "assigned-clocks", &len)) 199 parse_assigned_clock(fdt, child); 200 } 201 } 202 203 static TEE_Result clk_dt_probe(void) 204 { 205 const void *fdt = get_secure_dt(); 206 207 DMSG("Probing clocks from devicetree"); 208 if (!fdt) 209 panic(); 210 211 clk_probe_node(fdt, -1); 212 213 clk_probe_assigned(fdt, -1); 214 215 return TEE_SUCCESS; 216 } 217 early_init(clk_dt_probe); 218 #endif 219