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