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 devm_kfree(&ndev->dev, port->qos.est_oper); 170 171 port->qos.est_oper = port->qos.est_admin; 172 port->qos.est_admin = NULL; 173 } 174 175 static void am65_cpsw_port_est_get_buf_num(struct net_device *ndev, 176 struct am65_cpsw_est *est_new) 177 { 178 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 179 u32 val; 180 181 val = readl(port->port_base + AM65_CPSW_PN_REG_EST_CTL); 182 val &= ~AM65_CPSW_PN_EST_ONEBUF; 183 writel(val, port->port_base + AM65_CPSW_PN_REG_EST_CTL); 184 185 est_new->buf = am65_cpsw_port_est_get_free_buf_num(ndev); 186 187 /* rolled buf num means changed buf while configuring */ 188 if (port->qos.est_oper && port->qos.est_admin && 189 est_new->buf == port->qos.est_oper->buf) 190 am65_cpsw_admin_to_oper(ndev); 191 } 192 193 static void am65_cpsw_est_set(struct net_device *ndev, int enable) 194 { 195 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 196 struct am65_cpsw_common *common = port->common; 197 int common_enable = 0; 198 int i; 199 200 am65_cpsw_port_est_enable(port, enable); 201 202 for (i = 0; i < common->port_num; i++) 203 common_enable |= am65_cpsw_port_est_enabled(&common->ports[i]); 204 205 common_enable |= enable; 206 am65_cpsw_est_enable(common, common_enable); 207 } 208 209 /* This update is supposed to be used in any routine before getting real state 210 * of admin -> oper transition, particularly it's supposed to be used in some 211 * generic routine for providing real state to Taprio Qdisc. 212 */ 213 static void am65_cpsw_est_update_state(struct net_device *ndev) 214 { 215 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 216 int oper, admin; 217 218 if (!port->qos.est_admin) 219 return; 220 221 if (!am65_cpsw_port_est_is_swapped(ndev, &oper, &admin)) 222 return; 223 224 am65_cpsw_admin_to_oper(ndev); 225 } 226 227 /* Fetch command count it's number of bytes in Gigabit mode or nibbles in 228 * 10/100Mb mode. So, having speed and time in ns, recalculate ns to number of 229 * bytes/nibbles that can be sent while transmission on given speed. 230 */ 231 static int am65_est_cmd_ns_to_cnt(u64 ns, int link_speed) 232 { 233 u64 temp; 234 235 temp = ns * link_speed; 236 if (link_speed < SPEED_1000) 237 temp <<= 1; 238 239 return DIV_ROUND_UP(temp, 8 * 1000); 240 } 241 242 static void __iomem *am65_cpsw_est_set_sched_cmds(void __iomem *addr, 243 int fetch_cnt, 244 int fetch_allow) 245 { 246 u32 prio_mask, cmd_fetch_cnt, cmd; 247 248 do { 249 if (fetch_cnt > AM65_CPSW_FETCH_CNT_MAX) { 250 fetch_cnt -= AM65_CPSW_FETCH_CNT_MAX; 251 cmd_fetch_cnt = AM65_CPSW_FETCH_CNT_MAX; 252 } else { 253 cmd_fetch_cnt = fetch_cnt; 254 /* fetch count can't be less than 16? */ 255 if (cmd_fetch_cnt && cmd_fetch_cnt < 16) 256 cmd_fetch_cnt = 16; 257 258 fetch_cnt = 0; 259 } 260 261 prio_mask = fetch_allow & AM65_CPSW_FETCH_ALLOW_MSK; 262 cmd = (cmd_fetch_cnt << AM65_CPSW_FETCH_CNT_OFFSET) | prio_mask; 263 264 writel(cmd, addr); 265 addr += 4; 266 } while (fetch_cnt); 267 268 return addr; 269 } 270 271 static int am65_cpsw_est_calc_cmd_num(struct net_device *ndev, 272 struct tc_taprio_qopt_offload *taprio, 273 int link_speed) 274 { 275 int i, cmd_cnt, cmd_sum = 0; 276 u32 fetch_cnt; 277 278 for (i = 0; i < taprio->num_entries; i++) { 279 if (taprio->entries[i].command != TC_TAPRIO_CMD_SET_GATES) { 280 dev_err(&ndev->dev, "Only SET command is supported"); 281 return -EINVAL; 282 } 283 284 fetch_cnt = am65_est_cmd_ns_to_cnt(taprio->entries[i].interval, 285 link_speed); 286 287 cmd_cnt = DIV_ROUND_UP(fetch_cnt, AM65_CPSW_FETCH_CNT_MAX); 288 if (!cmd_cnt) 289 cmd_cnt++; 290 291 cmd_sum += cmd_cnt; 292 293 if (!fetch_cnt) 294 break; 295 } 296 297 return cmd_sum; 298 } 299 300 static int am65_cpsw_est_check_scheds(struct net_device *ndev, 301 struct am65_cpsw_est *est_new) 302 { 303 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 304 int cmd_num; 305 306 cmd_num = am65_cpsw_est_calc_cmd_num(ndev, &est_new->taprio, 307 port->qos.link_speed); 308 if (cmd_num < 0) 309 return cmd_num; 310 311 if (cmd_num > AM65_CPSW_FETCH_RAM_CMD_NUM / 2) { 312 dev_err(&ndev->dev, "No fetch RAM"); 313 return -ENOMEM; 314 } 315 316 return 0; 317 } 318 319 static void am65_cpsw_est_set_sched_list(struct net_device *ndev, 320 struct am65_cpsw_est *est_new) 321 { 322 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 323 u32 fetch_cnt, fetch_allow, all_fetch_allow = 0; 324 void __iomem *ram_addr, *max_ram_addr; 325 struct tc_taprio_sched_entry *entry; 326 int i, ram_size; 327 328 ram_addr = port->fetch_ram_base; 329 ram_size = AM65_CPSW_FETCH_RAM_CMD_NUM * 2; 330 ram_addr += est_new->buf * ram_size; 331 332 max_ram_addr = ram_size + ram_addr; 333 for (i = 0; i < est_new->taprio.num_entries; i++) { 334 entry = &est_new->taprio.entries[i]; 335 336 fetch_cnt = am65_est_cmd_ns_to_cnt(entry->interval, 337 port->qos.link_speed); 338 fetch_allow = entry->gate_mask; 339 if (fetch_allow > AM65_CPSW_FETCH_ALLOW_MAX) 340 dev_dbg(&ndev->dev, "fetch_allow > 8 bits: %d\n", 341 fetch_allow); 342 343 ram_addr = am65_cpsw_est_set_sched_cmds(ram_addr, fetch_cnt, 344 fetch_allow); 345 346 if (!fetch_cnt && i < est_new->taprio.num_entries - 1) { 347 dev_info(&ndev->dev, 348 "next scheds after %d have no impact", i + 1); 349 break; 350 } 351 352 all_fetch_allow |= fetch_allow; 353 } 354 355 /* end cmd, enabling non-timed queues for potential over cycle time */ 356 if (ram_addr < max_ram_addr) 357 writel(~all_fetch_allow & AM65_CPSW_FETCH_ALLOW_MSK, ram_addr); 358 } 359 360 /* 361 * Enable ESTf periodic output, set cycle start time and interval. 362 */ 363 static int am65_cpsw_timer_set(struct net_device *ndev, 364 struct am65_cpsw_est *est_new) 365 { 366 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 367 struct am65_cpsw_common *common = port->common; 368 struct am65_cpts *cpts = common->cpts; 369 struct am65_cpts_estf_cfg cfg; 370 371 cfg.ns_period = est_new->taprio.cycle_time; 372 cfg.ns_start = est_new->taprio.base_time; 373 374 return am65_cpts_estf_enable(cpts, port->port_id - 1, &cfg); 375 } 376 377 static void am65_cpsw_timer_stop(struct net_device *ndev) 378 { 379 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 380 struct am65_cpts *cpts = port->common->cpts; 381 382 am65_cpts_estf_disable(cpts, port->port_id - 1); 383 } 384 385 static enum timer_act am65_cpsw_timer_act(struct net_device *ndev, 386 struct am65_cpsw_est *est_new) 387 { 388 struct tc_taprio_qopt_offload *taprio_oper, *taprio_new; 389 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 390 struct am65_cpts *cpts = port->common->cpts; 391 u64 cur_time; 392 s64 diff; 393 394 if (!port->qos.est_oper) 395 return TACT_PROG; 396 397 taprio_new = &est_new->taprio; 398 taprio_oper = &port->qos.est_oper->taprio; 399 400 if (taprio_new->cycle_time != taprio_oper->cycle_time) 401 return TACT_NEED_STOP; 402 403 /* in order to avoid timer reset get base_time form oper taprio */ 404 if (!taprio_new->base_time && taprio_oper) 405 taprio_new->base_time = taprio_oper->base_time; 406 407 if (taprio_new->base_time == taprio_oper->base_time) 408 return TACT_SKIP_PROG; 409 410 /* base times are cycle synchronized */ 411 diff = taprio_new->base_time - taprio_oper->base_time; 412 diff = diff < 0 ? -diff : diff; 413 if (diff % taprio_new->cycle_time) 414 return TACT_NEED_STOP; 415 416 cur_time = am65_cpts_ns_gettime(cpts); 417 if (taprio_new->base_time <= cur_time + taprio_new->cycle_time) 418 return TACT_SKIP_PROG; 419 420 /* TODO: Admin schedule at future time is not currently supported */ 421 return TACT_NEED_STOP; 422 } 423 424 static void am65_cpsw_stop_est(struct net_device *ndev) 425 { 426 am65_cpsw_est_set(ndev, 0); 427 am65_cpsw_timer_stop(ndev); 428 } 429 430 static void am65_cpsw_purge_est(struct net_device *ndev) 431 { 432 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 433 434 am65_cpsw_stop_est(ndev); 435 436 devm_kfree(&ndev->dev, port->qos.est_admin); 437 devm_kfree(&ndev->dev, port->qos.est_oper); 438 439 port->qos.est_oper = NULL; 440 port->qos.est_admin = NULL; 441 } 442 443 static int am65_cpsw_configure_taprio(struct net_device *ndev, 444 struct am65_cpsw_est *est_new) 445 { 446 struct am65_cpsw_common *common = am65_ndev_to_common(ndev); 447 struct am65_cpts *cpts = common->cpts; 448 int ret = 0, tact = TACT_PROG; 449 450 am65_cpsw_est_update_state(ndev); 451 452 if (!est_new->taprio.enable) { 453 am65_cpsw_stop_est(ndev); 454 return ret; 455 } 456 457 ret = am65_cpsw_est_check_scheds(ndev, est_new); 458 if (ret < 0) 459 return ret; 460 461 tact = am65_cpsw_timer_act(ndev, est_new); 462 if (tact == TACT_NEED_STOP) { 463 dev_err(&ndev->dev, 464 "Can't toggle estf timer, stop taprio first"); 465 return -EINVAL; 466 } 467 468 if (tact == TACT_PROG) 469 am65_cpsw_timer_stop(ndev); 470 471 if (!est_new->taprio.base_time) 472 est_new->taprio.base_time = am65_cpts_ns_gettime(cpts); 473 474 am65_cpsw_port_est_get_buf_num(ndev, est_new); 475 am65_cpsw_est_set_sched_list(ndev, est_new); 476 am65_cpsw_port_est_assign_buf_num(ndev, est_new->buf); 477 478 am65_cpsw_est_set(ndev, est_new->taprio.enable); 479 480 if (tact == TACT_PROG) { 481 ret = am65_cpsw_timer_set(ndev, est_new); 482 if (ret) { 483 dev_err(&ndev->dev, "Failed to set cycle time"); 484 return ret; 485 } 486 } 487 488 return ret; 489 } 490 491 static void am65_cpsw_cp_taprio(struct tc_taprio_qopt_offload *from, 492 struct tc_taprio_qopt_offload *to) 493 { 494 int i; 495 496 *to = *from; 497 for (i = 0; i < from->num_entries; i++) 498 to->entries[i] = from->entries[i]; 499 } 500 501 static int am65_cpsw_set_taprio(struct net_device *ndev, void *type_data) 502 { 503 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 504 struct tc_taprio_qopt_offload *taprio = type_data; 505 struct am65_cpsw_est *est_new; 506 int ret = 0; 507 508 if (taprio->cycle_time_extension) { 509 dev_err(&ndev->dev, "Failed to set cycle time extension"); 510 return -EOPNOTSUPP; 511 } 512 513 est_new = devm_kzalloc(&ndev->dev, 514 struct_size(est_new, taprio.entries, taprio->num_entries), 515 GFP_KERNEL); 516 if (!est_new) 517 return -ENOMEM; 518 519 am65_cpsw_cp_taprio(taprio, &est_new->taprio); 520 ret = am65_cpsw_configure_taprio(ndev, est_new); 521 if (!ret) { 522 if (taprio->enable) { 523 devm_kfree(&ndev->dev, port->qos.est_admin); 524 525 port->qos.est_admin = est_new; 526 } else { 527 devm_kfree(&ndev->dev, est_new); 528 am65_cpsw_purge_est(ndev); 529 } 530 } else { 531 devm_kfree(&ndev->dev, est_new); 532 } 533 534 return ret; 535 } 536 537 static void am65_cpsw_est_link_up(struct net_device *ndev, int link_speed) 538 { 539 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 540 ktime_t cur_time; 541 s64 delta; 542 543 port->qos.link_speed = link_speed; 544 if (!am65_cpsw_port_est_enabled(port)) 545 return; 546 547 if (port->qos.link_down_time) { 548 cur_time = ktime_get(); 549 delta = ktime_us_delta(cur_time, port->qos.link_down_time); 550 if (delta > USEC_PER_SEC) { 551 dev_err(&ndev->dev, 552 "Link has been lost too long, stopping TAS"); 553 goto purge_est; 554 } 555 } 556 557 return; 558 559 purge_est: 560 am65_cpsw_purge_est(ndev); 561 } 562 563 static int am65_cpsw_setup_taprio(struct net_device *ndev, void *type_data) 564 { 565 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 566 struct am65_cpsw_common *common = port->common; 567 568 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 569 return -ENODEV; 570 571 if (!netif_running(ndev)) { 572 dev_err(&ndev->dev, "interface is down, link speed unknown\n"); 573 return -ENETDOWN; 574 } 575 576 if (common->pf_p0_rx_ptype_rrobin) { 577 dev_err(&ndev->dev, 578 "p0-rx-ptype-rrobin flag conflicts with taprio qdisc\n"); 579 return -EINVAL; 580 } 581 582 if (port->qos.link_speed == SPEED_UNKNOWN) 583 return -ENOLINK; 584 585 return am65_cpsw_set_taprio(ndev, type_data); 586 } 587 588 static int am65_cpsw_tc_query_caps(struct net_device *ndev, void *type_data) 589 { 590 struct tc_query_caps_base *base = type_data; 591 592 switch (base->type) { 593 case TC_SETUP_QDISC_TAPRIO: { 594 struct tc_taprio_caps *caps = base->caps; 595 596 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 597 return -EOPNOTSUPP; 598 599 caps->gate_mask_per_txq = true; 600 601 return 0; 602 } 603 default: 604 return -EOPNOTSUPP; 605 } 606 } 607 608 static int am65_cpsw_qos_clsflower_add_policer(struct am65_cpsw_port *port, 609 struct netlink_ext_ack *extack, 610 struct flow_cls_offload *cls, 611 u64 rate_pkt_ps) 612 { 613 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 614 struct flow_dissector *dissector = rule->match.dissector; 615 static const u8 mc_mac[] = {0x01, 0x00, 0x00, 0x00, 0x00, 0x00}; 616 struct am65_cpsw_qos *qos = &port->qos; 617 struct flow_match_eth_addrs match; 618 int ret; 619 620 if (dissector->used_keys & 621 ~(BIT(FLOW_DISSECTOR_KEY_BASIC) | 622 BIT(FLOW_DISSECTOR_KEY_CONTROL) | 623 BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS))) { 624 NL_SET_ERR_MSG_MOD(extack, 625 "Unsupported keys used"); 626 return -EOPNOTSUPP; 627 } 628 629 if (!flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) { 630 NL_SET_ERR_MSG_MOD(extack, "Not matching on eth address"); 631 return -EOPNOTSUPP; 632 } 633 634 flow_rule_match_eth_addrs(rule, &match); 635 636 if (!is_zero_ether_addr(match.mask->src)) { 637 NL_SET_ERR_MSG_MOD(extack, 638 "Matching on source MAC not supported"); 639 return -EOPNOTSUPP; 640 } 641 642 if (is_broadcast_ether_addr(match.key->dst) && 643 is_broadcast_ether_addr(match.mask->dst)) { 644 ret = cpsw_ale_rx_ratelimit_bc(port->common->ale, port->port_id, rate_pkt_ps); 645 if (ret) 646 return ret; 647 648 qos->ale_bc_ratelimit.cookie = cls->cookie; 649 qos->ale_bc_ratelimit.rate_packet_ps = rate_pkt_ps; 650 } else if (ether_addr_equal_unaligned(match.key->dst, mc_mac) && 651 ether_addr_equal_unaligned(match.mask->dst, mc_mac)) { 652 ret = cpsw_ale_rx_ratelimit_mc(port->common->ale, port->port_id, rate_pkt_ps); 653 if (ret) 654 return ret; 655 656 qos->ale_mc_ratelimit.cookie = cls->cookie; 657 qos->ale_mc_ratelimit.rate_packet_ps = rate_pkt_ps; 658 } else { 659 NL_SET_ERR_MSG_MOD(extack, "Not supported matching key"); 660 return -EOPNOTSUPP; 661 } 662 663 return 0; 664 } 665 666 static int am65_cpsw_qos_clsflower_policer_validate(const struct flow_action *action, 667 const struct flow_action_entry *act, 668 struct netlink_ext_ack *extack) 669 { 670 if (act->police.exceed.act_id != FLOW_ACTION_DROP) { 671 NL_SET_ERR_MSG_MOD(extack, 672 "Offload not supported when exceed action is not drop"); 673 return -EOPNOTSUPP; 674 } 675 676 if (act->police.notexceed.act_id != FLOW_ACTION_PIPE && 677 act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) { 678 NL_SET_ERR_MSG_MOD(extack, 679 "Offload not supported when conform action is not pipe or ok"); 680 return -EOPNOTSUPP; 681 } 682 683 if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT && 684 !flow_action_is_last_entry(action, act)) { 685 NL_SET_ERR_MSG_MOD(extack, 686 "Offload not supported when conform action is ok, but action is not last"); 687 return -EOPNOTSUPP; 688 } 689 690 if (act->police.rate_bytes_ps || act->police.peakrate_bytes_ps || 691 act->police.avrate || act->police.overhead) { 692 NL_SET_ERR_MSG_MOD(extack, 693 "Offload not supported when bytes per second/peakrate/avrate/overhead is configured"); 694 return -EOPNOTSUPP; 695 } 696 697 return 0; 698 } 699 700 static int am65_cpsw_qos_configure_clsflower(struct am65_cpsw_port *port, 701 struct flow_cls_offload *cls) 702 { 703 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 704 struct netlink_ext_ack *extack = cls->common.extack; 705 const struct flow_action_entry *act; 706 int i, ret; 707 708 flow_action_for_each(i, act, &rule->action) { 709 switch (act->id) { 710 case FLOW_ACTION_POLICE: 711 ret = am65_cpsw_qos_clsflower_policer_validate(&rule->action, act, extack); 712 if (ret) 713 return ret; 714 715 return am65_cpsw_qos_clsflower_add_policer(port, extack, cls, 716 act->police.rate_pkt_ps); 717 default: 718 NL_SET_ERR_MSG_MOD(extack, 719 "Action not supported"); 720 return -EOPNOTSUPP; 721 } 722 } 723 return -EOPNOTSUPP; 724 } 725 726 static int am65_cpsw_qos_delete_clsflower(struct am65_cpsw_port *port, struct flow_cls_offload *cls) 727 { 728 struct am65_cpsw_qos *qos = &port->qos; 729 730 if (cls->cookie == qos->ale_bc_ratelimit.cookie) { 731 qos->ale_bc_ratelimit.cookie = 0; 732 qos->ale_bc_ratelimit.rate_packet_ps = 0; 733 cpsw_ale_rx_ratelimit_bc(port->common->ale, port->port_id, 0); 734 } 735 736 if (cls->cookie == qos->ale_mc_ratelimit.cookie) { 737 qos->ale_mc_ratelimit.cookie = 0; 738 qos->ale_mc_ratelimit.rate_packet_ps = 0; 739 cpsw_ale_rx_ratelimit_mc(port->common->ale, port->port_id, 0); 740 } 741 742 return 0; 743 } 744 745 static int am65_cpsw_qos_setup_tc_clsflower(struct am65_cpsw_port *port, 746 struct flow_cls_offload *cls_flower) 747 { 748 switch (cls_flower->command) { 749 case FLOW_CLS_REPLACE: 750 return am65_cpsw_qos_configure_clsflower(port, cls_flower); 751 case FLOW_CLS_DESTROY: 752 return am65_cpsw_qos_delete_clsflower(port, cls_flower); 753 default: 754 return -EOPNOTSUPP; 755 } 756 } 757 758 static int am65_cpsw_qos_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv) 759 { 760 struct am65_cpsw_port *port = cb_priv; 761 762 if (!tc_cls_can_offload_and_chain0(port->ndev, type_data)) 763 return -EOPNOTSUPP; 764 765 switch (type) { 766 case TC_SETUP_CLSFLOWER: 767 return am65_cpsw_qos_setup_tc_clsflower(port, type_data); 768 default: 769 return -EOPNOTSUPP; 770 } 771 } 772 773 static LIST_HEAD(am65_cpsw_qos_block_cb_list); 774 775 static int am65_cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f) 776 { 777 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 778 779 return flow_block_cb_setup_simple(f, &am65_cpsw_qos_block_cb_list, 780 am65_cpsw_qos_setup_tc_block_cb, 781 port, port, true); 782 } 783 784 int am65_cpsw_qos_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type, 785 void *type_data) 786 { 787 switch (type) { 788 case TC_QUERY_CAPS: 789 return am65_cpsw_tc_query_caps(ndev, type_data); 790 case TC_SETUP_QDISC_TAPRIO: 791 return am65_cpsw_setup_taprio(ndev, type_data); 792 case TC_SETUP_BLOCK: 793 return am65_cpsw_qos_setup_tc_block(ndev, type_data); 794 default: 795 return -EOPNOTSUPP; 796 } 797 } 798 799 void am65_cpsw_qos_link_up(struct net_device *ndev, int link_speed) 800 { 801 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 802 803 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 804 return; 805 806 am65_cpsw_est_link_up(ndev, link_speed); 807 port->qos.link_down_time = 0; 808 } 809 810 void am65_cpsw_qos_link_down(struct net_device *ndev) 811 { 812 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 813 814 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 815 return; 816 817 if (!port->qos.link_down_time) 818 port->qos.link_down_time = ktime_get(); 819 820 port->qos.link_speed = SPEED_UNKNOWN; 821 } 822