ipq8064: modify SPI controller driver to work in coreboot

A typical SPI operation consists of two phases - command and data
transfers. Command transfer is always from the host to the chip (i.e.
is going in the 'write' direction), data transfer could be either read
or write.

We don't want the receive FIFO to be operating while the command phase
is in progress. A simple way to keep the receive FIFO shut down is to
not to enable it until the command phase is completed.

Selective control of the receive FIFO allows to consolidate the
receive and transmit functions in a single spi_xfer() function, as it
happens in other SPI controller drivers.

The FIFO FULL and FIFO NOT EMPTY conditions are used to decide if the
next byte can be written or received, respectively. While data is
being received the 0xFF bytes are transmitted per each received byte,
to keep the SPI bus clocking.

The data structure describing the three GSBI ports is moved from the
.h file into .c file. A version of the clrsetbits macro is added to
work with integer addresses instead of pointers.

BUG=chrome-os-partner:27784
TEST=not yet, but with the res of the changes the bootblock loads and
     starts the rombase section successfully.

Original-Change-Id: I78cd0054f1a8f5e1d7213f38ef8de31486238aba
Original-Signed-off-by: Vadim Bendebury <vbendeb@chromium.org>
Original-Reviewed-on: https://chromium-review.googlesource.com/197779
Original-Reviewed-by: Stefan Reinauer <reinauer@chromium.org>
Original-Reviewed-by: David Hendricks <dhendrix@chromium.org>
(cherry picked from commit c101ae306d182bbe14935ee139a25968388d745a)
Signed-off-by: Marc Jones <marc.jones@se-eng.com>

Change-Id: I7f3fd0524ec6c10008ff514e8a8f1d14a700732f
Reviewed-on: http://review.coreboot.org/7983
Reviewed-by: David Hendricks <dhendrix@chromium.org>
Tested-by: build bot (Jenkins)

4 files changed, 255 insertions, 487 deletions

diff --git a/src/soc/qualcomm/ipq806x/Kconfig b/src/soc/qualcomm/ipq806x/Kconfig
index 92ef3a1d10..09e1c1eae0 100644
--- a/src/soc/qualcomm/ipq806x/Kconfig
+++ b/src/soc/qualcomm/ipq806x/Kconfig
@@ -7,6 +7,7 @@ config SOC_QC_IPQ806X
 	select ARM_LPAE
 	select BOOTBLOCK_CONSOLE
 	select HAVE_UART_SPECIAL
+	select SPI_ATOMIC_SEQUENCING
 
 if SOC_QC_IPQ806X
 
diff --git a/src/soc/qualcomm/ipq806x/include/iomap.h b/src/soc/qualcomm/ipq806x/include/iomap.h
index 8a4155ce6b..be523a65cd 100644
--- a/src/soc/qualcomm/ipq806x/include/iomap.h
+++ b/src/soc/qualcomm/ipq806x/include/iomap.h
@@ -36,6 +36,9 @@
 #ifndef __SOC_QUALCOMM_IPQ806X_IOMAP_H_
 #define __SOC_QUALCOMM_IPQ806X_IOMAP_H_
 
+#include <arch/io.h>
+#include <cdp.h>
+
 /* Typecast to allow integers being passed as address
    This needs to be included because vendor code is not compliant with our
    macros for read/write. Hence, special macros for readl_i and writel_i are
@@ -43,8 +46,8 @@
  */
 #define readl_i(a)           read32((const void *)(a))
 #define writel_i(v,a)        write32(v,(void *)a)
-#include <arch/io.h>
-#include <cdp.h>
+#define clrsetbits_le32_i(addr, clear, set)  \
+	clrsetbits_le32(((void *)(addr)), (clear), (set))
 
 #define MSM_CLK_CTL_BASE    0x00900000
 
diff --git a/src/soc/qualcomm/ipq806x/include/spi.h b/src/soc/qualcomm/ipq806x/include/spi.h
index 2020043c6c..48073fadd0 100644
--- a/src/soc/qualcomm/ipq806x/include/spi.h
+++ b/src/soc/qualcomm/ipq806x/include/spi.h
@@ -3,8 +3,10 @@
  * Copyright (c) 2012 The Linux Foundation. All rights reserved.
  */
 
