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