video: driver: amend power on and power off sequences

Amend power on and off sequences for iris2 video hardware.

Change-Id: Icada0b95ba5990a6911803ee9fa650a296c357db
Signed-off-by: Maheshwar Ajja <[email protected]>

driver/variant/iris2/src/msm_vidc_iris2.c

@@ -3,8 +3,6 @@
  * Copyright (c) 2020-2021,, The Linux Foundation. All rights reserved.
  */
 
-#include <linux/interrupt.h>
-
 #include "msm_vidc_iris2.h"
 #include "msm_vidc_buffer_iris2.h"
 #include "msm_vidc_power_iris2.h"
@@ -20,7 +18,8 @@
 
 #define VIDEO_ARCH_LX 1
 
-#define VBIF_BASE_OFFS_IRIS2                   0x00080000
+#define VCODEC_BASE_OFFS_IRIS2                 0x00000000
+#define AON_MVP_NOC_RESET                      0x0001F000
 #define CPU_BASE_OFFS_IRIS2                    0x000A0000
 #define AON_BASE_OFFS			               0x000E0000
 #define CPU_CS_BASE_OFFS_IRIS2		           (CPU_BASE_OFFS_IRIS2)
@@ -65,6 +64,9 @@
 /* UC_REGION_ADDR */
 #define CPU_CS_SCIBARG2_IRIS2		(CPU_CS_BASE_OFFS_IRIS2 + 0x68)
 
+#define CPU_CS_AHB_BRIDGE_SYNC_RESET            (CPU_CS_BASE_OFFS_IRIS2 + 0x160)
+#define CPU_CS_AHB_BRIDGE_SYNC_RESET_STATUS     (CPU_CS_BASE_OFFS_IRIS2 + 0x164)
+
 /* FAL10 Feature Control */
 #define CPU_CS_X2RPMh_IRIS2		(CPU_CS_BASE_OFFS_IRIS2 + 0x168)
 #define CPU_CS_X2RPMh_MASK0_BMSK_IRIS2	0x1
@@ -77,6 +79,14 @@
 #define CPU_IC_SOFTINT_IRIS2		(CPU_IC_BASE_OFFS_IRIS2 + 0x150)
 #define CPU_IC_SOFTINT_H2A_SHFT_IRIS2	0x0
 
