Merge branches 'clk-leak', 'clk-rockchip', 'clk-renesas' and 'clk-at91' into clk-next

- Clock power management for new SAMA7G5 SoC - Updates to the master clock driver and sam9x60-pll to be able to use cpufreq-dt driver and avoid overclocking of CPU and MCK0 domains while changing the frequency via DVFS - Power management refinement with the use of save_context()/restore_context() on each clock driver to specify their use in case of Backup mode only * clk-leak: clk: mvebu: ap-cpu-clk: Fix a memory leak in error handling paths * clk-rockchip: clk: rockchip: use module_platform_driver_probe clk: rockchip: rk3399: expose PCLK_COREDBG_{B,L} clk: rockchip: rk3399: make CPU clocks critical * clk-renesas: clk: renesas: r8a779[56]x: Add MLP clocks clk: renesas: r9a07g044: Add SDHI clock and reset entries clk: renesas: rzg2l: Add SDHI clk mux support clk: renesas: r8a779a0: Add RPC support clk: renesas: cpg-lib: Move RPC clock registration to the library clk: renesas: r9a07g044: Add clock and reset entries for SPI Multi I/O Bus Controller clk: renesas: r8a779a0: Add Z0 and Z1 clock support clk: renesas: r9a07g044: Add GbEthernet clock/reset clk: renesas: rzg2l: Add support to handle coupled clocks clk: renesas: r9a07g044: Add ethernet clock sources clk: renesas: rzg2l: Add support to handle MUX clocks clk: renesas: r8a779a0: Add TPU clock clk: renesas: rzg2l: Fix clk status function clk: renesas: r9a07g044: Mark IA55_CLK and DMAC_ACLK critical * clk-at91: clk: at91: sama7g5: set low limit for mck0 at 32KHz clk: at91: sama7g5: remove prescaler part of master clock clk: at91: clk-master: add notifier for divider clk: at91: clk-sam9x60-pll: add notifier for div part of PLL clk: at91: clk-master: fix prescaler logic clk: at91: clk-master: mask mckr against layout->mask clk: at91: clk-master: check if div or pres is zero clk: at91: sam9x60-pll: use DIV_ROUND_CLOSEST_ULL clk: at91: pmc: add sama7g5 to the list of available pmcs clk: at91: clk-master: improve readability by using local variables clk: at91: clk-master: add register definition for sama7g5's master clock clk: at91: sama7g5: add securam's peripheral clock clk: at91: pmc: execute suspend/resume only for backup mode clk: at91: re-factor clocks suspend/resume clk: at91: check pmc node status before registering syscore ops

Merge branches 'clk-leak', 'clk-rockchip', 'clk-renesas' and 'clk-at91' into clk-next
- Clock power management for new SAMA7G5 SoC - Updates to the master clock driver and sam9x60-pll to be able to use cpufreq-dt driver and avoid overclocking of CPU and MCK0 domains while changing the frequency via DVFS - Power management refinement with the use of save_context()/restore_context() on each clock driver to specify their use in case of Backup mode only * clk-leak: clk: mvebu: ap-cpu-clk: Fix a memory leak in error handling paths * clk-rockchip: clk: rockchip: use module_platform_driver_probe clk: rockchip: rk3399: expose PCLK_COREDBG_{B,L} clk: rockchip: rk3399: make CPU clocks critical * clk-renesas: clk: renesas: r8a779[56]x: Add MLP clocks clk: renesas: r9a07g044: Add SDHI clock and reset entries clk: renesas: rzg2l: Add SDHI clk mux support clk: renesas: r8a779a0: Add RPC support clk: renesas: cpg-lib: Move RPC clock registration to the library clk: renesas: r9a07g044: Add clock and reset entries for SPI Multi I/O Bus Controller clk: renesas: r8a779a0: Add Z0 and Z1 clock support clk: renesas: r9a07g044: Add GbEthernet clock/reset clk: renesas: rzg2l: Add support to handle coupled clocks clk: renesas: r9a07g044: Add ethernet clock sources clk: renesas: rzg2l: Add support to handle MUX clocks clk: renesas: r8a779a0: Add TPU clock clk: renesas: rzg2l: Fix clk status function clk: renesas: r9a07g044: Mark IA55_CLK and DMAC_ACLK critical * clk-at91: clk: at91: sama7g5: set low limit for mck0 at 32KHz clk: at91: sama7g5: remove prescaler part of master clock clk: at91: clk-master: add notifier for divider clk: at91: clk-sam9x60-pll: add notifier for div part of PLL clk: at91: clk-master: fix prescaler logic clk: at91: clk-master: mask mckr against layout->mask clk: at91: clk-master: check if div or pres is zero clk: at91: sam9x60-pll: use DIV_ROUND_CLOSEST_ULL clk: at91: pmc: add sama7g5 to the list of available pmcs clk: at91: clk-master: improve readability by using local variables clk: at91: clk-master: add register definition for sama7g5's master clock clk: at91: sama7g5: add securam's peripheral clock clk: at91: pmc: execute suspend/resume only for backup mode clk: at91: re-factor clocks suspend/resume clk: at91: check pmc node status before registering syscore ops
b43e2d55 · Stephen Boyd · a379e16a · af9617b4 · 8d27b147 · bada0389
Commit b43e2d55 authored Nov 02, 2021 by Stephen Boyd
37 changed files
--- a/drivers/clk/at91/at91rm9200.c
+++ b/drivers/clk/at91/at91rm9200.c
@@ -152,7 +152,7 @@ static void __init at91rm9200_pmc_setup(struct device_node *np)
 					  "masterck_pres",
 					  &at91rm9200_master_layout,
 					  &rm9200_mck_characteristics,
-					  &rm9200_mck_lock, CLK_SET_RATE_GATE);
+					  &rm9200_mck_lock, CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto err_free;


--- a/drivers/clk/at91/at91sam9260.c
+++ b/drivers/clk/at91/at91sam9260.c
@@ -429,7 +429,7 @@ static void __init at91sam926x_pmc_setup(struct device_node *np,
 					  &at91rm9200_master_layout,
 					  data->mck_characteristics,
 					  &at91sam9260_mck_lock,
-					  CLK_SET_RATE_GATE);
+					  CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto err_free;


--- a/drivers/clk/at91/at91sam9g45.c
+++ b/drivers/clk/at91/at91sam9g45.c
@@ -164,7 +164,7 @@ static void __init at91sam9g45_pmc_setup(struct device_node *np)
 					  &at91rm9200_master_layout,
 					  &mck_characteristics,
 					  &at91sam9g45_mck_lock,
-					  CLK_SET_RATE_GATE);
+					  CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto err_free;


--- a/drivers/clk/at91/at91sam9n12.c
+++ b/drivers/clk/at91/at91sam9n12.c
@@ -191,7 +191,7 @@ static void __init at91sam9n12_pmc_setup(struct device_node *np)
 					  &at91sam9x5_master_layout,
 					  &mck_characteristics,
 					  &at91sam9n12_mck_lock,
-					  CLK_SET_RATE_GATE);
+					  CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto err_free;


--- a/drivers/clk/at91/at91sam9rl.c
+++ b/drivers/clk/at91/at91sam9rl.c
@@ -132,7 +132,7 @@ static void __init at91sam9rl_pmc_setup(struct device_node *np)
 					  "masterck_pres",
 					  &at91rm9200_master_layout,
 					  &sam9rl_mck_characteristics,
-					  &sam9rl_mck_lock, CLK_SET_RATE_GATE);
+					  &sam9rl_mck_lock, CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto err_free;


--- a/drivers/clk/at91/at91sam9x5.c
+++ b/drivers/clk/at91/at91sam9x5.c
@@ -210,7 +210,7 @@ static void __init at91sam9x5_pmc_setup(struct device_node *np,
 					  "masterck_pres",
 					  &at91sam9x5_master_layout,
 					  &mck_characteristics, &mck_lock,
-					  CLK_SET_RATE_GATE);
+					  CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto err_free;


--- a/drivers/clk/at91/clk-generated.c
+++ b/drivers/clk/at91/clk-generated.c
@@ -27,6 +27,7 @@ struct clk_generated {
 	u32 id;
 	u32 gckdiv;
 	const struct clk_pcr_layout *layout;
+	struct at91_clk_pms pms;
 	u8 parent_id;
 	int chg_pid;
 };
@@ -34,25 +35,35 @@ struct clk_generated {
 #define to_clk_generated(hw) \
 	container_of(hw, struct clk_generated, hw)

-static int clk_generated_enable(struct clk_hw *hw)
+static int clk_generated_set(struct clk_generated *gck, int status)
 {
-	struct clk_generated *gck = to_clk_generated(hw);
 	unsigned long flags;
-
-	pr_debug("GCLK: %s, gckdiv = %d, parent id = %d\n",
-		 __func__, gck->gckdiv, gck->parent_id);
+	unsigned int enable = status ? AT91_PMC_PCR_GCKEN : 0;

 	spin_lock_irqsave(gck->lock, flags);
 	regmap_write(gck->regmap, gck->layout->offset,
 		     (gck->id & gck->layout->pid_mask));
 	regmap_update_bits(gck->regmap, gck->layout->offset,
 			   AT91_PMC_PCR_GCKDIV_MASK | gck->layout->gckcss_mask |
-			   gck->layout->cmd | AT91_PMC_PCR_GCKEN,
+			   gck->layout->cmd | enable,
 			   field_prep(gck->layout->gckcss_mask, gck->parent_id) |
 			   gck->layout->cmd |
 			   FIELD_PREP(AT91_PMC_PCR_GCKDIV_MASK, gck->gckdiv) |
-			   AT91_PMC_PCR_GCKEN);
+			   enable);
 	spin_unlock_irqrestore(gck->lock, flags);
+
+	return 0;
+}
+
+static int clk_generated_enable(struct clk_hw *hw)
+{
+	struct clk_generated *gck = to_clk_generated(hw);
+
+	pr_debug("GCLK: %s, gckdiv = %d, parent id = %d\n",
+		 __func__, gck->gckdiv, gck->parent_id);
+
+	clk_generated_set(gck, 1);
+
 	return 0;
 }

@@ -245,6 +256,23 @@ static int clk_generated_set_rate(struct clk_hw *hw,
 	return 0;
 }

