diff --git a/drivers/spi/fsl_espi.c b/drivers/spi/fsl_espi.c
index 7c84582..ae0fe58 100644
--- a/drivers/spi/fsl_espi.c
+++ b/drivers/spi/fsl_espi.c
@@ -15,8 +15,10 @@
 
 struct fsl_spi_slave {
 	struct spi_slave slave;
+	ccsr_espi_t	*espi;
 	unsigned int	div16;
 	unsigned int	pm;
+	int		tx_timeout;
 	unsigned int	mode;
 	size_t		cmd_len;
 	u8		cmd_buf[16];
@@ -25,11 +27,17 @@ struct fsl_spi_slave {
 };
 
 #define to_fsl_spi_slave(s) container_of(s, struct fsl_spi_slave, slave)
+#define US_PER_SECOND		1000000UL
 
 #define ESPI_MAX_CS_NUM		4
+#define ESPI_FIFO_WIDTH_BIT	32
 
 #define ESPI_EV_RNE		(1 << 9)
 #define ESPI_EV_TNF		(1 << 8)
+#define ESPI_EV_DON		(1 << 14)
+#define ESPI_EV_TXE		(1 << 15)
+#define ESPI_EV_RFCNT_SHIFT	24
+#define ESPI_EV_RFCNT_MASK	(0x3f << ESPI_EV_RFCNT_SHIFT)
 
 #define ESPI_MODE_EN		(1 << 31)	/* Enable interface */
 #define ESPI_MODE_TXTHR(x)	((x) << 8)	/* Tx FIFO threshold */
@@ -61,6 +69,7 @@ struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
 	struct fsl_spi_slave *fsl;
 	sys_info_t sysinfo;
 	unsigned long spibrg = 0;
+	unsigned long spi_freq = 0;
 	unsigned char pm = 0;
 
 	if (!spi_cs_is_valid(bus, cs))
@@ -70,6 +79,7 @@ struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
 	if (!fsl)
 		return NULL;
 
+	fsl->espi = (void *)(CONFIG_SYS_MPC85xx_ESPI_ADDR);
 	fsl->mode = mode;
 	fsl->max_transfer_length = ESPI_MAX_DATA_TRANSFER_LEN;
 
@@ -91,6 +101,15 @@ struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
 		pm--;
 	fsl->pm = pm;
 
+	if (fsl->div16)
+		spi_freq = spibrg / ((pm + 1) * 2 * 16);
+	else
+		spi_freq = spibrg / ((pm + 1) * 2);
+
+	/* set tx_timeout to 10 times of one espi FIFO entry go out */
+	fsl->tx_timeout = DIV_ROUND_UP((US_PER_SECOND * ESPI_FIFO_WIDTH_BIT
+				* 10), spi_freq);
+
 	return &fsl->slave;
 }
 
@@ -108,7 +127,7 @@ void spi_init(void)
 int spi_claim_bus(struct spi_slave *slave)
 {
 	struct fsl_spi_slave *fsl = to_fsl_spi_slave(slave);
-	ccsr_espi_t *espi = (void *)(CONFIG_SYS_MPC85xx_ESPI_ADDR);
+	ccsr_espi_t *espi = fsl->espi;
 	unsigned char pm = fsl->pm;
 	unsigned int cs = slave->cs;
 	unsigned int mode =  fsl->mode;
@@ -161,24 +180,86 @@ void spi_release_bus(struct spi_slave *slave)
 
 }
 
