msm: cpp: Ensure CPP RX fifo is empty before writing payload

CPP RX FIFO empty status is not checked for payload start code and
trailer. A continuous counter must be used across the payload
instead of separate check for CPP RX FIFO empty status for
stripe and plane based. Ensure that CPP RX FIFO empty before
writing the second payload.

Change-Id: I11d0f77798addf4d654122e346dc6ec736a47113
Signed-off-by: Krishnankutty Kolathappilly <kkolatha@codeaurora.org>

drivers/media/platform/msm/camera_v2/pproc/cpp/msm_cpp.c

@@ -1735,7 +1735,7 @@ static void msm_cpp_set_micro_irq_mask(struct cpp_device *cpp_dev,
 static void msm_cpp_do_timeout_work(struct work_struct *work)
 {
 	uint32_t j = 0, i = 0, i1 = 0, i2 = 0;
-	int32_t queue_len = 0, rc = 0;
+	int32_t queue_len = 0, rc = 0, fifo_counter = 0;
 	struct msm_device_queue *queue = NULL;
 	struct msm_cpp_frame_info_t *processed_frame[MAX_CPP_PROCESSING_FRAME];
 	struct cpp_device *cpp_dev = cpp_timer.data.cpp_dev;
@@ -1775,12 +1775,7 @@ static void msm_cpp_do_timeout_work(struct work_struct *work)
 		cpp_timer.data.cpp_dev->fw_name_bin);
 	if (rc) {
 		pr_warn("Firmware loading failed\n");
-		cpp_dev->state = CPP_STATE_OFF;
+		goto error;
-		/* clean buf queue here */
-		msm_cpp_flush_queue_and_release_buffer(cpp_dev, queue_len);
-		msm_cpp_set_micro_irq_mask(cpp_dev, 1, 0x0);
-		cpp_dev->timeout_trial_cnt = 0;
-		goto end;
 	} else {
 		pr_debug("Firmware loading done\n");
 	}
@@ -1817,34 +1812,33 @@ static void msm_cpp_do_timeout_work(struct work_struct *work)
 			processed_frame[i]->identity,
 			processed_frame[i]->frame_id);
 
+		rc = msm_cpp_poll_rx_empty(cpp_dev->base);
+		if (rc) {
+			pr_err("%s:%d: Reschedule payload failed %d\n",
+				__func__, __LINE__, rc);
+			goto error;
+		}
 		msm_cpp_write(0x6, cpp_dev->base);
+		fifo_counter++;
 		/* send top level and plane level */
