1 files changed, 41 insertions, 2 deletions

diff --git a/drivers/spi/spi-sh-hspi.c b/drivers/spi/spi-sh-hspi.c
index 796c077ef439..47c4beee8a0e 100644
--- a/drivers/spi/spi-sh-hspi.c
+++ b/drivers/spi/spi-sh-hspi.c
@@ -68,6 +68,16 @@ static u32 hspi_read(struct hspi_priv *hspi, int reg)
 	return ioread32(hspi->addr + reg);
 }
 
+static void hspi_bit_set(struct hspi_priv *hspi, int reg, u32 mask, u32 set)
+{
+	u32 val = hspi_read(hspi, reg);
+
+	val &= ~mask;
+	val |= set & mask;
+
+	hspi_write(hspi, reg, val);
+}
+
 /*
  *		transfer function
  */
@@ -105,6 +115,13 @@ static int hspi_unprepare_transfer(struct spi_master *master)
 	return 0;
 }
 
+#define hspi_hw_cs_enable(hspi)		hspi_hw_cs_ctrl(hspi, 0)
+#define hspi_hw_cs_disable(hspi)	hspi_hw_cs_ctrl(hspi, 1)
+static void hspi_hw_cs_ctrl(struct hspi_priv *hspi, int hi)
+{
+	hspi_bit_set(hspi, SPSCR, (1 << 6), (hi) << 6);
+}
+
 static void hspi_hw_setup(struct hspi_priv *hspi,
 			  struct spi_message *msg,
 			  struct spi_transfer *t)
@@ -155,7 +172,7 @@ static void hspi_hw_setup(struct hspi_priv *hspi,
 
 	hspi_write(hspi, SPCR, spcr);
 	hspi_write(hspi, SPSR, 0x0);
-	hspi_write(hspi, SPSCR, 0x1);	/* master mode */
+	hspi_write(hspi, SPSCR, 0x21);	/* master mode / CS control */
 }
 
 static int hspi_transfer_one_message(struct spi_master *master,
@@ -166,12 +183,21 @@ static int hspi_transfer_one_message(struct spi_master *master,
 	u32 tx;
 	u32 rx;
 	int ret, i;
+	unsigned int cs_change;
+	const int nsecs = 50;
 
 	dev_dbg(hspi->dev, "%s\n", __func__);
 
+	cs_change = 1;
 	ret = 0;
 	list_for_each_entry(t, &msg->transfers, transfer_list) {
-		hspi_hw_setup(hspi, msg, t);
+
+		if (cs_change) {
+			hspi_hw_setup(hspi, msg, t);
+			hspi_hw_cs_enable(hspi);
+			ndelay(nsecs);
+		}
+		cs_change = t->cs_change;
 
 		for (i = 0; i < t->len; i++) {
 
@@ -198,9 +224,22 @@ static int hspi_transfer_one_message(struct spi_master *master,
 		}
 
 		msg->actual_length += t->len;
+
+		if (t->delay_usecs)
+			udelay(t->delay_usecs);
+
+		if (cs_change) {
+			ndelay(nsecs);
+			hspi_hw_cs_disable(hspi);
+			ndelay(nsecs);
+		}
 	}
 
 	msg->status = ret;
+	if (!cs_change) {
+		ndelay(nsecs);
+		hspi_hw_cs_disable(hspi);
+	}
 	spi_finalize_current_message(master);
 
 	return ret;