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_qos_clsflower_add_policer(struct am65_cpsw_port *port, 589 struct netlink_ext_ack *extack, 590 struct flow_cls_offload *cls, 591 u64 rate_pkt_ps) 592 { 593 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 594 struct flow_dissector *dissector = rule->match.dissector; 595 static const u8 mc_mac[] = {0x01, 0x00, 0x00, 0x00, 0x00, 0x00}; 596 struct am65_cpsw_qos *qos = &port->qos; 597 struct flow_match_eth_addrs match; 598 int ret; 599 600 if (dissector->used_keys & 601 ~(BIT(FLOW_DISSECTOR_KEY_BASIC) | 602 BIT(FLOW_DISSECTOR_KEY_CONTROL) | 603 BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS))) { 604 NL_SET_ERR_MSG_MOD(extack, 605 "Unsupported keys used"); 606 return -EOPNOTSUPP; 607 } 608 609 if (!flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) { 610 NL_SET_ERR_MSG_MOD(extack, "Not matching on eth address"); 611 return -EOPNOTSUPP; 612 } 613 614 flow_rule_match_eth_addrs(rule, &match); 615 616 if (!is_zero_ether_addr(match.mask->src)) { 617 NL_SET_ERR_MSG_MOD(extack, 618 "Matching on source MAC not supported"); 619 return -EOPNOTSUPP; 620 } 621 622 if (is_broadcast_ether_addr(match.key->dst) && 623 is_broadcast_ether_addr(match.mask->dst)) { 624 ret = cpsw_ale_rx_ratelimit_bc(port->common->ale, port->port_id, rate_pkt_ps); 625 if (ret) 626 return ret; 627 628 qos->ale_bc_ratelimit.cookie = cls->cookie; 629 qos->ale_bc_ratelimit.rate_packet_ps = rate_pkt_ps; 630 } else if (ether_addr_equal_unaligned(match.key->dst, mc_mac) && 631 ether_addr_equal_unaligned(match.mask->dst, mc_mac)) { 632 ret = cpsw_ale_rx_ratelimit_mc(port->common->ale, port->port_id, rate_pkt_ps); 633 if (ret) 634 return ret; 635 636 qos->ale_mc_ratelimit.cookie = cls->cookie; 637 qos->ale_mc_ratelimit.rate_packet_ps = rate_pkt_ps; 638 } else { 639 NL_SET_ERR_MSG_MOD(extack, "Not supported matching key"); 640 return -EOPNOTSUPP; 641 } 642 643 return 0; 644 } 645 646 static int am65_cpsw_qos_clsflower_policer_validate(const struct flow_action *action, 647 const struct flow_action_entry *act, 648 struct netlink_ext_ack *extack) 649 { 650 if (act->police.exceed.act_id != FLOW_ACTION_DROP) { 651 NL_SET_ERR_MSG_MOD(extack, 652 "Offload not supported when exceed action is not drop"); 653 return -EOPNOTSUPP; 654 } 655 656 if (act->police.notexceed.act_id != FLOW_ACTION_PIPE && 657 act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) { 658 NL_SET_ERR_MSG_MOD(extack, 659 "Offload not supported when conform action is not pipe or ok"); 660 return -EOPNOTSUPP; 661 } 662 663 if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT && 664 !flow_action_is_last_entry(action, act)) { 665 NL_SET_ERR_MSG_MOD(extack, 666 "Offload not supported when conform action is ok, but action is not last"); 667 return -EOPNOTSUPP; 668 } 669 670 if (act->police.rate_bytes_ps || act->police.peakrate_bytes_ps || 671 act->police.avrate || act->police.overhead) { 672 NL_SET_ERR_MSG_MOD(extack, 673 "Offload not supported when bytes per second/peakrate/avrate/overhead is configured"); 674 return -EOPNOTSUPP; 675 } 676 677 return 0; 678 } 679 680 static int am65_cpsw_qos_configure_clsflower(struct am65_cpsw_port *port, 681 struct flow_cls_offload *cls) 682 { 683 struct flow_rule *rule = flow_cls_offload_flow_rule(cls); 684 struct netlink_ext_ack *extack = cls->common.extack; 685 const struct flow_action_entry *act; 686 int i, ret; 687 688 flow_action_for_each(i, act, &rule->action) { 689 switch (act->id) { 690 case FLOW_ACTION_POLICE: 691 ret = am65_cpsw_qos_clsflower_policer_validate(&rule->action, act, extack); 692 if (ret) 693 return ret; 694 695 return am65_cpsw_qos_clsflower_add_policer(port, extack, cls, 696 act->police.rate_pkt_ps); 697 default: 698 NL_SET_ERR_MSG_MOD(extack, 699 "Action not supported"); 700 return -EOPNOTSUPP; 701 } 702 } 703 return -EOPNOTSUPP; 704 } 705 706 static int am65_cpsw_qos_delete_clsflower(struct am65_cpsw_port *port, struct flow_cls_offload *cls) 707 { 708 struct am65_cpsw_qos *qos = &port->qos; 709 710 if (cls->cookie == qos->ale_bc_ratelimit.cookie) { 711 qos->ale_bc_ratelimit.cookie = 0; 712 qos->ale_bc_ratelimit.rate_packet_ps = 0; 713 cpsw_ale_rx_ratelimit_bc(port->common->ale, port->port_id, 0); 714 } 715 716 if (cls->cookie == qos->ale_mc_ratelimit.cookie) { 717 qos->ale_mc_ratelimit.cookie = 0; 718 qos->ale_mc_ratelimit.rate_packet_ps = 0; 719 cpsw_ale_rx_ratelimit_mc(port->common->ale, port->port_id, 0); 720 } 721 722 return 0; 723 } 724 725 static int am65_cpsw_qos_setup_tc_clsflower(struct am65_cpsw_port *port, 726 struct flow_cls_offload *cls_flower) 727 { 728 switch (cls_flower->command) { 729 case FLOW_CLS_REPLACE: 730 return am65_cpsw_qos_configure_clsflower(port, cls_flower); 731 case FLOW_CLS_DESTROY: 732 return am65_cpsw_qos_delete_clsflower(port, cls_flower); 733 default: 734 return -EOPNOTSUPP; 735 } 736 } 737 738 static int am65_cpsw_qos_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv) 739 { 740 struct am65_cpsw_port *port = cb_priv; 741 742 if (!tc_cls_can_offload_and_chain0(port->ndev, type_data)) 743 return -EOPNOTSUPP; 744 745 switch (type) { 746 case TC_SETUP_CLSFLOWER: 747 return am65_cpsw_qos_setup_tc_clsflower(port, type_data); 748 default: 749 return -EOPNOTSUPP; 750 } 751 } 752 753 static LIST_HEAD(am65_cpsw_qos_block_cb_list); 754 755 static int am65_cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f) 756 { 757 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 758 759 return flow_block_cb_setup_simple(f, &am65_cpsw_qos_block_cb_list, 760 am65_cpsw_qos_setup_tc_block_cb, 761 port, port, true); 762 } 763 764 int am65_cpsw_qos_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type, 765 void *type_data) 766 { 767 switch (type) { 768 case TC_SETUP_QDISC_TAPRIO: 769 return am65_cpsw_setup_taprio(ndev, type_data); 770 case TC_SETUP_BLOCK: 771 return am65_cpsw_qos_setup_tc_block(ndev, type_data); 772 default: 773 return -EOPNOTSUPP; 774 } 775 } 776 777 void am65_cpsw_qos_link_up(struct net_device *ndev, int link_speed) 778 { 779 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 780 781 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 782 return; 783 784 am65_cpsw_est_link_up(ndev, link_speed); 785 port->qos.link_down_time = 0; 786 } 787 788 void am65_cpsw_qos_link_down(struct net_device *ndev) 789 { 790 struct am65_cpsw_port *port = am65_ndev_to_port(ndev); 791 792 if (!IS_ENABLED(CONFIG_TI_AM65_CPSW_TAS)) 793 return; 794 795 if (!port->qos.link_down_time) 796 port->qos.link_down_time = ktime_get(); 797 798 port->qos.link_speed = SPEED_UNKNOWN; 799 } 800