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 <[email protected]>

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;