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/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>
15 #include "bus.h"
16
17 #define SDW_STRM_RATE_GROUPING 1
18
19 struct sdw_group_params {
20 unsigned int rate;
21 unsigned int lane;
22 int full_bw;
23 int payload_bw;
24 int hwidth;
25 };
26
27 struct sdw_group {
28 unsigned int count;
29 unsigned int max_size;
30 unsigned int *rates;
31 unsigned int *lanes;
32 };
33
sdw_compute_slave_ports(struct sdw_master_runtime * m_rt,struct sdw_transport_data * t_data)34 void sdw_compute_slave_ports(struct sdw_master_runtime *m_rt,
35 struct sdw_transport_data *t_data)
36 {
37 struct sdw_slave_runtime *s_rt = NULL;
38 struct sdw_port_runtime *p_rt;
39 int port_bo, sample_int;
40 unsigned int rate, bps, ch = 0;
41 unsigned int slave_total_ch;
42 struct sdw_bus_params *b_params = &m_rt->bus->params;
43
44 port_bo = t_data->block_offset;
45
46 list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
47 rate = m_rt->stream->params.rate;
48 bps = m_rt->stream->params.bps;
49 sample_int = (m_rt->bus->params.curr_dr_freq / rate);
50 slave_total_ch = 0;
51
52 list_for_each_entry(p_rt, &s_rt->port_list, port_node) {
53 if (p_rt->lane != t_data->lane)
54 continue;
55
56 ch = hweight32(p_rt->ch_mask);
57
58 sdw_fill_xport_params(&p_rt->transport_params,
59 p_rt->num, false,
60 SDW_BLK_GRP_CNT_1,
61 sample_int, port_bo, port_bo >> 8,
62 t_data->hstart,
63 t_data->hstop,
64 SDW_BLK_PKG_PER_PORT, p_rt->lane);
65
66 sdw_fill_port_params(&p_rt->port_params,
67 p_rt->num, bps,
68 SDW_PORT_FLOW_MODE_ISOCH,
69 b_params->s_data_mode);
70
71 port_bo += bps * ch;
72 slave_total_ch += ch;
73 }
74
75 if (m_rt->direction == SDW_DATA_DIR_TX &&
76 m_rt->ch_count == slave_total_ch) {
77 /*
78 * Slave devices were configured to access all channels
79 * of the stream, which indicates that they operate in
80 * 'mirror mode'. Make sure we reset the port offset for
81 * the next device in the list
82 */
83 port_bo = t_data->block_offset;
84 }
85 }
86 }
87 EXPORT_SYMBOL(sdw_compute_slave_ports);
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;
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 if (p_rt->lane != params->lane)
114 continue;
115
116 sdw_fill_xport_params(&p_rt->transport_params, p_rt->num,
117 false, SDW_BLK_GRP_CNT_1, sample_int,
118 *port_bo, (*port_bo) >> 8, hstart, hstop,
119 SDW_BLK_PKG_PER_PORT, p_rt->lane);
120
121 sdw_fill_port_params(&p_rt->port_params,
122 p_rt->num, bps,
123 SDW_PORT_FLOW_MODE_ISOCH,
124 b_params->m_data_mode);
125
126 /* Check for first entry */
127 if (!(p_rt == list_first_entry(&m_rt->port_list,
128 struct sdw_port_runtime,
129 port_node))) {
130 (*port_bo) += bps * ch;
131 continue;
132 }
133
134 t_data.hstart = hstart;
135 t_data.hstop = hstop;
136 t_data.block_offset = *port_bo;
137 t_data.sub_block_offset = 0;
138 (*port_bo) += bps * ch;
139 }
140
141 t_data.lane = params->lane;
142 sdw_compute_slave_ports(m_rt, &t_data);
143 }
144
_sdw_compute_port_params(struct sdw_bus * bus,struct sdw_group_params * params,int count)145 static void _sdw_compute_port_params(struct sdw_bus *bus,
146 struct sdw_group_params *params, int count)
147 {
148 struct sdw_master_runtime *m_rt;
149 int port_bo, i, l;
150 int hstop;
151
152 /* Run loop for all groups to compute transport parameters */
153 for (l = 0; l < SDW_MAX_LANES; l++) {
154 if (l > 0 && !bus->lane_used_bandwidth[l])
155 continue;
156 /* reset hstop for each lane */
157 hstop = bus->params.col - 1;
158 for (i = 0; i < count; i++) {
159 if (params[i].lane != l)
160 continue;
161 port_bo = 1;
162
163 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
164 sdw_compute_master_ports(m_rt, ¶ms[i], &port_bo, hstop);
165 }
166
167 hstop = hstop - params[i].hwidth;
168 }
169 }
170 }
171
sdw_compute_group_params(struct sdw_bus * bus,struct sdw_stream_runtime * stream,struct sdw_group_params * params,struct sdw_group * group)172 static int sdw_compute_group_params(struct sdw_bus *bus,
173 struct sdw_stream_runtime *stream,
174 struct sdw_group_params *params,
175 struct sdw_group *group)
176 {
177 struct sdw_master_runtime *m_rt;
178 struct sdw_port_runtime *p_rt;
179 int sel_col = bus->params.col;
180 unsigned int rate, bps, ch;
181 int i, l, column_needed;
182
183 /* Calculate bandwidth per group */
184 for (i = 0; i < group->count; i++) {
185 params[i].rate = group->rates[i];
186 params[i].lane = group->lanes[i];
187 params[i].full_bw = bus->params.curr_dr_freq / params[i].rate;
188 }
189
190 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
191 if (m_rt->stream == stream) {
192 /* Only runtime during prepare should be added */
193 if (stream->state != SDW_STREAM_CONFIGURED)
194 continue;
195 } else {
196 /*
197 * Include runtimes with running (ENABLED/PREPARED state) and
198 * paused (DISABLED state) streams
199 */
200 if (m_rt->stream->state != SDW_STREAM_ENABLED &&
201 m_rt->stream->state != SDW_STREAM_PREPARED &&
202 m_rt->stream->state != SDW_STREAM_DISABLED)
203 continue;
204 }
205 list_for_each_entry(p_rt, &m_rt->port_list, port_node) {
206 rate = m_rt->stream->params.rate;
207 bps = m_rt->stream->params.bps;
208 ch = hweight32(p_rt->ch_mask);
209
210 for (i = 0; i < group->count; i++) {
211 if (rate == params[i].rate && p_rt->lane == params[i].lane)
212 params[i].payload_bw += bps * ch;
213 }
214 }
215 }
216
217 for (l = 0; l < SDW_MAX_LANES; l++) {
218 if (l > 0 && !bus->lane_used_bandwidth[l])
219 continue;
220 /* reset column_needed for each lane */
221 column_needed = 0;
222 for (i = 0; i < group->count; i++) {
223 if (params[i].lane != l)
224 continue;
225
226 params[i].hwidth = (sel_col * params[i].payload_bw +
227 params[i].full_bw - 1) / params[i].full_bw;
228
229 column_needed += params[i].hwidth;
230 /* There is no control column for lane 1 and above */
231 if (column_needed > sel_col)
232 return -EINVAL;
233 /* Column 0 is control column on lane 0 */
234 if (params[i].lane == 0 && column_needed > sel_col - 1)
235 return -EINVAL;
236 }
237 }
238
239
240 return 0;
241 }
242
sdw_add_element_group_count(struct sdw_group * group,unsigned int rate,unsigned int lane)243 static int sdw_add_element_group_count(struct sdw_group *group,
244 unsigned int rate, unsigned int lane)
245 {
246 int num = group->count;
247 int i;
248
249 for (i = 0; i <= num; i++) {
250 if (rate == group->rates[i] && lane == group->lanes[i])
251 break;
252
253 if (i != num)
254 continue;
255
256 if (group->count >= group->max_size) {
257 unsigned int *rates;
258 unsigned int *lanes;
259
260 group->max_size += 1;
261 rates = krealloc(group->rates,
262 (sizeof(int) * group->max_size),
263 GFP_KERNEL);
264 if (!rates)
265 return -ENOMEM;
266
267 group->rates = rates;
268
269 lanes = krealloc(group->lanes,
270 (sizeof(int) * group->max_size),
271 GFP_KERNEL);
272 if (!lanes)
273 return -ENOMEM;
274
275 group->lanes = lanes;
276 }
277
278 group->rates[group->count] = rate;
279 group->lanes[group->count++] = lane;
280 }
281
282 return 0;
283 }
284
sdw_get_group_count(struct sdw_bus * bus,struct sdw_group * group)285 static int sdw_get_group_count(struct sdw_bus *bus,
286 struct sdw_group *group)
287 {
288 struct sdw_master_runtime *m_rt;
289 struct sdw_port_runtime *p_rt;
290 unsigned int rate;
291 int ret = 0;
292
293 group->count = 0;
294 group->max_size = SDW_STRM_RATE_GROUPING;
295 group->rates = kcalloc(group->max_size, sizeof(int), GFP_KERNEL);
296 if (!group->rates)
297 return -ENOMEM;
298
299 group->lanes = kcalloc(group->max_size, sizeof(int), GFP_KERNEL);
300 if (!group->lanes) {
301 kfree(group->rates);
302 group->rates = NULL;
303 return -ENOMEM;
304 }
305
306 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
307 if (m_rt->stream->state == SDW_STREAM_DEPREPARED)
308 continue;
309
310 rate = m_rt->stream->params.rate;
311 if (m_rt == list_first_entry(&bus->m_rt_list,
312 struct sdw_master_runtime,
313 bus_node)) {
314 group->rates[group->count++] = rate;
315 }
316 /*
317 * Different ports could use different lane, add group element
318 * even if m_rt is the first entry
319 */
320 list_for_each_entry(p_rt, &m_rt->port_list, port_node) {
321 ret = sdw_add_element_group_count(group, rate, p_rt->lane);
322 if (ret < 0) {
323 kfree(group->rates);
324 kfree(group->lanes);
325 return ret;
326 }
327 }
328 }
329
330 return ret;
331 }
332
333 /**
334 * sdw_compute_port_params: Compute transport and port parameters
335 *
336 * @bus: SDW Bus instance
337 * @stream: Soundwire stream
338 */
sdw_compute_port_params(struct sdw_bus * bus,struct sdw_stream_runtime * stream)339 static int sdw_compute_port_params(struct sdw_bus *bus, struct sdw_stream_runtime *stream)
340 {
341 struct sdw_group_params *params = NULL;
342 struct sdw_group group;
343 int ret;
344
345 ret = sdw_get_group_count(bus, &group);
346 if (ret < 0)
347 return ret;
348
349 if (group.count == 0)
350 goto out;
351
352 params = kcalloc(group.count, sizeof(*params), GFP_KERNEL);
353 if (!params) {
354 ret = -ENOMEM;
355 goto out;
356 }
357
358 /* Compute transport parameters for grouped streams */
359 ret = sdw_compute_group_params(bus, stream, params, &group);
360 if (ret < 0)
361 goto free_params;
362
363 _sdw_compute_port_params(bus, params, group.count);
364
365 free_params:
366 kfree(params);
367 out:
368 kfree(group.rates);
369 kfree(group.lanes);
370
371 return ret;
372 }
373
sdw_select_row_col(struct sdw_bus * bus,int clk_freq)374 static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq)
375 {
376 struct sdw_master_prop *prop = &bus->prop;
377 int r, c;
378
379 for (c = 0; c < SDW_FRAME_COLS; c++) {
380 for (r = 0; r < SDW_FRAME_ROWS; r++) {
381 if (sdw_rows[r] != prop->default_row ||
382 sdw_cols[c] != prop->default_col)
383 continue;
384
385 if (clk_freq * (sdw_cols[c] - 1) <
386 bus->params.bandwidth * sdw_cols[c])
387 continue;
388
389 bus->params.row = sdw_rows[r];
390 bus->params.col = sdw_cols[c];
391 return 0;
392 }
393 }
394
395 return -EINVAL;
396 }
397
is_clock_scaling_supported(struct sdw_bus * bus)398 static bool is_clock_scaling_supported(struct sdw_bus *bus)
399 {
400 struct sdw_master_runtime *m_rt;
401 struct sdw_slave_runtime *s_rt;
402
403 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node)
404 list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node)
405 if (!is_clock_scaling_supported_by_slave(s_rt->slave))
406 return false;
407
408 return true;
409 }
410
411 /**
412 * is_lane_connected_to_all_peripherals: Check if the given manager lane connects to all peripherals
413 * So that all peripherals can use the manager lane.
414 *
415 * @m_rt: Manager runtime
416 * @lane: Lane number
417 */
is_lane_connected_to_all_peripherals(struct sdw_master_runtime * m_rt,unsigned int lane)418 static bool is_lane_connected_to_all_peripherals(struct sdw_master_runtime *m_rt, unsigned int lane)
419 {
420 struct sdw_slave_prop *slave_prop;
421 struct sdw_slave_runtime *s_rt;
422 int i;
423
424 list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
425 slave_prop = &s_rt->slave->prop;
426 for (i = 1; i < SDW_MAX_LANES; i++) {
427 if (slave_prop->lane_maps[i] == lane) {
428 dev_dbg(&s_rt->slave->dev,
429 "M lane %d is connected to P lane %d\n",
430 lane, i);
431 break;
432 }
433 }
434 if (i == SDW_MAX_LANES) {
435 dev_dbg(&s_rt->slave->dev, "M lane %d is not connected\n", lane);
436 return false;
437 }
438 }
439 return true;
440 }
441
get_manager_lane(struct sdw_bus * bus,struct sdw_master_runtime * m_rt,struct sdw_slave_runtime * s_rt,unsigned int curr_dr_freq)442 static int get_manager_lane(struct sdw_bus *bus, struct sdw_master_runtime *m_rt,
443 struct sdw_slave_runtime *s_rt, unsigned int curr_dr_freq)
444 {
445 struct sdw_slave_prop *slave_prop = &s_rt->slave->prop;
446 struct sdw_port_runtime *m_p_rt;
447 unsigned int required_bandwidth;
448 int m_lane;
449 int l;
450
451 for (l = 1; l < SDW_MAX_LANES; l++) {
452 if (!slave_prop->lane_maps[l])
453 continue;
454
455 required_bandwidth = 0;
456 list_for_each_entry(m_p_rt, &m_rt->port_list, port_node) {
457 required_bandwidth += m_rt->stream->params.rate *
458 hweight32(m_p_rt->ch_mask) *
459 m_rt->stream->params.bps;
460 }
461 if (required_bandwidth <=
462 curr_dr_freq - bus->lane_used_bandwidth[l]) {
463 /* Check if m_lane is connected to all Peripherals */
464 if (!is_lane_connected_to_all_peripherals(m_rt,
465 slave_prop->lane_maps[l])) {
466 dev_dbg(bus->dev,
467 "Not all Peripherals are connected to M lane %d\n",
468 slave_prop->lane_maps[l]);
469 continue;
470 }
471 m_lane = slave_prop->lane_maps[l];
472 dev_dbg(&s_rt->slave->dev, "M lane %d is used\n", m_lane);
473 bus->lane_used_bandwidth[l] += required_bandwidth;
474 /*
475 * Use non-zero manager lane, subtract the lane 0
476 * bandwidth that is already calculated
477 */
478 bus->params.bandwidth -= required_bandwidth;
479 return m_lane;
480 }
481 }
482
483 /* No available multi lane found, only lane 0 can be used */
484 return 0;
485 }
486
487 /**
488 * sdw_compute_bus_params: Compute bus parameters
489 *
490 * @bus: SDW Bus instance
491 */
sdw_compute_bus_params(struct sdw_bus * bus)492 static int sdw_compute_bus_params(struct sdw_bus *bus)
493 {
494 struct sdw_master_prop *mstr_prop = &bus->prop;
495 struct sdw_slave_prop *slave_prop;
496 struct sdw_port_runtime *m_p_rt;
497 struct sdw_port_runtime *s_p_rt;
498 struct sdw_master_runtime *m_rt;
499 struct sdw_slave_runtime *s_rt;
500 unsigned int curr_dr_freq = 0;
501 int i, l, clk_values, ret;
502 bool is_gear = false;
503 int m_lane = 0;
504 u32 *clk_buf;
505
506 if (mstr_prop->num_clk_gears) {
507 clk_values = mstr_prop->num_clk_gears;
508 clk_buf = mstr_prop->clk_gears;
509 is_gear = true;
510 } else if (mstr_prop->num_clk_freq) {
511 clk_values = mstr_prop->num_clk_freq;
512 clk_buf = mstr_prop->clk_freq;
513 } else {
514 clk_values = 1;
515 clk_buf = NULL;
516 }
517
518 /* If dynamic scaling is not supported, don't try higher freq */
519 if (!is_clock_scaling_supported(bus))
520 clk_values = 1;
521
522 for (i = 0; i < clk_values; i++) {
523 if (!clk_buf)
524 curr_dr_freq = bus->params.max_dr_freq;
525 else
526 curr_dr_freq = (is_gear) ?
527 (bus->params.max_dr_freq >> clk_buf[i]) :
528 clk_buf[i] * SDW_DOUBLE_RATE_FACTOR;
529
530 if (curr_dr_freq * (mstr_prop->default_col - 1) >=
531 bus->params.bandwidth * mstr_prop->default_col)
532 break;
533
534 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) {
535 /*
536 * Get the first s_rt that will be used to find the available lane that
537 * can be used. No need to check all Peripherals because we can't use
538 * multi-lane if we can't find any available lane for the first Peripheral.
539 */
540 s_rt = list_first_entry(&m_rt->slave_rt_list,
541 struct sdw_slave_runtime, m_rt_node);
542
543 /*
544 * Find the available Manager lane that connected to the first Peripheral.
545 */
546 m_lane = get_manager_lane(bus, m_rt, s_rt, curr_dr_freq);
547 if (m_lane > 0)
548 goto out;
549 }
550
551 /*
552 * TODO: Check all the Slave(s) port(s) audio modes and find
553 * whether given clock rate is supported with glitchless
554 * transition.
555 */
556 }
557
558 if (i == clk_values) {
559 dev_err(bus->dev, "%s: could not find clock value for bandwidth %d\n",
560 __func__, bus->params.bandwidth);
561 return -EINVAL;
562 }
563 out:
564 /* multilane can be used */
565 if (m_lane > 0) {
566 /* Set Peripheral lanes */
567 list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
568 slave_prop = &s_rt->slave->prop;
569 for (l = 1; l < SDW_MAX_LANES; l++) {
570 if (slave_prop->lane_maps[l] == m_lane) {
571 list_for_each_entry(s_p_rt, &s_rt->port_list, port_node) {
572 s_p_rt->lane = l;
573 dev_dbg(&s_rt->slave->dev,
574 "Set P lane %d for port %d\n",
575 l, s_p_rt->num);
576 }
577 break;
578 }
579 }
580 }
581 /*
582 * Set Manager lanes. Configure the last m_rt in bus->m_rt_list only since
583 * we don't want to touch other m_rts that are already working.
584 */
585 list_for_each_entry(m_p_rt, &m_rt->port_list, port_node) {
586 m_p_rt->lane = m_lane;
587 }
588 }
589
590 if (!mstr_prop->default_frame_rate || !mstr_prop->default_row)
591 return -EINVAL;
592
593 mstr_prop->default_col = curr_dr_freq / mstr_prop->default_frame_rate /
594 mstr_prop->default_row;
595
596 ret = sdw_select_row_col(bus, curr_dr_freq);
597 if (ret < 0) {
598 dev_err(bus->dev, "%s: could not find frame configuration for bus dr_freq %d\n",
599 __func__, curr_dr_freq);
600 return -EINVAL;
601 }
602
603 bus->params.curr_dr_freq = curr_dr_freq;
604 return 0;
605 }
606
607 /**
608 * sdw_compute_params: Compute bus, transport and port parameters
609 *
610 * @bus: SDW Bus instance
611 * @stream: Soundwire stream
612 */
sdw_compute_params(struct sdw_bus * bus,struct sdw_stream_runtime * stream)613 int sdw_compute_params(struct sdw_bus *bus, struct sdw_stream_runtime *stream)
614 {
615 int ret;
616
617 /* Computes clock frequency, frame shape and frame frequency */
618 ret = sdw_compute_bus_params(bus);
619 if (ret < 0)
620 return ret;
621
622 /* Compute transport and port params */
623 ret = sdw_compute_port_params(bus, stream);
624 if (ret < 0) {
625 dev_err(bus->dev, "Compute transport params failed: %d\n", ret);
626 return ret;
627 }
628
629 return 0;
630 }
631 EXPORT_SYMBOL(sdw_compute_params);
632
633 MODULE_LICENSE("Dual BSD/GPL");
634 MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation");
635