clear-pkgs-linux-iot-lts2018/0199-SoundWire-Add-support-for-the-aggregation.patch

From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Hardik Shah <hardik.t.shah@intel.com>
Date: Sun, 1 May 2016 18:29:30 +0530
Subject: [PATCH] SoundWire: Add support for the aggregation

Add support for the master aggregation in
SoundWire bus driver. For the aggregated masters
bus driver calls async message transfer APIs for
the bank swith to make sure that bank switch
for multiple masters happen at same time.

Change-Id: I4272b2ff35c6aee9ee3d1919113b2d11be766abc
Signed-off-by: Hardik Shah <hardik.t.shah@intel.com>
Reviewed-on:
---
 drivers/sdw/sdw_bwcalc.c | 1031 ++++++++++++++++++++++++++++----------
 1 file changed, 755 insertions(+), 276 deletions(-)

diff --git a/drivers/sdw/sdw_bwcalc.c b/drivers/sdw/sdw_bwcalc.c
index 5b362dc0a2f4..cafaccbeea3a 100644
--- a/drivers/sdw/sdw_bwcalc.c
+++ b/drivers/sdw/sdw_bwcalc.c
@@ -17,7 +17,7 @@
  *
  * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  */
-
+#include <linux/slab.h>
 #include <linux/kernel.h>
 #include <linux/sdw_bus.h>
 #include "sdw_priv.h"
@@ -116,7 +116,7 @@ int sdw_mstr_bw_init(struct sdw_bus *sdw_bs)
 	sdw_bs->system_interval = 0;
 	sdw_bs->frame_freq = 0;
 	sdw_bs->clk_state = SDW_CLK_STATE_ON;
-
+	sdw_mstr_cap = &sdw_bs->mstr->mstr_capabilities;
 #ifdef CONFIG_SND_SOC_SVFPGA
 	/* TBD: For PDM capture to be removed later */
 	sdw_bs->clk_freq = 9.6 * 1000 * 1000 * 2;
@@ -1457,6 +1457,103 @@ int sdw_configure_frmshp_bnkswtch(struct sdw_bus *mstr_bs, int col, int row)
 	return ret;
 }
 
+/*
+ * sdw_configure_frmshp_bnkswtch - returns Success
+ * -EINVAL - In case of error.
+ *
+ *
+ * This function broadcast frameshape on framectrl
+ * register and performs bank switch.
+ */
+int sdw_configure_frmshp_bnkswtch_mm(struct sdw_bus *mstr_bs, int col, int row)
+{
+	int ret = 0;
+	int banktouse, numcol, numrow;
+	u8 *wbuf;
+	struct sdw_msg *wr_msg;
+
+	wr_msg = kzalloc(sizeof(struct sdw_msg), GFP_KERNEL);
+	mstr_bs->async_data.msg = wr_msg;
+	if (!wr_msg)
+		return -ENOMEM;
+	wbuf = kzalloc(sizeof(*wbuf), GFP_KERNEL);
+	if (!wbuf)
+		return -ENOMEM;
+	numcol = sdw_get_col_to_num(col);
+	numrow = sdw_get_row_to_num(row);
+
+	wbuf[0] = numcol | (numrow << 3);
+	/* Get current bank in use from bus structure*/
+	banktouse = mstr_bs->active_bank;
+	banktouse = !banktouse;
+
+	if (banktouse) {
+		wr_msg->addr = (SDW_SCP_FRAMECTRL + SDW_BANK1_REGISTER_OFFSET) +
+			(SDW_NUM_DATA_PORT_REGISTERS * 0); /* Data port 0 */
+	} else {
+
+		wr_msg->addr = SDW_SCP_FRAMECTRL +
+			(SDW_NUM_DATA_PORT_REGISTERS * 0); /* Data port 0 */
+	}
+
+	wr_msg->ssp_tag = 0x1;
+	wr_msg->flag = SDW_MSG_FLAG_WRITE;
+	wr_msg->len = 1;
+	wr_msg->slave_addr = 0xF; /* Broadcast address*/
+	wr_msg->buf = wbuf;
+	wr_msg->addr_page1 = 0x0;
+	wr_msg->addr_page2 = 0x0;
+
+	if (in_atomic() || irqs_disabled()) {
+		ret = sdw_trylock_mstr(mstr_bs->mstr);
+		if (!ret) {
+			/* SDW activity is ongoing. */
+			ret = -EAGAIN;
+			goto out;
+		}
+	} else {
+		sdw_lock_mstr(mstr_bs->mstr);
+	}
+
+	ret = sdw_slave_transfer_async(mstr_bs->mstr, wr_msg,
+					1, &mstr_bs->async_data);
+	if (ret != 1) {
+		ret = -EINVAL;
+		dev_err(&mstr_bs->mstr->dev, "Register transfer failed\n");
+		goto out;
+	}
+
+	msleep(100); /* TBD: Remove this */
+
+	/*
+	 * TBD: check whether we need to poll on
+	 * mcp active bank bit to switch bank
+	 */
+	mstr_bs->active_bank = banktouse;
+
+out:
+
+	return ret;
+}
+
+int sdw_configure_frmshp_bnkswtch_mm_wait(struct sdw_bus *mstr_bs)
+{
+	unsigned long time_left;
+	struct sdw_master *mstr = mstr_bs->mstr;
+
+	time_left = wait_for_completion_timeout(
+			&mstr_bs->async_data.xfer_complete,
+			3000);
+	if (!time_left) {
+		dev_err(&mstr->dev, "Controller Timed out\n");
+		sdw_unlock_mstr(mstr);
+		return -ETIMEDOUT;
+	}
+	kfree(mstr_bs->async_data.msg->buf);
+	kfree(mstr_bs->async_data.msg);
+	sdw_unlock_mstr(mstr);
+	return 0;
+}
 
 /*
  * sdw_cfg_bs_params - returns Success
@@ -1937,6 +2034,21 @@ int sdw_prep_unprep_mstr_slv(struct sdw_bus *sdw_mstr_bs,
 	return 0;
 }
 
+struct sdw_bus *master_to_bus(struct sdw_master *mstr)
+{
+	struct sdw_bus *sdw_mstr_bs = NULL;
+
+	list_for_each_entry(sdw_mstr_bs, &sdw_core.bus_list, bus_node) {
+		/* Match master structure pointer */
+		if (sdw_mstr_bs->mstr != mstr)
+			continue;
+		return sdw_mstr_bs;
+	}
+	/* This should never happen, added to suppress warning */
+	WARN_ON(1);
+
+	return NULL;
+}
 
 /**
  * sdw_bus_calc_bw - returns Success
@@ -1956,13 +2068,16 @@ int sdw_bus_calc_bw(struct sdw_stream_tag *stream_tag, bool enable)
 	struct sdw_runtime *sdw_rt = stream_tag->sdw_rt;
 	struct sdw_stream_params *stream_params = &sdw_rt->stream_params;
 	struct sdw_mstr_runtime *sdw_mstr_rt = NULL, *sdw_mstr_bs_rt = NULL;
-	struct sdw_bus *sdw_mstr_bs = NULL;
+	struct sdw_mstr_runtime *mstr_rt_act = NULL, *last_rt = NULL;
+	struct sdw_bus *sdw_mstr_bs = NULL, *mstr_bs_act = NULL;
 	struct sdw_master *sdw_mstr = NULL;
 	struct sdw_master_capabilities *sdw_mstr_cap = NULL;
 	struct sdw_stream_params *mstr_params;
 	int stream_frame_size;
 	int frame_interval = 0, sel_row = 0, sel_col = 0;
 	int ret = 0;
+	bool last_node = false;
+	struct sdw_master_port_ops *ops;
 
 	/* TBD: Add PCM/PDM flag in sdw_config_stream */
 
