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/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 unsigned int lane; 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 unsigned int *lanes; 31 }; 32 33 void sdw_compute_slave_ports(struct sdw_master_runtime *m_rt, 34 struct sdw_transport_data *t_data) 35 { 36 struct sdw_slave_runtime *s_rt = NULL; 37 struct sdw_port_runtime *p_rt; 38 int port_bo, sample_int; 39 unsigned int rate, bps, ch = 0; 40 unsigned int slave_total_ch; 41 struct sdw_bus_params *b_params = &m_rt->bus->params; 42 43 port_bo = t_data->block_offset; 44 45 list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) { 46 rate = m_rt->stream->params.rate; 47 bps = m_rt->stream->params.bps; 48 sample_int = (m_rt->bus->params.curr_dr_freq / rate); 49 slave_total_ch = 0; 50 51 list_for_each_entry(p_rt, &s_rt->port_list, port_node) { 52 if (p_rt->lane != t_data->lane) 53 continue; 54 55 ch = hweight32(p_rt->ch_mask); 56 57 sdw_fill_xport_params(&p_rt->transport_params, 58 p_rt->num, false, 59 SDW_BLK_GRP_CNT_1, 60 sample_int, port_bo, port_bo >> 8, 61 t_data->hstart, 62 t_data->hstop, 63 SDW_BLK_PKG_PER_PORT, p_rt->lane); 64 65 sdw_fill_port_params(&p_rt->port_params, 66 p_rt->num, bps, 67 SDW_PORT_FLOW_MODE_ISOCH, 68 b_params->s_data_mode); 69 70 port_bo += bps * ch; 71 slave_total_ch += ch; 72 } 73 74 if (m_rt->direction == SDW_DATA_DIR_TX && 75 m_rt->ch_count == slave_total_ch) { 76 /* 77 * Slave devices were configured to access all channels 78 * of the stream, which indicates that they operate in 79 * 'mirror mode'. Make sure we reset the port offset for 80 * the next device in the list 81 */ 82 port_bo = t_data->block_offset; 83 } 84 } 85 } 86 EXPORT_SYMBOL(sdw_compute_slave_ports); 87 88 static void sdw_compute_dp0_slave_ports(struct sdw_master_runtime *m_rt) 89 { 90 struct sdw_bus *bus = m_rt->bus; 91 struct sdw_slave_runtime *s_rt; 92 struct sdw_port_runtime *p_rt; 93 94 list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) { 95 list_for_each_entry(p_rt, &s_rt->port_list, port_node) { 96 sdw_fill_xport_params(&p_rt->transport_params, p_rt->num, false, 97 SDW_BLK_GRP_CNT_1, bus->params.col, 0, 0, 1, 98 bus->params.col - 1, SDW_BLK_PKG_PER_PORT, 0x0); 99 100 sdw_fill_port_params(&p_rt->port_params, p_rt->num, bus->params.col - 1, 101 SDW_PORT_FLOW_MODE_ISOCH, SDW_PORT_DATA_MODE_NORMAL); 102 } 103 } 104 } 105 106 static void sdw_compute_dp0_master_ports(struct sdw_master_runtime *m_rt) 107 { 108 struct sdw_port_runtime *p_rt; 109 struct sdw_bus *bus = m_rt->bus; 110 111 list_for_each_entry(p_rt, &m_rt->port_list, port_node) { 112 sdw_fill_xport_params(&p_rt->transport_params, p_rt->num, false, 113 SDW_BLK_GRP_CNT_1, bus->params.col, 0, 0, 1, 114 bus->params.col - 1, SDW_BLK_PKG_PER_PORT, 0x0); 115 116 sdw_fill_port_params(&p_rt->port_params, p_rt->num, bus->params.col - 1, 117 SDW_PORT_FLOW_MODE_ISOCH, SDW_PORT_DATA_MODE_NORMAL); 118 } 119 } 120 121 static void sdw_compute_dp0_port_params(struct sdw_bus *bus) 122 { 123 struct sdw_master_runtime *m_rt; 124 125 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) { 126 /* DP0 is for BPT only */ 127 if (m_rt->stream->type != SDW_STREAM_BPT) 128 continue; 129 sdw_compute_dp0_master_ports(m_rt); 130 sdw_compute_dp0_slave_ports(m_rt); 131 } 132 } 133 134 static void sdw_compute_master_ports(struct sdw_master_runtime *m_rt, 135 struct sdw_group_params *params, 136 int *port_bo, int hstop) 137 { 138 struct sdw_transport_data t_data = {0}; 139 struct sdw_port_runtime *p_rt; 140 struct sdw_bus *bus = m_rt->bus; 141 struct sdw_bus_params *b_params = &bus->params; 142 int sample_int, hstart = 0; 143 unsigned int rate, bps, ch; 144 145 rate = m_rt->stream->params.rate; 146 bps = m_rt->stream->params.bps; 147 ch = m_rt->ch_count; 148 sample_int = (bus->params.curr_dr_freq / rate); 149 150 if (rate != params->rate) 151 return; 152 153 t_data.hstop = hstop; 154 hstart = hstop - params->hwidth + 1; 155 t_data.hstart = hstart; 156 157 list_for_each_entry(p_rt, &m_rt->port_list, port_node) { 158 if (p_rt->lane != params->lane) 159 continue; 160 161 sdw_fill_xport_params(&p_rt->transport_params, p_rt->num, 162 false, SDW_BLK_GRP_CNT_1, sample_int, 163 *port_bo, (*port_bo) >> 8, hstart, hstop, 164 SDW_BLK_PKG_PER_PORT, p_rt->lane); 165 166 sdw_fill_port_params(&p_rt->port_params, 167 p_rt->num, bps, 168 SDW_PORT_FLOW_MODE_ISOCH, 169 b_params->m_data_mode); 170 171 /* Check for first entry */ 172 if (!(p_rt == list_first_entry(&m_rt->port_list, 173 struct sdw_port_runtime, 174 port_node))) { 175 (*port_bo) += bps * ch; 176 continue; 177 } 178 179 t_data.hstart = hstart; 180 t_data.hstop = hstop; 181 t_data.block_offset = *port_bo; 182 t_data.sub_block_offset = 0; 183 (*port_bo) += bps * ch; 184 } 185 186 t_data.lane = params->lane; 187 sdw_compute_slave_ports(m_rt, &t_data); 188 } 189 190 static void _sdw_compute_port_params(struct sdw_bus *bus, 191 struct sdw_group_params *params, int count) 192 { 193 struct sdw_master_runtime *m_rt; 194 int port_bo, i, l; 195 int hstop; 196 197 /* Run loop for all groups to compute transport parameters */ 198 for (l = 0; l < SDW_MAX_LANES; l++) { 199 if (l > 0 && !bus->lane_used_bandwidth[l]) 200 continue; 201 /* reset hstop for each lane */ 202 hstop = bus->params.col - 1; 203 for (i = 0; i < count; i++) { 204 if (params[i].lane != l) 205 continue; 206 port_bo = 1; 207 208 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) { 209 /* 210 * Only runtimes with CONFIGURED, PREPARED, ENABLED, and DISABLED 211 * states should be included in the bandwidth calculation. 212 */ 213 if (m_rt->stream->state > SDW_STREAM_DISABLED || 214 m_rt->stream->state < SDW_STREAM_CONFIGURED) 215 continue; 216 sdw_compute_master_ports(m_rt, ¶ms[i], &port_bo, hstop); 217 } 218 219 hstop = hstop - params[i].hwidth; 220 } 221 } 222 } 223 224 static int sdw_compute_group_params(struct sdw_bus *bus, 225 struct sdw_stream_runtime *stream, 226 struct sdw_group_params *params, 227 struct sdw_group *group) 228 { 229 struct sdw_master_runtime *m_rt; 230 struct sdw_port_runtime *p_rt; 231 int sel_col = bus->params.col; 232 unsigned int rate, bps, ch; 233 int i, l, column_needed; 234 235 /* Calculate bandwidth per group */ 236 for (i = 0; i < group->count; i++) { 237 params[i].rate = group->rates[i]; 238 params[i].lane = group->lanes[i]; 239 params[i].full_bw = bus->params.curr_dr_freq / params[i].rate; 240 } 241 242 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) { 243 if (m_rt->stream == stream) { 244 /* Only runtime during prepare should be added */ 245 if (stream->state != SDW_STREAM_CONFIGURED) 246 continue; 247 } else { 248 /* 249 * Include runtimes with running (ENABLED/PREPARED state) and 250 * paused (DISABLED state) streams 251 */ 252 if (m_rt->stream->state != SDW_STREAM_ENABLED && 253 m_rt->stream->state != SDW_STREAM_PREPARED && 254 m_rt->stream->state != SDW_STREAM_DISABLED) 255 continue; 256 } 257 list_for_each_entry(p_rt, &m_rt->port_list, port_node) { 258 rate = m_rt->stream->params.rate; 259 bps = m_rt->stream->params.bps; 260 ch = hweight32(p_rt->ch_mask); 261 262 for (i = 0; i < group->count; i++) { 263 if (rate == params[i].rate && p_rt->lane == params[i].lane) 264 params[i].payload_bw += bps * ch; 265 } 266 } 267 } 268 269 for (l = 0; l < SDW_MAX_LANES; l++) { 270 if (l > 0 && !bus->lane_used_bandwidth[l]) 271 continue; 272 /* reset column_needed for each lane */ 273 column_needed = 0; 274 for (i = 0; i < group->count; i++) { 275 if (params[i].lane != l) 276 continue; 277 278 params[i].hwidth = (sel_col * params[i].payload_bw + 279 params[i].full_bw - 1) / params[i].full_bw; 280 281 column_needed += params[i].hwidth; 282 /* There is no control column for lane 1 and above */ 283 if (column_needed > sel_col) 284 return -EINVAL; 285 /* Column 0 is control column on lane 0 */ 286 if (params[i].lane == 0 && column_needed > sel_col - 1) 287 return -EINVAL; 288 } 289 } 290 291 292 return 0; 293 } 294 295 static int sdw_add_element_group_count(struct sdw_group *group, 296 unsigned int rate, unsigned int lane) 297 { 298 int num = group->count; 299 int i; 300 301 for (i = 0; i < num; i++) { 302 if (rate == group->rates[i] && lane == group->lanes[i]) 303 return 0; 304 } 305 306 if (group->count >= group->max_size) { 307 unsigned int *rates; 308 unsigned int *lanes; 309 310 rates = krealloc_array(group->rates, group->max_size + 1, 311 sizeof(*group->rates), GFP_KERNEL); 312 if (!rates) 313 return -ENOMEM; 314 315 group->rates = rates; 316 317 lanes = krealloc_array(group->lanes, group->max_size + 1, 318 sizeof(*group->lanes), GFP_KERNEL); 319 if (!lanes) 320 return -ENOMEM; 321 322 group->lanes = lanes; 323 324 group->max_size += 1; 325 } 326 327 group->rates[group->count] = rate; 328 group->lanes[group->count++] = lane; 329 330 return 0; 331 } 332 333 static int sdw_get_group_count(struct sdw_bus *bus, 334 struct sdw_group *group) 335 { 336 struct sdw_master_runtime *m_rt; 337 struct sdw_port_runtime *p_rt; 338 unsigned int rate; 339 int ret = 0; 340 341 group->count = 0; 342 group->max_size = SDW_STRM_RATE_GROUPING; 343 group->rates = kcalloc(group->max_size, sizeof(int), GFP_KERNEL); 344 if (!group->rates) 345 return -ENOMEM; 346 347 group->lanes = kcalloc(group->max_size, sizeof(int), GFP_KERNEL); 348 if (!group->lanes) { 349 kfree(group->rates); 350 group->rates = NULL; 351 return -ENOMEM; 352 } 353 354 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) { 355 if (m_rt->stream->state == SDW_STREAM_DEPREPARED) 356 continue; 357 358 rate = m_rt->stream->params.rate; 359 if (m_rt == list_first_entry(&bus->m_rt_list, 360 struct sdw_master_runtime, 361 bus_node)) { 362 group->rates[group->count++] = rate; 363 } 364 /* 365 * Different ports could use different lane, add group element 366 * even if m_rt is the first entry 367 */ 368 list_for_each_entry(p_rt, &m_rt->port_list, port_node) { 369 ret = sdw_add_element_group_count(group, rate, p_rt->lane); 370 if (ret < 0) { 371 kfree(group->rates); 372 kfree(group->lanes); 373 return ret; 374 } 375 } 376 } 377 378 return ret; 379 } 380 381 /** 382 * sdw_compute_port_params: Compute transport and port parameters 383 * 384 * @bus: SDW Bus instance 385 * @stream: Soundwire stream 386 */ 387 static int sdw_compute_port_params(struct sdw_bus *bus, struct sdw_stream_runtime *stream) 388 { 389 struct sdw_group_params *params = NULL; 390 struct sdw_group group; 391 int ret; 392 393 ret = sdw_get_group_count(bus, &group); 394 if (ret < 0) 395 return ret; 396 397 if (group.count == 0) 398 goto out; 399 400 params = kzalloc_objs(*params, group.count); 401 if (!params) { 402 ret = -ENOMEM; 403 goto out; 404 } 405 406 /* Compute transport parameters for grouped streams */ 407 ret = sdw_compute_group_params(bus, stream, params, &group); 408 if (ret < 0) 409 goto free_params; 410 411 _sdw_compute_port_params(bus, params, group.count); 412 413 free_params: 414 kfree(params); 415 out: 416 kfree(group.rates); 417 kfree(group.lanes); 418 419 return ret; 420 } 421 422 static int sdw_select_row_col(struct sdw_bus *bus, int clk_freq) 423 { 424 struct sdw_master_prop *prop = &bus->prop; 425 int r, c; 426 427 for (c = 0; c < SDW_FRAME_COLS; c++) { 428 for (r = 0; r < SDW_FRAME_ROWS; r++) { 429 if (sdw_rows[r] != prop->default_row || 430 sdw_cols[c] != prop->default_col) 431 continue; 432 433 if (clk_freq * (sdw_cols[c] - 1) < 434 bus->params.bandwidth * sdw_cols[c]) 435 continue; 436 437 bus->params.row = sdw_rows[r]; 438 bus->params.col = sdw_cols[c]; 439 return 0; 440 } 441 } 442 443 return -EINVAL; 444 } 445 446 static bool is_clock_scaling_supported(struct sdw_bus *bus) 447 { 448 struct sdw_master_runtime *m_rt; 449 struct sdw_slave_runtime *s_rt; 450 451 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) 452 list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) 453 if (!is_clock_scaling_supported_by_slave(s_rt->slave)) 454 return false; 455 456 return true; 457 } 458 459 /** 460 * is_lane_connected_to_all_peripherals: Check if the given manager lane connects to all peripherals 461 * So that all peripherals can use the manager lane. 462 * 463 * @m_rt: Manager runtime 464 * @lane: Lane number 465 */ 466 static bool is_lane_connected_to_all_peripherals(struct sdw_master_runtime *m_rt, unsigned int lane) 467 { 468 struct sdw_slave_prop *slave_prop; 469 struct sdw_slave_runtime *s_rt; 470 int i; 471 472 list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) { 473 slave_prop = &s_rt->slave->prop; 474 for (i = 1; i < SDW_MAX_LANES; i++) { 475 if (slave_prop->lane_maps[i] == lane) { 476 dev_dbg(&s_rt->slave->dev, 477 "M lane %d is connected to P lane %d\n", 478 lane, i); 479 break; 480 } 481 } 482 if (i == SDW_MAX_LANES) { 483 dev_dbg(&s_rt->slave->dev, "M lane %d is not connected\n", lane); 484 return false; 485 } 486 } 487 return true; 488 } 489 490 static int get_manager_lane(struct sdw_bus *bus, struct sdw_master_runtime *m_rt, 491 struct sdw_slave_runtime *s_rt, unsigned int curr_dr_freq) 492 { 493 struct sdw_slave_prop *slave_prop = &s_rt->slave->prop; 494 struct sdw_port_runtime *m_p_rt; 495 unsigned int required_bandwidth; 496 int m_lane; 497 int l; 498 499 for (l = 1; l < SDW_MAX_LANES; l++) { 500 if (!slave_prop->lane_maps[l]) 501 continue; 502 503 required_bandwidth = 0; 504 list_for_each_entry(m_p_rt, &m_rt->port_list, port_node) { 505 required_bandwidth += m_rt->stream->params.rate * 506 hweight32(m_p_rt->ch_mask) * 507 m_rt->stream->params.bps; 508 } 509 if (required_bandwidth <= 510 curr_dr_freq - bus->lane_used_bandwidth[l]) { 511 /* Check if m_lane is connected to all Peripherals */ 512 if (!is_lane_connected_to_all_peripherals(m_rt, 513 slave_prop->lane_maps[l])) { 514 dev_dbg(bus->dev, 515 "Not all Peripherals are connected to M lane %d\n", 516 slave_prop->lane_maps[l]); 517 continue; 518 } 519 m_lane = slave_prop->lane_maps[l]; 520 dev_dbg(&s_rt->slave->dev, "M lane %d is used\n", m_lane); 521 bus->lane_used_bandwidth[l] += required_bandwidth; 522 /* 523 * Use non-zero manager lane, subtract the lane 0 524 * bandwidth that is already calculated 525 */ 526 bus->params.bandwidth -= required_bandwidth; 527 return m_lane; 528 } 529 } 530 531 /* No available multi lane found, only lane 0 can be used */ 532 return 0; 533 } 534 535 /** 536 * sdw_compute_bus_params: Compute bus parameters 537 * 538 * @bus: SDW Bus instance 539 */ 540 static int sdw_compute_bus_params(struct sdw_bus *bus) 541 { 542 struct sdw_master_prop *mstr_prop = &bus->prop; 543 struct sdw_slave_prop *slave_prop; 544 struct sdw_port_runtime *m_p_rt; 545 struct sdw_port_runtime *s_p_rt; 546 struct sdw_master_runtime *m_rt; 547 struct sdw_slave_runtime *s_rt; 548 unsigned int curr_dr_freq = 0; 549 int i, l, clk_values, ret; 550 bool is_gear = false; 551 int m_lane = 0; 552 u32 *clk_buf; 553 554 if (mstr_prop->num_clk_gears) { 555 clk_values = mstr_prop->num_clk_gears; 556 clk_buf = mstr_prop->clk_gears; 557 is_gear = true; 558 } else if (mstr_prop->num_clk_freq) { 559 clk_values = mstr_prop->num_clk_freq; 560 clk_buf = mstr_prop->clk_freq; 561 } else { 562 clk_values = 1; 563 clk_buf = NULL; 564 } 565 566 /* If dynamic scaling is not supported, don't try higher freq */ 567 if (!is_clock_scaling_supported(bus)) 568 clk_values = 1; 569 570 for (i = 0; i < clk_values; i++) { 571 if (!clk_buf) 572 curr_dr_freq = bus->params.max_dr_freq; 573 else 574 curr_dr_freq = (is_gear) ? 575 (bus->params.max_dr_freq >> clk_buf[i]) : 576 clk_buf[i] * SDW_DOUBLE_RATE_FACTOR; 577 578 if (curr_dr_freq * (mstr_prop->default_col - 1) >= 579 bus->params.bandwidth * mstr_prop->default_col) 580 break; 581 582 list_for_each_entry(m_rt, &bus->m_rt_list, bus_node) { 583 /* 584 * Get the first s_rt that will be used to find the available lane that 585 * can be used. No need to check all Peripherals because we can't use 586 * multi-lane if we can't find any available lane for the first Peripheral. 587 */ 588 s_rt = list_first_entry(&m_rt->slave_rt_list, 589 struct sdw_slave_runtime, m_rt_node); 590 591 /* 592 * Find the available Manager lane that connected to the first Peripheral. 593 */ 594 m_lane = get_manager_lane(bus, m_rt, s_rt, curr_dr_freq); 595 if (m_lane > 0) 596 goto out; 597 } 598 599 /* 600 * TODO: Check all the Slave(s) port(s) audio modes and find 601 * whether given clock rate is supported with glitchless 602 * transition. 603 */ 604 } 605 606 if (i == clk_values) { 607 dev_err(bus->dev, "%s: could not find clock value for bandwidth %d\n", 608 __func__, bus->params.bandwidth); 609 return -EINVAL; 610 } 611 out: 612 /* multilane can be used */ 613 if (m_lane > 0) { 614 /* Set Peripheral lanes */ 615 list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) { 616 slave_prop = &s_rt->slave->prop; 617 for (l = 1; l < SDW_MAX_LANES; l++) { 618 if (slave_prop->lane_maps[l] == m_lane) { 619 list_for_each_entry(s_p_rt, &s_rt->port_list, port_node) { 620 s_p_rt->lane = l; 621 dev_dbg(&s_rt->slave->dev, 622 "Set P lane %d for port %d\n", 623 l, s_p_rt->num); 624 } 625 break; 626 } 627 } 628 } 629 /* 630 * Set Manager lanes. Configure the last m_rt in bus->m_rt_list only since 631 * we don't want to touch other m_rts that are already working. 632 */ 633 list_for_each_entry(m_p_rt, &m_rt->port_list, port_node) { 634 m_p_rt->lane = m_lane; 635 } 636 } 637 638 if (!mstr_prop->default_frame_rate || !mstr_prop->default_row) 639 return -EINVAL; 640 641 mstr_prop->default_col = curr_dr_freq / mstr_prop->default_frame_rate / 642 mstr_prop->default_row; 643 644 ret = sdw_select_row_col(bus, curr_dr_freq); 645 if (ret < 0) { 646 dev_err(bus->dev, "%s: could not find frame configuration for bus dr_freq %d\n", 647 __func__, curr_dr_freq); 648 return -EINVAL; 649 } 650 651 bus->params.curr_dr_freq = curr_dr_freq; 652 return 0; 653 } 654 655 /** 656 * sdw_compute_params: Compute bus, transport and port parameters 657 * 658 * @bus: SDW Bus instance 659 * @stream: Soundwire stream 660 */ 661 int sdw_compute_params(struct sdw_bus *bus, struct sdw_stream_runtime *stream) 662 { 663 int ret; 664 665 /* Computes clock frequency, frame shape and frame frequency */ 666 ret = sdw_compute_bus_params(bus); 667 if (ret < 0) 668 return ret; 669 670 if (stream->type == SDW_STREAM_BPT) { 671 sdw_compute_dp0_port_params(bus); 672 return 0; 673 } 674 675 /* Compute transport and port params */ 676 ret = sdw_compute_port_params(bus, stream); 677 if (ret < 0) { 678 dev_err(bus->dev, "Compute transport params failed: %d\n", ret); 679 return ret; 680 } 681 682 return 0; 683 } 684 EXPORT_SYMBOL(sdw_compute_params); 685 686 MODULE_LICENSE("Dual BSD/GPL"); 687 MODULE_DESCRIPTION("SoundWire Generic Bandwidth Allocation"); 688