-		for (j = 0; j < cpp_dev->payload_params.stripe_base; j++) {
+		for (j = 0; j < cpp_dev->payload_params.stripe_base; j++,
-			if (j % MSM_CPP_RX_FIFO_LEVEL == 0) {
+			fifo_counter++) {
+			if (fifo_counter % MSM_CPP_RX_FIFO_LEVEL == 0) {
 				rc = msm_cpp_poll_rx_empty(cpp_dev->base);
 				if (rc) {
 					pr_err("%s:%d] poll failed %d rc %d",
 						__func__, __LINE__, j, rc);
-					cpp_dev->state = CPP_STATE_OFF;
+					goto error;
-					msm_cpp_set_micro_irq_mask(cpp_dev,
-						0, 0x0);
-					cpp_dev->timeout_trial_cnt = 0;
-					goto end;
 				}
+				fifo_counter = 0;
 			}
 			msm_cpp_write(processed_frame[i]->cpp_cmd_msg[j],
 				cpp_dev->base);
 		}
 		if (rc) {
-			pr_err("%s: Rescheduling plane info failed %d\n",
+			pr_err("%s:%d: Rescheduling plane info failed %d\n",
-				__func__, rc);
+				__func__, __LINE__, rc);
-			/* flush the queue */
+			goto error;
-			cpp_dev->state = CPP_STATE_OFF;
-			msm_cpp_flush_queue_and_release_buffer(cpp_dev,
-				queue_len);
-			msm_cpp_set_micro_irq_mask(cpp_dev, 0, 0x0);
-			cpp_dev->timeout_trial_cnt = 0;
-			goto end;
 		}
 		/* send stripes */
 		i1 = cpp_dev->payload_params.stripe_base +
@@ -1853,14 +1847,15 @@ static void msm_cpp_do_timeout_work(struct work_struct *work)
 		i2 = cpp_dev->payload_params.stripe_size *
 			(processed_frame[i]->last_stripe_index -
 			processed_frame[i]->first_stripe_index + 1);
-		for (j = 0; j < i2; j++) {
+		for (j = 0; j < i2; j++, fifo_counter++) {
-			if (j % MSM_CPP_RX_FIFO_LEVEL == 0) {
+			if (fifo_counter % MSM_CPP_RX_FIFO_LEVEL == 0) {
 				rc = msm_cpp_poll_rx_empty(cpp_dev->base);
 				if (rc) {
 					pr_err("%s:%d] poll failed %d rc %d",
 						__func__, __LINE__, j, rc);
 					break;
 				}
+				fifo_counter = 0;
 			}
 			msm_cpp_write(processed_frame[i]->cpp_cmd_msg[j+i1],
 				cpp_dev->base);
@@ -1868,15 +1863,19 @@ static void msm_cpp_do_timeout_work(struct work_struct *work)
 		if (rc) {
 			pr_err("%s:%d] Rescheduling stripe info failed %d\n",
 				__func__, __LINE__, rc);
-			/* flush the queue */
+			goto error;
-			cpp_dev->state = CPP_STATE_OFF;
-			msm_cpp_flush_queue_and_release_buffer(cpp_dev,
-				queue_len);
-			msm_cpp_set_micro_irq_mask(cpp_dev, 0, 0x0);
-			cpp_dev->timeout_trial_cnt = 0;
-			goto end;
 		}
 		/* send trailer */
+
+		if (fifo_counter % MSM_CPP_RX_FIFO_LEVEL == 0) {
+			rc = msm_cpp_poll_rx_empty(cpp_dev->base);
+			if (rc) {
+				pr_err("%s:%d] Reschedule trailer failed %d\n",
+					__func__, __LINE__, rc);
+				goto error;
+			}
+			fifo_counter = 0;
+		}
 		msm_cpp_write(0xabcdefaa, cpp_dev->base);
 		pr_debug("After frame:%d write\n", i+1);
 	}
@@ -1887,6 +1886,16 @@ end:
 	mutex_unlock(&cpp_dev->mutex);
 	pr_debug("%s:%d] exit\n", __func__, __LINE__);
 	return;
+error:
+	cpp_dev->state = CPP_STATE_OFF;
+	/* flush the queue */
+	msm_cpp_flush_queue_and_release_buffer(cpp_dev,
+		queue_len);
+	msm_cpp_set_micro_irq_mask(cpp_dev, 0, 0x0);
+	cpp_dev->timeout_trial_cnt = 0;
+	mutex_unlock(&cpp_dev->mutex);
+	pr_debug("%s:%d] exit\n", __func__, __LINE__);
+	return;
 }
 
 void cpp_timer_callback(unsigned long data)
@@ -1903,10 +1912,9 @@ static int msm_cpp_send_frame_to_hardware(struct cpp_device *cpp_dev,
 	unsigned long flags;
 	uint32_t i, i1, i2;
 	int32_t rc = -EAGAIN;
-	int ret;
 	struct msm_cpp_frame_info_t *process_frame;
 	struct msm_queue_cmd *qcmd = NULL;
-	uint32_t queue_len = 0;
+	uint32_t queue_len = 0, fifo_counter = 0;
 
 	if (cpp_dev->processing_q.len < MAX_CPP_PROCESSING_FRAME) {
 		process_frame = frame_qcmd->command;
@@ -1923,25 +1931,33 @@ static int msm_cpp_send_frame_to_hardware(struct cpp_device *cpp_dev,
 
 		CPP_DBG("Starting timer to fire in %d ms. (jiffies=%lu)\n",
 			CPP_CMD_TIMEOUT_MS, jiffies);
-		ret = mod_timer(&cpp_timer.cpp_timer,
+		if (mod_timer(&cpp_timer.cpp_timer,
-			jiffies + msecs_to_jiffies(CPP_CMD_TIMEOUT_MS));
+			(jiffies + msecs_to_jiffies(CPP_CMD_TIMEOUT_MS))) != 0)
-		if (ret)
 			CPP_DBG("Timer has not expired yet\n");
 
+		rc = msm_cpp_poll_rx_empty(cpp_dev->base);
+		if (rc) {
+			pr_err("%s:%d: Scheduling payload failed %d",
+				__func__, __LINE__, rc);
+			goto dequeue_frame;
+		}
 		msm_cpp_write(0x6, cpp_dev->base);
+		fifo_counter++;
 		/* send top level and plane level */
-		for (i = 0; i < cpp_dev->payload_params.stripe_base; i++) {
+		for (i = 0; i < cpp_dev->payload_params.stripe_base; i++,
-			if (i % MSM_CPP_RX_FIFO_LEVEL == 0) {
+			fifo_counter++) {
+			if ((fifo_counter % MSM_CPP_RX_FIFO_LEVEL) == 0) {
 				rc = msm_cpp_poll_rx_empty(cpp_dev->base);
 				if (rc)
 					break;
+				fifo_counter = 0;
 			}
 			msm_cpp_write(process_frame->cpp_cmd_msg[i],
 				cpp_dev->base);
 		}
 		if (rc) {
-			pr_err("%s: Rescheduling plane info failed %d\n",
+			pr_err("%s:%d: Scheduling plane info failed %d\n",
-				__func__, rc);
+				__func__, __LINE__, rc);
 			goto dequeue_frame;
 		}
 		/* send stripes */
@@ -1951,21 +1967,31 @@ static int msm_cpp_send_frame_to_hardware(struct cpp_device *cpp_dev,
 		i2 = cpp_dev->payload_params.stripe_size *
 			(process_frame->last_stripe_index -
 			process_frame->first_stripe_index + 1);
-		for (i = 0; i < i2; i++) {
+		for (i = 0; i < i2; i++, fifo_counter++) {
-			if (i % MSM_CPP_RX_FIFO_LEVEL == 0) {
+			if ((fifo_counter % MSM_CPP_RX_FIFO_LEVEL) == 0) {
 				rc = msm_cpp_poll_rx_empty(cpp_dev->base);
 				if (rc)
 					break;
+				fifo_counter = 0;
 			}
 			msm_cpp_write(process_frame->cpp_cmd_msg[i+i1],
 				cpp_dev->base);
 		}
 		if (rc) {
-			pr_err("%s: Rescheduling stripe info failed %d\n",
+			pr_err("%s:%d: Scheduling stripe info failed %d\n",
-				__func__, rc);
+				__func__, __LINE__, rc);
 			goto dequeue_frame;
 		}
 		/* send trailer */
+		if ((fifo_counter % MSM_CPP_RX_FIFO_LEVEL) == 0) {
+			rc = msm_cpp_poll_rx_empty(cpp_dev->base);
+			if (rc) {
+				pr_err("%s: Scheduling trailer failed %d\n",
+				__func__, rc);
+				goto dequeue_frame;
+			}
+			fifo_counter = 0;
+		}
 		msm_cpp_write(MSM_CPP_MSG_ID_TRAILER, cpp_dev->base);
 
 		do_gettimeofday(&(process_frame->in_time));