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