// 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_PKG_PER_PORT, 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; 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) { 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_PKG_PER_PORT, 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; 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, ¶ms[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; 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) { dev_err(bus->dev, "%s: could not find clock value for bandwidth %d\n", __func__, bus->params.bandwidth); return -EINVAL; } 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; } 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) 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\n", ret); return ret; } return 0; } EXPORT_SYMBOL(sdw_compute_params); MODULE_LICENSE("Dual BSD/GPL"); MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation");