diff --git a/drivers/gpu/msm/adreno_a6xx.c b/drivers/gpu/msm/adreno_a6xx.c
index 423b7a4e7a39..e533930aba74 100644
--- a/drivers/gpu/msm/adreno_a6xx.c
+++ b/drivers/gpu/msm/adreno_a6xx.c
@@ -686,6 +686,25 @@ void a6xx_start(struct adreno_device *adreno_dev)
 	}
 }
 
+void a6xx_unhalt_sqe(struct adreno_device *adreno_dev)
+{
+	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
+	struct adreno_firmware *fw = ADRENO_FW(adreno_dev, ADRENO_FW_SQE);
+	uint64_t gpuaddr;
+
+	gpuaddr = fw->memdesc->gpuaddr;
+
+	/* Program the ucode base for CP */
+	kgsl_regwrite(device, A6XX_CP_SQE_INSTR_BASE_LO,
+				lower_32_bits(gpuaddr));
+	kgsl_regwrite(device, A6XX_CP_SQE_INSTR_BASE_HI,
+				upper_32_bits(gpuaddr));
+
+	/* Clear the SQE_HALT to start the CP engine */
+	kgsl_regwrite(device, A6XX_CP_SQE_CNTL, 1);
+}
+
+
 /*
  * CP_INIT_MAX_CONTEXT bit tells if the multiple hardware contexts can
  * be used at once of if they should be serialized
@@ -737,14 +756,14 @@ static int a6xx_get_cp_init_cmds(struct adreno_device *adreno_dev)
 	if (adreno_dev->cp_init_cmds)
 		return 0;
 
-	adreno_dev->cp_init_cmds = devm_kzalloc(&device->pdev->dev, 12 << 2,
-				GFP_KERNEL);
+	adreno_dev->cp_init_cmds = devm_kzalloc(&device->pdev->dev,
+			A6XX_CP_INIT_DWORDS << 2, GFP_KERNEL);
 	if (!adreno_dev->cp_init_cmds)
 		return -ENOMEM;
 
 	cmds = (u32 *)adreno_dev->cp_init_cmds;
 
-	cmds[i++] = cp_type7_packet(CP_ME_INIT, 11);
+	cmds[i++] = cp_type7_packet(CP_ME_INIT, A6XX_CP_INIT_DWORDS - 1);
 
 	/* Enabled ordinal mask */
 	cmds[i++] = CP_INIT_MASK;
@@ -779,7 +798,7 @@ static int a6xx_get_cp_init_cmds(struct adreno_device *adreno_dev)
 	return 0;
 }
 