+/*
+ * --------------------------------------------------------------------------
+ * MODULE: AON_MVP_NOC_RESET_REGISTERS
+ * --------------------------------------------------------------------------
+ */
+#define AON_WRAPPER_MVP_NOC_RESET_REQ   (AON_MVP_NOC_RESET + 0x000)
+#define AON_WRAPPER_MVP_NOC_RESET_ACK   (AON_MVP_NOC_RESET + 0x004)
+
 /*
  * --------------------------------------------------------------------------
  * MODULE: wrapper
@@ -97,6 +107,8 @@
 
 #define WRAPPER_DEBUG_BRIDGE_LPI_CONTROL_IRIS2	(WRAPPER_BASE_OFFS_IRIS2 + 0x54)
 #define WRAPPER_DEBUG_BRIDGE_LPI_STATUS_IRIS2	(WRAPPER_BASE_OFFS_IRIS2 + 0x58)
+#define WRAPPER_CORE_CLOCK_CONFIG_IRIS2		(WRAPPER_BASE_OFFS_IRIS2 + 0x88)
+
 /*
  * --------------------------------------------------------------------------
  * MODULE: tz_wrapper
@@ -131,6 +143,13 @@
 #define AON_WRAPPER_MVP_NOC_LPI_CONTROL	(AON_BASE_OFFS)
 #define AON_WRAPPER_MVP_NOC_LPI_STATUS	(AON_BASE_OFFS + 0x4)
 
+/*
+ * --------------------------------------------------------------------------
+ * MODULE: VCODEC_SS registers
+ * --------------------------------------------------------------------------
+ */
+#define VCODEC_SS_IDLE_STATUSn           (VCODEC_BASE_OFFS_IRIS2 + 0x70)
+
 /*
  * --------------------------------------------------------------------------
  * MODULE: vcodec noc error log registers (iris2)
@@ -151,6 +170,190 @@
 #define VCODEC_NOC_ERL_MAIN_ERRLOG3_LOW			0x00011238
 #define VCODEC_NOC_ERL_MAIN_ERRLOG3_HIGH		0x0001123C
 
+
+static int __disable_unprepare_clock_iris2(struct msm_vidc_core *core,
+		const char *clk_name)
+{
+	int rc = 0;
+	struct clock_info *cl;
+	bool found;
+
+	if (!core || !clk_name) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	found = false;
+	venus_hfi_for_each_clock(core, cl) {
+		if (!cl->clk) {
+			d_vpr_e("%s: invalid clock %s\n", __func__, cl->name);
+			return -EINVAL;
+		}
+		if (strcmp(cl->name, clk_name))
+			continue;
+		found = true;
+
+		clk_disable_unprepare(cl->clk);
+		cl->prev = 0;
+		d_vpr_h("%s: clock %s disable unprepared\n", __func__, cl->name);
+		break;
+	}
+	if (!found) {
+		d_vpr_e("%s: clock %s not found\n", __func__, clk_name);
+		return -EINVAL;
+	}
+
+	return rc;
+}
+
+static int __prepare_enable_clock_iris2(struct msm_vidc_core *core,
+		const char *clk_name)
+{
+	int rc = 0;
+	struct clock_info *cl;
+	bool found;
+
+	if (!core || !clk_name) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	found = false;
+	venus_hfi_for_each_clock(core, cl) {
+		if (!cl->clk) {
+			d_vpr_e("%s: invalid clock\n", __func__);
+			return -EINVAL;
+		}
+		if (strcmp(cl->name, clk_name))
+			continue;
+		found = true;
+		/*
+		 * For the clocks we control, set the rate prior to preparing
+		 * them.  Since we don't really have a load at this point, scale
+		 * it to the lowest frequency possible
+		 */
+		if (cl->has_scaling)
+			__set_clk_rate(core, cl, clk_round_rate(cl->clk, 0));
+
+		rc = clk_prepare_enable(cl->clk);
+		if (rc) {
+			d_vpr_e("%s: failed to enable clock %s\n",
+				__func__, cl->name);
+			return rc;
+		}
+		if (!__clk_is_enabled(cl->clk)) {
+			d_vpr_e("%s: clock %s not enabled\n",
+				__func__, cl->name);
+			clk_disable_unprepare(cl->clk);
+			return -EINVAL;
+		}
+		d_vpr_h("%s: clock %s prepare enabled\n", __func__, cl->name);
+		break;
+	}
+	if (!found) {
+		d_vpr_e("%s: clock %s not found\n", __func__, clk_name);
+		return -EINVAL;
+	}
+
+	return rc;
+}
+
+static int __disable_regulator_iris2(struct msm_vidc_core *core,
+		const char *reg_name)
+{
+	int rc = 0;
+	struct regulator_info *rinfo;
+	bool found;
+
+	if (!core || !reg_name) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	found = false;
+	venus_hfi_for_each_regulator(core, rinfo) {
+		if (!rinfo->regulator) {
+			d_vpr_e("%s: invalid regulator %s\n",
+				__func__, rinfo->name);
+			return -EINVAL;
+		}
+		if (strcmp(rinfo->name, reg_name))
+			continue;
+		found = true;
+
+		rc = __acquire_regulator(core, rinfo);
+		if (rc) {
+			d_vpr_e("%s: failed to acquire %s, rc = %d\n",
+				rinfo->name, rc);
+			/* Bring attention to this issue */
+			WARN_ON(true);
+			return rc;
+		}
+		core->handoff_done = false;
+
+		rc = regulator_disable(rinfo->regulator);
+		if (rc) {
+			d_vpr_e("%s: failed to disable %s, rc = %d\n",
+				rinfo->name, rc);
+			return rc;
+		}
+		d_vpr_h("%s: disabled regulator %s\n", __func__, rinfo->name);
+		break;
+	}
+	if (!found) {
+		d_vpr_e("%s: regulator %s not found\n", __func__, reg_name);
+		return -EINVAL;
+	}
+
+	return rc;
+}
+
+static int __enable_regulator_iris2(struct msm_vidc_core *core,
+		const char *reg_name)
+{
+	int rc = 0;
+	struct regulator_info *rinfo;
+	bool found;
+
+	if (!core || !reg_name) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	found = false;
+	venus_hfi_for_each_regulator(core, rinfo) {
+		if (!rinfo->regulator) {
+			d_vpr_e("%s: invalid regulator %s\n",
+				__func__, rinfo->name);
+			return -EINVAL;
+		}
+		if (strcmp(rinfo->name, reg_name))
+			continue;
+		found = true;
+
+		rc = regulator_enable(rinfo->regulator);
+		if (rc) {
+			d_vpr_e("%s: failed to enable %s, rc = %d\n",
+				__func__, rinfo->name, rc);
+			return rc;
+		}
+		if (!regulator_is_enabled(rinfo->regulator)) {
+			d_vpr_e("%s: regulator %s not enabled\n",
+				__func__, rinfo->name);
+			regulator_disable(rinfo->regulator);
+			return -EINVAL;
+		}
+		d_vpr_h("%s: enabled regulator %s\n", __func__, rinfo->name);
+		break;
+	}
+	if (!found) {
+		d_vpr_e("%s: regulator %s not found\n", __func__, reg_name);
+		return -EINVAL;
+	}
+
+	return rc;
+}
+
 static int __interrupt_init_iris2(struct msm_vidc_core *vidc_core)
 {
 	struct msm_vidc_core *core = vidc_core;
@@ -226,100 +429,341 @@ static int __setup_ucregion_memory_map_iris2(struct msm_vidc_core *vidc_core)
 	return 0;
 }
 
