drivers/clk/clk-fractional-divider.c

   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 }