1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (c) 2021, Bootlin 4 * Copyright (c) 2023, STMicroelectronics 5 */ 6 7 #include <config.h> 8 #include <drivers/clk.h> 9 #include <kernel/boot.h> 10 #include <kernel/panic.h> 11 #include <kernel/spinlock.h> 12 #include <libfdt.h> 13 #include <malloc.h> 14 #include <stddef.h> 15 #include <stdio.h> 16 17 /* Global clock tree lock */ 18 static unsigned int clk_lock = SPINLOCK_UNLOCK; 19 20 #ifdef CFG_DRIVERS_CLK_PRINT_TREE 21 static STAILQ_HEAD(, clk) clock_list = STAILQ_HEAD_INITIALIZER(clock_list); 22 #endif 23 24 struct clk *clk_alloc(const char *name, const struct clk_ops *ops, 25 struct clk **parent_clks, size_t parent_count) 26 { 27 struct clk *clk = NULL; 28 size_t parent = 0; 29 30 clk = calloc(1, sizeof(*clk) + parent_count * sizeof(clk)); 31 if (!clk) 32 return NULL; 33 34 clk->num_parents = parent_count; 35 for (parent = 0; parent < parent_count; parent++) 36 clk->parents[parent] = parent_clks[parent]; 37 38 clk->name = name; 39 clk->ops = ops; 40 refcount_set(&clk->enabled_count, 0); 41 42 return clk; 43 } 44 45 void clk_free(struct clk *clk) 46 { 47 free(clk); 48 } 49 50 static bool __maybe_unused clk_check(struct clk *clk) 51 { 52 if (!clk->ops) 53 return false; 54 55 if (clk->ops->set_parent && !clk->ops->get_parent) 56 return false; 57 58 if (clk->num_parents > 1 && !clk->ops->get_parent) 59 return false; 60 61 return true; 62 } 63 64 static void clk_compute_rate_no_lock(struct clk *clk) 65 { 66 unsigned long parent_rate = 0; 67 68 if (clk->parent) 69 parent_rate = clk->parent->rate; 70 71 if (clk->ops->get_rate) 72 clk->rate = clk->ops->get_rate(clk, parent_rate); 73 else 74 clk->rate = parent_rate; 75 } 76 77 struct clk *clk_get_parent_by_index(struct clk *clk, size_t pidx) 78 { 79 if (pidx >= clk->num_parents) 80 return NULL; 81 82 return clk->parents[pidx]; 83 } 84 85 static void clk_init_parent(struct clk *clk) 86 { 87 size_t pidx = 0; 88 89 switch (clk->num_parents) { 90 case 0: 91 break; 92 case 1: 93 clk->parent = clk->parents[0]; 94 break; 95 default: 96 pidx = clk->ops->get_parent(clk); 97 assert(pidx < clk->num_parents); 98 99 clk->parent = clk->parents[pidx]; 100 break; 101 } 102 } 103 104 TEE_Result clk_register(struct clk *clk) 105 { 106 assert(clk_check(clk)); 107 108 clk_init_parent(clk); 109 clk_compute_rate_no_lock(clk); 110 111 #ifdef CFG_DRIVERS_CLK_PRINT_TREE 112 STAILQ_INSERT_TAIL(&clock_list, clk, link); 113 #endif 114 115 DMSG("Registered clock %s, freq %lu", clk->name, clk_get_rate(clk)); 116 117 return TEE_SUCCESS; 118 } 119 120 static bool clk_is_enabled_no_lock(struct clk *clk) 121 { 122 return refcount_val(&clk->enabled_count) != 0; 123 } 124 125 bool clk_is_enabled(struct clk *clk) 126 { 127 return clk_is_enabled_no_lock(clk); 128 } 129 130 static void clk_disable_no_lock(struct clk *clk) 131 { 132 struct clk *parent = NULL; 133 134 if (!refcount_dec(&clk->enabled_count)) 135 return; 136 137 if (clk->ops->disable) 138 clk->ops->disable(clk); 139 140 parent = clk_get_parent(clk); 141 if (parent) 142 clk_disable_no_lock(parent); 143 } 144 145 static TEE_Result clk_enable_no_lock(struct clk *clk) 146 { 147 TEE_Result res = TEE_ERROR_GENERIC; 148 struct clk *parent = NULL; 149 150 if (refcount_inc(&clk->enabled_count)) 151 return TEE_SUCCESS; 152 153 parent = clk_get_parent(clk); 154 if (parent) { 155 res = clk_enable_no_lock(parent); 156 if (res) 157 return res; 158 } 159 160 if (clk->ops->enable) { 161 res = clk->ops->enable(clk); 162 if (res) { 163 if (parent) 164 clk_disable_no_lock(parent); 165 166 return res; 167 } 168 } 169 170 refcount_set(&clk->enabled_count, 1); 171 172 return TEE_SUCCESS; 173 } 174 175 TEE_Result clk_enable(struct clk *clk) 176 { 177 uint32_t exceptions = 0; 178 TEE_Result res = TEE_ERROR_GENERIC; 179 180 exceptions = cpu_spin_lock_xsave(&clk_lock); 181 res = clk_enable_no_lock(clk); 182 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 183 184 return res; 185 } 186 187 void clk_disable(struct clk *clk) 188 { 189 uint32_t exceptions = 0; 190 191 exceptions = cpu_spin_lock_xsave(&clk_lock); 192 clk_disable_no_lock(clk); 193 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 194 } 195 196 unsigned long clk_get_rate(struct clk *clk) 197 { 198 return clk->rate; 199 } 200 201 static TEE_Result clk_set_rate_no_lock(struct clk *clk, unsigned long rate) 202 { 203 TEE_Result res = TEE_ERROR_GENERIC; 204 unsigned long parent_rate = 0; 205 206 if (clk->parent) 207 parent_rate = clk_get_rate(clk->parent); 208 209 res = clk->ops->set_rate(clk, rate, parent_rate); 210 if (res) 211 return res; 212 213 clk_compute_rate_no_lock(clk); 214 215 return TEE_SUCCESS; 216 } 217 218 TEE_Result clk_set_rate(struct clk *clk, unsigned long rate) 219 { 220 uint32_t exceptions = 0; 221 TEE_Result res = TEE_ERROR_GENERIC; 222 223 if (!clk->ops->set_rate) 224 return TEE_ERROR_NOT_SUPPORTED; 225 226 exceptions = cpu_spin_lock_xsave(&clk_lock); 227 228 if (clk->flags & CLK_SET_RATE_GATE && clk_is_enabled_no_lock(clk)) 229 res = TEE_ERROR_BAD_STATE; 230 else 231 res = clk_set_rate_no_lock(clk, rate); 232 233 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 234 235 return res; 236 } 237 238 struct clk *clk_get_parent(struct clk *clk) 239 { 240 return clk->parent; 241 } 242 243 static TEE_Result clk_get_parent_idx(struct clk *clk, struct clk *parent, 244 size_t *pidx) 245 { 246 size_t i = 0; 247 248 for (i = 0; i < clk_get_num_parents(clk); i++) { 249 if (clk_get_parent_by_index(clk, i) == parent) { 250 *pidx = i; 251 return TEE_SUCCESS; 252 } 253 } 254 EMSG("Clock %s is not a parent of clock %s", parent->name, clk->name); 255 256 return TEE_ERROR_BAD_PARAMETERS; 257 } 258 259 static TEE_Result clk_set_parent_no_lock(struct clk *clk, struct clk *parent, 260 size_t pidx) 261 { 262 TEE_Result res = TEE_ERROR_GENERIC; 263 bool was_enabled = false; 264 265 /* Requested parent is already the one set */ 266 if (clk->parent == parent) 267 return TEE_SUCCESS; 268 269 was_enabled = clk_is_enabled_no_lock(clk); 270 /* Call is needed to decrement refcount on current parent tree */ 271 if (was_enabled) 272 clk_disable_no_lock(clk); 273 274 res = clk->ops->set_parent(clk, pidx); 275 if (res) 276 goto out; 277 278 clk->parent = parent; 279 280 /* The parent changed and the rate might also have changed */ 281 clk_compute_rate_no_lock(clk); 282 283 out: 284 /* Call is needed to increment refcount on the new parent tree */ 285 if (was_enabled) { 286 res = clk_enable_no_lock(clk); 287 if (res) 288 panic("Failed to re-enable clock after setting parent"); 289 } 290 291 return res; 292 } 293 294 TEE_Result clk_set_parent(struct clk *clk, struct clk *parent) 295 { 296 size_t pidx = 0; 297 uint32_t exceptions = 0; 298 TEE_Result res = TEE_ERROR_GENERIC; 299 300 if (clk_get_parent_idx(clk, parent, &pidx) || !clk->ops->set_parent) 301 return TEE_ERROR_BAD_PARAMETERS; 302 303 exceptions = cpu_spin_lock_xsave(&clk_lock); 304 if (clk->flags & CLK_SET_PARENT_GATE && clk_is_enabled_no_lock(clk)) { 305 res = TEE_ERROR_BAD_STATE; 306 goto out; 307 } 308 309 res = clk_set_parent_no_lock(clk, parent, pidx); 310 out: 311 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 312 313 return res; 314 } 315 316 TEE_Result clk_get_rates_array(struct clk *clk, size_t start_index, 317 unsigned long *rates, size_t *nb_elts) 318 { 319 if (!clk->ops->get_rates_array) 320 return TEE_ERROR_NOT_SUPPORTED; 321 322 return clk->ops->get_rates_array(clk, start_index, rates, nb_elts); 323 } 324 325 /* Return updated message buffer position of NULL on failure */ 326 static __printf(3, 4) char *add_msg(char *cur, char *end, const char *fmt, ...) 327 { 328 va_list ap = { }; 329 int max_len = end - cur; 330 int ret = 0; 331 332 va_start(ap, fmt); 333 ret = vsnprintf(cur, max_len, fmt, ap); 334 va_end(ap); 335 336 if (ret < 0 || ret >= max_len) 337 return NULL; 338 339 return cur + ret; 340 } 341 342 static void __maybe_unused print_clock(struct clk *clk, int indent) 343 { 344 static const char * const rate_unit[] = { "Hz", "kHz", "MHz", "GHz" }; 345 int max_unit = ARRAY_SIZE(rate_unit); 346 unsigned long rate = 0; 347 char msg_buf[128] = { }; 348 char *msg_end = msg_buf + sizeof(msg_buf); 349 char *msg = msg_buf; 350 int n = 0; 351 352 /* 353 * Currently prints the clock state based on the clock refcount. 354 * A future change could print the hardware clock state when 355 * related clock driver provides a struct clk_ops::is_enabled handler 356 */ 357 358 if (indent) { 359 for (n = 0; n < indent - 1; n++) { 360 msg = add_msg(msg, msg_end, "| "); 361 if (!msg) 362 goto out; 363 } 364 365 msg = add_msg(msg, msg_end, "+-- "); 366 if (!msg) 367 goto out; 368 } 369 370 rate = clk_get_rate(clk); 371 for (n = 1; rate && !(rate % 1000) && n < max_unit; n++) 372 rate /= 1000; 373 374 msg = add_msg(msg, msg_end, "%s \t(%3s / refcnt %u / %ld %s)", 375 clk_get_name(clk), 376 refcount_val(&clk->enabled_count) ? "on " : "off", 377 refcount_val(&clk->enabled_count), 378 rate, rate_unit[n - 1]); 379 if (!msg) 380 goto out; 381 382 out: 383 if (!msg) 384 snprintf(msg_end - 4, 4, "..."); 385 386 IMSG("%s", msg_buf); 387 } 388 389 static void print_clock_subtree(struct clk *clk_root __maybe_unused, 390 int indent __maybe_unused) 391 { 392 #ifdef CFG_DRIVERS_CLK_PRINT_TREE 393 struct clk *clk = NULL; 394 395 STAILQ_FOREACH(clk, &clock_list, link) { 396 if (clk_get_parent(clk) == clk_root) { 397 print_clock(clk, indent + 1); 398 print_clock_subtree(clk, indent + 1); 399 if (indent == -1) 400 IMSG("%s", ""); 401 } 402 } 403 #endif 404 } 405 406 void clk_print_tree(void) 407 { 408 if (IS_ENABLED(CFG_DRIVERS_CLK_PRINT_TREE)) { 409 uint32_t exceptions = 0; 410 411 exceptions = cpu_spin_lock_xsave(&clk_lock); 412 IMSG("Clock tree summary"); 413 IMSG("%s", ""); 414 print_clock_subtree(NULL, -1); 415 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 416 } 417 } 418