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