Staging: rts5208: helper function to manage delink states

Use a helper function to manage delink states

Signed-off-by: Fabio Falzoi <fabio.falzoi84@gmail.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>
This commit is contained in:
Fabio Falzoi 2015-06-14 15:48:53 +02:00 committed by Greg Kroah-Hartman
parent 00aa7161fc
commit a9b693cd77

View File

@ -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)