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