504 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*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 16DEFINE_SPINLOCK(pmc_pcr_lock);17 18#define PERIPHERAL_ID_MIN 219#define PERIPHERAL_ID_MAX 3120#define PERIPHERAL_MASK(id) (1 << ((id) & PERIPHERAL_ID_MAX))21 22#define PERIPHERAL_MAX_SHIFT 323 24struct 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 32struct 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 48static 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 63static 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 76static 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 92static 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 98struct clk_hw * __init99at91_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 else120 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 138static 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 163static 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 185static 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 192static 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 209static 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 227static unsigned long228clk_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 254static 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 271static 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 }320end: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 335static 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 else367 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 381static 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 else391 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 408static 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 417static 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 425static 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 436static 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 447struct clk_hw * __init448at91_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 else471 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