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