[PATCH 3/3] venus: Add new interface queues reinit

[Stanimir Varbanov <stanimir.varbanov@linaro.org>]

Presently the recovery mechanism is using two hfi functions
to destroy and create interface queues. For the purpose of
recovery we don't need to free and allocate the memory used
for interface message queues, that's why we introduce new
function which just reinit the queues.  Also this will give
to the recovery procedure one less reason to fail (if for
some reason we couldn't allocate memory).

Signed-off-by: Stanimir Varbanov <stanimir.varbanov@linaro.org>
---
 drivers/media/platform/qcom/venus/core.c      |  3 +-
 drivers/media/platform/qcom/venus/hfi.c       |  5 ++
 drivers/media/platform/qcom/venus/hfi.h       |  1 +
 drivers/media/platform/qcom/venus/hfi_venus.c | 51 +++++++++++++++++++
 drivers/media/platform/qcom/venus/hfi_venus.h |  1 +
 5 files changed, 59 insertions(+), 2 deletions(-)

diff --git a/drivers/media/platform/qcom/venus/core.c b/drivers/media/platform/qcom/venus/core.c
index 46f6e34d435a..eda8f35425df 100644
--- a/drivers/media/platform/qcom/venus/core.c
+++ b/drivers/media/platform/qcom/venus/core.c
@@ -72,8 +72,7 @@ static void venus_sys_error_handler(struct work_struct *work)
 	while (core->pmdomains[0] && pm_runtime_active(core->pmdomains[0]))
 		usleep_range(1000, 1500);
 
-	hfi_destroy(core);
-	ret |= hfi_create(core, &venus_core_ops);
+	hfi_reinit(core);
 
 	pm_runtime_get_sync(core->dev);
 
diff --git a/drivers/media/platform/qcom/venus/hfi.c b/drivers/media/platform/qcom/venus/hfi.c
index a211eb93e0f9..a59022adb14c 100644
--- a/drivers/media/platform/qcom/venus/hfi.c
+++ b/drivers/media/platform/qcom/venus/hfi.c
@@ -517,3 +517,8 @@ void hfi_destroy(struct venus_core *core)
 {
 	venus_hfi_destroy(core);
 }
+
+void hfi_reinit(struct venus_core *core)
+{
+	venus_hfi_queues_reinit(core);
+}
diff --git a/drivers/media/platform/qcom/venus/hfi.h b/drivers/media/platform/qcom/venus/hfi.h
index 62c315291484..f25d412d6553 100644
--- a/drivers/media/platform/qcom/venus/hfi.h
+++ b/drivers/media/platform/qcom/venus/hfi.h
@@ -145,6 +145,7 @@ struct hfi_ops {
 
 int hfi_create(struct venus_core *core, const struct hfi_core_ops *ops);
 void hfi_destroy(struct venus_core *core);
+void hfi_reinit(struct venus_core *core);
 
 int hfi_core_init(struct venus_core *core);
 int hfi_core_deinit(struct venus_core *core, bool blocking);
diff --git a/drivers/media/platform/qcom/venus/hfi_venus.c b/drivers/media/platform/qcom/venus/hfi_venus.c
index 3392fd177d22..90067cd8c892 100644
--- a/drivers/media/platform/qcom/venus/hfi_venus.c
+++ b/drivers/media/platform/qcom/venus/hfi_venus.c
@@ -1603,3 +1603,54 @@ int venus_hfi_create(struct venus_core *core)
 	core->ops = NULL;
 	return ret;
 }
+
+void venus_hfi_queues_reinit(struct venus_core *core)
+{
+	struct venus_hfi_device *hdev = to_hfi_priv(core);
+	struct hfi_queue_table_header *tbl_hdr;
+	struct iface_queue *queue;
+	struct hfi_sfr *sfr;
+	unsigned int i;
+
+	mutex_lock(&hdev->lock);
+
+	for (i = 0; i < IFACEQ_NUM; i++) {
+		queue = &hdev->queues[i];
+		queue->qhdr =
+			IFACEQ_GET_QHDR_START_ADDR(hdev->ifaceq_table.kva, i);
+
+		venus_set_qhdr_defaults(queue->qhdr);
+
+		queue->qhdr->start_addr = queue->qmem.da;
+
+		if (i == IFACEQ_CMD_IDX)
+			queue->qhdr->type |= HFI_HOST_TO_CTRL_CMD_Q;
+		else if (i == IFACEQ_MSG_IDX)
+			queue->qhdr->type |= HFI_CTRL_TO_HOST_MSG_Q;
+		else if (i == IFACEQ_DBG_IDX)
+			queue->qhdr->type |= HFI_CTRL_TO_HOST_DBG_Q;
+	}
+
+	tbl_hdr = hdev->ifaceq_table.kva;
+	tbl_hdr->version = 0;
+	tbl_hdr->size = IFACEQ_TABLE_SIZE;
+	tbl_hdr->qhdr0_offset = sizeof(struct hfi_queue_table_header);
+	tbl_hdr->qhdr_size = sizeof(struct hfi_queue_header);
+	tbl_hdr->num_q = IFACEQ_NUM;
+	tbl_hdr->num_active_q = IFACEQ_NUM;
+
+	/*
+	 * Set receive request to zero on debug queue as there is no
+	 * need of interrupt from video hardware for debug messages
+	 */
+	queue = &hdev->queues[IFACEQ_DBG_IDX];
+	queue->qhdr->rx_req = 0;
+
+	sfr = hdev->sfr.kva;
+	sfr->buf_size = ALIGNED_SFR_SIZE;
+
+	/* ensure table and queue header structs are settled in memory */
+	wmb();
+
+	mutex_unlock(&hdev->lock);
+}
diff --git a/drivers/media/platform/qcom/venus/hfi_venus.h b/drivers/media/platform/qcom/venus/hfi_venus.h
index 57154832090e..1b656ef2bf07 100644
--- a/drivers/media/platform/qcom/venus/hfi_venus.h
+++ b/drivers/media/platform/qcom/venus/hfi_venus.h
@@ -10,5 +10,6 @@ struct venus_core;
 
 void venus_hfi_destroy(struct venus_core *core);
 int venus_hfi_create(struct venus_core *core);
+void venus_hfi_queues_reinit(struct venus_core *core);
 
 #endif
-- 
2.17.1