@@ -1973,22 +2088,21 @@ int sdw_bus_calc_bw(struct sdw_stream_tag *stream_tag, bool enable)
 	 */
 
 	/* BW calulation for active master controller for given stream tag */
-	list_for_each_entry(sdw_mstr_rt, &sdw_rt->mstr_rt_list, mstr_sdw_node) {
+	list_for_each_entry(sdw_mstr_rt, &sdw_rt->mstr_rt_list,
+							mstr_sdw_node) {
 
 		if (sdw_mstr_rt->mstr == NULL)
 			break;
+		last_rt = list_last_entry(&sdw_rt->mstr_rt_list,
+				struct sdw_mstr_runtime, mstr_sdw_node);
+		if (sdw_mstr_rt == last_rt)
+			last_node = true;
+		else
+			last_node = false;
 
 		/* Get bus structure for master */
-		list_for_each_entry(sdw_mstr_bs, &sdw_core.bus_list, bus_node) {
-
-			/* Match master structure pointer */
-			if (sdw_mstr_bs->mstr != sdw_mstr_rt->mstr)
-				continue;
-
-
-			sdw_mstr = sdw_mstr_bs->mstr;
-			break;
-		}
+		sdw_mstr_bs = master_to_bus(sdw_mstr_rt->mstr);
+		sdw_mstr = sdw_mstr_bs->mstr;
 
 		/*
 		 * All data structures required available,
@@ -1999,180 +2113,384 @@ int sdw_bus_calc_bw(struct sdw_stream_tag *stream_tag, bool enable)
 		sdw_mstr_cap = &sdw_mstr_bs->mstr->mstr_capabilities;
 		mstr_params = &sdw_mstr_rt->stream_params;
 
-		if ((sdw_rt->stream_state == SDW_STATE_CONFIG_STREAM) ||
-				(sdw_rt->stream_state ==
-					SDW_STATE_UNPREPARE_STREAM)) {
+		if ((sdw_rt->stream_state != SDW_STATE_CONFIG_STREAM) &&
+			(sdw_rt->stream_state != SDW_STATE_UNPREPARE_STREAM))
+			goto enable_stream;
 
-			/* we do not support asynchronous mode Return Error */
-			if ((sdw_mstr_cap->base_clk_freq % mstr_params->rate)
-					!= 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"Asynchronous mode not supported\n");
-				return -EINVAL;
-			}
+		/* we do not support asynchronous mode Return Error */
+		if ((sdw_mstr_cap->base_clk_freq % mstr_params->rate) != 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "Async mode not supported\n");
+			return -EINVAL;
+		}
 
