ipq806x: add i2c driver

this change ports i2c and other relevant drivers from depthcharge for ipq806x.

BUG=chrome-os-partner:33647
BRANCH=ToT
TEST=Booted storm using vboot2

Change-Id: I3d9a431aa8adb9b91dbccdf031647dfadbafc24c
Signed-off-by: Patrick Georgi <pgeorgi@chromium.org>
Original-Commit-Id: a0c615d0a49fd9c0ffa231353800882fff6ab90b
Original-Signed-off-by: Daisuke Nojiri <dnojiri@chromium.org>
Original-Change-Id: Id7cc3932ed4ae54f46336aaebde35e84125ebebd
Original-Reviewed-on: https://chromium-review.googlesource.com/229428
Original-Reviewed-by: Vadim Bendebury <vbendeb@chromium.org>
Original-Tested-by: Vadim Bendebury <vbendeb@chromium.org>
Original-Commit-Queue: Vadim Bendebury <vbendeb@chromium.org>
Reviewed-on: http://review.coreboot.org/9685
Tested-by: build bot (Jenkins)
Reviewed-by: Stefan Reinauer <stefan.reinauer@coreboot.org>

10 files changed, 412 insertions, 405 deletions

diff --git a/src/mainboard/google/storm/Kconfig b/src/mainboard/google/storm/Kconfig
index 6959d50ed8..c3406a0733 100644
--- a/src/mainboard/google/storm/Kconfig
+++ b/src/mainboard/google/storm/Kconfig
@@ -49,4 +49,12 @@ config DRAM_SIZE_MB
 	default 512 if BOARD_VARIANT_AP148
 	default 1024
 
+config DRIVER_TPM_I2C_BUS
+	hex
+	default 0x1
+
+config DRIVER_TPM_I2C_ADDR
+	hex
+	default 0x20
+
 endif # BOARD_GOOGLE_STORM
diff --git a/src/mainboard/google/storm/Makefile.inc b/src/mainboard/google/storm/Makefile.inc
index bcd786865a..6b7190cb65 100644
--- a/src/mainboard/google/storm/Makefile.inc
+++ b/src/mainboard/google/storm/Makefile.inc
@@ -22,6 +22,7 @@ bootblock-y += reset.c
 
 verstage-y += cdp.c
 verstage-y += chromeos.c
+verstage-y += gsbi.c
 verstage-y += memlayout.ld
 verstage-y += reset.c
 
diff --git a/src/soc/qualcomm/ipq806x/gsbi.h b/src/mainboard/google/storm/gsbi.c
index 0da7a4796e..17b74e5f3e 100644
--- a/src/soc/qualcomm/ipq806x/gsbi.h
+++ b/src/mainboard/google/storm/gsbi.c
@@ -27,34 +27,32 @@
  * SUCH DAMAGE.
  */
 
-#ifndef __GSBI_TYPES_H__
-#define __GSBI_TYPES_H__
+#include <soc/gpio.h>
+#include <soc/gsbi.h>
+#include <soc/qup.h>
 
-typedef enum {
-	GSBI_ID_1 = 1,
-	GSBI_ID_2,
-	GSBI_ID_3,
-	GSBI_ID_4,
-	GSBI_ID_5,
-	GSBI_ID_6,
-	GSBI_ID_7,
-} gsbi_id_t;
+#define GPIO_FUNC_I2C		0x1
 
-typedef enum {
-	GSBI_SUCCESS = 0,
-	GSBI_ID_ERROR,
-	GSBI_ERROR,
-	GSBI_UNSUPPORTED
-} gsbi_return_t;
+int gsbi_init_board(gsbi_id_t gsbi_id)
+{
+	switch (gsbi_id) {
+	case GSBI_ID_4:
+			/* Configure GPIOs 13 - SCL, 12 - SDA, 2mA gpio_en */
+			gpio_tlmm_config_set(12, GPIO_FUNC_I2C,
+					     GPIO_NO_PULL, GPIO_2MA, 1);
+			gpio_tlmm_config_set(13, GPIO_FUNC_I2C,
+					     GPIO_NO_PULL, GPIO_2MA, 1);
+		break;
+	case GSBI_ID_1:
+			/* Configure GPIOs 54 - SCL, 53 - SDA, 2mA gpio_en */
+			gpio_tlmm_config_set(54, GPIO_FUNC_I2C,
+					     GPIO_NO_PULL, GPIO_2MA, 1);
+			gpio_tlmm_config_set(53, GPIO_FUNC_I2C,
+					     GPIO_NO_PULL, GPIO_2MA, 1);
+		break;
+	default:
+		return 1;
+	}
 
-typedef enum {
-	GSBI_PROTO_I2C_UIM = 1,
-	GSBI_PROTO_I2C_ONLY,
-	GSBI_PROTO_SPI_ONLY,
-	GSBI_PROTO_UART_FLOW_CTL,
-	GSBI_PROTO_UIM,
-	GSBI_PROTO_I2C_UART,
-} gsbi_protocol_t;
-
-gsbi_return_t gsbi_init(gsbi_id_t gsbi_id, gsbi_protocol_t protocol);
-#endif
+	return 0;
+}
diff --git a/src/soc/qualcomm/ipq806x/Makefile.inc b/src/soc/qualcomm/ipq806x/Makefile.inc
index c0378bcdc9..0d5d59ca7f 100644
--- a/src/soc/qualcomm/ipq806x/Makefile.inc
+++ b/src/soc/qualcomm/ipq806x/Makefile.inc
@@ -25,6 +25,9 @@ bootblock-$(CONFIG_DRIVERS_UART) += uart.c
 
 verstage-y += clock.c
 verstage-y += gpio.c
+verstage-y += gsbi.c
+verstage-y += i2c.c
+verstage-y += qup.c
 verstage-y += spi.c
 verstage-y += timer.c
 verstage-$(CONFIG_CONSOLE_SERIAL_IPQ806X) += uart.c
