Stephen Boyd | e1bd55e | 2018-12-11 09:57:48 -0800 | [diff] [blame] | 1 | // SPDX-License-Identifier: GPL-2.0 |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 2 | /* |
| 3 | * Copyright (C) 2014 Intel Corporation |
| 4 | * |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 5 | * Adjustable fractional divider clock implementation. |
| 6 | * Output rate = (m / n) * parent_rate. |
Andy Shevchenko | 0777591 | 2015-09-22 18:54:11 +0300 | [diff] [blame] | 7 | * Uses rational best approximation algorithm. |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 8 | */ |
| 9 | |
| 10 | #include <linux/clk-provider.h> |
| 11 | #include <linux/module.h> |
| 12 | #include <linux/device.h> |
| 13 | #include <linux/slab.h> |
Andy Shevchenko | 0777591 | 2015-09-22 18:54:11 +0300 | [diff] [blame] | 14 | #include <linux/rational.h> |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 15 | |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 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; |
Andy Shevchenko | 0777591 | 2015-09-22 18:54:11 +0300 | [diff] [blame] | 21 | unsigned long m, n; |
| 22 | u32 val; |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 23 | u64 ret; |
| 24 | |
| 25 | if (fd->lock) |
| 26 | spin_lock_irqsave(fd->lock, flags); |
Stephen Boyd | 661e218 | 2015-07-24 12:21:12 -0700 | [diff] [blame] | 27 | else |
| 28 | __acquire(fd->lock); |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 29 | |
| 30 | val = clk_readl(fd->reg); |
| 31 | |
| 32 | if (fd->lock) |
| 33 | spin_unlock_irqrestore(fd->lock, flags); |
Stephen Boyd | 661e218 | 2015-07-24 12:21:12 -0700 | [diff] [blame] | 34 | else |
| 35 | __release(fd->lock); |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 36 | |
| 37 | m = (val & fd->mmask) >> fd->mshift; |
| 38 | n = (val & fd->nmask) >> fd->nshift; |
| 39 | |
A.s. Dong | e983da2 | 2018-11-14 13:01:39 +0000 | [diff] [blame] | 40 | if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) { |
| 41 | m++; |
| 42 | n++; |
| 43 | } |
| 44 | |
Heikki Krogerus | 6b54783 | 2015-02-02 15:37:04 +0200 | [diff] [blame] | 45 | if (!n || !m) |
| 46 | return parent_rate; |
| 47 | |
Heiko Stübner | feaefa0ea | 2014-08-28 12:46:10 +0200 | [diff] [blame] | 48 | ret = (u64)parent_rate * m; |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 49 | do_div(ret, n); |
| 50 | |
| 51 | return ret; |
| 52 | } |
| 53 | |
Elaine Zhang | ec52e46 | 2017-08-01 18:21:22 +0200 | [diff] [blame] | 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) |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 57 | { |
| 58 | struct clk_fractional_divider *fd = to_clk_fd(hw); |
Andy Shevchenko | 0777591 | 2015-09-22 18:54:11 +0300 | [diff] [blame] | 59 | unsigned long scale; |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 60 | |
Andy Shevchenko | 0777591 | 2015-09-22 18:54:11 +0300 | [diff] [blame] | 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; |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 69 | |
Andy Shevchenko | 0777591 | 2015-09-22 18:54:11 +0300 | [diff] [blame] | 70 | rational_best_approximation(rate, *parent_rate, |
| 71 | GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0), |
Elaine Zhang | ec52e46 | 2017-08-01 18:21:22 +0200 | [diff] [blame] | 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 || 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); |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 89 | |
Andy Shevchenko | 0777591 | 2015-09-22 18:54:11 +0300 | [diff] [blame] | 90 | ret = (u64)*parent_rate * m; |
| 91 | do_div(ret, n); |
| 92 | |
| 93 | return ret; |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 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; |
Andy Shevchenko | 0777591 | 2015-09-22 18:54:11 +0300 | [diff] [blame] | 101 | unsigned long m, n; |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 102 | u32 val; |
| 103 | |
Andy Shevchenko | 0777591 | 2015-09-22 18:54:11 +0300 | [diff] [blame] | 104 | rational_best_approximation(rate, parent_rate, |
| 105 | GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0), |
| 106 | &m, &n); |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 107 | |
A.s. Dong | e983da2 | 2018-11-14 13:01:39 +0000 | [diff] [blame] | 108 | if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) { |
| 109 | m--; |
| 110 | n--; |
| 111 | } |
| 112 | |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 113 | if (fd->lock) |
| 114 | spin_lock_irqsave(fd->lock, flags); |
Stephen Boyd | 661e218 | 2015-07-24 12:21:12 -0700 | [diff] [blame] | 115 | else |
| 116 | __acquire(fd->lock); |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 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); |
Stephen Boyd | 661e218 | 2015-07-24 12:21:12 -0700 | [diff] [blame] | 125 | else |
| 126 | __release(fd->lock); |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 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 | |
Stephen Boyd | 39b44cff | 2016-02-07 00:15:09 -0800 | [diff] [blame] | 138 | struct clk_hw *clk_hw_register_fractional_divider(struct device *dev, |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 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; |
Stephen Boyd | 39b44cff | 2016-02-07 00:15:09 -0800 | [diff] [blame] | 145 | struct clk_hw *hw; |
| 146 | int ret; |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 147 | |
| 148 | fd = kzalloc(sizeof(*fd), GFP_KERNEL); |
Stephen Boyd | d122db7 | 2015-05-14 16:47:10 -0700 | [diff] [blame] | 149 | if (!fd) |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 150 | return ERR_PTR(-ENOMEM); |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 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; |
Andy Shevchenko | 934e253 | 2015-09-22 18:54:09 +0300 | [diff] [blame] | 160 | fd->mwidth = mwidth; |
| 161 | fd->mmask = GENMASK(mwidth - 1, 0) << mshift; |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 162 | fd->nshift = nshift; |
Andy Shevchenko | 934e253 | 2015-09-22 18:54:09 +0300 | [diff] [blame] | 163 | fd->nwidth = nwidth; |
| 164 | fd->nmask = GENMASK(nwidth - 1, 0) << nshift; |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 165 | fd->flags = clk_divider_flags; |
| 166 | fd->lock = lock; |
| 167 | fd->hw.init = &init; |
| 168 | |
Stephen Boyd | 39b44cff | 2016-02-07 00:15:09 -0800 | [diff] [blame] | 169 | hw = &fd->hw; |
| 170 | ret = clk_hw_register(dev, hw); |
| 171 | if (ret) { |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 172 | kfree(fd); |
Stephen Boyd | 39b44cff | 2016-02-07 00:15:09 -0800 | [diff] [blame] | 173 | hw = ERR_PTR(ret); |
| 174 | } |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 175 | |
Stephen Boyd | 39b44cff | 2016-02-07 00:15:09 -0800 | [diff] [blame] | 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; |
Heikki Krogerus | e2d0e90 | 2014-05-15 16:40:25 +0300 | [diff] [blame] | 193 | } |
| 194 | EXPORT_SYMBOL_GPL(clk_register_fractional_divider); |
Stephen Boyd | 39b44cff | 2016-02-07 00:15:09 -0800 | [diff] [blame] | 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 | } |