projects/linux·blob master · refs

687 lines · 17.5 KiB

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