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