clk-fractional-divider.c 4.33 KB
Newer Older
1 2 3 4 5 6 7 8 9
/*
 * Copyright (C) 2014 Intel Corporation
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * Adjustable fractional divider clock implementation.
 * Output rate = (m / n) * parent_rate.
10
 * Uses rational best approximation algorithm.
11 12 13 14 15 16
 */

#include <linux/clk-provider.h>
#include <linux/module.h>
#include <linux/device.h>
#include <linux/slab.h>
17
#include <linux/rational.h>
18 19 20 21 22 23

static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
					unsigned long parent_rate)
{
	struct clk_fractional_divider *fd = to_clk_fd(hw);
	unsigned long flags = 0;
24 25
	unsigned long m, n;
	u32 val;
26 27 28 29
	u64 ret;

	if (fd->lock)
		spin_lock_irqsave(fd->lock, flags);
30 31
	else
		__acquire(fd->lock);
32 33 34 35 36

	val = clk_readl(fd->reg);

	if (fd->lock)
		spin_unlock_irqrestore(fd->lock, flags);
37 38
	else
		__release(fd->lock);
39 40 41 42

	m = (val & fd->mmask) >> fd->mshift;
	n = (val & fd->nmask) >> fd->nshift;

43 44 45
	if (!n || !m)
		return parent_rate;

46
	ret = (u64)parent_rate * m;
47 48 49 50 51 52
	do_div(ret, n);

	return ret;
}

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 57 58
	unsigned long scale;
	unsigned long m, n;
	u64 ret;
59

60 61
	if (!rate || rate >= *parent_rate)
		return *parent_rate;
62

63 64 65 66 67 68 69 70
	/*
	 * Get rate closer to *parent_rate to guarantee there is no overflow
	 * for m and n. In the result it will be the nearest rate left shifted
	 * by (scale - fd->nwidth) bits.
	 */
	scale = fls_long(*parent_rate / rate - 1);
	if (scale > fd->nwidth)
		rate <<= scale - fd->nwidth;
71

72 73 74
	rational_best_approximation(rate, *parent_rate,
			GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
			&m, &n);
75

76 77 78 79
	ret = (u64)*parent_rate * m;
	do_div(ret, n);

	return ret;
80 81 82 83 84 85 86
}

static int clk_fd_set_rate(struct clk_hw *hw, unsigned long rate,
			   unsigned long parent_rate)
{
	struct clk_fractional_divider *fd = to_clk_fd(hw);
	unsigned long flags = 0;
87
	unsigned long m, n;
88 89
	u32 val;

90 91 92
	rational_best_approximation(rate, parent_rate,
			GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
			&m, &n);
93 94 95

	if (fd->lock)
		spin_lock_irqsave(fd->lock, flags);
96 97
	else
		__acquire(fd->lock);
98 99 100 101 102 103 104 105

	val = clk_readl(fd->reg);
	val &= ~(fd->mmask | fd->nmask);
	val |= (m << fd->mshift) | (n << fd->nshift);
	clk_writel(val, fd->reg);

	if (fd->lock)
		spin_unlock_irqrestore(fd->lock, flags);
106 107
	else
		__release(fd->lock);
108 109 110 111 112 113 114 115 116 117 118

	return 0;
}

const struct clk_ops clk_fractional_divider_ops = {
	.recalc_rate = clk_fd_recalc_rate,
	.round_rate = clk_fd_round_rate,
	.set_rate = clk_fd_set_rate,
};
EXPORT_SYMBOL_GPL(clk_fractional_divider_ops);

119
struct clk_hw *clk_hw_register_fractional_divider(struct device *dev,
120 121 122 123 124 125
		const char *name, const char *parent_name, unsigned long flags,
		void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
		u8 clk_divider_flags, spinlock_t *lock)
{
	struct clk_fractional_divider *fd;
	struct clk_init_data init;
126 127
	struct clk_hw *hw;
	int ret;
128 129

	fd = kzalloc(sizeof(*fd), GFP_KERNEL);
130
	if (!fd)
131 132 133 134 135 136 137 138 139 140
		return ERR_PTR(-ENOMEM);

	init.name = name;
	init.ops = &clk_fractional_divider_ops;
	init.flags = flags | CLK_IS_BASIC;
	init.parent_names = parent_name ? &parent_name : NULL;
	init.num_parents = parent_name ? 1 : 0;

	fd->reg = reg;
	fd->mshift = mshift;
141 142
	fd->mwidth = mwidth;
	fd->mmask = GENMASK(mwidth - 1, 0) << mshift;
143
	fd->nshift = nshift;
144 145
	fd->nwidth = nwidth;
	fd->nmask = GENMASK(nwidth - 1, 0) << nshift;
146 147 148 149
	fd->flags = clk_divider_flags;
	fd->lock = lock;
	fd->hw.init = &init;

150 151 152
	hw = &fd->hw;
	ret = clk_hw_register(dev, hw);
	if (ret) {
153
		kfree(fd);
154 155 156 157 158 159
		hw = ERR_PTR(ret);
	}

	return hw;
}
EXPORT_SYMBOL_GPL(clk_hw_register_fractional_divider);
160

161 162 163 164 165 166 167 168 169 170 171 172 173
struct clk *clk_register_fractional_divider(struct device *dev,
		const char *name, const char *parent_name, unsigned long flags,
		void __iomem *reg, u8 mshift, u8 mwidth, u8 nshift, u8 nwidth,
		u8 clk_divider_flags, spinlock_t *lock)
{
	struct clk_hw *hw;

	hw = clk_hw_register_fractional_divider(dev, name, parent_name, flags,
			reg, mshift, mwidth, nshift, nwidth, clk_divider_flags,
			lock);
	if (IS_ERR(hw))
		return ERR_CAST(hw);
	return hw->clk;
174 175
}
EXPORT_SYMBOL_GPL(clk_register_fractional_divider);
176 177 178 179 180 181 182 183 184 185

void clk_hw_unregister_fractional_divider(struct clk_hw *hw)
{
	struct clk_fractional_divider *fd;

	fd = to_clk_fd(hw);

	clk_hw_unregister(hw);
	kfree(fd);
}