-#ifndef _IPQ_SPI_H_
-#define _IPQ_SPI_H_
+#ifndef _IPQ806X_SPI_H_
+#define _IPQ806X_SPI_H_
+
+#include <spi_flash.h>
 
 #define QUP5_BASE	0x1a280000
 #define QUP6_BASE	0x16580000
@@ -70,6 +72,14 @@
 #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) \
@@ -144,10 +154,14 @@
 #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 QUP_STATE_RUN_STATE			0x1
+#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)
@@ -196,6 +210,7 @@
 #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 SPI_INPUT_BLOCK_SIZE			4
 #define SPI_OUTPUT_BLOCK_SIZE			4
 #define GSBI5_SPI_CLK				21
@@ -226,6 +241,7 @@
 
 #define SPI_RESET_STATE				0
 #define SPI_RUN_STATE				1
+#define SPI_PAUSE_STATE				3
 #define SPI_CORE_RESET				0
 #define SPI_CORE_RUNNING			1
 #define GSBI_SPI_MODE_0				0
@@ -257,79 +273,13 @@ struct gsbi_spi {
 	unsigned int     qup_md_reg;
 };
 
-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)
-	}
-};
-
 struct ipq_spi_slave {
 	struct spi_slave slave;
 	const struct gsbi_spi *regs;
-	unsigned int core_state;
 	unsigned int mode;
 	unsigned int initialized;
 	unsigned long freq;
+	int allocated;
 };
 
 static inline struct ipq_spi_slave *to_ipq_spi(struct spi_slave *slave)
diff --git a/src/soc/qualcomm/ipq806x/spi.c b/src/soc/qualcomm/ipq806x/spi.c
index 45ecc0e079..52d11c19c4 100644
--- a/src/soc/qualcomm/ipq806x/spi.c
+++ b/src/soc/qualcomm/ipq806x/spi.c
@@ -2,14 +2,13 @@
  * Copyright (c) 2012 The Linux Foundation. All rights reserved.
  */
 
-#include <common.h>
+#include <arch/io.h>
+#include <delay.h>
+#include <stdlib.h>
+#include <string.h>
 #include <spi.h>
-#include <malloc.h>
-#include <asm/io.h>
-#include <asm/errno.h>
-#include "ipq_spi.h"
-#include <asm/arch-ipq806x/gpio.h>
-#include <asm/arch-ipq806x/iomap.h>
+#include <gpio.h>
+#include <iomap.h>
 
 #define SUCCESS		0
 
@@ -28,6 +27,11 @@
 #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)
 
 /*
@@ -138,14 +142,14 @@ static unsigned int qup_apps_clk_state[NUM_PORTS] = {
 static int check_bit_state(uint32_t reg_addr, int bit_num, int val, int us_delay)
 {
 	unsigned int count = TIMEOUT_CNT;
-	unsigned int bit_val = ((readl(reg_addr) >> bit_num) & 0x01);
+	unsigned int bit_val = ((readl_i(reg_addr) >> bit_num) & 0x01);
 
 	while (bit_val != val) {
 		count--;
 		if (count == 0)
 			return -ETIMEDOUT;
 		udelay(us_delay);
-		bit_val = ((readl(reg_addr) >> bit_num) & 0x01);
+		bit_val = ((readl_i(reg_addr) >> bit_num) & 0x01);
 	}
 
 	return SUCCESS;
@@ -168,7 +172,8 @@ static int check_qup_state_valid(struct ipq_spi_slave *ds)
 static int config_spi_state(struct ipq_spi_slave *ds, unsigned int state)
 {
 	uint32_t val;
-	int ret = SUCCESS;
+	int ret;
+	uint32_t new_state;
 
 	ret = check_qup_state_valid(ds);
 	if (ret != SUCCESS)
@@ -176,32 +181,28 @@ static int config_spi_state(struct ipq_spi_slave *ds, unsigned int state)
 
 	switch (state) {
 	case SPI_RUN_STATE:
-		/* Set the state to RUN */
-		val = ((readl(ds->regs->qup_state) & ~QUP_STATE_MASK)
-					| QUP_STATE_RUN_STATE);
-		writel(val, ds->regs->qup_state);
-		ret = check_qup_state_valid(ds);
-		if (ret != SUCCESS)
-			return ret;
-		ds->core_state = SPI_CORE_RUNNING;
+		new_state = QUP_STATE_RUN_STATE;
 		break;
