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