1 files changed, 24 insertions, 32 deletions

diff --git a/drivers/spi/spi_mpc8xxx.c b/drivers/spi/spi_mpc8xxx.c
index 518671b8afe5..4b119eaf4e6b 100644
--- a/drivers/spi/spi_mpc8xxx.c
+++ b/drivers/spi/spi_mpc8xxx.c
@@ -155,6 +155,26 @@ MPC83XX_SPI_TX_BUF(u8)
 MPC83XX_SPI_TX_BUF(u16)
 MPC83XX_SPI_TX_BUF(u32)
 
+static void mpc8xxx_spi_change_mode(struct spi_device *spi)
+{
+        struct mpc8xxx_spi *mspi = spi_master_get_devdata(spi->master);
+        struct spi_mpc8xxx_cs *cs = spi->controller_state;
+        __be32 __iomem *mode = &mspi->base->mode;
+        unsigned long flags;
+
+        if (cs->hw_mode == mpc8xxx_spi_read_reg(mode))
+                return;
+
+        /* Turn off IRQs locally to minimize time that SPI is disabled. */
+        local_irq_save(flags);
+
+        /* Turn off SPI unit prior changing mode */
+        mpc8xxx_spi_write_reg(mode, cs->hw_mode & ~SPMODE_ENABLE);
+        mpc8xxx_spi_write_reg(mode, cs->hw_mode);
+
+        local_irq_restore(flags);
+}
+
 static void mpc8xxx_spi_chipselect(struct spi_device *spi, int value)
 {
         struct mpc8xxx_spi *mpc8xxx_spi = spi_master_get_devdata(spi->master);
@@ -168,27 +188,13 @@ static void mpc8xxx_spi_chipselect(struct spi_device *spi, int value)
         }
 
         if (value == BITBANG_CS_ACTIVE) {
-                u32 regval = mpc8xxx_spi_read_reg(&mpc8xxx_spi->base->mode);
-
                 mpc8xxx_spi->rx_shift = cs->rx_shift;
                 mpc8xxx_spi->tx_shift = cs->tx_shift;
                 mpc8xxx_spi->get_rx = cs->get_rx;
                 mpc8xxx_spi->get_tx = cs->get_tx;
 
-                if (cs->hw_mode != regval) {
-                        unsigned long flags;
-                        __be32 __iomem *mode = &mpc8xxx_spi->base->mode;
-
-                        regval = cs->hw_mode;
-                        /* Turn off IRQs locally to minimize time that
-                         * SPI is disabled
-                         */
-                        local_irq_save(flags);
-                        /* Turn off SPI unit prior changing mode */
-                        mpc8xxx_spi_write_reg(mode, regval & ~SPMODE_ENABLE);
-                        mpc8xxx_spi_write_reg(mode, regval);
-                        local_irq_restore(flags);
-                }
+                mpc8xxx_spi_change_mode(spi);
+
                 if (pdata->cs_control)
                         pdata->cs_control(spi, pol);
         }
@@ -198,7 +204,6 @@ static
 int mpc8xxx_spi_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 {
         struct mpc8xxx_spi *mpc8xxx_spi;
-        u32 regval;
         u8 bits_per_word, pm;
         u32 hz;
         struct spi_mpc8xxx_cs   *cs = spi->controller_state;
@@ -286,21 +291,8 @@ int mpc8xxx_spi_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
                 pm--;
 
         cs->hw_mode |= SPMODE_PM(pm);
-        regval =  mpc8xxx_spi_read_reg(&mpc8xxx_spi->base->mode);
-        if (cs->hw_mode != regval) {
-                unsigned long flags;
-                __be32 __iomem *mode = &mpc8xxx_spi->base->mode;
-
-                regval = cs->hw_mode;
-                /* Turn off IRQs locally to minimize time
-                 * that SPI is disabled
-                 */
-                local_irq_save(flags);
-                /* Turn off SPI unit prior changing mode */
-                mpc8xxx_spi_write_reg(mode, regval & ~SPMODE_ENABLE);
-                mpc8xxx_spi_write_reg(mode, regval);
-                local_irq_restore(flags);
-        }
+
+        mpc8xxx_spi_change_mode(spi);
         return 0;
 }