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  * Uses rational best approximation algorithm.
   7  *
   8  * Output is calculated as
   9  *
  10  *      rate = (m / n) * parent_rate                            (1)
  11  *
  12  * This is useful when we have a prescaler block which asks for
  13  * m (numerator) and n (denominator) values to be provided to satisfy
  14  * the (1) as much as possible.
  15  *
  16  * Since m and n have the limitation by a range, e.g.
  17  *
  18  *      n >= 1, n < N_width, where N_width = 2^nwidth           (2)
  19  *
  20  * for some cases the output may be saturated. Hence, from (1) and (2),
  21  * assuming the worst case when m = 1, the inequality
  22  *
  23  *      floor(log2(parent_rate / rate)) <= nwidth               (3)
  24  *
  25  * may be derived. Thus, in cases when
  26  *
  27  *      (parent_rate / rate) >> N_width                         (4)
  28  *
  29  * we might scale up the rate by 2^scale (see the description of
  30  * CLK_FRAC_DIVIDER_POWER_OF_TWO_PS for additional information), where
  31  *
  32  *      scale = floor(log2(parent_rate / rate)) - nwidth        (5)
  33  *
  34  * and assume that the IP, that needs m and n, has also its own
  35  * prescaler, which is capable to divide by 2^scale. In this way
  36  * we get the denominator to satisfy the desired range (2) and
  37  * at the same time a much better result of m and n than simple
  38  * saturated values.
  39  */
  40
  41 #include <linux/debugfs.h>
  42 #include <linux/device.h>
  43 #include <linux/io.h>
  44 #include <linux/math.h>
  45 #include <linux/module.h>
  46 #include <linux/rational.h>
  47 #include <linux/slab.h>
  48
  49 #include <linux/clk-provider.h>
  50
  51 #include "clk-fractional-divider.h"
  52
  53 static inline u32 clk_fd_readl(struct clk_fractional_divider *fd)
  54 {
  55         if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
  56                 return ioread32be(fd->reg);
  57
  58         return readl(fd->reg);
  59 }
  60
  61 static inline void clk_fd_writel(struct clk_fractional_divider *fd, u32 val)
  62 {
  63         if (fd->flags & CLK_FRAC_DIVIDER_BIG_ENDIAN)
  64                 iowrite32be(val, fd->reg);
  65         else
  66                 writel(val, fd->reg);
  67 }
  68
  69 static void clk_fd_get_div(struct clk_hw *hw, struct u32_fract *fract)
  70 {
  71         struct clk_fractional_divider *fd = to_clk_fd(hw);
  72         unsigned long flags = 0;
  73         unsigned long m, n;
  74         u32 val;
  75
  76         if (fd->lock)
  77                 spin_lock_irqsave(fd->lock, flags);
  78         else
  79                 __acquire(fd->lock);
  80
  81         val = clk_fd_readl(fd);
  82
  83         if (fd->lock)
  84                 spin_unlock_irqrestore(fd->lock, flags);
  85         else
  86                 __release(fd->lock);
  87
  88         m = (val & fd->mmask) >> fd->mshift;
  89         n = (val & fd->nmask) >> fd->nshift;
  90
  91         if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
  92                 m++;
  93                 n++;
  94         }
  95
  96         fract->numerator = m;
  97         fract->denominator = n;
  98 }
  99
 100 static unsigned long clk_fd_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 101 {
 102         struct u32_fract fract;
 103         u64 ret;
 104
 105         clk_fd_get_div(hw, &fract);
 106
 107         if (!fract.numerator || !fract.denominator)
 108                 return parent_rate;
 109
 110         ret = (u64)parent_rate * fract.numerator;
 111         do_div(ret, fract.denominator);
 112
 113         return ret;
 114 }
 115
 116 void clk_fractional_divider_general_approximation(struct clk_hw *hw,
 117                                                   unsigned long rate,
 118                                                   unsigned long *parent_rate,
 119                                                   unsigned long *m, unsigned long *n)
 120 {
 121         struct clk_fractional_divider *fd = to_clk_fd(hw);
 122
 123         /*
 124          * Get rate closer to *parent_rate to guarantee there is no overflow
 125          * for m and n. In the result it will be the nearest rate left shifted
 126          * by (scale - fd->nwidth) bits.
 127          *
 128          * For the detailed explanation see the top comment in this file.
 129          */
 130         if (fd->flags & CLK_FRAC_DIVIDER_POWER_OF_TWO_PS) {
 131                 unsigned long scale = fls_long(*parent_rate / rate - 1);
 132
 133                 if (scale > fd->nwidth)
 134                         rate <<= scale - fd->nwidth;
 135         }
 136
 137         rational_best_approximation(rate, *parent_rate,
 138                         GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
 139                         m, n);
 140 }
 141
 142 static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
 143                               unsigned long *parent_rate)
 144 {
 145         struct clk_fractional_divider *fd = to_clk_fd(hw);
 146         unsigned long m, n;
 147         u64 ret;
 148
 149         if (!rate || (!clk_hw_can_set_rate_parent(hw) && rate >= *parent_rate))
 150                 return *parent_rate;
 151
 152         if (fd->approximation)
 153                 fd->approximation(hw, rate, parent_rate, &m, &n);
 154         else
 155                 clk_fractional_divider_general_approximation(hw, rate, parent_rate, &m, &n);
 156
 157         ret = (u64)*parent_rate * m;
 158         do_div(ret, n);
 159
 160         return ret;
 161 }
 162
 163 static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
 164                            unsigned long parent_rate)
 165 {
 166         struct clk_fractional_divider *fd = to_clk_fd(hw);
 167         unsigned long flags = 0;
 168         unsigned long m, n;
 169         u32 val;
 170
 171         rational_best_approximation(rate, parent_rate,
 172                         GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
 173                         &m, &n);
 174
 175         if (fd->flags & CLK_FRAC_DIVIDER_ZERO_BASED) {
 176                 m--;
 177                 n--;
 178         }
 179
 180         if (fd->lock)
 181                 spin_lock_irqsave(fd->lock, flags);
 182         else
 183                 __acquire(fd->lock);
 184
 185         val = clk_fd_readl(fd);
 186         val &= ~(fd->mmask | fd->nmask);
 187         val |= (m << fd->mshift) | (n << fd->nshift);
 188         clk_fd_writel(fd, val);
 189
 190         if (fd->lock)
 191                 spin_unlock_irqrestore(fd->lock, flags);
 192         else
 193                 __release(fd->lock);
 194
 195         return 0;
 196 }
 197
 198 #ifdef CONFIG_DEBUG_FS
 199 static int clk_fd_numerator_get(void *hw, u64 *val)
 200 {
 201         struct u32_fract fract;
 202
 203         clk_fd_get_div(hw, &fract);
 204
 205         *val = fract.numerator;
 206
 207         return 0;
 208 }
 209 DEFINE_DEBUGFS_ATTRIBUTE(clk_fd_numerator_fops, clk_fd_numerator_get, NULL, "%llu\n");
 210
 211 static int clk_fd_denominator_get(void *hw, u64 *val)
 212 {
 213         struct u32_fract fract;
 214
 215         clk_fd_get_div(hw, &fract);
 216
 217         *val = fract.denominator;
 218
 219         return 0;
 220 }
 221 DEFINE_DEBUGFS_ATTRIBUTE(clk_fd_denominator_fops, clk_fd_denominator_get, NULL, "%llu\n");
 222
 223 static void clk_fd_debug_init(struct clk_hw *hw, struct dentry *dentry)
 224 {
 225         debugfs_create_file("numerator", 0444, dentry, hw, &clk_fd_numerator_fops);
 226         debugfs_create_file("denominator", 0444, dentry, hw, &clk_fd_denominator_fops);
 227 }
 228 #endif
 229
 230 const struct clk_ops clk_fractional_divider_ops = {
 231         .recalc_rate = clk_fd_recalc_rate,
 232         .round_rate = clk_fd_round_rate,
 233         .set_rate = clk_fd_set_rate,
 234 #ifdef CONFIG_DEBUG_FS
 235         .debug_init = clk_fd_debug_init,
 236 #endif
 237 };
 238 EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
 239
 240 struct clk_hw *clk_hw_register_fractional_divider(struct device *dev,
 241                 const char *name, const char *parent_name, unsigned long flags,
 242                 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
 243                 u8 clk_divider_flags, spinlock_t *lock)
 244 {
 245         struct clk_fractional_divider *fd;
 246         struct clk_init_data init;
 247         struct clk_hw *hw;
 248         int ret;
 249
 250         fd = kzalloc(sizeof(*fd), GFP_KERNEL);
 251         if (!fd)
 252                 return ERR_PTR(-ENOMEM);
 253
 254         init.name = name;
 255         init.ops = &clk_fractional_divider_ops;
 256         init.flags = flags;
 257         init.parent_names = parent_name ? &parent_name : NULL;
 258         init.num_parents = parent_name ? 1 : 0;
 259
 260         fd->reg = reg;
 261         fd->mshift = mshift;
 262         fd->mwidth = mwidth;
 263         fd->mmask = GENMASK(mwidth - 1, 0) << mshift;
 264         fd->nshift = nshift;
 265         fd->nwidth = nwidth;
 266         fd->nmask = GENMASK(nwidth - 1, 0) << nshift;
 267         fd->flags = clk_divider_flags;
 268         fd->lock = lock;
 269         fd->hw.init = &init;
 270
 271         hw = &fd->hw;
 272         ret = clk_hw_register(dev, hw);
 273         if (ret) {
 274                 kfree(fd);
 275                 hw = ERR_PTR(ret);
 276         }
 277
 278         return hw;
 279 }
 280 EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);
 281
 282 struct clk *clk_register_fractional_divider(struct device *dev,
 283                 const char *name, const char *parent_name, unsigned long flags,
 284                 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
 285                 u8 clk_divider_flags, spinlock_t *lock)
 286 {
 287         struct clk_hw *hw;
 288
 289         hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
 290                         reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
 291                         lock);
 292         if (IS_ERR(hw))
 293                 return ERR_CAST(hw);
 294         return hw->clk;
 295 }
 296 EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
 297
 298 void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
 299 {
 300         struct clk_fractional_divider *fd;
 301
 302         fd = to_clk_fd(hw);
 303
 304         clk_hw_unregister(hw);
 305         kfree(fd);
 306 }