[PATCH 7/7] Staging: rts5208: helper function to manage delink states

[Fabio Falzoi <fabio.falzoi84@gmail.com>]

Use a helper function to manage delink states

Signed-off-by: Fabio Falzoi <fabio.falzoi84@gmail.com>
---
 drivers/staging/rts5208/rtsx_chip.c | 141 ++++++++++++++++++------------------
 1 file changed, 72 insertions(+), 69 deletions(-)

diff --git a/drivers/staging/rts5208/rtsx_chip.c b/drivers/staging/rts5208/rtsx_chip.c
index 298163a..d6fb6cd 100644
--- a/drivers/staging/rts5208/rtsx_chip.c
+++ b/drivers/staging/rts5208/rtsx_chip.c
@@ -1318,6 +1318,77 @@ static void rtsx_manage_1lun_mode(struct rtsx_chip *chip)
 #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 = 0x03;
+	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;
+	}
+
+	enter_L1 = chip->auto_delink_in_L1 &&
+		(chip->aspm_l0s_l1_en || chip->ss_en);
+
+	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;
+
+	if (chip->auto_delink_cnt > delink_stage3_cnt)
+		return;
+
+	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))
@@ -1372,75 +1443,7 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 		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)
-- 
2.1.4