1587 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Texas Instruments Ethernet Switch Driver4 *5 * Copyright (C) 2019 Texas Instruments6 */7 8#include <linux/bpf.h>9#include <linux/bpf_trace.h>10#include <linux/if_ether.h>11#include <linux/if_vlan.h>12#include <linux/kmemleak.h>13#include <linux/module.h>14#include <linux/netdevice.h>15#include <linux/net_tstamp.h>16#include <linux/of.h>17#include <linux/phy.h>18#include <linux/platform_device.h>19#include <linux/pm_runtime.h>20#include <linux/skbuff.h>21#include <net/page_pool/helpers.h>22#include <net/pkt_cls.h>23#include <net/pkt_sched.h>24 25#include "cpsw.h"26#include "cpts.h"27#include "cpsw_ale.h"28#include "cpsw_priv.h"29#include "cpsw_sl.h"30#include "davinci_cpdma.h"31 32#define CPTS_N_ETX_TS 433 34int (*cpsw_slave_index)(struct cpsw_common *cpsw, struct cpsw_priv *priv);35 36void cpsw_intr_enable(struct cpsw_common *cpsw)37{38 writel_relaxed(0xFF, &cpsw->wr_regs->tx_en);39 writel_relaxed(0xFF, &cpsw->wr_regs->rx_en);40 41 cpdma_ctlr_int_ctrl(cpsw->dma, true);42}43 44void cpsw_intr_disable(struct cpsw_common *cpsw)45{46 writel_relaxed(0, &cpsw->wr_regs->tx_en);47 writel_relaxed(0, &cpsw->wr_regs->rx_en);48 49 cpdma_ctlr_int_ctrl(cpsw->dma, false);50}51 52void cpsw_tx_handler(void *token, int len, int status)53{54 struct cpsw_meta_xdp *xmeta;55 struct xdp_frame *xdpf;56 struct net_device *ndev;57 struct netdev_queue *txq;58 struct sk_buff *skb;59 int ch;60 61 if (cpsw_is_xdpf_handle(token)) {62 xdpf = cpsw_handle_to_xdpf(token);63 xmeta = (void *)xdpf + CPSW_XMETA_OFFSET;64 ndev = xmeta->ndev;65 ch = xmeta->ch;66 xdp_return_frame(xdpf);67 } else {68 skb = token;69 ndev = skb->dev;70 ch = skb_get_queue_mapping(skb);71 cpts_tx_timestamp(ndev_to_cpsw(ndev)->cpts, skb);72 dev_kfree_skb_any(skb);73 }74 75 /* Check whether the queue is stopped due to stalled tx dma, if the76 * queue is stopped then start the queue as we have free desc for tx77 */78 txq = netdev_get_tx_queue(ndev, ch);79 if (unlikely(netif_tx_queue_stopped(txq)))80 netif_tx_wake_queue(txq);81 82 ndev->stats.tx_packets++;83 ndev->stats.tx_bytes += len;84}85 86irqreturn_t cpsw_tx_interrupt(int irq, void *dev_id)87{88 struct cpsw_common *cpsw = dev_id;89 90 writel(0, &cpsw->wr_regs->tx_en);91 cpdma_ctlr_eoi(cpsw->dma, CPDMA_EOI_TX);92 93 if (cpsw->quirk_irq) {94 disable_irq_nosync(cpsw->irqs_table[1]);95 cpsw->tx_irq_disabled = true;96 }97 98 napi_schedule(&cpsw->napi_tx);99 return IRQ_HANDLED;100}101 102irqreturn_t cpsw_rx_interrupt(int irq, void *dev_id)103{104 struct cpsw_common *cpsw = dev_id;105 106 writel(0, &cpsw->wr_regs->rx_en);107 cpdma_ctlr_eoi(cpsw->dma, CPDMA_EOI_RX);108 109 if (cpsw->quirk_irq) {110 disable_irq_nosync(cpsw->irqs_table[0]);111 cpsw->rx_irq_disabled = true;112 }113 114 napi_schedule(&cpsw->napi_rx);115 return IRQ_HANDLED;116}117 118irqreturn_t cpsw_misc_interrupt(int irq, void *dev_id)119{120 struct cpsw_common *cpsw = dev_id;121 122 writel(0, &cpsw->wr_regs->misc_en);123 cpdma_ctlr_eoi(cpsw->dma, CPDMA_EOI_MISC);124 cpts_misc_interrupt(cpsw->cpts);125 writel(0x10, &cpsw->wr_regs->misc_en);126 127 return IRQ_HANDLED;128}129 130int cpsw_tx_mq_poll(struct napi_struct *napi_tx, int budget)131{132 struct cpsw_common *cpsw = napi_to_cpsw(napi_tx);133 int num_tx, cur_budget, ch;134 u32 ch_map;135 struct cpsw_vector *txv;136 137 /* process every unprocessed channel */138 ch_map = cpdma_ctrl_txchs_state(cpsw->dma);139 for (ch = 0, num_tx = 0; ch_map & 0xff; ch_map <<= 1, ch++) {140 if (!(ch_map & 0x80))141 continue;142 143 txv = &cpsw->txv[ch];144 if (unlikely(txv->budget > budget - num_tx))145 cur_budget = budget - num_tx;146 else147 cur_budget = txv->budget;148 149 num_tx += cpdma_chan_process(txv->ch, cur_budget);150 if (num_tx >= budget)151 break;152 }153 154 if (num_tx < budget) {155 napi_complete(napi_tx);156 writel(0xff, &cpsw->wr_regs->tx_en);157 }158 159 return num_tx;160}161 162int cpsw_tx_poll(struct napi_struct *napi_tx, int budget)163{164 struct cpsw_common *cpsw = napi_to_cpsw(napi_tx);165 int num_tx;166 167 num_tx = cpdma_chan_process(cpsw->txv[0].ch, budget);168 if (num_tx < budget) {169 napi_complete(napi_tx);170 writel(0xff, &cpsw->wr_regs->tx_en);171 if (cpsw->tx_irq_disabled) {172 cpsw->tx_irq_disabled = false;173 enable_irq(cpsw->irqs_table[1]);174 }175 }176 177 return num_tx;178}179 180int cpsw_rx_mq_poll(struct napi_struct *napi_rx, int budget)181{182 struct cpsw_common *cpsw = napi_to_cpsw(napi_rx);183 int num_rx, cur_budget, ch;184 u32 ch_map;185 struct cpsw_vector *rxv;186 187 /* process every unprocessed channel */188 ch_map = cpdma_ctrl_rxchs_state(cpsw->dma);189 for (ch = 0, num_rx = 0; ch_map; ch_map >>= 1, ch++) {190 if (!(ch_map & 0x01))191 continue;192 193 rxv = &cpsw->rxv[ch];194 if (unlikely(rxv->budget > budget - num_rx))195 cur_budget = budget - num_rx;196 else197 cur_budget = rxv->budget;198 199 num_rx += cpdma_chan_process(rxv->ch, cur_budget);200 if (num_rx >= budget)201 break;202 }203 204 if (num_rx < budget) {205 napi_complete_done(napi_rx, num_rx);206 writel(0xff, &cpsw->wr_regs->rx_en);207 }208 209 return num_rx;210}211 212int cpsw_rx_poll(struct napi_struct *napi_rx, int budget)213{214 struct cpsw_common *cpsw = napi_to_cpsw(napi_rx);215 int num_rx;216 217 num_rx = cpdma_chan_process(cpsw->rxv[0].ch, budget);218 if (num_rx < budget) {219 napi_complete_done(napi_rx, num_rx);220 writel(0xff, &cpsw->wr_regs->rx_en);221 if (cpsw->rx_irq_disabled) {222 cpsw->rx_irq_disabled = false;223 enable_irq(cpsw->irqs_table[0]);224 }225 }226 227 return num_rx;228}229 230void cpsw_rx_vlan_encap(struct sk_buff *skb)231{232 struct cpsw_priv *priv = netdev_priv(skb->dev);233 u32 rx_vlan_encap_hdr = *((u32 *)skb->data);234 struct cpsw_common *cpsw = priv->cpsw;235 u16 vtag, vid, prio, pkt_type;236 237 /* Remove VLAN header encapsulation word */238 skb_pull(skb, CPSW_RX_VLAN_ENCAP_HDR_SIZE);239 240 pkt_type = (rx_vlan_encap_hdr >>241 CPSW_RX_VLAN_ENCAP_HDR_PKT_TYPE_SHIFT) &242 CPSW_RX_VLAN_ENCAP_HDR_PKT_TYPE_MSK;243 /* Ignore unknown & Priority-tagged packets*/244 if (pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_RESERV ||245 pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_PRIO_TAG)246 return;247 248 vid = (rx_vlan_encap_hdr >>249 CPSW_RX_VLAN_ENCAP_HDR_VID_SHIFT) &250 VLAN_VID_MASK;251 /* Ignore vid 0 and pass packet as is */252 if (!vid)253 return;254 255 /* Untag P0 packets if set for vlan */256 if (!cpsw_ale_get_vlan_p0_untag(cpsw->ale, vid)) {257 prio = (rx_vlan_encap_hdr >>258 CPSW_RX_VLAN_ENCAP_HDR_PRIO_SHIFT) &259 CPSW_RX_VLAN_ENCAP_HDR_PRIO_MSK;260 261 vtag = (prio << VLAN_PRIO_SHIFT) | vid;262 __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vtag);263 }264 265 /* strip vlan tag for VLAN-tagged packet */266 if (pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_VLAN_TAG) {267 memmove(skb->data + VLAN_HLEN, skb->data, 2 * ETH_ALEN);268 skb_pull(skb, VLAN_HLEN);269 }270}271 272void cpsw_set_slave_mac(struct cpsw_slave *slave, struct cpsw_priv *priv)273{274 slave_write(slave, mac_hi(priv->mac_addr), SA_HI);275 slave_write(slave, mac_lo(priv->mac_addr), SA_LO);276}277 278void soft_reset(const char *module, void __iomem *reg)279{280 unsigned long timeout = jiffies + HZ;281 282 writel_relaxed(1, reg);283 do {284 cpu_relax();285 } while ((readl_relaxed(reg) & 1) && time_after(timeout, jiffies));286 287 WARN(readl_relaxed(reg) & 1, "failed to soft-reset %s\n", module);288}289 290void cpsw_ndo_tx_timeout(struct net_device *ndev, unsigned int txqueue)291{292 struct cpsw_priv *priv = netdev_priv(ndev);293 struct cpsw_common *cpsw = priv->cpsw;294 int ch;295 296 cpsw_err(priv, tx_err, "transmit timeout, restarting dma\n");297 ndev->stats.tx_errors++;298 cpsw_intr_disable(cpsw);299 for (ch = 0; ch < cpsw->tx_ch_num; ch++) {300 cpdma_chan_stop(cpsw->txv[ch].ch);301 cpdma_chan_start(cpsw->txv[ch].ch);302 }303 304 cpsw_intr_enable(cpsw);305 netif_trans_update(ndev);306 netif_tx_wake_all_queues(ndev);307}308 309static int cpsw_get_common_speed(struct cpsw_common *cpsw)310{311 int i, speed;312 313 for (i = 0, speed = 0; i < cpsw->data.slaves; i++)314 if (cpsw->slaves[i].phy && cpsw->slaves[i].phy->link)315 speed += cpsw->slaves[i].phy->speed;316 317 return speed;318}319 320int cpsw_need_resplit(struct cpsw_common *cpsw)321{322 int i, rlim_ch_num;323 int speed, ch_rate;324 325 /* re-split resources only in case speed was changed */326 speed = cpsw_get_common_speed(cpsw);327 if (speed == cpsw->speed || !speed)328 return 0;329 330 cpsw->speed = speed;331 332 for (i = 0, rlim_ch_num = 0; i < cpsw->tx_ch_num; i++) {333 ch_rate = cpdma_chan_get_rate(cpsw->txv[i].ch);334 if (!ch_rate)335 break;336 337 rlim_ch_num++;338 }339 340 /* cases not dependent on speed */341 if (!rlim_ch_num || rlim_ch_num == cpsw->tx_ch_num)342 return 0;343 344 return 1;345}346 347void cpsw_split_res(struct cpsw_common *cpsw)348{349 u32 consumed_rate = 0, bigest_rate = 0;350 struct cpsw_vector *txv = cpsw->txv;351 int i, ch_weight, rlim_ch_num = 0;352 int budget, bigest_rate_ch = 0;353 u32 ch_rate, max_rate;354 int ch_budget = 0;355 356 for (i = 0; i < cpsw->tx_ch_num; i++) {357 ch_rate = cpdma_chan_get_rate(txv[i].ch);358 if (!ch_rate)359 continue;360 361 rlim_ch_num++;362 consumed_rate += ch_rate;363 }364 365 if (cpsw->tx_ch_num == rlim_ch_num) {366 max_rate = consumed_rate;367 } else if (!rlim_ch_num) {368 ch_budget = NAPI_POLL_WEIGHT / cpsw->tx_ch_num;369 bigest_rate = 0;370 max_rate = consumed_rate;371 } else {372 max_rate = cpsw->speed * 1000;373 374 /* if max_rate is less then expected due to reduced link speed,375 * split proportionally according next potential max speed376 */377 if (max_rate < consumed_rate)378 max_rate *= 10;379 380 if (max_rate < consumed_rate)381 max_rate *= 10;382 383 ch_budget = (consumed_rate * NAPI_POLL_WEIGHT) / max_rate;384 ch_budget = (NAPI_POLL_WEIGHT - ch_budget) /385 (cpsw->tx_ch_num - rlim_ch_num);386 bigest_rate = (max_rate - consumed_rate) /387 (cpsw->tx_ch_num - rlim_ch_num);388 }389 390 /* split tx weight/budget */391 budget = NAPI_POLL_WEIGHT;392 for (i = 0; i < cpsw->tx_ch_num; i++) {393 ch_rate = cpdma_chan_get_rate(txv[i].ch);394 if (ch_rate) {395 txv[i].budget = (ch_rate * NAPI_POLL_WEIGHT) / max_rate;396 if (!txv[i].budget)397 txv[i].budget++;398 if (ch_rate > bigest_rate) {399 bigest_rate_ch = i;400 bigest_rate = ch_rate;401 }402 403 ch_weight = (ch_rate * 100) / max_rate;404 if (!ch_weight)405 ch_weight++;406 cpdma_chan_set_weight(cpsw->txv[i].ch, ch_weight);407 } else {408 txv[i].budget = ch_budget;409 if (!bigest_rate_ch)410 bigest_rate_ch = i;411 cpdma_chan_set_weight(cpsw->txv[i].ch, 0);412 }413 414 budget -= txv[i].budget;415 }416 417 if (budget)418 txv[bigest_rate_ch].budget += budget;419 420 /* split rx budget */421 budget = NAPI_POLL_WEIGHT;422 ch_budget = budget / cpsw->rx_ch_num;423 for (i = 0; i < cpsw->rx_ch_num; i++) {424 cpsw->rxv[i].budget = ch_budget;425 budget -= ch_budget;426 }427 428 if (budget)429 cpsw->rxv[0].budget += budget;430}431 432int cpsw_init_common(struct cpsw_common *cpsw, void __iomem *ss_regs,433 int ale_ageout, phys_addr_t desc_mem_phys,434 int descs_pool_size)435{436 u32 slave_offset, sliver_offset, slave_size;437 struct cpsw_ale_params ale_params;438 struct cpsw_platform_data *data;439 struct cpdma_params dma_params;440 struct device *dev = cpsw->dev;441 struct device_node *cpts_node;442 void __iomem *cpts_regs;443 int ret = 0, i;444 445 data = &cpsw->data;446 cpsw->rx_ch_num = 1;447 cpsw->tx_ch_num = 1;448 449 cpsw->version = readl(&cpsw->regs->id_ver);450 451 memset(&dma_params, 0, sizeof(dma_params));452 memset(&ale_params, 0, sizeof(ale_params));453 454 switch (cpsw->version) {455 case CPSW_VERSION_1:456 cpsw->host_port_regs = ss_regs + CPSW1_HOST_PORT_OFFSET;457 cpts_regs = ss_regs + CPSW1_CPTS_OFFSET;458 cpsw->hw_stats = ss_regs + CPSW1_HW_STATS;459 dma_params.dmaregs = ss_regs + CPSW1_CPDMA_OFFSET;460 dma_params.txhdp = ss_regs + CPSW1_STATERAM_OFFSET;461 ale_params.ale_regs = ss_regs + CPSW1_ALE_OFFSET;462 slave_offset = CPSW1_SLAVE_OFFSET;463 slave_size = CPSW1_SLAVE_SIZE;464 sliver_offset = CPSW1_SLIVER_OFFSET;465 dma_params.desc_mem_phys = 0;466 break;467 case CPSW_VERSION_2:468 case CPSW_VERSION_3:469 case CPSW_VERSION_4:470 cpsw->host_port_regs = ss_regs + CPSW2_HOST_PORT_OFFSET;471 cpts_regs = ss_regs + CPSW2_CPTS_OFFSET;472 cpsw->hw_stats = ss_regs + CPSW2_HW_STATS;473 dma_params.dmaregs = ss_regs + CPSW2_CPDMA_OFFSET;474 dma_params.txhdp = ss_regs + CPSW2_STATERAM_OFFSET;475 ale_params.ale_regs = ss_regs + CPSW2_ALE_OFFSET;476 slave_offset = CPSW2_SLAVE_OFFSET;477 slave_size = CPSW2_SLAVE_SIZE;478 sliver_offset = CPSW2_SLIVER_OFFSET;479 dma_params.desc_mem_phys = desc_mem_phys;480 break;481 default:482 dev_err(dev, "unknown version 0x%08x\n", cpsw->version);483 return -ENODEV;484 }485 486 for (i = 0; i < cpsw->data.slaves; i++) {487 struct cpsw_slave *slave = &cpsw->slaves[i];488 void __iomem *regs = cpsw->regs;489 490 slave->slave_num = i;491 slave->data = &cpsw->data.slave_data[i];492 slave->regs = regs + slave_offset;493 slave->port_vlan = slave->data->dual_emac_res_vlan;494 slave->mac_sl = cpsw_sl_get("cpsw", dev, regs + sliver_offset);495 if (IS_ERR(slave->mac_sl))496 return PTR_ERR(slave->mac_sl);497 498 slave_offset += slave_size;499 sliver_offset += SLIVER_SIZE;500 }501 502 ale_params.dev = dev;503 ale_params.ale_ageout = ale_ageout;504 ale_params.ale_ports = CPSW_ALE_PORTS_NUM;505 ale_params.dev_id = "cpsw";506 ale_params.bus_freq = cpsw->bus_freq_mhz * 1000000;507 508 cpsw->ale = cpsw_ale_create(&ale_params);509 if (IS_ERR(cpsw->ale)) {510 dev_err(dev, "error initializing ale engine\n");511 return PTR_ERR(cpsw->ale);512 }513 514 dma_params.dev = dev;515 dma_params.rxthresh = dma_params.dmaregs + CPDMA_RXTHRESH;516 dma_params.rxfree = dma_params.dmaregs + CPDMA_RXFREE;517 dma_params.rxhdp = dma_params.txhdp + CPDMA_RXHDP;518 dma_params.txcp = dma_params.txhdp + CPDMA_TXCP;519 dma_params.rxcp = dma_params.txhdp + CPDMA_RXCP;520 521 dma_params.num_chan = data->channels;522 dma_params.has_soft_reset = true;523 dma_params.min_packet_size = CPSW_MIN_PACKET_SIZE;524 dma_params.desc_mem_size = data->bd_ram_size;525 dma_params.desc_align = 16;526 dma_params.has_ext_regs = true;527 dma_params.desc_hw_addr = dma_params.desc_mem_phys;528 dma_params.bus_freq_mhz = cpsw->bus_freq_mhz;529 dma_params.descs_pool_size = descs_pool_size;530 531 cpsw->dma = cpdma_ctlr_create(&dma_params);532 if (!cpsw->dma) {533 dev_err(dev, "error initializing dma\n");534 return -ENOMEM;535 }536 537 cpts_node = of_get_child_by_name(cpsw->dev->of_node, "cpts");538 if (!cpts_node)539 cpts_node = cpsw->dev->of_node;540 541 cpsw->cpts = cpts_create(cpsw->dev, cpts_regs, cpts_node,542 CPTS_N_ETX_TS);543 if (IS_ERR(cpsw->cpts)) {544 ret = PTR_ERR(cpsw->cpts);545 cpdma_ctlr_destroy(cpsw->dma);546 }547 of_node_put(cpts_node);548 549 return ret;550}551 552#if IS_ENABLED(CONFIG_TI_CPTS)553 554static void cpsw_hwtstamp_v1(struct cpsw_priv *priv)555{556 struct cpsw_common *cpsw = priv->cpsw;557 struct cpsw_slave *slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];558 u32 ts_en, seq_id;559 560 if (!priv->tx_ts_enabled && !priv->rx_ts_enabled) {561 slave_write(slave, 0, CPSW1_TS_CTL);562 return;563 }564 565 seq_id = (30 << CPSW_V1_SEQ_ID_OFS_SHIFT) | ETH_P_1588;566 ts_en = EVENT_MSG_BITS << CPSW_V1_MSG_TYPE_OFS;567 568 if (priv->tx_ts_enabled)569 ts_en |= CPSW_V1_TS_TX_EN;570 571 if (priv->rx_ts_enabled)572 ts_en |= CPSW_V1_TS_RX_EN;573 574 slave_write(slave, ts_en, CPSW1_TS_CTL);575 slave_write(slave, seq_id, CPSW1_TS_SEQ_LTYPE);576}577 578static void cpsw_hwtstamp_v2(struct cpsw_priv *priv)579{580 struct cpsw_common *cpsw = priv->cpsw;581 struct cpsw_slave *slave;582 u32 ctrl, mtype;583 584 slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];585 586 ctrl = slave_read(slave, CPSW2_CONTROL);587 switch (cpsw->version) {588 case CPSW_VERSION_2:589 ctrl &= ~CTRL_V2_ALL_TS_MASK;590 591 if (priv->tx_ts_enabled)592 ctrl |= CTRL_V2_TX_TS_BITS;593 594 if (priv->rx_ts_enabled)595 ctrl |= CTRL_V2_RX_TS_BITS;596 break;597 case CPSW_VERSION_3:598 default:599 ctrl &= ~CTRL_V3_ALL_TS_MASK;600 601 if (priv->tx_ts_enabled)602 ctrl |= CTRL_V3_TX_TS_BITS;603 604 if (priv->rx_ts_enabled)605 ctrl |= CTRL_V3_RX_TS_BITS;606 break;607 }608 609 mtype = (30 << TS_SEQ_ID_OFFSET_SHIFT) | EVENT_MSG_BITS;610 611 slave_write(slave, mtype, CPSW2_TS_SEQ_MTYPE);612 slave_write(slave, ctrl, CPSW2_CONTROL);613 writel_relaxed(ETH_P_1588, &cpsw->regs->ts_ltype);614 writel_relaxed(ETH_P_8021Q, &cpsw->regs->vlan_ltype);615}616 617static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)618{619 struct cpsw_priv *priv = netdev_priv(dev);620 struct cpsw_common *cpsw = priv->cpsw;621 struct hwtstamp_config cfg;622 623 if (cpsw->version != CPSW_VERSION_1 &&624 cpsw->version != CPSW_VERSION_2 &&625 cpsw->version != CPSW_VERSION_3)626 return -EOPNOTSUPP;627 628 if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))629 return -EFAULT;630 631 if (cfg.tx_type != HWTSTAMP_TX_OFF && cfg.tx_type != HWTSTAMP_TX_ON)632 return -ERANGE;633 634 switch (cfg.rx_filter) {635 case HWTSTAMP_FILTER_NONE:636 priv->rx_ts_enabled = 0;637 break;638 case HWTSTAMP_FILTER_ALL:639 case HWTSTAMP_FILTER_NTP_ALL:640 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:641 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:642 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:643 return -ERANGE;644 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:645 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:646 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:647 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:648 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:649 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:650 case HWTSTAMP_FILTER_PTP_V2_EVENT:651 case HWTSTAMP_FILTER_PTP_V2_SYNC:652 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:653 priv->rx_ts_enabled = HWTSTAMP_FILTER_PTP_V2_EVENT;654 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;655 break;656 default:657 return -ERANGE;658 }659 660 priv->tx_ts_enabled = cfg.tx_type == HWTSTAMP_TX_ON;661 662 switch (cpsw->version) {663 case CPSW_VERSION_1:664 cpsw_hwtstamp_v1(priv);665 break;666 case CPSW_VERSION_2:667 case CPSW_VERSION_3:668 cpsw_hwtstamp_v2(priv);669 break;670 default:671 WARN_ON(1);672 }673 674 return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;675}676 677static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)678{679 struct cpsw_common *cpsw = ndev_to_cpsw(dev);680 struct cpsw_priv *priv = netdev_priv(dev);681 struct hwtstamp_config cfg;682 683 if (cpsw->version != CPSW_VERSION_1 &&684 cpsw->version != CPSW_VERSION_2 &&685 cpsw->version != CPSW_VERSION_3)686 return -EOPNOTSUPP;687 688 cfg.flags = 0;689 cfg.tx_type = priv->tx_ts_enabled ? HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;690 cfg.rx_filter = priv->rx_ts_enabled;691 692 return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;693}694#else695static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)696{697 return -EOPNOTSUPP;698}699 700static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)701{702 return -EOPNOTSUPP;703}704#endif /*CONFIG_TI_CPTS*/705 706int cpsw_ndo_ioctl(struct net_device *dev, struct ifreq *req, int cmd)707{708 struct cpsw_priv *priv = netdev_priv(dev);709 struct cpsw_common *cpsw = priv->cpsw;710 int slave_no = cpsw_slave_index(cpsw, priv);711 struct phy_device *phy;712 713 if (!netif_running(dev))714 return -EINVAL;715 716 phy = cpsw->slaves[slave_no].phy;717 718 if (!phy_has_hwtstamp(phy)) {719 switch (cmd) {720 case SIOCSHWTSTAMP:721 return cpsw_hwtstamp_set(dev, req);722 case SIOCGHWTSTAMP:723 return cpsw_hwtstamp_get(dev, req);724 }725 }726 727 if (phy)728 return phy_mii_ioctl(phy, req, cmd);729 730 return -EOPNOTSUPP;731}732 733int cpsw_ndo_set_tx_maxrate(struct net_device *ndev, int queue, u32 rate)734{735 struct cpsw_priv *priv = netdev_priv(ndev);736 struct cpsw_common *cpsw = priv->cpsw;737 struct cpsw_slave *slave;738 u32 min_rate;739 u32 ch_rate;740 int i, ret;741 742 ch_rate = netdev_get_tx_queue(ndev, queue)->tx_maxrate;743 if (ch_rate == rate)744 return 0;745 746 ch_rate = rate * 1000;747 min_rate = cpdma_chan_get_min_rate(cpsw->dma);748 if ((ch_rate < min_rate && ch_rate)) {749 dev_err(priv->dev, "The channel rate cannot be less than %dMbps",750 min_rate);751 return -EINVAL;752 }753 754 if (rate > cpsw->speed) {755 dev_err(priv->dev, "The channel rate cannot be more than 2Gbps");756 return -EINVAL;757 }758 759 ret = pm_runtime_resume_and_get(cpsw->dev);760 if (ret < 0)761 return ret;762 763 ret = cpdma_chan_set_rate(cpsw->txv[queue].ch, ch_rate);764 pm_runtime_put(cpsw->dev);765 766 if (ret)767 return ret;768 769 /* update rates for slaves tx queues */770 for (i = 0; i < cpsw->data.slaves; i++) {771 slave = &cpsw->slaves[i];772 if (!slave->ndev)773 continue;774 775 netdev_get_tx_queue(slave->ndev, queue)->tx_maxrate = rate;776 }777 778 cpsw_split_res(cpsw);779 return ret;780}781 782static int cpsw_tc_to_fifo(int tc, int num_tc)783{784 if (tc == num_tc - 1)785 return 0;786 787 return CPSW_FIFO_SHAPERS_NUM - tc;788}789 790bool cpsw_shp_is_off(struct cpsw_priv *priv)791{792 struct cpsw_common *cpsw = priv->cpsw;793 struct cpsw_slave *slave;794 u32 shift, mask, val;795 796 val = readl_relaxed(&cpsw->regs->ptype);797 798 slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];799 shift = CPSW_FIFO_SHAPE_EN_SHIFT + 3 * slave->slave_num;800 mask = 7 << shift;801 val = val & mask;802 803 return !val;804}805 806static void cpsw_fifo_shp_on(struct cpsw_priv *priv, int fifo, int on)807{808 struct cpsw_common *cpsw = priv->cpsw;809 struct cpsw_slave *slave;810 u32 shift, mask, val;811 812 val = readl_relaxed(&cpsw->regs->ptype);813 814 slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];815 shift = CPSW_FIFO_SHAPE_EN_SHIFT + 3 * slave->slave_num;816 mask = (1 << --fifo) << shift;817 val = on ? val | mask : val & ~mask;818 819 writel_relaxed(val, &cpsw->regs->ptype);820}821 822static int cpsw_set_fifo_bw(struct cpsw_priv *priv, int fifo, int bw)823{824 struct cpsw_common *cpsw = priv->cpsw;825 u32 val = 0, send_pct, shift;826 struct cpsw_slave *slave;827 int pct = 0, i;828 829 if (bw > priv->shp_cfg_speed * 1000)830 goto err;831 832 /* shaping has to stay enabled for highest fifos linearly833 * and fifo bw no more then interface can allow834 */835 slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];836 send_pct = slave_read(slave, SEND_PERCENT);837 for (i = CPSW_FIFO_SHAPERS_NUM; i > 0; i--) {838 if (!bw) {839 if (i >= fifo || !priv->fifo_bw[i])840 continue;841 842 dev_warn(priv->dev, "Prev FIFO%d is shaped", i);843 continue;844 }845 846 if (!priv->fifo_bw[i] && i > fifo) {847 dev_err(priv->dev, "Upper FIFO%d is not shaped", i);848 return -EINVAL;849 }850 851 shift = (i - 1) * 8;852 if (i == fifo) {853 send_pct &= ~(CPSW_PCT_MASK << shift);854 val = DIV_ROUND_UP(bw, priv->shp_cfg_speed * 10);855 if (!val)856 val = 1;857 858 send_pct |= val << shift;859 pct += val;860 continue;861 }862 863 if (priv->fifo_bw[i])864 pct += (send_pct >> shift) & CPSW_PCT_MASK;865 }866 867 if (pct >= 100)868 goto err;869 870 slave_write(slave, send_pct, SEND_PERCENT);871 priv->fifo_bw[fifo] = bw;872 873 dev_warn(priv->dev, "set FIFO%d bw = %d\n", fifo,874 DIV_ROUND_CLOSEST(val * priv->shp_cfg_speed, 100));875 876 return 0;877err:878 dev_err(priv->dev, "Bandwidth doesn't fit in tc configuration");879 return -EINVAL;880}881 882static int cpsw_set_fifo_rlimit(struct cpsw_priv *priv, int fifo, int bw)883{884 struct cpsw_common *cpsw = priv->cpsw;885 struct cpsw_slave *slave;886 u32 tx_in_ctl_rg, val;887 int ret;888 889 ret = cpsw_set_fifo_bw(priv, fifo, bw);890 if (ret)891 return ret;892 893 slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];894 tx_in_ctl_rg = cpsw->version == CPSW_VERSION_1 ?895 CPSW1_TX_IN_CTL : CPSW2_TX_IN_CTL;896 897 if (!bw)898 cpsw_fifo_shp_on(priv, fifo, bw);899 900 val = slave_read(slave, tx_in_ctl_rg);901 if (cpsw_shp_is_off(priv)) {902 /* disable FIFOs rate limited queues */903 val &= ~(0xf << CPSW_FIFO_RATE_EN_SHIFT);904 905 /* set type of FIFO queues to normal priority mode */906 val &= ~(3 << CPSW_FIFO_QUEUE_TYPE_SHIFT);907 908 /* set type of FIFO queues to be rate limited */909 if (bw)910 val |= 2 << CPSW_FIFO_QUEUE_TYPE_SHIFT;911 else912 priv->shp_cfg_speed = 0;913 }914 915 /* toggle a FIFO rate limited queue */916 if (bw)917 val |= BIT(fifo + CPSW_FIFO_RATE_EN_SHIFT);918 else919 val &= ~BIT(fifo + CPSW_FIFO_RATE_EN_SHIFT);920 slave_write(slave, val, tx_in_ctl_rg);921 922 /* FIFO transmit shape enable */923 cpsw_fifo_shp_on(priv, fifo, bw);924 return 0;925}926 927/* Defaults:928 * class A - prio 3929 * class B - prio 2930 * shaping for class A should be set first931 */932static int cpsw_set_cbs(struct net_device *ndev,933 struct tc_cbs_qopt_offload *qopt)934{935 struct cpsw_priv *priv = netdev_priv(ndev);936 struct cpsw_common *cpsw = priv->cpsw;937 struct cpsw_slave *slave;938 int prev_speed = 0;939 int tc, ret, fifo;940 u32 bw = 0;941 942 tc = netdev_txq_to_tc(priv->ndev, qopt->queue);943 944 /* enable channels in backward order, as highest FIFOs must be rate945 * limited first and for compliance with CPDMA rate limited channels946 * that also used in bacward order. FIFO0 cannot be rate limited.947 */948 fifo = cpsw_tc_to_fifo(tc, ndev->num_tc);949 if (!fifo) {950 dev_err(priv->dev, "Last tc%d can't be rate limited", tc);951 return -EINVAL;952 }953 954 /* do nothing, it's disabled anyway */955 if (!qopt->enable && !priv->fifo_bw[fifo])956 return 0;957 958 /* shapers can be set if link speed is known */959 slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];960 if (slave->phy && slave->phy->link) {961 if (priv->shp_cfg_speed &&962 priv->shp_cfg_speed != slave->phy->speed)963 prev_speed = priv->shp_cfg_speed;964 965 priv->shp_cfg_speed = slave->phy->speed;966 }967 968 if (!priv->shp_cfg_speed) {969 dev_err(priv->dev, "Link speed is not known");970 return -1;971 }972 973 ret = pm_runtime_resume_and_get(cpsw->dev);974 if (ret < 0)975 return ret;976 977 bw = qopt->enable ? qopt->idleslope : 0;978 ret = cpsw_set_fifo_rlimit(priv, fifo, bw);979 if (ret) {980 priv->shp_cfg_speed = prev_speed;981 prev_speed = 0;982 }983 984 if (bw && prev_speed)985 dev_warn(priv->dev,986 "Speed was changed, CBS shaper speeds are changed!");987 988 pm_runtime_put_sync(cpsw->dev);989 return ret;990}991 992static int cpsw_set_mqprio(struct net_device *ndev, void *type_data)993{994 struct tc_mqprio_qopt_offload *mqprio = type_data;995 struct cpsw_priv *priv = netdev_priv(ndev);996 struct cpsw_common *cpsw = priv->cpsw;997 int fifo, num_tc, count, offset;998 struct cpsw_slave *slave;999 u32 tx_prio_map = 0;1000 int i, tc, ret;1001 1002 num_tc = mqprio->qopt.num_tc;1003 if (num_tc > CPSW_TC_NUM)1004 return -EINVAL;1005 1006 if (mqprio->mode != TC_MQPRIO_MODE_DCB)1007 return -EINVAL;1008 1009 ret = pm_runtime_resume_and_get(cpsw->dev);1010 if (ret < 0)1011 return ret;1012 1013 if (num_tc) {1014 for (i = 0; i < 8; i++) {1015 tc = mqprio->qopt.prio_tc_map[i];1016 fifo = cpsw_tc_to_fifo(tc, num_tc);1017 tx_prio_map |= fifo << (4 * i);1018 }1019 1020 netdev_set_num_tc(ndev, num_tc);1021 for (i = 0; i < num_tc; i++) {1022 count = mqprio->qopt.count[i];1023 offset = mqprio->qopt.offset[i];1024 netdev_set_tc_queue(ndev, i, count, offset);1025 }1026 }1027 1028 if (!mqprio->qopt.hw) {1029 /* restore default configuration */1030 netdev_reset_tc(ndev);1031 tx_prio_map = TX_PRIORITY_MAPPING;1032 }1033 1034 priv->mqprio_hw = mqprio->qopt.hw;1035 1036 offset = cpsw->version == CPSW_VERSION_1 ?1037 CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP;1038 1039 slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];1040 slave_write(slave, tx_prio_map, offset);1041 1042 pm_runtime_put_sync(cpsw->dev);1043 1044 return 0;1045}1046 1047static int cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f);1048 1049int cpsw_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type,1050 void *type_data)1051{1052 switch (type) {1053 case TC_SETUP_QDISC_CBS:1054 return cpsw_set_cbs(ndev, type_data);1055 1056 case TC_SETUP_QDISC_MQPRIO:1057 return cpsw_set_mqprio(ndev, type_data);1058 1059 case TC_SETUP_BLOCK:1060 return cpsw_qos_setup_tc_block(ndev, type_data);1061 1062 default:1063 return -EOPNOTSUPP;1064 }1065}1066 1067void cpsw_cbs_resume(struct cpsw_slave *slave, struct cpsw_priv *priv)1068{1069 int fifo, bw;1070 1071 for (fifo = CPSW_FIFO_SHAPERS_NUM; fifo > 0; fifo--) {1072 bw = priv->fifo_bw[fifo];1073 if (!bw)1074 continue;1075 1076 cpsw_set_fifo_rlimit(priv, fifo, bw);1077 }1078}1079 1080void cpsw_mqprio_resume(struct cpsw_slave *slave, struct cpsw_priv *priv)1081{1082 struct cpsw_common *cpsw = priv->cpsw;1083 u32 tx_prio_map = 0;1084 int i, tc, fifo;1085 u32 tx_prio_rg;1086 1087 if (!priv->mqprio_hw)1088 return;1089 1090 for (i = 0; i < 8; i++) {1091 tc = netdev_get_prio_tc_map(priv->ndev, i);1092 fifo = CPSW_FIFO_SHAPERS_NUM - tc;1093 tx_prio_map |= fifo << (4 * i);1094 }1095 1096 tx_prio_rg = cpsw->version == CPSW_VERSION_1 ?1097 CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP;1098 1099 slave_write(slave, tx_prio_map, tx_prio_rg);1100}1101 1102int cpsw_fill_rx_channels(struct cpsw_priv *priv)1103{1104 struct cpsw_common *cpsw = priv->cpsw;1105 struct cpsw_meta_xdp *xmeta;1106 struct page_pool *pool;1107 struct page *page;1108 int ch_buf_num;1109 int ch, i, ret;1110 dma_addr_t dma;1111 1112 for (ch = 0; ch < cpsw->rx_ch_num; ch++) {1113 pool = cpsw->page_pool[ch];1114 ch_buf_num = cpdma_chan_get_rx_buf_num(cpsw->rxv[ch].ch);1115 for (i = 0; i < ch_buf_num; i++) {1116 page = page_pool_dev_alloc_pages(pool);1117 if (!page) {1118 cpsw_err(priv, ifup, "allocate rx page err\n");1119 return -ENOMEM;1120 }1121 1122 xmeta = page_address(page) + CPSW_XMETA_OFFSET;1123 xmeta->ndev = priv->ndev;1124 xmeta->ch = ch;1125 1126 dma = page_pool_get_dma_addr(page) + CPSW_HEADROOM_NA;1127 ret = cpdma_chan_idle_submit_mapped(cpsw->rxv[ch].ch,1128 page, dma,1129 cpsw->rx_packet_max,1130 0);1131 if (ret < 0) {1132 cpsw_err(priv, ifup,1133 "cannot submit page to channel %d rx, error %d\n",1134 ch, ret);1135 page_pool_recycle_direct(pool, page);1136 return ret;1137 }1138 }1139 1140 cpsw_info(priv, ifup, "ch %d rx, submitted %d descriptors\n",1141 ch, ch_buf_num);1142 }1143 1144 return 0;1145}1146 1147static struct page_pool *cpsw_create_page_pool(struct cpsw_common *cpsw,1148 int size)1149{1150 struct page_pool_params pp_params = {};1151 struct page_pool *pool;1152 1153 pp_params.order = 0;1154 pp_params.flags = PP_FLAG_DMA_MAP;1155 pp_params.pool_size = size;1156 pp_params.nid = NUMA_NO_NODE;1157 pp_params.dma_dir = DMA_BIDIRECTIONAL;1158 pp_params.dev = cpsw->dev;1159 1160 pool = page_pool_create(&pp_params);1161 if (IS_ERR(pool))1162 dev_err(cpsw->dev, "cannot create rx page pool\n");1163 1164 return pool;1165}1166 1167static int cpsw_create_rx_pool(struct cpsw_common *cpsw, int ch)1168{1169 struct page_pool *pool;1170 int ret = 0, pool_size;1171 1172 pool_size = cpdma_chan_get_rx_buf_num(cpsw->rxv[ch].ch);1173 pool = cpsw_create_page_pool(cpsw, pool_size);1174 if (IS_ERR(pool))1175 ret = PTR_ERR(pool);1176 else1177 cpsw->page_pool[ch] = pool;1178 1179 return ret;1180}1181 1182static int cpsw_ndev_create_xdp_rxq(struct cpsw_priv *priv, int ch)1183{1184 struct cpsw_common *cpsw = priv->cpsw;1185 struct xdp_rxq_info *rxq;1186 struct page_pool *pool;1187 int ret;1188 1189 pool = cpsw->page_pool[ch];1190 rxq = &priv->xdp_rxq[ch];1191 1192 ret = xdp_rxq_info_reg(rxq, priv->ndev, ch, 0);1193 if (ret)1194 return ret;1195 1196 ret = xdp_rxq_info_reg_mem_model(rxq, MEM_TYPE_PAGE_POOL, pool);1197 if (ret)1198 xdp_rxq_info_unreg(rxq);1199 1200 return ret;1201}1202 1203static void cpsw_ndev_destroy_xdp_rxq(struct cpsw_priv *priv, int ch)1204{1205 struct xdp_rxq_info *rxq = &priv->xdp_rxq[ch];1206 1207 if (!xdp_rxq_info_is_reg(rxq))1208 return;1209 1210 xdp_rxq_info_unreg(rxq);1211}1212 1213void cpsw_destroy_xdp_rxqs(struct cpsw_common *cpsw)1214{1215 struct net_device *ndev;1216 int i, ch;1217 1218 for (ch = 0; ch < cpsw->rx_ch_num; ch++) {1219 for (i = 0; i < cpsw->data.slaves; i++) {1220 ndev = cpsw->slaves[i].ndev;1221 if (!ndev)1222 continue;1223 1224 cpsw_ndev_destroy_xdp_rxq(netdev_priv(ndev), ch);1225 }1226 1227 page_pool_destroy(cpsw->page_pool[ch]);1228 cpsw->page_pool[ch] = NULL;1229 }1230}1231 1232int cpsw_create_xdp_rxqs(struct cpsw_common *cpsw)1233{1234 struct net_device *ndev;1235 int i, ch, ret;1236 1237 for (ch = 0; ch < cpsw->rx_ch_num; ch++) {1238 ret = cpsw_create_rx_pool(cpsw, ch);1239 if (ret)1240 goto err_cleanup;1241 1242 /* using same page pool is allowed as no running rx handlers1243 * simultaneously for both ndevs1244 */1245 for (i = 0; i < cpsw->data.slaves; i++) {1246 ndev = cpsw->slaves[i].ndev;1247 if (!ndev)1248 continue;1249 1250 ret = cpsw_ndev_create_xdp_rxq(netdev_priv(ndev), ch);1251 if (ret)1252 goto err_cleanup;1253 }1254 }1255 1256 return 0;1257 1258err_cleanup:1259 cpsw_destroy_xdp_rxqs(cpsw);1260 1261 return ret;1262}1263 1264static int cpsw_xdp_prog_setup(struct cpsw_priv *priv, struct netdev_bpf *bpf)1265{1266 struct bpf_prog *prog = bpf->prog;1267 1268 if (!priv->xdpi.prog && !prog)1269 return 0;1270 1271 WRITE_ONCE(priv->xdp_prog, prog);1272 1273 xdp_attachment_setup(&priv->xdpi, bpf);1274 1275 return 0;1276}1277 1278int cpsw_ndo_bpf(struct net_device *ndev, struct netdev_bpf *bpf)1279{1280 struct cpsw_priv *priv = netdev_priv(ndev);1281 1282 switch (bpf->command) {1283 case XDP_SETUP_PROG:1284 return cpsw_xdp_prog_setup(priv, bpf);1285 1286 default:1287 return -EINVAL;1288 }1289}1290 1291int cpsw_xdp_tx_frame(struct cpsw_priv *priv, struct xdp_frame *xdpf,1292 struct page *page, int port)1293{1294 struct cpsw_common *cpsw = priv->cpsw;1295 struct cpsw_meta_xdp *xmeta;1296 struct cpdma_chan *txch;1297 dma_addr_t dma;1298 int ret;1299 1300 xmeta = (void *)xdpf + CPSW_XMETA_OFFSET;1301 xmeta->ndev = priv->ndev;1302 xmeta->ch = 0;1303 txch = cpsw->txv[0].ch;1304 1305 if (page) {1306 dma = page_pool_get_dma_addr(page);1307 dma += xdpf->headroom + sizeof(struct xdp_frame);1308 ret = cpdma_chan_submit_mapped(txch, cpsw_xdpf_to_handle(xdpf),1309 dma, xdpf->len, port);1310 } else {1311 if (sizeof(*xmeta) > xdpf->headroom)1312 return -EINVAL;1313 1314 ret = cpdma_chan_submit(txch, cpsw_xdpf_to_handle(xdpf),1315 xdpf->data, xdpf->len, port);1316 }1317 1318 if (ret)1319 priv->ndev->stats.tx_dropped++;1320 1321 return ret;1322}1323 1324int cpsw_run_xdp(struct cpsw_priv *priv, int ch, struct xdp_buff *xdp,1325 struct page *page, int port, int *len)1326{1327 struct cpsw_common *cpsw = priv->cpsw;1328 struct net_device *ndev = priv->ndev;1329 int ret = CPSW_XDP_CONSUMED;1330 struct xdp_frame *xdpf;1331 struct bpf_prog *prog;1332 u32 act;1333 1334 prog = READ_ONCE(priv->xdp_prog);1335 if (!prog)1336 return CPSW_XDP_PASS;1337 1338 act = bpf_prog_run_xdp(prog, xdp);1339 /* XDP prog might have changed packet data and boundaries */1340 *len = xdp->data_end - xdp->data;1341 1342 switch (act) {1343 case XDP_PASS:1344 ret = CPSW_XDP_PASS;1345 goto out;1346 case XDP_TX:1347 xdpf = xdp_convert_buff_to_frame(xdp);1348 if (unlikely(!xdpf))1349 goto drop;1350 1351 if (cpsw_xdp_tx_frame(priv, xdpf, page, port))1352 xdp_return_frame_rx_napi(xdpf);1353 break;1354 case XDP_REDIRECT:1355 if (xdp_do_redirect(ndev, xdp, prog))1356 goto drop;1357 1358 /* Have to flush here, per packet, instead of doing it in bulk1359 * at the end of the napi handler. The RX devices on this1360 * particular hardware is sharing a common queue, so the1361 * incoming device might change per packet.1362 */1363 xdp_do_flush();1364 break;1365 default:1366 bpf_warn_invalid_xdp_action(ndev, prog, act);1367 fallthrough;1368 case XDP_ABORTED:1369 trace_xdp_exception(ndev, prog, act);1370 fallthrough; /* handle aborts by dropping packet */1371 case XDP_DROP:1372 ndev->stats.rx_bytes += *len;1373 ndev->stats.rx_packets++;1374 goto drop;1375 }1376 1377 ndev->stats.rx_bytes += *len;1378 ndev->stats.rx_packets++;1379out:1380 return ret;1381drop:1382 page_pool_recycle_direct(cpsw->page_pool[ch], page);1383 return ret;1384}1385 1386static int cpsw_qos_clsflower_add_policer(struct cpsw_priv *priv,1387 struct netlink_ext_ack *extack,1388 struct flow_cls_offload *cls,1389 u64 rate_pkt_ps)1390{1391 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);1392 struct flow_dissector *dissector = rule->match.dissector;1393 static const u8 mc_mac[] = {0x01, 0x00, 0x00, 0x00, 0x00, 0x00};1394 struct flow_match_eth_addrs match;1395 u32 port_id;1396 int ret;1397 1398 if (dissector->used_keys &1399 ~(BIT_ULL(FLOW_DISSECTOR_KEY_BASIC) |1400 BIT_ULL(FLOW_DISSECTOR_KEY_CONTROL) |1401 BIT_ULL(FLOW_DISSECTOR_KEY_ETH_ADDRS))) {1402 NL_SET_ERR_MSG_MOD(extack,1403 "Unsupported keys used");1404 return -EOPNOTSUPP;1405 }1406 1407 if (flow_rule_match_has_control_flags(rule, extack))1408 return -EOPNOTSUPP;1409 1410 if (!flow_rule_match_key(rule, FLOW_DISSECTOR_KEY_ETH_ADDRS)) {1411 NL_SET_ERR_MSG_MOD(extack, "Not matching on eth address");1412 return -EOPNOTSUPP;1413 }1414 1415 flow_rule_match_eth_addrs(rule, &match);1416 1417 if (!is_zero_ether_addr(match.mask->src)) {1418 NL_SET_ERR_MSG_MOD(extack,1419 "Matching on source MAC not supported");1420 return -EOPNOTSUPP;1421 }1422 1423 port_id = cpsw_slave_index(priv->cpsw, priv) + 1;1424 1425 if (is_broadcast_ether_addr(match.key->dst) &&1426 is_broadcast_ether_addr(match.mask->dst)) {1427 ret = cpsw_ale_rx_ratelimit_bc(priv->cpsw->ale, port_id, rate_pkt_ps);1428 if (ret)1429 return ret;1430 1431 priv->ale_bc_ratelimit.cookie = cls->cookie;1432 priv->ale_bc_ratelimit.rate_packet_ps = rate_pkt_ps;1433 } else if (ether_addr_equal_unaligned(match.key->dst, mc_mac) &&1434 ether_addr_equal_unaligned(match.mask->dst, mc_mac)) {1435 ret = cpsw_ale_rx_ratelimit_mc(priv->cpsw->ale, port_id, rate_pkt_ps);1436 if (ret)1437 return ret;1438 1439 priv->ale_mc_ratelimit.cookie = cls->cookie;1440 priv->ale_mc_ratelimit.rate_packet_ps = rate_pkt_ps;1441 } else {1442 NL_SET_ERR_MSG_MOD(extack, "Not supported matching key");1443 return -EOPNOTSUPP;1444 }1445 1446 return 0;1447}1448 1449static int cpsw_qos_clsflower_policer_validate(const struct flow_action *action,1450 const struct flow_action_entry *act,1451 struct netlink_ext_ack *extack)1452{1453 if (act->police.exceed.act_id != FLOW_ACTION_DROP) {1454 NL_SET_ERR_MSG_MOD(extack,1455 "Offload not supported when exceed action is not drop");1456 return -EOPNOTSUPP;1457 }1458 1459 if (act->police.notexceed.act_id != FLOW_ACTION_PIPE &&1460 act->police.notexceed.act_id != FLOW_ACTION_ACCEPT) {1461 NL_SET_ERR_MSG_MOD(extack,1462 "Offload not supported when conform action is not pipe or ok");1463 return -EOPNOTSUPP;1464 }1465 1466 if (act->police.notexceed.act_id == FLOW_ACTION_ACCEPT &&1467 !flow_action_is_last_entry(action, act)) {1468 NL_SET_ERR_MSG_MOD(extack,1469 "Offload not supported when conform action is ok, but action is not last");1470 return -EOPNOTSUPP;1471 }1472 1473 if (act->police.rate_bytes_ps || act->police.peakrate_bytes_ps ||1474 act->police.avrate || act->police.overhead) {1475 NL_SET_ERR_MSG_MOD(extack,1476 "Offload not supported when bytes per second/peakrate/avrate/overhead is configured");1477 return -EOPNOTSUPP;1478 }1479 1480 return 0;1481}1482 1483static int cpsw_qos_configure_clsflower(struct cpsw_priv *priv, struct flow_cls_offload *cls)1484{1485 struct flow_rule *rule = flow_cls_offload_flow_rule(cls);1486 struct netlink_ext_ack *extack = cls->common.extack;1487 const struct flow_action_entry *act;1488 int i, ret;1489 1490 flow_action_for_each(i, act, &rule->action) {1491 switch (act->id) {1492 case FLOW_ACTION_POLICE:1493 ret = cpsw_qos_clsflower_policer_validate(&rule->action, act, extack);1494 if (ret)1495 return ret;1496 1497 return cpsw_qos_clsflower_add_policer(priv, extack, cls,1498 act->police.rate_pkt_ps);1499 default:1500 NL_SET_ERR_MSG_MOD(extack, "Action not supported");1501 return -EOPNOTSUPP;1502 }1503 }1504 return -EOPNOTSUPP;1505}1506 1507static int cpsw_qos_delete_clsflower(struct cpsw_priv *priv, struct flow_cls_offload *cls)1508{1509 u32 port_id = cpsw_slave_index(priv->cpsw, priv) + 1;1510 1511 if (cls->cookie == priv->ale_bc_ratelimit.cookie) {1512 priv->ale_bc_ratelimit.cookie = 0;1513 priv->ale_bc_ratelimit.rate_packet_ps = 0;1514 cpsw_ale_rx_ratelimit_bc(priv->cpsw->ale, port_id, 0);1515 }1516 1517 if (cls->cookie == priv->ale_mc_ratelimit.cookie) {1518 priv->ale_mc_ratelimit.cookie = 0;1519 priv->ale_mc_ratelimit.rate_packet_ps = 0;1520 cpsw_ale_rx_ratelimit_mc(priv->cpsw->ale, port_id, 0);1521 }1522 1523 return 0;1524}1525 1526static int cpsw_qos_setup_tc_clsflower(struct cpsw_priv *priv, struct flow_cls_offload *cls_flower)1527{1528 switch (cls_flower->command) {1529 case FLOW_CLS_REPLACE:1530 return cpsw_qos_configure_clsflower(priv, cls_flower);1531 case FLOW_CLS_DESTROY:1532 return cpsw_qos_delete_clsflower(priv, cls_flower);1533 default:1534 return -EOPNOTSUPP;1535 }1536}1537 1538static int cpsw_qos_setup_tc_block_cb(enum tc_setup_type type, void *type_data, void *cb_priv)1539{1540 struct cpsw_priv *priv = cb_priv;1541 int ret;1542 1543 if (!tc_cls_can_offload_and_chain0(priv->ndev, type_data))1544 return -EOPNOTSUPP;1545 1546 ret = pm_runtime_get_sync(priv->dev);1547 if (ret < 0) {1548 pm_runtime_put_noidle(priv->dev);1549 return ret;1550 }1551 1552 switch (type) {1553 case TC_SETUP_CLSFLOWER:1554 ret = cpsw_qos_setup_tc_clsflower(priv, type_data);1555 break;1556 default:1557 ret = -EOPNOTSUPP;1558 }1559 1560 pm_runtime_put(priv->dev);1561 return ret;1562}1563 1564static LIST_HEAD(cpsw_qos_block_cb_list);1565 1566static int cpsw_qos_setup_tc_block(struct net_device *ndev, struct flow_block_offload *f)1567{1568 struct cpsw_priv *priv = netdev_priv(ndev);1569 1570 return flow_block_cb_setup_simple(f, &cpsw_qos_block_cb_list,1571 cpsw_qos_setup_tc_block_cb,1572 priv, priv, true);1573}1574 1575void cpsw_qos_clsflower_resume(struct cpsw_priv *priv)1576{1577 u32 port_id = cpsw_slave_index(priv->cpsw, priv) + 1;1578 1579 if (priv->ale_bc_ratelimit.cookie)1580 cpsw_ale_rx_ratelimit_bc(priv->cpsw->ale, port_id,1581 priv->ale_bc_ratelimit.rate_packet_ps);1582 1583 if (priv->ale_mc_ratelimit.cookie)1584 cpsw_ale_rx_ratelimit_mc(priv->cpsw->ale, port_id,1585 priv->ale_mc_ratelimit.rate_packet_ps);1586}1587