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;