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