1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> 4 */ 5 6 #include <linux/bitfield.h> 7 #include <linux/bitops.h> 8 #include <linux/clk-provider.h> 9 #include <linux/clkdev.h> 10 #include <linux/clk/at91_pmc.h> 11 #include <linux/of.h> 12 #include <linux/mfd/syscon.h> 13 #include <linux/regmap.h> 14 15 #include "pmc.h" 16 17 DEFINE_SPINLOCK(pmc_pcr_lock); 18 19 #define PERIPHERAL_ID_MIN 2 20 #define PERIPHERAL_ID_MAX 31 21 #define PERIPHERAL_MASK(id) (1 << ((id) & PERIPHERAL_ID_MAX)) 22 23 #define PERIPHERAL_MAX_SHIFT 3 24 25 struct clk_peripheral { 26 struct clk_hw hw; 27 struct regmap *regmap; 28 u32 id; 29 }; 30 31 #define to_clk_peripheral(hw) container_of(hw, struct clk_peripheral, hw) 32 33 struct clk_sam9x5_peripheral { 34 struct clk_hw hw; 35 struct regmap *regmap; 36 struct clk_range range; 37 spinlock_t *lock; 38 u32 id; 39 u32 div; 40 const struct clk_pcr_layout *layout; 41 struct at91_clk_pms pms; 42 bool auto_div; 43 int chg_pid; 44 }; 45 46 #define to_clk_sam9x5_peripheral(hw) \ 47 container_of(hw, struct clk_sam9x5_peripheral, hw) 48 49 static int clk_peripheral_enable(struct clk_hw *hw) 50 { 51 struct clk_peripheral *periph = to_clk_peripheral(hw); 52 int offset = AT91_PMC_PCER; 53 u32 id = periph->id; 54 55 if (id < PERIPHERAL_ID_MIN) 56 return 0; 57 if (id > PERIPHERAL_ID_MAX) 58 offset = AT91_PMC_PCER1; 59 regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id)); 60 61 return 0; 62 } 63 64 static void clk_peripheral_disable(struct clk_hw *hw) 65 { 66 struct clk_peripheral *periph = to_clk_peripheral(hw); 67 int offset = AT91_PMC_PCDR; 68 u32 id = periph->id; 69 70 if (id < PERIPHERAL_ID_MIN) 71 return; 72 if (id > PERIPHERAL_ID_MAX) 73 offset = AT91_PMC_PCDR1; 74 regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id)); 75 } 76 77 static int clk_peripheral_is_enabled(struct clk_hw *hw) 78 { 79 struct clk_peripheral *periph = to_clk_peripheral(hw); 80 int offset = AT91_PMC_PCSR; 81 unsigned int status; 82 u32 id = periph->id; 83 84 if (id < PERIPHERAL_ID_MIN) 85 return 1; 86 if (id > PERIPHERAL_ID_MAX) 87 offset = AT91_PMC_PCSR1; 88 regmap_read(periph->regmap, offset, &status); 89 90 return status & PERIPHERAL_MASK(id) ? 1 : 0; 91 } 92 93 static const struct clk_ops peripheral_ops = { 94 .enable = clk_peripheral_enable, 95 .disable = clk_peripheral_disable, 96 .is_enabled = clk_peripheral_is_enabled, 97 }; 98 99 struct clk_hw * __init 100 at91_clk_register_peripheral(struct regmap *regmap, const char *name, 101 const char *parent_name, struct clk_hw *parent_hw, 102 u32 id) 103 { 104 struct clk_peripheral *periph; 105 struct clk_init_data init = {}; 106 struct clk_hw *hw; 107 int ret; 108 109 if (!name || !(parent_name || parent_hw) || id > PERIPHERAL_ID_MAX) 110 return ERR_PTR(-EINVAL); 111 112 periph = kzalloc(sizeof(*periph), GFP_KERNEL); 113 if (!periph) 114 return ERR_PTR(-ENOMEM); 115 116 init.name = name; 117 init.ops = &peripheral_ops; 118 if (parent_hw) 119 init.parent_hws = (const struct clk_hw **)&parent_hw; 120 else 121 init.parent_names = &parent_name; 122 init.num_parents = 1; 123 init.flags = 0; 124 125 periph->id = id; 126 periph->hw.init = &init; 127 periph->regmap = regmap; 128 129 hw = &periph->hw; 130 ret = clk_hw_register(NULL, &periph->hw); 131 if (ret) { 132 kfree(periph); 133 hw = ERR_PTR(ret); 134 } 135 136 return hw; 137 } 138 139 static void clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral *periph) 140 { 141 struct clk_hw *parent; 142 unsigned long parent_rate; 143 int shift = 0; 144 145 if (!periph->auto_div) 146 return; 147 148 if (periph->range.max) { 149 parent = clk_hw_get_parent_by_index(&periph->hw, 0); 150 parent_rate = clk_hw_get_rate(parent); 151 if (!parent_rate) 152 return; 153 154 for (; shift < PERIPHERAL_MAX_SHIFT; shift++) { 155 if (parent_rate >> shift <= periph->range.max) 156 break; 157 } 158 } 159 160 periph->auto_div = false; 161 periph->div = shift; 162 } 163 164 static int clk_sam9x5_peripheral_set(struct clk_sam9x5_peripheral *periph, 165 unsigned int status) 166 { 167 unsigned long flags; 168 unsigned int enable = status ? AT91_PMC_PCR_EN : 0; 169 170 if (periph->id < PERIPHERAL_ID_MIN) 171 return 0; 172 173 spin_lock_irqsave(periph->lock, flags); 174 regmap_write(periph->regmap, periph->layout->offset, 175 (periph->id & periph->layout->pid_mask)); 176 regmap_update_bits(periph->regmap, periph->layout->offset, 177 periph->layout->div_mask | periph->layout->cmd | 178 enable, 179 field_prep(periph->layout->div_mask, periph->div) | 180 periph->layout->cmd | enable); 181 spin_unlock_irqrestore(periph->lock, flags); 182 183 return 0; 184 } 185 186 static int clk_sam9x5_peripheral_enable(struct clk_hw *hw) 187 { 188 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 189 190 return clk_sam9x5_peripheral_set(periph, 1); 191 } 192 193 static void clk_sam9x5_peripheral_disable(struct clk_hw *hw) 194 { 195 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 196 unsigned long flags; 197 198 if (periph->id < PERIPHERAL_ID_MIN) 199 return; 200 201 spin_lock_irqsave(periph->lock, flags); 202 regmap_write(periph->regmap, periph->layout->offset, 203 (periph->id & periph->layout->pid_mask)); 204 regmap_update_bits(periph->regmap, periph->layout->offset, 205 AT91_PMC_PCR_EN | periph->layout->cmd, 206 periph->layout->cmd); 207 spin_unlock_irqrestore(periph->lock, flags); 208 } 209 210 static int clk_sam9x5_peripheral_is_enabled(struct clk_hw *hw) 211 { 212 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 213 unsigned long flags; 214 unsigned int status; 215 216 if (periph->id < PERIPHERAL_ID_MIN) 217 return 1; 218 219 spin_lock_irqsave(periph->lock, flags); 220 regmap_write(periph->regmap, periph->layout->offset, 221 (periph->id & periph->layout->pid_mask)); 222 regmap_read(periph->regmap, periph->layout->offset, &status); 223 spin_unlock_irqrestore(periph->lock, flags); 224 225 return !!(status & AT91_PMC_PCR_EN); 226 } 227 228 static unsigned long 229 clk_sam9x5_peripheral_recalc_rate(struct clk_hw *hw, 230 unsigned long parent_rate) 231 { 232 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 233 unsigned long flags; 234 unsigned int status; 235 236 if (periph->id < PERIPHERAL_ID_MIN) 237 return parent_rate; 238 239 spin_lock_irqsave(periph->lock, flags); 240 regmap_write(periph->regmap, periph->layout->offset, 241 (periph->id & periph->layout->pid_mask)); 242 regmap_read(periph->regmap, periph->layout->offset, &status); 243 spin_unlock_irqrestore(periph->lock, flags); 244 245 if (status & AT91_PMC_PCR_EN) { 246 periph->div = field_get(periph->layout->div_mask, status); 247 periph->auto_div = false; 248 } else { 249 clk_sam9x5_peripheral_autodiv(periph); 250 } 251 252 return parent_rate >> periph->div; 253 } 254 255 static void clk_sam9x5_peripheral_best_diff(struct clk_rate_request *req, 256 struct clk_hw *parent, 257 unsigned long parent_rate, 258 u32 shift, long *best_diff, 259 long *best_rate) 260 { 261 unsigned long tmp_rate = parent_rate >> shift; 262 unsigned long tmp_diff = abs(req->rate - tmp_rate); 263 264 if (*best_diff < 0 || *best_diff >= tmp_diff) { 265 *best_rate = tmp_rate; 266 *best_diff = tmp_diff; 267 req->best_parent_rate = parent_rate; 268 req->best_parent_hw = parent; 269 } 270 } 271 272 static int clk_sam9x5_peripheral_determine_rate(struct clk_hw *hw, 273 struct clk_rate_request *req) 274 { 275 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 276 struct clk_hw *parent = clk_hw_get_parent(hw); 277 unsigned long parent_rate = clk_hw_get_rate(parent); 278 unsigned long tmp_rate; 279 long best_rate = LONG_MIN; 280 long best_diff = LONG_MIN; 281 u32 shift; 282 283 if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) { 284 req->rate = parent_rate; 285 286 return 0; 287 } 288 289 /* Fist step: check the available dividers. */ 290 for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 291 tmp_rate = parent_rate >> shift; 292 293 if (periph->range.max && tmp_rate > periph->range.max) 294 continue; 295 296 clk_sam9x5_peripheral_best_diff(req, parent, parent_rate, 297 shift, &best_diff, &best_rate); 298 299 if (!best_diff || best_rate <= req->rate) 300 break; 301 } 302 303 if (periph->chg_pid < 0) 304 goto end; 305 306 /* Step two: try to request rate from parent. */ 307 parent = clk_hw_get_parent_by_index(hw, periph->chg_pid); 308 if (!parent) 309 goto end; 310 311 for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 312 struct clk_rate_request req_parent; 313 314 clk_hw_forward_rate_request(hw, req, parent, &req_parent, req->rate << shift); 315 if (__clk_determine_rate(parent, &req_parent)) 316 continue; 317 318 clk_sam9x5_peripheral_best_diff(req, parent, req_parent.rate, 319 shift, &best_diff, &best_rate); 320 321 if (!best_diff) 322 break; 323 } 324 end: 325 if (best_rate < 0 || 326 (periph->range.max && best_rate > periph->range.max)) 327 return -EINVAL; 328 329 pr_debug("PCK: %s, best_rate = %ld, parent clk: %s @ %ld\n", 330 __func__, best_rate, 331 __clk_get_name((req->best_parent_hw)->clk), 332 req->best_parent_rate); 333 334 req->rate = best_rate; 335 336 return 0; 337 } 338 339 static int clk_sam9x5_peripheral_no_parent_determine_rate(struct clk_hw *hw, 340 struct clk_rate_request *req) 341 { 342 int shift = 0; 343 unsigned long best_rate; 344 unsigned long best_diff; 345 unsigned long cur_rate = req->best_parent_rate; 346 unsigned long cur_diff; 347 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 348 349 if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) { 350 req->rate = req->best_parent_rate; 351 352 return 0; 353 } 354 355 if (periph->range.max) { 356 for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 357 cur_rate = req->best_parent_rate >> shift; 358 if (cur_rate <= periph->range.max) 359 break; 360 } 361 } 362 363 if (req->rate >= cur_rate) { 364 req->rate = cur_rate; 365 366 return 0; 367 } 368 369 best_diff = cur_rate - req->rate; 370 best_rate = cur_rate; 371 for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 372 cur_rate = req->best_parent_rate >> shift; 373 if (cur_rate < req->rate) 374 cur_diff = req->rate - cur_rate; 375 else 376 cur_diff = cur_rate - req->rate; 377 378 if (cur_diff < best_diff) { 379 best_diff = cur_diff; 380 best_rate = cur_rate; 381 } 382 383 if (!best_diff || cur_rate < req->rate) 384 break; 385 } 386 387 req->rate = best_rate; 388 389 return 0; 390 } 391 392 static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw, 393 unsigned long rate, 394 unsigned long parent_rate) 395 { 396 int shift; 397 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 398 if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) { 399 if (parent_rate == rate) 400 return 0; 401 else 402 return -EINVAL; 403 } 404 405 if (periph->range.max && rate > periph->range.max) 406 return -EINVAL; 407 408 for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 409 if (parent_rate >> shift == rate) { 410 periph->auto_div = false; 411 periph->div = shift; 412 return 0; 413 } 414 } 415 416 return -EINVAL; 417 } 418 419 static int clk_sam9x5_peripheral_save_context(struct clk_hw *hw) 420 { 421 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 422 423 periph->pms.status = clk_sam9x5_peripheral_is_enabled(hw); 424 425 return 0; 426 } 427 428 static void clk_sam9x5_peripheral_restore_context(struct clk_hw *hw) 429 { 430 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 431 432 if (periph->pms.status) 433 clk_sam9x5_peripheral_set(periph, periph->pms.status); 434 } 435 436 static const struct clk_ops sam9x5_peripheral_ops = { 437 .enable = clk_sam9x5_peripheral_enable, 438 .disable = clk_sam9x5_peripheral_disable, 439 .is_enabled = clk_sam9x5_peripheral_is_enabled, 440 .recalc_rate = clk_sam9x5_peripheral_recalc_rate, 441 .determine_rate = clk_sam9x5_peripheral_no_parent_determine_rate, 442 .set_rate = clk_sam9x5_peripheral_set_rate, 443 .save_context = clk_sam9x5_peripheral_save_context, 444 .restore_context = clk_sam9x5_peripheral_restore_context, 445 }; 446 447 static const struct clk_ops sam9x5_peripheral_chg_ops = { 448 .enable = clk_sam9x5_peripheral_enable, 449 .disable = clk_sam9x5_peripheral_disable, 450 .is_enabled = clk_sam9x5_peripheral_is_enabled, 451 .recalc_rate = clk_sam9x5_peripheral_recalc_rate, 452 .determine_rate = clk_sam9x5_peripheral_determine_rate, 453 .set_rate = clk_sam9x5_peripheral_set_rate, 454 .save_context = clk_sam9x5_peripheral_save_context, 455 .restore_context = clk_sam9x5_peripheral_restore_context, 456 }; 457 458 struct clk_hw * __init 459 at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock, 460 const struct clk_pcr_layout *layout, 461 const char *name, const char *parent_name, 462 struct clk_hw *parent_hw, 463 u32 id, const struct clk_range *range, 464 int chg_pid, unsigned long flags) 465 { 466 struct clk_sam9x5_peripheral *periph; 467 struct clk_init_data init = {}; 468 struct clk_hw *hw; 469 int ret; 470 471 if (!name || !(parent_name || parent_hw)) 472 return ERR_PTR(-EINVAL); 473 474 periph = kzalloc(sizeof(*periph), GFP_KERNEL); 475 if (!periph) 476 return ERR_PTR(-ENOMEM); 477 478 init.name = name; 479 if (parent_hw) 480 init.parent_hws = (const struct clk_hw **)&parent_hw; 481 else 482 init.parent_names = &parent_name; 483 init.num_parents = 1; 484 init.flags = flags; 485 if (chg_pid < 0) { 486 init.ops = &sam9x5_peripheral_ops; 487 } else { 488 init.flags |= CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE | 489 CLK_SET_RATE_PARENT; 490 init.ops = &sam9x5_peripheral_chg_ops; 491 } 492 493 periph->id = id; 494 periph->hw.init = &init; 495 periph->div = 0; 496 periph->regmap = regmap; 497 periph->lock = lock; 498 if (layout->div_mask) 499 periph->auto_div = true; 500 periph->layout = layout; 501 periph->range = *range; 502 periph->chg_pid = chg_pid; 503 504 hw = &periph->hw; 505 ret = clk_hw_register(NULL, &periph->hw); 506 if (ret) { 507 kfree(periph); 508 hw = ERR_PTR(ret); 509 } else { 510 clk_sam9x5_peripheral_autodiv(periph); 511 } 512 513 return hw; 514 } 515