1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (c) 2021, Bootlin 4 */ 5 6 #include <drivers/clk.h> 7 #include <kernel/boot.h> 8 #include <kernel/panic.h> 9 #include <kernel/spinlock.h> 10 #include <libfdt.h> 11 #include <malloc.h> 12 #include <stddef.h> 13 14 /* Global clock tree lock */ 15 static unsigned int clk_lock = SPINLOCK_UNLOCK; 16 17 struct clk *clk_alloc(const char *name, const struct clk_ops *ops, 18 struct clk **parent_clks, size_t parent_count) 19 { 20 struct clk *clk = NULL; 21 size_t parent = 0; 22 23 clk = calloc(1, sizeof(*clk) + parent_count * sizeof(clk)); 24 if (!clk) 25 return NULL; 26 27 clk->num_parents = parent_count; 28 for (parent = 0; parent < parent_count; parent++) 29 clk->parents[parent] = parent_clks[parent]; 30 31 clk->name = name; 32 clk->ops = ops; 33 refcount_set(&clk->enabled_count, 0); 34 35 return clk; 36 } 37 38 void clk_free(struct clk *clk) 39 { 40 free(clk); 41 } 42 43 static bool __maybe_unused clk_check(struct clk *clk) 44 { 45 if (!clk->ops) 46 return false; 47 48 if (clk->ops->set_parent && !clk->ops->get_parent) 49 return false; 50 51 if (clk->num_parents > 1 && !clk->ops->get_parent) 52 return false; 53 54 return true; 55 } 56 57 static void clk_compute_rate_no_lock(struct clk *clk) 58 { 59 unsigned long parent_rate = 0; 60 61 if (clk->parent) 62 parent_rate = clk->parent->rate; 63 64 if (clk->ops->get_rate) 65 clk->rate = clk->ops->get_rate(clk, parent_rate); 66 else 67 clk->rate = parent_rate; 68 } 69 70 struct clk *clk_get_parent_by_index(struct clk *clk, size_t pidx) 71 { 72 if (pidx >= clk->num_parents) 73 return NULL; 74 75 return clk->parents[pidx]; 76 } 77 78 static void clk_init_parent(struct clk *clk) 79 { 80 size_t pidx = 0; 81 82 switch (clk->num_parents) { 83 case 0: 84 break; 85 case 1: 86 clk->parent = clk->parents[0]; 87 break; 88 default: 89 pidx = clk->ops->get_parent(clk); 90 assert(pidx < clk->num_parents); 91 92 clk->parent = clk->parents[pidx]; 93 break; 94 } 95 } 96 97 TEE_Result clk_register(struct clk *clk) 98 { 99 assert(clk_check(clk)); 100 101 clk_init_parent(clk); 102 clk_compute_rate_no_lock(clk); 103 104 DMSG("Registered clock %s, freq %lu", clk->name, clk_get_rate(clk)); 105 106 return TEE_SUCCESS; 107 } 108 109 static bool clk_is_enabled_no_lock(struct clk *clk) 110 { 111 return refcount_val(&clk->enabled_count) != 0; 112 } 113 114 bool clk_is_enabled(struct clk *clk) 115 { 116 return clk_is_enabled_no_lock(clk); 117 } 118 119 static void clk_disable_no_lock(struct clk *clk) 120 { 121 struct clk *parent = NULL; 122 123 if (!refcount_dec(&clk->enabled_count)) 124 return; 125 126 if (clk->ops->disable) 127 clk->ops->disable(clk); 128 129 parent = clk_get_parent(clk); 130 if (parent) 131 clk_disable_no_lock(parent); 132 } 133 134 static TEE_Result clk_enable_no_lock(struct clk *clk) 135 { 136 TEE_Result res = TEE_ERROR_GENERIC; 137 struct clk *parent = NULL; 138 139 if (refcount_inc(&clk->enabled_count)) 140 return TEE_SUCCESS; 141 142 parent = clk_get_parent(clk); 143 if (parent) { 144 res = clk_enable_no_lock(parent); 145 if (res) 146 return res; 147 } 148 149 if (clk->ops->enable) { 150 res = clk->ops->enable(clk); 151 if (res) { 152 if (parent) 153 clk_disable_no_lock(parent); 154 155 return res; 156 } 157 } 158 159 refcount_set(&clk->enabled_count, 1); 160 161 return TEE_SUCCESS; 162 } 163 164 TEE_Result clk_enable(struct clk *clk) 165 { 166 uint32_t exceptions = 0; 167 TEE_Result res = TEE_ERROR_GENERIC; 168 169 exceptions = cpu_spin_lock_xsave(&clk_lock); 170 res = clk_enable_no_lock(clk); 171 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 172 173 return res; 174 } 175 176 void clk_disable(struct clk *clk) 177 { 178 uint32_t exceptions = 0; 179 180 exceptions = cpu_spin_lock_xsave(&clk_lock); 181 clk_disable_no_lock(clk); 182 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 183 } 184 185 unsigned long clk_get_rate(struct clk *clk) 186 { 187 return clk->rate; 188 } 189 190 static TEE_Result clk_set_rate_no_lock(struct clk *clk, unsigned long rate) 191 { 192 TEE_Result res = TEE_ERROR_GENERIC; 193 unsigned long parent_rate = 0; 194 195 if (clk->parent) 196 parent_rate = clk_get_rate(clk->parent); 197 198 res = clk->ops->set_rate(clk, rate, parent_rate); 199 if (res) 200 return res; 201 202 clk_compute_rate_no_lock(clk); 203 204 return TEE_SUCCESS; 205 } 206 207 TEE_Result clk_set_rate(struct clk *clk, unsigned long rate) 208 { 209 uint32_t exceptions = 0; 210 TEE_Result res = TEE_ERROR_GENERIC; 211 212 if (!clk->ops->set_rate) 213 return TEE_ERROR_NOT_SUPPORTED; 214 215 exceptions = cpu_spin_lock_xsave(&clk_lock); 216 217 if (clk->flags & CLK_SET_RATE_GATE && clk_is_enabled_no_lock(clk)) 218 res = TEE_ERROR_BAD_STATE; 219 else 220 res = clk_set_rate_no_lock(clk, rate); 221 222 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 223 224 return res; 225 } 226 227 struct clk *clk_get_parent(struct clk *clk) 228 { 229 return clk->parent; 230 } 231 232 static TEE_Result clk_get_parent_idx(struct clk *clk, struct clk *parent, 233 size_t *pidx) 234 { 235 size_t i = 0; 236 237 for (i = 0; i < clk_get_num_parents(clk); i++) { 238 if (clk_get_parent_by_index(clk, i) == parent) { 239 *pidx = i; 240 return TEE_SUCCESS; 241 } 242 } 243 EMSG("Clock %s is not a parent of clock %s", parent->name, clk->name); 244 245 return TEE_ERROR_BAD_PARAMETERS; 246 } 247 248 static TEE_Result clk_set_parent_no_lock(struct clk *clk, struct clk *parent, 249 size_t pidx) 250 { 251 TEE_Result res = TEE_ERROR_GENERIC; 252 bool was_enabled = false; 253 254 /* Requested parent is already the one set */ 255 if (clk->parent == parent) 256 return TEE_SUCCESS; 257 258 was_enabled = clk_is_enabled_no_lock(clk); 259 /* Call is needed to decrement refcount on current parent tree */ 260 if (was_enabled) 261 clk_disable_no_lock(clk); 262 263 res = clk->ops->set_parent(clk, pidx); 264 if (res) 265 goto out; 266 267 clk->parent = parent; 268 269 /* The parent changed and the rate might also have changed */ 270 clk_compute_rate_no_lock(clk); 271 272 out: 273 /* Call is needed to increment refcount on the new parent tree */ 274 if (was_enabled) { 275 res = clk_enable_no_lock(clk); 276 if (res) 277 panic("Failed to re-enable clock after setting parent"); 278 } 279 280 return res; 281 } 282 283 TEE_Result clk_set_parent(struct clk *clk, struct clk *parent) 284 { 285 size_t pidx = 0; 286 uint32_t exceptions = 0; 287 TEE_Result res = TEE_ERROR_GENERIC; 288 289 if (clk_get_parent_idx(clk, parent, &pidx) || !clk->ops->set_parent) 290 return TEE_ERROR_BAD_PARAMETERS; 291 292 exceptions = cpu_spin_lock_xsave(&clk_lock); 293 if (clk->flags & CLK_SET_PARENT_GATE && clk_is_enabled_no_lock(clk)) { 294 res = TEE_ERROR_BAD_STATE; 295 goto out; 296 } 297 298 res = clk_set_parent_no_lock(clk, parent, pidx); 299 out: 300 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 301 302 return res; 303 } 304 305 TEE_Result clk_get_rates_array(struct clk *clk, size_t start_index, 306 unsigned long *rates, size_t *nb_elts) 307 { 308 if (!clk->ops->get_rates_array) 309 return TEE_ERROR_NOT_SUPPORTED; 310 311 return clk->ops->get_rates_array(clk, start_index, rates, nb_elts); 312 } 313