[PATCH] soc: imx: imx8m-blk-ctrl: fix i.MX8MP VPU_H1 sequence

[Peng Fan (OSS)]

From: Peng Fan <peng.fan@xxxxxxx>

Per errata:
ERR050531: VPU_NOC power down handshake may hang during VC8000E/VPUMIX
power up/down cycling.
Description: VC8000E reset de-assertion edge and AXI clock may have a
timing issue.
Workaround: Set bit2 (vc8000e_clk_en) of BLK_CLK_EN_CSR to 0 to gate off
both AXI clock and VC8000E clock sent to VC8000E and AXI clock sent to
VPU_NOC m_v_2 interface during VC8000E power up(VC8000E reset is
de-asserted by HW)

Need clear BIT2 of BLK_CLK_EN_CSR before power up VPU_H1, so
add a notifier with BIT2 cleared when GENPD_NOTIFY_PRE_ON and BIT2 set
when GENPD_NOTIFY_ON to match NXP downstream Arm Trusted Firmware
implementation.

NOTE: The NXP downstream ATF has VPU_H1 CLK SET before do ADB400 HDSK,
so follow that procdure to avoid any suprise.

Reviewed-by: Jacky Bai <ping.bai@xxxxxxx>
Signed-off-by: Peng Fan <peng.fan@xxxxxxx>
---
 drivers/soc/imx/gpcv2.c          |  3 +++
 drivers/soc/imx/imx8m-blk-ctrl.c | 28 ++++++++++++++++++++++++++++
 include/soc/imx/gpcv2.h          |  8 ++++++++
 3 files changed, 39 insertions(+)
 create mode 100644 include/soc/imx/gpcv2.h

diff --git a/drivers/soc/imx/gpcv2.c b/drivers/soc/imx/gpcv2.c
index 4b3300b090a8..81e3c09e004b 100644
--- a/drivers/soc/imx/gpcv2.c
+++ b/drivers/soc/imx/gpcv2.c
@@ -17,6 +17,7 @@
 #include <linux/regulator/consumer.h>
 #include <linux/reset.h>
 #include <linux/sizes.h>
+#include <soc/imx/gpcv2.h>
 #include <dt-bindings/power/imx7-power.h>
 #include <dt-bindings/power/imx8mq-power.h>
 #include <dt-bindings/power/imx8mm-power.h>
@@ -376,6 +377,8 @@ static int imx_pgc_power_up(struct generic_pm_domain *genpd)
 
 	reset_control_deassert(domain->reset);
 
+	raw_notifier_call_chain(&genpd->power_notifiers, IMX_GPCV2_NOTIFY_ON_ADB400, NULL);
+
 	/* request the ADB400 to power up */
 	if (domain->bits.hskreq) {
 		regmap_update_bits(domain->regmap, domain->regs->hsk,
diff --git a/drivers/soc/imx/imx8m-blk-ctrl.c b/drivers/soc/imx/imx8m-blk-ctrl.c
index 9129c97b7a01..7c4045068804 100644
--- a/drivers/soc/imx/imx8m-blk-ctrl.c
+++ b/drivers/soc/imx/imx8m-blk-ctrl.c
@@ -14,6 +14,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/regmap.h>
 #include <linux/clk.h>
+#include <soc/imx/gpcv2.h>
 
 #include <dt-bindings/power/imx8mm-power.h>
 #include <dt-bindings/power/imx8mn-power.h>
@@ -53,6 +54,7 @@ struct imx8m_blk_ctrl_domain_data {
 	 * register.
 	 */
 	u32 mipi_phy_rst_mask;
+	notifier_fn_t power_notifier_fn;
 };
 
 #define DOMAIN_MAX_CLKS 4
@@ -66,6 +68,7 @@ struct imx8m_blk_ctrl_domain {
 	struct device *power_dev;
 	struct imx8m_blk_ctrl *bc;
 	int num_paths;
+	struct notifier_block power_nb;
 };
 
 struct imx8m_blk_ctrl_data {
@@ -265,6 +268,15 @@ static int imx8m_blk_ctrl_probe(struct platform_device *pdev)
 			goto cleanup_pds;
 		}
 
+		if (data->power_notifier_fn) {
+			domain->power_nb.notifier_call = data->power_notifier_fn;
+			ret = dev_pm_genpd_add_notifier(domain->power_dev, &domain->power_nb);
+			if (ret) {
+				dev_err_probe(dev, ret, "failed to add power notifier\n");
+				goto cleanup_pds;
+			}
+		}
+
 		domain->genpd.name = data->name;
 		domain->genpd.power_on = imx8m_blk_ctrl_power_on;
 		domain->genpd.power_off = imx8m_blk_ctrl_power_off;
@@ -479,6 +491,21 @@ static const struct imx8m_blk_ctrl_data imx8mm_vpu_blk_ctl_dev_data = {
 	.num_domains = ARRAY_SIZE(imx8mm_vpu_blk_ctl_domain_data),
 };
 
+static int imx8mp_vpu_h1_power_notifier(struct notifier_block *nb,
+					unsigned long action, void *data)
+{
+	struct imx8m_blk_ctrl_domain *domain = container_of(nb, struct imx8m_blk_ctrl_domain,
+							    power_nb);
+	struct imx8m_blk_ctrl *bc = domain->bc;
+
+	if (action == GENPD_NOTIFY_PRE_ON)
+		regmap_clear_bits(bc->regmap, BLK_CLK_EN, BIT(2));
+	else if (action == IMX_GPCV2_NOTIFY_ON_ADB400)
+		regmap_set_bits(bc->regmap, BLK_CLK_EN, BIT(2));
+
+	return NOTIFY_OK;
+}
+
 static const struct imx8m_blk_ctrl_domain_data imx8mp_vpu_blk_ctl_domain_data[] = {
 	[IMX8MP_VPUBLK_PD_G1] = {
 		.name = "vpublk-g1",
@@ -509,6 +536,7 @@ static const struct imx8m_blk_ctrl_domain_data imx8mp_vpu_blk_ctl_domain_data[]
 		.clk_mask = BIT(2),
 		.path_names = (const char *[]){"vc8000e"},
 		.num_paths = 1,
+		.power_notifier_fn = imx8mp_vpu_h1_power_notifier,
 	},
 };
 
diff --git a/include/soc/imx/gpcv2.h b/include/soc/imx/gpcv2.h
new file mode 100644
index 000000000000..db09720bf638
--- /dev/null
+++ b/include/soc/imx/gpcv2.h
@@ -0,0 +1,8 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+#ifndef __SOC_IMX_GPCV2_H
+#define __SOC_IMX_GPCV2_H
+
+/* Avoid conflict with GENPD_NOTIFY_XX */
+#define IMX_GPCV2_NOTIFY_ON_ADB400	0x80000000
+
+#endif /* __SOC_IMX_GPC_H */
-- 
2.37.1