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 #define to_clk_fd(_hw) container_of(_hw, struct clk_fractional_divider, hw)
  20
  21 static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
  22                                         unsigned long parent_rate)
  23 {
  24         struct clk_fractional_divider *fd = to_clk_fd(hw);
  25         unsigned long flags = 0;
  26         unsigned long m, n;
  27         u32 val;
  28         u64 ret;
  29
  30         if (fd->lock)
  31                 spin_lock_irqsave(fd->lock, flags);
  32         else
  33                 __acquire(fd->lock);
  34
  35         val = clk_readl(fd->reg);
  36
  37         if (fd->lock)
  38                 spin_unlock_irqrestore(fd->lock, flags);
  39         else
  40                 __release(fd->lock);
  41
  42         m = (val & fd->mmask) >> fd->mshift;
  43         n = (val & fd->nmask) >> fd->nshift;
  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 long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
  55                               unsigned long *parent_rate)
  56 {
  57         struct clk_fractional_divider *fd = to_clk_fd(hw);
  58         unsigned long scale;
  59         unsigned long m, n;
  60         u64 ret;
  61
  62         if (!rate || rate >= *parent_rate)
  63                 return *parent_rate;
  64
  65         /*
  66          * Get rate closer to *parent_rate to guarantee there is no overflow
  67          * for m and n. In the result it will be the nearest rate left shifted
  68          * by (scale - fd->nwidth) bits.
  69          */
  70         scale = fls_long(*parent_rate / rate - 1);
  71         if (scale > fd->nwidth)
  72                 rate <<= scale - fd->nwidth;
  73
  74         rational_best_approximation(rate, *parent_rate,
  75                         GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
  76                         &m, &n);
  77
  78         ret = (u64)*parent_rate * m;
  79         do_div(ret, n);
  80
  81         return ret;
  82 }
  83
  84 static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
  85                            unsigned long parent_rate)
  86 {
  87         struct clk_fractional_divider *fd = to_clk_fd(hw);
  88         unsigned long flags = 0;
  89         unsigned long m, n;
  90         u32 val;
  91
  92         rational_best_approximation(rate, parent_rate,
  93                         GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
  94                         &m, &n);
  95
  96         if (fd->lock)
  97                 spin_lock_irqsave(fd->lock, flags);
  98         else
  99                 __acquire(fd->lock);
 100
 101         val = clk_readl(fd->reg);
 102         val &= ~(fd->mmask | fd->nmask);
 103         val |= (m << fd->mshift) | (n << fd->nshift);
 104         clk_writel(val, fd->reg);
 105
 106         if (fd->lock)
 107                 spin_unlock_irqrestore(fd->lock, flags);
 108         else
 109                 __release(fd->lock);
 110
 111         return 0;
 112 }
 113
 114 const struct clk_ops clk_fractional_divider_ops = {
 115         .recalc_rate = clk_fd_recalc_rate,
 116         .round_rate = clk_fd_round_rate,
 117         .set_rate = clk_fd_set_rate,
 118 };
 119 EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
 120
 121 struct clk *clk_register_fractional_divider(struct device *dev,
 122                 const char *name, const char *parent_name, unsigned long flags,
 123                 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
 124                 u8 clk_divider_flags, spinlock_t *lock)
 125 {
 126         struct clk_fractional_divider *fd;
 127         struct clk_init_data init;
 128         struct clk *clk;
 129
 130         fd = kzalloc(sizeof(*fd), GFP_KERNEL);
 131         if (!fd)
 132                 return ERR_PTR(-ENOMEM);
 133
 134         init.name = name;
 135         init.ops = &clk_fractional_divider_ops;
 136         init.flags = flags | CLK_IS_BASIC;
 137         init.parent_names = parent_name ? &parent_name : NULL;
 138         init.num_parents = parent_name ? 1 : 0;
 139
 140         fd->reg = reg;
 141         fd->mshift = mshift;
 142         fd->mwidth = mwidth;
 143         fd->mmask = GENMASK(mwidth - 1, 0) << mshift;
 144         fd->nshift = nshift;
 145         fd->nwidth = nwidth;
 146         fd->nmask = GENMASK(nwidth - 1, 0) << nshift;
 147         fd->flags = clk_divider_flags;
 148         fd->lock = lock;
 149         fd->hw.init = &init;
 150
 151         clk = clk_register(dev, &fd->hw);
 152         if (IS_ERR(clk))
 153                 kfree(fd);
 154
 155         return clk;
 156 }
 157 EXPORT_SYMBOL_GPL(clk_register_fractional_divider);