-			/* Check for sampling frequency */
-			if (stream_params->rate != mstr_params->rate) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"Sampling frequency mismatch\n");
-				return -EINVAL;
-			}
+		/* Check for sampling frequency */
+		if (stream_params->rate != mstr_params->rate) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "Sample frequency mismatch\n");
+			return -EINVAL;
+		}
 
-			/*
-			 * Calculate stream bandwidth, frame size and
-			 * total BW required for master controller
-			 */
-			sdw_mstr_rt->stream_bw = mstr_params->rate *
-				mstr_params->channel_count * mstr_params->bps;
-			stream_frame_size = mstr_params->channel_count *
-				mstr_params->bps;
+		/*
+		 * Calculate stream bandwidth, frame size and
+		 * total BW required for master controller
+		 */
+		sdw_mstr_rt->stream_bw = mstr_params->rate *
+			mstr_params->channel_count * mstr_params->bps;
+		stream_frame_size = mstr_params->channel_count *
+					mstr_params->bps;
 
-			sdw_mstr_bs->bandwidth += sdw_mstr_rt->stream_bw;
+		sdw_mstr_bs->bandwidth += sdw_mstr_rt->stream_bw;
 
-			ret = sdw_get_clock_frmshp(sdw_mstr_bs,
-					&frame_interval, &sel_col, &sel_row);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev, "clock/frameshape config failed\n");
-				return ret;
-			}
+		ret = sdw_get_clock_frmshp(sdw_mstr_bs,
+				&frame_interval, &sel_col, &sel_row);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "clock/frameshape config failed\n");
+			return ret;
+		}
 
+		/*
+		 * TBD: find right place to run sorting on
+		 * master rt_list. Below sorting is done based on
+		 * bps from low to high, that means PDM streams
+		 * will be placed before PCM.
+		 */
 
-			/*
-			 * TBD: find right place to run sorting on
-			 * master rt_list. Below sorting is done based on
-			 * bps from low to high, that means PDM streams
-			 * will be placed before PCM.
-			 */
+		/*
+		 * TBD Should we also perform sorting based on rate
+		 * for PCM stream check. if yes then how??
+		 * creating two different list.
+		 */
 
-			/*
-			 * TBD Should we also perform sorting based on rate
-			 * for PCM stream check. if yes then how??
-			 * creating two different list.
-			 */
+		/* Compute system interval */
+		ret = sdw_compute_sys_interval(sdw_mstr_bs, sdw_mstr_cap,
+						frame_interval);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "compute system interval failed\n");
+			return ret;
+		}
 
-			/* Compute system interval */
-			ret = sdw_compute_sys_interval(sdw_mstr_bs,
-					sdw_mstr_cap, frame_interval);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev, "compute system interval failed\n");
-				return ret;
+		/* Compute hstart/hstop */
+		ret = sdw_compute_hstart_hstop(sdw_mstr_bs, sel_col);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "compute hstart/hstop failed\n");
+			return ret;
+		}
+
+		/* Compute block offset */
+		ret = sdw_compute_blk_subblk_offset(sdw_mstr_bs);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "compute block offset failed\n");
+			return ret;
+		}
+
+		/* Change Stream State */
+		if (last_node)
+			sdw_rt->stream_state = SDW_STATE_COMPUTE_STREAM;
+
+		/* Configure bus parameters */
+		ret = sdw_cfg_bs_params(sdw_mstr_bs, sdw_mstr_bs_rt, true);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "xport param config failed\n");
+			return ret;
+		}
+
+		sel_col = sdw_mstr_bs->col;
+		sel_row = sdw_mstr_bs->row;
+
+		if ((last_node) && (sdw_mstr->link_sync_mask)) {
+
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+							mstr_sdw_node) {
+
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+				ops = mstr_bs_act->mstr->driver->mstr_port_ops;
+
+				/* Run for all mstr_list and
+				 * pre_activate ports
+				 */
+				if (ops->dpn_port_activate_ch_pre) {
+					ret = ops->dpn_port_activate_ch_pre
+						(mstr_bs_act->mstr, NULL, 0);
+					if (ret < 0)
+						return ret;
+				}
 			}
 
-			/* Compute hstart/hstop */
-			ret = sdw_compute_hstart_hstop(sdw_mstr_bs, sel_col);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"compute hstart/hstop failed\n");
-				return ret;
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+						mstr_sdw_node) {
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+
+				/* Configure Frame Shape/Switch Bank */
+				ret = sdw_configure_frmshp_bnkswtch_mm(
+						mstr_bs_act, sel_col, sel_row);
+				if (ret < 0) {
+					/* TBD: Undo all the computation */
+					dev_err(&sdw_mstr->dev, "bank switch failed\n");
+					return ret;
+				}
 			}
 
-			/* Compute block offset */
-			ret = sdw_compute_blk_subblk_offset(sdw_mstr_bs);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(
-						&sdw_mstr_bs->mstr->dev,
-						"compute block offset failed\n");
-				return ret;
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+						mstr_sdw_node) {
+
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+
+				ops = mstr_bs_act->mstr->driver->mstr_port_ops;
+
+				/* Run for all mstr_list and
+				 * post_activate ports
+				 */
+				if (ops->dpn_port_activate_ch_post) {
+					ret = ops->dpn_port_activate_ch_post
+						(mstr_bs_act->mstr, NULL, 0);
+					if (ret < 0)
+						return ret;
+				}
 			}
 