-static void a6xx_spin_idle_debug(struct adreno_device *adreno_dev,
+void a6xx_spin_idle_debug(struct adreno_device *adreno_dev,
 				const char *str)
 {
 	struct kgsl_device *device = &adreno_dev->dev;
@@ -911,7 +930,6 @@ static int a6xx_post_start(struct adreno_device *adreno_dev)
 int a6xx_rb_start(struct adreno_device *adreno_dev)
 {
 	const struct adreno_a6xx_core *a6xx_core = to_a6xx_core(adreno_dev);
-	struct adreno_firmware *fw = ADRENO_FW(adreno_dev, ADRENO_FW_SQE);
 	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
 	u32 cp_rb_cntl = A6XX_CP_RB_CNTL_DEFAULT |
 		(ADRENO_FEATURE(adreno_dev, ADRENO_APRIV) ? 0 : (1 << 27));
@@ -952,15 +970,7 @@ int a6xx_rb_start(struct adreno_device *adreno_dev)
 	kgsl_regwrite(device, A6XX_CP_RB_BASE_HI,
 		upper_32_bits(rb->buffer_desc->gpuaddr));
 
-	/* Program the ucode base for CP */
-	kgsl_regwrite(device, A6XX_CP_SQE_INSTR_BASE_LO,
-			lower_32_bits(fw->memdesc->gpuaddr));
-
-	kgsl_regwrite(device, A6XX_CP_SQE_INSTR_BASE_HI,
-			upper_32_bits(fw->memdesc->gpuaddr));
-
-	/* Clear the SQE_HALT to start the CP engine */
-	kgsl_regwrite(device, A6XX_CP_SQE_CNTL, 1);
+	a6xx_unhalt_sqe(adreno_dev);
 
 	ret = a6xx_send_cp_init(adreno_dev, rb);
 	if (ret)
diff --git a/drivers/gpu/msm/adreno_a6xx.h b/drivers/gpu/msm/adreno_a6xx.h
index 616327df33d0..5fa428139b4f 100644
--- a/drivers/gpu/msm/adreno_a6xx.h
+++ b/drivers/gpu/msm/adreno_a6xx.h
@@ -183,6 +183,9 @@ struct cpu_gpu_lock {
 #define A6XX_CP_RB_CNTL_DEFAULT (((ilog2(4) << 8) & 0x1F00) | \
 		(ilog2(KGSL_RB_DWORDS >> 1) & 0x3F))
 
+/* Size of the CP_INIT pm4 stream in dwords */
+#define A6XX_CP_INIT_DWORDS 12
+
 /**
  * to_a6xx_core - return the a6xx specific GPU core struct
  * @adreno_dev: An Adreno GPU device handle
@@ -274,6 +277,15 @@ u64 a6xx_read_alwayson(struct adreno_device *adreno_dev);
  */
 void a6xx_start(struct adreno_device *adreno_dev);
 
+/**
+ * a6xx_sqe_unhalt - Unhalt the SQE engine
+ * @adreno_dev: An Adreno GPU handle
+ *
+ * Points the hardware to the microcode location in memory and then
+ * unhalts the SQE so that it can fetch instructions from DDR
+ */
+void a6xx_unhalt_sqe(struct adreno_device *adreno_dev);
+
 /**
  * a6xx_init - Initialize a6xx resources
  * @adreno_dev: An Adreno GPU handle
@@ -329,4 +341,13 @@ int a6xx_probe_common(struct platform_device *pdev,
  * Return: True if gpu is idle, otherwise false
  */
 bool a6xx_hw_isidle(struct adreno_device *adreno_dev);
+
+/**
+ * a6xx_spin_idle_debug - Debug logging used when gpu fails to idle
+ * @adreno_dev: An Adreno GPU handle
+ *
+ * This function logs interesting registers and triggers a snapshot
+ */
+void a6xx_spin_idle_debug(struct adreno_device *adreno_dev,
+	const char *str);
 #endif
diff --git a/drivers/gpu/msm/adreno_a6xx_hfi.c b/drivers/gpu/msm/adreno_a6xx_hfi.c
index 2fde866540b5..f1f25c314f57 100644
--- a/drivers/gpu/msm/adreno_a6xx_hfi.c
+++ b/drivers/gpu/msm/adreno_a6xx_hfi.c
@@ -286,8 +286,8 @@ static int poll_gmu_reg(struct adreno_device *adreno_dev,
 	return -ETIMEDOUT;
 }
 
-static int a6xx_hfi_send_cmd(struct adreno_device *adreno_dev,
-	uint32_t queue_idx, void *data, struct pending_cmd *ret_cmd)
+int a6xx_hfi_send_cmd(struct adreno_device *adreno_dev, uint32_t queue_idx,
+		void *data, struct pending_cmd *ret_cmd)
 {
 	struct a6xx_gmu_device *gmu = to_a6xx_gmu(adreno_dev);
 	struct kgsl_device *device = KGSL_DEVICE(adreno_dev);
diff --git a/drivers/gpu/msm/adreno_a6xx_hfi.h b/drivers/gpu/msm/adreno_a6xx_hfi.h
index 88ce3f7f7f36..929193bffd32 100644
--- a/drivers/gpu/msm/adreno_a6xx_hfi.h
+++ b/drivers/gpu/msm/adreno_a6xx_hfi.h
@@ -675,4 +675,16 @@ int a6xx_hfi_send_generic_req(struct adreno_device *adreno_dev,
  * Return: 0 on success or negative error on failure
  */
 int a6xx_hfi_send_bcl_feature_ctrl(struct adreno_device *adreno_dev);
+
+/*
+ * a6xx_hfi_send_cmd - Send and wait for a hfi packet
+ * @adreno_dev: Pointer to the adreno device
+ * @queue_idx: Destination queue id
+ * @data: Pointer to hfi packet header and data
+ * @ret_cmd: Container for data needed for waiting for the ack
+ *
+ * Return: 0 on success or negative error on failure
+ */
+int a6xx_hfi_send_cmd(struct adreno_device *adreno_dev, u32 queue_idx,
+	void *data, struct pending_cmd *ret_cmd);
 #endif
diff --git a/drivers/gpu/msm/adreno_a6xx_hwsched.c b/drivers/gpu/msm/adreno_a6xx_hwsched.c
index 23f826ed7d58..b49704337cab 100644
--- a/drivers/gpu/msm/adreno_a6xx_hwsched.c
+++ b/drivers/gpu/msm/adreno_a6xx_hwsched.c
@@ -256,6 +256,12 @@ static int a6xx_hwsched_gpu_boot(struct adreno_device *adreno_dev)
 
 	a6xx_enable_gpu_irq(adreno_dev);
 
+	ret = a6xx_hwsched_cp_init(adreno_dev);
+	if (ret) {
+		a6xx_disable_gpu_irq(adreno_dev);
+		goto err;
+	}
+
 	device->reset_counter++;
 err:
 	a6xx_gmu_oob_clear(device, oob_gpu);
diff --git a/drivers/gpu/msm/adreno_a6xx_hwsched_hfi.c b/drivers/gpu/msm/adreno_a6xx_hwsched_hfi.c
index 8db3f9c05b9b..8fdda837f075 100644
--- a/drivers/gpu/msm/adreno_a6xx_hwsched_hfi.c
+++ b/drivers/gpu/msm/adreno_a6xx_hwsched_hfi.c
@@ -8,6 +8,7 @@
 #include "adreno.h"
 #include "adreno_a6xx.h"
 #include "adreno_a6xx_hwsched.h"
+#include "adreno_pm4types.h"
 #include "kgsl_device.h"
 #include "kgsl_pwrctrl.h"
 #include "kgsl_trace.h"
@@ -457,3 +458,69 @@ err:
 
 	return ret;
 }
+
+static int submit_raw_cmds(struct adreno_device *adreno_dev, void *cmds,
+	const char *str)
+{
+	struct pending_cmd ret_cmd = {0};
+	int ret;
+
+	ret = a6xx_hfi_send_cmd(adreno_dev, HFI_CMD_ID, cmds, &ret_cmd);
+	if (ret)
+		return ret;
+
+	ret = timed_poll_check(KGSL_DEVICE(adreno_dev),
+			A6XX_GPU_GMU_AO_GPU_CX_BUSY_STATUS, 0, 200, BIT(23));
+	if (ret)
+		a6xx_spin_idle_debug(adreno_dev, str);
+
+	return ret;
+}
+
+static int cp_init(struct adreno_device *adreno_dev)
+{
+	u32 cmds[A6XX_CP_INIT_DWORDS + 1];
+
+	cmds[0] = CMD_MSG_HDR(H2F_MSG_ISSUE_CMD_RAW,
+			(A6XX_CP_INIT_DWORDS + 1) << 2);
+	memcpy(&cmds[1], adreno_dev->cp_init_cmds, A6XX_CP_INIT_DWORDS << 2);
+
+	return submit_raw_cmds(adreno_dev, cmds,
+			"CP initialization failed to idle\n");
+}
+
+static int send_switch_to_unsecure(struct adreno_device *adreno_dev)
+{
+	u32 cmds[3];
+
+	cmds[0] = CMD_MSG_HDR(H2F_MSG_ISSUE_CMD_RAW, sizeof(cmds));
+	cmds[1] = cp_type7_packet(CP_SET_SECURE_MODE, 1);
+	cmds[2] = 0;
+
+	return  submit_raw_cmds(adreno_dev, cmds,
+			"Switch to unsecure failed to idle\n");
+}
+
+int a6xx_hwsched_cp_init(struct adreno_device *adreno_dev)
+{
+	const struct adreno_a6xx_core *a6xx_core = to_a6xx_core(adreno_dev);
+	int ret;
+
+	a6xx_unhalt_sqe(adreno_dev);
+
+	ret = cp_init(adreno_dev);
+	if (ret)
+		return ret;
+
+	ret = adreno_zap_shader_load(adreno_dev, a6xx_core->zap_name);
+	if (ret)
+		return ret;
+
+	if (!adreno_dev->zap_loaded)
+		kgsl_regwrite(KGSL_DEVICE(adreno_dev),
+			A6XX_RBBM_SECVID_TRUST_CNTL, 0x0);
+	else
+		ret = send_switch_to_unsecure(adreno_dev);
+
+	return ret;
+}
diff --git a/drivers/gpu/msm/adreno_a6xx_hwsched_hfi.h b/drivers/gpu/msm/adreno_a6xx_hwsched_hfi.h
index e3c37395bec0..9bbf9a520fbc 100644
--- a/drivers/gpu/msm/adreno_a6xx_hwsched_hfi.h
+++ b/drivers/gpu/msm/adreno_a6xx_hwsched_hfi.h
@@ -109,9 +109,12 @@ struct a6xx_hwsched_hfi {
 	u32 mem_alloc_entries;
 };
 
+/* Interrupt handler for hfi interrupts */
+irqreturn_t a6xx_hfi_handler(int irq, void *data);
+
 /**
  * a6xx_hwsched_hfi_init - Initialize hfi resources
- * @adreno_dev - Pointer to adreno device structure
+ * @adreno_dev: Pointer to adreno device structure
  *
  * This function is used to initialize hfi resources
  * once before the very first gmu boot
@@ -137,4 +140,15 @@ int a6xx_hwsched_hfi_start(struct adreno_device *adreno_dev);
  * This function does the hfi cleanup when powering down the gmu
  */
 void a6xx_hwsched_hfi_stop(struct adreno_device *adreno_dev);
+
+/**
+ * a6xx_hwched_cp_init - Send CP_INIT via HFI
+ * @adreno_dev: Pointer to adreno device structure
+ *
+ * This function is used to send CP INIT packet and bring
+ * GPU out of secure mode using hfi raw packets.
+ *
+ * Return: 0 on success and negative error on failure.
+ */
+int a6xx_hwsched_cp_init(struct adreno_device *adreno_dev);
 #endif