drivers/net/ethernet/stmicro/stmmac/stmmac_main.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*******************************************************************************
   3   This is the driver for the ST MAC 10/100/1000 on-chip Ethernet controllers.
   4   ST Ethernet IPs are built around a Synopsys IP Core.
   5
   6         Copyright(C) 2007-2011 STMicroelectronics Ltd
   7
   8
   9   Author: Giuseppe Cavallaro <peppe.cavallaro@st.com>
  10
  11   Documentation available at:
  12         http://www.stlinux.com
  13   Support available at:
  14         https://bugzilla.stlinux.com/
  15 *******************************************************************************/
  16
  17 #include <linux/clk.h>
  18 #include <linux/kernel.h>
  19 #include <linux/interrupt.h>
  20 #include <linux/ip.h>
  21 #include <linux/tcp.h>
  22 #include <linux/skbuff.h>
  23 #include <linux/ethtool.h>
  24 #include <linux/if_ether.h>
  25 #include <linux/crc32.h>
  26 #include <linux/mii.h>
  27 #include <linux/if.h>
  28 #include <linux/if_vlan.h>
  29 #include <linux/dma-mapping.h>
  30 #include <linux/slab.h>
  31 #include <linux/pm_runtime.h>
  32 #include <linux/prefetch.h>
  33 #include <linux/pinctrl/consumer.h>
  34 #ifdef CONFIG_DEBUG_FS
  35 #include <linux/debugfs.h>
  36 #include <linux/seq_file.h>
  37 #endif /* CONFIG_DEBUG_FS */
  38 #include <linux/net_tstamp.h>
  39 #include <linux/phylink.h>
  40 #include <linux/udp.h>
  41 #include <linux/bpf_trace.h>
  42 #include <net/pkt_cls.h>
  43 #include <net/xdp_sock_drv.h>
  44 #include "stmmac_ptp.h"
  45 #include "stmmac.h"
  46 #include "stmmac_xdp.h"
  47 #include <linux/reset.h>
  48 #include <linux/of_mdio.h>
  49 #include "dwmac1000.h"
  50 #include "dwxgmac2.h"
  51 #include "hwif.h"
  52
  53 /* As long as the interface is active, we keep the timestamping counter enabled
  54  * with fine resolution and binary rollover. This avoid non-monotonic behavior
  55  * (clock jumps) when changing timestamping settings at runtime.
  56  */
  57 #define STMMAC_HWTS_ACTIVE      (PTP_TCR_TSENA | PTP_TCR_TSCFUPDT | \
  58                                  PTP_TCR_TSCTRLSSR)
  59
  60 #define STMMAC_ALIGN(x)         ALIGN(ALIGN(x, SMP_CACHE_BYTES), 16)
  61 #define TSO_MAX_BUFF_SIZE       (SZ_16K - 1)
  62
  63 /* Module parameters */
  64 #define TX_TIMEO        5000
  65 static int watchdog = TX_TIMEO;
  66 module_param(watchdog, int, 0644);
  67 MODULE_PARM_DESC(watchdog, "Transmit timeout in milliseconds (default 5s)");
  68
  69 static int debug = -1;
  70 module_param(debug, int, 0644);
  71 MODULE_PARM_DESC(debug, "Message Level (-1: default, 0: no output, 16: all)");
  72
  73 static int phyaddr = -1;
  74 module_param(phyaddr, int, 0444);
  75 MODULE_PARM_DESC(phyaddr, "Physical device address");
  76
  77 #define STMMAC_TX_THRESH(x)     ((x)->dma_conf.dma_tx_size / 4)
  78 #define STMMAC_RX_THRESH(x)     ((x)->dma_conf.dma_rx_size / 4)
  79
  80 /* Limit to make sure XDP TX and slow path can coexist */
  81 #define STMMAC_XSK_TX_BUDGET_MAX        256
  82 #define STMMAC_TX_XSK_AVAIL             16
  83 #define STMMAC_RX_FILL_BATCH            16
  84
  85 #define STMMAC_XDP_PASS         0
  86 #define STMMAC_XDP_CONSUMED     BIT(0)
  87 #define STMMAC_XDP_TX           BIT(1)
  88 #define STMMAC_XDP_REDIRECT     BIT(2)
  89
  90 static int flow_ctrl = FLOW_AUTO;
  91 module_param(flow_ctrl, int, 0644);
  92 MODULE_PARM_DESC(flow_ctrl, "Flow control ability [on/off]");
  93
  94 static int pause = PAUSE_TIME;
  95 module_param(pause, int, 0644);
  96 MODULE_PARM_DESC(pause, "Flow Control Pause Time");
  97
  98 #define TC_DEFAULT 64
  99 static int tc = TC_DEFAULT;
 100 module_param(tc, int, 0644);
 101 MODULE_PARM_DESC(tc, "DMA threshold control value");
 102
 103 #define DEFAULT_BUFSIZE 1536
 104 static int buf_sz = DEFAULT_BUFSIZE;
 105 module_param(buf_sz, int, 0644);
 106 MODULE_PARM_DESC(buf_sz, "DMA buffer size");
 107
 108 #define STMMAC_RX_COPYBREAK     256
 109
 110 static const u32 default_msg_level = (NETIF_MSG_DRV | NETIF_MSG_PROBE |
 111                                       NETIF_MSG_LINK | NETIF_MSG_IFUP |
 112                                       NETIF_MSG_IFDOWN | NETIF_MSG_TIMER);
 113
 114 #define STMMAC_DEFAULT_LPI_TIMER        1000
 115 static int eee_timer = STMMAC_DEFAULT_LPI_TIMER;
 116 module_param(eee_timer, int, 0644);
 117 MODULE_PARM_DESC(eee_timer, "LPI tx expiration time in msec");
 118 #define STMMAC_LPI_T(x) (jiffies + usecs_to_jiffies(x))
 119
 120 /* By default the driver will use the ring mode to manage tx and rx descriptors,
 121  * but allow user to force to use the chain instead of the ring
 122  */
 123 static unsigned int chain_mode;
 124 module_param(chain_mode, int, 0444);
 125 MODULE_PARM_DESC(chain_mode, "To use chain instead of ring mode");
 126
 127 static irqreturn_t stmmac_interrupt(int irq, void *dev_id);
 128 /* For MSI interrupts handling */
 129 static irqreturn_t stmmac_mac_interrupt(int irq, void *dev_id);
 130 static irqreturn_t stmmac_safety_interrupt(int irq, void *dev_id);
 131 static irqreturn_t stmmac_msi_intr_tx(int irq, void *data);
 132 static irqreturn_t stmmac_msi_intr_rx(int irq, void *data);
 133 static void stmmac_reset_rx_queue(struct stmmac_priv *priv, u32 queue);
 134 static void stmmac_reset_tx_queue(struct stmmac_priv *priv, u32 queue);
 135 static void stmmac_reset_queues_param(struct stmmac_priv *priv);
 136 static void stmmac_tx_timer_arm(struct stmmac_priv *priv, u32 queue);
 137 static void stmmac_flush_tx_descriptors(struct stmmac_priv *priv, int queue);
 138 static void stmmac_set_dma_operation_mode(struct stmmac_priv *priv, u32 txmode,
 139                                           u32 rxmode, u32 chan);
 140
 141 #ifdef CONFIG_DEBUG_FS
 142 static const struct net_device_ops stmmac_netdev_ops;
 143 static void stmmac_init_fs(struct net_device *dev);
 144 static void stmmac_exit_fs(struct net_device *dev);
 145 #endif
 146
 147 #define STMMAC_COAL_TIMER(x) (ns_to_ktime((x) * NSEC_PER_USEC))
 148
 149 int stmmac_bus_clks_config(struct stmmac_priv *priv, bool enabled)
 150 {
 151         int ret = 0;
 152
 153         if (enabled) {
 154                 ret = clk_prepare_enable(priv->plat->stmmac_clk);
 155                 if (ret)
 156                         return ret;
 157                 ret = clk_prepare_enable(priv->plat->pclk);
 158                 if (ret) {
 159                         clk_disable_unprepare(priv->plat->stmmac_clk);
 160                         return ret;
 161                 }
 162                 if (priv->plat->clks_config) {
 163                         ret = priv->plat->clks_config(priv->plat->bsp_priv, enabled);
 164                         if (ret) {
 165                                 clk_disable_unprepare(priv->plat->stmmac_clk);
 166                                 clk_disable_unprepare(priv->plat->pclk);
 167                                 return ret;
 168                         }
 169                 }
 170         } else {
 171                 clk_disable_unprepare(priv->plat->stmmac_clk);
 172                 clk_disable_unprepare(priv->plat->pclk);
 173                 if (priv->plat->clks_config)
 174                         priv->plat->clks_config(priv->plat->bsp_priv, enabled);
 175         }
 176
 177         return ret;
 178 }
 179 EXPORT_SYMBOL_GPL(stmmac_bus_clks_config);
 180
 181 /**
 182  * stmmac_verify_args - verify the driver parameters.
 183  * Description: it checks the driver parameters and set a default in case of
 184  * errors.
 185  */
 186 static void stmmac_verify_args(void)
 187 {
 188         if (unlikely(watchdog < 0))
 189                 watchdog = TX_TIMEO;
 190         if (unlikely((buf_sz < DEFAULT_BUFSIZE) || (buf_sz > BUF_SIZE_16KiB)))
 191                 buf_sz = DEFAULT_BUFSIZE;
 192         if (unlikely(flow_ctrl > 1))
 193                 flow_ctrl = FLOW_AUTO;
 194         else if (likely(flow_ctrl < 0))
 195                 flow_ctrl = FLOW_OFF;
 196         if (unlikely((pause < 0) || (pause > 0xffff)))
 197                 pause = PAUSE_TIME;
 198         if (eee_timer < 0)
 199                 eee_timer = STMMAC_DEFAULT_LPI_TIMER;
 200 }
 201
 202 static void __stmmac_disable_all_queues(struct stmmac_priv *priv)
 203 {
 204         u32 rx_queues_cnt = priv->plat->rx_queues_to_use;
 205         u32 tx_queues_cnt = priv->plat->tx_queues_to_use;
 206         u32 maxq = max(rx_queues_cnt, tx_queues_cnt);
 207         u32 queue;
 208
 209         for (queue = 0; queue < maxq; queue++) {
 210                 struct stmmac_channel *ch = &priv->channel[queue];
 211
 212                 if (stmmac_xdp_is_enabled(priv) &&
 213                     test_bit(queue, priv->af_xdp_zc_qps)) {
 214                         napi_disable(&ch->rxtx_napi);
 215                         continue;
 216                 }
 217
 218                 if (queue < rx_queues_cnt)
 219                         napi_disable(&ch->rx_napi);
 220                 if (queue < tx_queues_cnt)
 221                         napi_disable(&ch->tx_napi);
 222         }
 223 }
 224
 225 /**
 226  * stmmac_disable_all_queues - Disable all queues
 227  * @priv: driver private structure
 228  */
 229 static void stmmac_disable_all_queues(struct stmmac_priv *priv)
 230 {
 231         u32 rx_queues_cnt = priv->plat->rx_queues_to_use;
 232         struct stmmac_rx_queue *rx_q;
 233         u32 queue;
 234
 235         /* synchronize_rcu() needed for pending XDP buffers to drain */
 236         for (queue = 0; queue < rx_queues_cnt; queue++) {
 237                 rx_q = &priv->dma_conf.rx_queue[queue];
 238                 if (rx_q->xsk_pool) {
 239                         synchronize_rcu();
 240                         break;
 241                 }
 242         }
 243
 244         __stmmac_disable_all_queues(priv);
 245 }
 246
 247 /**
 248  * stmmac_enable_all_queues - Enable all queues
 249  * @priv: driver private structure
 250  */
 251 static void stmmac_enable_all_queues(struct stmmac_priv *priv)
 252 {
 253         u32 rx_queues_cnt = priv->plat->rx_queues_to_use;
 254         u32 tx_queues_cnt = priv->plat->tx_queues_to_use;
 255         u32 maxq = max(rx_queues_cnt, tx_queues_cnt);
 256         u32 queue;
 257
 258         for (queue = 0; queue < maxq; queue++) {
 259                 struct stmmac_channel *ch = &priv->channel[queue];
 260
 261                 if (stmmac_xdp_is_enabled(priv) &&
 262                     test_bit(queue, priv->af_xdp_zc_qps)) {
 263                         napi_enable(&ch->rxtx_napi);
 264                         continue;
 265                 }
 266
 267                 if (queue < rx_queues_cnt)
 268                         napi_enable(&ch->rx_napi);
 269                 if (queue < tx_queues_cnt)
 270                         napi_enable(&ch->tx_napi);
 271         }
 272 }
 273
 274 static void stmmac_service_event_schedule(struct stmmac_priv *priv)
 275 {
 276         if (!test_bit(STMMAC_DOWN, &priv->state) &&
 277             !test_and_set_bit(STMMAC_SERVICE_SCHED, &priv->state))
 278                 queue_work(priv->wq, &priv->service_task);
 279 }
 280
 281 static void stmmac_global_err(struct stmmac_priv *priv)
 282 {
 283         netif_carrier_off(priv->dev);
 284         set_bit(STMMAC_RESET_REQUESTED, &priv->state);
 285         stmmac_service_event_schedule(priv);
 286 }
 287
 288 /**
 289  * stmmac_clk_csr_set - dynamically set the MDC clock
 290  * @priv: driver private structure
 291  * Description: this is to dynamically set the MDC clock according to the csr
 292  * clock input.
 293  * Note:
 294  *      If a specific clk_csr value is passed from the platform
 295  *      this means that the CSR Clock Range selection cannot be
 296  *      changed at run-time and it is fixed (as reported in the driver
 297  *      documentation). Viceversa the driver will try to set the MDC
 298  *      clock dynamically according to the actual clock input.
 299  */
 300 static void stmmac_clk_csr_set(struct stmmac_priv *priv)
 301 {
 302         u32 clk_rate;
 303
 304         clk_rate = clk_get_rate(priv->plat->stmmac_clk);
 305
 306         /* Platform provided default clk_csr would be assumed valid
 307          * for all other cases except for the below mentioned ones.
 308          * For values higher than the IEEE 802.3 specified frequency
 309          * we can not estimate the proper divider as it is not known
 310          * the frequency of clk_csr_i. So we do not change the default
 311          * divider.
 312          */
 313         if (!(priv->clk_csr & MAC_CSR_H_FRQ_MASK)) {
 314                 if (clk_rate < CSR_F_35M)
 315                         priv->clk_csr = STMMAC_CSR_20_35M;
 316                 else if ((clk_rate >= CSR_F_35M) && (clk_rate < CSR_F_60M))
 317                         priv->clk_csr = STMMAC_CSR_35_60M;
 318                 else if ((clk_rate >= CSR_F_60M) && (clk_rate < CSR_F_100M))
 319                         priv->clk_csr = STMMAC_CSR_60_100M;
 320                 else if ((clk_rate >= CSR_F_100M) && (clk_rate < CSR_F_150M))
 321                         priv->clk_csr = STMMAC_CSR_100_150M;
 322                 else if ((clk_rate >= CSR_F_150M) && (clk_rate < CSR_F_250M))
 323                         priv->clk_csr = STMMAC_CSR_150_250M;
 324                 else if ((clk_rate >= CSR_F_250M) && (clk_rate <= CSR_F_300M))
 325                         priv->clk_csr = STMMAC_CSR_250_300M;
 326         }
 327
 328         if (priv->plat->has_sun8i) {
 329                 if (clk_rate > 160000000)
 330                         priv->clk_csr = 0x03;
 331                 else if (clk_rate > 80000000)
 332                         priv->clk_csr = 0x02;
 333                 else if (clk_rate > 40000000)
 334                         priv->clk_csr = 0x01;
 335                 else
 336                         priv->clk_csr = 0;
 337         }
 338
 339         if (priv->plat->has_xgmac) {
 340                 if (clk_rate > 400000000)
 341                         priv->clk_csr = 0x5;
 342                 else if (clk_rate > 350000000)
 343                         priv->clk_csr = 0x4;
 344                 else if (clk_rate > 300000000)
 345                         priv->clk_csr = 0x3;
 346                 else if (clk_rate > 250000000)
 347                         priv->clk_csr = 0x2;
 348                 else if (clk_rate > 150000000)
 349                         priv->clk_csr = 0x1;
 350                 else
 351                         priv->clk_csr = 0x0;
 352         }
 353 }
 354
 355 static void print_pkt(unsigned char *buf, int len)
 356 {
 357         pr_debug("len = %d byte, buf addr: 0x%p\n", len, buf);
 358         print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, buf, len);
 359 }
 360
 361 static inline u32 stmmac_tx_avail(struct stmmac_priv *priv, u32 queue)
 362 {
 363         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[queue];
 364         u32 avail;
 365
 366         if (tx_q->dirty_tx > tx_q->cur_tx)
 367                 avail = tx_q->dirty_tx - tx_q->cur_tx - 1;
 368         else
 369                 avail = priv->dma_conf.dma_tx_size - tx_q->cur_tx + tx_q->dirty_tx - 1;
 370
 371         return avail;
 372 }
 373
 374 /**
 375  * stmmac_rx_dirty - Get RX queue dirty
 376  * @priv: driver private structure
 377  * @queue: RX queue index
 378  */
 379 static inline u32 stmmac_rx_dirty(struct stmmac_priv *priv, u32 queue)
 380 {
 381         struct stmmac_rx_queue *rx_q = &priv->dma_conf.rx_queue[queue];
 382         u32 dirty;
 383
 384         if (rx_q->dirty_rx <= rx_q->cur_rx)
 385                 dirty = rx_q->cur_rx - rx_q->dirty_rx;
 386         else
 387                 dirty = priv->dma_conf.dma_rx_size - rx_q->dirty_rx + rx_q->cur_rx;
 388
 389         return dirty;
 390 }
 391
 392 static void stmmac_lpi_entry_timer_config(struct stmmac_priv *priv, bool en)
 393 {
 394         int tx_lpi_timer;
 395
 396         /* Clear/set the SW EEE timer flag based on LPI ET enablement */
 397         priv->eee_sw_timer_en = en ? 0 : 1;
 398         tx_lpi_timer  = en ? priv->tx_lpi_timer : 0;
 399         stmmac_set_eee_lpi_timer(priv, priv->hw, tx_lpi_timer);
 400 }
 401
 402 /**
 403  * stmmac_enable_eee_mode - check and enter in LPI mode
 404  * @priv: driver private structure
 405  * Description: this function is to verify and enter in LPI mode in case of
 406  * EEE.
 407  */
 408 static int stmmac_enable_eee_mode(struct stmmac_priv *priv)
 409 {
 410         u32 tx_cnt = priv->plat->tx_queues_to_use;
 411         u32 queue;
 412
 413         /* check if all TX queues have the work finished */
 414         for (queue = 0; queue < tx_cnt; queue++) {
 415                 struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[queue];
 416
 417                 if (tx_q->dirty_tx != tx_q->cur_tx)
 418                         return -EBUSY; /* still unfinished work */
 419         }
 420
 421         /* Check and enter in LPI mode */
 422         if (!priv->tx_path_in_lpi_mode)
 423                 stmmac_set_eee_mode(priv, priv->hw,
 424                                 priv->plat->en_tx_lpi_clockgating);
 425         return 0;
 426 }
 427
 428 /**
 429  * stmmac_disable_eee_mode - disable and exit from LPI mode
 430  * @priv: driver private structure
 431  * Description: this function is to exit and disable EEE in case of
 432  * LPI state is true. This is called by the xmit.
 433  */
 434 void stmmac_disable_eee_mode(struct stmmac_priv *priv)
 435 {
 436         if (!priv->eee_sw_timer_en) {
 437                 stmmac_lpi_entry_timer_config(priv, 0);
 438                 return;
 439         }
 440
 441         stmmac_reset_eee_mode(priv, priv->hw);
 442         del_timer_sync(&priv->eee_ctrl_timer);
 443         priv->tx_path_in_lpi_mode = false;
 444 }
 445
 446 /**
 447  * stmmac_eee_ctrl_timer - EEE TX SW timer.
 448  * @t:  timer_list struct containing private info
 449  * Description:
 450  *  if there is no data transfer and if we are not in LPI state,
 451  *  then MAC Transmitter can be moved to LPI state.
 452  */
 453 static void stmmac_eee_ctrl_timer(struct timer_list *t)
 454 {
 455         struct stmmac_priv *priv = from_timer(priv, t, eee_ctrl_timer);
 456
 457         if (stmmac_enable_eee_mode(priv))
 458                 mod_timer(&priv->eee_ctrl_timer, STMMAC_LPI_T(priv->tx_lpi_timer));
 459 }
 460
 461 /**
 462  * stmmac_eee_init - init EEE
 463  * @priv: driver private structure
 464  * Description:
 465  *  if the GMAC supports the EEE (from the HW cap reg) and the phy device
 466  *  can also manage EEE, this function enable the LPI state and start related
 467  *  timer.
 468  */
 469 bool stmmac_eee_init(struct stmmac_priv *priv)
 470 {
 471         int eee_tw_timer = priv->eee_tw_timer;
 472
 473         /* Using PCS we cannot dial with the phy registers at this stage
 474          * so we do not support extra feature like EEE.
 475          */
 476         if (priv->hw->pcs == STMMAC_PCS_TBI ||
 477             priv->hw->pcs == STMMAC_PCS_RTBI)
 478                 return false;
 479
 480         /* Check if MAC core supports the EEE feature. */
 481         if (!priv->dma_cap.eee)
 482                 return false;
 483
 484         mutex_lock(&priv->lock);
 485
 486         /* Check if it needs to be deactivated */
 487         if (!priv->eee_active) {
 488                 if (priv->eee_enabled) {
 489                         netdev_dbg(priv->dev, "disable EEE\n");
 490                         stmmac_lpi_entry_timer_config(priv, 0);
 491                         del_timer_sync(&priv->eee_ctrl_timer);
 492                         stmmac_set_eee_timer(priv, priv->hw, 0, eee_tw_timer);
 493                         if (priv->hw->xpcs)
 494                                 xpcs_config_eee(priv->hw->xpcs,
 495                                                 priv->plat->mult_fact_100ns,
 496                                                 false);
 497                 }
 498                 mutex_unlock(&priv->lock);
 499                 return false;
 500         }
 501
 502         if (priv->eee_active && !priv->eee_enabled) {
 503                 timer_setup(&priv->eee_ctrl_timer, stmmac_eee_ctrl_timer, 0);
 504                 stmmac_set_eee_timer(priv, priv->hw, STMMAC_DEFAULT_LIT_LS,
 505                                      eee_tw_timer);
 506                 if (priv->hw->xpcs)
 507                         xpcs_config_eee(priv->hw->xpcs,
 508                                         priv->plat->mult_fact_100ns,
 509                                         true);
 510         }
 511
 512         if (priv->plat->has_gmac4 && priv->tx_lpi_timer <= STMMAC_ET_MAX) {
 513                 del_timer_sync(&priv->eee_ctrl_timer);
 514                 priv->tx_path_in_lpi_mode = false;
 515                 stmmac_lpi_entry_timer_config(priv, 1);
 516         } else {
 517                 stmmac_lpi_entry_timer_config(priv, 0);
 518                 mod_timer(&priv->eee_ctrl_timer,
 519                           STMMAC_LPI_T(priv->tx_lpi_timer));
 520         }
 521
 522         mutex_unlock(&priv->lock);
 523         netdev_dbg(priv->dev, "Energy-Efficient Ethernet initialized\n");
 524         return true;
 525 }
 526
 527 /* stmmac_get_tx_hwtstamp - get HW TX timestamps
 528  * @priv: driver private structure
 529  * @p : descriptor pointer
 530  * @skb : the socket buffer
 531  * Description :
 532  * This function will read timestamp from the descriptor & pass it to stack.
 533  * and also perform some sanity checks.
 534  */
 535 static void stmmac_get_tx_hwtstamp(struct stmmac_priv *priv,
 536                                    struct dma_desc *p, struct sk_buff *skb)
 537 {
 538         struct skb_shared_hwtstamps shhwtstamp;
 539         bool found = false;
 540         u64 ns = 0;
 541
 542         if (!priv->hwts_tx_en)
 543                 return;
 544
 545         /* exit if skb doesn't support hw tstamp */
 546         if (likely(!skb || !(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS)))
 547                 return;
 548
 549         /* check tx tstamp status */
 550         if (stmmac_get_tx_timestamp_status(priv, p)) {
 551                 stmmac_get_timestamp(priv, p, priv->adv_ts, &ns);
 552                 found = true;
 553         } else if (!stmmac_get_mac_tx_timestamp(priv, priv->hw, &ns)) {
 554                 found = true;
 555         }
 556
 557         if (found) {
 558                 ns -= priv->plat->cdc_error_adj;
 559
 560                 memset(&shhwtstamp, 0, sizeof(struct skb_shared_hwtstamps));
 561                 shhwtstamp.hwtstamp = ns_to_ktime(ns);
 562
 563                 netdev_dbg(priv->dev, "get valid TX hw timestamp %llu\n", ns);
 564                 /* pass tstamp to stack */
 565                 skb_tstamp_tx(skb, &shhwtstamp);
 566         }
 567 }
 568
 569 /* stmmac_get_rx_hwtstamp - get HW RX timestamps
 570  * @priv: driver private structure
 571  * @p : descriptor pointer
 572  * @np : next descriptor pointer
 573  * @skb : the socket buffer
 574  * Description :
 575  * This function will read received packet's timestamp from the descriptor
 576  * and pass it to stack. It also perform some sanity checks.
 577  */
 578 static void stmmac_get_rx_hwtstamp(struct stmmac_priv *priv, struct dma_desc *p,
 579                                    struct dma_desc *np, struct sk_buff *skb)
 580 {
 581         struct skb_shared_hwtstamps *shhwtstamp = NULL;
 582         struct dma_desc *desc = p;
 583         u64 ns = 0;
 584
 585         if (!priv->hwts_rx_en)
 586                 return;
 587         /* For GMAC4, the valid timestamp is from CTX next desc. */
 588         if (priv->plat->has_gmac4 || priv->plat->has_xgmac)
 589                 desc = np;
 590
 591         /* Check if timestamp is available */
 592         if (stmmac_get_rx_timestamp_status(priv, p, np, priv->adv_ts)) {
 593                 stmmac_get_timestamp(priv, desc, priv->adv_ts, &ns);
 594
 595                 ns -= priv->plat->cdc_error_adj;
 596
 597                 netdev_dbg(priv->dev, "get valid RX hw timestamp %llu\n", ns);
 598                 shhwtstamp = skb_hwtstamps(skb);
 599                 memset(shhwtstamp, 0, sizeof(struct skb_shared_hwtstamps));
 600                 shhwtstamp->hwtstamp = ns_to_ktime(ns);
 601         } else  {
 602                 netdev_dbg(priv->dev, "cannot get RX hw timestamp\n");
 603         }
 604 }
 605
 606 /**
 607  *  stmmac_hwtstamp_set - control hardware timestamping.
 608  *  @dev: device pointer.
 609  *  @ifr: An IOCTL specific structure, that can contain a pointer to
 610  *  a proprietary structure used to pass information to the driver.
 611  *  Description:
 612  *  This function configures the MAC to enable/disable both outgoing(TX)
 613  *  and incoming(RX) packets time stamping based on user input.
 614  *  Return Value:
 615  *  0 on success and an appropriate -ve integer on failure.
 616  */
 617 static int stmmac_hwtstamp_set(struct net_device *dev, struct ifreq *ifr)
 618 {
 619         struct stmmac_priv *priv = netdev_priv(dev);
 620         struct hwtstamp_config config;
 621         u32 ptp_v2 = 0;
 622         u32 tstamp_all = 0;
 623         u32 ptp_over_ipv4_udp = 0;
 624         u32 ptp_over_ipv6_udp = 0;
 625         u32 ptp_over_ethernet = 0;
 626         u32 snap_type_sel = 0;
 627         u32 ts_master_en = 0;
 628         u32 ts_event_en = 0;
 629
 630         if (!(priv->dma_cap.time_stamp || priv->adv_ts)) {
 631                 netdev_alert(priv->dev, "No support for HW time stamping\n");
 632                 priv->hwts_tx_en = 0;
 633                 priv->hwts_rx_en = 0;
 634
 635                 return -EOPNOTSUPP;
 636         }
 637
 638         if (copy_from_user(&config, ifr->ifr_data,
 639                            sizeof(config)))
 640                 return -EFAULT;
 641
 642         netdev_dbg(priv->dev, "%s config flags:0x%x, tx_type:0x%x, rx_filter:0x%x\n",
 643                    __func__, config.flags, config.tx_type, config.rx_filter);
 644
 645         if (config.tx_type != HWTSTAMP_TX_OFF &&
 646             config.tx_type != HWTSTAMP_TX_ON)
 647                 return -ERANGE;
 648
 649         if (priv->adv_ts) {
 650                 switch (config.rx_filter) {
 651                 case HWTSTAMP_FILTER_NONE:
 652                         /* time stamp no incoming packet at all */
 653                         config.rx_filter = HWTSTAMP_FILTER_NONE;
 654                         break;
 655
 656                 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 657                         /* PTP v1, UDP, any kind of event packet */
 658                         config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
 659                         /* 'xmac' hardware can support Sync, Pdelay_Req and
 660                          * Pdelay_resp by setting bit14 and bits17/16 to 01
 661                          * This leaves Delay_Req timestamps out.
 662                          * Enable all events *and* general purpose message
 663                          * timestamping
 664                          */
 665                         snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
 666                         ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 667                         ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 668                         break;
 669
 670                 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 671                         /* PTP v1, UDP, Sync packet */
 672                         config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_SYNC;
 673                         /* take time stamp for SYNC messages only */
 674                         ts_event_en = PTP_TCR_TSEVNTENA;
 675
 676                         ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 677                         ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 678                         break;
 679
 680                 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
 681                         /* PTP v1, UDP, Delay_req packet */
 682                         config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ;
 683                         /* take time stamp for Delay_Req messages only */
 684                         ts_master_en = PTP_TCR_TSMSTRENA;
 685                         ts_event_en = PTP_TCR_TSEVNTENA;
 686
 687                         ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 688                         ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 689                         break;
 690
 691                 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 692                         /* PTP v2, UDP, any kind of event packet */
 693                         config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
 694                         ptp_v2 = PTP_TCR_TSVER2ENA;
 695                         /* take time stamp for all event messages */
 696                         snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
 697
 698                         ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 699                         ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 700                         break;
 701
 702                 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 703                         /* PTP v2, UDP, Sync packet */
 704                         config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_SYNC;
 705                         ptp_v2 = PTP_TCR_TSVER2ENA;
 706                         /* take time stamp for SYNC messages only */
 707                         ts_event_en = PTP_TCR_TSEVNTENA;
 708
 709                         ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 710                         ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 711                         break;
 712
 713                 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
 714                         /* PTP v2, UDP, Delay_req packet */
 715                         config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ;
 716                         ptp_v2 = PTP_TCR_TSVER2ENA;
 717                         /* take time stamp for Delay_Req messages only */
 718                         ts_master_en = PTP_TCR_TSMSTRENA;
 719                         ts_event_en = PTP_TCR_TSEVNTENA;
 720
 721                         ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 722                         ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 723                         break;
 724
 725                 case HWTSTAMP_FILTER_PTP_V2_EVENT:
 726                         /* PTP v2/802.AS1 any layer, any kind of event packet */
 727                         config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 728                         ptp_v2 = PTP_TCR_TSVER2ENA;
 729                         snap_type_sel = PTP_TCR_SNAPTYPSEL_1;
 730                         if (priv->synopsys_id < DWMAC_CORE_4_10)
 731                                 ts_event_en = PTP_TCR_TSEVNTENA;
 732                         ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 733                         ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 734                         ptp_over_ethernet = PTP_TCR_TSIPENA;
 735                         break;
 736
 737                 case HWTSTAMP_FILTER_PTP_V2_SYNC:
 738                         /* PTP v2/802.AS1, any layer, Sync packet */
 739                         config.rx_filter = HWTSTAMP_FILTER_PTP_V2_SYNC;
 740                         ptp_v2 = PTP_TCR_TSVER2ENA;
 741                         /* take time stamp for SYNC messages only */
 742                         ts_event_en = PTP_TCR_TSEVNTENA;
 743
 744                         ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 745                         ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 746                         ptp_over_ethernet = PTP_TCR_TSIPENA;
 747                         break;
 748
 749                 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 750                         /* PTP v2/802.AS1, any layer, Delay_req packet */
 751                         config.rx_filter = HWTSTAMP_FILTER_PTP_V2_DELAY_REQ;
 752                         ptp_v2 = PTP_TCR_TSVER2ENA;
 753                         /* take time stamp for Delay_Req messages only */
 754                         ts_master_en = PTP_TCR_TSMSTRENA;
 755                         ts_event_en = PTP_TCR_TSEVNTENA;
 756
 757                         ptp_over_ipv4_udp = PTP_TCR_TSIPV4ENA;
 758                         ptp_over_ipv6_udp = PTP_TCR_TSIPV6ENA;
 759                         ptp_over_ethernet = PTP_TCR_TSIPENA;
 760                         break;
 761
 762                 case HWTSTAMP_FILTER_NTP_ALL:
 763                 case HWTSTAMP_FILTER_ALL:
 764                         /* time stamp any incoming packet */
 765                         config.rx_filter = HWTSTAMP_FILTER_ALL;
 766                         tstamp_all = PTP_TCR_TSENALL;
 767                         break;
 768
 769                 default:
 770                         return -ERANGE;
 771                 }
 772         } else {
 773                 switch (config.rx_filter) {
 774                 case HWTSTAMP_FILTER_NONE:
 775                         config.rx_filter = HWTSTAMP_FILTER_NONE;
 776                         break;
 777                 default:
 778                         /* PTP v1, UDP, any kind of event packet */
 779                         config.rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
 780                         break;
 781                 }
 782         }
 783         priv->hwts_rx_en = ((config.rx_filter == HWTSTAMP_FILTER_NONE) ? 0 : 1);
 784         priv->hwts_tx_en = config.tx_type == HWTSTAMP_TX_ON;
 785
 786         priv->systime_flags = STMMAC_HWTS_ACTIVE;
 787
 788         if (priv->hwts_tx_en || priv->hwts_rx_en) {
 789                 priv->systime_flags |= tstamp_all | ptp_v2 |
 790                                        ptp_over_ethernet | ptp_over_ipv6_udp |
 791                                        ptp_over_ipv4_udp | ts_event_en |
 792                                        ts_master_en | snap_type_sel;
 793         }
 794
 795         stmmac_config_hw_tstamping(priv, priv->ptpaddr, priv->systime_flags);
 796
 797         memcpy(&priv->tstamp_config, &config, sizeof(config));
 798
 799         return copy_to_user(ifr->ifr_data, &config,
 800                             sizeof(config)) ? -EFAULT : 0;
 801 }
 802
 803 /**
 804  *  stmmac_hwtstamp_get - read hardware timestamping.
 805  *  @dev: device pointer.
 806  *  @ifr: An IOCTL specific structure, that can contain a pointer to
 807  *  a proprietary structure used to pass information to the driver.
 808  *  Description:
 809  *  This function obtain the current hardware timestamping settings
 810  *  as requested.
 811  */
 812 static int stmmac_hwtstamp_get(struct net_device *dev, struct ifreq *ifr)
 813 {
 814         struct stmmac_priv *priv = netdev_priv(dev);
 815         struct hwtstamp_config *config = &priv->tstamp_config;
 816
 817         if (!(priv->dma_cap.time_stamp || priv->dma_cap.atime_stamp))
 818                 return -EOPNOTSUPP;
 819
 820         return copy_to_user(ifr->ifr_data, config,
 821                             sizeof(*config)) ? -EFAULT : 0;
 822 }
 823
 824 /**
 825  * stmmac_init_tstamp_counter - init hardware timestamping counter
 826  * @priv: driver private structure
 827  * @systime_flags: timestamping flags
 828  * Description:
 829  * Initialize hardware counter for packet timestamping.
 830  * This is valid as long as the interface is open and not suspended.
 831  * Will be rerun after resuming from suspend, case in which the timestamping
 832  * flags updated by stmmac_hwtstamp_set() also need to be restored.
 833  */
 834 int stmmac_init_tstamp_counter(struct stmmac_priv *priv, u32 systime_flags)
 835 {
 836         bool xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac;
 837         struct timespec64 now;
 838         u32 sec_inc = 0;
 839         u64 temp = 0;
 840
 841         if (!(priv->dma_cap.time_stamp || priv->dma_cap.atime_stamp))
 842                 return -EOPNOTSUPP;
 843
 844         stmmac_config_hw_tstamping(priv, priv->ptpaddr, systime_flags);
 845         priv->systime_flags = systime_flags;
 846
 847         /* program Sub Second Increment reg */
 848         stmmac_config_sub_second_increment(priv, priv->ptpaddr,
 849                                            priv->plat->clk_ptp_rate,
 850                                            xmac, &sec_inc);
 851         temp = div_u64(1000000000ULL, sec_inc);
 852
 853         /* Store sub second increment for later use */
 854         priv->sub_second_inc = sec_inc;
 855
 856         /* calculate default added value:
 857          * formula is :
 858          * addend = (2^32)/freq_div_ratio;
 859          * where, freq_div_ratio = 1e9ns/sec_inc
 860          */
 861         temp = (u64)(temp << 32);
 862         priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate);
 863         stmmac_config_addend(priv, priv->ptpaddr, priv->default_addend);
 864
 865         /* initialize system time */
 866         ktime_get_real_ts64(&now);
 867
 868         /* lower 32 bits of tv_sec are safe until y2106 */
 869         stmmac_init_systime(priv, priv->ptpaddr, (u32)now.tv_sec, now.tv_nsec);
 870
 871         return 0;
 872 }
 873 EXPORT_SYMBOL_GPL(stmmac_init_tstamp_counter);
 874
 875 /**
 876  * stmmac_init_ptp - init PTP
 877  * @priv: driver private structure
 878  * Description: this is to verify if the HW supports the PTPv1 or PTPv2.
 879  * This is done by looking at the HW cap. register.
 880  * This function also registers the ptp driver.
 881  */
 882 static int stmmac_init_ptp(struct stmmac_priv *priv)
 883 {
 884         bool xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac;
 885         int ret;
 886
 887         if (priv->plat->ptp_clk_freq_config)
 888                 priv->plat->ptp_clk_freq_config(priv);
 889
 890         ret = stmmac_init_tstamp_counter(priv, STMMAC_HWTS_ACTIVE);
 891         if (ret)
 892                 return ret;
 893
 894         priv->adv_ts = 0;
 895         /* Check if adv_ts can be enabled for dwmac 4.x / xgmac core */
 896         if (xmac && priv->dma_cap.atime_stamp)
 897                 priv->adv_ts = 1;
 898         /* Dwmac 3.x core with extend_desc can support adv_ts */
 899         else if (priv->extend_desc && priv->dma_cap.atime_stamp)
 900                 priv->adv_ts = 1;
 901
 902         if (priv->dma_cap.time_stamp)
 903                 netdev_info(priv->dev, "IEEE 1588-2002 Timestamp supported\n");
 904
 905         if (priv->adv_ts)
 906                 netdev_info(priv->dev,
 907                             "IEEE 1588-2008 Advanced Timestamp supported\n");
 908
 909         priv->hwts_tx_en = 0;
 910         priv->hwts_rx_en = 0;
 911
 912         return 0;
 913 }
 914
 915 static void stmmac_release_ptp(struct stmmac_priv *priv)
 916 {
 917         clk_disable_unprepare(priv->plat->clk_ptp_ref);
 918         stmmac_ptp_unregister(priv);
 919 }
 920
 921 /**
 922  *  stmmac_mac_flow_ctrl - Configure flow control in all queues
 923  *  @priv: driver private structure
 924  *  @duplex: duplex passed to the next function
 925  *  Description: It is used for configuring the flow control in all queues
 926  */
 927 static void stmmac_mac_flow_ctrl(struct stmmac_priv *priv, u32 duplex)
 928 {
 929         u32 tx_cnt = priv->plat->tx_queues_to_use;
 930
 931         stmmac_flow_ctrl(priv, priv->hw, duplex, priv->flow_ctrl,
 932                         priv->pause, tx_cnt);
 933 }
 934
 935 static struct phylink_pcs *stmmac_mac_select_pcs(struct phylink_config *config,
 936                                                  phy_interface_t interface)
 937 {
 938         struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev));
 939
 940         if (!priv->hw->xpcs)
 941                 return NULL;
 942
 943         return &priv->hw->xpcs->pcs;
 944 }
 945
 946 static void stmmac_mac_config(struct phylink_config *config, unsigned int mode,
 947                               const struct phylink_link_state *state)
 948 {
 949         /* Nothing to do, xpcs_config() handles everything */
 950 }
 951
 952 static void stmmac_fpe_link_state_handle(struct stmmac_priv *priv, bool is_up)
 953 {
 954         struct stmmac_fpe_cfg *fpe_cfg = priv->plat->fpe_cfg;
 955         enum stmmac_fpe_state *lo_state = &fpe_cfg->lo_fpe_state;
 956         enum stmmac_fpe_state *lp_state = &fpe_cfg->lp_fpe_state;
 957         bool *hs_enable = &fpe_cfg->hs_enable;
 958
 959         if (is_up && *hs_enable) {
 960                 stmmac_fpe_send_mpacket(priv, priv->ioaddr, MPACKET_VERIFY);
 961         } else {
 962                 *lo_state = FPE_STATE_OFF;
 963                 *lp_state = FPE_STATE_OFF;
 964         }
 965 }
 966
 967 static void stmmac_mac_link_down(struct phylink_config *config,
 968                                  unsigned int mode, phy_interface_t interface)
 969 {
 970         struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev));
 971
 972         stmmac_mac_set(priv, priv->ioaddr, false);
 973         priv->eee_active = false;
 974         priv->tx_lpi_enabled = false;
 975         priv->eee_enabled = stmmac_eee_init(priv);
 976         stmmac_set_eee_pls(priv, priv->hw, false);
 977
 978         if (priv->dma_cap.fpesel)
 979                 stmmac_fpe_link_state_handle(priv, false);
 980 }
 981
 982 static void stmmac_mac_link_up(struct phylink_config *config,
 983                                struct phy_device *phy,
 984                                unsigned int mode, phy_interface_t interface,
 985                                int speed, int duplex,
 986                                bool tx_pause, bool rx_pause)
 987 {
 988         struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev));
 989         u32 old_ctrl, ctrl;
 990
 991         old_ctrl = readl(priv->ioaddr + MAC_CTRL_REG);
 992         ctrl = old_ctrl & ~priv->hw->link.speed_mask;
 993
 994         if (interface == PHY_INTERFACE_MODE_USXGMII) {
 995                 switch (speed) {
 996                 case SPEED_10000:
 997                         ctrl |= priv->hw->link.xgmii.speed10000;
 998                         break;
 999                 case SPEED_5000:
1000                         ctrl |= priv->hw->link.xgmii.speed5000;
1001                         break;
1002                 case SPEED_2500:
1003                         ctrl |= priv->hw->link.xgmii.speed2500;
1004                         break;
1005                 default:
1006                         return;
1007                 }
1008         } else if (interface == PHY_INTERFACE_MODE_XLGMII) {
1009                 switch (speed) {
1010                 case SPEED_100000:
1011                         ctrl |= priv->hw->link.xlgmii.speed100000;
1012                         break;
1013                 case SPEED_50000:
1014                         ctrl |= priv->hw->link.xlgmii.speed50000;
1015                         break;
1016                 case SPEED_40000:
1017                         ctrl |= priv->hw->link.xlgmii.speed40000;
1018                         break;
1019                 case SPEED_25000:
1020                         ctrl |= priv->hw->link.xlgmii.speed25000;
1021                         break;
1022                 case SPEED_10000:
1023                         ctrl |= priv->hw->link.xgmii.speed10000;
1024                         break;
1025                 case SPEED_2500:
1026                         ctrl |= priv->hw->link.speed2500;
1027                         break;
1028                 case SPEED_1000:
1029                         ctrl |= priv->hw->link.speed1000;
1030                         break;
1031                 default:
1032                         return;
1033                 }
1034         } else {
1035                 switch (speed) {
1036                 case SPEED_2500:
1037                         ctrl |= priv->hw->link.speed2500;
1038                         break;
1039                 case SPEED_1000:
1040                         ctrl |= priv->hw->link.speed1000;
1041                         break;
1042                 case SPEED_100:
1043                         ctrl |= priv->hw->link.speed100;
1044                         break;
1045                 case SPEED_10:
1046                         ctrl |= priv->hw->link.speed10;
1047                         break;
1048                 default:
1049                         return;
1050                 }
1051         }
1052
1053         priv->speed = speed;
1054
1055         if (priv->plat->fix_mac_speed)
1056                 priv->plat->fix_mac_speed(priv->plat->bsp_priv, speed);
1057
1058         if (!duplex)
1059                 ctrl &= ~priv->hw->link.duplex;
1060         else
1061                 ctrl |= priv->hw->link.duplex;
1062
1063         /* Flow Control operation */
1064         if (tx_pause && rx_pause)
1065                 stmmac_mac_flow_ctrl(priv, duplex);
1066
1067         if (ctrl != old_ctrl)
1068                 writel(ctrl, priv->ioaddr + MAC_CTRL_REG);
1069
1070         stmmac_mac_set(priv, priv->ioaddr, true);
1071         if (phy && priv->dma_cap.eee) {
1072                 priv->eee_active = phy_init_eee(phy, 1) >= 0;
1073                 priv->eee_enabled = stmmac_eee_init(priv);
1074                 priv->tx_lpi_enabled = priv->eee_enabled;
1075                 stmmac_set_eee_pls(priv, priv->hw, true);
1076         }
1077
1078         if (priv->dma_cap.fpesel)
1079                 stmmac_fpe_link_state_handle(priv, true);
1080 }
1081
1082 static const struct phylink_mac_ops stmmac_phylink_mac_ops = {
1083         .validate = phylink_generic_validate,
1084         .mac_select_pcs = stmmac_mac_select_pcs,
1085         .mac_config = stmmac_mac_config,
1086         .mac_link_down = stmmac_mac_link_down,
1087         .mac_link_up = stmmac_mac_link_up,
1088 };
1089
1090 /**
1091  * stmmac_check_pcs_mode - verify if RGMII/SGMII is supported
1092  * @priv: driver private structure
1093  * Description: this is to verify if the HW supports the PCS.
1094  * Physical Coding Sublayer (PCS) interface that can be used when the MAC is
1095  * configured for the TBI, RTBI, or SGMII PHY interface.
1096  */
1097 static void stmmac_check_pcs_mode(struct stmmac_priv *priv)
1098 {
1099         int interface = priv->plat->interface;
1100
1101         if (priv->dma_cap.pcs) {
1102                 if ((interface == PHY_INTERFACE_MODE_RGMII) ||
1103                     (interface == PHY_INTERFACE_MODE_RGMII_ID) ||
1104                     (interface == PHY_INTERFACE_MODE_RGMII_RXID) ||
1105                     (interface == PHY_INTERFACE_MODE_RGMII_TXID)) {
1106                         netdev_dbg(priv->dev, "PCS RGMII support enabled\n");
1107                         priv->hw->pcs = STMMAC_PCS_RGMII;
1108                 } else if (interface == PHY_INTERFACE_MODE_SGMII) {
1109                         netdev_dbg(priv->dev, "PCS SGMII support enabled\n");
1110                         priv->hw->pcs = STMMAC_PCS_SGMII;
1111                 }
1112         }
1113 }
1114
1115 /**
1116  * stmmac_init_phy - PHY initialization
1117  * @dev: net device structure
1118  * Description: it initializes the driver's PHY state, and attaches the PHY
1119  * to the mac driver.
1120  *  Return value:
1121  *  0 on success
1122  */
1123 static int stmmac_init_phy(struct net_device *dev)
1124 {
1125         struct stmmac_priv *priv = netdev_priv(dev);
1126         struct fwnode_handle *fwnode;
1127         int ret;
1128
1129         fwnode = of_fwnode_handle(priv->plat->phylink_node);
1130         if (!fwnode)
1131                 fwnode = dev_fwnode(priv->device);
1132
1133         if (fwnode)
1134                 ret = phylink_fwnode_phy_connect(priv->phylink, fwnode, 0);
1135
1136         /* Some DT bindings do not set-up the PHY handle. Let's try to
1137          * manually parse it
1138          */
1139         if (!fwnode || ret) {
1140                 int addr = priv->plat->phy_addr;
1141                 struct phy_device *phydev;
1142
1143                 phydev = mdiobus_get_phy(priv->mii, addr);
1144                 if (!phydev) {
1145                         netdev_err(priv->dev, "no phy at addr %d\n", addr);
1146                         return -ENODEV;
1147                 }
1148
1149                 ret = phylink_connect_phy(priv->phylink, phydev);
1150         }
1151
1152         if (!priv->plat->pmt) {
1153                 struct ethtool_wolinfo wol = { .cmd = ETHTOOL_GWOL };
1154
1155                 phylink_ethtool_get_wol(priv->phylink, &wol);
1156                 device_set_wakeup_capable(priv->device, !!wol.supported);
1157         }
1158
1159         return ret;
1160 }
1161
1162 static int stmmac_phy_setup(struct stmmac_priv *priv)
1163 {
1164         struct stmmac_mdio_bus_data *mdio_bus_data = priv->plat->mdio_bus_data;
1165         struct fwnode_handle *fwnode = of_fwnode_handle(priv->plat->phylink_node);
1166         int max_speed = priv->plat->max_speed;
1167         int mode = priv->plat->phy_interface;
1168         struct phylink *phylink;
1169
1170         priv->phylink_config.dev = &priv->dev->dev;
1171         priv->phylink_config.type = PHYLINK_NETDEV;
1172         if (priv->plat->mdio_bus_data)
1173                 priv->phylink_config.ovr_an_inband =
1174                         mdio_bus_data->xpcs_an_inband;
1175
1176         if (!fwnode)
1177                 fwnode = dev_fwnode(priv->device);
1178
1179         /* Set the platform/firmware specified interface mode */
1180         __set_bit(mode, priv->phylink_config.supported_interfaces);
1181
1182         /* If we have an xpcs, it defines which PHY interfaces are supported. */
1183         if (priv->hw->xpcs)
1184                 xpcs_get_interfaces(priv->hw->xpcs,
1185                                     priv->phylink_config.supported_interfaces);
1186
1187         priv->phylink_config.mac_capabilities = MAC_ASYM_PAUSE | MAC_SYM_PAUSE |
1188                 MAC_10 | MAC_100;
1189
1190         if (!max_speed || max_speed >= 1000)
1191                 priv->phylink_config.mac_capabilities |= MAC_1000;
1192
1193         if (priv->plat->has_gmac4) {
1194                 if (!max_speed || max_speed >= 2500)
1195                         priv->phylink_config.mac_capabilities |= MAC_2500FD;
1196         } else if (priv->plat->has_xgmac) {
1197                 if (!max_speed || max_speed >= 2500)
1198                         priv->phylink_config.mac_capabilities |= MAC_2500FD;
1199                 if (!max_speed || max_speed >= 5000)
1200                         priv->phylink_config.mac_capabilities |= MAC_5000FD;
1201                 if (!max_speed || max_speed >= 10000)
1202                         priv->phylink_config.mac_capabilities |= MAC_10000FD;
1203                 if (!max_speed || max_speed >= 25000)
1204                         priv->phylink_config.mac_capabilities |= MAC_25000FD;
1205                 if (!max_speed || max_speed >= 40000)
1206                         priv->phylink_config.mac_capabilities |= MAC_40000FD;
1207                 if (!max_speed || max_speed >= 50000)
1208                         priv->phylink_config.mac_capabilities |= MAC_50000FD;
1209                 if (!max_speed || max_speed >= 100000)
1210                         priv->phylink_config.mac_capabilities |= MAC_100000FD;
1211         }
1212
1213         /* Half-Duplex can only work with single queue */
1214         if (priv->plat->tx_queues_to_use > 1)
1215                 priv->phylink_config.mac_capabilities &=
1216                         ~(MAC_10HD | MAC_100HD | MAC_1000HD);
1217
1218         phylink = phylink_create(&priv->phylink_config, fwnode,
1219                                  mode, &stmmac_phylink_mac_ops);
1220         if (IS_ERR(phylink))
1221                 return PTR_ERR(phylink);
1222
1223         priv->phylink = phylink;
1224         return 0;
1225 }
1226
1227 static void stmmac_display_rx_rings(struct stmmac_priv *priv,
1228                                     struct stmmac_dma_conf *dma_conf)
1229 {
1230         u32 rx_cnt = priv->plat->rx_queues_to_use;
1231         unsigned int desc_size;
1232         void *head_rx;
1233         u32 queue;
1234
1235         /* Display RX rings */
1236         for (queue = 0; queue < rx_cnt; queue++) {
1237                 struct stmmac_rx_queue *rx_q = &dma_conf->rx_queue[queue];
1238
1239                 pr_info("\tRX Queue %u rings\n", queue);
1240
1241                 if (priv->extend_desc) {
1242                         head_rx = (void *)rx_q->dma_erx;
1243                         desc_size = sizeof(struct dma_extended_desc);
1244                 } else {
1245                         head_rx = (void *)rx_q->dma_rx;
1246                         desc_size = sizeof(struct dma_desc);
1247                 }
1248
1249                 /* Display RX ring */
1250                 stmmac_display_ring(priv, head_rx, dma_conf->dma_rx_size, true,
1251                                     rx_q->dma_rx_phy, desc_size);
1252         }
1253 }
1254
1255 static void stmmac_display_tx_rings(struct stmmac_priv *priv,
1256                                     struct stmmac_dma_conf *dma_conf)
1257 {
1258         u32 tx_cnt = priv->plat->tx_queues_to_use;
1259         unsigned int desc_size;
1260         void *head_tx;
1261         u32 queue;
1262
1263         /* Display TX rings */
1264         for (queue = 0; queue < tx_cnt; queue++) {
1265                 struct stmmac_tx_queue *tx_q = &dma_conf->tx_queue[queue];
1266
1267                 pr_info("\tTX Queue %d rings\n", queue);
1268
1269                 if (priv->extend_desc) {
1270                         head_tx = (void *)tx_q->dma_etx;
1271                         desc_size = sizeof(struct dma_extended_desc);
1272                 } else if (tx_q->tbs & STMMAC_TBS_AVAIL) {
1273                         head_tx = (void *)tx_q->dma_entx;
1274                         desc_size = sizeof(struct dma_edesc);
1275                 } else {
1276                         head_tx = (void *)tx_q->dma_tx;
1277                         desc_size = sizeof(struct dma_desc);
1278                 }
1279
1280                 stmmac_display_ring(priv, head_tx, dma_conf->dma_tx_size, false,
1281                                     tx_q->dma_tx_phy, desc_size);
1282         }
1283 }
1284
1285 static void stmmac_display_rings(struct stmmac_priv *priv,
1286                                  struct stmmac_dma_conf *dma_conf)
1287 {
1288         /* Display RX ring */
1289         stmmac_display_rx_rings(priv, dma_conf);
1290
1291         /* Display TX ring */
1292         stmmac_display_tx_rings(priv, dma_conf);
1293 }
1294
1295 static int stmmac_set_bfsize(int mtu, int bufsize)
1296 {
1297         int ret = bufsize;
1298
1299         if (mtu >= BUF_SIZE_8KiB)
1300                 ret = BUF_SIZE_16KiB;
1301         else if (mtu >= BUF_SIZE_4KiB)
1302                 ret = BUF_SIZE_8KiB;
1303         else if (mtu >= BUF_SIZE_2KiB)
1304                 ret = BUF_SIZE_4KiB;
1305         else if (mtu > DEFAULT_BUFSIZE)
1306                 ret = BUF_SIZE_2KiB;
1307         else
1308                 ret = DEFAULT_BUFSIZE;
1309
1310         return ret;
1311 }
1312
1313 /**
1314  * stmmac_clear_rx_descriptors - clear RX descriptors
1315  * @priv: driver private structure
1316  * @dma_conf: structure to take the dma data
1317  * @queue: RX queue index
1318  * Description: this function is called to clear the RX descriptors
1319  * in case of both basic and extended descriptors are used.
1320  */
1321 static void stmmac_clear_rx_descriptors(struct stmmac_priv *priv,
1322                                         struct stmmac_dma_conf *dma_conf,
1323                                         u32 queue)
1324 {
1325         struct stmmac_rx_queue *rx_q = &dma_conf->rx_queue[queue];
1326         int i;
1327
1328         /* Clear the RX descriptors */
1329         for (i = 0; i < dma_conf->dma_rx_size; i++)
1330                 if (priv->extend_desc)
1331                         stmmac_init_rx_desc(priv, &rx_q->dma_erx[i].basic,
1332                                         priv->use_riwt, priv->mode,
1333                                         (i == dma_conf->dma_rx_size - 1),
1334                                         dma_conf->dma_buf_sz);
1335                 else
1336                         stmmac_init_rx_desc(priv, &rx_q->dma_rx[i],
1337                                         priv->use_riwt, priv->mode,
1338                                         (i == dma_conf->dma_rx_size - 1),
1339                                         dma_conf->dma_buf_sz);
1340 }
1341
1342 /**
1343  * stmmac_clear_tx_descriptors - clear tx descriptors
1344  * @priv: driver private structure
1345  * @dma_conf: structure to take the dma data
1346  * @queue: TX queue index.
1347  * Description: this function is called to clear the TX descriptors
1348  * in case of both basic and extended descriptors are used.
1349  */
1350 static void stmmac_clear_tx_descriptors(struct stmmac_priv *priv,
1351                                         struct stmmac_dma_conf *dma_conf,
1352                                         u32 queue)
1353 {
1354         struct stmmac_tx_queue *tx_q = &dma_conf->tx_queue[queue];
1355         int i;
1356
1357         /* Clear the TX descriptors */
1358         for (i = 0; i < dma_conf->dma_tx_size; i++) {
1359                 int last = (i == (dma_conf->dma_tx_size - 1));
1360                 struct dma_desc *p;
1361
1362                 if (priv->extend_desc)
1363                         p = &tx_q->dma_etx[i].basic;
1364                 else if (tx_q->tbs & STMMAC_TBS_AVAIL)
1365                         p = &tx_q->dma_entx[i].basic;
1366                 else
1367                         p = &tx_q->dma_tx[i];
1368
1369                 stmmac_init_tx_desc(priv, p, priv->mode, last);
1370         }
1371 }
1372
1373 /**
1374  * stmmac_clear_descriptors - clear descriptors
1375  * @priv: driver private structure
1376  * @dma_conf: structure to take the dma data
1377  * Description: this function is called to clear the TX and RX descriptors
1378  * in case of both basic and extended descriptors are used.
1379  */
1380 static void stmmac_clear_descriptors(struct stmmac_priv *priv,
1381                                      struct stmmac_dma_conf *dma_conf)
1382 {
1383         u32 rx_queue_cnt = priv->plat->rx_queues_to_use;
1384         u32 tx_queue_cnt = priv->plat->tx_queues_to_use;
1385         u32 queue;
1386
1387         /* Clear the RX descriptors */
1388         for (queue = 0; queue < rx_queue_cnt; queue++)
1389                 stmmac_clear_rx_descriptors(priv, dma_conf, queue);
1390
1391         /* Clear the TX descriptors */
1392         for (queue = 0; queue < tx_queue_cnt; queue++)
1393                 stmmac_clear_tx_descriptors(priv, dma_conf, queue);
1394 }
1395
1396 /**
1397  * stmmac_init_rx_buffers - init the RX descriptor buffer.
1398  * @priv: driver private structure
1399  * @dma_conf: structure to take the dma data
1400  * @p: descriptor pointer
1401  * @i: descriptor index
1402  * @flags: gfp flag
1403  * @queue: RX queue index
1404  * Description: this function is called to allocate a receive buffer, perform
1405  * the DMA mapping and init the descriptor.
1406  */
1407 static int stmmac_init_rx_buffers(struct stmmac_priv *priv,
1408                                   struct stmmac_dma_conf *dma_conf,
1409                                   struct dma_desc *p,
1410                                   int i, gfp_t flags, u32 queue)
1411 {
1412         struct stmmac_rx_queue *rx_q = &dma_conf->rx_queue[queue];
1413         struct stmmac_rx_buffer *buf = &rx_q->buf_pool[i];
1414         gfp_t gfp = (GFP_ATOMIC | __GFP_NOWARN);
1415
1416         if (priv->dma_cap.addr64 <= 32)
1417                 gfp |= GFP_DMA32;
1418
1419         if (!buf->page) {
1420                 buf->page = page_pool_alloc_pages(rx_q->page_pool, gfp);
1421                 if (!buf->page)
1422                         return -ENOMEM;
1423                 buf->page_offset = stmmac_rx_offset(priv);
1424         }
1425
1426         if (priv->sph && !buf->sec_page) {
1427                 buf->sec_page = page_pool_alloc_pages(rx_q->page_pool, gfp);
1428                 if (!buf->sec_page)
1429                         return -ENOMEM;
1430
1431                 buf->sec_addr = page_pool_get_dma_addr(buf->sec_page);
1432                 stmmac_set_desc_sec_addr(priv, p, buf->sec_addr, true);
1433         } else {
1434                 buf->sec_page = NULL;
1435                 stmmac_set_desc_sec_addr(priv, p, buf->sec_addr, false);
1436         }
1437
1438         buf->addr = page_pool_get_dma_addr(buf->page) + buf->page_offset;
1439
1440         stmmac_set_desc_addr(priv, p, buf->addr);
1441         if (dma_conf->dma_buf_sz == BUF_SIZE_16KiB)
1442                 stmmac_init_desc3(priv, p);
1443
1444         return 0;
1445 }
1446
1447 /**
1448  * stmmac_free_rx_buffer - free RX dma buffers
1449  * @priv: private structure
1450  * @rx_q: RX queue
1451  * @i: buffer index.
1452  */
1453 static void stmmac_free_rx_buffer(struct stmmac_priv *priv,
1454                                   struct stmmac_rx_queue *rx_q,
1455                                   int i)
1456 {
1457         struct stmmac_rx_buffer *buf = &rx_q->buf_pool[i];
1458
1459         if (buf->page)
1460                 page_pool_put_full_page(rx_q->page_pool, buf->page, false);
1461         buf->page = NULL;
1462
1463         if (buf->sec_page)
1464                 page_pool_put_full_page(rx_q->page_pool, buf->sec_page, false);
1465         buf->sec_page = NULL;
1466 }
1467
1468 /**
1469  * stmmac_free_tx_buffer - free RX dma buffers
1470  * @priv: private structure
1471  * @dma_conf: structure to take the dma data
1472  * @queue: RX queue index
1473  * @i: buffer index.
1474  */
1475 static void stmmac_free_tx_buffer(struct stmmac_priv *priv,
1476                                   struct stmmac_dma_conf *dma_conf,
1477                                   u32 queue, int i)
1478 {
1479         struct stmmac_tx_queue *tx_q = &dma_conf->tx_queue[queue];
1480
1481         if (tx_q->tx_skbuff_dma[i].buf &&
1482             tx_q->tx_skbuff_dma[i].buf_type != STMMAC_TXBUF_T_XDP_TX) {
1483                 if (tx_q->tx_skbuff_dma[i].map_as_page)
1484                         dma_unmap_page(priv->device,
1485                                        tx_q->tx_skbuff_dma[i].buf,
1486                                        tx_q->tx_skbuff_dma[i].len,
1487                                        DMA_TO_DEVICE);
1488                 else
1489                         dma_unmap_single(priv->device,
1490                                          tx_q->tx_skbuff_dma[i].buf,
1491                                          tx_q->tx_skbuff_dma[i].len,
1492                                          DMA_TO_DEVICE);
1493         }
1494
1495         if (tx_q->xdpf[i] &&
1496             (tx_q->tx_skbuff_dma[i].buf_type == STMMAC_TXBUF_T_XDP_TX ||
1497              tx_q->tx_skbuff_dma[i].buf_type == STMMAC_TXBUF_T_XDP_NDO)) {
1498                 xdp_return_frame(tx_q->xdpf[i]);
1499                 tx_q->xdpf[i] = NULL;
1500         }
1501
1502         if (tx_q->tx_skbuff_dma[i].buf_type == STMMAC_TXBUF_T_XSK_TX)
1503                 tx_q->xsk_frames_done++;
1504
1505         if (tx_q->tx_skbuff[i] &&
1506             tx_q->tx_skbuff_dma[i].buf_type == STMMAC_TXBUF_T_SKB) {
1507                 dev_kfree_skb_any(tx_q->tx_skbuff[i]);
1508                 tx_q->tx_skbuff[i] = NULL;
1509         }
1510
1511         tx_q->tx_skbuff_dma[i].buf = 0;
1512         tx_q->tx_skbuff_dma[i].map_as_page = false;
1513 }
1514
1515 /**
1516  * dma_free_rx_skbufs - free RX dma buffers
1517  * @priv: private structure
1518  * @dma_conf: structure to take the dma data
1519  * @queue: RX queue index
1520  */
1521 static void dma_free_rx_skbufs(struct stmmac_priv *priv,
1522                                struct stmmac_dma_conf *dma_conf,
1523                                u32 queue)
1524 {
1525         struct stmmac_rx_queue *rx_q = &dma_conf->rx_queue[queue];
1526         int i;
1527
1528         for (i = 0; i < dma_conf->dma_rx_size; i++)
1529                 stmmac_free_rx_buffer(priv, rx_q, i);
1530 }
1531
1532 static int stmmac_alloc_rx_buffers(struct stmmac_priv *priv,
1533                                    struct stmmac_dma_conf *dma_conf,
1534                                    u32 queue, gfp_t flags)
1535 {
1536         struct stmmac_rx_queue *rx_q = &dma_conf->rx_queue[queue];
1537         int i;
1538
1539         for (i = 0; i < dma_conf->dma_rx_size; i++) {
1540                 struct dma_desc *p;
1541                 int ret;
1542
1543                 if (priv->extend_desc)
1544                         p = &((rx_q->dma_erx + i)->basic);
1545                 else
1546                         p = rx_q->dma_rx + i;
1547
1548                 ret = stmmac_init_rx_buffers(priv, dma_conf, p, i, flags,
1549                                              queue);
1550                 if (ret)
1551                         return ret;
1552
1553                 rx_q->buf_alloc_num++;
1554         }
1555
1556         return 0;
1557 }
1558
1559 /**
1560  * dma_free_rx_xskbufs - free RX dma buffers from XSK pool
1561  * @priv: private structure
1562  * @dma_conf: structure to take the dma data
1563  * @queue: RX queue index
1564  */
1565 static void dma_free_rx_xskbufs(struct stmmac_priv *priv,
1566                                 struct stmmac_dma_conf *dma_conf,
1567                                 u32 queue)
1568 {
1569         struct stmmac_rx_queue *rx_q = &dma_conf->rx_queue[queue];
1570         int i;
1571
1572         for (i = 0; i < dma_conf->dma_rx_size; i++) {
1573                 struct stmmac_rx_buffer *buf = &rx_q->buf_pool[i];
1574
1575                 if (!buf->xdp)
1576                         continue;
1577
1578                 xsk_buff_free(buf->xdp);
1579                 buf->xdp = NULL;
1580         }
1581 }
1582
1583 static int stmmac_alloc_rx_buffers_zc(struct stmmac_priv *priv,
1584                                       struct stmmac_dma_conf *dma_conf,
1585                                       u32 queue)
1586 {
1587         struct stmmac_rx_queue *rx_q = &dma_conf->rx_queue[queue];
1588         int i;
1589
1590         for (i = 0; i < dma_conf->dma_rx_size; i++) {
1591                 struct stmmac_rx_buffer *buf;
1592                 dma_addr_t dma_addr;
1593                 struct dma_desc *p;
1594
1595                 if (priv->extend_desc)
1596                         p = (struct dma_desc *)(rx_q->dma_erx + i);
1597                 else
1598                         p = rx_q->dma_rx + i;
1599
1600                 buf = &rx_q->buf_pool[i];
1601
1602                 buf->xdp = xsk_buff_alloc(rx_q->xsk_pool);
1603                 if (!buf->xdp)
1604                         return -ENOMEM;
1605
1606                 dma_addr = xsk_buff_xdp_get_dma(buf->xdp);
1607                 stmmac_set_desc_addr(priv, p, dma_addr);
1608                 rx_q->buf_alloc_num++;
1609         }
1610
1611         return 0;
1612 }
1613
1614 static struct xsk_buff_pool *stmmac_get_xsk_pool(struct stmmac_priv *priv, u32 queue)
1615 {
1616         if (!stmmac_xdp_is_enabled(priv) || !test_bit(queue, priv->af_xdp_zc_qps))
1617                 return NULL;
1618
1619         return xsk_get_pool_from_qid(priv->dev, queue);
1620 }
1621
1622 /**
1623  * __init_dma_rx_desc_rings - init the RX descriptor ring (per queue)
1624  * @priv: driver private structure
1625  * @dma_conf: structure to take the dma data
1626  * @queue: RX queue index
1627  * @flags: gfp flag.
1628  * Description: this function initializes the DMA RX descriptors
1629  * and allocates the socket buffers. It supports the chained and ring
1630  * modes.
1631  */
1632 static int __init_dma_rx_desc_rings(struct stmmac_priv *priv,
1633                                     struct stmmac_dma_conf *dma_conf,
1634                                     u32 queue, gfp_t flags)
1635 {
1636         struct stmmac_rx_queue *rx_q = &dma_conf->rx_queue[queue];
1637         int ret;
1638
1639         netif_dbg(priv, probe, priv->dev,
1640                   "(%s) dma_rx_phy=0x%08x\n", __func__,
1641                   (u32)rx_q->dma_rx_phy);
1642
1643         stmmac_clear_rx_descriptors(priv, dma_conf, queue);
1644
1645         xdp_rxq_info_unreg_mem_model(&rx_q->xdp_rxq);
1646
1647         rx_q->xsk_pool = stmmac_get_xsk_pool(priv, queue);
1648
1649         if (rx_q->xsk_pool) {
1650                 WARN_ON(xdp_rxq_info_reg_mem_model(&rx_q->xdp_rxq,
1651                                                    MEM_TYPE_XSK_BUFF_POOL,
1652                                                    NULL));
1653                 netdev_info(priv->dev,
1654                             "Register MEM_TYPE_XSK_BUFF_POOL RxQ-%d\n",
1655                             rx_q->queue_index);
1656                 xsk_pool_set_rxq_info(rx_q->xsk_pool, &rx_q->xdp_rxq);
1657         } else {
1658                 WARN_ON(xdp_rxq_info_reg_mem_model(&rx_q->xdp_rxq,
1659                                                    MEM_TYPE_PAGE_POOL,
1660                                                    rx_q->page_pool));
1661                 netdev_info(priv->dev,
1662                             "Register MEM_TYPE_PAGE_POOL RxQ-%d\n",
1663                             rx_q->queue_index);
1664         }
1665
1666         if (rx_q->xsk_pool) {
1667                 /* RX XDP ZC buffer pool may not be populated, e.g.
1668                  * xdpsock TX-only.
1669                  */
1670                 stmmac_alloc_rx_buffers_zc(priv, dma_conf, queue);
1671         } else {
1672                 ret = stmmac_alloc_rx_buffers(priv, dma_conf, queue, flags);
1673                 if (ret < 0)
1674                         return -ENOMEM;
1675         }
1676
1677         /* Setup the chained descriptor addresses */
1678         if (priv->mode == STMMAC_CHAIN_MODE) {
1679                 if (priv->extend_desc)
1680                         stmmac_mode_init(priv, rx_q->dma_erx,
1681                                          rx_q->dma_rx_phy,
1682                                          dma_conf->dma_rx_size, 1);
1683                 else
1684                         stmmac_mode_init(priv, rx_q->dma_rx,
1685                                          rx_q->dma_rx_phy,
1686                                          dma_conf->dma_rx_size, 0);
1687         }
1688
1689         return 0;
1690 }
1691
1692 static int init_dma_rx_desc_rings(struct net_device *dev,
1693                                   struct stmmac_dma_conf *dma_conf,
1694                                   gfp_t flags)
1695 {
1696         struct stmmac_priv *priv = netdev_priv(dev);
1697         u32 rx_count = priv->plat->rx_queues_to_use;
1698         int queue;
1699         int ret;
1700
1701         /* RX INITIALIZATION */
1702         netif_dbg(priv, probe, priv->dev,
1703                   "SKB addresses:\nskb\t\tskb data\tdma data\n");
1704
1705         for (queue = 0; queue < rx_count; queue++) {
1706                 ret = __init_dma_rx_desc_rings(priv, dma_conf, queue, flags);
1707                 if (ret)
1708                         goto err_init_rx_buffers;
1709         }
1710
1711         return 0;
1712
1713 err_init_rx_buffers:
1714         while (queue >= 0) {
1715                 struct stmmac_rx_queue *rx_q = &dma_conf->rx_queue[queue];
1716
1717                 if (rx_q->xsk_pool)
1718                         dma_free_rx_xskbufs(priv, dma_conf, queue);
1719                 else
1720                         dma_free_rx_skbufs(priv, dma_conf, queue);
1721
1722                 rx_q->buf_alloc_num = 0;
1723                 rx_q->xsk_pool = NULL;
1724
1725                 queue--;
1726         }
1727
1728         return ret;
1729 }
1730
1731 /**
1732  * __init_dma_tx_desc_rings - init the TX descriptor ring (per queue)
1733  * @priv: driver private structure
1734  * @dma_conf: structure to take the dma data
1735  * @queue: TX queue index
1736  * Description: this function initializes the DMA TX descriptors
1737  * and allocates the socket buffers. It supports the chained and ring
1738  * modes.
1739  */
1740 static int __init_dma_tx_desc_rings(struct stmmac_priv *priv,
1741                                     struct stmmac_dma_conf *dma_conf,
1742                                     u32 queue)
1743 {
1744         struct stmmac_tx_queue *tx_q = &dma_conf->tx_queue[queue];
1745         int i;
1746
1747         netif_dbg(priv, probe, priv->dev,
1748                   "(%s) dma_tx_phy=0x%08x\n", __func__,
1749                   (u32)tx_q->dma_tx_phy);
1750
1751         /* Setup the chained descriptor addresses */
1752         if (priv->mode == STMMAC_CHAIN_MODE) {
1753                 if (priv->extend_desc)
1754                         stmmac_mode_init(priv, tx_q->dma_etx,
1755                                          tx_q->dma_tx_phy,
1756                                          dma_conf->dma_tx_size, 1);
1757                 else if (!(tx_q->tbs & STMMAC_TBS_AVAIL))
1758                         stmmac_mode_init(priv, tx_q->dma_tx,
1759                                          tx_q->dma_tx_phy,
1760                                          dma_conf->dma_tx_size, 0);
1761         }
1762
1763         tx_q->xsk_pool = stmmac_get_xsk_pool(priv, queue);
1764
1765         for (i = 0; i < dma_conf->dma_tx_size; i++) {
1766                 struct dma_desc *p;
1767
1768                 if (priv->extend_desc)
1769                         p = &((tx_q->dma_etx + i)->basic);
1770                 else if (tx_q->tbs & STMMAC_TBS_AVAIL)
1771                         p = &((tx_q->dma_entx + i)->basic);
1772                 else
1773                         p = tx_q->dma_tx + i;
1774
1775                 stmmac_clear_desc(priv, p);
1776
1777                 tx_q->tx_skbuff_dma[i].buf = 0;
1778                 tx_q->tx_skbuff_dma[i].map_as_page = false;
1779                 tx_q->tx_skbuff_dma[i].len = 0;
1780                 tx_q->tx_skbuff_dma[i].last_segment = false;
1781                 tx_q->tx_skbuff[i] = NULL;
1782         }
1783
1784         return 0;
1785 }
1786
1787 static int init_dma_tx_desc_rings(struct net_device *dev,
1788                                   struct stmmac_dma_conf *dma_conf)
1789 {
1790         struct stmmac_priv *priv = netdev_priv(dev);
1791         u32 tx_queue_cnt;
1792         u32 queue;
1793
1794         tx_queue_cnt = priv->plat->tx_queues_to_use;
1795
1796         for (queue = 0; queue < tx_queue_cnt; queue++)
1797                 __init_dma_tx_desc_rings(priv, dma_conf, queue);
1798
1799         return 0;
1800 }
1801
1802 /**
1803  * init_dma_desc_rings - init the RX/TX descriptor rings
1804  * @dev: net device structure
1805  * @dma_conf: structure to take the dma data
1806  * @flags: gfp flag.
1807  * Description: this function initializes the DMA RX/TX descriptors
1808  * and allocates the socket buffers. It supports the chained and ring
1809  * modes.
1810  */
1811 static int init_dma_desc_rings(struct net_device *dev,
1812                                struct stmmac_dma_conf *dma_conf,
1813                                gfp_t flags)
1814 {
1815         struct stmmac_priv *priv = netdev_priv(dev);
1816         int ret;
1817
1818         ret = init_dma_rx_desc_rings(dev, dma_conf, flags);
1819         if (ret)
1820                 return ret;
1821
1822         ret = init_dma_tx_desc_rings(dev, dma_conf);
1823
1824         stmmac_clear_descriptors(priv, dma_conf);
1825
1826         if (netif_msg_hw(priv))
1827                 stmmac_display_rings(priv, dma_conf);
1828
1829         return ret;
1830 }
1831
1832 /**
1833  * dma_free_tx_skbufs - free TX dma buffers
1834  * @priv: private structure
1835  * @dma_conf: structure to take the dma data
1836  * @queue: TX queue index
1837  */
1838 static void dma_free_tx_skbufs(struct stmmac_priv *priv,
1839                                struct stmmac_dma_conf *dma_conf,
1840                                u32 queue)
1841 {
1842         struct stmmac_tx_queue *tx_q = &dma_conf->tx_queue[queue];
1843         int i;
1844
1845         tx_q->xsk_frames_done = 0;
1846
1847         for (i = 0; i < dma_conf->dma_tx_size; i++)
1848                 stmmac_free_tx_buffer(priv, dma_conf, queue, i);
1849
1850         if (tx_q->xsk_pool && tx_q->xsk_frames_done) {
1851                 xsk_tx_completed(tx_q->xsk_pool, tx_q->xsk_frames_done);
1852                 tx_q->xsk_frames_done = 0;
1853                 tx_q->xsk_pool = NULL;
1854         }
1855 }
1856
1857 /**
1858  * stmmac_free_tx_skbufs - free TX skb buffers
1859  * @priv: private structure
1860  */
1861 static void stmmac_free_tx_skbufs(struct stmmac_priv *priv)
1862 {
1863         u32 tx_queue_cnt = priv->plat->tx_queues_to_use;
1864         u32 queue;
1865
1866         for (queue = 0; queue < tx_queue_cnt; queue++)
1867                 dma_free_tx_skbufs(priv, &priv->dma_conf, queue);
1868 }
1869
1870 /**
1871  * __free_dma_rx_desc_resources - free RX dma desc resources (per queue)
1872  * @priv: private structure
1873  * @dma_conf: structure to take the dma data
1874  * @queue: RX queue index
1875  */
1876 static void __free_dma_rx_desc_resources(struct stmmac_priv *priv,
1877                                          struct stmmac_dma_conf *dma_conf,
1878                                          u32 queue)
1879 {
1880         struct stmmac_rx_queue *rx_q = &dma_conf->rx_queue[queue];
1881
1882         /* Release the DMA RX socket buffers */
1883         if (rx_q->xsk_pool)
1884                 dma_free_rx_xskbufs(priv, dma_conf, queue);
1885         else
1886                 dma_free_rx_skbufs(priv, dma_conf, queue);
1887
1888         rx_q->buf_alloc_num = 0;
1889         rx_q->xsk_pool = NULL;
1890
1891         /* Free DMA regions of consistent memory previously allocated */
1892         if (!priv->extend_desc)
1893                 dma_free_coherent(priv->device, dma_conf->dma_rx_size *
1894                                   sizeof(struct dma_desc),
1895                                   rx_q->dma_rx, rx_q->dma_rx_phy);
1896         else
1897                 dma_free_coherent(priv->device, dma_conf->dma_rx_size *
1898                                   sizeof(struct dma_extended_desc),
1899                                   rx_q->dma_erx, rx_q->dma_rx_phy);
1900
1901         if (xdp_rxq_info_is_reg(&rx_q->xdp_rxq))
1902                 xdp_rxq_info_unreg(&rx_q->xdp_rxq);
1903
1904         kfree(rx_q->buf_pool);
1905         if (rx_q->page_pool)
1906                 page_pool_destroy(rx_q->page_pool);
1907 }
1908
1909 static void free_dma_rx_desc_resources(struct stmmac_priv *priv,
1910                                        struct stmmac_dma_conf *dma_conf)
1911 {
1912         u32 rx_count = priv->plat->rx_queues_to_use;
1913         u32 queue;
1914
1915         /* Free RX queue resources */
1916         for (queue = 0; queue < rx_count; queue++)
1917                 __free_dma_rx_desc_resources(priv, dma_conf, queue);
1918 }
1919
1920 /**
1921  * __free_dma_tx_desc_resources - free TX dma desc resources (per queue)
1922  * @priv: private structure
1923  * @dma_conf: structure to take the dma data
1924  * @queue: TX queue index
1925  */
1926 static void __free_dma_tx_desc_resources(struct stmmac_priv *priv,
1927                                          struct stmmac_dma_conf *dma_conf,
1928                                          u32 queue)
1929 {
1930         struct stmmac_tx_queue *tx_q = &dma_conf->tx_queue[queue];
1931         size_t size;
1932         void *addr;
1933
1934         /* Release the DMA TX socket buffers */
1935         dma_free_tx_skbufs(priv, dma_conf, queue);
1936
1937         if (priv->extend_desc) {
1938                 size = sizeof(struct dma_extended_desc);
1939                 addr = tx_q->dma_etx;
1940         } else if (tx_q->tbs & STMMAC_TBS_AVAIL) {
1941                 size = sizeof(struct dma_edesc);
1942                 addr = tx_q->dma_entx;
1943         } else {
1944                 size = sizeof(struct dma_desc);
1945                 addr = tx_q->dma_tx;
1946         }
1947
1948         size *= dma_conf->dma_tx_size;
1949
1950         dma_free_coherent(priv->device, size, addr, tx_q->dma_tx_phy);
1951
1952         kfree(tx_q->tx_skbuff_dma);
1953         kfree(tx_q->tx_skbuff);
1954 }
1955
1956 static void free_dma_tx_desc_resources(struct stmmac_priv *priv,
1957                                        struct stmmac_dma_conf *dma_conf)
1958 {
1959         u32 tx_count = priv->plat->tx_queues_to_use;
1960         u32 queue;
1961
1962         /* Free TX queue resources */
1963         for (queue = 0; queue < tx_count; queue++)
1964                 __free_dma_tx_desc_resources(priv, dma_conf, queue);
1965 }
1966
1967 /**
1968  * __alloc_dma_rx_desc_resources - alloc RX resources (per queue).
1969  * @priv: private structure
1970  * @dma_conf: structure to take the dma data
1971  * @queue: RX queue index
1972  * Description: according to which descriptor can be used (extend or basic)
1973  * this function allocates the resources for TX and RX paths. In case of
1974  * reception, for example, it pre-allocated the RX socket buffer in order to
1975  * allow zero-copy mechanism.
1976  */
1977 static int __alloc_dma_rx_desc_resources(struct stmmac_priv *priv,
1978                                          struct stmmac_dma_conf *dma_conf,
1979                                          u32 queue)
1980 {
1981         struct stmmac_rx_queue *rx_q = &dma_conf->rx_queue[queue];
1982         struct stmmac_channel *ch = &priv->channel[queue];
1983         bool xdp_prog = stmmac_xdp_is_enabled(priv);
1984         struct page_pool_params pp_params = { 0 };
1985         unsigned int num_pages;
1986         unsigned int napi_id;
1987         int ret;
1988
1989         rx_q->queue_index = queue;
1990         rx_q->priv_data = priv;
1991
1992         pp_params.flags = PP_FLAG_DMA_MAP | PP_FLAG_DMA_SYNC_DEV;
1993         pp_params.pool_size = dma_conf->dma_rx_size;
1994         num_pages = DIV_ROUND_UP(dma_conf->dma_buf_sz, PAGE_SIZE);
1995         pp_params.order = ilog2(num_pages);
1996         pp_params.nid = dev_to_node(priv->device);
1997         pp_params.dev = priv->device;
1998         pp_params.dma_dir = xdp_prog ? DMA_BIDIRECTIONAL : DMA_FROM_DEVICE;
1999         pp_params.offset = stmmac_rx_offset(priv);
2000         pp_params.max_len = STMMAC_MAX_RX_BUF_SIZE(num_pages);
2001
2002         rx_q->page_pool = page_pool_create(&pp_params);
2003         if (IS_ERR(rx_q->page_pool)) {
2004                 ret = PTR_ERR(rx_q->page_pool);
2005                 rx_q->page_pool = NULL;
2006                 return ret;
2007         }
2008
2009         rx_q->buf_pool = kcalloc(dma_conf->dma_rx_size,
2010                                  sizeof(*rx_q->buf_pool),
2011                                  GFP_KERNEL);
2012         if (!rx_q->buf_pool)
2013                 return -ENOMEM;
2014
2015         if (priv->extend_desc) {
2016                 rx_q->dma_erx = dma_alloc_coherent(priv->device,
2017                                                    dma_conf->dma_rx_size *
2018                                                    sizeof(struct dma_extended_desc),
2019                                                    &rx_q->dma_rx_phy,
2020                                                    GFP_KERNEL);
2021                 if (!rx_q->dma_erx)
2022                         return -ENOMEM;
2023
2024         } else {
2025                 rx_q->dma_rx = dma_alloc_coherent(priv->device,
2026                                                   dma_conf->dma_rx_size *
2027                                                   sizeof(struct dma_desc),
2028                                                   &rx_q->dma_rx_phy,
2029                                                   GFP_KERNEL);
2030                 if (!rx_q->dma_rx)
2031                         return -ENOMEM;
2032         }
2033
2034         if (stmmac_xdp_is_enabled(priv) &&
2035             test_bit(queue, priv->af_xdp_zc_qps))
2036                 napi_id = ch->rxtx_napi.napi_id;
2037         else
2038                 napi_id = ch->rx_napi.napi_id;
2039
2040         ret = xdp_rxq_info_reg(&rx_q->xdp_rxq, priv->dev,
2041                                rx_q->queue_index,
2042                                napi_id);
2043         if (ret) {
2044                 netdev_err(priv->dev, "Failed to register xdp rxq info\n");
2045                 return -EINVAL;
2046         }
2047
2048         return 0;
2049 }
2050
2051 static int alloc_dma_rx_desc_resources(struct stmmac_priv *priv,
2052                                        struct stmmac_dma_conf *dma_conf)
2053 {
2054         u32 rx_count = priv->plat->rx_queues_to_use;
2055         u32 queue;
2056         int ret;
2057
2058         /* RX queues buffers and DMA */
2059         for (queue = 0; queue < rx_count; queue++) {
2060                 ret = __alloc_dma_rx_desc_resources(priv, dma_conf, queue);
2061                 if (ret)
2062                         goto err_dma;
2063         }
2064
2065         return 0;
2066
2067 err_dma:
2068         free_dma_rx_desc_resources(priv, dma_conf);
2069
2070         return ret;
2071 }
2072
2073 /**
2074  * __alloc_dma_tx_desc_resources - alloc TX resources (per queue).
2075  * @priv: private structure
2076  * @dma_conf: structure to take the dma data
2077  * @queue: TX queue index
2078  * Description: according to which descriptor can be used (extend or basic)
2079  * this function allocates the resources for TX and RX paths. In case of
2080  * reception, for example, it pre-allocated the RX socket buffer in order to
2081  * allow zero-copy mechanism.
2082  */
2083 static int __alloc_dma_tx_desc_resources(struct stmmac_priv *priv,
2084                                          struct stmmac_dma_conf *dma_conf,
2085                                          u32 queue)
2086 {
2087         struct stmmac_tx_queue *tx_q = &dma_conf->tx_queue[queue];
2088         size_t size;
2089         void *addr;
2090
2091         tx_q->queue_index = queue;
2092         tx_q->priv_data = priv;
2093
2094         tx_q->tx_skbuff_dma = kcalloc(dma_conf->dma_tx_size,
2095                                       sizeof(*tx_q->tx_skbuff_dma),
2096                                       GFP_KERNEL);
2097         if (!tx_q->tx_skbuff_dma)
2098                 return -ENOMEM;
2099
2100         tx_q->tx_skbuff = kcalloc(dma_conf->dma_tx_size,
2101                                   sizeof(struct sk_buff *),
2102                                   GFP_KERNEL);
2103         if (!tx_q->tx_skbuff)
2104                 return -ENOMEM;
2105
2106         if (priv->extend_desc)
2107                 size = sizeof(struct dma_extended_desc);
2108         else if (tx_q->tbs & STMMAC_TBS_AVAIL)
2109                 size = sizeof(struct dma_edesc);
2110         else
2111                 size = sizeof(struct dma_desc);
2112
2113         size *= dma_conf->dma_tx_size;
2114
2115         addr = dma_alloc_coherent(priv->device, size,
2116                                   &tx_q->dma_tx_phy, GFP_KERNEL);
2117         if (!addr)
2118                 return -ENOMEM;
2119
2120         if (priv->extend_desc)
2121                 tx_q->dma_etx = addr;
2122         else if (tx_q->tbs & STMMAC_TBS_AVAIL)
2123                 tx_q->dma_entx = addr;
2124         else
2125                 tx_q->dma_tx = addr;
2126
2127         return 0;
2128 }
2129
2130 static int alloc_dma_tx_desc_resources(struct stmmac_priv *priv,
2131                                        struct stmmac_dma_conf *dma_conf)
2132 {
2133         u32 tx_count = priv->plat->tx_queues_to_use;
2134         u32 queue;
2135         int ret;
2136
2137         /* TX queues buffers and DMA */
2138         for (queue = 0; queue < tx_count; queue++) {
2139                 ret = __alloc_dma_tx_desc_resources(priv, dma_conf, queue);
2140                 if (ret)
2141                         goto err_dma;
2142         }
2143
2144         return 0;
2145
2146 err_dma:
2147         free_dma_tx_desc_resources(priv, dma_conf);
2148         return ret;
2149 }
2150
2151 /**
2152  * alloc_dma_desc_resources - alloc TX/RX resources.
2153  * @priv: private structure
2154  * @dma_conf: structure to take the dma data
2155  * Description: according to which descriptor can be used (extend or basic)
2156  * this function allocates the resources for TX and RX paths. In case of
2157  * reception, for example, it pre-allocated the RX socket buffer in order to
2158  * allow zero-copy mechanism.
2159  */
2160 static int alloc_dma_desc_resources(struct stmmac_priv *priv,
2161                                     struct stmmac_dma_conf *dma_conf)
2162 {
2163         /* RX Allocation */
2164         int ret = alloc_dma_rx_desc_resources(priv, dma_conf);
2165
2166         if (ret)
2167                 return ret;
2168
2169         ret = alloc_dma_tx_desc_resources(priv, dma_conf);
2170
2171         return ret;
2172 }
2173
2174 /**
2175  * free_dma_desc_resources - free dma desc resources
2176  * @priv: private structure
2177  * @dma_conf: structure to take the dma data
2178  */
2179 static void free_dma_desc_resources(struct stmmac_priv *priv,
2180                                     struct stmmac_dma_conf *dma_conf)
2181 {
2182         /* Release the DMA TX socket buffers */
2183         free_dma_tx_desc_resources(priv, dma_conf);
2184
2185         /* Release the DMA RX socket buffers later
2186          * to ensure all pending XDP_TX buffers are returned.
2187          */
2188         free_dma_rx_desc_resources(priv, dma_conf);
2189 }
2190
2191 /**
2192  *  stmmac_mac_enable_rx_queues - Enable MAC rx queues
2193  *  @priv: driver private structure
2194  *  Description: It is used for enabling the rx queues in the MAC
2195  */
2196 static void stmmac_mac_enable_rx_queues(struct stmmac_priv *priv)
2197 {
2198         u32 rx_queues_count = priv->plat->rx_queues_to_use;
2199         int queue;
2200         u8 mode;
2201
2202         for (queue = 0; queue < rx_queues_count; queue++) {
2203                 mode = priv->plat->rx_queues_cfg[queue].mode_to_use;
2204                 stmmac_rx_queue_enable(priv, priv->hw, mode, queue);
2205         }
2206 }
2207
2208 /**
2209  * stmmac_start_rx_dma - start RX DMA channel
2210  * @priv: driver private structure
2211  * @chan: RX channel index
2212  * Description:
2213  * This starts a RX DMA channel
2214  */
2215 static void stmmac_start_rx_dma(struct stmmac_priv *priv, u32 chan)
2216 {
2217         netdev_dbg(priv->dev, "DMA RX processes started in channel %d\n", chan);
2218         stmmac_start_rx(priv, priv->ioaddr, chan);
2219 }
2220
2221 /**
2222  * stmmac_start_tx_dma - start TX DMA channel
2223  * @priv: driver private structure
2224  * @chan: TX channel index
2225  * Description:
2226  * This starts a TX DMA channel
2227  */
2228 static void stmmac_start_tx_dma(struct stmmac_priv *priv, u32 chan)
2229 {
2230         netdev_dbg(priv->dev, "DMA TX processes started in channel %d\n", chan);
2231         stmmac_start_tx(priv, priv->ioaddr, chan);
2232 }
2233
2234 /**
2235  * stmmac_stop_rx_dma - stop RX DMA channel
2236  * @priv: driver private structure
2237  * @chan: RX channel index
2238  * Description:
2239  * This stops a RX DMA channel
2240  */
2241 static void stmmac_stop_rx_dma(struct stmmac_priv *priv, u32 chan)
2242 {
2243         netdev_dbg(priv->dev, "DMA RX processes stopped in channel %d\n", chan);
2244         stmmac_stop_rx(priv, priv->ioaddr, chan);
2245 }
2246
2247 /**
2248  * stmmac_stop_tx_dma - stop TX DMA channel
2249  * @priv: driver private structure
2250  * @chan: TX channel index
2251  * Description:
2252  * This stops a TX DMA channel
2253  */
2254 static void stmmac_stop_tx_dma(struct stmmac_priv *priv, u32 chan)
2255 {
2256         netdev_dbg(priv->dev, "DMA TX processes stopped in channel %d\n", chan);
2257         stmmac_stop_tx(priv, priv->ioaddr, chan);
2258 }
2259
2260 static void stmmac_enable_all_dma_irq(struct stmmac_priv *priv)
2261 {
2262         u32 rx_channels_count = priv->plat->rx_queues_to_use;
2263         u32 tx_channels_count = priv->plat->tx_queues_to_use;
2264         u32 dma_csr_ch = max(rx_channels_count, tx_channels_count);
2265         u32 chan;
2266
2267         for (chan = 0; chan < dma_csr_ch; chan++) {
2268                 struct stmmac_channel *ch = &priv->channel[chan];
2269                 unsigned long flags;
2270
2271                 spin_lock_irqsave(&ch->lock, flags);
2272                 stmmac_enable_dma_irq(priv, priv->ioaddr, chan, 1, 1);
2273                 spin_unlock_irqrestore(&ch->lock, flags);
2274         }
2275 }
2276
2277 /**
2278  * stmmac_start_all_dma - start all RX and TX DMA channels
2279  * @priv: driver private structure
2280  * Description:
2281  * This starts all the RX and TX DMA channels
2282  */
2283 static void stmmac_start_all_dma(struct stmmac_priv *priv)
2284 {
2285         u32 rx_channels_count = priv->plat->rx_queues_to_use;
2286         u32 tx_channels_count = priv->plat->tx_queues_to_use;
2287         u32 chan = 0;
2288
2289         for (chan = 0; chan < rx_channels_count; chan++)
2290                 stmmac_start_rx_dma(priv, chan);
2291
2292         for (chan = 0; chan < tx_channels_count; chan++)
2293                 stmmac_start_tx_dma(priv, chan);
2294 }
2295
2296 /**
2297  * stmmac_stop_all_dma - stop all RX and TX DMA channels
2298  * @priv: driver private structure
2299  * Description:
2300  * This stops the RX and TX DMA channels
2301  */
2302 static void stmmac_stop_all_dma(struct stmmac_priv *priv)
2303 {
2304         u32 rx_channels_count = priv->plat->rx_queues_to_use;
2305         u32 tx_channels_count = priv->plat->tx_queues_to_use;
2306         u32 chan = 0;
2307
2308         for (chan = 0; chan < rx_channels_count; chan++)
2309                 stmmac_stop_rx_dma(priv, chan);
2310
2311         for (chan = 0; chan < tx_channels_count; chan++)
2312                 stmmac_stop_tx_dma(priv, chan);
2313 }
2314
2315 /**
2316  *  stmmac_dma_operation_mode - HW DMA operation mode
2317  *  @priv: driver private structure
2318  *  Description: it is used for configuring the DMA operation mode register in
2319  *  order to program the tx/rx DMA thresholds or Store-And-Forward mode.
2320  */
2321 static void stmmac_dma_operation_mode(struct stmmac_priv *priv)
2322 {
2323         u32 rx_channels_count = priv->plat->rx_queues_to_use;
2324         u32 tx_channels_count = priv->plat->tx_queues_to_use;
2325         int rxfifosz = priv->plat->rx_fifo_size;
2326         int txfifosz = priv->plat->tx_fifo_size;
2327         u32 txmode = 0;
2328         u32 rxmode = 0;
2329         u32 chan = 0;
2330         u8 qmode = 0;
2331
2332         if (rxfifosz == 0)
2333                 rxfifosz = priv->dma_cap.rx_fifo_size;
2334         if (txfifosz == 0)
2335                 txfifosz = priv->dma_cap.tx_fifo_size;
2336
2337         /* Adjust for real per queue fifo size */
2338         rxfifosz /= rx_channels_count;
2339         txfifosz /= tx_channels_count;
2340
2341         if (priv->plat->force_thresh_dma_mode) {
2342                 txmode = tc;
2343                 rxmode = tc;
2344         } else if (priv->plat->force_sf_dma_mode || priv->plat->tx_coe) {
2345                 /*
2346                  * In case of GMAC, SF mode can be enabled
2347                  * to perform the TX COE in HW. This depends on:
2348                  * 1) TX COE if actually supported
2349                  * 2) There is no bugged Jumbo frame support
2350                  *    that needs to not insert csum in the TDES.
2351                  */
2352                 txmode = SF_DMA_MODE;
2353                 rxmode = SF_DMA_MODE;
2354                 priv->xstats.threshold = SF_DMA_MODE;
2355         } else {
2356                 txmode = tc;
2357                 rxmode = SF_DMA_MODE;
2358         }
2359
2360         /* configure all channels */
2361         for (chan = 0; chan < rx_channels_count; chan++) {
2362                 struct stmmac_rx_queue *rx_q = &priv->dma_conf.rx_queue[chan];
2363                 u32 buf_size;
2364
2365                 qmode = priv->plat->rx_queues_cfg[chan].mode_to_use;
2366
2367                 stmmac_dma_rx_mode(priv, priv->ioaddr, rxmode, chan,
2368                                 rxfifosz, qmode);
2369
2370                 if (rx_q->xsk_pool) {
2371                         buf_size = xsk_pool_get_rx_frame_size(rx_q->xsk_pool);
2372                         stmmac_set_dma_bfsize(priv, priv->ioaddr,
2373                                               buf_size,
2374                                               chan);
2375                 } else {
2376                         stmmac_set_dma_bfsize(priv, priv->ioaddr,
2377                                               priv->dma_conf.dma_buf_sz,
2378                                               chan);
2379                 }
2380         }
2381
2382         for (chan = 0; chan < tx_channels_count; chan++) {
2383                 qmode = priv->plat->tx_queues_cfg[chan].mode_to_use;
2384
2385                 stmmac_dma_tx_mode(priv, priv->ioaddr, txmode, chan,
2386                                 txfifosz, qmode);
2387         }
2388 }
2389
2390 static bool stmmac_xdp_xmit_zc(struct stmmac_priv *priv, u32 queue, u32 budget)
2391 {
2392         struct netdev_queue *nq = netdev_get_tx_queue(priv->dev, queue);
2393         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[queue];
2394         struct xsk_buff_pool *pool = tx_q->xsk_pool;
2395         unsigned int entry = tx_q->cur_tx;
2396         struct dma_desc *tx_desc = NULL;
2397         struct xdp_desc xdp_desc;
2398         bool work_done = true;
2399
2400         /* Avoids TX time-out as we are sharing with slow path */
2401         txq_trans_cond_update(nq);
2402
2403         budget = min(budget, stmmac_tx_avail(priv, queue));
2404
2405         while (budget-- > 0) {
2406                 dma_addr_t dma_addr;
2407                 bool set_ic;
2408
2409                 /* We are sharing with slow path and stop XSK TX desc submission when
2410                  * available TX ring is less than threshold.
2411                  */
2412                 if (unlikely(stmmac_tx_avail(priv, queue) < STMMAC_TX_XSK_AVAIL) ||
2413                     !netif_carrier_ok(priv->dev)) {
2414                         work_done = false;
2415                         break;
2416                 }
2417
2418                 if (!xsk_tx_peek_desc(pool, &xdp_desc))
2419                         break;
2420
2421                 if (likely(priv->extend_desc))
2422                         tx_desc = (struct dma_desc *)(tx_q->dma_etx + entry);
2423                 else if (tx_q->tbs & STMMAC_TBS_AVAIL)
2424                         tx_desc = &tx_q->dma_entx[entry].basic;
2425                 else
2426                         tx_desc = tx_q->dma_tx + entry;
2427
2428                 dma_addr = xsk_buff_raw_get_dma(pool, xdp_desc.addr);
2429                 xsk_buff_raw_dma_sync_for_device(pool, dma_addr, xdp_desc.len);
2430
2431                 tx_q->tx_skbuff_dma[entry].buf_type = STMMAC_TXBUF_T_XSK_TX;
2432
2433                 /* To return XDP buffer to XSK pool, we simple call
2434                  * xsk_tx_completed(), so we don't need to fill up
2435                  * 'buf' and 'xdpf'.
2436                  */
2437                 tx_q->tx_skbuff_dma[entry].buf = 0;
2438                 tx_q->xdpf[entry] = NULL;
2439
2440                 tx_q->tx_skbuff_dma[entry].map_as_page = false;
2441                 tx_q->tx_skbuff_dma[entry].len = xdp_desc.len;
2442                 tx_q->tx_skbuff_dma[entry].last_segment = true;
2443                 tx_q->tx_skbuff_dma[entry].is_jumbo = false;
2444
2445                 stmmac_set_desc_addr(priv, tx_desc, dma_addr);
2446
2447                 tx_q->tx_count_frames++;
2448
2449                 if (!priv->tx_coal_frames[queue])
2450                         set_ic = false;
2451                 else if (tx_q->tx_count_frames % priv->tx_coal_frames[queue] == 0)
2452                         set_ic = true;
2453                 else
2454                         set_ic = false;
2455
2456                 if (set_ic) {
2457                         tx_q->tx_count_frames = 0;
2458                         stmmac_set_tx_ic(priv, tx_desc);
2459                         priv->xstats.tx_set_ic_bit++;
2460                 }
2461
2462                 stmmac_prepare_tx_desc(priv, tx_desc, 1, xdp_desc.len,
2463                                        true, priv->mode, true, true,
2464                                        xdp_desc.len);
2465
2466                 stmmac_enable_dma_transmission(priv, priv->ioaddr);
2467
2468                 tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, priv->dma_conf.dma_tx_size);
2469                 entry = tx_q->cur_tx;
2470         }
2471
2472         if (tx_desc) {
2473                 stmmac_flush_tx_descriptors(priv, queue);
2474                 xsk_tx_release(pool);
2475         }
2476
2477         /* Return true if all of the 3 conditions are met
2478          *  a) TX Budget is still available
2479          *  b) work_done = true when XSK TX desc peek is empty (no more
2480          *     pending XSK TX for transmission)
2481          */
2482         return !!budget && work_done;
2483 }
2484
2485 static void stmmac_bump_dma_threshold(struct stmmac_priv *priv, u32 chan)
2486 {
2487         if (unlikely(priv->xstats.threshold != SF_DMA_MODE) && tc <= 256) {
2488                 tc += 64;
2489
2490                 if (priv->plat->force_thresh_dma_mode)
2491                         stmmac_set_dma_operation_mode(priv, tc, tc, chan);
2492                 else
2493                         stmmac_set_dma_operation_mode(priv, tc, SF_DMA_MODE,
2494                                                       chan);
2495
2496                 priv->xstats.threshold = tc;
2497         }
2498 }
2499
2500 /**
2501  * stmmac_tx_clean - to manage the transmission completion
2502  * @priv: driver private structure
2503  * @budget: napi budget limiting this functions packet handling
2504  * @queue: TX queue index
2505  * Description: it reclaims the transmit resources after transmission completes.
2506  */
2507 static int stmmac_tx_clean(struct stmmac_priv *priv, int budget, u32 queue)
2508 {
2509         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[queue];
2510         unsigned int bytes_compl = 0, pkts_compl = 0;
2511         unsigned int entry, xmits = 0, count = 0;
2512
2513         __netif_tx_lock_bh(netdev_get_tx_queue(priv->dev, queue));
2514
2515         priv->xstats.tx_clean++;
2516
2517         tx_q->xsk_frames_done = 0;
2518
2519         entry = tx_q->dirty_tx;
2520
2521         /* Try to clean all TX complete frame in 1 shot */
2522         while ((entry != tx_q->cur_tx) && count < priv->dma_conf.dma_tx_size) {
2523                 struct xdp_frame *xdpf;
2524                 struct sk_buff *skb;
2525                 struct dma_desc *p;
2526                 int status;
2527
2528                 if (tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_XDP_TX ||
2529                     tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_XDP_NDO) {
2530                         xdpf = tx_q->xdpf[entry];
2531                         skb = NULL;
2532                 } else if (tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_SKB) {
2533                         xdpf = NULL;
2534                         skb = tx_q->tx_skbuff[entry];
2535                 } else {
2536                         xdpf = NULL;
2537                         skb = NULL;
2538                 }
2539
2540                 if (priv->extend_desc)
2541                         p = (struct dma_desc *)(tx_q->dma_etx + entry);
2542                 else if (tx_q->tbs & STMMAC_TBS_AVAIL)
2543                         p = &tx_q->dma_entx[entry].basic;
2544                 else
2545                         p = tx_q->dma_tx + entry;
2546
2547                 status = stmmac_tx_status(priv, &priv->dev->stats,
2548                                 &priv->xstats, p, priv->ioaddr);
2549                 /* Check if the descriptor is owned by the DMA */
2550                 if (unlikely(status & tx_dma_own))
2551                         break;
2552
2553                 count++;
2554
2555                 /* Make sure descriptor fields are read after reading
2556                  * the own bit.
2557                  */
2558                 dma_rmb();
2559
2560                 /* Just consider the last segment and ...*/
2561                 if (likely(!(status & tx_not_ls))) {
2562                         /* ... verify the status error condition */
2563                         if (unlikely(status & tx_err)) {
2564                                 priv->dev->stats.tx_errors++;
2565                                 if (unlikely(status & tx_err_bump_tc))
2566                                         stmmac_bump_dma_threshold(priv, queue);
2567                         } else {
2568                                 priv->dev->stats.tx_packets++;
2569                                 priv->xstats.tx_pkt_n++;
2570                                 priv->xstats.txq_stats[queue].tx_pkt_n++;
2571                         }
2572                         if (skb)
2573                                 stmmac_get_tx_hwtstamp(priv, p, skb);
2574                 }
2575
2576                 if (likely(tx_q->tx_skbuff_dma[entry].buf &&
2577                            tx_q->tx_skbuff_dma[entry].buf_type != STMMAC_TXBUF_T_XDP_TX)) {
2578                         if (tx_q->tx_skbuff_dma[entry].map_as_page)
2579                                 dma_unmap_page(priv->device,
2580                                                tx_q->tx_skbuff_dma[entry].buf,
2581                                                tx_q->tx_skbuff_dma[entry].len,
2582                                                DMA_TO_DEVICE);
2583                         else
2584                                 dma_unmap_single(priv->device,
2585                                                  tx_q->tx_skbuff_dma[entry].buf,
2586                                                  tx_q->tx_skbuff_dma[entry].len,
2587                                                  DMA_TO_DEVICE);
2588                         tx_q->tx_skbuff_dma[entry].buf = 0;
2589                         tx_q->tx_skbuff_dma[entry].len = 0;
2590                         tx_q->tx_skbuff_dma[entry].map_as_page = false;
2591                 }
2592
2593                 stmmac_clean_desc3(priv, tx_q, p);
2594
2595                 tx_q->tx_skbuff_dma[entry].last_segment = false;
2596                 tx_q->tx_skbuff_dma[entry].is_jumbo = false;
2597
2598                 if (xdpf &&
2599                     tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_XDP_TX) {
2600                         xdp_return_frame_rx_napi(xdpf);
2601                         tx_q->xdpf[entry] = NULL;
2602                 }
2603
2604                 if (xdpf &&
2605                     tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_XDP_NDO) {
2606                         xdp_return_frame(xdpf);
2607                         tx_q->xdpf[entry] = NULL;
2608                 }
2609
2610                 if (tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_XSK_TX)
2611                         tx_q->xsk_frames_done++;
2612
2613                 if (tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_SKB) {
2614                         if (likely(skb)) {
2615                                 pkts_compl++;
2616                                 bytes_compl += skb->len;
2617                                 dev_consume_skb_any(skb);
2618                                 tx_q->tx_skbuff[entry] = NULL;
2619                         }
2620                 }
2621
2622                 stmmac_release_tx_desc(priv, p, priv->mode);
2623
2624                 entry = STMMAC_GET_ENTRY(entry, priv->dma_conf.dma_tx_size);
2625         }
2626         tx_q->dirty_tx = entry;
2627
2628         netdev_tx_completed_queue(netdev_get_tx_queue(priv->dev, queue),
2629                                   pkts_compl, bytes_compl);
2630
2631         if (unlikely(netif_tx_queue_stopped(netdev_get_tx_queue(priv->dev,
2632                                                                 queue))) &&
2633             stmmac_tx_avail(priv, queue) > STMMAC_TX_THRESH(priv)) {
2634
2635                 netif_dbg(priv, tx_done, priv->dev,
2636                           "%s: restart transmit\n", __func__);
2637                 netif_tx_wake_queue(netdev_get_tx_queue(priv->dev, queue));
2638         }
2639
2640         if (tx_q->xsk_pool) {
2641                 bool work_done;
2642
2643                 if (tx_q->xsk_frames_done)
2644                         xsk_tx_completed(tx_q->xsk_pool, tx_q->xsk_frames_done);
2645
2646                 if (xsk_uses_need_wakeup(tx_q->xsk_pool))
2647                         xsk_set_tx_need_wakeup(tx_q->xsk_pool);
2648
2649                 /* For XSK TX, we try to send as many as possible.
2650                  * If XSK work done (XSK TX desc empty and budget still
2651                  * available), return "budget - 1" to reenable TX IRQ.
2652                  * Else, return "budget" to make NAPI continue polling.
2653                  */
2654                 work_done = stmmac_xdp_xmit_zc(priv, queue,
2655                                                STMMAC_XSK_TX_BUDGET_MAX);
2656                 if (work_done)
2657                         xmits = budget - 1;
2658                 else
2659                         xmits = budget;
2660         }
2661
2662         if (priv->eee_enabled && !priv->tx_path_in_lpi_mode &&
2663             priv->eee_sw_timer_en) {
2664                 if (stmmac_enable_eee_mode(priv))
2665                         mod_timer(&priv->eee_ctrl_timer, STMMAC_LPI_T(priv->tx_lpi_timer));
2666         }
2667
2668         /* We still have pending packets, let's call for a new scheduling */
2669         if (tx_q->dirty_tx != tx_q->cur_tx)
2670                 hrtimer_start(&tx_q->txtimer,
2671                               STMMAC_COAL_TIMER(priv->tx_coal_timer[queue]),
2672                               HRTIMER_MODE_REL);
2673
2674         __netif_tx_unlock_bh(netdev_get_tx_queue(priv->dev, queue));
2675
2676         /* Combine decisions from TX clean and XSK TX */
2677         return max(count, xmits);
2678 }
2679
2680 /**
2681  * stmmac_tx_err - to manage the tx error
2682  * @priv: driver private structure
2683  * @chan: channel index
2684  * Description: it cleans the descriptors and restarts the transmission
2685  * in case of transmission errors.
2686  */
2687 static void stmmac_tx_err(struct stmmac_priv *priv, u32 chan)
2688 {
2689         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[chan];
2690
2691         netif_tx_stop_queue(netdev_get_tx_queue(priv->dev, chan));
2692
2693         stmmac_stop_tx_dma(priv, chan);
2694         dma_free_tx_skbufs(priv, &priv->dma_conf, chan);
2695         stmmac_clear_tx_descriptors(priv, &priv->dma_conf, chan);
2696         stmmac_reset_tx_queue(priv, chan);
2697         stmmac_init_tx_chan(priv, priv->ioaddr, priv->plat->dma_cfg,
2698                             tx_q->dma_tx_phy, chan);
2699         stmmac_start_tx_dma(priv, chan);
2700
2701         priv->dev->stats.tx_errors++;
2702         netif_tx_wake_queue(netdev_get_tx_queue(priv->dev, chan));
2703 }
2704
2705 /**
2706  *  stmmac_set_dma_operation_mode - Set DMA operation mode by channel
2707  *  @priv: driver private structure
2708  *  @txmode: TX operating mode
2709  *  @rxmode: RX operating mode
2710  *  @chan: channel index
2711  *  Description: it is used for configuring of the DMA operation mode in
2712  *  runtime in order to program the tx/rx DMA thresholds or Store-And-Forward
2713  *  mode.
2714  */
2715 static void stmmac_set_dma_operation_mode(struct stmmac_priv *priv, u32 txmode,
2716                                           u32 rxmode, u32 chan)
2717 {
2718         u8 rxqmode = priv->plat->rx_queues_cfg[chan].mode_to_use;
2719         u8 txqmode = priv->plat->tx_queues_cfg[chan].mode_to_use;
2720         u32 rx_channels_count = priv->plat->rx_queues_to_use;
2721         u32 tx_channels_count = priv->plat->tx_queues_to_use;
2722         int rxfifosz = priv->plat->rx_fifo_size;
2723         int txfifosz = priv->plat->tx_fifo_size;
2724
2725         if (rxfifosz == 0)
2726                 rxfifosz = priv->dma_cap.rx_fifo_size;
2727         if (txfifosz == 0)
2728                 txfifosz = priv->dma_cap.tx_fifo_size;
2729
2730         /* Adjust for real per queue fifo size */
2731         rxfifosz /= rx_channels_count;
2732         txfifosz /= tx_channels_count;
2733
2734         stmmac_dma_rx_mode(priv, priv->ioaddr, rxmode, chan, rxfifosz, rxqmode);
2735         stmmac_dma_tx_mode(priv, priv->ioaddr, txmode, chan, txfifosz, txqmode);
2736 }
2737
2738 static bool stmmac_safety_feat_interrupt(struct stmmac_priv *priv)
2739 {
2740         int ret;
2741
2742         ret = stmmac_safety_feat_irq_status(priv, priv->dev,
2743                         priv->ioaddr, priv->dma_cap.asp, &priv->sstats);
2744         if (ret && (ret != -EINVAL)) {
2745                 stmmac_global_err(priv);
2746                 return true;
2747         }
2748
2749         return false;
2750 }
2751
2752 static int stmmac_napi_check(struct stmmac_priv *priv, u32 chan, u32 dir)
2753 {
2754         int status = stmmac_dma_interrupt_status(priv, priv->ioaddr,
2755                                                  &priv->xstats, chan, dir);
2756         struct stmmac_rx_queue *rx_q = &priv->dma_conf.rx_queue[chan];
2757         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[chan];
2758         struct stmmac_channel *ch = &priv->channel[chan];
2759         struct napi_struct *rx_napi;
2760         struct napi_struct *tx_napi;
2761         unsigned long flags;
2762
2763         rx_napi = rx_q->xsk_pool ? &ch->rxtx_napi : &ch->rx_napi;
2764         tx_napi = tx_q->xsk_pool ? &ch->rxtx_napi : &ch->tx_napi;
2765
2766         if ((status & handle_rx) && (chan < priv->plat->rx_queues_to_use)) {
2767                 if (napi_schedule_prep(rx_napi)) {
2768                         spin_lock_irqsave(&ch->lock, flags);
2769                         stmmac_disable_dma_irq(priv, priv->ioaddr, chan, 1, 0);
2770                         spin_unlock_irqrestore(&ch->lock, flags);
2771                         __napi_schedule(rx_napi);
2772                 }
2773         }
2774
2775         if ((status & handle_tx) && (chan < priv->plat->tx_queues_to_use)) {
2776                 if (napi_schedule_prep(tx_napi)) {
2777                         spin_lock_irqsave(&ch->lock, flags);
2778                         stmmac_disable_dma_irq(priv, priv->ioaddr, chan, 0, 1);
2779                         spin_unlock_irqrestore(&ch->lock, flags);
2780                         __napi_schedule(tx_napi);
2781                 }
2782         }
2783
2784         return status;
2785 }
2786
2787 /**
2788  * stmmac_dma_interrupt - DMA ISR
2789  * @priv: driver private structure
2790  * Description: this is the DMA ISR. It is called by the main ISR.
2791  * It calls the dwmac dma routine and schedule poll method in case of some
2792  * work can be done.
2793  */
2794 static void stmmac_dma_interrupt(struct stmmac_priv *priv)
2795 {
2796         u32 tx_channel_count = priv->plat->tx_queues_to_use;
2797         u32 rx_channel_count = priv->plat->rx_queues_to_use;
2798         u32 channels_to_check = tx_channel_count > rx_channel_count ?
2799                                 tx_channel_count : rx_channel_count;
2800         u32 chan;
2801         int status[max_t(u32, MTL_MAX_TX_QUEUES, MTL_MAX_RX_QUEUES)];
2802
2803         /* Make sure we never check beyond our status buffer. */
2804         if (WARN_ON_ONCE(channels_to_check > ARRAY_SIZE(status)))
2805                 channels_to_check = ARRAY_SIZE(status);
2806
2807         for (chan = 0; chan < channels_to_check; chan++)
2808                 status[chan] = stmmac_napi_check(priv, chan,
2809                                                  DMA_DIR_RXTX);
2810
2811         for (chan = 0; chan < tx_channel_count; chan++) {
2812                 if (unlikely(status[chan] & tx_hard_error_bump_tc)) {
2813                         /* Try to bump up the dma threshold on this failure */
2814                         stmmac_bump_dma_threshold(priv, chan);
2815                 } else if (unlikely(status[chan] == tx_hard_error)) {
2816                         stmmac_tx_err(priv, chan);
2817                 }
2818         }
2819 }
2820
2821 /**
2822  * stmmac_mmc_setup: setup the Mac Management Counters (MMC)
2823  * @priv: driver private structure
2824  * Description: this masks the MMC irq, in fact, the counters are managed in SW.
2825  */
2826 static void stmmac_mmc_setup(struct stmmac_priv *priv)
2827 {
2828         unsigned int mode = MMC_CNTRL_RESET_ON_READ | MMC_CNTRL_COUNTER_RESET |
2829                             MMC_CNTRL_PRESET | MMC_CNTRL_FULL_HALF_PRESET;
2830
2831         stmmac_mmc_intr_all_mask(priv, priv->mmcaddr);
2832
2833         if (priv->dma_cap.rmon) {
2834                 stmmac_mmc_ctrl(priv, priv->mmcaddr, mode);
2835                 memset(&priv->mmc, 0, sizeof(struct stmmac_counters));
2836         } else
2837                 netdev_info(priv->dev, "No MAC Management Counters available\n");
2838 }
2839
2840 /**
2841  * stmmac_get_hw_features - get MAC capabilities from the HW cap. register.
2842  * @priv: driver private structure
2843  * Description:
2844  *  new GMAC chip generations have a new register to indicate the
2845  *  presence of the optional feature/functions.
2846  *  This can be also used to override the value passed through the
2847  *  platform and necessary for old MAC10/100 and GMAC chips.
2848  */
2849 static int stmmac_get_hw_features(struct stmmac_priv *priv)
2850 {
2851         return stmmac_get_hw_feature(priv, priv->ioaddr, &priv->dma_cap) == 0;
2852 }
2853
2854 /**
2855  * stmmac_check_ether_addr - check if the MAC addr is valid
2856  * @priv: driver private structure
2857  * Description:
2858  * it is to verify if the MAC address is valid, in case of failures it
2859  * generates a random MAC address
2860  */
2861 static void stmmac_check_ether_addr(struct stmmac_priv *priv)
2862 {
2863         u8 addr[ETH_ALEN];
2864
2865         if (!is_valid_ether_addr(priv->dev->dev_addr)) {
2866                 stmmac_get_umac_addr(priv, priv->hw, addr, 0);
2867                 if (is_valid_ether_addr(addr))
2868                         eth_hw_addr_set(priv->dev, addr);
2869                 else
2870                         eth_hw_addr_random(priv->dev);
2871                 dev_info(priv->device, "device MAC address %pM\n",
2872                          priv->dev->dev_addr);
2873         }
2874 }
2875
2876 /**
2877  * stmmac_init_dma_engine - DMA init.
2878  * @priv: driver private structure
2879  * Description:
2880  * It inits the DMA invoking the specific MAC/GMAC callback.
2881  * Some DMA parameters can be passed from the platform;
2882  * in case of these are not passed a default is kept for the MAC or GMAC.
2883  */
2884 static int stmmac_init_dma_engine(struct stmmac_priv *priv)
2885 {
2886         u32 rx_channels_count = priv->plat->rx_queues_to_use;
2887         u32 tx_channels_count = priv->plat->tx_queues_to_use;
2888         u32 dma_csr_ch = max(rx_channels_count, tx_channels_count);
2889         struct stmmac_rx_queue *rx_q;
2890         struct stmmac_tx_queue *tx_q;
2891         u32 chan = 0;
2892         int atds = 0;
2893         int ret = 0;
2894
2895         if (!priv->plat->dma_cfg || !priv->plat->dma_cfg->pbl) {
2896                 dev_err(priv->device, "Invalid DMA configuration\n");
2897                 return -EINVAL;
2898         }
2899
2900         if (priv->extend_desc && (priv->mode == STMMAC_RING_MODE))
2901                 atds = 1;
2902
2903         ret = stmmac_reset(priv, priv->ioaddr);
2904         if (ret) {
2905                 dev_err(priv->device, "Failed to reset the dma\n");
2906                 return ret;
2907         }
2908
2909         /* DMA Configuration */
2910         stmmac_dma_init(priv, priv->ioaddr, priv->plat->dma_cfg, atds);
2911
2912         if (priv->plat->axi)
2913                 stmmac_axi(priv, priv->ioaddr, priv->plat->axi);
2914
2915         /* DMA CSR Channel configuration */
2916         for (chan = 0; chan < dma_csr_ch; chan++) {
2917                 stmmac_init_chan(priv, priv->ioaddr, priv->plat->dma_cfg, chan);
2918                 stmmac_disable_dma_irq(priv, priv->ioaddr, chan, 1, 1);
2919         }
2920
2921         /* DMA RX Channel Configuration */
2922         for (chan = 0; chan < rx_channels_count; chan++) {
2923                 rx_q = &priv->dma_conf.rx_queue[chan];
2924
2925                 stmmac_init_rx_chan(priv, priv->ioaddr, priv->plat->dma_cfg,
2926                                     rx_q->dma_rx_phy, chan);
2927
2928                 rx_q->rx_tail_addr = rx_q->dma_rx_phy +
2929                                      (rx_q->buf_alloc_num *
2930                                       sizeof(struct dma_desc));
2931                 stmmac_set_rx_tail_ptr(priv, priv->ioaddr,
2932                                        rx_q->rx_tail_addr, chan);
2933         }
2934
2935         /* DMA TX Channel Configuration */
2936         for (chan = 0; chan < tx_channels_count; chan++) {
2937                 tx_q = &priv->dma_conf.tx_queue[chan];
2938
2939                 stmmac_init_tx_chan(priv, priv->ioaddr, priv->plat->dma_cfg,
2940                                     tx_q->dma_tx_phy, chan);
2941
2942                 tx_q->tx_tail_addr = tx_q->dma_tx_phy;
2943                 stmmac_set_tx_tail_ptr(priv, priv->ioaddr,
2944                                        tx_q->tx_tail_addr, chan);
2945         }
2946
2947         return ret;
2948 }
2949
2950 static void stmmac_tx_timer_arm(struct stmmac_priv *priv, u32 queue)
2951 {
2952         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[queue];
2953
2954         hrtimer_start(&tx_q->txtimer,
2955                       STMMAC_COAL_TIMER(priv->tx_coal_timer[queue]),
2956                       HRTIMER_MODE_REL);
2957 }
2958
2959 /**
2960  * stmmac_tx_timer - mitigation sw timer for tx.
2961  * @t: data pointer
2962  * Description:
2963  * This is the timer handler to directly invoke the stmmac_tx_clean.
2964  */
2965 static enum hrtimer_restart stmmac_tx_timer(struct hrtimer *t)
2966 {
2967         struct stmmac_tx_queue *tx_q = container_of(t, struct stmmac_tx_queue, txtimer);
2968         struct stmmac_priv *priv = tx_q->priv_data;
2969         struct stmmac_channel *ch;
2970         struct napi_struct *napi;
2971
2972         ch = &priv->channel[tx_q->queue_index];
2973         napi = tx_q->xsk_pool ? &ch->rxtx_napi : &ch->tx_napi;
2974
2975         if (likely(napi_schedule_prep(napi))) {
2976                 unsigned long flags;
2977
2978                 spin_lock_irqsave(&ch->lock, flags);
2979                 stmmac_disable_dma_irq(priv, priv->ioaddr, ch->index, 0, 1);
2980                 spin_unlock_irqrestore(&ch->lock, flags);
2981                 __napi_schedule(napi);
2982         }
2983
2984         return HRTIMER_NORESTART;
2985 }
2986
2987 /**
2988  * stmmac_init_coalesce - init mitigation options.
2989  * @priv: driver private structure
2990  * Description:
2991  * This inits the coalesce parameters: i.e. timer rate,
2992  * timer handler and default threshold used for enabling the
2993  * interrupt on completion bit.
2994  */
2995 static void stmmac_init_coalesce(struct stmmac_priv *priv)
2996 {
2997         u32 tx_channel_count = priv->plat->tx_queues_to_use;
2998         u32 rx_channel_count = priv->plat->rx_queues_to_use;
2999         u32 chan;
3000
3001         for (chan = 0; chan < tx_channel_count; chan++) {
3002                 struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[chan];
3003
3004                 priv->tx_coal_frames[chan] = STMMAC_TX_FRAMES;
3005                 priv->tx_coal_timer[chan] = STMMAC_COAL_TX_TIMER;
3006
3007                 hrtimer_init(&tx_q->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
3008                 tx_q->txtimer.function = stmmac_tx_timer;
3009         }
3010
3011         for (chan = 0; chan < rx_channel_count; chan++)
3012                 priv->rx_coal_frames[chan] = STMMAC_RX_FRAMES;
3013 }
3014
3015 static void stmmac_set_rings_length(struct stmmac_priv *priv)
3016 {
3017         u32 rx_channels_count = priv->plat->rx_queues_to_use;
3018         u32 tx_channels_count = priv->plat->tx_queues_to_use;
3019         u32 chan;
3020
3021         /* set TX ring length */
3022         for (chan = 0; chan < tx_channels_count; chan++)
3023                 stmmac_set_tx_ring_len(priv, priv->ioaddr,
3024                                        (priv->dma_conf.dma_tx_size - 1), chan);
3025
3026         /* set RX ring length */
3027         for (chan = 0; chan < rx_channels_count; chan++)
3028                 stmmac_set_rx_ring_len(priv, priv->ioaddr,
3029                                        (priv->dma_conf.dma_rx_size - 1), chan);
3030 }
3031
3032 /**
3033  *  stmmac_set_tx_queue_weight - Set TX queue weight
3034  *  @priv: driver private structure
3035  *  Description: It is used for setting TX queues weight
3036  */
3037 static void stmmac_set_tx_queue_weight(struct stmmac_priv *priv)
3038 {
3039         u32 tx_queues_count = priv->plat->tx_queues_to_use;
3040         u32 weight;
3041         u32 queue;
3042
3043         for (queue = 0; queue < tx_queues_count; queue++) {
3044                 weight = priv->plat->tx_queues_cfg[queue].weight;
3045                 stmmac_set_mtl_tx_queue_weight(priv, priv->hw, weight, queue);
3046         }
3047 }
3048
3049 /**
3050  *  stmmac_configure_cbs - Configure CBS in TX queue
3051  *  @priv: driver private structure
3052  *  Description: It is used for configuring CBS in AVB TX queues
3053  */
3054 static void stmmac_configure_cbs(struct stmmac_priv *priv)
3055 {
3056         u32 tx_queues_count = priv->plat->tx_queues_to_use;
3057         u32 mode_to_use;
3058         u32 queue;
3059
3060         /* queue 0 is reserved for legacy traffic */
3061         for (queue = 1; queue < tx_queues_count; queue++) {
3062                 mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
3063                 if (mode_to_use == MTL_QUEUE_DCB)
3064                         continue;
3065
3066                 stmmac_config_cbs(priv, priv->hw,
3067                                 priv->plat->tx_queues_cfg[queue].send_slope,
3068                                 priv->plat->tx_queues_cfg[queue].idle_slope,
3069                                 priv->plat->tx_queues_cfg[queue].high_credit,
3070                                 priv->plat->tx_queues_cfg[queue].low_credit,
3071                                 queue);
3072         }
3073 }
3074
3075 /**
3076  *  stmmac_rx_queue_dma_chan_map - Map RX queue to RX dma channel
3077  *  @priv: driver private structure
3078  *  Description: It is used for mapping RX queues to RX dma channels
3079  */
3080 static void stmmac_rx_queue_dma_chan_map(struct stmmac_priv *priv)
3081 {
3082         u32 rx_queues_count = priv->plat->rx_queues_to_use;
3083         u32 queue;
3084         u32 chan;
3085
3086         for (queue = 0; queue < rx_queues_count; queue++) {
3087                 chan = priv->plat->rx_queues_cfg[queue].chan;
3088                 stmmac_map_mtl_to_dma(priv, priv->hw, queue, chan);
3089         }
3090 }
3091
3092 /**
3093  *  stmmac_mac_config_rx_queues_prio - Configure RX Queue priority
3094  *  @priv: driver private structure
3095  *  Description: It is used for configuring the RX Queue Priority
3096  */
3097 static void stmmac_mac_config_rx_queues_prio(struct stmmac_priv *priv)
3098 {
3099         u32 rx_queues_count = priv->plat->rx_queues_to_use;
3100         u32 queue;
3101         u32 prio;
3102
3103         for (queue = 0; queue < rx_queues_count; queue++) {
3104                 if (!priv->plat->rx_queues_cfg[queue].use_prio)
3105                         continue;
3106
3107                 prio = priv->plat->rx_queues_cfg[queue].prio;
3108                 stmmac_rx_queue_prio(priv, priv->hw, prio, queue);
3109         }
3110 }
3111
3112 /**
3113  *  stmmac_mac_config_tx_queues_prio - Configure TX Queue priority
3114  *  @priv: driver private structure
3115  *  Description: It is used for configuring the TX Queue Priority
3116  */
3117 static void stmmac_mac_config_tx_queues_prio(struct stmmac_priv *priv)
3118 {
3119         u32 tx_queues_count = priv->plat->tx_queues_to_use;
3120         u32 queue;
3121         u32 prio;
3122
3123         for (queue = 0; queue < tx_queues_count; queue++) {
3124                 if (!priv->plat->tx_queues_cfg[queue].use_prio)
3125                         continue;
3126
3127                 prio = priv->plat->tx_queues_cfg[queue].prio;
3128                 stmmac_tx_queue_prio(priv, priv->hw, prio, queue);
3129         }
3130 }
3131
3132 /**
3133  *  stmmac_mac_config_rx_queues_routing - Configure RX Queue Routing
3134  *  @priv: driver private structure
3135  *  Description: It is used for configuring the RX queue routing
3136  */
3137 static void stmmac_mac_config_rx_queues_routing(struct stmmac_priv *priv)
3138 {
3139         u32 rx_queues_count = priv->plat->rx_queues_to_use;
3140         u32 queue;
3141         u8 packet;
3142
3143         for (queue = 0; queue < rx_queues_count; queue++) {
3144                 /* no specific packet type routing specified for the queue */
3145                 if (priv->plat->rx_queues_cfg[queue].pkt_route == 0x0)
3146                         continue;
3147
3148                 packet = priv->plat->rx_queues_cfg[queue].pkt_route;
3149                 stmmac_rx_queue_routing(priv, priv->hw, packet, queue);
3150         }
3151 }
3152
3153 static void stmmac_mac_config_rss(struct stmmac_priv *priv)
3154 {
3155         if (!priv->dma_cap.rssen || !priv->plat->rss_en) {
3156                 priv->rss.enable = false;
3157                 return;
3158         }
3159
3160         if (priv->dev->features & NETIF_F_RXHASH)
3161                 priv->rss.enable = true;
3162         else
3163                 priv->rss.enable = false;
3164
3165         stmmac_rss_configure(priv, priv->hw, &priv->rss,
3166                              priv->plat->rx_queues_to_use);
3167 }
3168
3169 /**
3170  *  stmmac_mtl_configuration - Configure MTL
3171  *  @priv: driver private structure
3172  *  Description: It is used for configurring MTL
3173  */
3174 static void stmmac_mtl_configuration(struct stmmac_priv *priv)
3175 {
3176         u32 rx_queues_count = priv->plat->rx_queues_to_use;
3177         u32 tx_queues_count = priv->plat->tx_queues_to_use;
3178
3179         if (tx_queues_count > 1)
3180                 stmmac_set_tx_queue_weight(priv);
3181
3182         /* Configure MTL RX algorithms */
3183         if (rx_queues_count > 1)
3184                 stmmac_prog_mtl_rx_algorithms(priv, priv->hw,
3185                                 priv->plat->rx_sched_algorithm);
3186
3187         /* Configure MTL TX algorithms */
3188         if (tx_queues_count > 1)
3189                 stmmac_prog_mtl_tx_algorithms(priv, priv->hw,
3190                                 priv->plat->tx_sched_algorithm);
3191
3192         /* Configure CBS in AVB TX queues */
3193         if (tx_queues_count > 1)
3194                 stmmac_configure_cbs(priv);
3195
3196         /* Map RX MTL to DMA channels */
3197         stmmac_rx_queue_dma_chan_map(priv);
3198
3199         /* Enable MAC RX Queues */
3200         stmmac_mac_enable_rx_queues(priv);
3201
3202         /* Set RX priorities */
3203         if (rx_queues_count > 1)
3204                 stmmac_mac_config_rx_queues_prio(priv);
3205
3206         /* Set TX priorities */
3207         if (tx_queues_count > 1)
3208                 stmmac_mac_config_tx_queues_prio(priv);
3209
3210         /* Set RX routing */
3211         if (rx_queues_count > 1)
3212                 stmmac_mac_config_rx_queues_routing(priv);
3213
3214         /* Receive Side Scaling */
3215         if (rx_queues_count > 1)
3216                 stmmac_mac_config_rss(priv);
3217 }
3218
3219 static void stmmac_safety_feat_configuration(struct stmmac_priv *priv)
3220 {
3221         if (priv->dma_cap.asp) {
3222                 netdev_info(priv->dev, "Enabling Safety Features\n");
3223                 stmmac_safety_feat_config(priv, priv->ioaddr, priv->dma_cap.asp,
3224                                           priv->plat->safety_feat_cfg);
3225         } else {
3226                 netdev_info(priv->dev, "No Safety Features support found\n");
3227         }
3228 }
3229
3230 static int stmmac_fpe_start_wq(struct stmmac_priv *priv)
3231 {
3232         char *name;
3233
3234         clear_bit(__FPE_TASK_SCHED, &priv->fpe_task_state);
3235         clear_bit(__FPE_REMOVING,  &priv->fpe_task_state);
3236
3237         name = priv->wq_name;
3238         sprintf(name, "%s-fpe", priv->dev->name);
3239
3240         priv->fpe_wq = create_singlethread_workqueue(name);
3241         if (!priv->fpe_wq) {
3242                 netdev_err(priv->dev, "%s: Failed to create workqueue\n", name);
3243
3244                 return -ENOMEM;
3245         }
3246         netdev_info(priv->dev, "FPE workqueue start");
3247
3248         return 0;
3249 }
3250
3251 /**
3252  * stmmac_hw_setup - setup mac in a usable state.
3253  *  @dev : pointer to the device structure.
3254  *  @ptp_register: register PTP if set
3255  *  Description:
3256  *  this is the main function to setup the HW in a usable state because the
3257  *  dma engine is reset, the core registers are configured (e.g. AXI,
3258  *  Checksum features, timers). The DMA is ready to start receiving and
3259  *  transmitting.
3260  *  Return value:
3261  *  0 on success and an appropriate (-)ve integer as defined in errno.h
3262  *  file on failure.
3263  */
3264 static int stmmac_hw_setup(struct net_device *dev, bool ptp_register)
3265 {
3266         struct stmmac_priv *priv = netdev_priv(dev);
3267         u32 rx_cnt = priv->plat->rx_queues_to_use;
3268         u32 tx_cnt = priv->plat->tx_queues_to_use;
3269         bool sph_en;
3270         u32 chan;
3271         int ret;
3272
3273         /* DMA initialization and SW reset */
3274         ret = stmmac_init_dma_engine(priv);
3275         if (ret < 0) {
3276                 netdev_err(priv->dev, "%s: DMA engine initialization failed\n",
3277                            __func__);
3278                 return ret;
3279         }
3280
3281         /* Copy the MAC addr into the HW  */
3282         stmmac_set_umac_addr(priv, priv->hw, dev->dev_addr, 0);
3283
3284         /* PS and related bits will be programmed according to the speed */
3285         if (priv->hw->pcs) {
3286                 int speed = priv->plat->mac_port_sel_speed;
3287
3288                 if ((speed == SPEED_10) || (speed == SPEED_100) ||
3289                     (speed == SPEED_1000)) {
3290                         priv->hw->ps = speed;
3291                 } else {
3292                         dev_warn(priv->device, "invalid port speed\n");
3293                         priv->hw->ps = 0;
3294                 }
3295         }
3296
3297         /* Initialize the MAC Core */
3298         stmmac_core_init(priv, priv->hw, dev);
3299
3300         /* Initialize MTL*/
3301         stmmac_mtl_configuration(priv);
3302
3303         /* Initialize Safety Features */
3304         stmmac_safety_feat_configuration(priv);
3305
3306         ret = stmmac_rx_ipc(priv, priv->hw);
3307         if (!ret) {
3308                 netdev_warn(priv->dev, "RX IPC Checksum Offload disabled\n");
3309                 priv->plat->rx_coe = STMMAC_RX_COE_NONE;
3310                 priv->hw->rx_csum = 0;
3311         }
3312
3313         /* Enable the MAC Rx/Tx */
3314         stmmac_mac_set(priv, priv->ioaddr, true);
3315
3316         /* Set the HW DMA mode and the COE */
3317         stmmac_dma_operation_mode(priv);
3318
3319         stmmac_mmc_setup(priv);
3320
3321         if (ptp_register) {
3322                 ret = clk_prepare_enable(priv->plat->clk_ptp_ref);
3323                 if (ret < 0)
3324                         netdev_warn(priv->dev,
3325                                     "failed to enable PTP reference clock: %pe\n",
3326                                     ERR_PTR(ret));
3327         }
3328
3329         ret = stmmac_init_ptp(priv);
3330         if (ret == -EOPNOTSUPP)
3331                 netdev_info(priv->dev, "PTP not supported by HW\n");
3332         else if (ret)
3333                 netdev_warn(priv->dev, "PTP init failed\n");
3334         else if (ptp_register)
3335                 stmmac_ptp_register(priv);
3336
3337         priv->eee_tw_timer = STMMAC_DEFAULT_TWT_LS;
3338
3339         /* Convert the timer from msec to usec */
3340         if (!priv->tx_lpi_timer)
3341                 priv->tx_lpi_timer = eee_timer * 1000;
3342
3343         if (priv->use_riwt) {
3344                 u32 queue;
3345
3346                 for (queue = 0; queue < rx_cnt; queue++) {
3347                         if (!priv->rx_riwt[queue])
3348                                 priv->rx_riwt[queue] = DEF_DMA_RIWT;
3349
3350                         stmmac_rx_watchdog(priv, priv->ioaddr,
3351                                            priv->rx_riwt[queue], queue);
3352                 }
3353         }
3354
3355         if (priv->hw->pcs)
3356                 stmmac_pcs_ctrl_ane(priv, priv->ioaddr, 1, priv->hw->ps, 0);
3357
3358         /* set TX and RX rings length */
3359         stmmac_set_rings_length(priv);
3360
3361         /* Enable TSO */
3362         if (priv->tso) {
3363                 for (chan = 0; chan < tx_cnt; chan++) {
3364                         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[chan];
3365
3366                         /* TSO and TBS cannot co-exist */
3367                         if (tx_q->tbs & STMMAC_TBS_AVAIL)
3368                                 continue;
3369
3370                         stmmac_enable_tso(priv, priv->ioaddr, 1, chan);
3371                 }
3372         }
3373
3374         /* Enable Split Header */
3375         sph_en = (priv->hw->rx_csum > 0) && priv->sph;
3376         for (chan = 0; chan < rx_cnt; chan++)
3377                 stmmac_enable_sph(priv, priv->ioaddr, sph_en, chan);
3378
3379
3380         /* VLAN Tag Insertion */
3381         if (priv->dma_cap.vlins)
3382                 stmmac_enable_vlan(priv, priv->hw, STMMAC_VLAN_INSERT);
3383
3384         /* TBS */
3385         for (chan = 0; chan < tx_cnt; chan++) {
3386                 struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[chan];
3387                 int enable = tx_q->tbs & STMMAC_TBS_AVAIL;
3388
3389                 stmmac_enable_tbs(priv, priv->ioaddr, enable, chan);
3390         }
3391
3392         /* Configure real RX and TX queues */
3393         netif_set_real_num_rx_queues(dev, priv->plat->rx_queues_to_use);
3394         netif_set_real_num_tx_queues(dev, priv->plat->tx_queues_to_use);
3395
3396         /* Start the ball rolling... */
3397         stmmac_start_all_dma(priv);
3398
3399         if (priv->dma_cap.fpesel) {
3400                 stmmac_fpe_start_wq(priv);
3401
3402                 if (priv->plat->fpe_cfg->enable)
3403                         stmmac_fpe_handshake(priv, true);
3404         }
3405
3406         return 0;
3407 }
3408
3409 static void stmmac_hw_teardown(struct net_device *dev)
3410 {
3411         struct stmmac_priv *priv = netdev_priv(dev);
3412
3413         clk_disable_unprepare(priv->plat->clk_ptp_ref);
3414 }
3415
3416 static void stmmac_free_irq(struct net_device *dev,
3417                             enum request_irq_err irq_err, int irq_idx)
3418 {
3419         struct stmmac_priv *priv = netdev_priv(dev);
3420         int j;
3421
3422         switch (irq_err) {
3423         case REQ_IRQ_ERR_ALL:
3424                 irq_idx = priv->plat->tx_queues_to_use;
3425                 fallthrough;
3426         case REQ_IRQ_ERR_TX:
3427                 for (j = irq_idx - 1; j >= 0; j--) {
3428                         if (priv->tx_irq[j] > 0) {
3429                                 irq_set_affinity_hint(priv->tx_irq[j], NULL);
3430                                 free_irq(priv->tx_irq[j], &priv->dma_conf.tx_queue[j]);
3431                         }
3432                 }
3433                 irq_idx = priv->plat->rx_queues_to_use;
3434                 fallthrough;
3435         case REQ_IRQ_ERR_RX:
3436                 for (j = irq_idx - 1; j >= 0; j--) {
3437                         if (priv->rx_irq[j] > 0) {
3438                                 irq_set_affinity_hint(priv->rx_irq[j], NULL);
3439                                 free_irq(priv->rx_irq[j], &priv->dma_conf.rx_queue[j]);
3440                         }
3441                 }
3442
3443                 if (priv->sfty_ue_irq > 0 && priv->sfty_ue_irq != dev->irq)
3444                         free_irq(priv->sfty_ue_irq, dev);
3445                 fallthrough;
3446         case REQ_IRQ_ERR_SFTY_UE:
3447                 if (priv->sfty_ce_irq > 0 && priv->sfty_ce_irq != dev->irq)
3448                         free_irq(priv->sfty_ce_irq, dev);
3449                 fallthrough;
3450         case REQ_IRQ_ERR_SFTY_CE:
3451                 if (priv->lpi_irq > 0 && priv->lpi_irq != dev->irq)
3452                         free_irq(priv->lpi_irq, dev);
3453                 fallthrough;
3454         case REQ_IRQ_ERR_LPI:
3455                 if (priv->wol_irq > 0 && priv->wol_irq != dev->irq)
3456                         free_irq(priv->wol_irq, dev);
3457                 fallthrough;
3458         case REQ_IRQ_ERR_WOL:
3459                 free_irq(dev->irq, dev);
3460                 fallthrough;
3461         case REQ_IRQ_ERR_MAC:
3462         case REQ_IRQ_ERR_NO:
3463                 /* If MAC IRQ request error, no more IRQ to free */
3464                 break;
3465         }
3466 }
3467
3468 static int stmmac_request_irq_multi_msi(struct net_device *dev)
3469 {
3470         struct stmmac_priv *priv = netdev_priv(dev);
3471         enum request_irq_err irq_err;
3472         cpumask_t cpu_mask;
3473         int irq_idx = 0;
3474         char *int_name;
3475         int ret;
3476         int i;
3477
3478         /* For common interrupt */
3479         int_name = priv->int_name_mac;
3480         sprintf(int_name, "%s:%s", dev->name, "mac");
3481         ret = request_irq(dev->irq, stmmac_mac_interrupt,
3482                           0, int_name, dev);
3483         if (unlikely(ret < 0)) {
3484                 netdev_err(priv->dev,
3485                            "%s: alloc mac MSI %d (error: %d)\n",
3486                            __func__, dev->irq, ret);
3487                 irq_err = REQ_IRQ_ERR_MAC;
3488                 goto irq_error;
3489         }
3490
3491         /* Request the Wake IRQ in case of another line
3492          * is used for WoL
3493          */
3494         if (priv->wol_irq > 0 && priv->wol_irq != dev->irq) {
3495                 int_name = priv->int_name_wol;
3496                 sprintf(int_name, "%s:%s", dev->name, "wol");
3497                 ret = request_irq(priv->wol_irq,
3498                                   stmmac_mac_interrupt,
3499                                   0, int_name, dev);
3500                 if (unlikely(ret < 0)) {
3501                         netdev_err(priv->dev,
3502                                    "%s: alloc wol MSI %d (error: %d)\n",
3503                                    __func__, priv->wol_irq, ret);
3504                         irq_err = REQ_IRQ_ERR_WOL;
3505                         goto irq_error;
3506                 }
3507         }
3508
3509         /* Request the LPI IRQ in case of another line
3510          * is used for LPI
3511          */
3512         if (priv->lpi_irq > 0 && priv->lpi_irq != dev->irq) {
3513                 int_name = priv->int_name_lpi;
3514                 sprintf(int_name, "%s:%s", dev->name, "lpi");
3515                 ret = request_irq(priv->lpi_irq,
3516                                   stmmac_mac_interrupt,
3517                                   0, int_name, dev);
3518                 if (unlikely(ret < 0)) {
3519                         netdev_err(priv->dev,
3520                                    "%s: alloc lpi MSI %d (error: %d)\n",
3521                                    __func__, priv->lpi_irq, ret);
3522                         irq_err = REQ_IRQ_ERR_LPI;
3523                         goto irq_error;
3524                 }
3525         }
3526
3527         /* Request the Safety Feature Correctible Error line in
3528          * case of another line is used
3529          */
3530         if (priv->sfty_ce_irq > 0 && priv->sfty_ce_irq != dev->irq) {
3531                 int_name = priv->int_name_sfty_ce;
3532                 sprintf(int_name, "%s:%s", dev->name, "safety-ce");
3533                 ret = request_irq(priv->sfty_ce_irq,
3534                                   stmmac_safety_interrupt,
3535                                   0, int_name, dev);
3536                 if (unlikely(ret < 0)) {
3537                         netdev_err(priv->dev,
3538                                    "%s: alloc sfty ce MSI %d (error: %d)\n",
3539                                    __func__, priv->sfty_ce_irq, ret);
3540                         irq_err = REQ_IRQ_ERR_SFTY_CE;
3541                         goto irq_error;
3542                 }
3543         }
3544
3545         /* Request the Safety Feature Uncorrectible Error line in
3546          * case of another line is used
3547          */
3548         if (priv->sfty_ue_irq > 0 && priv->sfty_ue_irq != dev->irq) {
3549                 int_name = priv->int_name_sfty_ue;
3550                 sprintf(int_name, "%s:%s", dev->name, "safety-ue");
3551                 ret = request_irq(priv->sfty_ue_irq,
3552                                   stmmac_safety_interrupt,
3553                                   0, int_name, dev);
3554                 if (unlikely(ret < 0)) {
3555                         netdev_err(priv->dev,
3556                                    "%s: alloc sfty ue MSI %d (error: %d)\n",
3557                                    __func__, priv->sfty_ue_irq, ret);
3558                         irq_err = REQ_IRQ_ERR_SFTY_UE;
3559                         goto irq_error;
3560                 }
3561         }
3562
3563         /* Request Rx MSI irq */
3564         for (i = 0; i < priv->plat->rx_queues_to_use; i++) {
3565                 if (i >= MTL_MAX_RX_QUEUES)
3566                         break;
3567                 if (priv->rx_irq[i] == 0)
3568                         continue;
3569
3570                 int_name = priv->int_name_rx_irq[i];
3571                 sprintf(int_name, "%s:%s-%d", dev->name, "rx", i);
3572                 ret = request_irq(priv->rx_irq[i],
3573                                   stmmac_msi_intr_rx,
3574                                   0, int_name, &priv->dma_conf.rx_queue[i]);
3575                 if (unlikely(ret < 0)) {
3576                         netdev_err(priv->dev,
3577                                    "%s: alloc rx-%d  MSI %d (error: %d)\n",
3578                                    __func__, i, priv->rx_irq[i], ret);
3579                         irq_err = REQ_IRQ_ERR_RX;
3580                         irq_idx = i;
3581                         goto irq_error;
3582                 }
3583                 cpumask_clear(&cpu_mask);
3584                 cpumask_set_cpu(i % num_online_cpus(), &cpu_mask);
3585                 irq_set_affinity_hint(priv->rx_irq[i], &cpu_mask);
3586         }
3587
3588         /* Request Tx MSI irq */
3589         for (i = 0; i < priv->plat->tx_queues_to_use; i++) {
3590                 if (i >= MTL_MAX_TX_QUEUES)
3591                         break;
3592                 if (priv->tx_irq[i] == 0)
3593                         continue;
3594
3595                 int_name = priv->int_name_tx_irq[i];
3596                 sprintf(int_name, "%s:%s-%d", dev->name, "tx", i);
3597                 ret = request_irq(priv->tx_irq[i],
3598                                   stmmac_msi_intr_tx,
3599                                   0, int_name, &priv->dma_conf.tx_queue[i]);
3600                 if (unlikely(ret < 0)) {
3601                         netdev_err(priv->dev,
3602                                    "%s: alloc tx-%d  MSI %d (error: %d)\n",
3603                                    __func__, i, priv->tx_irq[i], ret);
3604                         irq_err = REQ_IRQ_ERR_TX;
3605                         irq_idx = i;
3606                         goto irq_error;
3607                 }
3608                 cpumask_clear(&cpu_mask);
3609                 cpumask_set_cpu(i % num_online_cpus(), &cpu_mask);
3610                 irq_set_affinity_hint(priv->tx_irq[i], &cpu_mask);
3611         }
3612
3613         return 0;
3614
3615 irq_error:
3616         stmmac_free_irq(dev, irq_err, irq_idx);
3617         return ret;
3618 }
3619
3620 static int stmmac_request_irq_single(struct net_device *dev)
3621 {
3622         struct stmmac_priv *priv = netdev_priv(dev);
3623         enum request_irq_err irq_err;
3624         int ret;
3625
3626         ret = request_irq(dev->irq, stmmac_interrupt,
3627                           IRQF_SHARED, dev->name, dev);
3628         if (unlikely(ret < 0)) {
3629                 netdev_err(priv->dev,
3630                            "%s: ERROR: allocating the IRQ %d (error: %d)\n",
3631                            __func__, dev->irq, ret);
3632                 irq_err = REQ_IRQ_ERR_MAC;
3633                 goto irq_error;
3634         }
3635
3636         /* Request the Wake IRQ in case of another line
3637          * is used for WoL
3638          */
3639         if (priv->wol_irq > 0 && priv->wol_irq != dev->irq) {
3640                 ret = request_irq(priv->wol_irq, stmmac_interrupt,
3641                                   IRQF_SHARED, dev->name, dev);
3642                 if (unlikely(ret < 0)) {
3643                         netdev_err(priv->dev,
3644                                    "%s: ERROR: allocating the WoL IRQ %d (%d)\n",
3645                                    __func__, priv->wol_irq, ret);
3646                         irq_err = REQ_IRQ_ERR_WOL;
3647                         goto irq_error;
3648                 }
3649         }
3650
3651         /* Request the IRQ lines */
3652         if (priv->lpi_irq > 0 && priv->lpi_irq != dev->irq) {
3653                 ret = request_irq(priv->lpi_irq, stmmac_interrupt,
3654                                   IRQF_SHARED, dev->name, dev);
3655                 if (unlikely(ret < 0)) {
3656                         netdev_err(priv->dev,
3657                                    "%s: ERROR: allocating the LPI IRQ %d (%d)\n",
3658                                    __func__, priv->lpi_irq, ret);
3659                         irq_err = REQ_IRQ_ERR_LPI;
3660                         goto irq_error;
3661                 }
3662         }
3663
3664         return 0;
3665
3666 irq_error:
3667         stmmac_free_irq(dev, irq_err, 0);
3668         return ret;
3669 }
3670
3671 static int stmmac_request_irq(struct net_device *dev)
3672 {
3673         struct stmmac_priv *priv = netdev_priv(dev);
3674         int ret;
3675
3676         /* Request the IRQ lines */
3677         if (priv->plat->multi_msi_en)
3678                 ret = stmmac_request_irq_multi_msi(dev);
3679         else
3680                 ret = stmmac_request_irq_single(dev);
3681
3682         return ret;
3683 }
3684
3685 /**
3686  *  stmmac_setup_dma_desc - Generate a dma_conf and allocate DMA queue
3687  *  @priv: driver private structure
3688  *  @mtu: MTU to setup the dma queue and buf with
3689  *  Description: Allocate and generate a dma_conf based on the provided MTU.
3690  *  Allocate the Tx/Rx DMA queue and init them.
3691  *  Return value:
3692  *  the dma_conf allocated struct on success and an appropriate ERR_PTR on failure.
3693  */
3694 static struct stmmac_dma_conf *
3695 stmmac_setup_dma_desc(struct stmmac_priv *priv, unsigned int mtu)
3696 {
3697         struct stmmac_dma_conf *dma_conf;
3698         int chan, bfsize, ret;
3699
3700         dma_conf = kzalloc(sizeof(*dma_conf), GFP_KERNEL);
3701         if (!dma_conf) {
3702                 netdev_err(priv->dev, "%s: DMA conf allocation failed\n",
3703                            __func__);
3704                 return ERR_PTR(-ENOMEM);
3705         }
3706
3707         bfsize = stmmac_set_16kib_bfsize(priv, mtu);
3708         if (bfsize < 0)
3709                 bfsize = 0;
3710
3711         if (bfsize < BUF_SIZE_16KiB)
3712                 bfsize = stmmac_set_bfsize(mtu, 0);
3713
3714         dma_conf->dma_buf_sz = bfsize;
3715         /* Chose the tx/rx size from the already defined one in the
3716          * priv struct. (if defined)
3717          */
3718         dma_conf->dma_tx_size = priv->dma_conf.dma_tx_size;
3719         dma_conf->dma_rx_size = priv->dma_conf.dma_rx_size;
3720
3721         if (!dma_conf->dma_tx_size)
3722                 dma_conf->dma_tx_size = DMA_DEFAULT_TX_SIZE;
3723         if (!dma_conf->dma_rx_size)
3724                 dma_conf->dma_rx_size = DMA_DEFAULT_RX_SIZE;
3725
3726         /* Earlier check for TBS */
3727         for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++) {
3728                 struct stmmac_tx_queue *tx_q = &dma_conf->tx_queue[chan];
3729                 int tbs_en = priv->plat->tx_queues_cfg[chan].tbs_en;
3730
3731                 /* Setup per-TXQ tbs flag before TX descriptor alloc */
3732                 tx_q->tbs |= tbs_en ? STMMAC_TBS_AVAIL : 0;
3733         }
3734
3735         ret = alloc_dma_desc_resources(priv, dma_conf);
3736         if (ret < 0) {
3737                 netdev_err(priv->dev, "%s: DMA descriptors allocation failed\n",
3738                            __func__);
3739                 goto alloc_error;
3740         }
3741
3742         ret = init_dma_desc_rings(priv->dev, dma_conf, GFP_KERNEL);
3743         if (ret < 0) {
3744                 netdev_err(priv->dev, "%s: DMA descriptors initialization failed\n",
3745                            __func__);
3746                 goto init_error;
3747         }
3748
3749         return dma_conf;
3750
3751 init_error:
3752         free_dma_desc_resources(priv, dma_conf);
3753 alloc_error:
3754         kfree(dma_conf);
3755         return ERR_PTR(ret);
3756 }
3757
3758 /**
3759  *  __stmmac_open - open entry point of the driver
3760  *  @dev : pointer to the device structure.
3761  *  @dma_conf :  structure to take the dma data
3762  *  Description:
3763  *  This function is the open entry point of the driver.
3764  *  Return value:
3765  *  0 on success and an appropriate (-)ve integer as defined in errno.h
3766  *  file on failure.
3767  */
3768 static int __stmmac_open(struct net_device *dev,
3769                          struct stmmac_dma_conf *dma_conf)
3770 {
3771         struct stmmac_priv *priv = netdev_priv(dev);
3772         int mode = priv->plat->phy_interface;
3773         u32 chan;
3774         int ret;
3775
3776         ret = pm_runtime_resume_and_get(priv->device);
3777         if (ret < 0)
3778                 return ret;
3779
3780         if (priv->hw->pcs != STMMAC_PCS_TBI &&
3781             priv->hw->pcs != STMMAC_PCS_RTBI &&
3782             (!priv->hw->xpcs ||
3783              xpcs_get_an_mode(priv->hw->xpcs, mode) != DW_AN_C73)) {
3784                 ret = stmmac_init_phy(dev);
3785                 if (ret) {
3786                         netdev_err(priv->dev,
3787                                    "%s: Cannot attach to PHY (error: %d)\n",
3788                                    __func__, ret);
3789                         goto init_phy_error;
3790                 }
3791         }
3792
3793         /* Extra statistics */
3794         memset(&priv->xstats, 0, sizeof(struct stmmac_extra_stats));
3795         priv->xstats.threshold = tc;
3796
3797         priv->rx_copybreak = STMMAC_RX_COPYBREAK;
3798
3799         buf_sz = dma_conf->dma_buf_sz;
3800         memcpy(&priv->dma_conf, dma_conf, sizeof(*dma_conf));
3801
3802         stmmac_reset_queues_param(priv);
3803
3804         if (priv->plat->serdes_powerup) {
3805                 ret = priv->plat->serdes_powerup(dev, priv->plat->bsp_priv);
3806                 if (ret < 0) {
3807                         netdev_err(priv->dev, "%s: Serdes powerup failed\n",
3808                                    __func__);
3809                         goto init_error;
3810                 }
3811         }
3812
3813         ret = stmmac_hw_setup(dev, true);
3814         if (ret < 0) {
3815                 netdev_err(priv->dev, "%s: Hw setup failed\n", __func__);
3816                 goto init_error;
3817         }
3818
3819         stmmac_init_coalesce(priv);
3820
3821         phylink_start(priv->phylink);
3822         /* We may have called phylink_speed_down before */
3823         phylink_speed_up(priv->phylink);
3824
3825         ret = stmmac_request_irq(dev);
3826         if (ret)
3827                 goto irq_error;
3828
3829         stmmac_enable_all_queues(priv);
3830         netif_tx_start_all_queues(priv->dev);
3831         stmmac_enable_all_dma_irq(priv);
3832
3833         return 0;
3834
3835 irq_error:
3836         phylink_stop(priv->phylink);
3837
3838         for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++)
3839                 hrtimer_cancel(&priv->dma_conf.tx_queue[chan].txtimer);
3840
3841         stmmac_hw_teardown(dev);
3842 init_error:
3843         free_dma_desc_resources(priv, &priv->dma_conf);
3844         phylink_disconnect_phy(priv->phylink);
3845 init_phy_error:
3846         pm_runtime_put(priv->device);
3847         return ret;
3848 }
3849
3850 static int stmmac_open(struct net_device *dev)
3851 {
3852         struct stmmac_priv *priv = netdev_priv(dev);
3853         struct stmmac_dma_conf *dma_conf;
3854         int ret;
3855
3856         dma_conf = stmmac_setup_dma_desc(priv, dev->mtu);
3857         if (IS_ERR(dma_conf))
3858                 return PTR_ERR(dma_conf);
3859
3860         ret = __stmmac_open(dev, dma_conf);
3861         kfree(dma_conf);
3862         return ret;
3863 }
3864
3865 static void stmmac_fpe_stop_wq(struct stmmac_priv *priv)
3866 {
3867         set_bit(__FPE_REMOVING, &priv->fpe_task_state);
3868
3869         if (priv->fpe_wq)
3870                 destroy_workqueue(priv->fpe_wq);
3871
3872         netdev_info(priv->dev, "FPE workqueue stop");
3873 }
3874
3875 /**
3876  *  stmmac_release - close entry point of the driver
3877  *  @dev : device pointer.
3878  *  Description:
3879  *  This is the stop entry point of the driver.
3880  */
3881 static int stmmac_release(struct net_device *dev)
3882 {
3883         struct stmmac_priv *priv = netdev_priv(dev);
3884         u32 chan;
3885
3886         if (device_may_wakeup(priv->device))
3887                 phylink_speed_down(priv->phylink, false);
3888         /* Stop and disconnect the PHY */
3889         phylink_stop(priv->phylink);
3890         phylink_disconnect_phy(priv->phylink);
3891
3892         stmmac_disable_all_queues(priv);
3893
3894         for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++)
3895                 hrtimer_cancel(&priv->dma_conf.tx_queue[chan].txtimer);
3896
3897         netif_tx_disable(dev);
3898
3899         /* Free the IRQ lines */
3900         stmmac_free_irq(dev, REQ_IRQ_ERR_ALL, 0);
3901
3902         if (priv->eee_enabled) {
3903                 priv->tx_path_in_lpi_mode = false;
3904                 del_timer_sync(&priv->eee_ctrl_timer);
3905         }
3906
3907         /* Stop TX/RX DMA and clear the descriptors */
3908         stmmac_stop_all_dma(priv);
3909
3910         /* Release and free the Rx/Tx resources */
3911         free_dma_desc_resources(priv, &priv->dma_conf);
3912
3913         /* Disable the MAC Rx/Tx */
3914         stmmac_mac_set(priv, priv->ioaddr, false);
3915
3916         /* Powerdown Serdes if there is */
3917         if (priv->plat->serdes_powerdown)
3918                 priv->plat->serdes_powerdown(dev, priv->plat->bsp_priv);
3919
3920         netif_carrier_off(dev);
3921
3922         stmmac_release_ptp(priv);
3923
3924         pm_runtime_put(priv->device);
3925
3926         if (priv->dma_cap.fpesel)
3927                 stmmac_fpe_stop_wq(priv);
3928
3929         return 0;
3930 }
3931
3932 static bool stmmac_vlan_insert(struct stmmac_priv *priv, struct sk_buff *skb,
3933                                struct stmmac_tx_queue *tx_q)
3934 {
3935         u16 tag = 0x0, inner_tag = 0x0;
3936         u32 inner_type = 0x0;
3937         struct dma_desc *p;
3938
3939         if (!priv->dma_cap.vlins)
3940                 return false;
3941         if (!skb_vlan_tag_present(skb))
3942                 return false;
3943         if (skb->vlan_proto == htons(ETH_P_8021AD)) {
3944                 inner_tag = skb_vlan_tag_get(skb);
3945                 inner_type = STMMAC_VLAN_INSERT;
3946         }
3947
3948         tag = skb_vlan_tag_get(skb);
3949
3950         if (tx_q->tbs & STMMAC_TBS_AVAIL)
3951                 p = &tx_q->dma_entx[tx_q->cur_tx].basic;
3952         else
3953                 p = &tx_q->dma_tx[tx_q->cur_tx];
3954
3955         if (stmmac_set_desc_vlan_tag(priv, p, tag, inner_tag, inner_type))
3956                 return false;
3957
3958         stmmac_set_tx_owner(priv, p);
3959         tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, priv->dma_conf.dma_tx_size);
3960         return true;
3961 }
3962
3963 /**
3964  *  stmmac_tso_allocator - close entry point of the driver
3965  *  @priv: driver private structure
3966  *  @des: buffer start address
3967  *  @total_len: total length to fill in descriptors
3968  *  @last_segment: condition for the last descriptor
3969  *  @queue: TX queue index
3970  *  Description:
3971  *  This function fills descriptor and request new descriptors according to
3972  *  buffer length to fill
3973  */
3974 static void stmmac_tso_allocator(struct stmmac_priv *priv, dma_addr_t des,
3975                                  int total_len, bool last_segment, u32 queue)
3976 {
3977         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[queue];
3978         struct dma_desc *desc;
3979         u32 buff_size;
3980         int tmp_len;
3981
3982         tmp_len = total_len;
3983
3984         while (tmp_len > 0) {
3985                 dma_addr_t curr_addr;
3986
3987                 tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx,
3988                                                 priv->dma_conf.dma_tx_size);
3989                 WARN_ON(tx_q->tx_skbuff[tx_q->cur_tx]);
3990
3991                 if (tx_q->tbs & STMMAC_TBS_AVAIL)
3992                         desc = &tx_q->dma_entx[tx_q->cur_tx].basic;
3993                 else
3994                         desc = &tx_q->dma_tx[tx_q->cur_tx];
3995
3996                 curr_addr = des + (total_len - tmp_len);
3997                 if (priv->dma_cap.addr64 <= 32)
3998                         desc->des0 = cpu_to_le32(curr_addr);
3999                 else
4000                         stmmac_set_desc_addr(priv, desc, curr_addr);
4001
4002                 buff_size = tmp_len >= TSO_MAX_BUFF_SIZE ?
4003                             TSO_MAX_BUFF_SIZE : tmp_len;
4004
4005                 stmmac_prepare_tso_tx_desc(priv, desc, 0, buff_size,
4006                                 0, 1,
4007                                 (last_segment) && (tmp_len <= TSO_MAX_BUFF_SIZE),
4008                                 0, 0);
4009
4010                 tmp_len -= TSO_MAX_BUFF_SIZE;
4011         }
4012 }
4013
4014 static void stmmac_flush_tx_descriptors(struct stmmac_priv *priv, int queue)
4015 {
4016         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[queue];
4017         int desc_size;
4018
4019         if (likely(priv->extend_desc))
4020                 desc_size = sizeof(struct dma_extended_desc);
4021         else if (tx_q->tbs & STMMAC_TBS_AVAIL)
4022                 desc_size = sizeof(struct dma_edesc);
4023         else
4024                 desc_size = sizeof(struct dma_desc);
4025
4026         /* The own bit must be the latest setting done when prepare the
4027          * descriptor and then barrier is needed to make sure that
4028          * all is coherent before granting the DMA engine.
4029          */
4030         wmb();
4031
4032         tx_q->tx_tail_addr = tx_q->dma_tx_phy + (tx_q->cur_tx * desc_size);
4033         stmmac_set_tx_tail_ptr(priv, priv->ioaddr, tx_q->tx_tail_addr, queue);
4034 }
4035
4036 /**
4037  *  stmmac_tso_xmit - Tx entry point of the driver for oversized frames (TSO)
4038  *  @skb : the socket buffer
4039  *  @dev : device pointer
4040  *  Description: this is the transmit function that is called on TSO frames
4041  *  (support available on GMAC4 and newer chips).
4042  *  Diagram below show the ring programming in case of TSO frames:
4043  *
4044  *  First Descriptor
4045  *   --------
4046  *   | DES0 |---> buffer1 = L2/L3/L4 header
4047  *   | DES1 |---> TCP Payload (can continue on next descr...)
4048  *   | DES2 |---> buffer 1 and 2 len
4049  *   | DES3 |---> must set TSE, TCP hdr len-> [22:19]. TCP payload len [17:0]
4050  *   --------
4051  *      |
4052  *     ...
4053  *      |
4054  *   --------
4055  *   | DES0 | --| Split TCP Payload on Buffers 1 and 2
4056  *   | DES1 | --|
4057  *   | DES2 | --> buffer 1 and 2 len
4058  *   | DES3 |
4059  *   --------
4060  *
4061  * mss is fixed when enable tso, so w/o programming the TDES3 ctx field.
4062  */
4063 static netdev_tx_t stmmac_tso_xmit(struct sk_buff *skb, struct net_device *dev)
4064 {
4065         struct dma_desc *desc, *first, *mss_desc = NULL;
4066         struct stmmac_priv *priv = netdev_priv(dev);
4067         int nfrags = skb_shinfo(skb)->nr_frags;
4068         u32 queue = skb_get_queue_mapping(skb);
4069         unsigned int first_entry, tx_packets;
4070         int tmp_pay_len = 0, first_tx;
4071         struct stmmac_tx_queue *tx_q;
4072         bool has_vlan, set_ic;
4073         u8 proto_hdr_len, hdr;
4074         u32 pay_len, mss;
4075         dma_addr_t des;
4076         int i;
4077
4078         tx_q = &priv->dma_conf.tx_queue[queue];
4079         first_tx = tx_q->cur_tx;
4080
4081         /* Compute header lengths */
4082         if (skb_shinfo(skb)->gso_type & SKB_GSO_UDP_L4) {
4083                 proto_hdr_len = skb_transport_offset(skb) + sizeof(struct udphdr);
4084                 hdr = sizeof(struct udphdr);
4085         } else {
4086                 proto_hdr_len = skb_tcp_all_headers(skb);
4087                 hdr = tcp_hdrlen(skb);
4088         }
4089
4090         /* Desc availability based on threshold should be enough safe */
4091         if (unlikely(stmmac_tx_avail(priv, queue) <
4092                 (((skb->len - proto_hdr_len) / TSO_MAX_BUFF_SIZE + 1)))) {
4093                 if (!netif_tx_queue_stopped(netdev_get_tx_queue(dev, queue))) {
4094                         netif_tx_stop_queue(netdev_get_tx_queue(priv->dev,
4095                                                                 queue));
4096                         /* This is a hard error, log it. */
4097                         netdev_err(priv->dev,
4098                                    "%s: Tx Ring full when queue awake\n",
4099                                    __func__);
4100                 }
4101                 return NETDEV_TX_BUSY;
4102         }
4103
4104         pay_len = skb_headlen(skb) - proto_hdr_len; /* no frags */
4105
4106         mss = skb_shinfo(skb)->gso_size;
4107
4108         /* set new MSS value if needed */
4109         if (mss != tx_q->mss) {
4110                 if (tx_q->tbs & STMMAC_TBS_AVAIL)
4111                         mss_desc = &tx_q->dma_entx[tx_q->cur_tx].basic;
4112                 else
4113                         mss_desc = &tx_q->dma_tx[tx_q->cur_tx];
4114
4115                 stmmac_set_mss(priv, mss_desc, mss);
4116                 tx_q->mss = mss;
4117                 tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx,
4118                                                 priv->dma_conf.dma_tx_size);
4119                 WARN_ON(tx_q->tx_skbuff[tx_q->cur_tx]);
4120         }
4121
4122         if (netif_msg_tx_queued(priv)) {
4123                 pr_info("%s: hdrlen %d, hdr_len %d, pay_len %d, mss %d\n",
4124                         __func__, hdr, proto_hdr_len, pay_len, mss);
4125                 pr_info("\tskb->len %d, skb->data_len %d\n", skb->len,
4126                         skb->data_len);
4127         }
4128
4129         /* Check if VLAN can be inserted by HW */
4130         has_vlan = stmmac_vlan_insert(priv, skb, tx_q);
4131
4132         first_entry = tx_q->cur_tx;
4133         WARN_ON(tx_q->tx_skbuff[first_entry]);
4134
4135         if (tx_q->tbs & STMMAC_TBS_AVAIL)
4136                 desc = &tx_q->dma_entx[first_entry].basic;
4137         else
4138                 desc = &tx_q->dma_tx[first_entry];
4139         first = desc;
4140
4141         if (has_vlan)
4142                 stmmac_set_desc_vlan(priv, first, STMMAC_VLAN_INSERT);
4143
4144         /* first descriptor: fill Headers on Buf1 */
4145         des = dma_map_single(priv->device, skb->data, skb_headlen(skb),
4146                              DMA_TO_DEVICE);
4147         if (dma_mapping_error(priv->device, des))
4148                 goto dma_map_err;
4149
4150         tx_q->tx_skbuff_dma[first_entry].buf = des;
4151         tx_q->tx_skbuff_dma[first_entry].len = skb_headlen(skb);
4152         tx_q->tx_skbuff_dma[first_entry].map_as_page = false;
4153         tx_q->tx_skbuff_dma[first_entry].buf_type = STMMAC_TXBUF_T_SKB;
4154
4155         if (priv->dma_cap.addr64 <= 32) {
4156                 first->des0 = cpu_to_le32(des);
4157
4158                 /* Fill start of payload in buff2 of first descriptor */
4159                 if (pay_len)
4160                         first->des1 = cpu_to_le32(des + proto_hdr_len);
4161
4162                 /* If needed take extra descriptors to fill the remaining payload */
4163                 tmp_pay_len = pay_len - TSO_MAX_BUFF_SIZE;
4164         } else {
4165                 stmmac_set_desc_addr(priv, first, des);
4166                 tmp_pay_len = pay_len;
4167                 des += proto_hdr_len;
4168                 pay_len = 0;
4169         }
4170
4171         stmmac_tso_allocator(priv, des, tmp_pay_len, (nfrags == 0), queue);
4172
4173         /* Prepare fragments */
4174         for (i = 0; i < nfrags; i++) {
4175                 const skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
4176
4177                 des = skb_frag_dma_map(priv->device, frag, 0,
4178                                        skb_frag_size(frag),
4179                                        DMA_TO_DEVICE);
4180                 if (dma_mapping_error(priv->device, des))
4181                         goto dma_map_err;
4182
4183                 stmmac_tso_allocator(priv, des, skb_frag_size(frag),
4184                                      (i == nfrags - 1), queue);
4185
4186                 tx_q->tx_skbuff_dma[tx_q->cur_tx].buf = des;
4187                 tx_q->tx_skbuff_dma[tx_q->cur_tx].len = skb_frag_size(frag);
4188                 tx_q->tx_skbuff_dma[tx_q->cur_tx].map_as_page = true;
4189                 tx_q->tx_skbuff_dma[tx_q->cur_tx].buf_type = STMMAC_TXBUF_T_SKB;
4190         }
4191
4192         tx_q->tx_skbuff_dma[tx_q->cur_tx].last_segment = true;
4193
4194         /* Only the last descriptor gets to point to the skb. */
4195         tx_q->tx_skbuff[tx_q->cur_tx] = skb;
4196         tx_q->tx_skbuff_dma[tx_q->cur_tx].buf_type = STMMAC_TXBUF_T_SKB;
4197
4198         /* Manage tx mitigation */
4199         tx_packets = (tx_q->cur_tx + 1) - first_tx;
4200         tx_q->tx_count_frames += tx_packets;
4201
4202         if ((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) && priv->hwts_tx_en)
4203                 set_ic = true;
4204         else if (!priv->tx_coal_frames[queue])
4205                 set_ic = false;
4206         else if (tx_packets > priv->tx_coal_frames[queue])
4207                 set_ic = true;
4208         else if ((tx_q->tx_count_frames %
4209                   priv->tx_coal_frames[queue]) < tx_packets)
4210                 set_ic = true;
4211         else
4212                 set_ic = false;
4213
4214         if (set_ic) {
4215                 if (tx_q->tbs & STMMAC_TBS_AVAIL)
4216                         desc = &tx_q->dma_entx[tx_q->cur_tx].basic;
4217                 else
4218                         desc = &tx_q->dma_tx[tx_q->cur_tx];
4219
4220                 tx_q->tx_count_frames = 0;
4221                 stmmac_set_tx_ic(priv, desc);
4222                 priv->xstats.tx_set_ic_bit++;
4223         }
4224
4225         /* We've used all descriptors we need for this skb, however,
4226          * advance cur_tx so that it references a fresh descriptor.
4227          * ndo_start_xmit will fill this descriptor the next time it's
4228          * called and stmmac_tx_clean may clean up to this descriptor.
4229          */
4230         tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, priv->dma_conf.dma_tx_size);
4231
4232         if (unlikely(stmmac_tx_avail(priv, queue) <= (MAX_SKB_FRAGS + 1))) {
4233                 netif_dbg(priv, hw, priv->dev, "%s: stop transmitted packets\n",
4234                           __func__);
4235                 netif_tx_stop_queue(netdev_get_tx_queue(priv->dev, queue));
4236         }
4237
4238         dev->stats.tx_bytes += skb->len;
4239         priv->xstats.tx_tso_frames++;
4240         priv->xstats.tx_tso_nfrags += nfrags;
4241
4242         if (priv->sarc_type)
4243                 stmmac_set_desc_sarc(priv, first, priv->sarc_type);
4244
4245         skb_tx_timestamp(skb);
4246
4247         if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
4248                      priv->hwts_tx_en)) {
4249                 /* declare that device is doing timestamping */
4250                 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
4251                 stmmac_enable_tx_timestamp(priv, first);
4252         }
4253
4254         /* Complete the first descriptor before granting the DMA */
4255         stmmac_prepare_tso_tx_desc(priv, first, 1,
4256                         proto_hdr_len,
4257                         pay_len,
4258                         1, tx_q->tx_skbuff_dma[first_entry].last_segment,
4259                         hdr / 4, (skb->len - proto_hdr_len));
4260
4261         /* If context desc is used to change MSS */
4262         if (mss_desc) {
4263                 /* Make sure that first descriptor has been completely
4264                  * written, including its own bit. This is because MSS is
4265                  * actually before first descriptor, so we need to make
4266                  * sure that MSS's own bit is the last thing written.
4267                  */
4268                 dma_wmb();
4269                 stmmac_set_tx_owner(priv, mss_desc);
4270         }
4271
4272         if (netif_msg_pktdata(priv)) {
4273                 pr_info("%s: curr=%d dirty=%d f=%d, e=%d, f_p=%p, nfrags %d\n",
4274                         __func__, tx_q->cur_tx, tx_q->dirty_tx, first_entry,
4275                         tx_q->cur_tx, first, nfrags);
4276                 pr_info(">>> frame to be transmitted: ");
4277                 print_pkt(skb->data, skb_headlen(skb));
4278         }
4279
4280         netdev_tx_sent_queue(netdev_get_tx_queue(dev, queue), skb->len);
4281
4282         stmmac_flush_tx_descriptors(priv, queue);
4283         stmmac_tx_timer_arm(priv, queue);
4284
4285         return NETDEV_TX_OK;
4286
4287 dma_map_err:
4288         dev_err(priv->device, "Tx dma map failed\n");
4289         dev_kfree_skb(skb);
4290         priv->dev->stats.tx_dropped++;
4291         return NETDEV_TX_OK;
4292 }
4293
4294 /**
4295  *  stmmac_xmit - Tx entry point of the driver
4296  *  @skb : the socket buffer
4297  *  @dev : device pointer
4298  *  Description : this is the tx entry point of the driver.
4299  *  It programs the chain or the ring and supports oversized frames
4300  *  and SG feature.
4301  */
4302 static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
4303 {
4304         unsigned int first_entry, tx_packets, enh_desc;
4305         struct stmmac_priv *priv = netdev_priv(dev);
4306         unsigned int nopaged_len = skb_headlen(skb);
4307         int i, csum_insertion = 0, is_jumbo = 0;
4308         u32 queue = skb_get_queue_mapping(skb);
4309         int nfrags = skb_shinfo(skb)->nr_frags;
4310         int gso = skb_shinfo(skb)->gso_type;
4311         struct dma_edesc *tbs_desc = NULL;
4312         struct dma_desc *desc, *first;
4313         struct stmmac_tx_queue *tx_q;
4314         bool has_vlan, set_ic;
4315         int entry, first_tx;
4316         dma_addr_t des;
4317
4318         tx_q = &priv->dma_conf.tx_queue[queue];
4319         first_tx = tx_q->cur_tx;
4320
4321         if (priv->tx_path_in_lpi_mode && priv->eee_sw_timer_en)
4322                 stmmac_disable_eee_mode(priv);
4323
4324         /* Manage oversized TCP frames for GMAC4 device */
4325         if (skb_is_gso(skb) && priv->tso) {
4326                 if (gso & (SKB_GSO_TCPV4 | SKB_GSO_TCPV6))
4327                         return stmmac_tso_xmit(skb, dev);
4328                 if (priv->plat->has_gmac4 && (gso & SKB_GSO_UDP_L4))
4329                         return stmmac_tso_xmit(skb, dev);
4330         }
4331
4332         if (unlikely(stmmac_tx_avail(priv, queue) < nfrags + 1)) {
4333                 if (!netif_tx_queue_stopped(netdev_get_tx_queue(dev, queue))) {
4334                         netif_tx_stop_queue(netdev_get_tx_queue(priv->dev,
4335                                                                 queue));
4336                         /* This is a hard error, log it. */
4337                         netdev_err(priv->dev,
4338                                    "%s: Tx Ring full when queue awake\n",
4339                                    __func__);
4340                 }
4341                 return NETDEV_TX_BUSY;
4342         }
4343
4344         /* Check if VLAN can be inserted by HW */
4345         has_vlan = stmmac_vlan_insert(priv, skb, tx_q);
4346
4347         entry = tx_q->cur_tx;
4348         first_entry = entry;
4349         WARN_ON(tx_q->tx_skbuff[first_entry]);
4350
4351         csum_insertion = (skb->ip_summed == CHECKSUM_PARTIAL);
4352
4353         if (likely(priv->extend_desc))
4354                 desc = (struct dma_desc *)(tx_q->dma_etx + entry);
4355         else if (tx_q->tbs & STMMAC_TBS_AVAIL)
4356                 desc = &tx_q->dma_entx[entry].basic;
4357         else
4358                 desc = tx_q->dma_tx + entry;
4359
4360         first = desc;
4361
4362         if (has_vlan)
4363                 stmmac_set_desc_vlan(priv, first, STMMAC_VLAN_INSERT);
4364
4365         enh_desc = priv->plat->enh_desc;
4366         /* To program the descriptors according to the size of the frame */
4367         if (enh_desc)
4368                 is_jumbo = stmmac_is_jumbo_frm(priv, skb->len, enh_desc);
4369
4370         if (unlikely(is_jumbo)) {
4371                 entry = stmmac_jumbo_frm(priv, tx_q, skb, csum_insertion);
4372                 if (unlikely(entry < 0) && (entry != -EINVAL))
4373                         goto dma_map_err;
4374         }
4375
4376         for (i = 0; i < nfrags; i++) {
4377                 const skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
4378                 int len = skb_frag_size(frag);
4379                 bool last_segment = (i == (nfrags - 1));
4380
4381                 entry = STMMAC_GET_ENTRY(entry, priv->dma_conf.dma_tx_size);
4382                 WARN_ON(tx_q->tx_skbuff[entry]);
4383
4384                 if (likely(priv->extend_desc))
4385                         desc = (struct dma_desc *)(tx_q->dma_etx + entry);
4386                 else if (tx_q->tbs & STMMAC_TBS_AVAIL)
4387                         desc = &tx_q->dma_entx[entry].basic;
4388                 else
4389                         desc = tx_q->dma_tx + entry;
4390
4391                 des = skb_frag_dma_map(priv->device, frag, 0, len,
4392                                        DMA_TO_DEVICE);
4393                 if (dma_mapping_error(priv->device, des))
4394                         goto dma_map_err; /* should reuse desc w/o issues */
4395
4396                 tx_q->tx_skbuff_dma[entry].buf = des;
4397
4398                 stmmac_set_desc_addr(priv, desc, des);
4399
4400                 tx_q->tx_skbuff_dma[entry].map_as_page = true;
4401                 tx_q->tx_skbuff_dma[entry].len = len;
4402                 tx_q->tx_skbuff_dma[entry].last_segment = last_segment;
4403                 tx_q->tx_skbuff_dma[entry].buf_type = STMMAC_TXBUF_T_SKB;
4404
4405                 /* Prepare the descriptor and set the own bit too */
4406                 stmmac_prepare_tx_desc(priv, desc, 0, len, csum_insertion,
4407                                 priv->mode, 1, last_segment, skb->len);
4408         }
4409
4410         /* Only the last descriptor gets to point to the skb. */
4411         tx_q->tx_skbuff[entry] = skb;
4412         tx_q->tx_skbuff_dma[entry].buf_type = STMMAC_TXBUF_T_SKB;
4413
4414         /* According to the coalesce parameter the IC bit for the latest
4415          * segment is reset and the timer re-started to clean the tx status.
4416          * This approach takes care about the fragments: desc is the first
4417          * element in case of no SG.
4418          */
4419         tx_packets = (entry + 1) - first_tx;
4420         tx_q->tx_count_frames += tx_packets;
4421
4422         if ((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) && priv->hwts_tx_en)
4423                 set_ic = true;
4424         else if (!priv->tx_coal_frames[queue])
4425                 set_ic = false;
4426         else if (tx_packets > priv->tx_coal_frames[queue])
4427                 set_ic = true;
4428         else if ((tx_q->tx_count_frames %
4429                   priv->tx_coal_frames[queue]) < tx_packets)
4430                 set_ic = true;
4431         else
4432                 set_ic = false;
4433
4434         if (set_ic) {
4435                 if (likely(priv->extend_desc))
4436                         desc = &tx_q->dma_etx[entry].basic;
4437                 else if (tx_q->tbs & STMMAC_TBS_AVAIL)
4438                         desc = &tx_q->dma_entx[entry].basic;
4439                 else
4440                         desc = &tx_q->dma_tx[entry];
4441
4442                 tx_q->tx_count_frames = 0;
4443                 stmmac_set_tx_ic(priv, desc);
4444                 priv->xstats.tx_set_ic_bit++;
4445         }
4446
4447         /* We've used all descriptors we need for this skb, however,
4448          * advance cur_tx so that it references a fresh descriptor.
4449          * ndo_start_xmit will fill this descriptor the next time it's
4450          * called and stmmac_tx_clean may clean up to this descriptor.
4451          */
4452         entry = STMMAC_GET_ENTRY(entry, priv->dma_conf.dma_tx_size);
4453         tx_q->cur_tx = entry;
4454
4455         if (netif_msg_pktdata(priv)) {
4456                 netdev_dbg(priv->dev,
4457                            "%s: curr=%d dirty=%d f=%d, e=%d, first=%p, nfrags=%d",
4458                            __func__, tx_q->cur_tx, tx_q->dirty_tx, first_entry,
4459                            entry, first, nfrags);
4460
4461                 netdev_dbg(priv->dev, ">>> frame to be transmitted: ");
4462                 print_pkt(skb->data, skb->len);
4463         }
4464
4465         if (unlikely(stmmac_tx_avail(priv, queue) <= (MAX_SKB_FRAGS + 1))) {
4466                 netif_dbg(priv, hw, priv->dev, "%s: stop transmitted packets\n",
4467                           __func__);
4468                 netif_tx_stop_queue(netdev_get_tx_queue(priv->dev, queue));
4469         }
4470
4471         dev->stats.tx_bytes += skb->len;
4472
4473         if (priv->sarc_type)
4474                 stmmac_set_desc_sarc(priv, first, priv->sarc_type);
4475
4476         skb_tx_timestamp(skb);
4477
4478         /* Ready to fill the first descriptor and set the OWN bit w/o any
4479          * problems because all the descriptors are actually ready to be
4480          * passed to the DMA engine.
4481          */
4482         if (likely(!is_jumbo)) {
4483                 bool last_segment = (nfrags == 0);
4484
4485                 des = dma_map_single(priv->device, skb->data,
4486                                      nopaged_len, DMA_TO_DEVICE);
4487                 if (dma_mapping_error(priv->device, des))
4488                         goto dma_map_err;
4489
4490                 tx_q->tx_skbuff_dma[first_entry].buf = des;
4491                 tx_q->tx_skbuff_dma[first_entry].buf_type = STMMAC_TXBUF_T_SKB;
4492                 tx_q->tx_skbuff_dma[first_entry].map_as_page = false;
4493
4494                 stmmac_set_desc_addr(priv, first, des);
4495
4496                 tx_q->tx_skbuff_dma[first_entry].len = nopaged_len;
4497                 tx_q->tx_skbuff_dma[first_entry].last_segment = last_segment;
4498
4499                 if (unlikely((skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
4500                              priv->hwts_tx_en)) {
4501                         /* declare that device is doing timestamping */
4502                         skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
4503                         stmmac_enable_tx_timestamp(priv, first);
4504                 }
4505
4506                 /* Prepare the first descriptor setting the OWN bit too */
4507                 stmmac_prepare_tx_desc(priv, first, 1, nopaged_len,
4508                                 csum_insertion, priv->mode, 0, last_segment,
4509                                 skb->len);
4510         }
4511
4512         if (tx_q->tbs & STMMAC_TBS_EN) {
4513                 struct timespec64 ts = ns_to_timespec64(skb->tstamp);
4514
4515                 tbs_desc = &tx_q->dma_entx[first_entry];
4516                 stmmac_set_desc_tbs(priv, tbs_desc, ts.tv_sec, ts.tv_nsec);
4517         }
4518
4519         stmmac_set_tx_owner(priv, first);
4520
4521         netdev_tx_sent_queue(netdev_get_tx_queue(dev, queue), skb->len);
4522
4523         stmmac_enable_dma_transmission(priv, priv->ioaddr);
4524
4525         stmmac_flush_tx_descriptors(priv, queue);
4526         stmmac_tx_timer_arm(priv, queue);
4527
4528         return NETDEV_TX_OK;
4529
4530 dma_map_err:
4531         netdev_err(priv->dev, "Tx DMA map failed\n");
4532         dev_kfree_skb(skb);
4533         priv->dev->stats.tx_dropped++;
4534         return NETDEV_TX_OK;
4535 }
4536
4537 static void stmmac_rx_vlan(struct net_device *dev, struct sk_buff *skb)
4538 {
4539         struct vlan_ethhdr *veth;
4540         __be16 vlan_proto;
4541         u16 vlanid;
4542
4543         veth = (struct vlan_ethhdr *)skb->data;
4544         vlan_proto = veth->h_vlan_proto;
4545
4546         if ((vlan_proto == htons(ETH_P_8021Q) &&
4547              dev->features & NETIF_F_HW_VLAN_CTAG_RX) ||
4548             (vlan_proto == htons(ETH_P_8021AD) &&
4549              dev->features & NETIF_F_HW_VLAN_STAG_RX)) {
4550                 /* pop the vlan tag */
4551                 vlanid = ntohs(veth->h_vlan_TCI);
4552                 memmove(skb->data + VLAN_HLEN, veth, ETH_ALEN * 2);
4553                 skb_pull(skb, VLAN_HLEN);
4554                 __vlan_hwaccel_put_tag(skb, vlan_proto, vlanid);
4555         }
4556 }
4557
4558 /**
4559  * stmmac_rx_refill - refill used skb preallocated buffers
4560  * @priv: driver private structure
4561  * @queue: RX queue index
4562  * Description : this is to reallocate the skb for the reception process
4563  * that is based on zero-copy.
4564  */
4565 static inline void stmmac_rx_refill(struct stmmac_priv *priv, u32 queue)
4566 {
4567         struct stmmac_rx_queue *rx_q = &priv->dma_conf.rx_queue[queue];
4568         int dirty = stmmac_rx_dirty(priv, queue);
4569         unsigned int entry = rx_q->dirty_rx;
4570         gfp_t gfp = (GFP_ATOMIC | __GFP_NOWARN);
4571
4572         if (priv->dma_cap.addr64 <= 32)
4573                 gfp |= GFP_DMA32;
4574
4575         while (dirty-- > 0) {
4576                 struct stmmac_rx_buffer *buf = &rx_q->buf_pool[entry];
4577                 struct dma_desc *p;
4578                 bool use_rx_wd;
4579
4580                 if (priv->extend_desc)
4581                         p = (struct dma_desc *)(rx_q->dma_erx + entry);
4582                 else
4583                         p = rx_q->dma_rx + entry;
4584
4585                 if (!buf->page) {
4586                         buf->page = page_pool_alloc_pages(rx_q->page_pool, gfp);
4587                         if (!buf->page)
4588                                 break;
4589                 }
4590
4591                 if (priv->sph && !buf->sec_page) {
4592                         buf->sec_page = page_pool_alloc_pages(rx_q->page_pool, gfp);
4593                         if (!buf->sec_page)
4594                                 break;
4595
4596                         buf->sec_addr = page_pool_get_dma_addr(buf->sec_page);
4597                 }
4598
4599                 buf->addr = page_pool_get_dma_addr(buf->page) + buf->page_offset;
4600
4601                 stmmac_set_desc_addr(priv, p, buf->addr);
4602                 if (priv->sph)
4603                         stmmac_set_desc_sec_addr(priv, p, buf->sec_addr, true);
4604                 else
4605                         stmmac_set_desc_sec_addr(priv, p, buf->sec_addr, false);
4606                 stmmac_refill_desc3(priv, rx_q, p);
4607
4608                 rx_q->rx_count_frames++;
4609                 rx_q->rx_count_frames += priv->rx_coal_frames[queue];
4610                 if (rx_q->rx_count_frames > priv->rx_coal_frames[queue])
4611                         rx_q->rx_count_frames = 0;
4612
4613                 use_rx_wd = !priv->rx_coal_frames[queue];
4614                 use_rx_wd |= rx_q->rx_count_frames > 0;
4615                 if (!priv->use_riwt)
4616                         use_rx_wd = false;
4617
4618                 dma_wmb();
4619                 stmmac_set_rx_owner(priv, p, use_rx_wd);
4620
4621                 entry = STMMAC_GET_ENTRY(entry, priv->dma_conf.dma_rx_size);
4622         }
4623         rx_q->dirty_rx = entry;
4624         rx_q->rx_tail_addr = rx_q->dma_rx_phy +
4625                             (rx_q->dirty_rx * sizeof(struct dma_desc));
4626         stmmac_set_rx_tail_ptr(priv, priv->ioaddr, rx_q->rx_tail_addr, queue);
4627 }
4628
4629 static unsigned int stmmac_rx_buf1_len(struct stmmac_priv *priv,
4630                                        struct dma_desc *p,
4631                                        int status, unsigned int len)
4632 {
4633         unsigned int plen = 0, hlen = 0;
4634         int coe = priv->hw->rx_csum;
4635
4636         /* Not first descriptor, buffer is always zero */
4637         if (priv->sph && len)
4638                 return 0;
4639
4640         /* First descriptor, get split header length */
4641         stmmac_get_rx_header_len(priv, p, &hlen);
4642         if (priv->sph && hlen) {
4643                 priv->xstats.rx_split_hdr_pkt_n++;
4644                 return hlen;
4645         }
4646
4647         /* First descriptor, not last descriptor and not split header */
4648         if (status & rx_not_ls)
4649                 return priv->dma_conf.dma_buf_sz;
4650
4651         plen = stmmac_get_rx_frame_len(priv, p, coe);
4652
4653         /* First descriptor and last descriptor and not split header */
4654         return min_t(unsigned int, priv->dma_conf.dma_buf_sz, plen);
4655 }
4656
4657 static unsigned int stmmac_rx_buf2_len(struct stmmac_priv *priv,
4658                                        struct dma_desc *p,
4659                                        int status, unsigned int len)
4660 {
4661         int coe = priv->hw->rx_csum;
4662         unsigned int plen = 0;
4663
4664         /* Not split header, buffer is not available */
4665         if (!priv->sph)
4666                 return 0;
4667
4668         /* Not last descriptor */
4669         if (status & rx_not_ls)
4670                 return priv->dma_conf.dma_buf_sz;
4671
4672         plen = stmmac_get_rx_frame_len(priv, p, coe);
4673
4674         /* Last descriptor */
4675         return plen - len;
4676 }
4677
4678 static int stmmac_xdp_xmit_xdpf(struct stmmac_priv *priv, int queue,
4679                                 struct xdp_frame *xdpf, bool dma_map)
4680 {
4681         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[queue];
4682         unsigned int entry = tx_q->cur_tx;
4683         struct dma_desc *tx_desc;
4684         dma_addr_t dma_addr;
4685         bool set_ic;
4686
4687         if (stmmac_tx_avail(priv, queue) < STMMAC_TX_THRESH(priv))
4688                 return STMMAC_XDP_CONSUMED;
4689
4690         if (likely(priv->extend_desc))
4691                 tx_desc = (struct dma_desc *)(tx_q->dma_etx + entry);
4692         else if (tx_q->tbs & STMMAC_TBS_AVAIL)
4693                 tx_desc = &tx_q->dma_entx[entry].basic;
4694         else
4695                 tx_desc = tx_q->dma_tx + entry;
4696
4697         if (dma_map) {
4698                 dma_addr = dma_map_single(priv->device, xdpf->data,
4699                                           xdpf->len, DMA_TO_DEVICE);
4700                 if (dma_mapping_error(priv->device, dma_addr))
4701                         return STMMAC_XDP_CONSUMED;
4702
4703                 tx_q->tx_skbuff_dma[entry].buf_type = STMMAC_TXBUF_T_XDP_NDO;
4704         } else {
4705                 struct page *page = virt_to_page(xdpf->data);
4706
4707                 dma_addr = page_pool_get_dma_addr(page) + sizeof(*xdpf) +
4708                            xdpf->headroom;
4709                 dma_sync_single_for_device(priv->device, dma_addr,
4710                                            xdpf->len, DMA_BIDIRECTIONAL);
4711
4712                 tx_q->tx_skbuff_dma[entry].buf_type = STMMAC_TXBUF_T_XDP_TX;
4713         }
4714
4715         tx_q->tx_skbuff_dma[entry].buf = dma_addr;
4716         tx_q->tx_skbuff_dma[entry].map_as_page = false;
4717         tx_q->tx_skbuff_dma[entry].len = xdpf->len;
4718         tx_q->tx_skbuff_dma[entry].last_segment = true;
4719         tx_q->tx_skbuff_dma[entry].is_jumbo = false;
4720
4721         tx_q->xdpf[entry] = xdpf;
4722
4723         stmmac_set_desc_addr(priv, tx_desc, dma_addr);
4724
4725         stmmac_prepare_tx_desc(priv, tx_desc, 1, xdpf->len,
4726                                true, priv->mode, true, true,
4727                                xdpf->len);
4728
4729         tx_q->tx_count_frames++;
4730
4731         if (tx_q->tx_count_frames % priv->tx_coal_frames[queue] == 0)
4732                 set_ic = true;
4733         else
4734                 set_ic = false;
4735
4736         if (set_ic) {
4737                 tx_q->tx_count_frames = 0;
4738                 stmmac_set_tx_ic(priv, tx_desc);
4739                 priv->xstats.tx_set_ic_bit++;
4740         }
4741
4742         stmmac_enable_dma_transmission(priv, priv->ioaddr);
4743
4744         entry = STMMAC_GET_ENTRY(entry, priv->dma_conf.dma_tx_size);
4745         tx_q->cur_tx = entry;
4746
4747         return STMMAC_XDP_TX;
4748 }
4749
4750 static int stmmac_xdp_get_tx_queue(struct stmmac_priv *priv,
4751                                    int cpu)
4752 {
4753         int index = cpu;
4754
4755         if (unlikely(index < 0))
4756                 index = 0;
4757
4758         while (index >= priv->plat->tx_queues_to_use)
4759                 index -= priv->plat->tx_queues_to_use;
4760
4761         return index;
4762 }
4763
4764 static int stmmac_xdp_xmit_back(struct stmmac_priv *priv,
4765                                 struct xdp_buff *xdp)
4766 {
4767         struct xdp_frame *xdpf = xdp_convert_buff_to_frame(xdp);
4768         int cpu = smp_processor_id();
4769         struct netdev_queue *nq;
4770         int queue;
4771         int res;
4772
4773         if (unlikely(!xdpf))
4774                 return STMMAC_XDP_CONSUMED;
4775
4776         queue = stmmac_xdp_get_tx_queue(priv, cpu);
4777         nq = netdev_get_tx_queue(priv->dev, queue);
4778
4779         __netif_tx_lock(nq, cpu);
4780         /* Avoids TX time-out as we are sharing with slow path */
4781         txq_trans_cond_update(nq);
4782
4783         res = stmmac_xdp_xmit_xdpf(priv, queue, xdpf, false);
4784         if (res == STMMAC_XDP_TX)
4785                 stmmac_flush_tx_descriptors(priv, queue);
4786
4787         __netif_tx_unlock(nq);
4788
4789         return res;
4790 }
4791
4792 static int __stmmac_xdp_run_prog(struct stmmac_priv *priv,
4793                                  struct bpf_prog *prog,
4794                                  struct xdp_buff *xdp)
4795 {
4796         u32 act;
4797         int res;
4798
4799         act = bpf_prog_run_xdp(prog, xdp);
4800         switch (act) {
4801         case XDP_PASS:
4802                 res = STMMAC_XDP_PASS;
4803                 break;
4804         case XDP_TX:
4805                 res = stmmac_xdp_xmit_back(priv, xdp);
4806                 break;
4807         case XDP_REDIRECT:
4808                 if (xdp_do_redirect(priv->dev, xdp, prog) < 0)
4809                         res = STMMAC_XDP_CONSUMED;
4810                 else
4811                         res = STMMAC_XDP_REDIRECT;
4812                 break;
4813         default:
4814                 bpf_warn_invalid_xdp_action(priv->dev, prog, act);
4815                 fallthrough;
4816         case XDP_ABORTED:
4817                 trace_xdp_exception(priv->dev, prog, act);
4818                 fallthrough;
4819         case XDP_DROP:
4820                 res = STMMAC_XDP_CONSUMED;
4821                 break;
4822         }
4823
4824         return res;
4825 }
4826
4827 static struct sk_buff *stmmac_xdp_run_prog(struct stmmac_priv *priv,
4828                                            struct xdp_buff *xdp)
4829 {
4830         struct bpf_prog *prog;
4831         int res;
4832
4833         prog = READ_ONCE(priv->xdp_prog);
4834         if (!prog) {
4835                 res = STMMAC_XDP_PASS;
4836                 goto out;
4837         }
4838
4839         res = __stmmac_xdp_run_prog(priv, prog, xdp);
4840 out:
4841         return ERR_PTR(-res);
4842 }
4843
4844 static void stmmac_finalize_xdp_rx(struct stmmac_priv *priv,
4845                                    int xdp_status)
4846 {
4847         int cpu = smp_processor_id();
4848         int queue;
4849
4850         queue = stmmac_xdp_get_tx_queue(priv, cpu);
4851
4852         if (xdp_status & STMMAC_XDP_TX)
4853                 stmmac_tx_timer_arm(priv, queue);
4854
4855         if (xdp_status & STMMAC_XDP_REDIRECT)
4856                 xdp_do_flush();
4857 }
4858
4859 static struct sk_buff *stmmac_construct_skb_zc(struct stmmac_channel *ch,
4860                                                struct xdp_buff *xdp)
4861 {
4862         unsigned int metasize = xdp->data - xdp->data_meta;
4863         unsigned int datasize = xdp->data_end - xdp->data;
4864         struct sk_buff *skb;
4865
4866         skb = __napi_alloc_skb(&ch->rxtx_napi,
4867                                xdp->data_end - xdp->data_hard_start,
4868                                GFP_ATOMIC | __GFP_NOWARN);
4869         if (unlikely(!skb))
4870                 return NULL;
4871
4872         skb_reserve(skb, xdp->data - xdp->data_hard_start);
4873         memcpy(__skb_put(skb, datasize), xdp->data, datasize);
4874         if (metasize)
4875                 skb_metadata_set(skb, metasize);
4876
4877         return skb;
4878 }
4879
4880 static void stmmac_dispatch_skb_zc(struct stmmac_priv *priv, u32 queue,
4881                                    struct dma_desc *p, struct dma_desc *np,
4882                                    struct xdp_buff *xdp)
4883 {
4884         struct stmmac_channel *ch = &priv->channel[queue];
4885         unsigned int len = xdp->data_end - xdp->data;
4886         enum pkt_hash_types hash_type;
4887         int coe = priv->hw->rx_csum;
4888         struct sk_buff *skb;
4889         u32 hash;
4890
4891         skb = stmmac_construct_skb_zc(ch, xdp);
4892         if (!skb) {
4893                 priv->dev->stats.rx_dropped++;
4894                 return;
4895         }
4896
4897         stmmac_get_rx_hwtstamp(priv, p, np, skb);
4898         stmmac_rx_vlan(priv->dev, skb);
4899         skb->protocol = eth_type_trans(skb, priv->dev);
4900
4901         if (unlikely(!coe))
4902                 skb_checksum_none_assert(skb);
4903         else
4904                 skb->ip_summed = CHECKSUM_UNNECESSARY;
4905
4906         if (!stmmac_get_rx_hash(priv, p, &hash, &hash_type))
4907                 skb_set_hash(skb, hash, hash_type);
4908
4909         skb_record_rx_queue(skb, queue);
4910         napi_gro_receive(&ch->rxtx_napi, skb);
4911
4912         priv->dev->stats.rx_packets++;
4913         priv->dev->stats.rx_bytes += len;
4914 }
4915
4916 static bool stmmac_rx_refill_zc(struct stmmac_priv *priv, u32 queue, u32 budget)
4917 {
4918         struct stmmac_rx_queue *rx_q = &priv->dma_conf.rx_queue[queue];
4919         unsigned int entry = rx_q->dirty_rx;
4920         struct dma_desc *rx_desc = NULL;
4921         bool ret = true;
4922
4923         budget = min(budget, stmmac_rx_dirty(priv, queue));
4924
4925         while (budget-- > 0 && entry != rx_q->cur_rx) {
4926                 struct stmmac_rx_buffer *buf = &rx_q->buf_pool[entry];
4927                 dma_addr_t dma_addr;
4928                 bool use_rx_wd;
4929
4930                 if (!buf->xdp) {
4931                         buf->xdp = xsk_buff_alloc(rx_q->xsk_pool);
4932                         if (!buf->xdp) {
4933                                 ret = false;
4934                                 break;
4935                         }
4936                 }
4937
4938                 if (priv->extend_desc)
4939                         rx_desc = (struct dma_desc *)(rx_q->dma_erx + entry);
4940                 else
4941                         rx_desc = rx_q->dma_rx + entry;
4942
4943                 dma_addr = xsk_buff_xdp_get_dma(buf->xdp);
4944                 stmmac_set_desc_addr(priv, rx_desc, dma_addr);
4945                 stmmac_set_desc_sec_addr(priv, rx_desc, 0, false);
4946                 stmmac_refill_desc3(priv, rx_q, rx_desc);
4947
4948                 rx_q->rx_count_frames++;
4949                 rx_q->rx_count_frames += priv->rx_coal_frames[queue];
4950                 if (rx_q->rx_count_frames > priv->rx_coal_frames[queue])
4951                         rx_q->rx_count_frames = 0;
4952
4953                 use_rx_wd = !priv->rx_coal_frames[queue];
4954                 use_rx_wd |= rx_q->rx_count_frames > 0;
4955                 if (!priv->use_riwt)
4956                         use_rx_wd = false;
4957
4958                 dma_wmb();
4959                 stmmac_set_rx_owner(priv, rx_desc, use_rx_wd);
4960
4961                 entry = STMMAC_GET_ENTRY(entry, priv->dma_conf.dma_rx_size);
4962         }
4963
4964         if (rx_desc) {
4965                 rx_q->dirty_rx = entry;
4966                 rx_q->rx_tail_addr = rx_q->dma_rx_phy +
4967                                      (rx_q->dirty_rx * sizeof(struct dma_desc));
4968                 stmmac_set_rx_tail_ptr(priv, priv->ioaddr, rx_q->rx_tail_addr, queue);
4969         }
4970
4971         return ret;
4972 }
4973
4974 static int stmmac_rx_zc(struct stmmac_priv *priv, int limit, u32 queue)
4975 {
4976         struct stmmac_rx_queue *rx_q = &priv->dma_conf.rx_queue[queue];
4977         unsigned int count = 0, error = 0, len = 0;
4978         int dirty = stmmac_rx_dirty(priv, queue);
4979         unsigned int next_entry = rx_q->cur_rx;
4980         unsigned int desc_size;
4981         struct bpf_prog *prog;
4982         bool failure = false;
4983         int xdp_status = 0;
4984         int status = 0;
4985
4986         if (netif_msg_rx_status(priv)) {
4987                 void *rx_head;
4988
4989                 netdev_dbg(priv->dev, "%s: descriptor ring:\n", __func__);
4990                 if (priv->extend_desc) {
4991                         rx_head = (void *)rx_q->dma_erx;
4992                         desc_size = sizeof(struct dma_extended_desc);
4993                 } else {
4994                         rx_head = (void *)rx_q->dma_rx;
4995                         desc_size = sizeof(struct dma_desc);
4996                 }
4997
4998                 stmmac_display_ring(priv, rx_head, priv->dma_conf.dma_rx_size, true,
4999                                     rx_q->dma_rx_phy, desc_size);
5000         }
5001         while (count < limit) {
5002                 struct stmmac_rx_buffer *buf;
5003                 unsigned int buf1_len = 0;
5004                 struct dma_desc *np, *p;
5005                 int entry;
5006                 int res;
5007
5008                 if (!count && rx_q->state_saved) {
5009                         error = rx_q->state.error;
5010                         len = rx_q->state.len;
5011                 } else {
5012                         rx_q->state_saved = false;
5013                         error = 0;
5014                         len = 0;
5015                 }
5016
5017                 if (count >= limit)
5018                         break;
5019
5020 read_again:
5021                 buf1_len = 0;
5022                 entry = next_entry;
5023                 buf = &rx_q->buf_pool[entry];
5024
5025                 if (dirty >= STMMAC_RX_FILL_BATCH) {
5026                         failure = failure ||
5027                                   !stmmac_rx_refill_zc(priv, queue, dirty);
5028                         dirty = 0;
5029                 }
5030
5031                 if (priv->extend_desc)
5032                         p = (struct dma_desc *)(rx_q->dma_erx + entry);
5033                 else
5034                         p = rx_q->dma_rx + entry;
5035
5036                 /* read the status of the incoming frame */
5037                 status = stmmac_rx_status(priv, &priv->dev->stats,
5038                                           &priv->xstats, p);
5039                 /* check if managed by the DMA otherwise go ahead */
5040                 if (unlikely(status & dma_own))
5041                         break;
5042
5043                 /* Prefetch the next RX descriptor */
5044                 rx_q->cur_rx = STMMAC_GET_ENTRY(rx_q->cur_rx,
5045                                                 priv->dma_conf.dma_rx_size);
5046                 next_entry = rx_q->cur_rx;
5047
5048                 if (priv->extend_desc)
5049                         np = (struct dma_desc *)(rx_q->dma_erx + next_entry);
5050                 else
5051                         np = rx_q->dma_rx + next_entry;
5052
5053                 prefetch(np);
5054
5055                 /* Ensure a valid XSK buffer before proceed */
5056                 if (!buf->xdp)
5057                         break;
5058
5059                 if (priv->extend_desc)
5060                         stmmac_rx_extended_status(priv, &priv->dev->stats,
5061                                                   &priv->xstats,
5062                                                   rx_q->dma_erx + entry);
5063                 if (unlikely(status == discard_frame)) {
5064                         xsk_buff_free(buf->xdp);
5065                         buf->xdp = NULL;
5066                         dirty++;
5067                         error = 1;
5068                         if (!priv->hwts_rx_en)
5069                                 priv->dev->stats.rx_errors++;
5070                 }
5071
5072                 if (unlikely(error && (status & rx_not_ls)))
5073                         goto read_again;
5074                 if (unlikely(error)) {
5075                         count++;
5076                         continue;
5077                 }
5078
5079                 /* XSK pool expects RX frame 1:1 mapped to XSK buffer */
5080                 if (likely(status & rx_not_ls)) {
5081                         xsk_buff_free(buf->xdp);
5082                         buf->xdp = NULL;
5083                         dirty++;
5084                         count++;
5085                         goto read_again;
5086                 }
5087
5088                 /* XDP ZC Frame only support primary buffers for now */
5089                 buf1_len = stmmac_rx_buf1_len(priv, p, status, len);
5090                 len += buf1_len;
5091
5092                 /* ACS is disabled; strip manually. */
5093                 if (likely(!(status & rx_not_ls))) {
5094                         buf1_len -= ETH_FCS_LEN;
5095                         len -= ETH_FCS_LEN;
5096                 }
5097
5098                 /* RX buffer is good and fit into a XSK pool buffer */
5099                 buf->xdp->data_end = buf->xdp->data + buf1_len;
5100                 xsk_buff_dma_sync_for_cpu(buf->xdp, rx_q->xsk_pool);
5101
5102                 prog = READ_ONCE(priv->xdp_prog);
5103                 res = __stmmac_xdp_run_prog(priv, prog, buf->xdp);
5104
5105                 switch (res) {
5106                 case STMMAC_XDP_PASS:
5107                         stmmac_dispatch_skb_zc(priv, queue, p, np, buf->xdp);
5108                         xsk_buff_free(buf->xdp);
5109                         break;
5110                 case STMMAC_XDP_CONSUMED:
5111                         xsk_buff_free(buf->xdp);
5112                         priv->dev->stats.rx_dropped++;
5113                         break;
5114                 case STMMAC_XDP_TX:
5115                 case STMMAC_XDP_REDIRECT:
5116                         xdp_status |= res;
5117                         break;
5118                 }
5119
5120                 buf->xdp = NULL;
5121                 dirty++;
5122                 count++;
5123         }
5124
5125         if (status & rx_not_ls) {
5126                 rx_q->state_saved = true;
5127                 rx_q->state.error = error;
5128                 rx_q->state.len = len;
5129         }
5130
5131         stmmac_finalize_xdp_rx(priv, xdp_status);
5132
5133         priv->xstats.rx_pkt_n += count;
5134         priv->xstats.rxq_stats[queue].rx_pkt_n += count;
5135
5136         if (xsk_uses_need_wakeup(rx_q->xsk_pool)) {
5137                 if (failure || stmmac_rx_dirty(priv, queue) > 0)
5138                         xsk_set_rx_need_wakeup(rx_q->xsk_pool);
5139                 else
5140                         xsk_clear_rx_need_wakeup(rx_q->xsk_pool);
5141
5142                 return (int)count;
5143         }
5144
5145         return failure ? limit : (int)count;
5146 }
5147
5148 /**
5149  * stmmac_rx - manage the receive process
5150  * @priv: driver private structure
5151  * @limit: napi bugget
5152  * @queue: RX queue index.
5153  * Description :  this the function called by the napi poll method.
5154  * It gets all the frames inside the ring.
5155  */
5156 static int stmmac_rx(struct stmmac_priv *priv, int limit, u32 queue)
5157 {
5158         struct stmmac_rx_queue *rx_q = &priv->dma_conf.rx_queue[queue];
5159         struct stmmac_channel *ch = &priv->channel[queue];
5160         unsigned int count = 0, error = 0, len = 0;
5161         int status = 0, coe = priv->hw->rx_csum;
5162         unsigned int next_entry = rx_q->cur_rx;
5163         enum dma_data_direction dma_dir;
5164         unsigned int desc_size;
5165         struct sk_buff *skb = NULL;
5166         struct xdp_buff xdp;
5167         int xdp_status = 0;
5168         int buf_sz;
5169
5170         dma_dir = page_pool_get_dma_dir(rx_q->page_pool);
5171         buf_sz = DIV_ROUND_UP(priv->dma_conf.dma_buf_sz, PAGE_SIZE) * PAGE_SIZE;
5172
5173         if (netif_msg_rx_status(priv)) {
5174                 void *rx_head;
5175
5176                 netdev_dbg(priv->dev, "%s: descriptor ring:\n", __func__);
5177                 if (priv->extend_desc) {
5178                         rx_head = (void *)rx_q->dma_erx;
5179                         desc_size = sizeof(struct dma_extended_desc);
5180                 } else {
5181                         rx_head = (void *)rx_q->dma_rx;
5182                         desc_size = sizeof(struct dma_desc);
5183                 }
5184
5185                 stmmac_display_ring(priv, rx_head, priv->dma_conf.dma_rx_size, true,
5186                                     rx_q->dma_rx_phy, desc_size);
5187         }
5188         while (count < limit) {
5189                 unsigned int buf1_len = 0, buf2_len = 0;
5190                 enum pkt_hash_types hash_type;
5191                 struct stmmac_rx_buffer *buf;
5192                 struct dma_desc *np, *p;
5193                 int entry;
5194                 u32 hash;
5195
5196                 if (!count && rx_q->state_saved) {
5197                         skb = rx_q->state.skb;
5198                         error = rx_q->state.error;
5199                         len = rx_q->state.len;
5200                 } else {
5201                         rx_q->state_saved = false;
5202                         skb = NULL;
5203                         error = 0;
5204                         len = 0;
5205                 }
5206
5207                 if (count >= limit)
5208                         break;
5209
5210 read_again:
5211                 buf1_len = 0;
5212                 buf2_len = 0;
5213                 entry = next_entry;
5214                 buf = &rx_q->buf_pool[entry];
5215
5216                 if (priv->extend_desc)
5217                         p = (struct dma_desc *)(rx_q->dma_erx + entry);
5218                 else
5219                         p = rx_q->dma_rx + entry;
5220
5221                 /* read the status of the incoming frame */
5222                 status = stmmac_rx_status(priv, &priv->dev->stats,
5223                                 &priv->xstats, p);
5224                 /* check if managed by the DMA otherwise go ahead */
5225                 if (unlikely(status & dma_own))
5226                         break;
5227
5228                 rx_q->cur_rx = STMMAC_GET_ENTRY(rx_q->cur_rx,
5229                                                 priv->dma_conf.dma_rx_size);
5230                 next_entry = rx_q->cur_rx;
5231
5232                 if (priv->extend_desc)
5233                         np = (struct dma_desc *)(rx_q->dma_erx + next_entry);
5234                 else
5235                         np = rx_q->dma_rx + next_entry;
5236
5237                 prefetch(np);
5238
5239                 if (priv->extend_desc)
5240                         stmmac_rx_extended_status(priv, &priv->dev->stats,
5241                                         &priv->xstats, rx_q->dma_erx + entry);
5242                 if (unlikely(status == discard_frame)) {
5243                         page_pool_recycle_direct(rx_q->page_pool, buf->page);
5244                         buf->page = NULL;
5245                         error = 1;
5246                         if (!priv->hwts_rx_en)
5247                                 priv->dev->stats.rx_errors++;
5248                 }
5249
5250                 if (unlikely(error && (status & rx_not_ls)))
5251                         goto read_again;
5252                 if (unlikely(error)) {
5253                         dev_kfree_skb(skb);
5254                         skb = NULL;
5255                         count++;
5256                         continue;
5257                 }
5258
5259                 /* Buffer is good. Go on. */
5260
5261                 prefetch(page_address(buf->page) + buf->page_offset);
5262                 if (buf->sec_page)
5263                         prefetch(page_address(buf->sec_page));
5264
5265                 buf1_len = stmmac_rx_buf1_len(priv, p, status, len);
5266                 len += buf1_len;
5267                 buf2_len = stmmac_rx_buf2_len(priv, p, status, len);
5268                 len += buf2_len;
5269
5270                 /* ACS is disabled; strip manually. */
5271                 if (likely(!(status & rx_not_ls))) {
5272                         if (buf2_len) {
5273                                 buf2_len -= ETH_FCS_LEN;
5274                                 len -= ETH_FCS_LEN;
5275                         } else if (buf1_len) {
5276                                 buf1_len -= ETH_FCS_LEN;
5277                                 len -= ETH_FCS_LEN;
5278                         }
5279                 }
5280
5281                 if (!skb) {
5282                         unsigned int pre_len, sync_len;
5283
5284                         dma_sync_single_for_cpu(priv->device, buf->addr,
5285                                                 buf1_len, dma_dir);
5286
5287                         xdp_init_buff(&xdp, buf_sz, &rx_q->xdp_rxq);
5288                         xdp_prepare_buff(&xdp, page_address(buf->page),
5289                                          buf->page_offset, buf1_len, false);
5290
5291                         pre_len = xdp.data_end - xdp.data_hard_start -
5292                                   buf->page_offset;
5293                         skb = stmmac_xdp_run_prog(priv, &xdp);
5294                         /* Due xdp_adjust_tail: DMA sync for_device
5295                          * cover max len CPU touch
5296                          */
5297                         sync_len = xdp.data_end - xdp.data_hard_start -
5298                                    buf->page_offset;
5299                         sync_len = max(sync_len, pre_len);
5300
5301                         /* For Not XDP_PASS verdict */
5302                         if (IS_ERR(skb)) {
5303                                 unsigned int xdp_res = -PTR_ERR(skb);
5304
5305                                 if (xdp_res & STMMAC_XDP_CONSUMED) {
5306                                         page_pool_put_page(rx_q->page_pool,
5307                                                            virt_to_head_page(xdp.data),
5308                                                            sync_len, true);
5309                                         buf->page = NULL;
5310                                         priv->dev->stats.rx_dropped++;
5311
5312                                         /* Clear skb as it was set as
5313                                          * status by XDP program.
5314                                          */
5315                                         skb = NULL;
5316
5317                                         if (unlikely((status & rx_not_ls)))
5318                                                 goto read_again;
5319
5320                                         count++;
5321                                         continue;
5322                                 } else if (xdp_res & (STMMAC_XDP_TX |
5323                                                       STMMAC_XDP_REDIRECT)) {
5324                                         xdp_status |= xdp_res;
5325                                         buf->page = NULL;
5326                                         skb = NULL;
5327                                         count++;
5328                                         continue;
5329                                 }
5330                         }
5331                 }
5332
5333                 if (!skb) {
5334                         /* XDP program may expand or reduce tail */
5335                         buf1_len = xdp.data_end - xdp.data;
5336
5337                         skb = napi_alloc_skb(&ch->rx_napi, buf1_len);
5338                         if (!skb) {
5339                                 priv->dev->stats.rx_dropped++;
5340                                 count++;
5341                                 goto drain_data;
5342                         }
5343
5344                         /* XDP program may adjust header */
5345                         skb_copy_to_linear_data(skb, xdp.data, buf1_len);
5346                         skb_put(skb, buf1_len);
5347
5348                         /* Data payload copied into SKB, page ready for recycle */
5349                         page_pool_recycle_direct(rx_q->page_pool, buf->page);
5350                         buf->page = NULL;
5351                 } else if (buf1_len) {
5352                         dma_sync_single_for_cpu(priv->device, buf->addr,
5353                                                 buf1_len, dma_dir);
5354                         skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
5355                                         buf->page, buf->page_offset, buf1_len,
5356                                         priv->dma_conf.dma_buf_sz);
5357
5358                         /* Data payload appended into SKB */
5359                         page_pool_release_page(rx_q->page_pool, buf->page);
5360                         buf->page = NULL;
5361                 }
5362
5363                 if (buf2_len) {
5364                         dma_sync_single_for_cpu(priv->device, buf->sec_addr,
5365                                                 buf2_len, dma_dir);
5366                         skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
5367                                         buf->sec_page, 0, buf2_len,
5368                                         priv->dma_conf.dma_buf_sz);
5369
5370                         /* Data payload appended into SKB */
5371                         page_pool_release_page(rx_q->page_pool, buf->sec_page);
5372                         buf->sec_page = NULL;
5373                 }
5374
5375 drain_data:
5376                 if (likely(status & rx_not_ls))
5377                         goto read_again;
5378                 if (!skb)
5379                         continue;
5380
5381                 /* Got entire packet into SKB. Finish it. */
5382
5383                 stmmac_get_rx_hwtstamp(priv, p, np, skb);
5384                 stmmac_rx_vlan(priv->dev, skb);
5385                 skb->protocol = eth_type_trans(skb, priv->dev);
5386
5387                 if (unlikely(!coe))
5388                         skb_checksum_none_assert(skb);
5389                 else
5390                         skb->ip_summed = CHECKSUM_UNNECESSARY;
5391
5392                 if (!stmmac_get_rx_hash(priv, p, &hash, &hash_type))
5393                         skb_set_hash(skb, hash, hash_type);
5394
5395                 skb_record_rx_queue(skb, queue);
5396                 napi_gro_receive(&ch->rx_napi, skb);
5397                 skb = NULL;
5398
5399                 priv->dev->stats.rx_packets++;
5400                 priv->dev->stats.rx_bytes += len;
5401                 count++;
5402         }
5403
5404         if (status & rx_not_ls || skb) {
5405                 rx_q->state_saved = true;
5406                 rx_q->state.skb = skb;
5407                 rx_q->state.error = error;
5408                 rx_q->state.len = len;
5409         }
5410
5411         stmmac_finalize_xdp_rx(priv, xdp_status);
5412
5413         stmmac_rx_refill(priv, queue);
5414
5415         priv->xstats.rx_pkt_n += count;
5416         priv->xstats.rxq_stats[queue].rx_pkt_n += count;
5417
5418         return count;
5419 }
5420
5421 static int stmmac_napi_poll_rx(struct napi_struct *napi, int budget)
5422 {
5423         struct stmmac_channel *ch =
5424                 container_of(napi, struct stmmac_channel, rx_napi);
5425         struct stmmac_priv *priv = ch->priv_data;
5426         u32 chan = ch->index;
5427         int work_done;
5428
5429         priv->xstats.napi_poll++;
5430
5431         work_done = stmmac_rx(priv, budget, chan);
5432         if (work_done < budget && napi_complete_done(napi, work_done)) {
5433                 unsigned long flags;
5434
5435                 spin_lock_irqsave(&ch->lock, flags);
5436                 stmmac_enable_dma_irq(priv, priv->ioaddr, chan, 1, 0);
5437                 spin_unlock_irqrestore(&ch->lock, flags);
5438         }
5439
5440         return work_done;
5441 }
5442
5443 static int stmmac_napi_poll_tx(struct napi_struct *napi, int budget)
5444 {
5445         struct stmmac_channel *ch =
5446                 container_of(napi, struct stmmac_channel, tx_napi);
5447         struct stmmac_priv *priv = ch->priv_data;
5448         u32 chan = ch->index;
5449         int work_done;
5450
5451         priv->xstats.napi_poll++;
5452
5453         work_done = stmmac_tx_clean(priv, budget, chan);
5454         work_done = min(work_done, budget);
5455
5456         if (work_done < budget && napi_complete_done(napi, work_done)) {
5457                 unsigned long flags;
5458
5459                 spin_lock_irqsave(&ch->lock, flags);
5460                 stmmac_enable_dma_irq(priv, priv->ioaddr, chan, 0, 1);
5461                 spin_unlock_irqrestore(&ch->lock, flags);
5462         }
5463
5464         return work_done;
5465 }
5466
5467 static int stmmac_napi_poll_rxtx(struct napi_struct *napi, int budget)
5468 {
5469         struct stmmac_channel *ch =
5470                 container_of(napi, struct stmmac_channel, rxtx_napi);
5471         struct stmmac_priv *priv = ch->priv_data;
5472         int rx_done, tx_done, rxtx_done;
5473         u32 chan = ch->index;
5474
5475         priv->xstats.napi_poll++;
5476
5477         tx_done = stmmac_tx_clean(priv, budget, chan);
5478         tx_done = min(tx_done, budget);
5479
5480         rx_done = stmmac_rx_zc(priv, budget, chan);
5481
5482         rxtx_done = max(tx_done, rx_done);
5483
5484         /* If either TX or RX work is not complete, return budget
5485          * and keep pooling
5486          */
5487         if (rxtx_done >= budget)
5488                 return budget;
5489
5490         /* all work done, exit the polling mode */
5491         if (napi_complete_done(napi, rxtx_done)) {
5492                 unsigned long flags;
5493
5494                 spin_lock_irqsave(&ch->lock, flags);
5495                 /* Both RX and TX work done are compelte,
5496                  * so enable both RX & TX IRQs.
5497                  */
5498                 stmmac_enable_dma_irq(priv, priv->ioaddr, chan, 1, 1);
5499                 spin_unlock_irqrestore(&ch->lock, flags);
5500         }
5501
5502         return min(rxtx_done, budget - 1);
5503 }
5504
5505 /**
5506  *  stmmac_tx_timeout
5507  *  @dev : Pointer to net device structure
5508  *  @txqueue: the index of the hanging transmit queue
5509  *  Description: this function is called when a packet transmission fails to
5510  *   complete within a reasonable time. The driver will mark the error in the
5511  *   netdev structure and arrange for the device to be reset to a sane state
5512  *   in order to transmit a new packet.
5513  */
5514 static void stmmac_tx_timeout(struct net_device *dev, unsigned int txqueue)
5515 {
5516         struct stmmac_priv *priv = netdev_priv(dev);
5517
5518         stmmac_global_err(priv);
5519 }
5520
5521 /**
5522  *  stmmac_set_rx_mode - entry point for multicast addressing
5523  *  @dev : pointer to the device structure
5524  *  Description:
5525  *  This function is a driver entry point which gets called by the kernel
5526  *  whenever multicast addresses must be enabled/disabled.
5527  *  Return value:
5528  *  void.
5529  */
5530 static void stmmac_set_rx_mode(struct net_device *dev)
5531 {
5532         struct stmmac_priv *priv = netdev_priv(dev);
5533
5534         stmmac_set_filter(priv, priv->hw, dev);
5535 }
5536
5537 /**
5538  *  stmmac_change_mtu - entry point to change MTU size for the device.
5539  *  @dev : device pointer.
5540  *  @new_mtu : the new MTU size for the device.
5541  *  Description: the Maximum Transfer Unit (MTU) is used by the network layer
5542  *  to drive packet transmission. Ethernet has an MTU of 1500 octets
5543  *  (ETH_DATA_LEN). This value can be changed with ifconfig.
5544  *  Return value:
5545  *  0 on success and an appropriate (-)ve integer as defined in errno.h
5546  *  file on failure.
5547  */
5548 static int stmmac_change_mtu(struct net_device *dev, int new_mtu)
5549 {
5550         struct stmmac_priv *priv = netdev_priv(dev);
5551         int txfifosz = priv->plat->tx_fifo_size;
5552         struct stmmac_dma_conf *dma_conf;
5553         const int mtu = new_mtu;
5554         int ret;
5555
5556         if (txfifosz == 0)
5557                 txfifosz = priv->dma_cap.tx_fifo_size;
5558
5559         txfifosz /= priv->plat->tx_queues_to_use;
5560
5561         if (stmmac_xdp_is_enabled(priv) && new_mtu > ETH_DATA_LEN) {
5562                 netdev_dbg(priv->dev, "Jumbo frames not supported for XDP\n");
5563                 return -EINVAL;
5564         }
5565
5566         new_mtu = STMMAC_ALIGN(new_mtu);
5567
5568         /* If condition true, FIFO is too small or MTU too large */
5569         if ((txfifosz < new_mtu) || (new_mtu > BUF_SIZE_16KiB))
5570                 return -EINVAL;
5571
5572         if (netif_running(dev)) {
5573                 netdev_dbg(priv->dev, "restarting interface to change its MTU\n");
5574                 /* Try to allocate the new DMA conf with the new mtu */
5575                 dma_conf = stmmac_setup_dma_desc(priv, mtu);
5576                 if (IS_ERR(dma_conf)) {
5577                         netdev_err(priv->dev, "failed allocating new dma conf for new MTU %d\n",
5578                                    mtu);
5579                         return PTR_ERR(dma_conf);
5580                 }
5581
5582                 stmmac_release(dev);
5583
5584                 ret = __stmmac_open(dev, dma_conf);
5585                 kfree(dma_conf);
5586                 if (ret) {
5587                         netdev_err(priv->dev, "failed reopening the interface after MTU change\n");
5588                         return ret;
5589                 }
5590
5591                 stmmac_set_rx_mode(dev);
5592         }
5593
5594         dev->mtu = mtu;
5595         netdev_update_features(dev);
5596
5597         return 0;
5598 }
5599
5600 static netdev_features_t stmmac_fix_features(struct net_device *dev,
5601                                              netdev_features_t features)
5602 {
5603         struct stmmac_priv *priv = netdev_priv(dev);
5604
5605         if (priv->plat->rx_coe == STMMAC_RX_COE_NONE)
5606                 features &= ~NETIF_F_RXCSUM;
5607
5608         if (!priv->plat->tx_coe)
5609                 features &= ~NETIF_F_CSUM_MASK;
5610
5611         /* Some GMAC devices have a bugged Jumbo frame support that
5612          * needs to have the Tx COE disabled for oversized frames
5613          * (due to limited buffer sizes). In this case we disable
5614          * the TX csum insertion in the TDES and not use SF.
5615          */
5616         if (priv->plat->bugged_jumbo && (dev->mtu > ETH_DATA_LEN))
5617                 features &= ~NETIF_F_CSUM_MASK;
5618
5619         /* Disable tso if asked by ethtool */
5620         if ((priv->plat->tso_en) && (priv->dma_cap.tsoen)) {
5621                 if (features & NETIF_F_TSO)
5622                         priv->tso = true;
5623                 else
5624                         priv->tso = false;
5625         }
5626
5627         return features;
5628 }
5629
5630 static int stmmac_set_features(struct net_device *netdev,
5631                                netdev_features_t features)
5632 {
5633         struct stmmac_priv *priv = netdev_priv(netdev);
5634
5635         /* Keep the COE Type in case of csum is supporting */
5636         if (features & NETIF_F_RXCSUM)
5637                 priv->hw->rx_csum = priv->plat->rx_coe;
5638         else
5639                 priv->hw->rx_csum = 0;
5640         /* No check needed because rx_coe has been set before and it will be
5641          * fixed in case of issue.
5642          */
5643         stmmac_rx_ipc(priv, priv->hw);
5644
5645         if (priv->sph_cap) {
5646                 bool sph_en = (priv->hw->rx_csum > 0) && priv->sph;
5647                 u32 chan;
5648
5649                 for (chan = 0; chan < priv->plat->rx_queues_to_use; chan++)
5650                         stmmac_enable_sph(priv, priv->ioaddr, sph_en, chan);
5651         }
5652
5653         return 0;
5654 }
5655
5656 static void stmmac_fpe_event_status(struct stmmac_priv *priv, int status)
5657 {
5658         struct stmmac_fpe_cfg *fpe_cfg = priv->plat->fpe_cfg;
5659         enum stmmac_fpe_state *lo_state = &fpe_cfg->lo_fpe_state;
5660         enum stmmac_fpe_state *lp_state = &fpe_cfg->lp_fpe_state;
5661         bool *hs_enable = &fpe_cfg->hs_enable;
5662
5663         if (status == FPE_EVENT_UNKNOWN || !*hs_enable)
5664                 return;
5665
5666         /* If LP has sent verify mPacket, LP is FPE capable */
5667         if ((status & FPE_EVENT_RVER) == FPE_EVENT_RVER) {
5668                 if (*lp_state < FPE_STATE_CAPABLE)
5669                         *lp_state = FPE_STATE_CAPABLE;
5670
5671                 /* If user has requested FPE enable, quickly response */
5672                 if (*hs_enable)
5673                         stmmac_fpe_send_mpacket(priv, priv->ioaddr,
5674                                                 MPACKET_RESPONSE);
5675         }
5676
5677         /* If Local has sent verify mPacket, Local is FPE capable */
5678         if ((status & FPE_EVENT_TVER) == FPE_EVENT_TVER) {
5679                 if (*lo_state < FPE_STATE_CAPABLE)
5680                         *lo_state = FPE_STATE_CAPABLE;
5681         }
5682
5683         /* If LP has sent response mPacket, LP is entering FPE ON */
5684         if ((status & FPE_EVENT_RRSP) == FPE_EVENT_RRSP)
5685                 *lp_state = FPE_STATE_ENTERING_ON;
5686
5687         /* If Local has sent response mPacket, Local is entering FPE ON */
5688         if ((status & FPE_EVENT_TRSP) == FPE_EVENT_TRSP)
5689                 *lo_state = FPE_STATE_ENTERING_ON;
5690
5691         if (!test_bit(__FPE_REMOVING, &priv->fpe_task_state) &&
5692             !test_and_set_bit(__FPE_TASK_SCHED, &priv->fpe_task_state) &&
5693             priv->fpe_wq) {
5694                 queue_work(priv->fpe_wq, &priv->fpe_task);
5695         }
5696 }
5697
5698 static void stmmac_common_interrupt(struct stmmac_priv *priv)
5699 {
5700         u32 rx_cnt = priv->plat->rx_queues_to_use;
5701         u32 tx_cnt = priv->plat->tx_queues_to_use;
5702         u32 queues_count;
5703         u32 queue;
5704         bool xmac;
5705
5706         xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac;
5707         queues_count = (rx_cnt > tx_cnt) ? rx_cnt : tx_cnt;
5708
5709         if (priv->irq_wake)
5710                 pm_wakeup_event(priv->device, 0);
5711
5712         if (priv->dma_cap.estsel)
5713                 stmmac_est_irq_status(priv, priv->ioaddr, priv->dev,
5714                                       &priv->xstats, tx_cnt);
5715
5716         if (priv->dma_cap.fpesel) {
5717                 int status = stmmac_fpe_irq_status(priv, priv->ioaddr,
5718                                                    priv->dev);
5719
5720                 stmmac_fpe_event_status(priv, status);
5721         }
5722
5723         /* To handle GMAC own interrupts */
5724         if ((priv->plat->has_gmac) || xmac) {
5725                 int status = stmmac_host_irq_status(priv, priv->hw, &priv->xstats);
5726
5727                 if (unlikely(status)) {
5728                         /* For LPI we need to save the tx status */
5729                         if (status & CORE_IRQ_TX_PATH_IN_LPI_MODE)
5730                                 priv->tx_path_in_lpi_mode = true;
5731                         if (status & CORE_IRQ_TX_PATH_EXIT_LPI_MODE)
5732                                 priv->tx_path_in_lpi_mode = false;
5733                 }
5734
5735                 for (queue = 0; queue < queues_count; queue++) {
5736                         status = stmmac_host_mtl_irq_status(priv, priv->hw,
5737                                                             queue);
5738                 }
5739
5740                 /* PCS link status */
5741                 if (priv->hw->pcs) {
5742                         if (priv->xstats.pcs_link)
5743                                 netif_carrier_on(priv->dev);
5744                         else
5745                                 netif_carrier_off(priv->dev);
5746                 }
5747
5748                 stmmac_timestamp_interrupt(priv, priv);
5749         }
5750 }
5751
5752 /**
5753  *  stmmac_interrupt - main ISR
5754  *  @irq: interrupt number.
5755  *  @dev_id: to pass the net device pointer.
5756  *  Description: this is the main driver interrupt service routine.
5757  *  It can call:
5758  *  o DMA service routine (to manage incoming frame reception and transmission
5759  *    status)
5760  *  o Core interrupts to manage: remote wake-up, management counter, LPI
5761  *    interrupts.
5762  */
5763 static irqreturn_t stmmac_interrupt(int irq, void *dev_id)
5764 {
5765         struct net_device *dev = (struct net_device *)dev_id;
5766         struct stmmac_priv *priv = netdev_priv(dev);
5767
5768         /* Check if adapter is up */
5769         if (test_bit(STMMAC_DOWN, &priv->state))
5770                 return IRQ_HANDLED;
5771
5772         /* Check if a fatal error happened */
5773         if (stmmac_safety_feat_interrupt(priv))
5774                 return IRQ_HANDLED;
5775
5776         /* To handle Common interrupts */
5777         stmmac_common_interrupt(priv);
5778
5779         /* To handle DMA interrupts */
5780         stmmac_dma_interrupt(priv);
5781
5782         return IRQ_HANDLED;
5783 }
5784
5785 static irqreturn_t stmmac_mac_interrupt(int irq, void *dev_id)
5786 {
5787         struct net_device *dev = (struct net_device *)dev_id;
5788         struct stmmac_priv *priv = netdev_priv(dev);
5789
5790         if (unlikely(!dev)) {
5791                 netdev_err(priv->dev, "%s: invalid dev pointer\n", __func__);
5792                 return IRQ_NONE;
5793         }
5794
5795         /* Check if adapter is up */
5796         if (test_bit(STMMAC_DOWN, &priv->state))
5797                 return IRQ_HANDLED;
5798
5799         /* To handle Common interrupts */
5800         stmmac_common_interrupt(priv);
5801
5802         return IRQ_HANDLED;
5803 }
5804
5805 static irqreturn_t stmmac_safety_interrupt(int irq, void *dev_id)
5806 {
5807         struct net_device *dev = (struct net_device *)dev_id;
5808         struct stmmac_priv *priv = netdev_priv(dev);
5809
5810         if (unlikely(!dev)) {
5811                 netdev_err(priv->dev, "%s: invalid dev pointer\n", __func__);
5812                 return IRQ_NONE;
5813         }
5814
5815         /* Check if adapter is up */
5816         if (test_bit(STMMAC_DOWN, &priv->state))
5817                 return IRQ_HANDLED;
5818
5819         /* Check if a fatal error happened */
5820         stmmac_safety_feat_interrupt(priv);
5821
5822         return IRQ_HANDLED;
5823 }
5824
5825 static irqreturn_t stmmac_msi_intr_tx(int irq, void *data)
5826 {
5827         struct stmmac_tx_queue *tx_q = (struct stmmac_tx_queue *)data;
5828         struct stmmac_dma_conf *dma_conf;
5829         int chan = tx_q->queue_index;
5830         struct stmmac_priv *priv;
5831         int status;
5832
5833         dma_conf = container_of(tx_q, struct stmmac_dma_conf, tx_queue[chan]);
5834         priv = container_of(dma_conf, struct stmmac_priv, dma_conf);
5835
5836         if (unlikely(!data)) {
5837                 netdev_err(priv->dev, "%s: invalid dev pointer\n", __func__);
5838                 return IRQ_NONE;
5839         }
5840
5841         /* Check if adapter is up */
5842         if (test_bit(STMMAC_DOWN, &priv->state))
5843                 return IRQ_HANDLED;
5844
5845         status = stmmac_napi_check(priv, chan, DMA_DIR_TX);
5846
5847         if (unlikely(status & tx_hard_error_bump_tc)) {
5848                 /* Try to bump up the dma threshold on this failure */
5849                 stmmac_bump_dma_threshold(priv, chan);
5850         } else if (unlikely(status == tx_hard_error)) {
5851                 stmmac_tx_err(priv, chan);
5852         }
5853
5854         return IRQ_HANDLED;
5855 }
5856
5857 static irqreturn_t stmmac_msi_intr_rx(int irq, void *data)
5858 {
5859         struct stmmac_rx_queue *rx_q = (struct stmmac_rx_queue *)data;
5860         struct stmmac_dma_conf *dma_conf;
5861         int chan = rx_q->queue_index;
5862         struct stmmac_priv *priv;
5863
5864         dma_conf = container_of(rx_q, struct stmmac_dma_conf, rx_queue[chan]);
5865         priv = container_of(dma_conf, struct stmmac_priv, dma_conf);
5866
5867         if (unlikely(!data)) {
5868                 netdev_err(priv->dev, "%s: invalid dev pointer\n", __func__);
5869                 return IRQ_NONE;
5870         }
5871
5872         /* Check if adapter is up */
5873         if (test_bit(STMMAC_DOWN, &priv->state))
5874                 return IRQ_HANDLED;
5875
5876         stmmac_napi_check(priv, chan, DMA_DIR_RX);
5877
5878         return IRQ_HANDLED;
5879 }
5880
5881 #ifdef CONFIG_NET_POLL_CONTROLLER
5882 /* Polling receive - used by NETCONSOLE and other diagnostic tools
5883  * to allow network I/O with interrupts disabled.
5884  */
5885 static void stmmac_poll_controller(struct net_device *dev)
5886 {
5887         struct stmmac_priv *priv = netdev_priv(dev);
5888         int i;
5889
5890         /* If adapter is down, do nothing */
5891         if (test_bit(STMMAC_DOWN, &priv->state))
5892                 return;
5893
5894         if (priv->plat->multi_msi_en) {
5895                 for (i = 0; i < priv->plat->rx_queues_to_use; i++)
5896                         stmmac_msi_intr_rx(0, &priv->dma_conf.rx_queue[i]);
5897
5898                 for (i = 0; i < priv->plat->tx_queues_to_use; i++)
5899                         stmmac_msi_intr_tx(0, &priv->dma_conf.tx_queue[i]);
5900         } else {
5901                 disable_irq(dev->irq);
5902                 stmmac_interrupt(dev->irq, dev);
5903                 enable_irq(dev->irq);
5904         }
5905 }
5906 #endif
5907
5908 /**
5909  *  stmmac_ioctl - Entry point for the Ioctl
5910  *  @dev: Device pointer.
5911  *  @rq: An IOCTL specefic structure, that can contain a pointer to
5912  *  a proprietary structure used to pass information to the driver.
5913  *  @cmd: IOCTL command
5914  *  Description:
5915  *  Currently it supports the phy_mii_ioctl(...) and HW time stamping.
5916  */
5917 static int stmmac_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
5918 {
5919         struct stmmac_priv *priv = netdev_priv (dev);
5920         int ret = -EOPNOTSUPP;
5921
5922         if (!netif_running(dev))
5923                 return -EINVAL;
5924
5925         switch (cmd) {
5926         case SIOCGMIIPHY:
5927         case SIOCGMIIREG:
5928         case SIOCSMIIREG:
5929                 ret = phylink_mii_ioctl(priv->phylink, rq, cmd);
5930                 break;
5931         case SIOCSHWTSTAMP:
5932                 ret = stmmac_hwtstamp_set(dev, rq);
5933                 break;
5934         case SIOCGHWTSTAMP:
5935                 ret = stmmac_hwtstamp_get(dev, rq);
5936                 break;
5937         default:
5938                 break;
5939         }
5940
5941         return ret;
5942 }
5943
5944 static int stmmac_setup_tc_block_cb(enum tc_setup_type type, void *type_data,
5945                                     void *cb_priv)
5946 {
5947         struct stmmac_priv *priv = cb_priv;
5948         int ret = -EOPNOTSUPP;
5949
5950         if (!tc_cls_can_offload_and_chain0(priv->dev, type_data))
5951                 return ret;
5952
5953         __stmmac_disable_all_queues(priv);
5954
5955         switch (type) {
5956         case TC_SETUP_CLSU32:
5957                 ret = stmmac_tc_setup_cls_u32(priv, priv, type_data);
5958                 break;
5959         case TC_SETUP_CLSFLOWER:
5960                 ret = stmmac_tc_setup_cls(priv, priv, type_data);
5961                 break;
5962         default:
5963                 break;
5964         }
5965
5966         stmmac_enable_all_queues(priv);
5967         return ret;
5968 }
5969
5970 static LIST_HEAD(stmmac_block_cb_list);
5971
5972 static int stmmac_setup_tc(struct net_device *ndev, enum tc_setup_type type,
5973                            void *type_data)
5974 {
5975         struct stmmac_priv *priv = netdev_priv(ndev);
5976
5977         switch (type) {
5978         case TC_SETUP_BLOCK:
5979                 return flow_block_cb_setup_simple(type_data,
5980                                                   &stmmac_block_cb_list,
5981                                                   stmmac_setup_tc_block_cb,
5982                                                   priv, priv, true);
5983         case TC_SETUP_QDISC_CBS:
5984                 return stmmac_tc_setup_cbs(priv, priv, type_data);
5985         case TC_SETUP_QDISC_TAPRIO:
5986                 return stmmac_tc_setup_taprio(priv, priv, type_data);
5987         case TC_SETUP_QDISC_ETF:
5988                 return stmmac_tc_setup_etf(priv, priv, type_data);
5989         default:
5990                 return -EOPNOTSUPP;
5991         }
5992 }
5993
5994 static u16 stmmac_select_queue(struct net_device *dev, struct sk_buff *skb,
5995                                struct net_device *sb_dev)
5996 {
5997         int gso = skb_shinfo(skb)->gso_type;
5998
5999         if (gso & (SKB_GSO_TCPV4 | SKB_GSO_TCPV6 | SKB_GSO_UDP_L4)) {
6000                 /*
6001                  * There is no way to determine the number of TSO/USO
6002                  * capable Queues. Let's use always the Queue 0
6003                  * because if TSO/USO is supported then at least this
6004                  * one will be capable.
6005                  */
6006                 return 0;
6007         }
6008
6009         return netdev_pick_tx(dev, skb, NULL) % dev->real_num_tx_queues;
6010 }
6011
6012 static int stmmac_set_mac_address(struct net_device *ndev, void *addr)
6013 {
6014         struct stmmac_priv *priv = netdev_priv(ndev);
6015         int ret = 0;
6016
6017         ret = pm_runtime_resume_and_get(priv->device);
6018         if (ret < 0)
6019                 return ret;
6020
6021         ret = eth_mac_addr(ndev, addr);
6022         if (ret)
6023                 goto set_mac_error;
6024
6025         stmmac_set_umac_addr(priv, priv->hw, ndev->dev_addr, 0);
6026
6027 set_mac_error:
6028         pm_runtime_put(priv->device);
6029
6030         return ret;
6031 }
6032
6033 #ifdef CONFIG_DEBUG_FS
6034 static struct dentry *stmmac_fs_dir;
6035
6036 static void sysfs_display_ring(void *head, int size, int extend_desc,
6037                                struct seq_file *seq, dma_addr_t dma_phy_addr)
6038 {
6039         int i;
6040         struct dma_extended_desc *ep = (struct dma_extended_desc *)head;
6041         struct dma_desc *p = (struct dma_desc *)head;
6042         dma_addr_t dma_addr;
6043
6044         for (i = 0; i < size; i++) {
6045                 if (extend_desc) {
6046                         dma_addr = dma_phy_addr + i * sizeof(*ep);
6047                         seq_printf(seq, "%d [%pad]: 0x%x 0x%x 0x%x 0x%x\n",
6048                                    i, &dma_addr,
6049                                    le32_to_cpu(ep->basic.des0),
6050                                    le32_to_cpu(ep->basic.des1),
6051                                    le32_to_cpu(ep->basic.des2),
6052                                    le32_to_cpu(ep->basic.des3));
6053                         ep++;
6054                 } else {
6055                         dma_addr = dma_phy_addr + i * sizeof(*p);
6056                         seq_printf(seq, "%d [%pad]: 0x%x 0x%x 0x%x 0x%x\n",
6057                                    i, &dma_addr,
6058                                    le32_to_cpu(p->des0), le32_to_cpu(p->des1),
6059                                    le32_to_cpu(p->des2), le32_to_cpu(p->des3));
6060                         p++;
6061                 }
6062                 seq_printf(seq, "\n");
6063         }
6064 }
6065
6066 static int stmmac_rings_status_show(struct seq_file *seq, void *v)
6067 {
6068         struct net_device *dev = seq->private;
6069         struct stmmac_priv *priv = netdev_priv(dev);
6070         u32 rx_count = priv->plat->rx_queues_to_use;
6071         u32 tx_count = priv->plat->tx_queues_to_use;
6072         u32 queue;
6073
6074         if ((dev->flags & IFF_UP) == 0)
6075                 return 0;
6076
6077         for (queue = 0; queue < rx_count; queue++) {
6078                 struct stmmac_rx_queue *rx_q = &priv->dma_conf.rx_queue[queue];
6079
6080                 seq_printf(seq, "RX Queue %d:\n", queue);
6081
6082                 if (priv->extend_desc) {
6083                         seq_printf(seq, "Extended descriptor ring:\n");
6084                         sysfs_display_ring((void *)rx_q->dma_erx,
6085                                            priv->dma_conf.dma_rx_size, 1, seq, rx_q->dma_rx_phy);
6086                 } else {
6087                         seq_printf(seq, "Descriptor ring:\n");
6088                         sysfs_display_ring((void *)rx_q->dma_rx,
6089                                            priv->dma_conf.dma_rx_size, 0, seq, rx_q->dma_rx_phy);
6090                 }
6091         }
6092
6093         for (queue = 0; queue < tx_count; queue++) {
6094                 struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[queue];
6095
6096                 seq_printf(seq, "TX Queue %d:\n", queue);
6097
6098                 if (priv->extend_desc) {
6099                         seq_printf(seq, "Extended descriptor ring:\n");
6100                         sysfs_display_ring((void *)tx_q->dma_etx,
6101                                            priv->dma_conf.dma_tx_size, 1, seq, tx_q->dma_tx_phy);
6102                 } else if (!(tx_q->tbs & STMMAC_TBS_AVAIL)) {
6103                         seq_printf(seq, "Descriptor ring:\n");
6104                         sysfs_display_ring((void *)tx_q->dma_tx,
6105                                            priv->dma_conf.dma_tx_size, 0, seq, tx_q->dma_tx_phy);
6106                 }
6107         }
6108
6109         return 0;
6110 }
6111 DEFINE_SHOW_ATTRIBUTE(stmmac_rings_status);
6112
6113 static int stmmac_dma_cap_show(struct seq_file *seq, void *v)
6114 {
6115         struct net_device *dev = seq->private;
6116         struct stmmac_priv *priv = netdev_priv(dev);
6117
6118         if (!priv->hw_cap_support) {
6119                 seq_printf(seq, "DMA HW features not supported\n");
6120                 return 0;
6121         }
6122
6123         seq_printf(seq, "==============================\n");
6124         seq_printf(seq, "\tDMA HW features\n");
6125         seq_printf(seq, "==============================\n");
6126
6127         seq_printf(seq, "\t10/100 Mbps: %s\n",
6128                    (priv->dma_cap.mbps_10_100) ? "Y" : "N");
6129         seq_printf(seq, "\t1000 Mbps: %s\n",
6130                    (priv->dma_cap.mbps_1000) ? "Y" : "N");
6131         seq_printf(seq, "\tHalf duplex: %s\n",
6132                    (priv->dma_cap.half_duplex) ? "Y" : "N");
6133         seq_printf(seq, "\tHash Filter: %s\n",
6134                    (priv->dma_cap.hash_filter) ? "Y" : "N");
6135         seq_printf(seq, "\tMultiple MAC address registers: %s\n",
6136                    (priv->dma_cap.multi_addr) ? "Y" : "N");
6137         seq_printf(seq, "\tPCS (TBI/SGMII/RTBI PHY interfaces): %s\n",
6138                    (priv->dma_cap.pcs) ? "Y" : "N");
6139         seq_printf(seq, "\tSMA (MDIO) Interface: %s\n",
6140                    (priv->dma_cap.sma_mdio) ? "Y" : "N");
6141         seq_printf(seq, "\tPMT Remote wake up: %s\n",
6142                    (priv->dma_cap.pmt_remote_wake_up) ? "Y" : "N");
6143         seq_printf(seq, "\tPMT Magic Frame: %s\n",
6144                    (priv->dma_cap.pmt_magic_frame) ? "Y" : "N");
6145         seq_printf(seq, "\tRMON module: %s\n",
6146                    (priv->dma_cap.rmon) ? "Y" : "N");
6147         seq_printf(seq, "\tIEEE 1588-2002 Time Stamp: %s\n",
6148                    (priv->dma_cap.time_stamp) ? "Y" : "N");
6149         seq_printf(seq, "\tIEEE 1588-2008 Advanced Time Stamp: %s\n",
6150                    (priv->dma_cap.atime_stamp) ? "Y" : "N");
6151         seq_printf(seq, "\t802.3az - Energy-Efficient Ethernet (EEE): %s\n",
6152                    (priv->dma_cap.eee) ? "Y" : "N");
6153         seq_printf(seq, "\tAV features: %s\n", (priv->dma_cap.av) ? "Y" : "N");
6154         seq_printf(seq, "\tChecksum Offload in TX: %s\n",
6155                    (priv->dma_cap.tx_coe) ? "Y" : "N");
6156         if (priv->synopsys_id >= DWMAC_CORE_4_00) {
6157                 seq_printf(seq, "\tIP Checksum Offload in RX: %s\n",
6158                            (priv->dma_cap.rx_coe) ? "Y" : "N");
6159         } else {
6160                 seq_printf(seq, "\tIP Checksum Offload (type1) in RX: %s\n",
6161                            (priv->dma_cap.rx_coe_type1) ? "Y" : "N");
6162                 seq_printf(seq, "\tIP Checksum Offload (type2) in RX: %s\n",
6163                            (priv->dma_cap.rx_coe_type2) ? "Y" : "N");
6164         }
6165         seq_printf(seq, "\tRXFIFO > 2048bytes: %s\n",
6166                    (priv->dma_cap.rxfifo_over_2048) ? "Y" : "N");
6167         seq_printf(seq, "\tNumber of Additional RX channel: %d\n",
6168                    priv->dma_cap.number_rx_channel);
6169         seq_printf(seq, "\tNumber of Additional TX channel: %d\n",
6170                    priv->dma_cap.number_tx_channel);
6171         seq_printf(seq, "\tNumber of Additional RX queues: %d\n",
6172                    priv->dma_cap.number_rx_queues);
6173         seq_printf(seq, "\tNumber of Additional TX queues: %d\n",
6174                    priv->dma_cap.number_tx_queues);
6175         seq_printf(seq, "\tEnhanced descriptors: %s\n",
6176                    (priv->dma_cap.enh_desc) ? "Y" : "N");
6177         seq_printf(seq, "\tTX Fifo Size: %d\n", priv->dma_cap.tx_fifo_size);
6178         seq_printf(seq, "\tRX Fifo Size: %d\n", priv->dma_cap.rx_fifo_size);
6179         seq_printf(seq, "\tHash Table Size: %d\n", priv->dma_cap.hash_tb_sz);
6180         seq_printf(seq, "\tTSO: %s\n", priv->dma_cap.tsoen ? "Y" : "N");
6181         seq_printf(seq, "\tNumber of PPS Outputs: %d\n",
6182                    priv->dma_cap.pps_out_num);
6183         seq_printf(seq, "\tSafety Features: %s\n",
6184                    priv->dma_cap.asp ? "Y" : "N");
6185         seq_printf(seq, "\tFlexible RX Parser: %s\n",
6186                    priv->dma_cap.frpsel ? "Y" : "N");
6187         seq_printf(seq, "\tEnhanced Addressing: %d\n",
6188                    priv->dma_cap.addr64);
6189         seq_printf(seq, "\tReceive Side Scaling: %s\n",
6190                    priv->dma_cap.rssen ? "Y" : "N");
6191         seq_printf(seq, "\tVLAN Hash Filtering: %s\n",
6192                    priv->dma_cap.vlhash ? "Y" : "N");
6193         seq_printf(seq, "\tSplit Header: %s\n",
6194                    priv->dma_cap.sphen ? "Y" : "N");
6195         seq_printf(seq, "\tVLAN TX Insertion: %s\n",
6196                    priv->dma_cap.vlins ? "Y" : "N");
6197         seq_printf(seq, "\tDouble VLAN: %s\n",
6198                    priv->dma_cap.dvlan ? "Y" : "N");
6199         seq_printf(seq, "\tNumber of L3/L4 Filters: %d\n",
6200                    priv->dma_cap.l3l4fnum);
6201         seq_printf(seq, "\tARP Offloading: %s\n",
6202                    priv->dma_cap.arpoffsel ? "Y" : "N");
6203         seq_printf(seq, "\tEnhancements to Scheduled Traffic (EST): %s\n",
6204                    priv->dma_cap.estsel ? "Y" : "N");
6205         seq_printf(seq, "\tFrame Preemption (FPE): %s\n",
6206                    priv->dma_cap.fpesel ? "Y" : "N");
6207         seq_printf(seq, "\tTime-Based Scheduling (TBS): %s\n",
6208                    priv->dma_cap.tbssel ? "Y" : "N");
6209         return 0;
6210 }
6211 DEFINE_SHOW_ATTRIBUTE(stmmac_dma_cap);
6212
6213 /* Use network device events to rename debugfs file entries.
6214  */
6215 static int stmmac_device_event(struct notifier_block *unused,
6216                                unsigned long event, void *ptr)
6217 {
6218         struct net_device *dev = netdev_notifier_info_to_dev(ptr);
6219         struct stmmac_priv *priv = netdev_priv(dev);
6220
6221         if (dev->netdev_ops != &stmmac_netdev_ops)
6222                 goto done;
6223
6224         switch (event) {
6225         case NETDEV_CHANGENAME:
6226                 if (priv->dbgfs_dir)
6227                         priv->dbgfs_dir = debugfs_rename(stmmac_fs_dir,
6228                                                          priv->dbgfs_dir,
6229                                                          stmmac_fs_dir,
6230                                                          dev->name);
6231                 break;
6232         }
6233 done:
6234         return NOTIFY_DONE;
6235 }
6236
6237 static struct notifier_block stmmac_notifier = {
6238         .notifier_call = stmmac_device_event,
6239 };
6240
6241 static void stmmac_init_fs(struct net_device *dev)
6242 {
6243         struct stmmac_priv *priv = netdev_priv(dev);
6244
6245         rtnl_lock();
6246
6247         /* Create per netdev entries */
6248         priv->dbgfs_dir = debugfs_create_dir(dev->name, stmmac_fs_dir);
6249
6250         /* Entry to report DMA RX/TX rings */
6251         debugfs_create_file("descriptors_status", 0444, priv->dbgfs_dir, dev,
6252                             &stmmac_rings_status_fops);
6253
6254         /* Entry to report the DMA HW features */
6255         debugfs_create_file("dma_cap", 0444, priv->dbgfs_dir, dev,
6256                             &stmmac_dma_cap_fops);
6257
6258         rtnl_unlock();
6259 }
6260
6261 static void stmmac_exit_fs(struct net_device *dev)
6262 {
6263         struct stmmac_priv *priv = netdev_priv(dev);
6264
6265         debugfs_remove_recursive(priv->dbgfs_dir);
6266 }
6267 #endif /* CONFIG_DEBUG_FS */
6268
6269 static u32 stmmac_vid_crc32_le(__le16 vid_le)
6270 {
6271         unsigned char *data = (unsigned char *)&vid_le;
6272         unsigned char data_byte = 0;
6273         u32 crc = ~0x0;
6274         u32 temp = 0;
6275         int i, bits;
6276
6277         bits = get_bitmask_order(VLAN_VID_MASK);
6278         for (i = 0; i < bits; i++) {
6279                 if ((i % 8) == 0)
6280                         data_byte = data[i / 8];
6281
6282                 temp = ((crc & 1) ^ data_byte) & 1;
6283                 crc >>= 1;
6284                 data_byte >>= 1;
6285
6286                 if (temp)
6287                         crc ^= 0xedb88320;
6288         }
6289
6290         return crc;
6291 }
6292
6293 static int stmmac_vlan_update(struct stmmac_priv *priv, bool is_double)
6294 {
6295         u32 crc, hash = 0;
6296         __le16 pmatch = 0;
6297         int count = 0;
6298         u16 vid = 0;
6299
6300         for_each_set_bit(vid, priv->active_vlans, VLAN_N_VID) {
6301                 __le16 vid_le = cpu_to_le16(vid);
6302                 crc = bitrev32(~stmmac_vid_crc32_le(vid_le)) >> 28;
6303                 hash |= (1 << crc);
6304                 count++;
6305         }
6306
6307         if (!priv->dma_cap.vlhash) {
6308                 if (count > 2) /* VID = 0 always passes filter */
6309                         return -EOPNOTSUPP;
6310
6311                 pmatch = cpu_to_le16(vid);
6312                 hash = 0;
6313         }
6314
6315         return stmmac_update_vlan_hash(priv, priv->hw, hash, pmatch, is_double);
6316 }
6317
6318 static int stmmac_vlan_rx_add_vid(struct net_device *ndev, __be16 proto, u16 vid)
6319 {
6320         struct stmmac_priv *priv = netdev_priv(ndev);
6321         bool is_double = false;
6322         int ret;
6323
6324         if (be16_to_cpu(proto) == ETH_P_8021AD)
6325                 is_double = true;
6326
6327         set_bit(vid, priv->active_vlans);
6328         ret = stmmac_vlan_update(priv, is_double);
6329         if (ret) {
6330                 clear_bit(vid, priv->active_vlans);
6331                 return ret;
6332         }
6333
6334         if (priv->hw->num_vlan) {
6335                 ret = stmmac_add_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid);
6336                 if (ret)
6337                         return ret;
6338         }
6339
6340         return 0;
6341 }
6342
6343 static int stmmac_vlan_rx_kill_vid(struct net_device *ndev, __be16 proto, u16 vid)
6344 {
6345         struct stmmac_priv *priv = netdev_priv(ndev);
6346         bool is_double = false;
6347         int ret;
6348
6349         ret = pm_runtime_resume_and_get(priv->device);
6350         if (ret < 0)
6351                 return ret;
6352
6353         if (be16_to_cpu(proto) == ETH_P_8021AD)
6354                 is_double = true;
6355
6356         clear_bit(vid, priv->active_vlans);
6357
6358         if (priv->hw->num_vlan) {
6359                 ret = stmmac_del_hw_vlan_rx_fltr(priv, ndev, priv->hw, proto, vid);
6360                 if (ret)
6361                         goto del_vlan_error;
6362         }
6363
6364         ret = stmmac_vlan_update(priv, is_double);
6365
6366 del_vlan_error:
6367         pm_runtime_put(priv->device);
6368
6369         return ret;
6370 }
6371
6372 static int stmmac_bpf(struct net_device *dev, struct netdev_bpf *bpf)
6373 {
6374         struct stmmac_priv *priv = netdev_priv(dev);
6375
6376         switch (bpf->command) {
6377         case XDP_SETUP_PROG:
6378                 return stmmac_xdp_set_prog(priv, bpf->prog, bpf->extack);
6379         case XDP_SETUP_XSK_POOL:
6380                 return stmmac_xdp_setup_pool(priv, bpf->xsk.pool,
6381                                              bpf->xsk.queue_id);
6382         default:
6383                 return -EOPNOTSUPP;
6384         }
6385 }
6386
6387 static int stmmac_xdp_xmit(struct net_device *dev, int num_frames,
6388                            struct xdp_frame **frames, u32 flags)
6389 {
6390         struct stmmac_priv *priv = netdev_priv(dev);
6391         int cpu = smp_processor_id();
6392         struct netdev_queue *nq;
6393         int i, nxmit = 0;
6394         int queue;
6395
6396         if (unlikely(test_bit(STMMAC_DOWN, &priv->state)))
6397                 return -ENETDOWN;
6398
6399         if (unlikely(flags & ~XDP_XMIT_FLAGS_MASK))
6400                 return -EINVAL;
6401
6402         queue = stmmac_xdp_get_tx_queue(priv, cpu);
6403         nq = netdev_get_tx_queue(priv->dev, queue);
6404
6405         __netif_tx_lock(nq, cpu);
6406         /* Avoids TX time-out as we are sharing with slow path */
6407         txq_trans_cond_update(nq);
6408
6409         for (i = 0; i < num_frames; i++) {
6410                 int res;
6411
6412                 res = stmmac_xdp_xmit_xdpf(priv, queue, frames[i], true);
6413                 if (res == STMMAC_XDP_CONSUMED)
6414                         break;
6415
6416                 nxmit++;
6417         }
6418
6419         if (flags & XDP_XMIT_FLUSH) {
6420                 stmmac_flush_tx_descriptors(priv, queue);
6421                 stmmac_tx_timer_arm(priv, queue);
6422         }
6423
6424         __netif_tx_unlock(nq);
6425
6426         return nxmit;
6427 }
6428
6429 void stmmac_disable_rx_queue(struct stmmac_priv *priv, u32 queue)
6430 {
6431         struct stmmac_channel *ch = &priv->channel[queue];
6432         unsigned long flags;
6433
6434         spin_lock_irqsave(&ch->lock, flags);
6435         stmmac_disable_dma_irq(priv, priv->ioaddr, queue, 1, 0);
6436         spin_unlock_irqrestore(&ch->lock, flags);
6437
6438         stmmac_stop_rx_dma(priv, queue);
6439         __free_dma_rx_desc_resources(priv, &priv->dma_conf, queue);
6440 }
6441
6442 void stmmac_enable_rx_queue(struct stmmac_priv *priv, u32 queue)
6443 {
6444         struct stmmac_rx_queue *rx_q = &priv->dma_conf.rx_queue[queue];
6445         struct stmmac_channel *ch = &priv->channel[queue];
6446         unsigned long flags;
6447         u32 buf_size;
6448         int ret;
6449
6450         ret = __alloc_dma_rx_desc_resources(priv, &priv->dma_conf, queue);
6451         if (ret) {
6452                 netdev_err(priv->dev, "Failed to alloc RX desc.\n");
6453                 return;
6454         }
6455
6456         ret = __init_dma_rx_desc_rings(priv, &priv->dma_conf, queue, GFP_KERNEL);
6457         if (ret) {
6458                 __free_dma_rx_desc_resources(priv, &priv->dma_conf, queue);
6459                 netdev_err(priv->dev, "Failed to init RX desc.\n");
6460                 return;
6461         }
6462
6463         stmmac_reset_rx_queue(priv, queue);
6464         stmmac_clear_rx_descriptors(priv, &priv->dma_conf, queue);
6465
6466         stmmac_init_rx_chan(priv, priv->ioaddr, priv->plat->dma_cfg,
6467                             rx_q->dma_rx_phy, rx_q->queue_index);
6468
6469         rx_q->rx_tail_addr = rx_q->dma_rx_phy + (rx_q->buf_alloc_num *
6470                              sizeof(struct dma_desc));
6471         stmmac_set_rx_tail_ptr(priv, priv->ioaddr,
6472                                rx_q->rx_tail_addr, rx_q->queue_index);
6473
6474         if (rx_q->xsk_pool && rx_q->buf_alloc_num) {
6475                 buf_size = xsk_pool_get_rx_frame_size(rx_q->xsk_pool);
6476                 stmmac_set_dma_bfsize(priv, priv->ioaddr,
6477                                       buf_size,
6478                                       rx_q->queue_index);
6479         } else {
6480                 stmmac_set_dma_bfsize(priv, priv->ioaddr,
6481                                       priv->dma_conf.dma_buf_sz,
6482                                       rx_q->queue_index);
6483         }
6484
6485         stmmac_start_rx_dma(priv, queue);
6486
6487         spin_lock_irqsave(&ch->lock, flags);
6488         stmmac_enable_dma_irq(priv, priv->ioaddr, queue, 1, 0);
6489         spin_unlock_irqrestore(&ch->lock, flags);
6490 }
6491
6492 void stmmac_disable_tx_queue(struct stmmac_priv *priv, u32 queue)
6493 {
6494         struct stmmac_channel *ch = &priv->channel[queue];
6495         unsigned long flags;
6496
6497         spin_lock_irqsave(&ch->lock, flags);
6498         stmmac_disable_dma_irq(priv, priv->ioaddr, queue, 0, 1);
6499         spin_unlock_irqrestore(&ch->lock, flags);
6500
6501         stmmac_stop_tx_dma(priv, queue);
6502         __free_dma_tx_desc_resources(priv, &priv->dma_conf, queue);
6503 }
6504
6505 void stmmac_enable_tx_queue(struct stmmac_priv *priv, u32 queue)
6506 {
6507         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[queue];
6508         struct stmmac_channel *ch = &priv->channel[queue];
6509         unsigned long flags;
6510         int ret;
6511
6512         ret = __alloc_dma_tx_desc_resources(priv, &priv->dma_conf, queue);
6513         if (ret) {
6514                 netdev_err(priv->dev, "Failed to alloc TX desc.\n");
6515                 return;
6516         }
6517
6518         ret = __init_dma_tx_desc_rings(priv,  &priv->dma_conf, queue);
6519         if (ret) {
6520                 __free_dma_tx_desc_resources(priv, &priv->dma_conf, queue);
6521                 netdev_err(priv->dev, "Failed to init TX desc.\n");
6522                 return;
6523         }
6524
6525         stmmac_reset_tx_queue(priv, queue);
6526         stmmac_clear_tx_descriptors(priv, &priv->dma_conf, queue);
6527
6528         stmmac_init_tx_chan(priv, priv->ioaddr, priv->plat->dma_cfg,
6529                             tx_q->dma_tx_phy, tx_q->queue_index);
6530
6531         if (tx_q->tbs & STMMAC_TBS_AVAIL)
6532                 stmmac_enable_tbs(priv, priv->ioaddr, 1, tx_q->queue_index);
6533
6534         tx_q->tx_tail_addr = tx_q->dma_tx_phy;
6535         stmmac_set_tx_tail_ptr(priv, priv->ioaddr,
6536                                tx_q->tx_tail_addr, tx_q->queue_index);
6537
6538         stmmac_start_tx_dma(priv, queue);
6539
6540         spin_lock_irqsave(&ch->lock, flags);
6541         stmmac_enable_dma_irq(priv, priv->ioaddr, queue, 0, 1);
6542         spin_unlock_irqrestore(&ch->lock, flags);
6543 }
6544
6545 void stmmac_xdp_release(struct net_device *dev)
6546 {
6547         struct stmmac_priv *priv = netdev_priv(dev);
6548         u32 chan;
6549
6550         /* Disable NAPI process */
6551         stmmac_disable_all_queues(priv);
6552
6553         for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++)
6554                 hrtimer_cancel(&priv->dma_conf.tx_queue[chan].txtimer);
6555
6556         /* Free the IRQ lines */
6557         stmmac_free_irq(dev, REQ_IRQ_ERR_ALL, 0);
6558
6559         /* Stop TX/RX DMA channels */
6560         stmmac_stop_all_dma(priv);
6561
6562         /* Release and free the Rx/Tx resources */
6563         free_dma_desc_resources(priv, &priv->dma_conf);
6564
6565         /* Disable the MAC Rx/Tx */
6566         stmmac_mac_set(priv, priv->ioaddr, false);
6567
6568         /* set trans_start so we don't get spurious
6569          * watchdogs during reset
6570          */
6571         netif_trans_update(dev);
6572         netif_carrier_off(dev);
6573 }
6574
6575 int stmmac_xdp_open(struct net_device *dev)
6576 {
6577         struct stmmac_priv *priv = netdev_priv(dev);
6578         u32 rx_cnt = priv->plat->rx_queues_to_use;
6579         u32 tx_cnt = priv->plat->tx_queues_to_use;
6580         u32 dma_csr_ch = max(rx_cnt, tx_cnt);
6581         struct stmmac_rx_queue *rx_q;
6582         struct stmmac_tx_queue *tx_q;
6583         u32 buf_size;
6584         bool sph_en;
6585         u32 chan;
6586         int ret;
6587
6588         ret = alloc_dma_desc_resources(priv, &priv->dma_conf);
6589         if (ret < 0) {
6590                 netdev_err(dev, "%s: DMA descriptors allocation failed\n",
6591                            __func__);
6592                 goto dma_desc_error;
6593         }
6594
6595         ret = init_dma_desc_rings(dev, &priv->dma_conf, GFP_KERNEL);
6596         if (ret < 0) {
6597                 netdev_err(dev, "%s: DMA descriptors initialization failed\n",
6598                            __func__);
6599                 goto init_error;
6600         }
6601
6602         /* DMA CSR Channel configuration */
6603         for (chan = 0; chan < dma_csr_ch; chan++) {
6604                 stmmac_init_chan(priv, priv->ioaddr, priv->plat->dma_cfg, chan);
6605                 stmmac_disable_dma_irq(priv, priv->ioaddr, chan, 1, 1);
6606         }
6607
6608         /* Adjust Split header */
6609         sph_en = (priv->hw->rx_csum > 0) && priv->sph;
6610
6611         /* DMA RX Channel Configuration */
6612         for (chan = 0; chan < rx_cnt; chan++) {
6613                 rx_q = &priv->dma_conf.rx_queue[chan];
6614
6615                 stmmac_init_rx_chan(priv, priv->ioaddr, priv->plat->dma_cfg,
6616                                     rx_q->dma_rx_phy, chan);
6617
6618                 rx_q->rx_tail_addr = rx_q->dma_rx_phy +
6619                                      (rx_q->buf_alloc_num *
6620                                       sizeof(struct dma_desc));
6621                 stmmac_set_rx_tail_ptr(priv, priv->ioaddr,
6622                                        rx_q->rx_tail_addr, chan);
6623
6624                 if (rx_q->xsk_pool && rx_q->buf_alloc_num) {
6625                         buf_size = xsk_pool_get_rx_frame_size(rx_q->xsk_pool);
6626                         stmmac_set_dma_bfsize(priv, priv->ioaddr,
6627                                               buf_size,
6628                                               rx_q->queue_index);
6629                 } else {
6630                         stmmac_set_dma_bfsize(priv, priv->ioaddr,
6631                                               priv->dma_conf.dma_buf_sz,
6632                                               rx_q->queue_index);
6633                 }
6634
6635                 stmmac_enable_sph(priv, priv->ioaddr, sph_en, chan);
6636         }
6637
6638         /* DMA TX Channel Configuration */
6639         for (chan = 0; chan < tx_cnt; chan++) {
6640                 tx_q = &priv->dma_conf.tx_queue[chan];
6641
6642                 stmmac_init_tx_chan(priv, priv->ioaddr, priv->plat->dma_cfg,
6643                                     tx_q->dma_tx_phy, chan);
6644
6645                 tx_q->tx_tail_addr = tx_q->dma_tx_phy;
6646                 stmmac_set_tx_tail_ptr(priv, priv->ioaddr,
6647                                        tx_q->tx_tail_addr, chan);
6648
6649                 hrtimer_init(&tx_q->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
6650                 tx_q->txtimer.function = stmmac_tx_timer;
6651         }
6652
6653         /* Enable the MAC Rx/Tx */
6654         stmmac_mac_set(priv, priv->ioaddr, true);
6655
6656         /* Start Rx & Tx DMA Channels */
6657         stmmac_start_all_dma(priv);
6658
6659         ret = stmmac_request_irq(dev);
6660         if (ret)
6661                 goto irq_error;
6662
6663         /* Enable NAPI process*/
6664         stmmac_enable_all_queues(priv);
6665         netif_carrier_on(dev);
6666         netif_tx_start_all_queues(dev);
6667         stmmac_enable_all_dma_irq(priv);
6668
6669         return 0;
6670
6671 irq_error:
6672         for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++)
6673                 hrtimer_cancel(&priv->dma_conf.tx_queue[chan].txtimer);
6674
6675         stmmac_hw_teardown(dev);
6676 init_error:
6677         free_dma_desc_resources(priv, &priv->dma_conf);
6678 dma_desc_error:
6679         return ret;
6680 }
6681
6682 int stmmac_xsk_wakeup(struct net_device *dev, u32 queue, u32 flags)
6683 {
6684         struct stmmac_priv *priv = netdev_priv(dev);
6685         struct stmmac_rx_queue *rx_q;
6686         struct stmmac_tx_queue *tx_q;
6687         struct stmmac_channel *ch;
6688
6689         if (test_bit(STMMAC_DOWN, &priv->state) ||
6690             !netif_carrier_ok(priv->dev))
6691                 return -ENETDOWN;
6692
6693         if (!stmmac_xdp_is_enabled(priv))
6694                 return -EINVAL;
6695
6696         if (queue >= priv->plat->rx_queues_to_use ||
6697             queue >= priv->plat->tx_queues_to_use)
6698                 return -EINVAL;
6699
6700         rx_q = &priv->dma_conf.rx_queue[queue];
6701         tx_q = &priv->dma_conf.tx_queue[queue];
6702         ch = &priv->channel[queue];
6703
6704         if (!rx_q->xsk_pool && !tx_q->xsk_pool)
6705                 return -EINVAL;
6706
6707         if (!napi_if_scheduled_mark_missed(&ch->rxtx_napi)) {
6708                 /* EQoS does not have per-DMA channel SW interrupt,
6709                  * so we schedule RX Napi straight-away.
6710                  */
6711                 if (likely(napi_schedule_prep(&ch->rxtx_napi)))
6712                         __napi_schedule(&ch->rxtx_napi);
6713         }
6714
6715         return 0;
6716 }
6717
6718 static const struct net_device_ops stmmac_netdev_ops = {
6719         .ndo_open = stmmac_open,
6720         .ndo_start_xmit = stmmac_xmit,
6721         .ndo_stop = stmmac_release,
6722         .ndo_change_mtu = stmmac_change_mtu,
6723         .ndo_fix_features = stmmac_fix_features,
6724         .ndo_set_features = stmmac_set_features,
6725         .ndo_set_rx_mode = stmmac_set_rx_mode,
6726         .ndo_tx_timeout = stmmac_tx_timeout,
6727         .ndo_eth_ioctl = stmmac_ioctl,
6728         .ndo_setup_tc = stmmac_setup_tc,
6729         .ndo_select_queue = stmmac_select_queue,
6730 #ifdef CONFIG_NET_POLL_CONTROLLER
6731         .ndo_poll_controller = stmmac_poll_controller,
6732 #endif
6733         .ndo_set_mac_address = stmmac_set_mac_address,
6734         .ndo_vlan_rx_add_vid = stmmac_vlan_rx_add_vid,
6735         .ndo_vlan_rx_kill_vid = stmmac_vlan_rx_kill_vid,
6736         .ndo_bpf = stmmac_bpf,
6737         .ndo_xdp_xmit = stmmac_xdp_xmit,
6738         .ndo_xsk_wakeup = stmmac_xsk_wakeup,
6739 };
6740
6741 static void stmmac_reset_subtask(struct stmmac_priv *priv)
6742 {
6743         if (!test_and_clear_bit(STMMAC_RESET_REQUESTED, &priv->state))
6744                 return;
6745         if (test_bit(STMMAC_DOWN, &priv->state))
6746                 return;
6747
6748         netdev_err(priv->dev, "Reset adapter.\n");
6749
6750         rtnl_lock();
6751         netif_trans_update(priv->dev);
6752         while (test_and_set_bit(STMMAC_RESETING, &priv->state))
6753                 usleep_range(1000, 2000);
6754
6755         set_bit(STMMAC_DOWN, &priv->state);
6756         dev_close(priv->dev);
6757         dev_open(priv->dev, NULL);
6758         clear_bit(STMMAC_DOWN, &priv->state);
6759         clear_bit(STMMAC_RESETING, &priv->state);
6760         rtnl_unlock();
6761 }
6762
6763 static void stmmac_service_task(struct work_struct *work)
6764 {
6765         struct stmmac_priv *priv = container_of(work, struct stmmac_priv,
6766                         service_task);
6767
6768         stmmac_reset_subtask(priv);
6769         clear_bit(STMMAC_SERVICE_SCHED, &priv->state);
6770 }
6771
6772 /**
6773  *  stmmac_hw_init - Init the MAC device
6774  *  @priv: driver private structure
6775  *  Description: this function is to configure the MAC device according to
6776  *  some platform parameters or the HW capability register. It prepares the
6777  *  driver to use either ring or chain modes and to setup either enhanced or
6778  *  normal descriptors.
6779  */
6780 static int stmmac_hw_init(struct stmmac_priv *priv)
6781 {
6782         int ret;
6783
6784         /* dwmac-sun8i only work in chain mode */
6785         if (priv->plat->has_sun8i)
6786                 chain_mode = 1;
6787         priv->chain_mode = chain_mode;
6788
6789         /* Initialize HW Interface */
6790         ret = stmmac_hwif_init(priv);
6791         if (ret)
6792                 return ret;
6793
6794         /* Get the HW capability (new GMAC newer than 3.50a) */
6795         priv->hw_cap_support = stmmac_get_hw_features(priv);
6796         if (priv->hw_cap_support) {
6797                 dev_info(priv->device, "DMA HW capability register supported\n");
6798
6799                 /* We can override some gmac/dma configuration fields: e.g.
6800                  * enh_desc, tx_coe (e.g. that are passed through the
6801                  * platform) with the values from the HW capability
6802                  * register (if supported).
6803                  */
6804                 priv->plat->enh_desc = priv->dma_cap.enh_desc;
6805                 priv->plat->pmt = priv->dma_cap.pmt_remote_wake_up &&
6806                                 !priv->plat->use_phy_wol;
6807                 priv->hw->pmt = priv->plat->pmt;
6808                 if (priv->dma_cap.hash_tb_sz) {
6809                         priv->hw->multicast_filter_bins =
6810                                         (BIT(priv->dma_cap.hash_tb_sz) << 5);
6811                         priv->hw->mcast_bits_log2 =
6812                                         ilog2(priv->hw->multicast_filter_bins);
6813                 }
6814
6815                 /* TXCOE doesn't work in thresh DMA mode */
6816                 if (priv->plat->force_thresh_dma_mode)
6817                         priv->plat->tx_coe = 0;
6818                 else
6819                         priv->plat->tx_coe = priv->dma_cap.tx_coe;
6820
6821                 /* In case of GMAC4 rx_coe is from HW cap register. */
6822                 priv->plat->rx_coe = priv->dma_cap.rx_coe;
6823
6824                 if (priv->dma_cap.rx_coe_type2)
6825                         priv->plat->rx_coe = STMMAC_RX_COE_TYPE2;
6826                 else if (priv->dma_cap.rx_coe_type1)
6827                         priv->plat->rx_coe = STMMAC_RX_COE_TYPE1;
6828
6829         } else {
6830                 dev_info(priv->device, "No HW DMA feature register supported\n");
6831         }
6832
6833         if (priv->plat->rx_coe) {
6834                 priv->hw->rx_csum = priv->plat->rx_coe;
6835                 dev_info(priv->device, "RX Checksum Offload Engine supported\n");
6836                 if (priv->synopsys_id < DWMAC_CORE_4_00)
6837                         dev_info(priv->device, "COE Type %d\n", priv->hw->rx_csum);
6838         }
6839         if (priv->plat->tx_coe)
6840                 dev_info(priv->device, "TX Checksum insertion supported\n");
6841
6842         if (priv->plat->pmt) {
6843                 dev_info(priv->device, "Wake-Up On Lan supported\n");
6844                 device_set_wakeup_capable(priv->device, 1);
6845         }
6846
6847         if (priv->dma_cap.tsoen)
6848                 dev_info(priv->device, "TSO supported\n");
6849
6850         priv->hw->vlan_fail_q_en = priv->plat->vlan_fail_q_en;
6851         priv->hw->vlan_fail_q = priv->plat->vlan_fail_q;
6852
6853         /* Run HW quirks, if any */
6854         if (priv->hwif_quirks) {
6855                 ret = priv->hwif_quirks(priv);
6856                 if (ret)
6857                         return ret;
6858         }
6859
6860         /* Rx Watchdog is available in the COREs newer than the 3.40.
6861          * In some case, for example on bugged HW this feature
6862          * has to be disable and this can be done by passing the
6863          * riwt_off field from the platform.
6864          */
6865         if (((priv->synopsys_id >= DWMAC_CORE_3_50) ||
6866             (priv->plat->has_xgmac)) && (!priv->plat->riwt_off)) {
6867                 priv->use_riwt = 1;
6868                 dev_info(priv->device,
6869                          "Enable RX Mitigation via HW Watchdog Timer\n");
6870         }
6871
6872         return 0;
6873 }
6874
6875 static void stmmac_napi_add(struct net_device *dev)
6876 {
6877         struct stmmac_priv *priv = netdev_priv(dev);
6878         u32 queue, maxq;
6879
6880         maxq = max(priv->plat->rx_queues_to_use, priv->plat->tx_queues_to_use);
6881
6882         for (queue = 0; queue < maxq; queue++) {
6883                 struct stmmac_channel *ch = &priv->channel[queue];
6884
6885                 ch->priv_data = priv;
6886                 ch->index = queue;
6887                 spin_lock_init(&ch->lock);
6888
6889                 if (queue < priv->plat->rx_queues_to_use) {
6890                         netif_napi_add(dev, &ch->rx_napi, stmmac_napi_poll_rx);
6891                 }
6892                 if (queue < priv->plat->tx_queues_to_use) {
6893                         netif_napi_add_tx(dev, &ch->tx_napi,
6894                                           stmmac_napi_poll_tx);
6895                 }
6896                 if (queue < priv->plat->rx_queues_to_use &&
6897                     queue < priv->plat->tx_queues_to_use) {
6898                         netif_napi_add(dev, &ch->rxtx_napi,
6899                                        stmmac_napi_poll_rxtx);
6900                 }
6901         }
6902 }
6903
6904 static void stmmac_napi_del(struct net_device *dev)
6905 {
6906         struct stmmac_priv *priv = netdev_priv(dev);
6907         u32 queue, maxq;
6908
6909         maxq = max(priv->plat->rx_queues_to_use, priv->plat->tx_queues_to_use);
6910
6911         for (queue = 0; queue < maxq; queue++) {
6912                 struct stmmac_channel *ch = &priv->channel[queue];
6913
6914                 if (queue < priv->plat->rx_queues_to_use)
6915                         netif_napi_del(&ch->rx_napi);
6916                 if (queue < priv->plat->tx_queues_to_use)
6917                         netif_napi_del(&ch->tx_napi);
6918                 if (queue < priv->plat->rx_queues_to_use &&
6919                     queue < priv->plat->tx_queues_to_use) {
6920                         netif_napi_del(&ch->rxtx_napi);
6921                 }
6922         }
6923 }
6924
6925 int stmmac_reinit_queues(struct net_device *dev, u32 rx_cnt, u32 tx_cnt)
6926 {
6927         struct stmmac_priv *priv = netdev_priv(dev);
6928         int ret = 0;
6929
6930         if (netif_running(dev))
6931                 stmmac_release(dev);
6932
6933         stmmac_napi_del(dev);
6934
6935         priv->plat->rx_queues_to_use = rx_cnt;
6936         priv->plat->tx_queues_to_use = tx_cnt;
6937
6938         stmmac_napi_add(dev);
6939
6940         if (netif_running(dev))
6941                 ret = stmmac_open(dev);
6942
6943         return ret;
6944 }
6945
6946 int stmmac_reinit_ringparam(struct net_device *dev, u32 rx_size, u32 tx_size)
6947 {
6948         struct stmmac_priv *priv = netdev_priv(dev);
6949         int ret = 0;
6950
6951         if (netif_running(dev))
6952                 stmmac_release(dev);
6953
6954         priv->dma_conf.dma_rx_size = rx_size;
6955         priv->dma_conf.dma_tx_size = tx_size;
6956
6957         if (netif_running(dev))
6958                 ret = stmmac_open(dev);
6959
6960         return ret;
6961 }
6962
6963 #define SEND_VERIFY_MPAKCET_FMT "Send Verify mPacket lo_state=%d lp_state=%d\n"
6964 static void stmmac_fpe_lp_task(struct work_struct *work)
6965 {
6966         struct stmmac_priv *priv = container_of(work, struct stmmac_priv,
6967                                                 fpe_task);
6968         struct stmmac_fpe_cfg *fpe_cfg = priv->plat->fpe_cfg;
6969         enum stmmac_fpe_state *lo_state = &fpe_cfg->lo_fpe_state;
6970         enum stmmac_fpe_state *lp_state = &fpe_cfg->lp_fpe_state;
6971         bool *hs_enable = &fpe_cfg->hs_enable;
6972         bool *enable = &fpe_cfg->enable;
6973         int retries = 20;
6974
6975         while (retries-- > 0) {
6976                 /* Bail out immediately if FPE handshake is OFF */
6977                 if (*lo_state == FPE_STATE_OFF || !*hs_enable)
6978                         break;
6979
6980                 if (*lo_state == FPE_STATE_ENTERING_ON &&
6981                     *lp_state == FPE_STATE_ENTERING_ON) {
6982                         stmmac_fpe_configure(priv, priv->ioaddr,
6983                                              priv->plat->tx_queues_to_use,
6984                                              priv->plat->rx_queues_to_use,
6985                                              *enable);
6986
6987                         netdev_info(priv->dev, "configured FPE\n");
6988
6989                         *lo_state = FPE_STATE_ON;
6990                         *lp_state = FPE_STATE_ON;
6991                         netdev_info(priv->dev, "!!! BOTH FPE stations ON\n");
6992                         break;
6993                 }
6994
6995                 if ((*lo_state == FPE_STATE_CAPABLE ||
6996                      *lo_state == FPE_STATE_ENTERING_ON) &&
6997                      *lp_state != FPE_STATE_ON) {
6998                         netdev_info(priv->dev, SEND_VERIFY_MPAKCET_FMT,
6999                                     *lo_state, *lp_state);
7000                         stmmac_fpe_send_mpacket(priv, priv->ioaddr,
7001                                                 MPACKET_VERIFY);
7002                 }
7003                 /* Sleep then retry */
7004                 msleep(500);
7005         }
7006
7007         clear_bit(__FPE_TASK_SCHED, &priv->fpe_task_state);
7008 }
7009
7010 void stmmac_fpe_handshake(struct stmmac_priv *priv, bool enable)
7011 {
7012         if (priv->plat->fpe_cfg->hs_enable != enable) {
7013                 if (enable) {
7014                         stmmac_fpe_send_mpacket(priv, priv->ioaddr,
7015                                                 MPACKET_VERIFY);
7016                 } else {
7017                         priv->plat->fpe_cfg->lo_fpe_state = FPE_STATE_OFF;
7018                         priv->plat->fpe_cfg->lp_fpe_state = FPE_STATE_OFF;
7019                 }
7020
7021                 priv->plat->fpe_cfg->hs_enable = enable;
7022         }
7023 }
7024
7025 /**
7026  * stmmac_dvr_probe
7027  * @device: device pointer
7028  * @plat_dat: platform data pointer
7029  * @res: stmmac resource pointer
7030  * Description: this is the main probe function used to
7031  * call the alloc_etherdev, allocate the priv structure.
7032  * Return:
7033  * returns 0 on success, otherwise errno.
7034  */
7035 int stmmac_dvr_probe(struct device *device,
7036                      struct plat_stmmacenet_data *plat_dat,
7037                      struct stmmac_resources *res)
7038 {
7039         struct net_device *ndev = NULL;
7040         struct stmmac_priv *priv;
7041         u32 rxq;
7042         int i, ret = 0;
7043
7044         ndev = devm_alloc_etherdev_mqs(device, sizeof(struct stmmac_priv),
7045                                        MTL_MAX_TX_QUEUES, MTL_MAX_RX_QUEUES);
7046         if (!ndev)
7047                 return -ENOMEM;
7048
7049         SET_NETDEV_DEV(ndev, device);
7050
7051         priv = netdev_priv(ndev);
7052         priv->device = device;
7053         priv->dev = ndev;
7054
7055         stmmac_set_ethtool_ops(ndev);
7056         priv->pause = pause;
7057         priv->plat = plat_dat;
7058         priv->ioaddr = res->addr;
7059         priv->dev->base_addr = (unsigned long)res->addr;
7060         priv->plat->dma_cfg->multi_msi_en = priv->plat->multi_msi_en;
7061
7062         priv->dev->irq = res->irq;
7063         priv->wol_irq = res->wol_irq;
7064         priv->lpi_irq = res->lpi_irq;
7065         priv->sfty_ce_irq = res->sfty_ce_irq;
7066         priv->sfty_ue_irq = res->sfty_ue_irq;
7067         for (i = 0; i < MTL_MAX_RX_QUEUES; i++)
7068                 priv->rx_irq[i] = res->rx_irq[i];
7069         for (i = 0; i < MTL_MAX_TX_QUEUES; i++)
7070                 priv->tx_irq[i] = res->tx_irq[i];
7071
7072         if (!is_zero_ether_addr(res->mac))
7073                 eth_hw_addr_set(priv->dev, res->mac);
7074
7075         dev_set_drvdata(device, priv->dev);
7076
7077         /* Verify driver arguments */
7078         stmmac_verify_args();
7079
7080         priv->af_xdp_zc_qps = bitmap_zalloc(MTL_MAX_TX_QUEUES, GFP_KERNEL);
7081         if (!priv->af_xdp_zc_qps)
7082                 return -ENOMEM;
7083
7084         /* Allocate workqueue */
7085         priv->wq = create_singlethread_workqueue("stmmac_wq");
7086         if (!priv->wq) {
7087                 dev_err(priv->device, "failed to create workqueue\n");
7088                 return -ENOMEM;
7089         }
7090
7091         INIT_WORK(&priv->service_task, stmmac_service_task);
7092
7093         /* Initialize Link Partner FPE workqueue */
7094         INIT_WORK(&priv->fpe_task, stmmac_fpe_lp_task);
7095
7096         /* Override with kernel parameters if supplied XXX CRS XXX
7097          * this needs to have multiple instances
7098          */
7099         if ((phyaddr >= 0) && (phyaddr <= 31))
7100                 priv->plat->phy_addr = phyaddr;
7101
7102         if (priv->plat->stmmac_rst) {
7103                 ret = reset_control_assert(priv->plat->stmmac_rst);
7104                 reset_control_deassert(priv->plat->stmmac_rst);
7105                 /* Some reset controllers have only reset callback instead of
7106                  * assert + deassert callbacks pair.
7107                  */
7108                 if (ret == -ENOTSUPP)
7109                         reset_control_reset(priv->plat->stmmac_rst);
7110         }
7111
7112         ret = reset_control_deassert(priv->plat->stmmac_ahb_rst);
7113         if (ret == -ENOTSUPP)
7114                 dev_err(priv->device, "unable to bring out of ahb reset: %pe\n",
7115                         ERR_PTR(ret));
7116
7117         /* Init MAC and get the capabilities */
7118         ret = stmmac_hw_init(priv);
7119         if (ret)
7120                 goto error_hw_init;
7121
7122         /* Only DWMAC core version 5.20 onwards supports HW descriptor prefetch.
7123          */
7124         if (priv->synopsys_id < DWMAC_CORE_5_20)
7125                 priv->plat->dma_cfg->dche = false;
7126
7127         stmmac_check_ether_addr(priv);
7128
7129         ndev->netdev_ops = &stmmac_netdev_ops;
7130
7131         ndev->hw_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
7132                             NETIF_F_RXCSUM;
7133
7134         ret = stmmac_tc_init(priv, priv);
7135         if (!ret) {
7136                 ndev->hw_features |= NETIF_F_HW_TC;
7137         }
7138
7139         if ((priv->plat->tso_en) && (priv->dma_cap.tsoen)) {
7140                 ndev->hw_features |= NETIF_F_TSO | NETIF_F_TSO6;
7141                 if (priv->plat->has_gmac4)
7142                         ndev->hw_features |= NETIF_F_GSO_UDP_L4;
7143                 priv->tso = true;
7144                 dev_info(priv->device, "TSO feature enabled\n");
7145         }
7146
7147         if (priv->dma_cap.sphen && !priv->plat->sph_disable) {
7148                 ndev->hw_features |= NETIF_F_GRO;
7149                 priv->sph_cap = true;
7150                 priv->sph = priv->sph_cap;
7151                 dev_info(priv->device, "SPH feature enabled\n");
7152         }
7153
7154         /* The current IP register MAC_HW_Feature1[ADDR64] only define
7155          * 32/40/64 bit width, but some SOC support others like i.MX8MP
7156          * support 34 bits but it map to 40 bits width in MAC_HW_Feature1[ADDR64].
7157          * So overwrite dma_cap.addr64 according to HW real design.
7158          */
7159         if (priv->plat->addr64)
7160                 priv->dma_cap.addr64 = priv->plat->addr64;
7161
7162         if (priv->dma_cap.addr64) {
7163                 ret = dma_set_mask_and_coherent(device,
7164                                 DMA_BIT_MASK(priv->dma_cap.addr64));
7165                 if (!ret) {
7166                         dev_info(priv->device, "Using %d bits DMA width\n",
7167                                  priv->dma_cap.addr64);
7168
7169                         /*
7170                          * If more than 32 bits can be addressed, make sure to
7171                          * enable enhanced addressing mode.
7172                          */
7173                         if (IS_ENABLED(CONFIG_ARCH_DMA_ADDR_T_64BIT))
7174                                 priv->plat->dma_cfg->eame = true;
7175                 } else {
7176                         ret = dma_set_mask_and_coherent(device, DMA_BIT_MASK(32));
7177                         if (ret) {
7178                                 dev_err(priv->device, "Failed to set DMA Mask\n");
7179                                 goto error_hw_init;
7180                         }
7181
7182                         priv->dma_cap.addr64 = 32;
7183                 }
7184         }
7185
7186         ndev->features |= ndev->hw_features | NETIF_F_HIGHDMA;
7187         ndev->watchdog_timeo = msecs_to_jiffies(watchdog);
7188 #ifdef STMMAC_VLAN_TAG_USED
7189         /* Both mac100 and gmac support receive VLAN tag detection */
7190         ndev->features |= NETIF_F_HW_VLAN_CTAG_RX | NETIF_F_HW_VLAN_STAG_RX;
7191         if (priv->dma_cap.vlhash) {
7192                 ndev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
7193                 ndev->features |= NETIF_F_HW_VLAN_STAG_FILTER;
7194         }
7195         if (priv->dma_cap.vlins) {
7196                 ndev->features |= NETIF_F_HW_VLAN_CTAG_TX;
7197                 if (priv->dma_cap.dvlan)
7198                         ndev->features |= NETIF_F_HW_VLAN_STAG_TX;
7199         }
7200 #endif
7201         priv->msg_enable = netif_msg_init(debug, default_msg_level);
7202
7203         /* Initialize RSS */
7204         rxq = priv->plat->rx_queues_to_use;
7205         netdev_rss_key_fill(priv->rss.key, sizeof(priv->rss.key));
7206         for (i = 0; i < ARRAY_SIZE(priv->rss.table); i++)
7207                 priv->rss.table[i] = ethtool_rxfh_indir_default(i, rxq);
7208
7209         if (priv->dma_cap.rssen && priv->plat->rss_en)
7210                 ndev->features |= NETIF_F_RXHASH;
7211
7212         /* MTU range: 46 - hw-specific max */
7213         ndev->min_mtu = ETH_ZLEN - ETH_HLEN;
7214         if (priv->plat->has_xgmac)
7215                 ndev->max_mtu = XGMAC_JUMBO_LEN;
7216         else if ((priv->plat->enh_desc) || (priv->synopsys_id >= DWMAC_CORE_4_00))
7217                 ndev->max_mtu = JUMBO_LEN;
7218         else
7219                 ndev->max_mtu = SKB_MAX_HEAD(NET_SKB_PAD + NET_IP_ALIGN);
7220         /* Will not overwrite ndev->max_mtu if plat->maxmtu > ndev->max_mtu
7221          * as well as plat->maxmtu < ndev->min_mtu which is a invalid range.
7222          */
7223         if ((priv->plat->maxmtu < ndev->max_mtu) &&
7224             (priv->plat->maxmtu >= ndev->min_mtu))
7225                 ndev->max_mtu = priv->plat->maxmtu;
7226         else if (priv->plat->maxmtu < ndev->min_mtu)
7227                 dev_warn(priv->device,
7228                          "%s: warning: maxmtu having invalid value (%d)\n",
7229                          __func__, priv->plat->maxmtu);
7230
7231         if (flow_ctrl)
7232                 priv->flow_ctrl = FLOW_AUTO;    /* RX/TX pause on */
7233
7234         /* Setup channels NAPI */
7235         stmmac_napi_add(ndev);
7236
7237         mutex_init(&priv->lock);
7238
7239         /* If a specific clk_csr value is passed from the platform
7240          * this means that the CSR Clock Range selection cannot be
7241          * changed at run-time and it is fixed. Viceversa the driver'll try to
7242          * set the MDC clock dynamically according to the csr actual
7243          * clock input.
7244          */
7245         if (priv->plat->clk_csr >= 0)
7246                 priv->clk_csr = priv->plat->clk_csr;
7247         else
7248                 stmmac_clk_csr_set(priv);
7249
7250         stmmac_check_pcs_mode(priv);
7251
7252         pm_runtime_get_noresume(device);
7253         pm_runtime_set_active(device);
7254         if (!pm_runtime_enabled(device))
7255                 pm_runtime_enable(device);
7256
7257         if (priv->hw->pcs != STMMAC_PCS_TBI &&
7258             priv->hw->pcs != STMMAC_PCS_RTBI) {
7259                 /* MDIO bus Registration */
7260                 ret = stmmac_mdio_register(ndev);
7261                 if (ret < 0) {
7262                         dev_err_probe(priv->device, ret,
7263                                       "%s: MDIO bus (id: %d) registration failed\n",
7264                                       __func__, priv->plat->bus_id);
7265                         goto error_mdio_register;
7266                 }
7267         }
7268
7269         if (priv->plat->speed_mode_2500)
7270                 priv->plat->speed_mode_2500(ndev, priv->plat->bsp_priv);
7271
7272         if (priv->plat->mdio_bus_data && priv->plat->mdio_bus_data->has_xpcs) {
7273                 ret = stmmac_xpcs_setup(priv->mii);
7274                 if (ret)
7275                         goto error_xpcs_setup;
7276         }
7277
7278         ret = stmmac_phy_setup(priv);
7279         if (ret) {
7280                 netdev_err(ndev, "failed to setup phy (%d)\n", ret);
7281                 goto error_phy_setup;
7282         }
7283
7284         ret = register_netdev(ndev);
7285         if (ret) {
7286                 dev_err(priv->device, "%s: ERROR %i registering the device\n",
7287                         __func__, ret);
7288                 goto error_netdev_register;
7289         }
7290
7291 #ifdef CONFIG_DEBUG_FS
7292         stmmac_init_fs(ndev);
7293 #endif
7294
7295         if (priv->plat->dump_debug_regs)
7296                 priv->plat->dump_debug_regs(priv->plat->bsp_priv);
7297
7298         /* Let pm_runtime_put() disable the clocks.
7299          * If CONFIG_PM is not enabled, the clocks will stay powered.
7300          */
7301         pm_runtime_put(device);
7302
7303         return ret;
7304
7305 error_netdev_register:
7306         phylink_destroy(priv->phylink);
7307 error_xpcs_setup:
7308 error_phy_setup:
7309         if (priv->hw->pcs != STMMAC_PCS_TBI &&
7310             priv->hw->pcs != STMMAC_PCS_RTBI)
7311                 stmmac_mdio_unregister(ndev);
7312 error_mdio_register:
7313         stmmac_napi_del(ndev);
7314 error_hw_init:
7315         destroy_workqueue(priv->wq);
7316         bitmap_free(priv->af_xdp_zc_qps);
7317
7318         return ret;
7319 }
7320 EXPORT_SYMBOL_GPL(stmmac_dvr_probe);
7321
7322 /**
7323  * stmmac_dvr_remove
7324  * @dev: device pointer
7325  * Description: this function resets the TX/RX processes, disables the MAC RX/TX
7326  * changes the link status, releases the DMA descriptor rings.
7327  */
7328 int stmmac_dvr_remove(struct device *dev)
7329 {
7330         struct net_device *ndev = dev_get_drvdata(dev);
7331         struct stmmac_priv *priv = netdev_priv(ndev);
7332
7333         netdev_info(priv->dev, "%s: removing driver", __func__);
7334
7335         pm_runtime_get_sync(dev);
7336
7337         stmmac_stop_all_dma(priv);
7338         stmmac_mac_set(priv, priv->ioaddr, false);
7339         netif_carrier_off(ndev);
7340         unregister_netdev(ndev);
7341
7342         /* Serdes power down needs to happen after VLAN filter
7343          * is deleted that is triggered by unregister_netdev().
7344          */
7345         if (priv->plat->serdes_powerdown)
7346                 priv->plat->serdes_powerdown(ndev, priv->plat->bsp_priv);
7347
7348 #ifdef CONFIG_DEBUG_FS
7349         stmmac_exit_fs(ndev);
7350 #endif
7351         phylink_destroy(priv->phylink);
7352         if (priv->plat->stmmac_rst)
7353                 reset_control_assert(priv->plat->stmmac_rst);
7354         reset_control_assert(priv->plat->stmmac_ahb_rst);
7355         if (priv->hw->pcs != STMMAC_PCS_TBI &&
7356             priv->hw->pcs != STMMAC_PCS_RTBI)
7357                 stmmac_mdio_unregister(ndev);
7358         destroy_workqueue(priv->wq);
7359         mutex_destroy(&priv->lock);
7360         bitmap_free(priv->af_xdp_zc_qps);
7361
7362         pm_runtime_disable(dev);
7363         pm_runtime_put_noidle(dev);
7364
7365         return 0;
7366 }
7367 EXPORT_SYMBOL_GPL(stmmac_dvr_remove);
7368
7369 /**
7370  * stmmac_suspend - suspend callback
7371  * @dev: device pointer
7372  * Description: this is the function to suspend the device and it is called
7373  * by the platform driver to stop the network queue, release the resources,
7374  * program the PMT register (for WoL), clean and release driver resources.
7375  */
7376 int stmmac_suspend(struct device *dev)
7377 {
7378         struct net_device *ndev = dev_get_drvdata(dev);
7379         struct stmmac_priv *priv = netdev_priv(ndev);
7380         u32 chan;
7381
7382         if (!ndev || !netif_running(ndev))
7383                 return 0;
7384
7385         mutex_lock(&priv->lock);
7386
7387         netif_device_detach(ndev);
7388
7389         stmmac_disable_all_queues(priv);
7390
7391         for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++)
7392                 hrtimer_cancel(&priv->dma_conf.tx_queue[chan].txtimer);
7393
7394         if (priv->eee_enabled) {
7395                 priv->tx_path_in_lpi_mode = false;
7396                 del_timer_sync(&priv->eee_ctrl_timer);
7397         }
7398
7399         /* Stop TX/RX DMA */
7400         stmmac_stop_all_dma(priv);
7401
7402         if (priv->plat->serdes_powerdown)
7403                 priv->plat->serdes_powerdown(ndev, priv->plat->bsp_priv);
7404
7405         /* Enable Power down mode by programming the PMT regs */
7406         if (device_may_wakeup(priv->device) && priv->plat->pmt) {
7407                 stmmac_pmt(priv, priv->hw, priv->wolopts);
7408                 priv->irq_wake = 1;
7409         } else {
7410                 stmmac_mac_set(priv, priv->ioaddr, false);
7411                 pinctrl_pm_select_sleep_state(priv->device);
7412         }
7413
7414         mutex_unlock(&priv->lock);
7415
7416         rtnl_lock();
7417         if (device_may_wakeup(priv->device) && priv->plat->pmt) {
7418                 phylink_suspend(priv->phylink, true);
7419         } else {
7420                 if (device_may_wakeup(priv->device))
7421                         phylink_speed_down(priv->phylink, false);
7422                 phylink_suspend(priv->phylink, false);
7423         }
7424         rtnl_unlock();
7425
7426         if (priv->dma_cap.fpesel) {
7427                 /* Disable FPE */
7428                 stmmac_fpe_configure(priv, priv->ioaddr,
7429                                      priv->plat->tx_queues_to_use,
7430                                      priv->plat->rx_queues_to_use, false);
7431
7432                 stmmac_fpe_handshake(priv, false);
7433                 stmmac_fpe_stop_wq(priv);
7434         }
7435
7436         priv->speed = SPEED_UNKNOWN;
7437         return 0;
7438 }
7439 EXPORT_SYMBOL_GPL(stmmac_suspend);
7440
7441 static void stmmac_reset_rx_queue(struct stmmac_priv *priv, u32 queue)
7442 {
7443         struct stmmac_rx_queue *rx_q = &priv->dma_conf.rx_queue[queue];
7444
7445         rx_q->cur_rx = 0;
7446         rx_q->dirty_rx = 0;
7447 }
7448
7449 static void stmmac_reset_tx_queue(struct stmmac_priv *priv, u32 queue)
7450 {
7451         struct stmmac_tx_queue *tx_q = &priv->dma_conf.tx_queue[queue];
7452
7453         tx_q->cur_tx = 0;
7454         tx_q->dirty_tx = 0;
7455         tx_q->mss = 0;
7456
7457         netdev_tx_reset_queue(netdev_get_tx_queue(priv->dev, queue));
7458 }
7459
7460 /**
7461  * stmmac_reset_queues_param - reset queue parameters
7462  * @priv: device pointer
7463  */
7464 static void stmmac_reset_queues_param(struct stmmac_priv *priv)
7465 {
7466         u32 rx_cnt = priv->plat->rx_queues_to_use;
7467         u32 tx_cnt = priv->plat->tx_queues_to_use;
7468         u32 queue;
7469
7470         for (queue = 0; queue < rx_cnt; queue++)
7471                 stmmac_reset_rx_queue(priv, queue);
7472
7473         for (queue = 0; queue < tx_cnt; queue++)
7474                 stmmac_reset_tx_queue(priv, queue);
7475 }
7476
7477 /**
7478  * stmmac_resume - resume callback
7479  * @dev: device pointer
7480  * Description: when resume this function is invoked to setup the DMA and CORE
7481  * in a usable state.
7482  */
7483 int stmmac_resume(struct device *dev)
7484 {
7485         struct net_device *ndev = dev_get_drvdata(dev);
7486         struct stmmac_priv *priv = netdev_priv(ndev);
7487         int ret;
7488
7489         if (!netif_running(ndev))
7490                 return 0;
7491
7492         /* Power Down bit, into the PM register, is cleared
7493          * automatically as soon as a magic packet or a Wake-up frame
7494          * is received. Anyway, it's better to manually clear
7495          * this bit because it can generate problems while resuming
7496          * from another devices (e.g. serial console).
7497          */
7498         if (device_may_wakeup(priv->device) && priv->plat->pmt) {
7499                 mutex_lock(&priv->lock);
7500                 stmmac_pmt(priv, priv->hw, 0);
7501                 mutex_unlock(&priv->lock);
7502                 priv->irq_wake = 0;
7503         } else {
7504                 pinctrl_pm_select_default_state(priv->device);
7505                 /* reset the phy so that it's ready */
7506                 if (priv->mii)
7507                         stmmac_mdio_reset(priv->mii);
7508         }
7509
7510         if (priv->plat->serdes_powerup) {
7511                 ret = priv->plat->serdes_powerup(ndev,
7512                                                  priv->plat->bsp_priv);
7513
7514                 if (ret < 0)
7515                         return ret;
7516         }
7517
7518         rtnl_lock();
7519         if (device_may_wakeup(priv->device) && priv->plat->pmt) {
7520                 phylink_resume(priv->phylink);
7521         } else {
7522                 phylink_resume(priv->phylink);
7523                 if (device_may_wakeup(priv->device))
7524                         phylink_speed_up(priv->phylink);
7525         }
7526         rtnl_unlock();
7527
7528         rtnl_lock();
7529         mutex_lock(&priv->lock);
7530
7531         stmmac_reset_queues_param(priv);
7532
7533         stmmac_free_tx_skbufs(priv);
7534         stmmac_clear_descriptors(priv, &priv->dma_conf);
7535
7536         stmmac_hw_setup(ndev, false);
7537         stmmac_init_coalesce(priv);
7538         stmmac_set_rx_mode(ndev);
7539
7540         stmmac_restore_hw_vlan_rx_fltr(priv, ndev, priv->hw);
7541
7542         stmmac_enable_all_queues(priv);
7543         stmmac_enable_all_dma_irq(priv);
7544
7545         mutex_unlock(&priv->lock);
7546         rtnl_unlock();
7547
7548         netif_device_attach(ndev);
7549
7550         return 0;
7551 }
7552 EXPORT_SYMBOL_GPL(stmmac_resume);
7553
7554 #ifndef MODULE
7555 static int __init stmmac_cmdline_opt(char *str)
7556 {
7557         char *opt;
7558
7559         if (!str || !*str)
7560                 return 1;
7561         while ((opt = strsep(&str, ",")) != NULL) {
7562                 if (!strncmp(opt, "debug:", 6)) {
7563                         if (kstrtoint(opt + 6, 0, &debug))
7564                                 goto err;
7565                 } else if (!strncmp(opt, "phyaddr:", 8)) {
7566                         if (kstrtoint(opt + 8, 0, &phyaddr))
7567                                 goto err;
7568                 } else if (!strncmp(opt, "buf_sz:", 7)) {
7569                         if (kstrtoint(opt + 7, 0, &buf_sz))
7570                                 goto err;
7571                 } else if (!strncmp(opt, "tc:", 3)) {
7572                         if (kstrtoint(opt + 3, 0, &tc))
7573                                 goto err;
7574                 } else if (!strncmp(opt, "watchdog:", 9)) {
7575                         if (kstrtoint(opt + 9, 0, &watchdog))
7576                                 goto err;
7577                 } else if (!strncmp(opt, "flow_ctrl:", 10)) {
7578                         if (kstrtoint(opt + 10, 0, &flow_ctrl))
7579                                 goto err;
7580                 } else if (!strncmp(opt, "pause:", 6)) {
7581                         if (kstrtoint(opt + 6, 0, &pause))
7582                                 goto err;
7583                 } else if (!strncmp(opt, "eee_timer:", 10)) {
7584                         if (kstrtoint(opt + 10, 0, &eee_timer))
7585                                 goto err;
7586                 } else if (!strncmp(opt, "chain_mode:", 11)) {
7587                         if (kstrtoint(opt + 11, 0, &chain_mode))
7588                                 goto err;
7589                 }
7590         }
7591         return 1;
7592
7593 err:
7594         pr_err("%s: ERROR broken module parameter conversion", __func__);
7595         return 1;
7596 }
7597
7598 __setup("stmmaceth=", stmmac_cmdline_opt);
7599 #endif /* MODULE */
7600
7601 static int __init stmmac_init(void)
7602 {
7603 #ifdef CONFIG_DEBUG_FS
7604         /* Create debugfs main directory if it doesn't exist yet */
7605         if (!stmmac_fs_dir)
7606                 stmmac_fs_dir = debugfs_create_dir(STMMAC_RESOURCE_NAME, NULL);
7607         register_netdevice_notifier(&stmmac_notifier);
7608 #endif
7609
7610         return 0;
7611 }
7612
7613 static void __exit stmmac_exit(void)
7614 {
7615 #ifdef CONFIG_DEBUG_FS
7616         unregister_netdevice_notifier(&stmmac_notifier);
7617         debugfs_remove_recursive(stmmac_fs_dir);
7618 #endif
7619 }
7620
7621 module_init(stmmac_init)
7622 module_exit(stmmac_exit)
7623
7624 MODULE_DESCRIPTION("STMMAC 10/100/1000 Ethernet device driver");
7625 MODULE_AUTHOR("Giuseppe Cavallaro <peppe.cavallaro@st.com>");
7626 MODULE_LICENSE("GPL");