[PATCH] spi: spi-synquacer: fix set_cs handling

[Masahisa Kojima <masahisa.kojima@xxxxxxxxxx>]

DMSTOP register must be updated when the slave is
selected/desected.
RXFIFO needs to be cleaned to safely deselect the device.

Fixes: b0823ee35cf9b ("spi: Add spi driver for Socionext SynQuacer platform")
Signed-off-by: Masahisa Kojima <masahisa.kojima@xxxxxxxxxx>
---
 drivers/spi/spi-synquacer.c | 35 ++++++++++++++++++++++++-----------
 1 file changed, 24 insertions(+), 11 deletions(-)

diff --git a/drivers/spi/spi-synquacer.c b/drivers/spi/spi-synquacer.c
index f99abd85c50a..3905d1e1dea6 100644
--- a/drivers/spi/spi-synquacer.c
+++ b/drivers/spi/spi-synquacer.c
@@ -454,22 +454,12 @@ static int synquacer_spi_transfer_one(struct spi_master *master,
 	}
 
 	if (xfer->rx_buf) {
-		u32 buf[SYNQUACER_HSSPI_FIFO_DEPTH];
-
 		val = SYNQUACER_HSSPI_RXE_FIFO_MORE_THAN_THRESHOLD |
 		      SYNQUACER_HSSPI_RXE_SLAVE_RELEASED;
 		writel(val, sspi->regs + SYNQUACER_HSSPI_REG_RXE);
 		status = wait_for_completion_timeout(&sspi->transfer_done,
 			msecs_to_jiffies(SYNQUACER_HSSPI_TRANSFER_TMOUT_MSEC));
 		writel(0, sspi->regs + SYNQUACER_HSSPI_REG_RXE);
-
-		/* stop RX and clean RXFIFO */
-		val = readl(sspi->regs + SYNQUACER_HSSPI_REG_DMSTART);
-		val |= SYNQUACER_HSSPI_DMSTOP_STOP;
-		writel(val, sspi->regs + SYNQUACER_HSSPI_REG_DMSTART);
-		sspi->rx_buf = buf;
-		sspi->rx_words = SYNQUACER_HSSPI_FIFO_DEPTH;
-		read_fifo(sspi);
 	}
 
 	if (status < 0) {
@@ -485,12 +475,35 @@ static void synquacer_spi_set_cs(struct spi_device *spi, bool enable)
 {
 	struct synquacer_spi *sspi = spi_master_get_devdata(spi->master);
 	u32 val;
+	bool selected;
 
 	val = readl(sspi->regs + SYNQUACER_HSSPI_REG_DMSTART);
 	val &= ~(SYNQUACER_HSSPI_DMPSEL_CS_MASK <<
 		 SYNQUACER_HSSPI_DMPSEL_CS_SHIFT);
 	val |= spi->chip_select << SYNQUACER_HSSPI_DMPSEL_CS_SHIFT;
-	writel(val, sspi->regs + SYNQUACER_HSSPI_REG_DMSTART);
+
+	if (spi->mode & SPI_CS_HIGH)
+		selected = enable;
+	else
+		selected = !enable;
+
+	if (selected) {
+		val &= ~SYNQUACER_HSSPI_DMSTOP_STOP;
+		writel_relaxed(val, sspi->regs + SYNQUACER_HSSPI_REG_DMSTART);
+	} else {
+		u32 buf[SYNQUACER_HSSPI_FIFO_DEPTH];
+
+		/*
+		 * Stop transaction and cleanup RXFIFO to safely deselect
+		 * the device.
+		 */
+		val |= SYNQUACER_HSSPI_DMSTOP_STOP;
+		writel_relaxed(val, sspi->regs + SYNQUACER_HSSPI_REG_DMSTART);
+
+		sspi->rx_buf = buf;
+		sspi->rx_words = SYNQUACER_HSSPI_FIFO_DEPTH;
+		read_fifo(sspi);
+	}
 }
 
 static int synquacer_spi_wait_status_update(struct synquacer_spi *sspi,
-- 
2.14.2