+
 	case SPI_RESET_STATE:
-		/* Set the state to RESET */
-		val = ((readl(ds->regs->qup_state) & ~QUP_STATE_MASK)
-					| QUP_STATE_RESET_STATE);
-		writel(val, ds->regs->qup_state);
-		ret = check_qup_state_valid(ds);
-		if (ret != SUCCESS)
-			return ret;
-		ds->core_state = SPI_CORE_RESET;
+		new_state = QUP_STATE_RESET_STATE;
 		break;
-	default:
-		printf("err: unsupported GSBI SPI state : %d\n", state);
-		ret = -EINVAL;
+
+	case SPI_PAUSE_STATE:
+		new_state = QUP_STATE_PAUSE_STATE;
 		break;
+
+	default:
+		printk(BIOS_ERR,
+		       "err: unsupported GSBI SPI state : %d\n", state);
+		return -EINVAL;
 	}
 
-	return ret;
+	/* 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);
 }
 
 /*
@@ -231,21 +232,22 @@ static void spi_set_mode(struct ipq_spi_slave *ds, unsigned int mode)
 		input_first_mode = 0;
 		break;
 	default:
-		printf("err : unsupported spi mode : %d\n", mode);
+		printk(BIOS_ERR,
+		       "err : unsupported spi mode : %d\n", mode);
 		return;
 	}
 
-	val = readl(ds->regs->spi_config);
+	val = readl_i(ds->regs->spi_config);
 	val |= input_first_mode;
-	writel(val, ds->regs->spi_config);
+	writel_i(val, ds->regs->spi_config);
 
-	val = readl(ds->regs->io_control);
+	val = readl_i(ds->regs->io_control);
 	if (clk_idle_state)
 		val |= SPI_IO_CONTROL_CLOCK_IDLE_HIGH;
 	else
 		val &= ~SPI_IO_CONTROL_CLOCK_IDLE_HIGH;
 
-	writel(val, ds->regs->io_control);
+	writel_i(val, ds->regs->io_control);
 }
 
 /*
@@ -253,9 +255,6 @@ static void spi_set_mode(struct ipq_spi_slave *ds, unsigned int mode)
  */
 static int check_hclk_state(unsigned int core_num, int enable)
 {
-	if (clk_is_dummy())
-		return 0;
-
 	return check_bit_state(CLK_HALT_CFPB_STATEB_REG,
 		hclk_state[core_num], enable, 5);
 }
@@ -265,9 +264,6 @@ static int check_hclk_state(unsigned int core_num, int enable)
  */
 static int check_qup_clk_state(unsigned int core_num, int enable)
 {
-	if (clk_is_dummy())
-		return 0;
-
 	return check_bit_state(CLK_HALT_CFPB_STATEB_REG,
 		qup_apps_clk_state[core_num], enable, 5);
 }
@@ -279,12 +275,12 @@ static void CS_change(int port_num, int cs_num, int enable)
 {
 	unsigned int cs_gpio = cs_gpio_array[port_num][cs_num];
 	uint32_t addr = GPIO_IN_OUT_ADDR(cs_gpio);
-	uint32_t val = readl(addr);
+	uint32_t val = readl_i(addr);
 
 	val &= (~(1 << GPIO_OUT));
 	if (!enable)
 		val |= (1 << GPIO_OUT);
-	writel(val, addr);
+	writel_i(val, addr);
 }
 
 /*
@@ -295,7 +291,7 @@ 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(GSBIn_RESET_REG(GSBI_IDX_TO_GSBI(port_num)),
+	clrsetbits_le32_i(GSBIn_RESET_REG(GSBI_IDX_TO_GSBI(port_num)),
 			GSBI1_RESET_MSK, GSBI1_RESET);
 
 	/*
@@ -336,76 +332,79 @@ static int gsbi_clock_init(struct ipq_spi_slave *ds)
 	int ret;
 
 	/* Hold the GSBIn (core_num) core in reset */