-			/* Change Stream State */
-			sdw_rt->stream_state = SDW_STATE_COMPUTE_STREAM;
+			list_for_each_entry(mstr_rt_act,
+				&sdw_rt->mstr_rt_list, mstr_sdw_node) {
 
-			/* Configure bus parameters */
-			ret = sdw_cfg_bs_params(sdw_mstr_bs,
-					sdw_mstr_bs_rt, true);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"xport params config failed\n");
-				return ret;
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				mstr_bs_act = master_to_bus(
+					mstr_rt_act->mstr);
+				ret = sdw_configure_frmshp_bnkswtch_mm_wait(
+								mstr_bs_act);
 			}
 
-			sel_col = sdw_mstr_bs->col;
-			sel_row = sdw_mstr_bs->row;
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+						mstr_sdw_node) {
+
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+
+				/* Disable all channels
+				 * enabled on previous bank
+				 */
+				ret = sdw_dis_chan(mstr_bs_act, sdw_mstr_bs_rt);
+				if (ret < 0) {
+					/* TBD: Undo all the computation */
+					dev_err(&sdw_mstr->dev, "Channel disabled faile\n");
+					return ret;
+				}
+			}
+		}
+		if (!sdw_mstr->link_sync_mask) {
 
 			/* Configure Frame Shape/Switch Bank */
 			ret = sdw_configure_frmshp_bnkswtch(sdw_mstr_bs,
 					sel_col, sel_row);
 			if (ret < 0) {
 				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"bank switch failed\n");
+				dev_err(&sdw_mstr->dev, "bank switch failed\n");
 				return ret;
 			}
-
 			/* Disable all channels enabled on previous bank */
 			ret = sdw_dis_chan(sdw_mstr_bs, sdw_mstr_bs_rt);
 			if (ret < 0) {
 				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"Channel disabled failed\n");
-				return ret;
-			}
-
-			/* Prepare new port for master and slave */
-			ret = sdw_prep_unprep_mstr_slv(sdw_mstr_bs,
-					sdw_rt, true);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"Channel prepare failed\n");
+				dev_err(&sdw_mstr->dev, "Channel disabled failed\n");
 				return ret;
 			}
+		}
+		/* Prepare new port for master and slave */
+		ret = sdw_prep_unprep_mstr_slv(sdw_mstr_bs, sdw_rt, true);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "Channel prepare failed\n");
+			return ret;
+		}
 
-			/* change stream state to prepare */
+		/* change stream state to prepare */
+		if (last_node)
 			sdw_rt->stream_state = SDW_STATE_PREPARE_STREAM;
+	}
+enable_stream:
+	list_for_each_entry(sdw_mstr_rt, &sdw_rt->mstr_rt_list, mstr_sdw_node) {
+
+
+		if (sdw_mstr_rt->mstr == NULL)
+			break;
+		last_rt = list_last_entry(&sdw_rt->mstr_rt_list,
+				struct sdw_mstr_runtime, mstr_sdw_node);
+		if (sdw_mstr_rt == last_rt)
+			last_node = true;
+		else
+			last_node = false;
+
+		/* Get bus structure for master */
+		sdw_mstr_bs = master_to_bus(sdw_mstr_rt->mstr);
+		sdw_mstr = sdw_mstr_bs->mstr;
+
+		sdw_mstr_cap = &sdw_mstr_bs->mstr->mstr_capabilities;
+		mstr_params = &sdw_mstr_rt->stream_params;
+
+		if ((!enable) ||
+			(sdw_rt->stream_state != SDW_STATE_PREPARE_STREAM))
+			return 0;
+
+		ret = sdw_cfg_bs_params(sdw_mstr_bs, sdw_mstr_bs_rt, false);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "xport params config failed\n");
+			return ret;
+		}
+
+		/* Enable new port for master and slave */
+		ret = sdw_en_dis_mstr_slv(sdw_mstr_bs, sdw_rt, true);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "Channel enable failed\n");
+			return ret;
 		}
 
-		if ((enable) && (SDW_STATE_PREPARE_STREAM
-					== sdw_rt->stream_state)) {
+		/* change stream state to enable */
+		if (last_node)
+			sdw_rt->stream_state = SDW_STATE_ENABLE_STREAM;
 
-			ret = sdw_cfg_bs_params(sdw_mstr_bs,
-					sdw_mstr_bs_rt, false);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"xport params config failed\n");
-				return ret;
+		sel_col = sdw_mstr_bs->col;
+		sel_row = sdw_mstr_bs->row;
+
+		if ((last_node) && (sdw_mstr->link_sync_mask)) {
+
+
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+						mstr_sdw_node) {
+
+
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+
+				ops = mstr_bs_act->mstr->driver->mstr_port_ops;
+
+				/* Run for all mstr_list and
+				 * pre_activate ports
+				 */
+				if (ops->dpn_port_activate_ch_pre) {
+					ret = ops->dpn_port_activate_ch_pre
+						(mstr_bs_act->mstr, NULL, 0);
+					if (ret < 0)
+						return ret;
+				}
 			}
 
