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