soc/qualcomm/ipq40xx: Add support for BLSP QUP I2C

Able to talk to the TPM device and the commands
seem to succeed.

BUG=chrome-os-partner:49249 chrome-os-partner:49250
TEST=All commands to the TPM succeed
BRANCH=none

Original-Commit-Id: c13900108f524c8422c38dee88469c8bfe24d0bd
Original-Change-Id: Ie8c3c1ab1290cd8d7e6ddd1ae22f765c7be81019
Original-Signed-off-by: Varadarajan Narayanan <varada@codeaurora.org>
Original-Reviewed-on: https://chromium-review.googlesource.com/333314
Original-Commit-Ready: David Hendricks <dhendrix@chromium.org>
Original-Tested-by: David Hendricks <dhendrix@chromium.org>
Original-Reviewed-by: David Hendricks <dhendrix@chromium.org>

squashed:

soc/qualcomm/ipq40xx: Add support for BLSP QUP SPI

- Enable BLSP SPI driver for ipq40xx
- supports only FIFO mode

BUG=chrome-os-partner:49249
TEST=None. Initial code not sure if it will even compile
BRANCH=none

Original-Commit-Id: 0714025975854dd048d35fe602824ead4c7d94e9
Original-Change-Id: If809b0fdf7d6c9405db6fd3747a3774c00ea9870
Original-Signed-off-by: Varadarajan Narayanan <varada@codeaurora.org>
Original-Reviewed-on: https://chromium-review.googlesource.com/333303
Original-Commit-Ready: David Hendricks <dhendrix@chromium.org>
Original-Tested-by: David Hendricks <dhendrix@chromium.org>
Original-Reviewed-by: David Hendricks <dhendrix@chromium.org>

Change-Id: Ia518af5bfc782b08a0883ac93224d476d07e2426
Signed-off-by: Patrick Georgi <pgeorgi@chromium.org>
Reviewed-on: https://review.coreboot.org/14677
Tested-by: build bot (Jenkins)
Reviewed-by: Stefan Reinauer <stefan.reinauer@coreboot.org>

19 files changed, 1076 insertions, 1205 deletions

diff --git a/src/mainboard/google/gale/Kconfig b/src/mainboard/google/gale/Kconfig
index 6888e41ce5..b839fb45a1 100644
--- a/src/mainboard/google/gale/Kconfig
+++ b/src/mainboard/google/gale/Kconfig
@@ -56,7 +56,7 @@ config DRAM_SIZE_MB
 
 config DRIVER_TPM_I2C_BUS
 	hex
-	default 0x1
+	default 0x2
 
 config DRIVER_TPM_I2C_ADDR
 	hex
diff --git a/src/mainboard/google/gale/Makefile.inc b/src/mainboard/google/gale/Makefile.inc
index 491b5b5075..7ce1e0d38d 100644
--- a/src/mainboard/google/gale/Makefile.inc
+++ b/src/mainboard/google/gale/Makefile.inc
@@ -21,7 +21,7 @@ bootblock-y += reset.c
 verstage-y += boardid.c
 verstage-y += cdp.c
 verstage-y += chromeos.c
-verstage-y += gsbi.c
+verstage-y += blsp.c
 verstage-y += memlayout.ld
 verstage-y += reset.c
 
diff --git a/src/mainboard/google/gale/gsbi.c b/src/mainboard/google/gale/blsp.c
index 915f317df9..3be075f141 100644
--- a/src/mainboard/google/gale/gsbi.c
+++ b/src/mainboard/google/gale/blsp.c
@@ -27,34 +27,47 @@
  * SUCH DAMAGE.
  */
 
+#include <gpio.h>
 #include <soc/gpio.h>
-#include <soc/gsbi.h>
+#include <soc/blsp.h>
 #include <soc/qup.h>
 
-#define GPIO_FUNC_I2C		0x1
+#define IPQ40XX_I2C0_PINGROUP_1		1
+#define IPQ40XX_I2C0_PINGROUP_2		(!IPQ40XX_I2C0_PINGROUP_1)
 
