1 // SPDX-License-Identifier: (GPL-2.0+ OR BSD-3-Clause) 2 /* Copyright 2019 NXP */ 3 4 #include "enetc.h" 5 6 #include <net/pkt_sched.h> 7 #include <linux/math64.h> 8 #include <linux/refcount.h> 9 #include <net/pkt_cls.h> 10 #include <net/tc_act/tc_gate.h> 11 12 static u16 enetc_get_max_gcl_len(struct enetc_hw *hw) 13 { 14 return enetc_rd(hw, ENETC_PTGCAPR) & ENETC_PTGCAPR_MAX_GCL_LEN_MASK; 15 } 16 17 void enetc_sched_speed_set(struct enetc_ndev_priv *priv, int speed) 18 { 19 struct enetc_hw *hw = &priv->si->hw; 20 u32 old_speed = priv->speed; 21 u32 pspeed, tmp; 22 23 if (speed == old_speed) 24 return; 25 26 switch (speed) { 27 case SPEED_1000: 28 pspeed = ENETC_PMR_PSPEED_1000M; 29 break; 30 case SPEED_2500: 31 pspeed = ENETC_PMR_PSPEED_2500M; 32 break; 33 case SPEED_100: 34 pspeed = ENETC_PMR_PSPEED_100M; 35 break; 36 case SPEED_10: 37 default: 38 pspeed = ENETC_PMR_PSPEED_10M; 39 } 40 41 priv->speed = speed; 42 tmp = enetc_port_rd(hw, ENETC_PMR); 43 enetc_port_wr(hw, ENETC_PMR, (tmp & ~ENETC_PMR_PSPEED_MASK) | pspeed); 44 } 45 46 static int enetc_setup_taprio(struct net_device *ndev, 47 struct tc_taprio_qopt_offload *admin_conf) 48 { 49 struct enetc_ndev_priv *priv = netdev_priv(ndev); 50 struct enetc_hw *hw = &priv->si->hw; 51 struct enetc_cbd cbd = {.cmd = 0}; 52 struct tgs_gcl_conf *gcl_config; 53 struct tgs_gcl_data *gcl_data; 54 dma_addr_t dma; 55 struct gce *gce; 56 u16 data_size; 57 u16 gcl_len; 58 void *tmp; 59 u32 tge; 60 int err; 61 int i; 62 63 if (admin_conf->num_entries > enetc_get_max_gcl_len(hw)) 64 return -EINVAL; 65 gcl_len = admin_conf->num_entries; 66 67 tge = enetc_rd(hw, ENETC_PTGCR); 68 if (!admin_conf->enable) { 69 enetc_wr(hw, ENETC_PTGCR, tge & ~ENETC_PTGCR_TGE); 70 enetc_reset_ptcmsdur(hw); 71 72 priv->active_offloads &= ~ENETC_F_QBV; 73 74 return 0; 75 } 76 77 if (admin_conf->cycle_time > U32_MAX || 78 admin_conf->cycle_time_extension > U32_MAX) 79 return -EINVAL; 80 81 /* Configure the (administrative) gate control list using the 82 * control BD descriptor. 83 */ 84 gcl_config = &cbd.gcl_conf; 85 86 data_size = struct_size(gcl_data, entry, gcl_len); 87 tmp = enetc_cbd_alloc_data_mem(priv->si, &cbd, data_size, 88 &dma, (void *)&gcl_data); 89 if (!tmp) 90 return -ENOMEM; 91 92 gce = (struct gce *)(gcl_data + 1); 93 94 /* Set all gates open as default */ 95 gcl_config->atc = 0xff; 96 gcl_config->acl_len = cpu_to_le16(gcl_len); 97 98 gcl_data->btl = cpu_to_le32(lower_32_bits(admin_conf->base_time)); 99 gcl_data->bth = cpu_to_le32(upper_32_bits(admin_conf->base_time)); 100 gcl_data->ct = cpu_to_le32(admin_conf->cycle_time); 101 gcl_data->cte = cpu_to_le32(admin_conf->cycle_time_extension); 102 103 for (i = 0; i < gcl_len; i++) { 104 struct tc_taprio_sched_entry *temp_entry; 105 struct gce *temp_gce = gce + i; 106 107 temp_entry = &admin_conf->entries[i]; 108 109 temp_gce->gate = (u8)temp_entry->gate_mask; 110 temp_gce->period = cpu_to_le32(temp_entry->interval); 111 } 112 113 cbd.status_flags = 0; 114 115 cbd.cls = BDCR_CMD_PORT_GCL; 116 cbd.status_flags = 0; 117 118 enetc_wr(hw, ENETC_PTGCR, tge | ENETC_PTGCR_TGE); 119 120 err = enetc_send_cmd(priv->si, &cbd); 121 if (err) 122 enetc_wr(hw, ENETC_PTGCR, tge & ~ENETC_PTGCR_TGE); 123 124 enetc_cbd_free_data_mem(priv->si, data_size, tmp, &dma); 125 126 if (err) 127 return err; 128 129 enetc_set_ptcmsdur(hw, admin_conf->max_sdu); 130 priv->active_offloads |= ENETC_F_QBV; 131 132 return 0; 133 } 134 135 int enetc_setup_tc_taprio(struct net_device *ndev, void *type_data) 136 { 137 struct tc_taprio_qopt_offload *taprio = type_data; 138 struct enetc_ndev_priv *priv = netdev_priv(ndev); 139 int err, i; 140 141 /* TSD and Qbv are mutually exclusive in hardware */ 142 for (i = 0; i < priv->num_tx_rings; i++) 143 if (priv->tx_ring[i]->tsd_enable) 144 return -EBUSY; 145 146 err = enetc_setup_tc_mqprio(ndev, &taprio->mqprio); 147 if (err) 148 return err; 149 150 err = enetc_setup_taprio(ndev, taprio); 151 if (err) { 152 taprio->mqprio.qopt.num_tc = 0; 153 enetc_setup_tc_mqprio(ndev, &taprio->mqprio); 154 } 155 156 return err; 157 } 158 159 static u32 enetc_get_cbs_enable(struct enetc_hw *hw, u8 tc) 160 { 161 return enetc_port_rd(hw, ENETC_PTCCBSR0(tc)) & ENETC_CBSE; 162 } 163 164 static u8 enetc_get_cbs_bw(struct enetc_hw *hw, u8 tc) 165 { 166 return enetc_port_rd(hw, ENETC_PTCCBSR0(tc)) & ENETC_CBS_BW_MASK; 167 } 168 169 int enetc_setup_tc_cbs(struct net_device *ndev, void *type_data) 170 { 171 struct enetc_ndev_priv *priv = netdev_priv(ndev); 172 struct tc_cbs_qopt_offload *cbs = type_data; 173 u32 port_transmit_rate = priv->speed; 174 u8 tc_nums = netdev_get_num_tc(ndev); 175 struct enetc_hw *hw = &priv->si->hw; 176 u32 hi_credit_bit, hi_credit_reg; 177 u32 max_interference_size; 178 u32 port_frame_max_size; 179 u8 tc = cbs->queue; 180 u8 prio_top, prio_next; 181 int bw_sum = 0; 182 u8 bw; 183 184 prio_top = tc_nums - 1; 185 prio_next = tc_nums - 2; 186 187 /* Support highest prio and second prio tc in cbs mode */ 188 if (tc != prio_top && tc != prio_next) 189 return -EOPNOTSUPP; 190 191 if (!cbs->enable) { 192 /* Make sure the other TC that are numerically 193 * lower than this TC have been disabled. 194 */ 195 if (tc == prio_top && 196 enetc_get_cbs_enable(hw, prio_next)) { 197 dev_err(&ndev->dev, 198 "Disable TC%d before disable TC%d\n", 199 prio_next, tc); 200 return -EINVAL; 201 } 202 203 enetc_port_wr(hw, ENETC_PTCCBSR1(tc), 0); 204 enetc_port_wr(hw, ENETC_PTCCBSR0(tc), 0); 205 206 return 0; 207 } 208 209 if (cbs->idleslope - cbs->sendslope != port_transmit_rate * 1000L || 210 cbs->idleslope < 0 || cbs->sendslope > 0) 211 return -EOPNOTSUPP; 212 213 port_frame_max_size = ndev->mtu + VLAN_ETH_HLEN + ETH_FCS_LEN; 214 215 bw = cbs->idleslope / (port_transmit_rate * 10UL); 216 217 /* Make sure the other TC that are numerically 218 * higher than this TC have been enabled. 219 */ 220 if (tc == prio_next) { 221 if (!enetc_get_cbs_enable(hw, prio_top)) { 222 dev_err(&ndev->dev, 223 "Enable TC%d first before enable TC%d\n", 224 prio_top, prio_next); 225 return -EINVAL; 226 } 227 bw_sum += enetc_get_cbs_bw(hw, prio_top); 228 } 229 230 if (bw_sum + bw >= 100) { 231 dev_err(&ndev->dev, 232 "The sum of all CBS Bandwidth can't exceed 100\n"); 233 return -EINVAL; 234 } 235 236 enetc_port_rd(hw, ENETC_PTCMSDUR(tc)); 237 238 /* For top prio TC, the max_interfrence_size is maxSizedFrame. 239 * 240 * For next prio TC, the max_interfrence_size is calculated as below: 241 * 242 * max_interference_size = M0 + Ma + Ra * M0 / (R0 - Ra) 243 * 244 * - RA: idleSlope for AVB Class A 245 * - R0: port transmit rate 246 * - M0: maximum sized frame for the port 247 * - MA: maximum sized frame for AVB Class A 248 */ 249 250 if (tc == prio_top) { 251 max_interference_size = port_frame_max_size * 8; 252 } else { 253 u32 m0, ma, r0, ra; 254 255 m0 = port_frame_max_size * 8; 256 ma = enetc_port_rd(hw, ENETC_PTCMSDUR(prio_top)) * 8; 257 ra = enetc_get_cbs_bw(hw, prio_top) * 258 port_transmit_rate * 10000ULL; 259 r0 = port_transmit_rate * 1000000ULL; 260 max_interference_size = m0 + ma + 261 (u32)div_u64((u64)ra * m0, r0 - ra); 262 } 263 264 /* hiCredit bits calculate by: 265 * 266 * maxSizedFrame * (idleSlope/portTxRate) 267 */ 268 hi_credit_bit = max_interference_size * bw / 100; 269 270 /* hiCredit bits to hiCredit register need to calculated as: 271 * 272 * (enetClockFrequency / portTransmitRate) * 100 273 */ 274 hi_credit_reg = (u32)div_u64((ENETC_CLK * 100ULL) * hi_credit_bit, 275 port_transmit_rate * 1000000ULL); 276 277 enetc_port_wr(hw, ENETC_PTCCBSR1(tc), hi_credit_reg); 278 279 /* Set bw register and enable this traffic class */ 280 enetc_port_wr(hw, ENETC_PTCCBSR0(tc), bw | ENETC_CBSE); 281 282 return 0; 283 } 284 285 int enetc_setup_tc_txtime(struct net_device *ndev, void *type_data) 286 { 287 struct enetc_ndev_priv *priv = netdev_priv(ndev); 288 struct tc_etf_qopt_offload *qopt = type_data; 289 u8 tc_nums = netdev_get_num_tc(ndev); 290 struct enetc_hw *hw = &priv->si->hw; 291 int tc; 292 293 if (!tc_nums) 294 return -EOPNOTSUPP; 295 296 tc = qopt->queue; 297 298 if (tc < 0 || tc >= priv->num_tx_rings) 299 return -EINVAL; 300 301 /* TSD and Qbv are mutually exclusive in hardware */ 302 if (enetc_rd(hw, ENETC_PTGCR) & ENETC_PTGCR_TGE) 303 return -EBUSY; 304 305 priv->tx_ring[tc]->tsd_enable = qopt->enable; 306 enetc_port_wr(hw, ENETC_PTCTSDR(tc), qopt->enable ? ENETC_TSDE : 0); 307 308 return 0; 309 } 310 311 enum streamid_type { 312 STREAMID_TYPE_RESERVED = 0, 313 STREAMID_TYPE_NULL, 314 STREAMID_TYPE_SMAC, 315 }; 316 317 enum streamid_vlan_tagged { 318 STREAMID_VLAN_RESERVED = 0, 319 STREAMID_VLAN_TAGGED, 320 STREAMID_VLAN_UNTAGGED, 321 STREAMID_VLAN_ALL, 322 }; 323 324 #define ENETC_PSFP_WILDCARD -1 325 #define HANDLE_OFFSET 100 326 327 enum forward_type { 328 FILTER_ACTION_TYPE_PSFP = BIT(0), 329 FILTER_ACTION_TYPE_ACL = BIT(1), 330 FILTER_ACTION_TYPE_BOTH = GENMASK(1, 0), 331 }; 332 333 /* This is for limit output type for input actions */ 334 struct actions_fwd { 335 u64 actions; 336 u64 keys; /* include the must needed keys */ 337 enum forward_type output; 338 }; 339 340 struct psfp_streamfilter_counters { 341 u64 matching_frames_count; 342 u64 passing_frames_count; 343 u64 not_passing_frames_count; 344 u64 passing_sdu_count; 345 u64 not_passing_sdu_count; 346 u64 red_frames_count; 347 }; 348 349 struct enetc_streamid { 350 u32 index; 351 union { 352 u8 src_mac[6]; 353 u8 dst_mac[6]; 354 }; 355 u8 filtertype; 356 u16 vid; 357 u8 tagged; 358 s32 handle; 359 }; 360 361 struct enetc_psfp_filter { 362 u32 index; 363 s32 handle; 364 s8 prio; 365 u32 maxsdu; 366 u32 gate_id; 367 s32 meter_id; 368 refcount_t refcount; 369 struct hlist_node node; 370 }; 371 372 struct enetc_psfp_gate { 373 u32 index; 374 s8 init_ipv; 375 u64 basetime; 376 u64 cycletime; 377 u64 cycletimext; 378 u32 num_entries; 379 refcount_t refcount; 380 struct hlist_node node; 381 struct action_gate_entry entries[]; 382 }; 383 384 /* Only enable the green color frame now 385 * Will add eir and ebs color blind, couple flag etc when 386 * policing action add more offloading parameters 387 */ 388 struct enetc_psfp_meter { 389 u32 index; 390 u32 cir; 391 u32 cbs; 392 refcount_t refcount; 393 struct hlist_node node; 394 }; 395 396 #define ENETC_PSFP_FLAGS_FMI BIT(0) 397 398 struct enetc_stream_filter { 399 struct enetc_streamid sid; 400 u32 sfi_index; 401 u32 sgi_index; 402 u32 flags; 403 u32 fmi_index; 404 struct flow_stats stats; 405 struct hlist_node node; 406 }; 407 408 struct enetc_psfp { 409 unsigned long dev_bitmap; 410 unsigned long *psfp_sfi_bitmap; 411 struct hlist_head stream_list; 412 struct hlist_head psfp_filter_list; 413 struct hlist_head psfp_gate_list; 414 struct hlist_head psfp_meter_list; 415 spinlock_t psfp_lock; /* spinlock for the struct enetc_psfp r/w */ 416 }; 417 418 static struct actions_fwd enetc_act_fwd[] = { 419 { 420 BIT(FLOW_ACTION_GATE), 421 BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS), 422 FILTER_ACTION_TYPE_PSFP 423 }, 424 { 425 BIT(FLOW_ACTION_POLICE) | 426 BIT(FLOW_ACTION_GATE), 427 BIT(FLOW_DISSECTOR_KEY_ETH_ADDRS), 428 FILTER_ACTION_TYPE_PSFP 429 }, 430 /* example for ACL actions */ 431 { 432 BIT(FLOW_ACTION_DROP), 433 0, 434 FILTER_ACTION_TYPE_ACL 435 } 436 }; 437 438 static struct enetc_psfp epsfp = { 439 .dev_bitmap = 0, 440 .psfp_sfi_bitmap = NULL, 441 }; 442 443 static LIST_HEAD(enetc_block_cb_list); 444 445 /* Stream Identity Entry Set Descriptor */ 446 static int enetc_streamid_hw_set(struct enetc_ndev_priv *priv, 447 struct enetc_streamid *sid, 448 u8 enable) 449 { 450 struct enetc_cbd cbd = {.cmd = 0}; 451 struct streamid_data *si_data; 452 struct streamid_conf *si_conf; 453 dma_addr_t dma; 454 u16 data_size; 455 void *tmp; 456 int port; 457 int err; 458 459 port = enetc_pf_to_port(priv->si->pdev); 460 if (port < 0) 461 return -EINVAL; 462 463 if (sid->index >= priv->psfp_cap.max_streamid) 464 return -EINVAL; 465 466 if (sid->filtertype != STREAMID_TYPE_NULL && 467 sid->filtertype != STREAMID_TYPE_SMAC) 468 return -EOPNOTSUPP; 469 470 /* Disable operation before enable */ 471 cbd.index = cpu_to_le16((u16)sid->index); 472 cbd.cls = BDCR_CMD_STREAM_IDENTIFY; 473 cbd.status_flags = 0; 474 475 data_size = sizeof(struct streamid_data); 476 tmp = enetc_cbd_alloc_data_mem(priv->si, &cbd, data_size, 477 &dma, (void *)&si_data); 478 if (!tmp) 479 return -ENOMEM; 480 481 eth_broadcast_addr(si_data->dmac); 482 si_data->vid_vidm_tg = (ENETC_CBDR_SID_VID_MASK 483 + ((0x3 << 14) | ENETC_CBDR_SID_VIDM)); 484 485 si_conf = &cbd.sid_set; 486 /* Only one port supported for one entry, set itself */ 487 si_conf->iports = cpu_to_le32(1 << port); 488 si_conf->id_type = 1; 489 si_conf->oui[2] = 0x0; 490 si_conf->oui[1] = 0x80; 491 si_conf->oui[0] = 0xC2; 492 493 err = enetc_send_cmd(priv->si, &cbd); 494 if (err) 495 goto out; 496 497 if (!enable) 498 goto out; 499 500 /* Enable the entry overwrite again incase space flushed by hardware */ 501 cbd.status_flags = 0; 502 503 si_conf->en = 0x80; 504 si_conf->stream_handle = cpu_to_le32(sid->handle); 505 si_conf->iports = cpu_to_le32(1 << port); 506 si_conf->id_type = sid->filtertype; 507 si_conf->oui[2] = 0x0; 508 si_conf->oui[1] = 0x80; 509 si_conf->oui[0] = 0xC2; 510 511 memset(si_data, 0, data_size); 512 513 /* VIDM default to be 1. 514 * VID Match. If set (b1) then the VID must match, otherwise 515 * any VID is considered a match. VIDM setting is only used 516 * when TG is set to b01. 517 */ 518 if (si_conf->id_type == STREAMID_TYPE_NULL) { 519 ether_addr_copy(si_data->dmac, sid->dst_mac); 520 si_data->vid_vidm_tg = (sid->vid & ENETC_CBDR_SID_VID_MASK) + 521 ((((u16)(sid->tagged) & 0x3) << 14) 522 | ENETC_CBDR_SID_VIDM); 523 } else if (si_conf->id_type == STREAMID_TYPE_SMAC) { 524 ether_addr_copy(si_data->smac, sid->src_mac); 525 si_data->vid_vidm_tg = (sid->vid & ENETC_CBDR_SID_VID_MASK) + 526 ((((u16)(sid->tagged) & 0x3) << 14) 527 | ENETC_CBDR_SID_VIDM); 528 } 529 530 err = enetc_send_cmd(priv->si, &cbd); 531 out: 532 enetc_cbd_free_data_mem(priv->si, data_size, tmp, &dma); 533 534 return err; 535 } 536 537 /* Stream Filter Instance Set Descriptor */ 538 static int enetc_streamfilter_hw_set(struct enetc_ndev_priv *priv, 539 struct enetc_psfp_filter *sfi, 540 u8 enable) 541 { 542 struct enetc_cbd cbd = {.cmd = 0}; 543 struct sfi_conf *sfi_config; 544 int port; 545 546 port = enetc_pf_to_port(priv->si->pdev); 547 if (port < 0) 548 return -EINVAL; 549 550 cbd.index = cpu_to_le16(sfi->index); 551 cbd.cls = BDCR_CMD_STREAM_FILTER; 552 cbd.status_flags = 0x80; 553 cbd.length = cpu_to_le16(1); 554 555 sfi_config = &cbd.sfi_conf; 556 if (!enable) 557 goto exit; 558 559 sfi_config->en = 0x80; 560 561 if (sfi->handle >= 0) { 562 sfi_config->stream_handle = 563 cpu_to_le32(sfi->handle); 564 sfi_config->sthm |= 0x80; 565 } 566 567 sfi_config->sg_inst_table_index = cpu_to_le16(sfi->gate_id); 568 sfi_config->input_ports = cpu_to_le32(1 << port); 569 570 /* The priority value which may be matched against the 571 * frame’s priority value to determine a match for this entry. 572 */ 573 if (sfi->prio >= 0) 574 sfi_config->multi |= (sfi->prio & 0x7) | 0x8; 575 576 /* Filter Type. Identifies the contents of the MSDU/FM_INST_INDEX 577 * field as being either an MSDU value or an index into the Flow 578 * Meter Instance table. 579 */ 580 if (sfi->maxsdu) { 581 sfi_config->msdu = 582 cpu_to_le16(sfi->maxsdu); 583 sfi_config->multi |= 0x40; 584 } 585 586 if (sfi->meter_id >= 0) { 587 sfi_config->fm_inst_table_index = cpu_to_le16(sfi->meter_id); 588 sfi_config->multi |= 0x80; 589 } 590 591 exit: 592 return enetc_send_cmd(priv->si, &cbd); 593 } 594 595 static int enetc_streamcounter_hw_get(struct enetc_ndev_priv *priv, 596 u32 index, 597 struct psfp_streamfilter_counters *cnt) 598 { 599 struct enetc_cbd cbd = { .cmd = 2 }; 600 struct sfi_counter_data *data_buf; 601 dma_addr_t dma; 602 u16 data_size; 603 void *tmp; 604 int err; 605 606 cbd.index = cpu_to_le16((u16)index); 607 cbd.cmd = 2; 608 cbd.cls = BDCR_CMD_STREAM_FILTER; 609 cbd.status_flags = 0; 610 611 data_size = sizeof(struct sfi_counter_data); 612 613 tmp = enetc_cbd_alloc_data_mem(priv->si, &cbd, data_size, 614 &dma, (void *)&data_buf); 615 if (!tmp) 616 return -ENOMEM; 617 618 err = enetc_send_cmd(priv->si, &cbd); 619 if (err) 620 goto exit; 621 622 cnt->matching_frames_count = ((u64)data_buf->matchh << 32) + 623 data_buf->matchl; 624 625 cnt->not_passing_sdu_count = ((u64)data_buf->msdu_droph << 32) + 626 data_buf->msdu_dropl; 627 628 cnt->passing_sdu_count = cnt->matching_frames_count 629 - cnt->not_passing_sdu_count; 630 631 cnt->not_passing_frames_count = 632 ((u64)data_buf->stream_gate_droph << 32) + 633 data_buf->stream_gate_dropl; 634 635 cnt->passing_frames_count = cnt->matching_frames_count - 636 cnt->not_passing_sdu_count - 637 cnt->not_passing_frames_count; 638 639 cnt->red_frames_count = ((u64)data_buf->flow_meter_droph << 32) + 640 data_buf->flow_meter_dropl; 641 642 exit: 643 enetc_cbd_free_data_mem(priv->si, data_size, tmp, &dma); 644 645 return err; 646 } 647 648 static u64 get_ptp_now(struct enetc_hw *hw) 649 { 650 u64 now_lo, now_hi, now; 651 652 now_lo = enetc_rd(hw, ENETC_SICTR0); 653 now_hi = enetc_rd(hw, ENETC_SICTR1); 654 now = now_lo | now_hi << 32; 655 656 return now; 657 } 658 659 static int get_start_ns(u64 now, u64 cycle, u64 *start) 660 { 661 u64 n; 662 663 if (!cycle) 664 return -EFAULT; 665 666 n = div64_u64(now, cycle); 667 668 *start = (n + 1) * cycle; 669 670 return 0; 671 } 672 673 /* Stream Gate Instance Set Descriptor */ 674 static int enetc_streamgate_hw_set(struct enetc_ndev_priv *priv, 675 struct enetc_psfp_gate *sgi, 676 u8 enable) 677 { 678 struct enetc_cbd cbd = { .cmd = 0 }; 679 struct sgi_table *sgi_config; 680 struct sgcl_conf *sgcl_config; 681 struct sgcl_data *sgcl_data; 682 struct sgce *sgce; 683 dma_addr_t dma; 684 u16 data_size; 685 int err, i; 686 void *tmp; 687 u64 now; 688 689 cbd.index = cpu_to_le16(sgi->index); 690 cbd.cmd = 0; 691 cbd.cls = BDCR_CMD_STREAM_GCL; 692 cbd.status_flags = 0x80; 693 694 /* disable */ 695 if (!enable) 696 return enetc_send_cmd(priv->si, &cbd); 697 698 if (!sgi->num_entries) 699 return 0; 700 701 if (sgi->num_entries > priv->psfp_cap.max_psfp_gatelist || 702 !sgi->cycletime) 703 return -EINVAL; 704 705 /* enable */ 706 sgi_config = &cbd.sgi_table; 707 708 /* Keep open before gate list start */ 709 sgi_config->ocgtst = 0x80; 710 711 sgi_config->oipv = (sgi->init_ipv < 0) ? 712 0x0 : ((sgi->init_ipv & 0x7) | 0x8); 713 714 sgi_config->en = 0x80; 715 716 /* Basic config */ 717 err = enetc_send_cmd(priv->si, &cbd); 718 if (err) 719 return -EINVAL; 720 721 memset(&cbd, 0, sizeof(cbd)); 722 723 cbd.index = cpu_to_le16(sgi->index); 724 cbd.cmd = 1; 725 cbd.cls = BDCR_CMD_STREAM_GCL; 726 cbd.status_flags = 0; 727 728 sgcl_config = &cbd.sgcl_conf; 729 730 sgcl_config->acl_len = (sgi->num_entries - 1) & 0x3; 731 732 data_size = struct_size(sgcl_data, sgcl, sgi->num_entries); 733 tmp = enetc_cbd_alloc_data_mem(priv->si, &cbd, data_size, 734 &dma, (void *)&sgcl_data); 735 if (!tmp) 736 return -ENOMEM; 737 738 sgce = &sgcl_data->sgcl[0]; 739 740 sgcl_config->agtst = 0x80; 741 742 sgcl_data->ct = sgi->cycletime; 743 sgcl_data->cte = sgi->cycletimext; 744 745 if (sgi->init_ipv >= 0) 746 sgcl_config->aipv = (sgi->init_ipv & 0x7) | 0x8; 747 748 for (i = 0; i < sgi->num_entries; i++) { 749 struct action_gate_entry *from = &sgi->entries[i]; 750 struct sgce *to = &sgce[i]; 751 752 if (from->gate_state) 753 to->multi |= 0x10; 754 755 if (from->ipv >= 0) 756 to->multi |= ((from->ipv & 0x7) << 5) | 0x08; 757 758 if (from->maxoctets >= 0) { 759 to->multi |= 0x01; 760 to->msdu[0] = from->maxoctets & 0xFF; 761 to->msdu[1] = (from->maxoctets >> 8) & 0xFF; 762 to->msdu[2] = (from->maxoctets >> 16) & 0xFF; 763 } 764 765 to->interval = from->interval; 766 } 767 768 /* If basetime is less than now, calculate start time */ 769 now = get_ptp_now(&priv->si->hw); 770 771 if (sgi->basetime < now) { 772 u64 start; 773 774 err = get_start_ns(now, sgi->cycletime, &start); 775 if (err) 776 goto exit; 777 sgcl_data->btl = lower_32_bits(start); 778 sgcl_data->bth = upper_32_bits(start); 779 } else { 780 u32 hi, lo; 781 782 hi = upper_32_bits(sgi->basetime); 783 lo = lower_32_bits(sgi->basetime); 784 sgcl_data->bth = hi; 785 sgcl_data->btl = lo; 786 } 787 788 err = enetc_send_cmd(priv->si, &cbd); 789 790 exit: 791 enetc_cbd_free_data_mem(priv->si, data_size, tmp, &dma); 792 return err; 793 } 794 795 static int enetc_flowmeter_hw_set(struct enetc_ndev_priv *priv, 796 struct enetc_psfp_meter *fmi, 797 u8 enable) 798 { 799 struct enetc_cbd cbd = { .cmd = 0 }; 800 struct fmi_conf *fmi_config; 801 u64 temp = 0; 802 803 cbd.index = cpu_to_le16((u16)fmi->index); 804 cbd.cls = BDCR_CMD_FLOW_METER; 805 cbd.status_flags = 0x80; 806 807 if (!enable) 808 return enetc_send_cmd(priv->si, &cbd); 809 810 fmi_config = &cbd.fmi_conf; 811 fmi_config->en = 0x80; 812 813 if (fmi->cir) { 814 temp = (u64)8000 * fmi->cir; 815 temp = div_u64(temp, 3725); 816 } 817 818 fmi_config->cir = cpu_to_le32((u32)temp); 819 fmi_config->cbs = cpu_to_le32(fmi->cbs); 820 821 /* Default for eir ebs disable */ 822 fmi_config->eir = 0; 823 fmi_config->ebs = 0; 824 825 /* Default: 826 * mark red disable 827 * drop on yellow disable 828 * color mode disable 829 * couple flag disable 830 */ 831 fmi_config->conf = 0; 832 833 return enetc_send_cmd(priv->si, &cbd); 834 } 835 836 static struct enetc_stream_filter *enetc_get_stream_by_index(u32 index) 837 { 838 struct enetc_stream_filter *f; 839 840 hlist_for_each_entry(f, &epsfp.stream_list, node) 841 if (f->sid.index == index) 842 return f; 843 844 return NULL; 845 } 846 847 static struct enetc_psfp_gate *enetc_get_gate_by_index(u32 index) 848 { 849 struct enetc_psfp_gate *g; 850 851 hlist_for_each_entry(g, &epsfp.psfp_gate_list, node) 852 if (g->index == index) 853 return g; 854 855 return NULL; 856 } 857 858 static struct enetc_psfp_filter *enetc_get_filter_by_index(u32 index) 859 { 860 struct enetc_psfp_filter *s; 861 862 hlist_for_each_entry(s, &epsfp.psfp_filter_list, node) 863 if (s->index == index) 864 return s; 865 866 return NULL; 867 } 868 869 static struct enetc_psfp_meter *enetc_get_meter_by_index(u32 index) 870 { 871 struct enetc_psfp_meter *m; 872 873 hlist_for_each_entry(m, &epsfp.psfp_meter_list, node) 874 if (m->index == index) 875 return m; 876 877 return NULL; 878 } 879 880 static struct enetc_psfp_filter 881 *enetc_psfp_check_sfi(struct enetc_psfp_filter *sfi) 882 { 883 struct enetc_psfp_filter *s; 884 885 hlist_for_each_entry(s, &epsfp.psfp_filter_list, node) 886 if (s->gate_id == sfi->gate_id && 887 s->prio == sfi->prio && 888 s->maxsdu == sfi->maxsdu && 889 s->meter_id == sfi->meter_id) 890 return s; 891 892 return NULL; 893 } 894 895 static int enetc_get_free_index(struct enetc_ndev_priv *priv) 896 { 897 u32 max_size = priv->psfp_cap.max_psfp_filter; 898 unsigned long index; 899 900 index = find_first_zero_bit(epsfp.psfp_sfi_bitmap, max_size); 901 if (index == max_size) 902 return -1; 903 904 return index; 905 } 906 907 static void stream_filter_unref(struct enetc_ndev_priv *priv, u32 index) 908 { 909 struct enetc_psfp_filter *sfi; 910 u8 z; 911 912 sfi = enetc_get_filter_by_index(index); 913 WARN_ON(!sfi); 914 z = refcount_dec_and_test(&sfi->refcount); 915 916 if (z) { 917 enetc_streamfilter_hw_set(priv, sfi, false); 918 hlist_del(&sfi->node); 919 kfree(sfi); 920 clear_bit(index, epsfp.psfp_sfi_bitmap); 921 } 922 } 923 924 static void stream_gate_unref(struct enetc_ndev_priv *priv, u32 index) 925 { 926 struct enetc_psfp_gate *sgi; 927 u8 z; 928 929 sgi = enetc_get_gate_by_index(index); 930 WARN_ON(!sgi); 931 z = refcount_dec_and_test(&sgi->refcount); 932 if (z) { 933 enetc_streamgate_hw_set(priv, sgi, false); 934 hlist_del(&sgi->node); 935 kfree(sgi); 936 } 937 } 938 939 static void flow_meter_unref(struct enetc_ndev_priv *priv, u32 index) 940 { 941 struct enetc_psfp_meter *fmi; 942 u8 z; 943 944 fmi = enetc_get_meter_by_index(index); 945 WARN_ON(!fmi); 946 z = refcount_dec_and_test(&fmi->refcount); 947 if (z) { 948 enetc_flowmeter_hw_set(priv, fmi, false); 949 hlist_del(&fmi->node); 950 kfree(fmi); 951 } 952 } 953 954 static void remove_one_chain(struct enetc_ndev_priv *priv, 955 struct enetc_stream_filter *filter) 956 { 957 if (filter->flags & ENETC_PSFP_FLAGS_FMI) 958 flow_meter_unref(priv, filter->fmi_index); 959 960 stream_gate_unref(priv, filter->sgi_index); 961 stream_filter_unref(priv, filter->sfi_index); 962 963 hlist_del(&filter->node); 964 kfree(filter); 965 } 966 967 static int enetc_psfp_hw_set(struct enetc_ndev_priv *priv, 968 struct enetc_streamid *sid, 969 struct enetc_psfp_filter *sfi, 970 struct enetc_psfp_gate *sgi, 971 struct enetc_psfp_meter *fmi) 972 { 973 int err; 974 975 err = enetc_streamid_hw_set(priv, sid, true); 976 if (err) 977 return err; 978 979 if (sfi) { 980 err = enetc_streamfilter_hw_set(priv, sfi, true); 981 if (err) 982 goto revert_sid; 983 } 984 985 err = enetc_streamgate_hw_set(priv, sgi, true); 986 if (err) 987 goto revert_sfi; 988 989 if (fmi) { 990 err = enetc_flowmeter_hw_set(priv, fmi, true); 991 if (err) 992 goto revert_sgi; 993 } 994 995 return 0; 996 997 revert_sgi: 998 enetc_streamgate_hw_set(priv, sgi, false); 999 revert_sfi: 1000 if (sfi) 1001 enetc_streamfilter_hw_set(priv, sfi, false); 1002 revert_sid: 1003 enetc_streamid_hw_set(priv, sid, false); 1004 return err; 1005 } 1006 1007 static struct actions_fwd *enetc_check_flow_actions(u64 acts, 1008 unsigned int inputkeys) 1009 { 1010 int i; 1011 1012 for (i = 0; i < ARRAY_SIZE(enetc_act_fwd); i++) 1013 if (acts == enetc_act_fwd[i].actions && 1014 inputkeys & enetc_act_fwd[i].keys) 1015 return &enetc_act_fwd[i]; 1016 1017 return NULL; 1018 } 1019 1020 static int enetc_psfp_policer_validate(const struct flow_action *action, 1021 const struct flow_action_entry *act, 1022 struct netlink_ext_ack *extack) 1023 { 1024 if (act->police.exceed.act_id != FLOW_ACTION_DROP) { 1025 NL_SET_ERR_MSG_MOD(extack, 1026 "Offload not supported when exceed action is not drop"); 1027 return -EOPNOTSUPP; 1028 } 1029 1030 if (act->police.notexceed.act_id != FLOW_ACTION_PIPE && 1031 act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) { 1032 NL_SET_ERR_MSG_MOD(extack, 1033 "Offload not supported when conform action is not pipe or ok"); 1034 return -EOPNOTSUPP; 1035 } 1036 1037 if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT && 1038 !flow_action_is_last_entry(action, act)) { 1039 NL_SET_ERR_MSG_MOD(extack, 1040 "Offload not supported when conform action is ok, but action is not last"); 1041 return -EOPNOTSUPP; 1042 } 1043 1044 if (act->police.peakrate_bytes_ps || 1045 act->police.avrate || act->police.overhead) { 1046 NL_SET_ERR_MSG_MOD(extack, 1047 "Offload not supported when peakrate/avrate/overhead is configured"); 1048 return -EOPNOTSUPP; 1049 } 1050 1051 if (act->police.rate_pkt_ps) { 1052 NL_SET_ERR_MSG_MOD(extack, 1053 "QoS offload not support packets per second"); 1054 return -EOPNOTSUPP; 1055 } 1056 1057 return 0; 1058 } 1059 1060 static int enetc_psfp_parse_clsflower(struct enetc_ndev_priv *priv, 1061 struct flow_cls_offload *f) 1062 { 1063 struct flow_action_entry *entryg = NULL, *entryp = NULL; 1064 struct flow_rule *rule = flow_cls_offload_flow_rule(f); 1065 struct netlink_ext_ack *extack = f->common.extack; 1066 struct enetc_stream_filter *filter, *old_filter; 1067 struct enetc_psfp_meter *fmi = NULL, *old_fmi; 1068 struct enetc_psfp_filter *sfi, *old_sfi; 1069 struct enetc_psfp_gate *sgi, *old_sgi; 1070 struct flow_action_entry *entry; 1071 struct action_gate_entry *e; 1072 u8 sfi_overwrite = 0; 1073 int entries_size; 1074 int i, err; 1075 1076 if (f->common.chain_index >= priv->psfp_cap.max_streamid) { 1077 NL_SET_ERR_MSG_MOD(extack, "No Stream identify resource!"); 1078 return -ENOSPC; 1079 } 1080 1081 flow_action_for_each(i, entry, &rule->action) 1082 if (entry->id == FLOW_ACTION_GATE) 1083 entryg = entry; 1084 else if (entry->id == FLOW_ACTION_POLICE) 1085 entryp = entry; 1086 1087 /* Not support without gate action */ 1088 if (!entryg) 1089 return -EINVAL; 1090 1091 filter = kzalloc(sizeof(*filter), GFP_KERNEL); 1092 if (!filter) 1093 return -ENOMEM; 1094 1095 filter->sid.index = f->common.chain_index; 1096 1097 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) { 1098 struct flow_match_eth_addrs match; 1099 1100 flow_rule_match_eth_addrs(rule, &match); 1101 1102 if (!is_zero_ether_addr(match.mask->dst) && 1103 !is_zero_ether_addr(match.mask->src)) { 1104 NL_SET_ERR_MSG_MOD(extack, 1105 "Cannot match on both source and destination MAC"); 1106 err = -EINVAL; 1107 goto free_filter; 1108 } 1109 1110 if (!is_zero_ether_addr(match.mask->dst)) { 1111 if (!is_broadcast_ether_addr(match.mask->dst)) { 1112 NL_SET_ERR_MSG_MOD(extack, 1113 "Masked matching on destination MAC not supported"); 1114 err = -EINVAL; 1115 goto free_filter; 1116 } 1117 ether_addr_copy(filter->sid.dst_mac, match.key->dst); 1118 filter->sid.filtertype = STREAMID_TYPE_NULL; 1119 } 1120 1121 if (!is_zero_ether_addr(match.mask->src)) { 1122 if (!is_broadcast_ether_addr(match.mask->src)) { 1123 NL_SET_ERR_MSG_MOD(extack, 1124 "Masked matching on source MAC not supported"); 1125 err = -EINVAL; 1126 goto free_filter; 1127 } 1128 ether_addr_copy(filter->sid.src_mac, match.key->src); 1129 filter->sid.filtertype = STREAMID_TYPE_SMAC; 1130 } 1131 } else { 1132 NL_SET_ERR_MSG_MOD(extack, "Unsupported, must include ETH_ADDRS"); 1133 err = -EINVAL; 1134 goto free_filter; 1135 } 1136 1137 if (flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_VLAN)) { 1138 struct flow_match_vlan match; 1139 1140 flow_rule_match_vlan(rule, &match); 1141 if (match.mask->vlan_priority) { 1142 if (match.mask->vlan_priority != 1143 (VLAN_PRIO_MASK >> VLAN_PRIO_SHIFT)) { 1144 NL_SET_ERR_MSG_MOD(extack, "Only full mask is supported for VLAN priority"); 1145 err = -EINVAL; 1146 goto free_filter; 1147 } 1148 } 1149 1150 if (match.mask->vlan_id) { 1151 if (match.mask->vlan_id != VLAN_VID_MASK) { 1152 NL_SET_ERR_MSG_MOD(extack, "Only full mask is supported for VLAN id"); 1153 err = -EINVAL; 1154 goto free_filter; 1155 } 1156 1157 filter->sid.vid = match.key->vlan_id; 1158 if (!filter->sid.vid) 1159 filter->sid.tagged = STREAMID_VLAN_UNTAGGED; 1160 else 1161 filter->sid.tagged = STREAMID_VLAN_TAGGED; 1162 } 1163 } else { 1164 filter->sid.tagged = STREAMID_VLAN_ALL; 1165 } 1166 1167 /* parsing gate action */ 1168 if (entryg->hw_index >= priv->psfp_cap.max_psfp_gate) { 1169 NL_SET_ERR_MSG_MOD(extack, "No Stream Gate resource!"); 1170 err = -ENOSPC; 1171 goto free_filter; 1172 } 1173 1174 if (entryg->gate.num_entries >= priv->psfp_cap.max_psfp_gatelist) { 1175 NL_SET_ERR_MSG_MOD(extack, "No Stream Gate resource!"); 1176 err = -ENOSPC; 1177 goto free_filter; 1178 } 1179 1180 entries_size = struct_size(sgi, entries, entryg->gate.num_entries); 1181 sgi = kzalloc(entries_size, GFP_KERNEL); 1182 if (!sgi) { 1183 err = -ENOMEM; 1184 goto free_filter; 1185 } 1186 1187 refcount_set(&sgi->refcount, 1); 1188 sgi->index = entryg->hw_index; 1189 sgi->init_ipv = entryg->gate.prio; 1190 sgi->basetime = entryg->gate.basetime; 1191 sgi->cycletime = entryg->gate.cycletime; 1192 sgi->num_entries = entryg->gate.num_entries; 1193 1194 e = sgi->entries; 1195 for (i = 0; i < entryg->gate.num_entries; i++) { 1196 e[i].gate_state = entryg->gate.entries[i].gate_state; 1197 e[i].interval = entryg->gate.entries[i].interval; 1198 e[i].ipv = entryg->gate.entries[i].ipv; 1199 e[i].maxoctets = entryg->gate.entries[i].maxoctets; 1200 } 1201 1202 filter->sgi_index = sgi->index; 1203 1204 sfi = kzalloc(sizeof(*sfi), GFP_KERNEL); 1205 if (!sfi) { 1206 err = -ENOMEM; 1207 goto free_gate; 1208 } 1209 1210 refcount_set(&sfi->refcount, 1); 1211 sfi->gate_id = sgi->index; 1212 sfi->meter_id = ENETC_PSFP_WILDCARD; 1213 1214 /* Flow meter and max frame size */ 1215 if (entryp) { 1216 err = enetc_psfp_policer_validate(&rule->action, entryp, extack); 1217 if (err) 1218 goto free_sfi; 1219 1220 if (entryp->police.burst) { 1221 fmi = kzalloc(sizeof(*fmi), GFP_KERNEL); 1222 if (!fmi) { 1223 err = -ENOMEM; 1224 goto free_sfi; 1225 } 1226 refcount_set(&fmi->refcount, 1); 1227 fmi->cir = entryp->police.rate_bytes_ps; 1228 fmi->cbs = entryp->police.burst; 1229 fmi->index = entryp->hw_index; 1230 filter->flags |= ENETC_PSFP_FLAGS_FMI; 1231 filter->fmi_index = fmi->index; 1232 sfi->meter_id = fmi->index; 1233 } 1234 1235 if (entryp->police.mtu) 1236 sfi->maxsdu = entryp->police.mtu; 1237 } 1238 1239 /* prio ref the filter prio */ 1240 if (f->common.prio && f->common.prio <= BIT(3)) 1241 sfi->prio = f->common.prio - 1; 1242 else 1243 sfi->prio = ENETC_PSFP_WILDCARD; 1244 1245 old_sfi = enetc_psfp_check_sfi(sfi); 1246 if (!old_sfi) { 1247 int index; 1248 1249 index = enetc_get_free_index(priv); 1250 if (index < 0) { 1251 NL_SET_ERR_MSG_MOD(extack, "No Stream Filter resource!"); 1252 err = -ENOSPC; 1253 goto free_fmi; 1254 } 1255 1256 sfi->index = index; 1257 sfi->handle = index + HANDLE_OFFSET; 1258 /* Update the stream filter handle also */ 1259 filter->sid.handle = sfi->handle; 1260 filter->sfi_index = sfi->index; 1261 sfi_overwrite = 0; 1262 } else { 1263 filter->sfi_index = old_sfi->index; 1264 filter->sid.handle = old_sfi->handle; 1265 sfi_overwrite = 1; 1266 } 1267 1268 err = enetc_psfp_hw_set(priv, &filter->sid, 1269 sfi_overwrite ? NULL : sfi, sgi, fmi); 1270 if (err) 1271 goto free_fmi; 1272 1273 spin_lock(&epsfp.psfp_lock); 1274 if (filter->flags & ENETC_PSFP_FLAGS_FMI) { 1275 old_fmi = enetc_get_meter_by_index(filter->fmi_index); 1276 if (old_fmi) { 1277 fmi->refcount = old_fmi->refcount; 1278 refcount_set(&fmi->refcount, 1279 refcount_read(&old_fmi->refcount) + 1); 1280 hlist_del(&old_fmi->node); 1281 kfree(old_fmi); 1282 } 1283 hlist_add_head(&fmi->node, &epsfp.psfp_meter_list); 1284 } 1285 1286 /* Remove the old node if exist and update with a new node */ 1287 old_sgi = enetc_get_gate_by_index(filter->sgi_index); 1288 if (old_sgi) { 1289 refcount_set(&sgi->refcount, 1290 refcount_read(&old_sgi->refcount) + 1); 1291 hlist_del(&old_sgi->node); 1292 kfree(old_sgi); 1293 } 1294 1295 hlist_add_head(&sgi->node, &epsfp.psfp_gate_list); 1296 1297 if (!old_sfi) { 1298 hlist_add_head(&sfi->node, &epsfp.psfp_filter_list); 1299 set_bit(sfi->index, epsfp.psfp_sfi_bitmap); 1300 } else { 1301 kfree(sfi); 1302 refcount_inc(&old_sfi->refcount); 1303 } 1304 1305 old_filter = enetc_get_stream_by_index(filter->sid.index); 1306 if (old_filter) 1307 remove_one_chain(priv, old_filter); 1308 1309 filter->stats.lastused = jiffies; 1310 hlist_add_head(&filter->node, &epsfp.stream_list); 1311 1312 spin_unlock(&epsfp.psfp_lock); 1313 1314 return 0; 1315 1316 free_fmi: 1317 kfree(fmi); 1318 free_sfi: 1319 kfree(sfi); 1320 free_gate: 1321 kfree(sgi); 1322 free_filter: 1323 kfree(filter); 1324 1325 return err; 1326 } 1327 1328 static int enetc_config_clsflower(struct enetc_ndev_priv *priv, 1329 struct flow_cls_offload *cls_flower) 1330 { 1331 struct flow_rule *rule = flow_cls_offload_flow_rule(cls_flower); 1332 struct netlink_ext_ack *extack = cls_flower->common.extack; 1333 struct flow_dissector *dissector = rule->match.dissector; 1334 struct flow_action *action = &rule->action; 1335 struct flow_action_entry *entry; 1336 struct actions_fwd *fwd; 1337 u64 actions = 0; 1338 int i, err; 1339 1340 if (!flow_action_has_entries(action)) { 1341 NL_SET_ERR_MSG_MOD(extack, "At least one action is needed"); 1342 return -EINVAL; 1343 } 1344 1345 flow_action_for_each(i, entry, action) 1346 actions |= BIT(entry->id); 1347 1348 fwd = enetc_check_flow_actions(actions, dissector->used_keys); 1349 if (!fwd) { 1350 NL_SET_ERR_MSG_MOD(extack, "Unsupported filter type!"); 1351 return -EOPNOTSUPP; 1352 } 1353 1354 if (fwd->output & FILTER_ACTION_TYPE_PSFP) { 1355 err = enetc_psfp_parse_clsflower(priv, cls_flower); 1356 if (err) { 1357 NL_SET_ERR_MSG_MOD(extack, "Invalid PSFP inputs"); 1358 return err; 1359 } 1360 } else { 1361 NL_SET_ERR_MSG_MOD(extack, "Unsupported actions"); 1362 return -EOPNOTSUPP; 1363 } 1364 1365 return 0; 1366 } 1367 1368 static int enetc_psfp_destroy_clsflower(struct enetc_ndev_priv *priv, 1369 struct flow_cls_offload *f) 1370 { 1371 struct enetc_stream_filter *filter; 1372 struct netlink_ext_ack *extack = f->common.extack; 1373 int err; 1374 1375 if (f->common.chain_index >= priv->psfp_cap.max_streamid) { 1376 NL_SET_ERR_MSG_MOD(extack, "No Stream identify resource!"); 1377 return -ENOSPC; 1378 } 1379 1380 filter = enetc_get_stream_by_index(f->common.chain_index); 1381 if (!filter) 1382 return -EINVAL; 1383 1384 err = enetc_streamid_hw_set(priv, &filter->sid, false); 1385 if (err) 1386 return err; 1387 1388 remove_one_chain(priv, filter); 1389 1390 return 0; 1391 } 1392 1393 static int enetc_destroy_clsflower(struct enetc_ndev_priv *priv, 1394 struct flow_cls_offload *f) 1395 { 1396 return enetc_psfp_destroy_clsflower(priv, f); 1397 } 1398 1399 static int enetc_psfp_get_stats(struct enetc_ndev_priv *priv, 1400 struct flow_cls_offload *f) 1401 { 1402 struct psfp_streamfilter_counters counters = {}; 1403 struct enetc_stream_filter *filter; 1404 struct flow_stats stats = {}; 1405 int err; 1406 1407 filter = enetc_get_stream_by_index(f->common.chain_index); 1408 if (!filter) 1409 return -EINVAL; 1410 1411 err = enetc_streamcounter_hw_get(priv, filter->sfi_index, &counters); 1412 if (err) 1413 return -EINVAL; 1414 1415 spin_lock(&epsfp.psfp_lock); 1416 stats.pkts = counters.matching_frames_count + 1417 counters.not_passing_sdu_count - 1418 filter->stats.pkts; 1419 stats.drops = counters.not_passing_frames_count + 1420 counters.not_passing_sdu_count + 1421 counters.red_frames_count - 1422 filter->stats.drops; 1423 stats.lastused = filter->stats.lastused; 1424 filter->stats.pkts += stats.pkts; 1425 filter->stats.drops += stats.drops; 1426 spin_unlock(&epsfp.psfp_lock); 1427 1428 flow_stats_update(&f->stats, 0x0, stats.pkts, stats.drops, 1429 stats.lastused, FLOW_ACTION_HW_STATS_DELAYED); 1430 1431 return 0; 1432 } 1433 1434 static int enetc_setup_tc_cls_flower(struct enetc_ndev_priv *priv, 1435 struct flow_cls_offload *cls_flower) 1436 { 1437 switch (cls_flower->command) { 1438 case FLOW_CLS_REPLACE: 1439 return enetc_config_clsflower(priv, cls_flower); 1440 case FLOW_CLS_DESTROY: 1441 return enetc_destroy_clsflower(priv, cls_flower); 1442 case FLOW_CLS_STATS: 1443 return enetc_psfp_get_stats(priv, cls_flower); 1444 default: 1445 return -EOPNOTSUPP; 1446 } 1447 } 1448 1449 static inline void clean_psfp_sfi_bitmap(void) 1450 { 1451 bitmap_free(epsfp.psfp_sfi_bitmap); 1452 epsfp.psfp_sfi_bitmap = NULL; 1453 } 1454 1455 static void clean_stream_list(void) 1456 { 1457 struct enetc_stream_filter *s; 1458 struct hlist_node *tmp; 1459 1460 hlist_for_each_entry_safe(s, tmp, &epsfp.stream_list, node) { 1461 hlist_del(&s->node); 1462 kfree(s); 1463 } 1464 } 1465 1466 static void clean_sfi_list(void) 1467 { 1468 struct enetc_psfp_filter *sfi; 1469 struct hlist_node *tmp; 1470 1471 hlist_for_each_entry_safe(sfi, tmp, &epsfp.psfp_filter_list, node) { 1472 hlist_del(&sfi->node); 1473 kfree(sfi); 1474 } 1475 } 1476 1477 static void clean_sgi_list(void) 1478 { 1479 struct enetc_psfp_gate *sgi; 1480 struct hlist_node *tmp; 1481 1482 hlist_for_each_entry_safe(sgi, tmp, &epsfp.psfp_gate_list, node) { 1483 hlist_del(&sgi->node); 1484 kfree(sgi); 1485 } 1486 } 1487 1488 static void clean_psfp_all(void) 1489 { 1490 /* Disable all list nodes and free all memory */ 1491 clean_sfi_list(); 1492 clean_sgi_list(); 1493 clean_stream_list(); 1494 epsfp.dev_bitmap = 0; 1495 clean_psfp_sfi_bitmap(); 1496 } 1497 1498 int enetc_setup_tc_block_cb(enum tc_setup_type type, void *type_data, 1499 void *cb_priv) 1500 { 1501 struct net_device *ndev = cb_priv; 1502 1503 if (!tc_can_offload(ndev)) 1504 return -EOPNOTSUPP; 1505 1506 switch (type) { 1507 case TC_SETUP_CLSFLOWER: 1508 return enetc_setup_tc_cls_flower(netdev_priv(ndev), type_data); 1509 default: 1510 return -EOPNOTSUPP; 1511 } 1512 } 1513 1514 int enetc_set_psfp(struct net_device *ndev, bool en) 1515 { 1516 struct enetc_ndev_priv *priv = netdev_priv(ndev); 1517 int err; 1518 1519 if (en) { 1520 err = enetc_psfp_enable(priv); 1521 if (err) 1522 return err; 1523 1524 priv->active_offloads |= ENETC_F_QCI; 1525 return 0; 1526 } 1527 1528 err = enetc_psfp_disable(priv); 1529 if (err) 1530 return err; 1531 1532 priv->active_offloads &= ~ENETC_F_QCI; 1533 1534 return 0; 1535 } 1536 1537 int enetc_psfp_init(struct enetc_ndev_priv *priv) 1538 { 1539 if (epsfp.psfp_sfi_bitmap) 1540 return 0; 1541 1542 epsfp.psfp_sfi_bitmap = bitmap_zalloc(priv->psfp_cap.max_psfp_filter, 1543 GFP_KERNEL); 1544 if (!epsfp.psfp_sfi_bitmap) 1545 return -ENOMEM; 1546 1547 spin_lock_init(&epsfp.psfp_lock); 1548 1549 if (list_empty(&enetc_block_cb_list)) 1550 epsfp.dev_bitmap = 0; 1551 1552 return 0; 1553 } 1554 1555 int enetc_psfp_clean(struct enetc_ndev_priv *priv) 1556 { 1557 if (!list_empty(&enetc_block_cb_list)) 1558 return -EBUSY; 1559 1560 clean_psfp_all(); 1561 1562 return 0; 1563 } 1564 1565 int enetc_setup_tc_psfp(struct net_device *ndev, void *type_data) 1566 { 1567 struct enetc_ndev_priv *priv = netdev_priv(ndev); 1568 struct flow_block_offload *f = type_data; 1569 int port, err; 1570 1571 err = flow_block_cb_setup_simple(f, &enetc_block_cb_list, 1572 enetc_setup_tc_block_cb, 1573 ndev, ndev, true); 1574 if (err) 1575 return err; 1576 1577 switch (f->command) { 1578 case FLOW_BLOCK_BIND: 1579 port = enetc_pf_to_port(priv->si->pdev); 1580 if (port < 0) 1581 return -EINVAL; 1582 1583 set_bit(port, &epsfp.dev_bitmap); 1584 break; 1585 case FLOW_BLOCK_UNBIND: 1586 port = enetc_pf_to_port(priv->si->pdev); 1587 if (port < 0) 1588 return -EINVAL; 1589 1590 clear_bit(port, &epsfp.dev_bitmap); 1591 if (!epsfp.dev_bitmap) 1592 clean_psfp_all(); 1593 break; 1594 } 1595 1596 return 0; 1597 } 1598 1599 int enetc_qos_query_caps(struct net_device *ndev, void *type_data) 1600 { 1601 struct enetc_ndev_priv *priv = netdev_priv(ndev); 1602 struct tc_query_caps_base *base = type_data; 1603 struct enetc_si *si = priv->si; 1604 1605 switch (base->type) { 1606 case TC_SETUP_QDISC_MQPRIO: { 1607 struct tc_mqprio_caps *caps = base->caps; 1608 1609 caps->validate_queue_counts = true; 1610 1611 return 0; 1612 } 1613 case TC_SETUP_QDISC_TAPRIO: { 1614 struct tc_taprio_caps *caps = base->caps; 1615 1616 if (si->hw_features & ENETC_SI_F_QBV) 1617 caps->supports_queue_max_sdu = true; 1618 1619 return 0; 1620 } 1621 default: 1622 return -EOPNOTSUPP; 1623 } 1624 } 1625