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, u32 id) 101 { 102 struct clk_peripheral *periph; 103 struct clk_init_data init; 104 struct clk_hw *hw; 105 int ret; 106 107 if (!name || !parent_name || id > PERIPHERAL_ID_MAX) 108 return ERR_PTR(-EINVAL); 109 110 periph = kzalloc(sizeof(*periph), GFP_KERNEL); 111 if (!periph) 112 return ERR_PTR(-ENOMEM); 113 114 init.name = name; 115 init.ops = &peripheral_ops; 116 init.parent_names = &parent_name; 117 init.num_parents = 1; 118 init.flags = 0; 119 120 periph->id = id; 121 periph->hw.init = &init; 122 periph->regmap = regmap; 123 124 hw = &periph->hw; 125 ret = clk_hw_register(NULL, &periph->hw); 126 if (ret) { 127 kfree(periph); 128 hw = ERR_PTR(ret); 129 } 130 131 return hw; 132 } 133 134 static void clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral *periph) 135 { 136 struct clk_hw *parent; 137 unsigned long parent_rate; 138 int shift = 0; 139 140 if (!periph->auto_div) 141 return; 142 143 if (periph->range.max) { 144 parent = clk_hw_get_parent_by_index(&periph->hw, 0); 145 parent_rate = clk_hw_get_rate(parent); 146 if (!parent_rate) 147 return; 148 149 for (; shift < PERIPHERAL_MAX_SHIFT; shift++) { 150 if (parent_rate >> shift <= periph->range.max) 151 break; 152 } 153 } 154 155 periph->auto_div = false; 156 periph->div = shift; 157 } 158 159 static int clk_sam9x5_peripheral_set(struct clk_sam9x5_peripheral *periph, 160 unsigned int status) 161 { 162 unsigned long flags; 163 unsigned int enable = status ? AT91_PMC_PCR_EN : 0; 164 165 if (periph->id < PERIPHERAL_ID_MIN) 166 return 0; 167 168 spin_lock_irqsave(periph->lock, flags); 169 regmap_write(periph->regmap, periph->layout->offset, 170 (periph->id & periph->layout->pid_mask)); 171 regmap_update_bits(periph->regmap, periph->layout->offset, 172 periph->layout->div_mask | periph->layout->cmd | 173 enable, 174 field_prep(periph->layout->div_mask, periph->div) | 175 periph->layout->cmd | enable); 176 spin_unlock_irqrestore(periph->lock, flags); 177 178 return 0; 179 } 180 181 static int clk_sam9x5_peripheral_enable(struct clk_hw *hw) 182 { 183 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 184 185 return clk_sam9x5_peripheral_set(periph, 1); 186 } 187 188 static void clk_sam9x5_peripheral_disable(struct clk_hw *hw) 189 { 190 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 191 unsigned long flags; 192 193 if (periph->id < PERIPHERAL_ID_MIN) 194 return; 195 196 spin_lock_irqsave(periph->lock, flags); 197 regmap_write(periph->regmap, periph->layout->offset, 198 (periph->id & periph->layout->pid_mask)); 199 regmap_update_bits(periph->regmap, periph->layout->offset, 200 AT91_PMC_PCR_EN | periph->layout->cmd, 201 periph->layout->cmd); 202 spin_unlock_irqrestore(periph->lock, flags); 203 } 204 205 static int clk_sam9x5_peripheral_is_enabled(struct clk_hw *hw) 206 { 207 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 208 unsigned long flags; 209 unsigned int status; 210 211 if (periph->id < PERIPHERAL_ID_MIN) 212 return 1; 213 214 spin_lock_irqsave(periph->lock, flags); 215 regmap_write(periph->regmap, periph->layout->offset, 216 (periph->id & periph->layout->pid_mask)); 217 regmap_read(periph->regmap, periph->layout->offset, &status); 218 spin_unlock_irqrestore(periph->lock, flags); 219 220 return !!(status & AT91_PMC_PCR_EN); 221 } 222 223 static unsigned long 224 clk_sam9x5_peripheral_recalc_rate(struct clk_hw *hw, 225 unsigned long parent_rate) 226 { 227 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 228 unsigned long flags; 229 unsigned int status; 230 231 if (periph->id < PERIPHERAL_ID_MIN) 232 return parent_rate; 233 234 spin_lock_irqsave(periph->lock, flags); 235 regmap_write(periph->regmap, periph->layout->offset, 236 (periph->id & periph->layout->pid_mask)); 237 regmap_read(periph->regmap, periph->layout->offset, &status); 238 spin_unlock_irqrestore(periph->lock, flags); 239 240 if (status & AT91_PMC_PCR_EN) { 241 periph->div = field_get(periph->layout->div_mask, status); 242 periph->auto_div = false; 243 } else { 244 clk_sam9x5_peripheral_autodiv(periph); 245 } 246 247 return parent_rate >> periph->div; 248 } 249 250 static void clk_sam9x5_peripheral_best_diff(struct clk_rate_request *req, 251 struct clk_hw *parent, 252 unsigned long parent_rate, 253 u32 shift, long *best_diff, 254 long *best_rate) 255 { 256 unsigned long tmp_rate = parent_rate >> shift; 257 unsigned long tmp_diff = abs(req->rate - tmp_rate); 258 259 if (*best_diff < 0 || *best_diff >= tmp_diff) { 260 *best_rate = tmp_rate; 261 *best_diff = tmp_diff; 262 req->best_parent_rate = parent_rate; 263 req->best_parent_hw = parent; 264 } 265 } 266 267 static int clk_sam9x5_peripheral_determine_rate(struct clk_hw *hw, 268 struct clk_rate_request *req) 269 { 270 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 271 struct clk_hw *parent = clk_hw_get_parent(hw); 272 unsigned long parent_rate = clk_hw_get_rate(parent); 273 unsigned long tmp_rate; 274 long best_rate = LONG_MIN; 275 long best_diff = LONG_MIN; 276 u32 shift; 277 278 if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) 279 return parent_rate; 280 281 /* Fist step: check the available dividers. */ 282 for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 283 tmp_rate = parent_rate >> shift; 284 285 if (periph->range.max && tmp_rate > periph->range.max) 286 continue; 287 288 clk_sam9x5_peripheral_best_diff(req, parent, parent_rate, 289 shift, &best_diff, &best_rate); 290 291 if (!best_diff || best_rate <= req->rate) 292 break; 293 } 294 295 if (periph->chg_pid < 0) 296 goto end; 297 298 /* Step two: try to request rate from parent. */ 299 parent = clk_hw_get_parent_by_index(hw, periph->chg_pid); 300 if (!parent) 301 goto end; 302 303 for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 304 struct clk_rate_request req_parent; 305 306 clk_hw_forward_rate_request(hw, req, parent, &req_parent, req->rate << shift); 307 if (__clk_determine_rate(parent, &req_parent)) 308 continue; 309 310 clk_sam9x5_peripheral_best_diff(req, parent, req_parent.rate, 311 shift, &best_diff, &best_rate); 312 313 if (!best_diff) 314 break; 315 } 316 end: 317 if (best_rate < 0 || 318 (periph->range.max && best_rate > periph->range.max)) 319 return -EINVAL; 320 321 pr_debug("PCK: %s, best_rate = %ld, parent clk: %s @ %ld\n", 322 __func__, best_rate, 323 __clk_get_name((req->best_parent_hw)->clk), 324 req->best_parent_rate); 325 326 req->rate = best_rate; 327 328 return 0; 329 } 330 331 static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw, 332 unsigned long rate, 333 unsigned long *parent_rate) 334 { 335 int shift = 0; 336 unsigned long best_rate; 337 unsigned long best_diff; 338 unsigned long cur_rate = *parent_rate; 339 unsigned long cur_diff; 340 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 341 342 if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) 343 return *parent_rate; 344 345 if (periph->range.max) { 346 for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 347 cur_rate = *parent_rate >> shift; 348 if (cur_rate <= periph->range.max) 349 break; 350 } 351 } 352 353 if (rate >= cur_rate) 354 return cur_rate; 355 356 best_diff = cur_rate - rate; 357 best_rate = cur_rate; 358 for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 359 cur_rate = *parent_rate >> shift; 360 if (cur_rate < rate) 361 cur_diff = rate - cur_rate; 362 else 363 cur_diff = cur_rate - rate; 364 365 if (cur_diff < best_diff) { 366 best_diff = cur_diff; 367 best_rate = cur_rate; 368 } 369 370 if (!best_diff || cur_rate < rate) 371 break; 372 } 373 374 return best_rate; 375 } 376 377 static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw, 378 unsigned long rate, 379 unsigned long parent_rate) 380 { 381 int shift; 382 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 383 if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) { 384 if (parent_rate == rate) 385 return 0; 386 else 387 return -EINVAL; 388 } 389 390 if (periph->range.max && rate > periph->range.max) 391 return -EINVAL; 392 393 for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) { 394 if (parent_rate >> shift == rate) { 395 periph->auto_div = false; 396 periph->div = shift; 397 return 0; 398 } 399 } 400 401 return -EINVAL; 402 } 403 404 static int clk_sam9x5_peripheral_save_context(struct clk_hw *hw) 405 { 406 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 407 408 periph->pms.status = clk_sam9x5_peripheral_is_enabled(hw); 409 410 return 0; 411 } 412 413 static void clk_sam9x5_peripheral_restore_context(struct clk_hw *hw) 414 { 415 struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw); 416 417 if (periph->pms.status) 418 clk_sam9x5_peripheral_set(periph, periph->pms.status); 419 } 420 421 static const struct clk_ops sam9x5_peripheral_ops = { 422 .enable = clk_sam9x5_peripheral_enable, 423 .disable = clk_sam9x5_peripheral_disable, 424 .is_enabled = clk_sam9x5_peripheral_is_enabled, 425 .recalc_rate = clk_sam9x5_peripheral_recalc_rate, 426 .round_rate = clk_sam9x5_peripheral_round_rate, 427 .set_rate = clk_sam9x5_peripheral_set_rate, 428 .save_context = clk_sam9x5_peripheral_save_context, 429 .restore_context = clk_sam9x5_peripheral_restore_context, 430 }; 431 432 static const struct clk_ops sam9x5_peripheral_chg_ops = { 433 .enable = clk_sam9x5_peripheral_enable, 434 .disable = clk_sam9x5_peripheral_disable, 435 .is_enabled = clk_sam9x5_peripheral_is_enabled, 436 .recalc_rate = clk_sam9x5_peripheral_recalc_rate, 437 .determine_rate = clk_sam9x5_peripheral_determine_rate, 438 .set_rate = clk_sam9x5_peripheral_set_rate, 439 .save_context = clk_sam9x5_peripheral_save_context, 440 .restore_context = clk_sam9x5_peripheral_restore_context, 441 }; 442 443 struct clk_hw * __init 444 at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock, 445 const struct clk_pcr_layout *layout, 446 const char *name, const char *parent_name, 447 u32 id, const struct clk_range *range, 448 int chg_pid, unsigned long flags) 449 { 450 struct clk_sam9x5_peripheral *periph; 451 struct clk_init_data init; 452 struct clk_hw *hw; 453 int ret; 454 455 if (!name || !parent_name) 456 return ERR_PTR(-EINVAL); 457 458 periph = kzalloc(sizeof(*periph), GFP_KERNEL); 459 if (!periph) 460 return ERR_PTR(-ENOMEM); 461 462 init.name = name; 463 init.parent_names = &parent_name; 464 init.num_parents = 1; 465 init.flags = flags; 466 if (chg_pid < 0) { 467 init.ops = &sam9x5_peripheral_ops; 468 } else { 469 init.flags |= CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE | 470 CLK_SET_RATE_PARENT; 471 init.ops = &sam9x5_peripheral_chg_ops; 472 } 473 474 periph->id = id; 475 periph->hw.init = &init; 476 periph->div = 0; 477 periph->regmap = regmap; 478 periph->lock = lock; 479 if (layout->div_mask) 480 periph->auto_div = true; 481 periph->layout = layout; 482 periph->range = *range; 483 periph->chg_pid = chg_pid; 484 485 hw = &periph->hw; 486 ret = clk_hw_register(NULL, &periph->hw); 487 if (ret) { 488 kfree(periph); 489 hw = ERR_PTR(ret); 490 } else { 491 clk_sam9x5_peripheral_autodiv(periph); 492 } 493 494 return hw; 495 } 496