-int gsbi_init_board(gsbi_id_t gsbi_id)
+#if IPQ40XX_I2C0_PINGROUP_1
+
+#define SCL_GPIO		20
+#define SDA_GPIO		21
+#define GPIO_FUNC_SCL		0x1
+#define GPIO_FUNC_SDA		0x1
+
+#elif IPQ40XX_I2C0_PINGROUP_2
+
+#define SCL_GPIO		58
+#define SDA_GPIO		59
+#define GPIO_FUNC_SCL		0x3
+#define GPIO_FUNC_SDA		0x2
+
+#else
+
+#warning "TPM: I2C pingroup not specified"
+
+#endif
+
+int blsp_i2c_init_board(blsp_qup_id_t id)
 {
-	switch (gsbi_id) {
-	case GSBI_ID_7:
-			gpio_tlmm_config_set(8, GPIO_FUNC_I2C,
-					     GPIO_NO_PULL, GPIO_2MA, 1);
-			gpio_tlmm_config_set(9, GPIO_FUNC_I2C,
-					     GPIO_NO_PULL, GPIO_2MA, 1);
-		break;
-	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);
+	switch (id) {
+	case BLSP_QUP_ID_0:
+	case BLSP_QUP_ID_1:
+	case BLSP_QUP_ID_2:
+	case BLSP_QUP_ID_3:
+#if defined(IPQ40XX_I2C0_PINGROUP_1) || defined(IPQ40XX_I2C0_PINGROUP_2)
+		gpio_tlmm_config_set(SDA_GPIO, GPIO_FUNC_SDA,
+				     GPIO_NO_PULL, GPIO_2MA, 1);
+		gpio_tlmm_config_set(SCL_GPIO, GPIO_FUNC_SCL,
+				     GPIO_NO_PULL, GPIO_2MA, 1);
+#endif /* Pin Group 1 or 2 */
 		break;
 	default:
 		return 1;
diff --git a/src/mainboard/google/gale/chromeos.c b/src/mainboard/google/gale/chromeos.c
index c4af04853c..7430407c89 100644
--- a/src/mainboard/google/gale/chromeos.c
+++ b/src/mainboard/google/gale/chromeos.c
@@ -20,7 +20,7 @@
 #include <drivers/i2c/ww_ring/ww_ring.h>
 #include <gpio.h>
 #include <soc/cdp.h>
-#include <soc/gsbi.h>
+#include <soc/blsp.h>
 #include <string.h>
 #include <timer.h>
 #include <vendorcode/google/chromeos/chromeos.h>
diff --git a/src/mainboard/google/gale/mainboard.c b/src/mainboard/google/gale/mainboard.c
index a8117af1e3..e1a23c937f 100644
--- a/src/mainboard/google/gale/mainboard.c
+++ b/src/mainboard/google/gale/mainboard.c
@@ -22,6 +22,7 @@
 #include <soc/clock.h>
 #include <soc/soc_services.h>
 #include <soc/usb.h>
+#include <soc/blsp.h>
 #include <symbols.h>
 
 #include <vendorcode/google/chromeos/chromeos.h>
@@ -37,7 +38,7 @@ static void setup_usb(void)
 }
 
 #define TPM_RESET_GPIO		19
-static void ipq_setup_tpm(void)
+void ipq_setup_tpm(void)
 {
 #ifdef CONFIG_I2C_TPM
 	gpio_tlmm_config_set(TPM_RESET_GPIO, FUNC_SEL_GPIO,
diff --git a/src/soc/qualcomm/ipq40xx/Makefile.inc b/src/soc/qualcomm/ipq40xx/Makefile.inc
index 9cf96cfedb..36444b28f0 100644
--- a/src/soc/qualcomm/ipq40xx/Makefile.inc
+++ b/src/soc/qualcomm/ipq40xx/Makefile.inc
@@ -24,7 +24,7 @@ bootblock-$(CONFIG_DRIVERS_UART) += uart.c
 
 verstage-y += clock.c
 verstage-y += gpio.c
-libverstage-y += gsbi.c
+libverstage-y += blsp.c
 libverstage-y += i2c.c
 libverstage-y += qup.c
 libverstage-y += spi.c
diff --git a/src/soc/qualcomm/ipq40xx/blsp.c b/src/soc/qualcomm/ipq40xx/blsp.c
new file mode 100644
index 0000000000..c9d08d584a
--- /dev/null
+++ b/src/soc/qualcomm/ipq40xx/blsp.c
@@ -0,0 +1,52 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright (C) 2014 - 2016 The Linux Foundation. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above
+ *       copyright notice, this list of conditions and the following
+ *       disclaimer in the documentation and/or other materials provided
+ *       with the distribution.
+ *     * Neither the name of The Linux Foundation nor the names of its
+ *       contributors may be used to endorse or promote products derived
+ *       from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
+ * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
+ * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
+ * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
+ * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
+ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <soc/blsp.h>
+#include <soc/clock.h>
+
+blsp_return_t blsp_i2c_init(blsp_qup_id_t id)
+{
+	void *base = blsp_qup_base(id);
+
+	if (!base)
+		return BLSP_ID_ERROR;
+
+	if (blsp_i2c_clock_config(id) != 0)
+		return BLSP_ID_ERROR;
+
+	if (blsp_i2c_init_board(id))
+		return BLSP_UNSUPPORTED;
+
+	/* Configure Mini core to I2C core */
+	clrsetbits_le32(base, BLSP_MINI_CORE_MASK, BLSP_MINI_CORE_I2C);
+
+	return BLSP_SUCCESS;
+}
diff --git a/src/soc/qualcomm/ipq40xx/clock.c b/src/soc/qualcomm/ipq40xx/clock.c
index fab0c9e07a..864f2b603b 100644
--- a/src/soc/qualcomm/ipq40xx/clock.c
+++ b/src/soc/qualcomm/ipq40xx/clock.c
@@ -28,8 +28,10 @@
  */
 
 #include <delay.h>
+#include <soc/blsp.h>
 #include <soc/clock.h>
 #include <types.h>
+#include <console/console.h>
 
 #define CLOCK_UPDATE_DELAY		1000
 
@@ -67,6 +69,7 @@ void uart_clock_config(unsigned int blsp_uart, unsigned int m,
 		udelay(1);
 	}
 
+	/* Please refer to the comments in blsp_i2c_clock_config() */
 	setbits_le32(GCC_CLK_BRANCH_ENA, BLSP1_AHB | BLSP1_SLEEP);
 }
 
@@ -110,3 +113,67 @@ void usb_clock_config(void)
 	udelay(5);
 	write32(USB30_RESET, 0);	/* deassert all USB resets again */
 }
+
+int blsp_i2c_clock_config(blsp_qup_id_t id)
+{
+	int i;
+	const int max_tries = 200;
+	struct { void *cbcr, *cmd, *cfg; } clk[] = {
+		{
+			GCC_BLSP1_QUP1_I2C_APPS_CBCR,
+			GCC_BLSP1_QUP1_I2C_APPS_CMD_RCGR,
+			GCC_BLSP1_QUP1_I2C_APPS_CFG_RCGR,
+		},
+		{
+			GCC_BLSP1_QUP1_I2C_APPS_CBCR,
+			GCC_BLSP1_QUP1_I2C_APPS_CMD_RCGR,
+			GCC_BLSP1_QUP1_I2C_APPS_CFG_RCGR,
+		},
+		{
+			GCC_BLSP1_QUP1_I2C_APPS_CBCR,
+			GCC_BLSP1_QUP1_I2C_APPS_CMD_RCGR,
+			GCC_BLSP1_QUP1_I2C_APPS_CFG_RCGR,
+		},
+		{
+			GCC_BLSP1_QUP1_I2C_APPS_CBCR,
+			GCC_BLSP1_QUP1_I2C_APPS_CMD_RCGR,
+			GCC_BLSP1_QUP1_I2C_APPS_CFG_RCGR,
+		},
+	};
+
+	/*
+	 * uart_clock_config() does this. Ideally, setting these bits once
+	 * should suffice. However, if for some reason the order of invocation
+	 * of uart_clock_config and blsp_i2c_clock_config gets changed or
+	 * something, then one of the functions might not work. Hence, to steer
+	 * clear of such dependencies, just replicating the setting of this
+	 * bits.
+	 *
+	 * Moreover, they are read-modify-write and HW wise repeated setting of
+	 * the same bits is harmless. Hence repeating them here should be ok.
+	 * This will ensure root and branch clocks remain on.
+	 */
+	setbits_le32(GCC_CLK_BRANCH_ENA, BLSP1_AHB | BLSP1_SLEEP);
+
+	/* Src Sel 1 (fepll 200), Src Div 10.5 */
+	write32(clk[id].cfg, (1u << 8) | (20u << 0));
+
+	write32(clk[id].cmd, BIT(0)); /* Update En */
+
+	for (i = 0; i < max_tries; i++) {
+		if (read32(clk[id].cmd) & BIT(0)) {
+			udelay(5);
+			continue;
+		}
+		break;
+	}
+
+	if (i == max_tries) {
+		printk(BIOS_ERR, "%s failed\n", __func__);
+		return -ETIMEDOUT;
+	}
+
+	write32(clk[id].cbcr, BIT(0));	/* Enable */
+
+	return 0;
+}
diff --git a/src/soc/qualcomm/ipq40xx/gsbi.c b/src/soc/qualcomm/ipq40xx/gsbi.c
deleted file mode 100644
index 98c4edd084..0000000000
--- a/src/soc/qualcomm/ipq40xx/gsbi.c
+++ /dev/null
@@ -1,109 +0,0 @@
-/*
- * This file is part of the coreboot project.
- *
- * Copyright (C) 2014 - 2015 The Linux Foundation. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are
- * met:
- *     * Redistributions of source code must retain the above copyright
- *       notice, this list of conditions and the following disclaimer.
- *     * Redistributions in binary form must reproduce the above
- *       copyright notice, this list of conditions and the following
- *       disclaimer in the documentation and/or other materials provided
- *       with the distribution.
- *     * Neither the name of The Linux Foundation nor the names of its
- *       contributors may be used to endorse or promote products derived
- *       from this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
- * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
- * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
- * ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
- * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
- * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
- * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
- * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
- * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- */
-
-#include <arch/io.h>
-#include <soc/iomap.h>
-#include <soc/gsbi.h>
-#include <console/console.h>
-
-static inline void *gsbi_ctl_reg_addr(gsbi_id_t gsbi_id)
-{
-	switch (gsbi_id) {
-	case GSBI_ID_1:
-		return GSBI1_CTL_REG;
-	case GSBI_ID_2:
-		return GSBI2_CTL_REG;
-	case GSBI_ID_3:
-		return GSBI3_CTL_REG;
-	case GSBI_ID_4:
-		return GSBI4_CTL_REG;
-	case GSBI_ID_5:
-		return GSBI5_CTL_REG;
-	case GSBI_ID_6:
-		return GSBI6_CTL_REG;
-	case GSBI_ID_7:
-		return GSBI7_CTL_REG;
-	default:
-		printk(BIOS_ERR, "Unsupported GSBI%d\n", gsbi_id);
-		return 0;
-	}
-}
-
-gsbi_return_t gsbi_init(gsbi_id_t gsbi_id, gsbi_protocol_t protocol)
-{
-	unsigned reg_val;
-	unsigned m = 1;
-	unsigned n = 4;
-	unsigned pre_div = 4;
-	unsigned src = 3;
-	unsigned mnctr_mode = 2;
-	void *gsbi_ctl = gsbi_ctl_reg_addr(gsbi_id);
-
-	if (!gsbi_ctl)
-		return GSBI_ID_ERROR;
-
-	write32(GSBI_HCLK_CTL(gsbi_id),
-		(1 << GSBI_HCLK_CTL_GATE_ENA) |
-		(1 << GSBI_HCLK_CTL_BRANCH_ENA));
-
-	if (gsbi_init_board(gsbi_id))
-		return GSBI_UNSUPPORTED;
-
-	write32(GSBI_QUP_APSS_NS_REG(gsbi_id), 0);
-	write32(GSBI_QUP_APSS_MD_REG(gsbi_id), 0);
-
-	reg_val = ((m & GSBI_QUP_APPS_M_MASK) << GSBI_QUP_APPS_M_SHFT) |
-		  ((~n & GSBI_QUP_APPS_D_MASK) << GSBI_QUP_APPS_D_SHFT);
-	write32(GSBI_QUP_APSS_MD_REG(gsbi_id), reg_val);
-
-	reg_val = (((~(n - m)) & GSBI_QUP_APPS_N_MASK) <<
-					GSBI_QUP_APPS_N_SHFT) |
-		  ((mnctr_mode & GSBI_QUP_APPS_MNCTR_MODE_MSK) <<
-				 GSBI_QUP_APPS_MNCTR_MODE_SFT) |
-		  (((pre_div - 1) & GSBI_QUP_APPS_PRE_DIV_MSK) <<
-				 GSBI_QUP_APPS_PRE_DIV_SFT) |
-		  (src & GSBI_QUP_APPS_SRC_SEL_MSK);
-	write32(GSBI_QUP_APSS_NS_REG(gsbi_id), reg_val);
-
-	reg_val |= (1 << GSBI_QUP_APPS_ROOT_ENA_SFT) |
-		   (1 << GSBI_QUP_APPS_MNCTR_EN_SFT);
-	write32(GSBI_QUP_APSS_NS_REG(gsbi_id), reg_val);
-
-	reg_val |= (1 << GSBI_QUP_APPS_BRANCH_ENA_SFT);
-	write32(GSBI_QUP_APSS_NS_REG(gsbi_id), reg_val);
-
-	/*Select i2c protocol*/
-	write32(gsbi_ctl,
-		((GSBI_CTL_PROTO_I2C & GSBI_CTL_PROTO_CODE_MSK)
-			<< GSBI_CTL_PROTO_CODE_SFT));
-
-	return GSBI_SUCCESS;
-}
diff --git a/src/soc/qualcomm/ipq40xx/i2c.c b/src/soc/qualcomm/ipq40xx/i2c.c
index f84b8e25c1..a2634acdf2 100644
--- a/src/soc/qualcomm/ipq40xx/i2c.c
+++ b/src/soc/qualcomm/ipq40xx/i2c.c
@@ -36,29 +36,38 @@
 #include <device/i2c.h>
 #include <stdlib.h>
 #include <string.h>
-#include <soc/gsbi.h>
+#include <soc/blsp.h>
 #include <soc/qup.h>
+#include <soc/gpio.h>
 
-static qup_config_t gsbi1_qup_config = {
+static qup_config_t blsp1_qup0_config = {
 	QUP_MINICORE_I2C_MASTER,
 	100000,
-	24000000,
+	19050000,
 	QUP_MODE_FIFO,
 	0
 };
 
-static qup_config_t gsbi4_qup_config = {
+static qup_config_t blsp1_qup1_config = {
 	QUP_MINICORE_I2C_MASTER,
 	100000,
-	24000000,
+	19050000,
 	QUP_MODE_FIFO,
 	0
 };
 
-static qup_config_t gsbi7_qup_config = {
+static qup_config_t blsp1_qup2_config = {
 	QUP_MINICORE_I2C_MASTER,
 	100000,
-	24000000,
+	19050000,
+	QUP_MODE_FIFO,
+	0
+};
+
+static qup_config_t blsp1_qup3_config = {
+	QUP_MINICORE_I2C_MASTER,
+	100000,
+	19050000,
 	QUP_MODE_FIFO,
 	0
 };
@@ -101,41 +110,43 @@ static int i2c_write(uint32_t gsbi_id, uint8_t slave,
 		return 0;
 }
 
-static int i2c_init(unsigned bus)
+static int i2c_init(blsp_qup_id_t id)
 {
-	unsigned gsbi_id = bus;
 	qup_config_t *qup_config;
 
-	switch (gsbi_id) {
-	case GSBI_ID_1:
-		qup_config = &gsbi1_qup_config;
+	switch (id) {
+	case BLSP_QUP_ID_0:
+		qup_config = &blsp1_qup0_config;
+		break;
+	case BLSP_QUP_ID_1:
+		qup_config = &blsp1_qup1_config;
 		break;
-	case GSBI_ID_4:
-		qup_config = &gsbi4_qup_config;
+	case BLSP_QUP_ID_2:
+		qup_config = &blsp1_qup2_config;
 		break;
-	case GSBI_ID_7:
-		qup_config = &gsbi7_qup_config;
+	case BLSP_QUP_ID_3:
+		qup_config = &blsp1_qup3_config;
 		break;
 	default:
-		printk(BIOS_ERR, "QUP configuration not defind for GSBI%d.\n",
-		       gsbi_id);
+		printk(BIOS_ERR, "QUP configuration not defined for BLSP%d.\n",
+		       id);
 		return 1;
 	}
 
 	if (qup_config->initialized)
 		return 0;
 
-	if (gsbi_init(gsbi_id, GSBI_PROTO_I2C_ONLY)) {
-		printk(BIOS_ERR, "failed to initialize gsbi\n");
+	if (blsp_i2c_init(id)) {
+		printk(BIOS_ERR, "failed to initialize blsp\n");
 		return 1;
 	}
 
-	if (qup_init(gsbi_id, qup_config)) {
+	if (qup_init(id, qup_config)) {
 		printk(BIOS_ERR, "failed to initialize qup\n");
 		return 1;
 	}
 
-	if (qup_reset_i2c_master_status(gsbi_id)) {
+	if (qup_reset_i2c_master_status(id)) {
 		printk(BIOS_ERR, "failed to reset i2c master status\n");
 		return 1;
 	}
diff --git a/src/soc/qualcomm/ipq40xx/include/soc/gsbi.h b/src/soc/qualcomm/ipq40xx/include/soc/blsp.h
index f2c375df0f..ce74e56dcc 100644
--- a/src/soc/qualcomm/ipq40xx/include/soc/gsbi.h
+++ b/src/soc/qualcomm/ipq40xx/include/soc/blsp.h
@@ -28,48 +28,33 @@
  */
 
 
-#ifndef __GSBI_H_
-#define __GSBI_H_
-
-/* GSBI Registers */
-#define GSBI_CTRL_REG(base)        ((base) + 0x0)
-
-#define GSBI_CTRL_REG_PROTOCOL_CODE_S   4
-#define GSBI_PROTOCOL_CODE_I2C          0x2
-#define GSBI_PROTOCOL_CODE_SPI          0x3
-#define GSBI_PROTOCOL_CODE_UART_FLOW    0x4
-#define GSBI_PROTOCOL_CODE_I2C_UART     0x6
-
-#define GSBI_HCLK_CTL_S                 4
-#define GSBI_HCLK_CTL_CLK_ENA           0x1
+#ifndef __BLSP_H_
+#define __BLSP_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;
+	BLSP_QUP_ID_0,
+	BLSP_QUP_ID_1,
+	BLSP_QUP_ID_2,
+	BLSP_QUP_ID_3,
+} blsp_qup_id_t;
 
 typedef enum {
-	GSBI_SUCCESS = 0,
-	GSBI_ID_ERROR,
-	GSBI_ERROR,
-	GSBI_UNSUPPORTED
-} gsbi_return_t;
+	BLSP_SUCCESS = 0,
+	BLSP_ID_ERROR,
+	BLSP_ERROR,
+	BLSP_UNSUPPORTED
+} blsp_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);
+	BLSP_PROTO_I2C_UIM = 1,
+	BLSP_PROTO_I2C_ONLY,
+	BLSP_PROTO_SPI_ONLY,
+	BLSP_PROTO_UART_FLOW_CTL,
+	BLSP_PROTO_UIM,
+	BLSP_PROTO_I2C_UART,
+} blsp_protocol_t;
+
+blsp_return_t blsp_i2c_init(blsp_qup_id_t id);
+int blsp_i2c_init_board(blsp_qup_id_t id);
 
 #endif
diff --git a/src/soc/qualcomm/ipq40xx/include/soc/clock.h b/src/soc/qualcomm/ipq40xx/include/soc/clock.h
index 89343f0168..8544c4382d 100644
--- a/src/soc/qualcomm/ipq40xx/include/soc/clock.h
+++ b/src/soc/qualcomm/ipq40xx/include/soc/clock.h
@@ -210,5 +210,7 @@ void uart_clock_config(unsigned int blsp_uart, unsigned int m, unsigned int n,
 void nand_clock_config(void);
 void usb_clock_config(void);
 int audio_clock_config(unsigned frequency);
+int blsp_i2c_clock_config(blsp_qup_id_t id);
+
 
 #endif  /*  __PLATFORM_IPQ40XX_CLOCK_H_ */
diff --git a/src/soc/qualcomm/ipq40xx/include/soc/gpio.h b/src/soc/qualcomm/ipq40xx/include/soc/gpio.h
index 56aef77829..d4ff32697f 100644
--- a/src/soc/qualcomm/ipq40xx/include/soc/gpio.h
+++ b/src/soc/qualcomm/ipq40xx/include/soc/gpio.h
@@ -113,4 +113,7 @@ static inline void gpio_tlmm_config(unsigned int gpio, unsigned int func,
 {
 	gpio_tlmm_config_set(gpio, func, pull, drvstr, enable);
 }
+
+void ipq_setup_tpm(void);
+
 #endif // __SOC_QUALCOMM_IPQ40XX_GPIO_H_
diff --git a/src/soc/qualcomm/ipq40xx/include/soc/iomap.h b/src/soc/qualcomm/ipq40xx/include/soc/iomap.h
index 0f3c6a5cfa..bfdfb56f8e 100644
--- a/src/soc/qualcomm/ipq40xx/include/soc/iomap.h
+++ b/src/soc/qualcomm/ipq40xx/include/soc/iomap.h
@@ -38,6 +38,7 @@
 
 #include <arch/io.h>
 #include <soc/cdp.h>
+#include <soc/blsp.h>
 
 /* Typecast to allow integers being passed as address
    This needs to be included because vendor code is not compliant with our
@@ -71,6 +72,13 @@
 #define		CRYPTO_AXI		(1 << 1)
 #define		CRYPTO_AHB		(1 << 0)
 
+#define GCC_BLSP1_QUP1_I2C_APPS_CBCR		(MSM_CLK_CTL_BASE + 0x2008)
+#define GCC_BLSP1_QUP1_I2C_APPS_CMD_RCGR	(MSM_CLK_CTL_BASE + 0x200c)
+#define GCC_BLSP1_QUP1_I2C_APPS_CFG_RCGR	(MSM_CLK_CTL_BASE + 0x2010)
+#define GCC_BLSP1_QUP2_I2C_APPS_CBCR		(MSM_CLK_CTL_BASE + 0x3010)
+#define GCC_BLSP1_QUP2_I2C_APPS_CMD_RCGR	(MSM_CLK_CTL_BASE + 0x3000)
+#define GCC_BLSP1_QUP2_I2C_APPS_CFG_RCGR	(MSM_CLK_CTL_BASE + 0x3004)
+
 #define GCNT_GLOBAL_CTRL_BASE	((void *)0x004a0000u)
 #define GCNT_CNTCR		(GCNT_GLOBAL_CTRL_BASE + 0x1000)
 #define GCNT_GLB_CNTCV_LO	(GCNT_GLOBAL_CTRL_BASE + 0x1008)
@@ -117,55 +125,28 @@ enum {
 #define GCC_BLSP1_UART_APPS_D(x)	(GCC_BLSP1_UART_APPS_N(x) + 4)
 #define GCC_BLSP1_UART_MISC(x)		(GCC_BLSP1_UART_APPS_D(x) + 4)
 
-#define GSBI1_BASE		((void *)0x12440000)
-#define GSBI2_BASE		((void *)0x12480000)
-#define GSBI3_BASE		((void *)0x16200000)
-#define GSBI4_BASE		((void *)0x16300000)
-#define GSBI5_BASE		((void *)0x1A200000)
-#define GSBI6_BASE		((void *)0x16500000)
-#define GSBI7_BASE		((void *)0x16600000)
-
-#define GSBI1_CTL_REG		(GSBI1_BASE + (0x0))
-#define GSBI2_CTL_REG		(GSBI2_BASE + (0x0))
-#define GSBI3_CTL_REG		(GSBI3_BASE + (0x0))
-#define GSBI4_CTL_REG		(GSBI4_BASE + (0x0))
-#define GSBI5_CTL_REG		(GSBI5_BASE + (0x0))
-#define GSBI6_CTL_REG		(GSBI6_BASE + (0x0))
-#define GSBI7_CTL_REG		(GSBI7_BASE + (0x0))
-
-#define GSBI_QUP1_BASE		(GSBI1_BASE + 0x20000)
-#define GSBI_QUP2_BASE		(GSBI2_BASE + 0x20000)
-#define GSBI_QUP3_BASE		(GSBI3_BASE + 0x80000)
-#define GSBI_QUP4_BASE		(GSBI4_BASE + 0x80000)
-#define GSBI_QUP5_BASE		(GSBI5_BASE + 0x80000)
-#define GSBI_QUP6_BASE		(GSBI6_BASE + 0x80000)
-#define GSBI_QUP7_BASE		(GSBI7_BASE + 0x80000)
-
-#define GSBI_CTL_PROTO_I2C              2
-#define GSBI_CTL_PROTO_CODE_SFT         4
-#define GSBI_CTL_PROTO_CODE_MSK         0x7
-#define GSBI_HCLK_CTL_GATE_ENA          6
-#define GSBI_HCLK_CTL_BRANCH_ENA        4
-#define GSBI_QUP_APPS_M_SHFT            16
-#define GSBI_QUP_APPS_M_MASK            0xFF
-#define GSBI_QUP_APPS_D_SHFT            0
-#define GSBI_QUP_APPS_D_MASK            0xFF
-#define GSBI_QUP_APPS_N_SHFT            16
-#define GSBI_QUP_APPS_N_MASK            0xFF
-#define GSBI_QUP_APPS_ROOT_ENA_SFT      11
-#define GSBI_QUP_APPS_BRANCH_ENA_SFT    9
-#define GSBI_QUP_APPS_MNCTR_EN_SFT      8
-#define GSBI_QUP_APPS_MNCTR_MODE_MSK    0x3
-#define GSBI_QUP_APPS_MNCTR_MODE_SFT    5
-#define GSBI_QUP_APPS_PRE_DIV_MSK       0x3
-#define GSBI_QUP_APPS_PRE_DIV_SFT       3
-#define GSBI_QUP_APPS_SRC_SEL_MSK       0x7
-
-
-#define GSBI_QUP_APSS_MD_REG(gsbi_n)	((MSM_CLK_CTL_BASE + 0x29c8) + \
-							(32*(gsbi_n-1)))
-#define GSBI_QUP_APSS_NS_REG(gsbi_n)	((MSM_CLK_CTL_BASE + 0x29cc) + \
-							(32*(gsbi_n-1)))
-#define GSBI_HCLK_CTL(n)		((MSM_CLK_CTL_BASE + 0x29C0) + \
-							(32*(n-1)))
+#define BLSP1_QUP0_BASE		((void *)0x078B5000)
+#define BLSP1_QUP1_BASE		((void *)0x078B6000)
+#define BLSP1_QUP2_BASE		((void *)0x078B7000)
+#define BLSP1_QUP3_BASE		((void *)0x078B8000)
+
+static inline void *blsp_qup_base(blsp_qup_id_t id)
+{
+	switch (id) {
+	case BLSP_QUP_ID_0: return BLSP1_QUP0_BASE;
+	case BLSP_QUP_ID_1: return BLSP1_QUP1_BASE;
+	case BLSP_QUP_ID_2: return BLSP1_QUP2_BASE;
+	case BLSP_QUP_ID_3: return BLSP1_QUP3_BASE;
+	}
+	return NULL;
+}
+
+#define BLSP_MINI_CORE_SHIFT		8
+#define BLSP_MINI_CORE_I2C		(0x2u << BLSP_MINI_CORE_SHIFT)
+#define BLSP_MINI_CORE_MASK		(0xfu << BLSP_MINI_CORE_SHIFT)
+
+#define ETIMEDOUT -10
+#define EINVAL -11
+#define EIO -12
+
 #endif // __SOC_QUALCOMM_IPQ40XX_IOMAP_H_
diff --git a/src/soc/qualcomm/ipq40xx/include/soc/qup.h b/src/soc/qualcomm/ipq40xx/include/soc/qup.h
index d0186f59b0..e55989a591 100644
--- a/src/soc/qualcomm/ipq40xx/include/soc/qup.h
+++ b/src/soc/qualcomm/ipq40xx/include/soc/qup.h
@@ -32,40 +32,50 @@
 #ifndef __QUP_H__
 #define __QUP_H__
 
-#include <soc/gsbi.h>
+#include <soc/blsp.h>
 
 /* QUP block registers */
-#define QUP_CONFIG			0x0
-#define QUP_STATE			0x4
-#define QUP_IO_MODES			0x8
-#define QUP_SW_RESET			0xc
-#define QUP_TIME_OUT			0x10
-#define QUP_TIME_OUT_CURRENT		0x14
-#define QUP_OPERATIONAL			0x18
-#define QUP_ERROR_FLAGS			0x1c
-#define QUP_ERROR_FLAGS_EN		0x20
-#define QUP_TEST_CTRL			0x24
+#define QUP_CONFIG			0x000
+#define QUP_STATE			0x004
+#define QUP_IO_MODES			0x008
+#define QUP_SW_RESET			0x00C
+#define QUP_TRANSFER_CANCEL		0x014
+#define QUP_OPERATIONAL			0x018
+#define QUP_ERROR_FLAGS			0x01C
+#define QUP_ERROR_FLAGS_EN		0x020
+#define QUP_TEST_CTRL			0x024
+#define QUP_OPERATIONAL_MASK		0x028
+#define QUP_HW_VERSION			0x030
 #define QUP_MX_OUTPUT_COUNT		0x100
 #define QUP_MX_OUTPUT_CNT_CURRENT	0x104
 #define QUP_OUTPUT_DEBUG		0x108
-#define QUP_OUTPUT_FIFO_WORD_CNT	0x10c
+#define QUP_OUTPUT_FIFO_WORD_CNT	0x10C
 #define QUP_OUTPUT_FIFO			0x110
+#define QUP_OUTPUT_FIFO_SIZE		64	/* bytes */
 #define QUP_MX_WRITE_COUNT		0x150
-#define QUP_WRITE_CNT_CURRENT		0x154
+#define QUP_MX_WRITE_CNT_CURRENT	0x154
 #define QUP_MX_INPUT_COUNT		0x200
-#define QUP_READ_COUNT			0x208
-#define QUP_MX_READ_CNT_CURRENT		0x20c
+#define QUP_MX_INPUT_CNT_CURRENT	0x204
+#define QUP_MX_READ_COUNT		0x208
+#define QUP_MX_READ_CNT_CURRENT		0x20C
 #define QUP_INPUT_DEBUG			0x210
 #define QUP_INPUT_FIFO_WORD_CNT		0x214
 #define QUP_INPUT_FIFO			0x218
+#define QUP_INPUT_FIFO_SIZE		64	/* bytes */
 #define QUP_I2C_MASTER_CLK_CTL		0x400
 #define QUP_I2C_MASTER_STATUS		0x404
+#define QUP_I2C_MASTER_CONFIG		0x408
+#define QUP_I2C_MASTER_BUS_CLEAR	0x40C
+#define QUP_I2C_MASTER_LOCAL_ID		0x410
+#define QUP_I2C_MASTER_COMMAND		0x414
 
 #define OUTPUT_FIFO_FULL		(1<<6)
 #define INPUT_FIFO_NOT_EMPTY		(1<<5)
 #define OUTPUT_FIFO_NOT_EMPTY		(1<<4)
 #define INPUT_SERVICE_FLAG		(1<<9)
 #define OUTPUT_SERVICE_FLAG		(1<<8)
+#define QUP_UNPACK_EN			(1<<14)
+#define QUP_PACK_EN			(1<<15)
 #define QUP_OUTPUT_BIT_SHIFT_EN		(1<<16)
 
 #define QUP_MODE_MASK			(0x03)
@@ -74,6 +84,8 @@
 
 #define QUP_FS_DIVIDER_MASK		(0xFF)
 
+#define QUP_APP_CLK_ON_EN		(1 << 12)
+#define QUP_CORE_CLK_ON_EN		(1 << 13)
 #define QUP_MINI_CORE_PROTO_SHFT	(8)
 #define QUP_MINI_CORE_PROTO_MASK	(0x0F)
 
@@ -170,50 +182,50 @@ typedef struct {
 } qup_data_t;
 
 /*
- * Initialize GSBI QUP block for FIFO I2C transfers.
- * gsbi_id[IN]: GSBI for which QUP is to be initialized.
+ * Initialize BLSP QUP block for FIFO I2C transfers.
+ * id[IN]: BLSP for which QUP is to be initialized.
  * config_ptr[IN]: configurations parameters for the QUP.
  *
  * return: QUP_SUCCESS, if initialization succeeds.
  */
-qup_return_t qup_init(gsbi_id_t gsbi_id, const qup_config_t *config_ptr);
+qup_return_t qup_init(blsp_qup_id_t id, const qup_config_t *config_ptr);
 
 /*
  * Set QUP state to run, pause, reset.
- * gsbi_id[IN]: GSBI block for which QUP state is to be set.
+ * id[IN]: BLSP block for which QUP state is to be set.
  * state[IN]: New state to transition to.
  *
  * return: QUP_SUCCESS, if state transition succeeds.
  */
-qup_return_t qup_set_state(gsbi_id_t gsbi_id, uint32_t state);
+qup_return_t qup_set_state(blsp_qup_id_t id, uint32_t state);
 
 /*
  * Reset the status bits set during an i2c transfer.
- * gsbi_id[IN]: GSBI block for which i2c status bits are to be cleared.
+ * id[IN]: BLSP block for which i2c status bits are to be cleared.
  *
  * return: QUP_SUCCESS, if status bits are cleared successfully.
  */
-qup_return_t qup_reset_i2c_master_status(gsbi_id_t gsbi_id);
+qup_return_t qup_reset_i2c_master_status(blsp_qup_id_t id);
 
 /*
  * Send data to the peripheral on the bus.
- * gsbi_id[IN]: GSBI block for which data is to be sent.
+ * id[IN]: BLSP block for which data is to be sent.
  * p_tx_obj[IN]: Data to be sent to the slave on the bus.
  * stop_seq[IN]: When set to non-zero QUP engine sends i2c stop sequnce.
  *
  * return: QUP_SUCCESS, when data is sent successfully to the peripheral.
  */
-qup_return_t qup_send_data(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj,
+qup_return_t qup_send_data(blsp_qup_id_t id, qup_data_t *p_tx_obj,
 			   uint8_t stop_seq);
 
 /*
  * Receive data from peripheral on the bus.
- * gsbi_id[IN]: GSBI block from which data is to be received.
+ * id[IN]: BLSP block from which data is to be received.
  * p_tx_obj[IN]: length of data to be received, slave address.
  *      [OUT]: buffer filled with data from slave.
  *
  * return: QUP_SUCCESS, when data is received successfully.
  */
-qup_return_t qup_recv_data(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj);
+qup_return_t qup_recv_data(blsp_qup_id_t id, qup_data_t *p_tx_obj);
 
 #endif //__QUP_H__
diff --git a/src/soc/qualcomm/ipq40xx/include/soc/spi.h b/src/soc/qualcomm/ipq40xx/include/soc/spi.h
index 8e52d62a01..014b333667 100644
--- a/src/soc/qualcomm/ipq40xx/include/soc/spi.h
+++ b/src/soc/qualcomm/ipq40xx/include/soc/spi.h
@@ -1,5 +1,5 @@
 /*
- * Register definitions for the IPQ GSBI Controller
+ * Register definitions for the IPQ BLSP SPI Controller
  *
  * Copyright (c) 2012 The Linux Foundation. All rights reserved.
  *
@@ -34,263 +34,152 @@
 
 #include <spi_flash.h>
 #include <soc/iomap.h>
+#include <soc/qup.h>
+
+#define BLSP0_QUP_REG_BASE		((void *)0x78b5000u)
+#define BLSP1_QUP_REG_BASE		((void *)0x78b6000u)
+
+#define BLSP0_SPI_CONFIG_REG		(BLSP0_QUP_REG_BASE + 0x00000300)
+#define BLSP1_SPI_CONFIG_REG		(BLSP1_QUP_REG_BASE + 0x00000300)
+
+#define BLSP0_SPI_IO_CONTROL_REG	(BLSP0_QUP_REG_BASE + 0x00000304)
+#define BLSP1_SPI_IO_CONTROL_REG	(BLSP1_QUP_REG_BASE + 0x00000304)
+
+#define BLSP0_SPI_ERROR_FLAGS_REG	(BLSP0_QUP_REG_BASE + 0x00000308)
+#define BLSP1_SPI_ERROR_FLAGS_REG	(BLSP1_QUP_REG_BASE + 0x00000308)
+
+#define BLSP0_SPI_DEASSERT_WAIT_REG	(BLSP0_QUP_REG_BASE + 0x00000310)
+#define BLSP1_SPI_DEASSERT_WAIT_REG	(BLSP1_QUP_REG_BASE + 0x00000310)
+#define BLSP0_SPI_ERROR_FLAGS_EN_REG	(BLSP0_QUP_REG_BASE + 0x0000030c)
+#define BLSP1_SPI_ERROR_FLAGS_EN_REG	(BLSP1_QUP_REG_BASE + 0x0000030c)
+
+#define BLSP0_QUP_CONFIG_REG		(BLSP0_QUP_REG_BASE + 0x00000000)
+#define BLSP1_QUP_CONFIG_REG		(BLSP1_QUP_REG_BASE + 0x00000000)
+
+#define BLSP0_QUP_ERROR_FLAGS_REG	(BLSP0_QUP_REG_BASE + 0x0000001c)
+#define BLSP1_QUP_ERROR_FLAGS_REG	(BLSP1_QUP_REG_BASE + 0x0000001c)
+
+#define BLSP0_QUP_ERROR_FLAGS_EN_REG	(BLSP0_QUP_REG_BASE + 0x00000020)
+#define BLSP1_QUP_ERROR_FLAGS_EN_REG	(BLSP1_QUP_REG_BASE + 0x00000020)
+
+#define BLSP0_QUP_OPERATIONAL_MASK	(BLSP0_QUP_REG_BASE + 0x00000028)
+#define BLSP1_QUP_OPERATIONAL_MASK	(BLSP1_QUP_REG_BASE + 0x00000028)
+
+#define BLSP0_QUP_OPERATIONAL_REG	(BLSP0_QUP_REG_BASE + 0x00000018)
+#define BLSP1_QUP_OPERATIONAL_REG	(BLSP1_QUP_REG_BASE + 0x00000018)
+
+#define BLSP0_QUP_IO_MODES_REG		(BLSP0_QUP_REG_BASE + 0x00000008)
+#define BLSP1_QUP_IO_MODES_REG		(BLSP1_QUP_REG_BASE + 0x00000008)
+
+#define BLSP0_QUP_STATE_REG		(BLSP0_QUP_REG_BASE + 0x00000004)
+#define BLSP1_QUP_STATE_REG		(BLSP1_QUP_REG_BASE + 0x00000004)
+
+#define BLSP0_QUP_INPUT_FIFOc_REG(c) \
+		(BLSP0_QUP_REG_BASE + 0x00000218 + 4 * (c))
+#define BLSP1_QUP_INPUT_FIFOc_REG(c) \
+		(BLSP1_QUP_REG_BASE + 0x00000218 + 4 * (c))
+
+#define BLSP0_QUP_OUTPUT_FIFOc_REG(c) \
+		(BLSP0_QUP_REG_BASE + 0x00000110 + 4 * (c))
+#define BLSP1_QUP_OUTPUT_FIFOc_REG(c) \
+		(BLSP1_QUP_REG_BASE + 0x00000110 + 4 * (c))
+
+#define BLSP0_QUP_MX_INPUT_COUNT_REG	(BLSP0_QUP_REG_BASE + 0x00000200)
+#define BLSP1_QUP_MX_INPUT_COUNT_REG	(BLSP1_QUP_REG_BASE + 0x00000200)
+
+#define BLSP0_QUP_MX_OUTPUT_COUNT_REG	(BLSP0_QUP_REG_BASE + 0x00000100)
+#define BLSP1_QUP_MX_OUTPUT_COUNT_REG	(BLSP1_QUP_REG_BASE + 0x00000100)
+
+#define BLSP0_QUP_SW_RESET_REG		(BLSP0_QUP_REG_BASE + 0x0000000c)
+#define BLSP1_QUP_SW_RESET_REG		(BLSP1_QUP_REG_BASE + 0x0000000c)
 
-#define QUP5_BASE	((uint32_t)GSBI_QUP5_BASE)
-#define QUP6_BASE	((uint32_t)GSBI_QUP6_BASE)
-#define QUP7_BASE	((uint32_t)GSBI_QUP7_BASE)
-
-#define GSBI5_QUP5_REG_BASE		(QUP5_BASE + 0x00000000)
-#define GSBI6_QUP6_REG_BASE		(QUP6_BASE + 0x00000000)
-#define GSBI7_QUP7_REG_BASE		(QUP7_BASE + 0x00000000)
-
-#define GSBI5_REG_BASE			((uint32_t)(GSBI5_BASE + 0x00000000))
-#define GSBI6_REG_BASE			((uint32_t)(GSBI6_BASE + 0x00000000))
-#define GSBI7_REG_BASE			((uint32_t)(GSBI7_BASE + 0x00000000))
-
-#define BOOT_SPI_PORT5_BASE		QUP5_BASE
-#define BOOT_SPI_PORT6_BASE		QUP6_BASE
-#define BOOT_SPI_PORT7_BASE		QUP7_BASE
-
-#define GSBI5_SPI_CONFIG_REG		(GSBI5_QUP5_REG_BASE + 0x00000300)
-#define GSBI6_SPI_CONFIG_REG		(GSBI6_QUP6_REG_BASE + 0x00000300)
-#define GSBI7_SPI_CONFIG_REG		(GSBI7_QUP7_REG_BASE + 0x00000300)
-
-#define GSBI5_SPI_IO_CONTROL_REG	(GSBI5_QUP5_REG_BASE + 0x00000304)
-#define GSBI6_SPI_IO_CONTROL_REG	(GSBI6_QUP6_REG_BASE + 0x00000304)
-#define GSBI7_SPI_IO_CONTROL_REG	(GSBI7_QUP7_REG_BASE + 0x00000304)
-
-#define GSBI5_SPI_ERROR_FLAGS_REG	(GSBI5_QUP5_REG_BASE + 0x00000308)
-#define GSBI6_SPI_ERROR_FLAGS_REG	(GSBI6_QUP6_REG_BASE + 0x00000308)
-#define GSBI7_SPI_ERROR_FLAGS_REG	(GSBI7_QUP7_REG_BASE + 0x00000308)
-
-#define GSBI5_SPI_ERROR_FLAGS_EN_REG	(GSBI5_QUP5_REG_BASE + 0x0000030c)
-#define GSBI6_SPI_ERROR_FLAGS_EN_REG	(GSBI6_QUP6_REG_BASE + 0x0000030c)
-#define GSBI7_SPI_ERROR_FLAGS_EN_REG	(GSBI7_QUP7_REG_BASE + 0x0000030c)
-
-#define GSBI5_GSBI_CTRL_REG_REG		(GSBI5_REG_BASE + 0x00000000)
-#define GSBI6_GSBI_CTRL_REG_REG		(GSBI6_REG_BASE + 0x00000000)
-#define GSBI7_GSBI_CTRL_REG_REG		(GSBI7_REG_BASE + 0x00000000)
-
-#define GSBI5_QUP_CONFIG_REG		(GSBI5_QUP5_REG_BASE + 0x00000000)
-#define GSBI6_QUP_CONFIG_REG		(GSBI6_QUP6_REG_BASE + 0x00000000)
-#define GSBI7_QUP_CONFIG_REG		(GSBI7_QUP7_REG_BASE + 0x00000000)
-
-#define GSBI5_QUP_ERROR_FLAGS_REG	(GSBI5_QUP5_REG_BASE      + 0x0000001c)
-#define GSBI6_QUP_ERROR_FLAGS_REG	(GSBI6_QUP6_REG_BASE      + 0x0000001c)
-#define GSBI7_QUP_ERROR_FLAGS_REG	(GSBI7_QUP7_REG_BASE      + 0x0000001c)
-
-#define GSBI5_QUP_ERROR_FLAGS_EN_REG	(GSBI5_QUP5_REG_BASE + 0x00000020)
-#define GSBI6_QUP_ERROR_FLAGS_EN_REG	(GSBI6_QUP6_REG_BASE + 0x00000020)
-#define GSBI7_QUP_ERROR_FLAGS_EN_REG	(GSBI7_QUP7_REG_BASE + 0x00000020)
-
-#define GSBI5_QUP_OPERATIONAL_REG	(GSBI5_QUP5_REG_BASE + 0x00000018)
-#define GSBI6_QUP_OPERATIONAL_REG	(GSBI6_QUP6_REG_BASE + 0x00000018)
-#define GSBI7_QUP_OPERATIONAL_REG	(GSBI7_QUP7_REG_BASE + 0x00000018)
-
-#define GSBI5_QUP_IO_MODES_REG		(GSBI5_QUP5_REG_BASE + 0x00000008)
-#define GSBI6_QUP_IO_MODES_REG		(GSBI6_QUP6_REG_BASE + 0x00000008)
-#define GSBI7_QUP_IO_MODES_REG		(GSBI7_QUP7_REG_BASE + 0x00000008)
-
-#define GSBI5_QUP_STATE_REG		(GSBI5_QUP5_REG_BASE + 0x00000004)
-#define GSBI6_QUP_STATE_REG		(GSBI6_QUP6_REG_BASE + 0x00000004)
-#define GSBI7_QUP_STATE_REG		(GSBI7_QUP7_REG_BASE + 0x00000004)
-
-#define GSBI5_QUP_OUT_FIFO_WORD_CNT_REG	(GSBI5_QUP5_REG_BASE + 0x0000010c)
-#define GSBI6_QUP_OUT_FIFO_WORD_CNT_REG	(GSBI6_QUP6_REG_BASE + 0x0000010c)
-#define GSBI7_QUP_OUT_FIFO_WORD_CNT_REG	(GSBI7_QUP7_REG_BASE + 0x0000010c)
-
-#define GSBI5_QUP_IN_FIFO_WORD_CNT_REG	(GSBI5_QUP5_REG_BASE + 0x00000214)
-#define GSBI6_QUP_IN_FIFO_WORD_CNT_REG	(GSBI6_QUP6_REG_BASE + 0x00000214)
-#define GSBI7_QUP_IN_FIFO_WORD_CNT_REG	(GSBI7_QUP7_REG_BASE + 0x00000214)
-
-#define GSBI5_QUP_INPUT_FIFOc_REG(c) \
-		(GSBI5_QUP5_REG_BASE + 0x00000218 + 4 * (c))
-#define GSBI6_QUP_INPUT_FIFOc_REG(c) \
-		(GSBI6_QUP6_REG_BASE + 0x00000218 + 4 * (c))
-#define GSBI7_QUP_INPUT_FIFOc_REG(c) \
-		(GSBI7_QUP7_REG_BASE + 0x00000218 + 4 * (c))
-
-#define GSBI5_QUP_OUTPUT_FIFOc_REG(c) \
-		(GSBI5_QUP5_REG_BASE + 0x00000110 + 4 * (c))
-#define GSBI6_QUP_OUTPUT_FIFOc_REG(c) \
-		(GSBI6_QUP6_REG_BASE + 0x00000110 + 4 * (c))
-#define GSBI7_QUP_OUTPUT_FIFOc_REG(c) \
-		(GSBI7_QUP7_REG_BASE + 0x00000110 + 4 * (c))
-
-#define GSBI5_QUP_MX_INPUT_COUNT_REG	(GSBI5_QUP5_REG_BASE + 0x00000200)
-#define GSBI6_QUP_MX_INPUT_COUNT_REG	(GSBI6_QUP6_REG_BASE + 0x00000200)
-#define GSBI7_QUP_MX_INPUT_COUNT_REG	(GSBI7_QUP7_REG_BASE + 0x00000200)
-
-#define GSBI5_QUP_MX_OUTPUT_COUNT_REG	(GSBI5_QUP5_REG_BASE + 0x00000100)
-#define GSBI6_QUP_MX_OUTPUT_COUNT_REG	(GSBI6_QUP6_REG_BASE + 0x00000100)
-#define GSBI7_QUP_MX_OUTPUT_COUNT_REG	(GSBI7_QUP7_REG_BASE + 0x00000100)
-
-#define GSBI5_QUP_SW_RESET_REG		(GSBI5_QUP5_REG_BASE + 0x0000000c)
-#define GSBI6_QUP_SW_RESET_REG		(GSBI6_QUP6_REG_BASE + 0x0000000c)
-#define GSBI7_QUP_SW_RESET_REG		(GSBI7_QUP7_REG_BASE + 0x0000000c)
-
-#define CLK_CTL_REG_BASE		0x00900000
-#define GSBIn_RESET_REG(n) \
-		(CLK_CTL_REG_BASE      + 0x000029dc + 32 * ((n)-1))
-
-#define SFAB_AHB_S3_FCLK_CTL_REG \
-		(CLK_CTL_REG_BASE      + 0x0000216c)
-#define CFPB_CLK_NS_REG \
-		(CLK_CTL_REG_BASE      + 0x0000264c)
-#define SFAB_CFPB_S_HCLK_CTL_REG \
-		(CLK_CTL_REG_BASE      + 0x000026c0)
-#define CFPB_SPLITTER_HCLK_CTL_REG \
-		(CLK_CTL_REG_BASE      + 0x000026e0)
-#define CFPB0_HCLK_CTL_REG \
-		(CLK_CTL_REG_BASE      + 0x00002650)
-#define CFPB2_HCLK_CTL_REG \
-		(CLK_CTL_REG_BASE      + 0x00002658)
-#define GSBIn_HCLK_CTL_REG(n) \
-		(CLK_CTL_REG_BASE      + 0x000029c0 + 32 * ((n)-1))
-#define GSBIn_QUP_APPS_NS_REG(n) \
-	(CLK_CTL_REG_BASE      + 0x000029cc + 32 * ((n)-1))
-#define GSBIn_QUP_APPS_MD_REG(n) \
-		(CLK_CTL_REG_BASE      + 0x000029c8 + 32 * ((n)-1))
-#define CLK_HALT_CFPB_STATEB_REG \
-		(CLK_CTL_REG_BASE      + 0x00002fd0)
-
-#define GSBI5_HCLK				23
-#define GSBI6_HCLK				19
-#define GSBI7_HCLK				15
-#define GSBI5_QUP_APPS_CLK			20
-#define GSBI6_QUP_APPS_CLK			16
-#define GSBI7_QUP_APPS_CLK			12
-#define GSBI_CLK_BRANCH_ENA_MSK			(1 << 4)
-#define GSBI_CLK_BRANCH_ENA			(1 << 4)
-#define GSBI_CLK_BRANCH_DIS			(0 << 4)
-#define QUP_CLK_BRANCH_ENA_MSK			(1 << 9)
-#define QUP_CLK_BRANCH_ENA			(1 << 9)
-#define QUP_CLK_BRANCH_DIS			(0 << 9)
-#define CLK_ROOT_ENA_MSK			(1 << 11)
-#define CLK_ROOT_ENA				(1 << 11)
-#define CLK_ROOT_DIS				(0 << 11)
-
-#define QUP_STATE_VALID_BIT			2
-#define QUP_STATE_VALID				1
-#define QUP_STATE_MASK				0x3
 #define QUP_CONFIG_MINI_CORE_MSK		(0x0F << 8)
 #define QUP_CONFIG_MINI_CORE_SPI		(1 << 8)
-#define SPI_QUP_CONF_INPUT_MSK			(1 << 7)
-#define SPI_QUP_CONF_INPUT_ENA			(0 << 7)
-#define SPI_QUP_CONF_NO_INPUT			(1 << 7)
-#define SPI_QUP_CONF_OUTPUT_MSK			(1 << 6)
-#define SPI_QUP_CONF_OUTPUT_ENA			(0 << 6)
-#define SPI_QUP_CONF_NO_OUTPUT			(1 << 6)
-#define SPI_QUP_CONF_OUTPUT_ENA			(0 << 6)
-#define QUP_STATE_RESET_STATE			0x0
-#define QUP_STATE_RUN_STATE			0x1
-#define QUP_STATE_PAUSE_STATE			0x3
-#define SPI_BIT_WORD_MSK			0x1F
-#define SPI_8_BIT_WORD				0x07
-#define PROTOCOL_CODE_MSK			(0x07 << 4)
-#define PROTOCOL_CODE_SPI			(0x03 << 4)
-#define LOOP_BACK_MSK				(1 << 8)
-#define NO_LOOP_BACK				(0 << 8)
-#define SLAVE_OPERATION_MSK			(1 << 5)
-#define SLAVE_OPERATION				(0 << 5)
-#define CLK_ALWAYS_ON				(0 << 9)
-#define MX_CS_MODE				(0 << 8)
-#define NO_TRI_STATE				(1 << 0)
-#define OUTPUT_BIT_SHIFT_MSK			(1 << 16)
-#define OUTPUT_BIT_SHIFT_EN			(1 << 16)
-#define INPUT_BLOCK_MODE_MSK			(0x03 << 12)
-#define INPUT_BLOCK_MODE			(0x01 << 12)
-#define OUTPUT_BLOCK_MODE_MSK			(0x03 << 10)
-#define OUTPUT_BLOCK_MODE			(0x01 << 10)
-#define GSBI1_RESET				(1 << 0)
-#define GSBI1_RESET_MSK				0x1
-
-#define GSBI_M_VAL_SHFT				16
-#define GSBIn_M_VAL_MSK				(0xFF << GSBI_M_VAL_SHFT)
-#define GSBI_N_VAL_SHFT				16
-#define GSBIn_N_VAL_MSK				(0xFF << GSBI_N_VAL_SHFT)
-#define GSBI_D_VAL_SHFT				0
-#define GSBIn_D_VAL_MSK				(0xFF << GSBI_D_VAL_SHFT)
-#define MNCNTR_RST_MSK				(1 << 7)
-#define MNCNTR_RST_ENA				(1 << 7)
-#define MNCNTR_RST_DIS				(0 << 7)
-#define MNCNTR_MSK				(1 << 8)
-#define MNCNTR_EN				(1 << 8)
-#define MNCNTR_DIS				(0 << 8)
-#define MNCNTR_MODE_MSK				(0x3 << 5)
-#define MNCNTR_MODE_BYPASS			(0 << 5)
-#define MNCNTR_MODE_DUAL_EDGE			(0x2 << 5)
-#define GSBI_PRE_DIV_SEL_SHFT			3
-#define GSBIn_PRE_DIV_SEL_MSK			(0x3 << GSBI_PRE_DIV_SEL_SHFT)
-#define GSBIn_PLL_SRC_MSK			(0x03 << 0)
-#define GSBIn_PLL_SRC_PXO			(0 << 0)
-#define GSBIn_PLL_SRC_PLL8			(0x3 << 0)
-
-#define SPI_INPUT_FIRST_MODE			(1 << 9)
-#define SPI_IO_CONTROL_CLOCK_IDLE_HIGH		(1 << 10)
-#define QUP_DATA_AVAILABLE_FOR_READ		(1 << 5)
-#define QUP_OUTPUT_FIFO_NOT_EMPTY		(1 << 4)
-#define OUTPUT_SERVICE_FLAG			(1 << 8)
-#define INPUT_SERVICE_FLAG			(1 << 9)
-#define QUP_OUTPUT_FIFO_FULL			(1 << 6)
-#define QUP_INPUT_FIFO_NOT_EMPTY		(1 << 5)
+#define QUP_CONF_INPUT_MSK			(1 << 7)
+#define QUP_CONF_INPUT_ENA			(0 << 7)
+#define QUP_CONF_NO_INPUT			(1 << 7)
+#define QUP_CONF_OUTPUT_MSK			(1 << 6)
+#define QUP_CONF_OUTPUT_ENA			(0 << 6)
+#define QUP_CONF_NO_OUTPUT			(1 << 6)
+#define QUP_CONF_N_MASK				0x1F
+#define QUP_CONF_N_SPI_8_BIT_WORD		0x07
+
+#define SPI_CONFIG_INPUT_FIRST			(1 << 9)
+#define SPI_CONFIG_INPUT_FIRST_BACK		(0 << 9)
+#define SPI_CONFIG_LOOP_BACK_MSK		(1 << 8)
+#define SPI_CONFIG_NO_LOOP_BACK			(0 << 8)
+#define SPI_CONFIG_NO_SLAVE_OPER_MSK		(1 << 5)
+#define SPI_CONFIG_NO_SLAVE_OPER		(0 << 5)
+
+#define SPI_IO_CTRL_CLK_ALWAYS_ON		(0 << 9)
+#define SPI_IO_CTRL_MX_CS_MODE			(1 << 8)
+#define SPI_IO_CTRL_NO_TRI_STATE		(1 << 0)
+#define SPI_IO_CTRL_FORCE_CS_MSK		(1 << 11)
+#define SPI_IO_CTRL_FORCE_CS_EN			(1 << 11)
+#define SPI_IO_CTRL_FORCE_CS_DIS		(0 << 11)
+#define SPI_IO_CTRL_CLOCK_IDLE_HIGH		(1 << 10)
+
+#define QUP_IO_MODES_OUTPUT_BIT_SHIFT_MSK	(1 << 16)
+#define QUP_IO_MODES_OUTPUT_BIT_SHIFT_EN	(1 << 16)
+#define QUP_IO_MODES_INPUT_MODE_MSK		(0x03 << 12)
+#define QUP_IO_MODES_INPUT_BLOCK_MODE		(0x01 << 12)
+#define QUP_IO_MODES_OUTPUT_MODE_MSK		(0x03 << 10)
+#define QUP_IO_MODES_OUTPUT_BLOCK_MODE		(0x01 << 10)
+
 #define SPI_INPUT_BLOCK_SIZE			4
 #define SPI_OUTPUT_BLOCK_SIZE			4
-#define GSBI5_SPI_CLK				21
-#define GSBI5_SPI_MISO				19
-#define GSBI5_SPI_MOSI				18
-#define GSBI5_SPI_CS_0				20
-#define GSBI5_SPI_CS_1				61
-#define GSBI5_SPI_CS_2				62
-#define GSBI5_SPI_CS_3				2
-#define GSBI6_SPI_CLK				30
-#define GSBI6_SPI_CS_0				29
-#define GSBI6_SPI_MISO				28
-#define GSBI6_SPI_MOSI				27
-#define GSBI7_SPI_CLK				9
-#define GSBI7_SPI_CS_0				8
-#define GSBI7_SPI_MISO				7
-#define GSBI7_SPI_MOSI				6
-
-#define MSM_GSBI_MAX_FREQ			51200000
-
-#define SPI_RESET_STATE				0
-#define SPI_RUN_STATE				1
-#define SPI_PAUSE_STATE				3
+
+#define MAX_COUNT_SIZE				0xffff
+
 #define SPI_CORE_RESET				0
 #define SPI_CORE_RUNNING			1
-#define GSBI_SPI_MODE_0				0
-#define GSBI_SPI_MODE_1				1
-#define GSBI_SPI_MODE_2				2
-#define GSBI_SPI_MODE_3				3
-#define GSBI5_SPI				0
-#define GSBI6_SPI				1
-#define GSBI7_SPI				2
-
-struct gsbi_spi {
-	unsigned int     spi_config;
-	unsigned int     io_control;
-	unsigned int     error_flags;
-	unsigned int     error_flags_en;
-	unsigned int     gsbi_ctrl;
-	unsigned int     qup_config;
-	unsigned int     qup_error_flags;
-	unsigned int     qup_error_flags_en;
-	unsigned int     qup_operational;
-	unsigned int     qup_io_modes;
-	unsigned int     qup_state;
-	unsigned int     qup_input_fifo;
-	unsigned int     qup_output_fifo;
-	unsigned int     qup_mx_input_count;
-	unsigned int     qup_mx_output_count;
-	unsigned int     qup_sw_reset;
-	unsigned int     qup_ns_reg;
-	unsigned int     qup_md_reg;
+#define SPI_MODE0				0
+#define SPI_MODE1				1
+#define SPI_MODE2				2
+#define SPI_MODE3				3
+#define BLSP0_SPI				0
+#define BLSP1_SPI				1
+
+struct blsp_spi {
+	void *spi_config;
+	void *io_control;
+	void *error_flags;
+	void *error_flags_en;
+	void *qup_config;
+	void *qup_error_flags;
+	void *qup_error_flags_en;
+	void *qup_operational;
+	void *qup_io_modes;
+	void *qup_state;
+	void *qup_input_fifo;
+	void *qup_output_fifo;
+	void *qup_mx_input_count;
+	void *qup_mx_output_count;
+	void *qup_sw_reset;
+	void *qup_ns_reg;
+	void *qup_md_reg;
+	void *qup_op_mask;
+	void *qup_deassert_wait;
 };
 
+
+#define SUCCESS		0
+
+#define DUMMY_DATA_VAL		0
+#define TIMEOUT_CNT		100
+
+#define ETIMEDOUT -10
+#define EINVAL -11
+#define EIO -12
+
+/* MX_INPUT_COUNT and MX_OUTPUT_COUNT are 16-bits. Zero has a special meaning
+ * (count function disabled) and does not hold significance in the count. */
+#define MAX_PACKET_COUNT	((64 * KiB) - 1)
+
+
 struct ipq_spi_slave {
 	struct spi_slave slave;
-	const struct gsbi_spi *regs;
+	const struct blsp_spi *regs;
 	unsigned int mode;
 	unsigned int initialized;
 	unsigned long freq;
diff --git a/src/soc/qualcomm/ipq40xx/qup.c b/src/soc/qualcomm/ipq40xx/qup.c
index c876d30c67..63b37ef7b4 100644
--- a/src/soc/qualcomm/ipq40xx/qup.c
+++ b/src/soc/qualcomm/ipq40xx/qup.c
@@ -36,27 +36,35 @@
 #include <stdlib.h>
 #include <soc/qup.h>
 
-#define TIMEOUT_CNT	100000
-
-//TODO: refactor the following array to iomap driver.
-static unsigned gsbi_qup_base[] = {
-	(unsigned)GSBI_QUP1_BASE,
-	(unsigned)GSBI_QUP2_BASE,
-	(unsigned)GSBI_QUP3_BASE,
-	(unsigned)GSBI_QUP4_BASE,
-	(unsigned)GSBI_QUP5_BASE,
-	(unsigned)GSBI_QUP6_BASE,
-	(unsigned)GSBI_QUP7_BASE,
-};
-
-#define QUP_ADDR(gsbi_num, reg)	((void *)((gsbi_qup_base[gsbi_num-1]) + (reg)))
-
-static qup_return_t qup_i2c_master_status(gsbi_id_t gsbi_id)
+#define TIMEOUT_CNT	100
+
+#define QUP_ADDR(id, reg)	(blsp_qup_base(id) + (reg))
+
+#define QUP_DEBUG	0
+
+#define QUPDBG		BIOS_ERR, "\t-> "
+
+#if QUP_DEBUG
+#define qup_write32(a, v) do {				\
+	write32(a, v);					\
+	printk(QUPDBG "%s(%d): write32(0x%p, 0x%x)\n",	\
+			__func__, __LINE__, a, v);	\
+} while (0)
+#else
+#define qup_write32	write32
+#endif
+
+static qup_return_t qup_i2c_master_status(blsp_qup_id_t id)
 {
-	uint32_t reg_val = read32(QUP_ADDR(gsbi_id, QUP_I2C_MASTER_STATUS));
+	uint32_t reg_val = read32(QUP_ADDR(id, QUP_I2C_MASTER_STATUS));
 
-	if (read32(QUP_ADDR(gsbi_id, QUP_ERROR_FLAGS)))
+	if (read32(QUP_ADDR(id, QUP_ERROR_FLAGS)))
 		return QUP_ERR_XFER_FAIL;
+
+#if QUP_DEBUG
+	printk(QUPDBG "%s: 0x%x\n", __func__, reg_val);
+#endif
+
 	if (reg_val & QUP_I2C_INVALID_READ_ADDR)
 		return QUP_ERR_I2C_INVALID_SLAVE_ADDR;
 	if (reg_val & QUP_I2C_FAILED_MASK)
@@ -84,7 +92,6 @@ static int check_bit_state(uint32_t *reg, int wait_for)
 		if (count == 0)
 			return QUP_ERR_TIMEOUT;
 		count--;
-		udelay(1);
 	}
 
 	return QUP_SUCCESS;
@@ -93,37 +100,36 @@ static int check_bit_state(uint32_t *reg, int wait_for)
 /*
  * Check whether GSBIn_QUP State is valid
  */
-static qup_return_t qup_wait_for_state(gsbi_id_t gsbi_id, unsigned wait_for)
+static qup_return_t qup_wait_for_state(blsp_qup_id_t id, unsigned wait_for)
 {
-	return check_bit_state(QUP_ADDR(gsbi_id, QUP_STATE), wait_for);
+	return check_bit_state(QUP_ADDR(id, QUP_STATE), wait_for);
 }
 
-qup_return_t qup_reset_i2c_master_status(gsbi_id_t gsbi_id)
+qup_return_t qup_reset_i2c_master_status(blsp_qup_id_t id)
 {
 	/*
-	 * Writing a one clears the status bits.
-	 * Bit31-25, Bit1 and Bit0 are reserved.
+	 * The I2C_STATUS is a status register.
+	 * Writing any value clears the status bits.
 	 */
-	//TODO: Define each status bit. OR all status bits in a single macro.
-	write32(QUP_ADDR(gsbi_id, QUP_I2C_MASTER_STATUS), 0x3FFFFFC);
+	qup_write32(QUP_ADDR(id, QUP_I2C_MASTER_STATUS), 0);
 	return QUP_SUCCESS;
 }
 
-static qup_return_t qup_reset_master_status(gsbi_id_t gsbi_id)
+static qup_return_t qup_reset_master_status(blsp_qup_id_t id)
 {
-	write32(QUP_ADDR(gsbi_id, QUP_ERROR_FLAGS), 0x7C);
-	write32(QUP_ADDR(gsbi_id, QUP_ERROR_FLAGS_EN), 0x7C);
-	qup_reset_i2c_master_status(gsbi_id);
+	qup_write32(QUP_ADDR(id, QUP_ERROR_FLAGS), 0x3C);
+	qup_write32(QUP_ADDR(id, QUP_ERROR_FLAGS_EN), 0x3C);
+	qup_reset_i2c_master_status(id);
 	return QUP_SUCCESS;
 }
 
-static qup_return_t qup_fifo_wait_for(gsbi_id_t gsbi_id, uint32_t status)
+static qup_return_t qup_fifo_wait_for(blsp_qup_id_t id, uint32_t status)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
 	unsigned int count = TIMEOUT_CNT;
 
-	while (!(read32(QUP_ADDR(gsbi_id, QUP_OPERATIONAL)) & status)) {
-		ret = qup_i2c_master_status(gsbi_id);
+	while (!(read32(QUP_ADDR(id, QUP_OPERATIONAL)) & status)) {
+		ret = qup_i2c_master_status(id);
 		if (ret)
 			return ret;
 		if (count == 0)
@@ -134,13 +140,13 @@ static qup_return_t qup_fifo_wait_for(gsbi_id_t gsbi_id, uint32_t status)
 	return QUP_SUCCESS;
 }
 
-static qup_return_t qup_fifo_wait_while(gsbi_id_t gsbi_id, uint32_t status)
+static qup_return_t qup_fifo_wait_while(blsp_qup_id_t id, uint32_t status)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
 	unsigned int count = TIMEOUT_CNT;
 
-	while (read32(QUP_ADDR(gsbi_id, QUP_OPERATIONAL)) & status) {
-		ret = qup_i2c_master_status(gsbi_id);
+	while (read32(QUP_ADDR(id, QUP_OPERATIONAL)) & status) {
+		ret = qup_i2c_master_status(id);
 		if (ret)
 			return ret;
 		if (count == 0)
@@ -151,7 +157,39 @@ static qup_return_t qup_fifo_wait_while(gsbi_id_t gsbi_id, uint32_t status)
 	return QUP_SUCCESS;
 }
 
-static qup_return_t qup_i2c_write_fifo(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj,
+static inline uint32_t qup_i2c_create_output_tag(int stop, u8 data)
+{
+	uint32_t tag;
+
+	if (stop)
+		tag = QUP_I2C_STOP_SEQ | QUP_I2C_DATA(data);
+	else
+		tag = QUP_I2C_DATA_SEQ | QUP_I2C_DATA(data);
+
+	return tag;
+}
+
+static inline qup_return_t qup_i2c_write_fifo_flush(blsp_qup_id_t id)
+{
+	qup_return_t ret = QUP_ERR_UNDEFINED;
+
+	qup_write32(QUP_ADDR(id, QUP_OPERATIONAL), OUTPUT_SERVICE_FLAG);
+
+	mdelay(10);	/* TPM seems to need this */
+
+	ret = qup_fifo_wait_while(id, OUTPUT_FIFO_NOT_EMPTY);
+	if (ret)
+		return ret;
+
+	ret = qup_i2c_master_status(id);
+
+	if (ret)
+		printk(BIOS_DEBUG, "%s: error\n", __func__);
+
+	return ret;
+}
+
+static qup_return_t qup_i2c_write_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj,
 				       uint8_t stop_seq)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
@@ -159,180 +197,235 @@ static qup_return_t qup_i2c_write_fifo(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj,
 	uint8_t *data_ptr = p_tx_obj->p.iic.data;
 	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(gsbi_id);
-	qup_set_state(gsbi_id, QUP_STATE_RUN);
+	qup_reset_master_status(id);
 
-	write32(QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO),
-		(QUP_I2C_START_SEQ | QUP_I2C_ADDR(addr)));
+	qup_set_state(id, QUP_STATE_RUN);
+
+	/*
+	 * Since UNPACK enable is set in io mode register, populate 2 tags
+	 * for each fifo register.
+	 *
+	 * Create the first tag as follows, with the start tag and first byte
+	 * of the data to be written
+	 *	+--------+--------+--------+--------+
+	 *	| STOP / |  data  | START  | ADDR   |
+	 *	|DATA tag|  byte  |  tag   | << 1   |
+	 *	+--------+--------+--------+--------+
+	 * rest will be created in the following while loop.
+	 */
+	tag = qup_i2c_create_output_tag(data_len == 1 && stop_seq,
+					data_ptr[idx]);
+	tag = ((tag << 16) & 0xffff0000) |
+			(QUP_I2C_START_SEQ | QUP_I2C_ADDR(addr));
+	data_len--;
+	idx++;
+
+	qup_write32(fifo, tag);
+	fifo++;
 
 	while (data_len) {
-		if (data_len == 1 && stop_seq) {
-			write32(QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO),
-				QUP_I2C_STOP_SEQ | QUP_I2C_DATA(data_ptr[idx]));
-		} else {
-			write32(QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO),
-				QUP_I2C_DATA_SEQ | QUP_I2C_DATA(data_ptr[idx]));
-		}
+
+		tag = qup_i2c_create_output_tag(data_len == 1 && stop_seq,
+						data_ptr[idx]);
 		data_len--;
 		idx++;
+
 		if (data_len) {
-			ret = qup_fifo_wait_while(gsbi_id, OUTPUT_FIFO_FULL);
-			if (ret)
-				return ret;
+			tag |= qup_i2c_create_output_tag(
+						data_len == 1 && stop_seq,
+						data_ptr[idx]) << 16;
+			data_len--;
+			idx++;
 		}
-		/* 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 (read32(QUP_ADDR(gsbi_id, QUP_OPERATIONAL)) &
-				OUTPUT_SERVICE_FLAG) {
-			write32(QUP_ADDR(gsbi_id, QUP_OPERATIONAL),
-				OUTPUT_SERVICE_FLAG);
+
+		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_fifo_wait_while(gsbi_id, OUTPUT_FIFO_NOT_EMPTY);
-	if (ret)
-		return ret;
+	ret = qup_i2c_write_fifo_flush(id);
 
-	qup_set_state(gsbi_id, QUP_STATE_PAUSE);
-	return qup_i2c_master_status(gsbi_id);
+	qup_set_state(id, QUP_STATE_RESET);
+
+	return ret;
 }
 
-static qup_return_t qup_i2c_write(gsbi_id_t gsbi_id, uint8_t mode,
+static qup_return_t qup_i2c_write(blsp_qup_id_t 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);
+		ret = qup_i2c_write_fifo(id, p_tx_obj, stop_seq);
 		break;
 	default:
 		ret = QUP_ERR_UNSUPPORTED;
 	}
 
 	if (ret) {
-		qup_set_state(gsbi_id, QUP_STATE_RESET);
-		printk(BIOS_ERR, "%s() failed (%d)\n", __func__, ret);
+		qup_set_state(id, QUP_STATE_RESET);
+		printk(QUPDBG "%s() failed (%d)\n", __func__, ret);
 	}
 
 	return ret;
 }
 
-static qup_return_t qup_i2c_read_fifo(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj)
+static int qup_i2c_parse_tag(uint32_t data, uint8_t *data_ptr, uint32_t len)
+{
+	int i, idx = 0;
+	int max = (len > 2) ? 2 : len;
+
+	for (i = 0; i < max; i++) {
+		switch (QUP_I2C_MI_TAG(data)) {
+		case QUP_I2C_MIDATA_SEQ:
+			data_ptr[idx] = QUP_I2C_DATA(data);
+			idx++;
+			break;
+		case QUP_I2C_MISTOP_SEQ:
+			data_ptr[idx] = QUP_I2C_DATA(data);
+			idx++;
+			return idx;
+		default:
+			printk(QUPDBG "%s: Unexpected tag (0x%x)\n", __func__,
+				QUP_I2C_MI_TAG(data));
+			return -1;
+		}
+
+		data = (data >> 16);
+	}
+
+	return idx;
+}
+
+static qup_return_t qup_i2c_read_fifo(blsp_qup_id_t id, qup_data_t *p_tx_obj)
 {
 	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;
+	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(gsbi_id);
-	qup_set_state(gsbi_id, QUP_STATE_RUN);
+	qup_reset_master_status(id);
 
-	write32(QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO),
-		QUP_I2C_START_SEQ | (QUP_I2C_ADDR(addr) | QUP_I2C_SLAVE_READ));
+	qup_set_state(id, QUP_STATE_RUN);
 
-	write32(QUP_ADDR(gsbi_id, QUP_OUTPUT_FIFO),
-		QUP_I2C_RECV_SEQ | data_len);
+	qup_write32(fifo, (QUP_I2C_START_SEQ |
+				  (QUP_I2C_ADDR(addr) | QUP_I2C_SLAVE_READ)) |
+				((QUP_I2C_RECV_SEQ | data_len) << 16));
 
-	ret = qup_fifo_wait_while(gsbi_id, OUTPUT_FIFO_NOT_EMPTY);
-	if (ret)
+	ret = qup_i2c_write_fifo_flush(id);
+	if (ret) {
+		printk(QUPDBG "%s: OUTPUT_FIFO_NOT_EMPTY\n", __func__);
+		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__);
 		return ret;
+	}
 
-	write32(QUP_ADDR(gsbi_id, QUP_OPERATIONAL), OUTPUT_SERVICE_FLAG);
+	fifo = QUP_ADDR(id, QUP_INPUT_FIFO);
 
 	while (data_len) {
 		uint32_t data;
+		int count;
 
-		ret = qup_fifo_wait_for(gsbi_id, INPUT_SERVICE_FLAG);
-		if (ret)
-			return ret;
+		data = read32(fifo);
+		fifo++;
 
-		data = read32(QUP_ADDR(gsbi_id, QUP_INPUT_FIFO));
+		count = qup_i2c_parse_tag(data, data_ptr + idx, data_len);
 
-		/*
-		 * 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--;
-			write32(QUP_ADDR(gsbi_id, QUP_OPERATIONAL),
-				INPUT_SERVICE_FLAG);
-		} 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;
-			}
-			/* Tag: MINACK: Invalid master input data.*/
-			break;
+		if (count < 0) {
+			printk(QUPDBG "%s: Cannot parse tag 0x%x\n",
+						__func__, data);
+			qup_set_state(id, QUP_STATE_PAUSE);
+
+			return QUP_ERR_I2C_INVALID_TAG;
+		}
+
+		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);
 		}
 	}
 
-	write32(QUP_ADDR(gsbi_id, QUP_OPERATIONAL), INPUT_SERVICE_FLAG);
 	p_tx_obj->p.iic.data_len = idx;
-	qup_set_state(gsbi_id, QUP_STATE_PAUSE);
+
+	qup_write32(QUP_ADDR(id, QUP_MX_READ_COUNT), 0);
+
+	qup_set_state(id, QUP_STATE_RESET);
 
 	return QUP_SUCCESS;
 }
 
-static qup_return_t qup_i2c_read(gsbi_id_t gsbi_id, uint8_t mode,
+static qup_return_t qup_i2c_read(blsp_qup_id_t id, uint8_t mode,
 				 qup_data_t *p_tx_obj)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
 
+	qup_set_state(id, QUP_STATE_RESET);
+
 	switch (mode) {
 	case QUP_MODE_FIFO:
-		ret = qup_i2c_read_fifo(gsbi_id, p_tx_obj);
+		ret = qup_i2c_read_fifo(id, p_tx_obj);
 		break;
 	default:
 		ret = QUP_ERR_UNSUPPORTED;
 	}
 
 	if (ret) {
-		qup_set_state(gsbi_id, QUP_STATE_RESET);
-		printk(BIOS_ERR, "%s() failed (%d)\n", __func__, ret);
+		qup_set_state(id, QUP_STATE_RESET);
+		printk(QUPDBG "%s() failed (%d)\n", __func__, ret);
 	}
 
 	return ret;
 }
 
-qup_return_t qup_init(gsbi_id_t gsbi_id, const qup_config_t *config_ptr)
+qup_return_t qup_init(blsp_qup_id_t id, const qup_config_t *config_ptr)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
 	uint32_t reg_val;
 
 	/* Reset the QUP core.*/
-	write32(QUP_ADDR(gsbi_id, QUP_SW_RESET), 0x1);
+	qup_write32(QUP_ADDR(id, QUP_SW_RESET), 0x1);
 
-	/*Wait till the reset takes effect */
-	ret = qup_wait_for_state(gsbi_id, QUP_STATE_RESET);
+	/* Wait till the reset takes effect */
+	ret = qup_wait_for_state(id, QUP_STATE_RESET);
 	if (ret)
 		goto bailout;
 
 	/* Reset the config */
-	write32(QUP_ADDR(gsbi_id, QUP_CONFIG), 0);
+	qup_write32(QUP_ADDR(id, QUP_CONFIG), 0);
 
-	/*Program the config register*/
-	/*Set N value*/
+	/* Program the config register */
+	/* Set N value */
 	reg_val = 0x0F;
-	/*Set protocol*/
+	/* Set protocol */
 	switch (config_ptr->protocol) {
 	case QUP_MINICORE_I2C_MASTER:
 		reg_val |= ((config_ptr->protocol &
@@ -343,15 +436,19 @@ qup_return_t qup_init(gsbi_id_t gsbi_id, const qup_config_t *config_ptr)
 		ret = QUP_ERR_UNSUPPORTED;
 		goto bailout;
 	}
-	write32(QUP_ADDR(gsbi_id, QUP_CONFIG), reg_val);
+	reg_val |= QUP_APP_CLK_ON_EN | QUP_CORE_CLK_ON_EN;
+	qup_write32(QUP_ADDR(id, QUP_CONFIG), reg_val);
+
+	/* Choose version 1 tag */
+	qup_write32(QUP_ADDR(id, QUP_I2C_MASTER_CONFIG), 0);
 
-	/*Reset i2c clk cntl register*/
-	write32(QUP_ADDR(gsbi_id, QUP_I2C_MASTER_CLK_CTL), 0);
+	/* Reset i2c clk cntl register */
+	qup_write32(QUP_ADDR(id, QUP_I2C_MASTER_CLK_CTL), 0);
 
-	/*Set QUP IO Mode*/
+	/* Set QUP IO Mode */
 	switch (config_ptr->mode) {
 	case QUP_MODE_FIFO:
-		reg_val = QUP_OUTPUT_BIT_SHIFT_EN |
+		reg_val = QUP_UNPACK_EN | QUP_PACK_EN |
 			  ((config_ptr->mode & QUP_MODE_MASK) <<
 					QUP_OUTPUT_MODE_SHFT) |
 			  ((config_ptr->mode & QUP_MODE_MASK) <<
@@ -361,26 +458,27 @@ qup_return_t qup_init(gsbi_id_t gsbi_id, const qup_config_t *config_ptr)
 		ret = QUP_ERR_UNSUPPORTED;
 		goto bailout;
 	}
-	write32(QUP_ADDR(gsbi_id, QUP_IO_MODES), reg_val);
+	qup_write32(QUP_ADDR(id, QUP_IO_MODES), reg_val);
 
 	/*Set i2c clk cntl*/
 	reg_val = (QUP_DIVIDER_MIN_VAL << QUP_HS_DIVIDER_SHFT);
 	reg_val |= ((((config_ptr->src_frequency / config_ptr->clk_frequency)
 			/ 2) - QUP_DIVIDER_MIN_VAL) &
 				QUP_FS_DIVIDER_MASK);
-	write32(QUP_ADDR(gsbi_id, QUP_I2C_MASTER_CLK_CTL), reg_val);
+	qup_write32(QUP_ADDR(id, QUP_I2C_MASTER_CLK_CTL), reg_val);
 
+	qup_set_state(id, QUP_STATE_RESET);
 bailout:
 	if (ret)
-		printk(BIOS_ERR, "failed to init qup (%d)\n", ret);
+		printk(QUPDBG "failed to init qup (%d)\n", ret);
 
 	return ret;
 }
 
-qup_return_t qup_set_state(gsbi_id_t gsbi_id, uint32_t state)
+qup_return_t qup_set_state(blsp_qup_id_t id, uint32_t state)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
-	unsigned curr_state = read32(QUP_ADDR(gsbi_id, QUP_STATE));
+	unsigned curr_state = read32(QUP_ADDR(id, QUP_STATE));
 
 	if ((state >= QUP_STATE_RESET && state <= QUP_STATE_PAUSE)
 		&& (curr_state & QUP_STATE_VALID_MASK)) {
@@ -390,30 +488,30 @@ qup_return_t qup_set_state(gsbi_id_t gsbi_id, uint32_t state)
 		* transition to complete.
 		*/
 		if (QUP_STATE_PAUSE == curr_state && QUP_STATE_RESET == state) {
-			write32(QUP_ADDR(gsbi_id, QUP_STATE), 0x2);
-			write32(QUP_ADDR(gsbi_id, QUP_STATE), 0x2);
+			qup_write32(QUP_ADDR(id, QUP_STATE), 0x2);
+			qup_write32(QUP_ADDR(id, QUP_STATE), 0x2);
 		} else {
-			write32(QUP_ADDR(gsbi_id, QUP_STATE), state);
+			qup_write32(QUP_ADDR(id, QUP_STATE), state);
 		}
-		ret = qup_wait_for_state(gsbi_id, state);
+		ret = qup_wait_for_state(id, state);
 	}
 
 	return ret;
 }
 
-static qup_return_t qup_i2c_send_data(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj,
+static qup_return_t qup_i2c_send_data(blsp_qup_id_t id, qup_data_t *p_tx_obj,
 				      uint8_t stop_seq)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
-	uint8_t mode = (read32(QUP_ADDR(gsbi_id, QUP_IO_MODES)) >>
+	uint8_t mode = (read32(QUP_ADDR(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) {
+	ret = qup_i2c_write(id, mode, p_tx_obj, stop_seq);
+	if (QUP_DEBUG) {
 		int i;
 
 		printk(BIOS_DEBUG, "i2c tx bus %d device %2.2x:",
-		       gsbi_id, p_tx_obj->p.iic.addr);
+		       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");
@@ -422,16 +520,16 @@ static qup_return_t qup_i2c_send_data(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj,
 	return ret;
 }
 
-qup_return_t qup_send_data(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj,
+qup_return_t qup_send_data(blsp_qup_id_t id, qup_data_t *p_tx_obj,
 			   uint8_t stop_seq)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
 
-	if (p_tx_obj->protocol == ((read32(QUP_ADDR(gsbi_id, QUP_CONFIG)) >>
+	if (p_tx_obj->protocol == ((read32(QUP_ADDR(id, QUP_CONFIG)) >>
 			QUP_MINI_CORE_PROTO_SHFT) & QUP_MINI_CORE_PROTO_MASK)) {
 		switch (p_tx_obj->protocol) {
 		case QUP_MINICORE_I2C_MASTER:
-			ret = qup_i2c_send_data(gsbi_id, p_tx_obj, stop_seq);
+			ret = qup_i2c_send_data(id, p_tx_obj, stop_seq);
 			break;
 		default:
 			ret = QUP_ERR_UNSUPPORTED;
@@ -441,18 +539,18 @@ qup_return_t qup_send_data(gsbi_id_t gsbi_id, qup_data_t *p_tx_obj,
 	return ret;
 }
 
-static qup_return_t qup_i2c_recv_data(gsbi_id_t gsbi_id, qup_data_t *p_rx_obj)
+static qup_return_t qup_i2c_recv_data(blsp_qup_id_t id, qup_data_t *p_rx_obj)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
-	uint8_t mode = (read32(QUP_ADDR(gsbi_id, QUP_IO_MODES)) >>
+	uint8_t mode = (read32(QUP_ADDR(id, QUP_IO_MODES)) >>
 			QUP_INPUT_MODE_SHFT) & QUP_MODE_MASK;
 
-	ret = qup_i2c_read(gsbi_id, mode, p_rx_obj);
-	if (0) {
+	ret = qup_i2c_read(id, mode, p_rx_obj);
+	if (QUP_DEBUG) {
 		int i;
 
 		printk(BIOS_DEBUG, "i2c rxed on bus %d device %2.2x:",
-		       gsbi_id, p_rx_obj->p.iic.addr);
+		       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");
@@ -461,15 +559,15 @@ static qup_return_t qup_i2c_recv_data(gsbi_id_t gsbi_id, qup_data_t *p_rx_obj)
 	return ret;
 }
 
-qup_return_t qup_recv_data(gsbi_id_t gsbi_id, qup_data_t *p_rx_obj)
+qup_return_t qup_recv_data(blsp_qup_id_t id, qup_data_t *p_rx_obj)
 {
 	qup_return_t ret = QUP_ERR_UNDEFINED;
 
-	if (p_rx_obj->protocol == ((read32(QUP_ADDR(gsbi_id, QUP_CONFIG)) >>
+	if (p_rx_obj->protocol == ((read32(QUP_ADDR(id, QUP_CONFIG)) >>
 			QUP_MINI_CORE_PROTO_SHFT) & QUP_MINI_CORE_PROTO_MASK)) {
 		switch (p_rx_obj->protocol) {
 		case QUP_MINICORE_I2C_MASTER:
-			ret = qup_i2c_recv_data(gsbi_id, p_rx_obj);
+			ret = qup_i2c_recv_data(id, p_rx_obj);
 			break;
 		default:
 			ret = QUP_ERR_UNSUPPORTED;
diff --git a/src/soc/qualcomm/ipq40xx/spi.c b/src/soc/qualcomm/ipq40xx/spi.c
index be98fba665..8d39f77ae4 100644
--- a/src/soc/qualcomm/ipq40xx/spi.c
+++ b/src/soc/qualcomm/ipq40xx/spi.c
@@ -35,209 +35,117 @@
 #include <stdlib.h>
 #include <string.h>
 
-#define SUCCESS		0
-
-#define DUMMY_DATA_VAL		0
-#define TIMEOUT_CNT		100
-#define CS_ASSERT		1
-#define CS_DEASSERT		0
-#define NUM_PORTS		3
-#define NUM_GSBI_PINS		3
-#define TLMM_ARGS		6
-#define NUM_CS			4
-#define GSBI_PIN_IDX		0
-#define FUNC_SEL_IDX		1
-#define GPIO_DIR_IDX		2
-#define PULL_CONF_IDX		3
-#define DRV_STR_IDX		4
-#define GPIO_EN_IDX		5
-
-/* Arbitrarily assigned error code values */
-#define ETIMEDOUT -10
-#define EINVAL -11
-#define EIO -12
-
-#define GSBI_IDX_TO_GSBI(idx)   (idx + 5)
-
-
-/* MX_INPUT_COUNT and MX_OUTPUT_COUNT are 16-bits. Zero has a special meaning
- * (count function disabled) and does not hold significance in the count. */
-#define MAX_PACKET_COUNT	((64 * KiB) - 1)
-
-/*
- * TLMM Configuration for SPI NOR
- * gsbi_pin_conf[bus_num][GPIO_NUM, FUNC_SEL, I/O,
- *			       PULL UP/DOWN, DRV_STR, GPIO_FUNC]
- * gsbi_pin_conf[0][x][y] -- GSBI5
- * gsbi_pin_conf[1][x][y] -- GSBI6
- * gsbi_pin_conf[2][x][y] -- GSBI7
-*/
-static unsigned int gsbi_pin_conf[NUM_PORTS][NUM_GSBI_PINS][TLMM_ARGS] = {
+static const struct blsp_spi spi_reg[] = {
+	/* BLSP0 registers for SPI interface */
 	{
-		/* GSBI5 CLK */
-		{
-			GSBI5_SPI_CLK,  FUNC_SEL_1, GPIO_INPUT,
-			GPIO_PULL_DOWN, GPIO_DRV_STR_11MA, GPIO_FUNC_DISABLE
-		},
-		/* GSBI5 MISO */
-		{
-			GSBI5_SPI_MISO, FUNC_SEL_1, GPIO_INPUT,
-			GPIO_PULL_DOWN, GPIO_DRV_STR_10MA, GPIO_FUNC_DISABLE
-		},
-		/* GSBI5 MOSI */
-		{
-			GSBI5_SPI_MOSI, FUNC_SEL_1, GPIO_INPUT,
-			GPIO_PULL_DOWN, GPIO_DRV_STR_10MA, GPIO_FUNC_DISABLE
-		}
+		BLSP0_SPI_CONFIG_REG,
+		BLSP0_SPI_IO_CONTROL_REG,
+		BLSP0_SPI_ERROR_FLAGS_REG,
+		BLSP0_SPI_ERROR_FLAGS_EN_REG,
+		BLSP0_QUP_CONFIG_REG,
+		BLSP0_QUP_ERROR_FLAGS_REG,
+		BLSP0_QUP_ERROR_FLAGS_EN_REG,
+		BLSP0_QUP_OPERATIONAL_REG,
+		BLSP0_QUP_IO_MODES_REG,
+		BLSP0_QUP_STATE_REG,
+		BLSP0_QUP_INPUT_FIFOc_REG(0),
+		BLSP0_QUP_OUTPUT_FIFOc_REG(0),
+		BLSP0_QUP_MX_INPUT_COUNT_REG,
+		BLSP0_QUP_MX_OUTPUT_COUNT_REG,
+		BLSP0_QUP_SW_RESET_REG,
+		0,
+		0,
+		BLSP0_QUP_OPERATIONAL_MASK,
+		BLSP0_SPI_DEASSERT_WAIT_REG,
 	},
+	/* BLSP1 registers for SPI interface */
 	{
-		/* GSBI6 CLK */
-		{
-			GSBI6_SPI_CLK,  FUNC_SEL_3, GPIO_INPUT,
-			GPIO_PULL_DOWN, GPIO_DRV_STR_11MA, GPIO_FUNC_DISABLE
-		},
-		/* GSBI6 MISO */
-		{
-			GSBI6_SPI_MISO, FUNC_SEL_3, GPIO_INPUT,
-			GPIO_PULL_DOWN, GPIO_DRV_STR_10MA, GPIO_FUNC_DISABLE
-		},
-		/* GSBI6 MOSI */
-		{
-			GSBI6_SPI_MOSI, FUNC_SEL_3, GPIO_INPUT,
-			GPIO_PULL_DOWN, GPIO_DRV_STR_10MA, GPIO_FUNC_DISABLE
-		}
+		BLSP1_SPI_CONFIG_REG,
+		BLSP1_SPI_IO_CONTROL_REG,
+		BLSP1_SPI_ERROR_FLAGS_REG,
+		BLSP1_SPI_ERROR_FLAGS_EN_REG,
+		BLSP1_QUP_CONFIG_REG,
+		BLSP1_QUP_ERROR_FLAGS_REG,
+		BLSP1_QUP_ERROR_FLAGS_EN_REG,
+		BLSP1_QUP_OPERATIONAL_REG,
+		BLSP1_QUP_IO_MODES_REG,
+		BLSP1_QUP_STATE_REG,
+		BLSP1_QUP_INPUT_FIFOc_REG(0),
+		BLSP1_QUP_OUTPUT_FIFOc_REG(0),
+		BLSP1_QUP_MX_INPUT_COUNT_REG,
+		BLSP1_QUP_MX_OUTPUT_COUNT_REG,
+		BLSP1_QUP_SW_RESET_REG,
+		0,
+		0,
+		BLSP1_QUP_OPERATIONAL_MASK,
+		BLSP1_SPI_DEASSERT_WAIT_REG,
 	},
-	{
-		/* GSBI7 CLK */
-		{
-			GSBI7_SPI_CLK,  FUNC_SEL_1, GPIO_INPUT,
-			GPIO_PULL_DOWN, GPIO_DRV_STR_11MA, GPIO_FUNC_DISABLE
-		},
-		/* GSBI7 MISO */
-		{
-			GSBI7_SPI_MISO, FUNC_SEL_1, GPIO_INPUT,
-			GPIO_PULL_DOWN, GPIO_DRV_STR_10MA, GPIO_FUNC_DISABLE
-		},
-		/* GSBI7 MOSI */
-		{
-			GSBI7_SPI_MOSI, FUNC_SEL_1, GPIO_INPUT,
-			GPIO_PULL_DOWN, GPIO_DRV_STR_10MA, GPIO_FUNC_DISABLE
-		}
-	}
 };
 
-/*
- * CS GPIO number array cs_gpio_array[port_num][cs_num]
- * cs_gpio_array[0][x] -- GSBI5
- * cs_gpio_array[1][x] -- GSBI6
- * cs_gpio_array[2][x] -- GSBI7
- */
-static unsigned int cs_gpio_array[NUM_PORTS][NUM_CS] = {
-	{
-		GSBI5_SPI_CS_0, GSBI5_SPI_CS_1, GSBI5_SPI_CS_2, GSBI5_SPI_CS_3
-	},
-	{
-		GSBI6_SPI_CS_0,              0,              0,              0
-	},
-	{
-		GSBI7_SPI_CS_0,              0,              0,              0
-	}
-};
-
-/*
- * GSBI HCLK state register bit
- * hclk_state[0] -- GSBI5
- * hclk_state[1] -- GSBI6
- * hclk_state[2] -- GSBI7
-*/
-static unsigned int hclk_state[NUM_PORTS] = {
-	GSBI5_HCLK,
-	GSBI6_HCLK,
-	GSBI7_HCLK
-};
-
-/*
- * GSBI QUP_APPS_CLK state register bit
- * qup_apps_clk_state[0] -- GSBI5
- * qup_apps_clk_state[1] -- GSBI6
- * qup_apps_clk_state[2] -- GSBI7
-*/
-static unsigned int qup_apps_clk_state[NUM_PORTS] = {
-	GSBI5_QUP_APPS_CLK,
-	GSBI6_QUP_APPS_CLK,
-	GSBI7_QUP_APPS_CLK
-};
-
-
-static int check_bit_state(uint32_t reg_addr, int bit_num,
+static int check_bit_state(void *reg_addr, int mask,
 				int val, int us_delay)
 {
 	unsigned int count = TIMEOUT_CNT;
-	unsigned int bit_val = ((readl_i(reg_addr) >> bit_num) & 0x01);
 
-	while (bit_val != val) {
+	while ((read32(reg_addr) & mask) != val) {
 		count--;
 		if (count == 0)
 			return -ETIMEDOUT;
 		udelay(us_delay);
-		bit_val = ((readl_i(reg_addr) >> bit_num) & 0x01);
 	}
 
 	return SUCCESS;
 }
 
 /*
- * Check whether GSBIn_QUP State is valid
+ * Check whether QUPn State is valid
  */
 static int check_qup_state_valid(struct ipq_spi_slave *ds)
 {
 
-	return check_bit_state(ds->regs->qup_state, QUP_STATE_VALID_BIT,
+	return check_bit_state(ds->regs->qup_state, QUP_STATE_VALID_MASK,
 				QUP_STATE_VALID, 1);
 
 }
 
 /*
- * Configure GSBIn Core state
+ * Configure QUPn Core state
  */
 static int config_spi_state(struct ipq_spi_slave *ds, unsigned int state)
 {
 	uint32_t val;
-	int ret;
-	uint32_t new_state;
+	int ret = SUCCESS;
 
 	ret = check_qup_state_valid(ds);
 	if (ret != SUCCESS)
 		return ret;
 
 	switch (state) {
-	case SPI_RUN_STATE:
-		new_state = QUP_STATE_RUN_STATE;
+	case QUP_STATE_RUN:
+		/* Set the state to RUN */
+		val = ((read32(ds->regs->qup_state) & ~QUP_STATE_MASK)
+				| QUP_STATE_RUN);
+		write32(ds->regs->qup_state, val);
+		ret = check_qup_state_valid(ds);
 		break;
-
-	case SPI_RESET_STATE:
-		new_state = QUP_STATE_RESET_STATE;
-		break;
-
-	case SPI_PAUSE_STATE:
-		new_state = QUP_STATE_PAUSE_STATE;
+	case QUP_STATE_RESET:
+		/* Set the state to RESET */
+		val = ((read32(ds->regs->qup_state) & ~QUP_STATE_MASK)
+				| QUP_STATE_RESET);
+		write32(ds->regs->qup_state, val);
+		ret = check_qup_state_valid(ds);
 		break;
-
 	default:
-		printk(BIOS_ERR,
-		       "err: unsupported GSBI SPI state : %d\n", state);
-		return -EINVAL;
+		printk(BIOS_ERR, "unsupported QUP SPI state : %d\n", state);
+		ret = -EINVAL;
+		break;
 	}
 
-	/* Set the state as requested */
-	val = (readl_i(ds->regs->qup_state) & ~QUP_STATE_MASK)
-		| new_state;
-	writel_i(val, ds->regs->qup_state);
-	return check_qup_state_valid(ds);
+	return ret;
 }
 
 /*
- * Set GSBIn SPI Mode
+ * Set QUPn SPI Mode
  */
 static void spi_set_mode(struct ipq_spi_slave *ds, unsigned int mode)
 {
@@ -246,199 +154,38 @@ static void spi_set_mode(struct ipq_spi_slave *ds, unsigned int mode)
 	uint32_t val;
 
 	switch (mode) {
-	case GSBI_SPI_MODE_0:
+	case SPI_MODE0:
 		clk_idle_state = 0;
-		input_first_mode = SPI_INPUT_FIRST_MODE;
+		input_first_mode = SPI_CONFIG_INPUT_FIRST;
 		break;
-	case GSBI_SPI_MODE_1:
+	case SPI_MODE1:
 		clk_idle_state = 0;
 		input_first_mode = 0;
 		break;
-	case GSBI_SPI_MODE_2:
+	case SPI_MODE2:
 		clk_idle_state = 1;
-		input_first_mode = SPI_INPUT_FIRST_MODE;
+		input_first_mode = SPI_CONFIG_INPUT_FIRST;
 		break;
-	case GSBI_SPI_MODE_3:
+	case SPI_MODE3:
 		clk_idle_state = 1;
 		input_first_mode = 0;
 		break;
 	default:
-		printk(BIOS_ERR,
-		       "err : unsupported spi mode : %d\n", mode);
+		printk(BIOS_ERR, "unsupported spi mode : %d\n", mode);
 		return;
 	}
 
-	val = readl_i(ds->regs->spi_config);
+	val = read32(ds->regs->spi_config);
 	val |= input_first_mode;
-	writel_i(val, ds->regs->spi_config);
+	write32(ds->regs->spi_config, val);
 
-	val = readl_i(ds->regs->io_control);
+	val = read32(ds->regs->io_control);
 	if (clk_idle_state)
-		val |= SPI_IO_CONTROL_CLOCK_IDLE_HIGH;
+		val |= SPI_IO_CTRL_CLOCK_IDLE_HIGH;
 	else
-		val &= ~SPI_IO_CONTROL_CLOCK_IDLE_HIGH;
-
-	writel_i(val, ds->regs->io_control);
-}
+		val &= ~SPI_IO_CTRL_CLOCK_IDLE_HIGH;
 
-/*
- * Check for HCLK state
- */
-static int check_hclk_state(unsigned int core_num, int enable)
-{
-	return check_bit_state(CLK_HALT_CFPB_STATEB_REG,
-		hclk_state[core_num], enable, 5);
-}
-
-/*
- * Check for QUP APPS CLK state
- */
-static int check_qup_clk_state(unsigned int core_num, int enable)
-{
-	return check_bit_state(CLK_HALT_CFPB_STATEB_REG,
-		qup_apps_clk_state[core_num], enable, 5);
-}
-
-/*
- * Function to assert and De-assert chip select
- */
-static void CS_change(int port_num, int cs_num, int enable)
-{
-	unsigned int cs_gpio = cs_gpio_array[port_num][cs_num];
-	void *addr = GPIO_IN_OUT_ADDR(cs_gpio);
-	uint32_t val = readl_i(addr);
-
-	val &= (~(1 << GPIO_OUTPUT));
-	if (!enable)
-		val |= (1 << GPIO_OUTPUT);
-	write32(addr, val);
-}
-
-/*
- * GSBIn TLMM configuration
- */
-static void gsbi_pin_config(unsigned int port_num, int cs_num)
-{
-	unsigned int gpio;
-	unsigned int i;
-	/* Hold the GSBIn (core_num) core in reset */
-	clrsetbits_le32_i(GSBIn_RESET_REG(GSBI_IDX_TO_GSBI(port_num)),
-			GSBI1_RESET_MSK, GSBI1_RESET);
-
-	/*
-	 * Configure SPI_CLK, SPI_MISO and SPI_MOSI
-	 */
-	for (i = 0; i < NUM_GSBI_PINS; i++) {
-		unsigned int func_sel;
-		unsigned int io_config;
-		unsigned int pull_config;
-		unsigned int drv_strength;
-		unsigned int gpio_en;
-		unsigned int *ptr;
-
-		ptr = gsbi_pin_conf[port_num][i];
-		gpio		= *(ptr + GSBI_PIN_IDX);
-		func_sel	= *(ptr + FUNC_SEL_IDX);
-		io_config	= *(ptr + GPIO_DIR_IDX);
-		pull_config	= *(ptr + PULL_CONF_IDX);
-		drv_strength	= *(ptr + DRV_STR_IDX);
-		gpio_en	= *(ptr + GPIO_EN_IDX);
-
-		gpio_tlmm_config(gpio, func_sel, io_config,
-				 pull_config, drv_strength, gpio_en);
-	}
-
-	gpio = cs_gpio_array[port_num][cs_num];
-	/* configure CS */
-	gpio_tlmm_config(gpio, FUNC_SEL_GPIO, GPIO_OUTPUT, GPIO_PULL_UP,
-				GPIO_DRV_STR_10MA, GPIO_FUNC_ENABLE);
-	CS_change(port_num, cs_num, CS_DEASSERT);
-}
-
-/*
- * Clock configuration for GSBIn Core
- */
-static int gsbi_clock_init(struct ipq_spi_slave *ds)
-{
-	int ret;
-
-	/* Hold the GSBIn (core_num) core in reset */
-	clrsetbits_le32_i(GSBIn_RESET_REG(GSBI_IDX_TO_GSBI(ds->slave.bus)),
-			GSBI1_RESET_MSK, GSBI1_RESET);
-
-	/* Disable GSBIn (core_num) QUP core clock branch */
-	clrsetbits_le32_i(ds->regs->qup_ns_reg, QUP_CLK_BRANCH_ENA_MSK,
-					QUP_CLK_BRANCH_DIS);
-
-	ret = check_qup_clk_state(ds->slave.bus, 1);
-	if (ret) {
-		printk(BIOS_ERR,
-		       "QUP Clock Halt For GSBI%d failed!\n", ds->slave.bus);
-		return ret;
-	}
-
-	/* Disable M/N:D counter and hold M/N:D counter in reset */
-	clrsetbits_le32_i(ds->regs->qup_ns_reg, (MNCNTR_MSK | MNCNTR_RST_MSK),
-					(MNCNTR_RST_ENA | MNCNTR_DIS));
-
-	/* Disable GSBIn (core_num) QUP core clock root */
-	clrsetbits_le32_i(ds->regs->qup_ns_reg, CLK_ROOT_ENA_MSK, CLK_ROOT_DIS);
-
-	clrsetbits_le32_i(ds->regs->qup_ns_reg, GSBIn_PLL_SRC_MSK,
-					GSBIn_PLL_SRC_PLL8);
-	clrsetbits_le32_i(ds->regs->qup_ns_reg, GSBIn_PRE_DIV_SEL_MSK,
-						(0 << GSBI_PRE_DIV_SEL_SHFT));
-
-	/* Program M/N:D values for GSBIn_QUP_APPS_CLK @50MHz */
-	clrsetbits_le32_i(ds->regs->qup_md_reg, GSBIn_M_VAL_MSK,
-						(0x01 << GSBI_M_VAL_SHFT));
-	clrsetbits_le32_i(ds->regs->qup_md_reg, GSBIn_D_VAL_MSK,
-						(0xF7 << GSBI_D_VAL_SHFT));
-	clrsetbits_le32_i(ds->regs->qup_ns_reg, GSBIn_N_VAL_MSK,
-						(0xF8 << GSBI_N_VAL_SHFT));
-
-	/* Set MNCNTR_MODE = 0: Bypass mode */
-	clrsetbits_le32_i(ds->regs->qup_ns_reg, MNCNTR_MODE_MSK,
-					MNCNTR_MODE_DUAL_EDGE);
-
-	/* De-assert the M/N:D counter reset */
-	clrsetbits_le32_i(ds->regs->qup_ns_reg, MNCNTR_RST_MSK, MNCNTR_RST_DIS);
-	clrsetbits_le32_i(ds->regs->qup_ns_reg, MNCNTR_MSK, MNCNTR_EN);
-
-	/*
-	 * Enable the GSBIn (core_num) QUP core clock root.
-	 * Keep MND counter disabled
-	 */
-	clrsetbits_le32_i(ds->regs->qup_ns_reg, CLK_ROOT_ENA_MSK, CLK_ROOT_ENA);
-
-	/* Enable GSBIn (core_num) QUP core clock branch */
-	clrsetbits_le32_i(ds->regs->qup_ns_reg, QUP_CLK_BRANCH_ENA_MSK,
-						QUP_CLK_BRANCH_ENA);
-
-	ret = check_qup_clk_state(ds->slave.bus, 0);
-	if (ret) {
-		printk(BIOS_ERR,
-		       "QUP Clock Enable For GSBI%d failed!\n", ds->slave.bus);
-		return ret;
-	}
-
-	/* Enable GSBIn (core_num) core clock branch */
-	clrsetbits_le32_i(GSBIn_HCLK_CTL_REG(GSBI_IDX_TO_GSBI(ds->slave.bus)),
-			GSBI_CLK_BRANCH_ENA_MSK, GSBI_CLK_BRANCH_ENA);
-
-	ret = check_hclk_state(ds->slave.bus, 0);
-	if (ret) {
-		printk(BIOS_ERR,
-		       "HCLK Enable For GSBI%d failed!\n", ds->slave.bus);
-		return ret;
-	}
-
-	/* Release GSBIn (core_num) core from reset */
-	clrsetbits_le32_i(GSBIn_RESET_REG(GSBI_IDX_TO_GSBI(ds->slave.bus)),
-						GSBI1_RESET_MSK, 0);
-	udelay(50);
-
-	return SUCCESS;
+	write32(ds->regs->io_control, val);
 }
 
 /*
@@ -446,75 +193,11 @@ static int gsbi_clock_init(struct ipq_spi_slave *ds)
  */
 static void spi_reset(struct ipq_spi_slave *ds)
 {
-	writel_i(0x1, ds->regs->qup_sw_reset);
+	write32(ds->regs->qup_sw_reset, 0x1);
 	udelay(5);
+	check_qup_state_valid(ds);
 }
 
-static const struct gsbi_spi spi_reg[] = {
-	/* GSBI5 registers for SPI interface */
-	{
-		GSBI5_SPI_CONFIG_REG,
-		GSBI5_SPI_IO_CONTROL_REG,
-		GSBI5_SPI_ERROR_FLAGS_REG,
-		GSBI5_SPI_ERROR_FLAGS_EN_REG,
-		GSBI5_GSBI_CTRL_REG_REG,
-		GSBI5_QUP_CONFIG_REG,
-		GSBI5_QUP_ERROR_FLAGS_REG,
-		GSBI5_QUP_ERROR_FLAGS_EN_REG,
-		GSBI5_QUP_OPERATIONAL_REG,
-		GSBI5_QUP_IO_MODES_REG,
-		GSBI5_QUP_STATE_REG,
-		GSBI5_QUP_INPUT_FIFOc_REG(0),
-		GSBI5_QUP_OUTPUT_FIFOc_REG(0),
-		GSBI5_QUP_MX_INPUT_COUNT_REG,
-		GSBI5_QUP_MX_OUTPUT_COUNT_REG,
-		GSBI5_QUP_SW_RESET_REG,
-		GSBIn_QUP_APPS_NS_REG(5),
-		GSBIn_QUP_APPS_MD_REG(5),
-	},
-	/* GSBI6 registers for SPI interface */
-	{
-		GSBI6_SPI_CONFIG_REG,
-		GSBI6_SPI_IO_CONTROL_REG,
-		GSBI6_SPI_ERROR_FLAGS_REG,
-		GSBI6_SPI_ERROR_FLAGS_EN_REG,
-		GSBI6_GSBI_CTRL_REG_REG,
-		GSBI6_QUP_CONFIG_REG,
-		GSBI6_QUP_ERROR_FLAGS_REG,
-		GSBI6_QUP_ERROR_FLAGS_EN_REG,
-		GSBI6_QUP_OPERATIONAL_REG,
-		GSBI6_QUP_IO_MODES_REG,
-		GSBI6_QUP_STATE_REG,
-		GSBI6_QUP_INPUT_FIFOc_REG(0),
-		GSBI6_QUP_OUTPUT_FIFOc_REG(0),
-		GSBI6_QUP_MX_INPUT_COUNT_REG,
-		GSBI6_QUP_MX_OUTPUT_COUNT_REG,
-		GSBI6_QUP_SW_RESET_REG,
-		GSBIn_QUP_APPS_NS_REG(6),
-		GSBIn_QUP_APPS_MD_REG(6),
-	},
-	/* GSBI7 registers for SPI interface */
-	{
-		GSBI7_SPI_CONFIG_REG,
-		GSBI7_SPI_IO_CONTROL_REG,
-		GSBI7_SPI_ERROR_FLAGS_REG,
-		GSBI7_SPI_ERROR_FLAGS_EN_REG,
-		GSBI7_GSBI_CTRL_REG_REG,
-		GSBI7_QUP_CONFIG_REG,
-		GSBI7_QUP_ERROR_FLAGS_REG,
-		GSBI7_QUP_ERROR_FLAGS_EN_REG,
-		GSBI7_QUP_OPERATIONAL_REG,
-		GSBI7_QUP_IO_MODES_REG,
-		GSBI7_QUP_STATE_REG,
-		GSBI7_QUP_INPUT_FIFOc_REG(0),
-		GSBI7_QUP_OUTPUT_FIFOc_REG(0),
-		GSBI7_QUP_MX_INPUT_COUNT_REG,
-		GSBI7_QUP_MX_OUTPUT_COUNT_REG,
-		GSBI7_QUP_SW_RESET_REG,
-		GSBIn_QUP_APPS_NS_REG(7),
-		GSBIn_QUP_APPS_MD_REG(7),
-	}
-};
 static struct ipq_spi_slave spi_slave_pool[2];
 
 void spi_init(void)
@@ -528,17 +211,13 @@ struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs)
 	struct ipq_spi_slave *ds = NULL;
 	int i;
 
-	/*
-	 * IPQ GSBI (Generic Serial Bus Interface) supports SPI Flash
-	 * on different GSBI5, GSBI6 and GSBI7
-	 * with different number of chip selects (CS, channels):
-	*/
-	if ((bus < GSBI5_SPI) || (bus > GSBI7_SPI)
-		|| ((bus == GSBI5_SPI) && (cs > 3))
-		|| ((bus == GSBI6_SPI) && (cs > 0))
-		|| ((bus == GSBI7_SPI) && (cs > 0))) {
-		printk(BIOS_ERR, "SPI error: unsupported bus %d (Supported busses 0,1 and 2) or chipselect\n",
-			bus);
+	if ((bus < BLSP0_SPI) || (bus > BLSP1_SPI)
+		|| ((bus == BLSP0_SPI) && (cs > 2))
+		|| ((bus == BLSP1_SPI) && (cs > 0))) {
+		printk(BIOS_ERR,
+			"SPI error: unsupported bus %d (Supported busses 0, 1 and 2) "
+                        "or chipselect\n", bus);
+		return NULL;
 	}
 
 	for (i = 0; i < ARRAY_SIZE(spi_slave_pool); i++) {
@@ -555,7 +234,7 @@ struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs)
 		 * to configure this
 		 */
 		ds->freq = 10000000;
-		ds->mode = GSBI_SPI_MODE_0;
+		ds->mode = SPI_MODE3;
 		ds->allocated = 1;
 
 		return &ds->slave;
@@ -566,48 +245,43 @@ struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs)
 }
 
 /*
- * GSBIn SPI Hardware Initialisation
+ * BLSP QUPn SPI Hardware Initialisation
  */
 static int spi_hw_init(struct ipq_spi_slave *ds)
 {
 	int ret;
 
-	if (ds->initialized)
-		return 0;
+	ds->initialized = 0;
 
-	/* GSBI module configuration */
+	/* QUPn module configuration */
 	spi_reset(ds);
 
-	/* Set the GSBIn QUP state */
-	ret = config_spi_state(ds, SPI_RESET_STATE);
+	/* Set the QUPn state */
+	ret = config_spi_state(ds, QUP_STATE_RESET);
 	if (ret)
 		return ret;
 
-	/* Configure GSBI_CTRL register to set protocol_mode to SPI:011 */
-	clrsetbits_le32_i(ds->regs->gsbi_ctrl, PROTOCOL_CODE_MSK,
-					PROTOCOL_CODE_SPI);
-
 	/*
 	 * Configure Mini core to SPI core with Input Output enabled,
 	 * SPI master, N = 8 bits
 	 */
-	clrsetbits_le32_i(ds->regs->qup_config, (QUP_CONFIG_MINI_CORE_MSK |
-					       SPI_QUP_CONF_INPUT_MSK |
-					       SPI_QUP_CONF_OUTPUT_MSK |
-					       SPI_BIT_WORD_MSK),
-					      (QUP_CONFIG_MINI_CORE_SPI |
-					       SPI_QUP_CONF_NO_INPUT |
-					       SPI_QUP_CONF_NO_OUTPUT |
-					       SPI_8_BIT_WORD));
+	clrsetbits_le32(ds->regs->qup_config, QUP_CONFIG_MINI_CORE_MSK |
+						QUP_CONF_INPUT_MSK |
+						QUP_CONF_OUTPUT_MSK |
+						QUP_CONF_N_MASK,
+						QUP_CONFIG_MINI_CORE_SPI |
+						QUP_CONF_INPUT_ENA |
+						QUP_CONF_OUTPUT_ENA |
+						QUP_CONF_N_SPI_8_BIT_WORD);
 
 	/*
 	 * Configure Input first SPI protocol,
 	 * SPI master mode and no loopback
 	 */
-	clrsetbits_le32_i(ds->regs->spi_config, (LOOP_BACK_MSK |
-					       SLAVE_OPERATION_MSK),
-					      (NO_LOOP_BACK |
-					       SLAVE_OPERATION));
+	clrsetbits_le32(ds->regs->spi_config, SPI_CONFIG_LOOP_BACK_MSK |
+						SPI_CONFIG_NO_SLAVE_OPER_MSK,
+						SPI_CONFIG_NO_LOOP_BACK |
+						SPI_CONFIG_NO_SLAVE_OPER);
 
 	/*
 	 * Configure SPI IO Control Register
@@ -615,8 +289,8 @@ static int spi_hw_init(struct ipq_spi_slave *ds)
 	 * MX_CS_MODE = 0
 	 * NO_TRI_STATE = 1
 	 */
-	writel_i((CLK_ALWAYS_ON | MX_CS_MODE | NO_TRI_STATE),
-				ds->regs->io_control);
+	write32(ds->regs->io_control, SPI_IO_CTRL_CLK_ALWAYS_ON |
+					SPI_IO_CTRL_NO_TRI_STATE);
 
 	/*
 	 * Configure SPI IO Modes.
@@ -624,18 +298,21 @@ static int spi_hw_init(struct ipq_spi_slave *ds)
 	 * INPUT_MODE = Block Mode
 	 * OUTPUT MODE = Block Mode
 	 */
-	clrsetbits_le32_i(ds->regs->qup_io_modes, (OUTPUT_BIT_SHIFT_MSK |
-						 INPUT_BLOCK_MODE_MSK |
-						 OUTPUT_BLOCK_MODE_MSK),
-						(OUTPUT_BIT_SHIFT_EN |
-						 INPUT_BLOCK_MODE |
-						 OUTPUT_BLOCK_MODE));
+	clrsetbits_le32(ds->regs->qup_io_modes,
+				QUP_IO_MODES_OUTPUT_BIT_SHIFT_MSK |
+				QUP_IO_MODES_INPUT_MODE_MSK |
+				QUP_IO_MODES_OUTPUT_MODE_MSK,
+				QUP_IO_MODES_OUTPUT_BIT_SHIFT_EN |
+				QUP_IO_MODES_INPUT_BLOCK_MODE |
+				QUP_IO_MODES_OUTPUT_BLOCK_MODE);
 
 	spi_set_mode(ds, ds->mode);
 
 	/* Disable Error mask */
-	writel_i(0, ds->regs->error_flags_en);
-	writel_i(0, ds->regs->qup_error_flags_en);
+	write32(ds->regs->error_flags_en, 0);
+	write32(ds->regs->qup_error_flags_en, 0);
+
+	write32(ds->regs->qup_deassert_wait, 0);
 
 	ds->initialized = 1;
 
@@ -647,17 +324,6 @@ int spi_claim_bus(struct spi_slave *slave)
 	struct ipq_spi_slave *ds = to_ipq_spi(slave);
 	unsigned int ret;
 
-	if (ds->initialized)
-		return SUCCESS;
-
-	/* GPIO Configuration for SPI port */
-	gsbi_pin_config(ds->slave.bus, ds->slave.cs);
-
-	/* Clock configuration */
-	ret = gsbi_clock_init(ds);
-	if (ret)
-		return ret;
-
 	ret = spi_hw_init(ds);
 	if (ret)
 		return -EIO;
@@ -674,60 +340,92 @@ void spi_release_bus(struct spi_slave *slave)
 	ds->initialized = 0;
 }
 
-static int spi_xfer_tx_packet(struct ipq_spi_slave *ds,
-		const uint8_t *dout, unsigned out_bytes)
+static void write_force_cs(struct spi_slave *slave, int assert)
 {
-	int ret;
+	struct ipq_spi_slave *ds = to_ipq_spi(slave);
 
-	writel_i(out_bytes, ds->regs->qup_mx_output_count);
+	if (assert)
+		clrsetbits_le32(ds->regs->io_control,
+			SPI_IO_CTRL_FORCE_CS_MSK, SPI_IO_CTRL_FORCE_CS_EN);
+	else
+		clrsetbits_le32(ds->regs->io_control,
+			SPI_IO_CTRL_FORCE_CS_MSK, SPI_IO_CTRL_FORCE_CS_DIS);
 
-	ret = config_spi_state(ds, SPI_RUN_STATE);
-	if (ret)
-		return ret;
+	return;
+}
 
-	while (out_bytes) {
-		if (readl_i(ds->regs->qup_operational) & QUP_OUTPUT_FIFO_FULL)
-			continue;
+/*
+ * Function to write data to OUTPUT FIFO
+ */
+static void spi_write_byte(struct ipq_spi_slave *ds, unsigned char data)
+{
+	/* Wait for space in the FIFO */
+	while ((read32(ds->regs->qup_operational) & OUTPUT_FIFO_FULL))
+		udelay(1);
 
-		writel_i(*dout++, ds->regs->qup_output_fifo);
-		out_bytes--;
+	/* Write the byte of data */
+	write32(ds->regs->qup_output_fifo, data);
+}
 
-		/* Wait for output FIFO to drain. */
-		if (!out_bytes)
-			while (readl_i(ds->regs->qup_operational) &
-			       QUP_OUTPUT_FIFO_NOT_EMPTY)
-				;
-	}
+/*
+ * Function to read data from Input FIFO
+ */
+static unsigned char spi_read_byte(struct ipq_spi_slave *ds)
+{
+	/* Wait for Data in FIFO */
+	while (!(read32(ds->regs->qup_operational) & INPUT_FIFO_NOT_EMPTY))
+		udelay(1);
 
-	return config_spi_state(ds, SPI_RESET_STATE);
+	/* Read a byte of data */
+	return read32(ds->regs->qup_input_fifo) & 0xff;
 }
 
-static int spi_xfer_rx_packet(struct ipq_spi_slave *ds,
-		uint8_t *din, unsigned in_bytes)
+/*
+ * Function to check wheather Input or Output FIFO
+ * has data to be serviced
+ */
+static int check_fifo_status(void *reg_addr)
 {
-	int ret;
+	unsigned int count = TIMEOUT_CNT;
+	unsigned int status_flag;
+	unsigned int val;
 
-	writel_i(in_bytes, ds->regs->qup_mx_input_count);
-	writel_i(in_bytes, ds->regs->qup_mx_output_count);
+	do {
+		val = read32(reg_addr);
+		count--;
+		if (count == 0)
+			return -ETIMEDOUT;
+		status_flag = ((val & OUTPUT_SERVICE_FLAG) |
+					(val & INPUT_SERVICE_FLAG));
+	} while (!status_flag);
 
-	ret = config_spi_state(ds, SPI_RUN_STATE);
-	if (ret)
-		return ret;
+	return SUCCESS;
+}
 
-	/* Seed clocking */
-	writel_i(0xff, ds->regs->qup_output_fifo);
-	while (in_bytes) {
-		if (!(readl_i(ds->regs->qup_operational) &
-		      QUP_INPUT_FIFO_NOT_EMPTY))
-			continue;
-		/* Keep it clocking */
-		writel_i(0xff, ds->regs->qup_output_fifo);
+/*
+ * Function to configure Input and Output enable/disable
+ */
+static void enable_io_config(struct ipq_spi_slave *ds,
+				uint32_t write_cnt, uint32_t read_cnt)
+{
 
-		*din++ = readl_i(ds->regs->qup_input_fifo) & 0xff;
-		in_bytes--;
+	if (write_cnt) {
+		clrsetbits_le32(ds->regs->qup_config,
+				QUP_CONF_OUTPUT_MSK, QUP_CONF_OUTPUT_ENA);
+	} else {
+		clrsetbits_le32(ds->regs->qup_config,
+				QUP_CONF_OUTPUT_MSK, QUP_CONF_NO_OUTPUT);
 	}
 
-	return config_spi_state(ds, SPI_RESET_STATE);
+	if (read_cnt) {
+		clrsetbits_le32(ds->regs->qup_config,
+				QUP_CONF_INPUT_MSK, QUP_CONF_INPUT_ENA);
+	} else {
+		clrsetbits_le32(ds->regs->qup_config,
+				QUP_CONF_INPUT_MSK, QUP_CONF_NO_INPUT);
+	}
+
+	return;
 }
 
 unsigned int spi_crop_chunk(unsigned int cmd_len, unsigned int buf_len)
@@ -735,71 +433,239 @@ unsigned int spi_crop_chunk(unsigned int cmd_len, unsigned int buf_len)
 	return min(MAX_PACKET_COUNT, buf_len);
 }
 
-int spi_xfer(struct spi_slave *slave, const void *dout,
-	     unsigned out_bytes, void *din, unsigned in_bytes)
+/*
+ * Function to read bytes number of data from the Input FIFO
+ */
+static int __blsp_spi_read(struct ipq_spi_slave *ds, u8 *data_buffer,
+				unsigned int bytes)
 {
-	int ret;
-	struct ipq_spi_slave *ds = to_ipq_spi(slave);
+	uint32_t val;
+	unsigned int i;
+	unsigned int fifo_count;
+	int ret = SUCCESS;
+	int state_config;
+
+	/* Configure no of bytes to read */
+	state_config = config_spi_state(ds, QUP_STATE_RESET);
+	if (state_config)
+		return state_config;
+
+	/* Configure input and output enable */
+	enable_io_config(ds, 0, bytes);
+
+	write32(ds->regs->qup_mx_input_count, bytes);
+
+	state_config = config_spi_state(ds, QUP_STATE_RUN);
+	if (state_config)
+		return state_config;
+
+	while (bytes) {
+		ret = check_fifo_status(ds->regs->qup_operational);
+		if (ret != SUCCESS)
+			goto out;
+
+		val = read32(ds->regs->qup_operational);
+		if (val & INPUT_SERVICE_FLAG) {
+			/*
+			 * acknowledge to hw that software will
+			 * read input data
+			 */
+			val &= INPUT_SERVICE_FLAG;
+			write32(ds->regs->qup_operational, val);
+
+			fifo_count = ((bytes > SPI_INPUT_BLOCK_SIZE) ?
+					SPI_INPUT_BLOCK_SIZE : bytes);
+
+			for (i = 0; i < fifo_count; i++) {
+				*data_buffer = spi_read_byte(ds);
+				data_buffer++;
+				bytes--;
+			}
+		}
+	}
 
-	/* Assert the chip select */
-	CS_change(ds->slave.bus, ds->slave.cs, CS_ASSERT);
+out:
+	/*
+	 * Put the SPI Core back in the Reset State
+	 * to end the transfer
+	 */
+	(void)config_spi_state(ds, QUP_STATE_RESET);
+	return ret;
+}
 
-	ret = config_spi_state(ds, SPI_RESET_STATE);
-	if (ret)
-		goto out;
+static int blsp_spi_read(struct ipq_spi_slave *ds, u8 *data_buffer,
+				unsigned int bytes)
+{
+	int length, ret;
+
+	while (bytes) {
+		length = (bytes < MAX_COUNT_SIZE) ? bytes : MAX_COUNT_SIZE;
 
-	if (!out_bytes)
-		goto spi_receive;
+		ret = __blsp_spi_read(ds, data_buffer, length);
+		if (ret != SUCCESS)
+			return ret;
+
+		data_buffer += length;
+		bytes -= length;
+	}
+
+	return 0;
+}
+
+/*
+ * Function to write data to the Output FIFO
+ */
+static int __blsp_spi_write(struct ipq_spi_slave *ds, const u8 *cmd_buffer,
+				unsigned int bytes)
+{
+	uint32_t val;
+	unsigned int i;
+	unsigned int write_len = bytes;
+	unsigned int read_len = bytes;
+	unsigned int fifo_count;
+	int ret = SUCCESS;
+	int state_config;
+
+	state_config = config_spi_state(ds, QUP_STATE_RESET);
+	if (state_config)
+		return state_config;
+
+	/* No of bytes to be written in Output FIFO */
+	write32(ds->regs->qup_mx_output_count, bytes);
+	write32(ds->regs->qup_mx_input_count, bytes);
+	state_config = config_spi_state(ds, QUP_STATE_RUN);
+	if (state_config)
+		return state_config;
+
+	/* Configure input and output enable */
+	enable_io_config(ds, write_len, read_len);
 
 	/*
-	 * Let's do the write side of the transaction first. Enable output
-	 * FIFO.
+	 * read_len considered to ensure that we read the dummy data for the
+	 * write we performed. This is needed to ensure with WR-RD transaction
+	 * to get the actual data on the subsequent read cycle that happens
 	 */
-	clrsetbits_le32_i(ds->regs->qup_config, SPI_QUP_CONF_OUTPUT_MSK,
-			  SPI_QUP_CONF_OUTPUT_ENA);
+	while (write_len || read_len) {
+
+		ret = check_fifo_status(ds->regs->qup_operational);
+		if (ret != SUCCESS)
+			goto out;
+
+		val = read32(ds->regs->qup_operational);
+		if (val & OUTPUT_SERVICE_FLAG) {
+			/*
+			 * acknowledge to hw that software will write
+			 * expected output data
+			 */
+			val &= OUTPUT_SERVICE_FLAG;
+			write32(ds->regs->qup_operational, val);
+
+			if (write_len > SPI_OUTPUT_BLOCK_SIZE)
+				fifo_count = SPI_OUTPUT_BLOCK_SIZE;
+			else
+				fifo_count = write_len;
+
+			for (i = 0; i < fifo_count; i++) {
+				/* Write actual data to output FIFO */
+				spi_write_byte(ds, *cmd_buffer);
+				cmd_buffer++;
+				write_len--;
+			}
+		}
+		if (val & INPUT_SERVICE_FLAG) {
+			/*
+			 * acknowledge to hw that software
+			 * will read input data
+			 */
+			val &= INPUT_SERVICE_FLAG;
+			write32(ds->regs->qup_operational, val);
+
+			if (read_len > SPI_INPUT_BLOCK_SIZE)
+				fifo_count = SPI_INPUT_BLOCK_SIZE;
+			else
+				fifo_count = read_len;
+
+			for (i = 0; i < fifo_count; i++) {
+				/* Read dummy data for the data written */
+				(void)spi_read_byte(ds);
+
+				/* Decrement the read count after reading the
+				 * dummy data from the device. This is to make
+				 * sure we read dummy data before we write the
+				 * data to fifo
+				 */
+				read_len--;
+			}
+		}
+	}
+
+out:
+	/*
+	 * Put the SPI Core back in the Reset State
+	 * to end the transfer
+	 */
+	(void)config_spi_state(ds, QUP_STATE_RESET);
+
+	return ret;
+}
+
+static int blsp_spi_write(struct ipq_spi_slave *ds, u8 *cmd_buffer,
+				unsigned int bytes)
+{
+	int length, ret;
 
-	while (out_bytes) {
-		unsigned todo = MIN(out_bytes, MAX_PACKET_COUNT);
+	while (bytes) {
+		length = (bytes < MAX_COUNT_SIZE) ? bytes : MAX_COUNT_SIZE;
 
-		ret = spi_xfer_tx_packet(ds, dout, todo);
-		if (ret)
-			break;
+		ret = __blsp_spi_write(ds, cmd_buffer, length);
+		if (ret != SUCCESS)
+			return ret;
 
-		out_bytes -= todo;
-		dout += todo;
+		cmd_buffer += length;
+		bytes -= length;
 	}
 
-	if (ret)
-		goto out;
+	return 0;
+}
 
-spi_receive:
-	if (!in_bytes) /* Nothing to read. */
-		goto out;
+/*
+ * This function is invoked with either tx_buf or rx_buf.
+ * Calling this function with both null does a chip select change.
+ */
+int spi_xfer(struct spi_slave *slave, const void *dout,
+	     unsigned out_bytes, void *din, unsigned in_bytes)
+{
+	struct ipq_spi_slave *ds = to_ipq_spi(slave);
+	u8 *txp = (u8 *)dout;
+	u8 *rxp = (u8 *)din;
+	int ret;
 
-	/* Enable input FIFO */
-	clrsetbits_le32_i(ds->regs->qup_config, SPI_QUP_CONF_INPUT_MSK,
-			  SPI_QUP_CONF_INPUT_ENA);
+	ret = config_spi_state(ds, QUP_STATE_RESET);
+	if (ret != SUCCESS)
+		return ret;
 
-	while (in_bytes) {
-		unsigned todo = MIN(in_bytes, MAX_PACKET_COUNT);
+	write_force_cs(slave, 1);
 
-		ret = spi_xfer_rx_packet(ds, din, todo);
-		if (ret)
-			break;
+	if (dout != NULL) {
+		ret = blsp_spi_write(ds, txp, (unsigned int) out_bytes);
+		if (ret != SUCCESS)
+			goto out;
+	}
 
-		in_bytes -= todo;
-		din += todo;
+	if (din != NULL) {
+		ret = blsp_spi_read(ds, rxp, in_bytes);
+		if (ret != SUCCESS)
+			goto out;
 	}
 
 out:
-	/* Deassert CS */
-	CS_change(ds->slave.bus, ds->slave.cs, CS_DEASSERT);
+	write_force_cs(slave, 0);
 
 	/*
 	 * Put the SPI Core back in the Reset State
 	 * to end the transfer
 	 */
-	(void)config_spi_state(ds, SPI_RESET_STATE);
+	(void)config_spi_state(ds, QUP_STATE_RESET);
 
 	return ret;
 }
diff --git a/src/soc/qualcomm/ipq40xx/uart.c b/src/soc/qualcomm/ipq40xx/uart.c
index 0f8add5501..e08a440129 100644
--- a/src/soc/qualcomm/ipq40xx/uart.c
+++ b/src/soc/qualcomm/ipq40xx/uart.c
@@ -35,7 +35,7 @@
 #include <delay.h>
 #include <gpio.h>
 #include <soc/clock.h>
-#include <soc/gsbi.h>
+#include <soc/blsp.h>
 #include <soc/ipq_uart.h>
 #include <stdint.h>
 #include <stdlib.h>