-	clrsetbits_le32(GSBIn_RESET_REG(GSBI_IDX_TO_GSBI(ds->slave.bus)),
+	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(ds->regs->qup_ns_reg, QUP_CLK_BRANCH_ENA_MSK,
+	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) {
-		printf("QUP Clock Halt For GSBI%d failed!\n", ds->slave.bus);
+		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(ds->regs->qup_ns_reg, (MNCNTR_MSK | MNCNTR_RST_MSK),
+	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(ds->regs->qup_ns_reg, CLK_ROOT_ENA_MSK, CLK_ROOT_DIS);
+	clrsetbits_le32_i(ds->regs->qup_ns_reg, CLK_ROOT_ENA_MSK, CLK_ROOT_DIS);
 
-	clrsetbits_le32(ds->regs->qup_ns_reg, GSBIn_PLL_SRC_MSK,
+	clrsetbits_le32_i(ds->regs->qup_ns_reg, GSBIn_PLL_SRC_MSK,
 					GSBIn_PLL_SRC_PLL8);
-	clrsetbits_le32(ds->regs->qup_ns_reg, GSBIn_PRE_DIV_SEL_MSK,
+	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(ds->regs->qup_md_reg, GSBIn_M_VAL_MSK,
+	clrsetbits_le32_i(ds->regs->qup_md_reg, GSBIn_M_VAL_MSK,
 						(0x01 << GSBI_M_VAL_SHFT));
-	clrsetbits_le32(ds->regs->qup_md_reg, GSBIn_D_VAL_MSK,
+	clrsetbits_le32_i(ds->regs->qup_md_reg, GSBIn_D_VAL_MSK,
 						(0xF7 << GSBI_D_VAL_SHFT));
-	clrsetbits_le32(ds->regs->qup_ns_reg, GSBIn_N_VAL_MSK,
+	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(ds->regs->qup_ns_reg, MNCNTR_MODE_MSK,
+	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(ds->regs->qup_ns_reg, MNCNTR_RST_MSK, MNCNTR_RST_DIS);
-	clrsetbits_le32(ds->regs->qup_ns_reg, MNCNTR_MSK, MNCNTR_EN);
+	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(ds->regs->qup_ns_reg, CLK_ROOT_ENA_MSK, CLK_ROOT_ENA);
+	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(ds->regs->qup_ns_reg, QUP_CLK_BRANCH_ENA_MSK,
+	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) {
-		printf("QUP Clock Enable For GSBI%d"
+		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(GSBIn_HCLK_CTL_REG(GSBI_IDX_TO_GSBI(ds->slave.bus)),
+	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) {
-		printf("HCLK Enable For GSBI%d failed!\n", ds->slave.bus);
+		printk(BIOS_ERR,
+		       "HCLK Enable For GSBI%d failed!\n", ds->slave.bus);
 		return ret;
 	}
 
 	/* Release GSBIn (core_num) core from reset */
-	clrsetbits_le32(GSBIn_RESET_REG(GSBI_IDX_TO_GSBI(ds->slave.bus)),
+	clrsetbits_le32_i(GSBIn_RESET_REG(GSBI_IDX_TO_GSBI(ds->slave.bus)),
 						GSBI1_RESET_MSK, 0);
 	udelay(50);
 
@@ -417,26 +416,87 @@ static int gsbi_clock_init(struct ipq_spi_slave *ds)
  */
 static void spi_reset(struct ipq_spi_slave *ds)
 {
-	writel(0x1, ds->regs->qup_sw_reset);
+	writel_i(0x1, ds->regs->qup_sw_reset);
 	udelay(5);
 }
 
+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()
 {
-	/* do nothing */
-
+	/* just in case */
+	memset(spi_slave_pool, 0, sizeof(spi_slave_pool));
 }
 
-struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
-				unsigned int max_hz, unsigned int mode)
+struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs)
 {
-	struct ipq_spi_slave *ds;
-
-	ds = malloc(sizeof(struct ipq_spi_slave));
-	if (!ds) {
-		printf("SPI error: malloc of SPI structure failed\n");
-		return NULL;
-	}
+	struct ipq_spi_slave *ds = NULL;
+	int i;
 
 	/*
 	 * IPQ GSBI (Generic Serial Bus Interface) supports SPI Flash
@@ -447,44 +507,34 @@ struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
 		|| ((bus == GSBI5_SPI) && (cs > 3))
 		|| ((bus == GSBI6_SPI) && (cs > 0))
 		|| ((bus == GSBI7_SPI) && (cs > 0))) {
-		printf("SPI error: unsupported bus %d "
+		printk(BIOS_ERR, "SPI error: unsupported bus %d "
 			"(Supported busses 0,1 and 2) or chipselect\n", bus);
-		goto err;
-	}
-	ds->slave.bus	= bus;
-	ds->slave.cs	= cs;
-
-	ds->regs	= &spi_reg[bus];
-
-	/* TODO For different clock frequency */
-	if (max_hz > MSM_GSBI_MAX_FREQ) {
-		printf("SPI error: unsupported frequency %d Hz "
-			"Max frequency is %d Hz\n", max_hz, MSM_GSBI_MAX_FREQ);
-		goto err;
 	}
-	ds->freq = max_hz;
 
-	if (mode > GSBI_SPI_MODE_3) {
-		printf("SPI error: unsupported SPI mode %d\n", mode);
-		goto err;
+	for (i = 0; i < ARRAY_SIZE(spi_slave_pool); i++) {
+		if (spi_slave_pool[i].allocated)
+			continue;
+		ds = spi_slave_pool + i;
+		ds->slave.bus	= bus;
+		ds->slave.cs	= cs;
+		ds->regs	= &spi_reg[bus];
+
+		/*
+		 * TODO(vbendeb):
+		 * hardcoded frequency and mode - we might need to find a way
+		 * to configure this
+		 */
+		ds->freq = 10000000;
+		ds->mode = GSBI_SPI_MODE_0;
+		ds->allocated = 1;
+
+		return &ds->slave;
 	}
-	ds->mode = mode;
 
-	return &ds->slave;
-
-err:
-	free(ds);
+	printk(BIOS_ERR, "SPI error: all %d pools busy\n", i);
 	return NULL;
 }
 
-void spi_free_slave(struct spi_slave *slave)
-{
-	struct ipq_spi_slave *ds = to_ipq_spi(slave);
-
-	if (ds != NULL)
-		free(ds);
-}
-
 /*
  * GSBIn SPI Hardware Initialisation
  */
@@ -492,7 +542,8 @@ static int spi_hw_init(struct ipq_spi_slave *ds)
 {
 	int ret;
 
-	ds->initialized = 0;
+	if (ds->initialized)
+		return 0;
 
 	/* GSBI module configuration */
 	spi_reset(ds);
@@ -503,27 +554,27 @@ static int spi_hw_init(struct ipq_spi_slave *ds)
 		return ret;
 
 	/* Configure GSBI_CTRL register to set protocol_mode to SPI:011 */
-	clrsetbits_le32(ds->regs->gsbi_ctrl, PROTOCOL_CODE_MSK,
+	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(ds->regs->qup_config, (QUP_CONFIG_MINI_CORE_MSK |
+	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_INPUT_ENA |
-					       SPI_QUP_CONF_OUTPUT_ENA |
+					       SPI_QUP_CONF_NO_INPUT |
+					       SPI_QUP_CONF_NO_OUTPUT |
 					       SPI_8_BIT_WORD));
 
 	/*
 	 * Configure Input first SPI protocol,
 	 * SPI master mode and no loopback
 	 */
-	clrsetbits_le32(ds->regs->spi_config, (LOOP_BACK_MSK |
+	clrsetbits_le32_i(ds->regs->spi_config, (LOOP_BACK_MSK |
 					       SLAVE_OPERATION_MSK),
 					      (NO_LOOP_BACK |
 					       SLAVE_OPERATION));
@@ -534,7 +585,7 @@ static int spi_hw_init(struct ipq_spi_slave *ds)
 	 * MX_CS_MODE = 0
 	 * NO_TRI_STATE = 1
 	 */
-	writel((CLK_ALWAYS_ON | MX_CS_MODE | NO_TRI_STATE),
+	writel_i((CLK_ALWAYS_ON | MX_CS_MODE | NO_TRI_STATE),
 				ds->regs->io_control);
 
 	/*
@@ -543,7 +594,7 @@ static int spi_hw_init(struct ipq_spi_slave *ds)
 	 * INPUT_MODE = Block Mode
 	 * OUTPUT MODE = Block Mode
 	 */
-	clrsetbits_le32(ds->regs->qup_io_modes, (OUTPUT_BIT_SHIFT_MSK |
+	clrsetbits_le32_i(ds->regs->qup_io_modes, (OUTPUT_BIT_SHIFT_MSK |
 						 INPUT_BLOCK_MODE_MSK |
 						 OUTPUT_BLOCK_MODE_MSK),
 						(OUTPUT_BIT_SHIFT_EN |
@@ -553,8 +604,8 @@ static int spi_hw_init(struct ipq_spi_slave *ds)
 	spi_set_mode(ds, ds->mode);
 
 	/* Disable Error mask */
-	writel(0, ds->regs->error_flags_en);
-	writel(0, ds->regs->qup_error_flags_en);
+	writel_i(0, ds->regs->error_flags_en);
+	writel_i(0, ds->regs->qup_error_flags_en);
 
 	ds->initialized = 1;
 
@@ -566,6 +617,9 @@ 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);
 
@@ -590,276 +644,87 @@ void spi_release_bus(struct spi_slave *slave)
 	ds->initialized = 0;
 }
 
-/* Drain input fifo
- * If input fifo is not empty drain the input FIFO. When the
- * input fifo is drained make sure that the output fifo is also
- * empty and break when the input fifo is completely drained.
- */
-static void flush_fifos(struct ipq_spi_slave *ds)
+int spi_xfer(struct spi_slave *slave, const void *dout,
+	     unsigned out_bytes, void *din, unsigned in_bytes)
 {
-	unsigned int fifo_data;
-
-	while (1) {
-		if (readl(ds->regs->qup_operational) &
-			QUP_DATA_AVAILABLE_FOR_READ) {
-			fifo_data = readl(ds->regs->qup_input_fifo);
-		} else {
-			if (!(readl(ds->regs->qup_operational) &
-				QUP_OUTPUT_FIFO_NOT_EMPTY)) {
-				if (!(readl(ds->regs->qup_operational) &
-					QUP_DATA_AVAILABLE_FOR_READ))
-					break;
-			}
-		}
-	}
+	int ret;
+	uint8_t* dbuf;
+	const uint8_t* dobuf;
+	struct ipq_spi_slave *ds = to_ipq_spi(slave);
 
-	(void)fifo_data;
-}
+	/* Assert the chip select */
+	CS_change(ds->slave.bus, ds->slave.cs, CS_ASSERT);
 
-/*
- * 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 ((readl(ds->regs->qup_operational) & QUP_OUTPUT_FIFO_FULL))
-		udelay(1);
+	ret = config_spi_state(ds, SPI_RESET_STATE);
+	if (ret)
+		goto out;
 
-	/* Write the byte of data */
-	writel(data, ds->regs->qup_output_fifo);
-}
+	if (!out_bytes)
+		goto spi_receive;
 
-/*
- * Function to read data from Input FIFO
- */
-static unsigned char spi_read_byte(struct ipq_spi_slave *ds)
-{
-	/* Wait for Data in FIFO */
-	while (!(readl(ds->regs->qup_operational) &
-			QUP_DATA_AVAILABLE_FOR_READ)) {
-		udelay(1);
-	}
+	/*
+	 * Let's do the write side of the transaction first. Enable output
+	 * FIFO.
+	 */
+	clrsetbits_le32_i(ds->regs->qup_config, SPI_QUP_CONF_OUTPUT_MSK,
+			  SPI_QUP_CONF_OUTPUT_ENA);
 
-	/* Read a byte of data */
-	return readl(ds->regs->qup_input_fifo) & 0xff;
-}
+	writel_i(out_bytes, ds->regs->qup_mx_output_count);
 
-/*
- * Function to check wheather Input or Output FIFO
- * has data to be serviced
- */
-static int check_fifo_status(uint32_t reg_addr)
-{
-	unsigned int count = TIMEOUT_CNT;
-	unsigned int status_flag;
-	unsigned int val;
+	ret = config_spi_state(ds, SPI_RUN_STATE);
+	if (ret)
+		goto out;
 
-	do {
-		val = readl(reg_addr);
-		count--;
-		if (count == 0)
-			return -ETIMEDOUT;
-		status_flag = ((val & OUTPUT_SERVICE_FLAG) | (val & INPUT_SERVICE_FLAG));
-	} while (!status_flag);
+	dobuf = dout; /* Alias to make it possible to use pointer autoinc. */
 
-	return SUCCESS;
-}
+	while (out_bytes) {
+		if (readl_i(ds->regs->qup_operational) & QUP_OUTPUT_FIFO_FULL)
+			continue;
 
-/*
- * Function to read bytes number of data from the Input FIFO
- */
-static int gsbi_spi_read(struct ipq_spi_slave *ds, u8 *data_buffer,
-				unsigned int bytes, unsigned long flags)
-{
-	uint32_t val;
-	unsigned int i;
-	unsigned int read_bytes = bytes;
-	unsigned int fifo_count;
-	int ret = SUCCESS;
-	int state_config;
-
-	if (flags & SPI_XFER_BEGIN) {
-		/* Assert chip select */
-		CS_change(ds->slave.bus, ds->slave.cs, CS_ASSERT);
-	}
-
-	/* Configure no of bytes to read */
-	state_config = config_spi_state(ds, SPI_RESET_STATE);
-	if (state_config)
-		return state_config;
-
-	writel(bytes, ds->regs->qup_mx_output_count);
-	writel(bytes, ds->regs->qup_mx_input_count);
-
-	state_config = config_spi_state(ds, SPI_RUN_STATE);
-	if (state_config)
-		return state_config;
-
-	while (read_bytes) {
-
-		ret = check_fifo_status(ds->regs->qup_operational);
-		if (ret != SUCCESS)
-			goto out;
-
-		val = readl(ds->regs->qup_operational);
-		if (val & INPUT_SERVICE_FLAG) {
-			/*
-			 * acknowledge to hw that software will
-			 * read input data
-			 */
-			val &= INPUT_SERVICE_FLAG;
-			writel(val, ds->regs->qup_operational);
-
-			fifo_count = ((read_bytes > SPI_INPUT_BLOCK_SIZE) ?
-					SPI_INPUT_BLOCK_SIZE : read_bytes);
-
-			for (i = 0; i < fifo_count; i++) {
-				*data_buffer = spi_read_byte(ds);
-				data_buffer++;
-				read_bytes--;
-			}
-		}
+		writel_i(*dobuf++, ds->regs->qup_output_fifo);
+		out_bytes--;
 
-		if (val & OUTPUT_SERVICE_FLAG) {
-			/*
-			 * acknowledge to hw that software will
-			 * write output data
-			 */
-			val &= OUTPUT_SERVICE_FLAG;
-			writel(val, ds->regs->qup_operational);
-
-			fifo_count = ((read_bytes > SPI_OUTPUT_BLOCK_SIZE) ?
-					SPI_OUTPUT_BLOCK_SIZE : read_bytes);
-
-			for (i = 0; i < fifo_count; i++) {
-				/*
-				 * Write dummy data byte for the device
-				 * to shift in actual data. Most of the SPI devices
-				 * accepts dummy data value as 0. In case of any
-				 * other value change DUMMY_DATA_VAL.
-				 */
-				spi_write_byte(ds, DUMMY_DATA_VAL);
-			}
-		}
+		/* Wait for output FIFO to drain. */
+		if (!out_bytes)
+			while (readl_i(ds->regs->qup_operational) &
+			       QUP_OUTPUT_FIFO_NOT_EMPTY)
+				;
 	}
 
-	if (flags & SPI_XFER_END) {
-		flush_fifos(ds);
+	ret = config_spi_state(ds, SPI_RESET_STATE);
+	if (ret)
 		goto out;
-	}
 
-	return ret;
-
-out:
-	/* Deassert CS */
-	CS_change(ds->slave.bus, ds->slave.cs, CS_DEASSERT);
-
-	/*
-	 * Put the SPI Core back in the Reset State
-	 * to end the transfer
-	 */
-	(void)config_spi_state(ds, SPI_RESET_STATE);
-
-	return ret;
-
-}
-
-/*
- * Function to write data to the Output FIFO
- */
-static int gsbi_spi_write(struct ipq_spi_slave *ds, const u8 *cmd_buffer,
-				unsigned int bytes, unsigned long flags)
-{
-	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;
-
-	if (flags & SPI_XFER_BEGIN) {
-		/* Select the chip select */
-		CS_change(ds->slave.bus, ds->slave.cs, CS_ASSERT);
-	}
+spi_receive:
+	if (!in_bytes) /* Nothing to read. */
+		goto out;
 
-	state_config = config_spi_state(ds, SPI_RESET_STATE);
-	if (state_config)
-		return state_config;
+	/* Enable input FIFO */
+	clrsetbits_le32_i(ds->regs->qup_config, SPI_QUP_CONF_INPUT_MSK,
+			  SPI_QUP_CONF_INPUT_ENA);
 
-	/* No of bytes to be written in Output FIFO */
-	writel(bytes, ds->regs->qup_mx_output_count);
-	writel(bytes, ds->regs->qup_mx_input_count);
-	state_config = config_spi_state(ds, SPI_RUN_STATE);
-	if (state_config)
-		return state_config;
+	writel_i(in_bytes, ds->regs->qup_mx_input_count);
+	writel_i(in_bytes, ds->regs->qup_mx_output_count);
 
-	/*
-	 * 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
-	 */
-	while (write_len || read_len) {
-
-		ret = check_fifo_status(ds->regs->qup_operational);
-		if (ret != SUCCESS)
-			goto out;
-
-		val = readl(ds->regs->qup_operational);
-		if (val & OUTPUT_SERVICE_FLAG) {
-			/*
-			 * acknowledge to hw that software will write
-			 * expected output data
-			 */
-			val &= OUTPUT_SERVICE_FLAG;
-			writel(val, ds->regs->qup_operational);
-
-			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;
-			writel(val, ds->regs->qup_operational);
-
-			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 write 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--;
-			}
-		}
-	}
-
-	if (flags & SPI_XFER_END) {
-		flush_fifos(ds);
+	ret = config_spi_state(ds, SPI_RUN_STATE);
+	if (ret)
 		goto out;
-	}
 
-	return ret;
+	/* Seed clocking */
+	writel_i(0xff, ds->regs->qup_output_fifo);
+	dbuf = din;  /* Alias for pointer autoincrement again. */
+	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);
+
+		*dbuf++ = readl_i(ds->regs->qup_input_fifo) & 0xff;
+		in_bytes--;
+	}
 
-out:
+ out:
 	/* Deassert CS */
 	CS_change(ds->slave.bus, ds->slave.cs, CS_DEASSERT);
 
@@ -871,54 +736,3 @@ out:
 
 	return ret;
 }
-
-/*
- * 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, unsigned int bitlen,
-		const void *dout, void *din, unsigned long flags)
-{
-	struct ipq_spi_slave *ds = to_ipq_spi(slave);
-	unsigned int len;
-	const u8 *txp = dout;
-	u8 *rxp = din;
-	int ret;
-
-	if (bitlen & 0x07) {
-		printf("err : Invalid bit length");
-		return -EINVAL;
-	}
-
-	len = bitlen >> 3;
-
-	if (dout != NULL) {
-		ret = gsbi_spi_write(ds, txp, len, flags);
-		if (ret != SUCCESS)
-			return ret;
-	}
-
-	if (din != NULL)
-		return gsbi_spi_read(ds, rxp, len, flags);
-
-	if ((din == NULL) && (dout == NULL))
-		/* To handle only when chip select change is needed */
-		ret = gsbi_spi_write(ds, NULL, 0, flags);
-
-	return ret;
-}
-
-int spi_cs_is_valid(unsigned int bus, unsigned int cs)
-{
-	return 1;
-}
-
-void spi_cs_activate(struct spi_slave *slave)
-{
-
-}
-
-void spi_cs_deactivate(struct spi_slave *slave)
-{
-
-}