[PATCH 10/11] soundwire: amd: add ACP7x generic bandwidth allocation support

[Vijendar Mukunda <Vijendar.Mukunda@amd.com>]

Add amd_acp7x_sdw_compute_bus_params() and
amd_acp7x_sdw_compute_params() for ACP7B/7E/7F managers.

The bus parameter computation performs dynamic frame-shape selection
across candidate clock frequencies, using sdw_check_column_allocation()
as a pre-check before accepting a clock rate to avoid configurations
that cannot satisfy all active stream bandwidth constraints.
ACP7x managers register amd_acp7x_sdw_compute_params as their
bus.compute_params callback.

Add and export sdw_check_column_allocation() in
generic_bandwidth_allocation.c, and declare it in bus.h, so this
ACP7x-specific code can call it across the module boundary.

Signed-off-by: Poojitha N <Poojitha.N@amd.com>
Signed-off-by: Vijendar Mukunda <Vijendar.Mukunda@amd.com>
---
 drivers/soundwire/amd_acp7x_manager.c         | 191 ++++++++++++++++++
 drivers/soundwire/amd_manager.h               |   1 +
 drivers/soundwire/amd_manager_common.c        |   1 +
 drivers/soundwire/bus.h                       |   4 +
 .../soundwire/generic_bandwidth_allocation.c  |  95 +++++++++
 5 files changed, 292 insertions(+)

diff --git a/drivers/soundwire/amd_acp7x_manager.c b/drivers/soundwire/amd_acp7x_manager.c
index 721d4e97371b..9605577a8d35 100644
--- a/drivers/soundwire/amd_acp7x_manager.c
+++ b/drivers/soundwire/amd_acp7x_manager.c
@@ -921,6 +921,197 @@ static int amd_acp7x_sdw_post_bank_switch(struct sdw_bus *bus)
 	return 0;
 }
 