+static void fsl_espi_tx(struct fsl_spi_slave *fsl, const void *dout)
+{
+	ccsr_espi_t *espi = fsl->espi;
+	unsigned int tmpdout, event;
+	int tmp_tx_timeout;
+
+	if (dout)
+		tmpdout = *(u32 *)dout;
+	else
+		tmpdout = 0;
+
+	out_be32(&espi->tx, tmpdout);
+	out_be32(&espi->event, ESPI_EV_TNF);
+	debug("***spi_xfer:...%08x written\n", tmpdout);
+
+	tmp_tx_timeout = fsl->tx_timeout;
+	/* Wait for eSPI transmit to go out */
+	while (tmp_tx_timeout--) {
+		event = in_be32(&espi->event);
+		if (event & ESPI_EV_DON || event & ESPI_EV_TXE) {
+			out_be32(&espi->event, ESPI_EV_TXE);
+			break;
+		}
+		udelay(1);
+	}
+
+	if (tmp_tx_timeout < 0)
+		debug("***spi_xfer:...Tx timeout! event = %08x\n", event);
+}
+
+static int fsl_espi_rx(struct fsl_spi_slave *fsl, void *din, unsigned int bytes)
+{
+	ccsr_espi_t *espi = fsl->espi;
+	unsigned int tmpdin, rx_times;
+	unsigned char *buf, *p_cursor;
+
+	if (bytes <= 0)
+		return 0;
+
+	rx_times = DIV_ROUND_UP(bytes, 4);
+	buf = (unsigned char *)malloc(4 * rx_times);
+	if (!buf) {
+		debug("SF: Failed to malloc memory.\n");
+		return -1;
+	}
+	p_cursor = buf;
+	while (rx_times--) {
+		tmpdin = in_be32(&espi->rx);
+		debug("***spi_xfer:...%08x readed\n", tmpdin);
+		*(u32 *)p_cursor = tmpdin;
+		p_cursor += 4;
+	}
+
+	if (din)
+		memcpy(din, buf, bytes);
+
+	free(buf);
+	out_be32(&espi->event, ESPI_EV_RNE);
+
+	return bytes;
+}
+
 int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *data_out,
 		void *data_in, unsigned long flags)
 {
 	struct fsl_spi_slave *fsl = to_fsl_spi_slave(slave);
-	ccsr_espi_t *espi = (void *)(CONFIG_SYS_MPC85xx_ESPI_ADDR);
-	unsigned int tmpdout, tmpdin, event;
+	ccsr_espi_t *espi = fsl->espi;
+	unsigned int event, rx_bytes;
 	const void *dout = NULL;
 	void *din = NULL;
 	int len = 0;
 	int num_blks, num_chunks, max_tran_len, tran_len;
 	int num_bytes;
-	unsigned char *ch;
 	unsigned char *buffer = NULL;
 	size_t buf_len;
 	u8 *cmd_buf = fsl->cmd_buf;
 	size_t cmd_len = fsl->cmd_len;
 	size_t data_len = bitlen / 8;
 	size_t rx_offset = 0;
+	int rf_cnt;
 
 	max_tran_len = fsl->max_transfer_length;
 	switch (flags) {
@@ -217,9 +298,8 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *data_out,
 		break;
 	}
 
-	debug("spi_xfer: slave %u:%u dout %08X(%p) din %08X(%p) len %u\n",
-	      slave->bus, slave->cs, *(uint *) dout,
-	      dout, *(uint *) din, din, len);
+	debug("spi_xfer: data_out %08X(%p) data_in %08X(%p) len %u\n",
+	      *(uint *)data_out, data_out, *(uint *)data_in, data_in, len);
 
 	num_chunks = DIV_ROUND_UP(data_len, max_tran_len);
 	while (num_chunks--) {
@@ -235,41 +315,34 @@ int spi_xfer(struct spi_slave *slave, unsigned int bitlen, const void *data_out,
 		/* Clear all eSPI events */
 		out_be32(&espi->event , 0xffffffff);
 		/* handle data in 32-bit chunks */
-		while (num_blks--) {
-
+		while (num_blks) {
 			event = in_be32(&espi->event);
 			if (event & ESPI_EV_TNF) {
-				tmpdout = *(u32 *)dout;
-
+				fsl_espi_tx(fsl, dout);
 				/* Set up the next iteration */
 				if (len > 4) {
 					len -= 4;
 					dout += 4;
 				}
-
-				out_be32(&espi->tx, tmpdout);
-				out_be32(&espi->event, ESPI_EV_TNF);
-				debug("***spi_xfer:...%08x written\n", tmpdout);
 			}
 
-			/* Wait for eSPI transmit to get out */
-			udelay(80);
-
 			event = in_be32(&espi->event);
 			if (event & ESPI_EV_RNE) {
-				tmpdin = in_be32(&espi->rx);
-				if (num_blks == 0 && num_bytes != 0) {
-					ch = (unsigned char *)&tmpdin;
-					while (num_bytes--)
-						*(unsigned char *)din++ = *ch++;
-				} else {
-					*(u32 *) din = tmpdin;
-					din += 4;
+				rf_cnt = ((event & ESPI_EV_RFCNT_MASK)
+						>> ESPI_EV_RFCNT_SHIFT);
+				if (rf_cnt >= 4)
+					rx_bytes = 4;
+				else if (num_blks == 1 && rf_cnt == num_bytes)
+					rx_bytes = num_bytes;
+				else
+					continue;
+				if (fsl_espi_rx(fsl, din, rx_bytes)
+						== rx_bytes) {
+					num_blks--;
+					if (din)
+						din = (unsigned char *)din
+							+ rx_bytes;
 				}
-
-				out_be32(&espi->event, in_be32(&espi->event)
-						| ESPI_EV_RNE);
-				debug("***spi_xfer:...%08x readed\n", tmpdin);
 			}
 		}
 		if (data_in) {
@@ -295,7 +368,7 @@ int spi_cs_is_valid(unsigned int bus, unsigned int cs)
 void spi_cs_activate(struct spi_slave *slave)
 {
 	struct fsl_spi_slave *fsl = to_fsl_spi_slave(slave);
-	ccsr_espi_t *espi = (void *)(CONFIG_SYS_MPC85xx_ESPI_ADDR);
+	ccsr_espi_t *espi = fsl->espi;
 	unsigned int com = 0;
 	size_t data_len = fsl->data_len;
 
@@ -307,7 +380,8 @@ void spi_cs_activate(struct spi_slave *slave)
 
 void spi_cs_deactivate(struct spi_slave *slave)
 {
-	ccsr_espi_t *espi = (void *)(CONFIG_SYS_MPC85xx_ESPI_ADDR);
+	struct fsl_spi_slave *fsl = to_fsl_spi_slave(slave);
+	ccsr_espi_t *espi = fsl->espi;
 
 	/* clear the RXCNT and TXCNT */
 	out_be32(&espi->mode, in_be32(&espi->mode) & (~ESPI_MODE_EN));