scsi: ufs: ufs-mediatek: Fix MCQ mode TM cmd timeout

Fix TM cmd timeout issue in MCQ mode using the default resume call
ufshcd_make_hba_operational() to set TM cmd DMA address.

This flow is the same as UFS initialization after link startup and then
setting MCQ related registers if using MCQ mode.

Signed-off-by: Peter Wang <peter.wang@mediatek.com>
Link: https://lore.kernel.org/r/20231221110416.16176-3-peter.wang@mediatek.com
Reviewed-by: Chun-Hung Wu <chun-hung.wu@mediatek.com>
Signed-off-by: Martin K. Petersen <martin.petersen@oracle.com>

diff --git a/drivers/ufs/host/ufs-mediatek.c b/drivers/ufs/host/ufs-mediatek.c
index dcccb63f74b9443db856db0ecabac1666efacb94..47b5f49cda9da6672b8982236a746d043af07512 100644 (file)
--- a/drivers/ufs/host/ufs-mediatek.c
+++ b/drivers/ufs/host/ufs-mediatek.c
@@ -1201,9 +1201,11 @@ static int ufs_mtk_link_set_hpm(struct ufs_hba *hba)
        }
        ufshcd_set_link_active(hba);
 
-       if (!hba->mcq_enabled) {
-               err = ufshcd_make_hba_operational(hba);
-       } else {
+       err = ufshcd_make_hba_operational(hba);
+       if (err)
+               return err;
+
+       if (is_mcq_enabled(hba)) {
                ufs_mtk_config_mcq(hba, false);
                ufshcd_mcq_make_queues_operational(hba);
                ufshcd_mcq_config_mac(hba, hba->nutrs);
@@ -1212,9 +1214,6 @@ static int ufs_mtk_link_set_hpm(struct ufs_hba *hba)
                              REG_UFS_MEM_CFG);
        }
 
-       if (err)
-               return err;
-
        return 0;
 }