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