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