-			/* Enable new port for master and slave */
-			ret = sdw_en_dis_mstr_slv(sdw_mstr_bs, sdw_rt, true);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"Channel enable failed\n");
-				return ret;
+			list_for_each_entry(mstr_rt_act,
+				&sdw_rt->mstr_rt_list, mstr_sdw_node) {
+
+
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+
+				/* Configure Frame Shape/Switch Bank */
+				ret = sdw_configure_frmshp_bnkswtch_mm(
+						mstr_bs_act,
+						sel_col, sel_row);
+				if (ret < 0) {
+					/* TBD: Undo all the computation */
+					dev_err(&sdw_mstr->dev, "bank switch failed\n");
+					return ret;
+				}
 			}
 
-			/* change stream state to enable */
-			sdw_rt->stream_state = SDW_STATE_ENABLE_STREAM;
+			list_for_each_entry(mstr_rt_act,
+				&sdw_rt->mstr_rt_list, mstr_sdw_node) {
+
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+
+				ops = mstr_bs_act->mstr->driver->mstr_port_ops;
+
+				/* Run for all mstr_list and
+				 * post_activate ports
+				 */
+				if (ops->dpn_port_activate_ch_post) {
+					ret = ops->dpn_port_activate_ch_post
+						(mstr_bs_act->mstr, NULL, 0);
+					if (ret < 0)
+						return ret;
+				}
+			}
+
+			list_for_each_entry(mstr_rt_act,
+				&sdw_rt->mstr_rt_list, mstr_sdw_node) {
 
-			sel_col = sdw_mstr_bs->col;
-			sel_row = sdw_mstr_bs->row;
+				if (mstr_rt_act->mstr == NULL)
+					break;
 
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+				ret = sdw_configure_frmshp_bnkswtch_mm_wait(
+							mstr_bs_act);
+			}
+			list_for_each_entry(mstr_rt_act,
+				&sdw_rt->mstr_rt_list, mstr_sdw_node) {
+
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+
+				/* Disable all channels
+				 * enabled on previous bank
+				 */
+				ret = sdw_dis_chan(mstr_bs_act,
+							sdw_mstr_bs_rt);
+				if (ret < 0) {
+					/* TBD: Undo all the computation */
+					dev_err(&sdw_mstr->dev,
+							"Channel disabled faile\n");
+					return ret;
+				}
+			}
+		}
+		if (!sdw_mstr->link_sync_mask) {
 			/* Configure Frame Shape/Switch Bank */
-			ret = sdw_configure_frmshp_bnkswtch(sdw_mstr_bs,
+			ret = sdw_configure_frmshp_bnkswtch(
+					sdw_mstr_bs,
 					sel_col, sel_row);
 			if (ret < 0) {
 				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"bank switch failed\n");
+				dev_err(&sdw_mstr->dev, "bank switch failed\n");
 				return ret;
 			}
-
 			/* Disable all channels enabled on previous bank */
 			ret = sdw_dis_chan(sdw_mstr_bs, sdw_mstr_bs_rt);
 			if (ret < 0) {
 				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"Channel disabled faile\n");
+				dev_err(&sdw_mstr->dev, "Ch disabled failed\n");
 				return ret;
 			}
 		}
@@ -2182,7 +2500,6 @@ int sdw_bus_calc_bw(struct sdw_stream_tag *stream_tag, bool enable)
 }
 EXPORT_SYMBOL_GPL(sdw_bus_calc_bw);
 
-
 /**
  * sdw_bus_calc_bw_dis - returns Success
  * -EINVAL - In case of error.
@@ -2200,221 +2517,383 @@ int sdw_bus_calc_bw_dis(struct sdw_stream_tag *stream_tag, bool unprepare)
 {
 	struct sdw_runtime *sdw_rt = stream_tag->sdw_rt;
 	struct sdw_mstr_runtime *sdw_mstr_rt = NULL, *sdw_mstr_bs_rt = NULL;
-	struct sdw_bus *sdw_mstr_bs = NULL;
+	struct sdw_mstr_runtime *mstr_rt_act = NULL, *last_rt = NULL;
+	struct sdw_bus *sdw_mstr_bs = NULL, *mstr_bs_act = NULL;
 	struct sdw_master *sdw_mstr = NULL;
 	struct sdw_master_capabilities *sdw_mstr_cap = NULL;
 	struct sdw_stream_params *mstr_params;
 	int stream_frame_size;
 	int frame_interval = 0, sel_row = 0, sel_col = 0;
 	int ret = 0;
-
+	bool last_node = false;
+	struct sdw_master_port_ops *ops;
 
 	/* BW calulation for active master controller for given stream tag */
