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