diff --git a/drivers/ufs/host/ufs-mediatek.c b/drivers/ufs/host/ufs-mediatek.c index 47b5f49cda9da6672b8982236a746d043af07512..0b1b8efa864b6e96e4efeaba3f80cfa6b2d0766a 100644 --- a/drivers/ufs/host/ufs-mediatek.c +++ b/drivers/ufs/host/ufs-mediatek.c @@ -647,6 +647,45 @@ static void ufs_mtk_pwr_ctrl(struct ufs_hba *hba, bool on) } } +static void ufs_mtk_mcq_disable_irq(struct ufs_hba *hba) +{ + struct ufs_mtk_host *host = ufshcd_get_variant(hba); + u32 irq, i; + + if (!is_mcq_enabled(hba)) + return; + + if (host->mcq_nr_intr == 0) + return; + + for (i = 0; i < host->mcq_nr_intr; i++) { + irq = host->mcq_intr_info[i].irq; + disable_irq(irq); + } + host->is_mcq_intr_enabled = false; +} + +static void ufs_mtk_mcq_enable_irq(struct ufs_hba *hba) +{ + struct ufs_mtk_host *host = ufshcd_get_variant(hba); + u32 irq, i; + + if (!is_mcq_enabled(hba)) + return; + + if (host->mcq_nr_intr == 0) + return; + + if (host->is_mcq_intr_enabled == true) + return; + + for (i = 0; i < host->mcq_nr_intr; i++) { + irq = host->mcq_intr_info[i].irq; + enable_irq(irq); + } + host->is_mcq_intr_enabled = true; +} + /** * ufs_mtk_setup_clocks - enables/disable clocks * @hba: host controller instance @@ -690,8 +729,10 @@ static int ufs_mtk_setup_clocks(struct ufs_hba *hba, bool on, if (clk_pwr_off) ufs_mtk_pwr_ctrl(hba, false); + ufs_mtk_mcq_disable_irq(hba); } else if (on && status == POST_CHANGE) { ufs_mtk_pwr_ctrl(hba, true); + ufs_mtk_mcq_enable_irq(hba); } return ret; diff --git a/drivers/ufs/host/ufs-mediatek.h b/drivers/ufs/host/ufs-mediatek.h index 38eab95b0f798f2b90416bfd437ad233e1eb7fd8..d0673f1021ecedd6d827522cf3a4e6eabe9516ab 100644 --- a/drivers/ufs/host/ufs-mediatek.h +++ b/drivers/ufs/host/ufs-mediatek.h @@ -183,6 +183,7 @@ struct ufs_mtk_host { u32 ip_ver; bool mcq_set_intr; + bool is_mcq_intr_enabled; int mcq_nr_intr; struct ufs_mtk_mcq_intr_info mcq_intr_info[UFSHCD_MAX_Q_NR]; };