From aac2feb907f0395a94e8b855644912c65960fcd9 Mon Sep 17 00:00:00 2001
From: Bard Liao <yung-chuan.liao@linux.intel.com>
Date: Wed, 20 Mar 2024 18:45:31 +0800
Subject: [PATCH] soundwire: generic_bandwidth_allocation: select data lane

If a peripheral supports multi-lane, we can use data lane x to extend
the bendwidth. The patch suggests to select data lane x where x > 0
when bandwidth is not enough on data lane 0.

Signed-off-by: Bard Liao <yung-chuan.liao@linux.intel.com>
---
 .../soundwire/generic_bandwidth_allocation.c  | 57 ++++++++++++++++++-
 drivers/soundwire/stream.c                    | 15 +++++
 2 files changed, 69 insertions(+), 3 deletions(-)

diff --git a/drivers/soundwire/generic_bandwidth_allocation.c b/drivers/soundwire/generic_bandwidth_allocation.c
index 7172c80fb8561c..ebd17913fe63f7 100644
--- a/drivers/soundwire/generic_bandwidth_allocation.c
+++ b/drivers/soundwire/generic_bandwidth_allocation.c
@@ -333,10 +333,18 @@ static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq)
  */
 static int sdw_compute_bus_params(struct sdw_bus *bus)
 {
-	unsigned int curr_dr_freq = 0;
 	struct sdw_master_prop *mstr_prop = &bus->prop;
+	struct sdw_slave_prop *slave_prop;
+	struct sdw_port_runtime *m_p_rt;
+	struct sdw_port_runtime *s_p_rt;
+	struct sdw_master_runtime *m_rt;
+	unsigned int required_bandwidth;
+	struct sdw_slave_runtime *s_rt;
+	unsigned int curr_dr_freq = 0;
+	bool use_multi_lane = false;
 	int i, clk_values, ret;
 	bool is_gear = false;
+	int m_lane, s_lane;
 	u32 *clk_buf;
 
 	if (mstr_prop->num_clk_gears) {
@@ -372,9 +380,52 @@ static int sdw_compute_bus_params(struct sdw_bus *bus)
 	}
 
 	if (i == clk_values) {
-		dev_err(bus->dev, "%s: could not find clock value for bandwidth %d\n",
+		dev_dbg(bus->dev, "%s: could not find clock value for bandwidth %d, checking multi-lane\n",
 			__func__, bus->params.bandwidth);
-		return -EINVAL;
+
+		list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
+			list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
+				slave_prop = &s_rt->slave->prop;
+
+				/* Find a non-zero manager lane */
+				for (i = 1; i < SDW_MAX_LANES; i++) {
+					if (!slave_prop->lane_maps[i])
+						continue;
+
+					required_bandwidth = 0;
+					list_for_each_entry(m_p_rt, &m_rt->port_list, port_node) {
+						required_bandwidth += m_rt->stream->params.rate *
+							hweight32(m_p_rt->ch_mask) *
+							m_rt->stream->params.bps;
+					}
+					if (required_bandwidth <= curr_dr_freq - bus->lane_used_bandwidth[i]) {
+						s_lane = i;
+						m_lane = slave_prop->lane_maps[i];
+						dev_dbg(&s_rt->slave->dev,
+							"M lane %d P lane %d can be used\n",
+							m_lane, s_lane);
+						bus->lane_used_bandwidth[i] += required_bandwidth;
+						/*
+						 * Use non-zero manager lane, subtract the lane 0
+						 * bandwidth that is already calculated
+						 * */
+						use_multi_lane = true;
+						break;
+					}
+				}
+
+				list_for_each_entry(s_p_rt, &s_rt->port_list, port_node) {
+					s_p_rt->lane = s_lane;
+				}
+				list_for_each_entry(m_p_rt, &m_rt->port_list, port_node) {
+					m_p_rt->lane = m_lane;
+				}
+			}
+		}
+		if (!use_multi_lane) {
+			dev_err(bus->dev, "%s: could not find a useable lane\n", __func__);
+			return -EINVAL;
+		}
 	}
 
 	ret = sdw_select_row_col(bus, curr_dr_freq);
diff --git a/drivers/soundwire/stream.c b/drivers/soundwire/stream.c
index 7aa4900dcf3172..7dc5d07efde3d2 100644
--- a/drivers/soundwire/stream.c
+++ b/drivers/soundwire/stream.c
@@ -1641,7 +1641,9 @@ EXPORT_SYMBOL(sdw_disable_stream);
 
 static int _sdw_deprepare_stream(struct sdw_stream_runtime *stream)
 {
+	unsigned int multi_lane_bandwidth;
 	struct sdw_master_runtime *m_rt;
+	struct sdw_port_runtime *p_rt;
 	struct sdw_bus *bus;
 	int ret = 0;
 
@@ -1655,6 +1657,19 @@ static int _sdw_deprepare_stream(struct sdw_stream_runtime *stream)
 			return ret;
 		}
 
+		multi_lane_bandwidth = 0;
+
+		list_for_each_entry(p_rt, &m_rt->port_list, port_node) {
+			unsigned int bandwidth;
+
+			if (!p_rt->lane)
+				continue;
+
+			bandwidth = m_rt->stream->params.rate * hweight32(p_rt->ch_mask) *
+				m_rt->stream->params.bps;
+			multi_lane_bandwidth += bandwidth;
+			bus->lane_used_bandwidth[p_rt->lane] -= bandwidth;
+		}
 		/* TODO: Update this during Device-Device support */
 		bus->params.bandwidth -= m_rt->stream->params.rate *
 			m_rt->ch_count * m_rt->stream->params.bps;