diff --git a/src/soc/qualcomm/ipq806x/gsbi.c b/src/soc/qualcomm/ipq806x/gsbi.c
index be75d9a043..3fc723a555 100644
--- a/src/soc/qualcomm/ipq806x/gsbi.c
+++ b/src/soc/qualcomm/ipq806x/gsbi.c
@@ -1,5 +1,5 @@
 /*
- * This file is part of the depthcharge project.
+ * This file is part of the coreboot project.
  *
  * Copyright (C) 2014 The Linux Foundation. All rights reserved.
  *
@@ -28,8 +28,8 @@
  */
 
 #include <arch/io.h>
-#include "drivers/gpio/ipq806x.h"
-#include "ipq806x_gsbi.h"
+#include <soc/gsbi.h>
+#include <soc/gpio.h>
 
 //TODO: To be implemented as part of the iomap.
 static int gsbi_base[] = {
@@ -51,8 +51,6 @@ static int gsbi_base[] = {
 #define GSBI_APPS_NS_OFFSET	0x4
 #define GSBI_APPS_MAX_OFFSET	0xff
 
-#define GPIO_FUNC_I2C		0x1
-
 gsbi_return_t gsbi_init(gsbi_id_t gsbi_id, gsbi_protocol_t protocol)
 {
 	unsigned i = 0;
@@ -76,27 +74,9 @@ gsbi_return_t gsbi_init(gsbi_id_t gsbi_id, gsbi_protocol_t protocol)
 
 	writel(0, GSBI_RESET(gsbi_id));
 
-	switch (gsbi_id) {
-	case GSBI_ID_4: {
-			/* Configure GPIOs 13 - SCL, 12 - SDA, 2mA gpio_en */
-			gpio_tlmm_config_set(12, GPIO_FUNC_I2C,
-					     GPIO_NO_PULL, GPIO_2MA, 1);
-			gpio_tlmm_config_set(13, GPIO_FUNC_I2C,
-					     GPIO_NO_PULL, GPIO_2MA, 1);
-		}
-		break;
-	case GSBI_ID_1: {
-			/* Configure GPIOs 54 - SCL, 53 - SDA, 2mA gpio_en */
-			gpio_tlmm_config_set(54, GPIO_FUNC_I2C,
-					     GPIO_NO_PULL, GPIO_2MA, 1);
-			gpio_tlmm_config_set(53, GPIO_FUNC_I2C,
-					     GPIO_NO_PULL, GPIO_2MA, 1);
-		}
-		break;
-	default: {
+	if (gsbi_init_board(gsbi_id)) {
 		ret = GSBI_UNSUPPORTED;
 		goto bail_out;
-		}
 	}
 
 	/*Select i2c protocol*/
diff --git a/src/soc/qualcomm/ipq806x/i2c.c b/src/soc/qualcomm/ipq806x/i2c.c
index 9bae822a8e..a4d1c00856 100644
--- a/src/soc/qualcomm/ipq806x/i2c.c
+++ b/src/soc/qualcomm/ipq806x/i2c.c
@@ -1,5 +1,5 @@
 /*
- * This file is part of the depthcharge project.
+ * This file is part of the coreboot project.
  *
  * Copyright (C) 2014 The Linux Foundation. All rights reserved.
  *
@@ -27,43 +27,25 @@
  * SUCH DAMAGE.
  */
 
+#include <arch/io.h>
 #include <assert.h>
-#include <libpayload.h>
-
-#include "base/container_of.h"
-#include "drivers/bus/i2c/i2c.h"
-#include "drivers/bus/i2c/ipq806x_qup.h"
-#include "drivers/bus/i2c/ipq806x_gsbi.h"
-#include "drivers/bus/i2c/ipq806x.h"
-
-static int i2c_init(unsigned gsbi_id)
-{
-	gsbi_return_t gsbi_ret = 0;
-	qup_return_t qup_ret = 0;
-	qup_config_t gsbi4_qup_config = {
-		QUP_MINICORE_I2C_MASTER,
-		100000,
-		24000000,
-		QUP_MODE_FIFO
-	};
-
-	gsbi_ret = gsbi_init(gsbi_id, GSBI_PROTO_I2C_ONLY);
-	if (GSBI_SUCCESS != gsbi_ret)
-		return 1;
-
-	qup_ret = qup_init(gsbi_id, &gsbi4_qup_config);
-	if (QUP_SUCCESS != qup_ret)
-		return 1;
-
-	qup_ret = qup_reset_i2c_master_status(gsbi_id);
-	if (QUP_SUCCESS != qup_ret)
-		return 1;
-
-	return 0;
-}
+#include <console/console.h>
+#include <delay.h>
+#include <device/i2c.h>
+#include <stdlib.h>
+#include <string.h>
+#include <soc/gsbi.h>
+#include <soc/qup.h>
+
+static const qup_config_t gsbi4_qup_config = {
+	QUP_MINICORE_I2C_MASTER,
+	100000,
+	24000000,
+	QUP_MODE_FIFO
+};
 
 static int i2c_read(uint32_t gsbi_id, uint8_t slave,
-			uint8_t *data, int data_len)
+		    uint8_t *data, int data_len)
 {
 	qup_data_t obj;
 	qup_return_t qup_ret = 0;
@@ -100,44 +82,54 @@ static int i2c_write(uint32_t gsbi_id, uint8_t slave,
 		return 0;
 }
 
-static int i2c_transfer(struct I2cOps *me, I2cSeg *segments, int seg_count)
+static int i2c_init(unsigned bus)
 {
-	Ipq806xI2c *bus = container_of(me, Ipq806xI2c, ops);
-	I2cSeg *seg = segments;
-	int ret = 0;
+	static uint8_t initialized = 0;
+	unsigned gsbi_id = bus;
 
-	if (!bus->initialized)
-		if (0 != i2c_init(bus->gsbi_id))
-			return 1;
+	if (initialized)
+		return 0;
 
-	while (seg_count--) {
-		if (seg->read)
-			ret = i2c_read(bus->gsbi_id, seg->chip,
-				       seg->buf, seg->len);
-		else
-			ret  = i2c_write(bus->gsbi_id, seg->chip,
-					 seg->buf, seg->len,
-					 (seg_count ? 0 : 1));
-		seg++;
+	if (gsbi_init(gsbi_id, GSBI_PROTO_I2C_ONLY)) {
+		printk(BIOS_ERR, "failed to initialize gsbi\n");
+		return 1;
 	}
 
-	if (QUP_SUCCESS != ret) {
-		qup_set_state(bus->gsbi_id, QUP_STATE_RESET);
+	if (qup_init(gsbi_id, &gsbi4_qup_config)) {
+		printk(BIOS_ERR, "failed to initialize qup\n");
 		return 1;
 	}
 
+	if (qup_reset_i2c_master_status(gsbi_id)) {
+		printk(BIOS_ERR, "failed to reset i2c master status\n");
+		return 1;
+	}
+
+	initialized = 1;
 	return 0;
 }
 
-Ipq806xI2c *new_ipq806x_i2c(unsigned gsbi_id)
+int platform_i2c_transfer(unsigned bus, struct i2c_seg *segments, int seg_count)
 {
-	Ipq806xI2c *bus = 0;
+	struct i2c_seg *seg = segments;
+	int ret = 0;
+
+	if (i2c_init(bus))
+		return 1;
 
-	if (!i2c_init(gsbi_id)) {
-		bus = xzalloc(sizeof(*bus));
-		bus->gsbi_id = gsbi_id;
-		bus->initialized = 1;
-		bus->ops.transfer = &i2c_transfer;
+	while (seg_count--) {
+		if (seg->read)
+			ret = i2c_read(bus, seg->chip, seg->buf, seg->len);
+		else
+			ret = i2c_write(bus, seg->chip, seg->buf, seg->len,
+					(seg_count ? 0 : 1));
+		seg++;
 	}
-	return bus;
+
+	if (ret) {
+		qup_set_state(bus, QUP_STATE_RESET);
+		return 1;
+	}
+
+	return 0;
 }
diff --git a/src/soc/qualcomm/ipq806x/include/soc/gsbi.h b/src/soc/qualcomm/ipq806x/include/soc/gsbi.h
index c12d6fd7b4..00c257c4b8 100644
--- a/src/soc/qualcomm/ipq806x/include/soc/gsbi.h
+++ b/src/soc/qualcomm/ipq806x/include/soc/gsbi.h
@@ -31,5 +31,33 @@
 #define GSBI_HCLK_CTL_S                 4
 #define GSBI_HCLK_CTL_CLK_ENA           0x1
 
-#endif
+typedef enum {
+	GSBI_ID_1 = 1,
+	GSBI_ID_2,
+	GSBI_ID_3,
+	GSBI_ID_4,
+	GSBI_ID_5,
+	GSBI_ID_6,
+	GSBI_ID_7,
+} gsbi_id_t;
+
+typedef enum {
+	GSBI_SUCCESS = 0,
+	GSBI_ID_ERROR,
+	GSBI_ERROR,
+	GSBI_UNSUPPORTED
+} gsbi_return_t;
+
+typedef enum {
+	GSBI_PROTO_I2C_UIM = 1,
+	GSBI_PROTO_I2C_ONLY,
+	GSBI_PROTO_SPI_ONLY,
+	GSBI_PROTO_UART_FLOW_CTL,
+	GSBI_PROTO_UIM,
+	GSBI_PROTO_I2C_UART,
+} gsbi_protocol_t;
 
+gsbi_return_t gsbi_init(gsbi_id_t gsbi_id, gsbi_protocol_t protocol);
+int gsbi_init_board(gsbi_id_t gsbi_id);
+
+#endif
diff --git a/src/soc/qualcomm/ipq806x/include/soc/iomap.h b/src/soc/qualcomm/ipq806x/include/soc/iomap.h
index ad7056e0fe..333e2615eb 100644
--- a/src/soc/qualcomm/ipq806x/include/soc/iomap.h
+++ b/src/soc/qualcomm/ipq806x/include/soc/iomap.h
@@ -107,4 +107,11 @@
 #define UART2_DM_BASE           0x12490000
 #define UART_GSBI2_BASE         0x12480000
 
+#define GSBI_QUP1_BASE		0x12460000
+#define GSBI_QUP2_BASE		0x124A0000
+#define GSBI_QUP3_BASE		0x16280000
+#define GSBI_QUP4_BASE		0x16380000
+#define GSBI_QUP5_BASE		0x1A280000
+#define GSBI_QUP6_BASE		0x16580000
+#define GSBI_QUP7_BASE		0x16680000
 #endif // __SOC_QUALCOMM_IPQ806X_IOMAP_H_
diff --git a/src/soc/qualcomm/ipq806x/qup.h b/src/soc/qualcomm/ipq806x/include/soc/qup.h
index f848cf6413..8f6f6bcca9 100644
--- a/src/soc/qualcomm/ipq806x/qup.h
+++ b/src/soc/qualcomm/ipq806x/include/soc/qup.h
@@ -1,5 +1,5 @@
 /*
- * This file is part of the depthcharge project.
+ * This file is part of the coreboot project.
  *
  * Copyright (C) 2014 The Linux Foundation. All rights reserved.
  *
@@ -29,7 +29,8 @@
 
 #ifndef __QUP_H__
 #define __QUP_H__
-#include "ipq806x_gsbi.h"
+
+#include <soc/gsbi.h>
 
 /* QUP block registers */
 #define QUP_CONFIG			0x0
@@ -117,6 +118,9 @@ typedef enum {
 	QUP_ERR_STATE_SET,
 	QUP_ERR_TIMEOUT,
 	QUP_ERR_UNSUPPORTED,
+	QUP_ERR_I2C_FAILED,
+	QUP_ERR_I2C_ARB_LOST,
+	QUP_ERR_I2C_BUS_ERROR,
 	QUP_ERR_I2C_INVALID_SLAVE_ADDR,
 	QUP_ERR_XFER_FAIL,
 	QUP_ERR_UNDEFINED,
@@ -164,7 +168,7 @@ typedef struct {
  *
  * return: QUP_SUCCESS, if initialization succeeds.
  */
-qup_return_t qup_init(gsbi_id_t gsbi_id, qup_config_t *config_ptr);
+qup_return_t qup_init(gsbi_id_t gsbi_id, const qup_config_t *config_ptr);
 
 /*
  * Set QUP state to run, pause, reset.
diff --git a/src/soc/qualcomm/ipq806x/qup.c b/src/soc/qualcomm/ipq806x/qup.c
index 5ac7f07e58..0ae3990183 100644
--- a/src/soc/qualcomm/ipq806x/qup.c
+++ b/src/soc/qualcomm/ipq806x/qup.c
@@ -1,5 +1,5 @@
 /*
- * This file is part of the depthcharge project.
+ * This file is part of the coreboot project.
  *
  * Copyright (C) 2014 The Linux Foundation. All rights reserved.
  *
@@ -28,292 +28,283 @@
  */
 
 #include <arch/io.h>
-#include <libpayload.h>
-#include "ipq806x_qup.h"
+#include <console/console.h>
+#include <delay.h>
+#include <soc/iomap.h>
+#include <stdlib.h>
+#include <soc/qup.h>
+
+#define TIMEOUT_CNT	100000
 
 //TODO: refactor the following array to iomap driver.
 static unsigned gsbi_qup_base[] = {
-	0x12460000, /*gsbi 1*/
-	0x124A0000, /*gsbi 2*/
-	0x16280000, /*gsbi 3*/
-	0x16380000, /*gsbi 4*/
-	0x1A280000, /*gsbi 5*/
-	0x16580000, /*gsbi 6*/
-	0x16680000, /*gsbi 7*/
+	GSBI_QUP1_BASE,
+	GSBI_QUP2_BASE,
+	GSBI_QUP3_BASE,
+	GSBI_QUP4_BASE,
+	GSBI_QUP5_BASE,
+	GSBI_QUP6_BASE,
+	GSBI_QUP7_BASE,
 };
 
 #define QUP_ADDR(gsbi_num, reg)	((void *)((gsbi_qup_base[gsbi_num-1]) + (reg)))
-#define MAX_DELAY_MS	100
-
-static char *get_error_string(qup_return_t error)
-{
-	char *msg;
-	switch (error) {
-	case QUP_ERR_BAD_PARAM:
-		msg = "bad parameter";
-		break;
-	case QUP_ERR_STATE_SET:
-		msg = "setting state failed";
-		break;
-	case QUP_ERR_TIMEOUT:
-		msg = "timeout";
-		break;
-	case QUP_ERR_UNSUPPORTED:
-		msg = "unsupported";
-		break;
-	case QUP_ERR_I2C_INVALID_SLAVE_ADDR:
-		msg = "invalid slave address";
-		break;
-	case QUP_ERR_XFER_FAIL:
-		msg = "transfer failed";
-		break;
-	case QUP_ERR_UNDEFINED:
-	default:
-		msg = "undefined";
-		break;
-	}
-	return msg;
-}
 
 static qup_return_t qup_i2c_master_status(gsbi_id_t gsbi_id)
 {
-	qup_return_t ret = QUP_SUCCESS;
 	uint32_t reg_val = readl(QUP_ADDR(gsbi_id, QUP_I2C_MASTER_STATUS));
 
 	if (readl(QUP_ADDR(gsbi_id, QUP_ERROR_FLAGS)))
-		ret = QUP_ERR_XFER_FAIL;
-	else if (reg_val & QUP_I2C_INVALID_READ_ADDR)
-		ret = QUP_ERR_I2C_INVALID_SLAVE_ADDR;
-	else if (reg_val & (QUP_I2C_FAILED_MASK |
-			    QUP_I2C_ARB_LOST |
-			    QUP_I2C_BUS_ERROR))
-		ret = QUP_ERR_XFER_FAIL;
+		return QUP_ERR_XFER_FAIL;
+	if (reg_val & QUP_I2C_INVALID_READ_ADDR)
+		return QUP_ERR_I2C_INVALID_SLAVE_ADDR;
+	if (reg_val & QUP_I2C_FAILED_MASK)
+		return QUP_ERR_I2C_FAILED;
+	if (reg_val & QUP_I2C_ARB_LOST)
+		return QUP_ERR_I2C_ARB_LOST;
+	if (reg_val & QUP_I2C_BUS_ERROR)
+		return QUP_ERR_I2C_BUS_ERROR;
 
-	return ret;
+	return QUP_SUCCESS;
+}
+
+static int check_bit_state(uint32_t *reg, int wait_for)
+{
+	unsigned int count = TIMEOUT_CNT;
+
+	while ((readl(reg) & (QUP_STATE_VALID_MASK | QUP_STATE_MASK)) !=
+			(QUP_STATE_VALID | wait_for)) {
+		if (count == 0)
+			return QUP_ERR_TIMEOUT;
+		count--;
+		udelay(1);
+	}
+
+	return QUP_SUCCESS;
 }
 
+/*
+ * Check whether GSBIn_QUP State is valid
+ */
 static qup_return_t qup_wait_for_state(gsbi_id_t gsbi_id, unsigned wait_for)
 {
-	qup_return_t ret = QUP_ERR_STATE_SET;
-	uint32_t val = 0;
-	uint32_t start_ts;
-	uint32_t d = MAX_DELAY_MS * timer_hz() / 1000;
-	uint8_t final_state = 0;
-
-	start_ts = timer_raw_value();
-	do {
-		val = readl(QUP_ADDR(gsbi_id, QUP_STATE));
-		final_state = ((val & (QUP_STATE_VALID_MASK|QUP_STATE_MASK))
-						 == (QUP_STATE_VALID|wait_for));
-	} while ((!final_state) && (start_ts > (timer_raw_value() - d)));
-
-	if (final_state)
-		ret = QUP_SUCCESS;
+	return check_bit_state(QUP_ADDR(gsbi_id, QUP_STATE), wait_for);
+}
 
-	return ret;
+qup_return_t qup_reset_i2c_master_status(gsbi_id_t gsbi_id)
+{
+	/*
+	 * Writing a one clears the status bits.
+	 * Bit31-25, Bit1 and Bit0 are reserved.
+	 */
+	//TODO: Define each status bit. OR all status bits in a single macro.
+	writel(0x3FFFFFC, QUP_ADDR(gsbi_id, QUP_I2C_MASTER_STATUS));
+	return QUP_SUCCESS;
 }
 
-static qup_return_t qup_i2c_write(gsbi_id_t gsbi_id, uint8_t mode,
-				  qup_data_t *p_tx_obj, uint8_t stop_seq)
+static qup_return_t qup_reset_master_status(gsbi_id_t gsbi_id)
+{
+	writel(0x7C, QUP_ADDR(gsbi_id, QUP_ERROR_FLAGS));
+	writel(0x7C, QUP_ADDR(gsbi_id, QUP_ERROR_FLAGS_EN));
+	qup_reset_i2c_master_status(gsbi_id);
+	return QUP_SUCCESS;
+}
+
+static qup_return_t qup_fifo_wait_for(gsbi_id_t gsbi_id, uint32_t status)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
-	uint32_t start_ts;
-	uint32_t d = MAX_DELAY_MS * timer_hz() / 1000;
+	unsigned int count = TIMEOUT_CNT;
+
+	while (!(readl(QUP_ADDR(gsbi_id, QUP_OPERATIONAL)) & status)) {
+		ret = qup_i2c_master_status(gsbi_id);
+		if (ret)
+			return ret;
+		if (count == 0)
+			return QUP_ERR_TIMEOUT;
+		count--;
+	}
 
-	switch (mode) {
-	case QUP_MODE_FIFO: {
-		uint8_t addr = p_tx_obj->p.iic.addr;
-		uint8_t *data_ptr = p_tx_obj->p.iic.data;
-		unsigned data_len = p_tx_obj->p.iic.data_len;
-		unsigned idx = 0;
-
-		writel(0x7C, QUP_ADDR(gsbi_id, QUP_ERROR_FLAGS));
-		writel(0x7C, QUP_ADDR(gsbi_id, QUP_ERROR_FLAGS_EN));
-		qup_reset_i2c_master_status(gsbi_id);
-		qup_set_state(gsbi_id, QUP_STATE_RUN);
-
-		writel((QUP_I2C_START_SEQ | QUP_I2C_ADDR(addr)),
-			QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO));
-
-		while (data_len) {
-			if (data_len == 1 && stop_seq) {
-				writel((QUP_I2C_STOP_SEQ |
-					QUP_I2C_DATA(data_ptr[idx])),
-					QUP_ADDR(gsbi_id,
-						 QUP_OUTPUT_FIFO));
-			} else {
-				writel((QUP_I2C_DATA_SEQ |
-					QUP_I2C_DATA(data_ptr[idx])),
-					QUP_ADDR(gsbi_id,
-						 QUP_OUTPUT_FIFO));
-			}
-			data_len--;
-			idx++;
-			start_ts = timer_raw_value();
-			while (data_len && readl(QUP_ADDR(gsbi_id,
-						  QUP_OPERATIONAL)) &
-						  OUTPUT_FIFO_FULL) {
-				ret = qup_i2c_master_status(gsbi_id);
-				if (QUP_SUCCESS != ret)
-					goto bailout;
-				if (start_ts < (timer_raw_value() - d)) {
-					ret = QUP_ERR_TIMEOUT;
-					goto bailout;
-				}
-			}
-			/* Hardware sets the OUTPUT_SERVICE_FLAG flag to 1 when
+	return QUP_SUCCESS;
+}
+
+static qup_return_t qup_fifo_wait_while(gsbi_id_t gsbi_id, uint32_t status)
+{
+	qup_return_t ret = QUP_ERR_UNDEFINED;
+	unsigned int count = TIMEOUT_CNT;
+
+	while (readl(QUP_ADDR(gsbi_id, QUP_OPERATIONAL)) & status) {
+		ret = qup_i2c_master_status(gsbi_id);
+		if (ret)
+			return ret;
+		if (count == 0)
+			return QUP_ERR_TIMEOUT;
+		count--;
+	}
+
+	return QUP_SUCCESS;
+}
+
+static qup_return_t qup_i2c_write_fifo(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj,
+				       uint8_t stop_seq)
+{
+	qup_return_t ret = QUP_ERR_UNDEFINED;
+	uint8_t addr = p_tx_obj->p.iic.addr;
+	uint8_t *data_ptr = p_tx_obj->p.iic.data;
+	unsigned data_len = p_tx_obj->p.iic.data_len;
+	unsigned idx = 0;
+
+	qup_reset_master_status(gsbi_id);
+	qup_set_state(gsbi_id, QUP_STATE_RUN);
+
+	writel((QUP_I2C_START_SEQ | QUP_I2C_ADDR(addr)),
+	       QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO));
+
+	while (data_len) {
+		if (data_len == 1 && stop_seq) {
+			writel((QUP_I2C_STOP_SEQ | QUP_I2C_DATA(data_ptr[idx])),
+			       QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO));
+		} else {
+			writel((QUP_I2C_DATA_SEQ | QUP_I2C_DATA(data_ptr[idx])),
+			       QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO));
+		}
+		data_len--;
+		idx++;
+		if (data_len) {
+			ret = qup_fifo_wait_while(gsbi_id, OUTPUT_FIFO_FULL);
+			if (ret)
+				return ret;
+		}
+		/* Hardware sets the OUTPUT_SERVICE_FLAG flag to 1 when
 			OUTPUT_FIFO_NOT_EMPTY flag in the QUP_OPERATIONAL
 			register changes from 1 to 0, indicating that software
 			can write more data to the output FIFO. Software should
 			set OUTPUT_SERVICE_FLAG to 1 to clear it to 0, which
 			means that software knows to return to fill the output
 			FIFO with data.
-			*/
-			if (readl(QUP_ADDR(gsbi_id, QUP_OPERATIONAL)) &
-					      OUTPUT_SERVICE_FLAG) {
-				writel(OUTPUT_SERVICE_FLAG,
-				       QUP_ADDR(gsbi_id,
-				       QUP_OPERATIONAL));
-			}
+		 */
+		if (readl(QUP_ADDR(gsbi_id, QUP_OPERATIONAL)) &
+				OUTPUT_SERVICE_FLAG) {
+			writel(OUTPUT_SERVICE_FLAG,
+			       QUP_ADDR(gsbi_id, QUP_OPERATIONAL));
 		}
+	}
 
-		start_ts = timer_raw_value();
-		while (((readl(QUP_ADDR(gsbi_id, QUP_OPERATIONAL))) &
-					 OUTPUT_FIFO_NOT_EMPTY)) {
-			ret = qup_i2c_master_status(gsbi_id);
-			if (QUP_SUCCESS != ret)
-				goto bailout;
-			if (start_ts < (timer_raw_value() - d)) {
-				ret = QUP_ERR_TIMEOUT;
-				goto bailout;
-			}
-		}
+	ret = qup_fifo_wait_while(gsbi_id, OUTPUT_FIFO_NOT_EMPTY);
+	if (ret)
+		return ret;
 
-		qup_set_state(gsbi_id, QUP_STATE_PAUSE);
-		ret = QUP_SUCCESS;
-	}
-	break;
+	qup_set_state(gsbi_id, QUP_STATE_PAUSE);
+	return QUP_SUCCESS;
+}
 
+static qup_return_t qup_i2c_write(gsbi_id_t gsbi_id, uint8_t mode,
+				  qup_data_t *p_tx_obj, uint8_t stop_seq)
+{
+	qup_return_t ret = QUP_ERR_UNDEFINED;
+
+	switch (mode) {
+	case QUP_MODE_FIFO:
+		ret = qup_i2c_write_fifo(gsbi_id, p_tx_obj, stop_seq);
+		break;
 	default:
 		ret = QUP_ERR_UNSUPPORTED;
 	}
 
-bailout:
-	if (QUP_SUCCESS != ret) {
+	if (ret) {
 		qup_set_state(gsbi_id, QUP_STATE_RESET);
-		printf("%s() returns %s\n", __func__, get_error_string(ret));
+		printk(BIOS_ERR, "%s() failed (%d)\n", __func__, ret);
 	}
 
 	return ret;
 }
 
-static qup_return_t qup_i2c_read(gsbi_id_t gsbi_id, uint8_t mode,
-				  qup_data_t *p_tx_obj)
+static qup_return_t qup_i2c_read_fifo(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
-	uint32_t start_ts;
-	uint32_t d = MAX_DELAY_MS * timer_hz() / 1000;
+	uint8_t addr = p_tx_obj->p.iic.addr;
+	uint8_t *data_ptr = p_tx_obj->p.iic.data;
+	unsigned data_len = p_tx_obj->p.iic.data_len;
+	unsigned idx = 0;
 
-	switch (mode) {
-	case QUP_MODE_FIFO: {
-		uint8_t addr = p_tx_obj->p.iic.addr;
-		uint8_t *data_ptr = p_tx_obj->p.iic.data;
-		unsigned data_len = p_tx_obj->p.iic.data_len;
-		unsigned idx = 0;
-
-		writel(0x7C, QUP_ADDR(gsbi_id, QUP_ERROR_FLAGS));
-		writel(0x7C, QUP_ADDR(gsbi_id, QUP_ERROR_FLAGS_EN));
-		qup_reset_i2c_master_status(gsbi_id);
-		qup_set_state(gsbi_id, QUP_STATE_RUN);
-
-		writel((QUP_I2C_START_SEQ |
-		       (QUP_I2C_ADDR(addr)|
-			QUP_I2C_SLAVE_READ)),
-			QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO));
-
-		writel((QUP_I2C_RECV_SEQ | data_len),
-			QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO));
-
-		start_ts = timer_raw_value();
-		while ((readl(QUP_ADDR(gsbi_id, QUP_OPERATIONAL)) &
-				 OUTPUT_FIFO_NOT_EMPTY)) {
-			ret = qup_i2c_master_status(gsbi_id);
-			if (QUP_SUCCESS != ret)
-				goto bailout;
-			if (start_ts < (timer_raw_value() - d)) {
-				ret = QUP_ERR_TIMEOUT;
-				goto bailout;
-			}
-		}
+	qup_reset_master_status(gsbi_id);
+	qup_set_state(gsbi_id, QUP_STATE_RUN);
+
+	writel((QUP_I2C_START_SEQ | (QUP_I2C_ADDR(addr) | QUP_I2C_SLAVE_READ)),
+	       QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO));
 
-		writel(OUTPUT_SERVICE_FLAG,
-		       QUP_ADDR(gsbi_id, QUP_OPERATIONAL));
-
-		while (data_len) {
-			unsigned data;
-			start_ts = timer_raw_value();
-			while ((!((readl(QUP_ADDR(gsbi_id, QUP_OPERATIONAL))) &
-				   INPUT_SERVICE_FLAG))) {
-				ret = qup_i2c_master_status(gsbi_id);
-				if (QUP_SUCCESS != ret)
-					goto bailout;
-				if (start_ts < (timer_raw_value() - d)) {
-					ret = QUP_ERR_TIMEOUT;
-					goto bailout;
-				}
-			}
-
-			data = readl(QUP_ADDR(gsbi_id, QUP_INPUT_FIFO));
-
-			/*Process tag and corresponding data value.
-			For I2C master mini-core, data in FIFO is composed of
-			16 bits and is divided into an 8-bit tag for the upper
-			bits and 8-bit data for the lower bits.
-			The 8-bit tag indicates whether the byte is the last
-			byte, or if a bus error happened during the receipt of
-			the byte.*/
-			if ((QUP_I2C_MI_TAG(data)) == QUP_I2C_MIDATA_SEQ) {
-				/* Tag: MIDATA = Master input data.*/
-				data_ptr[idx] = QUP_I2C_DATA(data);
-				idx++;
-				data_len--;
-				writel(INPUT_SERVICE_FLAG, QUP_ADDR(gsbi_id,
-				       QUP_OPERATIONAL));
-			} else if (QUP_I2C_MI_TAG(data) ==
-							QUP_I2C_MISTOP_SEQ) {
-				/* Tag: MISTOP: Last byte of master input. */
-				data_ptr[idx] = QUP_I2C_DATA(data);
-				idx++;
-				data_len--;
-				goto recv_done;
-			} else {
-				/* Tag: MINACK: Invalid master input data.*/
-				goto recv_done;
-			}
+	writel((QUP_I2C_RECV_SEQ | data_len),
+	       QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO));
+
+	ret = qup_fifo_wait_while(gsbi_id, OUTPUT_FIFO_NOT_EMPTY);
+	if (ret)
+		return ret;
+
+	writel(OUTPUT_SERVICE_FLAG, QUP_ADDR(gsbi_id, QUP_OPERATIONAL));
+
+	while (data_len) {
+		uint32_t data;
+
+		ret = qup_fifo_wait_for(gsbi_id, INPUT_SERVICE_FLAG);
+		if (ret)
+			return ret;
+
+		data = readl(QUP_ADDR(gsbi_id, QUP_INPUT_FIFO));
+
+		/*
+		 * Process tag and corresponding data value. For I2C master
+		 * mini-core, data in FIFO is composed of 16 bits and is divided
+		 * into an 8-bit tag for the upper bits and 8-bit data for the
+		 * lower bits. The 8-bit tag indicates whether the byte is the
+		 * last byte, or if a bus error happened during the receipt of
+		 * the byte.
+		 */
+		if ((QUP_I2C_MI_TAG(data)) == QUP_I2C_MIDATA_SEQ) {
+			/* Tag: MIDATA = Master input data.*/
+			data_ptr[idx] = QUP_I2C_DATA(data);
+			idx++;
+			data_len--;
+			writel(INPUT_SERVICE_FLAG,
+			       QUP_ADDR(gsbi_id, QUP_OPERATIONAL));
+		} else if (QUP_I2C_MI_TAG(data) == QUP_I2C_MISTOP_SEQ) {
+			/* Tag: MISTOP: Last byte of master input. */
+			data_ptr[idx] = QUP_I2C_DATA(data);
+			idx++;
+			data_len--;
+			break;
+		} else {
+			/* Tag: MINACK: Invalid master input data.*/
+			break;
 		}
-recv_done:
-		writel(INPUT_SERVICE_FLAG,
-		       QUP_ADDR(gsbi_id, QUP_OPERATIONAL));
-		p_tx_obj->p.iic.data_len = idx;
-		qup_set_state(gsbi_id, QUP_STATE_PAUSE);
-		ret = QUP_SUCCESS;
 	}
-	break;
 
+	writel(INPUT_SERVICE_FLAG, QUP_ADDR(gsbi_id, QUP_OPERATIONAL));
+	p_tx_obj->p.iic.data_len = idx;
+	qup_set_state(gsbi_id, QUP_STATE_PAUSE);
+
+	return QUP_SUCCESS;
+}
+
+static qup_return_t qup_i2c_read(gsbi_id_t gsbi_id, uint8_t mode,
+				 qup_data_t *p_tx_obj)
+{
+	qup_return_t ret = QUP_ERR_UNDEFINED;
+
+	switch (mode) {
+	case QUP_MODE_FIFO:
+		ret = qup_i2c_read_fifo(gsbi_id, p_tx_obj);
+		break;
 	default:
 		ret = QUP_ERR_UNSUPPORTED;
 	}
 
-bailout:
-	if (QUP_SUCCESS != ret) {
+	if (ret) {
 		qup_set_state(gsbi_id, QUP_STATE_RESET);
-		printf("%s() returns %s\n", __func__, get_error_string(ret));
+		printk(BIOS_ERR, "%s() failed (%d)\n", __func__, ret);
 	}
 
 	return ret;
 }
 
-qup_return_t qup_init(gsbi_id_t gsbi_id, qup_config_t *config_ptr)
+qup_return_t qup_init(gsbi_id_t gsbi_id, const qup_config_t *config_ptr)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
 	uint32_t reg_val;
@@ -323,11 +314,10 @@ qup_return_t qup_init(gsbi_id_t gsbi_id, qup_config_t *config_ptr)
 
 	/*Wait till the reset takes effect */
 	ret = qup_wait_for_state(gsbi_id, QUP_STATE_RESET);
+	if (ret)
+		goto bailout;
 
-	if (QUP_SUCCESS != ret)
-		return ret;
-
-	/*Reset the config*/
+	/* Reset the config */
 	writel(0, QUP_ADDR(gsbi_id, QUP_CONFIG));
 
 	/*Program the config register*/
@@ -335,16 +325,14 @@ qup_return_t qup_init(gsbi_id_t gsbi_id, qup_config_t *config_ptr)
 	reg_val = 0x0F;
 	/*Set protocol*/
 	switch (config_ptr->protocol) {
-	case QUP_MINICORE_I2C_MASTER: {
+	case QUP_MINICORE_I2C_MASTER:
 		reg_val |= ((config_ptr->protocol &
 				QUP_MINI_CORE_PROTO_MASK) <<
 				QUP_MINI_CORE_PROTO_SHFT);
-		}
 		break;
-	default: {
+	default:
 		ret = QUP_ERR_UNSUPPORTED;
 		goto bailout;
-		}
 	}
 	writel(reg_val, QUP_ADDR(gsbi_id, QUP_CONFIG));
 
@@ -353,18 +341,16 @@ qup_return_t qup_init(gsbi_id_t gsbi_id, qup_config_t *config_ptr)
 
 	/*Set QUP IO Mode*/
 	switch (config_ptr->mode) {
-	case QUP_MODE_FIFO: {
+	case QUP_MODE_FIFO:
 		reg_val = QUP_OUTPUT_BIT_SHIFT_EN |
 			  ((config_ptr->mode & QUP_MODE_MASK) <<
 					QUP_OUTPUT_MODE_SHFT) |
 			  ((config_ptr->mode & QUP_MODE_MASK) <<
 					QUP_INPUT_MODE_SHFT);
-		}
 		break;
-	default: {
+	default:
 		ret = QUP_ERR_UNSUPPORTED;
 		goto bailout;
-		}
 	}
 	writel(reg_val, QUP_ADDR(gsbi_id, QUP_IO_MODES));
 
@@ -376,8 +362,8 @@ qup_return_t qup_init(gsbi_id_t gsbi_id, qup_config_t *config_ptr)
 	writel(reg_val, QUP_ADDR(gsbi_id, QUP_I2C_MASTER_CLK_CTL));
 
 bailout:
-	if (QUP_SUCCESS != ret)
-		printf("%s() returns %s\n", __func__, get_error_string(ret));
+	if (ret)
+		printk(BIOS_ERR, "failed to init qup (%d)\n", ret);
 
 	return ret;
 }
@@ -394,8 +380,7 @@ qup_return_t qup_set_state(gsbi_id_t gsbi_id, uint32_t state)
 		* two writes of  10[binary]) are required for the
 		* transition to complete.
 		*/
-		if (QUP_STATE_PAUSE == curr_state &&
-		    QUP_STATE_RESET == state) {
+		if (QUP_STATE_PAUSE == curr_state && QUP_STATE_RESET == state) {
 			writel(0x2, QUP_ADDR(gsbi_id, QUP_STATE));
 			writel(0x2, QUP_ADDR(gsbi_id, QUP_STATE));
 		} else {
@@ -403,18 +388,28 @@ qup_return_t qup_set_state(gsbi_id_t gsbi_id, uint32_t state)
 		}
 		ret = qup_wait_for_state(gsbi_id, state);
 	}
+
 	return ret;
 }
 
-qup_return_t qup_reset_i2c_master_status(gsbi_id_t gsbi_id)
+static qup_return_t qup_i2c_send_data(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj,
+				      uint8_t stop_seq)
 {
-	/*
-	 * Writing a one clears the status bits.
-	 * Bit31-25, Bit1 and Bit0 are reserved.
-	 */
-	//TODO: Define each status bit. OR all status bits in a single macro.
-	writel(0x3FFFFFC, QUP_ADDR(gsbi_id, QUP_I2C_MASTER_STATUS));
-	return QUP_SUCCESS;
+	qup_return_t ret = QUP_ERR_UNDEFINED;
+	uint8_t mode = (readl(QUP_ADDR(gsbi_id, QUP_IO_MODES)) >>
+			QUP_OUTPUT_MODE_SHFT) & QUP_MODE_MASK;
+
+	ret = qup_i2c_write(gsbi_id, mode, p_tx_obj, stop_seq);
+	if (0) {
+		int i;
+		printk(BIOS_DEBUG, "i2c tx bus %d device %2.2x:",
+		       gsbi_id, p_tx_obj->p.iic.addr);
+		for (i = 0; i < p_tx_obj->p.iic.data_len; i++)
+			printk(BIOS_DEBUG, " %2.2x", p_tx_obj->p.iic.data[i]);
+		printk(BIOS_DEBUG, "\n");
+	}
+
+	return ret;
 }
 
 qup_return_t qup_send_data(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj,
@@ -423,61 +418,52 @@ qup_return_t qup_send_data(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj,
 	qup_return_t ret = QUP_ERR_UNDEFINED;
 
 	if (p_tx_obj->protocol == ((readl(QUP_ADDR(gsbi_id, QUP_CONFIG)) >>
-						    QUP_MINI_CORE_PROTO_SHFT) &
-						    QUP_MINI_CORE_PROTO_MASK)) {
+			QUP_MINI_CORE_PROTO_SHFT) & QUP_MINI_CORE_PROTO_MASK)) {
 		switch (p_tx_obj->protocol) {
-		case QUP_MINICORE_I2C_MASTER: {
-			uint8_t mode = (readl(QUP_ADDR(gsbi_id,
-						       QUP_IO_MODES)) >>
-					      QUP_OUTPUT_MODE_SHFT) &
-					QUP_MODE_MASK;
-			ret = qup_i2c_write(gsbi_id, mode, p_tx_obj, stop_seq);
-			if (0) {
-				int i;
-				printf("i2c tx bus %d device %2.2x:",
-					gsbi_id, p_tx_obj->p.iic.addr);
-				for (i = 0; i < p_tx_obj->p.iic.data_len; i++)
-					printf(" %2.2x",
-						p_tx_obj->p.iic.data[i]);
-				printf("\n");
-			}
+		case QUP_MINICORE_I2C_MASTER:
+			ret = qup_i2c_send_data(gsbi_id, p_tx_obj, stop_seq);
 			break;
-		}
-
 		default:
 			ret = QUP_ERR_UNSUPPORTED;
 		}
 	}
+
+	return ret;
+}
+
+static qup_return_t qup_i2c_recv_data(gsbi_id_t gsbi_id, qup_data_t *p_rx_obj)
+{
+	qup_return_t ret = QUP_ERR_UNDEFINED;
+	uint8_t mode = (readl(QUP_ADDR(gsbi_id, QUP_IO_MODES)) >>
+			QUP_INPUT_MODE_SHFT) & QUP_MODE_MASK;
+
+	ret = qup_i2c_read(gsbi_id, mode, p_rx_obj);
+	if (0) {
+		int i;
+		printk(BIOS_DEBUG, "i2c rxed on bus %d device %2.2x:",
+		       gsbi_id, p_rx_obj->p.iic.addr);
+		for (i = 0; i < p_rx_obj->p.iic.data_len; i++)
+			printk(BIOS_DEBUG, " %2.2x", p_rx_obj->p.iic.data[i]);
+		printk(BIOS_DEBUG, "\n");
+	}
+
 	return ret;
 }
 
 qup_return_t qup_recv_data(gsbi_id_t gsbi_id, qup_data_t *p_rx_obj)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
+
 	if (p_rx_obj->protocol == ((readl(QUP_ADDR(gsbi_id, QUP_CONFIG)) >>
-						QUP_MINI_CORE_PROTO_SHFT) &
-						QUP_MINI_CORE_PROTO_MASK)) {
+			QUP_MINI_CORE_PROTO_SHFT) & QUP_MINI_CORE_PROTO_MASK)) {
 		switch (p_rx_obj->protocol) {
-		case QUP_MINICORE_I2C_MASTER: {
-			uint8_t mode = (readl(QUP_ADDR(gsbi_id,
-						       QUP_IO_MODES)) >>
-						QUP_INPUT_MODE_SHFT) &
-						QUP_MODE_MASK;
-			ret = qup_i2c_read(gsbi_id, mode, p_rx_obj);
-			if (0) {
-				int i;
-				printf("i2c rxed on bus %d device %2.2x:",
-					gsbi_id, p_rx_obj->p.iic.addr);
-				for (i = 0; i < p_rx_obj->p.iic.data_len; i++)
-					printf(" %2.2x",
-						p_rx_obj->p.iic.data[i]);
-				printf("\n");
-			}
+		case QUP_MINICORE_I2C_MASTER:
+			ret = qup_i2c_recv_data(gsbi_id, p_rx_obj);
 			break;
-		}
 		default:
 			ret = QUP_ERR_UNSUPPORTED;
 		}
 	}
+
 	return ret;
 }