416 lines · 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 gears6 *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 118 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 channels74 * of the stream, which indicates that they operate in75 * 'mirror mode'. Make sure we reset the port offset for76 * the next device in the list77 */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, ¶ms[i], &port_bo, hstop);150 }151 152 hstop = hstop - params[i].hwidth;153 }154}155 156static 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 196static 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 227static 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 parameters261 *262 * @bus: SDW Bus instance263 */264static 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 291free_params:292 kfree(params);293out:294 kfree(group.rates);295 296 return ret;297}298 299static 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 parameters329 *330 * @bus: SDW Bus instance331 */332static 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 else356 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 find367 * whether given clock rate is supported with glitchless368 * 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 parameters391 *392 * @bus: SDW Bus instance393 */394int 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}412EXPORT_SYMBOL(sdw_compute_params);413 414MODULE_LICENSE("Dual BSD/GPL");415MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation");416