Merge pull request #1772 from glneo/clear-proxy-queue

TI K3 Clear proxy receive queue on transmit

3 files changed, 55 insertions, 0 deletions

diff --git a/plat/ti/k3/common/drivers/sec_proxy/sec_proxy.c b/plat/ti/k3/common/drivers/sec_proxy/sec_proxy.c
index 2a013ed0..49cecd44 100644
--- a/plat/ti/k3/common/drivers/sec_proxy/sec_proxy.c
+++ b/plat/ti/k3/common/drivers/sec_proxy/sec_proxy.c
@@ -163,6 +163,44 @@ static inline int k3_sec_proxy_verify_thread(struct k3_sec_proxy_thread *spt,
 }
 
 /**
+ * k3_sec_proxy_clear_rx_thread() - Clear Secure Proxy thread
+ *
+ * @id: Channel Identifier
+ *
+ * Return: 0 if all goes well, else appropriate error message
+ */
+int k3_sec_proxy_clear_rx_thread(enum k3_sec_proxy_chan_id id)
+{
+	struct k3_sec_proxy_thread *spt = &spm.threads[id];
+
+	/* Check for any errors already available */
+	if (mmio_read_32(spt->rt + RT_THREAD_STATUS) &
+	    RT_THREAD_STATUS_ERROR_MASK) {
+		ERROR("Thread %d is corrupted, cannot send data\n", spt->id);
+		return -EINVAL;
+	}
+
+	/* Make sure thread is configured for right direction */
+	if (!(mmio_read_32(spt->scfg + SCFG_THREAD_CTRL) & SCFG_THREAD_CTRL_DIR_MASK)) {
+		ERROR("Cannot clear a transmit thread %d\n", spt->id);
+		return -EINVAL;
+	}
+
+	/* Read off messages from thread until empty */
+	uint32_t try_count = 10;
+	while (mmio_read_32(spt->rt + RT_THREAD_STATUS) & RT_THREAD_STATUS_CUR_CNT_MASK) {
+		if (!(try_count--)) {
+			ERROR("Could not clear all messages from thread %d\n", spt->id);
+			return -ETIMEDOUT;
+		}
+		WARN("Clearing message from thread %d\n", spt->id);
+		mmio_read_32(spt->data + spm.desc.data_end_offset);
+	}
+
+	return 0;
+}
+
+/**
  * k3_sec_proxy_send() - Send data over a Secure Proxy thread
  * @id: Channel Identifier
  * @msg: Pointer to k3_sec_proxy_msg
diff --git a/plat/ti/k3/common/drivers/sec_proxy/sec_proxy.h b/plat/ti/k3/common/drivers/sec_proxy/sec_proxy.h
index 2d987f83..6c4f5dff 100644
--- a/plat/ti/k3/common/drivers/sec_proxy/sec_proxy.h
+++ b/plat/ti/k3/common/drivers/sec_proxy/sec_proxy.h
@@ -44,6 +44,15 @@ struct k3_sec_proxy_msg {
  *
  * Return: 0 if all goes well, else appropriate error message
  */
+int k3_sec_proxy_clear_rx_thread(enum k3_sec_proxy_chan_id id);
+
+/**
+ * k3_sec_proxy_send() - Send data over a Secure Proxy thread
+ * @id: Channel Identifier
+ * @msg: Pointer to k3_sec_proxy_msg
+ *
+ * Return: 0 if all goes well, else appropriate error message
+ */
 int k3_sec_proxy_send(enum k3_sec_proxy_chan_id id, const struct k3_sec_proxy_msg *msg);
 
 /**
diff --git a/plat/ti/k3/common/drivers/ti_sci/ti_sci.c b/plat/ti/k3/common/drivers/ti_sci/ti_sci.c
index 81488a15..4a33d344 100644
--- a/plat/ti/k3/common/drivers/ti_sci/ti_sci.c
+++ b/plat/ti/k3/common/drivers/ti_sci/ti_sci.c
@@ -158,6 +158,13 @@ static inline int ti_sci_do_xfer(struct ti_sci_xfer *xfer)
 	struct k3_sec_proxy_msg *msg = &xfer->tx_message;
 	int ret;
 
+	/* Clear any spurious messages in receive queue */
+	ret = k3_sec_proxy_clear_rx_thread(SP_RESPONSE);
+	if (ret) {
+		ERROR("Could not clear response queue (%d)\n", ret);
+		return ret;
+	}
+
 	/* Send the message */
 	ret = k3_sec_proxy_send(SP_HIGH_PRIORITY, msg);
 	if (ret) {
@@ -165,6 +172,7 @@ static inline int ti_sci_do_xfer(struct ti_sci_xfer *xfer)
 		return ret;
 	}
 
+	/* Get the response */
 	ret = ti_sci_get_response(xfer, SP_RESPONSE);
 	if (ret) {
 		ERROR("Failed to get response (%d)\n", ret);