Loading...
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 | /*
* 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>
#define to_clk_multiplier(_hw) container_of(_hw, struct clk_multiplier, hw)
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)
{
unsigned long orig_parent_rate = *best_parent_rate;
unsigned long parent_rate, current_rate, best_rate = ~0;
unsigned int i, bestmult = 0;
if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT))
return rate / *best_parent_rate;
for (i = 1; i < ((1 << width) - 1); 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);
|