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 23 /* AM65_CPSW_REG_CTL register fields */ 24 #define AM65_CPSW_CTL_EST_EN BIT(18) 25 26 /* AM65_CPSW_PN_REG_CTL register fields */ 27 #define AM65_CPSW_PN_CTL_EST_PORT_EN BIT(17) 28 29 /* AM65_CPSW_PN_REG_EST_CTL register fields */ 30 #define AM65_CPSW_PN_EST_ONEBUF BIT(0) 31 #define AM65_CPSW_PN_EST_BUFSEL BIT(1) 32 #define AM65_CPSW_PN_EST_TS_EN BIT(2) 33 #define AM65_CPSW_PN_EST_TS_FIRST BIT(3) 34 #define AM65_CPSW_PN_EST_ONEPRI BIT(4) 35 #define AM65_CPSW_PN_EST_TS_PRI_MSK GENMASK(7, 5) 36 37 /* AM65_CPSW_PN_REG_FIFO_STATUS register fields */ 38 #define AM65_CPSW_PN_FST_TX_PRI_ACTIVE_MSK GENMASK(7, 0) 39 #define AM65_CPSW_PN_FST_TX_E_MAC_ALLOW_MSK GENMASK(15, 8) 40 #define AM65_CPSW_PN_FST_EST_CNT_ERR BIT(16) 41 #define AM65_CPSW_PN_FST_EST_ADD_ERR BIT(17) 42 #define AM65_CPSW_PN_FST_EST_BUFACT BIT(18) 43 44 /* EST FETCH COMMAND RAM */ 45 #define AM65_CPSW_FETCH_RAM_CMD_NUM 0x80 46 #define AM65_CPSW_FETCH_CNT_MSK GENMASK(21, 8) 47 #define AM65_CPSW_FETCH_CNT_MAX (AM65_CPSW_FETCH_CNT_MSK >> 8) 48 #define AM65_CPSW_FETCH_CNT_OFFSET 8 49 #define AM65_CPSW_FETCH_ALLOW_MSK GENMASK(7, 0) 50 #define AM65_CPSW_FETCH_ALLOW_MAX AM65_CPSW_FETCH_ALLOW_MSK 51 52 enum timer_act { 53 TACT_PROG, /* need program timer */ 54 TACT_NEED_STOP, /* need stop first */ 55 TACT_SKIP_PROG, /* just buffer can be updated */ 56 }; 57 58 static int am65_cpsw_port_est_enabled(struct am65_cpsw_port *port) 59 { 60 return port->qos.est_oper || port->qos.est_admin; 61 } 62 63 static void am65_cpsw_est_enable(struct am65_cpsw_common *common, int enable) 64 { 65 u32 val; 66 67 val = readl(common->cpsw_base + AM65_CPSW_REG_CTL); 68 69 if (enable) 70 val |= AM65_CPSW_CTL_EST_EN; 71 else 72 val &= ~AM65_CPSW_CTL_EST_EN; 73 74 writel(val, common->cpsw_base + AM65_CPSW_REG_CTL); 75 common->est_enabled = enable; 76 } 77 78 static void am65_cpsw_port_est_enable(struct am65_cpsw_port *port, int enable) 79 { 80 u32 val; 81 82 val = readl(port->port_base + AM65_CPSW_PN_REG_CTL); 83 if (enable) 84 val |= AM65_CPSW_PN_CTL_EST_PORT_EN; 85 else 86 val &= ~AM65_CPSW_PN_CTL_EST_PORT_EN; 87 88 writel(val, port->port_base + AM65_CPSW_PN_REG_CTL); 89 } 90 91 /* target new EST RAM buffer, actual toggle happens after cycle completion */ 92 static void am65_cpsw_port_est_assign_buf_num(struct net_device *ndev, 93 int buf_num) 94 { 95 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 96 u32 val; 97 98 val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL); 99 if (buf_num) 100 val |= AM65_CPSW_PN_EST_BUFSEL; 101 else 102 val &= ~AM65_CPSW_PN_EST_BUFSEL; 103 104 writel(val, port->port_base + AM65_CPSW_PN_REG_EST_CTL); 105 } 106 107 /* am65_cpsw_port_est_is_swapped() - Indicate if h/w is transitioned 108 * admin -> oper or not 109 * 110 * Return true if already transitioned. i.e oper is equal to admin and buf 111 * numbers match (est_oper->buf match with est_admin->buf). 112 * false if before transition. i.e oper is not equal to admin, (i.e a 113 * previous admin command is waiting to be transitioned to oper state 114 * and est_oper->buf not match with est_oper->buf). 115 */ 116 static int am65_cpsw_port_est_is_swapped(struct net_device *ndev, int *oper, 117 int *admin) 118 { 119 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 120 u32 val; 121 122 val = readl(port->port_base + AM65_CPSW_PN_REG_FIFO_STATUS); 123 *oper = !!(val & AM65_CPSW_PN_FST_EST_BUFACT); 124 125 val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL); 126 *admin = !!(val & AM65_CPSW_PN_EST_BUFSEL); 127 128 return *admin == *oper; 129 } 130 131 /* am65_cpsw_port_est_get_free_buf_num() - Get free buffer number for 132 * Admin to program the new schedule. 133 * 134 * Logic as follows:- 135 * If oper is same as admin, return the other buffer (!oper) as the admin 136 * buffer. If oper is not the same, driver let the current oper to continue 137 * as it is in the process of transitioning from admin -> oper. So keep the 138 * oper by selecting the same oper buffer by writing to EST_BUFSEL bit in 139 * EST CTL register. In the second iteration they will match and code returns. 140 * The actual buffer to write command is selected later before it is ready 141 * to update the schedule. 142 */ 143 static int am65_cpsw_port_est_get_free_buf_num(struct net_device *ndev) 144 { 145 int oper, admin; 146 int roll = 2; 147 148 while (roll--) { 149 if (am65_cpsw_port_est_is_swapped(ndev, &oper, &admin)) 150 return !oper; 151 152 /* admin is not set, so hinder transition as it's not allowed 153 * to touch memory in-flight, by targeting same oper buf. 154 */ 155 am65_cpsw_port_est_assign_buf_num(ndev, oper); 156 157 dev_info(&ndev->dev, 158 "Prev. EST admin cycle is in transit %d -> %d\n", 159 oper, admin); 160 } 161 162 return admin; 163 } 164 165 static void am65_cpsw_admin_to_oper(struct net_device *ndev) 166 { 167 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 168 169 if (port->qos.est_oper) 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 if (port->qos.est_admin) 438 devm_kfree(&ndev->dev, port->qos.est_admin); 439 440 if (port->qos.est_oper) 441 devm_kfree(&ndev->dev, port->qos.est_oper); 442 443 port->qos.est_oper = NULL; 444 port->qos.est_admin = NULL; 445 } 446 447 static int am65_cpsw_configure_taprio(struct net_device *ndev, 448 struct am65_cpsw_est *est_new) 449 { 450 struct am65_cpsw_common *common = am65_ndev_to_common(ndev); 451 struct am65_cpts *cpts = common->cpts; 452 int ret = 0, tact = TACT_PROG; 453 454 am65_cpsw_est_update_state(ndev); 455 456 if (!est_new->taprio.enable) { 457 am65_cpsw_stop_est(ndev); 458 return ret; 459 } 460 461 ret = am65_cpsw_est_check_scheds(ndev, est_new); 462 if (ret < 0) 463 return ret; 464 465 tact = am65_cpsw_timer_act(ndev, est_new); 466 if (tact == TACT_NEED_STOP) { 467 dev_err(&ndev->dev, 468 "Can't toggle estf timer, stop taprio first"); 469 return -EINVAL; 470 } 471 472 if (tact == TACT_PROG) 473 am65_cpsw_timer_stop(ndev); 474 475 if (!est_new->taprio.base_time) 476 est_new->taprio.base_time = am65_cpts_ns_gettime(cpts); 477 478 am65_cpsw_port_est_get_buf_num(ndev, est_new); 479 am65_cpsw_est_set_sched_list(ndev, est_new); 480 am65_cpsw_port_est_assign_buf_num(ndev, est_new->buf); 481 482 am65_cpsw_est_set(ndev, est_new->taprio.enable); 483 484 if (tact == TACT_PROG) { 485 ret = am65_cpsw_timer_set(ndev, est_new); 486 if (ret) { 487 dev_err(&ndev->dev, "Failed to set cycle time"); 488 return ret; 489 } 490 } 491 492 return ret; 493 } 494 495 static void am65_cpsw_cp_taprio(struct tc_taprio_qopt_offload *from, 496 struct tc_taprio_qopt_offload *to) 497 { 498 int i; 499 500 *to = *from; 501 for (i = 0; i < from->num_entries; i++) 502 to->entries[i] = from->entries[i]; 503 } 504 505 static int am65_cpsw_set_taprio(struct net_device *ndev, void *type_data) 506 { 507 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 508 struct tc_taprio_qopt_offload *taprio = type_data; 509 struct am65_cpsw_est *est_new; 510 int ret = 0; 511 512 if (taprio->cycle_time_extension) { 513 dev_err(&ndev->dev, "Failed to set cycle time extension"); 514 return -EOPNOTSUPP; 515 } 516 517 est_new = devm_kzalloc(&ndev->dev, 518 struct_size(est_new, taprio.entries, taprio->num_entries), 519 GFP_KERNEL); 520 if (!est_new) 521 return -ENOMEM; 522 523 am65_cpsw_cp_taprio(taprio, &est_new->taprio); 524 ret = am65_cpsw_configure_taprio(ndev, est_new); 525 if (!ret) { 526 if (taprio->enable) { 527 if (port->qos.est_admin) 528 devm_kfree(&ndev->dev, port->qos.est_admin); 529 530 port->qos.est_admin = est_new; 531 } else { 532 devm_kfree(&ndev->dev, est_new); 533 am65_cpsw_purge_est(ndev); 534 } 535 } else { 536 devm_kfree(&ndev->dev, est_new); 537 } 538 539 return ret; 540 } 541 542 static void am65_cpsw_est_link_up(struct net_device *ndev, int link_speed) 543 { 544 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 545 ktime_t cur_time; 546 s64 delta; 547 548 port->qos.link_speed = link_speed; 549 if (!am65_cpsw_port_est_enabled(port)) 550 return; 551 552 if (port->qos.link_down_time) { 553 cur_time = ktime_get(); 554 delta = ktime_us_delta(cur_time, port->qos.link_down_time); 555 if (delta > USEC_PER_SEC) { 556 dev_err(&ndev->dev, 557 "Link has been lost too long, stopping TAS"); 558 goto purge_est; 559 } 560 } 561 562 return; 563 564 purge_est: 565 am65_cpsw_purge_est(ndev); 566 } 567 568 static int am65_cpsw_setup_taprio(struct net_device *ndev, void *type_data) 569 { 570 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 571 struct am65_cpsw_common *common = port->common; 572 573 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 574 return -ENODEV; 575 576 if (!netif_running(ndev)) { 577 dev_err(&ndev->dev, "interface is down, link speed unknown\n"); 578 return -ENETDOWN; 579 } 580 581 if (common->pf_p0_rx_ptype_rrobin) { 582 dev_err(&ndev->dev, 583 "p0-rx-ptype-rrobin flag conflicts with taprio qdisc\n"); 584 return -EINVAL; 585 } 586 587 if (port->qos.link_speed == SPEED_UNKNOWN) 588 return -ENOLINK; 589 590 return am65_cpsw_set_taprio(ndev, type_data); 591 } 592 593 static int am65_cpsw_qos_clsflower_add_policer(struct am65_cpsw_port *port, 594 struct netlink_ext_ack *extack, 595 struct flow_cls_offload *cls, 596 u64 rate_pkt_ps) 597 { 598 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 599 struct flow_dissector *dissector = rule->match.dissector; 600 static const u8 mc_mac[] = {0x01, 0x00, 0x00, 0x00, 0x00, 0x00}; 601 struct am65_cpsw_qos *qos = &port->qos; 602 struct flow_match_eth_addrs match; 603 int ret; 604 605 if (dissector->used_keys & 606 ~(BIT(FLOW_DISSECTOR_KEY_BASIC) | 607 BIT(FLOW_DISSECTOR_KEY_CONTROL) | 608 BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS))) { 609 NL_SET_ERR_MSG_MOD(extack, 610 "Unsupported keys used"); 611 return -EOPNOTSUPP; 612 } 613 614 if (!flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) { 615 NL_SET_ERR_MSG_MOD(extack, "Not matching on eth address"); 616 return -EOPNOTSUPP; 617 } 618 619 flow_rule_match_eth_addrs(rule, &match); 620 621 if (!is_zero_ether_addr(match.mask->src)) { 622 NL_SET_ERR_MSG_MOD(extack, 623 "Matching on source MAC not supported"); 624 return -EOPNOTSUPP; 625 } 626 627 if (is_broadcast_ether_addr(match.key->dst) && 628 is_broadcast_ether_addr(match.mask->dst)) { 629 ret = cpsw_ale_rx_ratelimit_bc(port->common->ale, port->port_id, rate_pkt_ps); 630 if (ret) 631 return ret; 632 633 qos->ale_bc_ratelimit.cookie = cls->cookie; 634 qos->ale_bc_ratelimit.rate_packet_ps = rate_pkt_ps; 635 } else if (ether_addr_equal_unaligned(match.key->dst, mc_mac) && 636 ether_addr_equal_unaligned(match.mask->dst, mc_mac)) { 637 ret = cpsw_ale_rx_ratelimit_mc(port->common->ale, port->port_id, rate_pkt_ps); 638 if (ret) 639 return ret; 640 641 qos->ale_mc_ratelimit.cookie = cls->cookie; 642 qos->ale_mc_ratelimit.rate_packet_ps = rate_pkt_ps; 643 } else { 644 NL_SET_ERR_MSG_MOD(extack, "Not supported matching key"); 645 return -EOPNOTSUPP; 646 } 647 648 return 0; 649 } 650 651 static int am65_cpsw_qos_clsflower_policer_validate(const struct flow_action *action, 652 const struct flow_action_entry *act, 653 struct netlink_ext_ack *extack) 654 { 655 if (act->police.exceed.act_id != FLOW_ACTION_DROP) { 656 NL_SET_ERR_MSG_MOD(extack, 657 "Offload not supported when exceed action is not drop"); 658 return -EOPNOTSUPP; 659 } 660 661 if (act->police.notexceed.act_id != FLOW_ACTION_PIPE && 662 act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) { 663 NL_SET_ERR_MSG_MOD(extack, 664 "Offload not supported when conform action is not pipe or ok"); 665 return -EOPNOTSUPP; 666 } 667 668 if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT && 669 !flow_action_is_last_entry(action, act)) { 670 NL_SET_ERR_MSG_MOD(extack, 671 "Offload not supported when conform action is ok, but action is not last"); 672 return -EOPNOTSUPP; 673 } 674 675 if (act->police.rate_bytes_ps || act->police.peakrate_bytes_ps || 676 act->police.avrate || act->police.overhead) { 677 NL_SET_ERR_MSG_MOD(extack, 678 "Offload not supported when bytes per second/peakrate/avrate/overhead is configured"); 679 return -EOPNOTSUPP; 680 } 681 682 return 0; 683 } 684 685 static int am65_cpsw_qos_configure_clsflower(struct am65_cpsw_port *port, 686 struct flow_cls_offload *cls) 687 { 688 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 689 struct netlink_ext_ack *extack = cls->common.extack; 690 const struct flow_action_entry *act; 691 int i, ret; 692 693 flow_action_for_each(i, act, &rule->action) { 694 switch (act->id) { 695 case FLOW_ACTION_POLICE: 696 ret = am65_cpsw_qos_clsflower_policer_validate(&rule->action, act, extack); 697 if (ret) 698 return ret; 699 700 return am65_cpsw_qos_clsflower_add_policer(port, extack, cls, 701 act->police.rate_pkt_ps); 702 default: 703 NL_SET_ERR_MSG_MOD(extack, 704 "Action not supported"); 705 return -EOPNOTSUPP; 706 } 707 } 708 return -EOPNOTSUPP; 709 } 710 711 static int am65_cpsw_qos_delete_clsflower(struct am65_cpsw_port *port, struct flow_cls_offload *cls) 712 { 713 struct am65_cpsw_qos *qos = &port->qos; 714 715 if (cls->cookie == qos->ale_bc_ratelimit.cookie) { 716 qos->ale_bc_ratelimit.cookie = 0; 717 qos->ale_bc_ratelimit.rate_packet_ps = 0; 718 cpsw_ale_rx_ratelimit_bc(port->common->ale, port->port_id, 0); 719 } 720 721 if (cls->cookie == qos->ale_mc_ratelimit.cookie) { 722 qos->ale_mc_ratelimit.cookie = 0; 723 qos->ale_mc_ratelimit.rate_packet_ps = 0; 724 cpsw_ale_rx_ratelimit_mc(port->common->ale, port->port_id, 0); 725 } 726 727 return 0; 728 } 729 730 static int am65_cpsw_qos_setup_tc_clsflower(struct am65_cpsw_port *port, 731 struct flow_cls_offload *cls_flower) 732 { 733 switch (cls_flower->command) { 734 case FLOW_CLS_REPLACE: 735 return am65_cpsw_qos_configure_clsflower(port, cls_flower); 736 case FLOW_CLS_DESTROY: 737 return am65_cpsw_qos_delete_clsflower(port, cls_flower); 738 default: 739 return -EOPNOTSUPP; 740 } 741 } 742 743 static int am65_cpsw_qos_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv) 744 { 745 struct am65_cpsw_port *port = cb_priv; 746 747 if (!tc_cls_can_offload_and_chain0(port->ndev, type_data)) 748 return -EOPNOTSUPP; 749 750 switch (type) { 751 case TC_SETUP_CLSFLOWER: 752 return am65_cpsw_qos_setup_tc_clsflower(port, type_data); 753 default: 754 return -EOPNOTSUPP; 755 } 756 } 757 758 static LIST_HEAD(am65_cpsw_qos_block_cb_list); 759 760 static int am65_cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f) 761 { 762 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 763 764 return flow_block_cb_setup_simple(f, &am65_cpsw_qos_block_cb_list, 765 am65_cpsw_qos_setup_tc_block_cb, 766 port, port, true); 767 } 768 769 int am65_cpsw_qos_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type, 770 void *type_data) 771 { 772 switch (type) { 773 case TC_SETUP_QDISC_TAPRIO: 774 return am65_cpsw_setup_taprio(ndev, type_data); 775 case TC_SETUP_BLOCK: 776 return am65_cpsw_qos_setup_tc_block(ndev, type_data); 777 default: 778 return -EOPNOTSUPP; 779 } 780 } 781 782 void am65_cpsw_qos_link_up(struct net_device *ndev, int link_speed) 783 { 784 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 785 786 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 787 return; 788 789 am65_cpsw_est_link_up(ndev, link_speed); 790 port->qos.link_down_time = 0; 791 } 792 793 void am65_cpsw_qos_link_down(struct net_device *ndev) 794 { 795 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 796 797 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 798 return; 799 800 if (!port->qos.link_down_time) 801 port->qos.link_down_time = ktime_get(); 802 803 port->qos.link_speed = SPEED_UNKNOWN; 804 } 805