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/io.h> 11 #include <linux/kernel.h> 12 #include <linux/of.h> 13 #include <linux/slab.h> 14 15 static 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 23 static 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 else 28 writel(val, mult->reg); 29 } 30 31 static 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 41 static 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 56 static 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 65 static 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 a 94 * perfect match without changing the parent 95 * 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 115 static 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 125 static 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 else 136 __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 else 146 __release(mult->lock); 147 148 return 0; 149 } 150 151 const 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 }; 156 EXPORT_SYMBOL_GPL(clk_multiplier_ops); 157