+static int clk_generated_save_context(struct clk_hw *hw)
+{
+	struct clk_generated *gck = to_clk_generated(hw);
+
+	gck->pms.status = clk_generated_is_enabled(&gck->hw);
+
+	return 0;
+}
+
+static void clk_generated_restore_context(struct clk_hw *hw)
+{
+	struct clk_generated *gck = to_clk_generated(hw);
+
+	if (gck->pms.status)
+		clk_generated_set(gck, gck->pms.status);
+}
+
 static const struct clk_ops generated_ops = {
 	.enable = clk_generated_enable,
 	.disable = clk_generated_disable,
@@ -254,6 +282,8 @@ static const struct clk_ops generated_ops = {
 	.get_parent = clk_generated_get_parent,
 	.set_parent = clk_generated_set_parent,
 	.set_rate = clk_generated_set_rate,
+	.save_context = clk_generated_save_context,
+	.restore_context = clk_generated_restore_context,
 };

 /**
@@ -320,8 +350,6 @@ at91_clk_register_generated(struct regmap *regmap, spinlock_t *lock,
 	if (ret) {
 		kfree(gck);
 		hw = ERR_PTR(ret);
-	} else {
-		pmc_register_id(id);
 	}

 	return hw;

--- a/drivers/clk/at91/clk-main.c
+++ b/drivers/clk/at91/clk-main.c
@@ -28,6 +28,7 @@
 struct clk_main_osc {
 	struct clk_hw hw;
 	struct regmap *regmap;
+	struct at91_clk_pms pms;
 };

 #define to_clk_main_osc(hw) container_of(hw, struct clk_main_osc, hw)
@@ -37,6 +38,7 @@ struct clk_main_rc_osc {
 	struct regmap *regmap;
 	unsigned long frequency;
 	unsigned long accuracy;
+	struct at91_clk_pms pms;
 };

 #define to_clk_main_rc_osc(hw) container_of(hw, struct clk_main_rc_osc, hw)
@@ -51,6 +53,7 @@ struct clk_rm9200_main {
 struct clk_sam9x5_main {
 	struct clk_hw hw;
 	struct regmap *regmap;
+	struct at91_clk_pms pms;
 	u8 parent;
 };

@@ -120,10 +123,29 @@ static int clk_main_osc_is_prepared(struct clk_hw *hw)
 	return (status & AT91_PMC_MOSCS) && clk_main_parent_select(tmp);
 }

+static int clk_main_osc_save_context(struct clk_hw *hw)
+{
+	struct clk_main_osc *osc = to_clk_main_osc(hw);
+
+	osc->pms.status = clk_main_osc_is_prepared(hw);
+
+	return 0;
+}
+
+static void clk_main_osc_restore_context(struct clk_hw *hw)
+{
+	struct clk_main_osc *osc = to_clk_main_osc(hw);
+
+	if (osc->pms.status)
+		clk_main_osc_prepare(hw);
+}
+
 static const struct clk_ops main_osc_ops = {
 	.prepare = clk_main_osc_prepare,
 	.unprepare = clk_main_osc_unprepare,
 	.is_prepared = clk_main_osc_is_prepared,
+	.save_context = clk_main_osc_save_context,
+	.restore_context = clk_main_osc_restore_context,
 };

 struct clk_hw * __init
@@ -240,12 +262,31 @@ static unsigned long clk_main_rc_osc_recalc_accuracy(struct clk_hw *hw,
 	return osc->accuracy;
 }

+static int clk_main_rc_osc_save_context(struct clk_hw *hw)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+
+	osc->pms.status = clk_main_rc_osc_is_prepared(hw);
+
+	return 0;
+}
+
+static void clk_main_rc_osc_restore_context(struct clk_hw *hw)
+{
+	struct clk_main_rc_osc *osc = to_clk_main_rc_osc(hw);
+
+	if (osc->pms.status)
+		clk_main_rc_osc_prepare(hw);
+}
+
 static const struct clk_ops main_rc_osc_ops = {
 	.prepare = clk_main_rc_osc_prepare,
 	.unprepare = clk_main_rc_osc_unprepare,
 	.is_prepared = clk_main_rc_osc_is_prepared,
 	.recalc_rate = clk_main_rc_osc_recalc_rate,
 	.recalc_accuracy = clk_main_rc_osc_recalc_accuracy,
+	.save_context = clk_main_rc_osc_save_context,
+	.restore_context = clk_main_rc_osc_restore_context,
 };

 struct clk_hw * __init
@@ -465,12 +506,37 @@ static u8 clk_sam9x5_main_get_parent(struct clk_hw *hw)
 	return clk_main_parent_select(status);
 }

+static int clk_sam9x5_main_save_context(struct clk_hw *hw)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
+
+	clkmain->pms.status = clk_main_rc_osc_is_prepared(&clkmain->hw);
+	clkmain->pms.parent = clk_sam9x5_main_get_parent(&clkmain->hw);
+
+	return 0;
+}
+
+static void clk_sam9x5_main_restore_context(struct clk_hw *hw)
+{
+	struct clk_sam9x5_main *clkmain = to_clk_sam9x5_main(hw);
+	int ret;
+
+	ret = clk_sam9x5_main_set_parent(hw, clkmain->pms.parent);
+	if (ret)
+		return;
+
+	if (clkmain->pms.status)
+		clk_sam9x5_main_prepare(hw);
+}
+
 static const struct clk_ops sam9x5_main_ops = {
 	.prepare = clk_sam9x5_main_prepare,
 	.is_prepared = clk_sam9x5_main_is_prepared,
 	.recalc_rate = clk_sam9x5_main_recalc_rate,
 	.set_parent = clk_sam9x5_main_set_parent,
 	.get_parent = clk_sam9x5_main_get_parent,
+	.save_context = clk_sam9x5_main_save_context,
+	.restore_context = clk_sam9x5_main_restore_context,
 };

 struct clk_hw * __init

--- a/drivers/clk/at91/clk-master.c
+++ b/drivers/clk/at91/clk-master.c
--- a/drivers/clk/at91/clk-peripheral.c
+++ b/drivers/clk/at91/clk-peripheral.c
@@ -37,6 +37,7 @@ struct clk_sam9x5_peripheral {
 	u32 id;
 	u32 div;
 	const struct clk_pcr_layout *layout;
+	struct at91_clk_pms pms;
 	bool auto_div;
 	int chg_pid;
 };
@@ -155,10 +156,11 @@ static void clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral *periph)
 	periph->div = shift;
 }

-static int clk_sam9x5_peripheral_enable(struct clk_hw *hw)
+static int clk_sam9x5_peripheral_set(struct clk_sam9x5_peripheral *periph,
+				     unsigned int status)
 {
-	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 	unsigned long flags;
+	unsigned int enable = status ? AT91_PMC_PCR_EN : 0;

 	if (periph->id < PERIPHERAL_ID_MIN)
 		return 0;
@@ -168,15 +170,21 @@ static int clk_sam9x5_peripheral_enable(struct clk_hw *hw)
 		     (periph->id & periph->layout->pid_mask));
 	regmap_update_bits(periph->regmap, periph->layout->offset,
 			   periph->layout->div_mask | periph->layout->cmd |
-			   AT91_PMC_PCR_EN,
+			   enable,
 			   field_prep(periph->layout->div_mask, periph->div) |
-			   periph->layout->cmd |
-			   AT91_PMC_PCR_EN);
+			   periph->layout->cmd | enable);
 	spin_unlock_irqrestore(periph->lock, flags);

 	return 0;
 }

+static int clk_sam9x5_peripheral_enable(struct clk_hw *hw)
+{
+	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
+
+	return clk_sam9x5_peripheral_set(periph, 1);
+}
+
 static void clk_sam9x5_peripheral_disable(struct clk_hw *hw)
 {
 	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
@@ -393,6 +401,23 @@ static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw,
 	return -EINVAL;
 }

+static int clk_sam9x5_peripheral_save_context(struct clk_hw *hw)
+{
+	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
+
+	periph->pms.status = clk_sam9x5_peripheral_is_enabled(hw);
+
+	return 0;
+}
+
+static void clk_sam9x5_peripheral_restore_context(struct clk_hw *hw)
+{
+	struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
+
+	if (periph->pms.status)
+		clk_sam9x5_peripheral_set(periph, periph->pms.status);
+}
+
 static const struct clk_ops sam9x5_peripheral_ops = {
 	.enable = clk_sam9x5_peripheral_enable,
 	.disable = clk_sam9x5_peripheral_disable,
@@ -400,6 +425,8 @@ static const struct clk_ops sam9x5_peripheral_ops = {
 	.recalc_rate = clk_sam9x5_peripheral_recalc_rate,
 	.round_rate = clk_sam9x5_peripheral_round_rate,
 	.set_rate = clk_sam9x5_peripheral_set_rate,
+	.save_context = clk_sam9x5_peripheral_save_context,
+	.restore_context = clk_sam9x5_peripheral_restore_context,
 };

 static const struct clk_ops sam9x5_peripheral_chg_ops = {
@@ -409,6 +436,8 @@ static const struct clk_ops sam9x5_peripheral_chg_ops = {
 	.recalc_rate = clk_sam9x5_peripheral_recalc_rate,
 	.determine_rate = clk_sam9x5_peripheral_determine_rate,
 	.set_rate = clk_sam9x5_peripheral_set_rate,
+	.save_context = clk_sam9x5_peripheral_save_context,
+	.restore_context = clk_sam9x5_peripheral_restore_context,
 };

 struct clk_hw * __init
@@ -460,7 +489,6 @@ at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
 		hw = ERR_PTR(ret);
 	} else {
 		clk_sam9x5_peripheral_autodiv(periph);
-		pmc_register_id(id);
 	}

 	return hw;

--- a/drivers/clk/at91/clk-pll.c
+++ b/drivers/clk/at91/clk-pll.c
@@ -40,6 +40,7 @@ struct clk_pll {
 	u16 mul;
 	const struct clk_pll_layout *layout;
 	const struct clk_pll_characteristics *characteristics;
+	struct at91_clk_pms pms;
 };

 static inline bool clk_pll_ready(struct regmap *regmap, int id)
@@ -260,6 +261,42 @@ static int clk_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }

+static int clk_pll_save_context(struct clk_hw *hw)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+	struct clk_hw *parent_hw = clk_hw_get_parent(hw);
+
+	pll->pms.parent_rate = clk_hw_get_rate(parent_hw);
+	pll->pms.rate = clk_pll_recalc_rate(&pll->hw, pll->pms.parent_rate);
+	pll->pms.status = clk_pll_ready(pll->regmap, PLL_REG(pll->id));
+
+	return 0;
+}
+
+static void clk_pll_restore_context(struct clk_hw *hw)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+	unsigned long calc_rate;
+	unsigned int pllr, pllr_out, pllr_count;
+	u8 out = 0;
+
+	if (pll->characteristics->out)
+		out = pll->characteristics->out[pll->range];
+
+	regmap_read(pll->regmap, PLL_REG(pll->id), &pllr);
+
+	calc_rate = (pll->pms.parent_rate / PLL_DIV(pllr)) *
+		     (PLL_MUL(pllr, pll->layout) + 1);
+	pllr_count = (pllr >> PLL_COUNT_SHIFT) & PLL_MAX_COUNT;
+	pllr_out = (pllr >> PLL_OUT_SHIFT) & out;
+
+	if (pll->pms.rate != calc_rate ||
+	    pll->pms.status != clk_pll_ready(pll->regmap, PLL_REG(pll->id)) ||
+	    pllr_count != PLL_MAX_COUNT ||
+	    (out && pllr_out != out))
+		pr_warn("PLLAR was not configured properly by firmware\n");
+}
+
 static const struct clk_ops pll_ops = {
 	.prepare = clk_pll_prepare,
 	.unprepare = clk_pll_unprepare,
@@ -267,6 +304,8 @@ static const struct clk_ops pll_ops = {
 	.recalc_rate = clk_pll_recalc_rate,
 	.round_rate = clk_pll_round_rate,
 	.set_rate = clk_pll_set_rate,
+	.save_context = clk_pll_save_context,
+	.restore_context = clk_pll_restore_context,
 };

 struct clk_hw * __init

--- a/drivers/clk/at91/clk-programmable.c
+++ b/drivers/clk/at91/clk-programmable.c
@@ -24,6 +24,7 @@ struct clk_programmable {
 	u32 *mux_table;
 	u8 id;
 	const struct clk_programmable_layout *layout;
+	struct at91_clk_pms pms;
 };

 #define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw)
@@ -177,12 +178,38 @@ static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }

+static int clk_programmable_save_context(struct clk_hw *hw)
+{
+	struct clk_programmable *prog = to_clk_programmable(hw);
+	struct clk_hw *parent_hw = clk_hw_get_parent(hw);
+
+	prog->pms.parent = clk_programmable_get_parent(hw);
+	prog->pms.parent_rate = clk_hw_get_rate(parent_hw);
+	prog->pms.rate = clk_programmable_recalc_rate(hw, prog->pms.parent_rate);
+
+	return 0;
+}
+
+static void clk_programmable_restore_context(struct clk_hw *hw)
+{
+	struct clk_programmable *prog = to_clk_programmable(hw);
+	int ret;
+
+	ret = clk_programmable_set_parent(hw, prog->pms.parent);
+	if (ret)
+		return;
+
+	clk_programmable_set_rate(hw, prog->pms.rate, prog->pms.parent_rate);
+}
+
 static const struct clk_ops programmable_ops = {
 	.recalc_rate = clk_programmable_recalc_rate,
 	.determine_rate = clk_programmable_determine_rate,
 	.get_parent = clk_programmable_get_parent,
 	.set_parent = clk_programmable_set_parent,
 	.set_rate = clk_programmable_set_rate,
+	.save_context = clk_programmable_save_context,
+	.restore_context = clk_programmable_restore_context,
 };

 struct clk_hw * __init
@@ -221,8 +248,6 @@ at91_clk_register_programmable(struct regmap *regmap,
 	if (ret) {
 		kfree(prog);
 		hw = ERR_PTR(ret);
-	} else {
-		pmc_register_pck(id);
 	}

 	return hw;

--- a/drivers/clk/at91/clk-sam9x60-pll.c
+++ b/drivers/clk/at91/clk-sam9x60-pll.c
@@ -5,6 +5,7 @@
 */

 #include <linux/bitfield.h>
+#include <linux/clk.h>
 #include <linux/clk-provider.h>
 #include <linux/clkdev.h>
 #include <linux/clk/at91_pmc.h>
@@ -38,19 +39,24 @@ struct sam9x60_pll_core {

 struct sam9x60_frac {
 	struct sam9x60_pll_core core;
+	struct at91_clk_pms pms;
 	u32 frac;
 	u16 mul;
 };

 struct sam9x60_div {
 	struct sam9x60_pll_core core;
+	struct at91_clk_pms pms;
 	u8 div;
+	u8 safe_div;
 };

 #define to_sam9x60_pll_core(hw)	container_of(hw, struct sam9x60_pll_core, hw)
 #define to_sam9x60_frac(core)	container_of(core, struct sam9x60_frac, core)
 #define to_sam9x60_div(core)	container_of(core, struct sam9x60_div, core)

+static struct sam9x60_div *notifier_div;
+
 static inline bool sam9x60_pll_ready(struct regmap *regmap, int id)
 {
 	unsigned int status;
@@ -71,13 +77,12 @@ static unsigned long sam9x60_frac_pll_recalc_rate(struct clk_hw *hw,
 	struct sam9x60_pll_core *core = to_sam9x60_pll_core(hw);
 	struct sam9x60_frac *frac = to_sam9x60_frac(core);

-	return (parent_rate * (frac->mul + 1) +
-		((u64)parent_rate * frac->frac >> 22));
+	return parent_rate * (frac->mul + 1) +
+		DIV_ROUND_CLOSEST_ULL((u64)parent_rate * frac->frac, (1 << 22));
 }

-static int sam9x60_frac_pll_prepare(struct clk_hw *hw)
+static int sam9x60_frac_pll_set(struct sam9x60_pll_core *core)
 {
-	struct sam9x60_pll_core *core = to_sam9x60_pll_core(hw);
 	struct sam9x60_frac *frac = to_sam9x60_frac(core);
 	struct regmap *regmap = core->regmap;
 	unsigned int val, cfrac, cmul;
@@ -141,6 +146,13 @@ static int sam9x60_frac_pll_prepare(struct clk_hw *hw)
 	return 0;
 }

+static int sam9x60_frac_pll_prepare(struct clk_hw *hw)
+{
+	struct sam9x60_pll_core *core = to_sam9x60_pll_core(hw);
+
+	return sam9x60_frac_pll_set(core);
+}
+
 static void sam9x60_frac_pll_unprepare(struct clk_hw *hw)
 {
 	struct sam9x60_pll_core *core = to_sam9x60_pll_core(hw);
@@ -280,6 +292,25 @@ static int sam9x60_frac_pll_set_rate_chg(struct clk_hw *hw, unsigned long rate,
 	return ret;
 }

+static int sam9x60_frac_pll_save_context(struct clk_hw *hw)
+{
+	struct sam9x60_pll_core *core = to_sam9x60_pll_core(hw);
+	struct sam9x60_frac *frac = to_sam9x60_frac(core);
+
+	frac->pms.status = sam9x60_pll_ready(core->regmap, core->id);
+
+	return 0;
+}
+
+static void sam9x60_frac_pll_restore_context(struct clk_hw *hw)
+{
+	struct sam9x60_pll_core *core = to_sam9x60_pll_core(hw);
+	struct sam9x60_frac *frac = to_sam9x60_frac(core);
+
+	if (frac->pms.status)
+		sam9x60_frac_pll_set(core);
+}
+
 static const struct clk_ops sam9x60_frac_pll_ops = {
 	.prepare = sam9x60_frac_pll_prepare,
 	.unprepare = sam9x60_frac_pll_unprepare,
@@ -287,6 +318,8 @@ static const struct clk_ops sam9x60_frac_pll_ops = {
 	.recalc_rate = sam9x60_frac_pll_recalc_rate,
 	.round_rate = sam9x60_frac_pll_round_rate,
 	.set_rate = sam9x60_frac_pll_set_rate,
+	.save_context = sam9x60_frac_pll_save_context,
+	.restore_context = sam9x60_frac_pll_restore_context,
 };

 static const struct clk_ops sam9x60_frac_pll_ops_chg = {
@@ -296,11 +329,32 @@ static const struct clk_ops sam9x60_frac_pll_ops_chg = {
 	.recalc_rate = sam9x60_frac_pll_recalc_rate,
 	.round_rate = sam9x60_frac_pll_round_rate,
 	.set_rate = sam9x60_frac_pll_set_rate_chg,
+	.save_context = sam9x60_frac_pll_save_context,
+	.restore_context = sam9x60_frac_pll_restore_context,
 };

-static int sam9x60_div_pll_prepare(struct clk_hw *hw)
+/* This function should be called with spinlock acquired. */
+static void sam9x60_div_pll_set_div(struct sam9x60_pll_core *core, u32 div,
+				    bool enable)
+{
+	struct regmap *regmap = core->regmap;
+	u32 ena_msk = enable ? core->layout->endiv_mask : 0;
+	u32 ena_val = enable ? (1 << core->layout->endiv_shift) : 0;
+
+	regmap_update_bits(regmap, AT91_PMC_PLL_CTRL0,
+			   core->layout->div_mask | ena_msk,
+			   (div << core->layout->div_shift) | ena_val);
+
+	regmap_update_bits(regmap, AT91_PMC_PLL_UPDT,
+			   AT91_PMC_PLL_UPDT_UPDATE | AT91_PMC_PLL_UPDT_ID_MSK,
+			   AT91_PMC_PLL_UPDT_UPDATE | core->id);
+
+	while (!sam9x60_pll_ready(regmap, core->id))
+		cpu_relax();
+}
+
+static int sam9x60_div_pll_set(struct sam9x60_pll_core *core)
 {
-	struct sam9x60_pll_core *core = to_sam9x60_pll_core(hw);
 	struct sam9x60_div *div = to_sam9x60_div(core);
 	struct regmap *regmap = core->regmap;
 	unsigned long flags;
@@ -316,17 +370,7 @@ static int sam9x60_div_pll_prepare(struct clk_hw *hw)
 	if (!!(val & core->layout->endiv_mask) && cdiv == div->div)
 		goto unlock;

-	regmap_update_bits(regmap, AT91_PMC_PLL_CTRL0,
-			   core->layout->div_mask | core->layout->endiv_mask,
-			   (div->div << core->layout->div_shift) |
-			   (1 << core->layout->endiv_shift));
-
-	regmap_update_bits(regmap, AT91_PMC_PLL_UPDT,
-			   AT91_PMC_PLL_UPDT_UPDATE | AT91_PMC_PLL_UPDT_ID_MSK,
-			   AT91_PMC_PLL_UPDT_UPDATE | core->id);
-
-	while (!sam9x60_pll_ready(regmap, core->id))
-		cpu_relax();
+	sam9x60_div_pll_set_div(core, div->div, 1);

 unlock:
 	spin_unlock_irqrestore(core->lock, flags);
@@ -334,6 +378,13 @@ static int sam9x60_div_pll_prepare(struct clk_hw *hw)
 	return 0;
 }

+static int sam9x60_div_pll_prepare(struct clk_hw *hw)
+{
+	struct sam9x60_pll_core *core = to_sam9x60_pll_core(hw);
+
+	return sam9x60_div_pll_set(core);
+}
+
 static void sam9x60_div_pll_unprepare(struct clk_hw *hw)
 {
 	struct sam9x60_pll_core *core = to_sam9x60_pll_core(hw);
@@ -465,16 +516,7 @@ static int sam9x60_div_pll_set_rate_chg(struct clk_hw *hw, unsigned long rate,
 	if (cdiv == div->div)
 		goto unlock;

-	regmap_update_bits(regmap, AT91_PMC_PLL_CTRL0,
-			   core->layout->div_mask,
-			   (div->div << core->layout->div_shift));
-
-	regmap_update_bits(regmap, AT91_PMC_PLL_UPDT,
-			   AT91_PMC_PLL_UPDT_UPDATE | AT91_PMC_PLL_UPDT_ID_MSK,
-			   AT91_PMC_PLL_UPDT_UPDATE | core->id);
-
-	while (!sam9x60_pll_ready(regmap, core->id))
-		cpu_relax();
+	sam9x60_div_pll_set_div(core, div->div, 0);

 unlock:
 	spin_unlock_irqrestore(core->lock, irqflags);
@@ -482,6 +524,67 @@ static int sam9x60_div_pll_set_rate_chg(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }

+static int sam9x60_div_pll_save_context(struct clk_hw *hw)
+{
+	struct sam9x60_pll_core *core = to_sam9x60_pll_core(hw);
+	struct sam9x60_div *div = to_sam9x60_div(core);
+
+	div->pms.status = sam9x60_div_pll_is_prepared(hw);
+
+	return 0;
+}
+
+static void sam9x60_div_pll_restore_context(struct clk_hw *hw)
+{
+	struct sam9x60_pll_core *core = to_sam9x60_pll_core(hw);
+	struct sam9x60_div *div = to_sam9x60_div(core);
+
+	if (div->pms.status)
+		sam9x60_div_pll_set(core);
+}
+
+static int sam9x60_div_pll_notifier_fn(struct notifier_block *notifier,
+				       unsigned long code, void *data)
+{
+	struct sam9x60_div *div = notifier_div;
+	struct sam9x60_pll_core core = div->core;
+	struct regmap *regmap = core.regmap;
+	unsigned long irqflags;
+	u32 val, cdiv;
+	int ret = NOTIFY_DONE;
+
+	if (code != PRE_RATE_CHANGE)
+		return ret;
+
+	/*
+	 * We switch to safe divider to avoid overclocking of other domains
+	 * feed by us while the frac PLL (our parent) is changed.
+	 */
+	div->div = div->safe_div;
+
+	spin_lock_irqsave(core.lock, irqflags);
+	regmap_update_bits(regmap, AT91_PMC_PLL_UPDT, AT91_PMC_PLL_UPDT_ID_MSK,
+			   core.id);
+	regmap_read(regmap, AT91_PMC_PLL_CTRL0, &val);
+	cdiv = (val & core.layout->div_mask) >> core.layout->div_shift;
+
+	/* Stop if nothing changed. */
+	if (cdiv == div->safe_div)
+		goto unlock;
+
+	sam9x60_div_pll_set_div(&core, div->div, 0);
+	ret = NOTIFY_OK;
+
+unlock:
+	spin_unlock_irqrestore(core.lock, irqflags);
+
+	return ret;
+}
+
+static struct notifier_block sam9x60_div_pll_notifier = {
+	.notifier_call = sam9x60_div_pll_notifier_fn,
+};
+
 static const struct clk_ops sam9x60_div_pll_ops = {
 	.prepare = sam9x60_div_pll_prepare,
 	.unprepare = sam9x60_div_pll_unprepare,
@@ -489,6 +592,8 @@ static const struct clk_ops sam9x60_div_pll_ops = {
 	.recalc_rate = sam9x60_div_pll_recalc_rate,
 	.round_rate = sam9x60_div_pll_round_rate,
 	.set_rate = sam9x60_div_pll_set_rate,
+	.save_context = sam9x60_div_pll_save_context,
+	.restore_context = sam9x60_div_pll_restore_context,
 };

 static const struct clk_ops sam9x60_div_pll_ops_chg = {
@@ -498,6 +603,8 @@ static const struct clk_ops sam9x60_div_pll_ops_chg = {
 	.recalc_rate = sam9x60_div_pll_recalc_rate,
 	.round_rate = sam9x60_div_pll_round_rate,
 	.set_rate = sam9x60_div_pll_set_rate_chg,
+	.save_context = sam9x60_div_pll_save_context,
+	.restore_context = sam9x60_div_pll_restore_context,
 };

 struct clk_hw * __init
@@ -587,7 +694,8 @@ struct clk_hw * __init
 sam9x60_clk_register_div_pll(struct regmap *regmap, spinlock_t *lock,
 			     const char *name, const char *parent_name, u8 id,
 			     const struct clk_pll_characteristics *characteristics,
-			     const struct clk_pll_layout *layout, u32 flags)
+			     const struct clk_pll_layout *layout, u32 flags,
+			     u32 safe_div)
 {
 	struct sam9x60_div *div;
 	struct clk_hw *hw;
@@ -596,9 +704,13 @@ sam9x60_clk_register_div_pll(struct regmap *regmap, spinlock_t *lock,
 	unsigned int val;
 	int ret;

-	if (id > PLL_MAX_ID || !lock)
+	/* We only support one changeable PLL. */
+	if (id > PLL_MAX_ID || !lock || (safe_div && notifier_div))
 		return ERR_PTR(-EINVAL);

+	if (safe_div >= PLL_DIV_MAX)
+		safe_div = PLL_DIV_MAX - 1;
+
 	div = kzalloc(sizeof(*div), GFP_KERNEL);
 	if (!div)
 		return ERR_PTR(-ENOMEM);
@@ -618,6 +730,7 @@ sam9x60_clk_register_div_pll(struct regmap *regmap, spinlock_t *lock,
 	div->core.layout = layout;
 	div->core.regmap = regmap;
 	div->core.lock = lock;
+	div->safe_div = safe_div;

 	spin_lock_irqsave(div->core.lock, irqflags);

@@ -633,6 +746,9 @@ sam9x60_clk_register_div_pll(struct regmap *regmap, spinlock_t *lock,
 	if (ret) {
 		kfree(div);
 		hw = ERR_PTR(ret);
+	} else if (div->safe_div) {
+		notifier_div = div;
+		clk_notifier_register(hw->clk, &sam9x60_div_pll_notifier);
 	}

 	return hw;

--- a/drivers/clk/at91/clk-system.c
+++ b/drivers/clk/at91/clk-system.c
@@ -20,6 +20,7 @@
 struct clk_system {
 	struct clk_hw hw;
 	struct regmap *regmap;
+	struct at91_clk_pms pms;
 	u8 id;
 };

@@ -77,10 +78,29 @@ static int clk_system_is_prepared(struct clk_hw *hw)
 	return !!(status & (1 << sys->id));
 }

+static int clk_system_save_context(struct clk_hw *hw)
+{
+	struct clk_system *sys = to_clk_system(hw);
+
+	sys->pms.status = clk_system_is_prepared(hw);
+
+	return 0;
+}
+
+static void clk_system_restore_context(struct clk_hw *hw)
+{
+	struct clk_system *sys = to_clk_system(hw);
+
+	if (sys->pms.status)
+		clk_system_prepare(&sys->hw);
+}
+
 static const struct clk_ops system_ops = {
 	.prepare = clk_system_prepare,
 	.unprepare = clk_system_unprepare,
 	.is_prepared = clk_system_is_prepared,
+	.save_context = clk_system_save_context,
+	.restore_context = clk_system_restore_context,
 };

 struct clk_hw * __init

--- a/drivers/clk/at91/clk-usb.c
+++ b/drivers/clk/at91/clk-usb.c
@@ -24,6 +24,7 @@
 struct at91sam9x5_clk_usb {
 	struct clk_hw hw;
 	struct regmap *regmap;
+	struct at91_clk_pms pms;
 	u32 usbs_mask;
 	u8 num_parents;
 };
@@ -148,12 +149,38 @@ static int at91sam9x5_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }

+static int at91sam9x5_usb_save_context(struct clk_hw *hw)
+{
+	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
+	struct clk_hw *parent_hw = clk_hw_get_parent(hw);
+
+	usb->pms.parent = at91sam9x5_clk_usb_get_parent(hw);
+	usb->pms.parent_rate = clk_hw_get_rate(parent_hw);
+	usb->pms.rate = at91sam9x5_clk_usb_recalc_rate(hw, usb->pms.parent_rate);
+
+	return 0;
+}
+
+static void at91sam9x5_usb_restore_context(struct clk_hw *hw)
+{
+	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
+	int ret;
+
+	ret = at91sam9x5_clk_usb_set_parent(hw, usb->pms.parent);
+	if (ret)
+		return;
+
+	at91sam9x5_clk_usb_set_rate(hw, usb->pms.rate, usb->pms.parent_rate);
+}
+
 static const struct clk_ops at91sam9x5_usb_ops = {
 	.recalc_rate = at91sam9x5_clk_usb_recalc_rate,
 	.determine_rate = at91sam9x5_clk_usb_determine_rate,
 	.get_parent = at91sam9x5_clk_usb_get_parent,
 	.set_parent = at91sam9x5_clk_usb_set_parent,
 	.set_rate = at91sam9x5_clk_usb_set_rate,
+	.save_context = at91sam9x5_usb_save_context,
+	.restore_context = at91sam9x5_usb_restore_context,
 };

 static int at91sam9n12_clk_usb_enable(struct clk_hw *hw)

--- a/drivers/clk/at91/clk-utmi.c
+++ b/drivers/clk/at91/clk-utmi.c
@@ -23,6 +23,7 @@ struct clk_utmi {
 	struct clk_hw hw;
 	struct regmap *regmap_pmc;
 	struct regmap *regmap_sfr;
+	struct at91_clk_pms pms;
 };

 #define to_clk_utmi(hw) container_of(hw, struct clk_utmi, hw)
@@ -113,11 +114,30 @@ static unsigned long clk_utmi_recalc_rate(struct clk_hw *hw,
 	return UTMI_RATE;
 }

+static int clk_utmi_save_context(struct clk_hw *hw)
+{
+	struct clk_utmi *utmi = to_clk_utmi(hw);
+
+	utmi->pms.status = clk_utmi_is_prepared(hw);
+
+	return 0;
+}
+
+static void clk_utmi_restore_context(struct clk_hw *hw)
+{
+	struct clk_utmi *utmi = to_clk_utmi(hw);
+
+	if (utmi->pms.status)
+		clk_utmi_prepare(hw);
+}
+
 static const struct clk_ops utmi_ops = {
 	.prepare = clk_utmi_prepare,
 	.unprepare = clk_utmi_unprepare,
 	.is_prepared = clk_utmi_is_prepared,
 	.recalc_rate = clk_utmi_recalc_rate,
+	.save_context = clk_utmi_save_context,
+	.restore_context = clk_utmi_restore_context,
 };

 static struct clk_hw * __init
@@ -232,10 +252,29 @@ static int clk_utmi_sama7g5_is_prepared(struct clk_hw *hw)
 	return 0;
 }

+static int clk_utmi_sama7g5_save_context(struct clk_hw *hw)
+{
+	struct clk_utmi *utmi = to_clk_utmi(hw);
+
+	utmi->pms.status = clk_utmi_sama7g5_is_prepared(hw);
+
+	return 0;
+}
+
+static void clk_utmi_sama7g5_restore_context(struct clk_hw *hw)
+{
+	struct clk_utmi *utmi = to_clk_utmi(hw);
+
+	if (utmi->pms.status)
+		clk_utmi_sama7g5_prepare(hw);
+}
+
 static const struct clk_ops sama7g5_utmi_ops = {
 	.prepare = clk_utmi_sama7g5_prepare,
 	.is_prepared = clk_utmi_sama7g5_is_prepared,
 	.recalc_rate = clk_utmi_recalc_rate,
+	.save_context = clk_utmi_sama7g5_save_context,
+	.restore_context = clk_utmi_sama7g5_restore_context,
 };

 struct clk_hw * __init

--- a/drivers/clk/at91/dt-compat.c
+++ b/drivers/clk/at91/dt-compat.c
@@ -399,7 +399,7 @@ of_at91_clk_master_setup(struct device_node *np,

 	hw = at91_clk_register_master_div(regmap, name, "masterck_pres",
 					  layout, characteristics,
-					  &mck_lock, CLK_SET_RATE_GATE);
+					  &mck_lock, CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto out_free_characteristics;


--- a/drivers/clk/at91/pmc.c
+++ b/drivers/clk/at91/pmc.c
@@ -3,10 +3,12 @@
 *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
 */

+#include <linux/clk.h>
 #include <linux/clk-provider.h>
 #include <linux/clkdev.h>
 #include <linux/clk/at91_pmc.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/mfd/syscon.h>
 #include <linux/platform_device.h>
 #include <linux/regmap.h>
@@ -14,8 +16,6 @@

 #include <asm/proc-fns.h>

-#include <dt-bindings/clock/at91.h>
-
 #include "pmc.h"

 #define PMC_MAX_IDS 128
@@ -111,151 +111,46 @@ struct pmc_data *pmc_data_allocate(unsigned int ncore, unsigned int nsystem,
 }

 #ifdef CONFIG_PM
-static struct regmap *pmcreg;

-static u8 registered_ids[PMC_MAX_IDS];
-static u8 registered_pcks[PMC_MAX_PCKS];
+/* Address in SECURAM that say if we suspend to backup mode. */
+static void __iomem *at91_pmc_backup_suspend;

-static struct
+static int at91_pmc_suspend(void)
 {
-	u32 scsr;
-	u32 pcsr0;
-	u32 uckr;
-	u32 mor;
-	u32 mcfr;
-	u32 pllar;
-	u32 mckr;
-	u32 usb;
-	u32 imr;
-	u32 pcsr1;
-	u32 pcr[PMC_MAX_IDS];
-	u32 audio_pll0;
-	u32 audio_pll1;
-	u32 pckr[PMC_MAX_PCKS];
-} pmc_cache;
+	unsigned int backup;

-/*
- * As Peripheral ID 0 is invalid on AT91 chips, the identifier is stored
- * without alteration in the table, and 0 is for unused clocks.
- */
-void pmc_register_id(u8 id)
-{
-	int i;
-
-	for (i = 0; i < PMC_MAX_IDS; i++) {
-		if (registered_ids[i] == 0) {
-			registered_ids[i] = id;
-			break;
-		}
-		if (registered_ids[i] == id)
-			break;
-	}
-}
+	if (!at91_pmc_backup_suspend)
+		return 0;

-/*
- * As Programmable Clock 0 is valid on AT91 chips, there is an offset
- * of 1 between the stored value and the real clock ID.
- */
-void pmc_register_pck(u8 pck)
-{
-	int i;
-
-	for (i = 0; i < PMC_MAX_PCKS; i++) {
-		if (registered_pcks[i] == 0) {
-			registered_pcks[i] = pck + 1;
-			break;
-		}
-		if (registered_pcks[i] == (pck + 1))
-			break;
-	}
-}
-
-static int pmc_suspend(void)
-{
-	int i;
-	u8 num;
-
-	regmap_read(pmcreg, AT91_PMC_SCSR, &pmc_cache.scsr);
-	regmap_read(pmcreg, AT91_PMC_PCSR, &pmc_cache.pcsr0);
-	regmap_read(pmcreg, AT91_CKGR_UCKR, &pmc_cache.uckr);
-	regmap_read(pmcreg, AT91_CKGR_MOR, &pmc_cache.mor);
-	regmap_read(pmcreg, AT91_CKGR_MCFR, &pmc_cache.mcfr);
-	regmap_read(pmcreg, AT91_CKGR_PLLAR, &pmc_cache.pllar);
-	regmap_read(pmcreg, AT91_PMC_MCKR, &pmc_cache.mckr);
-	regmap_read(pmcreg, AT91_PMC_USB, &pmc_cache.usb);
-	regmap_read(pmcreg, AT91_PMC_IMR, &pmc_cache.imr);
-	regmap_read(pmcreg, AT91_PMC_PCSR1, &pmc_cache.pcsr1);
-
-	for (i = 0; registered_ids[i]; i++) {
-		regmap_write(pmcreg, AT91_PMC_PCR,
-			     (registered_ids[i] & AT91_PMC_PCR_PID_MASK));
-		regmap_read(pmcreg, AT91_PMC_PCR,
-			    &pmc_cache.pcr[registered_ids[i]]);
-	}
-	for (i = 0; registered_pcks[i]; i++) {
-		num = registered_pcks[i] - 1;
-		regmap_read(pmcreg, AT91_PMC_PCKR(num), &pmc_cache.pckr[num]);
-	}
+	backup = readl_relaxed(at91_pmc_backup_suspend);
+	if (!backup)
+		return 0;

-	return 0;
+	return clk_save_context();
 }

-static bool pmc_ready(unsigned int mask)
+static void at91_pmc_resume(void)
 {
-	unsigned int status;
+	unsigned int backup;

-	regmap_read(pmcreg, AT91_PMC_SR, &status);
+	if (!at91_pmc_backup_suspend)
+		return;

-	return ((status & mask) == mask) ? 1 : 0;
-}
+	backup = readl_relaxed(at91_pmc_backup_suspend);
+	if (!backup)
+		return;

-static void pmc_resume(void)
-{
-	int i;
-	u8 num;
-	u32 tmp;
-	u32 mask = AT91_PMC_MCKRDY | AT91_PMC_LOCKA;
-
-	regmap_read(pmcreg, AT91_PMC_MCKR, &tmp);
-	if (pmc_cache.mckr != tmp)
-		pr_warn("MCKR was not configured properly by the firmware\n");
-	regmap_read(pmcreg, AT91_CKGR_PLLAR, &tmp);
-	if (pmc_cache.pllar != tmp)
-		pr_warn("PLLAR was not configured properly by the firmware\n");
-
-	regmap_write(pmcreg, AT91_PMC_SCER, pmc_cache.scsr);
-	regmap_write(pmcreg, AT91_PMC_PCER, pmc_cache.pcsr0);
-	regmap_write(pmcreg, AT91_CKGR_UCKR, pmc_cache.uckr);
-	regmap_write(pmcreg, AT91_CKGR_MOR, pmc_cache.mor);
-	regmap_write(pmcreg, AT91_CKGR_MCFR, pmc_cache.mcfr);
-	regmap_write(pmcreg, AT91_PMC_USB, pmc_cache.usb);
-	regmap_write(pmcreg, AT91_PMC_IMR, pmc_cache.imr);
-	regmap_write(pmcreg, AT91_PMC_PCER1, pmc_cache.pcsr1);
-
-	for (i = 0; registered_ids[i]; i++) {
-		regmap_write(pmcreg, AT91_PMC_PCR,
-			     pmc_cache.pcr[registered_ids[i]] |
-			     AT91_PMC_PCR_CMD);
-	}
-	for (i = 0; registered_pcks[i]; i++) {
-		num = registered_pcks[i] - 1;
-		regmap_write(pmcreg, AT91_PMC_PCKR(num), pmc_cache.pckr[num]);
-	}
-
-	if (pmc_cache.uckr & AT91_PMC_UPLLEN)
-		mask |= AT91_PMC_LOCKU;
-
-	while (!pmc_ready(mask))
-		cpu_relax();
+	clk_restore_context();
 }

 static struct syscore_ops pmc_syscore_ops = {
-	.suspend = pmc_suspend,
-	.resume = pmc_resume,
+	.suspend = at91_pmc_suspend,
+	.resume = at91_pmc_resume,
 };

-static const struct of_device_id sama5d2_pmc_dt_ids[] = {
+static const struct of_device_id pmc_dt_ids[] = {
 	{ .compatible = "atmel,sama5d2-pmc" },
+	{ .compatible = "microchip,sama7g5-pmc", },
 	{ /* sentinel */ }
 };

@@ -263,14 +158,31 @@ static int __init pmc_register_ops(void)
 {
 	struct device_node *np;

-	np = of_find_matching_node(NULL, sama5d2_pmc_dt_ids);
+	np = of_find_matching_node(NULL, pmc_dt_ids);
+	if (!np)
+		return -ENODEV;
+
+	if (!of_device_is_available(np)) {
+		of_node_put(np);
+		return -ENODEV;
+	}
+	of_node_put(np);
+
+	np = of_find_compatible_node(NULL, NULL, "atmel,sama5d2-securam");
 	if (!np)
 		return -ENODEV;

-	pmcreg = device_node_to_regmap(np);
+	if (!of_device_is_available(np)) {
+		of_node_put(np);
+		return -ENODEV;
+	}
 	of_node_put(np);
-	if (IS_ERR(pmcreg))
-		return PTR_ERR(pmcreg);
+
+	at91_pmc_backup_suspend = of_iomap(np, 0);
+	if (!at91_pmc_backup_suspend) {
+		pr_warn("%s(): unable to map securam\n", __func__);
+		return -ENOMEM;
+	}

 	register_syscore_ops(&pmc_syscore_ops);


--- a/drivers/clk/at91/pmc.h
+++ b/drivers/clk/at91/pmc.h
@@ -13,6 +13,8 @@
 #include <linux/regmap.h>
 #include <linux/spinlock.h>

+#include <dt-bindings/clock/at91.h>
+
 extern spinlock_t pmc_pcr_lock;

 struct pmc_data {
@@ -98,6 +100,20 @@ struct clk_pcr_layout {
 	u32 pid_mask;
 };

+/**
+ * struct at91_clk_pms - Power management state for AT91 clock
+ * @rate: clock rate
+ * @parent_rate: clock parent rate
+ * @status: clock status (enabled or disabled)
+ * @parent: clock parent index
+ */
+struct at91_clk_pms {
+	unsigned long rate;
+	unsigned long parent_rate;
+	unsigned int status;
+	unsigned int parent;
+};
+
 #define field_get(_mask, _reg) (((_reg) & (_mask)) >> (ffs(_mask) - 1))
 #define field_prep(_mask, _val) (((_val) << (ffs(_mask) - 1)) & (_mask))

@@ -166,7 +182,7 @@ at91_clk_register_master_div(struct regmap *regmap, const char *name,
 			     const char *parent_names,
 			     const struct clk_master_layout *layout,
 			     const struct clk_master_characteristics *characteristics,
-			     spinlock_t *lock, u32 flags);
+			     spinlock_t *lock, u32 flags, u32 safe_div);

 struct clk_hw * __init
 at91_clk_sama7g5_register_master(struct regmap *regmap,
@@ -198,7 +214,8 @@ struct clk_hw * __init
 sam9x60_clk_register_div_pll(struct regmap *regmap, spinlock_t *lock,
 			     const char *name, const char *parent_name, u8 id,
 			     const struct clk_pll_characteristics *characteristics,
-			     const struct clk_pll_layout *layout, u32 flags);
+			     const struct clk_pll_layout *layout, u32 flags,
+			     u32 safe_div);

 struct clk_hw * __init
 sam9x60_clk_register_frac_pll(struct regmap *regmap, spinlock_t *lock,
@@ -248,12 +265,4 @@ struct clk_hw * __init
 at91_clk_sama7g5_register_utmi(struct regmap *regmap, const char *name,
 			       const char *parent_name);

-#ifdef CONFIG_PM
-void pmc_register_id(u8 id);
-void pmc_register_pck(u8 pck);
-#else
-static inline void pmc_register_id(u8 id) {}
-static inline void pmc_register_pck(u8 pck) {}
-#endif
-
 #endif /* __PMC_H_ */
--- a/drivers/clk/at91/sam9x60.c
+++ b/drivers/clk/at91/sam9x60.c
@@ -242,7 +242,7 @@ static void __init sam9x60_pmc_setup(struct device_node *np)
 					    * This feeds CPU. It should not
 					    * be disabled.
 					    */
-					  CLK_IS_CRITICAL | CLK_SET_RATE_GATE);
+					  CLK_IS_CRITICAL | CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto err_free;

@@ -260,7 +260,7 @@ static void __init sam9x60_pmc_setup(struct device_node *np)
 					  &pll_div_layout,
 					  CLK_SET_RATE_GATE |
 					  CLK_SET_PARENT_GATE |
-					  CLK_SET_RATE_PARENT);
+					  CLK_SET_RATE_PARENT, 0);
 	if (IS_ERR(hw))
 		goto err_free;

@@ -279,7 +279,7 @@ static void __init sam9x60_pmc_setup(struct device_node *np)
 	hw = at91_clk_register_master_div(regmap, "masterck_div",
 					  "masterck_pres", &sam9x60_master_layout,
 					  &mck_characteristics, &mck_lock,
-					  CLK_SET_RATE_GATE);
+					  CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto err_free;


--- a/drivers/clk/at91/sama5d2.c
+++ b/drivers/clk/at91/sama5d2.c
@@ -249,7 +249,7 @@ static void __init sama5d2_pmc_setup(struct device_node *np)
 					  "masterck_pres",
 					  &at91sam9x5_master_layout,
 					  &mck_characteristics, &mck_lock,
-					  CLK_SET_RATE_GATE);
+					  CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto err_free;


--- a/drivers/clk/at91/sama5d3.c
+++ b/drivers/clk/at91/sama5d3.c
@@ -184,7 +184,7 @@ static void __init sama5d3_pmc_setup(struct device_node *np)
 					  "masterck_pres",
 					  &at91sam9x5_master_layout,
 					  &mck_characteristics, &mck_lock,
-					  CLK_SET_RATE_GATE);
+					  CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto err_free;


--- a/drivers/clk/at91/sama5d4.c
+++ b/drivers/clk/at91/sama5d4.c
@@ -199,7 +199,7 @@ static void __init sama5d4_pmc_setup(struct device_node *np)
 					  "masterck_pres",
 					  &at91sam9x5_master_layout,
 					  &mck_characteristics, &mck_lock,
-					  CLK_SET_RATE_GATE);
+					  CLK_SET_RATE_GATE, 0);
 	if (IS_ERR(hw))
 		goto err_free;


--- a/drivers/clk/at91/sama7g5.c
+++ b/drivers/clk/at91/sama7g5.c
@@ -127,6 +127,8 @@ static const struct clk_pll_characteristics pll_characteristics = {
 * @t:		clock type
 * @f:		clock flags
 * @eid:	export index in sama7g5->chws[] array
+ * @safe_div:	intermediate divider need to be set on PRE_RATE_CHANGE
+ *		notification
 */
 static const struct {
 	const char *n;
@@ -136,6 +138,7 @@ static const struct {
 	unsigned long f;
 	u8 t;
 	u8 eid;
+	u8 safe_div;
 } sama7g5_plls[][PLL_ID_MAX] = {
 	[PLL_ID_CPU] = {
 		{ .n = "cpupll_fracck",
@@ -156,7 +159,12 @@ static const struct {
 		  .t = PLL_TYPE_DIV,
 		   /* This feeds CPU. It should not be disabled. */
 		  .f = CLK_IS_CRITICAL | CLK_SET_RATE_PARENT,
-		  .eid = PMC_CPUPLL, },
+		  .eid = PMC_CPUPLL,
+		  /*
+		   * Safe div=15 should be safe even for switching b/w 1GHz and
+		   * 90MHz (frac pll might go up to 1.2GHz).
+		   */
+		  .safe_div = 15, },
 	},

 	[PLL_ID_SYS] = {
@@ -377,6 +385,7 @@ static const struct {
 	u8 id;
 } sama7g5_periphck[] = {
 	{ .n = "pioA_clk",	.p = "mck0", .id = 11, },
+	{ .n = "securam_clk",	.p = "mck0", .id = 18, },
 	{ .n = "sfr_clk",	.p = "mck1", .id = 19, },
 	{ .n = "hsmc_clk",	.p = "mck1", .id = 21, },
 	{ .n = "xdmac0_clk",	.p = "mck1", .id = 22, },
@@ -841,7 +850,7 @@ static const struct {

 /* MCK0 characteristics. */
 static const struct clk_master_characteristics mck0_characteristics = {
-	.output = { .min = 50000000, .max = 200000000 },
+	.output = { .min = 32768, .max = 200000000 },
 	.divisors = { 1, 2, 4, 3, 5 },
 	.have_div3_pres = 1,
 };
@@ -966,7 +975,8 @@ static void __init sama7g5_pmc_setup(struct device_node *np)
 					sama7g5_plls[i][j].p, i,
 					sama7g5_plls[i][j].c,
 					sama7g5_plls[i][j].l,
-					sama7g5_plls[i][j].f);
+					sama7g5_plls[i][j].f,
+					sama7g5_plls[i][j].safe_div);
 				break;

 			default:
@@ -982,18 +992,9 @@ static void __init sama7g5_pmc_setup(struct device_node *np)
 	}

 	parent_names[0] = "cpupll_divpmcck";
-	hw = at91_clk_register_master_pres(regmap, "cpuck", 1, parent_names,
-					   &mck0_layout, &mck0_characteristics,
-					   &pmc_mck0_lock,
-					   CLK_SET_RATE_PARENT, 0);
-	if (IS_ERR(hw))
-		goto err_free;
-
-	sama7g5_pmc->chws[PMC_CPU] = hw;
-
-	hw = at91_clk_register_master_div(regmap, "mck0", "cpuck",
+	hw = at91_clk_register_master_div(regmap, "mck0", "cpupll_divpmcck",
 					  &mck0_layout, &mck0_characteristics,
-					  &pmc_mck0_lock, 0);
+					  &pmc_mck0_lock, CLK_GET_RATE_NOCACHE, 5);
 	if (IS_ERR(hw))
 		goto err_free;


--- a/drivers/clk/mvebu/ap-cpu-clk.c
+++ b/drivers/clk/mvebu/ap-cpu-clk.c
@@ -256,12 +256,15 @@ static int ap_cpu_clock_probe(struct platform_device *pdev)
 		int cpu, err;

 		err = of_property_read_u32(dn, "reg", &cpu);
-		if (WARN_ON(err))
+		if (WARN_ON(err)) {
+			of_node_put(dn);
 			return err;
+		}

 		/* If cpu2 or cpu3 is enabled */
 		if (cpu & APN806_CLUSTER_NUM_MASK) {
 			nclusters = 2;
+			of_node_put(dn);
 			break;
 		}
 	}
@@ -288,8 +291,10 @@ static int ap_cpu_clock_probe(struct platform_device *pdev)
 		int cpu, err;

 		err = of_property_read_u32(dn, "reg", &cpu);
-		if (WARN_ON(err))
+		if (WARN_ON(err)) {
+			of_node_put(dn);
 			return err;
+		}

 		cluster_index = cpu & APN806_CLUSTER_NUM_MASK;
 		cluster_index >>= APN806_CLUSTER_NUM_OFFSET;
@@ -301,6 +306,7 @@ static int ap_cpu_clock_probe(struct platform_device *pdev)
 		parent = of_clk_get(np, cluster_index);
 		if (IS_ERR(parent)) {
 			dev_err(dev, "Could not get the clock parent\n");
+			of_node_put(dn);
 			return -EINVAL;
 		}
 		parent_name =  __clk_get_name(parent);
@@ -319,8 +325,10 @@ static int ap_cpu_clock_probe(struct platform_device *pdev)
 		init.parent_names = &parent_name;

 		ret = devm_clk_hw_register(dev, &ap_cpu_clk[cluster_index].hw);
-		if (ret)
+		if (ret) {
+			of_node_put(dn);
 			return ret;
+		}
 		ap_cpu_data->hws[cluster_index] = &ap_cpu_clk[cluster_index].hw;
 	}


--- a/drivers/clk/renesas/r8a7795-cpg-mssr.c
+++ b/drivers/clk/renesas/r8a7795-cpg-mssr.c
@@ -229,6 +229,7 @@ static struct mssr_mod_clk r8a7795_mod_clks[] __initdata = {
 	DEF_MOD("lvds",			 727,	R8A7795_CLK_S0D4),
 	DEF_MOD("hdmi1",		 728,	R8A7795_CLK_HDMI),
 	DEF_MOD("hdmi0",		 729,	R8A7795_CLK_HDMI),
+	DEF_MOD("mlp",			 802,	R8A7795_CLK_S2D1),
 	DEF_MOD("vin7",			 804,	R8A7795_CLK_S0D2),
 	DEF_MOD("vin6",			 805,	R8A7795_CLK_S0D2),
 	DEF_MOD("vin5",			 806,	R8A7795_CLK_S0D2),

--- a/drivers/clk/renesas/r8a7796-cpg-mssr.c
+++ b/drivers/clk/renesas/r8a7796-cpg-mssr.c
@@ -207,6 +207,7 @@ static struct mssr_mod_clk r8a7796_mod_clks[] __initdata = {
 	DEF_MOD("du0",			 724,	R8A7796_CLK_S2D1),
 	DEF_MOD("lvds",			 727,	R8A7796_CLK_S2D1),
 	DEF_MOD("hdmi0",		 729,	R8A7796_CLK_HDMI),
+	DEF_MOD("mlp",			 802,	R8A7796_CLK_S2D1),
 	DEF_MOD("vin7",			 804,	R8A7796_CLK_S0D2),
 	DEF_MOD("vin6",			 805,	R8A7796_CLK_S0D2),
 	DEF_MOD("vin5",			 806,	R8A7796_CLK_S0D2),

--- a/drivers/clk/renesas/r8a77965-cpg-mssr.c
+++ b/drivers/clk/renesas/r8a77965-cpg-mssr.c
@@ -205,6 +205,7 @@ static const struct mssr_mod_clk r8a77965_mod_clks[] __initconst = {
 	DEF_MOD("lvds",			727,	R8A77965_CLK_S2D1),
 	DEF_MOD("hdmi0",		729,	R8A77965_CLK_HDMI),

+	DEF_MOD("mlp",			802,	R8A77965_CLK_S2D1),
 	DEF_MOD("vin7",			804,	R8A77965_CLK_S0D2),
 	DEF_MOD("vin6",			805,	R8A77965_CLK_S0D2),
 	DEF_MOD("vin5",			806,	R8A77965_CLK_S0D2),

--- a/drivers/clk/renesas/r8a779a0-cpg-mssr.c
+++ b/drivers/clk/renesas/r8a779a0-cpg-mssr.c
@@ -33,9 +33,13 @@ enum rcar_r8a779a0_clk_types {
 	CLK_TYPE_R8A779A0_PLL1,
 	CLK_TYPE_R8A779A0_PLL2X_3X,	/* PLL[23][01] */
 	CLK_TYPE_R8A779A0_PLL5,
+	CLK_TYPE_R8A779A0_Z,
 	CLK_TYPE_R8A779A0_SD,
 	CLK_TYPE_R8A779A0_MDSEL,	/* Select parent/divider using mode pin */
 	CLK_TYPE_R8A779A0_OSC,	/* OSC EXTAL predivider and fixed divider */
+	CLK_TYPE_R8A779A0_RPCSRC,
+	CLK_TYPE_R8A779A0_RPC,
+	CLK_TYPE_R8A779A0_RPCD2,
 };

 struct rcar_r8a779a0_cpg_pll_config {
@@ -84,6 +88,10 @@ enum clk_ids {
 	DEF_BASE(_name, _id, CLK_TYPE_R8A779A0_PLL2X_3X, CLK_MAIN, \
 		 .offset = _offset)

+#define DEF_Z(_name, _id, _parent, _div, _offset)	\
+	DEF_BASE(_name, _id, CLK_TYPE_R8A779A0_Z, _parent, .div = _div,	\
+		 .offset = _offset)
+
 #define DEF_SD(_name, _id, _parent, _offset)   \
 	DEF_BASE(_name, _id, CLK_TYPE_R8A779A0_SD, _parent, .offset = _offset)

@@ -120,8 +128,14 @@ static const struct cpg_core_clk r8a779a0_core_clks[] __initconst = {
 	DEF_FIXED(".s3",		CLK_S3,		CLK_PLL1_DIV2,	4, 1),
 	DEF_FIXED(".sdsrc",		CLK_SDSRC,	CLK_PLL5_DIV4,	1, 1),
 	DEF_RATE(".oco",		CLK_OCO,	32768),
+	DEF_BASE(".rpcsrc",	 CLK_RPCSRC,	   CLK_TYPE_R8A779A0_RPCSRC, CLK_PLL5),
+	DEF_BASE("rpc",		 R8A779A0_CLK_RPC, CLK_TYPE_R8A779A0_RPC, CLK_RPCSRC),
+	DEF_BASE("rpcd2",	 R8A779A0_CLK_RPCD2, CLK_TYPE_R8A779A0_RPCD2,
+		 R8A779A0_CLK_RPC),

 	/* Core Clock Outputs */
+	DEF_Z("z0",		R8A779A0_CLK_Z0,	CLK_PLL20,	2, 0),
+	DEF_Z("z1",		R8A779A0_CLK_Z1,	CLK_PLL21,	2, 8),
 	DEF_FIXED("zx",		R8A779A0_CLK_ZX,	CLK_PLL20_DIV2,	2, 1),
 	DEF_FIXED("s1d1",	R8A779A0_CLK_S1D1,	CLK_S1,		1, 1),
 	DEF_FIXED("s1d2",	R8A779A0_CLK_S1D2,	CLK_S1,		2, 1),
@@ -193,6 +207,7 @@ static const struct mssr_mod_clk r8a779a0_mod_clks[] __initconst = {
 	DEF_MOD("msi3",		621,	R8A779A0_CLK_MSO),
 	DEF_MOD("msi4",		622,	R8A779A0_CLK_MSO),
 	DEF_MOD("msi5",		623,	R8A779A0_CLK_MSO),
+	DEF_MOD("rpc-if",	629,	R8A779A0_CLK_RPCD2),
 	DEF_MOD("scif0",	702,	R8A779A0_CLK_S1D8),
 	DEF_MOD("scif1",	703,	R8A779A0_CLK_S1D8),
 	DEF_MOD("scif3",	704,	R8A779A0_CLK_S1D8),
@@ -205,6 +220,7 @@ static const struct mssr_mod_clk r8a779a0_mod_clks[] __initconst = {
 	DEF_MOD("tmu2",		715,	R8A779A0_CLK_S1D4),
 	DEF_MOD("tmu3",		716,	R8A779A0_CLK_S1D4),
 	DEF_MOD("tmu4",		717,	R8A779A0_CLK_S1D4),
+	DEF_MOD("tpu0",		718,	R8A779A0_CLK_S1D8),
 	DEF_MOD("vin00",	730,	R8A779A0_CLK_S1D1),
 	DEF_MOD("vin01",	731,	R8A779A0_CLK_S1D1),
 	DEF_MOD("vin02",	800,	R8A779A0_CLK_S1D1),
@@ -259,6 +275,162 @@ static const struct rcar_r8a779a0_cpg_pll_config *cpg_pll_config __initdata;
 static unsigned int cpg_clk_extalr __initdata;
 static u32 cpg_mode __initdata;

+/*
+ * Z0 Clock & Z1 Clock
+ */
+#define CPG_FRQCRB			0x00000804
+#define CPG_FRQCRB_KICK			BIT(31)
+#define CPG_FRQCRC			0x00000808
+
+struct cpg_z_clk {
+	struct clk_hw hw;
+	void __iomem *reg;
+	void __iomem *kick_reg;
+	unsigned long max_rate;		/* Maximum rate for normal mode */
+	unsigned int fixed_div;
+	u32 mask;
+};
+
+#define to_z_clk(_hw)	container_of(_hw, struct cpg_z_clk, hw)
+
+static unsigned long cpg_z_clk_recalc_rate(struct clk_hw *hw,
+					   unsigned long parent_rate)
+{
+	struct cpg_z_clk *zclk = to_z_clk(hw);
+	unsigned int mult;
+	u32 val;
+
+	val = readl(zclk->reg) & zclk->mask;
+	mult = 32 - (val >> __ffs(zclk->mask));
+
+	return DIV_ROUND_CLOSEST_ULL((u64)parent_rate * mult,
+				     32 * zclk->fixed_div);
+}
+
+static int cpg_z_clk_determine_rate(struct clk_hw *hw,
+				    struct clk_rate_request *req)
+{
+	struct cpg_z_clk *zclk = to_z_clk(hw);
+	unsigned int min_mult, max_mult, mult;
+	unsigned long rate, prate;
+
+	rate = min(req->rate, req->max_rate);
+	if (rate <= zclk->max_rate) {
+		/* Set parent rate to initial value for normal modes */
+		prate = zclk->max_rate;
+	} else {
+		/* Set increased parent rate for boost modes */
+		prate = rate;
+	}
+	req->best_parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
+						  prate * zclk->fixed_div);
+
+	prate = req->best_parent_rate / zclk->fixed_div;
+	min_mult = max(div64_ul(req->min_rate * 32ULL, prate), 1ULL);
+	max_mult = min(div64_ul(req->max_rate * 32ULL, prate), 32ULL);
+	if (max_mult < min_mult)
+		return -EINVAL;
+
+	mult = DIV_ROUND_CLOSEST_ULL(rate * 32ULL, prate);
+	mult = clamp(mult, min_mult, max_mult);
+
+	req->rate = DIV_ROUND_CLOSEST_ULL((u64)prate * mult, 32);
+	return 0;
+}
+
+static int cpg_z_clk_set_rate(struct clk_hw *hw, unsigned long rate,
+			      unsigned long parent_rate)
+{
+	struct cpg_z_clk *zclk = to_z_clk(hw);
+	unsigned int mult;
+	unsigned int i;
+
+	mult = DIV64_U64_ROUND_CLOSEST(rate * 32ULL * zclk->fixed_div,
+				       parent_rate);
+	mult = clamp(mult, 1U, 32U);
+
+	if (readl(zclk->kick_reg) & CPG_FRQCRB_KICK)
+		return -EBUSY;
+
+	cpg_reg_modify(zclk->reg, zclk->mask, (32 - mult) << __ffs(zclk->mask));
+
+	/*
+	 * Set KICK bit in FRQCRB to update hardware setting and wait for
+	 * clock change completion.
+	 */
+	cpg_reg_modify(zclk->kick_reg, 0, CPG_FRQCRB_KICK);
+
+	/*
+	 * Note: There is no HW information about the worst case latency.
+	 *
+	 * Using experimental measurements, it seems that no more than
+	 * ~10 iterations are needed, independently of the CPU rate.
+	 * Since this value might be dependent on external xtal rate, pll1
+	 * rate or even the other emulation clocks rate, use 1000 as a
+	 * "super" safe value.
+	 */
+	for (i = 1000; i; i--) {
+		if (!(readl(zclk->kick_reg) & CPG_FRQCRB_KICK))
+			return 0;
+
+		cpu_relax();
+	}
+
+	return -ETIMEDOUT;
+}
+
+static const struct clk_ops cpg_z_clk_ops = {
+	.recalc_rate = cpg_z_clk_recalc_rate,
+	.determine_rate = cpg_z_clk_determine_rate,
+	.set_rate = cpg_z_clk_set_rate,
+};
+
+static struct clk * __init cpg_z_clk_register(const char *name,
+					      const char *parent_name,
+					      void __iomem *reg,
+					      unsigned int div,
+					      unsigned int offset)
+{
+	struct clk_init_data init = {};
+	struct cpg_z_clk *zclk;
+	struct clk *clk;
+
+	zclk = kzalloc(sizeof(*zclk), GFP_KERNEL);
+	if (!zclk)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.ops = &cpg_z_clk_ops;
+	init.flags = CLK_SET_RATE_PARENT;
+	init.parent_names = &parent_name;
+	init.num_parents = 1;
+
+	zclk->reg = reg + CPG_FRQCRC;
+	zclk->kick_reg = reg + CPG_FRQCRB;
+	zclk->hw.init = &init;
+	zclk->mask = GENMASK(offset + 4, offset);
+	zclk->fixed_div = div; /* PLLVCO x 1/div x SYS-CPU divider */
+
+	clk = clk_register(NULL, &zclk->hw);
+	if (IS_ERR(clk)) {
+		kfree(zclk);
+		return clk;
+	}
+
+	zclk->max_rate = clk_hw_get_rate(clk_hw_get_parent(&zclk->hw)) /
+			 zclk->fixed_div;
+	return clk;
+}
+
+/*
+ * RPC Clocks
+ */
+#define CPG_RPCCKCR 0x874
+
+static const struct clk_div_table cpg_rpcsrc_div_table[] = {
+	{ 0, 4 }, { 1, 6 }, { 2, 5 }, { 3, 6 }, { 0, 0 },
+};
+
 static struct clk * __init rcar_r8a779a0_cpg_clk_register(struct device *dev,
 	const struct cpg_core_clk *core, const struct cpg_mssr_info *info,
 	struct clk **clks, void __iomem *base,
@@ -293,6 +465,10 @@ static struct clk * __init rcar_r8a779a0_cpg_clk_register(struct device *dev,
 		div = cpg_pll_config->pll5_div;
 		break;

+	case CLK_TYPE_R8A779A0_Z:
+		return cpg_z_clk_register(core->name, __clk_get_name(parent),
+					  base, core->div, core->offset);
+
 	case CLK_TYPE_R8A779A0_SD:
 		return cpg_sd_clk_register(core->name, base, core->offset,
 					   __clk_get_name(parent), notifiers,
@@ -322,6 +498,21 @@ static struct clk * __init rcar_r8a779a0_cpg_clk_register(struct device *dev,
 		div = cpg_pll_config->osc_prediv * core->div;
 		break;

+	case CLK_TYPE_R8A779A0_RPCSRC:
+		return clk_register_divider_table(NULL, core->name,
+						  __clk_get_name(parent), 0,
+						  base + CPG_RPCCKCR, 3, 2, 0,
+						  cpg_rpcsrc_div_table,
+						  &cpg_lock);
+
+	case CLK_TYPE_R8A779A0_RPC:
+		return cpg_rpc_clk_register(core->name, base + CPG_RPCCKCR,
+					    __clk_get_name(parent), notifiers);
+
+	case CLK_TYPE_R8A779A0_RPCD2:
+		return cpg_rpcd2_clk_register(core->name, base + CPG_RPCCKCR,
+					      __clk_get_name(parent));
+
 	default:
 		return ERR_PTR(-EINVAL);
 	}

--- a/drivers/clk/renesas/r9a07g044-cpg.c
+++ b/drivers/clk/renesas/r9a07g044-cpg.c
@@ -29,15 +29,27 @@ enum clk_ids {
 	CLK_PLL2_DIV16,
 	CLK_PLL2_DIV20,
 	CLK_PLL3,
+	CLK_PLL3_400,
+	CLK_PLL3_533,
 	CLK_PLL3_DIV2,
 	CLK_PLL3_DIV2_4,
 	CLK_PLL3_DIV2_4_2,
 	CLK_PLL3_DIV4,
+	CLK_SEL_PLL3_3,
+	CLK_DIV_PLL3_C,
 	CLK_PLL4,
 	CLK_PLL5,
-	CLK_PLL5_DIV2,
+	CLK_PLL5_FOUT3,
+	CLK_PLL5_250,
 	CLK_PLL6,
+	CLK_PLL6_250,
 	CLK_P1_DIV2,
+	CLK_PLL2_800,
+	CLK_PLL2_SDHI_533,
+	CLK_PLL2_SDHI_400,
+	CLK_PLL2_SDHI_266,
+	CLK_SD0_DIV4,
+	CLK_SD1_DIV4,

 	/* Module Clocks */
 	MOD_CLK_BASE,
@@ -53,6 +65,11 @@ static const struct clk_div_table dtable_1_32[] = {
 	{0, 0},
 };

+/* Mux clock tables */
+static const char * const sel_pll3_3[] = { ".pll3_533", ".pll3_400" };
+static const char * const sel_pll6_2[]	= { ".pll6_250", ".pll5_250" };
+static const char * const sel_shdi[] = { ".clk_533", ".clk_400", ".clk_266" };
+
 static const struct cpg_core_clk r9a07g044_core_clks[] __initconst = {
 	/* External Clock Inputs */
 	DEF_INPUT("extal", CLK_EXTAL),
@@ -63,8 +80,20 @@ static const struct cpg_core_clk r9a07g044_core_clks[] __initconst = {
 	DEF_SAMPLL(".pll1", CLK_PLL1, CLK_EXTAL, PLL146_CONF(0)),
 	DEF_FIXED(".pll2", CLK_PLL2, CLK_EXTAL, 133, 2),
 	DEF_FIXED(".pll3", CLK_PLL3, CLK_EXTAL, 133, 2),
+	DEF_FIXED(".pll3_400", CLK_PLL3_400, CLK_PLL3, 1, 4),
+	DEF_FIXED(".pll3_533", CLK_PLL3_533, CLK_PLL3, 1, 3),
+
+	DEF_FIXED(".pll5", CLK_PLL5, CLK_EXTAL, 125, 1),
+	DEF_FIXED(".pll5_fout3", CLK_PLL5_FOUT3, CLK_PLL5, 1, 6),
+
+	DEF_FIXED(".pll6", CLK_PLL6, CLK_EXTAL, 125, 6),

 	DEF_FIXED(".pll2_div2", CLK_PLL2_DIV2, CLK_PLL2, 1, 2),
+	DEF_FIXED(".clk_800", CLK_PLL2_800, CLK_PLL2, 1, 2),
+	DEF_FIXED(".clk_533", CLK_PLL2_SDHI_533, CLK_PLL2, 1, 3),
+	DEF_FIXED(".clk_400", CLK_PLL2_SDHI_400, CLK_PLL2_800, 1, 2),
+	DEF_FIXED(".clk_266", CLK_PLL2_SDHI_266, CLK_PLL2_SDHI_533, 1, 2),
+
 	DEF_FIXED(".pll2_div16", CLK_PLL2_DIV16, CLK_PLL2, 1, 16),
 	DEF_FIXED(".pll2_div20", CLK_PLL2_DIV20, CLK_PLL2, 1, 20),

@@ -72,6 +101,13 @@ static const struct cpg_core_clk r9a07g044_core_clks[] __initconst = {
 	DEF_FIXED(".pll3_div2_4", CLK_PLL3_DIV2_4, CLK_PLL3_DIV2, 1, 4),
 	DEF_FIXED(".pll3_div2_4_2", CLK_PLL3_DIV2_4_2, CLK_PLL3_DIV2_4, 1, 2),
 	DEF_FIXED(".pll3_div4", CLK_PLL3_DIV4, CLK_PLL3, 1, 4),
+	DEF_MUX(".sel_pll3_3", CLK_SEL_PLL3_3, SEL_PLL3_3,
+		sel_pll3_3, ARRAY_SIZE(sel_pll3_3), 0, CLK_MUX_READ_ONLY),
+	DEF_DIV("divpl3c", CLK_DIV_PLL3_C, CLK_SEL_PLL3_3,
+		DIVPL3C, dtable_1_32, CLK_DIVIDER_HIWORD_MASK),
+
+	DEF_FIXED(".pll5_250", CLK_PLL5_250, CLK_PLL5_FOUT3, 1, 2),
+	DEF_FIXED(".pll6_250", CLK_PLL6_250, CLK_PLL6, 1, 2),

 	/* Core output clk */
 	DEF_FIXED("I", R9A07G044_CLK_I, CLK_PLL1, 1, 1),
@@ -84,6 +120,18 @@ static const struct cpg_core_clk r9a07g044_core_clks[] __initconst = {
 	DEF_FIXED("P1_DIV2", CLK_P1_DIV2, R9A07G044_CLK_P1, 1, 2),
 	DEF_DIV("P2", R9A07G044_CLK_P2, CLK_PLL3_DIV2_4_2,
 		DIVPL3A, dtable_1_32, CLK_DIVIDER_HIWORD_MASK),
+	DEF_FIXED("M0", R9A07G044_CLK_M0, CLK_PLL3_DIV2_4, 1, 1),
+	DEF_FIXED("ZT", R9A07G044_CLK_ZT, CLK_PLL3_DIV2_4_2, 1, 1),
+	DEF_MUX("HP", R9A07G044_CLK_HP, SEL_PLL6_2,
+		sel_pll6_2, ARRAY_SIZE(sel_pll6_2), 0, CLK_MUX_HIWORD_MASK),
+	DEF_FIXED("SPI0", R9A07G044_CLK_SPI0, CLK_DIV_PLL3_C, 1, 2),
+	DEF_FIXED("SPI1", R9A07G044_CLK_SPI1, CLK_DIV_PLL3_C, 1, 4),
+	DEF_SD_MUX("SD0", R9A07G044_CLK_SD0, SEL_SDHI0,
+		   sel_shdi, ARRAY_SIZE(sel_shdi)),
+	DEF_SD_MUX("SD1", R9A07G044_CLK_SD1, SEL_SDHI1,
+		   sel_shdi, ARRAY_SIZE(sel_shdi)),
+	DEF_FIXED("SD0_DIV4", CLK_SD0_DIV4, R9A07G044_CLK_SD0, 1, 4),
+	DEF_FIXED("SD1_DIV4", CLK_SD1_DIV4, R9A07G044_CLK_SD1, 1, 4),
 };

 static struct rzg2l_mod_clk r9a07g044_mod_clks[] = {
@@ -97,6 +145,26 @@ static struct rzg2l_mod_clk r9a07g044_mod_clks[] = {
 				0x52c, 0),
 	DEF_MOD("dmac_pclk",	R9A07G044_DMAC_PCLK, CLK_P1_DIV2,
 				0x52c, 1),
+	DEF_MOD("spi_clk2",	R9A07G044_SPI_CLK2, R9A07G044_CLK_SPI1,
+				0x550, 0),
+	DEF_MOD("spi_clk",	R9A07G044_SPI_CLK, R9A07G044_CLK_SPI0,
+				0x550, 1),
+	DEF_MOD("sdhi0_imclk",	R9A07G044_SDHI0_IMCLK, CLK_SD0_DIV4,
+				0x554, 0),
+	DEF_MOD("sdhi0_imclk2",	R9A07G044_SDHI0_IMCLK2, CLK_SD0_DIV4,
+				0x554, 1),
+	DEF_MOD("sdhi0_clk_hs",	R9A07G044_SDHI0_CLK_HS, R9A07G044_CLK_SD0,
+				0x554, 2),
+	DEF_MOD("sdhi0_aclk",	R9A07G044_SDHI0_ACLK, R9A07G044_CLK_P1,
+				0x554, 3),
+	DEF_MOD("sdhi1_imclk",	R9A07G044_SDHI1_IMCLK, CLK_SD1_DIV4,
+				0x554, 4),
+	DEF_MOD("sdhi1_imclk2",	R9A07G044_SDHI1_IMCLK2, CLK_SD1_DIV4,
+				0x554, 5),
+	DEF_MOD("sdhi1_clk_hs",	R9A07G044_SDHI1_CLK_HS, R9A07G044_CLK_SD1,
+				0x554, 6),
+	DEF_MOD("sdhi1_aclk",	R9A07G044_SDHI1_ACLK, R9A07G044_CLK_P1,
+				0x554, 7),
 	DEF_MOD("ssi0_pclk",	R9A07G044_SSI0_PCLK2, R9A07G044_CLK_P0,
 				0x570, 0),
 	DEF_MOD("ssi0_sfr",	R9A07G044_SSI0_PCLK_SFR, R9A07G044_CLK_P0,
@@ -121,6 +189,14 @@ static struct rzg2l_mod_clk r9a07g044_mod_clks[] = {
 				0x578, 2),
 	DEF_MOD("usb_pclk",	R9A07G044_USB_PCLK, R9A07G044_CLK_P1,
 				0x578, 3),
+	DEF_COUPLED("eth0_axi",	R9A07G044_ETH0_CLK_AXI, R9A07G044_CLK_M0,
+				0x57c, 0),
+	DEF_COUPLED("eth0_chi",	R9A07G044_ETH0_CLK_CHI, R9A07G044_CLK_ZT,
+				0x57c, 0),
+	DEF_COUPLED("eth1_axi",	R9A07G044_ETH1_CLK_AXI, R9A07G044_CLK_M0,
+				0x57c, 1),
+	DEF_COUPLED("eth1_chi",	R9A07G044_ETH1_CLK_CHI, R9A07G044_CLK_ZT,
+				0x57c, 1),
 	DEF_MOD("i2c0",		R9A07G044_I2C0_PCLK, R9A07G044_CLK_P0,
 				0x580, 0),
 	DEF_MOD("i2c1",		R9A07G044_I2C1_PCLK, R9A07G044_CLK_P0,
@@ -157,6 +233,9 @@ static struct rzg2l_reset r9a07g044_resets[] = {
 	DEF_RST(R9A07G044_IA55_RESETN, 0x818, 0),
 	DEF_RST(R9A07G044_DMAC_ARESETN, 0x82c, 0),
 	DEF_RST(R9A07G044_DMAC_RST_ASYNC, 0x82c, 1),
+	DEF_RST(R9A07G044_SPI_RST, 0x850, 0),
+	DEF_RST(R9A07G044_SDHI0_IXRST, 0x854, 0),
+	DEF_RST(R9A07G044_SDHI1_IXRST, 0x854, 1),
 	DEF_RST(R9A07G044_SSI0_RST_M2_REG, 0x870, 0),
 	DEF_RST(R9A07G044_SSI1_RST_M2_REG, 0x870, 1),
 	DEF_RST(R9A07G044_SSI2_RST_M2_REG, 0x870, 2),
@@ -165,6 +244,8 @@ static struct rzg2l_reset r9a07g044_resets[] = {
 	DEF_RST(R9A07G044_USB_U2H1_HRESETN, 0x878, 1),
 	DEF_RST(R9A07G044_USB_U2P_EXL_SYSRST, 0x878, 2),
 	DEF_RST(R9A07G044_USB_PRESETN, 0x878, 3),
+	DEF_RST(R9A07G044_ETH0_RST_HW_N, 0x87c, 0),
+	DEF_RST(R9A07G044_ETH1_RST_HW_N, 0x87c, 1),
 	DEF_RST(R9A07G044_I2C0_MRST, 0x880, 0),
 	DEF_RST(R9A07G044_I2C1_MRST, 0x880, 1),
 	DEF_RST(R9A07G044_I2C2_MRST, 0x880, 2),
@@ -186,6 +267,8 @@ static struct rzg2l_reset r9a07g044_resets[] = {

 static const unsigned int r9a07g044_crit_mod_clks[] __initconst = {
 	MOD_CLK_BASE + R9A07G044_GIC600_GICCLK,
+	MOD_CLK_BASE + R9A07G044_IA55_CLK,
+	MOD_CLK_BASE + R9A07G044_DMAC_ACLK,
 };

 const struct rzg2l_cpg_info r9a07g044_cpg_info = {

--- a/drivers/clk/renesas/rcar-cpg-lib.c
+++ b/drivers/clk/renesas/rcar-cpg-lib.c
@@ -267,4 +267,87 @@ struct clk * __init cpg_sd_clk_register(const char *name,
 	return clk;
 }

+struct rpc_clock {
+	struct clk_divider div;
+	struct clk_gate gate;
+	/*
+	 * One notifier covers both RPC and RPCD2 clocks as they are both
+	 * controlled by the same RPCCKCR register...
+	 */
+	struct cpg_simple_notifier csn;
+};
+
+static const struct clk_div_table cpg_rpc_div_table[] = {
+	{ 1, 2 }, { 3, 4 }, { 5, 6 }, { 7, 8 }, { 0, 0 },
+};
+
+struct clk * __init cpg_rpc_clk_register(const char *name,
+	void __iomem *rpcckcr, const char *parent_name,
+	struct raw_notifier_head *notifiers)
+{
+	struct rpc_clock *rpc;
+	struct clk *clk;
+
+	rpc = kzalloc(sizeof(*rpc), GFP_KERNEL);
+	if (!rpc)
+		return ERR_PTR(-ENOMEM);
+
+	rpc->div.reg = rpcckcr;
+	rpc->div.width = 3;
+	rpc->div.table = cpg_rpc_div_table;
+	rpc->div.lock = &cpg_lock;
+
+	rpc->gate.reg = rpcckcr;
+	rpc->gate.bit_idx = 8;
+	rpc->gate.flags = CLK_GATE_SET_TO_DISABLE;
+	rpc->gate.lock = &cpg_lock;
+
+	rpc->csn.reg = rpcckcr;
+
+	clk = clk_register_composite(NULL, name, &parent_name, 1, NULL, NULL,
+				     &rpc->div.hw,  &clk_divider_ops,
+				     &rpc->gate.hw, &clk_gate_ops,
+				     CLK_SET_RATE_PARENT);
+	if (IS_ERR(clk)) {
+		kfree(rpc);
+		return clk;
+	}
+
+	cpg_simple_notifier_register(notifiers, &rpc->csn);
+	return clk;
+}
+
+struct rpcd2_clock {
+	struct clk_fixed_factor fixed;
+	struct clk_gate gate;
+};
+
+struct clk * __init cpg_rpcd2_clk_register(const char *name,
+					   void __iomem *rpcckcr,
+					   const char *parent_name)
+{
+	struct rpcd2_clock *rpcd2;
+	struct clk *clk;
+
+	rpcd2 = kzalloc(sizeof(*rpcd2), GFP_KERNEL);
+	if (!rpcd2)
+		return ERR_PTR(-ENOMEM);
+
+	rpcd2->fixed.mult = 1;
+	rpcd2->fixed.div = 2;
+
+	rpcd2->gate.reg = rpcckcr;
+	rpcd2->gate.bit_idx = 9;
+	rpcd2->gate.flags = CLK_GATE_SET_TO_DISABLE;
+	rpcd2->gate.lock = &cpg_lock;
+
+	clk = clk_register_composite(NULL, name, &parent_name, 1, NULL, NULL,
+				     &rpcd2->fixed.hw, &clk_fixed_factor_ops,
+				     &rpcd2->gate.hw, &clk_gate_ops,
+				     CLK_SET_RATE_PARENT);
+	if (IS_ERR(clk))
+		kfree(rpcd2);
+
+	return clk;
+}

--- a/drivers/clk/renesas/rcar-cpg-lib.h
+++ b/drivers/clk/renesas/rcar-cpg-lib.h
@@ -30,4 +30,11 @@ struct clk * __init cpg_sd_clk_register(const char *name,
 	void __iomem *base, unsigned int offset, const char *parent_name,
 	struct raw_notifier_head *notifiers, bool skip_first);

+struct clk * __init cpg_rpc_clk_register(const char *name,
+	void __iomem *rpcckcr, const char *parent_name,
+	struct raw_notifier_head *notifiers);
+
+struct clk * __init cpg_rpcd2_clk_register(const char *name,
+					   void __iomem *rpcckcr,
+					   const char *parent_name);
 #endif
--- a/drivers/clk/renesas/rcar-gen3-cpg.c
+++ b/drivers/clk/renesas/rcar-gen3-cpg.c
@@ -301,95 +301,10 @@ static struct clk * __init cpg_z_clk_register(const char *name,
 	return clk;
 }

-struct rpc_clock {
-	struct clk_divider div;
-	struct clk_gate gate;
-	/*
-	 * One notifier covers both RPC and RPCD2 clocks as they are both
-	 * controlled by the same RPCCKCR register...
-	 */
-	struct cpg_simple_notifier csn;
-};
-
 static const struct clk_div_table cpg_rpcsrc_div_table[] = {
 	{ 2, 5 }, { 3, 6 }, { 0, 0 },
 };

-static const struct clk_div_table cpg_rpc_div_table[] = {
-	{ 1, 2 }, { 3, 4 }, { 5, 6 }, { 7, 8 }, { 0, 0 },
-};
-
-static struct clk * __init cpg_rpc_clk_register(const char *name,
-	void __iomem *base, const char *parent_name,
-	struct raw_notifier_head *notifiers)
-{
-	struct rpc_clock *rpc;
-	struct clk *clk;
-
-	rpc = kzalloc(sizeof(*rpc), GFP_KERNEL);
-	if (!rpc)
-		return ERR_PTR(-ENOMEM);
-
-	rpc->div.reg = base + CPG_RPCCKCR;
-	rpc->div.width = 3;
-	rpc->div.table = cpg_rpc_div_table;
-	rpc->div.lock = &cpg_lock;
-
-	rpc->gate.reg = base + CPG_RPCCKCR;
-	rpc->gate.bit_idx = 8;
-	rpc->gate.flags = CLK_GATE_SET_TO_DISABLE;
-	rpc->gate.lock = &cpg_lock;
-
-	rpc->csn.reg = base + CPG_RPCCKCR;
-
-	clk = clk_register_composite(NULL, name, &parent_name, 1, NULL, NULL,
-				     &rpc->div.hw,  &clk_divider_ops,
-				     &rpc->gate.hw, &clk_gate_ops,
-				     CLK_SET_RATE_PARENT);
-	if (IS_ERR(clk)) {
-		kfree(rpc);
-		return clk;
-	}
-
-	cpg_simple_notifier_register(notifiers, &rpc->csn);
-	return clk;
-}
-
-struct rpcd2_clock {
-	struct clk_fixed_factor fixed;
-	struct clk_gate gate;
-};
-
-static struct clk * __init cpg_rpcd2_clk_register(const char *name,
-						  void __iomem *base,
-						  const char *parent_name)
-{
-	struct rpcd2_clock *rpcd2;
-	struct clk *clk;
-
-	rpcd2 = kzalloc(sizeof(*rpcd2), GFP_KERNEL);
-	if (!rpcd2)
-		return ERR_PTR(-ENOMEM);
-
-	rpcd2->fixed.mult = 1;
-	rpcd2->fixed.div = 2;
-
-	rpcd2->gate.reg = base + CPG_RPCCKCR;
-	rpcd2->gate.bit_idx = 9;
-	rpcd2->gate.flags = CLK_GATE_SET_TO_DISABLE;
-	rpcd2->gate.lock = &cpg_lock;
-
-	clk = clk_register_composite(NULL, name, &parent_name, 1, NULL, NULL,
-				     &rpcd2->fixed.hw, &clk_fixed_factor_ops,
-				     &rpcd2->gate.hw, &clk_gate_ops,
-				     CLK_SET_RATE_PARENT);
-	if (IS_ERR(clk))
-		kfree(rpcd2);
-
-	return clk;
-}
-
-
 static const struct rcar_gen3_cpg_pll_config *cpg_pll_config __initdata;
 static unsigned int cpg_clk_extalr __initdata;
 static u32 cpg_mode __initdata;
@@ -600,11 +515,11 @@ struct clk * __init rcar_gen3_cpg_clk_register(struct device *dev,
 		break;

 	case CLK_TYPE_GEN3_RPC:
-		return cpg_rpc_clk_register(core->name, base,
+		return cpg_rpc_clk_register(core->name, base + CPG_RPCCKCR,
 					    __clk_get_name(parent), notifiers);

 	case CLK_TYPE_GEN3_RPCD2:
-		return cpg_rpcd2_clk_register(core->name, base,
+		return cpg_rpcd2_clk_register(core->name, base + CPG_RPCCKCR,
 					      __clk_get_name(parent));

 	default:

--- a/drivers/clk/renesas/rzg2l-cpg.c
+++ b/drivers/clk/renesas/rzg2l-cpg.c
@@ -17,6 +17,7 @@
 #include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/init.h>
+#include <linux/iopoll.h>
 #include <linux/mod_devicetable.h>
 #include <linux/module.h>
 #include <linux/of_address.h>
@@ -55,6 +56,14 @@
 #define GET_REG_SAMPLL_CLK1(val)	((val >> 22) & 0xfff)
 #define GET_REG_SAMPLL_CLK2(val)	((val >> 12) & 0xfff)

+struct sd_hw_data {
+	struct clk_hw hw;
+	u32 conf;
+	struct rzg2l_cpg_priv *priv;
+};
+
+#define to_sd_hw_data(_hw)	container_of(_hw, struct sd_hw_data, hw)
+
 /**
 * struct rzg2l_cpg_priv - Clock Pulse Generator Private Data
 *
@@ -130,6 +139,132 @@ rzg2l_cpg_div_clk_register(const struct cpg_core_clk *core,
 	return clk_hw->clk;
 }

+static struct clk * __init
+rzg2l_cpg_mux_clk_register(const struct cpg_core_clk *core,
+			   void __iomem *base,
+			   struct rzg2l_cpg_priv *priv)
+{
+	const struct clk_hw *clk_hw;
+
+	clk_hw = devm_clk_hw_register_mux(priv->dev, core->name,
+					  core->parent_names, core->num_parents,
+					  core->flag,
+					  base + GET_REG_OFFSET(core->conf),
+					  GET_SHIFT(core->conf),
+					  GET_WIDTH(core->conf),
+					  core->mux_flags, &priv->rmw_lock);
+	if (IS_ERR(clk_hw))
+		return ERR_CAST(clk_hw);
+
+	return clk_hw->clk;
+}
+
+static int rzg2l_cpg_sd_clk_mux_determine_rate(struct clk_hw *hw,
+					       struct clk_rate_request *req)
+{
+	return clk_mux_determine_rate_flags(hw, req, 0);
+}
+
+static int rzg2l_cpg_sd_clk_mux_set_parent(struct clk_hw *hw, u8 index)
+{
+	struct sd_hw_data *hwdata = to_sd_hw_data(hw);
+	struct rzg2l_cpg_priv *priv = hwdata->priv;
+	u32 off = GET_REG_OFFSET(hwdata->conf);
+	u32 shift = GET_SHIFT(hwdata->conf);
+	const u32 clk_src_266 = 2;
+	u32 bitmask;
+
+	/*
+	 * As per the HW manual, we should not directly switch from 533 MHz to
+	 * 400 MHz and vice versa. To change the setting from 2’b01 (533 MHz)
+	 * to 2’b10 (400 MHz) or vice versa, Switch to 2’b11 (266 MHz) first,
+	 * and then switch to the target setting (2’b01 (533 MHz) or 2’b10
+	 * (400 MHz)).
+	 * Setting a value of '0' to the SEL_SDHI0_SET or SEL_SDHI1_SET clock
+	 * switching register is prohibited.
+	 * The clock mux has 3 input clocks(533 MHz, 400 MHz, and 266 MHz), and
+	 * the index to value mapping is done by adding 1 to the index.
+	 */
+	bitmask = (GENMASK(GET_WIDTH(hwdata->conf) - 1, 0) << shift) << 16;
+	if (index != clk_src_266) {
+		u32 msk, val;
+		int ret;
+
+		writel(bitmask | ((clk_src_266 + 1) << shift), priv->base + off);
+
+		msk = off ? CPG_CLKSTATUS_SELSDHI1_STS : CPG_CLKSTATUS_SELSDHI0_STS;
+
+		ret = readl_poll_timeout(priv->base + CPG_CLKSTATUS, val,
+					 !(val & msk), 100,
+					 CPG_SDHI_CLK_SWITCH_STATUS_TIMEOUT_US);
+		if (ret) {
+			dev_err(priv->dev, "failed to switch clk source\n");
+			return ret;
+		}
+	}
+
+	writel(bitmask | ((index + 1) << shift), priv->base + off);
+
+	return 0;
+}
+
+static u8 rzg2l_cpg_sd_clk_mux_get_parent(struct clk_hw *hw)
+{
+	struct sd_hw_data *hwdata = to_sd_hw_data(hw);
+	struct rzg2l_cpg_priv *priv = hwdata->priv;
+	u32 val = readl(priv->base + GET_REG_OFFSET(hwdata->conf));
+
+	val >>= GET_SHIFT(hwdata->conf);
+	val &= GENMASK(GET_WIDTH(hwdata->conf) - 1, 0);
+	if (val) {
+		val--;
+	} else {
+		/* Prohibited clk source, change it to 533 MHz(reset value) */
+		rzg2l_cpg_sd_clk_mux_set_parent(hw, 0);
+	}
+
+	return val;
+}
+
+static const struct clk_ops rzg2l_cpg_sd_clk_mux_ops = {
+	.determine_rate = rzg2l_cpg_sd_clk_mux_determine_rate,
+	.set_parent	= rzg2l_cpg_sd_clk_mux_set_parent,
+	.get_parent	= rzg2l_cpg_sd_clk_mux_get_parent,
+};
+
+static struct clk * __init
+rzg2l_cpg_sd_mux_clk_register(const struct cpg_core_clk *core,
+			      void __iomem *base,
+			      struct rzg2l_cpg_priv *priv)
+{
+	struct sd_hw_data *clk_hw_data;
+	struct clk_init_data init;
+	struct clk_hw *clk_hw;
+	int ret;
+
+	clk_hw_data = devm_kzalloc(priv->dev, sizeof(*clk_hw_data), GFP_KERNEL);
+	if (!clk_hw_data)
+		return ERR_PTR(-ENOMEM);
+
+	clk_hw_data->priv = priv;
+	clk_hw_data->conf = core->conf;
+
+	init.name = GET_SHIFT(core->conf) ? "sd1" : "sd0";
+	init.ops = &rzg2l_cpg_sd_clk_mux_ops;
+	init.flags = 0;
+	init.num_parents = core->num_parents;
+	init.parent_names = core->parent_names;
+
+	clk_hw = &clk_hw_data->hw;
+	clk_hw->init = &init;
+
+	ret = devm_clk_hw_register(priv->dev, clk_hw);
+	if (ret)
+		return ERR_PTR(ret);
+
+	return clk_hw->clk;
+}
+
 struct pll_clk {
 	struct clk_hw hw;
 	unsigned int conf;
@@ -288,6 +423,12 @@ rzg2l_cpg_register_core_clk(const struct cpg_core_clk *core,
 		clk = rzg2l_cpg_div_clk_register(core, priv->clks,
 						 priv->base, priv);
 		break;
+	case CLK_TYPE_MUX:
+		clk = rzg2l_cpg_mux_clk_register(core, priv->base, priv);
+		break;
+	case CLK_TYPE_SD_MUX:
+		clk = rzg2l_cpg_sd_mux_clk_register(core, priv->base, priv);
+		break;
 	default:
 		goto fail;
 	}
@@ -310,13 +451,17 @@ rzg2l_cpg_register_core_clk(const struct cpg_core_clk *core,
 * @hw: handle between common and hardware-specific interfaces
 * @off: register offset
 * @bit: ON/MON bit
+ * @enabled: soft state of the clock, if it is coupled with another clock
 * @priv: CPG/MSTP private data
+ * @sibling: pointer to the other coupled clock
 */
 struct mstp_clock {
 	struct clk_hw hw;
 	u16 off;
 	u8 bit;
+	bool enabled;
 	struct rzg2l_cpg_priv *priv;
+	struct mstp_clock *sibling;
 };

 #define to_mod_clock(_hw) container_of(_hw, struct mstp_clock, hw)
@@ -369,11 +514,41 @@ static int rzg2l_mod_clock_endisable(struct clk_hw *hw, bool enable)

 static int rzg2l_mod_clock_enable(struct clk_hw *hw)
 {
+	struct mstp_clock *clock = to_mod_clock(hw);
+
+	if (clock->sibling) {
+		struct rzg2l_cpg_priv *priv = clock->priv;
+		unsigned long flags;
+		bool enabled;
+
+		spin_lock_irqsave(&priv->rmw_lock, flags);
+		enabled = clock->sibling->enabled;
+		clock->enabled = true;
+		spin_unlock_irqrestore(&priv->rmw_lock, flags);
+		if (enabled)
+			return 0;
+	}
+
 	return rzg2l_mod_clock_endisable(hw, true);
 }

 static void rzg2l_mod_clock_disable(struct clk_hw *hw)
 {
+	struct mstp_clock *clock = to_mod_clock(hw);
+
+	if (clock->sibling) {
+		struct rzg2l_cpg_priv *priv = clock->priv;
+		unsigned long flags;
+		bool enabled;
+
+		spin_lock_irqsave(&priv->rmw_lock, flags);
+		enabled = clock->sibling->enabled;
+		clock->enabled = false;
+		spin_unlock_irqrestore(&priv->rmw_lock, flags);
+		if (enabled)
+			return;
+	}
+
 	rzg2l_mod_clock_endisable(hw, false);
 }

@@ -389,9 +564,12 @@ static int rzg2l_mod_clock_is_enabled(struct clk_hw *hw)
 		return 1;
 	}

+	if (clock->sibling)
+		return clock->enabled;
+
 	value = readl(priv->base + CLK_MON_R(clock->off));

-	return !(value & bitmask);
+	return value & bitmask;
 }

 static const struct clk_ops rzg2l_mod_clock_ops = {
@@ -400,6 +578,28 @@ static const struct clk_ops rzg2l_mod_clock_ops = {
 	.is_enabled = rzg2l_mod_clock_is_enabled,
 };

+static struct mstp_clock
+*rzg2l_mod_clock__get_sibling(struct mstp_clock *clock,
+			      struct rzg2l_cpg_priv *priv)
+{
+	struct clk_hw *hw;
+	unsigned int i;
+
+	for (i = 0; i < priv->num_mod_clks; i++) {
+		struct mstp_clock *clk;
+
+		if (priv->clks[priv->num_core_clks + i] == ERR_PTR(-ENOENT))
+			continue;
+
+		hw = __clk_get_hw(priv->clks[priv->num_core_clks + i]);
+		clk = to_mod_clock(hw);
+		if (clock->off == clk->off && clock->bit == clk->bit)
+			return clk;
+	}
+
+	return NULL;
+}
+
 static void __init
 rzg2l_cpg_register_mod_clk(const struct rzg2l_mod_clk *mod,
 			   const struct rzg2l_cpg_info *info,
@@ -461,6 +661,18 @@ rzg2l_cpg_register_mod_clk(const struct rzg2l_mod_clk *mod,

 	dev_dbg(dev, "Module clock %pC at %lu Hz\n", clk, clk_get_rate(clk));
 	priv->clks[id] = clk;
+
+	if (mod->is_coupled) {
+		struct mstp_clock *sibling;
+
+		clock->enabled = rzg2l_mod_clock_is_enabled(&clock->hw);
+		sibling = rzg2l_mod_clock__get_sibling(clock, priv);
+		if (sibling) {
+			clock->sibling = sibling;
+			sibling->sibling = clock;
+		}
+	}
+
 	return;

 fail:

--- a/drivers/clk/renesas/rzg2l-cpg.h
+++ b/drivers/clk/renesas/rzg2l-cpg.h
@@ -11,6 +11,15 @@

 #define CPG_PL2_DDIV		(0x204)
 #define CPG_PL3A_DDIV		(0x208)
+#define CPG_PL2SDHI_DSEL	(0x218)
+#define CPG_CLKSTATUS		(0x280)
+#define CPG_PL3_SSEL		(0x408)
+#define CPG_PL6_ETH_SSEL	(0x418)
+
+#define CPG_CLKSTATUS_SELSDHI0_STS	BIT(28)
+#define CPG_CLKSTATUS_SELSDHI1_STS	BIT(29)
+
+#define CPG_SDHI_CLK_SWITCH_STATUS_TIMEOUT_US	20000

 /* n = 0/1/2 for PLL1/4/6 */
 #define CPG_SAMPLL_CLK1(n)	(0x04 + (16 * n))
@@ -23,6 +32,16 @@
 #define DIVPL2A		DDIV_PACK(CPG_PL2_DDIV, 0, 3)
 #define DIVPL3A		DDIV_PACK(CPG_PL3A_DDIV, 0, 3)
 #define DIVPL3B		DDIV_PACK(CPG_PL3A_DDIV, 4, 3)
+#define DIVPL3C		DDIV_PACK(CPG_PL3A_DDIV, 8, 3)
+
+#define SEL_PLL_PACK(offset, bitpos, size) \
+		(((offset) << 20) | ((bitpos) << 12) | ((size) << 8))
+
+#define SEL_PLL3_3	SEL_PLL_PACK(CPG_PL3_SSEL, 8, 1)
+#define SEL_PLL6_2	SEL_PLL_PACK(CPG_PL6_ETH_SSEL, 0, 1)
+
+#define SEL_SDHI0	DDIV_PACK(CPG_PL2SDHI_DSEL, 0, 2)
+#define SEL_SDHI1	DDIV_PACK(CPG_PL2SDHI_DSEL, 4, 2)

 /**
 * Definitions of CPG Core Clocks
@@ -43,6 +62,7 @@ struct cpg_core_clk {
 	const struct clk_div_table *dtable;
 	const char * const *parent_names;
 	int flag;
+	int mux_flags;
 	int num_parents;
 };

@@ -54,6 +74,12 @@ enum clk_types {

 	/* Clock with divider */
 	CLK_TYPE_DIV,
+
+	/* Clock with clock source selector */
+	CLK_TYPE_MUX,
+
+	/* Clock with SD clock source selector */
+	CLK_TYPE_SD_MUX,
 };

 #define DEF_TYPE(_name, _id, _type...) \
@@ -69,6 +95,14 @@ enum clk_types {
 #define DEF_DIV(_name, _id, _parent, _conf, _dtable, _flag) \
 	DEF_TYPE(_name, _id, CLK_TYPE_DIV, .conf = _conf, \
 		 .parent = _parent, .dtable = _dtable, .flag = _flag)
+#define DEF_MUX(_name, _id, _conf, _parent_names, _num_parents, _flag, \
+		_mux_flags) \
+	DEF_TYPE(_name, _id, CLK_TYPE_MUX, .conf = _conf, \
+		 .parent_names = _parent_names, .num_parents = _num_parents, \
+		 .flag = _flag, .mux_flags = _mux_flags)
+#define DEF_SD_MUX(_name, _id, _conf, _parent_names, _num_parents) \
+	DEF_TYPE(_name, _id, CLK_TYPE_SD_MUX, .conf = _conf, \
+		 .parent_names = _parent_names, .num_parents = _num_parents)

 /**
 * struct rzg2l_mod_clk - Module Clocks definitions
@@ -78,6 +112,7 @@ enum clk_types {
 * @parent: id of parent clock
 * @off: register offset
 * @bit: ON/MON bit
+ * @is_coupled: flag to indicate coupled clock
 */
 struct rzg2l_mod_clk {
 	const char *name;
@@ -85,17 +120,25 @@ struct rzg2l_mod_clk {
 	unsigned int parent;
 	u16 off;
 	u8 bit;
+	bool is_coupled;
 };

-#define DEF_MOD(_name, _id, _parent, _off, _bit)	\
+#define DEF_MOD_BASE(_name, _id, _parent, _off, _bit, _is_coupled)	\
 	{ \
 		.name = _name, \
 		.id = MOD_CLK_BASE + (_id), \
 		.parent = (_parent), \
 		.off = (_off), \
 		.bit = (_bit), \
+		.is_coupled = (_is_coupled), \
 	}

+#define DEF_MOD(_name, _id, _parent, _off, _bit)	\
+	DEF_MOD_BASE(_name, _id, _parent, _off, _bit, false)
+
+#define DEF_COUPLED(_name, _id, _parent, _off, _bit)	\
+	DEF_MOD_BASE(_name, _id, _parent, _off, _bit, true)
+
 /**
 * struct rzg2l_reset - Reset definitions
 *

--- a/drivers/clk/rockchip/clk-rk3399.c
+++ b/drivers/clk/rockchip/clk-rk3399.c
@@ -481,7 +481,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	COMPOSITE_NOMUX(0, "atclk_core_l", "armclkl", CLK_IGNORE_UNUSED,
 			RK3399_CLKSEL_CON(1), 0, 5, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3399_CLKGATE_CON(0), 5, GFLAGS),
-	COMPOSITE_NOMUX(0, "pclk_dbg_core_l", "armclkl", CLK_IGNORE_UNUSED,
+	COMPOSITE_NOMUX(PCLK_COREDBG_L, "pclk_dbg_core_l", "armclkl", CLK_IGNORE_UNUSED,
 			RK3399_CLKSEL_CON(1), 8, 5, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3399_CLKGATE_CON(0), 6, GFLAGS),

@@ -531,7 +531,7 @@ static struct rockchip_clk_branch rk3399_clk_branches[] __initdata = {
 	GATE(ACLK_GIC_ADB400_CORE_B_2_GIC, "aclk_core_adb400_core_b_2_gic", "armclkb", CLK_IGNORE_UNUSED,
 			RK3399_CLKGATE_CON(14), 4, GFLAGS),

-	DIV(0, "pclken_dbg_core_b", "pclk_dbg_core_b", CLK_IGNORE_UNUSED,
+	DIV(PCLK_COREDBG_B, "pclken_dbg_core_b", "pclk_dbg_core_b", CLK_IGNORE_UNUSED,
 			RK3399_CLKSEL_CON(3), 13, 2, DFLAGS | CLK_DIVIDER_READ_ONLY),

 	GATE(0, "pclk_dbg_cxcs_pd_core_b", "pclk_dbg_core_b", CLK_IGNORE_UNUSED,
@@ -1514,7 +1514,10 @@ static const char *const rk3399_cru_critical_clocks[] __initconst = {
 	"aclk_vio_noc",

 	/* ddrc */
-	"sclk_ddrc"
+	"sclk_ddrc",
+
+	"armclkl",
+	"armclkb",
 };

 static const char *const rk3399_pmucru_critical_clocks[] __initconst = {
@@ -1549,9 +1552,6 @@ static void __init rk3399_clk_init(struct device_node *np)
 	rockchip_clk_register_branches(ctx, rk3399_clk_branches,
 				  ARRAY_SIZE(rk3399_clk_branches));

-	rockchip_clk_protect_critical(rk3399_cru_critical_clocks,
-				      ARRAY_SIZE(rk3399_cru_critical_clocks));
-
 	rockchip_clk_register_armclk(ctx, ARMCLKL, "armclkl",
 			mux_armclkl_p, ARRAY_SIZE(mux_armclkl_p),
 			&rk3399_cpuclkl_data, rk3399_cpuclkl_rates,
@@ -1562,6 +1562,9 @@ static void __init rk3399_clk_init(struct device_node *np)
 			&rk3399_cpuclkb_data, rk3399_cpuclkb_rates,
 			ARRAY_SIZE(rk3399_cpuclkb_rates));

+	rockchip_clk_protect_critical(rk3399_cru_critical_clocks,
+				      ARRAY_SIZE(rk3399_cru_critical_clocks));
+
 	rockchip_register_softrst(np, 21, reg_base + RK3399_SOFTRST_CON(0),
 				  ROCKCHIP_SOFTRST_HIWORD_MASK);

@@ -1653,7 +1656,7 @@ static struct platform_driver clk_rk3399_driver = {
 		.suppress_bind_attrs = true,
 	},
 };
-builtin_platform_driver_probe(clk_rk3399_driver, clk_rk3399_probe);
+module_platform_driver_probe(clk_rk3399_driver, clk_rk3399_probe);

 MODULE_DESCRIPTION("Rockchip RK3399 Clock Driver");
 MODULE_LICENSE("GPL");
--- a/drivers/clk/rockchip/clk-rk3568.c
+++ b/drivers/clk/rockchip/clk-rk3568.c
@@ -1719,7 +1719,7 @@ static struct platform_driver clk_rk3568_driver = {
 		.suppress_bind_attrs = true,
 	},
 };
-builtin_platform_driver_probe(clk_rk3568_driver, clk_rk3568_probe);
+module_platform_driver_probe(clk_rk3568_driver, clk_rk3568_probe);

 MODULE_DESCRIPTION("Rockchip RK3568 Clock Driver");
 MODULE_LICENSE("GPL");