static int lpc18xx_ccu_gate_endisable(struct clk_hw *hw, bool enable)
{
struct clk_gate *gate = to_clk_gate(hw);
u32 val;
/*
* Divider field is write only, so divider stat field must
* be read so divider field can be set accordingly.
*/
val = clk_readl(gate->reg);
if (val & LPC18XX_CCU_DIVSTAT)
val |= LPC18XX_CCU_DIV;
if (enable) {
val |= LPC18XX_CCU_RUN;
} else {
/*
* To safely disable a branch clock a squence of two separate
* writes must be used. First write should set the AUTO bit
* and the next write should clear the RUN bit.
*/
val |= LPC18XX_CCU_AUTO;
clk_writel(val, gate->reg);
val &= ~LPC18XX_CCU_RUN;
}
clk_writel(val, gate->reg);
return 0;
}
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;
unsigned long m, n;
u32 val;
rational_best_approximation(rate, parent_rate,
GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
&m, &n);
if (fd->lock)
spin_lock_irqsave(fd->lock, flags);
#if 0
else
__acquire(fd->lock);
#endif
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);
#if 0
else
__release(fd->lock);
#endif
return 0;
}
/*
* It works on following logic:
*
* For enabling clock, enable = 1
* set2dis = 1 -> clear bit -> set = 0
* set2dis = 0 -> set bit -> set = 1
*
* For disabling clock, enable = 0
* set2dis = 1 -> set bit -> set = 1
* set2dis = 0 -> clear bit -> set = 0
*
* So, result is always: enable xor set2dis.
*/
static void clk_gate_endisable(struct clk_hw *hw, int enable)
{
struct clk_gate *gate = to_clk_gate(hw);
int set = gate->flags & CLK_GATE_SET_TO_DISABLE ? 1 : 0;
unsigned long uninitialized_var(flags);
u32 reg;
set ^= enable;
if (gate->lock)
spin_lock_irqsave(gate->lock, flags);
else
__acquire(gate->lock);
if (gate->flags & CLK_GATE_HIWORD_MASK) {
reg = BIT(gate->bit_idx + 16);
if (set)
reg |= BIT(gate->bit_idx);
} else {
reg = clk_readl(gate->reg);
if (set)
reg |= BIT(gate->bit_idx);
else
reg &= ~BIT(gate->bit_idx);
}
clk_writel(reg, gate->reg);
if (gate->lock)
spin_unlock_irqrestore(gate->lock, flags);
else
__release(gate->lock);
}
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;
unsigned long div;
unsigned n, m;
u32 val;
div = gcd(parent_rate, rate);
m = rate / div;
n = parent_rate / div;
if (fd->lock)
spin_lock_irqsave(fd->lock, flags);
else
__acquire(fd->lock);
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);
else
__release(fd->lock);
return 0;
}
/* clk_m functions */
static unsigned long tegra2_clk_m_autodetect_rate(struct clk *c)
{
u32 auto_clock_control = clk_readl(OSC_CTRL) & ~OSC_CTRL_OSC_FREQ_MASK;
c->rate = clk_measure_input_freq();
switch (c->rate) {
case 12000000:
auto_clock_control |= OSC_CTRL_OSC_FREQ_12MHZ;
break;
case 13000000:
auto_clock_control |= OSC_CTRL_OSC_FREQ_13MHZ;
break;
case 19200000:
auto_clock_control |= OSC_CTRL_OSC_FREQ_19_2MHZ;
break;
case 26000000:
auto_clock_control |= OSC_CTRL_OSC_FREQ_26MHZ;
break;
default:
pr_err("%s: Unexpected clock rate %ld", __func__, c->rate);
BUG();
}
clk_writel(auto_clock_control, OSC_CTRL);
return c->rate;
}
unsigned long tegra_clk_measure_input_freq(void)
{
u32 clock_autodetect;
if (osc_input_freq)
return osc_input_freq;
clk_writel(OSC_FREQ_DET_TRIG | 1, OSC_FREQ_DET);
do {} while (clk_readl(OSC_FREQ_DET_STATUS) & OSC_FREQ_DET_BUSY);
clock_autodetect = clk_readl(OSC_FREQ_DET_STATUS);
if (clock_autodetect >= 732 - 3 && clock_autodetect <= 732 + 3)
osc_input_freq = 12000000;
else if (clock_autodetect >= 794 - 3 && clock_autodetect <= 794 + 3)
osc_input_freq = 13000000;
else if (clock_autodetect >= 1172 - 3 && clock_autodetect <= 1172 + 3)
osc_input_freq = 19200000;
else if (clock_autodetect >= 1587 - 3 && clock_autodetect <= 1587 + 3)
osc_input_freq = 26000000;
#ifndef CONFIG_ARCH_TEGRA_2x_SOC
else if (clock_autodetect >= 1025 - 3 && clock_autodetect <= 1025 + 3)
osc_input_freq = 16800000;
else if (clock_autodetect >= 2344 - 3 && clock_autodetect <= 2344 + 3)
osc_input_freq = 38400000;
else if (clock_autodetect >= 2928 - 3 && clock_autodetect <= 2928 + 3)
osc_input_freq = 48000000;
#endif
else {
pr_err("%s: Unexpected clock autodetect value %d", __func__,
clock_autodetect);
BUG();
}
return osc_input_freq;
}
static int cpg_div6_clock_enable(struct clk_hw *hw)
{
struct div6_clock *clock = to_div6_clock(hw);
clk_writel(CPG_DIV6_DIV(clock->div - 1), clock->reg);
return 0;
}
static void cpg_div6_clock_disable(struct clk_hw *hw)
{
struct div6_clock *clock = to_div6_clock(hw);
/* DIV6 clocks require the divisor field to be non-zero when stopping
* the clock.
*/
clk_writel(CPG_DIV6_CKSTP | CPG_DIV6_DIV(CPG_DIV6_DIV_MASK),
clock->reg);
}
static int cpg_div6_clock_set_rate(struct clk_hw *hw, unsigned long rate,
unsigned long parent_rate)
{
struct div6_clock *clock = to_div6_clock(hw);
unsigned int div = cpg_div6_clock_calc_div(rate, parent_rate);
clock->div = div;
/* Only program the new divisor if the clock isn't stopped. */
if (!(clk_readl(clock->reg) & CPG_DIV6_CKSTP))
clk_writel(CPG_DIV6_DIV(clock->div - 1), clock->reg);
return 0;
}
static int cpg_mstp_clock_endisable(struct clk_hw *hw, bool enable)
{
struct mstp_clock *clock = to_mstp_clock(hw);
struct cpg_mssr_priv *priv = clock->priv;
unsigned int reg = clock->index / 32;
unsigned int bit = clock->index % 32;
struct device *dev = priv->dev;
u32 bitmask = BIT(bit);
unsigned long flags;
unsigned int i;
u32 value;
dev_dbg(dev, "MSTP %u%02u/%pC %s\n", reg, bit, hw->clk,
enable ? "ON" : "OFF");
spin_lock_irqsave(&priv->mstp_lock, flags);
value = clk_readl(priv->base + SMSTPCR(reg));
if (enable)
value &= ~bitmask;
else
value |= bitmask;
clk_writel(value, priv->base + SMSTPCR(reg));
spin_unlock_irqrestore(&priv->mstp_lock, flags);
if (!enable)
return 0;
for (i = 1000; i > 0; --i) {
if (!(clk_readl(priv->base + MSTPSR(reg)) &
bitmask))
break;
cpu_relax();
}
if (!i) {
dev_err(dev, "Failed to enable SMSTP %p[%d]\n",
priv->base + SMSTPCR(reg), bit);
return -ETIMEDOUT;
}
return 0;
}
unsigned long clk_measure_input_freq(void)
{
u32 clock_autodetect;
clk_writel(OSC_FREQ_DET_TRIG | 1, OSC_FREQ_DET);
do {} while (clk_readl(OSC_FREQ_DET_STATUS) & OSC_FREQ_DET_BUSY);
clock_autodetect = clk_readl(OSC_FREQ_DET_STATUS);
if (clock_autodetect >= 732 - 3 && clock_autodetect <= 732 + 3) {
return 12000000;
} else if (clock_autodetect >= 794 - 3 && clock_autodetect <= 794 + 3) {
return 13000000;
} else if (clock_autodetect >= 1172 - 3 && clock_autodetect <= 1172 + 3) {
return 19200000;
} else if (clock_autodetect >= 1587 - 3 && clock_autodetect <= 1587 + 3) {
return 26000000;
} else {
pr_err("%s: Unexpected clock autodetect value %d", __func__, clock_autodetect);
BUG();
return 0;
}
}
static int cpg_mstp_clock_endisable(struct clk_hw *hw, bool enable)
{
struct mstp_clock *clock = to_mstp_clock(hw);
struct mstp_clock_group *group = clock->group;
u32 bitmask = BIT(clock->bit_index);
unsigned long flags;
unsigned int i;
u32 value;
spin_lock_irqsave(&group->lock, flags);
value = clk_readl(group->smstpcr);
if (enable)
value &= ~bitmask;
else
value |= bitmask;
clk_writel(value, group->smstpcr);
spin_unlock_irqrestore(&group->lock, flags);
if (!enable || !group->mstpsr)
return 0;
for (i = 1000; i > 0; --i) {
if (!(clk_readl(group->mstpsr) & bitmask))
break;
cpu_relax();
}
if (!i) {
pr_err("%s: failed to enable %p[%d]\n", __func__,
group->smstpcr, clock->bit_index);
return -ETIMEDOUT;
}
return 0;
}