drivers/clk/clk-fractional-divider.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (C) 2014 Intel Corporation
0004  *
0005  * Adjustable fractional divider clock implementation.
0006  * Uses rational best approximation algorithm.
0007  *
0008  * Output is calculated as
0009  *
0010  *  rate = (m / n) * parent_rate                (1)
0011  *
0012  * This is useful when we have a prescaler block which asks for
0013  * m (numerator) and n (denominator) values to be provided to satisfy
0014  * the (1) as much as possible.
0015  *
0016  * Since m and n have the limitation by a range, e.g.
0017  *
0018  *  n >= 1, n < N_width, where N_width = 2^nwidth       (2)
0019  *
0020  * for some cases the output may be saturated. Hence, from (1) and (2),
0021  * assuming the worst case when m = 1, the inequality
0022  *
0023  *  floor(log2(parent_rate / rate)) <= nwidth       (3)
0024  *
0025  * may be derived. Thus, in cases when
0026  *
0027  *  (parent_rate / rate) >> N_width             (4)
0028  *
0029  * we might scale up the rate by 2^scale (see the description of
0030  * CLK_FRAC_DIVIDER_POWER_OF_TWO_PS for additional information), where
0031  *
0032  *  scale = floor(log2(parent_rate / rate)) - nwidth    (5)
0033  *
0034  * and assume that the IP, that needs m and n, has also its own
0035  * prescaler, which is capable to divide by 2^scale. In this way
0036  * we get the denominator to satisfy the desired range (2) and
0037  * at the same time a much better result of m and n than simple
0038  * saturated values.
0039  */
0040
0041 #include <linux/clk-provider.h>
0042 #include <linux/io.h>
0043 #include <linux/module.h>
0044 #include <linux/device.h>
0045 #include <linux/slab.h>
0046 #include <linux/rational.h>
0047
0048 #include "clk-fractional-divider.h"
0049
0050 static inline u32 clk_fd_readl(struct clk_fractional_divider *fd)
0051 {
0052     if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
0053         return ioread32be(fd->reg);
0054
0055     return readl(fd->reg);
0056 }
0057
0058 static inline void clk_fd_writel(struct clk_fractional_divider *fd, u32 val)
0059 {
0060     if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
0061         iowrite32be(val, fd->reg);
0062     else
0063         writel(val, fd->reg);
0064 }
0065
0066 static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
0067                     unsigned long parent_rate)
0068 {
0069     struct clk_fractional_divider *fd = to_clk_fd(hw);
0070     unsigned long flags = 0;
0071     unsigned long m, n;
0072     u32 val;
0073     u64 ret;
0074
0075     if (fd->lock)
0076         spin_lock_irqsave(fd->lock, flags);
0077     else
0078         __acquire(fd->lock);
0079
0080     val = clk_fd_readl(fd);
0081
0082     if (fd->lock)
0083         spin_unlock_irqrestore(fd->lock, flags);
0084     else
0085         __release(fd->lock);
0086
0087     m = (val & fd->mmask) >> fd->mshift;
0088     n = (val & fd->nmask) >> fd->nshift;
0089
0090     if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
0091         m++;
0092         n++;
0093     }
0094
0095     if (!n || !m)
0096         return parent_rate;
0097
0098     ret = (u64)parent_rate * m;
0099     do_div(ret, n);
0100
0101     return ret;
0102 }
0103
0104 void clk_fractional_divider_general_approximation(struct clk_hw *hw,
0105                           unsigned long rate,
0106                           unsigned long *parent_rate,
0107                           unsigned long *m, unsigned long *n)
0108 {
0109     struct clk_fractional_divider *fd = to_clk_fd(hw);
0110
0111     /*
0112      * Get rate closer to *parent_rate to guarantee there is no overflow
0113      * for m and n. In the result it will be the nearest rate left shifted
0114      * by (scale - fd->nwidth) bits.
0115      *
0116      * For the detailed explanation see the top comment in this file.
0117      */
0118     if (fd->flags & CLK_FRAC_DIVIDER_POWER_OF_TWO_PS) {
0119         unsigned long scale = fls_long(*parent_rate / rate - 1);
0120
0121         if (scale > fd->nwidth)
0122             rate <<= scale - fd->nwidth;
0123     }
0124
0125     rational_best_approximation(rate, *parent_rate,
0126             GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
0127             m, n);
0128 }
0129
0130 static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
0131                   unsigned long *parent_rate)
0132 {
0133     struct clk_fractional_divider *fd = to_clk_fd(hw);
0134     unsigned long m, n;
0135     u64 ret;
0136
0137     if (!rate || (!clk_hw_can_set_rate_parent(hw) && rate >= *parent_rate))
0138         return *parent_rate;
0139
0140     if (fd->approximation)
0141         fd->approximation(hw, rate, parent_rate, &m, &n);
0142     else
0143         clk_fractional_divider_general_approximation(hw, rate, parent_rate, &m, &n);
0144
0145     ret = (u64)*parent_rate * m;
0146     do_div(ret, n);
0147
0148     return ret;
0149 }
0150
0151 static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
0152                unsigned long parent_rate)
0153 {
0154     struct clk_fractional_divider *fd = to_clk_fd(hw);
0155     unsigned long flags = 0;
0156     unsigned long m, n;
0157     u32 val;
0158
0159     rational_best_approximation(rate, parent_rate,
0160             GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
0161             &m, &n);
0162
0163     if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
0164         m--;
0165         n--;
0166     }
0167
0168     if (fd->lock)
0169         spin_lock_irqsave(fd->lock, flags);
0170     else
0171         __acquire(fd->lock);
0172
0173     val = clk_fd_readl(fd);
0174     val &= ~(fd->mmask | fd->nmask);
0175     val |= (m << fd->mshift) | (n << fd->nshift);
0176     clk_fd_writel(fd, val);
0177
0178     if (fd->lock)
0179         spin_unlock_irqrestore(fd->lock, flags);
0180     else
0181         __release(fd->lock);
0182
0183     return 0;
0184 }
0185
0186 const struct clk_ops clk_fractional_divider_ops = {
0187     .recalc_rate = clk_fd_recalc_rate,
0188     .round_rate = clk_fd_round_rate,
0189     .set_rate = clk_fd_set_rate,
0190 };
0191 EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
0192
0193 struct clk_hw *clk_hw_register_fractional_divider(struct device *dev,
0194         const char *name, const char *parent_name, unsigned long flags,
0195         void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
0196         u8 clk_divider_flags, spinlock_t *lock)
0197 {
0198     struct clk_fractional_divider *fd;
0199     struct clk_init_data init;
0200     struct clk_hw *hw;
0201     int ret;
0202
0203     fd = kzalloc(sizeof(*fd), GFP_KERNEL);
0204     if (!fd)
0205         return ERR_PTR(-ENOMEM);
0206
0207     init.name = name;
0208     init.ops = &clk_fractional_divider_ops;
0209     init.flags = flags;
0210     init.parent_names = parent_name ? &parent_name : NULL;
0211     init.num_parents = parent_name ? 1 : 0;
0212
0213     fd->reg = reg;
0214     fd->mshift = mshift;
0215     fd->mwidth = mwidth;
0216     fd->mmask = GENMASK(mwidth - 1, 0) << mshift;
0217     fd->nshift = nshift;
0218     fd->nwidth = nwidth;
0219     fd->nmask = GENMASK(nwidth - 1, 0) << nshift;
0220     fd->flags = clk_divider_flags;
0221     fd->lock = lock;
0222     fd->hw.init = &init;
0223
0224     hw = &fd->hw;
0225     ret = clk_hw_register(dev, hw);
0226     if (ret) {
0227         kfree(fd);
0228         hw = ERR_PTR(ret);
0229     }
0230
0231     return hw;
0232 }
0233 EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);
0234
0235 struct clk *clk_register_fractional_divider(struct device *dev,
0236         const char *name, const char *parent_name, unsigned long flags,
0237         void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
0238         u8 clk_divider_flags, spinlock_t *lock)
0239 {
0240     struct clk_hw *hw;
0241
0242     hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
0243             reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
0244             lock);
0245     if (IS_ERR(hw))
0246         return ERR_CAST(hw);
0247     return hw->clk;
0248 }
0249 EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
0250
0251 void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
0252 {
0253     struct clk_fractional_divider *fd;
0254
0255     fd = to_clk_fd(hw);
0256
0257     clk_hw_unregister(hw);
0258     kfree(fd);
0259 }