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/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 }