drivers/clk/clk-fractional-divider.c

   1 /*
   2  * Copyright (C) 2014 Intel Corporation
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License version 2 as
   6  * published by the Free Software Foundation.
   7  *
   8  * Adjustable fractional divider clock implementation.
   9  * Output rate = (m / n) * parent_rate.
  10  * Uses rational best approximation algorithm.
  11  */
  12
  13 #include <linux/clk-provider.h>
  14 #include <linux/module.h>
  15 #include <linux/device.h>
  16 #include <linux/slab.h>
  17 #include <linux/rational.h>
  18
  19 static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
  20                                         unsigned long parent_rate)
  21 {
  22         struct clk_fractional_divider *fd = to_clk_fd(hw);
  23         unsigned long flags = 0;
  24         unsigned long m, n;
  25         u32 val;
  26         u64 ret;
  27
  28         if (fd->lock)
  29                 spin_lock_irqsave(fd->lock, flags);
  30         else
  31                 __acquire(fd->lock);
  32
  33         val = clk_readl(fd->reg);
  34
  35         if (fd->lock)
  36                 spin_unlock_irqrestore(fd->lock, flags);
  37         else
  38                 __release(fd->lock);
  39
  40         m = (val & fd->mmask) >> fd->mshift;
  41         n = (val & fd->nmask) >> fd->nshift;
  42
  43         if (!n || !m)
  44                 return parent_rate;
  45
  46         ret = (u64)parent_rate * m;
  47         do_div(ret, n);
  48
  49         return ret;
  50 }
  51
  52 static void clk_fd_general_approximation(struct clk_hw *hw, unsigned long rate,
  53                                          unsigned long *parent_rate,
  54                                          unsigned long *m, unsigned long *n)
  55 {
  56         struct clk_fractional_divider *fd = to_clk_fd(hw);
  57         unsigned long scale;
  58
  59         /*
  60          * Get rate closer to *parent_rate to guarantee there is no overflow
  61          * for m and n. In the result it will be the nearest rate left shifted
  62          * by (scale - fd->nwidth) bits.
  63          */
  64         scale = fls_long(*parent_rate / rate - 1);
  65         if (scale > fd->nwidth)
  66                 rate <<= scale - fd->nwidth;
  67
  68         rational_best_approximation(rate, *parent_rate,
  69                         GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
  70                         m, n);
  71 }
  72
  73 static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
  74                               unsigned long *parent_rate)
  75 {
  76         struct clk_fractional_divider *fd = to_clk_fd(hw);
  77         unsigned long m, n;
  78         u64 ret;
  79
  80         if (!rate || rate >= *parent_rate)
  81                 return *parent_rate;
  82
  83         if (fd->approximation)
  84                 fd->approximation(hw, rate, parent_rate, &m, &n);
  85         else
  86                 clk_fd_general_approximation(hw, rate, parent_rate, &m, &n);
  87
  88         ret = (u64)*parent_rate * m;
  89         do_div(ret, n);
  90
  91         return ret;
  92 }
  93
  94 static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
  95                            unsigned long parent_rate)
  96 {
  97         struct clk_fractional_divider *fd = to_clk_fd(hw);
  98         unsigned long flags = 0;
  99         unsigned long m, n;
 100         u32 val;
 101
 102         rational_best_approximation(rate, parent_rate,
 103                         GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
 104                         &m, &n);
 105
 106         if (fd->lock)
 107                 spin_lock_irqsave(fd->lock, flags);
 108         else
 109                 __acquire(fd->lock);
 110
 111         val = clk_readl(fd->reg);
 112         val &= ~(fd->mmask | fd->nmask);
 113         val |= (m << fd->mshift) | (n << fd->nshift);
 114         clk_writel(val, fd->reg);
 115
 116         if (fd->lock)
 117                 spin_unlock_irqrestore(fd->lock, flags);
 118         else
 119                 __release(fd->lock);
 120
 121         return 0;
 122 }
 123
 124 const struct clk_ops clk_fractional_divider_ops = {
 125         .recalc_rate = clk_fd_recalc_rate,
 126         .round_rate = clk_fd_round_rate,
 127         .set_rate = clk_fd_set_rate,
 128 };
 129 EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
 130
 131 struct clk_hw *clk_hw_register_fractional_divider(struct device *dev,
 132                 const char *name, const char *parent_name, unsigned long flags,
 133                 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
 134                 u8 clk_divider_flags, spinlock_t *lock)
 135 {
 136         struct clk_fractional_divider *fd;
 137         struct clk_init_data init;
 138         struct clk_hw *hw;
 139         int ret;
 140
 141         fd = kzalloc(sizeof(*fd), GFP_KERNEL);
 142         if (!fd)
 143                 return ERR_PTR(-ENOMEM);
 144
 145         init.name = name;
 146         init.ops = &clk_fractional_divider_ops;
 147         init.flags = flags | CLK_IS_BASIC;
 148         init.parent_names = parent_name ? &parent_name : NULL;
 149         init.num_parents = parent_name ? 1 : 0;
 150
 151         fd->reg = reg;
 152         fd->mshift = mshift;
 153         fd->mwidth = mwidth;
 154         fd->mmask = GENMASK(mwidth - 1, 0) << mshift;
 155         fd->nshift = nshift;
 156         fd->nwidth = nwidth;
 157         fd->nmask = GENMASK(nwidth - 1, 0) << nshift;
 158         fd->flags = clk_divider_flags;
 159         fd->lock = lock;
 160         fd->hw.init = &init;
 161
 162         hw = &fd->hw;
 163         ret = clk_hw_register(dev, hw);
 164         if (ret) {
 165                 kfree(fd);
 166                 hw = ERR_PTR(ret);
 167         }
 168
 169         return hw;
 170 }
 171 EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);
 172
 173 struct clk *clk_register_fractional_divider(struct device *dev,
 174                 const char *name, const char *parent_name, unsigned long flags,
 175                 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
 176                 u8 clk_divider_flags, spinlock_t *lock)
 177 {
 178         struct clk_hw *hw;
 179
 180         hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
 181                         reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
 182                         lock);
 183         if (IS_ERR(hw))
 184                 return ERR_CAST(hw);
 185         return hw->clk;
 186 }
 187 EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
 188
 189 void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
 190 {
 191         struct clk_fractional_divider *fd;
 192
 193         fd = to_clk_fd(hw);
 194
 195         clk_hw_unregister(hw);
 196         kfree(fd);
 197 }