-static int __power_off_iris2(struct msm_vidc_core *vidc_core)
+static int __power_off_iris2_hardware(struct msm_vidc_core *core)
 {
-	u32 lpi_status, reg_status = 0, count = 0, max_count = 10;
-	struct msm_vidc_core *core = vidc_core;
-	int rc = 0;
+	int rc = 0, i;
+	u32 value = 0, count = 0;
+	const u32 max_count = 10;
 
-	if (!core) {
-		d_vpr_e("%s: invalid params\n", __func__);
-		return -EINVAL;
+	if (core->hw_power_control) {
+		d_vpr_h("%s: hardware power control enabled\n", __func__);
+		goto disable_power;
 	}
 
-	if (!core->power_enabled)
-		return 0;
+	/*
+	 * check to make sure core clock branch enabled else
+	 * we cannot read vcodec top idle register
+	 */
+	value = __read_register(core, WRAPPER_CORE_CLOCK_CONFIG_IRIS2);
+	if (value) {
+		d_vpr_h(
+			"%s: core clock config not enabled, enabling it to read vcodec registers\n",
+			__func__);
+		rc = __write_register(core, WRAPPER_CORE_CLOCK_CONFIG_IRIS2, 0);
+		if (rc)
+			return rc;
+	}
 
-	if (!(core->intr_status & WRAPPER_INTR_STATUS_A2HWD_BMSK_IRIS2))
-		disable_irq_nosync(core->dt->irq);
-	core->intr_status = 0;
+	/*
+	 * add MNoC idle check before collapsing MVS0 per HPG update
+	 * poll for NoC DMA idle -> HPG 6.1.1
+	 */
+	for (i = 0; i < core->capabilities[NUM_VPP_PIPE].value; i++) {
+		count = 0;
+		do {
+			value = __read_register(core,
+				VCODEC_SS_IDLE_STATUSn + 4*i);
+			if (value & 0x400000)
+				break;
+			else
+				usleep_range(1000, 2000);
+			count++;
+		} while (count < max_count);
+
+		if (count == max_count)
+			d_vpr_e(
+				"%s: VCODEC_SS_IDLE_STATUSn (%d) is not idle (%#x)\n",
+				__func__, i, value);
+	}
 
-	/* HPG 6.1.2 Step 1  */
-	rc = __write_register(core, CPU_CS_X2RPMh_IRIS2, 0x3);
+	/* Apply partial reset on MSF interface and wait for ACK */
+	rc = __write_register(core, AON_WRAPPER_MVP_NOC_RESET_REQ, 0x3);
 	if (rc)
 		return rc;
+	count = 0;
+	do {
+		value = __read_register(core, AON_WRAPPER_MVP_NOC_RESET_ACK);
+		if ((value & 0x3) == 0x3)
+			break;
+		else
+			usleep_range(100, 200);
+		count++;
+	} while (count < max_count);
+	if (count == max_count)
+		d_vpr_e("%s: AON_WRAPPER_MVP_NOC_RESET assert failed\n",
+			__func__);
 
-	/* HPG 6.1.2 Step 2, noc to low power */
-	//if (core->res->vpu_ver == VPU_VERSION_IRIS2_1)
-	//	goto skip_aon_mvp_noc;
+	/* De-assert partial reset on MSF interface and wait for ACK */
+	rc = __write_register(core, AON_WRAPPER_MVP_NOC_RESET_REQ, 0x0);
+	if (rc)
+		return rc;
+	count = 0;
+	do {
+		value = __read_register(core, AON_WRAPPER_MVP_NOC_RESET_ACK);
+		if ((value & 0x3) == 0x0)
+			break;
+		else
+			usleep_range(100, 200);
+		count++;
+	} while (count < max_count);
+	if (count == max_count)
+		d_vpr_e("%s: AON_WRAPPER_MVP_NOC_RESET de-assert failed\n",
+			__func__);
+
+	/*
+	 * Reset both sides of 2 ahb2ahb_bridges (TZ and non-TZ)
+	 * do we need to check status register here?
+	 */
+	rc = __write_register(core, CPU_CS_AHB_BRIDGE_SYNC_RESET, 0x3);
+	if (rc)
+		return rc;
+	rc = __write_register(core, CPU_CS_AHB_BRIDGE_SYNC_RESET, 0x2);
+	if (rc)
+		return rc;
+	rc = __write_register(core, CPU_CS_AHB_BRIDGE_SYNC_RESET, 0x0);
+	if (rc)
+		return rc;
+
+disable_power:
+	/* power down process */
+	rc = __disable_regulator_iris2(core, "vcodec");
+	if (rc) {
+		d_vpr_e("%s: disable regulator vcodec failed\n", __func__);
+		rc = 0;
+	}
+	rc = __disable_unprepare_clock_iris2(core, "vcodec_clk");
+	if (rc) {
+		d_vpr_e("%s: disable unprepare vcodec_clk failed\n", __func__);
+		rc = 0;
+	}
+
+	return rc;
+}
+
+static int __power_off_iris2_controller(struct msm_vidc_core *core)
+{
+	int rc = 0;
+	u32 value = 0, count = 0;
+	const u32 max_count = 10;
 
-	rc = __write_register(core, AON_WRAPPER_MVP_NOC_LPI_CONTROL, 0x1);
+	/*
+	 * mask fal10_veto QLPAC error since fal10_veto can go 1
+	 * when pwwait == 0 and clamped to 0 -> HPG 6.1.2
+	 */
+	rc = __write_register(core, CPU_CS_X2RPMh_IRIS2, 0x3);
 	if (rc)
 		return rc;
 
-	while (!reg_status && count < max_count) {
-		lpi_status =
-			 __read_register(core,
-				AON_WRAPPER_MVP_NOC_LPI_STATUS);
-		reg_status = lpi_status & BIT(0);
-		d_vpr_l("Noc: lpi_status %d noc_status %d (count %d)\n",
-			lpi_status, reg_status, count);
-		usleep_range(50, 100);
+	/* set MNoC to low power, set PD_NOC_QREQ (bit 0) */
+	rc = __write_register_masked(core, AON_WRAPPER_MVP_NOC_LPI_CONTROL,
+			0x1, BIT(0));
+	if (rc)
+		return rc;
+	count = 0;
+	do {
+		value = __read_register(core, AON_WRAPPER_MVP_NOC_LPI_STATUS);
+		if ((value & 0x1) == 0x1)
+			break;
+		else
+			usleep_range(100, 200);
 		count++;
-	}
+	} while (count < max_count);
 	if (count == max_count)
-		d_vpr_e("NOC not in qaccept status %d\n", reg_status);
+		d_vpr_e("%s: AON_WRAPPER_MVP_NOC_LPI_CONTROL failed\n",
+			__func__);
 
-//skip_aon_mvp_noc:
-	/* HPG 6.1.2 Step 3, debug bridge to low power */
+	/* Set Debug bridge Low power */
 	rc = __write_register(core, WRAPPER_DEBUG_BRIDGE_LPI_CONTROL_IRIS2, 0x7);
 	if (rc)
 		return rc;
-
-	reg_status = 0;
 	count = 0;
-	while ((reg_status != 0x7) && count < max_count) {
-		lpi_status = __read_register(core,
-				 WRAPPER_DEBUG_BRIDGE_LPI_STATUS_IRIS2);
-		reg_status = lpi_status & 0x7;
-		d_vpr_l("DBLP Set : lpi_status %d reg_status %d (count %d)\n",
-			lpi_status, reg_status, count);
-		usleep_range(50, 100);
+	do {
+		value = __read_register(core,
+			WRAPPER_DEBUG_BRIDGE_LPI_STATUS_IRIS2);
+		if ((value & 0x7) == 0x7)
+			break;
+		else
+			usleep_range(100, 200);
 		count++;
-	}
+	} while (count < max_count);
 	if (count == max_count)
-		d_vpr_e("DBLP Set: status %d\n", reg_status);
+		d_vpr_e("%s: debug bridge low power failed\n", __func__);
 
-	/* HPG 6.1.2 Step 4, debug bridge to lpi release */
+	/* Debug bridge LPI release */
 	rc = __write_register(core, WRAPPER_DEBUG_BRIDGE_LPI_CONTROL_IRIS2, 0x0);
 	if (rc)
 		return rc;
 
-	lpi_status = 0x1;
 	count = 0;
-	while (lpi_status && count < max_count) {
-		lpi_status = __read_register(core,
-				 WRAPPER_DEBUG_BRIDGE_LPI_STATUS_IRIS2);
-		d_vpr_l("DBLP Release: lpi_status %d(count %d)\n",
-			lpi_status, count);
-		usleep_range(50, 100);
+	do {
+		value = __read_register(core,
+			WRAPPER_DEBUG_BRIDGE_LPI_STATUS_IRIS2);
+		if (value == 0x0)
+			break;
+		else
+			usleep_range(100, 200);
 		count++;
-	}
+	} while (count < max_count);
 	if (count == max_count)
-		d_vpr_e("DBLP Release: lpi_status %d\n", lpi_status);
+		d_vpr_e("%s: debug bridge release failed\n", __func__);
+
+	/* power down process */
+	rc = __disable_regulator_iris2(core, "iris-ctl");
+	if (rc) {
+		d_vpr_e("%s: disable regulator iris-ctl failed\n", __func__);
+		rc = 0;
+	}
+
+	/* Disable GCC_VIDEO_AXI0_CLK clock */
+	rc = __disable_unprepare_clock_iris2(core, "gcc_video_axi0");
+	if (rc) {
+		d_vpr_e("%s: disable unprepare gcc_video_axi0 failed\n", __func__);
+		rc = 0;
+	}
 
-	/* HPG 6.1.2 Step 6 */
-	__disable_unprepare_clks(core);
+	/* Turn off MVP MVS0C core clock */
+	rc = __disable_unprepare_clock_iris2(core, "core_clk");
+	if (rc) {
+		d_vpr_e("%s: disable unprepare core_clk failed\n", __func__);
+		rc = 0;
+	}
 
-	/* HPG 6.1.2 Step 5 */
-	if (__disable_regulators(core))
-		d_vpr_e("%s: Failed to disable regulators\n", __func__);
+	return rc;
+}
+
+static int __power_off_iris2(struct msm_vidc_core *core)
+{
+	int rc = 0;
+
+	if (!core || !core->capabilities) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	if (!core->power_enabled)
+		return 0;
+
+	if (__power_off_iris2_hardware(core))
+		d_vpr_e("%s: failed to power off hardware\n", __func__);
+
+	if (__power_off_iris2_controller(core))
+		d_vpr_e("%s: failed to power off controller\n", __func__);
 
 	if (__unvote_buses(core))
-		d_vpr_e("%s: Failed to unvote for buses\n", __func__);
+		d_vpr_e("%s: failed to unvote buses\n", __func__);
+
+	if (!(core->intr_status & WRAPPER_INTR_STATUS_A2HWD_BMSK_IRIS2))
+		disable_irq_nosync(core->dt->irq);
+	core->intr_status = 0;
+
 	core->power_enabled = false;
 
+	return rc;
+}
+
+static int __power_on_iris2_controller(struct msm_vidc_core *core)
+{
+	int rc = 0;
+
+	rc = __enable_regulator_iris2(core, "iris-ctl");
+	if (rc)
+		goto fail_regulator;
+
+	rc = call_venus_op(core, reset_ahb2axi_bridge, core);
+	if (rc)
+		goto fail_reset_ahb2axi;
+
+	rc = __prepare_enable_clock_iris2(core, "gcc_video_axi0");
+	if (rc)
+		goto fail_clk_axi;
+
+	rc = __prepare_enable_clock_iris2(core, "core_clk");
+	if (rc)
+		goto fail_clk_controller;
+
+	return 0;
+
+fail_clk_controller:
+	__disable_unprepare_clock_iris2(core, "gcc_video_axi0");
+fail_clk_axi:
+fail_reset_ahb2axi:
+	__disable_regulator_iris2(core, "iris-ctl");
+fail_regulator:
+	return rc;
+}
+
+static int __power_on_iris2_hardware(struct msm_vidc_core *core)
+{
+	int rc = 0;
+
+	rc = __enable_regulator_iris2(core, "vcodec");
+	if (rc)
+		goto fail_regulator;
+
+	rc = __prepare_enable_clock_iris2(core, "vcodec_clk");
+	if (rc)
+		goto fail_clk_controller;
+
 	return 0;
+
+fail_clk_controller:
+	__disable_regulator_iris2(core, "vcodec");
+fail_regulator:
+	return rc;
+}
+
+static int __power_on_iris2(struct msm_vidc_core *core)
+{
+	int rc = 0;
+
+	if (core->power_enabled)
+		return 0;
+
+	/* Vote for all hardware resources */
+	rc = __vote_buses(core, INT_MAX, INT_MAX);
+	if (rc) {
+		d_vpr_e("%s: failed to vote buses, rc %d\n", __func__, rc);
+		goto fail_vote_buses;
+	}
+
+	rc = __power_on_iris2_controller(core);
+	if (rc) {
+		d_vpr_e("%s: failed to power on iris2 controller\n", __func__);
+		goto fail_power_on_controller;
+	}
+
+	rc = __power_on_iris2_hardware(core);
+	if (rc) {
+		d_vpr_e("%s: failed to power on iris2 hardware\n", __func__);
+		goto fail_power_on_hardware;
+	}
+	/* video controller and hardware powered on successfully */
+	core->power_enabled = true;
+
+	rc = __scale_clocks(core);
+	if (rc) {
+		d_vpr_e("%s: failed to scale clocks\n", __func__);
+		rc = 0;
+	}
+	/*
+	 * Re-program all of the registers that get reset as a result of
+	 * regulator_disable() and _enable()
+	 */
+	__set_registers(core);
+
+	call_venus_op(core, interrupt_init, core);
+	core->intr_status = 0;
+	enable_irq(core->dt->irq);
+
+	return rc;
+
+fail_power_on_hardware:
+	__power_off_iris2_controller(core);
+fail_power_on_controller:
+	__unvote_buses(core);
+fail_vote_buses:
+	core->power_enabled = false;
+	return rc;
 }
 
 static int __prepare_pc_iris2(struct msm_vidc_core *vidc_core)
@@ -684,6 +1128,7 @@ static struct msm_vidc_venus_ops iris2_ops = {
 	.setup_ucregion_memmap = __setup_ucregion_memory_map_iris2,
 	.clock_config_on_enable = NULL,
 	.reset_ahb2axi_bridge = __reset_ahb2axi_bridge,
+	.power_on = __power_on_iris2,
 	.power_off = __power_off_iris2,
 	.prepare_pc = __prepare_pc_iris2,
 	.watchdog = __watchdog_iris2,

driver/vidc/inc/msm_vidc_core.h

@@ -25,6 +25,7 @@ struct msm_vidc_venus_ops {
 	int (*raise_interrupt)(struct msm_vidc_core *core);
 	int (*clear_interrupt)(struct msm_vidc_core *core);
 	int (*prepare_pc)(struct msm_vidc_core *core);
+	int (*power_on)(struct msm_vidc_core *core);
 	int (*power_off)(struct msm_vidc_core *core);
 	int (*watchdog)(struct msm_vidc_core *core, u32 intr_status);
 	int (*noc_error_info)(struct msm_vidc_core *core);
@@ -110,7 +111,8 @@ struct msm_vidc_core {
 	u32                                    header_id;
 	u32                                    packet_id;
 	struct completion                      init_done;
-	u32                                    handoff_done;
+	bool                                   handoff_done;
+	bool                                   hw_power_control;
 };
 
 #endif // _MSM_VIDC_CORE_H_

driver/vidc/inc/venus_hfi.h

@@ -7,7 +7,11 @@
 #define _VENUS_HFI_H_
 
 #include <linux/irqreturn.h>
+#include <linux/clk.h>
+#include <linux/regulator/consumer.h>
+#include <linux/clk-provider.h>
 
+#include "msm_vidc_dt.h"
 #include "msm_vidc_internal.h"
 #include "msm_vidc_inst.h"
 #include "msm_vidc_core.h"
@@ -58,6 +62,8 @@ void venus_hfi_work_handler(struct work_struct *work);
 void venus_hfi_pm_work_handler(struct work_struct *work);
 irqreturn_t venus_hfi_isr(int irq, void *data);
 
+int __write_register_masked(struct msm_vidc_core *core,
+		u32 reg, u32 value, u32 mask);
 int __write_register(struct msm_vidc_core *core,
 		u32 reg, u32 value);
 int __read_register(struct msm_vidc_core *core, u32 reg);
@@ -65,16 +71,27 @@ int __iface_cmdq_write(struct msm_vidc_core *core,
 	void *pkt);
 int __iface_msgq_read(struct msm_vidc_core *core, void *pkt);
 int __iface_dbgq_read(struct msm_vidc_core *core, void *pkt);
+int __scale_clocks(struct msm_vidc_core *core);
+int __set_clk_rate(struct msm_vidc_core *core,
+	struct clock_info *cl, u64 rate);
 void __disable_unprepare_clks(struct msm_vidc_core *core);
+int __prepare_enable_clks(struct msm_vidc_core *core);
 int __disable_regulators(struct msm_vidc_core *core);
+int __enable_regulators(struct msm_vidc_core *core);
+int __acquire_regulator(struct msm_vidc_core *core,
+	struct regulator_info *rinfo);
 int __unvote_buses(struct msm_vidc_core *core);
+int __vote_buses(struct msm_vidc_core *core, unsigned long bw_ddr,
+	unsigned long bw_llcc);
 int __prepare_pc(struct msm_vidc_core *core);
+int __set_registers(struct msm_vidc_core *core);
 
 int __reset_ahb2axi_bridge(struct msm_vidc_core *core);
 int __clock_config_on_enable(struct msm_vidc_core *core);
 int __interrupt_init(struct msm_vidc_core *core);
 int __setup_ucregion_memmap(struct msm_vidc_core *core);
 int __raise_interrupt(struct msm_vidc_core *core);
+int __power_on(struct msm_vidc_core *core);
 int __power_off(struct msm_vidc_core *core);
 bool __core_in_valid_state(struct msm_vidc_core *core);
 int __load_fw(struct msm_vidc_core *core);

driver/vidc/src/venus_hfi.c

@@ -3,9 +3,6 @@
  * Copyright (c) 2020-2021, The Linux Foundation. All rights reserved.
  */
 
-#include <linux/clk.h>
-#include <linux/regulator/consumer.h>
-#include <linux/clk-provider.h>
 #include <linux/iommu.h>
 #include <linux/qcom_scm.h>
 #include <linux/soc/qcom/smem.h>
@@ -21,7 +18,6 @@
 #include "venus_hfi.h"
 #include "msm_vidc_core.h"
 #include "msm_vidc_power.h"
-#include "msm_vidc_dt.h"
 #include "msm_vidc_platform.h"
 #include "msm_vidc_memory.h"
 #include "msm_vidc_driver.h"
@@ -267,7 +263,7 @@ int __write_register(struct msm_vidc_core *core,
  * only bits 0 & 4 will be updated with corresponding bits from value. To update
  * entire register with value, set mask = 0xFFFFFFFF.
  */
-static int __write_register_masked(struct msm_vidc_core *core,
+int __write_register_masked(struct msm_vidc_core *core,
 		u32 reg, u32 value, u32 mask)
 {
 	u32 prev_val, new_val;
@@ -373,11 +369,16 @@ static void __cancel_power_collapse_work(struct msm_vidc_core *core)
 	cancel_delayed_work(&core->pm_work);
 }
 
-static int __acquire_regulator(struct msm_vidc_core *core,
+int __acquire_regulator(struct msm_vidc_core *core,
 	struct regulator_info *rinfo)
 {
 	int rc = 0;
 
+	if (!core || !rinfo) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
 	if (rinfo->has_hw_power_collapse) {
 		if (!rinfo->regulator) {
 			d_vpr_e("%s: invalid regulator\n", __func__);
@@ -387,6 +388,7 @@ static int __acquire_regulator(struct msm_vidc_core *core,
 
 		if (regulator_get_mode(rinfo->regulator) ==
 				REGULATOR_MODE_NORMAL) {
+			core->handoff_done = false;
 			d_vpr_h("Skip acquire regulator %s\n", rinfo->name);
 			goto exit;
 		}
@@ -403,7 +405,7 @@ static int __acquire_regulator(struct msm_vidc_core *core,
 				rinfo->name);
 			goto exit;
 		} else {
-
+			core->handoff_done = false;
 			d_vpr_h("Acquired regulator control from HW: %s\n",
 					rinfo->name);
 
@@ -420,6 +422,17 @@ exit:
 	return rc;
 }
 
+static int __acquire_regulators(struct msm_vidc_core *core)
+{
+	int rc = 0;
+	struct regulator_info *rinfo;
+
+	venus_hfi_for_each_regulator(core, rinfo)
+		__acquire_regulator(core, rinfo);
+
+	return rc;
+}
+
 static int __hand_off_regulator(struct msm_vidc_core *core,
 	struct regulator_info *rinfo)
 {
@@ -434,12 +447,11 @@ static int __hand_off_regulator(struct msm_vidc_core *core,
 		rc = regulator_set_mode(rinfo->regulator,
 				REGULATOR_MODE_FAST);
 		if (rc) {
-			core->handoff_done = 0;
 			d_vpr_e("Failed to hand off regulator control: %s\n",
 				rinfo->name);
 			return rc;
 		} else {
-			core->handoff_done = 1;
+			core->handoff_done = true;
 			d_vpr_h("Hand off regulator control to HW: %s\n",
 					rinfo->name);
 		}
@@ -478,7 +490,7 @@ err_reg_handoff_failed:
 	return rc;
 }
 
-static int __set_registers(struct msm_vidc_core *core)
+int __set_registers(struct msm_vidc_core *core)
 {
 	struct reg_set *reg_set;
 	int i, rc = 0;
@@ -524,6 +536,11 @@ int __unvote_buses(struct msm_vidc_core *core)
 	int rc = 0;
 	struct bus_info *bus = NULL;
 
+	if (!core) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
 	core->power.bw_ddr = 0;
 	core->power.bw_llcc = 0;
 
@@ -537,7 +554,7 @@ err_unknown_device:
 	return rc;
 }
 
-static int __vote_buses(struct msm_vidc_core *core,
+int __vote_buses(struct msm_vidc_core *core,
 		unsigned long bw_ddr, unsigned long bw_llcc)
 {
 	int rc = 0;
@@ -545,6 +562,11 @@ static int __vote_buses(struct msm_vidc_core *core,
 	unsigned long bw_kbps = 0, bw_prev = 0;
 	enum vidc_bus_type type;
 
+	if (!core) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
 	venus_hfi_for_each_bus(core, bus) {
 		if (bus && bus->path) {
 			type = get_type_frm_name(bus->name);
@@ -599,17 +621,19 @@ static int __tzbsp_set_video_state(enum tzbsp_video_state state)
 	return 0;
 }
 
-static int __set_clk_rate(struct msm_vidc_core *core,
+int __set_clk_rate(struct msm_vidc_core *core,
 		struct clock_info *cl, u64 rate)
 {
 	int rc = 0;
-	// struct clk *clk = cl->clk;
 	struct mmrm_client_data client_data;
-	struct mmrm_client *client = cl->mmrm_client;
+	struct mmrm_client *client;
 
 	/* not registered */
-	if (!client)
+	if (!core || !cl || !cl->mmrm_client) {
+		d_vpr_e("%s: invalid params\n", __func__);
 		return -EINVAL;
+	}
+	client = cl->mmrm_client;
 
 	/* bail early if requested clk rate is not changed */
 	if (rate == cl->prev)
@@ -644,12 +668,17 @@ static int __set_clocks(struct msm_vidc_core *core, u32 freq)
 	return 0;
 }
 
-static int __scale_clocks(struct msm_vidc_core *core)
+int __scale_clocks(struct msm_vidc_core *core)
 {
 	int rc = 0;
 	struct allowed_clock_rates_table *allowed_clks_tbl;
 	u32 freq = 0;
 
+	if (!core || !core->dt) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
 	allowed_clks_tbl = core->dt->allowed_clks_tbl;
 	freq = core->power.clk_freq ? core->power.clk_freq :
 		allowed_clks_tbl[0].clock_rate;
@@ -1128,10 +1157,14 @@ static int __sys_set_power_control(struct msm_vidc_core *core, bool enable)
 {
 	int rc = 0;
 
-	if (!core->handoff_done)
+	if (!core->handoff_done) {
+		d_vpr_e("%s: skipping as power control hanfoff was not done\n",
+			__func__);
 		return rc;
+	}
 
-	rc = hfi_packet_sys_intraframe_powercollapse(core, core->packet, core->packet_size, enable);
+	rc = hfi_packet_sys_intraframe_powercollapse(core,
+		core->packet, core->packet_size, enable);
 	if (rc)
 		return rc;
 
@@ -1139,6 +1172,9 @@ static int __sys_set_power_control(struct msm_vidc_core *core, bool enable)
 	if (rc)
 		return rc;
 
+	core->hw_power_control = true;
+	d_vpr_h("%s: set hardware power control successful\n", __func__);
+
 	return rc;
 }
 
@@ -1530,7 +1566,7 @@ failed_to_reset:
 	return rc;
 }
 
-static int __prepare_enable_clks(struct msm_vidc_core *core)
+int __prepare_enable_clks(struct msm_vidc_core *core)
 {
 	struct clock_info *cl = NULL;
 	int rc = 0, c = 0;
@@ -1837,8 +1873,6 @@ static int __disable_regulator(struct regulator_info *rinfo,
 		goto disable_regulator_failed;
 	}
 
-	core->handoff_done = 0;
-
 	if (!regulator_is_enabled(rinfo->regulator))
 		d_vpr_e("%s: regulator %s already disabled\n",
 			__func__, rinfo->name);
@@ -1858,23 +1892,15 @@ disable_regulator_failed:
 	return rc;
 }
 
-static int __enable_hw_power_collapse(struct msm_vidc_core *core)
-{
-	int rc = 0;
-
-	rc = __hand_off_regulators(core);
-	if (rc)
-		d_vpr_e("%s: Failed to enable HW power collapse %d\n",
-			__func__, rc);
-
-	return rc;
-}
-
-static int __enable_regulators(struct msm_vidc_core *core)
+int __enable_regulators(struct msm_vidc_core *core)
 {
 	int rc = 0, c = 0;
 	struct regulator_info *rinfo;
 
+	if (!core) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
 	d_vpr_h("Enabling regulators\n");
 
 	venus_hfi_for_each_regulator(core, rinfo) {
@@ -2153,61 +2179,16 @@ static int __venus_power_on(struct msm_vidc_core *core)
 {
 	int rc = 0;
 
-	if (core->power_enabled) {
-		d_vpr_e("%s: Skip power on, core already enabled.\n", __func__);
+	if (core->power_enabled)
 		return 0;
-	}
-
-	core->power_enabled = true;
-	/* Vote for all hardware resources */
-	rc = __vote_buses(core, INT_MAX, INT_MAX);
-	if (rc) {
-		d_vpr_e("Failed to vote buses, err: %d\n", rc);
-		goto fail_vote_buses;
-	}
 
-	rc = __enable_regulators(core);
+	rc = call_venus_op(core, power_on, core);
 	if (rc) {
-		d_vpr_e("Failed to enable GDSC, err = %d\n", rc);
-		goto fail_enable_gdsc;
-	}
-
-	rc = call_venus_op(core, reset_ahb2axi_bridge, core);
-	if (rc) {
-		d_vpr_e("Failed to reset ahb2axi: %d\n", rc);
-		goto fail_enable_clks;
-	}
-
-	rc = __prepare_enable_clks(core);
-	if (rc) {
-		d_vpr_e("Failed to enable clocks: %d\n", rc);
-		goto fail_enable_clks;
-	}
-
-	rc = __scale_clocks(core);
-	if (rc) {
-		d_vpr_e("Failed to scale clocks, performance might be affected\n");
-		rc = 0;
+		d_vpr_e("Failed to power on, err: %d\n", rc);
+		return rc;
 	}
+	core->power_enabled = true;
 
-	/*
-	 * Re-program all of the registers that get reset as a result of
-	 * regulator_disable() and _enable()
-	 */
-	__set_registers(core);
-
-	call_venus_op(core, interrupt_init, core);
-	core->intr_status = 0;
-	enable_irq(core->dt->irq);
-
-	return rc;
-
-fail_enable_clks:
-	__disable_regulators(core);
-fail_enable_gdsc:
-	__unvote_buses(core);
-fail_vote_buses:
-	core->power_enabled = false;
 	return rc;
 }
 
@@ -2237,7 +2218,7 @@ static int __suspend(struct msm_vidc_core *core)
 
 	__disable_subcaches(core);
 
-	call_venus_op(core, power_off, core);
+	__venus_power_off(core);
 	d_vpr_h("Venus power off\n");
 	return rc;
 
@@ -2264,6 +2245,9 @@ static int __resume(struct msm_vidc_core *core)
 		return rc;
 
 	d_vpr_h("Resuming from power collapse\n");
+	core->handoff_done = false;
+	core->hw_power_control = false;
+
 	rc = __venus_power_on(core);
 	if (rc) {
 		d_vpr_e("Failed to power on venus\n");
@@ -2283,8 +2267,7 @@ static int __resume(struct msm_vidc_core *core)
 	 * (s/w triggered) to fast (HW triggered) unless the h/w vote is
 	 * present.
 	 */
-	if (__enable_hw_power_collapse(core))
-		d_vpr_e("Failed to enabled inter-frame PC\n");
+	__hand_off_regulators(core);
 
 	call_venus_op(core, setup_ucregion_memmap, core);
 
@@ -2304,7 +2287,12 @@ static int __resume(struct msm_vidc_core *core)
 	}
 	__set_subcaches(core);
 
-	__sys_set_power_control(core, true);
+	rc = __sys_set_power_control(core, true);
+	if (rc) {
+		d_vpr_e("%s: set power control failed\n", __func__);
+		__acquire_regulators(core);
+		rc = 0;
+	}
 
 	d_vpr_h("Resumed from power collapse\n");
 exit:
@@ -2315,7 +2303,7 @@ exit:
 err_reset_core:
 	__tzbsp_set_video_state(TZBSP_VIDEO_STATE_SUSPEND);
 err_set_video_state:
-	call_venus_op(core, power_off, core);
+	__venus_power_off(core);
 err_venus_power_on:
 	d_vpr_e("Failed to resume from power collapse\n");
 	return rc;
@@ -2588,6 +2576,10 @@ int __load_fw(struct msm_vidc_core *core)
 {
 	int rc = 0;
 
+	d_vpr_h("%s\n", __func__);
+	core->handoff_done = false;
+	core->hw_power_control = false;
+
 	rc = __init_resources(core);
 	if (rc) {
 		d_vpr_e("%s: Failed to init resources: %d\n", __func__, rc);
@@ -2624,7 +2616,7 @@ int __load_fw(struct msm_vidc_core *core)
 	* (s/w triggered) to fast (HW triggered) unless the h/w vote is
 	* present.
 	*/
-	__enable_hw_power_collapse(core);
+	__hand_off_regulators(core);
 
 	return rc;
 fail_protect_mem:
@@ -2632,7 +2624,7 @@ fail_protect_mem:
 		qcom_scm_pas_shutdown(core->dt->fw_cookie);
 	core->dt->fw_cookie = 0;
 fail_load_fw:
-	call_venus_op(core, power_off, core);
+	__venus_power_off(core);
 fail_venus_power_on:
 	__deinit_resources(core);
 fail_init_res:
@@ -2736,6 +2728,7 @@ void venus_hfi_pm_work_handler(struct work_struct *work)
 		return;
 	}
 
+	d_vpr_h("%s: try power collapse\n", __func__);
 	/*
 	 * It is ok to check this variable outside the lock since
 	 * it is being updated in this context only
@@ -2819,8 +2812,6 @@ int venus_hfi_core_init(struct msm_vidc_core *core)
 	if (rc)
 		return rc;
 
-	core->handoff_done = 0;
-
 	rc = __load_fw(core);
 	if (rc)
 		goto error;
@@ -2853,7 +2844,12 @@ int venus_hfi_core_init(struct msm_vidc_core *core)
 	if (rc)
 		goto error;
 
-	__sys_set_power_control(core, true);
+	rc = __sys_set_power_control(core, true);
+	if (rc) {
+		d_vpr_e("%s: set power control failed\n", __func__);
+		__acquire_regulators(core);
+		rc = 0;
+	}
 
 	d_vpr_h("%s(): successful\n", __func__);
 	return 0;