static int cpg_mstp_clock_is_enabled(struct clk_hw *hw)
{
struct mstp_clock *clock = to_mstp_clock(hw);
struct cpg_mssr_priv *priv = clock->priv;
u32 value;
value = clk_readl(priv->base + MSTPSR(clock->index / 32));
return !(value & BIT(clock->index % 32));
}
static int cpg_mstp_clock_is_enabled(struct clk_hw *hw)
{
struct mstp_clock *clock = to_mstp_clock(hw);
struct mstp_clock_group *group = clock->group;
u32 value;
if (group->mstpsr)
value = cpg_mstp_read(group, group->mstpsr);
else
value = cpg_mstp_read(group, group->smstpcr);
return !(value & BIT(clock->bit_index));
}
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 = cpg_mstp_read(group, group->smstpcr);
if (enable)
value &= ~bitmask;
else
value |= bitmask;
cpg_mstp_write(group, value, group->smstpcr);
if (!group->mstpsr) {
/* dummy read to ensure write has completed */
cpg_mstp_read(group, group->smstpcr);
barrier_data(group->smstpcr);
}
spin_unlock_irqrestore(&group->lock, flags);
if (!enable || !group->mstpsr)
return 0;
for (i = 1000; i > 0; --i) {
if (!(cpg_mstp_read(group, 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;
}
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;
}
