static unsigned long sunxi_clk_periph_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
{
unsigned long reg, flags = 0;
struct sunxi_clk_periph *periph = to_clk_periph(hw);
struct sunxi_clk_periph_div *divider = &periph->divider;
unsigned long div, div_m = 0, div_n = 0;
u64 rate = parent_rate;
if(!divider->reg)
return parent_rate;
if(periph->lock)
{
spin_lock_irqsave(periph->lock, flags);
reg = periph_readl(periph,divider->reg);
if(divider->mwidth)
div_m = GET_BITS(divider->mshift, divider->mwidth, reg);
if(divider->nwidth)
div_n = GET_BITS(divider->nshift, divider->nwidth, reg);
div = (div_m+1)*(1<<div_n);
do_div(rate, div);
spin_unlock_irqrestore(periph->lock, flags);
}
else
{
reg = periph_readl(periph,divider->reg);
if(divider->mwidth)
div_m = GET_BITS(divider->mshift, divider->mwidth, reg);
if(divider->nwidth)
div_n = GET_BITS(divider->nshift, divider->nwidth, reg);
div = (div_m+1)*(1<<div_n);
do_div(rate, div);
}
return rate;
}
static int sunxi_clk_periph_set_parent(struct clk_hw *hw, u8 index)
{
unsigned long reg, flags = 0;
struct sunxi_clk_periph *periph = to_clk_periph(hw);
if(periph->flags & CLK_READONLY)
return 0;
if(!periph->mux.reg)
return 0;
if(periph->lock)
{
spin_lock_irqsave(periph->lock, flags);
reg = periph_readl(periph,periph->mux.reg);
reg = SET_BITS(periph->mux.shift, periph->mux.width, reg, index);
periph_writel(periph,reg, periph->mux.reg);
spin_unlock_irqrestore(periph->lock, flags);
}
else
{
reg = periph_readl(periph,periph->mux.reg);
reg = SET_BITS(periph->mux.shift, periph->mux.width, reg, index);
periph_writel(periph,reg, periph->mux.reg);
}
return 0;
}
static int __sunxi_clk_periph_is_enabled(struct clk_hw *hw)
{
int state = 1;
unsigned long reg;
struct sunxi_clk_periph *periph = to_clk_periph(hw);
struct sunxi_clk_periph_gate *gate = &periph->gate;
/* enable bus gating */
if(gate->bus) {
reg = periph_readl(periph,gate->bus);
state &= GET_BITS(gate->bus_shift, 1, reg);
}
/* enable module gating */
if(gate->enable) {
reg = periph_readl(periph,gate->enable);
state &= GET_BITS(gate->enb_shift, 1, reg);
}
/* de-assert module */
if(gate->reset) {
reg = periph_readl(periph,gate->reset);
state &= GET_BITS(gate->rst_shift, 1, reg);
}
/* enable dram gating */
if(gate->dram) {
reg = periph_readl(periph,gate->dram);
state &= GET_BITS(gate->ddr_shift, 1, reg);
}
return state;
}
static int __sunxi_clk_periph_set_rate(struct clk_hw *hw, unsigned long rate, unsigned long parent_rate)
{
struct sunxi_clk_periph *periph = to_clk_periph(hw);
struct sunxi_clk_periph_div *divider = &periph->divider;
int i,j,m_max;
int m=0,n=0;
unsigned long cur_rate=0,new_rate=0;
unsigned long cur_delta,new_delta;
u32 reg;
if(parent_rate == 4000000)
m_max =1;
else
m_max = 8;
for(i=0;i<m_max;i++)
for(j=0;j<8;j++)
{
new_rate = parent_rate/(ac100_m_factor[i]*ac100_n_factor[j]);
new_delta = (new_rate >rate)?(new_rate-rate):(rate-new_rate);
cur_delta = (cur_rate >rate)?(cur_rate-rate):(rate-cur_rate);
if(new_delta < cur_delta)
{
cur_rate = new_rate;
m =i;
n = j;
}
}
reg = periph_readl(periph,divider->reg);
if(divider->mwidth)
reg = SET_BITS(divider->mshift, divider->mwidth, reg, m);
if(divider->nwidth)
reg = SET_BITS(divider->nshift, divider->nwidth, reg, n);
periph_writel(periph,reg, divider->reg);
return 0;
}
static int __sunxi_clk_periph_set_rate(struct clk_hw *hw, unsigned long rate, unsigned long parent_rate)
{
#ifdef NEW_RATE_CALULATE
unsigned long i=0,factor_m=0,factor_n=0,found=0;
#endif
unsigned long reg;
struct sunxi_clk_periph *periph = to_clk_periph(hw);
struct sunxi_clk_periph_div *divider = &periph->divider;
unsigned long div, div_m = 0, div_n = 0;
u64 tmp_rate = parent_rate;
if(periph->flags & CLK_READONLY)
return 0;
if(!divider->reg)
return 0;
do_div(tmp_rate, rate);
div = tmp_rate;
if(!div)
div_m = div_n =0;
else {
div_m = 1<<divider->mwidth;
div_n = (1<<divider->nwidth)-1;
#ifndef NEW_RATE_CALULATE
if(div < div_m) {
div_m = div;
div_n = 0;
} else if((div < div_m*2) && (div_n > 0)){
div_n = 1;
div_m = div>>1;
} else if((div < div_m*4) && (div_n > 1)){
static void clk_periph_disable(struct clk_hw *hw)
{
struct tegra_clk_periph *periph = to_clk_periph(hw);
const struct clk_ops *gate_ops = periph->gate_ops;
struct clk_hw *gate_hw = &periph->gate.hw;
gate_ops->disable(gate_hw);
}
static int clk_periph_is_enabled(struct clk_hw *hw)
{
struct tegra_clk_periph *periph = to_clk_periph(hw);
const struct clk_ops *gate_ops = periph->gate_ops;
struct clk_hw *gate_hw = &periph->gate.hw;
__clk_hw_set_clk(gate_hw, hw);
return gate_ops->is_enabled(gate_hw);
}
static int clk_periph_set_parent(struct clk_hw *hw, u8 index)
{
struct tegra_clk_periph *periph = to_clk_periph(hw);
const struct clk_ops *mux_ops = periph->mux_ops;
struct clk_hw *mux_hw = &periph->mux.hw;
mux_hw->clk = hw->clk;
return mux_ops->set_parent(mux_hw, index);
}
static int clk_periph_enable(struct clk_hw *hw)
{
struct tegra_clk_periph *periph = to_clk_periph(hw);
const struct clk_ops *gate_ops = periph->gate_ops;
struct clk_hw *gate_hw = &periph->gate.hw;
gate_hw->clk = hw->clk;
return gate_ops->enable(gate_hw);
}
static u8 clk_periph_get_parent(struct clk_hw *hw)
{
struct tegra_clk_periph *periph = to_clk_periph(hw);
const struct clk_ops *mux_ops = periph->mux_ops;
struct clk_hw *mux_hw = &periph->mux.hw;
mux_hw->clk = hw->clk;
return mux_ops->get_parent(mux_hw);
}
static int clk_periph_set_rate(struct clk_hw *hw, unsigned long rate,
unsigned long parent_rate)
{
struct tegra_clk_periph *periph = to_clk_periph(hw);
const struct clk_ops *div_ops = periph->div_ops;
struct clk_hw *div_hw = &periph->divider.hw;
div_hw->clk = hw->clk;
return div_ops->set_rate(div_hw, rate, parent_rate);
}
static long clk_periph_round_rate(struct clk_hw *hw, unsigned long rate,
unsigned long *prate)
{
struct tegra_clk_periph *periph = to_clk_periph(hw);
const struct clk_ops *div_ops = periph->div_ops;
struct clk_hw *div_hw = &periph->divider.hw;
div_hw->clk = hw->clk;
return div_ops->round_rate(div_hw, rate, prate);
}
static unsigned long clk_periph_recalc_rate(struct clk_hw *hw,
unsigned long parent_rate)
{
struct tegra_clk_periph *periph = to_clk_periph(hw);
const struct clk_ops *div_ops = periph->div_ops;
struct clk_hw *div_hw = &periph->divider.hw;
__clk_hw_set_clk(div_hw, hw);
return div_ops->recalc_rate(div_hw, parent_rate);
}
static int sunxi_clk_periph_is_enabled(struct clk_hw *hw)
{
int state = 0;
unsigned long flags = 0;
struct sunxi_clk_periph *periph = to_clk_periph(hw);
if(periph->lock)
{
spin_lock_irqsave(periph->lock, flags);
state = __sunxi_clk_periph_is_enabled(hw);
spin_unlock_irqrestore(periph->lock, flags);
}
else
state = __sunxi_clk_periph_is_enabled(hw);
return state;
}
void tegra_periph_reset_assert(struct clk *c)
{
struct clk_hw *hw = __clk_get_hw(c);
struct tegra_clk_periph *periph = to_clk_periph(hw);
struct tegra_clk_periph_gate *gate;
if (periph->magic != TEGRA_CLK_PERIPH_MAGIC) {
gate = to_clk_periph_gate(hw);
if (gate->magic != TEGRA_CLK_PERIPH_GATE_MAGIC) {
WARN_ON(1);
return;
}
} else {
gate = &periph->gate;
}
tegra_periph_reset(gate, 1);
}
static void sunxi_clk_periph_disable(struct clk_hw *hw)
{
unsigned long flags = 0;
struct sunxi_clk_periph *periph = to_clk_periph(hw);
if(periph->flags & CLK_READONLY)
return ;
if(periph->lock)
{
spin_lock_irqsave(periph->lock, flags);
__sunxi_clk_periph_disable(hw);
spin_unlock_irqrestore(periph->lock, flags);
}
else
__sunxi_clk_periph_disable(hw);
if(periph->com_gate)
sunxi_clk_periph_disable_shared(periph);
}
static u8 sunxi_clk_periph_get_parent(struct clk_hw *hw)
{
u8 parent;
unsigned long reg, flags = 0;
struct sunxi_clk_periph *periph = to_clk_periph(hw);
if(!periph->mux.reg)
return 0;
if(periph->lock)
{
spin_lock_irqsave(periph->lock, flags);
reg = periph_readl(periph,periph->mux.reg);
parent = GET_BITS(periph->mux.shift, periph->mux.width, reg);
spin_unlock_irqrestore(periph->lock, flags);
}
else
{
reg = periph_readl(periph,periph->mux.reg);
parent = GET_BITS(periph->mux.shift, periph->mux.width, reg);
}
return parent;
}
static unsigned long sunxi_ac100_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
{
unsigned long reg;
struct sunxi_clk_periph *periph = to_clk_periph(hw);
struct sunxi_clk_periph_div *divider = &periph->divider;
unsigned long div, div_m = 0, div_n = 0;
u64 rate = parent_rate;
if(!divider->reg)
return parent_rate;
reg = periph_readl(periph,divider->reg);
if(divider->mwidth)
div_m = GET_BITS(divider->mshift, divider->mwidth, reg);
if(divider->nwidth)
div_n = GET_BITS(divider->nshift, divider->nwidth, reg);
if(reg & 0x100)
div = ac100_m_factor[div_m]*ac100_n_factor[div_n];
else
div = ac100_n_factor[div_n];
do_div(rate, div);
return rate;
}
static int __sunxi_clk_periph_enable(struct clk_hw *hw)
{
unsigned long reg;
struct sunxi_clk_periph *periph = to_clk_periph(hw);
struct sunxi_clk_periph_gate *gate = &periph->gate;
/* de-assert module */
if(gate->reset && !(periph->flags & CLK_IGNORE_AUTORESET) && !IS_SHARE_RST_GATE(periph)) {
reg = periph_readl(periph,gate->reset);
reg = SET_BITS(gate->rst_shift, 1, reg, 1);
periph_writel(periph,reg, gate->reset);
}
/* enable bus gating */
if(gate->bus && !IS_SHARE_BUS_GATE(periph)) {
reg = periph_readl(periph,gate->bus);
reg = SET_BITS(gate->bus_shift, 1, reg, 1);
periph_writel(periph,reg, gate->bus);
}
/* enable module gating */
if(gate->enable&& !IS_SHARE_MOD_GATE(periph)) {
reg = periph_readl(periph,gate->enable);
if(periph->flags & CLK_REVERT_ENABLE)
reg = SET_BITS(gate->enb_shift, 1, reg, 0);
else
reg = SET_BITS(gate->enb_shift, 1, reg, 1);
periph_writel(periph,reg, gate->enable);
}
/* enable dram gating */
if(gate->dram&& !IS_SHARE_MBUS_GATE(periph)) {
reg = periph_readl(periph,gate->dram);
reg = SET_BITS(gate->ddr_shift, 1, reg, 1);
periph_writel(periph,reg, gate->dram);
}
return 0;
}
static long sunxi_clk_periph_round_rate(struct clk_hw *hw, unsigned long rate, unsigned long *prate)
{
struct sunxi_clk_periph *periph = to_clk_periph(hw);
struct sunxi_clk_periph_div *divider = &periph->divider;
#ifdef NEW_RATE_CALULATE
unsigned long i=0,factor_m=0,factor_n=0,found=0;
#endif
unsigned long div, div_m = 0, div_n = 0;
u64 parent_rate = (*prate+rate/2-1);
do_div(parent_rate, rate);
div = parent_rate;
if(!div)
return *prate;
parent_rate = *prate;
div_m = 1<<divider->mwidth;
if(divider->nwidth) {
div_n = 1<<divider->nwidth;
div_n = 1<<(div_n-1);
} else
div_n = 1;
#ifndef NEW_RATE_CALULATE
if(div <= div_m) {
do_div(parent_rate, div);
} else if((div <= div_m*2) && (div_n >= 2)) {
div &= ~(1<<0);
do_div(parent_rate, div);
} else if((div <= div_m*4) && (div_n >= 4)) {
div &= ~(3<<0);
do_div(parent_rate, div);
} else if((div <= div_m*8) && (div_n >= 8)) {
div &= ~(7<<0);
do_div(parent_rate, div);
} else if((div <= div_m*16) && (div_n >= 16)) {
div &= ~(15<<0);
do_div(parent_rate, div);
} else if((div <= div_m*32) && (div_n >= 32)) {
div &= ~(31<<0);
do_div(parent_rate, div);
} else if((div <= div_m*64) && (div_n >= 64)) {
div &= ~(63<<0);
do_div(parent_rate, div);
} else if((div <= div_m*128) && (div_n >= 128)) {
div &= ~(127<<0);
do_div(parent_rate, div);
} else {
do_div(parent_rate, div_m*div_n);
}
#else //NEW_RATE_CALULATE
while(i < (1<<divider->nwidth))
{
if(div <= div_m)
{
factor_m = div-1;
factor_n = i;
do_div(parent_rate, (factor_m+1)*(1 << factor_n));
found = 1;
break;
}
div = div >>1;
i++;
if(!div)
{
factor_m = 0;
factor_n = i;
do_div(parent_rate, (factor_m+1)*(1 << factor_n));
found = 1;
break;
}
}
if(!found)
{
factor_m = (div >div_m?div_m:div)-1;
factor_n = (1<<divider->nwidth) -1;
do_div(parent_rate, (factor_m+1)*(1 << factor_n));
}
#endif
return parent_rate;
}
