25f77a3aa4
In the current multiplier base clock implementation, if the
CLK_SET_RATE_PARENT flag isn't set, the code will not make sure that the
multiplier computed remains within the boundaries of our clock.
This means that if the clock we want to reach is below the parent rate,
or if the multiplier is above the maximum that we can reach, we will end up
with a completely bogus one that the clock cannot achieve.
Fixes: f2e0a53271 ("clk: Add a basic multiplier clock")
Signed-off-by: Maxime Ripard <maxime.ripard@free-electrons.com>
Signed-off-by: Michael Turquette <mturquette@baylibre.com>
Link: lkml.kernel.org/r/1463402840-17062-3-git-send-email-maxime.ripard@free-electrons.com
143 lines
3.6 KiB
C
143 lines
3.6 KiB
C
/*
|
|
* Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com>
|
|
*
|
|
* 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.
|
|
*/
|
|
|
|
#include <linux/bitops.h>
|
|
#include <linux/clk-provider.h>
|
|
#include <linux/err.h>
|
|
#include <linux/export.h>
|
|
#include <linux/kernel.h>
|
|
#include <linux/of.h>
|
|
#include <linux/slab.h>
|
|
|
|
static unsigned long __get_mult(struct clk_multiplier *mult,
|
|
unsigned long rate,
|
|
unsigned long parent_rate)
|
|
{
|
|
if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
|
|
return DIV_ROUND_CLOSEST(rate, parent_rate);
|
|
|
|
return rate / parent_rate;
|
|
}
|
|
|
|
static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
|
|
unsigned long parent_rate)
|
|
{
|
|
struct clk_multiplier *mult = to_clk_multiplier(hw);
|
|
unsigned long val;
|
|
|
|
val = clk_readl(mult->reg) >> mult->shift;
|
|
val &= GENMASK(mult->width - 1, 0);
|
|
|
|
if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
|
|
val = 1;
|
|
|
|
return parent_rate * val;
|
|
}
|
|
|
|
static bool __is_best_rate(unsigned long rate, unsigned long new,
|
|
unsigned long best, unsigned long flags)
|
|
{
|
|
if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
|
|
return abs(rate - new) < abs(rate - best);
|
|
|
|
return new >= rate && new < best;
|
|
}
|
|
|
|
static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
|
|
unsigned long *best_parent_rate,
|
|
u8 width, unsigned long flags)
|
|
{
|
|
struct clk_multiplier *mult = to_clk_multiplier(hw);
|
|
unsigned long orig_parent_rate = *best_parent_rate;
|
|
unsigned long parent_rate, current_rate, best_rate = ~0;
|
|
unsigned int i, bestmult = 0;
|
|
unsigned int maxmult = (1 << width) - 1;
|
|
|
|
if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
|
|
bestmult = rate / orig_parent_rate;
|
|
|
|
/* Make sure we don't end up with a 0 multiplier */
|
|
if ((bestmult == 0) &&
|
|
!(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
|
|
bestmult = 1;
|
|
|
|
/* Make sure we don't overflow the multiplier */
|
|
if (bestmult > maxmult)
|
|
bestmult = maxmult;
|
|
|
|
return bestmult;
|
|
}
|
|
|
|
for (i = 1; i < maxmult; i++) {
|
|
if (rate == orig_parent_rate * i) {
|
|
/*
|
|
* This is the best case for us if we have a
|
|
* perfect match without changing the parent
|
|
* rate.
|
|
*/
|
|
*best_parent_rate = orig_parent_rate;
|
|
return i;
|
|
}
|
|
|
|
parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
|
|
rate / i);
|
|
current_rate = parent_rate * i;
|
|
|
|
if (__is_best_rate(rate, current_rate, best_rate, flags)) {
|
|
bestmult = i;
|
|
best_rate = current_rate;
|
|
*best_parent_rate = parent_rate;
|
|
}
|
|
}
|
|
|
|
return bestmult;
|
|
}
|
|
|
|
static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
|
|
unsigned long *parent_rate)
|
|
{
|
|
struct clk_multiplier *mult = to_clk_multiplier(hw);
|
|
unsigned long factor = __bestmult(hw, rate, parent_rate,
|
|
mult->width, mult->flags);
|
|
|
|
return *parent_rate * factor;
|
|
}
|
|
|
|
static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
|
|
unsigned long parent_rate)
|
|
{
|
|
struct clk_multiplier *mult = to_clk_multiplier(hw);
|
|
unsigned long factor = __get_mult(mult, rate, parent_rate);
|
|
unsigned long flags = 0;
|
|
unsigned long val;
|
|
|
|
if (mult->lock)
|
|
spin_lock_irqsave(mult->lock, flags);
|
|
else
|
|
__acquire(mult->lock);
|
|
|
|
val = clk_readl(mult->reg);
|
|
val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
|
|
val |= factor << mult->shift;
|
|
clk_writel(val, mult->reg);
|
|
|
|
if (mult->lock)
|
|
spin_unlock_irqrestore(mult->lock, flags);
|
|
else
|
|
__release(mult->lock);
|
|
|
|
return 0;
|
|
}
|
|
|
|
const struct clk_ops clk_multiplier_ops = {
|
|
.recalc_rate = clk_multiplier_recalc_rate,
|
|
.round_rate = clk_multiplier_round_rate,
|
|
.set_rate = clk_multiplier_set_rate,
|
|
};
|
|
EXPORT_SYMBOL_GPL(clk_multiplier_ops);
|