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  */
  11
  12 #include <linux/clk-provider.h>
  13 #include <linux/module.h>
  14 #include <linux/device.h>
  15 #include <linux/slab.h>
  16 #include <linux/gcd.h>
  17
  18 #define to_clk_fd(_hw) container_of(_hw, struct clk_fractional_divider, hw)
  19
  20 static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
  21                                         unsigned long parent_rate)
  22 {
  23         struct clk_fractional_divider *fd = to_clk_fd(hw);
  24         unsigned long flags = 0;
  25         u32 val, m, n;
  26         u64 ret;
  27
  28         if (fd->lock)
  29                 spin_lock_irqsave(fd->lock, flags);
  30
  31         val = clk_readl(fd->reg);
  32
  33         if (fd->lock)
  34                 spin_unlock_irqrestore(fd->lock, flags);
  35
  36         m = (val & fd->mmask) >> fd->mshift;
  37         n = (val & fd->nmask) >> fd->nshift;
  38
  39         ret = (u64)parent_rate * m;
  40         do_div(ret, n);
  41
  42         return ret;
  43 }
  44
  45 static long clk_fd_round_rate(struct clk_hw *hw, unsigned long rate,
  46                               unsigned long *prate)
  47 {
  48         struct clk_fractional_divider *fd = to_clk_fd(hw);
  49         unsigned maxn = (fd->nmask >> fd->nshift) + 1;
  50         unsigned div;
  51
  52         if (!rate || rate >= *prate)
  53                 return *prate;
  54
  55         div = gcd(*prate, rate);
  56
  57         while ((*prate / div) > maxn) {
  58                 div <<= 1;
  59                 rate <<= 1;
  60         }
  61
  62         return rate;
  63 }
  64
  65 static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
  66                            unsigned long parent_rate)
  67 {
  68         struct clk_fractional_divider *fd = to_clk_fd(hw);
  69         unsigned long flags = 0;
  70         unsigned long div;
  71         unsigned n, m;
  72         u32 val;
  73
  74         div = gcd(parent_rate, rate);
  75         m = rate / div;
  76         n = parent_rate / div;
  77
  78         if (fd->lock)
  79                 spin_lock_irqsave(fd->lock, flags);
  80
  81         val = clk_readl(fd->reg);
  82         val &= ~(fd->mmask | fd->nmask);
  83         val |= (m << fd->mshift) | (n << fd->nshift);
  84         clk_writel(val, fd->reg);
  85
  86         if (fd->lock)
  87                 spin_unlock_irqrestore(fd->lock, flags);
  88
  89         return 0;
  90 }
  91
  92 const struct clk_ops clk_fractional_divider_ops = {
  93         .recalc_rate = clk_fd_recalc_rate,
  94         .round_rate = clk_fd_round_rate,
  95         .set_rate = clk_fd_set_rate,
  96 };
  97 EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);
  98
  99 struct clk *clk_register_fractional_divider(struct device *dev,
 100                 const char *name, const char *parent_name, unsigned long flags,
 101                 void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
 102                 u8 clk_divider_flags, spinlock_t *lock)
 103 {
 104         struct clk_fractional_divider *fd;
 105         struct clk_init_data init;
 106         struct clk *clk;
 107
 108         fd = kzalloc(sizeof(*fd), GFP_KERNEL);
 109         if (!fd) {
 110                 dev_err(dev, "could not allocate fractional divider clk\n");
 111                 return ERR_PTR(-ENOMEM);
 112         }
 113
 114         init.name = name;
 115         init.ops = &clk_fractional_divider_ops;
 116         init.flags = flags | CLK_IS_BASIC;
 117         init.parent_names = parent_name ? &parent_name : NULL;
 118         init.num_parents = parent_name ? 1 : 0;
 119
 120         fd->reg = reg;
 121         fd->mshift = mshift;
 122         fd->mmask = (BIT(mwidth) - 1) << mshift;
 123         fd->nshift = nshift;
 124         fd->nmask = (BIT(nwidth) - 1) << nshift;
 125         fd->flags = clk_divider_flags;
 126         fd->lock = lock;
 127         fd->hw.init = &init;
 128
 129         clk = clk_register(dev, &fd->hw);
 130         if (IS_ERR(clk))
 131                 kfree(fd);
 132
 133         return clk;
 134 }
 135 EXPORT_SYMBOL_GPL(clk_register_fractional_divider);