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 | ||
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 | ||
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; | |
dd87a72a | 46 | struct sdw_bus_params *b_params = &m_rt->bus->params; |
9026118f BL |
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, | |
8f29bb83 | 65 | SDW_BLK_PKG_PER_PORT, 0x0); |
9026118f BL |
66 | |
67 | sdw_fill_port_params(&p_rt->port_params, | |
68 | p_rt->num, bps, | |
69 | SDW_PORT_FLOW_MODE_ISOCH, | |
dd87a72a | 70 | b_params->s_data_mode); |
9026118f BL |
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 | ||
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; | |
dd87a72a | 96 | struct sdw_bus_params *b_params = &bus->params; |
9026118f | 97 | int sample_int, hstart = 0; |
8f29bb83 | 98 | unsigned int rate, bps, ch; |
9026118f BL |
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) { | |
9026118f BL |
113 | |
114 | sdw_fill_xport_params(&p_rt->transport_params, p_rt->num, | |
115 | false, SDW_BLK_GRP_CNT_1, sample_int, | |
116 | port_bo, port_bo >> 8, hstart, hstop, | |
8f29bb83 | 117 | SDW_BLK_PKG_PER_PORT, 0x0); |
9026118f BL |
118 | |
119 | sdw_fill_port_params(&p_rt->port_params, | |
120 | p_rt->num, bps, | |
121 | SDW_PORT_FLOW_MODE_ISOCH, | |
dd87a72a | 122 | b_params->m_data_mode); |
9026118f BL |
123 | |
124 | /* Check for first entry */ | |
125 | if (!(p_rt == list_first_entry(&m_rt->port_list, | |
126 | struct sdw_port_runtime, | |
127 | port_node))) { | |
128 | port_bo += bps * ch; | |
129 | continue; | |
130 | } | |
131 | ||
132 | t_data.hstart = hstart; | |
133 | t_data.hstop = hstop; | |
134 | t_data.block_offset = port_bo; | |
135 | t_data.sub_block_offset = 0; | |
136 | port_bo += bps * ch; | |
137 | } | |
138 | ||
139 | sdw_compute_slave_ports(m_rt, &t_data); | |
140 | } | |
141 | ||
142 | static void _sdw_compute_port_params(struct sdw_bus *bus, | |
143 | struct sdw_group_params *params, int count) | |
144 | { | |
6ae435bd | 145 | struct sdw_master_runtime *m_rt; |
9026118f BL |
146 | int hstop = bus->params.col - 1; |
147 | int block_offset, port_bo, i; | |
148 | ||
149 | /* Run loop for all groups to compute transport parameters */ | |
150 | for (i = 0; i < count; i++) { | |
151 | port_bo = 1; | |
152 | block_offset = 1; | |
153 | ||
154 | list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) { | |
155 | sdw_compute_master_ports(m_rt, ¶ms[i], | |
156 | port_bo, hstop); | |
157 | ||
158 | block_offset += m_rt->ch_count * | |
159 | m_rt->stream->params.bps; | |
160 | port_bo = block_offset; | |
161 | } | |
162 | ||
163 | hstop = hstop - params[i].hwidth; | |
164 | } | |
165 | } | |
166 | ||
167 | static int sdw_compute_group_params(struct sdw_bus *bus, | |
168 | struct sdw_group_params *params, | |
169 | int *rates, int count) | |
170 | { | |
6ae435bd | 171 | struct sdw_master_runtime *m_rt; |
9026118f BL |
172 | int sel_col = bus->params.col; |
173 | unsigned int rate, bps, ch; | |
174 | int i, column_needed = 0; | |
175 | ||
176 | /* Calculate bandwidth per group */ | |
177 | for (i = 0; i < count; i++) { | |
178 | params[i].rate = rates[i]; | |
179 | params[i].full_bw = bus->params.curr_dr_freq / params[i].rate; | |
180 | } | |
181 | ||
182 | list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) { | |
183 | rate = m_rt->stream->params.rate; | |
184 | bps = m_rt->stream->params.bps; | |
185 | ch = m_rt->ch_count; | |
186 | ||
187 | for (i = 0; i < count; i++) { | |
188 | if (rate == params[i].rate) | |
189 | params[i].payload_bw += bps * ch; | |
190 | } | |
191 | } | |
192 | ||
193 | for (i = 0; i < count; i++) { | |
194 | params[i].hwidth = (sel_col * | |
195 | params[i].payload_bw + params[i].full_bw - 1) / | |
196 | params[i].full_bw; | |
197 | ||
198 | column_needed += params[i].hwidth; | |
199 | } | |
200 | ||
201 | if (column_needed > sel_col - 1) | |
202 | return -EINVAL; | |
203 | ||
204 | return 0; | |
205 | } | |
206 | ||
207 | static int sdw_add_element_group_count(struct sdw_group *group, | |
208 | unsigned int rate) | |
209 | { | |
210 | int num = group->count; | |
211 | int i; | |
212 | ||
213 | for (i = 0; i <= num; i++) { | |
214 | if (rate == group->rates[i]) | |
215 | break; | |
216 | ||
217 | if (i != num) | |
218 | continue; | |
219 | ||
220 | if (group->count >= group->max_size) { | |
221 | unsigned int *rates; | |
222 | ||
223 | group->max_size += 1; | |
224 | rates = krealloc(group->rates, | |
225 | (sizeof(int) * group->max_size), | |
226 | GFP_KERNEL); | |
227 | if (!rates) | |
228 | return -ENOMEM; | |
229 | group->rates = rates; | |
230 | } | |
231 | ||
232 | group->rates[group->count++] = rate; | |
233 | } | |
234 | ||
235 | return 0; | |
236 | } | |
237 | ||
238 | static int sdw_get_group_count(struct sdw_bus *bus, | |
239 | struct sdw_group *group) | |
240 | { | |
241 | struct sdw_master_runtime *m_rt; | |
242 | unsigned int rate; | |
243 | int ret = 0; | |
244 | ||
245 | group->count = 0; | |
246 | group->max_size = SDW_STRM_RATE_GROUPING; | |
247 | group->rates = kcalloc(group->max_size, sizeof(int), GFP_KERNEL); | |
248 | if (!group->rates) | |
249 | return -ENOMEM; | |
250 | ||
251 | list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) { | |
252 | rate = m_rt->stream->params.rate; | |
253 | if (m_rt == list_first_entry(&bus->m_rt_list, | |
254 | struct sdw_master_runtime, | |
255 | bus_node)) { | |
256 | group->rates[group->count++] = rate; | |
257 | ||
258 | } else { | |
259 | ret = sdw_add_element_group_count(group, rate); | |
260 | if (ret < 0) { | |
261 | kfree(group->rates); | |
262 | return ret; | |
263 | } | |
264 | } | |
265 | } | |
266 | ||
267 | return ret; | |
268 | } | |
269 | ||
270 | /** | |
271 | * sdw_compute_port_params: Compute transport and port parameters | |
272 | * | |
273 | * @bus: SDW Bus instance | |
274 | */ | |
275 | static int sdw_compute_port_params(struct sdw_bus *bus) | |
276 | { | |
277 | struct sdw_group_params *params = NULL; | |
278 | struct sdw_group group; | |
279 | int ret; | |
280 | ||
281 | ret = sdw_get_group_count(bus, &group); | |
282 | if (ret < 0) | |
283 | return ret; | |
284 | ||
285 | if (group.count == 0) | |
286 | goto out; | |
287 | ||
288 | params = kcalloc(group.count, sizeof(*params), GFP_KERNEL); | |
289 | if (!params) { | |
290 | ret = -ENOMEM; | |
291 | goto out; | |
292 | } | |
293 | ||
294 | /* Compute transport parameters for grouped streams */ | |
295 | ret = sdw_compute_group_params(bus, params, | |
296 | &group.rates[0], group.count); | |
297 | if (ret < 0) | |
298 | goto free_params; | |
299 | ||
300 | _sdw_compute_port_params(bus, params, group.count); | |
301 | ||
302 | free_params: | |
303 | kfree(params); | |
304 | out: | |
305 | kfree(group.rates); | |
306 | ||
307 | return ret; | |
308 | } | |
309 | ||
310 | static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq) | |
311 | { | |
312 | struct sdw_master_prop *prop = &bus->prop; | |
313 | int frame_int, frame_freq; | |
314 | int r, c; | |
315 | ||
316 | for (c = 0; c < SDW_FRAME_COLS; c++) { | |
317 | for (r = 0; r < SDW_FRAME_ROWS; r++) { | |
318 | if (sdw_rows[r] != prop->default_row || | |
319 | sdw_cols[c] != prop->default_col) | |
320 | continue; | |
321 | ||
322 | frame_int = sdw_rows[r] * sdw_cols[c]; | |
323 | frame_freq = clk_freq / frame_int; | |
324 | ||
325 | if ((clk_freq - (frame_freq * SDW_FRAME_CTRL_BITS)) < | |
326 | bus->params.bandwidth) | |
327 | continue; | |
328 | ||
329 | bus->params.row = sdw_rows[r]; | |
330 | bus->params.col = sdw_cols[c]; | |
331 | return 0; | |
332 | } | |
333 | } | |
334 | ||
335 | return -EINVAL; | |
336 | } | |
337 | ||
338 | /** | |
339 | * sdw_compute_bus_params: Compute bus parameters | |
340 | * | |
341 | * @bus: SDW Bus instance | |
342 | */ | |
343 | static int sdw_compute_bus_params(struct sdw_bus *bus) | |
344 | { | |
345 | unsigned int max_dr_freq, curr_dr_freq = 0; | |
5ec3215e | 346 | struct sdw_master_prop *mstr_prop = &bus->prop; |
9026118f BL |
347 | int i, clk_values, ret; |
348 | bool is_gear = false; | |
349 | u32 *clk_buf; | |
350 | ||
9026118f BL |
351 | if (mstr_prop->num_clk_gears) { |
352 | clk_values = mstr_prop->num_clk_gears; | |
353 | clk_buf = mstr_prop->clk_gears; | |
354 | is_gear = true; | |
355 | } else if (mstr_prop->num_clk_freq) { | |
356 | clk_values = mstr_prop->num_clk_freq; | |
357 | clk_buf = mstr_prop->clk_freq; | |
358 | } else { | |
359 | clk_values = 1; | |
360 | clk_buf = NULL; | |
361 | } | |
362 | ||
363 | max_dr_freq = mstr_prop->max_clk_freq * SDW_DOUBLE_RATE_FACTOR; | |
364 | ||
365 | for (i = 0; i < clk_values; i++) { | |
366 | if (!clk_buf) | |
367 | curr_dr_freq = max_dr_freq; | |
368 | else | |
369 | curr_dr_freq = (is_gear) ? | |
370 | (max_dr_freq >> clk_buf[i]) : | |
371 | clk_buf[i] * SDW_DOUBLE_RATE_FACTOR; | |
372 | ||
373 | if (curr_dr_freq <= bus->params.bandwidth) | |
374 | continue; | |
375 | ||
376 | break; | |
377 | ||
378 | /* | |
379 | * TODO: Check all the Slave(s) port(s) audio modes and find | |
380 | * whether given clock rate is supported with glitchless | |
381 | * transition. | |
382 | */ | |
383 | } | |
384 | ||
0531e6b6 PLB |
385 | if (i == clk_values) { |
386 | dev_err(bus->dev, "%s: could not find clock value for bandwidth %d\n", | |
387 | __func__, bus->params.bandwidth); | |
9026118f | 388 | return -EINVAL; |
0531e6b6 | 389 | } |
9026118f BL |
390 | |
391 | ret = sdw_select_row_col(bus, curr_dr_freq); | |
0531e6b6 PLB |
392 | if (ret < 0) { |
393 | dev_err(bus->dev, "%s: could not find frame configuration for bus dr_freq %d\n", | |
394 | __func__, curr_dr_freq); | |
9026118f | 395 | return -EINVAL; |
0531e6b6 | 396 | } |
9026118f BL |
397 | |
398 | bus->params.curr_dr_freq = curr_dr_freq; | |
399 | return 0; | |
400 | } | |
401 | ||
402 | /** | |
403 | * sdw_compute_params: Compute bus, transport and port parameters | |
404 | * | |
405 | * @bus: SDW Bus instance | |
406 | */ | |
407 | int sdw_compute_params(struct sdw_bus *bus) | |
408 | { | |
409 | int ret; | |
410 | ||
411 | /* Computes clock frequency, frame shape and frame frequency */ | |
412 | ret = sdw_compute_bus_params(bus); | |
0531e6b6 | 413 | if (ret < 0) |
9026118f | 414 | return ret; |
9026118f BL |
415 | |
416 | /* Compute transport and port params */ | |
417 | ret = sdw_compute_port_params(bus); | |
418 | if (ret < 0) { | |
0eb7c387 | 419 | dev_err(bus->dev, "Compute transport params failed: %d\n", ret); |
9026118f BL |
420 | return ret; |
421 | } | |
422 | ||
423 | return 0; | |
424 | } | |
425 | EXPORT_SYMBOL(sdw_compute_params); | |
426 | ||
427 | MODULE_LICENSE("Dual BSD/GPL"); | |
428 | MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation"); |