[PATCH v4 13/15] spi: spi-zynqmp-gqspi: Add stacked memories support in GQSPI driver

[Amit Kumar Mahapatra]

GQSPI supports two chip select CS0 & CS1. Update the driver to
assert/de-assert the appropriate chip select as per the bits set in
qspi->cs_index_mask.

Signed-off-by: Amit Kumar Mahapatra <amit.kumar-mahapa...@amd.com>
---
 drivers/spi/spi-zynqmp-gqspi.c | 21 +++++++++++++--------
 1 file changed, 13 insertions(+), 8 deletions(-)

diff --git a/drivers/spi/spi-zynqmp-gqspi.c b/drivers/spi/spi-zynqmp-gqspi.c
index 319cdd5a0bdc..4759f704bf5c 100644
--- a/drivers/spi/spi-zynqmp-gqspi.c
+++ b/drivers/spi/spi-zynqmp-gqspi.c
@@ -156,6 +156,9 @@
 #define GQSPI_FREQ_100MHZ      100000000
 #define GQSPI_FREQ_150MHZ      150000000
 
+#define GQSPI_SELECT_LOWER_CS  BIT(0)
+#define GQSPI_SELECT_UPPER_CS  BIT(1)
+
 #define SPI_AUTOSUSPEND_TIMEOUT                3000
 enum mode_type {GQSPI_MODE_IO, GQSPI_MODE_DMA};
 
@@ -467,15 +470,17 @@ static void zynqmp_qspi_chipselect(struct spi_device 
*qspi, bool is_high)
 
        genfifoentry |= GQSPI_GENFIFO_MODE_SPI;
 
+       if (qspi->cs_index_mask & GQSPI_SELECT_UPPER_CS) {
+               zynqmp_gqspi_selectslave(xqspi,
+                                        GQSPI_SELECT_FLASH_CS_UPPER,
+                                        GQSPI_SELECT_FLASH_BUS_LOWER);
+       } else if (qspi->cs_index_mask & GQSPI_SELECT_LOWER_CS) {
+               zynqmp_gqspi_selectslave(xqspi,
+                                        GQSPI_SELECT_FLASH_CS_LOWER,
+                                        GQSPI_SELECT_FLASH_BUS_LOWER);
+       }
+       genfifoentry |= xqspi->genfifobus;
        if (!is_high) {
-               if (!spi_get_chipselect(qspi, 0)) {
-                       xqspi->genfifobus = GQSPI_GENFIFO_BUS_LOWER;
-                       xqspi->genfifocs = GQSPI_GENFIFO_CS_LOWER;
-               } else {
-                       xqspi->genfifobus = GQSPI_GENFIFO_BUS_UPPER;
-                       xqspi->genfifocs = GQSPI_GENFIFO_CS_UPPER;
-               }
-               genfifoentry |= xqspi->genfifobus;
                genfifoentry |= xqspi->genfifocs;
                genfifoentry |= GQSPI_GENFIFO_CS_SETUP;
        } else {
-- 
2.25.1