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 return parent_rate; 284 285 /* Fist step: check the available dividers. */ 286 for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 287 tmp_rate = parent_rate >> shift; 288 289 if (periph->range.max && tmp_rate > periph->range.max) 290 continue; 291 292 clk_sam9x5_peripheral_best_diff(req, parent, parent_rate, 293 shift, &best_diff, &best_rate); 294 295 if (!best_diff || best_rate <= req->rate) 296 break; 297 } 298 299 if (periph->chg_pid < 0) 300 goto end; 301 302 /* Step two: try to request rate from parent. */ 303 parent = clk_hw_get_parent_by_index(hw, periph->chg_pid); 304 if (!parent) 305 goto end; 306 307 for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 308 struct clk_rate_request req_parent; 309 310 clk_hw_forward_rate_request(hw, req, parent, &req_parent, req->rate << shift); 311 if (__clk_determine_rate(parent, &req_parent)) 312 continue; 313 314 clk_sam9x5_peripheral_best_diff(req, parent, req_parent.rate, 315 shift, &best_diff, &best_rate); 316 317 if (!best_diff) 318 break; 319 } 320 end: 321 if (best_rate < 0 || 322 (periph->range.max && best_rate > periph->range.max)) 323 return -EINVAL; 324 325 pr_debug("PCK: %s, best_rate = %ld, parent clk: %s @ %ld\n", 326 __func__, best_rate, 327 __clk_get_name((req->best_parent_hw)->clk), 328 req->best_parent_rate); 329 330 req->rate = best_rate; 331 332 return 0; 333 } 334 335 static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw, 336 unsigned long rate, 337 unsigned long *parent_rate) 338 { 339 int shift = 0; 340 unsigned long best_rate; 341 unsigned long best_diff; 342 unsigned long cur_rate = *parent_rate; 343 unsigned long cur_diff; 344 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 345 346 if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) 347 return *parent_rate; 348 349 if (periph->range.max) { 350 for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 351 cur_rate = *parent_rate >> shift; 352 if (cur_rate <= periph->range.max) 353 break; 354 } 355 } 356 357 if (rate >= cur_rate) 358 return cur_rate; 359 360 best_diff = cur_rate - rate; 361 best_rate = cur_rate; 362 for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 363 cur_rate = *parent_rate >> shift; 364 if (cur_rate < rate) 365 cur_diff = rate - cur_rate; 366 else 367 cur_diff = cur_rate - rate; 368 369 if (cur_diff < best_diff) { 370 best_diff = cur_diff; 371 best_rate = cur_rate; 372 } 373 374 if (!best_diff || cur_rate < rate) 375 break; 376 } 377 378 return best_rate; 379 } 380 381 static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw, 382 unsigned long rate, 383 unsigned long parent_rate) 384 { 385 int shift; 386 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 387 if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) { 388 if (parent_rate == rate) 389 return 0; 390 else 391 return -EINVAL; 392 } 393 394 if (periph->range.max && rate > periph->range.max) 395 return -EINVAL; 396 397 for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 398 if (parent_rate >> shift == rate) { 399 periph->auto_div = false; 400 periph->div = shift; 401 return 0; 402 } 403 } 404 405 return -EINVAL; 406 } 407 408 static int clk_sam9x5_peripheral_save_context(struct clk_hw *hw) 409 { 410 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 411 412 periph->pms.status = clk_sam9x5_peripheral_is_enabled(hw); 413 414 return 0; 415 } 416 417 static void clk_sam9x5_peripheral_restore_context(struct clk_hw *hw) 418 { 419 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 420 421 if (periph->pms.status) 422 clk_sam9x5_peripheral_set(periph, periph->pms.status); 423 } 424 425 static const struct clk_ops sam9x5_peripheral_ops = { 426 .enable = clk_sam9x5_peripheral_enable, 427 .disable = clk_sam9x5_peripheral_disable, 428 .is_enabled = clk_sam9x5_peripheral_is_enabled, 429 .recalc_rate = clk_sam9x5_peripheral_recalc_rate, 430 .round_rate = clk_sam9x5_peripheral_round_rate, 431 .set_rate = clk_sam9x5_peripheral_set_rate, 432 .save_context = clk_sam9x5_peripheral_save_context, 433 .restore_context = clk_sam9x5_peripheral_restore_context, 434 }; 435 436 static const struct clk_ops sam9x5_peripheral_chg_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_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 struct clk_hw * __init 448 at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock, 449 const struct clk_pcr_layout *layout, 450 const char *name, const char *parent_name, 451 struct clk_hw *parent_hw, 452 u32 id, const struct clk_range *range, 453 int chg_pid, unsigned long flags) 454 { 455 struct clk_sam9x5_peripheral *periph; 456 struct clk_init_data init = {}; 457 struct clk_hw *hw; 458 int ret; 459 460 if (!name || !(parent_name || parent_hw)) 461 return ERR_PTR(-EINVAL); 462 463 periph = kzalloc(sizeof(*periph), GFP_KERNEL); 464 if (!periph) 465 return ERR_PTR(-ENOMEM); 466 467 init.name = name; 468 if (parent_hw) 469 init.parent_hws = (const struct clk_hw **)&parent_hw; 470 else 471 init.parent_names = &parent_name; 472 init.num_parents = 1; 473 init.flags = flags; 474 if (chg_pid < 0) { 475 init.ops = &sam9x5_peripheral_ops; 476 } else { 477 init.flags |= CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE | 478 CLK_SET_RATE_PARENT; 479 init.ops = &sam9x5_peripheral_chg_ops; 480 } 481 482 periph->id = id; 483 periph->hw.init = &init; 484 periph->div = 0; 485 periph->regmap = regmap; 486 periph->lock = lock; 487 if (layout->div_mask) 488 periph->auto_div = true; 489 periph->layout = layout; 490 periph->range = *range; 491 periph->chg_pid = chg_pid; 492 493 hw = &periph->hw; 494 ret = clk_hw_register(NULL, &periph->hw); 495 if (ret) { 496 kfree(periph); 497 hw = ERR_PTR(ret); 498 } else { 499 clk_sam9x5_peripheral_autodiv(periph); 500 } 501 502 return hw; 503 } 504