1 files changed, 425 insertions, 0 deletions

diff --git a/drivers/soundwire/generic_bandwidth_allocation.c b/drivers/soundwire/generic_bandwidth_allocation.c
new file mode 100644
index 000000000000..0bdef38c9a30
--- /dev/null
+++ b/drivers/soundwire/generic_bandwidth_allocation.c
@@ -0,0 +1,425 @@
+// SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause)
+// Copyright(c) 2015-2020 Intel Corporation.
+
+/*
+ * Bandwidth management algorithm based on 2^n gears
+ *
+ */
+
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/mod_devicetable.h>
+#include <linux/slab.h>
+#include <linux/soundwire/sdw.h>
+#include "bus.h"
+
+#define SDW_STRM_RATE_GROUPING		1
+
+struct sdw_group_params {
+	unsigned int rate;
+	int full_bw;
+	int payload_bw;
+	int hwidth;
+};
+
+struct sdw_group {
+	unsigned int count;
+	unsigned int max_size;
+	unsigned int *rates;
+};
+
+struct sdw_transport_data {
+	int hstart;
+	int hstop;
+	int block_offset;
+	int sub_block_offset;
+};
+
+static void sdw_compute_slave_ports(struct sdw_master_runtime *m_rt,
+				    struct sdw_transport_data *t_data)
+{
+	struct sdw_slave_runtime *s_rt = NULL;
+	struct sdw_port_runtime *p_rt;
+	int port_bo, sample_int;
+	unsigned int rate, bps, ch = 0;
+	unsigned int slave_total_ch;
+	struct sdw_bus_params *b_params = &m_rt->bus->params;
+
+	port_bo = t_data->block_offset;
+
+	list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
+		rate = m_rt->stream->params.rate;
+		bps = m_rt->stream->params.bps;
+		sample_int = (m_rt->bus->params.curr_dr_freq / rate);
+		slave_total_ch = 0;
+
+		list_for_each_entry(p_rt, &s_rt->port_list, port_node) {
+			ch = sdw_ch_mask_to_ch(p_rt->ch_mask);
+
+			sdw_fill_xport_params(&p_rt->transport_params,
+					      p_rt->num, false,
+					      SDW_BLK_GRP_CNT_1,
+					      sample_int, port_bo, port_bo >> 8,
+					      t_data->hstart,
+					      t_data->hstop,
+					      (SDW_BLK_GRP_CNT_1 * ch), 0x0);
+
+			sdw_fill_port_params(&p_rt->port_params,
+					     p_rt->num, bps,
+					     SDW_PORT_FLOW_MODE_ISOCH,
+					     b_params->s_data_mode);
+
+			port_bo += bps * ch;
+			slave_total_ch += ch;
+		}
+
+		if (m_rt->direction == SDW_DATA_DIR_TX &&
+		    m_rt->ch_count == slave_total_ch) {
+			/*
+			 * Slave devices were configured to access all channels
+			 * of the stream, which indicates that they operate in
+			 * 'mirror mode'. Make sure we reset the port offset for
+			 * the next device in the list
+			 */
+			port_bo = t_data->block_offset;
+		}
+	}
+}
+
+static void sdw_compute_master_ports(struct sdw_master_runtime *m_rt,
+				     struct sdw_group_params *params,
+				     int port_bo, int hstop)
+{
+	struct sdw_transport_data t_data = {0};
+	struct sdw_port_runtime *p_rt;
+	struct sdw_bus *bus = m_rt->bus;
+	struct sdw_bus_params *b_params = &bus->params;
+	int sample_int, hstart = 0;
+	unsigned int rate, bps, ch, no_ch;
+
+	rate = m_rt->stream->params.rate;
+	bps = m_rt->stream->params.bps;
+	ch = m_rt->ch_count;
+	sample_int = (bus->params.curr_dr_freq / rate);
+
+	if (rate != params->rate)
+		return;
+
+	t_data.hstop = hstop;
+	hstart = hstop - params->hwidth + 1;
+	t_data.hstart = hstart;
+
+	list_for_each_entry(p_rt, &m_rt->port_list, port_node) {
+		no_ch = sdw_ch_mask_to_ch(p_rt->ch_mask);
+
+		sdw_fill_xport_params(&p_rt->transport_params, p_rt->num,
+				      false, SDW_BLK_GRP_CNT_1, sample_int,
+				      port_bo, port_bo >> 8, hstart, hstop,
+				      (SDW_BLK_GRP_CNT_1 * no_ch), 0x0);
+
+		sdw_fill_port_params(&p_rt->port_params,
+				     p_rt->num, bps,
+				     SDW_PORT_FLOW_MODE_ISOCH,
+				     b_params->m_data_mode);
+
+		/* Check for first entry */
+		if (!(p_rt == list_first_entry(&m_rt->port_list,
+					       struct sdw_port_runtime,
+					       port_node))) {
+			port_bo += bps * ch;
+			continue;
+		}
+
+		t_data.hstart = hstart;
+		t_data.hstop = hstop;
+		t_data.block_offset = port_bo;
+		t_data.sub_block_offset = 0;
+		port_bo += bps * ch;
+	}
+
+	sdw_compute_slave_ports(m_rt, &t_data);
+}
+
+static void _sdw_compute_port_params(struct sdw_bus *bus,
+				     struct sdw_group_params *params, int count)
+{
+	struct sdw_master_runtime *m_rt = NULL;
+	int hstop = bus->params.col - 1;
+	int block_offset, port_bo, i;
+
+	/* Run loop for all groups to compute transport parameters */
+	for (i = 0; i < count; i++) {
+		port_bo = 1;
+		block_offset = 1;
+
+		list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
+			sdw_compute_master_ports(m_rt, &params[i],
+						 port_bo, hstop);
+
+			block_offset += m_rt->ch_count *
+					m_rt->stream->params.bps;
+			port_bo = block_offset;
+		}
+
+		hstop = hstop - params[i].hwidth;
+	}
+}
+
+static int sdw_compute_group_params(struct sdw_bus *bus,
+				    struct sdw_group_params *params,
+				    int *rates, int count)
+{
+	struct sdw_master_runtime *m_rt = NULL;
+	int sel_col = bus->params.col;
+	unsigned int rate, bps, ch;
+	int i, column_needed = 0;
+
+	/* Calculate bandwidth per group */
+	for (i = 0; i < count; i++) {
+		params[i].rate = rates[i];
+		params[i].full_bw = bus->params.curr_dr_freq / params[i].rate;
+	}
+
+	list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
+		rate = m_rt->stream->params.rate;
+		bps = m_rt->stream->params.bps;
+		ch = m_rt->ch_count;
+
+		for (i = 0; i < count; i++) {
+			if (rate == params[i].rate)
+				params[i].payload_bw += bps * ch;
+		}
+	}
+
+	for (i = 0; i < count; i++) {
+		params[i].hwidth = (sel_col *
+			params[i].payload_bw + params[i].full_bw - 1) /
+			params[i].full_bw;
+
+		column_needed += params[i].hwidth;
+	}
+
+	if (column_needed > sel_col - 1)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int sdw_add_element_group_count(struct sdw_group *group,
+				       unsigned int rate)
+{
+	int num = group->count;
+	int i;
+
+	for (i = 0; i <= num; i++) {
+		if (rate == group->rates[i])
+			break;
+
+		if (i != num)
+			continue;
+
+		if (group->count >= group->max_size) {
+			unsigned int *rates;
+
+			group->max_size += 1;
+			rates = krealloc(group->rates,
+					 (sizeof(int) * group->max_size),
+					 GFP_KERNEL);
+			if (!rates)
+				return -ENOMEM;
+			group->rates = rates;
+		}
+
+		group->rates[group->count++] = rate;
+	}
+
+	return 0;
+}
+
+static int sdw_get_group_count(struct sdw_bus *bus,
+			       struct sdw_group *group)
+{
+	struct sdw_master_runtime *m_rt;
+	unsigned int rate;
+	int ret = 0;
+
+	group->count = 0;
+	group->max_size = SDW_STRM_RATE_GROUPING;
+	group->rates = kcalloc(group->max_size, sizeof(int), GFP_KERNEL);
+	if (!group->rates)
+		return -ENOMEM;
+
+	list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
+		rate = m_rt->stream->params.rate;
+		if (m_rt == list_first_entry(&bus->m_rt_list,
+					     struct sdw_master_runtime,
+					     bus_node)) {
+			group->rates[group->count++] = rate;
+
+		} else {
+			ret = sdw_add_element_group_count(group, rate);
+			if (ret < 0) {
+				kfree(group->rates);
+				return ret;
+			}
+		}
+	}
+
+	return ret;
+}
+
+/**
+ * sdw_compute_port_params: Compute transport and port parameters
+ *
+ * @bus: SDW Bus instance
+ */
+static int sdw_compute_port_params(struct sdw_bus *bus)
+{
+	struct sdw_group_params *params = NULL;
+	struct sdw_group group;
+	int ret;
+
+	ret = sdw_get_group_count(bus, &group);
+	if (ret < 0)
+		return ret;
+
+	if (group.count == 0)
+		goto out;
+
+	params = kcalloc(group.count, sizeof(*params), GFP_KERNEL);
+	if (!params) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* Compute transport parameters for grouped streams */
+	ret = sdw_compute_group_params(bus, params,
+				       &group.rates[0], group.count);
+	if (ret < 0)
+		goto free_params;
+
+	_sdw_compute_port_params(bus, params, group.count);
+
+free_params:
+	kfree(params);
+out:
+	kfree(group.rates);
+
+	return ret;
+}
+
+static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq)
+{
+	struct sdw_master_prop *prop = &bus->prop;
+	int frame_int, frame_freq;
+	int r, c;
+
+	for (c = 0; c < SDW_FRAME_COLS; c++) {
+		for (r = 0; r < SDW_FRAME_ROWS; r++) {
+			if (sdw_rows[r] != prop->default_row ||
+			    sdw_cols[c] != prop->default_col)
+				continue;
+
+			frame_int = sdw_rows[r] * sdw_cols[c];
+			frame_freq = clk_freq / frame_int;
+
+			if ((clk_freq - (frame_freq * SDW_FRAME_CTRL_BITS)) <
+			    bus->params.bandwidth)
+				continue;
+
+			bus->params.row = sdw_rows[r];
+			bus->params.col = sdw_cols[c];
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+/**
+ * sdw_compute_bus_params: Compute bus parameters
+ *
+ * @bus: SDW Bus instance
+ */
+static int sdw_compute_bus_params(struct sdw_bus *bus)
+{
+	unsigned int max_dr_freq, curr_dr_freq = 0;
+	struct sdw_master_prop *mstr_prop = &bus->prop;
+	int i, clk_values, ret;
+	bool is_gear = false;
+	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;
+	}
+
+	max_dr_freq = mstr_prop->max_clk_freq * SDW_DOUBLE_RATE_FACTOR;
+
+	for (i = 0; i < clk_values; i++) {
+		if (!clk_buf)
+			curr_dr_freq = max_dr_freq;
+		else
+			curr_dr_freq = (is_gear) ?
+				(max_dr_freq >>  clk_buf[i]) :
+				clk_buf[i] * SDW_DOUBLE_RATE_FACTOR;
+
+		if (curr_dr_freq <= bus->params.bandwidth)
+			continue;
+
+		break;
+
+		/*
+		 * TODO: Check all the Slave(s) port(s) audio modes and find
+		 * whether given clock rate is supported with glitchless
+		 * transition.
+		 */
+	}
+
+	if (i == clk_values)
+		return -EINVAL;
+
+	ret = sdw_select_row_col(bus, curr_dr_freq);
+	if (ret < 0)
+		return -EINVAL;
+
+	bus->params.curr_dr_freq = curr_dr_freq;
+	return 0;
+}
+
+/**
+ * sdw_compute_params: Compute bus, transport and port parameters
+ *
+ * @bus: SDW Bus instance
+ */
+int sdw_compute_params(struct sdw_bus *bus)
+{
+	int ret;
+
+	/* Computes clock frequency, frame shape and frame frequency */
+	ret = sdw_compute_bus_params(bus);
+	if (ret < 0) {
+		dev_err(bus->dev, "Compute bus params failed: %d", ret);
+		return ret;
+	}
+
+	/* Compute transport and port params */
+	ret = sdw_compute_port_params(bus);
+	if (ret < 0) {
+		dev_err(bus->dev, "Compute transport params failed: %d", ret);
+		return ret;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(sdw_compute_params);
+
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation");