1 // SPDX-License-Identifier: GPL-2.0+ 2 /* Microchip Sparx5 Switch driver 3 * 4 * Copyright (c) 2022 Microchip Technology Inc. and its subsidiaries. 5 */ 6 7 #include <net/pkt_cls.h> 8 9 #include "sparx5_main.h" 10 #include "sparx5_qos.h" 11 12 /* Calculate new base_time based on cycle_time. 13 * 14 * The hardware requires a base_time that is always in the future. 15 * We define threshold_time as current_time + (2 * cycle_time). 16 * If base_time is below threshold_time this function recalculates it to be in 17 * the interval: 18 * threshold_time <= base_time < (threshold_time + cycle_time) 19 * 20 * A very simple algorithm could be like this: 21 * new_base_time = org_base_time + N * cycle_time 22 * using the lowest N so (new_base_time >= threshold_time 23 */ 24 void sparx5_new_base_time(struct sparx5 *sparx5, const u32 cycle_time, 25 const ktime_t org_base_time, ktime_t *new_base_time) 26 { 27 ktime_t current_time, threshold_time, new_time; 28 struct timespec64 ts; 29 u64 nr_of_cycles_p2; 30 u64 nr_of_cycles; 31 u64 diff_time; 32 33 new_time = org_base_time; 34 35 sparx5_ptp_gettime64(&sparx5->phc[SPARX5_PHC_PORT].info, &ts); 36 current_time = timespec64_to_ktime(ts); 37 threshold_time = current_time + (2 * cycle_time); 38 diff_time = threshold_time - new_time; 39 nr_of_cycles = div_u64(diff_time, cycle_time); 40 nr_of_cycles_p2 = 1; /* Use 2^0 as start value */ 41 42 if (new_time >= threshold_time) { 43 *new_base_time = new_time; 44 return; 45 } 46 47 /* Calculate the smallest power of 2 (nr_of_cycles_p2) 48 * that is larger than nr_of_cycles. 49 */ 50 while (nr_of_cycles_p2 < nr_of_cycles) 51 nr_of_cycles_p2 <<= 1; /* Next (higher) power of 2 */ 52 53 /* Add as big chunks (power of 2 * cycle_time) 54 * as possible for each power of 2 55 */ 56 while (nr_of_cycles_p2) { 57 if (new_time < threshold_time) { 58 new_time += cycle_time * nr_of_cycles_p2; 59 while (new_time < threshold_time) 60 new_time += cycle_time * nr_of_cycles_p2; 61 new_time -= cycle_time * nr_of_cycles_p2; 62 } 63 nr_of_cycles_p2 >>= 1; /* Next (lower) power of 2 */ 64 } 65 new_time += cycle_time; 66 *new_base_time = new_time; 67 } 68 69 /* Max rates for leak groups */ 70 static const u32 spx5_hsch_max_group_rate[SPX5_HSCH_LEAK_GRP_CNT] = { 71 1048568, /* 1.049 Gbps */ 72 2621420, /* 2.621 Gbps */ 73 10485680, /* 10.486 Gbps */ 74 26214200 /* 26.214 Gbps */ 75 }; 76 77 u32 sparx5_get_hsch_max_group_rate(int grp) 78 { 79 return spx5_hsch_max_group_rate[grp]; 80 } 81 82 static struct sparx5_layer layers[SPX5_HSCH_LAYER_CNT]; 83 84 static u32 sparx5_lg_get_leak_time(struct sparx5 *sparx5, u32 layer, u32 group) 85 { 86 u32 value; 87 88 value = spx5_rd(sparx5, HSCH_HSCH_TIMER_CFG(layer, group)); 89 return HSCH_HSCH_TIMER_CFG_LEAK_TIME_GET(value); 90 } 91 92 static void sparx5_lg_set_leak_time(struct sparx5 *sparx5, u32 layer, u32 group, 93 u32 leak_time) 94 { 95 spx5_wr(HSCH_HSCH_TIMER_CFG_LEAK_TIME_SET(leak_time), sparx5, 96 HSCH_HSCH_TIMER_CFG(layer, group)); 97 } 98 99 static u32 sparx5_lg_get_first(struct sparx5 *sparx5, u32 layer, u32 group) 100 { 101 u32 value; 102 103 value = spx5_rd(sparx5, HSCH_HSCH_LEAK_CFG(layer, group)); 104 return HSCH_HSCH_LEAK_CFG_LEAK_FIRST_GET(value); 105 } 106 107 static u32 sparx5_lg_get_next(struct sparx5 *sparx5, u32 layer, u32 group, 108 u32 idx) 109 110 { 111 u32 value; 112 113 value = spx5_rd(sparx5, HSCH_SE_CONNECT(idx)); 114 return HSCH_SE_CONNECT_SE_LEAK_LINK_GET(value); 115 } 116 117 static u32 sparx5_lg_get_last(struct sparx5 *sparx5, u32 layer, u32 group) 118 { 119 u32 itr, next; 120 121 itr = sparx5_lg_get_first(sparx5, layer, group); 122 123 for (;;) { 124 next = sparx5_lg_get_next(sparx5, layer, group, itr); 125 if (itr == next) 126 return itr; 127 128 itr = next; 129 } 130 } 131 132 static bool sparx5_lg_is_last(struct sparx5 *sparx5, u32 layer, u32 group, 133 u32 idx) 134 { 135 return idx == sparx5_lg_get_next(sparx5, layer, group, idx); 136 } 137 138 static bool sparx5_lg_is_first(struct sparx5 *sparx5, u32 layer, u32 group, 139 u32 idx) 140 { 141 return idx == sparx5_lg_get_first(sparx5, layer, group); 142 } 143 144 static bool sparx5_lg_is_empty(struct sparx5 *sparx5, u32 layer, u32 group) 145 { 146 return sparx5_lg_get_leak_time(sparx5, layer, group) == 0; 147 } 148 149 static bool sparx5_lg_is_singular(struct sparx5 *sparx5, u32 layer, u32 group) 150 { 151 if (sparx5_lg_is_empty(sparx5, layer, group)) 152 return false; 153 154 return sparx5_lg_get_first(sparx5, layer, group) == 155 sparx5_lg_get_last(sparx5, layer, group); 156 } 157 158 static void sparx5_lg_enable(struct sparx5 *sparx5, u32 layer, u32 group, 159 u32 leak_time) 160 { 161 sparx5_lg_set_leak_time(sparx5, layer, group, leak_time); 162 } 163 164 static void sparx5_lg_disable(struct sparx5 *sparx5, u32 layer, u32 group) 165 { 166 sparx5_lg_set_leak_time(sparx5, layer, group, 0); 167 } 168 169 static int sparx5_lg_get_group_by_index(struct sparx5 *sparx5, u32 layer, 170 u32 idx, u32 *group) 171 { 172 u32 itr, next; 173 int i; 174 175 for (i = 0; i < SPX5_HSCH_LEAK_GRP_CNT; i++) { 176 if (sparx5_lg_is_empty(sparx5, layer, i)) 177 continue; 178 179 itr = sparx5_lg_get_first(sparx5, layer, i); 180 181 for (;;) { 182 next = sparx5_lg_get_next(sparx5, layer, i, itr); 183 184 if (itr == idx) { 185 *group = i; 186 return 0; /* Found it */ 187 } 188 if (itr == next) 189 break; /* Was not found */ 190 191 itr = next; 192 } 193 } 194 195 return -1; 196 } 197 198 static int sparx5_lg_get_group_by_rate(u32 layer, u32 rate, u32 *group) 199 { 200 struct sparx5_layer *l = &layers[layer]; 201 struct sparx5_lg *lg; 202 u32 i; 203 204 for (i = 0; i < SPX5_HSCH_LEAK_GRP_CNT; i++) { 205 lg = &l->leak_groups[i]; 206 if (rate <= lg->max_rate) { 207 *group = i; 208 return 0; 209 } 210 } 211 212 return -1; 213 } 214 215 static int sparx5_lg_get_adjacent(struct sparx5 *sparx5, u32 layer, u32 group, 216 u32 idx, u32 *prev, u32 *next, u32 *first) 217 { 218 u32 itr; 219 220 *first = sparx5_lg_get_first(sparx5, layer, group); 221 *prev = *first; 222 *next = *first; 223 itr = *first; 224 225 for (;;) { 226 *next = sparx5_lg_get_next(sparx5, layer, group, itr); 227 228 if (itr == idx) 229 return 0; /* Found it */ 230 231 if (itr == *next) 232 return -1; /* Was not found */ 233 234 *prev = itr; 235 itr = *next; 236 } 237 238 return -1; 239 } 240 241 static int sparx5_lg_conf_set(struct sparx5 *sparx5, u32 layer, u32 group, 242 u32 se_first, u32 idx, u32 idx_next, bool empty) 243 { 244 u32 leak_time = layers[layer].leak_groups[group].leak_time; 245 246 /* Stop leaking */ 247 sparx5_lg_disable(sparx5, layer, group); 248 249 if (empty) 250 return 0; 251 252 /* Select layer */ 253 spx5_rmw(HSCH_HSCH_CFG_CFG_HSCH_LAYER_SET(layer), 254 HSCH_HSCH_CFG_CFG_HSCH_LAYER, sparx5, HSCH_HSCH_CFG_CFG); 255 256 /* Link elements */ 257 spx5_wr(HSCH_SE_CONNECT_SE_LEAK_LINK_SET(idx_next), sparx5, 258 HSCH_SE_CONNECT(idx)); 259 260 /* Set the first element. */ 261 spx5_rmw(HSCH_HSCH_LEAK_CFG_LEAK_FIRST_SET(se_first), 262 HSCH_HSCH_LEAK_CFG_LEAK_FIRST, sparx5, 263 HSCH_HSCH_LEAK_CFG(layer, group)); 264 265 /* Start leaking */ 266 sparx5_lg_enable(sparx5, layer, group, leak_time); 267 268 return 0; 269 } 270 271 static int sparx5_lg_del(struct sparx5 *sparx5, u32 layer, u32 group, u32 idx) 272 { 273 u32 first, next, prev; 274 bool empty = false; 275 276 /* idx *must* be present in the leak group */ 277 WARN_ON(sparx5_lg_get_adjacent(sparx5, layer, group, idx, &prev, &next, 278 &first) < 0); 279 280 if (sparx5_lg_is_singular(sparx5, layer, group)) { 281 empty = true; 282 } else if (sparx5_lg_is_last(sparx5, layer, group, idx)) { 283 /* idx is removed, prev is now last */ 284 idx = prev; 285 next = prev; 286 } else if (sparx5_lg_is_first(sparx5, layer, group, idx)) { 287 /* idx is removed and points to itself, first is next */ 288 first = next; 289 next = idx; 290 } else { 291 /* Next is not touched */ 292 idx = prev; 293 } 294 295 return sparx5_lg_conf_set(sparx5, layer, group, first, idx, next, 296 empty); 297 } 298 299 static int sparx5_lg_add(struct sparx5 *sparx5, u32 layer, u32 new_group, 300 u32 idx) 301 { 302 u32 first, next, old_group; 303 304 pr_debug("ADD: layer: %d, new_group: %d, idx: %d", layer, new_group, 305 idx); 306 307 /* Is this SE already shaping ? */ 308 if (sparx5_lg_get_group_by_index(sparx5, layer, idx, &old_group) >= 0) { 309 if (old_group != new_group) { 310 /* Delete from old group */ 311 sparx5_lg_del(sparx5, layer, old_group, idx); 312 } else { 313 /* Nothing to do here */ 314 return 0; 315 } 316 } 317 318 /* We always add to head of the list */ 319 first = idx; 320 321 if (sparx5_lg_is_empty(sparx5, layer, new_group)) 322 next = idx; 323 else 324 next = sparx5_lg_get_first(sparx5, layer, new_group); 325 326 return sparx5_lg_conf_set(sparx5, layer, new_group, first, idx, next, 327 false); 328 } 329 330 static int sparx5_shaper_conf_set(struct sparx5_port *port, 331 const struct sparx5_shaper *sh, u32 layer, 332 u32 idx, u32 group) 333 { 334 int (*sparx5_lg_action)(struct sparx5 *, u32, u32, u32); 335 struct sparx5 *sparx5 = port->sparx5; 336 337 if (!sh->rate && !sh->burst) 338 sparx5_lg_action = &sparx5_lg_del; 339 else 340 sparx5_lg_action = &sparx5_lg_add; 341 342 /* Select layer */ 343 spx5_rmw(HSCH_HSCH_CFG_CFG_HSCH_LAYER_SET(layer), 344 HSCH_HSCH_CFG_CFG_HSCH_LAYER, sparx5, HSCH_HSCH_CFG_CFG); 345 346 /* Set frame mode */ 347 spx5_rmw(HSCH_SE_CFG_SE_FRM_MODE_SET(sh->mode), HSCH_SE_CFG_SE_FRM_MODE, 348 sparx5, HSCH_SE_CFG(idx)); 349 350 /* Set committed rate and burst */ 351 spx5_wr(HSCH_CIR_CFG_CIR_RATE_SET(sh->rate) | 352 HSCH_CIR_CFG_CIR_BURST_SET(sh->burst), 353 sparx5, HSCH_CIR_CFG(idx)); 354 355 /* This has to be done after the shaper configuration has been set */ 356 sparx5_lg_action(sparx5, layer, group, idx); 357 358 return 0; 359 } 360 361 static u32 sparx5_weight_to_hw_cost(u32 weight_min, u32 weight) 362 { 363 return ((((SPX5_DWRR_COST_MAX << 4) * weight_min / weight) + 8) >> 4) - 364 1; 365 } 366 367 static int sparx5_dwrr_conf_set(struct sparx5_port *port, 368 struct sparx5_dwrr *dwrr) 369 { 370 u32 layer = is_sparx5(port->sparx5) ? 2 : 1; 371 int i; 372 373 spx5_rmw(HSCH_HSCH_CFG_CFG_HSCH_LAYER_SET(layer) | 374 HSCH_HSCH_CFG_CFG_CFG_SE_IDX_SET(port->portno), 375 HSCH_HSCH_CFG_CFG_HSCH_LAYER | HSCH_HSCH_CFG_CFG_CFG_SE_IDX, 376 port->sparx5, HSCH_HSCH_CFG_CFG); 377 378 /* Number of *lower* indexes that are arbitrated dwrr */ 379 spx5_rmw(HSCH_SE_CFG_SE_DWRR_CNT_SET(dwrr->count), 380 HSCH_SE_CFG_SE_DWRR_CNT, port->sparx5, 381 HSCH_SE_CFG(port->portno)); 382 383 for (i = 0; i < dwrr->count; i++) { 384 spx5_rmw(HSCH_DWRR_ENTRY_DWRR_COST_SET(dwrr->cost[i]), 385 HSCH_DWRR_ENTRY_DWRR_COST, port->sparx5, 386 HSCH_DWRR_ENTRY(i)); 387 } 388 389 return 0; 390 } 391 392 static int sparx5_leak_groups_init(struct sparx5 *sparx5) 393 { 394 const struct sparx5_ops *ops = sparx5->data->ops; 395 struct sparx5_layer *layer; 396 u32 sys_clk_per_100ps; 397 struct sparx5_lg *lg; 398 u32 leak_time_us; 399 int i, ii; 400 401 sys_clk_per_100ps = spx5_rd(sparx5, HSCH_SYS_CLK_PER); 402 403 for (i = 0; i < SPX5_HSCH_LAYER_CNT; i++) { 404 layer = &layers[i]; 405 for (ii = 0; ii < SPX5_HSCH_LEAK_GRP_CNT; ii++) { 406 lg = &layer->leak_groups[ii]; 407 lg->max_rate = ops->get_hsch_max_group_rate(i); 408 409 /* Calculate the leak time in us, to serve a maximum 410 * rate of 'max_rate' for this group 411 */ 412 leak_time_us = (SPX5_SE_RATE_MAX * 1000) / lg->max_rate; 413 414 /* Hardware wants leak time in ns */ 415 lg->leak_time = 1000 * leak_time_us; 416 417 /* Calculate resolution */ 418 lg->resolution = 1000 / leak_time_us; 419 420 /* Maximum number of shapers that can be served by 421 * this leak group 422 */ 423 lg->max_ses = (1000 * leak_time_us) / sys_clk_per_100ps; 424 425 /* Example: 426 * Wanted bandwidth is 100Mbit: 427 * 428 * 100 mbps can be served by leak group zero. 429 * 430 * leak_time is 125000 ns. 431 * resolution is: 8 432 * 433 * cir = 100000 / 8 = 12500 434 * leaks_pr_sec = 125000 / 10^9 = 8000 435 * bw = 12500 * 8000 = 10^8 (100 Mbit) 436 */ 437 438 /* Disable by default - this also indicates an empty 439 * leak group 440 */ 441 sparx5_lg_disable(sparx5, i, ii); 442 } 443 } 444 445 return 0; 446 } 447 448 int sparx5_qos_init(struct sparx5 *sparx5) 449 { 450 int ret; 451 452 ret = sparx5_leak_groups_init(sparx5); 453 if (ret < 0) 454 return ret; 455 456 ret = sparx5_dcb_init(sparx5); 457 if (ret < 0) 458 return ret; 459 460 sparx5_psfp_init(sparx5); 461 462 return 0; 463 } 464 465 int sparx5_tc_mqprio_add(struct net_device *ndev, u8 num_tc) 466 { 467 int i; 468 469 if (num_tc != SPX5_PRIOS) { 470 netdev_err(ndev, "Only %d traffic classes supported\n", 471 SPX5_PRIOS); 472 return -EINVAL; 473 } 474 475 netdev_set_num_tc(ndev, num_tc); 476 477 for (i = 0; i < num_tc; i++) 478 netdev_set_tc_queue(ndev, i, 1, i); 479 480 netdev_dbg(ndev, "dev->num_tc %u dev->real_num_tx_queues %u\n", 481 ndev->num_tc, ndev->real_num_tx_queues); 482 483 return 0; 484 } 485 486 int sparx5_tc_mqprio_del(struct net_device *ndev) 487 { 488 netdev_reset_tc(ndev); 489 490 netdev_dbg(ndev, "dev->num_tc %u dev->real_num_tx_queues %u\n", 491 ndev->num_tc, ndev->real_num_tx_queues); 492 493 return 0; 494 } 495 496 int sparx5_tc_tbf_add(struct sparx5_port *port, 497 struct tc_tbf_qopt_offload_replace_params *params, 498 u32 layer, u32 idx) 499 { 500 struct sparx5_shaper sh = { 501 .mode = SPX5_SE_MODE_DATARATE, 502 .rate = div_u64(params->rate.rate_bytes_ps, 1000) * 8, 503 .burst = params->max_size, 504 }; 505 struct sparx5_lg *lg; 506 u32 group; 507 508 /* Find suitable group for this se */ 509 if (sparx5_lg_get_group_by_rate(layer, sh.rate, &group) < 0) { 510 pr_debug("Could not find leak group for se with rate: %d", 511 sh.rate); 512 return -EINVAL; 513 } 514 515 lg = &layers[layer].leak_groups[group]; 516 517 pr_debug("Found matching group (speed: %d)\n", lg->max_rate); 518 519 if (sh.rate < SPX5_SE_RATE_MIN || sh.burst < SPX5_SE_BURST_MIN) 520 return -EINVAL; 521 522 /* Calculate committed rate and burst */ 523 sh.rate = DIV_ROUND_UP(sh.rate, lg->resolution); 524 sh.burst = DIV_ROUND_UP(sh.burst, SPX5_SE_BURST_UNIT); 525 526 if (sh.rate > SPX5_SE_RATE_MAX || sh.burst > SPX5_SE_BURST_MAX) 527 return -EINVAL; 528 529 return sparx5_shaper_conf_set(port, &sh, layer, idx, group); 530 } 531 532 int sparx5_tc_tbf_del(struct sparx5_port *port, u32 layer, u32 idx) 533 { 534 struct sparx5_shaper sh = {0}; 535 u32 group; 536 537 sparx5_lg_get_group_by_index(port->sparx5, layer, idx, &group); 538 539 return sparx5_shaper_conf_set(port, &sh, layer, idx, group); 540 } 541 542 int sparx5_tc_ets_add(struct sparx5_port *port, 543 struct tc_ets_qopt_offload_replace_params *params) 544 { 545 struct sparx5_dwrr dwrr = {0}; 546 /* Minimum weight for each iteration */ 547 unsigned int w_min = 100; 548 int i; 549 550 /* Find minimum weight for all dwrr bands */ 551 for (i = 0; i < SPX5_PRIOS; i++) { 552 if (params->quanta[i] == 0) 553 continue; 554 w_min = min(w_min, params->weights[i]); 555 } 556 557 for (i = 0; i < SPX5_PRIOS; i++) { 558 /* Strict band; skip */ 559 if (params->quanta[i] == 0) 560 continue; 561 562 dwrr.count++; 563 564 /* On the sparx5, bands with higher indexes are preferred and 565 * arbitrated strict. Strict bands are put in the lower indexes, 566 * by tc, so we reverse the bands here. 567 * 568 * Also convert the weight to something the hardware 569 * understands. 570 */ 571 dwrr.cost[SPX5_PRIOS - i - 1] = 572 sparx5_weight_to_hw_cost(w_min, params->weights[i]); 573 } 574 575 return sparx5_dwrr_conf_set(port, &dwrr); 576 } 577 578 int sparx5_tc_ets_del(struct sparx5_port *port) 579 { 580 struct sparx5_dwrr dwrr = {0}; 581 582 return sparx5_dwrr_conf_set(port, &dwrr); 583 } 584