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