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