static long cpu_clk_set_rate(struct clk *clk, unsigned long rate, int apply)
{
u32 control;
unsigned long parent_rate, child_div, actual_rate, div;
parent_rate = clk->parent->get_rate(clk->parent);
control = pm_readl(CKSEL);
if (control & PM_BIT(HSBDIV))
child_div = 1 << (PM_BFEXT(HSBSEL, control) + 1);
else
child_div = 1;
if (rate > 3 * (parent_rate / 4) || child_div == 1) {
actual_rate = parent_rate;
control &= ~PM_BIT(CPUDIV);
} else {
unsigned int cpusel;
div = (parent_rate + rate / 2) / rate;
if (div > child_div)
div = child_div;
cpusel = (div > 1) ? (fls(div) - 2) : 0;
control = PM_BIT(CPUDIV) | PM_BFINS(CPUSEL, cpusel, control);
actual_rate = parent_rate / (1 << (cpusel + 1));
}
pr_debug("clk %s: new rate %lu (actual rate %lu)\n",
clk->name, rate, actual_rate);
if (apply)
pm_writel(CKSEL, control);
return actual_rate;
}
int genclk_set_parent(struct clk *clk, struct clk *parent)
{
u32 control;
dev_dbg(clk->dev, "clk %s: new parent %s (was %s)\n",
clk->name, parent->name, clk->parent->name);
control = pm_readl(GCCTRL(clk->index));
if (parent == &osc1 || parent == &pll1)
control |= PM_BIT(OSCSEL);
else if (parent == &osc0 || parent == &pll0)
control &= ~PM_BIT(OSCSEL);
else
return -EINVAL;
if (parent == &pll0 || parent == &pll1)
control |= PM_BIT(PLLSEL);
else
control &= ~PM_BIT(PLLSEL);
pm_writel(GCCTRL(clk->index), control);
clk->parent = parent;
return 0;
}
static void pll1_mode(struct clk *clk, int enabled)
{
unsigned long timeout;
u32 status;
u32 ctrl;
ctrl = pm_readl(PLL1);
if (enabled) {
if (!PM_BFEXT(PLLMUL, ctrl) && !PM_BFEXT(PLLDIV, ctrl)) {
pr_debug("clk %s: failed to enable, rate not set\n",
clk->name);
return;
}
ctrl |= PM_BIT(PLLEN);
pm_writel(PLL1, ctrl);
/* Wait for PLL lock. */
for (timeout = 10000; timeout; timeout--) {
status = pm_readl(ISR);
if (status & PM_BIT(LOCK1))
break;
udelay(10);
}
if (!(status & PM_BIT(LOCK1)))
printk(KERN_ERR "clk %s: timeout waiting for lock\n",
clk->name);
} else {
ctrl &= ~PM_BIT(PLLEN);
pm_writel(PLL1, ctrl);
}
}
void __init at32_clock_init(void)
{
u32 cpu_mask = 0, hsb_mask = 0, pba_mask = 0, pbb_mask = 0;
int i;
if (pm_readl(MCCTRL) & PM_BIT(PLLSEL)) {
main_clock = &pll0;
cpu_clk.parent = &pll0;
} else {
main_clock = &osc0;
cpu_clk.parent = &osc0;
}
if (pm_readl(PLL0) & PM_BIT(PLLOSC))
pll0.parent = &osc1;
if (pm_readl(PLL1) & PM_BIT(PLLOSC))
pll1.parent = &osc1;
genclk_init_parent(&gclk0);
genclk_init_parent(&gclk1);
genclk_init_parent(&gclk2);
genclk_init_parent(&gclk3);
genclk_init_parent(&gclk4);
genclk_init_parent(&atmel_lcdfb0_pixclk);
genclk_init_parent(&abdac0_sample_clk);
/*
* Turn on all clocks that have at least one user already, and
* turn off everything else. We only do this for module
* clocks, and even though it isn't particularly pretty to
* check the address of the mode function, it should do the
* trick...
*/
for (i = 0; i < ARRAY_SIZE(at32_clock_list); i++) {
struct clk *clk = at32_clock_list[i];
if (clk->users == 0)
continue;
if (clk->mode == &cpu_clk_mode)
cpu_mask |= 1 << clk->index;
else if (clk->mode == &hsb_clk_mode)
hsb_mask |= 1 << clk->index;
else if (clk->mode == &pba_clk_mode)
pba_mask |= 1 << clk->index;
else if (clk->mode == &pbb_clk_mode)
pbb_mask |= 1 << clk->index;
}
pm_writel(CPU_MASK, cpu_mask);
pm_writel(HSB_MASK, hsb_mask);
pm_writel(PBA_MASK, pba_mask);
pm_writel(PBB_MASK, pbb_mask);
}
static void __init genclk_init_parent(struct clk *clk)
{
u32 control;
struct clk *parent;
BUG_ON(clk->index > 7);
control = pm_readl(GCCTRL(clk->index));
if (control & PM_BIT(OSCSEL))
parent = (control & PM_BIT(PLLSEL)) ? &pll1 : &osc1;
else
parent = (control & PM_BIT(PLLSEL)) ? &pll0 : &osc0;
clk->parent = parent;
}
static long genclk_set_rate(struct clk *clk, unsigned long rate, int apply)
{
u32 control;
unsigned long parent_rate, actual_rate, div;
parent_rate = clk->parent->get_rate(clk->parent);
control = pm_readl(GCCTRL(clk->index));
if (rate > 3 * parent_rate / 4) {
actual_rate = parent_rate;
control &= ~PM_BIT(DIVEN);
} else {
div = (parent_rate + rate) / (2 * rate) - 1;
control = PM_BFINS(DIV, div, control) | PM_BIT(DIVEN);
actual_rate = parent_rate / (2 * (div + 1));
}
dev_dbg(clk->dev, "clk %s: new rate %lu (actual rate %lu)\n",
clk->name, rate, actual_rate);
if (apply)
pm_writel(GCCTRL(clk->index), control);
return actual_rate;
}
static unsigned long pbb_clk_get_rate(struct clk *clk)
{
unsigned long cksel, shift = 0;
cksel = pm_readl(CKSEL);
if (cksel & PM_BIT(PBBDIV))
shift = PM_BFEXT(PBBSEL, cksel) + 1;
return bus_clk_get_rate(clk, shift);
}
static unsigned long genclk_get_rate(struct clk *clk)
{
u32 control;
unsigned long div = 1;
control = pm_readl(GCCTRL(clk->index));
if (control & PM_BIT(DIVEN))
div = 2 * (PM_BFEXT(DIV, control) + 1);
return clk->parent->get_rate(clk->parent) / div;
}
static void genclk_mode(struct clk *clk, int enabled)
{
u32 control;
control = pm_readl(GCCTRL(clk->index));
if (enabled)
control |= PM_BIT(CEN);
else
control &= ~PM_BIT(CEN);
pm_writel(GCCTRL(clk->index), control);
}
static int pll1_set_parent(struct clk *clk, struct clk *parent)
{
u32 ctrl;
if (clk->users > 0)
return -EBUSY;
ctrl = pm_readl(PLL1);
WARN_ON(ctrl & PM_BIT(PLLEN));
if (parent == &osc0)
ctrl &= ~PM_BIT(PLLOSC);
else if (parent == &osc1)
ctrl |= PM_BIT(PLLOSC);
else
return -EINVAL;
pm_writel(PLL1, ctrl);
clk->parent = parent;
return 0;
}
static unsigned long pll_get_rate(struct clk *clk, unsigned long control)
{
unsigned long div, mul, rate;
if (!(control & PM_BIT(PLLEN)))
return 0;
div = PM_BFEXT(PLLDIV, control) + 1;
mul = PM_BFEXT(PLLMUL, control) + 1;
rate = clk->parent->get_rate(clk->parent);
rate = (rate + div / 2) / div;
rate *= mul;
return rate;
}
static long pll_set_rate(struct clk *clk, unsigned long rate,
u32 *pll_ctrl)
{
unsigned long mul;
unsigned long mul_best_fit = 0;
unsigned long div;
unsigned long div_min;
unsigned long div_max;
unsigned long div_best_fit = 0;
unsigned long base;
unsigned long pll_in;
unsigned long actual = 0;
unsigned long rate_error;
unsigned long rate_error_prev = ~0UL;
u32 ctrl;
/* Rate must be between 80 MHz and 200 Mhz. */
if (rate < 80000000UL || rate > 200000000UL)
return -EINVAL;
ctrl = PM_BF(PLLOPT, 4);
base = clk->parent->get_rate(clk->parent);
/* PLL input frequency must be between 6 MHz and 32 MHz. */
div_min = DIV_ROUND_UP(base, 32000000UL);
div_max = base / 6000000UL;
if (div_max < div_min)
return -EINVAL;
for (div = div_min; div <= div_max; div++) {
pll_in = (base + div / 2) / div;
mul = (rate + pll_in / 2) / pll_in;
if (mul == 0)
continue;
actual = pll_in * mul;
rate_error = abs(actual - rate);
if (rate_error < rate_error_prev) {
mul_best_fit = mul;
div_best_fit = div;
rate_error_prev = rate_error;
}
if (rate_error == 0)
break;
}
if (div_best_fit == 0)
return -EINVAL;
ctrl |= PM_BF(PLLMUL, mul_best_fit - 1);
ctrl |= PM_BF(PLLDIV, div_best_fit - 1);
ctrl |= PM_BF(PLLCOUNT, 16);
if (clk->parent == &osc1)
ctrl |= PM_BIT(PLLOSC);
*pll_ctrl = ctrl;
return actual;
}