-	list_for_each_entry(sdw_mstr_rt, &sdw_rt->mstr_rt_list, mstr_sdw_node) {
+	list_for_each_entry(sdw_mstr_rt,
+			&sdw_rt->mstr_rt_list, mstr_sdw_node) {
+
 
 		if (sdw_mstr_rt->mstr == NULL)
 			break;
 
+		last_rt = list_last_entry(&sdw_rt->mstr_rt_list,
+				struct sdw_mstr_runtime, mstr_sdw_node);
+		if (sdw_mstr_rt == last_rt)
+			last_node = true;
+		else
+			last_node = false;
+
 		/* Get bus structure for master */
-		list_for_each_entry(sdw_mstr_bs, &sdw_core.bus_list, bus_node) {
+		sdw_mstr_bs = master_to_bus(sdw_mstr_rt->mstr);
+		sdw_mstr = sdw_mstr_bs->mstr;
 
-			/* Match master structure pointer */
-			if (sdw_mstr_bs->mstr != sdw_mstr_rt->mstr)
-				continue;
 
+		sdw_mstr_cap = &sdw_mstr_bs->mstr->mstr_capabilities;
+		mstr_params = &sdw_mstr_rt->stream_params;
 
-			sdw_mstr = sdw_mstr_bs->mstr;
-			break;
-		}
+		if (sdw_rt->stream_state != SDW_STATE_ENABLE_STREAM)
+			goto unprepare_stream;
 
+		/* Lets do disabling of port for stream to be freed */
+		list_for_each_entry(sdw_mstr_bs_rt,
+				&sdw_mstr->mstr_rt_list, mstr_node) {
 
-		sdw_mstr_cap = &sdw_mstr_bs->mstr->mstr_capabilities;
-		mstr_params = &sdw_mstr_rt->stream_params;
+			if (sdw_mstr_bs_rt->mstr == NULL)
+				continue;
 
-		if (sdw_rt->stream_state == SDW_STATE_ENABLE_STREAM) {
+			/*
+			 * Disable channel for slave and
+			 * master on current bank
+			 */
+			ret = sdw_en_dis_mstr_slv(sdw_mstr_bs, sdw_rt, false);
+			if (ret < 0) {
+				/* TBD: Undo all the computation */
+				dev_err(&sdw_mstr->dev, "Ch dis failed\n");
+				return ret;
+			}
 
-			/* Lets do disabling of port for stream to be freed */
-			list_for_each_entry(sdw_mstr_bs_rt,
-					&sdw_mstr->mstr_rt_list, mstr_node) {
+			/* Change stream state to disable */
+			if (last_node)
+				sdw_rt->stream_state = SDW_STATE_DISABLE_STREAM;
+		}
 
-				if (sdw_mstr_bs_rt->mstr == NULL)
-					continue;
+		ret = sdw_cfg_bs_params(sdw_mstr_bs, sdw_mstr_bs_rt, false);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "xport params config failed\n");
+			return ret;
+		}
 
-				/*
-				 * Disable channel for slave and
-				 * master on current bank
+		sel_col = sdw_mstr_bs->col;
+		sel_row = sdw_mstr_bs->row;
+
+		if ((last_node) && (sdw_mstr->link_sync_mask)) {
+
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+						mstr_sdw_node) {
+				if (mstr_rt_act->mstr == NULL)
+					break;
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+				ops = mstr_bs_act->mstr->driver->mstr_port_ops;
+				/* Run for all mstr_list and
+				 * pre_activate ports
 				 */
-				ret = sdw_en_dis_mstr_slv(sdw_mstr_bs,
-						sdw_rt, false);
+				if (ops->dpn_port_activate_ch_pre) {
+					ret = ops->dpn_port_activate_ch_pre
+						(mstr_bs_act->mstr, NULL, 0);
+					if (ret < 0)
+						return ret;
+				}
+			}
+			list_for_each_entry(mstr_rt_act,
+				&sdw_rt->mstr_rt_list, mstr_sdw_node) {
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+				/* Configure Frame Shape/Switch Bank */
+				ret = sdw_configure_frmshp_bnkswtch_mm(
+						mstr_bs_act,
+						sel_col, sel_row);
 				if (ret < 0) {
 					/* TBD: Undo all the computation */
-					dev_err(&sdw_mstr_bs->mstr->dev,
-							"Channel dis failed\n");
+					dev_err(&sdw_mstr->dev, "bank switch failed\n");
 					return ret;
 				}
-
-				/* Change stream state to disable */
-				sdw_rt->stream_state = SDW_STATE_DISABLE_STREAM;
 			}
 
-			ret = sdw_cfg_bs_params(sdw_mstr_bs,
-					sdw_mstr_bs_rt, false);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"xport params config failed\n");
-				return ret;
-			}
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+						mstr_sdw_node) {
 
-			sel_col = sdw_mstr_bs->col;
-			sel_row = sdw_mstr_bs->row;
+				if (mstr_rt_act->mstr == NULL)
+					break;
 
-			/* Configure frame shape/Switch Bank  */
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+				ops = mstr_bs_act->mstr->driver->mstr_port_ops;
+
+				/* Run for all mstr_list and
+				 * post_activate ports
+				 */
+				if (ops->dpn_port_activate_ch_post) {
+					ret = ops->dpn_port_activate_ch_post
+						(mstr_bs_act->mstr, NULL, 0);
+					if (ret < 0)
+						return ret;
+				}
+
+			}
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+					mstr_sdw_node) {
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+				ret = sdw_configure_frmshp_bnkswtch_mm_wait(
+							mstr_bs_act);
+			}
+		}
+		if (!sdw_mstr->link_sync_mask) {
+
+			/* Configure Frame Shape/Switch Bank */
 			ret = sdw_configure_frmshp_bnkswtch(sdw_mstr_bs,
 					sel_col, sel_row);
 			if (ret < 0) {
 				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"bank switch failed\n");
+				dev_err(&sdw_mstr->dev, "bank switch failed\n");
 				return ret;
 			}
