diff options
| -rw-r--r-- | drivers/clk/renesas/renesas-cpg-mssr.c | 84 |
1 files changed, 84 insertions, 0 deletions
diff --git a/drivers/clk/renesas/renesas-cpg-mssr.c b/drivers/clk/renesas/renesas-cpg-mssr.c index 1779b0cc7a2a..15fc8679d342 100644 --- a/drivers/clk/renesas/renesas-cpg-mssr.c +++ b/drivers/clk/renesas/renesas-cpg-mssr.c @@ -26,6 +26,7 @@ #include <linux/platform_device.h> #include <linux/pm_clock.h> #include <linux/pm_domain.h> +#include <linux/psci.h> #include <linux/reset-controller.h> #include <linux/slab.h> @@ -106,6 +107,8 @@ static const u16 srcr[] = { * @num_core_clks: Number of Core Clocks in clks[] * @num_mod_clks: Number of Module Clocks in clks[] * @last_dt_core_clk: ID of the last Core Clock exported to DT + * @smstpcr_saved[].mask: Mask of SMSTPCR[] bits under our control + * @smstpcr_saved[].val: Saved values of SMSTPCR[] */ struct cpg_mssr_priv { #ifdef CONFIG_RESET_CONTROLLER @@ -119,6 +122,11 @@ struct cpg_mssr_priv { unsigned int num_core_clks; unsigned int num_mod_clks; unsigned int last_dt_core_clk; + + struct { + u32 mask; + u32 val; + } smstpcr_saved[ARRAY_SIZE(smstpcr)]; }; @@ -382,6 +390,7 @@ static void __init cpg_mssr_register_mod_clk(const struct mssr_mod_clk *mod, dev_dbg(dev, "Module clock %pC at %pCr Hz\n", clk, clk); priv->clks[id] = clk; + priv->smstpcr_saved[clock->index / 32].mask |= BIT(clock->index % 32); return; fail: @@ -700,6 +709,79 @@ static void cpg_mssr_del_clk_provider(void *data) of_clk_del_provider(data); } +#if defined(CONFIG_PM_SLEEP) && defined(CONFIG_ARM_PSCI_FW) +static int cpg_mssr_suspend_noirq(struct device *dev) +{ + struct cpg_mssr_priv *priv = dev_get_drvdata(dev); + unsigned int reg; + + /* This is the best we can do to check for the presence of PSCI */ + if (!psci_ops.cpu_suspend) + return 0; + + /* Save module registers with bits under our control */ + for (reg = 0; reg < ARRAY_SIZE(priv->smstpcr_saved); reg++) { + if (priv->smstpcr_saved[reg].mask) + priv->smstpcr_saved[reg].val = + readl(priv->base + SMSTPCR(reg)); + } + + return 0; +} + +static int cpg_mssr_resume_noirq(struct device *dev) +{ + struct cpg_mssr_priv *priv = dev_get_drvdata(dev); + unsigned int reg, i; + u32 mask, oldval, newval; + + /* This is the best we can do to check for the presence of PSCI */ + if (!psci_ops.cpu_suspend) + return 0; + + /* Restore module clocks */ + for (reg = 0; reg < ARRAY_SIZE(priv->smstpcr_saved); reg++) { + mask = priv->smstpcr_saved[reg].mask; + if (!mask) + continue; + + oldval = readl(priv->base + SMSTPCR(reg)); + newval = oldval & ~mask; + newval |= priv->smstpcr_saved[reg].val & mask; + if (newval == oldval) + continue; + + writel(newval, priv->base + SMSTPCR(reg)); + + /* Wait until enabled clocks are really enabled */ + mask &= ~priv->smstpcr_saved[reg].val; + if (!mask) + continue; + + for (i = 1000; i > 0; --i) { + oldval = readl(priv->base + MSTPSR(reg)); + if (!(oldval & mask)) + break; + cpu_relax(); + } + + if (!i) + dev_warn(dev, "Failed to enable SMSTP %p[0x%x]\n", + priv->base + SMSTPCR(reg), oldval & mask); + } + + return 0; +} + +static const struct dev_pm_ops cpg_mssr_pm = { + SET_NOIRQ_SYSTEM_SLEEP_PM_OPS(cpg_mssr_suspend_noirq, + cpg_mssr_resume_noirq) +}; +#define DEV_PM_OPS &cpg_mssr_pm +#else +#define DEV_PM_OPS NULL +#endif /* CONFIG_PM_SLEEP && CONFIG_ARM_PSCI_FW */ + static int __init cpg_mssr_probe(struct platform_device *pdev) { struct device *dev = &pdev->dev; @@ -735,6 +817,7 @@ static int __init cpg_mssr_probe(struct platform_device *pdev) if (!clks) return -ENOMEM; + dev_set_drvdata(dev, priv); priv->clks = clks; priv->num_core_clks = info->num_total_core_clks; priv->num_mod_clks = info->num_hw_mod_clks; @@ -775,6 +858,7 @@ static struct platform_driver cpg_mssr_driver = { .driver = { .name = "renesas-cpg-mssr", .of_match_table = cpg_mssr_match, + .pm = DEV_PM_OPS, }, }; |