[PATCH v1 1/2] ufs: core: Configure only active lanes during link

[palash.kambar@oss.qualcomm.com]

From: Palash Kambar <palash.kambar@oss.qualcomm.com>

The number of active lanes detected during UFS link startup can be
fewer than the lanes specified in the device tree. The current driver
logic attempts to configure all lanes defined in the device tree,
regardless of their actual availability. This mismatch may cause
failures during power mode changes.

Hence, add check to identify only the lanes that were successfully
discovered during link startup, to warn on power mode change errors
caused by mismatched lane counts.

Signed-off-by: Palash Kambar <palash.kambar@oss.qualcomm.com>
---
 drivers/ufs/core/ufshcd.c | 39 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 39 insertions(+)

diff --git a/drivers/ufs/core/ufshcd.c b/drivers/ufs/core/ufshcd.c
index 31950fc51a4c..c956fab32932 100644
--- a/drivers/ufs/core/ufshcd.c
+++ b/drivers/ufs/core/ufshcd.c
@@ -5035,6 +5035,42 @@ void ufshcd_update_evt_hist(struct ufs_hba *hba, u32 id, u32 val)
 }
 EXPORT_SYMBOL_GPL(ufshcd_update_evt_hist);
 
+static int ufshcd_get_connected_tx_lanes(struct ufs_hba *hba, u32 *tx_lanes)
+{
+	return ufshcd_dme_get(hba,
+			      UIC_ARG_MIB(PA_CONNECTEDTXDATALANES), tx_lanes);
+}
+
+static int ufshcd_get_connected_rx_lanes(struct ufs_hba *hba, u32 *rx_lanes)
+{
+	return ufshcd_dme_get(hba,
+			      UIC_ARG_MIB(PA_CONNECTEDRXDATALANES), rx_lanes);
+}
+
+static void ufshcd_validate_link_params(struct ufs_hba *hba)
+{
+	int val = 0;
+
+	if (ufshcd_get_connected_tx_lanes(hba, &val))
+		return;
+
+	if (val != hba->lanes_per_direction) {
+		dev_err(hba->dev, "Tx lane mismatch [config,reported] [%d,%d]\n",
+			hba->lanes_per_direction, val);
+		return;
+	}
+
+	val = 0;
+
+	if (ufshcd_get_connected_rx_lanes(hba, &val))
+		return;
+
+	if (val != hba->lanes_per_direction) {
+		dev_err(hba->dev, "Rx lane mismatch [config,reported] [%d,%d]\n",
+			hba->lanes_per_direction, val);
+	}
+}
+
 /**
  * ufshcd_link_startup - Initialize unipro link startup
  * @hba: per adapter instance
@@ -5108,6 +5144,9 @@ static int ufshcd_link_startup(struct ufs_hba *hba)
 			goto out;
 	}
 
+	/* Check successfully detected lanes */
+	ufshcd_validate_link_params(hba);
+
 	/* Include any host controller configuration via UIC commands */
 	ret = ufshcd_vops_link_startup_notify(hba, POST_CHANGE);
 	if (ret)
-- 
2.34.1