2 files changed, 24 insertions, 26 deletions

diff --git a/src/soc/qualcomm/ipq40xx/i2c.c b/src/soc/qualcomm/ipq40xx/i2c.c
index a2634acdf2..772bf8b631 100644
--- a/src/soc/qualcomm/ipq40xx/i2c.c
+++ b/src/soc/qualcomm/ipq40xx/i2c.c
@@ -44,7 +44,7 @@ static qup_config_t blsp1_qup0_config = {
 	QUP_MINICORE_I2C_MASTER,
 	100000,
 	19050000,
-	QUP_MODE_FIFO,
+	QUP_MODE_BLOCK,
 	0
 };
 
@@ -52,7 +52,7 @@ static qup_config_t blsp1_qup1_config = {
 	QUP_MINICORE_I2C_MASTER,
 	100000,
 	19050000,
-	QUP_MODE_FIFO,
+	QUP_MODE_BLOCK,
 	0
 };
 
@@ -60,7 +60,7 @@ static qup_config_t blsp1_qup2_config = {
 	QUP_MINICORE_I2C_MASTER,
 	100000,
 	19050000,
-	QUP_MODE_FIFO,
+	QUP_MODE_BLOCK,
 	0
 };
 
@@ -68,7 +68,7 @@ static qup_config_t blsp1_qup3_config = {
 	QUP_MINICORE_I2C_MASTER,
 	100000,
 	19050000,
-	QUP_MODE_FIFO,
+	QUP_MODE_BLOCK,
 	0
 };
 
diff --git a/src/soc/qualcomm/ipq40xx/qup.c b/src/soc/qualcomm/ipq40xx/qup.c
index 63b37ef7b4..d2c1f282b6 100644
--- a/src/soc/qualcomm/ipq40xx/qup.c
+++ b/src/soc/qualcomm/ipq40xx/qup.c
@@ -198,10 +198,11 @@ static qup_return_t qup_i2c_write_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj,
 	unsigned data_len = p_tx_obj->p.iic.data_len;
 	unsigned idx = 0;
 	uint32_t tag, *fifo = QUP_ADDR(id, QUP_OUTPUT_FIFO);
-	const uint32_t *fifo_end = fifo + QUP_OUTPUT_FIFO_SIZE / sizeof(*fifo);
 
 	qup_reset_master_status(id);
 
+	qup_write32(QUP_ADDR(id, QUP_MX_OUTPUT_COUNT), data_len + 1);
+
 	qup_set_state(id, QUP_STATE_RUN);
 
 	/*
@@ -224,7 +225,6 @@ static qup_return_t qup_i2c_write_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj,
 	idx++;
 
 	qup_write32(fifo, tag);
-	fifo++;
 
 	while (data_len) {
 
@@ -242,19 +242,12 @@ static qup_return_t qup_i2c_write_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj,
 		}
 
 		qup_write32(fifo, tag);
-		fifo++;
-
-		if ((fifo >= fifo_end) && data_len) {
-
-			fifo = QUP_ADDR(id, QUP_OUTPUT_FIFO);
 
-			ret = qup_i2c_write_fifo_flush(id);
-
-			if (ret) {
-				printk(BIOS_DEBUG, "%s: error\n", __func__);
-				return ret;
-			}
+		ret = qup_i2c_write_fifo_flush(id);
 
+		if (ret) {
+			printk(QUPDBG "%s: error\n", __func__);
+			return ret;
 		}
 	}
 
@@ -272,6 +265,7 @@ static qup_return_t qup_i2c_write(blsp_qup_id_t id, uint8_t mode,
 
 	switch (mode) {
 	case QUP_MODE_FIFO:
+	case QUP_MODE_BLOCK:
 		ret = qup_i2c_write_fifo(id, p_tx_obj, stop_seq);
 		break;
 	default:
@@ -321,10 +315,18 @@ static qup_return_t qup_i2c_read_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj)
 	unsigned data_len = p_tx_obj->p.iic.data_len;
 	unsigned idx = 0;
 	uint32_t *fifo = QUP_ADDR(id, QUP_OUTPUT_FIFO);
-	const uint32_t *fifo_end = fifo + QUP_INPUT_FIFO_SIZE / sizeof(*fifo);
 
 	qup_reset_master_status(id);
 
+	qup_write32(QUP_ADDR(id, QUP_IO_MODES),
+		QUP_UNPACK_EN | QUP_PACK_EN |
+			  ((QUP_MODE_BLOCK & QUP_MODE_MASK) <<
+					QUP_OUTPUT_MODE_SHFT) |
+			  ((QUP_MODE_BLOCK & QUP_MODE_MASK) <<
+					QUP_INPUT_MODE_SHFT));
+
+	qup_write32(QUP_ADDR(id, QUP_MX_INPUT_COUNT), data_len);
+
 	qup_set_state(id, QUP_STATE_RUN);
 
 	qup_write32(fifo, (QUP_I2C_START_SEQ |
@@ -337,8 +339,6 @@ static qup_return_t qup_i2c_read_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj)
 		return ret;
 	}
 
-	qup_write32(QUP_ADDR(id, QUP_MX_READ_COUNT), data_len);
-
 	ret = qup_fifo_wait_for(id, INPUT_SERVICE_FLAG);
 	if (ret) {
 		printk(QUPDBG "%s: INPUT_SERVICE_FLAG\n", __func__);
@@ -352,7 +352,7 @@ static qup_return_t qup_i2c_read_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj)
 		int count;
 
 		data = read32(fifo);
-		fifo++;
+		mdelay(1);
 
 		count = qup_i2c_parse_tag(data, data_ptr + idx, data_len);
 
@@ -367,11 +367,7 @@ static qup_return_t qup_i2c_read_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj)
 		idx += count;
 		data_len -= count;
 
-		if ((fifo >= fifo_end) || (data_len == 0)) {
-			fifo = QUP_ADDR(id, QUP_INPUT_FIFO);
-			qup_write32(QUP_ADDR(id, QUP_OPERATIONAL),
-					INPUT_SERVICE_FLAG);
-		}
+		qup_write32(QUP_ADDR(id, QUP_OPERATIONAL), INPUT_SERVICE_FLAG);
 	}
 
 	p_tx_obj->p.iic.data_len = idx;
@@ -392,6 +388,7 @@ static qup_return_t qup_i2c_read(blsp_qup_id_t id, uint8_t mode,
 
 	switch (mode) {
 	case QUP_MODE_FIFO:
+	case QUP_MODE_BLOCK:
 		ret = qup_i2c_read_fifo(id, p_tx_obj);
 		break;
 	default:
@@ -448,6 +445,7 @@ qup_return_t qup_init(blsp_qup_id_t id, const qup_config_t *config_ptr)
 	/* Set QUP IO Mode */
 	switch (config_ptr->mode) {
 	case QUP_MODE_FIFO:
+	case QUP_MODE_BLOCK:
 		reg_val = QUP_UNPACK_EN | QUP_PACK_EN |
 			  ((config_ptr->mode & QUP_MODE_MASK) <<
 					QUP_OUTPUT_MODE_SHFT) |