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 #include <net/pkt_cls.h> 12 13 #include "am65-cpsw-nuss.h" 14 #include "am65-cpsw-qos.h" 15 #include "am65-cpts.h" 16 #include "cpsw_ale.h" 17 18 #define AM65_CPSW_REG_CTL 0x004 19 #define AM65_CPSW_PN_REG_CTL 0x004 20 #define AM65_CPSW_PN_REG_FIFO_STATUS 0x050 21 #define AM65_CPSW_PN_REG_EST_CTL 0x060 22 #define AM65_CPSW_PN_REG_PRI_CIR(pri) (0x140 + 4 * (pri)) 23 24 /* AM65_CPSW_REG_CTL register fields */ 25 #define AM65_CPSW_CTL_EST_EN BIT(18) 26 27 /* AM65_CPSW_PN_REG_CTL register fields */ 28 #define AM65_CPSW_PN_CTL_EST_PORT_EN BIT(17) 29 30 /* AM65_CPSW_PN_REG_EST_CTL register fields */ 31 #define AM65_CPSW_PN_EST_ONEBUF BIT(0) 32 #define AM65_CPSW_PN_EST_BUFSEL BIT(1) 33 #define AM65_CPSW_PN_EST_TS_EN BIT(2) 34 #define AM65_CPSW_PN_EST_TS_FIRST BIT(3) 35 #define AM65_CPSW_PN_EST_ONEPRI BIT(4) 36 #define AM65_CPSW_PN_EST_TS_PRI_MSK GENMASK(7, 5) 37 38 /* AM65_CPSW_PN_REG_FIFO_STATUS register fields */ 39 #define AM65_CPSW_PN_FST_TX_PRI_ACTIVE_MSK GENMASK(7, 0) 40 #define AM65_CPSW_PN_FST_TX_E_MAC_ALLOW_MSK GENMASK(15, 8) 41 #define AM65_CPSW_PN_FST_EST_CNT_ERR BIT(16) 42 #define AM65_CPSW_PN_FST_EST_ADD_ERR BIT(17) 43 #define AM65_CPSW_PN_FST_EST_BUFACT BIT(18) 44 45 /* EST FETCH COMMAND RAM */ 46 #define AM65_CPSW_FETCH_RAM_CMD_NUM 0x80 47 #define AM65_CPSW_FETCH_CNT_MSK GENMASK(21, 8) 48 #define AM65_CPSW_FETCH_CNT_MAX (AM65_CPSW_FETCH_CNT_MSK >> 8) 49 #define AM65_CPSW_FETCH_CNT_OFFSET 8 50 #define AM65_CPSW_FETCH_ALLOW_MSK GENMASK(7, 0) 51 #define AM65_CPSW_FETCH_ALLOW_MAX AM65_CPSW_FETCH_ALLOW_MSK 52 53 enum timer_act { 54 TACT_PROG, /* need program timer */ 55 TACT_NEED_STOP, /* need stop first */ 56 TACT_SKIP_PROG, /* just buffer can be updated */ 57 }; 58 59 static int am65_cpsw_port_est_enabled(struct am65_cpsw_port *port) 60 { 61 return port->qos.est_oper || port->qos.est_admin; 62 } 63 64 static void am65_cpsw_est_enable(struct am65_cpsw_common *common, int enable) 65 { 66 u32 val; 67 68 val = readl(common->cpsw_base + AM65_CPSW_REG_CTL); 69 70 if (enable) 71 val |= AM65_CPSW_CTL_EST_EN; 72 else 73 val &= ~AM65_CPSW_CTL_EST_EN; 74 75 writel(val, common->cpsw_base + AM65_CPSW_REG_CTL); 76 common->est_enabled = enable; 77 } 78 79 static void am65_cpsw_port_est_enable(struct am65_cpsw_port *port, int enable) 80 { 81 u32 val; 82 83 val = readl(port->port_base + AM65_CPSW_PN_REG_CTL); 84 if (enable) 85 val |= AM65_CPSW_PN_CTL_EST_PORT_EN; 86 else 87 val &= ~AM65_CPSW_PN_CTL_EST_PORT_EN; 88 89 writel(val, port->port_base + AM65_CPSW_PN_REG_CTL); 90 } 91 92 /* target new EST RAM buffer, actual toggle happens after cycle completion */ 93 static void am65_cpsw_port_est_assign_buf_num(struct net_device *ndev, 94 int buf_num) 95 { 96 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 97 u32 val; 98 99 val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL); 100 if (buf_num) 101 val |= AM65_CPSW_PN_EST_BUFSEL; 102 else 103 val &= ~AM65_CPSW_PN_EST_BUFSEL; 104 105 writel(val, port->port_base + AM65_CPSW_PN_REG_EST_CTL); 106 } 107 108 /* am65_cpsw_port_est_is_swapped() - Indicate if h/w is transitioned 109 * admin -> oper or not 110 * 111 * Return true if already transitioned. i.e oper is equal to admin and buf 112 * numbers match (est_oper->buf match with est_admin->buf). 113 * false if before transition. i.e oper is not equal to admin, (i.e a 114 * previous admin command is waiting to be transitioned to oper state 115 * and est_oper->buf not match with est_oper->buf). 116 */ 117 static int am65_cpsw_port_est_is_swapped(struct net_device *ndev, int *oper, 118 int *admin) 119 { 120 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 121 u32 val; 122 123 val = readl(port->port_base + AM65_CPSW_PN_REG_FIFO_STATUS); 124 *oper = !!(val & AM65_CPSW_PN_FST_EST_BUFACT); 125 126 val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL); 127 *admin = !!(val & AM65_CPSW_PN_EST_BUFSEL); 128 129 return *admin == *oper; 130 } 131 132 /* am65_cpsw_port_est_get_free_buf_num() - Get free buffer number for 133 * Admin to program the new schedule. 134 * 135 * Logic as follows:- 136 * If oper is same as admin, return the other buffer (!oper) as the admin 137 * buffer. If oper is not the same, driver let the current oper to continue 138 * as it is in the process of transitioning from admin -> oper. So keep the 139 * oper by selecting the same oper buffer by writing to EST_BUFSEL bit in 140 * EST CTL register. In the second iteration they will match and code returns. 141 * The actual buffer to write command is selected later before it is ready 142 * to update the schedule. 143 */ 144 static int am65_cpsw_port_est_get_free_buf_num(struct net_device *ndev) 145 { 146 int oper, admin; 147 int roll = 2; 148 149 while (roll--) { 150 if (am65_cpsw_port_est_is_swapped(ndev, &oper, &admin)) 151 return !oper; 152 153 /* admin is not set, so hinder transition as it's not allowed 154 * to touch memory in-flight, by targeting same oper buf. 155 */ 156 am65_cpsw_port_est_assign_buf_num(ndev, oper); 157 158 dev_info(&ndev->dev, 159 "Prev. EST admin cycle is in transit %d -> %d\n", 160 oper, admin); 161 } 162 163 return admin; 164 } 165 166 static void am65_cpsw_admin_to_oper(struct net_device *ndev) 167 { 168 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 169 170 devm_kfree(&ndev->dev, port->qos.est_oper); 171 172 port->qos.est_oper = port->qos.est_admin; 173 port->qos.est_admin = NULL; 174 } 175 176 static void am65_cpsw_port_est_get_buf_num(struct net_device *ndev, 177 struct am65_cpsw_est *est_new) 178 { 179 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 180 u32 val; 181 182 val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL); 183 val &= ~AM65_CPSW_PN_EST_ONEBUF; 184 writel(val, port->port_base + AM65_CPSW_PN_REG_EST_CTL); 185 186 est_new->buf = am65_cpsw_port_est_get_free_buf_num(ndev); 187 188 /* rolled buf num means changed buf while configuring */ 189 if (port->qos.est_oper && port->qos.est_admin && 190 est_new->buf == port->qos.est_oper->buf) 191 am65_cpsw_admin_to_oper(ndev); 192 } 193 194 static void am65_cpsw_est_set(struct net_device *ndev, int enable) 195 { 196 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 197 struct am65_cpsw_common *common = port->common; 198 int common_enable = 0; 199 int i; 200 201 am65_cpsw_port_est_enable(port, enable); 202 203 for (i = 0; i < common->port_num; i++) 204 common_enable |= am65_cpsw_port_est_enabled(&common->ports[i]); 205 206 common_enable |= enable; 207 am65_cpsw_est_enable(common, common_enable); 208 } 209 210 /* This update is supposed to be used in any routine before getting real state 211 * of admin -> oper transition, particularly it's supposed to be used in some 212 * generic routine for providing real state to Taprio Qdisc. 213 */ 214 static void am65_cpsw_est_update_state(struct net_device *ndev) 215 { 216 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 217 int oper, admin; 218 219 if (!port->qos.est_admin) 220 return; 221 222 if (!am65_cpsw_port_est_is_swapped(ndev, &oper, &admin)) 223 return; 224 225 am65_cpsw_admin_to_oper(ndev); 226 } 227 228 /* Fetch command count it's number of bytes in Gigabit mode or nibbles in 229 * 10/100Mb mode. So, having speed and time in ns, recalculate ns to number of 230 * bytes/nibbles that can be sent while transmission on given speed. 231 */ 232 static int am65_est_cmd_ns_to_cnt(u64 ns, int link_speed) 233 { 234 u64 temp; 235 236 temp = ns * link_speed; 237 if (link_speed < SPEED_1000) 238 temp <<= 1; 239 240 return DIV_ROUND_UP(temp, 8 * 1000); 241 } 242 243 static void __iomem *am65_cpsw_est_set_sched_cmds(void __iomem *addr, 244 int fetch_cnt, 245 int fetch_allow) 246 { 247 u32 prio_mask, cmd_fetch_cnt, cmd; 248 249 do { 250 if (fetch_cnt > AM65_CPSW_FETCH_CNT_MAX) { 251 fetch_cnt -= AM65_CPSW_FETCH_CNT_MAX; 252 cmd_fetch_cnt = AM65_CPSW_FETCH_CNT_MAX; 253 } else { 254 cmd_fetch_cnt = fetch_cnt; 255 /* fetch count can't be less than 16? */ 256 if (cmd_fetch_cnt && cmd_fetch_cnt < 16) 257 cmd_fetch_cnt = 16; 258 259 fetch_cnt = 0; 260 } 261 262 prio_mask = fetch_allow & AM65_CPSW_FETCH_ALLOW_MSK; 263 cmd = (cmd_fetch_cnt << AM65_CPSW_FETCH_CNT_OFFSET) | prio_mask; 264 265 writel(cmd, addr); 266 addr += 4; 267 } while (fetch_cnt); 268 269 return addr; 270 } 271 272 static int am65_cpsw_est_calc_cmd_num(struct net_device *ndev, 273 struct tc_taprio_qopt_offload *taprio, 274 int link_speed) 275 { 276 int i, cmd_cnt, cmd_sum = 0; 277 u32 fetch_cnt; 278 279 for (i = 0; i < taprio->num_entries; i++) { 280 if (taprio->entries[i].command != TC_TAPRIO_CMD_SET_GATES) { 281 dev_err(&ndev->dev, "Only SET command is supported"); 282 return -EINVAL; 283 } 284 285 fetch_cnt = am65_est_cmd_ns_to_cnt(taprio->entries[i].interval, 286 link_speed); 287 288 cmd_cnt = DIV_ROUND_UP(fetch_cnt, AM65_CPSW_FETCH_CNT_MAX); 289 if (!cmd_cnt) 290 cmd_cnt++; 291 292 cmd_sum += cmd_cnt; 293 294 if (!fetch_cnt) 295 break; 296 } 297 298 return cmd_sum; 299 } 300 301 static int am65_cpsw_est_check_scheds(struct net_device *ndev, 302 struct am65_cpsw_est *est_new) 303 { 304 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 305 int cmd_num; 306 307 cmd_num = am65_cpsw_est_calc_cmd_num(ndev, &est_new->taprio, 308 port->qos.link_speed); 309 if (cmd_num < 0) 310 return cmd_num; 311 312 if (cmd_num > AM65_CPSW_FETCH_RAM_CMD_NUM / 2) { 313 dev_err(&ndev->dev, "No fetch RAM"); 314 return -ENOMEM; 315 } 316 317 return 0; 318 } 319 320 static void am65_cpsw_est_set_sched_list(struct net_device *ndev, 321 struct am65_cpsw_est *est_new) 322 { 323 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 324 u32 fetch_cnt, fetch_allow, all_fetch_allow = 0; 325 void __iomem *ram_addr, *max_ram_addr; 326 struct tc_taprio_sched_entry *entry; 327 int i, ram_size; 328 329 ram_addr = port->fetch_ram_base; 330 ram_size = AM65_CPSW_FETCH_RAM_CMD_NUM * 2; 331 ram_addr += est_new->buf * ram_size; 332 333 max_ram_addr = ram_size + ram_addr; 334 for (i = 0; i < est_new->taprio.num_entries; i++) { 335 entry = &est_new->taprio.entries[i]; 336 337 fetch_cnt = am65_est_cmd_ns_to_cnt(entry->interval, 338 port->qos.link_speed); 339 fetch_allow = entry->gate_mask; 340 if (fetch_allow > AM65_CPSW_FETCH_ALLOW_MAX) 341 dev_dbg(&ndev->dev, "fetch_allow > 8 bits: %d\n", 342 fetch_allow); 343 344 ram_addr = am65_cpsw_est_set_sched_cmds(ram_addr, fetch_cnt, 345 fetch_allow); 346 347 if (!fetch_cnt && i < est_new->taprio.num_entries - 1) { 348 dev_info(&ndev->dev, 349 "next scheds after %d have no impact", i + 1); 350 break; 351 } 352 353 all_fetch_allow |= fetch_allow; 354 } 355 356 /* end cmd, enabling non-timed queues for potential over cycle time */ 357 if (ram_addr < max_ram_addr) 358 writel(~all_fetch_allow & AM65_CPSW_FETCH_ALLOW_MSK, ram_addr); 359 } 360 361 /* 362 * Enable ESTf periodic output, set cycle start time and interval. 363 */ 364 static int am65_cpsw_timer_set(struct net_device *ndev, 365 struct am65_cpsw_est *est_new) 366 { 367 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 368 struct am65_cpsw_common *common = port->common; 369 struct am65_cpts *cpts = common->cpts; 370 struct am65_cpts_estf_cfg cfg; 371 372 cfg.ns_period = est_new->taprio.cycle_time; 373 cfg.ns_start = est_new->taprio.base_time; 374 375 return am65_cpts_estf_enable(cpts, port->port_id - 1, &cfg); 376 } 377 378 static void am65_cpsw_timer_stop(struct net_device *ndev) 379 { 380 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 381 struct am65_cpts *cpts = port->common->cpts; 382 383 am65_cpts_estf_disable(cpts, port->port_id - 1); 384 } 385 386 static enum timer_act am65_cpsw_timer_act(struct net_device *ndev, 387 struct am65_cpsw_est *est_new) 388 { 389 struct tc_taprio_qopt_offload *taprio_oper, *taprio_new; 390 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 391 struct am65_cpts *cpts = port->common->cpts; 392 u64 cur_time; 393 s64 diff; 394 395 if (!port->qos.est_oper) 396 return TACT_PROG; 397 398 taprio_new = &est_new->taprio; 399 taprio_oper = &port->qos.est_oper->taprio; 400 401 if (taprio_new->cycle_time != taprio_oper->cycle_time) 402 return TACT_NEED_STOP; 403 404 /* in order to avoid timer reset get base_time form oper taprio */ 405 if (!taprio_new->base_time && taprio_oper) 406 taprio_new->base_time = taprio_oper->base_time; 407 408 if (taprio_new->base_time == taprio_oper->base_time) 409 return TACT_SKIP_PROG; 410 411 /* base times are cycle synchronized */ 412 diff = taprio_new->base_time - taprio_oper->base_time; 413 diff = diff < 0 ? -diff : diff; 414 if (diff % taprio_new->cycle_time) 415 return TACT_NEED_STOP; 416 417 cur_time = am65_cpts_ns_gettime(cpts); 418 if (taprio_new->base_time <= cur_time + taprio_new->cycle_time) 419 return TACT_SKIP_PROG; 420 421 /* TODO: Admin schedule at future time is not currently supported */ 422 return TACT_NEED_STOP; 423 } 424 425 static void am65_cpsw_stop_est(struct net_device *ndev) 426 { 427 am65_cpsw_est_set(ndev, 0); 428 am65_cpsw_timer_stop(ndev); 429 } 430 431 static void am65_cpsw_purge_est(struct net_device *ndev) 432 { 433 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 434 435 am65_cpsw_stop_est(ndev); 436 437 devm_kfree(&ndev->dev, port->qos.est_admin); 438 devm_kfree(&ndev->dev, port->qos.est_oper); 439 440 port->qos.est_oper = NULL; 441 port->qos.est_admin = NULL; 442 } 443 444 static int am65_cpsw_configure_taprio(struct net_device *ndev, 445 struct am65_cpsw_est *est_new) 446 { 447 struct am65_cpsw_common *common = am65_ndev_to_common(ndev); 448 struct am65_cpts *cpts = common->cpts; 449 int ret = 0, tact = TACT_PROG; 450 451 am65_cpsw_est_update_state(ndev); 452 453 if (est_new->taprio.cmd == TAPRIO_CMD_DESTROY) { 454 am65_cpsw_stop_est(ndev); 455 return ret; 456 } 457 458 ret = am65_cpsw_est_check_scheds(ndev, est_new); 459 if (ret < 0) 460 return ret; 461 462 tact = am65_cpsw_timer_act(ndev, est_new); 463 if (tact == TACT_NEED_STOP) { 464 dev_err(&ndev->dev, 465 "Can't toggle estf timer, stop taprio first"); 466 return -EINVAL; 467 } 468 469 if (tact == TACT_PROG) 470 am65_cpsw_timer_stop(ndev); 471 472 if (!est_new->taprio.base_time) 473 est_new->taprio.base_time = am65_cpts_ns_gettime(cpts); 474 475 am65_cpsw_port_est_get_buf_num(ndev, est_new); 476 am65_cpsw_est_set_sched_list(ndev, est_new); 477 am65_cpsw_port_est_assign_buf_num(ndev, est_new->buf); 478 479 am65_cpsw_est_set(ndev, est_new->taprio.cmd == TAPRIO_CMD_REPLACE); 480 481 if (tact == TACT_PROG) { 482 ret = am65_cpsw_timer_set(ndev, est_new); 483 if (ret) { 484 dev_err(&ndev->dev, "Failed to set cycle time"); 485 return ret; 486 } 487 } 488 489 return ret; 490 } 491 492 static void am65_cpsw_cp_taprio(struct tc_taprio_qopt_offload *from, 493 struct tc_taprio_qopt_offload *to) 494 { 495 int i; 496 497 *to = *from; 498 for (i = 0; i < from->num_entries; i++) 499 to->entries[i] = from->entries[i]; 500 } 501 502 static int am65_cpsw_set_taprio(struct net_device *ndev, void *type_data) 503 { 504 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 505 struct tc_taprio_qopt_offload *taprio = type_data; 506 struct am65_cpsw_est *est_new; 507 int ret = 0; 508 509 if (taprio->cycle_time_extension) { 510 dev_err(&ndev->dev, "Failed to set cycle time extension"); 511 return -EOPNOTSUPP; 512 } 513 514 est_new = devm_kzalloc(&ndev->dev, 515 struct_size(est_new, taprio.entries, taprio->num_entries), 516 GFP_KERNEL); 517 if (!est_new) 518 return -ENOMEM; 519 520 am65_cpsw_cp_taprio(taprio, &est_new->taprio); 521 ret = am65_cpsw_configure_taprio(ndev, est_new); 522 if (!ret) { 523 if (taprio->cmd == TAPRIO_CMD_REPLACE) { 524 devm_kfree(&ndev->dev, port->qos.est_admin); 525 526 port->qos.est_admin = est_new; 527 } else { 528 devm_kfree(&ndev->dev, est_new); 529 am65_cpsw_purge_est(ndev); 530 } 531 } else { 532 devm_kfree(&ndev->dev, est_new); 533 } 534 535 return ret; 536 } 537 538 static void am65_cpsw_est_link_up(struct net_device *ndev, int link_speed) 539 { 540 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 541 ktime_t cur_time; 542 s64 delta; 543 544 port->qos.link_speed = link_speed; 545 if (!am65_cpsw_port_est_enabled(port)) 546 return; 547 548 if (port->qos.link_down_time) { 549 cur_time = ktime_get(); 550 delta = ktime_us_delta(cur_time, port->qos.link_down_time); 551 if (delta > USEC_PER_SEC) { 552 dev_err(&ndev->dev, 553 "Link has been lost too long, stopping TAS"); 554 goto purge_est; 555 } 556 } 557 558 return; 559 560 purge_est: 561 am65_cpsw_purge_est(ndev); 562 } 563 564 static int am65_cpsw_setup_taprio(struct net_device *ndev, void *type_data) 565 { 566 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 567 struct tc_taprio_qopt_offload *taprio = type_data; 568 struct am65_cpsw_common *common = port->common; 569 570 if (taprio->cmd != TAPRIO_CMD_REPLACE && 571 taprio->cmd != TAPRIO_CMD_DESTROY) 572 return -EOPNOTSUPP; 573 574 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 575 return -ENODEV; 576 577 if (!netif_running(ndev)) { 578 dev_err(&ndev->dev, "interface is down, link speed unknown\n"); 579 return -ENETDOWN; 580 } 581 582 if (common->pf_p0_rx_ptype_rrobin) { 583 dev_err(&ndev->dev, 584 "p0-rx-ptype-rrobin flag conflicts with taprio qdisc\n"); 585 return -EINVAL; 586 } 587 588 if (port->qos.link_speed == SPEED_UNKNOWN) 589 return -ENOLINK; 590 591 return am65_cpsw_set_taprio(ndev, type_data); 592 } 593 594 static int am65_cpsw_tc_query_caps(struct net_device *ndev, void *type_data) 595 { 596 struct tc_query_caps_base *base = type_data; 597 598 switch (base->type) { 599 case TC_SETUP_QDISC_TAPRIO: { 600 struct tc_taprio_caps *caps = base->caps; 601 602 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 603 return -EOPNOTSUPP; 604 605 caps->gate_mask_per_txq = true; 606 607 return 0; 608 } 609 default: 610 return -EOPNOTSUPP; 611 } 612 } 613 614 static int am65_cpsw_qos_clsflower_add_policer(struct am65_cpsw_port *port, 615 struct netlink_ext_ack *extack, 616 struct flow_cls_offload *cls, 617 u64 rate_pkt_ps) 618 { 619 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 620 struct flow_dissector *dissector = rule->match.dissector; 621 static const u8 mc_mac[] = {0x01, 0x00, 0x00, 0x00, 0x00, 0x00}; 622 struct am65_cpsw_qos *qos = &port->qos; 623 struct flow_match_eth_addrs match; 624 int ret; 625 626 if (dissector->used_keys & 627 ~(BIT_ULL(FLOW_DISSECTOR_KEY_BASIC) | 628 BIT_ULL(FLOW_DISSECTOR_KEY_CONTROL) | 629 BIT_ULL(FLOW_DISSECTOR_KEY_ETH_ADDRS))) { 630 NL_SET_ERR_MSG_MOD(extack, 631 "Unsupported keys used"); 632 return -EOPNOTSUPP; 633 } 634 635 if (!flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) { 636 NL_SET_ERR_MSG_MOD(extack, "Not matching on eth address"); 637 return -EOPNOTSUPP; 638 } 639 640 flow_rule_match_eth_addrs(rule, &match); 641 642 if (!is_zero_ether_addr(match.mask->src)) { 643 NL_SET_ERR_MSG_MOD(extack, 644 "Matching on source MAC not supported"); 645 return -EOPNOTSUPP; 646 } 647 648 if (is_broadcast_ether_addr(match.key->dst) && 649 is_broadcast_ether_addr(match.mask->dst)) { 650 ret = cpsw_ale_rx_ratelimit_bc(port->common->ale, port->port_id, rate_pkt_ps); 651 if (ret) 652 return ret; 653 654 qos->ale_bc_ratelimit.cookie = cls->cookie; 655 qos->ale_bc_ratelimit.rate_packet_ps = rate_pkt_ps; 656 } else if (ether_addr_equal_unaligned(match.key->dst, mc_mac) && 657 ether_addr_equal_unaligned(match.mask->dst, mc_mac)) { 658 ret = cpsw_ale_rx_ratelimit_mc(port->common->ale, port->port_id, rate_pkt_ps); 659 if (ret) 660 return ret; 661 662 qos->ale_mc_ratelimit.cookie = cls->cookie; 663 qos->ale_mc_ratelimit.rate_packet_ps = rate_pkt_ps; 664 } else { 665 NL_SET_ERR_MSG_MOD(extack, "Not supported matching key"); 666 return -EOPNOTSUPP; 667 } 668 669 return 0; 670 } 671 672 static int am65_cpsw_qos_clsflower_policer_validate(const struct flow_action *action, 673 const struct flow_action_entry *act, 674 struct netlink_ext_ack *extack) 675 { 676 if (act->police.exceed.act_id != FLOW_ACTION_DROP) { 677 NL_SET_ERR_MSG_MOD(extack, 678 "Offload not supported when exceed action is not drop"); 679 return -EOPNOTSUPP; 680 } 681 682 if (act->police.notexceed.act_id != FLOW_ACTION_PIPE && 683 act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) { 684 NL_SET_ERR_MSG_MOD(extack, 685 "Offload not supported when conform action is not pipe or ok"); 686 return -EOPNOTSUPP; 687 } 688 689 if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT && 690 !flow_action_is_last_entry(action, act)) { 691 NL_SET_ERR_MSG_MOD(extack, 692 "Offload not supported when conform action is ok, but action is not last"); 693 return -EOPNOTSUPP; 694 } 695 696 if (act->police.rate_bytes_ps || act->police.peakrate_bytes_ps || 697 act->police.avrate || act->police.overhead) { 698 NL_SET_ERR_MSG_MOD(extack, 699 "Offload not supported when bytes per second/peakrate/avrate/overhead is configured"); 700 return -EOPNOTSUPP; 701 } 702 703 return 0; 704 } 705 706 static int am65_cpsw_qos_configure_clsflower(struct am65_cpsw_port *port, 707 struct flow_cls_offload *cls) 708 { 709 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 710 struct netlink_ext_ack *extack = cls->common.extack; 711 const struct flow_action_entry *act; 712 int i, ret; 713 714 flow_action_for_each(i, act, &rule->action) { 715 switch (act->id) { 716 case FLOW_ACTION_POLICE: 717 ret = am65_cpsw_qos_clsflower_policer_validate(&rule->action, act, extack); 718 if (ret) 719 return ret; 720 721 return am65_cpsw_qos_clsflower_add_policer(port, extack, cls, 722 act->police.rate_pkt_ps); 723 default: 724 NL_SET_ERR_MSG_MOD(extack, 725 "Action not supported"); 726 return -EOPNOTSUPP; 727 } 728 } 729 return -EOPNOTSUPP; 730 } 731 732 static int am65_cpsw_qos_delete_clsflower(struct am65_cpsw_port *port, struct flow_cls_offload *cls) 733 { 734 struct am65_cpsw_qos *qos = &port->qos; 735 736 if (cls->cookie == qos->ale_bc_ratelimit.cookie) { 737 qos->ale_bc_ratelimit.cookie = 0; 738 qos->ale_bc_ratelimit.rate_packet_ps = 0; 739 cpsw_ale_rx_ratelimit_bc(port->common->ale, port->port_id, 0); 740 } 741 742 if (cls->cookie == qos->ale_mc_ratelimit.cookie) { 743 qos->ale_mc_ratelimit.cookie = 0; 744 qos->ale_mc_ratelimit.rate_packet_ps = 0; 745 cpsw_ale_rx_ratelimit_mc(port->common->ale, port->port_id, 0); 746 } 747 748 return 0; 749 } 750 751 static int am65_cpsw_qos_setup_tc_clsflower(struct am65_cpsw_port *port, 752 struct flow_cls_offload *cls_flower) 753 { 754 switch (cls_flower->command) { 755 case FLOW_CLS_REPLACE: 756 return am65_cpsw_qos_configure_clsflower(port, cls_flower); 757 case FLOW_CLS_DESTROY: 758 return am65_cpsw_qos_delete_clsflower(port, cls_flower); 759 default: 760 return -EOPNOTSUPP; 761 } 762 } 763 764 static int am65_cpsw_qos_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv) 765 { 766 struct am65_cpsw_port *port = cb_priv; 767 768 if (!tc_cls_can_offload_and_chain0(port->ndev, type_data)) 769 return -EOPNOTSUPP; 770 771 switch (type) { 772 case TC_SETUP_CLSFLOWER: 773 return am65_cpsw_qos_setup_tc_clsflower(port, type_data); 774 default: 775 return -EOPNOTSUPP; 776 } 777 } 778 779 static LIST_HEAD(am65_cpsw_qos_block_cb_list); 780 781 static int am65_cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f) 782 { 783 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 784 785 return flow_block_cb_setup_simple(f, &am65_cpsw_qos_block_cb_list, 786 am65_cpsw_qos_setup_tc_block_cb, 787 port, port, true); 788 } 789 790 int am65_cpsw_qos_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type, 791 void *type_data) 792 { 793 switch (type) { 794 case TC_QUERY_CAPS: 795 return am65_cpsw_tc_query_caps(ndev, type_data); 796 case TC_SETUP_QDISC_TAPRIO: 797 return am65_cpsw_setup_taprio(ndev, type_data); 798 case TC_SETUP_BLOCK: 799 return am65_cpsw_qos_setup_tc_block(ndev, type_data); 800 default: 801 return -EOPNOTSUPP; 802 } 803 } 804 805 void am65_cpsw_qos_link_up(struct net_device *ndev, int link_speed) 806 { 807 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 808 809 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 810 return; 811 812 am65_cpsw_est_link_up(ndev, link_speed); 813 port->qos.link_down_time = 0; 814 } 815 816 void am65_cpsw_qos_link_down(struct net_device *ndev) 817 { 818 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 819 820 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 821 return; 822 823 if (!port->qos.link_down_time) 824 port->qos.link_down_time = ktime_get(); 825 826 port->qos.link_speed = SPEED_UNKNOWN; 827 } 828 829 static u32 830 am65_cpsw_qos_tx_rate_calc(u32 rate_mbps, unsigned long bus_freq) 831 { 832 u32 ir; 833 834 bus_freq /= 1000000; 835 ir = DIV_ROUND_UP(((u64)rate_mbps * 32768), bus_freq); 836 return ir; 837 } 838 839 static void 840 am65_cpsw_qos_tx_p0_rate_apply(struct am65_cpsw_common *common, 841 int tx_ch, u32 rate_mbps) 842 { 843 struct am65_cpsw_host *host = am65_common_get_host(common); 844 u32 ch_cir; 845 int i; 846 847 ch_cir = am65_cpsw_qos_tx_rate_calc(rate_mbps, common->bus_freq); 848 writel(ch_cir, host->port_base + AM65_CPSW_PN_REG_PRI_CIR(tx_ch)); 849 850 /* update rates for every port tx queues */ 851 for (i = 0; i < common->port_num; i++) { 852 struct net_device *ndev = common->ports[i].ndev; 853 854 if (!ndev) 855 continue; 856 netdev_get_tx_queue(ndev, tx_ch)->tx_maxrate = rate_mbps; 857 } 858 } 859 860 int am65_cpsw_qos_ndo_tx_p0_set_maxrate(struct net_device *ndev, 861 int queue, u32 rate_mbps) 862 { 863 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 864 struct am65_cpsw_common *common = port->common; 865 struct am65_cpsw_tx_chn *tx_chn; 866 u32 ch_rate, tx_ch_rate_msk_new; 867 u32 ch_msk = 0; 868 int ret; 869 870 dev_dbg(common->dev, "apply TX%d rate limiting %uMbps tx_rate_msk%x\n", 871 queue, rate_mbps, common->tx_ch_rate_msk); 872 873 if (common->pf_p0_rx_ptype_rrobin) { 874 dev_err(common->dev, "TX Rate Limiting failed - rrobin mode\n"); 875 return -EINVAL; 876 } 877 878 ch_rate = netdev_get_tx_queue(ndev, queue)->tx_maxrate; 879 if (ch_rate == rate_mbps) 880 return 0; 881 882 ret = pm_runtime_get_sync(common->dev); 883 if (ret < 0) { 884 pm_runtime_put_noidle(common->dev); 885 return ret; 886 } 887 ret = 0; 888 889 tx_ch_rate_msk_new = common->tx_ch_rate_msk; 890 if (rate_mbps && !(tx_ch_rate_msk_new & BIT(queue))) { 891 tx_ch_rate_msk_new |= BIT(queue); 892 ch_msk = GENMASK(common->tx_ch_num - 1, queue); 893 ch_msk = tx_ch_rate_msk_new ^ ch_msk; 894 } else if (!rate_mbps) { 895 tx_ch_rate_msk_new &= ~BIT(queue); 896 ch_msk = queue ? GENMASK(queue - 1, 0) : 0; 897 ch_msk = tx_ch_rate_msk_new & ch_msk; 898 } 899 900 if (ch_msk) { 901 dev_err(common->dev, "TX rate limiting has to be enabled sequentially hi->lo tx_rate_msk:%x tx_rate_msk_new:%x\n", 902 common->tx_ch_rate_msk, tx_ch_rate_msk_new); 903 ret = -EINVAL; 904 goto exit_put; 905 } 906 907 tx_chn = &common->tx_chns[queue]; 908 tx_chn->rate_mbps = rate_mbps; 909 common->tx_ch_rate_msk = tx_ch_rate_msk_new; 910 911 if (!common->usage_count) 912 /* will be applied on next netif up */ 913 goto exit_put; 914 915 am65_cpsw_qos_tx_p0_rate_apply(common, queue, rate_mbps); 916 917 exit_put: 918 pm_runtime_put(common->dev); 919 return ret; 920 } 921 922 void am65_cpsw_qos_tx_p0_rate_init(struct am65_cpsw_common *common) 923 { 924 struct am65_cpsw_host *host = am65_common_get_host(common); 925 int tx_ch; 926 927 for (tx_ch = 0; tx_ch < common->tx_ch_num; tx_ch++) { 928 struct am65_cpsw_tx_chn *tx_chn = &common->tx_chns[tx_ch]; 929 u32 ch_cir; 930 931 if (!tx_chn->rate_mbps) 932 continue; 933 934 ch_cir = am65_cpsw_qos_tx_rate_calc(tx_chn->rate_mbps, 935 common->bus_freq); 936 writel(ch_cir, 937 host->port_base + AM65_CPSW_PN_REG_PRI_CIR(tx_ch)); 938 } 939 } 940