+		}
+		/* Disable all channels enabled on previous bank */
+		ret = sdw_dis_chan(sdw_mstr_bs, sdw_mstr_bs_rt);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "Channel disabled failed\n");
+			return ret;
+		}
+	}
+unprepare_stream:
+	list_for_each_entry(sdw_mstr_rt,
+				&sdw_rt->mstr_rt_list, mstr_sdw_node) {
+		if (sdw_mstr_rt->mstr == NULL)
+			break;
 
-			/* Disable all channels enabled on previous bank */
-			ret = sdw_dis_chan(sdw_mstr_bs, sdw_mstr_bs_rt);
+
+		last_rt = list_last_entry(&sdw_rt->mstr_rt_list,
+				struct sdw_mstr_runtime, mstr_sdw_node);
+		if (sdw_mstr_rt == last_rt)
+			last_node = true;
+		else
+			last_node = false;
+
+		/* Get bus structure for master */
+		sdw_mstr_bs = master_to_bus(sdw_mstr_rt->mstr);
+		sdw_mstr = sdw_mstr_bs->mstr;
+
+
+		sdw_mstr_cap = &sdw_mstr_bs->mstr->mstr_capabilities;
+		mstr_params = &sdw_mstr_rt->stream_params;
+
+		if ((!unprepare) ||
+			(sdw_rt->stream_state != SDW_STATE_DISABLE_STREAM))
+			return 0;
+
+		/* 1. Un-prepare master and slave port */
+		list_for_each_entry(sdw_mstr_bs_rt, &sdw_mstr->mstr_rt_list,
+						mstr_node) {
+			if (sdw_mstr_bs_rt->mstr == NULL)
+				continue;
+			ret = sdw_prep_unprep_mstr_slv(sdw_mstr_bs,
+					sdw_rt, false);
 			if (ret < 0) {
 				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"Channel disabled failed\n");
+				dev_err(&sdw_mstr->dev, "Ch unprep failed\n");
 				return ret;
 			}
-		}
 
-		if ((unprepare) &&
-				(SDW_STATE_DISABLE_STREAM ==
-				 sdw_rt->stream_state)) {
+			/* change stream state to unprepare */
+			if (last_node)
+				sdw_rt->stream_state =
+					SDW_STATE_UNPREPARE_STREAM;
+		}
 
-			/* 1. Un-prepare master and slave port */
-			list_for_each_entry(sdw_mstr_bs_rt,
-					&sdw_mstr->mstr_rt_list, mstr_node) {
+		/*
+		 * Calculate new bandwidth, frame size
+		 * and total BW required for master controller
+		 */
+		sdw_mstr_rt->stream_bw = mstr_params->rate *
+			mstr_params->channel_count * mstr_params->bps;
+		stream_frame_size = mstr_params->channel_count *
+			mstr_params->bps;
 
-				if (sdw_mstr_bs_rt->mstr == NULL)
-					continue;
+		sdw_mstr_bs->bandwidth -= sdw_mstr_rt->stream_bw;
 
-				ret = sdw_prep_unprep_mstr_slv(sdw_mstr_bs,
-						sdw_rt, false);
-				if (ret < 0) {
-					/* TBD: Undo all the computation */
-					dev_err(&sdw_mstr_bs->mstr->dev,
-							"Chan unprep failed\n");
-					return ret;
-				}
-
-				/* change stream state to unprepare */
-				sdw_rt->stream_state =
-					SDW_STATE_UNPREPARE_STREAM;
-			}
+		/* Something went wrong in bandwidth calulation */
+		if (sdw_mstr_bs->bandwidth < 0) {
+			dev_err(&sdw_mstr->dev, "BW calculation failed\n");
+			return -EINVAL;
+		}
 
+		if (!sdw_mstr_bs->bandwidth) {
 			/*
-			 * Calculate new bandwidth, frame size
-			 * and total BW required for master controller
+			 * Last stream on master should
+			 * return successfully
 			 */
-			sdw_mstr_rt->stream_bw = mstr_params->rate *
-				mstr_params->channel_count * mstr_params->bps;
-			stream_frame_size = mstr_params->channel_count *
-				mstr_params->bps;
+			if (last_node)
+				sdw_rt->stream_state =
+						SDW_STATE_UNCOMPUTE_STREAM;
+			continue;
+		}
 
-			sdw_mstr_bs->bandwidth -= sdw_mstr_rt->stream_bw;
 
