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