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
19struct sdw_group_params {
20	unsigned int rate;
21	int full_bw;
22	int payload_bw;
23	int hwidth;
24};
25
26struct sdw_group {
27	unsigned int count;
28	unsigned int max_size;
29	unsigned int *rates;
30};
31
32void 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}
82EXPORT_SYMBOL(sdw_compute_slave_ports);
83
84static 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
137static 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
158static 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
198static 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
229static 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 */
266static 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
293free_params:
294	kfree(params);
295out:
296	kfree(group.rates);
297
298	return ret;
299}
300
301static 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 */
334static 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 */
398int 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}
416EXPORT_SYMBOL(sdw_compute_params);
417
418MODULE_LICENSE("Dual BSD/GPL");
419MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation");
420