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