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