These changes are the raw update to linux-4.4.6-rt14. Kernel sources

are taken from kernel.org, and rt patch from the rt wiki download page.

During the rebasing, the following patch collided:

Force tick interrupt and get rid of softirq magic(I70131fb85).

Collisions have been removed because its logic was found on the
source already.

Change-Id: I7f57a4081d9deaa0d9ccfc41a6c8daccdee3b769
Signed-off-by: José Pekkarinen <jose.pekkarinen@nokia.com>

7 files changed, 294 insertions, 260 deletions

diff --git a/kernel/drivers/staging/rts5208/ms.c b/kernel/drivers/staging/rts5208/ms.c
index ee818b0dc..cdaa1aba5 100644
--- a/kernel/drivers/staging/rts5208/ms.c
+++ b/kernel/drivers/staging/rts5208/ms.c
@@ -1121,11 +1121,10 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 
 #ifdef SUPPORT_MSXC
 		if ((buf[cur_addr_off + 8] == 0x10) ||
-			(buf[cur_addr_off + 8] == 0x13))
+			(buf[cur_addr_off + 8] == 0x13)) {
 #else
-		if (buf[cur_addr_off + 8] == 0x10)
+		if (buf[cur_addr_off + 8] == 0x10) {
 #endif
-		{
 			sys_info_addr = ((u32)buf[cur_addr_off + 0] << 24) |
 				((u32)buf[cur_addr_off + 1] << 16) |
 				((u32)buf[cur_addr_off + 2] << 8) |
diff --git a/kernel/drivers/staging/rts5208/rtsx.c b/kernel/drivers/staging/rts5208/rtsx.c
index d64b6ed9c..1fe8e3e0a 100644
--- a/kernel/drivers/staging/rts5208/rtsx.c
+++ b/kernel/drivers/staging/rts5208/rtsx.c
@@ -230,7 +230,6 @@ static struct scsi_host_template rtsx_host_template = {
 
 	/* queue commands only, only one command per LUN */
 	.can_queue =			1,
-	.cmd_per_lun =			1,
 
 	/* unknown initiator id */
 	.this_id =			-1,
@@ -537,7 +536,7 @@ static int rtsx_polling_thread(void *__dev)
 	for (;;) {
 
 		set_current_state(TASK_INTERRUPTIBLE);
-		schedule_timeout(POLLING_INTERVAL);
+		schedule_timeout(msecs_to_jiffies(POLLING_INTERVAL));
 
 		/* lock the device pointers */
 		mutex_lock(&(dev->dev_mutex));
@@ -648,8 +647,6 @@ static void rtsx_release_resources(struct rtsx_dev *dev)
 	wait_timeout(200);
 
 	if (dev->rtsx_resv_buf) {
-		dma_free_coherent(&(dev->pci->dev), RTSX_RESV_BUF_LEN,
-				dev->rtsx_resv_buf, dev->rtsx_resv_buf_addr);
 		dev->chip->host_cmds_ptr = NULL;
 		dev->chip->host_sg_tbl_ptr = NULL;
 	}
@@ -919,8 +916,8 @@ static int rtsx_probe(struct pci_dev *pci,
 	dev_info(&pci->dev, "Original address: 0x%lx, remapped address: 0x%lx\n",
 		 (unsigned long)(dev->addr), (unsigned long)(dev->remap_addr));
 
-	dev->rtsx_resv_buf = dma_alloc_coherent(&(pci->dev), RTSX_RESV_BUF_LEN,
-			&(dev->rtsx_resv_buf_addr), GFP_KERNEL);
+	dev->rtsx_resv_buf = dmam_alloc_coherent(&pci->dev, RTSX_RESV_BUF_LEN,
+			&dev->rtsx_resv_buf_addr, GFP_KERNEL);
 	if (dev->rtsx_resv_buf == NULL) {
 		dev_err(&pci->dev, "alloc dma buffer fail\n");
 		err = -ENXIO;
diff --git a/kernel/drivers/staging/rts5208/rtsx.h b/kernel/drivers/staging/rts5208/rtsx.h
index 262441bcf..1396263e1 100644
--- a/kernel/drivers/staging/rts5208/rtsx.h
+++ b/kernel/drivers/staging/rts5208/rtsx.h
@@ -37,7 +37,7 @@
 #include <linux/cdrom.h>
 #include <linux/workqueue.h>
 #include <linux/timer.h>
-#include <linux/time.h>
+#include <linux/time64.h>
 
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
@@ -48,9 +48,6 @@
 
 #define CR_DRIVER_NAME		"rts5208"
 
-#define pci_get_bus_and_slot(bus, devfn)	\
-	pci_get_domain_bus_and_slot(0, (bus), (devfn))
-
 /*
  * macros for easy use
  */
@@ -148,21 +145,24 @@ static inline struct rtsx_dev *host_to_rtsx(struct Scsi_Host *host)
 
 static inline void get_current_time(u8 *timeval_buf, int buf_len)
 {
-	struct timeval tv;
+	struct timespec64 ts64;
+	u32 tv_usec;
 
 	if (!timeval_buf || (buf_len < 8))
 		return;
 
-	do_gettimeofday(&tv);
+	getnstimeofday64(&ts64);
+
+	tv_usec = ts64.tv_nsec/NSEC_PER_USEC;
 
-	timeval_buf[0] = (u8)(tv.tv_sec >> 24);
-	timeval_buf[1] = (u8)(tv.tv_sec >> 16);
-	timeval_buf[2] = (u8)(tv.tv_sec >> 8);
-	timeval_buf[3] = (u8)(tv.tv_sec);
-	timeval_buf[4] = (u8)(tv.tv_usec >> 24);
-	timeval_buf[5] = (u8)(tv.tv_usec >> 16);
-	timeval_buf[6] = (u8)(tv.tv_usec >> 8);
-	timeval_buf[7] = (u8)(tv.tv_usec);
+	timeval_buf[0] = (u8)(ts64.tv_sec >> 24);
+	timeval_buf[1] = (u8)(ts64.tv_sec >> 16);
+	timeval_buf[2] = (u8)(ts64.tv_sec >> 8);
+	timeval_buf[3] = (u8)(ts64.tv_sec);
+	timeval_buf[4] = (u8)(tv_usec >> 24);
+	timeval_buf[5] = (u8)(tv_usec >> 16);
+	timeval_buf[6] = (u8)(tv_usec >> 8);
+	timeval_buf[7] = (u8)(tv_usec);
 }
 
 /* The scsi_lock() and scsi_unlock() macros protect the sm_state and the
diff --git a/kernel/drivers/staging/rts5208/rtsx_chip.c b/kernel/drivers/staging/rts5208/rtsx_chip.c
index 0c1716ebc..c0ce659a5 100644
--- a/kernel/drivers/staging/rts5208/rtsx_chip.c
+++ b/kernel/drivers/staging/rts5208/rtsx_chip.c
@@ -521,13 +521,14 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 		}
 	}
 
-	/* Disable cd_pwr_save (u_force_rst_core_en=0, u_cd_rst_core_en=0)
-	      0xFE5B
-	      bit[1]    u_cd_rst_core_en	rst_value = 0
-	      bit[2]    u_force_rst_core_en	rst_value = 0
-	      bit[5]    u_mac_phy_rst_n_dbg	rst_value = 1
-	      bit[4]	u_non_sticky_rst_n_dbg	rst_value = 0
-	*/
+	/*
+	 * Disable cd_pwr_save (u_force_rst_core_en=0, u_cd_rst_core_en=0)
+	 *    0xFE5B
+	 *    bit[1]    u_cd_rst_core_en	rst_value = 0
+	 *    bit[2]    u_force_rst_core_en	rst_value = 0
+	 *    bit[5]    u_mac_phy_rst_n_dbg	rst_value = 1
+	 *    bit[4]	u_non_sticky_rst_n_dbg	rst_value = 0
+	 */
 	retval = rtsx_write_register(chip, CHANGE_LINK_STATE, 0x16, 0x10);
 	if (retval) {
 		rtsx_trace(chip);
@@ -1143,137 +1144,281 @@ static void rtsx_monitor_aspm_config(struct rtsx_chip *chip)
 	}
 }
 
-void rtsx_polling_func(struct rtsx_chip *chip)
+static void rtsx_manage_ocp(struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_OCP
+	if (!chip->ocp_int)
+		return;
+
+	rtsx_read_register(chip, OCPSTAT, &chip->ocp_stat);
+
+	if (chip->card_exist & SD_CARD)
+		sd_power_off_card3v3(chip);
+	else if (chip->card_exist & MS_CARD)
+		ms_power_off_card3v3(chip);
+	else if (chip->card_exist & XD_CARD)
+		xd_power_off_card3v3(chip);
+
+	chip->ocp_int = 0;
+#endif
+}
+
+static void rtsx_manage_sd_lock(struct rtsx_chip *chip)
 {
 #ifdef SUPPORT_SD_LOCK
 	struct sd_info *sd_card = &chip->sd_card;
-#endif
-	bool ss_allowed;
+	u8 val;
 
-	if (rtsx_chk_stat(chip, RTSX_STAT_SUSPEND))
+	if (!sd_card->sd_erase_status)
 		return;
 
-	if (rtsx_chk_stat(chip, RTSX_STAT_DELINK))
-		goto delink_stage;
+	if (chip->card_exist & SD_CARD) {
+		rtsx_read_register(chip, 0xFD30, &val);
+		if (val & 0x02) {
+			sd_card->sd_erase_status = SD_NOT_ERASE;
+			sd_card->sd_lock_notify = 1;
+			chip->need_reinit |= SD_CARD;
+		}
+	} else {
+		sd_card->sd_erase_status = SD_NOT_ERASE;
+	}
+#endif
+}
 
-	if (chip->polling_config) {
-		u8 val;
+static bool rtsx_is_ss_allowed(struct rtsx_chip *chip)
+{
+	u32 val;
 
-		rtsx_read_config_byte(chip, 0, &val);
+	if (!chip->ss_en || CHECK_PID(chip, 0x5288))
+		return false;
+
+	if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
+		rtsx_read_cfg_dw(chip, 1, 0x04, &val);
+		if (val & 0x07)
+			return false;
 	}
 
-	if (rtsx_chk_stat(chip, RTSX_STAT_SS))
+	return true;
+}
+
+static void rtsx_manage_ss(struct rtsx_chip *chip)
+{
+	if (!rtsx_is_ss_allowed(chip) || chip->sd_io)
 		return;
 
-#ifdef SUPPORT_OCP
-	if (chip->ocp_int) {
-		rtsx_read_register(chip, OCPSTAT, &chip->ocp_stat);
+	if (rtsx_get_stat(chip) != RTSX_STAT_IDLE) {
+		chip->ss_counter = 0;
+		return;
+	}
 
-		if (chip->card_exist & SD_CARD)
-			sd_power_off_card3v3(chip);
-		else if (chip->card_exist & MS_CARD)
-			ms_power_off_card3v3(chip);
-		else if (chip->card_exist & XD_CARD)
-			xd_power_off_card3v3(chip);
+	if (chip->ss_counter < (chip->ss_idle_period / POLLING_INTERVAL))
+		chip->ss_counter++;
+	else
+		rtsx_exclusive_enter_ss(chip);
+}
+
+static void rtsx_manage_aspm(struct rtsx_chip *chip)
+{
+	u8 data;
+
+	if (!CHECK_PID(chip, 0x5208))
+		return;
+
+	rtsx_monitor_aspm_config(chip);
 
-		chip->ocp_int = 0;
+#ifdef SUPPORT_SDIO_ASPM
+	if (!CHK_SDIO_EXIST(chip) || CHK_SDIO_IGNORED(chip) ||
+	    !chip->aspm_l0s_l1_en || !chip->dynamic_aspm)
+		return;
+
+	if (chip->sd_io) {
+		dynamic_configure_sdio_aspm(chip);
+		return;
 	}
+
+	if (chip->sdio_aspm)
+		return;
+
+	dev_dbg(rtsx_dev(chip), "SDIO enter ASPM!\n");
+	data = 0x30 | (chip->aspm_level[1] << 2);
+	rtsx_write_register(chip, ASPM_FORCE_CTL, 0xFC, data);
+	chip->sdio_aspm = 1;
 #endif
+}
 
-#ifdef SUPPORT_SD_LOCK
-	if (sd_card->sd_erase_status) {
-		if (chip->card_exist & SD_CARD) {
-			u8 val;
-
-			rtsx_read_register(chip, 0xFD30, &val);
-			if (val & 0x02) {
-				sd_card->sd_erase_status = SD_NOT_ERASE;
-				sd_card->sd_lock_notify = 1;
-				chip->need_reinit |= SD_CARD;
-			}
-		} else {
-			sd_card->sd_erase_status = SD_NOT_ERASE;
-		}
+static void rtsx_manage_idle(struct rtsx_chip *chip)
+{
+	if (chip->idle_counter < IDLE_MAX_COUNT) {
+		chip->idle_counter++;
+		return;
 	}
+
+	if (rtsx_get_stat(chip) == RTSX_STAT_IDLE)
+		return;
+
+	dev_dbg(rtsx_dev(chip), "Idle state!\n");
+	rtsx_set_stat(chip, RTSX_STAT_IDLE);
+
+#if !defined(LED_AUTO_BLINK) && defined(REGULAR_BLINK)
+	chip->led_toggle_counter = 0;
 #endif
+	rtsx_force_power_on(chip, SSC_PDCTL);
 
-	rtsx_init_cards(chip);
+	turn_off_led(chip, LED_GPIO);
 
-	if (chip->ss_en) {
-		ss_allowed = true;
+	if (chip->auto_power_down && !chip->card_ready && !chip->sd_io)
+		rtsx_force_power_down(chip, SSC_PDCTL | OC_PDCTL);
+}
 
-		if (CHECK_PID(chip, 0x5288)) {
-			ss_allowed = false;
-		} else {
-			if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
-				u32 val;
+static void rtsx_manage_2lun_mode(struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_OCP
+	u8 sd_oc, ms_oc;
 
-				rtsx_read_cfg_dw(chip, 1, 0x04, &val);
-				if (val & 0x07)
-					ss_allowed = false;
-			}
-		}
-	} else {
-		ss_allowed = false;
+	sd_oc = chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER);
+	ms_oc = chip->ocp_stat & (MS_OC_NOW | MS_OC_EVER);
+
+	if (sd_oc || ms_oc)
+		dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
+			chip->ocp_stat);
+
+	if (sd_oc && (chip->card_exist & SD_CARD)) {
+		rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
+		card_power_off(chip, SD_CARD);
+		chip->card_fail |= SD_CARD;
 	}
 
-	if (ss_allowed && !chip->sd_io) {
-		if (rtsx_get_stat(chip) != RTSX_STAT_IDLE) {
-			chip->ss_counter = 0;
-		} else {
-			if (chip->ss_counter <
-				(chip->ss_idle_period / POLLING_INTERVAL)) {
-				chip->ss_counter++;
-			} else {
-				rtsx_exclusive_enter_ss(chip);
-				return;
-			}
-		}
+	if (ms_oc && (chip->card_exist & MS_CARD)) {
+		rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
+		card_power_off(chip, MS_CARD);
+		chip->card_fail |= MS_CARD;
 	}
+#endif
+}
 
-	if (CHECK_PID(chip, 0x5208)) {
-		rtsx_monitor_aspm_config(chip);
+static void rtsx_manage_1lun_mode(struct rtsx_chip *chip)
+{
+#ifdef SUPPORT_OCP
+	if (!(chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)))
+		return;
 
-#ifdef SUPPORT_SDIO_ASPM
-		if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip) &&
-		    chip->aspm_l0s_l1_en && chip->dynamic_aspm) {
-			if (chip->sd_io) {
-				dynamic_configure_sdio_aspm(chip);
-			} else {
-				if (!chip->sdio_aspm) {
-					dev_dbg(rtsx_dev(chip), "SDIO enter ASPM!\n");
-					rtsx_write_register(chip,
-						ASPM_FORCE_CTL, 0xFC,
-						0x30 |
-						(chip->aspm_level[1] << 2));
-					chip->sdio_aspm = 1;
-				}
-			}
-		}
+	dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
+		chip->ocp_stat);
+
+	if (chip->card_exist & SD_CARD) {
+		rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN, 0);
+		chip->card_fail |= SD_CARD;
+	} else if (chip->card_exist & MS_CARD) {
+		rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
+		chip->card_fail |= MS_CARD;
+	} else if (chip->card_exist & XD_CARD) {
+		rtsx_write_register(chip, CARD_OE, XD_OUTPUT_EN, 0);
+		chip->card_fail |= XD_CARD;
+	}
+	card_power_off(chip, SD_CARD);
 #endif
+}
+
+static void rtsx_delink_stage1(struct rtsx_chip *chip, int enter_L1,
+			       int stage3_cnt)
+{
+	u8 val;
+
+	rtsx_set_stat(chip, RTSX_STAT_DELINK);
+
+	if (chip->asic_code && CHECK_PID(chip, 0x5208))
+		rtsx_set_phy_reg_bit(chip, 0x1C, 2);
+
+	if (chip->card_exist)
+		dev_dbg(rtsx_dev(chip), "False card inserted, do force delink\n");
+	else
+		dev_dbg(rtsx_dev(chip), "No card inserted, do delink\n");
+
+	if (enter_L1)
+		rtsx_write_register(chip, HOST_SLEEP_STATE, 0x03, 1);
+
+	if (chip->card_exist)
+		val = 0x02;
+	else
+		val = 0x0A;
+
+	rtsx_write_register(chip, CHANGE_LINK_STATE, val, val);
+
+	if (enter_L1)
+		rtsx_enter_L1(chip);
+
+	if (chip->card_exist)
+		chip->auto_delink_cnt = stage3_cnt + 1;
+}
+
+static void rtsx_delink_stage(struct rtsx_chip *chip)
+{
+	int delink_stage1_cnt, delink_stage2_cnt, delink_stage3_cnt;
+	int enter_L1;
+
+	if (!chip->auto_delink_en || !chip->auto_delink_allowed ||
+	    chip->card_ready || chip->card_ejected || chip->sd_io) {
+		chip->auto_delink_cnt = 0;
+		return;
 	}
 
-	if (chip->idle_counter < IDLE_MAX_COUNT) {
-		chip->idle_counter++;
-	} else {
-		if (rtsx_get_stat(chip) != RTSX_STAT_IDLE) {
-			dev_dbg(rtsx_dev(chip), "Idle state!\n");
-			rtsx_set_stat(chip, RTSX_STAT_IDLE);
+	enter_L1 = chip->auto_delink_in_L1 &&
+		(chip->aspm_l0s_l1_en || chip->ss_en);
 
-#if !defined(LED_AUTO_BLINK) && defined(REGULAR_BLINK)
-			chip->led_toggle_counter = 0;
-#endif
-			rtsx_force_power_on(chip, SSC_PDCTL);
+	delink_stage1_cnt = chip->delink_stage1_step;
+	delink_stage2_cnt = delink_stage1_cnt + chip->delink_stage2_step;
+	delink_stage3_cnt = delink_stage2_cnt + chip->delink_stage3_step;
 
-			turn_off_led(chip, LED_GPIO);
+	if (chip->auto_delink_cnt > delink_stage3_cnt)
+		return;
 
-			if (chip->auto_power_down && !chip->card_ready &&
-			    !chip->sd_io)
-				rtsx_force_power_down(chip,
-						      SSC_PDCTL | OC_PDCTL);
-		}
+	if (chip->auto_delink_cnt == delink_stage1_cnt)
+		rtsx_delink_stage1(chip, enter_L1, delink_stage3_cnt);
+
+	if (chip->auto_delink_cnt == delink_stage2_cnt) {
+		dev_dbg(rtsx_dev(chip), "Try to do force delink\n");
+
+		if (enter_L1)
+			rtsx_exit_L1(chip);
+
+		if (chip->asic_code && CHECK_PID(chip, 0x5208))
+			rtsx_set_phy_reg_bit(chip, 0x1C, 2);
+
+		rtsx_write_register(chip, CHANGE_LINK_STATE, 0x0A, 0x0A);
+	}
+
+	chip->auto_delink_cnt++;
+}
+
+void rtsx_polling_func(struct rtsx_chip *chip)
+{
+	if (rtsx_chk_stat(chip, RTSX_STAT_SUSPEND))
+		return;
+
+	if (rtsx_chk_stat(chip, RTSX_STAT_DELINK))
+		goto delink_stage;
+
+	if (chip->polling_config) {
+		u8 val;
+
+		rtsx_read_config_byte(chip, 0, &val);
 	}
 
+	if (rtsx_chk_stat(chip, RTSX_STAT_SS))
+		return;
+
+	rtsx_manage_ocp(chip);
+
+	rtsx_manage_sd_lock(chip);
+
+	rtsx_init_cards(chip);
+
+	rtsx_manage_ss(chip);
+
+	rtsx_manage_aspm(chip);
+
+	rtsx_manage_idle(chip);
+
 	switch (rtsx_get_stat(chip)) {
 	case RTSX_STAT_RUN:
 #if !defined(LED_AUTO_BLINK) && defined(REGULAR_BLINK)
@@ -1293,121 +1438,13 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 		break;
 	}
 
-#ifdef SUPPORT_OCP
-	if (CHECK_LUN_MODE(chip, SD_MS_2LUN)) {
-		if (chip->ocp_stat &
-			(SD_OC_NOW | SD_OC_EVER | MS_OC_NOW | MS_OC_EVER))
-			dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
-				chip->ocp_stat);
-
-		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)) {
-			if (chip->card_exist & SD_CARD) {
-				rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN,
-						    0);
-				card_power_off(chip, SD_CARD);
-				chip->card_fail |= SD_CARD;
-			}
-		}
-		if (chip->ocp_stat & (MS_OC_NOW | MS_OC_EVER)) {
-			if (chip->card_exist & MS_CARD) {
-				rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN,
-						    0);
-				card_power_off(chip, MS_CARD);
-				chip->card_fail |= MS_CARD;
-			}
-		}
-	} else {
-		if (chip->ocp_stat & (SD_OC_NOW | SD_OC_EVER)) {
-			dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
-				chip->ocp_stat);
-			if (chip->card_exist & SD_CARD) {
-				rtsx_write_register(chip, CARD_OE, SD_OUTPUT_EN,
-						    0);
-				chip->card_fail |= SD_CARD;
-			} else if (chip->card_exist & MS_CARD) {
-				rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN,
-						    0);
-				chip->card_fail |= MS_CARD;
-			} else if (chip->card_exist & XD_CARD) {
-				rtsx_write_register(chip, CARD_OE, XD_OUTPUT_EN,
-						    0);
-				chip->card_fail |= XD_CARD;
-			}
-			card_power_off(chip, SD_CARD);
-		}
-	}
-#endif
+	if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
+		rtsx_manage_2lun_mode(chip);
+	else
+		rtsx_manage_1lun_mode(chip);
 
 delink_stage:
