1 /* 2 * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com> 3 * 4 * This program is free software; you can redistribute it and/or modify 5 * it under the terms of the GNU General Public License version 2 as 6 * published by the Free Software Foundation. 7 */ 8 9 #include <linux/bitops.h> 10 #include <linux/clk-provider.h> 11 #include <linux/err.h> 12 #include <linux/export.h> 13 #include <linux/kernel.h> 14 #include <linux/of.h> 15 #include <linux/slab.h> 16 17 static unsigned long __get_mult(struct clk_multiplier *mult, 18 unsigned long rate, 19 unsigned long parent_rate) 20 { 21 if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST) 22 return DIV_ROUND_CLOSEST(rate, parent_rate); 23 24 return rate / parent_rate; 25 } 26 27 static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw, 28 unsigned long parent_rate) 29 { 30 struct clk_multiplier *mult = to_clk_multiplier(hw); 31 unsigned long val; 32 33 val = clk_readl(mult->reg) >> mult->shift; 34 val &= GENMASK(mult->width - 1, 0); 35 36 if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS) 37 val = 1; 38 39 return parent_rate * val; 40 } 41 42 static bool __is_best_rate(unsigned long rate, unsigned long new, 43 unsigned long best, unsigned long flags) 44 { 45 if (flags & CLK_MULTIPLIER_ROUND_CLOSEST) 46 return abs(rate - new) < abs(rate - best); 47 48 return new >= rate && new < best; 49 } 50 51 static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate, 52 unsigned long *best_parent_rate, 53 u8 width, unsigned long flags) 54 { 55 struct clk_multiplier *mult = to_clk_multiplier(hw); 56 unsigned long orig_parent_rate = *best_parent_rate; 57 unsigned long parent_rate, current_rate, best_rate = ~0; 58 unsigned int i, bestmult = 0; 59 unsigned int maxmult = (1 << width) - 1; 60 61 if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) { 62 bestmult = rate / orig_parent_rate; 63 64 /* Make sure we don't end up with a 0 multiplier */ 65 if ((bestmult == 0) && 66 !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)) 67 bestmult = 1; 68 69 /* Make sure we don't overflow the multiplier */ 70 if (bestmult > maxmult) 71 bestmult = maxmult; 72 73 return bestmult; 74 } 75 76 for (i = 1; i < maxmult; i++) { 77 if (rate == orig_parent_rate * i) { 78 /* 79 * This is the best case for us if we have a 80 * perfect match without changing the parent 81 * rate. 82 */ 83 *best_parent_rate = orig_parent_rate; 84 return i; 85 } 86 87 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw), 88 rate / i); 89 current_rate = parent_rate * i; 90 91 if (__is_best_rate(rate, current_rate, best_rate, flags)) { 92 bestmult = i; 93 best_rate = current_rate; 94 *best_parent_rate = parent_rate; 95 } 96 } 97 98 return bestmult; 99 } 100 101 static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate, 102 unsigned long *parent_rate) 103 { 104 struct clk_multiplier *mult = to_clk_multiplier(hw); 105 unsigned long factor = __bestmult(hw, rate, parent_rate, 106 mult->width, mult->flags); 107 108 return *parent_rate * factor; 109 } 110 111 static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate, 112 unsigned long parent_rate) 113 { 114 struct clk_multiplier *mult = to_clk_multiplier(hw); 115 unsigned long factor = __get_mult(mult, rate, parent_rate); 116 unsigned long flags = 0; 117 unsigned long val; 118 119 if (mult->lock) 120 spin_lock_irqsave(mult->lock, flags); 121 else 122 __acquire(mult->lock); 123 124 val = clk_readl(mult->reg); 125 val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift); 126 val |= factor << mult->shift; 127 clk_writel(val, mult->reg); 128 129 if (mult->lock) 130 spin_unlock_irqrestore(mult->lock, flags); 131 else 132 __release(mult->lock); 133 134 return 0; 135 } 136 137 const struct clk_ops clk_multiplier_ops = { 138 .recalc_rate = clk_multiplier_recalc_rate, 139 .round_rate = clk_multiplier_round_rate, 140 .set_rate = clk_multiplier_set_rate, 141 }; 142 EXPORT_SYMBOL_GPL(clk_multiplier_ops); 143