+/*
+ * Reuses grouping, lane-selection, and transport helpers exported by
+ * the generic bandwidth allocator (see bus.h) for ACP7x bus params.
+ */
+
+/**
+ * amd_acp7x_sdw_compute_bus_params() - Compute bus parameters for AMD ACP7x
+ *
+ * @bus: SDW Bus instance
+ */
+static int amd_acp7x_sdw_compute_bus_params(struct sdw_bus *bus,
+					    struct sdw_stream_runtime *stream)
+{
+	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;
+	struct sdw_slave_runtime *s_rt;
+	unsigned int curr_dr_freq = 0;
+	unsigned int old_default_row, old_default_col;
+	unsigned int default_row = 0, default_col = 0;
+	int i, l, clk_values, ret;
+	bool is_gear = false;
+	int m_lane = 0;
+	u32 *clk_buf;
+
+	if (mstr_prop->num_clk_gears) {
+		clk_values = mstr_prop->num_clk_gears;
+		clk_buf = mstr_prop->clk_gears;
+		is_gear = true;
+	} else if (mstr_prop->num_clk_freq) {
+		clk_values = mstr_prop->num_clk_freq;
+		clk_buf = mstr_prop->clk_freq;
+	} else {
+		clk_values = 1;
+		clk_buf = NULL;
+	}
+	if (!is_clock_scaling_supported(bus))
+		clk_values = 1;
+
+	if (!mstr_prop->default_frame_rate ||
+	    (!mstr_prop->dynamic_frame &&
+	     (!mstr_prop->default_row || !mstr_prop->default_col)))
+		return -EINVAL;
+	old_default_row = mstr_prop->default_row;
+	old_default_col = mstr_prop->default_col;
+
+	for (i = 0; i < clk_values; i++) {
+		if (!clk_buf) {
+			curr_dr_freq = bus->params.max_dr_freq;
+		} else {
+			curr_dr_freq = is_gear ?
+				(bus->params.max_dr_freq / clk_buf[i]) :
+				clk_buf[i] * SDW_DOUBLE_RATE_FACTOR;
+		}
+
+		if (mstr_prop->dynamic_frame) {
+			switch (curr_dr_freq) {
+			case 24000000:
+				default_row = 50;
+				default_col = 10;
+				break;
+			case 12000000:
+				default_row = 125;
+				default_col = 2;
+				break;
+			case 24576000:
+				default_row = 64;
+				default_col = 8;
+				break;
+			case 12288000:
+				default_row = 64;
+				default_col = 4;
+				break;
+			case 6144000:
+				default_row = 64;
+				default_col = 2;
+				break;
+			default:
+				dev_dbg(bus->dev, "unsupported clock frequency %u, skipping\n",
+					curr_dr_freq);
+				continue;
+			}
+		} else {
+			/* Static frame row and column are provided as a pair by ACPI. */
+			default_row = mstr_prop->default_row;
+			default_col = mstr_prop->default_col;
+		}
+		/*
+		 * The row and column pair is complete here: dynamic values are
+		 * selected by frequency above, while static values come from ACPI.
+		 */
+
+		dev_dbg(bus->dev, "Trying clock frequency %d bandwidth:%d default_col:%d\n",
+			curr_dr_freq, bus->params.bandwidth, default_col);
+
+		if (curr_dr_freq * (default_col - 1) >=
+		    bus->params.bandwidth * default_col) {
+			ret = sdw_check_column_allocation(bus, stream, curr_dr_freq,
+							  default_col);
+			if (ret < 0)
+				return ret;
+			if (ret)
+				break;
+			dev_dbg(bus->dev, "freq %d: column allocation failed\n",
+				curr_dr_freq);
+		}
+		/* Try multi-lane when single-lane at this frequency is insufficient. */
+
+		list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
+			/*
+			 * Get the first s_rt that will be used to find the available lane that
+			 * can be used. No need to check all Peripherals because we can't use
+			 * multi-lane if we can't find any available lane for the first Peripheral.
+			 */
+			if (list_empty(&m_rt->slave_rt_list))
+				continue;
+
+			s_rt = list_first_entry(&m_rt->slave_rt_list,
+						struct sdw_slave_runtime, m_rt_node);
+			m_lane = get_manager_lane(bus, m_rt, s_rt, curr_dr_freq);
+			if (m_lane > 0)
+				goto out;
+		}
+	}
+	if (i == clk_values) {
+		dev_err(bus->dev, "%s: could not find clock value for bandwidth %d\n",
+			__func__, bus->params.bandwidth);
+		return -EINVAL;
+	}
+out:
+	if (m_lane > 0) {
+		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) {
+					list_for_each_entry(s_p_rt, &s_rt->port_list, port_node) {
+						s_p_rt->lane = l;
+					}
+					break;
+				}
+			}
+		}
+		list_for_each_entry(m_p_rt, &m_rt->port_list, port_node)
+			m_p_rt->lane = m_lane;
+	}
+
+	mstr_prop->default_row = default_row;
+	mstr_prop->default_col = default_col;
+	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);
+		mstr_prop->default_row = old_default_row;
+		mstr_prop->default_col = old_default_col;
+		return -EINVAL;
+	}
+	bus->params.curr_dr_freq = curr_dr_freq;
+	return 0;
+}
+
+/**
+ * amd_acp7x_sdw_compute_params() - Compute bus, transport and port parameters
+ * for AMD ACP7x
+ *
+ * @bus: SDW Bus instance
+ * @stream: Soundwire stream
+ */
+int amd_acp7x_sdw_compute_params(struct sdw_bus *bus, struct sdw_stream_runtime *stream)
+{
+	int ret;
+
+	ret = amd_acp7x_sdw_compute_bus_params(bus, stream);
+	if (ret < 0)
+		return ret;
+
+	if (stream->type == SDW_STREAM_BPT) {
+		sdw_compute_dp0_port_params(bus);
+		return 0;
+	}
+
+	ret = sdw_compute_port_params(bus, stream);
+	if (ret < 0) {
+		dev_err(bus->dev, "Compute transport params failed: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
 const struct sdw_master_port_ops amd_acp7x_sdw_port_ops = {
 	.dpn_set_port_params = amd_acp7x_sdw_port_params,
 	.dpn_set_port_transport_params = amd_acp7x_sdw_transport_params,
diff --git a/drivers/soundwire/amd_manager.h b/drivers/soundwire/amd_manager.h
index f0d3c5684744..c3a7db663772 100644
--- a/drivers/soundwire/amd_manager.h
+++ b/drivers/soundwire/amd_manager.h
@@ -510,6 +510,7 @@ void amd_acp7x_sdw_update_slave_status_work(struct amd_sdw_manager *amd_manager)
 int amd_sdw_hw_init_ops_acp7x(struct amd_sdw_manager *amd_manager);
 extern const struct sdw_master_ops amd_acp7x_sdw_ops;
 extern const struct sdw_master_port_ops amd_acp7x_sdw_port_ops;
+int amd_acp7x_sdw_compute_params(struct sdw_bus *bus, struct sdw_stream_runtime *stream);
 
 #define AMD_SDW_HW_OPS(amd_manager, cb)	((amd_manager)->hw_ops->cb)
 
diff --git a/drivers/soundwire/amd_manager_common.c b/drivers/soundwire/amd_manager_common.c
index 03c92b6991f2..0920775a8fe1 100644
--- a/drivers/soundwire/amd_manager_common.c
+++ b/drivers/soundwire/amd_manager_common.c
@@ -862,6 +862,7 @@ static int amd_sdw_manager_probe(struct platform_device *pdev)
 		/* mmio already set in the port-count switch above */
 		amd_manager->bus.ops = &amd_acp7x_sdw_ops;
 		amd_manager->bus.port_ops = &amd_acp7x_sdw_port_ops;
+		amd_manager->bus.compute_params = &amd_acp7x_sdw_compute_params;
 		break;
 	default:
 		dev_err(dev, "unsupported ACP revision: 0x%x\n", amd_manager->acp_rev);
diff --git a/drivers/soundwire/bus.h b/drivers/soundwire/bus.h
index 0c5c343b2c1c..bcc7aa7e3a0e 100644
--- a/drivers/soundwire/bus.h
+++ b/drivers/soundwire/bus.h
@@ -245,6 +245,10 @@ void sdw_compute_dp0_port_params(struct sdw_bus *bus);
 int sdw_compute_port_params(struct sdw_bus *bus, struct sdw_stream_runtime *stream);
 int sdw_select_row_col(struct sdw_bus *bus, int clk_freq);
 bool is_clock_scaling_supported(struct sdw_bus *bus);
+int sdw_check_column_allocation(struct sdw_bus *bus,
+				struct sdw_stream_runtime *stream,
+				unsigned int curr_dr_freq,
+				int sel_col);
 int get_manager_lane(struct sdw_bus *bus, struct sdw_master_runtime *m_rt,
 		     struct sdw_slave_runtime *s_rt, unsigned int curr_dr_freq);
 
diff --git a/drivers/soundwire/generic_bandwidth_allocation.c b/drivers/soundwire/generic_bandwidth_allocation.c
index cf2d2275b6aa..44e4c0f78d9c 100644
--- a/drivers/soundwire/generic_bandwidth_allocation.c
+++ b/drivers/soundwire/generic_bandwidth_allocation.c
@@ -473,6 +473,101 @@ bool is_clock_scaling_supported(struct sdw_bus *bus)
 }
 EXPORT_SYMBOL(is_clock_scaling_supported);
 
+/**
+ * sdw_check_column_allocation - Pre-check if column allocation will succeed
+ * @bus: SDW Bus instance
+ * @stream: SoundWire stream being prepared
+ * @curr_dr_freq: Current double-rate frequency to check
+ * @sel_col: Number of columns
+ *
+ * Return: 1 if allocation succeeds, 0 if it does not fit, or a negative error.
+ */
+int sdw_check_column_allocation(struct sdw_bus *bus,
+				struct sdw_stream_runtime *stream,
+				unsigned int curr_dr_freq,
+				int sel_col)
+{
+	struct sdw_master_runtime *m_rt;
+	struct sdw_port_runtime *p_rt;
+	struct sdw_group group = {};
+	unsigned int rate, bps, ch;
+	int *payload_bw = NULL;
+	int *full_bw = NULL;
+	int i, l, ret;
+	int result = 1;
+
+	ret = sdw_get_group_count(bus, &group);
+	if (ret < 0)
+		return ret;
+	if (!group.count)
+		goto out;
+
+	payload_bw = kcalloc(group.count, sizeof(int), GFP_KERNEL);
+	full_bw = kcalloc(group.count, sizeof(int), GFP_KERNEL);
+	if (!payload_bw || !full_bw) {
+		result = -ENOMEM;
+		goto out;
+	}
+
+	for (i = 0; i < group.count; i++)
+		full_bw[i] = curr_dr_freq / group.rates[i];
+
+	list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
+		if (m_rt->stream == stream) {
+			if (stream->state != SDW_STREAM_CONFIGURED)
+				continue;
+		} else {
+			if (m_rt->stream->state != SDW_STREAM_ENABLED &&
+			    m_rt->stream->state != SDW_STREAM_PREPARED &&
+			    m_rt->stream->state != SDW_STREAM_DISABLED)
+				continue;
+		}
+
+		list_for_each_entry(p_rt, &m_rt->port_list, port_node) {
+			rate = m_rt->stream->params.rate;
+			bps = m_rt->stream->params.bps;
+			ch = hweight32(p_rt->ch_mask);
+
+			for (i = 0; i < group.count; i++) {
+				if (rate == group.rates[i] && p_rt->lane == group.lanes[i])
+					payload_bw[i] += bps * ch;
+			}
+		}
+	}
+
+	for (l = 0; l < SDW_MAX_LANES; l++) {
+		int available_columns = l ? sel_col : sel_col - 1;
+		int column_needed = 0;
+
+		for (i = 0; i < group.count; i++) {
+			int hwidth;
+
+			if (group.lanes[i] != l || !payload_bw[i])
+				continue;
+
+			/* Include the column consumed by the skipped block offset. */
+			hwidth = sdw_compute_hwidth(sel_col, payload_bw[i],
+						    full_bw[i]);
+			column_needed += hwidth;
+		}
+
+		if (column_needed > available_columns) {
+			dev_dbg(bus->dev, "Lane %d column check failed: required=%d available=%d\n",
+				l, column_needed, available_columns);
+			result = 0;
+			break;
+		}
+	}
+
+out:
+	kfree(payload_bw);
+	kfree(full_bw);
+	kfree(group.rates);
+	kfree(group.lanes);
+	return result;
+}
+EXPORT_SYMBOL(sdw_check_column_allocation);
+
 /**
  * is_lane_connected_to_all_peripherals: Check if the given manager lane connects to all peripherals
  * So that all peripherals can use the manager lane.
-- 
2.53.0