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 
sdw_compute_slave_ports(struct sdw_master_runtime * m_rt,struct sdw_transport_data * t_data)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 
sdw_compute_master_ports(struct sdw_master_runtime * m_rt,struct sdw_group_params * params,int port_bo,int hstop)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 
_sdw_compute_port_params(struct sdw_bus * bus,struct sdw_group_params * params,int count)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 			port_bo += m_rt->ch_count * m_rt->stream->params.bps;
152 		}
153 
154 		hstop = hstop - params[i].hwidth;
155 	}
156 }
157 
sdw_compute_group_params(struct sdw_bus * bus,struct sdw_group_params * params,int * rates,int count)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 
sdw_add_element_group_count(struct sdw_group * group,unsigned int rate)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(group->rates,
216 					 (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 
sdw_get_group_count(struct sdw_bus * bus,struct sdw_group * group)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(group->max_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(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  */
sdw_compute_port_params(struct sdw_bus * bus)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);
273 	if (ret < 0)
274 		return ret;
275 
276 	if (group.count == 0)
277 		goto out;
278 
279 	params = kcalloc(group.count, 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 				       &group.rates[0], group.count);
288 	if (ret < 0)
289 		goto free_params;
290 
291 	_sdw_compute_port_params(bus, params, group.count);
292 
293 free_params:
294 	kfree(params);
295 out:
296 	kfree(group.rates);
297 
298 	return ret;
299 }
300 
sdw_select_row_col(struct sdw_bus * bus,int clk_freq)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  */
sdw_compute_bus_params(struct sdw_bus * bus)334 static int sdw_compute_bus_params(struct sdw_bus *bus)
335 {
336 	unsigned int max_dr_freq, 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 	max_dr_freq = mstr_prop->max_clk_freq * SDW_DOUBLE_RATE_FACTOR;
355 
356 	for (i = 0; i < clk_values; i++) {
357 		if (!clk_buf)
358 			curr_dr_freq = max_dr_freq;
359 		else
360 			curr_dr_freq = (is_gear) ?
361 				(max_dr_freq >>  clk_buf[i]) :
362 				clk_buf[i] * SDW_DOUBLE_RATE_FACTOR;
363 
364 		if (curr_dr_freq <= bus->params.bandwidth)
365 			continue;
366 
367 		break;
368 
369 		/*
370 		 * TODO: Check all the Slave(s) port(s) audio modes and find
371 		 * whether given clock rate is supported with glitchless
372 		 * transition.
373 		 */
374 	}
375 
376 	if (i == clk_values) {
377 		dev_err(bus->dev, "%s: could not find clock value for bandwidth %d\n",
378 			__func__, bus->params.bandwidth);
379 		return -EINVAL;
380 	}
381 
382 	ret = sdw_select_row_col(bus, curr_dr_freq);
383 	if (ret < 0) {
384 		dev_err(bus->dev, "%s: could not find frame configuration for bus dr_freq %d\n",
385 			__func__, curr_dr_freq);
386 		return -EINVAL;
387 	}
388 
389 	bus->params.curr_dr_freq = curr_dr_freq;
390 	return 0;
391 }
392 
393 /**
394  * sdw_compute_params: Compute bus, transport and port parameters
395  *
396  * @bus: SDW Bus instance
397  */
sdw_compute_params(struct sdw_bus * bus)398 int sdw_compute_params(struct sdw_bus *bus)
399 {
400 	int ret;
401 
402 	/* Computes clock frequency, frame shape and frame frequency */
403 	ret = sdw_compute_bus_params(bus);
404 	if (ret < 0)
405 		return ret;
406 
407 	/* Compute transport and port params */
408 	ret = sdw_compute_port_params(bus);
409 	if (ret < 0) {
410 		dev_err(bus->dev, "Compute transport params failed: %d\n", ret);
411 		return ret;
412 	}
413 
414 	return 0;
415 }
416 EXPORT_SYMBOL(sdw_compute_params);
417 
418 MODULE_LICENSE("Dual BSD/GPL");
419 MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation");
420