1 // SPDX-License-Identifier: GPL-2.0 2 /* Texas Instruments K3 AM65 Ethernet QoS submodule 3 * Copyright (C) 2020 Texas Instruments Incorporated - http://www.ti.com/ 4 * 5 * quality of service module includes: 6 * Enhanced Scheduler Traffic (EST - P802.1Qbv/D2.2) 7 */ 8 9 #include <linux/pm_runtime.h> 10 #include <linux/time.h> 11 12 #include "am65-cpsw-nuss.h" 13 #include "am65-cpsw-qos.h" 14 #include "am65-cpts.h" 15 16 #define AM65_CPSW_REG_CTL 0x004 17 #define AM65_CPSW_PN_REG_CTL 0x004 18 #define AM65_CPSW_PN_REG_FIFO_STATUS 0x050 19 #define AM65_CPSW_PN_REG_EST_CTL 0x060 20 21 /* AM65_CPSW_REG_CTL register fields */ 22 #define AM65_CPSW_CTL_EST_EN BIT(18) 23 24 /* AM65_CPSW_PN_REG_CTL register fields */ 25 #define AM65_CPSW_PN_CTL_EST_PORT_EN BIT(17) 26 27 /* AM65_CPSW_PN_REG_EST_CTL register fields */ 28 #define AM65_CPSW_PN_EST_ONEBUF BIT(0) 29 #define AM65_CPSW_PN_EST_BUFSEL BIT(1) 30 #define AM65_CPSW_PN_EST_TS_EN BIT(2) 31 #define AM65_CPSW_PN_EST_TS_FIRST BIT(3) 32 #define AM65_CPSW_PN_EST_ONEPRI BIT(4) 33 #define AM65_CPSW_PN_EST_TS_PRI_MSK GENMASK(7, 5) 34 35 /* AM65_CPSW_PN_REG_FIFO_STATUS register fields */ 36 #define AM65_CPSW_PN_FST_TX_PRI_ACTIVE_MSK GENMASK(7, 0) 37 #define AM65_CPSW_PN_FST_TX_E_MAC_ALLOW_MSK GENMASK(15, 8) 38 #define AM65_CPSW_PN_FST_EST_CNT_ERR BIT(16) 39 #define AM65_CPSW_PN_FST_EST_ADD_ERR BIT(17) 40 #define AM65_CPSW_PN_FST_EST_BUFACT BIT(18) 41 42 /* EST FETCH COMMAND RAM */ 43 #define AM65_CPSW_FETCH_RAM_CMD_NUM 0x80 44 #define AM65_CPSW_FETCH_CNT_MSK GENMASK(21, 8) 45 #define AM65_CPSW_FETCH_CNT_MAX (AM65_CPSW_FETCH_CNT_MSK >> 8) 46 #define AM65_CPSW_FETCH_CNT_OFFSET 8 47 #define AM65_CPSW_FETCH_ALLOW_MSK GENMASK(7, 0) 48 #define AM65_CPSW_FETCH_ALLOW_MAX AM65_CPSW_FETCH_ALLOW_MSK 49 50 enum timer_act { 51 TACT_PROG, /* need program timer */ 52 TACT_NEED_STOP, /* need stop first */ 53 TACT_SKIP_PROG, /* just buffer can be updated */ 54 }; 55 56 static int am65_cpsw_port_est_enabled(struct am65_cpsw_port *port) 57 { 58 return port->qos.est_oper || port->qos.est_admin; 59 } 60 61 static void am65_cpsw_est_enable(struct am65_cpsw_common *common, int enable) 62 { 63 u32 val; 64 65 val = readl(common->cpsw_base + AM65_CPSW_REG_CTL); 66 67 if (enable) 68 val |= AM65_CPSW_CTL_EST_EN; 69 else 70 val &= ~AM65_CPSW_CTL_EST_EN; 71 72 writel(val, common->cpsw_base + AM65_CPSW_REG_CTL); 73 common->est_enabled = enable; 74 } 75 76 static void am65_cpsw_port_est_enable(struct am65_cpsw_port *port, int enable) 77 { 78 u32 val; 79 80 val = readl(port->port_base + AM65_CPSW_PN_REG_CTL); 81 if (enable) 82 val |= AM65_CPSW_PN_CTL_EST_PORT_EN; 83 else 84 val &= ~AM65_CPSW_PN_CTL_EST_PORT_EN; 85 86 writel(val, port->port_base + AM65_CPSW_PN_REG_CTL); 87 } 88 89 /* target new EST RAM buffer, actual toggle happens after cycle completion */ 90 static void am65_cpsw_port_est_assign_buf_num(struct net_device *ndev, 91 int buf_num) 92 { 93 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 94 u32 val; 95 96 val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL); 97 if (buf_num) 98 val |= AM65_CPSW_PN_EST_BUFSEL; 99 else 100 val &= ~AM65_CPSW_PN_EST_BUFSEL; 101 102 writel(val, port->port_base + AM65_CPSW_PN_REG_EST_CTL); 103 } 104 105 /* am65_cpsw_port_est_is_swapped() - Indicate if h/w is transitioned 106 * admin -> oper or not 107 * 108 * Return true if already transitioned. i.e oper is equal to admin and buf 109 * numbers match (est_oper->buf match with est_admin->buf). 110 * false if before transition. i.e oper is not equal to admin, (i.e a 111 * previous admin command is waiting to be transitioned to oper state 112 * and est_oper->buf not match with est_oper->buf). 113 */ 114 static int am65_cpsw_port_est_is_swapped(struct net_device *ndev, int *oper, 115 int *admin) 116 { 117 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 118 u32 val; 119 120 val = readl(port->port_base + AM65_CPSW_PN_REG_FIFO_STATUS); 121 *oper = !!(val & AM65_CPSW_PN_FST_EST_BUFACT); 122 123 val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL); 124 *admin = !!(val & AM65_CPSW_PN_EST_BUFSEL); 125 126 return *admin == *oper; 127 } 128 129 /* am65_cpsw_port_est_get_free_buf_num() - Get free buffer number for 130 * Admin to program the new schedule. 131 * 132 * Logic as follows:- 133 * If oper is same as admin, return the other buffer (!oper) as the admin 134 * buffer. If oper is not the same, driver let the current oper to continue 135 * as it is in the process of transitioning from admin -> oper. So keep the 136 * oper by selecting the same oper buffer by writing to EST_BUFSEL bit in 137 * EST CTL register. In the second iteration they will match and code returns. 138 * The actual buffer to write command is selected later before it is ready 139 * to update the schedule. 140 */ 141 static int am65_cpsw_port_est_get_free_buf_num(struct net_device *ndev) 142 { 143 int oper, admin; 144 int roll = 2; 145 146 while (roll--) { 147 if (am65_cpsw_port_est_is_swapped(ndev, &oper, &admin)) 148 return !oper; 149 150 /* admin is not set, so hinder transition as it's not allowed 151 * to touch memory in-flight, by targeting same oper buf. 152 */ 153 am65_cpsw_port_est_assign_buf_num(ndev, oper); 154 155 dev_info(&ndev->dev, 156 "Prev. EST admin cycle is in transit %d -> %d\n", 157 oper, admin); 158 } 159 160 return admin; 161 } 162 163 static void am65_cpsw_admin_to_oper(struct net_device *ndev) 164 { 165 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 166 167 if (port->qos.est_oper) 168 devm_kfree(&ndev->dev, port->qos.est_oper); 169 170 port->qos.est_oper = port->qos.est_admin; 171 port->qos.est_admin = NULL; 172 } 173 174 static void am65_cpsw_port_est_get_buf_num(struct net_device *ndev, 175 struct am65_cpsw_est *est_new) 176 { 177 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 178 u32 val; 179 180 val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL); 181 val &= ~AM65_CPSW_PN_EST_ONEBUF; 182 writel(val, port->port_base + AM65_CPSW_PN_REG_EST_CTL); 183 184 est_new->buf = am65_cpsw_port_est_get_free_buf_num(ndev); 185 186 /* rolled buf num means changed buf while configuring */ 187 if (port->qos.est_oper && port->qos.est_admin && 188 est_new->buf == port->qos.est_oper->buf) 189 am65_cpsw_admin_to_oper(ndev); 190 } 191 192 static void am65_cpsw_est_set(struct net_device *ndev, int enable) 193 { 194 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 195 struct am65_cpsw_common *common = port->common; 196 int common_enable = 0; 197 int i; 198 199 am65_cpsw_port_est_enable(port, enable); 200 201 for (i = 0; i < common->port_num; i++) 202 common_enable |= am65_cpsw_port_est_enabled(&common->ports[i]); 203 204 common_enable |= enable; 205 am65_cpsw_est_enable(common, common_enable); 206 } 207 208 /* This update is supposed to be used in any routine before getting real state 209 * of admin -> oper transition, particularly it's supposed to be used in some 210 * generic routine for providing real state to Taprio Qdisc. 211 */ 212 static void am65_cpsw_est_update_state(struct net_device *ndev) 213 { 214 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 215 int oper, admin; 216 217 if (!port->qos.est_admin) 218 return; 219 220 if (!am65_cpsw_port_est_is_swapped(ndev, &oper, &admin)) 221 return; 222 223 am65_cpsw_admin_to_oper(ndev); 224 } 225 226 /* Fetch command count it's number of bytes in Gigabit mode or nibbles in 227 * 10/100Mb mode. So, having speed and time in ns, recalculate ns to number of 228 * bytes/nibbles that can be sent while transmission on given speed. 229 */ 230 static int am65_est_cmd_ns_to_cnt(u64 ns, int link_speed) 231 { 232 u64 temp; 233 234 temp = ns * link_speed; 235 if (link_speed < SPEED_1000) 236 temp <<= 1; 237 238 return DIV_ROUND_UP(temp, 8 * 1000); 239 } 240 241 static void __iomem *am65_cpsw_est_set_sched_cmds(void __iomem *addr, 242 int fetch_cnt, 243 int fetch_allow) 244 { 245 u32 prio_mask, cmd_fetch_cnt, cmd; 246 247 do { 248 if (fetch_cnt > AM65_CPSW_FETCH_CNT_MAX) { 249 fetch_cnt -= AM65_CPSW_FETCH_CNT_MAX; 250 cmd_fetch_cnt = AM65_CPSW_FETCH_CNT_MAX; 251 } else { 252 cmd_fetch_cnt = fetch_cnt; 253 /* fetch count can't be less than 16? */ 254 if (cmd_fetch_cnt && cmd_fetch_cnt < 16) 255 cmd_fetch_cnt = 16; 256 257 fetch_cnt = 0; 258 } 259 260 prio_mask = fetch_allow & AM65_CPSW_FETCH_ALLOW_MSK; 261 cmd = (cmd_fetch_cnt << AM65_CPSW_FETCH_CNT_OFFSET) | prio_mask; 262 263 writel(cmd, addr); 264 addr += 4; 265 } while (fetch_cnt); 266 267 return addr; 268 } 269 270 static int am65_cpsw_est_calc_cmd_num(struct net_device *ndev, 271 struct tc_taprio_qopt_offload *taprio, 272 int link_speed) 273 { 274 int i, cmd_cnt, cmd_sum = 0; 275 u32 fetch_cnt; 276 277 for (i = 0; i < taprio->num_entries; i++) { 278 if (taprio->entries[i].command != TC_TAPRIO_CMD_SET_GATES) { 279 dev_err(&ndev->dev, "Only SET command is supported"); 280 return -EINVAL; 281 } 282 283 fetch_cnt = am65_est_cmd_ns_to_cnt(taprio->entries[i].interval, 284 link_speed); 285 286 cmd_cnt = DIV_ROUND_UP(fetch_cnt, AM65_CPSW_FETCH_CNT_MAX); 287 if (!cmd_cnt) 288 cmd_cnt++; 289 290 cmd_sum += cmd_cnt; 291 292 if (!fetch_cnt) 293 break; 294 } 295 296 return cmd_sum; 297 } 298 299 static int am65_cpsw_est_check_scheds(struct net_device *ndev, 300 struct am65_cpsw_est *est_new) 301 { 302 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 303 int cmd_num; 304 305 cmd_num = am65_cpsw_est_calc_cmd_num(ndev, &est_new->taprio, 306 port->qos.link_speed); 307 if (cmd_num < 0) 308 return cmd_num; 309 310 if (cmd_num > AM65_CPSW_FETCH_RAM_CMD_NUM / 2) { 311 dev_err(&ndev->dev, "No fetch RAM"); 312 return -ENOMEM; 313 } 314 315 return 0; 316 } 317 318 static void am65_cpsw_est_set_sched_list(struct net_device *ndev, 319 struct am65_cpsw_est *est_new) 320 { 321 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 322 u32 fetch_cnt, fetch_allow, all_fetch_allow = 0; 323 void __iomem *ram_addr, *max_ram_addr; 324 struct tc_taprio_sched_entry *entry; 325 int i, ram_size; 326 327 ram_addr = port->fetch_ram_base; 328 ram_size = AM65_CPSW_FETCH_RAM_CMD_NUM * 2; 329 ram_addr += est_new->buf * ram_size; 330 331 max_ram_addr = ram_size + ram_addr; 332 for (i = 0; i < est_new->taprio.num_entries; i++) { 333 entry = &est_new->taprio.entries[i]; 334 335 fetch_cnt = am65_est_cmd_ns_to_cnt(entry->interval, 336 port->qos.link_speed); 337 fetch_allow = entry->gate_mask; 338 if (fetch_allow > AM65_CPSW_FETCH_ALLOW_MAX) 339 dev_dbg(&ndev->dev, "fetch_allow > 8 bits: %d\n", 340 fetch_allow); 341 342 ram_addr = am65_cpsw_est_set_sched_cmds(ram_addr, fetch_cnt, 343 fetch_allow); 344 345 if (!fetch_cnt && i < est_new->taprio.num_entries - 1) { 346 dev_info(&ndev->dev, 347 "next scheds after %d have no impact", i + 1); 348 break; 349 } 350 351 all_fetch_allow |= fetch_allow; 352 } 353 354 /* end cmd, enabling non-timed queues for potential over cycle time */ 355 if (ram_addr < max_ram_addr) 356 writel(~all_fetch_allow & AM65_CPSW_FETCH_ALLOW_MSK, ram_addr); 357 } 358 359 /** 360 * Enable ESTf periodic output, set cycle start time and interval. 361 */ 362 static int am65_cpsw_timer_set(struct net_device *ndev, 363 struct am65_cpsw_est *est_new) 364 { 365 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 366 struct am65_cpsw_common *common = port->common; 367 struct am65_cpts *cpts = common->cpts; 368 struct am65_cpts_estf_cfg cfg; 369 370 cfg.ns_period = est_new->taprio.cycle_time; 371 cfg.ns_start = est_new->taprio.base_time; 372 373 return am65_cpts_estf_enable(cpts, port->port_id - 1, &cfg); 374 } 375 376 static void am65_cpsw_timer_stop(struct net_device *ndev) 377 { 378 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 379 struct am65_cpts *cpts = port->common->cpts; 380 381 am65_cpts_estf_disable(cpts, port->port_id - 1); 382 } 383 384 static enum timer_act am65_cpsw_timer_act(struct net_device *ndev, 385 struct am65_cpsw_est *est_new) 386 { 387 struct tc_taprio_qopt_offload *taprio_oper, *taprio_new; 388 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 389 struct am65_cpts *cpts = port->common->cpts; 390 u64 cur_time; 391 s64 diff; 392 393 if (!port->qos.est_oper) 394 return TACT_PROG; 395 396 taprio_new = &est_new->taprio; 397 taprio_oper = &port->qos.est_oper->taprio; 398 399 if (taprio_new->cycle_time != taprio_oper->cycle_time) 400 return TACT_NEED_STOP; 401 402 /* in order to avoid timer reset get base_time form oper taprio */ 403 if (!taprio_new->base_time && taprio_oper) 404 taprio_new->base_time = taprio_oper->base_time; 405 406 if (taprio_new->base_time == taprio_oper->base_time) 407 return TACT_SKIP_PROG; 408 409 /* base times are cycle synchronized */ 410 diff = taprio_new->base_time - taprio_oper->base_time; 411 diff = diff < 0 ? -diff : diff; 412 if (diff % taprio_new->cycle_time) 413 return TACT_NEED_STOP; 414 415 cur_time = am65_cpts_ns_gettime(cpts); 416 if (taprio_new->base_time <= cur_time + taprio_new->cycle_time) 417 return TACT_SKIP_PROG; 418 419 /* TODO: Admin schedule at future time is not currently supported */ 420 return TACT_NEED_STOP; 421 } 422 423 static void am65_cpsw_stop_est(struct net_device *ndev) 424 { 425 am65_cpsw_est_set(ndev, 0); 426 am65_cpsw_timer_stop(ndev); 427 } 428 429 static void am65_cpsw_purge_est(struct net_device *ndev) 430 { 431 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 432 433 am65_cpsw_stop_est(ndev); 434 435 if (port->qos.est_admin) 436 devm_kfree(&ndev->dev, port->qos.est_admin); 437 438 if (port->qos.est_oper) 439 devm_kfree(&ndev->dev, port->qos.est_oper); 440 441 port->qos.est_oper = NULL; 442 port->qos.est_admin = NULL; 443 } 444 445 static int am65_cpsw_configure_taprio(struct net_device *ndev, 446 struct am65_cpsw_est *est_new) 447 { 448 struct am65_cpsw_common *common = am65_ndev_to_common(ndev); 449 struct am65_cpts *cpts = common->cpts; 450 int ret = 0, tact = TACT_PROG; 451 452 am65_cpsw_est_update_state(ndev); 453 454 if (!est_new->taprio.enable) { 455 am65_cpsw_stop_est(ndev); 456 return ret; 457 } 458 459 ret = am65_cpsw_est_check_scheds(ndev, est_new); 460 if (ret < 0) 461 return ret; 462 463 tact = am65_cpsw_timer_act(ndev, est_new); 464 if (tact == TACT_NEED_STOP) { 465 dev_err(&ndev->dev, 466 "Can't toggle estf timer, stop taprio first"); 467 return -EINVAL; 468 } 469 470 if (tact == TACT_PROG) 471 am65_cpsw_timer_stop(ndev); 472 473 if (!est_new->taprio.base_time) 474 est_new->taprio.base_time = am65_cpts_ns_gettime(cpts); 475 476 am65_cpsw_port_est_get_buf_num(ndev, est_new); 477 am65_cpsw_est_set_sched_list(ndev, est_new); 478 am65_cpsw_port_est_assign_buf_num(ndev, est_new->buf); 479 480 am65_cpsw_est_set(ndev, est_new->taprio.enable); 481 482 if (tact == TACT_PROG) { 483 ret = am65_cpsw_timer_set(ndev, est_new); 484 if (ret) { 485 dev_err(&ndev->dev, "Failed to set cycle time"); 486 return ret; 487 } 488 } 489 490 return ret; 491 } 492 493 static void am65_cpsw_cp_taprio(struct tc_taprio_qopt_offload *from, 494 struct tc_taprio_qopt_offload *to) 495 { 496 int i; 497 498 *to = *from; 499 for (i = 0; i < from->num_entries; i++) 500 to->entries[i] = from->entries[i]; 501 } 502 503 static int am65_cpsw_set_taprio(struct net_device *ndev, void *type_data) 504 { 505 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 506 struct tc_taprio_qopt_offload *taprio = type_data; 507 struct am65_cpsw_est *est_new; 508 int ret = 0; 509 510 if (taprio->cycle_time_extension) { 511 dev_err(&ndev->dev, "Failed to set cycle time extension"); 512 return -EOPNOTSUPP; 513 } 514 515 est_new = devm_kzalloc(&ndev->dev, 516 struct_size(est_new, taprio.entries, taprio->num_entries), 517 GFP_KERNEL); 518 if (!est_new) 519 return -ENOMEM; 520 521 am65_cpsw_cp_taprio(taprio, &est_new->taprio); 522 ret = am65_cpsw_configure_taprio(ndev, est_new); 523 if (!ret) { 524 if (taprio->enable) { 525 if (port->qos.est_admin) 526 devm_kfree(&ndev->dev, port->qos.est_admin); 527 528 port->qos.est_admin = est_new; 529 } else { 530 devm_kfree(&ndev->dev, est_new); 531 am65_cpsw_purge_est(ndev); 532 } 533 } else { 534 devm_kfree(&ndev->dev, est_new); 535 } 536 537 return ret; 538 } 539 540 static void am65_cpsw_est_link_up(struct net_device *ndev, int link_speed) 541 { 542 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 543 ktime_t cur_time; 544 s64 delta; 545 546 port->qos.link_speed = link_speed; 547 if (!am65_cpsw_port_est_enabled(port)) 548 return; 549 550 if (port->qos.link_down_time) { 551 cur_time = ktime_get(); 552 delta = ktime_us_delta(cur_time, port->qos.link_down_time); 553 if (delta > USEC_PER_SEC) { 554 dev_err(&ndev->dev, 555 "Link has been lost too long, stopping TAS"); 556 goto purge_est; 557 } 558 } 559 560 return; 561 562 purge_est: 563 am65_cpsw_purge_est(ndev); 564 } 565 566 static int am65_cpsw_setup_taprio(struct net_device *ndev, void *type_data) 567 { 568 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 569 struct am65_cpsw_common *common = port->common; 570 571 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 572 return -ENODEV; 573 574 if (!netif_running(ndev)) { 575 dev_err(&ndev->dev, "interface is down, link speed unknown\n"); 576 return -ENETDOWN; 577 } 578 579 if (common->pf_p0_rx_ptype_rrobin) { 580 dev_err(&ndev->dev, 581 "p0-rx-ptype-rrobin flag conflicts with taprio qdisc\n"); 582 return -EINVAL; 583 } 584 585 if (port->qos.link_speed == SPEED_UNKNOWN) 586 return -ENOLINK; 587 588 return am65_cpsw_set_taprio(ndev, type_data); 589 } 590 591 int am65_cpsw_qos_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type, 592 void *type_data) 593 { 594 switch (type) { 595 case TC_SETUP_QDISC_TAPRIO: 596 return am65_cpsw_setup_taprio(ndev, type_data); 597 default: 598 return -EOPNOTSUPP; 599 } 600 } 601 602 void am65_cpsw_qos_link_up(struct net_device *ndev, int link_speed) 603 { 604 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 605 606 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 607 return; 608 609 am65_cpsw_est_link_up(ndev, link_speed); 610 port->qos.link_down_time = 0; 611 } 612 613 void am65_cpsw_qos_link_down(struct net_device *ndev) 614 { 615 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 616 617 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 618 return; 619 620 if (!port->qos.link_down_time) 621 port->qos.link_down_time = ktime_get(); 622 623 port->qos.link_speed = SPEED_UNKNOWN; 624 } 625