forked from Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
428 lines
9.6 KiB
428 lines
9.6 KiB
// 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");
|
|
|