-	if (chip->auto_delink_en && chip->auto_delink_allowed &&
-	    !chip->card_ready && !chip->card_ejected && !chip->sd_io) {
-		int enter_L1 = chip->auto_delink_in_L1 && (
-			chip->aspm_l0s_l1_en || chip->ss_en);
-		int delink_stage1_cnt = chip->delink_stage1_step;
-		int delink_stage2_cnt = delink_stage1_cnt +
-			chip->delink_stage2_step;
-		int delink_stage3_cnt = delink_stage2_cnt +
-			chip->delink_stage3_step;
-
-		if (chip->auto_delink_cnt <= delink_stage3_cnt) {
-			if (chip->auto_delink_cnt == delink_stage1_cnt) {
-				rtsx_set_stat(chip, RTSX_STAT_DELINK);
-
-				if (chip->asic_code && CHECK_PID(chip, 0x5208))
-					rtsx_set_phy_reg_bit(chip, 0x1C, 2);
-
-				if (chip->card_exist) {
-					dev_dbg(rtsx_dev(chip), "False card inserted, do force delink\n");
-
-					if (enter_L1)
-						rtsx_write_register(chip,
-							      HOST_SLEEP_STATE,
-							      0x03, 1);
-
-					rtsx_write_register(chip,
-							    CHANGE_LINK_STATE,
-							    0x0A, 0x0A);
-
-					if (enter_L1)
-						rtsx_enter_L1(chip);
-
-					chip->auto_delink_cnt =
-						delink_stage3_cnt + 1;
-				} else {
-					dev_dbg(rtsx_dev(chip), "No card inserted, do delink\n");
-
-					if (enter_L1)
-						rtsx_write_register(chip,
-							      HOST_SLEEP_STATE,
-							      0x03, 1);
-
-					rtsx_write_register(chip,
-							    CHANGE_LINK_STATE,
-							    0x02, 0x02);
-
-					if (enter_L1)
-						rtsx_enter_L1(chip);
-				}
-			}
-
-			if (chip->auto_delink_cnt == delink_stage2_cnt) {
-				dev_dbg(rtsx_dev(chip), "Try to do force delink\n");
-
-				if (enter_L1)
-					rtsx_exit_L1(chip);
-
-				if (chip->asic_code && CHECK_PID(chip, 0x5208))
-					rtsx_set_phy_reg_bit(chip, 0x1C, 2);
-
-				rtsx_write_register(chip, CHANGE_LINK_STATE,
-						    0x0A, 0x0A);
-			}
-
-			chip->auto_delink_cnt++;
-		}
-	} else {
-		chip->auto_delink_cnt = 0;
-	}
+	rtsx_delink_stage(chip);
 }
 
 void rtsx_undo_delink(struct rtsx_chip *chip)