-			/* Something went wrong in bandwidth calulation */
-			if (sdw_mstr_bs->bandwidth < 0) {
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"BW calculation failed\n");
-				return -EINVAL;
-			}
+		ret = sdw_get_clock_frmshp(sdw_mstr_bs, &frame_interval,
+							&sel_col, &sel_row);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "clock/frameshape failed\n");
+			return ret;
+		}
+
+		/* Compute new transport params for running streams */
+		/* No sorting required here */
+
+		/* Compute system interval */
+		ret = sdw_compute_sys_interval(sdw_mstr_bs, sdw_mstr_cap,
+						frame_interval);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "compute SI failed\n");
+			return ret;
+		}
+
+		/* Compute hstart/hstop */
+		ret = sdw_compute_hstart_hstop(sdw_mstr_bs, sel_col);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "compute hstart/hstop fail\n");
+			return ret;
+		}
+
+		/* Compute block offset */
+		ret = sdw_compute_blk_subblk_offset(sdw_mstr_bs);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "compute block offset failed\n");
+			return ret;
+		}
+
+		/* Configure bus params */
+		ret = sdw_cfg_bs_params(sdw_mstr_bs, sdw_mstr_bs_rt, true);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr->dev, "xport params config failed\n");
+			return ret;
+		}
+		if ((last_node) && (sdw_mstr->link_sync_mask)) {
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+					mstr_sdw_node) {
+
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+
+				ops = mstr_bs_act->mstr->driver->mstr_port_ops;
 
-			if (!sdw_mstr_bs->bandwidth) {
 				/*
-				 * Last stream on master should
-				 * return successfully
+				 * Run for all mstr_list and
+				 * pre_activate ports
 				 */
-				sdw_rt->stream_state =
-					SDW_STATE_UNCOMPUTE_STREAM;
-				return 0;
+				if (ops->dpn_port_activate_ch_pre) {
+					ret = ops->dpn_port_activate_ch_pre
+						(mstr_bs_act->mstr, NULL, 0);
+					if (ret < 0)
+						return ret;
+				}
 			}
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+						mstr_sdw_node) {
 
-			ret = sdw_get_clock_frmshp(sdw_mstr_bs,
-					&frame_interval, &sel_col, &sel_row);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"clock/frameshape failed\n");
-				return ret;
-			}
+				if (mstr_rt_act->mstr == NULL)
+					break;
 
-			/* Compute new transport params for running streams */
-			/* No sorting required here */
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(
+					mstr_rt_act->mstr);
 
-			/* Compute system interval */
-			ret = sdw_compute_sys_interval(sdw_mstr_bs,
-					sdw_mstr_cap, frame_interval);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"compute SI failed\n");
-				return ret;
+				/* Configure Frame Shape/Switch Bank */
+				ret = sdw_configure_frmshp_bnkswtch_mm(
+						mstr_bs_act,
+						sel_col, sel_row);
+				if (ret < 0) {
+					/* TBD: Undo all the computation */
+					dev_err(&sdw_mstr->dev,
+							"bank switch failed\n");
+					return ret;
+				}
 			}
 
-			/* Compute hstart/hstop */
-			ret = sdw_compute_hstart_hstop(sdw_mstr_bs, sel_col);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"compute hstart/hstop fail\n");
-				return ret;
-			}
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+						mstr_sdw_node) {
 
-			/* Compute block offset */
-			ret = sdw_compute_blk_subblk_offset(sdw_mstr_bs);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"compute block offset failed\n");
-				return ret;
-			}
 
-			/* Configure bus params */
-			ret = sdw_cfg_bs_params(sdw_mstr_bs,
-					sdw_mstr_bs_rt, true);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"xport params config failed\n");
-				return ret;
+				if (mstr_rt_act->mstr == NULL)
+					break;
+
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+
+				ops = mstr_bs_act->mstr->driver->mstr_port_ops;
+
+				/* Run for all mstr_list and
+				 * post_activate ports
+				 */
+				if (ops->dpn_port_activate_ch_post) {
+					ret = ops->dpn_port_activate_ch_post
+						(mstr_bs_act->mstr, NULL, 0);
+					if (ret < 0)
+						return ret;
+				}
 			}
+			list_for_each_entry(mstr_rt_act, &sdw_rt->mstr_rt_list,
+							mstr_sdw_node) {
+
+				if (mstr_rt_act->mstr == NULL)
+					break;
 
+				/* Get bus structure for master */
+				mstr_bs_act = master_to_bus(mstr_rt_act->mstr);
+				ret = sdw_configure_frmshp_bnkswtch_mm_wait(
+							mstr_bs_act);
+			}
+		}
+		if (!sdw_mstr->link_sync_mask) {
 			/* Configure Frame Shape/Switch Bank */
 			ret = sdw_configure_frmshp_bnkswtch(sdw_mstr_bs,
 					sel_col, sel_row);
 			if (ret < 0) {
 				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"bank switch failed\n");
+				dev_err(&sdw_mstr->dev, "bank switch failed\n");
 				return ret;
 			}
 
-			/* Change stream state to uncompute */
+		}
+		/* Change stream state to uncompute */
+		if (last_node)
 			sdw_rt->stream_state = SDW_STATE_UNCOMPUTE_STREAM;
 
-			/* Disable all channels enabled on previous bank */
-			ret = sdw_dis_chan(sdw_mstr_bs, sdw_mstr_bs_rt);
-			if (ret < 0) {
-				/* TBD: Undo all the computation */
-				dev_err(&sdw_mstr_bs->mstr->dev,
-						"Channel disabled failed\n");
-				return ret;
-			}
+		/* Disable all channels enabled on previous bank */
+		ret = sdw_dis_chan(sdw_mstr_bs, sdw_mstr_bs_rt);
+		if (ret < 0) {
+			/* TBD: Undo all the computation */
+			dev_err(&sdw_mstr_bs->mstr->dev,
+					"Channel disabled failed\n");
+			return ret;
 		}
-
 	}
 
 	return 0;
-- 
https://clearlinux.org