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