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 static void clk_disable_no_lock(struct clk *clk) 115 { 116 struct clk *parent = NULL; 117 118 if (!refcount_dec(&clk->enabled_count)) 119 return; 120 121 if (clk->ops->disable) 122 clk->ops->disable(clk); 123 124 parent = clk_get_parent(clk); 125 if (parent) 126 clk_disable_no_lock(parent); 127 } 128 129 static TEE_Result clk_enable_no_lock(struct clk *clk) 130 { 131 TEE_Result res = TEE_ERROR_GENERIC; 132 struct clk *parent = NULL; 133 134 if (refcount_inc(&clk->enabled_count)) 135 return TEE_SUCCESS; 136 137 parent = clk_get_parent(clk); 138 if (parent) { 139 res = clk_enable_no_lock(parent); 140 if (res) 141 return res; 142 } 143 144 if (clk->ops->enable) { 145 res = clk->ops->enable(clk); 146 if (res) { 147 if (parent) 148 clk_disable_no_lock(parent); 149 150 return res; 151 } 152 } 153 154 refcount_set(&clk->enabled_count, 1); 155 156 return TEE_SUCCESS; 157 } 158 159 TEE_Result clk_enable(struct clk *clk) 160 { 161 uint32_t exceptions = 0; 162 TEE_Result res = TEE_ERROR_GENERIC; 163 164 exceptions = cpu_spin_lock_xsave(&clk_lock); 165 res = clk_enable_no_lock(clk); 166 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 167 168 return res; 169 } 170 171 void clk_disable(struct clk *clk) 172 { 173 uint32_t exceptions = 0; 174 175 exceptions = cpu_spin_lock_xsave(&clk_lock); 176 clk_disable_no_lock(clk); 177 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 178 } 179 180 unsigned long clk_get_rate(struct clk *clk) 181 { 182 return clk->rate; 183 } 184 185 static TEE_Result clk_set_rate_no_lock(struct clk *clk, unsigned long rate) 186 { 187 TEE_Result res = TEE_ERROR_GENERIC; 188 unsigned long parent_rate = 0; 189 190 if (clk->parent) 191 parent_rate = clk_get_rate(clk->parent); 192 193 res = clk->ops->set_rate(clk, rate, parent_rate); 194 if (res) 195 return res; 196 197 clk_compute_rate_no_lock(clk); 198 199 return TEE_SUCCESS; 200 } 201 202 TEE_Result clk_set_rate(struct clk *clk, unsigned long rate) 203 { 204 uint32_t exceptions = 0; 205 TEE_Result res = TEE_ERROR_GENERIC; 206 207 if (!clk->ops->set_rate) 208 return TEE_ERROR_NOT_SUPPORTED; 209 210 exceptions = cpu_spin_lock_xsave(&clk_lock); 211 212 if (clk->flags & CLK_SET_RATE_GATE && clk_is_enabled_no_lock(clk)) 213 res = TEE_ERROR_BAD_STATE; 214 else 215 res = clk_set_rate_no_lock(clk, rate); 216 217 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 218 219 return res; 220 } 221 222 struct clk *clk_get_parent(struct clk *clk) 223 { 224 return clk->parent; 225 } 226 227 static TEE_Result clk_get_parent_idx(struct clk *clk, struct clk *parent, 228 size_t *pidx) 229 { 230 size_t i = 0; 231 232 for (i = 0; i < clk_get_num_parents(clk); i++) { 233 if (clk_get_parent_by_index(clk, i) == parent) { 234 *pidx = i; 235 return TEE_SUCCESS; 236 } 237 } 238 EMSG("Clock %s is not a parent of clock %s", parent->name, clk->name); 239 240 return TEE_ERROR_BAD_PARAMETERS; 241 } 242 243 static TEE_Result clk_set_parent_no_lock(struct clk *clk, struct clk *parent, 244 size_t pidx) 245 { 246 TEE_Result res = TEE_ERROR_GENERIC; 247 bool was_enabled = false; 248 249 /* Requested parent is already the one set */ 250 if (clk->parent == parent) 251 return TEE_SUCCESS; 252 253 was_enabled = clk_is_enabled_no_lock(clk); 254 /* Call is needed to decrement refcount on current parent tree */ 255 if (was_enabled) 256 clk_disable_no_lock(clk); 257 258 res = clk->ops->set_parent(clk, pidx); 259 if (res) 260 goto out; 261 262 clk->parent = parent; 263 264 /* The parent changed and the rate might also have changed */ 265 clk_compute_rate_no_lock(clk); 266 267 out: 268 /* Call is needed to increment refcount on the new parent tree */ 269 if (was_enabled) { 270 res = clk_enable_no_lock(clk); 271 if (res) 272 panic("Failed to re-enable clock after setting parent"); 273 } 274 275 return res; 276 } 277 278 TEE_Result clk_set_parent(struct clk *clk, struct clk *parent) 279 { 280 size_t pidx = 0; 281 uint32_t exceptions = 0; 282 TEE_Result res = TEE_ERROR_GENERIC; 283 284 if (clk_get_parent_idx(clk, parent, &pidx) || !clk->ops->set_parent) 285 return TEE_ERROR_BAD_PARAMETERS; 286 287 exceptions = cpu_spin_lock_xsave(&clk_lock); 288 if (clk->flags & CLK_SET_PARENT_GATE && clk_is_enabled_no_lock(clk)) { 289 res = TEE_ERROR_BAD_STATE; 290 goto out; 291 } 292 293 res = clk_set_parent_no_lock(clk, parent, pidx); 294 out: 295 cpu_spin_unlock_xrestore(&clk_lock, exceptions); 296 297 return res; 298 } 299