157 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>4 */5 6#include <linux/bitops.h>7#include <linux/clk-provider.h>8#include <linux/err.h>9#include <linux/export.h>10#include <linux/io.h>11#include <linux/kernel.h>12#include <linux/of.h>13#include <linux/slab.h>14 15static inline u32 clk_mult_readl(struct clk_multiplier *mult)16{17 if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)18 return ioread32be(mult->reg);19 20 return readl(mult->reg);21}22 23static inline void clk_mult_writel(struct clk_multiplier *mult, u32 val)24{25 if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)26 iowrite32be(val, mult->reg);27 else28 writel(val, mult->reg);29}30 31static unsigned long __get_mult(struct clk_multiplier *mult,32 unsigned long rate,33 unsigned long parent_rate)34{35 if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)36 return DIV_ROUND_CLOSEST(rate, parent_rate);37 38 return rate / parent_rate;39}40 41static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,42 unsigned long parent_rate)43{44 struct clk_multiplier *mult = to_clk_multiplier(hw);45 unsigned long val;46 47 val = clk_mult_readl(mult) >> mult->shift;48 val &= GENMASK(mult->width - 1, 0);49 50 if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)51 val = 1;52 53 return parent_rate * val;54}55 56static bool __is_best_rate(unsigned long rate, unsigned long new,57 unsigned long best, unsigned long flags)58{59 if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)60 return abs(rate - new) < abs(rate - best);61 62 return new >= rate && new < best;63}64 65static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,66 unsigned long *best_parent_rate,67 u8 width, unsigned long flags)68{69 struct clk_multiplier *mult = to_clk_multiplier(hw);70 unsigned long orig_parent_rate = *best_parent_rate;71 unsigned long parent_rate, current_rate, best_rate = ~0;72 unsigned int i, bestmult = 0;73 unsigned int maxmult = (1 << width) - 1;74 75 if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {76 bestmult = rate / orig_parent_rate;77 78 /* Make sure we don't end up with a 0 multiplier */79 if ((bestmult == 0) &&80 !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))81 bestmult = 1;82 83 /* Make sure we don't overflow the multiplier */84 if (bestmult > maxmult)85 bestmult = maxmult;86 87 return bestmult;88 }89 90 for (i = 1; i < maxmult; i++) {91 if (rate == orig_parent_rate * i) {92 /*93 * This is the best case for us if we have a94 * perfect match without changing the parent95 * rate.96 */97 *best_parent_rate = orig_parent_rate;98 return i;99 }100 101 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),102 rate / i);103 current_rate = parent_rate * i;104 105 if (__is_best_rate(rate, current_rate, best_rate, flags)) {106 bestmult = i;107 best_rate = current_rate;108 *best_parent_rate = parent_rate;109 }110 }111 112 return bestmult;113}114 115static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,116 unsigned long *parent_rate)117{118 struct clk_multiplier *mult = to_clk_multiplier(hw);119 unsigned long factor = __bestmult(hw, rate, parent_rate,120 mult->width, mult->flags);121 122 return *parent_rate * factor;123}124 125static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,126 unsigned long parent_rate)127{128 struct clk_multiplier *mult = to_clk_multiplier(hw);129 unsigned long factor = __get_mult(mult, rate, parent_rate);130 unsigned long flags = 0;131 unsigned long val;132 133 if (mult->lock)134 spin_lock_irqsave(mult->lock, flags);135 else136 __acquire(mult->lock);137 138 val = clk_mult_readl(mult);139 val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);140 val |= factor << mult->shift;141 clk_mult_writel(mult, val);142 143 if (mult->lock)144 spin_unlock_irqrestore(mult->lock, flags);145 else146 __release(mult->lock);147 148 return 0;149}150 151const struct clk_ops clk_multiplier_ops = {152 .recalc_rate = clk_multiplier_recalc_rate,153 .round_rate = clk_multiplier_round_rate,154 .set_rate = clk_multiplier_set_rate,155};156EXPORT_SYMBOL_GPL(clk_multiplier_ops);157