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