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