1 files changed, 18 insertions, 14 deletions

diff --git a/src/soc/qualcomm/ipq40xx/qup.c b/src/soc/qualcomm/ipq40xx/qup.c
index 76f079744c..88e9169ec7 100644
--- a/src/soc/qualcomm/ipq40xx/qup.c
+++ b/src/soc/qualcomm/ipq40xx/qup.c
@@ -3,6 +3,7 @@
 #include <device/mmio.h>
 #include <console/console.h>
 #include <delay.h>
+#include <timer.h>
 #include <soc/iomap.h>
 #include <soc/qup.h>
 
@@ -93,35 +94,33 @@ static qup_return_t qup_reset_master_status(blsp_qup_id_t id)
 	return QUP_SUCCESS;
 }
 
-static qup_return_t qup_fifo_wait_for(blsp_qup_id_t id, uint32_t status)
+static qup_return_t qup_fifo_wait_for(blsp_qup_id_t id, uint32_t status,
+				      struct stopwatch *timeout)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
-	unsigned int count = TIMEOUT_CNT;
 
 	while (!(read32(QUP_ADDR(id, QUP_OPERATIONAL)) & status)) {
 		ret = qup_i2c_master_status(id);
 		if (ret)
 			return ret;
-		if (count == 0)
+		if (stopwatch_expired(timeout))
 			return QUP_ERR_TIMEOUT;
-		count--;
 	}
 
 	return QUP_SUCCESS;
 }
 
-static qup_return_t qup_fifo_wait_while(blsp_qup_id_t id, uint32_t status)
+static qup_return_t qup_fifo_wait_while(blsp_qup_id_t id, uint32_t status,
+					struct stopwatch *timeout)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
-	unsigned int count = TIMEOUT_CNT;
 
 	while (read32(QUP_ADDR(id, QUP_OPERATIONAL)) & status) {
 		ret = qup_i2c_master_status(id);
 		if (ret)
 			return ret;
-		if (count == 0)
+		if (stopwatch_expired(timeout))
 			return QUP_ERR_TIMEOUT;
-		count--;
 	}
 
 	return QUP_SUCCESS;
@@ -139,7 +138,8 @@ static inline uint32_t qup_i2c_create_output_tag(int stop, u8 data)
 	return tag;
 }
 
-static inline qup_return_t qup_i2c_write_fifo_flush(blsp_qup_id_t id)
+static inline qup_return_t qup_i2c_write_fifo_flush(blsp_qup_id_t id,
+						    struct stopwatch *timeout)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
 
@@ -147,7 +147,7 @@ static inline qup_return_t qup_i2c_write_fifo_flush(blsp_qup_id_t id)
 
 	mdelay(4);	/* TPM seems to need this */
 
-	ret = qup_fifo_wait_while(id, OUTPUT_FIFO_NOT_EMPTY);
+	ret = qup_fifo_wait_while(id, OUTPUT_FIFO_NOT_EMPTY, timeout);
 	if (ret)
 		return ret;
 
@@ -168,6 +168,7 @@ static qup_return_t qup_i2c_write_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj,
 	unsigned int data_len = p_tx_obj->p.iic.data_len;
 	unsigned int idx = 0;
 	uint32_t tag, *fifo = QUP_ADDR(id, QUP_OUTPUT_FIFO);
+	struct stopwatch timeout;
 
 	qup_reset_master_status(id);
 
@@ -196,6 +197,7 @@ static qup_return_t qup_i2c_write_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj,
 
 	qup_write32(fifo, tag);
 
+	stopwatch_init_usecs_expire(&timeout, CONFIG_I2C_TRANSFER_TIMEOUT_US);
 	while (data_len) {
 
 		tag = qup_i2c_create_output_tag(data_len == 1 && stop_seq,
@@ -213,7 +215,7 @@ static qup_return_t qup_i2c_write_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj,
 
 		qup_write32(fifo, tag);
 
-		ret = qup_i2c_write_fifo_flush(id);
+		ret = qup_i2c_write_fifo_flush(id, &timeout);
 
 		if (ret) {
 			printk(QUPDBG "%s: error\n", __func__);
@@ -221,7 +223,7 @@ static qup_return_t qup_i2c_write_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj,
 		}
 	}
 
-	ret = qup_i2c_write_fifo_flush(id);
+	ret = qup_i2c_write_fifo_flush(id, &timeout);
 
 	qup_set_state(id, QUP_STATE_RESET);
 
@@ -285,6 +287,7 @@ static qup_return_t qup_i2c_read_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj)
 	unsigned int data_len = p_tx_obj->p.iic.data_len;
 	unsigned int idx = 0;
 	uint32_t *fifo = QUP_ADDR(id, QUP_OUTPUT_FIFO);
+	struct stopwatch timeout;
 
 	qup_reset_master_status(id);
 
@@ -303,13 +306,14 @@ static qup_return_t qup_i2c_read_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj)
 				  (QUP_I2C_ADDR(addr) | QUP_I2C_SLAVE_READ)) |
 				((QUP_I2C_RECV_SEQ | data_len) << 16));
 
-	ret = qup_i2c_write_fifo_flush(id);
+	stopwatch_init_usecs_expire(&timeout, CONFIG_I2C_TRANSFER_TIMEOUT_US);
+	ret = qup_i2c_write_fifo_flush(id, &timeout);
 	if (ret) {
 		printk(QUPDBG "%s: OUTPUT_FIFO_NOT_EMPTY\n", __func__);
 		return ret;
 	}
 
-	ret = qup_fifo_wait_for(id, INPUT_SERVICE_FLAG);
+	ret = qup_fifo_wait_for(id, INPUT_SERVICE_FLAG, &timeout);
 	if (ret) {
 		printk(QUPDBG "%s: INPUT_SERVICE_FLAG\n", __func__);
 		return ret;