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