1/*
  2 * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>
  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
  9#include <linux/bitops.h>
 10#include <linux/clk-provider.h>
 11#include <linux/err.h>
 12#include <linux/export.h>
 13#include <linux/kernel.h>
 14#include <linux/of.h>
 15#include <linux/slab.h>
 16
 17static unsigned long __get_mult(struct clk_multiplier *mult,
 18				unsigned long rate,
 19				unsigned long parent_rate)
 20{
 21	if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
 22		return DIV_ROUND_CLOSEST(rate, parent_rate);
 23
 24	return rate / parent_rate;
 25}
 26
 27static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
 28						unsigned long parent_rate)
 29{
 30	struct clk_multiplier *mult = to_clk_multiplier(hw);
 31	unsigned long val;
 32
 33	val = clk_readl(mult->reg) >> mult->shift;
 34	val &= GENMASK(mult->width - 1, 0);
 35
 36	if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
 37		val = 1;
 38
 39	return parent_rate * val;
 40}
 41
 42static bool __is_best_rate(unsigned long rate, unsigned long new,
 43			   unsigned long best, unsigned long flags)
 44{
 45	if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
 46		return abs(rate - new) < abs(rate - best);
 47
 48	return new >= rate && new < best;
 49}
 50
 51static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
 52				unsigned long *best_parent_rate,
 53				u8 width, unsigned long flags)
 54{
 55	struct clk_multiplier *mult = to_clk_multiplier(hw);
 56	unsigned long orig_parent_rate = *best_parent_rate;
 57	unsigned long parent_rate, current_rate, best_rate = ~0;
 58	unsigned int i, bestmult = 0;
 59	unsigned int maxmult = (1 << width) - 1;
 60
 61	if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
 62		bestmult = rate / orig_parent_rate;
 63
 64		/* Make sure we don't end up with a 0 multiplier */
 65		if ((bestmult == 0) &&
 66		    !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
 67			bestmult = 1;
 68
 69		/* Make sure we don't overflow the multiplier */
 70		if (bestmult > maxmult)
 71			bestmult = maxmult;
 72
 73		return bestmult;
 74	}
 75
 76	for (i = 1; i < maxmult; i++) {
 77		if (rate == orig_parent_rate * i) {
 78			/*
 79			 * This is the best case for us if we have a
 80			 * perfect match without changing the parent
 81			 * rate.
 82			 */
 83			*best_parent_rate = orig_parent_rate;
 84			return i;
 85		}
 86
 87		parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
 88						rate / i);
 89		current_rate = parent_rate * i;
 90
 91		if (__is_best_rate(rate, current_rate, best_rate, flags)) {
 92			bestmult = i;
 93			best_rate = current_rate;
 94			*best_parent_rate = parent_rate;
 95		}
 96	}
 97
 98	return bestmult;
 99}
100
101static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
102				  unsigned long *parent_rate)
103{
104	struct clk_multiplier *mult = to_clk_multiplier(hw);
105	unsigned long factor = __bestmult(hw, rate, parent_rate,
106					  mult->width, mult->flags);
107
108	return *parent_rate * factor;
109}
110
111static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
112			       unsigned long parent_rate)
113{
114	struct clk_multiplier *mult = to_clk_multiplier(hw);
115	unsigned long factor = __get_mult(mult, rate, parent_rate);
116	unsigned long flags = 0;
117	unsigned long val;
118
119	if (mult->lock)
120		spin_lock_irqsave(mult->lock, flags);
121	else
122		__acquire(mult->lock);
123
124	val = clk_readl(mult->reg);
125	val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
126	val |= factor << mult->shift;
127	clk_writel(val, mult->reg);
128
129	if (mult->lock)
130		spin_unlock_irqrestore(mult->lock, flags);
131	else
132		__release(mult->lock);
133
134	return 0;
135}
136
137const struct clk_ops clk_multiplier_ops = {
138	.recalc_rate	= clk_multiplier_recalc_rate,
139	.round_rate	= clk_multiplier_round_rate,
140	.set_rate	= clk_multiplier_set_rate,
141};
142EXPORT_SYMBOL_GPL(clk_multiplier_ops);