[PATCH 08/11] scsi: ufs: ufs-qcom: Implement vops tx_eqtr_notify()

[Can Guo <can.guo@oss.qualcomm.com>]

On some platforms, HW does not support triggering TX EQTR from the most
reliable high-speed gear (HS-G1). To work around the HW limitation,
implement tx_eqtr_notify() to change Power Mode to the target TX EQTR gear
prior to TX EQTR and change Power Mode back to HS-G1 (the most reliable
gear) post TX EQTR.

Signed-off-by: Can Guo <can.guo@oss.qualcomm.com>
---
 drivers/ufs/host/ufs-qcom.c | 64 +++++++++++++++++++++++++++++++++++++
 1 file changed, 64 insertions(+)

diff --git a/drivers/ufs/host/ufs-qcom.c b/drivers/ufs/host/ufs-qcom.c
index 3a9279066192..90b1496faf23 100644
--- a/drivers/ufs/host/ufs-qcom.c
+++ b/drivers/ufs/host/ufs-qcom.c
@@ -2512,6 +2512,69 @@ static u32 ufs_qcom_freq_to_gear_speed(struct ufs_hba *hba, unsigned long freq)
 	return min_t(u32, gear, hba->max_pwr_info.info.gear_rx);
 }
 
+static int ufs_qcom_change_power_mode(struct ufs_hba *hba,
+				      struct ufs_pa_layer_attr *pwr_mode,
+				      bool force_pmc)
+{
+	int ret;
+
+	ret = ufs_qcom_pwr_change_notify(hba, PRE_CHANGE, pwr_mode);
+	if (ret) {
+		dev_err(hba->dev, "Power change notify (PRE_CHANGE) failed: %d\n",
+			ret);
+		return ret;
+	}
+
+	ret = ufshcd_change_power_mode(hba, pwr_mode, force_pmc);
+	if (ret)
+		return ret;
+
+	ufs_qcom_pwr_change_notify(hba, POST_CHANGE, pwr_mode);
+
+	return ret;
+}
+
+static int ufs_qcom_tx_eqtr_notify(struct ufs_hba *hba,
+				   enum ufs_notify_change_status status,
+				   struct ufshcd_tx_eq_params *params,
+				   struct ufs_pa_layer_attr *pwr_mode)
+{
+	struct ufs_qcom_host *host = ufshcd_get_variant(hba);
+	struct ufs_pa_layer_attr pwr_mode_hs_g1 = {
+		.gear_rx = UFS_HS_G1,
+		.gear_tx = UFS_HS_G1,
+		.lane_rx = params->rx_lanes,
+		.lane_tx = params->tx_lanes,
+		.pwr_rx = FAST_MODE,
+		.pwr_tx = FAST_MODE,
+		.hs_rate = pwr_mode->hs_rate,
+	};
+	u32 gear = pwr_mode->gear_tx;
+	u32 rate = pwr_mode->hs_rate;
+	int ret;
+
+	if (host->hw_ver.major != 0x7 || host->hw_ver.minor > 0x1)
+		return 0;
+
+	if (status == PRE_CHANGE) {
+		/* PMC to target HS Gear. */
+		ret = ufs_qcom_change_power_mode(hba, pwr_mode,
+						 /*force_pmc=*/false);
+		if (ret)
+			dev_err(hba->dev, "%s: Failed to change power mode to target HS-G%u, Rate-%s: %d\n",
+				__func__, gear, UFS_HS_RATE_STRING(rate), ret);
+	} else {
+		/* PMC back to HS-G1. */
+		ret = ufs_qcom_change_power_mode(hba, &pwr_mode_hs_g1,
+						 /*force_pmc=*/false);
+		if (ret)
+			dev_err(hba->dev, "%s: Failed to change power mode to HS-G1, Rate-%s: %d\n",
+				__func__, UFS_HS_RATE_STRING(rate), ret);
+	}
+
+	return ret;
+}
+
 /*
  * struct ufs_hba_qcom_vops - UFS QCOM specific variant operations
  *
@@ -2542,6 +2605,7 @@ static const struct ufs_hba_variant_ops ufs_hba_qcom_vops = {
 	.get_outstanding_cqs	= ufs_qcom_get_outstanding_cqs,
 	.config_esi		= ufs_qcom_config_esi,
 	.freq_to_gear_speed	= ufs_qcom_freq_to_gear_speed,
+	.tx_eqtr_notify		= ufs_qcom_tx_eqtr_notify,
 };
 
 static const struct ufs_hba_variant_ops ufs_hba_qcom_sa8255p_vops = {
-- 
2.34.1