1 files changed, 41 insertions, 16 deletions

diff --git a/drivers/spi/spi-bcm2835aux.c b/drivers/spi/spi-bcm2835aux.c
index e1b2fec1db63..a6e25c6c66ca 100644
--- a/drivers/spi/spi-bcm2835aux.c
+++ b/drivers/spi/spi-bcm2835aux.c
@@ -218,9 +218,9 @@ static irqreturn_t bcm2835aux_spi_interrupt(int irq, void *dev_id)
                         BCM2835_AUX_SPI_CNTL1_IDLE);
         }
 
-        /* and if rx_len is 0 then wake up completion and disable spi */
+        /* and if rx_len is 0 then disable interrupts and wake up completion */
         if (!bs->rx_len) {
-                bcm2835aux_spi_reset_hw(bs);
+                bcm2835aux_wr(bs, BCM2835_AUX_SPI_CNTL1, bs->cntl[1]);
                 complete(&master->xfer_completion);
         }
 
@@ -313,9 +313,6 @@ static int bcm2835aux_spi_transfer_one_poll(struct spi_master *master,
                 }
         }
 
-        /* Transfer complete - reset SPI HW */
-        bcm2835aux_spi_reset_hw(bs);
-
         /* and return without waiting for completion */
         return 0;
 }
@@ -336,10 +333,6 @@ static int bcm2835aux_spi_transfer_one(struct spi_master *master,
          * resulting (potentially) in more interrupts when transferring
          * more than 12 bytes
          */
-        bs->cntl[0] = BCM2835_AUX_SPI_CNTL0_ENABLE |
-                      BCM2835_AUX_SPI_CNTL0_VAR_WIDTH |
-                      BCM2835_AUX_SPI_CNTL0_MSBF_OUT;
-        bs->cntl[1] = BCM2835_AUX_SPI_CNTL1_MSBF_IN;
 
         /* set clock */
         spi_hz = tfr->speed_hz;
@@ -354,17 +347,13 @@ static int bcm2835aux_spi_transfer_one(struct spi_master *master,
         } else { /* the slowest we can go */
                 speed = BCM2835_AUX_SPI_CNTL0_SPEED_MAX;
         }
+        /* mask out old speed from previous spi_transfer */
+        bs->cntl[0] &= ~(BCM2835_AUX_SPI_CNTL0_SPEED);
+        /* set the new speed */
         bs->cntl[0] |= speed << BCM2835_AUX_SPI_CNTL0_SPEED_SHIFT;
 
         spi_used_hz = clk_hz / (2 * (speed + 1));
 
-        /* handle all the modes */
-        if (spi->mode & SPI_CPOL)
-                bs->cntl[0] |= BCM2835_AUX_SPI_CNTL0_CPOL;
-        if (spi->mode & SPI_CPHA)
-                bs->cntl[0] |= BCM2835_AUX_SPI_CNTL0_CPHA_OUT |
-                               BCM2835_AUX_SPI_CNTL0_CPHA_IN;
-
         /* set transmit buffers and length */
         bs->tx_buf = tfr->tx_buf;
         bs->rx_buf = tfr->rx_buf;
@@ -388,6 +377,40 @@ static int bcm2835aux_spi_transfer_one(struct spi_master *master,
         return bcm2835aux_spi_transfer_one_irq(master, spi, tfr);
 }
 
+static int bcm2835aux_spi_prepare_message(struct spi_master *master,
+                                          struct spi_message *msg)
+{
+        struct spi_device *spi = msg->spi;
+        struct bcm2835aux_spi *bs = spi_master_get_devdata(master);
+
+        bs->cntl[0] = BCM2835_AUX_SPI_CNTL0_ENABLE |
+                      BCM2835_AUX_SPI_CNTL0_VAR_WIDTH |
+                      BCM2835_AUX_SPI_CNTL0_MSBF_OUT;
+        bs->cntl[1] = BCM2835_AUX_SPI_CNTL1_MSBF_IN;
+
+        /* handle all the modes */
+        if (spi->mode & SPI_CPOL)
+                bs->cntl[0] |= BCM2835_AUX_SPI_CNTL0_CPOL;
+        if (spi->mode & SPI_CPHA)
+                bs->cntl[0] |= BCM2835_AUX_SPI_CNTL0_CPHA_OUT |
+                               BCM2835_AUX_SPI_CNTL0_CPHA_IN;
+
+        bcm2835aux_wr(bs, BCM2835_AUX_SPI_CNTL1, bs->cntl[1]);
+        bcm2835aux_wr(bs, BCM2835_AUX_SPI_CNTL0, bs->cntl[0]);
+
+        return 0;
+}
+
+static int bcm2835aux_spi_unprepare_message(struct spi_master *master,
+                                            struct spi_message *msg)
+{
+        struct bcm2835aux_spi *bs = spi_master_get_devdata(master);
+
+        bcm2835aux_spi_reset_hw(bs);
+
+        return 0;
+}
+
 static void bcm2835aux_spi_handle_err(struct spi_master *master,
                                       struct spi_message *msg)
 {
@@ -416,6 +439,8 @@ static int bcm2835aux_spi_probe(struct platform_device *pdev)
         master->num_chipselect = -1;
         master->transfer_one = bcm2835aux_spi_transfer_one;
         master->handle_err = bcm2835aux_spi_handle_err;
+        master->prepare_message = bcm2835aux_spi_prepare_message;
+        master->unprepare_message = bcm2835aux_spi_unprepare_message;
         master->dev.of_node = pdev->dev.of_node;
 
         bs = spi_master_get_devdata(master);