1 // SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause)
2 // Copyright(c) 2015-2020 Intel Corporation.
5 * Bandwidth management algorithm based on 2^n gears
9 #include <linux/bitops.h>
10 #include <linux/device.h>
11 #include <linux/module.h>
12 #include <linux/mod_devicetable.h>
13 #include <linux/slab.h>
14 #include <linux/soundwire/sdw.h>
17 #define SDW_STRM_RATE_GROUPING 1
19 struct sdw_group_params {
28 unsigned int max_size;
32 void sdw_compute_slave_ports(struct sdw_master_runtime *m_rt,
33 struct sdw_transport_data *t_data)
35 struct sdw_slave_runtime *s_rt = NULL;
36 struct sdw_port_runtime *p_rt;
37 int port_bo, sample_int;
38 unsigned int rate, bps, ch = 0;
39 unsigned int slave_total_ch;
40 struct sdw_bus_params *b_params = &m_rt->bus->params;
42 port_bo = t_data->block_offset;
44 list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
45 rate = m_rt->stream->params.rate;
46 bps = m_rt->stream->params.bps;
47 sample_int = (m_rt->bus->params.curr_dr_freq / rate);
50 list_for_each_entry(p_rt, &s_rt->port_list, port_node) {
51 ch = hweight32(p_rt->ch_mask);
53 sdw_fill_xport_params(&p_rt->transport_params,
56 sample_int, port_bo, port_bo >> 8,
59 SDW_BLK_PKG_PER_PORT, 0x0);
61 sdw_fill_port_params(&p_rt->port_params,
63 SDW_PORT_FLOW_MODE_ISOCH,
64 b_params->s_data_mode);
70 if (m_rt->direction == SDW_DATA_DIR_TX &&
71 m_rt->ch_count == slave_total_ch) {
73 * Slave devices were configured to access all channels
74 * of the stream, which indicates that they operate in
75 * 'mirror mode'. Make sure we reset the port offset for
76 * the next device in the list
78 port_bo = t_data->block_offset;
82 EXPORT_SYMBOL(sdw_compute_slave_ports);
84 static void sdw_compute_master_ports(struct sdw_master_runtime *m_rt,
85 struct sdw_group_params *params,
86 int port_bo, int hstop)
88 struct sdw_transport_data t_data = {0};
89 struct sdw_port_runtime *p_rt;
90 struct sdw_bus *bus = m_rt->bus;
91 struct sdw_bus_params *b_params = &bus->params;
92 int sample_int, hstart = 0;
93 unsigned int rate, bps, ch;
95 rate = m_rt->stream->params.rate;
96 bps = m_rt->stream->params.bps;
98 sample_int = (bus->params.curr_dr_freq / rate);
100 if (rate != params->rate)
103 t_data.hstop = hstop;
104 hstart = hstop - params->hwidth + 1;
105 t_data.hstart = hstart;
107 list_for_each_entry(p_rt, &m_rt->port_list, port_node) {
109 sdw_fill_xport_params(&p_rt->transport_params, p_rt->num,
110 false, SDW_BLK_GRP_CNT_1, sample_int,
111 port_bo, port_bo >> 8, hstart, hstop,
112 SDW_BLK_PKG_PER_PORT, 0x0);
114 sdw_fill_port_params(&p_rt->port_params,
116 SDW_PORT_FLOW_MODE_ISOCH,
117 b_params->m_data_mode);
119 /* Check for first entry */
120 if (!(p_rt == list_first_entry(&m_rt->port_list,
121 struct sdw_port_runtime,
127 t_data.hstart = hstart;
128 t_data.hstop = hstop;
129 t_data.block_offset = port_bo;
130 t_data.sub_block_offset = 0;
134 sdw_compute_slave_ports(m_rt, &t_data);
137 static void _sdw_compute_port_params(struct sdw_bus *bus,
138 struct sdw_group_params *params, int count)
140 struct sdw_master_runtime *m_rt;
141 int hstop = bus->params.col - 1;
142 int block_offset, port_bo, i;
144 /* Run loop for all groups to compute transport parameters */
145 for (i = 0; i < count; i++) {
149 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
150 sdw_compute_master_ports(m_rt, ¶ms[i],
153 block_offset += m_rt->ch_count *
154 m_rt->stream->params.bps;
155 port_bo = block_offset;
158 hstop = hstop - params[i].hwidth;
162 static int sdw_compute_group_params(struct sdw_bus *bus,
163 struct sdw_group_params *params,
164 int *rates, int count)
166 struct sdw_master_runtime *m_rt;
167 int sel_col = bus->params.col;
168 unsigned int rate, bps, ch;
169 int i, column_needed = 0;
171 /* Calculate bandwidth per group */
172 for (i = 0; i < count; i++) {
173 params[i].rate = rates[i];
174 params[i].full_bw = bus->params.curr_dr_freq / params[i].rate;
177 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
178 rate = m_rt->stream->params.rate;
179 bps = m_rt->stream->params.bps;
182 for (i = 0; i < count; i++) {
183 if (rate == params[i].rate)
184 params[i].payload_bw += bps * ch;
188 for (i = 0; i < count; i++) {
189 params[i].hwidth = (sel_col *
190 params[i].payload_bw + params[i].full_bw - 1) /
193 column_needed += params[i].hwidth;
196 if (column_needed > sel_col - 1)
202 static int sdw_add_element_group_count(struct sdw_group *group,
205 int num = group->count;
208 for (i = 0; i <= num; i++) {
209 if (rate == group->rates[i])
215 if (group->count >= group->max_size) {
218 group->max_size += 1;
219 rates = krealloc(group->rates,
220 (sizeof(int) * group->max_size),
224 group->rates = rates;
227 group->rates[group->count++] = rate;
233 static int sdw_get_group_count(struct sdw_bus *bus,
234 struct sdw_group *group)
236 struct sdw_master_runtime *m_rt;
241 group->max_size = SDW_STRM_RATE_GROUPING;
242 group->rates = kcalloc(group->max_size, sizeof(int), GFP_KERNEL);
246 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
247 rate = m_rt->stream->params.rate;
248 if (m_rt == list_first_entry(&bus->m_rt_list,
249 struct sdw_master_runtime,
251 group->rates[group->count++] = rate;
254 ret = sdw_add_element_group_count(group, rate);
266 * sdw_compute_port_params: Compute transport and port parameters
268 * @bus: SDW Bus instance
270 static int sdw_compute_port_params(struct sdw_bus *bus)
272 struct sdw_group_params *params = NULL;
273 struct sdw_group group;
276 ret = sdw_get_group_count(bus, &group);
280 if (group.count == 0)
283 params = kcalloc(group.count, sizeof(*params), GFP_KERNEL);
289 /* Compute transport parameters for grouped streams */
290 ret = sdw_compute_group_params(bus, params,
291 &group.rates[0], group.count);
295 _sdw_compute_port_params(bus, params, group.count);
305 static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq)
307 struct sdw_master_prop *prop = &bus->prop;
308 int frame_int, frame_freq;
311 for (c = 0; c < SDW_FRAME_COLS; c++) {
312 for (r = 0; r < SDW_FRAME_ROWS; r++) {
313 if (sdw_rows[r] != prop->default_row ||
314 sdw_cols[c] != prop->default_col)
317 frame_int = sdw_rows[r] * sdw_cols[c];
318 frame_freq = clk_freq / frame_int;
320 if ((clk_freq - (frame_freq * SDW_FRAME_CTRL_BITS)) <
321 bus->params.bandwidth)
324 bus->params.row = sdw_rows[r];
325 bus->params.col = sdw_cols[c];
334 * sdw_compute_bus_params: Compute bus parameters
336 * @bus: SDW Bus instance
338 static int sdw_compute_bus_params(struct sdw_bus *bus)
340 unsigned int max_dr_freq, curr_dr_freq = 0;
341 struct sdw_master_prop *mstr_prop = &bus->prop;
342 int i, clk_values, ret;
343 bool is_gear = false;
346 if (mstr_prop->num_clk_gears) {
347 clk_values = mstr_prop->num_clk_gears;
348 clk_buf = mstr_prop->clk_gears;
350 } else if (mstr_prop->num_clk_freq) {
351 clk_values = mstr_prop->num_clk_freq;
352 clk_buf = mstr_prop->clk_freq;
358 max_dr_freq = mstr_prop->max_clk_freq * SDW_DOUBLE_RATE_FACTOR;
360 for (i = 0; i < clk_values; i++) {
362 curr_dr_freq = max_dr_freq;
364 curr_dr_freq = (is_gear) ?
365 (max_dr_freq >> clk_buf[i]) :
366 clk_buf[i] * SDW_DOUBLE_RATE_FACTOR;
368 if (curr_dr_freq <= bus->params.bandwidth)
374 * TODO: Check all the Slave(s) port(s) audio modes and find
375 * whether given clock rate is supported with glitchless
380 if (i == clk_values) {
381 dev_err(bus->dev, "%s: could not find clock value for bandwidth %d\n",
382 __func__, bus->params.bandwidth);
386 ret = sdw_select_row_col(bus, curr_dr_freq);
388 dev_err(bus->dev, "%s: could not find frame configuration for bus dr_freq %d\n",
389 __func__, curr_dr_freq);
393 bus->params.curr_dr_freq = curr_dr_freq;
398 * sdw_compute_params: Compute bus, transport and port parameters
400 * @bus: SDW Bus instance
402 int sdw_compute_params(struct sdw_bus *bus)
406 /* Computes clock frequency, frame shape and frame frequency */
407 ret = sdw_compute_bus_params(bus);
411 /* Compute transport and port params */
412 ret = sdw_compute_port_params(bus);
414 dev_err(bus->dev, "Compute transport params failed: %d\n", ret);
420 EXPORT_SYMBOL(sdw_compute_params);
422 MODULE_LICENSE("Dual BSD/GPL");
423 MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation");