Commit | Line | Data |
---|---|---|
9026118f BL |
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 | ||
9ddae9da | 9 | #include <linux/bitops.h> |
9026118f BL |
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 | int full_bw; | |
22 | int payload_bw; | |
23 | int hwidth; | |
24 | }; | |
25 | ||
26 | struct sdw_group { | |
27 | unsigned int count; | |
28 | unsigned int max_size; | |
29 | unsigned int *rates; | |
30 | }; | |
31 | ||
f346fdf9 VM |
32 | void sdw_compute_slave_ports(struct sdw_master_runtime *m_rt, |
33 | struct sdw_transport_data *t_data) | |
9026118f BL |
34 | { |
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; | |
dd87a72a | 40 | struct sdw_bus_params *b_params = &m_rt->bus->params; |
9026118f BL |
41 | |
42 | port_bo = t_data->block_offset; | |
43 | ||
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); | |
48 | slave_total_ch = 0; | |
49 | ||
50 | list_for_each_entry(p_rt, &s_rt->port_list, port_node) { | |
9ddae9da | 51 | ch = hweight32(p_rt->ch_mask); |
9026118f BL |
52 | |
53 | sdw_fill_xport_params(&p_rt->transport_params, | |
54 | p_rt->num, false, | |
55 | SDW_BLK_GRP_CNT_1, | |
56 | sample_int, port_bo, port_bo >> 8, | |
57 | t_data->hstart, | |
58 | t_data->hstop, | |
8f29bb83 | 59 | SDW_BLK_PKG_PER_PORT, 0x0); |
9026118f BL |
60 | |
61 | sdw_fill_port_params(&p_rt->port_params, | |
62 | p_rt->num, bps, | |
63 | SDW_PORT_FLOW_MODE_ISOCH, | |
dd87a72a | 64 | b_params->s_data_mode); |
9026118f BL |
65 | |
66 | port_bo += bps * ch; | |
67 | slave_total_ch += ch; | |
68 | } | |
69 | ||
70 | if (m_rt->direction == SDW_DATA_DIR_TX && | |
71 | m_rt->ch_count == slave_total_ch) { | |
72 | /* | |
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 | |
77 | */ | |
78 | port_bo = t_data->block_offset; | |
79 | } | |
80 | } | |
81 | } | |
f346fdf9 | 82 | EXPORT_SYMBOL(sdw_compute_slave_ports); |
9026118f BL |
83 | |
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) | |
87 | { | |
88 | struct sdw_transport_data t_data = {0}; | |
89 | struct sdw_port_runtime *p_rt; | |
90 | struct sdw_bus *bus = m_rt->bus; | |
dd87a72a | 91 | struct sdw_bus_params *b_params = &bus->params; |
9026118f | 92 | int sample_int, hstart = 0; |
8f29bb83 | 93 | unsigned int rate, bps, ch; |
9026118f BL |
94 | |
95 | rate = m_rt->stream->params.rate; | |
96 | bps = m_rt->stream->params.bps; | |
97 | ch = m_rt->ch_count; | |
98 | sample_int = (bus->params.curr_dr_freq / rate); | |
99 | ||
100 | if (rate != params->rate) | |
101 | return; | |
102 | ||
103 | t_data.hstop = hstop; | |
104 | hstart = hstop - params->hwidth + 1; | |
105 | t_data.hstart = hstart; | |
106 | ||
107 | list_for_each_entry(p_rt, &m_rt->port_list, port_node) { | |
9026118f BL |
108 | |
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, | |
8f29bb83 | 112 | SDW_BLK_PKG_PER_PORT, 0x0); |
9026118f BL |
113 | |
114 | sdw_fill_port_params(&p_rt->port_params, | |
115 | p_rt->num, bps, | |
116 | SDW_PORT_FLOW_MODE_ISOCH, | |
dd87a72a | 117 | b_params->m_data_mode); |
9026118f BL |
118 | |
119 | /* Check for first entry */ | |
120 | if (!(p_rt == list_first_entry(&m_rt->port_list, | |
121 | struct sdw_port_runtime, | |
122 | port_node))) { | |
123 | port_bo += bps * ch; | |
124 | continue; | |
125 | } | |
126 | ||
127 | t_data.hstart = hstart; | |
128 | t_data.hstop = hstop; | |
129 | t_data.block_offset = port_bo; | |
130 | t_data.sub_block_offset = 0; | |
131 | port_bo += bps * ch; | |
132 | } | |
133 | ||
134 | sdw_compute_slave_ports(m_rt, &t_data); | |
135 | } | |
136 | ||
137 | static void _sdw_compute_port_params(struct sdw_bus *bus, | |
138 | struct sdw_group_params *params, int count) | |
139 | { | |
6ae435bd | 140 | struct sdw_master_runtime *m_rt; |
9026118f BL |
141 | int hstop = bus->params.col - 1; |
142 | int block_offset, port_bo, i; | |
143 | ||
144 | /* Run loop for all groups to compute transport parameters */ | |
145 | for (i = 0; i < count; i++) { | |
146 | port_bo = 1; | |
147 | block_offset = 1; | |
148 | ||
149 | list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) { | |
150 | sdw_compute_master_ports(m_rt, ¶ms[i], | |
151 | port_bo, hstop); | |
152 | ||
153 | block_offset += m_rt->ch_count * | |
154 | m_rt->stream->params.bps; | |
155 | port_bo = block_offset; | |
156 | } | |
157 | ||
158 | hstop = hstop - params[i].hwidth; | |
159 | } | |
160 | } | |
161 | ||
162 | static int sdw_compute_group_params(struct sdw_bus *bus, | |
163 | struct sdw_group_params *params, | |
164 | int *rates, int count) | |
165 | { | |
6ae435bd | 166 | struct sdw_master_runtime *m_rt; |
9026118f BL |
167 | int sel_col = bus->params.col; |
168 | unsigned int rate, bps, ch; | |
169 | int i, column_needed = 0; | |
170 | ||
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; | |
175 | } | |
176 | ||
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; | |
180 | ch = m_rt->ch_count; | |
181 | ||
182 | for (i = 0; i < count; i++) { | |
183 | if (rate == params[i].rate) | |
184 | params[i].payload_bw += bps * ch; | |
185 | } | |
186 | } | |
187 | ||
188 | for (i = 0; i < count; i++) { | |
189 | params[i].hwidth = (sel_col * | |
190 | params[i].payload_bw + params[i].full_bw - 1) / | |
191 | params[i].full_bw; | |
192 | ||
193 | column_needed += params[i].hwidth; | |
194 | } | |
195 | ||
196 | if (column_needed > sel_col - 1) | |
197 | return -EINVAL; | |
198 | ||
199 | return 0; | |
200 | } | |
201 | ||
202 | static int sdw_add_element_group_count(struct sdw_group *group, | |
203 | unsigned int rate) | |
204 | { | |
205 | int num = group->count; | |
206 | int i; | |
207 | ||
208 | for (i = 0; i <= num; i++) { | |
209 | if (rate == group->rates[i]) | |
210 | break; | |
211 | ||
212 | if (i != num) | |
213 | continue; | |
214 | ||
215 | if (group->count >= group->max_size) { | |
216 | unsigned int *rates; | |
217 | ||
218 | group->max_size += 1; | |
219 | rates = krealloc(group->rates, | |
220 | (sizeof(int) * group->max_size), | |
221 | GFP_KERNEL); | |
222 | if (!rates) | |
223 | return -ENOMEM; | |
224 | group->rates = rates; | |
225 | } | |
226 | ||
227 | group->rates[group->count++] = rate; | |
228 | } | |
229 | ||
230 | return 0; | |
231 | } | |
232 | ||
233 | static int sdw_get_group_count(struct sdw_bus *bus, | |
234 | struct sdw_group *group) | |
235 | { | |
236 | struct sdw_master_runtime *m_rt; | |
237 | unsigned int rate; | |
238 | int ret = 0; | |
239 | ||
240 | group->count = 0; | |
241 | group->max_size = SDW_STRM_RATE_GROUPING; | |
242 | group->rates = kcalloc(group->max_size, sizeof(int), GFP_KERNEL); | |
243 | if (!group->rates) | |
244 | return -ENOMEM; | |
245 | ||
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, | |
250 | bus_node)) { | |
251 | group->rates[group->count++] = rate; | |
252 | ||
253 | } else { | |
254 | ret = sdw_add_element_group_count(group, rate); | |
255 | if (ret < 0) { | |
256 | kfree(group->rates); | |
257 | return ret; | |
258 | } | |
259 | } | |
260 | } | |
261 | ||
262 | return ret; | |
263 | } | |
264 | ||
265 | /** | |
266 | * sdw_compute_port_params: Compute transport and port parameters | |
267 | * | |
268 | * @bus: SDW Bus instance | |
269 | */ | |
270 | static int sdw_compute_port_params(struct sdw_bus *bus) | |
271 | { | |
272 | struct sdw_group_params *params = NULL; | |
273 | struct sdw_group group; | |
274 | int ret; | |
275 | ||
276 | ret = sdw_get_group_count(bus, &group); | |
277 | if (ret < 0) | |
278 | return ret; | |
279 | ||
280 | if (group.count == 0) | |
281 | goto out; | |
282 | ||
283 | params = kcalloc(group.count, sizeof(*params), GFP_KERNEL); | |
284 | if (!params) { | |
285 | ret = -ENOMEM; | |
286 | goto out; | |
287 | } | |
288 | ||
289 | /* Compute transport parameters for grouped streams */ | |
290 | ret = sdw_compute_group_params(bus, params, | |
291 | &group.rates[0], group.count); | |
292 | if (ret < 0) | |
293 | goto free_params; | |
294 | ||
295 | _sdw_compute_port_params(bus, params, group.count); | |
296 | ||
297 | free_params: | |
298 | kfree(params); | |
299 | out: | |
300 | kfree(group.rates); | |
301 | ||
302 | return ret; | |
303 | } | |
304 | ||
305 | static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq) | |
306 | { | |
307 | struct sdw_master_prop *prop = &bus->prop; | |
308 | int frame_int, frame_freq; | |
309 | int r, c; | |
310 | ||
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) | |
315 | continue; | |
316 | ||
317 | frame_int = sdw_rows[r] * sdw_cols[c]; | |
318 | frame_freq = clk_freq / frame_int; | |
319 | ||
320 | if ((clk_freq - (frame_freq * SDW_FRAME_CTRL_BITS)) < | |
321 | bus->params.bandwidth) | |
322 | continue; | |
323 | ||
324 | bus->params.row = sdw_rows[r]; | |
325 | bus->params.col = sdw_cols[c]; | |
326 | return 0; | |
327 | } | |
328 | } | |
329 | ||
330 | return -EINVAL; | |
331 | } | |
332 | ||
333 | /** | |
334 | * sdw_compute_bus_params: Compute bus parameters | |
335 | * | |
336 | * @bus: SDW Bus instance | |
337 | */ | |
338 | static int sdw_compute_bus_params(struct sdw_bus *bus) | |
339 | { | |
340 | unsigned int max_dr_freq, curr_dr_freq = 0; | |
5ec3215e | 341 | struct sdw_master_prop *mstr_prop = &bus->prop; |
9026118f BL |
342 | int i, clk_values, ret; |
343 | bool is_gear = false; | |
344 | u32 *clk_buf; | |
345 | ||
9026118f BL |
346 | if (mstr_prop->num_clk_gears) { |
347 | clk_values = mstr_prop->num_clk_gears; | |
348 | clk_buf = mstr_prop->clk_gears; | |
349 | is_gear = true; | |
350 | } else if (mstr_prop->num_clk_freq) { | |
351 | clk_values = mstr_prop->num_clk_freq; | |
352 | clk_buf = mstr_prop->clk_freq; | |
353 | } else { | |
354 | clk_values = 1; | |
355 | clk_buf = NULL; | |
356 | } | |
357 | ||
358 | max_dr_freq = mstr_prop->max_clk_freq * SDW_DOUBLE_RATE_FACTOR; | |
359 | ||
360 | for (i = 0; i < clk_values; i++) { | |
361 | if (!clk_buf) | |
362 | curr_dr_freq = max_dr_freq; | |
363 | else | |
364 | curr_dr_freq = (is_gear) ? | |
365 | (max_dr_freq >> clk_buf[i]) : | |
366 | clk_buf[i] * SDW_DOUBLE_RATE_FACTOR; | |
367 | ||
368 | if (curr_dr_freq <= bus->params.bandwidth) | |
369 | continue; | |
370 | ||
371 | break; | |
372 | ||
373 | /* | |
374 | * TODO: Check all the Slave(s) port(s) audio modes and find | |
375 | * whether given clock rate is supported with glitchless | |
376 | * transition. | |
377 | */ | |
378 | } | |
379 | ||
0531e6b6 PLB |
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); | |
9026118f | 383 | return -EINVAL; |
0531e6b6 | 384 | } |
9026118f BL |
385 | |
386 | ret = sdw_select_row_col(bus, curr_dr_freq); | |
0531e6b6 PLB |
387 | if (ret < 0) { |
388 | dev_err(bus->dev, "%s: could not find frame configuration for bus dr_freq %d\n", | |
389 | __func__, curr_dr_freq); | |
9026118f | 390 | return -EINVAL; |
0531e6b6 | 391 | } |
9026118f BL |
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 | */ | |
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); | |
0531e6b6 | 408 | if (ret < 0) |
9026118f | 409 | return ret; |
9026118f BL |
410 | |
411 | /* Compute transport and port params */ | |
412 | ret = sdw_compute_port_params(bus); | |
413 | if (ret < 0) { | |
0eb7c387 | 414 | dev_err(bus->dev, "Compute transport params failed: %d\n", ret); |
9026118f BL |
415 | return ret; |
416 | } | |
417 | ||
418 | return 0; | |
419 | } | |
420 | EXPORT_SYMBOL(sdw_compute_params); | |
421 | ||
422 | MODULE_LICENSE("Dual BSD/GPL"); | |
423 | MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation"); |