spi: zynq-qspi: Dual stacked fixes

Add configuration of U_PAGE/L_PAGE before asserting chip select.
(This was removed earlier when subsystem workqueue, set_cs and transfer_one
hooks were used)

Signed-off-by: Harini Katakam <harinik@xilinx.com>
Signed-off-by: Michal Simek <michal.simek@xilinx.com>

diff --git a/drivers/spi/spi-zynq-qspi.c b/drivers/spi/spi-zynq-qspi.c
index 50f5525e80ec7d16fbdfe6682288bff2dac722e0..a64cd77edec92414225fa48076cb0bb3b48e215d 100644 (file)
--- a/drivers/spi/spi-zynq-qspi.c
+++ b/drivers/spi/spi-zynq-qspi.c
@@ -311,9 +311,24 @@ static void zynq_qspi_chipselect(struct spi_device *qspi, bool is_high)
 {
        struct zynq_qspi *xqspi = spi_master_get_devdata(qspi->master);
        u32 config_reg;
+#ifdef CONFIG_SPI_ZYNQ_QSPI_DUAL_STACKED
+       u32 lqspi_cfg_reg;
+#endif
 
        config_reg = zynq_qspi_read(xqspi, ZYNQ_QSPI_CONFIG_OFFSET);
 
+       /* Select upper/lower page before asserting CS */
+#ifdef CONFIG_SPI_ZYNQ_QSPI_DUAL_STACKED
+               lqspi_cfg_reg = zynq_qspi_read(xqspi,
+                                              ZYNQ_QSPI_LINEAR_CFG_OFFSET);
+               if (qspi->master->flags & SPI_MASTER_U_PAGE)
+                       lqspi_cfg_reg |= ZYNQ_QSPI_LCFG_U_PAGE_MASK;
+               else
+                       lqspi_cfg_reg &= ~ZYNQ_QSPI_LCFG_U_PAGE_MASK;
+               zynq_qspi_write(xqspi, ZYNQ_QSPI_LINEAR_CFG_OFFSET,
+                               lqspi_cfg_reg);
+#endif
+
        if (is_high) {
                /* Deselect the slave */
                config_reg |= ZYNQ_QSPI_CONFIG_SSCTRL_MASK;