1 files changed, 27 insertions, 6 deletions

diff --git a/drivers/spi/spi_bfin5xx.c b/drivers/spi/spi_bfin5xx.c
index 3736c3596a41..6150a8cfb82a 100644
--- a/drivers/spi/spi_bfin5xx.c
+++ b/drivers/spi/spi_bfin5xx.c
@@ -188,8 +188,7 @@ static void bfin_spi_cs_active(struct driver_data *drv_data, struct chip_data *c
 	if (likely(chip->chip_select_num)) {
 		u16 flag = read_FLAG(drv_data);
 
-		flag |= chip->flag;
-		flag &= ~(chip->flag << 8);
+		flag &= ~chip->flag;
 
 		write_FLAG(drv_data, flag);
 	} else {
@@ -202,8 +201,7 @@ static void bfin_spi_cs_deactive(struct driver_data *drv_data, struct chip_data
 	if (likely(chip->chip_select_num)) {
 		u16 flag = read_FLAG(drv_data);
 
-		flag &= ~chip->flag;
-		flag |= (chip->flag << 8);
+		flag |= chip->flag;
 
 		write_FLAG(drv_data, flag);
 	} else {
@@ -215,6 +213,25 @@ static void bfin_spi_cs_deactive(struct driver_data *drv_data, struct chip_data
 		udelay(chip->cs_chg_udelay);
 }
 
+/* enable or disable the pin muxed by GPIO and SPI CS to work as SPI CS */
+static inline void bfin_spi_cs_enable(struct driver_data *drv_data, struct chip_data *chip)
+{
+	u16 flag = read_FLAG(drv_data);
+
+	flag |= (chip->flag >> 8);
+
+	write_FLAG(drv_data, flag);
+}
+
+static inline void bfin_spi_cs_disable(struct driver_data *drv_data, struct chip_data *chip)
+{
+	u16 flag = read_FLAG(drv_data);
+
+	flag &= ~(chip->flag >> 8);
+
+	write_FLAG(drv_data, flag);
+}
+
 /* stop controller and re-config current chip*/
 static void bfin_spi_restore_state(struct driver_data *drv_data)
 {
@@ -1169,7 +1186,7 @@ static int bfin_spi_setup(struct spi_device *spi)
 	 * SPI_BAUD, not the real baudrate
 	 */
 	chip->baud = hz_to_spi_baud(spi->max_speed_hz);
-	chip->flag = 1 << (spi->chip_select);
+	chip->flag = (1 << (spi->chip_select)) << 8;
 	chip->chip_select_num = spi->chip_select;
 
 	switch (chip->bits_per_word) {
@@ -1268,6 +1285,7 @@ static int bfin_spi_setup(struct spi_device *spi)
 		}
 	}
 
+	bfin_spi_cs_enable(drv_data, chip);
 	bfin_spi_cs_deactive(drv_data, chip);
 
 	return 0;
@@ -1299,14 +1317,17 @@ static int bfin_spi_setup(struct spi_device *spi)
 static void bfin_spi_cleanup(struct spi_device *spi)
 {
 	struct chip_data *chip = spi_get_ctldata(spi);
+	struct driver_data *drv_data = spi_master_get_devdata(spi->master);
 
 	if (!chip)
 		return;
 
 	if ((chip->chip_select_num > 0)
-		&& (chip->chip_select_num <= spi->master->num_chipselect))
+		&& (chip->chip_select_num <= spi->master->num_chipselect)) {
 		peripheral_free(ssel[spi->master->bus_num]
 					[chip->chip_select_num-1]);
+		bfin_spi_cs_disable(drv_data, chip);
+	}
 
 	if (chip->chip_select_num == 0)
 		gpio_free(chip->cs_gpio);