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 unsigned long orig_parent_rate = *best_parent_rate; 56 unsigned long parent_rate, current_rate, best_rate = ~0; 57 unsigned int i, bestmult = 0; 58 59 if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) 60 return rate / *best_parent_rate; 61 62 for (i = 1; i < ((1 << width) - 1); i++) { 63 if (rate == orig_parent_rate * i) { 64 /* 65 * This is the best case for us if we have a 66 * perfect match without changing the parent 67 * rate. 68 */ 69 *best_parent_rate = orig_parent_rate; 70 return i; 71 } 72 73 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw), 74 rate / i); 75 current_rate = parent_rate * i; 76 77 if (__is_best_rate(rate, current_rate, best_rate, flags)) { 78 bestmult = i; 79 best_rate = current_rate; 80 *best_parent_rate = parent_rate; 81 } 82 } 83 84 return bestmult; 85 } 86 87 static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate, 88 unsigned long *parent_rate) 89 { 90 struct clk_multiplier *mult = to_clk_multiplier(hw); 91 unsigned long factor = __bestmult(hw, rate, parent_rate, 92 mult->width, mult->flags); 93 94 return *parent_rate * factor; 95 } 96 97 static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate, 98 unsigned long parent_rate) 99 { 100 struct clk_multiplier *mult = to_clk_multiplier(hw); 101 unsigned long factor = __get_mult(mult, rate, parent_rate); 102 unsigned long flags = 0; 103 unsigned long val; 104 105 if (mult->lock) 106 spin_lock_irqsave(mult->lock, flags); 107 else 108 __acquire(mult->lock); 109 110 val = clk_readl(mult->reg); 111 val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift); 112 val |= factor << mult->shift; 113 clk_writel(val, mult->reg); 114 115 if (mult->lock) 116 spin_unlock_irqrestore(mult->lock, flags); 117 else 118 __release(mult->lock); 119 120 return 0; 121 } 122 123 const struct clk_ops clk_multiplier_ops = { 124 .recalc_rate = clk_multiplier_recalc_rate, 125 .round_rate = clk_multiplier_round_rate, 126 .set_rate = clk_multiplier_set_rate, 127 }; 128 EXPORT_SYMBOL_GPL(clk_multiplier_ops); 129