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