[PATCH v1 0/3] scsi: ufs-mediatek: add MediaTek vendor implementation part II

 drivers/scsi/ufs/ufs-mediatek.c | 16 ++++++++++++++++
 drivers/scsi/ufs/ufs-mediatek.h |  5 +++++
 2 files changed, 21 insertions(+)

diff --git a/drivers/scsi/ufs/ufs-mediatek.c b/drivers/scsi/ufs/ufs-mediatek.c
index 8d999c0e60fe..d5194d0c4ef5 100644
--- a/drivers/scsi/ufs/ufs-mediatek.c
+++ b/drivers/scsi/ufs/ufs-mediatek.c

@@ -406,6 +406,21 @@ static int ufs_mtk_resume(struct ufs_hba *hba, enum ufs_pm_op pm_op)
        return 0;
 }

+static void ufs_mtk_dbg_register_dump(struct ufs_hba *hba)
+{
+       ufshcd_dump_regs(hba, REG_UFS_REFCLK_CTRL, 0x4, "Ref-Clk Ctrl ");
+
+       ufshcd_dump_regs(hba, REG_UFS_EXTREG, 0x4, "Ext Reg ");
+
+       ufshcd_dump_regs(hba, REG_UFS_MPHYCTRL,
+                        REG_UFS_REJECT_MON - REG_UFS_MPHYCTRL + 4,
+                        "MPHY Ctrl ");
+
+       /* Direct debugging information to REG_MTK_PROBE */
+       ufshcd_writel(hba, 0x20, REG_UFS_DEBUG_SEL);
+       ufshcd_dump_regs(hba, REG_UFS_PROBE, 0x4, "Debug Probe ");
+}
+
 static int ufs_mtk_apply_dev_quirks(struct ufs_hba *hba,
                                    struct ufs_dev_desc *card)
 {

@@ -430,6 +445,7 @@ static struct ufs_hba_variant_ops ufs_hba_mtk_vops = {
        .apply_dev_quirks    = ufs_mtk_apply_dev_quirks,
        .suspend             = ufs_mtk_suspend,
        .resume              = ufs_mtk_resume,
+       .dbg_register_dump   = ufs_mtk_dbg_register_dump,
        .device_reset        = ufs_mtk_device_reset,
 };

diff --git a/drivers/scsi/ufs/ufs-mediatek.h b/drivers/scsi/ufs/ufs-mediatek.h
index 31b7fead19eb..fccdd979d6fb 100644
--- a/drivers/scsi/ufs/ufs-mediatek.h
+++ b/drivers/scsi/ufs/ufs-mediatek.h

@@ -13,6 +13,11 @@
  * Vendor specific UFSHCI Registers
  */
 #define REG_UFS_REFCLK_CTRL         0x144
+#define REG_UFS_EXTREG              0x2100
+#define REG_UFS_MPHYCTRL            0x2200
+#define REG_UFS_REJECT_MON          0x22AC
+#define REG_UFS_DEBUG_SEL           0x22C0
+#define REG_UFS_PROBE               0x22C8

 /*
  * Ref-clk control
--

2.18.0

 drivers/scsi/ufs/ufshcd.c | 11 +++++++----
 drivers/scsi/ufs/ufshcd.h |  3 +++
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index bea036ab189a..1168baf358ea 100644
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c

@@ -250,7 +250,6 @@ static int ufshcd_probe_hba(struct ufs_hba *hba);
 static int __ufshcd_setup_clocks(struct ufs_hba *hba, bool on,
                                 bool skip_ref_clk);
 static int ufshcd_setup_clocks(struct ufs_hba *hba, bool on);
-static int ufshcd_uic_hibern8_exit(struct ufs_hba *hba);
 static int ufshcd_uic_hibern8_enter(struct ufs_hba *hba);
 static inline void ufshcd_add_delay_before_dme_cmd(struct ufs_hba *hba);
 static int ufshcd_host_reset_and_restore(struct ufs_hba *hba);

@@ -3865,7 +3864,7 @@ static int ufshcd_uic_hibern8_enter(struct ufs_hba *hba)
        return ret;
 }

-static int ufshcd_uic_hibern8_exit(struct ufs_hba *hba)
+int ufshcd_uic_hibern8_exit(struct ufs_hba *hba)
 {
        struct uic_command uic_cmd = {0};
        int ret;

@@ -3891,6 +3890,7 @@ static int ufshcd_uic_hibern8_exit(struct ufs_hba *hba)

        return ret;
 }
+EXPORT_SYMBOL_GPL(ufshcd_uic_hibern8_exit);

 void ufshcd_auto_hibern8_update(struct ufs_hba *hba, u32 ahit)
 {

@@ -4162,7 +4162,7 @@ static int ufshcd_complete_dev_init(struct ufs_hba *hba)
  *
  * Returns 0 on success, non-zero value on failure
  */
-static int ufshcd_make_hba_operational(struct ufs_hba *hba)
+int ufshcd_make_hba_operational(struct ufs_hba *hba)
 {
        int err = 0;
        u32 reg;

@@ -4208,6 +4208,7 @@ static int ufshcd_make_hba_operational(struct ufs_hba *hba)
 out:
        return err;
 }
+EXPORT_SYMBOL_GPL(ufshcd_make_hba_operational);

 /**
  * ufshcd_hba_stop - Send controller to reset state

@@ -4285,7 +4286,7 @@ static int ufshcd_hba_execute_hce(struct ufs_hba *hba)
        return 0;
 }

-static int ufshcd_hba_enable(struct ufs_hba *hba)
+int ufshcd_hba_enable(struct ufs_hba *hba)
 {
        int ret;

@@ -4310,6 +4311,8 @@ static int ufshcd_hba_enable(struct ufs_hba *hba)

        return ret;
 }
+EXPORT_SYMBOL_GPL(ufshcd_hba_enable);
+
 static int ufshcd_disable_tx_lcc(struct ufs_hba *hba, bool peer)
 {
        int tx_lanes, i, err = 0;

diff --git a/drivers/scsi/ufs/ufshcd.h b/drivers/scsi/ufs/ufshcd.h
index b1a1c65be8b1..fca372d98495 100644
--- a/drivers/scsi/ufs/ufshcd.h
+++ b/drivers/scsi/ufs/ufshcd.h

@@ -799,8 +799,11 @@ static inline void ufshcd_rmwl(struct ufs_hba *hba, u32 mask, u32 val, u32 reg)

 int ufshcd_alloc_host(struct device *, struct ufs_hba **);
 void ufshcd_dealloc_host(struct ufs_hba *);
+int ufshcd_hba_enable(struct ufs_hba *hba);
 int ufshcd_init(struct ufs_hba * , void __iomem * , unsigned int);
+int ufshcd_make_hba_operational(struct ufs_hba *hba);
 void ufshcd_remove(struct ufs_hba *);
+int ufshcd_uic_hibern8_exit(struct ufs_hba *hba);
 int ufshcd_wait_for_register(struct ufs_hba *hba, u32 reg, u32 mask,
                                u32 val, unsigned long interval_us,
                                unsigned long timeout_ms, bool can_sleep);
--

2.18.0

 drivers/scsi/ufs/ufs-mediatek.c | 53 +++++++++++++++++++++++++++++++++
 1 file changed, 53 insertions(+)

diff --git a/drivers/scsi/ufs/ufs-mediatek.c b/drivers/scsi/ufs/ufs-mediatek.c
index d5194d0c4ef5..f32f3f34f6d0 100644
--- a/drivers/scsi/ufs/ufs-mediatek.c
+++ b/drivers/scsi/ufs/ufs-mediatek.c

@@ -382,11 +382,60 @@ static void ufs_mtk_device_reset(struct ufs_hba *hba)
        dev_info(hba->dev, "device reset done\n");
 }

+static int ufs_mtk_link_set_hpm(struct ufs_hba *hba)
+{
+       int err;
+
+       err = ufshcd_hba_enable(hba);
+       if (err)
+               return err;
+
+       err = ufshcd_dme_set(hba,
+                            UIC_ARG_MIB_SEL(VS_UNIPROPOWERDOWNCONTROL, 0),
+                            0);
+       if (err)
+               return err;
+
+       err = ufshcd_uic_hibern8_exit(hba);
+       if (!err)
+               ufshcd_set_link_active(hba);
+       else
+               return err;
+
+       err = ufshcd_make_hba_operational(hba);
+       if (err)
+               return err;
+
+       return 0;
+}
+
+static int ufs_mtk_link_set_lpm(struct ufs_hba *hba)
+{
+       int err;
+
+       err = ufshcd_dme_set(hba,
+                            UIC_ARG_MIB_SEL(VS_UNIPROPOWERDOWNCONTROL, 0),
+                            1);
+       if (err) {
+               /* Resume UniPro state for following error recovery */
+               ufshcd_dme_set(hba,
+                              UIC_ARG_MIB_SEL(VS_UNIPROPOWERDOWNCONTROL, 0),
+                              0);
+               return err;
+       }
+
+       return 0;
+}
+
 static int ufs_mtk_suspend(struct ufs_hba *hba, enum ufs_pm_op pm_op)
 {
+       int err;
        struct ufs_mtk_host *host = ufshcd_get_variant(hba);

        if (ufshcd_is_link_hibern8(hba)) {
+               err = ufs_mtk_link_set_lpm(hba);
+               if (err)
+                       return -EAGAIN;
                phy_power_off(host->mphy);
                ufs_mtk_setup_ref_clk(hba, false);
        }

@@ -397,10 +446,14 @@ static int ufs_mtk_suspend(struct ufs_hba *hba, enum ufs_pm_op pm_op)
 static int ufs_mtk_resume(struct ufs_hba *hba, enum ufs_pm_op pm_op)
 {
        struct ufs_mtk_host *host = ufshcd_get_variant(hba);
+       int err;

        if (ufshcd_is_link_hibern8(hba)) {
                ufs_mtk_setup_ref_clk(hba, true);
                phy_power_on(host->mphy);
+               err = ufs_mtk_link_set_hpm(hba);
+               if (err)
+                       return err;
        }

        return 0;
--

2.18.0

  drivers/scsi/ufs/ufshcd.c | 11 +++++++----
  drivers/scsi/ufs/ufshcd.h |  3 +++
  2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index bea036ab189a..1168baf358ea 100644
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c

@@ -250,7 +250,6 @@ static int ufshcd_probe_hba(struct ufs_hba *hba);
  static int __ufshcd_setup_clocks(struct ufs_hba *hba, bool on,
  				 bool skip_ref_clk);
  static int ufshcd_setup_clocks(struct ufs_hba *hba, bool on);
-static int ufshcd_uic_hibern8_exit(struct ufs_hba *hba);
  static int ufshcd_uic_hibern8_enter(struct ufs_hba *hba);
  static inline void ufshcd_add_delay_before_dme_cmd(struct ufs_hba *hba);
  static int ufshcd_host_reset_and_restore(struct ufs_hba *hba);

@@ -3865,7 +3864,7 @@ static int ufshcd_uic_hibern8_enter(struct ufs_hba *hba)
  	return ret;
  }
  
-static int ufshcd_uic_hibern8_exit(struct ufs_hba *hba)
+int ufshcd_uic_hibern8_exit(struct ufs_hba *hba)
  {
  	struct uic_command uic_cmd = {0};
  	int ret;

@@ -3891,6 +3890,7 @@ static int ufshcd_uic_hibern8_exit(struct ufs_hba *hba)
  
  	return ret;
  }
+EXPORT_SYMBOL_GPL(ufshcd_uic_hibern8_exit);
  
  void ufshcd_auto_hibern8_update(struct ufs_hba *hba, u32 ahit)
  {

@@ -4162,7 +4162,7 @@ static int ufshcd_complete_dev_init(struct ufs_hba *hba)
   *
   * Returns 0 on success, non-zero value on failure
   */
-static int ufshcd_make_hba_operational(struct ufs_hba *hba)
+int ufshcd_make_hba_operational(struct ufs_hba *hba)
  {
  	int err = 0;
  	u32 reg;

@@ -4208,6 +4208,7 @@ static int ufshcd_make_hba_operational(struct ufs_hba *hba)
  out:
  	return err;
  }
+EXPORT_SYMBOL_GPL(ufshcd_make_hba_operational);
  
  /**
   * ufshcd_hba_stop - Send controller to reset state

@@ -4285,7 +4286,7 @@ static int ufshcd_hba_execute_hce(struct ufs_hba *hba)
  	return 0;
  }
  
-static int ufshcd_hba_enable(struct ufs_hba *hba)
+int ufshcd_hba_enable(struct ufs_hba *hba)
  {
  	int ret;
  

@@ -4310,6 +4311,8 @@ static int ufshcd_hba_enable(struct ufs_hba *hba)
  
  	return ret;
  }
+EXPORT_SYMBOL_GPL(ufshcd_hba_enable);
+
  static int ufshcd_disable_tx_lcc(struct ufs_hba *hba, bool peer)
  {
  	int tx_lanes, i, err = 0;

diff --git a/drivers/scsi/ufs/ufshcd.h b/drivers/scsi/ufs/ufshcd.h
index b1a1c65be8b1..fca372d98495 100644
--- a/drivers/scsi/ufs/ufshcd.h
+++ b/drivers/scsi/ufs/ufshcd.h

@@ -799,8 +799,11 @@ static inline void ufshcd_rmwl(struct ufs_hba *hba, u32 mask, u32 val, u32 reg)
  
  int ufshcd_alloc_host(struct device *, struct ufs_hba **);
  void ufshcd_dealloc_host(struct ufs_hba *);
+int ufshcd_hba_enable(struct ufs_hba *hba);
  int ufshcd_init(struct ufs_hba * , void __iomem * , unsigned int);
+int ufshcd_make_hba_operational(struct ufs_hba *hba);
  void ufshcd_remove(struct ufs_hba *);
+int ufshcd_uic_hibern8_exit(struct ufs_hba *hba);
  int ufshcd_wait_for_register(struct ufs_hba *hba, u32 reg, u32 mask,
  				u32 val, unsigned long interval_us,
  				unsigned long timeout_ms, bool can_sleep);

  drivers/scsi/ufs/ufs-mediatek.c | 53 +++++++++++++++++++++++++++++++++
  1 file changed, 53 insertions(+)

diff --git a/drivers/scsi/ufs/ufs-mediatek.c b/drivers/scsi/ufs/ufs-mediatek.c
index d5194d0c4ef5..f32f3f34f6d0 100644
--- a/drivers/scsi/ufs/ufs-mediatek.c
+++ b/drivers/scsi/ufs/ufs-mediatek.c

@@ -382,11 +382,60 @@ static void ufs_mtk_device_reset(struct ufs_hba *hba)
  	dev_info(hba->dev, "device reset done\n");
  }
  
+static int ufs_mtk_link_set_hpm(struct ufs_hba *hba)
+{
+	int err;
+
+	err = ufshcd_hba_enable(hba);
+	if (err)
+		return err;
+
+	err = ufshcd_dme_set(hba,
+			     UIC_ARG_MIB_SEL(VS_UNIPROPOWERDOWNCONTROL, 0),
+			     0);
+	if (err)
+		return err;
+
+	err = ufshcd_uic_hibern8_exit(hba);
+	if (!err)
+		ufshcd_set_link_active(hba);
+	else
+		return err;
+
+	err = ufshcd_make_hba_operational(hba);
+	if (err)
+		return err;
+
+	return 0;
+}
+
+static int ufs_mtk_link_set_lpm(struct ufs_hba *hba)
+{
+	int err;
+
+	err = ufshcd_dme_set(hba,
+			     UIC_ARG_MIB_SEL(VS_UNIPROPOWERDOWNCONTROL, 0),
+			     1);
+	if (err) {
+		/* Resume UniPro state for following error recovery */
+		ufshcd_dme_set(hba,
+			       UIC_ARG_MIB_SEL(VS_UNIPROPOWERDOWNCONTROL, 0),
+			       0);
+		return err;
+	}
+
+	return 0;
+}
+
  static int ufs_mtk_suspend(struct ufs_hba *hba, enum ufs_pm_op pm_op)
  {
+	int err;
  	struct ufs_mtk_host *host = ufshcd_get_variant(hba);
  
  	if (ufshcd_is_link_hibern8(hba)) {
+		err = ufs_mtk_link_set_lpm(hba);
+		if (err)
+			return -EAGAIN;
  		phy_power_off(host->mphy);
  		ufs_mtk_setup_ref_clk(hba, false);
  	}

@@ -397,10 +446,14 @@ static int ufs_mtk_suspend(struct ufs_hba *hba, enum ufs_pm_op pm_op)
  static int ufs_mtk_resume(struct ufs_hba *hba, enum ufs_pm_op pm_op)
  {
  	struct ufs_mtk_host *host = ufshcd_get_variant(hba);
+	int err;
  
  	if (ufshcd_is_link_hibern8(hba)) {
  		ufs_mtk_setup_ref_clk(hba, true);
  		phy_power_on(host->mphy);
+		err = ufs_mtk_link_set_hpm(hba);
+		if (err)
+			return err;
  	}
  
  	return 0;