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