1 // SPDX-License-Identifier: (GPL-2.0-only OR BSD-3-Clause)
2 // Copyright(c) 2015-2020 Intel Corporation.
3
4 /*
5 * Bandwidth management algorithm based on 2^n gears
6 *
7 */
8
9 #include <linux/device.h>
10 #include <linux/module.h>
11 #include <linux/mod_devicetable.h>
12 #include <linux/slab.h>
13 #include <linux/soundwire/sdw.h>
14 #include "bus.h"
15
16 #define SDW_STRM_RATE_GROUPING 1
17
18 struct sdw_group_params {
19 unsigned int rate;
20 int full_bw;
21 int payload_bw;
22 int hwidth;
23 };
24
25 struct sdw_group {
26 unsigned int count;
27 unsigned int max_size;
28 unsigned int *rates;
29 };
30
31 struct sdw_transport_data {
32 int hstart;
33 int hstop;
34 int block_offset;
35 int sub_block_offset;
36 };
37
sdw_compute_slave_ports(struct sdw_master_runtime * m_rt,struct sdw_transport_data * t_data)38 static void sdw_compute_slave_ports(struct sdw_master_runtime *m_rt,
39 struct sdw_transport_data *t_data)
40 {
41 struct sdw_slave_runtime *s_rt = NULL;
42 struct sdw_port_runtime *p_rt;
43 int port_bo, sample_int;
44 unsigned int rate, bps, ch = 0;
45 unsigned int slave_total_ch;
46 struct sdw_bus_params *b_params = &m_rt->bus->params;
47
48 port_bo = t_data->block_offset;
49
50 list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
51 rate = m_rt->stream->params.rate;
52 bps = m_rt->stream->params.bps;
53 sample_int = (m_rt->bus->params.curr_dr_freq / rate);
54 slave_total_ch = 0;
55
56 list_for_each_entry(p_rt, &s_rt->port_list, port_node) {
57 ch = sdw_ch_mask_to_ch(p_rt->ch_mask);
58
59 sdw_fill_xport_params(&p_rt->transport_params,
60 p_rt->num, false,
61 SDW_BLK_GRP_CNT_1,
62 sample_int, port_bo, port_bo >> 8,
63 t_data->hstart,
64 t_data->hstop,
65 (SDW_BLK_GRP_CNT_1 * ch), 0x0);
66
67 sdw_fill_port_params(&p_rt->port_params,
68 p_rt->num, bps,
69 SDW_PORT_FLOW_MODE_ISOCH,
70 b_params->s_data_mode);
71
72 port_bo += bps * ch;
73 slave_total_ch += ch;
74 }
75
76 if (m_rt->direction == SDW_DATA_DIR_TX &&
77 m_rt->ch_count == slave_total_ch) {
78 /*
79 * Slave devices were configured to access all channels
80 * of the stream, which indicates that they operate in
81 * 'mirror mode'. Make sure we reset the port offset for
82 * the next device in the list
83 */
84 port_bo = t_data->block_offset;
85 }
86 }
87 }
88
sdw_compute_master_ports(struct sdw_master_runtime * m_rt,struct sdw_group_params * params,int port_bo,int hstop)89 static void sdw_compute_master_ports(struct sdw_master_runtime *m_rt,
90 struct sdw_group_params *params,
91 int port_bo, int hstop)
92 {
93 struct sdw_transport_data t_data = {0};
94 struct sdw_port_runtime *p_rt;
95 struct sdw_bus *bus = m_rt->bus;
96 struct sdw_bus_params *b_params = &bus->params;
97 int sample_int, hstart = 0;
98 unsigned int rate, bps, ch, no_ch;
99
100 rate = m_rt->stream->params.rate;
101 bps = m_rt->stream->params.bps;
102 ch = m_rt->ch_count;
103 sample_int = (bus->params.curr_dr_freq / rate);
104
105 if (rate != params->rate)
106 return;
107
108 t_data.hstop = hstop;
109 hstart = hstop - params->hwidth + 1;
110 t_data.hstart = hstart;
111
112 list_for_each_entry(p_rt, &m_rt->port_list, port_node) {
113 no_ch = sdw_ch_mask_to_ch(p_rt->ch_mask);
114
115 sdw_fill_xport_params(&p_rt->transport_params, p_rt->num,
116 false, SDW_BLK_GRP_CNT_1, sample_int,
117 port_bo, port_bo >> 8, hstart, hstop,
118 (SDW_BLK_GRP_CNT_1 * no_ch), 0x0);
119
120 sdw_fill_port_params(&p_rt->port_params,
121 p_rt->num, bps,
122 SDW_PORT_FLOW_MODE_ISOCH,
123 b_params->m_data_mode);
124
125 /* Check for first entry */
126 if (!(p_rt == list_first_entry(&m_rt->port_list,
127 struct sdw_port_runtime,
128 port_node))) {
129 port_bo += bps * ch;
130 continue;
131 }
132
133 t_data.hstart = hstart;
134 t_data.hstop = hstop;
135 t_data.block_offset = port_bo;
136 t_data.sub_block_offset = 0;
137 port_bo += bps * ch;
138 }
139
140 sdw_compute_slave_ports(m_rt, &t_data);
141 }
142
_sdw_compute_port_params(struct sdw_bus * bus,struct sdw_group_params * params,int count)143 static void _sdw_compute_port_params(struct sdw_bus *bus,
144 struct sdw_group_params *params, int count)
145 {
146 struct sdw_master_runtime *m_rt = NULL;
147 int hstop = bus->params.col - 1;
148 int block_offset, port_bo, i;
149
150 /* Run loop for all groups to compute transport parameters */
151 for (i = 0; i < count; i++) {
152 port_bo = 1;
153 block_offset = 1;
154
155 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
156 sdw_compute_master_ports(m_rt, ¶ms[i],
157 port_bo, hstop);
158
159 block_offset += m_rt->ch_count *
160 m_rt->stream->params.bps;
161 port_bo = block_offset;
162 }
163
164 hstop = hstop - params[i].hwidth;
165 }
166 }
167
sdw_compute_group_params(struct sdw_bus * bus,struct sdw_group_params * params,int * rates,int count)168 static int sdw_compute_group_params(struct sdw_bus *bus,
169 struct sdw_group_params *params,
170 int *rates, int count)
171 {
172 struct sdw_master_runtime *m_rt = NULL;
173 int sel_col = bus->params.col;
174 unsigned int rate, bps, ch;
175 int i, column_needed = 0;
176
177 /* Calculate bandwidth per group */
178 for (i = 0; i < count; i++) {
179 params[i].rate = rates[i];
180 params[i].full_bw = bus->params.curr_dr_freq / params[i].rate;
181 }
182
183 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
184 rate = m_rt->stream->params.rate;
185 bps = m_rt->stream->params.bps;
186 ch = m_rt->ch_count;
187
188 for (i = 0; i < count; i++) {
189 if (rate == params[i].rate)
190 params[i].payload_bw += bps * ch;
191 }
192 }
193
194 for (i = 0; i < count; i++) {
195 params[i].hwidth = (sel_col *
196 params[i].payload_bw + params[i].full_bw - 1) /
197 params[i].full_bw;
198
199 column_needed += params[i].hwidth;
200 }
201
202 if (column_needed > sel_col - 1)
203 return -EINVAL;
204
205 return 0;
206 }
207
sdw_add_element_group_count(struct sdw_group * group,unsigned int rate)208 static int sdw_add_element_group_count(struct sdw_group *group,
209 unsigned int rate)
210 {
211 int num = group->count;
212 int i;
213
214 for (i = 0; i <= num; i++) {
215 if (rate == group->rates[i])
216 break;
217
218 if (i != num)
219 continue;
220
221 if (group->count >= group->max_size) {
222 unsigned int *rates;
223
224 group->max_size += 1;
225 rates = krealloc(group->rates,
226 (sizeof(int) * group->max_size),
227 GFP_KERNEL);
228 if (!rates)
229 return -ENOMEM;
230 group->rates = rates;
231 }
232
233 group->rates[group->count++] = rate;
234 }
235
236 return 0;
237 }
238
sdw_get_group_count(struct sdw_bus * bus,struct sdw_group * group)239 static int sdw_get_group_count(struct sdw_bus *bus,
240 struct sdw_group *group)
241 {
242 struct sdw_master_runtime *m_rt;
243 unsigned int rate;
244 int ret = 0;
245
246 group->count = 0;
247 group->max_size = SDW_STRM_RATE_GROUPING;
248 group->rates = kcalloc(group->max_size, sizeof(int), GFP_KERNEL);
249 if (!group->rates)
250 return -ENOMEM;
251
252 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
253 rate = m_rt->stream->params.rate;
254 if (m_rt == list_first_entry(&bus->m_rt_list,
255 struct sdw_master_runtime,
256 bus_node)) {
257 group->rates[group->count++] = rate;
258
259 } else {
260 ret = sdw_add_element_group_count(group, rate);
261 if (ret < 0) {
262 kfree(group->rates);
263 return ret;
264 }
265 }
266 }
267
268 return ret;
269 }
270
271 /**
272 * sdw_compute_port_params: Compute transport and port parameters
273 *
274 * @bus: SDW Bus instance
275 */
sdw_compute_port_params(struct sdw_bus * bus)276 static int sdw_compute_port_params(struct sdw_bus *bus)
277 {
278 struct sdw_group_params *params = NULL;
279 struct sdw_group group;
280 int ret;
281
282 ret = sdw_get_group_count(bus, &group);
283 if (ret < 0)
284 return ret;
285
286 if (group.count == 0)
287 goto out;
288
289 params = kcalloc(group.count, sizeof(*params), GFP_KERNEL);
290 if (!params) {
291 ret = -ENOMEM;
292 goto out;
293 }
294
295 /* Compute transport parameters for grouped streams */
296 ret = sdw_compute_group_params(bus, params,
297 &group.rates[0], group.count);
298 if (ret < 0)
299 goto free_params;
300
301 _sdw_compute_port_params(bus, params, group.count);
302
303 free_params:
304 kfree(params);
305 out:
306 kfree(group.rates);
307
308 return ret;
309 }
310
sdw_select_row_col(struct sdw_bus * bus,int clk_freq)311 static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq)
312 {
313 struct sdw_master_prop *prop = &bus->prop;
314 int frame_int, frame_freq;
315 int r, c;
316
317 for (c = 0; c < SDW_FRAME_COLS; c++) {
318 for (r = 0; r < SDW_FRAME_ROWS; r++) {
319 if (sdw_rows[r] != prop->default_row ||
320 sdw_cols[c] != prop->default_col)
321 continue;
322
323 frame_int = sdw_rows[r] * sdw_cols[c];
324 frame_freq = clk_freq / frame_int;
325
326 if ((clk_freq - (frame_freq * SDW_FRAME_CTRL_BITS)) <
327 bus->params.bandwidth)
328 continue;
329
330 bus->params.row = sdw_rows[r];
331 bus->params.col = sdw_cols[c];
332 return 0;
333 }
334 }
335
336 return -EINVAL;
337 }
338
339 /**
340 * sdw_compute_bus_params: Compute bus parameters
341 *
342 * @bus: SDW Bus instance
343 */
sdw_compute_bus_params(struct sdw_bus * bus)344 static int sdw_compute_bus_params(struct sdw_bus *bus)
345 {
346 unsigned int max_dr_freq, curr_dr_freq = 0;
347 struct sdw_master_prop *mstr_prop = &bus->prop;
348 int i, clk_values, ret;
349 bool is_gear = false;
350 u32 *clk_buf;
351
352 if (mstr_prop->num_clk_gears) {
353 clk_values = mstr_prop->num_clk_gears;
354 clk_buf = mstr_prop->clk_gears;
355 is_gear = true;
356 } else if (mstr_prop->num_clk_freq) {
357 clk_values = mstr_prop->num_clk_freq;
358 clk_buf = mstr_prop->clk_freq;
359 } else {
360 clk_values = 1;
361 clk_buf = NULL;
362 }
363
364 max_dr_freq = mstr_prop->max_clk_freq * SDW_DOUBLE_RATE_FACTOR;
365
366 for (i = 0; i < clk_values; i++) {
367 if (!clk_buf)
368 curr_dr_freq = max_dr_freq;
369 else
370 curr_dr_freq = (is_gear) ?
371 (max_dr_freq >> clk_buf[i]) :
372 clk_buf[i] * SDW_DOUBLE_RATE_FACTOR;
373
374 if (curr_dr_freq <= bus->params.bandwidth)
375 continue;
376
377 break;
378
379 /*
380 * TODO: Check all the Slave(s) port(s) audio modes and find
381 * whether given clock rate is supported with glitchless
382 * transition.
383 */
384 }
385
386 if (i == clk_values)
387 return -EINVAL;
388
389 ret = sdw_select_row_col(bus, curr_dr_freq);
390 if (ret < 0)
391 return -EINVAL;
392
393 bus->params.curr_dr_freq = curr_dr_freq;
394 return 0;
395 }
396
397 /**
398 * sdw_compute_params: Compute bus, transport and port parameters
399 *
400 * @bus: SDW Bus instance
401 */
sdw_compute_params(struct sdw_bus * bus)402 int sdw_compute_params(struct sdw_bus *bus)
403 {
404 int ret;
405
406 /* Computes clock frequency, frame shape and frame frequency */
407 ret = sdw_compute_bus_params(bus);
408 if (ret < 0) {
409 dev_err(bus->dev, "Compute bus params failed: %d", ret);
410 return ret;
411 }
412
413 /* Compute transport and port params */
414 ret = sdw_compute_port_params(bus);
415 if (ret < 0) {
416 dev_err(bus->dev, "Compute transport params failed: %d", ret);
417 return ret;
418 }
419
420 return 0;
421 }
422 EXPORT_SYMBOL(sdw_compute_params);
423
424 MODULE_LICENSE("Dual BSD/GPL");
425 MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation");
426