drivers/net/ethernet/ti/cpsw_priv.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Texas Instruments Ethernet Switch Driver
   4  *
   5  * Copyright (C) 2019 Texas Instruments
   6  */
   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.h>
  22 #include <net/pkt_cls.h>
  23
  24 #include "cpsw.h"
  25 #include "cpts.h"
  26 #include "cpsw_ale.h"
  27 #include "cpsw_priv.h"
  28 #include "cpsw_sl.h"
  29 #include "davinci_cpdma.h"
  30
  31 #define CPTS_N_ETX_TS 4
  32
  33 int (*cpsw_slave_index)(struct cpsw_common *cpsw, struct cpsw_priv *priv);
  34
  35 void cpsw_intr_enable(struct cpsw_common *cpsw)
  36 {
  37         writel_relaxed(0xFF, &cpsw->wr_regs->tx_en);
  38         writel_relaxed(0xFF, &cpsw->wr_regs->rx_en);
  39
  40         cpdma_ctlr_int_ctrl(cpsw->dma, true);
  41 }
  42
  43 void cpsw_intr_disable(struct cpsw_common *cpsw)
  44 {
  45         writel_relaxed(0, &cpsw->wr_regs->tx_en);
  46         writel_relaxed(0, &cpsw->wr_regs->rx_en);
  47
  48         cpdma_ctlr_int_ctrl(cpsw->dma, false);
  49 }
  50
  51 void cpsw_tx_handler(void *token, int len, int status)
  52 {
  53         struct cpsw_meta_xdp    *xmeta;
  54         struct xdp_frame        *xdpf;
  55         struct net_device       *ndev;
  56         struct netdev_queue     *txq;
  57         struct sk_buff          *skb;
  58         int                     ch;
  59
  60         if (cpsw_is_xdpf_handle(token)) {
  61                 xdpf = cpsw_handle_to_xdpf(token);
  62                 xmeta = (void *)xdpf + CPSW_XMETA_OFFSET;
  63                 ndev = xmeta->ndev;
  64                 ch = xmeta->ch;
  65                 xdp_return_frame(xdpf);
  66         } else {
  67                 skb = token;
  68                 ndev = skb->dev;
  69                 ch = skb_get_queue_mapping(skb);
  70                 cpts_tx_timestamp(ndev_to_cpsw(ndev)->cpts, skb);
  71                 dev_kfree_skb_any(skb);
  72         }
  73
  74         /* Check whether the queue is stopped due to stalled tx dma, if the
  75          * queue is stopped then start the queue as we have free desc for tx
  76          */
  77         txq = netdev_get_tx_queue(ndev, ch);
  78         if (unlikely(netif_tx_queue_stopped(txq)))
  79                 netif_tx_wake_queue(txq);
  80
  81         ndev->stats.tx_packets++;
  82         ndev->stats.tx_bytes += len;
  83 }
  84
  85 irqreturn_t cpsw_tx_interrupt(int irq, void *dev_id)
  86 {
  87         struct cpsw_common *cpsw = dev_id;
  88
  89         writel(0, &cpsw->wr_regs->tx_en);
  90         cpdma_ctlr_eoi(cpsw->dma, CPDMA_EOI_TX);
  91
  92         if (cpsw->quirk_irq) {
  93                 disable_irq_nosync(cpsw->irqs_table[1]);
  94                 cpsw->tx_irq_disabled = true;
  95         }
  96
  97         napi_schedule(&cpsw->napi_tx);
  98         return IRQ_HANDLED;
  99 }
 100
 101 irqreturn_t cpsw_rx_interrupt(int irq, void *dev_id)
 102 {
 103         struct cpsw_common *cpsw = dev_id;
 104
 105         writel(0, &cpsw->wr_regs->rx_en);
 106         cpdma_ctlr_eoi(cpsw->dma, CPDMA_EOI_RX);
 107
 108         if (cpsw->quirk_irq) {
 109                 disable_irq_nosync(cpsw->irqs_table[0]);
 110                 cpsw->rx_irq_disabled = true;
 111         }
 112
 113         napi_schedule(&cpsw->napi_rx);
 114         return IRQ_HANDLED;
 115 }
 116
 117 irqreturn_t cpsw_misc_interrupt(int irq, void *dev_id)
 118 {
 119         struct cpsw_common *cpsw = dev_id;
 120
 121         writel(0, &cpsw->wr_regs->misc_en);
 122         cpdma_ctlr_eoi(cpsw->dma, CPDMA_EOI_MISC);
 123         cpts_misc_interrupt(cpsw->cpts);
 124         writel(0x10, &cpsw->wr_regs->misc_en);
 125
 126         return IRQ_HANDLED;
 127 }
 128
 129 int cpsw_tx_mq_poll(struct napi_struct *napi_tx, int budget)
 130 {
 131         struct cpsw_common      *cpsw = napi_to_cpsw(napi_tx);
 132         int                     num_tx, cur_budget, ch;
 133         u32                     ch_map;
 134         struct cpsw_vector      *txv;
 135
 136         /* process every unprocessed channel */
 137         ch_map = cpdma_ctrl_txchs_state(cpsw->dma);
 138         for (ch = 0, num_tx = 0; ch_map & 0xff; ch_map <<= 1, ch++) {
 139                 if (!(ch_map & 0x80))
 140                         continue;
 141
 142                 txv = &cpsw->txv[ch];
 143                 if (unlikely(txv->budget > budget - num_tx))
 144                         cur_budget = budget - num_tx;
 145                 else
 146                         cur_budget = txv->budget;
 147
 148                 num_tx += cpdma_chan_process(txv->ch, cur_budget);
 149                 if (num_tx >= budget)
 150                         break;
 151         }
 152
 153         if (num_tx < budget) {
 154                 napi_complete(napi_tx);
 155                 writel(0xff, &cpsw->wr_regs->tx_en);
 156         }
 157
 158         return num_tx;
 159 }
 160
 161 int cpsw_tx_poll(struct napi_struct *napi_tx, int budget)
 162 {
 163         struct cpsw_common *cpsw = napi_to_cpsw(napi_tx);
 164         int num_tx;
 165
 166         num_tx = cpdma_chan_process(cpsw->txv[0].ch, budget);
 167         if (num_tx < budget) {
 168                 napi_complete(napi_tx);
 169                 writel(0xff, &cpsw->wr_regs->tx_en);
 170                 if (cpsw->tx_irq_disabled) {
 171                         cpsw->tx_irq_disabled = false;
 172                         enable_irq(cpsw->irqs_table[1]);
 173                 }
 174         }
 175
 176         return num_tx;
 177 }
 178
 179 int cpsw_rx_mq_poll(struct napi_struct *napi_rx, int budget)
 180 {
 181         struct cpsw_common      *cpsw = napi_to_cpsw(napi_rx);
 182         int                     num_rx, cur_budget, ch;
 183         u32                     ch_map;
 184         struct cpsw_vector      *rxv;
 185
 186         /* process every unprocessed channel */
 187         ch_map = cpdma_ctrl_rxchs_state(cpsw->dma);
 188         for (ch = 0, num_rx = 0; ch_map; ch_map >>= 1, ch++) {
 189                 if (!(ch_map & 0x01))
 190                         continue;
 191
 192                 rxv = &cpsw->rxv[ch];
 193                 if (unlikely(rxv->budget > budget - num_rx))
 194                         cur_budget = budget - num_rx;
 195                 else
 196                         cur_budget = rxv->budget;
 197
 198                 num_rx += cpdma_chan_process(rxv->ch, cur_budget);
 199                 if (num_rx >= budget)
 200                         break;
 201         }
 202
 203         if (num_rx < budget) {
 204                 napi_complete_done(napi_rx, num_rx);
 205                 writel(0xff, &cpsw->wr_regs->rx_en);
 206         }
 207
 208         return num_rx;
 209 }
 210
 211 int cpsw_rx_poll(struct napi_struct *napi_rx, int budget)
 212 {
 213         struct cpsw_common *cpsw = napi_to_cpsw(napi_rx);
 214         int num_rx;
 215
 216         num_rx = cpdma_chan_process(cpsw->rxv[0].ch, budget);
 217         if (num_rx < budget) {
 218                 napi_complete_done(napi_rx, num_rx);
 219                 writel(0xff, &cpsw->wr_regs->rx_en);
 220                 if (cpsw->rx_irq_disabled) {
 221                         cpsw->rx_irq_disabled = false;
 222                         enable_irq(cpsw->irqs_table[0]);
 223                 }
 224         }
 225
 226         return num_rx;
 227 }
 228
 229 void cpsw_rx_vlan_encap(struct sk_buff *skb)
 230 {
 231         struct cpsw_priv *priv = netdev_priv(skb->dev);
 232         u32 rx_vlan_encap_hdr = *((u32 *)skb->data);
 233         struct cpsw_common *cpsw = priv->cpsw;
 234         u16 vtag, vid, prio, pkt_type;
 235
 236         /* Remove VLAN header encapsulation word */
 237         skb_pull(skb, CPSW_RX_VLAN_ENCAP_HDR_SIZE);
 238
 239         pkt_type = (rx_vlan_encap_hdr >>
 240                     CPSW_RX_VLAN_ENCAP_HDR_PKT_TYPE_SHIFT) &
 241                     CPSW_RX_VLAN_ENCAP_HDR_PKT_TYPE_MSK;
 242         /* Ignore unknown & Priority-tagged packets*/
 243         if (pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_RESERV ||
 244             pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_PRIO_TAG)
 245                 return;
 246
 247         vid = (rx_vlan_encap_hdr >>
 248                CPSW_RX_VLAN_ENCAP_HDR_VID_SHIFT) &
 249                VLAN_VID_MASK;
 250         /* Ignore vid 0 and pass packet as is */
 251         if (!vid)
 252                 return;
 253
 254         /* Untag P0 packets if set for vlan */
 255         if (!cpsw_ale_get_vlan_p0_untag(cpsw->ale, vid)) {
 256                 prio = (rx_vlan_encap_hdr >>
 257                         CPSW_RX_VLAN_ENCAP_HDR_PRIO_SHIFT) &
 258                         CPSW_RX_VLAN_ENCAP_HDR_PRIO_MSK;
 259
 260                 vtag = (prio << VLAN_PRIO_SHIFT) | vid;
 261                 __vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vtag);
 262         }
 263
 264         /* strip vlan tag for VLAN-tagged packet */
 265         if (pkt_type == CPSW_RX_VLAN_ENCAP_HDR_PKT_VLAN_TAG) {
 266                 memmove(skb->data + VLAN_HLEN, skb->data, 2 * ETH_ALEN);
 267                 skb_pull(skb, VLAN_HLEN);
 268         }
 269 }
 270
 271 void cpsw_set_slave_mac(struct cpsw_slave *slave, struct cpsw_priv *priv)
 272 {
 273         slave_write(slave, mac_hi(priv->mac_addr), SA_HI);
 274         slave_write(slave, mac_lo(priv->mac_addr), SA_LO);
 275 }
 276
 277 void soft_reset(const char *module, void __iomem *reg)
 278 {
 279         unsigned long timeout = jiffies + HZ;
 280
 281         writel_relaxed(1, reg);
 282         do {
 283                 cpu_relax();
 284         } while ((readl_relaxed(reg) & 1) && time_after(timeout, jiffies));
 285
 286         WARN(readl_relaxed(reg) & 1, "failed to soft-reset %s\n", module);
 287 }
 288
 289 void cpsw_ndo_tx_timeout(struct net_device *ndev, unsigned int txqueue)
 290 {
 291         struct cpsw_priv *priv = netdev_priv(ndev);
 292         struct cpsw_common *cpsw = priv->cpsw;
 293         int ch;
 294
 295         cpsw_err(priv, tx_err, "transmit timeout, restarting dma\n");
 296         ndev->stats.tx_errors++;
 297         cpsw_intr_disable(cpsw);
 298         for (ch = 0; ch < cpsw->tx_ch_num; ch++) {
 299                 cpdma_chan_stop(cpsw->txv[ch].ch);
 300                 cpdma_chan_start(cpsw->txv[ch].ch);
 301         }
 302
 303         cpsw_intr_enable(cpsw);
 304         netif_trans_update(ndev);
 305         netif_tx_wake_all_queues(ndev);
 306 }
 307
 308 static int cpsw_get_common_speed(struct cpsw_common *cpsw)
 309 {
 310         int i, speed;
 311
 312         for (i = 0, speed = 0; i < cpsw->data.slaves; i++)
 313                 if (cpsw->slaves[i].phy && cpsw->slaves[i].phy->link)
 314                         speed += cpsw->slaves[i].phy->speed;
 315
 316         return speed;
 317 }
 318
 319 int cpsw_need_resplit(struct cpsw_common *cpsw)
 320 {
 321         int i, rlim_ch_num;
 322         int speed, ch_rate;
 323
 324         /* re-split resources only in case speed was changed */
 325         speed = cpsw_get_common_speed(cpsw);
 326         if (speed == cpsw->speed || !speed)
 327                 return 0;
 328
 329         cpsw->speed = speed;
 330
 331         for (i = 0, rlim_ch_num = 0; i < cpsw->tx_ch_num; i++) {
 332                 ch_rate = cpdma_chan_get_rate(cpsw->txv[i].ch);
 333                 if (!ch_rate)
 334                         break;
 335
 336                 rlim_ch_num++;
 337         }
 338
 339         /* cases not dependent on speed */
 340         if (!rlim_ch_num || rlim_ch_num == cpsw->tx_ch_num)
 341                 return 0;
 342
 343         return 1;
 344 }
 345
 346 void cpsw_split_res(struct cpsw_common *cpsw)
 347 {
 348         u32 consumed_rate = 0, bigest_rate = 0;
 349         struct cpsw_vector *txv = cpsw->txv;
 350         int i, ch_weight, rlim_ch_num = 0;
 351         int budget, bigest_rate_ch = 0;
 352         u32 ch_rate, max_rate;
 353         int ch_budget = 0;
 354
 355         for (i = 0; i < cpsw->tx_ch_num; i++) {
 356                 ch_rate = cpdma_chan_get_rate(txv[i].ch);
 357                 if (!ch_rate)
 358                         continue;
 359
 360                 rlim_ch_num++;
 361                 consumed_rate += ch_rate;
 362         }
 363
 364         if (cpsw->tx_ch_num == rlim_ch_num) {
 365                 max_rate = consumed_rate;
 366         } else if (!rlim_ch_num) {
 367                 ch_budget = CPSW_POLL_WEIGHT / cpsw->tx_ch_num;
 368                 bigest_rate = 0;
 369                 max_rate = consumed_rate;
 370         } else {
 371                 max_rate = cpsw->speed * 1000;
 372
 373                 /* if max_rate is less then expected due to reduced link speed,
 374                  * split proportionally according next potential max speed
 375                  */
 376                 if (max_rate < consumed_rate)
 377                         max_rate *= 10;
 378
 379                 if (max_rate < consumed_rate)
 380                         max_rate *= 10;
 381
 382                 ch_budget = (consumed_rate * CPSW_POLL_WEIGHT) / max_rate;
 383                 ch_budget = (CPSW_POLL_WEIGHT - ch_budget) /
 384                             (cpsw->tx_ch_num - rlim_ch_num);
 385                 bigest_rate = (max_rate - consumed_rate) /
 386                               (cpsw->tx_ch_num - rlim_ch_num);
 387         }
 388
 389         /* split tx weight/budget */
 390         budget = CPSW_POLL_WEIGHT;
 391         for (i = 0; i < cpsw->tx_ch_num; i++) {
 392                 ch_rate = cpdma_chan_get_rate(txv[i].ch);
 393                 if (ch_rate) {
 394                         txv[i].budget = (ch_rate * CPSW_POLL_WEIGHT) / max_rate;
 395                         if (!txv[i].budget)
 396                                 txv[i].budget++;
 397                         if (ch_rate > bigest_rate) {
 398                                 bigest_rate_ch = i;
 399                                 bigest_rate = ch_rate;
 400                         }
 401
 402                         ch_weight = (ch_rate * 100) / max_rate;
 403                         if (!ch_weight)
 404                                 ch_weight++;
 405                         cpdma_chan_set_weight(cpsw->txv[i].ch, ch_weight);
 406                 } else {
 407                         txv[i].budget = ch_budget;
 408                         if (!bigest_rate_ch)
 409                                 bigest_rate_ch = i;
 410                         cpdma_chan_set_weight(cpsw->txv[i].ch, 0);
 411                 }
 412
 413                 budget -= txv[i].budget;
 414         }
 415
 416         if (budget)
 417                 txv[bigest_rate_ch].budget += budget;
 418
 419         /* split rx budget */
 420         budget = CPSW_POLL_WEIGHT;
 421         ch_budget = budget / cpsw->rx_ch_num;
 422         for (i = 0; i < cpsw->rx_ch_num; i++) {
 423                 cpsw->rxv[i].budget = ch_budget;
 424                 budget -= ch_budget;
 425         }
 426
 427         if (budget)
 428                 cpsw->rxv[0].budget += budget;
 429 }
 430
 431 int cpsw_init_common(struct cpsw_common *cpsw, void __iomem *ss_regs,
 432                      int ale_ageout, phys_addr_t desc_mem_phys,
 433                      int descs_pool_size)
 434 {
 435         u32 slave_offset, sliver_offset, slave_size;
 436         struct cpsw_ale_params ale_params;
 437         struct cpsw_platform_data *data;
 438         struct cpdma_params dma_params;
 439         struct device *dev = cpsw->dev;
 440         struct device_node *cpts_node;
 441         void __iomem *cpts_regs;
 442         int ret = 0, i;
 443
 444         data = &cpsw->data;
 445         cpsw->rx_ch_num = 1;
 446         cpsw->tx_ch_num = 1;
 447
 448         cpsw->version = readl(&cpsw->regs->id_ver);
 449
 450         memset(&dma_params, 0, sizeof(dma_params));
 451         memset(&ale_params, 0, sizeof(ale_params));
 452
 453         switch (cpsw->version) {
 454         case CPSW_VERSION_1:
 455                 cpsw->host_port_regs = ss_regs + CPSW1_HOST_PORT_OFFSET;
 456                 cpts_regs            = ss_regs + CPSW1_CPTS_OFFSET;
 457                 cpsw->hw_stats       = ss_regs + CPSW1_HW_STATS;
 458                 dma_params.dmaregs   = ss_regs + CPSW1_CPDMA_OFFSET;
 459                 dma_params.txhdp     = ss_regs + CPSW1_STATERAM_OFFSET;
 460                 ale_params.ale_regs  = ss_regs + CPSW1_ALE_OFFSET;
 461                 slave_offset         = CPSW1_SLAVE_OFFSET;
 462                 slave_size           = CPSW1_SLAVE_SIZE;
 463                 sliver_offset        = CPSW1_SLIVER_OFFSET;
 464                 dma_params.desc_mem_phys = 0;
 465                 break;
 466         case CPSW_VERSION_2:
 467         case CPSW_VERSION_3:
 468         case CPSW_VERSION_4:
 469                 cpsw->host_port_regs = ss_regs + CPSW2_HOST_PORT_OFFSET;
 470                 cpts_regs            = ss_regs + CPSW2_CPTS_OFFSET;
 471                 cpsw->hw_stats       = ss_regs + CPSW2_HW_STATS;
 472                 dma_params.dmaregs   = ss_regs + CPSW2_CPDMA_OFFSET;
 473                 dma_params.txhdp     = ss_regs + CPSW2_STATERAM_OFFSET;
 474                 ale_params.ale_regs  = ss_regs + CPSW2_ALE_OFFSET;
 475                 slave_offset         = CPSW2_SLAVE_OFFSET;
 476                 slave_size           = CPSW2_SLAVE_SIZE;
 477                 sliver_offset        = CPSW2_SLIVER_OFFSET;
 478                 dma_params.desc_mem_phys = desc_mem_phys;
 479                 break;
 480         default:
 481                 dev_err(dev, "unknown version 0x%08x\n", cpsw->version);
 482                 return -ENODEV;
 483         }
 484
 485         for (i = 0; i < cpsw->data.slaves; i++) {
 486                 struct cpsw_slave *slave = &cpsw->slaves[i];
 487                 void __iomem            *regs = cpsw->regs;
 488
 489                 slave->slave_num = i;
 490                 slave->data     = &cpsw->data.slave_data[i];
 491                 slave->regs     = regs + slave_offset;
 492                 slave->port_vlan = slave->data->dual_emac_res_vlan;
 493                 slave->mac_sl = cpsw_sl_get("cpsw", dev, regs + sliver_offset);
 494                 if (IS_ERR(slave->mac_sl))
 495                         return PTR_ERR(slave->mac_sl);
 496
 497                 slave_offset  += slave_size;
 498                 sliver_offset += SLIVER_SIZE;
 499         }
 500
 501         ale_params.dev                  = dev;
 502         ale_params.ale_ageout           = ale_ageout;
 503         ale_params.ale_ports            = CPSW_ALE_PORTS_NUM;
 504         ale_params.dev_id               = "cpsw";
 505
 506         cpsw->ale = cpsw_ale_create(&ale_params);
 507         if (IS_ERR(cpsw->ale)) {
 508                 dev_err(dev, "error initializing ale engine\n");
 509                 return PTR_ERR(cpsw->ale);
 510         }
 511
 512         dma_params.dev          = dev;
 513         dma_params.rxthresh     = dma_params.dmaregs + CPDMA_RXTHRESH;
 514         dma_params.rxfree       = dma_params.dmaregs + CPDMA_RXFREE;
 515         dma_params.rxhdp        = dma_params.txhdp + CPDMA_RXHDP;
 516         dma_params.txcp         = dma_params.txhdp + CPDMA_TXCP;
 517         dma_params.rxcp         = dma_params.txhdp + CPDMA_RXCP;
 518
 519         dma_params.num_chan             = data->channels;
 520         dma_params.has_soft_reset       = true;
 521         dma_params.min_packet_size      = CPSW_MIN_PACKET_SIZE;
 522         dma_params.desc_mem_size        = data->bd_ram_size;
 523         dma_params.desc_align           = 16;
 524         dma_params.has_ext_regs         = true;
 525         dma_params.desc_hw_addr         = dma_params.desc_mem_phys;
 526         dma_params.bus_freq_mhz         = cpsw->bus_freq_mhz;
 527         dma_params.descs_pool_size      = descs_pool_size;
 528
 529         cpsw->dma = cpdma_ctlr_create(&dma_params);
 530         if (!cpsw->dma) {
 531                 dev_err(dev, "error initializing dma\n");
 532                 return -ENOMEM;
 533         }
 534
 535         cpts_node = of_get_child_by_name(cpsw->dev->of_node, "cpts");
 536         if (!cpts_node)
 537                 cpts_node = cpsw->dev->of_node;
 538
 539         cpsw->cpts = cpts_create(cpsw->dev, cpts_regs, cpts_node,
 540                                  CPTS_N_ETX_TS);
 541         if (IS_ERR(cpsw->cpts)) {
 542                 ret = PTR_ERR(cpsw->cpts);
 543                 cpdma_ctlr_destroy(cpsw->dma);
 544         }
 545         of_node_put(cpts_node);
 546
 547         return ret;
 548 }
 549
 550 #if IS_ENABLED(CONFIG_TI_CPTS)
 551
 552 static void cpsw_hwtstamp_v1(struct cpsw_priv *priv)
 553 {
 554         struct cpsw_common *cpsw = priv->cpsw;
 555         struct cpsw_slave *slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
 556         u32 ts_en, seq_id;
 557
 558         if (!priv->tx_ts_enabled && !priv->rx_ts_enabled) {
 559                 slave_write(slave, 0, CPSW1_TS_CTL);
 560                 return;
 561         }
 562
 563         seq_id = (30 << CPSW_V1_SEQ_ID_OFS_SHIFT) | ETH_P_1588;
 564         ts_en = EVENT_MSG_BITS << CPSW_V1_MSG_TYPE_OFS;
 565
 566         if (priv->tx_ts_enabled)
 567                 ts_en |= CPSW_V1_TS_TX_EN;
 568
 569         if (priv->rx_ts_enabled)
 570                 ts_en |= CPSW_V1_TS_RX_EN;
 571
 572         slave_write(slave, ts_en, CPSW1_TS_CTL);
 573         slave_write(slave, seq_id, CPSW1_TS_SEQ_LTYPE);
 574 }
 575
 576 static void cpsw_hwtstamp_v2(struct cpsw_priv *priv)
 577 {
 578         struct cpsw_common *cpsw = priv->cpsw;
 579         struct cpsw_slave *slave;
 580         u32 ctrl, mtype;
 581
 582         slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
 583
 584         ctrl = slave_read(slave, CPSW2_CONTROL);
 585         switch (cpsw->version) {
 586         case CPSW_VERSION_2:
 587                 ctrl &= ~CTRL_V2_ALL_TS_MASK;
 588
 589                 if (priv->tx_ts_enabled)
 590                         ctrl |= CTRL_V2_TX_TS_BITS;
 591
 592                 if (priv->rx_ts_enabled)
 593                         ctrl |= CTRL_V2_RX_TS_BITS;
 594                 break;
 595         case CPSW_VERSION_3:
 596         default:
 597                 ctrl &= ~CTRL_V3_ALL_TS_MASK;
 598
 599                 if (priv->tx_ts_enabled)
 600                         ctrl |= CTRL_V3_TX_TS_BITS;
 601
 602                 if (priv->rx_ts_enabled)
 603                         ctrl |= CTRL_V3_RX_TS_BITS;
 604                 break;
 605         }
 606
 607         mtype = (30 << TS_SEQ_ID_OFFSET_SHIFT) | EVENT_MSG_BITS;
 608
 609         slave_write(slave, mtype, CPSW2_TS_SEQ_MTYPE);
 610         slave_write(slave, ctrl, CPSW2_CONTROL);
 611         writel_relaxed(ETH_P_1588, &cpsw->regs->ts_ltype);
 612         writel_relaxed(ETH_P_8021Q, &cpsw->regs->vlan_ltype);
 613 }
 614
 615 static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 616 {
 617         struct cpsw_priv *priv = netdev_priv(dev);
 618         struct cpsw_common *cpsw = priv->cpsw;
 619         struct hwtstamp_config cfg;
 620
 621         if (cpsw->version != CPSW_VERSION_1 &&
 622             cpsw->version != CPSW_VERSION_2 &&
 623             cpsw->version != CPSW_VERSION_3)
 624                 return -EOPNOTSUPP;
 625
 626         if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
 627                 return -EFAULT;
 628
 629         if (cfg.tx_type != HWTSTAMP_TX_OFF && cfg.tx_type != HWTSTAMP_TX_ON)
 630                 return -ERANGE;
 631
 632         switch (cfg.rx_filter) {
 633         case HWTSTAMP_FILTER_NONE:
 634                 priv->rx_ts_enabled = 0;
 635                 break;
 636         case HWTSTAMP_FILTER_ALL:
 637         case HWTSTAMP_FILTER_NTP_ALL:
 638         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 639         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 640         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
 641                 return -ERANGE;
 642         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 643         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 644         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
 645         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
 646         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
 647         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
 648         case HWTSTAMP_FILTER_PTP_V2_EVENT:
 649         case HWTSTAMP_FILTER_PTP_V2_SYNC:
 650         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 651                 priv->rx_ts_enabled = HWTSTAMP_FILTER_PTP_V2_EVENT;
 652                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 653                 break;
 654         default:
 655                 return -ERANGE;
 656         }
 657
 658         priv->tx_ts_enabled = cfg.tx_type == HWTSTAMP_TX_ON;
 659
 660         switch (cpsw->version) {
 661         case CPSW_VERSION_1:
 662                 cpsw_hwtstamp_v1(priv);
 663                 break;
 664         case CPSW_VERSION_2:
 665         case CPSW_VERSION_3:
 666                 cpsw_hwtstamp_v2(priv);
 667                 break;
 668         default:
 669                 WARN_ON(1);
 670         }
 671
 672         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
 673 }
 674
 675 static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
 676 {
 677         struct cpsw_common *cpsw = ndev_to_cpsw(dev);
 678         struct cpsw_priv *priv = netdev_priv(dev);
 679         struct hwtstamp_config cfg;
 680
 681         if (cpsw->version != CPSW_VERSION_1 &&
 682             cpsw->version != CPSW_VERSION_2 &&
 683             cpsw->version != CPSW_VERSION_3)
 684                 return -EOPNOTSUPP;
 685
 686         cfg.flags = 0;
 687         cfg.tx_type = priv->tx_ts_enabled ? HWTSTAMP_TX_ON : HWTSTAMP_TX_OFF;
 688         cfg.rx_filter = priv->rx_ts_enabled;
 689
 690         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
 691 }
 692 #else
 693 static int cpsw_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
 694 {
 695         return -EOPNOTSUPP;
 696 }
 697
 698 static int cpsw_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 699 {
 700         return -EOPNOTSUPP;
 701 }
 702 #endif /*CONFIG_TI_CPTS*/
 703
 704 int cpsw_ndo_ioctl(struct net_device *dev, struct ifreq *req, int cmd)
 705 {
 706         struct cpsw_priv *priv = netdev_priv(dev);
 707         struct cpsw_common *cpsw = priv->cpsw;
 708         int slave_no = cpsw_slave_index(cpsw, priv);
 709         struct phy_device *phy;
 710
 711         if (!netif_running(dev))
 712                 return -EINVAL;
 713
 714         phy = cpsw->slaves[slave_no].phy;
 715
 716         if (!phy_has_hwtstamp(phy)) {
 717                 switch (cmd) {
 718                 case SIOCSHWTSTAMP:
 719                         return cpsw_hwtstamp_set(dev, req);
 720                 case SIOCGHWTSTAMP:
 721                         return cpsw_hwtstamp_get(dev, req);
 722                 }
 723         }
 724
 725         if (phy)
 726                 return phy_mii_ioctl(phy, req, cmd);
 727
 728         return -EOPNOTSUPP;
 729 }
 730
 731 int cpsw_ndo_set_tx_maxrate(struct net_device *ndev, int queue, u32 rate)
 732 {
 733         struct cpsw_priv *priv = netdev_priv(ndev);
 734         struct cpsw_common *cpsw = priv->cpsw;
 735         struct cpsw_slave *slave;
 736         u32 min_rate;
 737         u32 ch_rate;
 738         int i, ret;
 739
 740         ch_rate = netdev_get_tx_queue(ndev, queue)->tx_maxrate;
 741         if (ch_rate == rate)
 742                 return 0;
 743
 744         ch_rate = rate * 1000;
 745         min_rate = cpdma_chan_get_min_rate(cpsw->dma);
 746         if ((ch_rate < min_rate && ch_rate)) {
 747                 dev_err(priv->dev, "The channel rate cannot be less than %dMbps",
 748                         min_rate);
 749                 return -EINVAL;
 750         }
 751
 752         if (rate > cpsw->speed) {
 753                 dev_err(priv->dev, "The channel rate cannot be more than 2Gbps");
 754                 return -EINVAL;
 755         }
 756
 757         ret = pm_runtime_get_sync(cpsw->dev);
 758         if (ret < 0) {
 759                 pm_runtime_put_noidle(cpsw->dev);
 760                 return ret;
 761         }
 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
 782 static 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
 790 bool 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
 806 static 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
 822 static 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 linearly
 833          * and fifo bw no more then interface can allow
 834          */
 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;
 877 err:
 878         dev_err(priv->dev, "Bandwidth doesn't fit in tc configuration");
 879         return -EINVAL;
 880 }
 881
 882 static 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                 else
 912                         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         else
 919                 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 3
 929  * class B - prio 2
 930  * shaping for class A should be set first
 931  */
 932 static 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 rate
 945          * limited first and for compliance with CPDMA rate limited channels
 946          * 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_get_sync(cpsw->dev);
 974         if (ret < 0) {
 975                 pm_runtime_put_noidle(cpsw->dev);
 976                 return ret;
 977         }
 978
 979         bw = qopt->enable ? qopt->idleslope : 0;
 980         ret = cpsw_set_fifo_rlimit(priv, fifo, bw);
 981         if (ret) {
 982                 priv->shp_cfg_speed = prev_speed;
 983                 prev_speed = 0;
 984         }
 985
 986         if (bw && prev_speed)
 987                 dev_warn(priv->dev,
 988                          "Speed was changed, CBS shaper speeds are changed!");
 989
 990         pm_runtime_put_sync(cpsw->dev);
 991         return ret;
 992 }
 993
 994 static int cpsw_set_mqprio(struct net_device *ndev, void *type_data)
 995 {
 996         struct tc_mqprio_qopt_offload *mqprio = type_data;
 997         struct cpsw_priv *priv = netdev_priv(ndev);
 998         struct cpsw_common *cpsw = priv->cpsw;
 999         int fifo, num_tc, count, offset;
1000         struct cpsw_slave *slave;
1001         u32 tx_prio_map = 0;
1002         int i, tc, ret;
1003
1004         num_tc = mqprio->qopt.num_tc;
1005         if (num_tc > CPSW_TC_NUM)
1006                 return -EINVAL;
1007
1008         if (mqprio->mode != TC_MQPRIO_MODE_DCB)
1009                 return -EINVAL;
1010
1011         ret = pm_runtime_get_sync(cpsw->dev);
1012         if (ret < 0) {
1013                 pm_runtime_put_noidle(cpsw->dev);
1014                 return ret;
1015         }
1016
1017         if (num_tc) {
1018                 for (i = 0; i < 8; i++) {
1019                         tc = mqprio->qopt.prio_tc_map[i];
1020                         fifo = cpsw_tc_to_fifo(tc, num_tc);
1021                         tx_prio_map |= fifo << (4 * i);
1022                 }
1023
1024                 netdev_set_num_tc(ndev, num_tc);
1025                 for (i = 0; i < num_tc; i++) {
1026                         count = mqprio->qopt.count[i];
1027                         offset = mqprio->qopt.offset[i];
1028                         netdev_set_tc_queue(ndev, i, count, offset);
1029                 }
1030         }
1031
1032         if (!mqprio->qopt.hw) {
1033                 /* restore default configuration */
1034                 netdev_reset_tc(ndev);
1035                 tx_prio_map = TX_PRIORITY_MAPPING;
1036         }
1037
1038         priv->mqprio_hw = mqprio->qopt.hw;
1039
1040         offset = cpsw->version == CPSW_VERSION_1 ?
1041                  CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP;
1042
1043         slave = &cpsw->slaves[cpsw_slave_index(cpsw, priv)];
1044         slave_write(slave, tx_prio_map, offset);
1045
1046         pm_runtime_put_sync(cpsw->dev);
1047
1048         return 0;
1049 }
1050
1051 int cpsw_ndo_setup_tc(struct net_device *ndev, enum tc_setup_type type,
1052                       void *type_data)
1053 {
1054         switch (type) {
1055         case TC_SETUP_QDISC_CBS:
1056                 return cpsw_set_cbs(ndev, type_data);
1057
1058         case TC_SETUP_QDISC_MQPRIO:
1059                 return cpsw_set_mqprio(ndev, type_data);
1060
1061         default:
1062                 return -EOPNOTSUPP;
1063         }
1064 }
1065
1066 void cpsw_cbs_resume(struct cpsw_slave *slave, struct cpsw_priv *priv)
1067 {
1068         int fifo, bw;
1069
1070         for (fifo = CPSW_FIFO_SHAPERS_NUM; fifo > 0; fifo--) {
1071                 bw = priv->fifo_bw[fifo];
1072                 if (!bw)
1073                         continue;
1074
1075                 cpsw_set_fifo_rlimit(priv, fifo, bw);
1076         }
1077 }
1078
1079 void cpsw_mqprio_resume(struct cpsw_slave *slave, struct cpsw_priv *priv)
1080 {
1081         struct cpsw_common *cpsw = priv->cpsw;
1082         u32 tx_prio_map = 0;
1083         int i, tc, fifo;
1084         u32 tx_prio_rg;
1085
1086         if (!priv->mqprio_hw)
1087                 return;
1088
1089         for (i = 0; i < 8; i++) {
1090                 tc = netdev_get_prio_tc_map(priv->ndev, i);
1091                 fifo = CPSW_FIFO_SHAPERS_NUM - tc;
1092                 tx_prio_map |= fifo << (4 * i);
1093         }
1094
1095         tx_prio_rg = cpsw->version == CPSW_VERSION_1 ?
1096                      CPSW1_TX_PRI_MAP : CPSW2_TX_PRI_MAP;
1097
1098         slave_write(slave, tx_prio_map, tx_prio_rg);
1099 }
1100
1101 int cpsw_fill_rx_channels(struct cpsw_priv *priv)
1102 {
1103         struct cpsw_common *cpsw = priv->cpsw;
1104         struct cpsw_meta_xdp *xmeta;
1105         struct page_pool *pool;
1106         struct page *page;
1107         int ch_buf_num;
1108         int ch, i, ret;
1109         dma_addr_t dma;
1110
1111         for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
1112                 pool = cpsw->page_pool[ch];
1113                 ch_buf_num = cpdma_chan_get_rx_buf_num(cpsw->rxv[ch].ch);
1114                 for (i = 0; i < ch_buf_num; i++) {
1115                         page = page_pool_dev_alloc_pages(pool);
1116                         if (!page) {
1117                                 cpsw_err(priv, ifup, "allocate rx page err\n");
1118                                 return -ENOMEM;
1119                         }
1120
1121                         xmeta = page_address(page) + CPSW_XMETA_OFFSET;
1122                         xmeta->ndev = priv->ndev;
1123                         xmeta->ch = ch;
1124
1125                         dma = page_pool_get_dma_addr(page) + CPSW_HEADROOM_NA;
1126                         ret = cpdma_chan_idle_submit_mapped(cpsw->rxv[ch].ch,
1127                                                             page, dma,
1128                                                             cpsw->rx_packet_max,
1129                                                             0);
1130                         if (ret < 0) {
1131                                 cpsw_err(priv, ifup,
1132                                          "cannot submit page to channel %d rx, error %d\n",
1133                                          ch, ret);
1134                                 page_pool_recycle_direct(pool, page);
1135                                 return ret;
1136                         }
1137                 }
1138
1139                 cpsw_info(priv, ifup, "ch %d rx, submitted %d descriptors\n",
1140                           ch, ch_buf_num);
1141         }
1142
1143         return 0;
1144 }
1145
1146 static struct page_pool *cpsw_create_page_pool(struct cpsw_common *cpsw,
1147                                                int size)
1148 {
1149         struct page_pool_params pp_params = {};
1150         struct page_pool *pool;
1151
1152         pp_params.order = 0;
1153         pp_params.flags = PP_FLAG_DMA_MAP;
1154         pp_params.pool_size = size;
1155         pp_params.nid = NUMA_NO_NODE;
1156         pp_params.dma_dir = DMA_BIDIRECTIONAL;
1157         pp_params.dev = cpsw->dev;
1158
1159         pool = page_pool_create(&pp_params);
1160         if (IS_ERR(pool))
1161                 dev_err(cpsw->dev, "cannot create rx page pool\n");
1162
1163         return pool;
1164 }
1165
1166 static int cpsw_create_rx_pool(struct cpsw_common *cpsw, int ch)
1167 {
1168         struct page_pool *pool;
1169         int ret = 0, pool_size;
1170
1171         pool_size = cpdma_chan_get_rx_buf_num(cpsw->rxv[ch].ch);
1172         pool = cpsw_create_page_pool(cpsw, pool_size);
1173         if (IS_ERR(pool))
1174                 ret = PTR_ERR(pool);
1175         else
1176                 cpsw->page_pool[ch] = pool;
1177
1178         return ret;
1179 }
1180
1181 static int cpsw_ndev_create_xdp_rxq(struct cpsw_priv *priv, int ch)
1182 {
1183         struct cpsw_common *cpsw = priv->cpsw;
1184         struct xdp_rxq_info *rxq;
1185         struct page_pool *pool;
1186         int ret;
1187
1188         pool = cpsw->page_pool[ch];
1189         rxq = &priv->xdp_rxq[ch];
1190
1191         ret = xdp_rxq_info_reg(rxq, priv->ndev, ch, 0);
1192         if (ret)
1193                 return ret;
1194
1195         ret = xdp_rxq_info_reg_mem_model(rxq, MEM_TYPE_PAGE_POOL, pool);
1196         if (ret)
1197                 xdp_rxq_info_unreg(rxq);
1198
1199         return ret;
1200 }
1201
1202 static void cpsw_ndev_destroy_xdp_rxq(struct cpsw_priv *priv, int ch)
1203 {
1204         struct xdp_rxq_info *rxq = &priv->xdp_rxq[ch];
1205
1206         if (!xdp_rxq_info_is_reg(rxq))
1207                 return;
1208
1209         xdp_rxq_info_unreg(rxq);
1210 }
1211
1212 void cpsw_destroy_xdp_rxqs(struct cpsw_common *cpsw)
1213 {
1214         struct net_device *ndev;
1215         int i, ch;
1216
1217         for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
1218                 for (i = 0; i < cpsw->data.slaves; i++) {
1219                         ndev = cpsw->slaves[i].ndev;
1220                         if (!ndev)
1221                                 continue;
1222
1223                         cpsw_ndev_destroy_xdp_rxq(netdev_priv(ndev), ch);
1224                 }
1225
1226                 page_pool_destroy(cpsw->page_pool[ch]);
1227                 cpsw->page_pool[ch] = NULL;
1228         }
1229 }
1230
1231 int cpsw_create_xdp_rxqs(struct cpsw_common *cpsw)
1232 {
1233         struct net_device *ndev;
1234         int i, ch, ret;
1235
1236         for (ch = 0; ch < cpsw->rx_ch_num; ch++) {
1237                 ret = cpsw_create_rx_pool(cpsw, ch);
1238                 if (ret)
1239                         goto err_cleanup;
1240
1241                 /* using same page pool is allowed as no running rx handlers
1242                  * simultaneously for both ndevs
1243                  */
1244                 for (i = 0; i < cpsw->data.slaves; i++) {
1245                         ndev = cpsw->slaves[i].ndev;
1246                         if (!ndev)
1247                                 continue;
1248
1249                         ret = cpsw_ndev_create_xdp_rxq(netdev_priv(ndev), ch);
1250                         if (ret)
1251                                 goto err_cleanup;
1252                 }
1253         }
1254
1255         return 0;
1256
1257 err_cleanup:
1258         cpsw_destroy_xdp_rxqs(cpsw);
1259
1260         return ret;
1261 }
1262
1263 static int cpsw_xdp_prog_setup(struct cpsw_priv *priv, struct netdev_bpf *bpf)
1264 {
1265         struct bpf_prog *prog = bpf->prog;
1266
1267         if (!priv->xdpi.prog && !prog)
1268                 return 0;
1269
1270         WRITE_ONCE(priv->xdp_prog, prog);
1271
1272         xdp_attachment_setup(&priv->xdpi, bpf);
1273
1274         return 0;
1275 }
1276
1277 int cpsw_ndo_bpf(struct net_device *ndev, struct netdev_bpf *bpf)
1278 {
1279         struct cpsw_priv *priv = netdev_priv(ndev);
1280
1281         switch (bpf->command) {
1282         case XDP_SETUP_PROG:
1283                 return cpsw_xdp_prog_setup(priv, bpf);
1284
1285         default:
1286                 return -EINVAL;
1287         }
1288 }
1289
1290 int cpsw_xdp_tx_frame(struct cpsw_priv *priv, struct xdp_frame *xdpf,
1291                       struct page *page, int port)
1292 {
1293         struct cpsw_common *cpsw = priv->cpsw;
1294         struct cpsw_meta_xdp *xmeta;
1295         struct cpdma_chan *txch;
1296         dma_addr_t dma;
1297         int ret;
1298
1299         xmeta = (void *)xdpf + CPSW_XMETA_OFFSET;
1300         xmeta->ndev = priv->ndev;
1301         xmeta->ch = 0;
1302         txch = cpsw->txv[0].ch;
1303
1304         if (page) {
1305                 dma = page_pool_get_dma_addr(page);
1306                 dma += xdpf->headroom + sizeof(struct xdp_frame);
1307                 ret = cpdma_chan_submit_mapped(txch, cpsw_xdpf_to_handle(xdpf),
1308                                                dma, xdpf->len, port);
1309         } else {
1310                 if (sizeof(*xmeta) > xdpf->headroom)
1311                         return -EINVAL;
1312
1313                 ret = cpdma_chan_submit(txch, cpsw_xdpf_to_handle(xdpf),
1314                                         xdpf->data, xdpf->len, port);
1315         }
1316
1317         if (ret)
1318                 priv->ndev->stats.tx_dropped++;
1319
1320         return ret;
1321 }
1322
1323 int cpsw_run_xdp(struct cpsw_priv *priv, int ch, struct xdp_buff *xdp,
1324                  struct page *page, int port, int *len)
1325 {
1326         struct cpsw_common *cpsw = priv->cpsw;
1327         struct net_device *ndev = priv->ndev;
1328         int ret = CPSW_XDP_CONSUMED;
1329         struct xdp_frame *xdpf;
1330         struct bpf_prog *prog;
1331         u32 act;
1332
1333         prog = READ_ONCE(priv->xdp_prog);
1334         if (!prog)
1335                 return CPSW_XDP_PASS;
1336
1337         act = bpf_prog_run_xdp(prog, xdp);
1338         /* XDP prog might have changed packet data and boundaries */
1339         *len = xdp->data_end - xdp->data;
1340
1341         switch (act) {
1342         case XDP_PASS:
1343                 ret = CPSW_XDP_PASS;
1344                 goto out;
1345         case XDP_TX:
1346                 xdpf = xdp_convert_buff_to_frame(xdp);
1347                 if (unlikely(!xdpf))
1348                         goto drop;
1349
1350                 if (cpsw_xdp_tx_frame(priv, xdpf, page, port))
1351                         xdp_return_frame_rx_napi(xdpf);
1352                 break;
1353         case XDP_REDIRECT:
1354                 if (xdp_do_redirect(ndev, xdp, prog))
1355                         goto drop;
1356
1357                 /*  Have to flush here, per packet, instead of doing it in bulk
1358                  *  at the end of the napi handler. The RX devices on this
1359                  *  particular hardware is sharing a common queue, so the
1360                  *  incoming device might change per packet.
1361                  */
1362                 xdp_do_flush_map();
1363                 break;
1364         default:
1365                 bpf_warn_invalid_xdp_action(ndev, prog, act);
1366                 fallthrough;
1367         case XDP_ABORTED:
1368                 trace_xdp_exception(ndev, prog, act);
1369                 fallthrough;    /* handle aborts by dropping packet */
1370         case XDP_DROP:
1371                 ndev->stats.rx_bytes += *len;
1372                 ndev->stats.rx_packets++;
1373                 goto drop;
1374         }
1375
1376         ndev->stats.rx_bytes += *len;
1377         ndev->stats.rx_packets++;
1378 out:
1379         return ret;
1380 drop:
1381         page_pool_recycle_direct(cpsw->page_pool[ch], page);
1382         return ret;
1383 }