@@ -2117,12 +2154,13 @@ int rtsx_pre_handle_interrupt(struct rtsx_chip *chip)
 				clear_bit(SD_NR, &chip->need_reset);
 			}
 		} else {
-			/* If multi-luns, it's possible that
-			   when plugging/unplugging one card
-			   there is another card which still
-			   exists in the slot. In this case,
-			   all existed cards should be reset.
-			*/
+			/*
+			 * If multi-luns, it's possible that
+			 * when plugging/unplugging one card
+			 * there is another card which still
+			 * exists in the slot. In this case,
+			 * all existed cards should be reset.
+			 */
 			if (exit_ss && (status & SD_EXIST))
 				set_bit(SD_NR, &chip->need_reinit);
 		}
diff --git a/kernel/drivers/staging/rts5208/rtsx_scsi.c b/kernel/drivers/staging/rts5208/rtsx_scsi.c
index 8a5d6a8e7..60871f302 100644
--- a/kernel/drivers/staging/rts5208/rtsx_scsi.c
+++ b/kernel/drivers/staging/rts5208/rtsx_scsi.c
@@ -915,6 +915,8 @@ static int read_write(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		start_sec = ((u32)(srb->cmnd[1] & 0x1F) << 16) |
 			((u32)srb->cmnd[2] << 8) | ((u32)srb->cmnd[3]);
 		sec_cnt = srb->cmnd[4];
+		if (sec_cnt == 0)
+			sec_cnt = 256;
 	} else if ((srb->cmnd[0] == VENDOR_CMND) &&
 		(srb->cmnd[1] == SCSI_APP_CMD) &&
 		((srb->cmnd[2] == PP_READ10) || (srb->cmnd[2] == PP_WRITE10))) {
@@ -2904,9 +2906,11 @@ void led_shine(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 
 	if ((srb->cmnd[0] == READ_10) || (srb->cmnd[0] == WRITE_10))
 		sec_cnt = ((u16)(srb->cmnd[7]) << 8) | srb->cmnd[8];
-	else if ((srb->cmnd[0] == READ_6) || (srb->cmnd[0] == WRITE_6))
+	else if ((srb->cmnd[0] == READ_6) || (srb->cmnd[0] == WRITE_6)) {
 		sec_cnt = srb->cmnd[4];
-	else
+		if (sec_cnt == 0)
+			sec_cnt = 256;
+	} else
 		return;
 
 	if (chip->rw_cap[lun] >= GPIO_TOGGLE_THRESHOLD) {
diff --git a/kernel/drivers/staging/rts5208/sd.c b/kernel/drivers/staging/rts5208/sd.c
index a8d657bb5..d6c498209 100644
--- a/kernel/drivers/staging/rts5208/sd.c
+++ b/kernel/drivers/staging/rts5208/sd.c
@@ -246,11 +246,10 @@ RTY_SEND_CMD:
 				}
 			}
 #ifdef SUPPORT_SD_LOCK
-			if (ptr[1] & 0x7D)
+			if (ptr[1] & 0x7D) {
 #else
-			if (ptr[1] & 0x7F)
+			if (ptr[1] & 0x7F) {
 #endif
-			{
 				dev_dbg(rtsx_dev(chip), "ptr[1]: 0x%02x\n",
 					ptr[1]);
 				rtsx_trace(chip);
@@ -3520,12 +3519,11 @@ int reset_sd_card(struct rtsx_chip *chip)
 			if (chip->sd_io) {
 				rtsx_trace(chip);
 				return STATUS_FAIL;
-			} else {
-				retval = reset_mmc(chip);
-				if (retval != STATUS_SUCCESS) {
-					rtsx_trace(chip);
-					return STATUS_FAIL;
-				}
+			}
+			retval = reset_mmc(chip);
+			if (retval != STATUS_SUCCESS) {
+				rtsx_trace(chip);
+				return STATUS_FAIL;
 			}
 		}
 	}
@@ -4149,11 +4147,10 @@ RTY_SEND_CMD:
 			}
 		}
 #ifdef SUPPORT_SD_LOCK
-		if (ptr[1] & 0x7D)
+		if (ptr[1] & 0x7D) {
 #else
-		if (ptr[1] & 0x7F)
+		if (ptr[1] & 0x7F) {
 #endif
-		{
 			rtsx_trace(chip);
 			return STATUS_FAIL;
 		}
diff --git a/kernel/drivers/staging/rts5208/xd.c b/kernel/drivers/staging/rts5208/xd.c
index 8fd108e50..10fea7bb8 100644
--- a/kernel/drivers/staging/rts5208/xd.c
+++ b/kernel/drivers/staging/rts5208/xd.c
@@ -1379,9 +1379,8 @@ static int xd_erase_block(struct rtsx_chip *chip, u32 phy_blk)
 				xd_set_err_code(chip, XD_PRG_ERROR);
 				rtsx_trace(chip);
 				return STATUS_FAIL;
-			} else {
-				xd_set_err_code(chip, XD_ERASE_FAIL);
 			}
+			xd_set_err_code(chip, XD_ERASE_FAIL);
 			retval = xd_reset_cmd(chip);
 			if (retval != STATUS_SUCCESS) {
 				rtsx_trace(chip);