1 /* 2 * Copyright (C) 2014 Intel Corporation 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 * Adjustable fractional divider clock implementation. 9 * Output rate = (m / n) * parent_rate. 10 * Uses rational best approximation algorithm. 11 */ 12 13 #include <linux/clk-provider.h> 14 #include <linux/module.h> 15 #include <linux/device.h> 16 #include <linux/slab.h> 17 #include <linux/rational.h> 18 19 static unsigned long clk_fd_recalc_rate(struct clk_hw *hw, 20 unsigned long parent_rate) 21 { 22 struct clk_fractional_divider *fd = to_clk_fd(hw); 23 unsigned long flags = 0; 24 unsigned long m, n; 25 u32 val; 26 u64 ret; 27 28 if (fd->lock) 29 spin_lock_irqsave(fd->lock, flags); 30 else 31 __acquire(fd->lock); 32 33 val = clk_readl(fd->reg); 34 35 if (fd->lock) 36 spin_unlock_irqrestore(fd->lock, flags); 37 else 38 __release(fd->lock); 39 40 m = (val & fd->mmask) >> fd->mshift; 41 n = (val & fd->nmask) >> fd->nshift; 42 43 if (!n || !m) 44 return parent_rate; 45 46 ret = (u64)parent_rate * m; 47 do_div(ret, n); 48 49 return ret; 50 } 51 52 static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate, 53 unsigned long *parent_rate) 54 { 55 struct clk_fractional_divider *fd = to_clk_fd(hw); 56 unsigned long scale; 57 unsigned long m, n; 58 u64 ret; 59 60 if (!rate || rate >= *parent_rate) 61 return *parent_rate; 62 63 /* 64 * Get rate closer to *parent_rate to guarantee there is no overflow 65 * for m and n. In the result it will be the nearest rate left shifted 66 * by (scale - fd->nwidth) bits. 67 */ 68 scale = fls_long(*parent_rate / rate - 1); 69 if (scale > fd->nwidth) 70 rate <<= scale - fd->nwidth; 71 72 rational_best_approximation(rate, *parent_rate, 73 GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0), 74 &m, &n); 75 76 ret = (u64)*parent_rate * m; 77 do_div(ret, n); 78 79 return ret; 80 } 81 82 static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate, 83 unsigned long parent_rate) 84 { 85 struct clk_fractional_divider *fd = to_clk_fd(hw); 86 unsigned long flags = 0; 87 unsigned long m, n; 88 u32 val; 89 90 rational_best_approximation(rate, parent_rate, 91 GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0), 92 &m, &n); 93 94 if (fd->lock) 95 spin_lock_irqsave(fd->lock, flags); 96 else 97 __acquire(fd->lock); 98 99 val = clk_readl(fd->reg); 100 val &= ~(fd->mmask | fd->nmask); 101 val |= (m << fd->mshift) | (n << fd->nshift); 102 clk_writel(val, fd->reg); 103 104 if (fd->lock) 105 spin_unlock_irqrestore(fd->lock, flags); 106 else 107 __release(fd->lock); 108 109 return 0; 110 } 111 112 const struct clk_ops clk_fractional_divider_ops = { 113 .recalc_rate = clk_fd_recalc_rate, 114 .round_rate = clk_fd_round_rate, 115 .set_rate = clk_fd_set_rate, 116 }; 117 EXPORT_SYMBOL_GPL(clk_fractional_divider_ops); 118 119 struct clk_hw *clk_hw_register_fractional_divider(struct device *dev, 120 const char *name, const char *parent_name, unsigned long flags, 121 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth, 122 u8 clk_divider_flags, spinlock_t *lock) 123 { 124 struct clk_fractional_divider *fd; 125 struct clk_init_data init; 126 struct clk_hw *hw; 127 int ret; 128 129 fd = kzalloc(sizeof(*fd), GFP_KERNEL); 130 if (!fd) 131 return ERR_PTR(-ENOMEM); 132 133 init.name = name; 134 init.ops = &clk_fractional_divider_ops; 135 init.flags = flags | CLK_IS_BASIC; 136 init.parent_names = parent_name ? &parent_name : NULL; 137 init.num_parents = parent_name ? 1 : 0; 138 139 fd->reg = reg; 140 fd->mshift = mshift; 141 fd->mwidth = mwidth; 142 fd->mmask = GENMASK(mwidth - 1, 0) << mshift; 143 fd->nshift = nshift; 144 fd->nwidth = nwidth; 145 fd->nmask = GENMASK(nwidth - 1, 0) << nshift; 146 fd->flags = clk_divider_flags; 147 fd->lock = lock; 148 fd->hw.init = &init; 149 150 hw = &fd->hw; 151 ret = clk_hw_register(dev, hw); 152 if (ret) { 153 kfree(fd); 154 hw = ERR_PTR(ret); 155 } 156 157 return hw; 158 } 159 EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider); 160 161 struct clk *clk_register_fractional_divider(struct device *dev, 162 const char *name, const char *parent_name, unsigned long flags, 163 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth, 164 u8 clk_divider_flags, spinlock_t *lock) 165 { 166 struct clk_hw *hw; 167 168 hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags, 169 reg, mshift, mwidth, nshift, nwidth, clk_divider_flags, 170 lock); 171 if (IS_ERR(hw)) 172 return ERR_CAST(hw); 173 return hw->clk; 174 } 175 EXPORT_SYMBOL_GPL(clk_register_fractional_divider); 176 177 void clk_hw_unregister_fractional_divider(struct clk_hw *hw) 178 { 179 struct clk_fractional_divider *fd; 180 181 fd = to_clk_fd(hw); 182 183 clk_hw_unregister(hw); 184 kfree(fd); 185 } 186