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