soundwire: generic_bandwidth_allocation: select data lane

If a peripheral supports multi-lane, we can use data lane x to extend
the bandwidth. 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>

drivers/soundwire/generic_bandwidth_allocation.c

@@ -326,18 +326,56 @@ static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq)
 	return -EINVAL;
 }
 
+/**
+ * check_all_peripherals_connected: Check if all peripherals can use the lane
+ *
+ * @m_rt: Manager runtime
+ * @lane: Lane number
+ */
+static bool check_all_peripherals_connected(struct sdw_master_runtime *m_rt, unsigned int lane)
+{
+	struct sdw_slave_prop *slave_prop;
+	struct sdw_slave_runtime *s_rt;
+	int i;
+
+	list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
+		slave_prop = &s_rt->slave->prop;
+		for (i = 1; i < SDW_MAX_LANES; i++) {
+			if (slave_prop->lane_maps[i] == lane) {
+				dev_dbg(&s_rt->slave->dev,
+					"lane_maps[%d] is connected to %d\n",
+					i, slave_prop->lane_maps[i]);
+				break;
+			}
+		}
+		if (i == SDW_MAX_LANES) {
+			dev_dbg(&s_rt->slave->dev, "%d is not connected\n", lane);
+			return false;
+		}
+	}
+	return true;
+}
+
 /**
  * sdw_compute_bus_params: Compute bus parameters
  *
  * @bus: SDW Bus instance
  */
 static int sdw_compute_bus_params(struct sdw_bus *bus)
 {
-	unsigned int curr_dr_freq = 0;
 	struct sdw_master_prop *mstr_prop = &bus->prop;
-	int i, clk_values, ret;
+	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, l, clk_values, ret;
 	bool is_gear = false;
 	u32 *clk_buf;
+	int m_lane;
 
 	if (mstr_prop->num_clk_gears) {
 		clk_values = mstr_prop->num_clk_gears;
@@ -372,16 +410,90 @@ 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",
+multilane:
+		dev_dbg(bus->dev, "%s: could not find clock value for bandwidth %d, checking multi-lane\n",
 			__func__, bus->params.bandwidth);
-		return -EINVAL;
+
+		m_rt = list_last_entry(&bus->m_rt_list, struct sdw_master_runtime, bus_node);
+		/*
+		 * Find available manager lanes that connected to the first Peripheral. No need
+		 * to check all Peripherals available lanes because we can't use multi-lane
+		 * if we can't find any available lane for the first Peripheral.
+		 */
+		s_rt = list_first_entry(&m_rt->slave_rt_list, struct sdw_slave_runtime, m_rt_node);
+		slave_prop = &s_rt->slave->prop;
+
+		for (l = 1; l < SDW_MAX_LANES; l++) {
+			if (!slave_prop->lane_maps[l])
+				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[l]) {
+				/* Check if m_lane is connected to all Peripherals */
+				if (!check_all_peripherals_connected(m_rt, slave_prop->lane_maps[l])) {
+					dev_dbg(bus->dev,
+						"some Peripherals are not connected to %d\n",
+						slave_prop->lane_maps[l]);
+					continue;
+				}
+				m_lane = slave_prop->lane_maps[l];
+				dev_dbg(&s_rt->slave->dev,
+					"M lane %d P lane %d can be used\n",
+					m_lane, l);
+				bus->lane_used_bandwidth[l] += required_bandwidth;
+				/*
+				 * Use non-zero manager lane, subtract the lane 0
+				 * bandwidth that is already calculated
+				 */
+				bus->params.bandwidth -= required_bandwidth;
+				use_multi_lane = true;
+				break;
+			}
+		}
+
+		if (!use_multi_lane) {
+			dev_err(bus->dev,
+				"%s: multilane is not available and could not find clock value for bandwidth %d\n",
+				__func__, bus->params.bandwidth);
+			return -EINVAL;
+		}
+
+		/* Set Peripheral lanes */
+		list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
+			slave_prop = &s_rt->slave->prop;
+			for (l = 1; l < SDW_MAX_LANES; l++) {
+				if (slave_prop->lane_maps[l] == m_lane) {
+					dev_dbg(&s_rt->slave->dev, "Set Peripheral lane = %d\n", l);
+					list_for_each_entry(s_p_rt, &s_rt->port_list, port_node) {
+						s_p_rt->lane = l;
+						break;
+					}
+				}
+			}
+		}
+		/*
+		 * Set Manager lanes. Configure the last m_rt in bus->m_rt_list only since
+		 * we don't want to touch other m_rts that are already working.
+		 */
+		list_for_each_entry(m_p_rt, &m_rt->port_list, port_node) {
+			m_p_rt->lane = m_lane;
+		}
 	}
 
 	ret = sdw_select_row_col(bus, curr_dr_freq);
 	if (ret < 0) {
-		dev_err(bus->dev, "%s: could not find frame configuration for bus dr_freq %d\n",
-			__func__, curr_dr_freq);
-		return -EINVAL;
+		if (use_multi_lane) {
+			dev_err(bus->dev,
+				"%s: could not find frame configuration for bus dr_freq %d\n",
+				__func__, curr_dr_freq);
+			return -EINVAL;
+		}
+		goto multilane;
 	}
 
 	bus->params.curr_dr_freq = curr_dr_freq;

drivers/soundwire/stream.c

@@ -1652,6 +1652,9 @@ EXPORT_SYMBOL(sdw_disable_stream);
 static int _sdw_deprepare_stream(struct sdw_stream_runtime *stream)
 {
 	struct sdw_master_runtime *m_rt;
+	struct sdw_port_runtime *p_rt;
+	unsigned int multi_lane_bandwidth;
+	unsigned int bandwidth;
 	struct sdw_bus *bus;
 	int ret = 0;
 
@@ -1665,12 +1668,24 @@ 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) {
+			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;
+		bandwidth = m_rt->stream->params.rate * m_rt->ch_count * m_rt->stream->params.bps;
+		bus->params.bandwidth -= bandwidth - multi_lane_bandwidth;
 
 		/* Compute params */
-		if (bus->compute_params) {
+		/* No need to compute params if bus->params.bandwidth is unchanged */
+		if (multi_lane_bandwidth != bandwidth && bus->compute_params) {
 			ret = bus->compute_params(bus);
 			if (ret < 0) {
 				dev_err(bus->dev, "Compute params failed: %d\n",