From 39a430ca29e7999f68c2cb2abdbbdbfa25656bd2 Mon Sep 17 00:00:00 2001
From: Vidyakumar Athota <vathota@codeaurora.org>
Date: Mon, 22 Jan 2018 15:44:58 -0800
Subject: [PATCH] swr-wcd-ctrl: update soundwire slave logical device number

Soundwire slave logical device number is not updated properly for
all the devices. Because of this, slave devices are pointed to wrong
register regmap. Update logical device number properly for all the
slave devices.

Change-Id: Ic480585301d8e53500714bc136e23e95ac52b13c
Signed-off-by: Vidyakumar Athota <vathota@codeaurora.org>
---
 soc/swr-wcd-ctrl.c | 33 ++++++++++++++++++++++-----------
 1 file changed, 22 insertions(+), 11 deletions(-)

diff --git a/soc/swr-wcd-ctrl.c b/soc/swr-wcd-ctrl.c
index dc1f7866d40c..fec252bc605a 100644
--- a/soc/swr-wcd-ctrl.c
+++ b/soc/swr-wcd-ctrl.c
@@ -1291,6 +1291,7 @@ static int swrm_get_logical_dev_num(struct swr_master *mstr, u64 dev_id,
 	u64 id = 0;
 	int ret = -EINVAL;
 	struct swr_mstr_ctrl *swrm = swr_get_ctrl_data(mstr);
+	struct swr_device *swr_dev;
 
 	if (!swrm) {
 		pr_err("%s: Invalid handle to swr controller\n",
@@ -1304,20 +1305,30 @@ static int swrm_get_logical_dev_num(struct swr_master *mstr, u64 dev_id,
 			    SWRM_ENUMERATOR_SLAVE_DEV_ID_2(i))) << 32);
 		id |= swrm->read(swrm->handle,
 			    SWRM_ENUMERATOR_SLAVE_DEV_ID_1(i));
-		if ((id & SWR_DEV_ID_MASK) == dev_id) {
-			if (swrm_get_device_status(swrm, i) == 0x01) {
-				*dev_num = i;
-				ret = 0;
-			} else {
-				dev_err(swrm->dev, "%s: device is not ready\n",
-					 __func__);
+		/*
+		 * As pm_runtime_get_sync() brings all slaves out of reset
+		 * update logical device number for all slaves.
+		 */
+		list_for_each_entry(swr_dev, &mstr->devices, dev_list) {
+			if (swr_dev->addr == (id & SWR_DEV_ID_MASK)) {
+				u32 status = swrm_get_device_status(swrm, i);
+
+				if ((status == 0x01) || (status == 0x02)) {
+					swr_dev->dev_num = i;
+					if ((id & SWR_DEV_ID_MASK) == dev_id) {
+						*dev_num = i;
+						ret = 0;
+					}
+					dev_dbg(swrm->dev, "%s: devnum %d is assigned for dev addr %lx\n",
+						__func__, i, swr_dev->addr);
+				}
 			}
-			goto found;
 		}
 	}
-	dev_err(swrm->dev, "%s: device id 0x%llx does not match with 0x%llx\n",
-		__func__, id, dev_id);
-found:
+	if (ret)
+		dev_err(swrm->dev, "%s: device 0x%llx is not ready\n",
+			__func__, dev_id);
+
 	pm_runtime_mark_last_busy(&swrm->pdev->dev);
 	pm_runtime_put_autosuspend(&swrm->pdev->dev);
 	return ret;