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