clk: starfive: Add funtions of saving and restoring data about SYS, AON and STG

Add 'save_context' ops to save register value of clock and
'restore_context' ops to restore the value to register.
The ops only suitable for SYS, AON and STG clock tree not ISP and VOUT.

Signed-off-by: Xingyu Wu <xingyu.wu@starfivetech.com>

2 files changed, 185 insertions, 0 deletions

diff --git a/drivers/clk/starfive/clk-starfive-jh7110-gen.c b/drivers/clk/starfive/clk-starfive-jh7110-gen.c
index d852c0a2d01f..26cbe0710b43 100644
--- a/drivers/clk/starfive/clk-starfive-jh7110-gen.c
+++ b/drivers/clk/starfive/clk-starfive-jh7110-gen.c
@@ -239,11 +239,145 @@ static void jh7110_clk_debug_init(struct clk_hw *hw, struct dentry *dentry)
 #define jh7110_clk_debug_init NULL
 #endif
 
+#ifdef CONFIG_PM_SLEEP
+static int jh7110_clk_save_context(struct clk_hw *hw)
+{
+	struct jh7110_clk *clk = jh7110_clk_from(hw);
+	void __iomem *reg = jh7110_clk_reg_addr_get(clk);
+	struct jh7110_clk_priv *priv = jh7110_priv_from(clk);
+
+	if (!clk || !priv)
+		return 0;
+
+	if ((clk->reg_flags == JH7110_CLK_ISP_FLAG) || (clk->reg_flags == JH7110_CLK_VOUT_FLAG))
+		return 0;
+
+	if (clk->idx >= JH7110_CLK_REG_END)
+		return 0;
+
+	spin_lock(&priv->rmw_lock);
+	clk->saved_reg_value = readl_relaxed(reg);
+	spin_unlock(&priv->rmw_lock);
+
+	return 0;
+}
+
+static void jh7110_clk_gate_restore_context(struct clk_hw *hw)
+{
+	struct jh7110_clk *clk = jh7110_clk_from(hw);
+
+	if (!clk)
+		return;
+
+	if ((clk->reg_flags == JH7110_CLK_ISP_FLAG) || (clk->reg_flags == JH7110_CLK_VOUT_FLAG))
+		return;
+
+	if (clk->idx >= JH7110_CLK_REG_END)
+		return;
+
+	jh7110_clk_reg_rmw(clk, JH7110_CLK_ENABLE, clk->saved_reg_value);
+
+	return;
+}
+
+static void jh7110_clk_div_restore_context(struct clk_hw *hw)
+{
+	struct jh7110_clk *clk = jh7110_clk_from(hw);
+
+	if (!clk)
+		return;
+
+	if ((clk->reg_flags == JH7110_CLK_ISP_FLAG) || (clk->reg_flags == JH7110_CLK_VOUT_FLAG))
+		return;
+
+	if (clk->idx >= JH7110_CLK_REG_END)
+		return;
+
+	jh7110_clk_reg_rmw(clk, JH7110_CLK_DIV_MASK, clk->saved_reg_value);
+
+	return;
+}
+
+static void jh7110_clk_mux_restore_context(struct clk_hw *hw)
+{
+	struct jh7110_clk *clk = jh7110_clk_from(hw);
+
+	if (!clk)
+		return;
+
+	if ((clk->reg_flags == JH7110_CLK_ISP_FLAG) || (clk->reg_flags == JH7110_CLK_VOUT_FLAG))
+		return;
+
+	if (clk->idx >= JH7110_CLK_REG_END)
+		return;
+
+	jh7110_clk_reg_rmw(clk, JH7110_CLK_MUX_MASK, clk->saved_reg_value);
+
+	return;
+}
+
+static void jh7110_clk_inv_restore_context(struct clk_hw *hw)
+{
+	struct jh7110_clk *clk = jh7110_clk_from(hw);
+
+	if (!clk)
+		return;
+
+	if ((clk->reg_flags == JH7110_CLK_ISP_FLAG) || (clk->reg_flags == JH7110_CLK_VOUT_FLAG))
+		return;
+
+	if (clk->idx >= JH7110_CLK_REG_END)
+		return;
+
+	jh7110_clk_reg_rmw(clk, JH7110_CLK_INVERT, clk->saved_reg_value);
+
+	return;
+}
+
+static void jh7110_clk_gdiv_restore_context(struct clk_hw *hw)
+{
+	jh7110_clk_div_restore_context(hw);
+	jh7110_clk_gate_restore_context(hw);
+
+	return;
+}
+
+static void jh7110_clk_gmux_restore_context(struct clk_hw *hw)
+{
+	jh7110_clk_mux_restore_context(hw);
+	jh7110_clk_gate_restore_context(hw);
+
+	return;
+}
+
+static void jh7110_clk_mdiv_restore_context(struct clk_hw *hw)
+{
+	jh7110_clk_mux_restore_context(hw);
+	jh7110_clk_div_restore_context(hw);
+
+	return;
+}
+
+static void jh7110_clk_gmd_restore_context(struct clk_hw *hw)
+{
+	jh7110_clk_mux_restore_context(hw);
+	jh7110_clk_div_restore_context(hw);
+	jh7110_clk_gate_restore_context(hw);
+
+	return;
+}
+
+#endif
+
 static const struct clk_ops jh7110_clk_gate_ops = {
 	.enable = jh7110_clk_enable,
 	.disable = jh7110_clk_disable,
 	.is_enabled = jh7110_clk_is_enabled,
 	.debug_init = jh7110_clk_debug_init,
+#ifdef CONFIG_PM_SLEEP
+	.save_context = jh7110_clk_save_context,
+	.restore_context = jh7110_clk_gate_restore_context,
+#endif
 };
 
 static const struct clk_ops jh7110_clk_div_ops = {
@@ -251,6 +385,10 @@ static const struct clk_ops jh7110_clk_div_ops = {
 	.determine_rate = jh7110_clk_determine_rate,
 	.set_rate = jh7110_clk_set_rate,
 	.debug_init = jh7110_clk_debug_init,
+#ifdef CONFIG_PM_SLEEP
+	.save_context = jh7110_clk_save_context,
+	.restore_context = jh7110_clk_div_restore_context,
+#endif
 };
 
 static const struct clk_ops jh7110_clk_gdiv_ops = {
@@ -261,6 +399,10 @@ static const struct clk_ops jh7110_clk_gdiv_ops = {
 	.determine_rate = jh7110_clk_determine_rate,
 	.set_rate = jh7110_clk_set_rate,
 	.debug_init = jh7110_clk_debug_init,
+#ifdef CONFIG_PM_SLEEP
+	.save_context = jh7110_clk_save_context,
+	.restore_context = jh7110_clk_gdiv_restore_context,
+#endif
 };
 
 static const struct clk_ops jh7110_clk_mux_ops = {
@@ -268,6 +410,10 @@ static const struct clk_ops jh7110_clk_mux_ops = {
 	.set_parent = jh7110_clk_set_parent,
 	.get_parent = jh7110_clk_get_parent,
 	.debug_init = jh7110_clk_debug_init,
+#ifdef CONFIG_PM_SLEEP
+	.save_context = jh7110_clk_save_context,
+	.restore_context = jh7110_clk_mux_restore_context,
+#endif
 };
 
 static const struct clk_ops jh7110_clk_gmux_ops = {
@@ -278,6 +424,10 @@ static const struct clk_ops jh7110_clk_gmux_ops = {
 	.set_parent = jh7110_clk_set_parent,
 	.get_parent = jh7110_clk_get_parent,
 	.debug_init = jh7110_clk_debug_init,
+#ifdef CONFIG_PM_SLEEP
+	.save_context = jh7110_clk_save_context,
+	.restore_context = jh7110_clk_gmux_restore_context,
+#endif
 };
 
 static const struct clk_ops jh7110_clk_mdiv_ops = {
@@ -287,6 +437,10 @@ static const struct clk_ops jh7110_clk_mdiv_ops = {
 	.set_parent = jh7110_clk_set_parent,
 	.set_rate = jh7110_clk_set_rate,
 	.debug_init = jh7110_clk_debug_init,
+#ifdef CONFIG_PM_SLEEP
+	.save_context = jh7110_clk_save_context,
+	.restore_context = jh7110_clk_mdiv_restore_context,
+#endif
 };
 
 static const struct clk_ops jh7110_clk_gmd_ops = {
@@ -299,12 +453,20 @@ static const struct clk_ops jh7110_clk_gmd_ops = {
 	.set_parent = jh7110_clk_set_parent,
 	.set_rate = jh7110_clk_set_rate,
 	.debug_init = jh7110_clk_debug_init,
+#ifdef CONFIG_PM_SLEEP
+	.save_context = jh7110_clk_save_context,
+	.restore_context = jh7110_clk_gmd_restore_context,
+#endif
 };
 
 static const struct clk_ops jh7110_clk_inv_ops = {
 	.get_phase = jh7110_clk_get_phase,
 	.set_phase = jh7110_clk_set_phase,
 	.debug_init = jh7110_clk_debug_init,
+#ifdef CONFIG_PM_SLEEP
+	.save_context = jh7110_clk_save_context,
+	.restore_context = jh7110_clk_inv_restore_context,
+#endif
 };
 
 const struct clk_ops *starfive_jh7110_clk_ops(u32 max)
@@ -335,6 +497,25 @@ const struct clk_ops *starfive_jh7110_clk_ops(u32 max)
 }
 EXPORT_SYMBOL_GPL(starfive_jh7110_clk_ops);
 
+#ifdef CONFIG_PM_SLEEP
+static int clk_starfive_jh7110_gen_system_suspend(struct device *dev)
+{
+	return clk_save_context();
+}
+
+static int clk_starfive_jh7110_gen_system_resume(struct device *dev)
+{
+	clk_restore_context();
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops clk_starfive_jh7110_gen_pm_ops = {
+	SET_LATE_SYSTEM_SLEEP_PM_OPS(clk_starfive_jh7110_gen_system_suspend,
+				     clk_starfive_jh7110_gen_system_resume)
+};
+
 static struct clk_hw *jh7110_clk_get(struct of_phandle_args *clkspec,
 						void *data)
 {
@@ -369,6 +550,8 @@ static int __init clk_starfive_jh7110_probe(struct platform_device *pdev)
 	spin_lock_init(&priv->rmw_lock);
 	priv->dev = &pdev->dev;
 
+	pm_runtime_enable(priv->dev);
+
 #ifdef CONFIG_CLK_STARFIVE_JH7110_PLL
 	ret = clk_starfive_jh7110_pll_init(pdev, priv->pll_priv);
 	if (ret)
@@ -439,6 +622,7 @@ static struct platform_driver clk_starfive_jh7110_driver = {
 	.driver = {
 		.name = "clk-starfive-jh7110",
 		.of_match_table = clk_starfive_jh7110_match,
+		.pm = &clk_starfive_jh7110_gen_pm_ops,
 	},
 };
 builtin_platform_driver_probe(clk_starfive_jh7110_driver,
diff --git a/drivers/clk/starfive/clk-starfive-jh7110.h b/drivers/clk/starfive/clk-starfive-jh7110.h
index 4560b75f52c9..c776ab38a7b3 100755..100644
--- a/drivers/clk/starfive/clk-starfive-jh7110.h
+++ b/drivers/clk/starfive/clk-starfive-jh7110.h
@@ -59,6 +59,7 @@ struct jh7110_clk {
 	unsigned int idx;
 	unsigned int max_div;
 	unsigned int reg_flags;
+	u32 saved_reg_value;
 };
 
 struct jh7110_clk_priv {