linux/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
<<
>>
Prefs
   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
  58static int watchdog = TX_TIMEO;
  59module_param(watchdog, int, 0644);
  60MODULE_PARM_DESC(watchdog, "Transmit timeout in milliseconds (default 5s)");
  61
  62static int debug = -1;
  63module_param(debug, int, 0644);
  64MODULE_PARM_DESC(debug, "Message Level (-1: default, 0: no output, 16: all)");
  65
  66static int phyaddr = -1;
  67module_param(phyaddr, int, 0444);
  68MODULE_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
  83static int flow_ctrl = FLOW_AUTO;
  84module_param(flow_ctrl, int, 0644);
  85MODULE_PARM_DESC(flow_ctrl, "Flow control ability [on/off]");
  86
  87static int pause = PAUSE_TIME;
  88module_param(pause, int, 0644);
  89MODULE_PARM_DESC(pause, "Flow Control Pause Time");
  90
  91#define TC_DEFAULT 64
  92static int tc = TC_DEFAULT;
  93module_param(tc, int, 0644);
  94MODULE_PARM_DESC(tc, "DMA threshold control value");
  95
  96#define DEFAULT_BUFSIZE 1536
  97static int buf_sz = DEFAULT_BUFSIZE;
  98module_param(buf_sz, int, 0644);
  99MODULE_PARM_DESC(buf_sz, "DMA buffer size");
 100
 101#define STMMAC_RX_COPYBREAK     256
 102
 103static 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
 108static int eee_timer = STMMAC_DEFAULT_LPI_TIMER;
 109module_param(eee_timer, int, 0644);
 110MODULE_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 */
 116static unsigned int chain_mode;
 117module_param(chain_mode, int, 0444);
 118MODULE_PARM_DESC(chain_mode, "To use chain instead of ring mode");
 119
 120static irqreturn_t stmmac_interrupt(int irq, void *dev_id);
 121/* For MSI interrupts handling */
 122static irqreturn_t stmmac_mac_interrupt(int irq, void *dev_id);
 123static irqreturn_t stmmac_safety_interrupt(int irq, void *dev_id);
 124static irqreturn_t stmmac_msi_intr_tx(int irq, void *data);
 125static irqreturn_t stmmac_msi_intr_rx(int irq, void *data);
 126static void stmmac_tx_timer_arm(struct stmmac_priv *priv, u32 queue);
 127static void stmmac_flush_tx_descriptors(struct stmmac_priv *priv, int queue);
 128
 129#ifdef CONFIG_DEBUG_FS
 130static const struct net_device_ops stmmac_netdev_ops;
 131static void stmmac_init_fs(struct net_device *dev);
 132static 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
 137int 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}
 167EXPORT_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 */
 174static 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
 190static 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 */
 217static 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 */
 239static 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
 262static 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
 269static 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 */
 288static 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
 343static 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
 349static 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 */
 367static 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
 380static 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 */
 396static 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 */
 421void 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 */
 440static 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 */
 456bool 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 */
 514static 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 */
 563static 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 */
 607static 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 */
 838static 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 */
 857static 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
 887static 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 */
 899static 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
 907static 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_gmac4) {
 935                if (!max_speed || max_speed >= 2500) {
 936                        phylink_set(mac_supported, 2500baseT_Full);
 937                        phylink_set(mac_supported, 2500baseX_Full);
 938                }
 939        } else if (priv->plat->has_xgmac) {
 940                if (!max_speed || (max_speed >= 2500)) {
 941                        phylink_set(mac_supported, 2500baseT_Full);
 942                        phylink_set(mac_supported, 2500baseX_Full);
 943                }
 944                if (!max_speed || (max_speed >= 5000)) {
 945                        phylink_set(mac_supported, 5000baseT_Full);
 946                }
 947                if (!max_speed || (max_speed >= 10000)) {
 948                        phylink_set(mac_supported, 10000baseSR_Full);
 949                        phylink_set(mac_supported, 10000baseLR_Full);
 950                        phylink_set(mac_supported, 10000baseER_Full);
 951                        phylink_set(mac_supported, 10000baseLRM_Full);
 952                        phylink_set(mac_supported, 10000baseT_Full);
 953                        phylink_set(mac_supported, 10000baseKX4_Full);
 954                        phylink_set(mac_supported, 10000baseKR_Full);
 955                }
 956                if (!max_speed || (max_speed >= 25000)) {
 957                        phylink_set(mac_supported, 25000baseCR_Full);
 958                        phylink_set(mac_supported, 25000baseKR_Full);
 959                        phylink_set(mac_supported, 25000baseSR_Full);
 960                }
 961                if (!max_speed || (max_speed >= 40000)) {
 962                        phylink_set(mac_supported, 40000baseKR4_Full);
 963                        phylink_set(mac_supported, 40000baseCR4_Full);
 964                        phylink_set(mac_supported, 40000baseSR4_Full);
 965                        phylink_set(mac_supported, 40000baseLR4_Full);
 966                }
 967                if (!max_speed || (max_speed >= 50000)) {
 968                        phylink_set(mac_supported, 50000baseCR2_Full);
 969                        phylink_set(mac_supported, 50000baseKR2_Full);
 970                        phylink_set(mac_supported, 50000baseSR2_Full);
 971                        phylink_set(mac_supported, 50000baseKR_Full);
 972                        phylink_set(mac_supported, 50000baseSR_Full);
 973                        phylink_set(mac_supported, 50000baseCR_Full);
 974                        phylink_set(mac_supported, 50000baseLR_ER_FR_Full);
 975                        phylink_set(mac_supported, 50000baseDR_Full);
 976                }
 977                if (!max_speed || (max_speed >= 100000)) {
 978                        phylink_set(mac_supported, 100000baseKR4_Full);
 979                        phylink_set(mac_supported, 100000baseSR4_Full);
 980                        phylink_set(mac_supported, 100000baseCR4_Full);
 981                        phylink_set(mac_supported, 100000baseLR4_ER4_Full);
 982                        phylink_set(mac_supported, 100000baseKR2_Full);
 983                        phylink_set(mac_supported, 100000baseSR2_Full);
 984                        phylink_set(mac_supported, 100000baseCR2_Full);
 985                        phylink_set(mac_supported, 100000baseLR2_ER2_FR2_Full);
 986                        phylink_set(mac_supported, 100000baseDR2_Full);
 987                }
 988        }
 989
 990        /* Half-Duplex can only work with single queue */
 991        if (tx_cnt > 1) {
 992                phylink_set(mask, 10baseT_Half);
 993                phylink_set(mask, 100baseT_Half);
 994                phylink_set(mask, 1000baseT_Half);
 995        }
 996
 997        linkmode_and(supported, supported, mac_supported);
 998        linkmode_andnot(supported, supported, mask);
 999
1000        linkmode_and(state->advertising, state->advertising, mac_supported);
1001        linkmode_andnot(state->advertising, state->advertising, mask);
1002
1003        /* If PCS is supported, check which modes it supports. */
1004        if (priv->hw->xpcs)
1005                xpcs_validate(priv->hw->xpcs, supported, state);
1006}
1007
1008static void stmmac_mac_config(struct phylink_config *config, unsigned int mode,
1009                              const struct phylink_link_state *state)
1010{
1011        /* Nothing to do, xpcs_config() handles everything */
1012}
1013
1014static void stmmac_fpe_link_state_handle(struct stmmac_priv *priv, bool is_up)
1015{
1016        struct stmmac_fpe_cfg *fpe_cfg = priv->plat->fpe_cfg;
1017        enum stmmac_fpe_state *lo_state = &fpe_cfg->lo_fpe_state;
1018        enum stmmac_fpe_state *lp_state = &fpe_cfg->lp_fpe_state;
1019        bool *hs_enable = &fpe_cfg->hs_enable;
1020
1021        if (is_up && *hs_enable) {
1022                stmmac_fpe_send_mpacket(priv, priv->ioaddr, MPACKET_VERIFY);
1023        } else {
1024                *lo_state = FPE_STATE_OFF;
1025                *lp_state = FPE_STATE_OFF;
1026        }
1027}
1028
1029static void stmmac_mac_link_down(struct phylink_config *config,
1030                                 unsigned int mode, phy_interface_t interface)
1031{
1032        struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev));
1033
1034        stmmac_mac_set(priv, priv->ioaddr, false);
1035        priv->eee_active = false;
1036        priv->tx_lpi_enabled = false;
1037        stmmac_eee_init(priv);
1038        stmmac_set_eee_pls(priv, priv->hw, false);
1039
1040        if (priv->dma_cap.fpesel)
1041                stmmac_fpe_link_state_handle(priv, false);
1042}
1043
1044static void stmmac_mac_link_up(struct phylink_config *config,
1045                               struct phy_device *phy,
1046                               unsigned int mode, phy_interface_t interface,
1047                               int speed, int duplex,
1048                               bool tx_pause, bool rx_pause)
1049{
1050        struct stmmac_priv *priv = netdev_priv(to_net_dev(config->dev));
1051        u32 ctrl;
1052
1053        ctrl = readl(priv->ioaddr + MAC_CTRL_REG);
1054        ctrl &= ~priv->hw->link.speed_mask;
1055
1056        if (interface == PHY_INTERFACE_MODE_USXGMII) {
1057                switch (speed) {
1058                case SPEED_10000:
1059                        ctrl |= priv->hw->link.xgmii.speed10000;
1060                        break;
1061                case SPEED_5000:
1062                        ctrl |= priv->hw->link.xgmii.speed5000;
1063                        break;
1064                case SPEED_2500:
1065                        ctrl |= priv->hw->link.xgmii.speed2500;
1066                        break;
1067                default:
1068                        return;
1069                }
1070        } else if (interface == PHY_INTERFACE_MODE_XLGMII) {
1071                switch (speed) {
1072                case SPEED_100000:
1073                        ctrl |= priv->hw->link.xlgmii.speed100000;
1074                        break;
1075                case SPEED_50000:
1076                        ctrl |= priv->hw->link.xlgmii.speed50000;
1077                        break;
1078                case SPEED_40000:
1079                        ctrl |= priv->hw->link.xlgmii.speed40000;
1080                        break;
1081                case SPEED_25000:
1082                        ctrl |= priv->hw->link.xlgmii.speed25000;
1083                        break;
1084                case SPEED_10000:
1085                        ctrl |= priv->hw->link.xgmii.speed10000;
1086                        break;
1087                case SPEED_2500:
1088                        ctrl |= priv->hw->link.speed2500;
1089                        break;
1090                case SPEED_1000:
1091                        ctrl |= priv->hw->link.speed1000;
1092                        break;
1093                default:
1094                        return;
1095                }
1096        } else {
1097                switch (speed) {
1098                case SPEED_2500:
1099                        ctrl |= priv->hw->link.speed2500;
1100                        break;
1101                case SPEED_1000:
1102                        ctrl |= priv->hw->link.speed1000;
1103                        break;
1104                case SPEED_100:
1105                        ctrl |= priv->hw->link.speed100;
1106                        break;
1107                case SPEED_10:
1108                        ctrl |= priv->hw->link.speed10;
1109                        break;
1110                default:
1111                        return;
1112                }
1113        }
1114
1115        priv->speed = speed;
1116
1117        if (priv->plat->fix_mac_speed)
1118                priv->plat->fix_mac_speed(priv->plat->bsp_priv, speed);
1119
1120        if (!duplex)
1121                ctrl &= ~priv->hw->link.duplex;
1122        else
1123                ctrl |= priv->hw->link.duplex;
1124
1125        /* Flow Control operation */
1126        if (tx_pause && rx_pause)
1127                stmmac_mac_flow_ctrl(priv, duplex);
1128
1129        writel(ctrl, priv->ioaddr + MAC_CTRL_REG);
1130
1131        stmmac_mac_set(priv, priv->ioaddr, true);
1132        if (phy && priv->dma_cap.eee) {
1133                priv->eee_active = phy_init_eee(phy, 1) >= 0;
1134                priv->eee_enabled = stmmac_eee_init(priv);
1135                priv->tx_lpi_enabled = priv->eee_enabled;
1136                stmmac_set_eee_pls(priv, priv->hw, true);
1137        }
1138
1139        if (priv->dma_cap.fpesel)
1140                stmmac_fpe_link_state_handle(priv, true);
1141}
1142
1143static const struct phylink_mac_ops stmmac_phylink_mac_ops = {
1144        .validate = stmmac_validate,
1145        .mac_config = stmmac_mac_config,
1146        .mac_link_down = stmmac_mac_link_down,
1147        .mac_link_up = stmmac_mac_link_up,
1148};
1149
1150/**
1151 * stmmac_check_pcs_mode - verify if RGMII/SGMII is supported
1152 * @priv: driver private structure
1153 * Description: this is to verify if the HW supports the PCS.
1154 * Physical Coding Sublayer (PCS) interface that can be used when the MAC is
1155 * configured for the TBI, RTBI, or SGMII PHY interface.
1156 */
1157static void stmmac_check_pcs_mode(struct stmmac_priv *priv)
1158{
1159        int interface = priv->plat->interface;
1160
1161        if (priv->dma_cap.pcs) {
1162                if ((interface == PHY_INTERFACE_MODE_RGMII) ||
1163                    (interface == PHY_INTERFACE_MODE_RGMII_ID) ||
1164                    (interface == PHY_INTERFACE_MODE_RGMII_RXID) ||
1165                    (interface == PHY_INTERFACE_MODE_RGMII_TXID)) {
1166                        netdev_dbg(priv->dev, "PCS RGMII support enabled\n");
1167                        priv->hw->pcs = STMMAC_PCS_RGMII;
1168                } else if (interface == PHY_INTERFACE_MODE_SGMII) {
1169                        netdev_dbg(priv->dev, "PCS SGMII support enabled\n");
1170                        priv->hw->pcs = STMMAC_PCS_SGMII;
1171                }
1172        }
1173}
1174
1175/**
1176 * stmmac_init_phy - PHY initialization
1177 * @dev: net device structure
1178 * Description: it initializes the driver's PHY state, and attaches the PHY
1179 * to the mac driver.
1180 *  Return value:
1181 *  0 on success
1182 */
1183static int stmmac_init_phy(struct net_device *dev)
1184{
1185        struct stmmac_priv *priv = netdev_priv(dev);
1186        struct device_node *node;
1187        int ret;
1188
1189        node = priv->plat->phylink_node;
1190
1191        if (node)
1192                ret = phylink_of_phy_connect(priv->phylink, node, 0);
1193
1194        /* Some DT bindings do not set-up the PHY handle. Let's try to
1195         * manually parse it
1196         */
1197        if (!node || ret) {
1198                int addr = priv->plat->phy_addr;
1199                struct phy_device *phydev;
1200
1201                phydev = mdiobus_get_phy(priv->mii, addr);
1202                if (!phydev) {
1203                        netdev_err(priv->dev, "no phy at addr %d\n", addr);
1204                        return -ENODEV;
1205                }
1206
1207                ret = phylink_connect_phy(priv->phylink, phydev);
1208        }
1209
1210        if (!priv->plat->pmt) {
1211                struct ethtool_wolinfo wol = { .cmd = ETHTOOL_GWOL };
1212
1213                phylink_ethtool_get_wol(priv->phylink, &wol);
1214                device_set_wakeup_capable(priv->device, !!wol.supported);
1215        }
1216
1217        return ret;
1218}
1219
1220static int stmmac_phy_setup(struct stmmac_priv *priv)
1221{
1222        struct stmmac_mdio_bus_data *mdio_bus_data = priv->plat->mdio_bus_data;
1223        struct fwnode_handle *fwnode = of_fwnode_handle(priv->plat->phylink_node);
1224        int mode = priv->plat->phy_interface;
1225        struct phylink *phylink;
1226
1227        priv->phylink_config.dev = &priv->dev->dev;
1228        priv->phylink_config.type = PHYLINK_NETDEV;
1229        priv->phylink_config.pcs_poll = true;
1230        if (priv->plat->mdio_bus_data)
1231                priv->phylink_config.ovr_an_inband =
1232                        mdio_bus_data->xpcs_an_inband;
1233
1234        if (!fwnode)
1235                fwnode = dev_fwnode(priv->device);
1236
1237        phylink = phylink_create(&priv->phylink_config, fwnode,
1238                                 mode, &stmmac_phylink_mac_ops);
1239        if (IS_ERR(phylink))
1240                return PTR_ERR(phylink);
1241
1242        if (priv->hw->xpcs)
1243                phylink_set_pcs(phylink, &priv->hw->xpcs->pcs);
1244
1245        priv->phylink = phylink;
1246        return 0;
1247}
1248
1249static void stmmac_display_rx_rings(struct stmmac_priv *priv)
1250{
1251        u32 rx_cnt = priv->plat->rx_queues_to_use;
1252        unsigned int desc_size;
1253        void *head_rx;
1254        u32 queue;
1255
1256        /* Display RX rings */
1257        for (queue = 0; queue < rx_cnt; queue++) {
1258                struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1259
1260                pr_info("\tRX Queue %u rings\n", queue);
1261
1262                if (priv->extend_desc) {
1263                        head_rx = (void *)rx_q->dma_erx;
1264                        desc_size = sizeof(struct dma_extended_desc);
1265                } else {
1266                        head_rx = (void *)rx_q->dma_rx;
1267                        desc_size = sizeof(struct dma_desc);
1268                }
1269
1270                /* Display RX ring */
1271                stmmac_display_ring(priv, head_rx, priv->dma_rx_size, true,
1272                                    rx_q->dma_rx_phy, desc_size);
1273        }
1274}
1275
1276static void stmmac_display_tx_rings(struct stmmac_priv *priv)
1277{
1278        u32 tx_cnt = priv->plat->tx_queues_to_use;
1279        unsigned int desc_size;
1280        void *head_tx;
1281        u32 queue;
1282
1283        /* Display TX rings */
1284        for (queue = 0; queue < tx_cnt; queue++) {
1285                struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
1286
1287                pr_info("\tTX Queue %d rings\n", queue);
1288
1289                if (priv->extend_desc) {
1290                        head_tx = (void *)tx_q->dma_etx;
1291                        desc_size = sizeof(struct dma_extended_desc);
1292                } else if (tx_q->tbs & STMMAC_TBS_AVAIL) {
1293                        head_tx = (void *)tx_q->dma_entx;
1294                        desc_size = sizeof(struct dma_edesc);
1295                } else {
1296                        head_tx = (void *)tx_q->dma_tx;
1297                        desc_size = sizeof(struct dma_desc);
1298                }
1299
1300                stmmac_display_ring(priv, head_tx, priv->dma_tx_size, false,
1301                                    tx_q->dma_tx_phy, desc_size);
1302        }
1303}
1304
1305static void stmmac_display_rings(struct stmmac_priv *priv)
1306{
1307        /* Display RX ring */
1308        stmmac_display_rx_rings(priv);
1309
1310        /* Display TX ring */
1311        stmmac_display_tx_rings(priv);
1312}
1313
1314static int stmmac_set_bfsize(int mtu, int bufsize)
1315{
1316        int ret = bufsize;
1317
1318        if (mtu >= BUF_SIZE_8KiB)
1319                ret = BUF_SIZE_16KiB;
1320        else if (mtu >= BUF_SIZE_4KiB)
1321                ret = BUF_SIZE_8KiB;
1322        else if (mtu >= BUF_SIZE_2KiB)
1323                ret = BUF_SIZE_4KiB;
1324        else if (mtu > DEFAULT_BUFSIZE)
1325                ret = BUF_SIZE_2KiB;
1326        else
1327                ret = DEFAULT_BUFSIZE;
1328
1329        return ret;
1330}
1331
1332/**
1333 * stmmac_clear_rx_descriptors - clear RX descriptors
1334 * @priv: driver private structure
1335 * @queue: RX queue index
1336 * Description: this function is called to clear the RX descriptors
1337 * in case of both basic and extended descriptors are used.
1338 */
1339static void stmmac_clear_rx_descriptors(struct stmmac_priv *priv, u32 queue)
1340{
1341        struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1342        int i;
1343
1344        /* Clear the RX descriptors */
1345        for (i = 0; i < priv->dma_rx_size; i++)
1346                if (priv->extend_desc)
1347                        stmmac_init_rx_desc(priv, &rx_q->dma_erx[i].basic,
1348                                        priv->use_riwt, priv->mode,
1349                                        (i == priv->dma_rx_size - 1),
1350                                        priv->dma_buf_sz);
1351                else
1352                        stmmac_init_rx_desc(priv, &rx_q->dma_rx[i],
1353                                        priv->use_riwt, priv->mode,
1354                                        (i == priv->dma_rx_size - 1),
1355                                        priv->dma_buf_sz);
1356}
1357
1358/**
1359 * stmmac_clear_tx_descriptors - clear tx descriptors
1360 * @priv: driver private structure
1361 * @queue: TX queue index.
1362 * Description: this function is called to clear the TX descriptors
1363 * in case of both basic and extended descriptors are used.
1364 */
1365static void stmmac_clear_tx_descriptors(struct stmmac_priv *priv, u32 queue)
1366{
1367        struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
1368        int i;
1369
1370        /* Clear the TX descriptors */
1371        for (i = 0; i < priv->dma_tx_size; i++) {
1372                int last = (i == (priv->dma_tx_size - 1));
1373                struct dma_desc *p;
1374
1375                if (priv->extend_desc)
1376                        p = &tx_q->dma_etx[i].basic;
1377                else if (tx_q->tbs & STMMAC_TBS_AVAIL)
1378                        p = &tx_q->dma_entx[i].basic;
1379                else
1380                        p = &tx_q->dma_tx[i];
1381
1382                stmmac_init_tx_desc(priv, p, priv->mode, last);
1383        }
1384}
1385
1386/**
1387 * stmmac_clear_descriptors - clear descriptors
1388 * @priv: driver private structure
1389 * Description: this function is called to clear the TX and RX descriptors
1390 * in case of both basic and extended descriptors are used.
1391 */
1392static void stmmac_clear_descriptors(struct stmmac_priv *priv)
1393{
1394        u32 rx_queue_cnt = priv->plat->rx_queues_to_use;
1395        u32 tx_queue_cnt = priv->plat->tx_queues_to_use;
1396        u32 queue;
1397
1398        /* Clear the RX descriptors */
1399        for (queue = 0; queue < rx_queue_cnt; queue++)
1400                stmmac_clear_rx_descriptors(priv, queue);
1401
1402        /* Clear the TX descriptors */
1403        for (queue = 0; queue < tx_queue_cnt; queue++)
1404                stmmac_clear_tx_descriptors(priv, queue);
1405}
1406
1407/**
1408 * stmmac_init_rx_buffers - init the RX descriptor buffer.
1409 * @priv: driver private structure
1410 * @p: descriptor pointer
1411 * @i: descriptor index
1412 * @flags: gfp flag
1413 * @queue: RX queue index
1414 * Description: this function is called to allocate a receive buffer, perform
1415 * the DMA mapping and init the descriptor.
1416 */
1417static int stmmac_init_rx_buffers(struct stmmac_priv *priv, struct dma_desc *p,
1418                                  int i, gfp_t flags, u32 queue)
1419{
1420        struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1421        struct stmmac_rx_buffer *buf = &rx_q->buf_pool[i];
1422
1423        if (!buf->page) {
1424                buf->page = page_pool_dev_alloc_pages(rx_q->page_pool);
1425                if (!buf->page)
1426                        return -ENOMEM;
1427                buf->page_offset = stmmac_rx_offset(priv);
1428        }
1429
1430        if (priv->sph && !buf->sec_page) {
1431                buf->sec_page = page_pool_dev_alloc_pages(rx_q->page_pool);
1432                if (!buf->sec_page)
1433                        return -ENOMEM;
1434
1435                buf->sec_addr = page_pool_get_dma_addr(buf->sec_page);
1436                stmmac_set_desc_sec_addr(priv, p, buf->sec_addr, true);
1437        } else {
1438                buf->sec_page = NULL;
1439                stmmac_set_desc_sec_addr(priv, p, buf->sec_addr, false);
1440        }
1441
1442        buf->addr = page_pool_get_dma_addr(buf->page) + buf->page_offset;
1443
1444        stmmac_set_desc_addr(priv, p, buf->addr);
1445        if (priv->dma_buf_sz == BUF_SIZE_16KiB)
1446                stmmac_init_desc3(priv, p);
1447
1448        return 0;
1449}
1450
1451/**
1452 * stmmac_free_rx_buffer - free RX dma buffers
1453 * @priv: private structure
1454 * @queue: RX queue index
1455 * @i: buffer index.
1456 */
1457static void stmmac_free_rx_buffer(struct stmmac_priv *priv, u32 queue, int i)
1458{
1459        struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1460        struct stmmac_rx_buffer *buf = &rx_q->buf_pool[i];
1461
1462        if (buf->page)
1463                page_pool_put_full_page(rx_q->page_pool, buf->page, false);
1464        buf->page = NULL;
1465
1466        if (buf->sec_page)
1467                page_pool_put_full_page(rx_q->page_pool, buf->sec_page, false);
1468        buf->sec_page = NULL;
1469}
1470
1471/**
1472 * stmmac_free_tx_buffer - free RX dma buffers
1473 * @priv: private structure
1474 * @queue: RX queue index
1475 * @i: buffer index.
1476 */
1477static void stmmac_free_tx_buffer(struct stmmac_priv *priv, u32 queue, int i)
1478{
1479        struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
1480
1481        if (tx_q->tx_skbuff_dma[i].buf &&
1482            tx_q->tx_skbuff_dma[i].buf_type != STMMAC_TXBUF_T_XDP_TX) {
1483                if (tx_q->tx_skbuff_dma[i].map_as_page)
1484                        dma_unmap_page(priv->device,
1485                                       tx_q->tx_skbuff_dma[i].buf,
1486                                       tx_q->tx_skbuff_dma[i].len,
1487                                       DMA_TO_DEVICE);
1488                else
1489                        dma_unmap_single(priv->device,
1490                                         tx_q->tx_skbuff_dma[i].buf,
1491                                         tx_q->tx_skbuff_dma[i].len,
1492                                         DMA_TO_DEVICE);
1493        }
1494
1495        if (tx_q->xdpf[i] &&
1496            (tx_q->tx_skbuff_dma[i].buf_type == STMMAC_TXBUF_T_XDP_TX ||
1497             tx_q->tx_skbuff_dma[i].buf_type == STMMAC_TXBUF_T_XDP_NDO)) {
1498                xdp_return_frame(tx_q->xdpf[i]);
1499                tx_q->xdpf[i] = NULL;
1500        }
1501
1502        if (tx_q->tx_skbuff_dma[i].buf_type == STMMAC_TXBUF_T_XSK_TX)
1503                tx_q->xsk_frames_done++;
1504
1505        if (tx_q->tx_skbuff[i] &&
1506            tx_q->tx_skbuff_dma[i].buf_type == STMMAC_TXBUF_T_SKB) {
1507                dev_kfree_skb_any(tx_q->tx_skbuff[i]);
1508                tx_q->tx_skbuff[i] = NULL;
1509        }
1510
1511        tx_q->tx_skbuff_dma[i].buf = 0;
1512        tx_q->tx_skbuff_dma[i].map_as_page = false;
1513}
1514
1515/**
1516 * dma_free_rx_skbufs - free RX dma buffers
1517 * @priv: private structure
1518 * @queue: RX queue index
1519 */
1520static void dma_free_rx_skbufs(struct stmmac_priv *priv, u32 queue)
1521{
1522        int i;
1523
1524        for (i = 0; i < priv->dma_rx_size; i++)
1525                stmmac_free_rx_buffer(priv, queue, i);
1526}
1527
1528static int stmmac_alloc_rx_buffers(struct stmmac_priv *priv, u32 queue,
1529                                   gfp_t flags)
1530{
1531        struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1532        int i;
1533
1534        for (i = 0; i < priv->dma_rx_size; i++) {
1535                struct dma_desc *p;
1536                int ret;
1537
1538                if (priv->extend_desc)
1539                        p = &((rx_q->dma_erx + i)->basic);
1540                else
1541                        p = rx_q->dma_rx + i;
1542
1543                ret = stmmac_init_rx_buffers(priv, p, i, flags,
1544                                             queue);
1545                if (ret)
1546                        return ret;
1547
1548                rx_q->buf_alloc_num++;
1549        }
1550
1551        return 0;
1552}
1553
1554/**
1555 * dma_free_rx_xskbufs - free RX dma buffers from XSK pool
1556 * @priv: private structure
1557 * @queue: RX queue index
1558 */
1559static void dma_free_rx_xskbufs(struct stmmac_priv *priv, u32 queue)
1560{
1561        struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1562        int i;
1563
1564        for (i = 0; i < priv->dma_rx_size; i++) {
1565                struct stmmac_rx_buffer *buf = &rx_q->buf_pool[i];
1566
1567                if (!buf->xdp)
1568                        continue;
1569
1570                xsk_buff_free(buf->xdp);
1571                buf->xdp = NULL;
1572        }
1573}
1574
1575static int stmmac_alloc_rx_buffers_zc(struct stmmac_priv *priv, u32 queue)
1576{
1577        struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1578        int i;
1579
1580        for (i = 0; i < priv->dma_rx_size; i++) {
1581                struct stmmac_rx_buffer *buf;
1582                dma_addr_t dma_addr;
1583                struct dma_desc *p;
1584
1585                if (priv->extend_desc)
1586                        p = (struct dma_desc *)(rx_q->dma_erx + i);
1587                else
1588                        p = rx_q->dma_rx + i;
1589
1590                buf = &rx_q->buf_pool[i];
1591
1592                buf->xdp = xsk_buff_alloc(rx_q->xsk_pool);
1593                if (!buf->xdp)
1594                        return -ENOMEM;
1595
1596                dma_addr = xsk_buff_xdp_get_dma(buf->xdp);
1597                stmmac_set_desc_addr(priv, p, dma_addr);
1598                rx_q->buf_alloc_num++;
1599        }
1600
1601        return 0;
1602}
1603
1604static struct xsk_buff_pool *stmmac_get_xsk_pool(struct stmmac_priv *priv, u32 queue)
1605{
1606        if (!stmmac_xdp_is_enabled(priv) || !test_bit(queue, priv->af_xdp_zc_qps))
1607                return NULL;
1608
1609        return xsk_get_pool_from_qid(priv->dev, queue);
1610}
1611
1612/**
1613 * __init_dma_rx_desc_rings - init the RX descriptor ring (per queue)
1614 * @priv: driver private structure
1615 * @queue: RX queue index
1616 * @flags: gfp flag.
1617 * Description: this function initializes the DMA RX descriptors
1618 * and allocates the socket buffers. It supports the chained and ring
1619 * modes.
1620 */
1621static int __init_dma_rx_desc_rings(struct stmmac_priv *priv, u32 queue, gfp_t flags)
1622{
1623        struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1624        int ret;
1625
1626        netif_dbg(priv, probe, priv->dev,
1627                  "(%s) dma_rx_phy=0x%08x\n", __func__,
1628                  (u32)rx_q->dma_rx_phy);
1629
1630        stmmac_clear_rx_descriptors(priv, queue);
1631
1632        xdp_rxq_info_unreg_mem_model(&rx_q->xdp_rxq);
1633
1634        rx_q->xsk_pool = stmmac_get_xsk_pool(priv, queue);
1635
1636        if (rx_q->xsk_pool) {
1637                WARN_ON(xdp_rxq_info_reg_mem_model(&rx_q->xdp_rxq,
1638                                                   MEM_TYPE_XSK_BUFF_POOL,
1639                                                   NULL));
1640                netdev_info(priv->dev,
1641                            "Register MEM_TYPE_XSK_BUFF_POOL RxQ-%d\n",
1642                            rx_q->queue_index);
1643                xsk_pool_set_rxq_info(rx_q->xsk_pool, &rx_q->xdp_rxq);
1644        } else {
1645                WARN_ON(xdp_rxq_info_reg_mem_model(&rx_q->xdp_rxq,
1646                                                   MEM_TYPE_PAGE_POOL,
1647                                                   rx_q->page_pool));
1648                netdev_info(priv->dev,
1649                            "Register MEM_TYPE_PAGE_POOL RxQ-%d\n",
1650                            rx_q->queue_index);
1651        }
1652
1653        if (rx_q->xsk_pool) {
1654                /* RX XDP ZC buffer pool may not be populated, e.g.
1655                 * xdpsock TX-only.
1656                 */
1657                stmmac_alloc_rx_buffers_zc(priv, queue);
1658        } else {
1659                ret = stmmac_alloc_rx_buffers(priv, queue, flags);
1660                if (ret < 0)
1661                        return -ENOMEM;
1662        }
1663
1664        rx_q->cur_rx = 0;
1665        rx_q->dirty_rx = 0;
1666
1667        /* Setup the chained descriptor addresses */
1668        if (priv->mode == STMMAC_CHAIN_MODE) {
1669                if (priv->extend_desc)
1670                        stmmac_mode_init(priv, rx_q->dma_erx,
1671                                         rx_q->dma_rx_phy,
1672                                         priv->dma_rx_size, 1);
1673                else
1674                        stmmac_mode_init(priv, rx_q->dma_rx,
1675                                         rx_q->dma_rx_phy,
1676                                         priv->dma_rx_size, 0);
1677        }
1678
1679        return 0;
1680}
1681
1682static int init_dma_rx_desc_rings(struct net_device *dev, gfp_t flags)
1683{
1684        struct stmmac_priv *priv = netdev_priv(dev);
1685        u32 rx_count = priv->plat->rx_queues_to_use;
1686        u32 queue;
1687        int ret;
1688
1689        /* RX INITIALIZATION */
1690        netif_dbg(priv, probe, priv->dev,
1691                  "SKB addresses:\nskb\t\tskb data\tdma data\n");
1692
1693        for (queue = 0; queue < rx_count; queue++) {
1694                ret = __init_dma_rx_desc_rings(priv, queue, flags);
1695                if (ret)
1696                        goto err_init_rx_buffers;
1697        }
1698
1699        return 0;
1700
1701err_init_rx_buffers:
1702        while (queue >= 0) {
1703                struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1704
1705                if (rx_q->xsk_pool)
1706                        dma_free_rx_xskbufs(priv, queue);
1707                else
1708                        dma_free_rx_skbufs(priv, queue);
1709
1710                rx_q->buf_alloc_num = 0;
1711                rx_q->xsk_pool = NULL;
1712
1713                if (queue == 0)
1714                        break;
1715
1716                queue--;
1717        }
1718
1719        return ret;
1720}
1721
1722/**
1723 * __init_dma_tx_desc_rings - init the TX descriptor ring (per queue)
1724 * @priv: driver private structure
1725 * @queue : TX queue index
1726 * Description: this function initializes the DMA TX descriptors
1727 * and allocates the socket buffers. It supports the chained and ring
1728 * modes.
1729 */
1730static int __init_dma_tx_desc_rings(struct stmmac_priv *priv, u32 queue)
1731{
1732        struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
1733        int i;
1734
1735        netif_dbg(priv, probe, priv->dev,
1736                  "(%s) dma_tx_phy=0x%08x\n", __func__,
1737                  (u32)tx_q->dma_tx_phy);
1738
1739        /* Setup the chained descriptor addresses */
1740        if (priv->mode == STMMAC_CHAIN_MODE) {
1741                if (priv->extend_desc)
1742                        stmmac_mode_init(priv, tx_q->dma_etx,
1743                                         tx_q->dma_tx_phy,
1744                                         priv->dma_tx_size, 1);
1745                else if (!(tx_q->tbs & STMMAC_TBS_AVAIL))
1746                        stmmac_mode_init(priv, tx_q->dma_tx,
1747                                         tx_q->dma_tx_phy,
1748                                         priv->dma_tx_size, 0);
1749        }
1750
1751        tx_q->xsk_pool = stmmac_get_xsk_pool(priv, queue);
1752
1753        for (i = 0; i < priv->dma_tx_size; i++) {
1754                struct dma_desc *p;
1755
1756                if (priv->extend_desc)
1757                        p = &((tx_q->dma_etx + i)->basic);
1758                else if (tx_q->tbs & STMMAC_TBS_AVAIL)
1759                        p = &((tx_q->dma_entx + i)->basic);
1760                else
1761                        p = tx_q->dma_tx + i;
1762
1763                stmmac_clear_desc(priv, p);
1764
1765                tx_q->tx_skbuff_dma[i].buf = 0;
1766                tx_q->tx_skbuff_dma[i].map_as_page = false;
1767                tx_q->tx_skbuff_dma[i].len = 0;
1768                tx_q->tx_skbuff_dma[i].last_segment = false;
1769                tx_q->tx_skbuff[i] = NULL;
1770        }
1771
1772        tx_q->dirty_tx = 0;
1773        tx_q->cur_tx = 0;
1774        tx_q->mss = 0;
1775
1776        netdev_tx_reset_queue(netdev_get_tx_queue(priv->dev, queue));
1777
1778        return 0;
1779}
1780
1781static int init_dma_tx_desc_rings(struct net_device *dev)
1782{
1783        struct stmmac_priv *priv = netdev_priv(dev);
1784        u32 tx_queue_cnt;
1785        u32 queue;
1786
1787        tx_queue_cnt = priv->plat->tx_queues_to_use;
1788
1789        for (queue = 0; queue < tx_queue_cnt; queue++)
1790                __init_dma_tx_desc_rings(priv, queue);
1791
1792        return 0;
1793}
1794
1795/**
1796 * init_dma_desc_rings - init the RX/TX descriptor rings
1797 * @dev: net device structure
1798 * @flags: gfp flag.
1799 * Description: this function initializes the DMA RX/TX descriptors
1800 * and allocates the socket buffers. It supports the chained and ring
1801 * modes.
1802 */
1803static int init_dma_desc_rings(struct net_device *dev, gfp_t flags)
1804{
1805        struct stmmac_priv *priv = netdev_priv(dev);
1806        int ret;
1807
1808        ret = init_dma_rx_desc_rings(dev, flags);
1809        if (ret)
1810                return ret;
1811
1812        ret = init_dma_tx_desc_rings(dev);
1813
1814        stmmac_clear_descriptors(priv);
1815
1816        if (netif_msg_hw(priv))
1817                stmmac_display_rings(priv);
1818
1819        return ret;
1820}
1821
1822/**
1823 * dma_free_tx_skbufs - free TX dma buffers
1824 * @priv: private structure
1825 * @queue: TX queue index
1826 */
1827static void dma_free_tx_skbufs(struct stmmac_priv *priv, u32 queue)
1828{
1829        struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
1830        int i;
1831
1832        tx_q->xsk_frames_done = 0;
1833
1834        for (i = 0; i < priv->dma_tx_size; i++)
1835                stmmac_free_tx_buffer(priv, queue, i);
1836
1837        if (tx_q->xsk_pool && tx_q->xsk_frames_done) {
1838                xsk_tx_completed(tx_q->xsk_pool, tx_q->xsk_frames_done);
1839                tx_q->xsk_frames_done = 0;
1840                tx_q->xsk_pool = NULL;
1841        }
1842}
1843
1844/**
1845 * stmmac_free_tx_skbufs - free TX skb buffers
1846 * @priv: private structure
1847 */
1848static void stmmac_free_tx_skbufs(struct stmmac_priv *priv)
1849{
1850        u32 tx_queue_cnt = priv->plat->tx_queues_to_use;
1851        u32 queue;
1852
1853        for (queue = 0; queue < tx_queue_cnt; queue++)
1854                dma_free_tx_skbufs(priv, queue);
1855}
1856
1857/**
1858 * __free_dma_rx_desc_resources - free RX dma desc resources (per queue)
1859 * @priv: private structure
1860 * @queue: RX queue index
1861 */
1862static void __free_dma_rx_desc_resources(struct stmmac_priv *priv, u32 queue)
1863{
1864        struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1865
1866        /* Release the DMA RX socket buffers */
1867        if (rx_q->xsk_pool)
1868                dma_free_rx_xskbufs(priv, queue);
1869        else
1870                dma_free_rx_skbufs(priv, queue);
1871
1872        rx_q->buf_alloc_num = 0;
1873        rx_q->xsk_pool = NULL;
1874
1875        /* Free DMA regions of consistent memory previously allocated */
1876        if (!priv->extend_desc)
1877                dma_free_coherent(priv->device, priv->dma_rx_size *
1878                                  sizeof(struct dma_desc),
1879                                  rx_q->dma_rx, rx_q->dma_rx_phy);
1880        else
1881                dma_free_coherent(priv->device, priv->dma_rx_size *
1882                                  sizeof(struct dma_extended_desc),
1883                                  rx_q->dma_erx, rx_q->dma_rx_phy);
1884
1885        if (xdp_rxq_info_is_reg(&rx_q->xdp_rxq))
1886                xdp_rxq_info_unreg(&rx_q->xdp_rxq);
1887
1888        kfree(rx_q->buf_pool);
1889        if (rx_q->page_pool)
1890                page_pool_destroy(rx_q->page_pool);
1891}
1892
1893static void free_dma_rx_desc_resources(struct stmmac_priv *priv)
1894{
1895        u32 rx_count = priv->plat->rx_queues_to_use;
1896        u32 queue;
1897
1898        /* Free RX queue resources */
1899        for (queue = 0; queue < rx_count; queue++)
1900                __free_dma_rx_desc_resources(priv, queue);
1901}
1902
1903/**
1904 * __free_dma_tx_desc_resources - free TX dma desc resources (per queue)
1905 * @priv: private structure
1906 * @queue: TX queue index
1907 */
1908static void __free_dma_tx_desc_resources(struct stmmac_priv *priv, u32 queue)
1909{
1910        struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
1911        size_t size;
1912        void *addr;
1913
1914        /* Release the DMA TX socket buffers */
1915        dma_free_tx_skbufs(priv, queue);
1916
1917        if (priv->extend_desc) {
1918                size = sizeof(struct dma_extended_desc);
1919                addr = tx_q->dma_etx;
1920        } else if (tx_q->tbs & STMMAC_TBS_AVAIL) {
1921                size = sizeof(struct dma_edesc);
1922                addr = tx_q->dma_entx;
1923        } else {
1924                size = sizeof(struct dma_desc);
1925                addr = tx_q->dma_tx;
1926        }
1927
1928        size *= priv->dma_tx_size;
1929
1930        dma_free_coherent(priv->device, size, addr, tx_q->dma_tx_phy);
1931
1932        kfree(tx_q->tx_skbuff_dma);
1933        kfree(tx_q->tx_skbuff);
1934}
1935
1936static void free_dma_tx_desc_resources(struct stmmac_priv *priv)
1937{
1938        u32 tx_count = priv->plat->tx_queues_to_use;
1939        u32 queue;
1940
1941        /* Free TX queue resources */
1942        for (queue = 0; queue < tx_count; queue++)
1943                __free_dma_tx_desc_resources(priv, queue);
1944}
1945
1946/**
1947 * __alloc_dma_rx_desc_resources - alloc RX resources (per queue).
1948 * @priv: private structure
1949 * @queue: RX queue index
1950 * Description: according to which descriptor can be used (extend or basic)
1951 * this function allocates the resources for TX and RX paths. In case of
1952 * reception, for example, it pre-allocated the RX socket buffer in order to
1953 * allow zero-copy mechanism.
1954 */
1955static int __alloc_dma_rx_desc_resources(struct stmmac_priv *priv, u32 queue)
1956{
1957        struct stmmac_rx_queue *rx_q = &priv->rx_queue[queue];
1958        struct stmmac_channel *ch = &priv->channel[queue];
1959        bool xdp_prog = stmmac_xdp_is_enabled(priv);
1960        struct page_pool_params pp_params = { 0 };
1961        unsigned int num_pages;
1962        unsigned int napi_id;
1963        int ret;
1964
1965        rx_q->queue_index = queue;
1966        rx_q->priv_data = priv;
1967
1968        pp_params.flags = PP_FLAG_DMA_MAP | PP_FLAG_DMA_SYNC_DEV;
1969        pp_params.pool_size = priv->dma_rx_size;
1970        num_pages = DIV_ROUND_UP(priv->dma_buf_sz, PAGE_SIZE);
1971        pp_params.order = ilog2(num_pages);
1972        pp_params.nid = dev_to_node(priv->device);
1973        pp_params.dev = priv->device;
1974        pp_params.dma_dir = xdp_prog ? DMA_BIDIRECTIONAL : DMA_FROM_DEVICE;
1975        pp_params.offset = stmmac_rx_offset(priv);
1976        pp_params.max_len = STMMAC_MAX_RX_BUF_SIZE(num_pages);
1977
1978        rx_q->page_pool = page_pool_create(&pp_params);
1979        if (IS_ERR(rx_q->page_pool)) {
1980                ret = PTR_ERR(rx_q->page_pool);
1981                rx_q->page_pool = NULL;
1982                return ret;
1983        }
1984
1985        rx_q->buf_pool = kcalloc(priv->dma_rx_size,
1986                                 sizeof(*rx_q->buf_pool),
1987                                 GFP_KERNEL);
1988        if (!rx_q->buf_pool)
1989                return -ENOMEM;
1990
1991        if (priv->extend_desc) {
1992                rx_q->dma_erx = dma_alloc_coherent(priv->device,
1993                                                   priv->dma_rx_size *
1994                                                   sizeof(struct dma_extended_desc),
1995                                                   &rx_q->dma_rx_phy,
1996                                                   GFP_KERNEL);
1997                if (!rx_q->dma_erx)
1998                        return -ENOMEM;
1999
2000        } else {
2001                rx_q->dma_rx = dma_alloc_coherent(priv->device,
2002                                                  priv->dma_rx_size *
2003                                                  sizeof(struct dma_desc),
2004                                                  &rx_q->dma_rx_phy,
2005                                                  GFP_KERNEL);
2006                if (!rx_q->dma_rx)
2007                        return -ENOMEM;
2008        }
2009
2010        if (stmmac_xdp_is_enabled(priv) &&
2011            test_bit(queue, priv->af_xdp_zc_qps))
2012                napi_id = ch->rxtx_napi.napi_id;
2013        else
2014                napi_id = ch->rx_napi.napi_id;
2015
2016        ret = xdp_rxq_info_reg(&rx_q->xdp_rxq, priv->dev,
2017                               rx_q->queue_index,
2018                               napi_id);
2019        if (ret) {
2020                netdev_err(priv->dev, "Failed to register xdp rxq info\n");
2021                return -EINVAL;
2022        }
2023
2024        return 0;
2025}
2026
2027static int alloc_dma_rx_desc_resources(struct stmmac_priv *priv)
2028{
2029        u32 rx_count = priv->plat->rx_queues_to_use;
2030        u32 queue;
2031        int ret;
2032
2033        /* RX queues buffers and DMA */
2034        for (queue = 0; queue < rx_count; queue++) {
2035                ret = __alloc_dma_rx_desc_resources(priv, queue);
2036                if (ret)
2037                        goto err_dma;
2038        }
2039
2040        return 0;
2041
2042err_dma:
2043        free_dma_rx_desc_resources(priv);
2044
2045        return ret;
2046}
2047
2048/**
2049 * __alloc_dma_tx_desc_resources - alloc TX resources (per queue).
2050 * @priv: private structure
2051 * @queue: TX queue index
2052 * Description: according to which descriptor can be used (extend or basic)
2053 * this function allocates the resources for TX and RX paths. In case of
2054 * reception, for example, it pre-allocated the RX socket buffer in order to
2055 * allow zero-copy mechanism.
2056 */
2057static int __alloc_dma_tx_desc_resources(struct stmmac_priv *priv, u32 queue)
2058{
2059        struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
2060        size_t size;
2061        void *addr;
2062
2063        tx_q->queue_index = queue;
2064        tx_q->priv_data = priv;
2065
2066        tx_q->tx_skbuff_dma = kcalloc(priv->dma_tx_size,
2067                                      sizeof(*tx_q->tx_skbuff_dma),
2068                                      GFP_KERNEL);
2069        if (!tx_q->tx_skbuff_dma)
2070                return -ENOMEM;
2071
2072        tx_q->tx_skbuff = kcalloc(priv->dma_tx_size,
2073                                  sizeof(struct sk_buff *),
2074                                  GFP_KERNEL);
2075        if (!tx_q->tx_skbuff)
2076                return -ENOMEM;
2077
2078        if (priv->extend_desc)
2079                size = sizeof(struct dma_extended_desc);
2080        else if (tx_q->tbs & STMMAC_TBS_AVAIL)
2081                size = sizeof(struct dma_edesc);
2082        else
2083                size = sizeof(struct dma_desc);
2084
2085        size *= priv->dma_tx_size;
2086
2087        addr = dma_alloc_coherent(priv->device, size,
2088                                  &tx_q->dma_tx_phy, GFP_KERNEL);
2089        if (!addr)
2090                return -ENOMEM;
2091
2092        if (priv->extend_desc)
2093                tx_q->dma_etx = addr;
2094        else if (tx_q->tbs & STMMAC_TBS_AVAIL)
2095                tx_q->dma_entx = addr;
2096        else
2097                tx_q->dma_tx = addr;
2098
2099        return 0;
2100}
2101
2102static int alloc_dma_tx_desc_resources(struct stmmac_priv *priv)
2103{
2104        u32 tx_count = priv->plat->tx_queues_to_use;
2105        u32 queue;
2106        int ret;
2107
2108        /* TX queues buffers and DMA */
2109        for (queue = 0; queue < tx_count; queue++) {
2110                ret = __alloc_dma_tx_desc_resources(priv, queue);
2111                if (ret)
2112                        goto err_dma;
2113        }
2114
2115        return 0;
2116
2117err_dma:
2118        free_dma_tx_desc_resources(priv);
2119        return ret;
2120}
2121
2122/**
2123 * alloc_dma_desc_resources - alloc TX/RX resources.
2124 * @priv: private structure
2125 * Description: according to which descriptor can be used (extend or basic)
2126 * this function allocates the resources for TX and RX paths. In case of
2127 * reception, for example, it pre-allocated the RX socket buffer in order to
2128 * allow zero-copy mechanism.
2129 */
2130static int alloc_dma_desc_resources(struct stmmac_priv *priv)
2131{
2132        /* RX Allocation */
2133        int ret = alloc_dma_rx_desc_resources(priv);
2134
2135        if (ret)
2136                return ret;
2137
2138        ret = alloc_dma_tx_desc_resources(priv);
2139
2140        return ret;
2141}
2142
2143/**
2144 * free_dma_desc_resources - free dma desc resources
2145 * @priv: private structure
2146 */
2147static void free_dma_desc_resources(struct stmmac_priv *priv)
2148{
2149        /* Release the DMA TX socket buffers */
2150        free_dma_tx_desc_resources(priv);
2151
2152        /* Release the DMA RX socket buffers later
2153         * to ensure all pending XDP_TX buffers are returned.
2154         */
2155        free_dma_rx_desc_resources(priv);
2156}
2157
2158/**
2159 *  stmmac_mac_enable_rx_queues - Enable MAC rx queues
2160 *  @priv: driver private structure
2161 *  Description: It is used for enabling the rx queues in the MAC
2162 */
2163static void stmmac_mac_enable_rx_queues(struct stmmac_priv *priv)
2164{
2165        u32 rx_queues_count = priv->plat->rx_queues_to_use;
2166        int queue;
2167        u8 mode;
2168
2169        for (queue = 0; queue < rx_queues_count; queue++) {
2170                mode = priv->plat->rx_queues_cfg[queue].mode_to_use;
2171                stmmac_rx_queue_enable(priv, priv->hw, mode, queue);
2172        }
2173}
2174
2175/**
2176 * stmmac_start_rx_dma - start RX DMA channel
2177 * @priv: driver private structure
2178 * @chan: RX channel index
2179 * Description:
2180 * This starts a RX DMA channel
2181 */
2182static void stmmac_start_rx_dma(struct stmmac_priv *priv, u32 chan)
2183{
2184        netdev_dbg(priv->dev, "DMA RX processes started in channel %d\n", chan);
2185        stmmac_start_rx(priv, priv->ioaddr, chan);
2186}
2187
2188/**
2189 * stmmac_start_tx_dma - start TX DMA channel
2190 * @priv: driver private structure
2191 * @chan: TX channel index
2192 * Description:
2193 * This starts a TX DMA channel
2194 */
2195static void stmmac_start_tx_dma(struct stmmac_priv *priv, u32 chan)
2196{
2197        netdev_dbg(priv->dev, "DMA TX processes started in channel %d\n", chan);
2198        stmmac_start_tx(priv, priv->ioaddr, chan);
2199}
2200
2201/**
2202 * stmmac_stop_rx_dma - stop RX DMA channel
2203 * @priv: driver private structure
2204 * @chan: RX channel index
2205 * Description:
2206 * This stops a RX DMA channel
2207 */
2208static void stmmac_stop_rx_dma(struct stmmac_priv *priv, u32 chan)
2209{
2210        netdev_dbg(priv->dev, "DMA RX processes stopped in channel %d\n", chan);
2211        stmmac_stop_rx(priv, priv->ioaddr, chan);
2212}
2213
2214/**
2215 * stmmac_stop_tx_dma - stop TX DMA channel
2216 * @priv: driver private structure
2217 * @chan: TX channel index
2218 * Description:
2219 * This stops a TX DMA channel
2220 */
2221static void stmmac_stop_tx_dma(struct stmmac_priv *priv, u32 chan)
2222{
2223        netdev_dbg(priv->dev, "DMA TX processes stopped in channel %d\n", chan);
2224        stmmac_stop_tx(priv, priv->ioaddr, chan);
2225}
2226
2227/**
2228 * stmmac_start_all_dma - start all RX and TX DMA channels
2229 * @priv: driver private structure
2230 * Description:
2231 * This starts all the RX and TX DMA channels
2232 */
2233static void stmmac_start_all_dma(struct stmmac_priv *priv)
2234{
2235        u32 rx_channels_count = priv->plat->rx_queues_to_use;
2236        u32 tx_channels_count = priv->plat->tx_queues_to_use;
2237        u32 chan = 0;
2238
2239        for (chan = 0; chan < rx_channels_count; chan++)
2240                stmmac_start_rx_dma(priv, chan);
2241
2242        for (chan = 0; chan < tx_channels_count; chan++)
2243                stmmac_start_tx_dma(priv, chan);
2244}
2245
2246/**
2247 * stmmac_stop_all_dma - stop all RX and TX DMA channels
2248 * @priv: driver private structure
2249 * Description:
2250 * This stops the RX and TX DMA channels
2251 */
2252static void stmmac_stop_all_dma(struct stmmac_priv *priv)
2253{
2254        u32 rx_channels_count = priv->plat->rx_queues_to_use;
2255        u32 tx_channels_count = priv->plat->tx_queues_to_use;
2256        u32 chan = 0;
2257
2258        for (chan = 0; chan < rx_channels_count; chan++)
2259                stmmac_stop_rx_dma(priv, chan);
2260
2261        for (chan = 0; chan < tx_channels_count; chan++)
2262                stmmac_stop_tx_dma(priv, chan);
2263}
2264
2265/**
2266 *  stmmac_dma_operation_mode - HW DMA operation mode
2267 *  @priv: driver private structure
2268 *  Description: it is used for configuring the DMA operation mode register in
2269 *  order to program the tx/rx DMA thresholds or Store-And-Forward mode.
2270 */
2271static void stmmac_dma_operation_mode(struct stmmac_priv *priv)
2272{
2273        u32 rx_channels_count = priv->plat->rx_queues_to_use;
2274        u32 tx_channels_count = priv->plat->tx_queues_to_use;
2275        int rxfifosz = priv->plat->rx_fifo_size;
2276        int txfifosz = priv->plat->tx_fifo_size;
2277        u32 txmode = 0;
2278        u32 rxmode = 0;
2279        u32 chan = 0;
2280        u8 qmode = 0;
2281
2282        if (rxfifosz == 0)
2283                rxfifosz = priv->dma_cap.rx_fifo_size;
2284        if (txfifosz == 0)
2285                txfifosz = priv->dma_cap.tx_fifo_size;
2286
2287        /* Adjust for real per queue fifo size */
2288        rxfifosz /= rx_channels_count;
2289        txfifosz /= tx_channels_count;
2290
2291        if (priv->plat->force_thresh_dma_mode) {
2292                txmode = tc;
2293                rxmode = tc;
2294        } else if (priv->plat->force_sf_dma_mode || priv->plat->tx_coe) {
2295                /*
2296                 * In case of GMAC, SF mode can be enabled
2297                 * to perform the TX COE in HW. This depends on:
2298                 * 1) TX COE if actually supported
2299                 * 2) There is no bugged Jumbo frame support
2300                 *    that needs to not insert csum in the TDES.
2301                 */
2302                txmode = SF_DMA_MODE;
2303                rxmode = SF_DMA_MODE;
2304                priv->xstats.threshold = SF_DMA_MODE;
2305        } else {
2306                txmode = tc;
2307                rxmode = SF_DMA_MODE;
2308        }
2309
2310        /* configure all channels */
2311        for (chan = 0; chan < rx_channels_count; chan++) {
2312                struct stmmac_rx_queue *rx_q = &priv->rx_queue[chan];
2313                u32 buf_size;
2314
2315                qmode = priv->plat->rx_queues_cfg[chan].mode_to_use;
2316
2317                stmmac_dma_rx_mode(priv, priv->ioaddr, rxmode, chan,
2318                                rxfifosz, qmode);
2319
2320                if (rx_q->xsk_pool) {
2321                        buf_size = xsk_pool_get_rx_frame_size(rx_q->xsk_pool);
2322                        stmmac_set_dma_bfsize(priv, priv->ioaddr,
2323                                              buf_size,
2324                                              chan);
2325                } else {
2326                        stmmac_set_dma_bfsize(priv, priv->ioaddr,
2327                                              priv->dma_buf_sz,
2328                                              chan);
2329                }
2330        }
2331
2332        for (chan = 0; chan < tx_channels_count; chan++) {
2333                qmode = priv->plat->tx_queues_cfg[chan].mode_to_use;
2334
2335                stmmac_dma_tx_mode(priv, priv->ioaddr, txmode, chan,
2336                                txfifosz, qmode);
2337        }
2338}
2339
2340static bool stmmac_xdp_xmit_zc(struct stmmac_priv *priv, u32 queue, u32 budget)
2341{
2342        struct netdev_queue *nq = netdev_get_tx_queue(priv->dev, queue);
2343        struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
2344        struct xsk_buff_pool *pool = tx_q->xsk_pool;
2345        unsigned int entry = tx_q->cur_tx;
2346        struct dma_desc *tx_desc = NULL;
2347        struct xdp_desc xdp_desc;
2348        bool work_done = true;
2349
2350        /* Avoids TX time-out as we are sharing with slow path */
2351        nq->trans_start = jiffies;
2352
2353        budget = min(budget, stmmac_tx_avail(priv, queue));
2354
2355        while (budget-- > 0) {
2356                dma_addr_t dma_addr;
2357                bool set_ic;
2358
2359                /* We are sharing with slow path and stop XSK TX desc submission when
2360                 * available TX ring is less than threshold.
2361                 */
2362                if (unlikely(stmmac_tx_avail(priv, queue) < STMMAC_TX_XSK_AVAIL) ||
2363                    !netif_carrier_ok(priv->dev)) {
2364                        work_done = false;
2365                        break;
2366                }
2367
2368                if (!xsk_tx_peek_desc(pool, &xdp_desc))
2369                        break;
2370
2371                if (likely(priv->extend_desc))
2372                        tx_desc = (struct dma_desc *)(tx_q->dma_etx + entry);
2373                else if (tx_q->tbs & STMMAC_TBS_AVAIL)
2374                        tx_desc = &tx_q->dma_entx[entry].basic;
2375                else
2376                        tx_desc = tx_q->dma_tx + entry;
2377
2378                dma_addr = xsk_buff_raw_get_dma(pool, xdp_desc.addr);
2379                xsk_buff_raw_dma_sync_for_device(pool, dma_addr, xdp_desc.len);
2380
2381                tx_q->tx_skbuff_dma[entry].buf_type = STMMAC_TXBUF_T_XSK_TX;
2382
2383                /* To return XDP buffer to XSK pool, we simple call
2384                 * xsk_tx_completed(), so we don't need to fill up
2385                 * 'buf' and 'xdpf'.
2386                 */
2387                tx_q->tx_skbuff_dma[entry].buf = 0;
2388                tx_q->xdpf[entry] = NULL;
2389
2390                tx_q->tx_skbuff_dma[entry].map_as_page = false;
2391                tx_q->tx_skbuff_dma[entry].len = xdp_desc.len;
2392                tx_q->tx_skbuff_dma[entry].last_segment = true;
2393                tx_q->tx_skbuff_dma[entry].is_jumbo = false;
2394
2395                stmmac_set_desc_addr(priv, tx_desc, dma_addr);
2396
2397                tx_q->tx_count_frames++;
2398
2399                if (!priv->tx_coal_frames[queue])
2400                        set_ic = false;
2401                else if (tx_q->tx_count_frames % priv->tx_coal_frames[queue] == 0)
2402                        set_ic = true;
2403                else
2404                        set_ic = false;
2405
2406                if (set_ic) {
2407                        tx_q->tx_count_frames = 0;
2408                        stmmac_set_tx_ic(priv, tx_desc);
2409                        priv->xstats.tx_set_ic_bit++;
2410                }
2411
2412                stmmac_prepare_tx_desc(priv, tx_desc, 1, xdp_desc.len,
2413                                       true, priv->mode, true, true,
2414                                       xdp_desc.len);
2415
2416                stmmac_enable_dma_transmission(priv, priv->ioaddr);
2417
2418                tx_q->cur_tx = STMMAC_GET_ENTRY(tx_q->cur_tx, priv->dma_tx_size);
2419                entry = tx_q->cur_tx;
2420        }
2421
2422        if (tx_desc) {
2423                stmmac_flush_tx_descriptors(priv, queue);
2424                xsk_tx_release(pool);
2425        }
2426
2427        /* Return true if all of the 3 conditions are met
2428         *  a) TX Budget is still available
2429         *  b) work_done = true when XSK TX desc peek is empty (no more
2430         *     pending XSK TX for transmission)
2431         */
2432        return !!budget && work_done;
2433}
2434
2435/**
2436 * stmmac_tx_clean - to manage the transmission completion
2437 * @priv: driver private structure
2438 * @budget: napi budget limiting this functions packet handling
2439 * @queue: TX queue index
2440 * Description: it reclaims the transmit resources after transmission completes.
2441 */
2442static int stmmac_tx_clean(struct stmmac_priv *priv, int budget, u32 queue)
2443{
2444        struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
2445        unsigned int bytes_compl = 0, pkts_compl = 0;
2446        unsigned int entry, xmits = 0, count = 0;
2447
2448        __netif_tx_lock_bh(netdev_get_tx_queue(priv->dev, queue));
2449
2450        priv->xstats.tx_clean++;
2451
2452        tx_q->xsk_frames_done = 0;
2453
2454        entry = tx_q->dirty_tx;
2455
2456        /* Try to clean all TX complete frame in 1 shot */
2457        while ((entry != tx_q->cur_tx) && count < priv->dma_tx_size) {
2458                struct xdp_frame *xdpf;
2459                struct sk_buff *skb;
2460                struct dma_desc *p;
2461                int status;
2462
2463                if (tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_XDP_TX ||
2464                    tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_XDP_NDO) {
2465                        xdpf = tx_q->xdpf[entry];
2466                        skb = NULL;
2467                } else if (tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_SKB) {
2468                        xdpf = NULL;
2469                        skb = tx_q->tx_skbuff[entry];
2470                } else {
2471                        xdpf = NULL;
2472                        skb = NULL;
2473                }
2474
2475                if (priv->extend_desc)
2476                        p = (struct dma_desc *)(tx_q->dma_etx + entry);
2477                else if (tx_q->tbs & STMMAC_TBS_AVAIL)
2478                        p = &tx_q->dma_entx[entry].basic;
2479                else
2480                        p = tx_q->dma_tx + entry;
2481
2482                status = stmmac_tx_status(priv, &priv->dev->stats,
2483                                &priv->xstats, p, priv->ioaddr);
2484                /* Check if the descriptor is owned by the DMA */
2485                if (unlikely(status & tx_dma_own))
2486                        break;
2487
2488                count++;
2489
2490                /* Make sure descriptor fields are read after reading
2491                 * the own bit.
2492                 */
2493                dma_rmb();
2494
2495                /* Just consider the last segment and ...*/
2496                if (likely(!(status & tx_not_ls))) {
2497                        /* ... verify the status error condition */
2498                        if (unlikely(status & tx_err)) {
2499                                priv->dev->stats.tx_errors++;
2500                        } else {
2501                                priv->dev->stats.tx_packets++;
2502                                priv->xstats.tx_pkt_n++;
2503                        }
2504                        if (skb)
2505                                stmmac_get_tx_hwtstamp(priv, p, skb);
2506                }
2507
2508                if (likely(tx_q->tx_skbuff_dma[entry].buf &&
2509                           tx_q->tx_skbuff_dma[entry].buf_type != STMMAC_TXBUF_T_XDP_TX)) {
2510                        if (tx_q->tx_skbuff_dma[entry].map_as_page)
2511                                dma_unmap_page(priv->device,
2512                                               tx_q->tx_skbuff_dma[entry].buf,
2513                                               tx_q->tx_skbuff_dma[entry].len,
2514                                               DMA_TO_DEVICE);
2515                        else
2516                                dma_unmap_single(priv->device,
2517                                                 tx_q->tx_skbuff_dma[entry].buf,
2518                                                 tx_q->tx_skbuff_dma[entry].len,
2519                                                 DMA_TO_DEVICE);
2520                        tx_q->tx_skbuff_dma[entry].buf = 0;
2521                        tx_q->tx_skbuff_dma[entry].len = 0;
2522                        tx_q->tx_skbuff_dma[entry].map_as_page = false;
2523                }
2524
2525                stmmac_clean_desc3(priv, tx_q, p);
2526
2527                tx_q->tx_skbuff_dma[entry].last_segment = false;
2528                tx_q->tx_skbuff_dma[entry].is_jumbo = false;
2529
2530                if (xdpf &&
2531                    tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_XDP_TX) {
2532                        xdp_return_frame_rx_napi(xdpf);
2533                        tx_q->xdpf[entry] = NULL;
2534                }
2535
2536                if (xdpf &&
2537                    tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_XDP_NDO) {
2538                        xdp_return_frame(xdpf);
2539                        tx_q->xdpf[entry] = NULL;
2540                }
2541
2542                if (tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_XSK_TX)
2543                        tx_q->xsk_frames_done++;
2544
2545                if (tx_q->tx_skbuff_dma[entry].buf_type == STMMAC_TXBUF_T_SKB) {
2546                        if (likely(skb)) {
2547                                pkts_compl++;
2548                                bytes_compl += skb->len;
2549                                dev_consume_skb_any(skb);
2550                                tx_q->tx_skbuff[entry] = NULL;
2551                        }
2552                }
2553
2554                stmmac_release_tx_desc(priv, p, priv->mode);
2555
2556                entry = STMMAC_GET_ENTRY(entry, priv->dma_tx_size);
2557        }
2558        tx_q->dirty_tx = entry;
2559
2560        netdev_tx_completed_queue(netdev_get_tx_queue(priv->dev, queue),
2561                                  pkts_compl, bytes_compl);
2562
2563        if (unlikely(netif_tx_queue_stopped(netdev_get_tx_queue(priv->dev,
2564                                                                queue))) &&
2565            stmmac_tx_avail(priv, queue) > STMMAC_TX_THRESH(priv)) {
2566
2567                netif_dbg(priv, tx_done, priv->dev,
2568                          "%s: restart transmit\n", __func__);
2569                netif_tx_wake_queue(netdev_get_tx_queue(priv->dev, queue));
2570        }
2571
2572        if (tx_q->xsk_pool) {
2573                bool work_done;
2574
2575                if (tx_q->xsk_frames_done)
2576                        xsk_tx_completed(tx_q->xsk_pool, tx_q->xsk_frames_done);
2577
2578                if (xsk_uses_need_wakeup(tx_q->xsk_pool))
2579                        xsk_set_tx_need_wakeup(tx_q->xsk_pool);
2580
2581                /* For XSK TX, we try to send as many as possible.
2582                 * If XSK work done (XSK TX desc empty and budget still
2583                 * available), return "budget - 1" to reenable TX IRQ.
2584                 * Else, return "budget" to make NAPI continue polling.
2585                 */
2586                work_done = stmmac_xdp_xmit_zc(priv, queue,
2587                                               STMMAC_XSK_TX_BUDGET_MAX);
2588                if (work_done)
2589                        xmits = budget - 1;
2590                else
2591                        xmits = budget;
2592        }
2593
2594        if (priv->eee_enabled && !priv->tx_path_in_lpi_mode &&
2595            priv->eee_sw_timer_en) {
2596                stmmac_enable_eee_mode(priv);
2597                mod_timer(&priv->eee_ctrl_timer, STMMAC_LPI_T(priv->tx_lpi_timer));
2598        }
2599
2600        /* We still have pending packets, let's call for a new scheduling */
2601        if (tx_q->dirty_tx != tx_q->cur_tx)
2602                hrtimer_start(&tx_q->txtimer,
2603                              STMMAC_COAL_TIMER(priv->tx_coal_timer[queue]),
2604                              HRTIMER_MODE_REL);
2605
2606        __netif_tx_unlock_bh(netdev_get_tx_queue(priv->dev, queue));
2607
2608        /* Combine decisions from TX clean and XSK TX */
2609        return max(count, xmits);
2610}
2611
2612/**
2613 * stmmac_tx_err - to manage the tx error
2614 * @priv: driver private structure
2615 * @chan: channel index
2616 * Description: it cleans the descriptors and restarts the transmission
2617 * in case of transmission errors.
2618 */
2619static void stmmac_tx_err(struct stmmac_priv *priv, u32 chan)
2620{
2621        struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan];
2622
2623        netif_tx_stop_queue(netdev_get_tx_queue(priv->dev, chan));
2624
2625        stmmac_stop_tx_dma(priv, chan);
2626        dma_free_tx_skbufs(priv, chan);
2627        stmmac_clear_tx_descriptors(priv, chan);
2628        tx_q->dirty_tx = 0;
2629        tx_q->cur_tx = 0;
2630        tx_q->mss = 0;
2631        netdev_tx_reset_queue(netdev_get_tx_queue(priv->dev, chan));
2632        stmmac_init_tx_chan(priv, priv->ioaddr, priv->plat->dma_cfg,
2633                            tx_q->dma_tx_phy, chan);
2634        stmmac_start_tx_dma(priv, chan);
2635
2636        priv->dev->stats.tx_errors++;
2637        netif_tx_wake_queue(netdev_get_tx_queue(priv->dev, chan));
2638}
2639
2640/**
2641 *  stmmac_set_dma_operation_mode - Set DMA operation mode by channel
2642 *  @priv: driver private structure
2643 *  @txmode: TX operating mode
2644 *  @rxmode: RX operating mode
2645 *  @chan: channel index
2646 *  Description: it is used for configuring of the DMA operation mode in
2647 *  runtime in order to program the tx/rx DMA thresholds or Store-And-Forward
2648 *  mode.
2649 */
2650static void stmmac_set_dma_operation_mode(struct stmmac_priv *priv, u32 txmode,
2651                                          u32 rxmode, u32 chan)
2652{
2653        u8 rxqmode = priv->plat->rx_queues_cfg[chan].mode_to_use;
2654        u8 txqmode = priv->plat->tx_queues_cfg[chan].mode_to_use;
2655        u32 rx_channels_count = priv->plat->rx_queues_to_use;
2656        u32 tx_channels_count = priv->plat->tx_queues_to_use;
2657        int rxfifosz = priv->plat->rx_fifo_size;
2658        int txfifosz = priv->plat->tx_fifo_size;
2659
2660        if (rxfifosz == 0)
2661                rxfifosz = priv->dma_cap.rx_fifo_size;
2662        if (txfifosz == 0)
2663                txfifosz = priv->dma_cap.tx_fifo_size;
2664
2665        /* Adjust for real per queue fifo size */
2666        rxfifosz /= rx_channels_count;
2667        txfifosz /= tx_channels_count;
2668
2669        stmmac_dma_rx_mode(priv, priv->ioaddr, rxmode, chan, rxfifosz, rxqmode);
2670        stmmac_dma_tx_mode(priv, priv->ioaddr, txmode, chan, txfifosz, txqmode);
2671}
2672
2673static bool stmmac_safety_feat_interrupt(struct stmmac_priv *priv)
2674{
2675        int ret;
2676
2677        ret = stmmac_safety_feat_irq_status(priv, priv->dev,
2678                        priv->ioaddr, priv->dma_cap.asp, &priv->sstats);
2679        if (ret && (ret != -EINVAL)) {
2680                stmmac_global_err(priv);
2681                return true;
2682        }
2683
2684        return false;
2685}
2686
2687static int stmmac_napi_check(struct stmmac_priv *priv, u32 chan, u32 dir)
2688{
2689        int status = stmmac_dma_interrupt_status(priv, priv->ioaddr,
2690                                                 &priv->xstats, chan, dir);
2691        struct stmmac_rx_queue *rx_q = &priv->rx_queue[chan];
2692        struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan];
2693        struct stmmac_channel *ch = &priv->channel[chan];
2694        struct napi_struct *rx_napi;
2695        struct napi_struct *tx_napi;
2696        unsigned long flags;
2697
2698        rx_napi = rx_q->xsk_pool ? &ch->rxtx_napi : &ch->rx_napi;
2699        tx_napi = tx_q->xsk_pool ? &ch->rxtx_napi : &ch->tx_napi;
2700
2701        if ((status & handle_rx) && (chan < priv->plat->rx_queues_to_use)) {
2702                if (napi_schedule_prep(rx_napi)) {
2703                        spin_lock_irqsave(&ch->lock, flags);
2704                        stmmac_disable_dma_irq(priv, priv->ioaddr, chan, 1, 0);
2705                        spin_unlock_irqrestore(&ch->lock, flags);
2706                        __napi_schedule(rx_napi);
2707                }
2708        }
2709
2710        if ((status & handle_tx) && (chan < priv->plat->tx_queues_to_use)) {
2711                if (napi_schedule_prep(tx_napi)) {
2712                        spin_lock_irqsave(&ch->lock, flags);
2713                        stmmac_disable_dma_irq(priv, priv->ioaddr, chan, 0, 1);
2714                        spin_unlock_irqrestore(&ch->lock, flags);
2715                        __napi_schedule(tx_napi);
2716                }
2717        }
2718
2719        return status;
2720}
2721
2722/**
2723 * stmmac_dma_interrupt - DMA ISR
2724 * @priv: driver private structure
2725 * Description: this is the DMA ISR. It is called by the main ISR.
2726 * It calls the dwmac dma routine and schedule poll method in case of some
2727 * work can be done.
2728 */
2729static void stmmac_dma_interrupt(struct stmmac_priv *priv)
2730{
2731        u32 tx_channel_count = priv->plat->tx_queues_to_use;
2732        u32 rx_channel_count = priv->plat->rx_queues_to_use;
2733        u32 channels_to_check = tx_channel_count > rx_channel_count ?
2734                                tx_channel_count : rx_channel_count;
2735        u32 chan;
2736        int status[max_t(u32, MTL_MAX_TX_QUEUES, MTL_MAX_RX_QUEUES)];
2737
2738        /* Make sure we never check beyond our status buffer. */
2739        if (WARN_ON_ONCE(channels_to_check > ARRAY_SIZE(status)))
2740                channels_to_check = ARRAY_SIZE(status);
2741
2742        for (chan = 0; chan < channels_to_check; chan++)
2743                status[chan] = stmmac_napi_check(priv, chan,
2744                                                 DMA_DIR_RXTX);
2745
2746        for (chan = 0; chan < tx_channel_count; chan++) {
2747                if (unlikely(status[chan] & tx_hard_error_bump_tc)) {
2748                        /* Try to bump up the dma threshold on this failure */
2749                        if (unlikely(priv->xstats.threshold != SF_DMA_MODE) &&
2750                            (tc <= 256)) {
2751                                tc += 64;
2752                                if (priv->plat->force_thresh_dma_mode)
2753                                        stmmac_set_dma_operation_mode(priv,
2754                                                                      tc,
2755                                                                      tc,
2756                                                                      chan);
2757                                else
2758                                        stmmac_set_dma_operation_mode(priv,
2759                                                                    tc,
2760                                                                    SF_DMA_MODE,
2761                                                                    chan);
2762                                priv->xstats.threshold = tc;
2763                        }
2764                } else if (unlikely(status[chan] == tx_hard_error)) {
2765                        stmmac_tx_err(priv, chan);
2766                }
2767        }
2768}
2769
2770/**
2771 * stmmac_mmc_setup: setup the Mac Management Counters (MMC)
2772 * @priv: driver private structure
2773 * Description: this masks the MMC irq, in fact, the counters are managed in SW.
2774 */
2775static void stmmac_mmc_setup(struct stmmac_priv *priv)
2776{
2777        unsigned int mode = MMC_CNTRL_RESET_ON_READ | MMC_CNTRL_COUNTER_RESET |
2778                            MMC_CNTRL_PRESET | MMC_CNTRL_FULL_HALF_PRESET;
2779
2780        stmmac_mmc_intr_all_mask(priv, priv->mmcaddr);
2781
2782        if (priv->dma_cap.rmon) {
2783                stmmac_mmc_ctrl(priv, priv->mmcaddr, mode);
2784                memset(&priv->mmc, 0, sizeof(struct stmmac_counters));
2785        } else
2786                netdev_info(priv->dev, "No MAC Management Counters available\n");
2787}
2788
2789/**
2790 * stmmac_get_hw_features - get MAC capabilities from the HW cap. register.
2791 * @priv: driver private structure
2792 * Description:
2793 *  new GMAC chip generations have a new register to indicate the
2794 *  presence of the optional feature/functions.
2795 *  This can be also used to override the value passed through the
2796 *  platform and necessary for old MAC10/100 and GMAC chips.
2797 */
2798static int stmmac_get_hw_features(struct stmmac_priv *priv)
2799{
2800        return stmmac_get_hw_feature(priv, priv->ioaddr, &priv->dma_cap) == 0;
2801}
2802
2803/**
2804 * stmmac_check_ether_addr - check if the MAC addr is valid
2805 * @priv: driver private structure
2806 * Description:
2807 * it is to verify if the MAC address is valid, in case of failures it
2808 * generates a random MAC address
2809 */
2810static void stmmac_check_ether_addr(struct stmmac_priv *priv)
2811{
2812        if (!is_valid_ether_addr(priv->dev->dev_addr)) {
2813                stmmac_get_umac_addr(priv, priv->hw, priv->dev->dev_addr, 0);
2814                if (!is_valid_ether_addr(priv->dev->dev_addr))
2815                        eth_hw_addr_random(priv->dev);
2816                dev_info(priv->device, "device MAC address %pM\n",
2817                         priv->dev->dev_addr);
2818        }
2819}
2820
2821/**
2822 * stmmac_init_dma_engine - DMA init.
2823 * @priv: driver private structure
2824 * Description:
2825 * It inits the DMA invoking the specific MAC/GMAC callback.
2826 * Some DMA parameters can be passed from the platform;
2827 * in case of these are not passed a default is kept for the MAC or GMAC.
2828 */
2829static int stmmac_init_dma_engine(struct stmmac_priv *priv)
2830{
2831        u32 rx_channels_count = priv->plat->rx_queues_to_use;
2832        u32 tx_channels_count = priv->plat->tx_queues_to_use;
2833        u32 dma_csr_ch = max(rx_channels_count, tx_channels_count);
2834        struct stmmac_rx_queue *rx_q;
2835        struct stmmac_tx_queue *tx_q;
2836        u32 chan = 0;
2837        int atds = 0;
2838        int ret = 0;
2839
2840        if (!priv->plat->dma_cfg || !priv->plat->dma_cfg->pbl) {
2841                dev_err(priv->device, "Invalid DMA configuration\n");
2842                return -EINVAL;
2843        }
2844
2845        if (priv->extend_desc && (priv->mode == STMMAC_RING_MODE))
2846                atds = 1;
2847
2848        ret = stmmac_reset(priv, priv->ioaddr);
2849        if (ret) {
2850                dev_err(priv->device, "Failed to reset the dma\n");
2851                return ret;
2852        }
2853
2854        /* DMA Configuration */
2855        stmmac_dma_init(priv, priv->ioaddr, priv->plat->dma_cfg, atds);
2856
2857        if (priv->plat->axi)
2858                stmmac_axi(priv, priv->ioaddr, priv->plat->axi);
2859
2860        /* DMA CSR Channel configuration */
2861        for (chan = 0; chan < dma_csr_ch; chan++)
2862                stmmac_init_chan(priv, priv->ioaddr, priv->plat->dma_cfg, chan);
2863
2864        /* DMA RX Channel Configuration */
2865        for (chan = 0; chan < rx_channels_count; chan++) {
2866                rx_q = &priv->rx_queue[chan];
2867
2868                stmmac_init_rx_chan(priv, priv->ioaddr, priv->plat->dma_cfg,
2869                                    rx_q->dma_rx_phy, chan);
2870
2871                rx_q->rx_tail_addr = rx_q->dma_rx_phy +
2872                                     (rx_q->buf_alloc_num *
2873                                      sizeof(struct dma_desc));
2874                stmmac_set_rx_tail_ptr(priv, priv->ioaddr,
2875                                       rx_q->rx_tail_addr, chan);
2876        }
2877
2878        /* DMA TX Channel Configuration */
2879        for (chan = 0; chan < tx_channels_count; chan++) {
2880                tx_q = &priv->tx_queue[chan];
2881
2882                stmmac_init_tx_chan(priv, priv->ioaddr, priv->plat->dma_cfg,
2883                                    tx_q->dma_tx_phy, chan);
2884
2885                tx_q->tx_tail_addr = tx_q->dma_tx_phy;
2886                stmmac_set_tx_tail_ptr(priv, priv->ioaddr,
2887                                       tx_q->tx_tail_addr, chan);
2888        }
2889
2890        return ret;
2891}
2892
2893static void stmmac_tx_timer_arm(struct stmmac_priv *priv, u32 queue)
2894{
2895        struct stmmac_tx_queue *tx_q = &priv->tx_queue[queue];
2896
2897        hrtimer_start(&tx_q->txtimer,
2898                      STMMAC_COAL_TIMER(priv->tx_coal_timer[queue]),
2899                      HRTIMER_MODE_REL);
2900}
2901
2902/**
2903 * stmmac_tx_timer - mitigation sw timer for tx.
2904 * @t: data pointer
2905 * Description:
2906 * This is the timer handler to directly invoke the stmmac_tx_clean.
2907 */
2908static enum hrtimer_restart stmmac_tx_timer(struct hrtimer *t)
2909{
2910        struct stmmac_tx_queue *tx_q = container_of(t, struct stmmac_tx_queue, txtimer);
2911        struct stmmac_priv *priv = tx_q->priv_data;
2912        struct stmmac_channel *ch;
2913        struct napi_struct *napi;
2914
2915        ch = &priv->channel[tx_q->queue_index];
2916        napi = tx_q->xsk_pool ? &ch->rxtx_napi : &ch->tx_napi;
2917
2918        if (likely(napi_schedule_prep(napi))) {
2919                unsigned long flags;
2920
2921                spin_lock_irqsave(&ch->lock, flags);
2922                stmmac_disable_dma_irq(priv, priv->ioaddr, ch->index, 0, 1);
2923                spin_unlock_irqrestore(&ch->lock, flags);
2924                __napi_schedule(napi);
2925        }
2926
2927        return HRTIMER_NORESTART;
2928}
2929
2930/**
2931 * stmmac_init_coalesce - init mitigation options.
2932 * @priv: driver private structure
2933 * Description:
2934 * This inits the coalesce parameters: i.e. timer rate,
2935 * timer handler and default threshold used for enabling the
2936 * interrupt on completion bit.
2937 */
2938static void stmmac_init_coalesce(struct stmmac_priv *priv)
2939{
2940        u32 tx_channel_count = priv->plat->tx_queues_to_use;
2941        u32 rx_channel_count = priv->plat->rx_queues_to_use;
2942        u32 chan;
2943
2944        for (chan = 0; chan < tx_channel_count; chan++) {
2945                struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan];
2946
2947                priv->tx_coal_frames[chan] = STMMAC_TX_FRAMES;
2948                priv->tx_coal_timer[chan] = STMMAC_COAL_TX_TIMER;
2949
2950                hrtimer_init(&tx_q->txtimer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
2951                tx_q->txtimer.function = stmmac_tx_timer;
2952        }
2953
2954        for (chan = 0; chan < rx_channel_count; chan++)
2955                priv->rx_coal_frames[chan] = STMMAC_RX_FRAMES;
2956}
2957
2958static void stmmac_set_rings_length(struct stmmac_priv *priv)
2959{
2960        u32 rx_channels_count = priv->plat->rx_queues_to_use;
2961        u32 tx_channels_count = priv->plat->tx_queues_to_use;
2962        u32 chan;
2963
2964        /* set TX ring length */
2965        for (chan = 0; chan < tx_channels_count; chan++)
2966                stmmac_set_tx_ring_len(priv, priv->ioaddr,
2967                                       (priv->dma_tx_size - 1), chan);
2968
2969        /* set RX ring length */
2970        for (chan = 0; chan < rx_channels_count; chan++)
2971                stmmac_set_rx_ring_len(priv, priv->ioaddr,
2972                                       (priv->dma_rx_size - 1), chan);
2973}
2974
2975/**
2976 *  stmmac_set_tx_queue_weight - Set TX queue weight
2977 *  @priv: driver private structure
2978 *  Description: It is used for setting TX queues weight
2979 */
2980static void stmmac_set_tx_queue_weight(struct stmmac_priv *priv)
2981{
2982        u32 tx_queues_count = priv->plat->tx_queues_to_use;
2983        u32 weight;
2984        u32 queue;
2985
2986        for (queue = 0; queue < tx_queues_count; queue++) {
2987                weight = priv->plat->tx_queues_cfg[queue].weight;
2988                stmmac_set_mtl_tx_queue_weight(priv, priv->hw, weight, queue);
2989        }
2990}
2991
2992/**
2993 *  stmmac_configure_cbs - Configure CBS in TX queue
2994 *  @priv: driver private structure
2995 *  Description: It is used for configuring CBS in AVB TX queues
2996 */
2997static void stmmac_configure_cbs(struct stmmac_priv *priv)
2998{
2999        u32 tx_queues_count = priv->plat->tx_queues_to_use;
3000        u32 mode_to_use;
3001        u32 queue;
3002
3003        /* queue 0 is reserved for legacy traffic */
3004        for (queue = 1; queue < tx_queues_count; queue++) {
3005                mode_to_use = priv->plat->tx_queues_cfg[queue].mode_to_use;
3006                if (mode_to_use == MTL_QUEUE_DCB)
3007                        continue;
3008
3009                stmmac_config_cbs(priv, priv->hw,
3010                                priv->plat->tx_queues_cfg[queue].send_slope,
3011                                priv->plat->tx_queues_cfg[queue].idle_slope,
3012                                priv->plat->tx_queues_cfg[queue].high_credit,
3013                                priv->plat->tx_queues_cfg[queue].low_credit,
3014                                queue);
3015        }
3016}
3017
3018/**
3019 *  stmmac_rx_queue_dma_chan_map - Map RX queue to RX dma channel
3020 *  @priv: driver private structure
3021 *  Description: It is used for mapping RX queues to RX dma channels
3022 */
3023static void stmmac_rx_queue_dma_chan_map(struct stmmac_priv *priv)
3024{
3025        u32 rx_queues_count = priv->plat->rx_queues_to_use;
3026        u32 queue;
3027        u32 chan;
3028
3029        for (queue = 0; queue < rx_queues_count; queue++) {
3030                chan = priv->plat->rx_queues_cfg[queue].chan;
3031                stmmac_map_mtl_to_dma(priv, priv->hw, queue, chan);
3032        }
3033}
3034
3035/**
3036 *  stmmac_mac_config_rx_queues_prio - Configure RX Queue priority
3037 *  @priv: driver private structure
3038 *  Description: It is used for configuring the RX Queue Priority
3039 */
3040static void stmmac_mac_config_rx_queues_prio(struct stmmac_priv *priv)
3041{
3042        u32 rx_queues_count = priv->plat->rx_queues_to_use;
3043        u32 queue;
3044        u32 prio;
3045
3046        for (queue = 0; queue < rx_queues_count; queue++) {
3047                if (!priv->plat->rx_queues_cfg[queue].use_prio)
3048                        continue;
3049
3050                prio = priv->plat->rx_queues_cfg[queue].prio;
3051                stmmac_rx_queue_prio(priv, priv->hw, prio, queue);
3052        }
3053}
3054
3055/**
3056 *  stmmac_mac_config_tx_queues_prio - Configure TX Queue priority
3057 *  @priv: driver private structure
3058 *  Description: It is used for configuring the TX Queue Priority
3059 */
3060static void stmmac_mac_config_tx_queues_prio(struct stmmac_priv *priv)
3061{
3062        u32 tx_queues_count = priv->plat->tx_queues_to_use;
3063        u32 queue;
3064        u32 prio;
3065
3066        for (queue = 0; queue < tx_queues_count; queue++) {
3067                if (!priv->plat->tx_queues_cfg[queue].use_prio)
3068                        continue;
3069
3070                prio = priv->plat->tx_queues_cfg[queue].prio;
3071                stmmac_tx_queue_prio(priv, priv->hw, prio, queue);
3072        }
3073}
3074
3075/**
3076 *  stmmac_mac_config_rx_queues_routing - Configure RX Queue Routing
3077 *  @priv: driver private structure
3078 *  Description: It is used for configuring the RX queue routing
3079 */
3080static void stmmac_mac_config_rx_queues_routing(struct stmmac_priv *priv)
3081{
3082        u32 rx_queues_count = priv->plat->rx_queues_to_use;
3083        u32 queue;
3084        u8 packet;
3085
3086        for (queue = 0; queue < rx_queues_count; queue++) {
3087                /* no specific packet type routing specified for the queue */
3088                if (priv->plat->rx_queues_cfg[queue].pkt_route == 0x0)
3089                        continue;
3090
3091                packet = priv->plat->rx_queues_cfg[queue].pkt_route;
3092                stmmac_rx_queue_routing(priv, priv->hw, packet, queue);
3093        }
3094}
3095
3096static void stmmac_mac_config_rss(struct stmmac_priv *priv)
3097{
3098        if (!priv->dma_cap.rssen || !priv->plat->rss_en) {
3099                priv->rss.enable = false;
3100                return;
3101        }
3102
3103        if (priv->dev->features & NETIF_F_RXHASH)
3104                priv->rss.enable = true;
3105        else
3106                priv->rss.enable = false;
3107
3108        stmmac_rss_configure(priv, priv->hw, &priv->rss,
3109                             priv->plat->rx_queues_to_use);
3110}
3111
3112/**
3113 *  stmmac_mtl_configuration - Configure MTL
3114 *  @priv: driver private structure
3115 *  Description: It is used for configurring MTL
3116 */
3117static void stmmac_mtl_configuration(struct stmmac_priv *priv)
3118{
3119        u32 rx_queues_count = priv->plat->rx_queues_to_use;
3120        u32 tx_queues_count = priv->plat->tx_queues_to_use;
3121
3122        if (tx_queues_count > 1)
3123                stmmac_set_tx_queue_weight(priv);
3124
3125        /* Configure MTL RX algorithms */
3126        if (rx_queues_count > 1)
3127                stmmac_prog_mtl_rx_algorithms(priv, priv->hw,
3128                                priv->plat->rx_sched_algorithm);
3129
3130        /* Configure MTL TX algorithms */
3131        if (tx_queues_count > 1)
3132                stmmac_prog_mtl_tx_algorithms(priv, priv->hw,
3133                                priv->plat->tx_sched_algorithm);
3134
3135        /* Configure CBS in AVB TX queues */
3136        if (tx_queues_count > 1)
3137                stmmac_configure_cbs(priv);
3138
3139        /* Map RX MTL to DMA channels */
3140        stmmac_rx_queue_dma_chan_map(priv);
3141
3142        /* Enable MAC RX Queues */
3143        stmmac_mac_enable_rx_queues(priv);
3144
3145        /* Set RX priorities */
3146        if (rx_queues_count > 1)
3147                stmmac_mac_config_rx_queues_prio(priv);
3148
3149        /* Set TX priorities */
3150        if (tx_queues_count > 1)
3151                stmmac_mac_config_tx_queues_prio(priv);
3152
3153        /* Set RX routing */
3154        if (rx_queues_count > 1)
3155                stmmac_mac_config_rx_queues_routing(priv);
3156
3157        /* Receive Side Scaling */
3158        if (rx_queues_count > 1)
3159                stmmac_mac_config_rss(priv);
3160}
3161
3162static void stmmac_safety_feat_configuration(struct stmmac_priv *priv)
3163{
3164        if (priv->dma_cap.asp) {
3165                netdev_info(priv->dev, "Enabling Safety Features\n");
3166                stmmac_safety_feat_config(priv, priv->ioaddr, priv->dma_cap.asp,
3167                                          priv->plat->safety_feat_cfg);
3168        } else {
3169                netdev_info(priv->dev, "No Safety Features support found\n");
3170        }
3171}
3172
3173static int stmmac_fpe_start_wq(struct stmmac_priv *priv)
3174{
3175        char *name;
3176
3177        clear_bit(__FPE_TASK_SCHED, &priv->fpe_task_state);
3178        clear_bit(__FPE_REMOVING,  &priv->fpe_task_state);
3179
3180        name = priv->wq_name;
3181        sprintf(name, "%s-fpe", priv->dev->name);
3182
3183        priv->fpe_wq = create_singlethread_workqueue(name);
3184        if (!priv->fpe_wq) {
3185                netdev_err(priv->dev, "%s: Failed to create workqueue\n", name);
3186
3187                return -ENOMEM;
3188        }
3189        netdev_info(priv->dev, "FPE workqueue start");
3190
3191        return 0;
3192}
3193
3194/**
3195 * stmmac_hw_setup - setup mac in a usable state.
3196 *  @dev : pointer to the device structure.
3197 *  @init_ptp: initialize PTP if set
3198 *  Description:
3199 *  this is the main function to setup the HW in a usable state because the
3200 *  dma engine is reset, the core registers are configured (e.g. AXI,
3201 *  Checksum features, timers). The DMA is ready to start receiving and
3202 *  transmitting.
3203 *  Return value:
3204 *  0 on success and an appropriate (-)ve integer as defined in errno.h
3205 *  file on failure.
3206 */
3207static int stmmac_hw_setup(struct net_device *dev, bool init_ptp)
3208{
3209        struct stmmac_priv *priv = netdev_priv(dev);
3210        u32 rx_cnt = priv->plat->rx_queues_to_use;
3211        u32 tx_cnt = priv->plat->tx_queues_to_use;
3212        bool sph_en;
3213        u32 chan;
3214        int ret;
3215
3216        /* DMA initialization and SW reset */
3217        ret = stmmac_init_dma_engine(priv);
3218        if (ret < 0) {
3219                netdev_err(priv->dev, "%s: DMA engine initialization failed\n",
3220                           __func__);
3221                return ret;
3222        }
3223
3224        /* Copy the MAC addr into the HW  */
3225        stmmac_set_umac_addr(priv, priv->hw, dev->dev_addr, 0);
3226
3227        /* PS and related bits will be programmed according to the speed */
3228        if (priv->hw->pcs) {
3229                int speed = priv->plat->mac_port_sel_speed;
3230
3231                if ((speed == SPEED_10) || (speed == SPEED_100) ||
3232                    (speed == SPEED_1000)) {
3233                        priv->hw->ps = speed;
3234                } else {
3235                        dev_warn(priv->device, "invalid port speed\n");
3236                        priv->hw->ps = 0;
3237                }
3238        }
3239
3240        /* Initialize the MAC Core */
3241        stmmac_core_init(priv, priv->hw, dev);
3242
3243        /* Initialize MTL*/
3244        stmmac_mtl_configuration(priv);
3245
3246        /* Initialize Safety Features */
3247        stmmac_safety_feat_configuration(priv);
3248
3249        ret = stmmac_rx_ipc(priv, priv->hw);
3250        if (!ret) {
3251                netdev_warn(priv->dev, "RX IPC Checksum Offload disabled\n");
3252                priv->plat->rx_coe = STMMAC_RX_COE_NONE;
3253                priv->hw->rx_csum = 0;
3254        }
3255
3256        /* Enable the MAC Rx/Tx */
3257        stmmac_mac_set(priv, priv->ioaddr, true);
3258
3259        /* Set the HW DMA mode and the COE */
3260        stmmac_dma_operation_mode(priv);
3261
3262        stmmac_mmc_setup(priv);
3263
3264        if (init_ptp) {
3265                ret = clk_prepare_enable(priv->plat->clk_ptp_ref);
3266                if (ret < 0)
3267                        netdev_warn(priv->dev, "failed to enable PTP reference clock: %d\n", ret);
3268
3269                ret = stmmac_init_ptp(priv);
3270                if (ret == -EOPNOTSUPP)
3271                        netdev_warn(priv->dev, "PTP not supported by HW\n");
3272                else if (ret)
3273                        netdev_warn(priv->dev, "PTP init failed\n");
3274        }
3275
3276        priv->eee_tw_timer = STMMAC_DEFAULT_TWT_LS;
3277
3278        /* Convert the timer from msec to usec */
3279        if (!priv->tx_lpi_timer)
3280                priv->tx_lpi_timer = eee_timer * 1000;
3281
3282        if (priv->use_riwt) {
3283                u32 queue;
3284
3285                for (queue = 0; queue < rx_cnt; queue++) {
3286                        if (!priv->rx_riwt[queue])
3287                                priv->rx_riwt[queue] = DEF_DMA_RIWT;
3288
3289                        stmmac_rx_watchdog(priv, priv->ioaddr,
3290                                           priv->rx_riwt[queue], queue);
3291                }
3292        }
3293
3294        if (priv->hw->pcs)
3295                stmmac_pcs_ctrl_ane(priv, priv->ioaddr, 1, priv->hw->ps, 0);
3296
3297        /* set TX and RX rings length */
3298        stmmac_set_rings_length(priv);
3299
3300        /* Enable TSO */
3301        if (priv->tso) {
3302                for (chan = 0; chan < tx_cnt; chan++) {
3303                        struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan];
3304
3305                        /* TSO and TBS cannot co-exist */
3306                        if (tx_q->tbs & STMMAC_TBS_AVAIL)
3307                                continue;
3308
3309                        stmmac_enable_tso(priv, priv->ioaddr, 1, chan);
3310                }
3311        }
3312
3313        /* Enable Split Header */
3314        sph_en = (priv->hw->rx_csum > 0) && priv->sph;
3315        for (chan = 0; chan < rx_cnt; chan++)
3316                stmmac_enable_sph(priv, priv->ioaddr, sph_en, chan);
3317
3318
3319        /* VLAN Tag Insertion */
3320        if (priv->dma_cap.vlins)
3321                stmmac_enable_vlan(priv, priv->hw, STMMAC_VLAN_INSERT);
3322
3323        /* TBS */
3324        for (chan = 0; chan < tx_cnt; chan++) {
3325                struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan];
3326                int enable = tx_q->tbs & STMMAC_TBS_AVAIL;
3327
3328                stmmac_enable_tbs(priv, priv->ioaddr, enable, chan);
3329        }
3330
3331        /* Configure real RX and TX queues */
3332        netif_set_real_num_rx_queues(dev, priv->plat->rx_queues_to_use);
3333        netif_set_real_num_tx_queues(dev, priv->plat->tx_queues_to_use);
3334
3335        /* Start the ball rolling... */
3336        stmmac_start_all_dma(priv);
3337
3338        if (priv->dma_cap.fpesel) {
3339                stmmac_fpe_start_wq(priv);
3340
3341                if (priv->plat->fpe_cfg->enable)
3342                        stmmac_fpe_handshake(priv, true);
3343        }
3344
3345        return 0;
3346}
3347
3348static void stmmac_hw_teardown(struct net_device *dev)
3349{
3350        struct stmmac_priv *priv = netdev_priv(dev);
3351
3352        clk_disable_unprepare(priv->plat->clk_ptp_ref);
3353}
3354
3355static void stmmac_free_irq(struct net_device *dev,
3356                            enum request_irq_err irq_err, int irq_idx)
3357{
3358        struct stmmac_priv *priv = netdev_priv(dev);
3359        int j;
3360
3361        switch (irq_err) {
3362        case REQ_IRQ_ERR_ALL:
3363                irq_idx = priv->plat->tx_queues_to_use;
3364                fallthrough;
3365        case REQ_IRQ_ERR_TX:
3366                for (j = irq_idx - 1; j >= 0; j--) {
3367                        if (priv->tx_irq[j] > 0) {
3368                                irq_set_affinity_hint(priv->tx_irq[j], NULL);
3369                                free_irq(priv->tx_irq[j], &priv->tx_queue[j]);
3370                        }
3371                }
3372                irq_idx = priv->plat->rx_queues_to_use;
3373                fallthrough;
3374        case REQ_IRQ_ERR_RX:
3375                for (j = irq_idx - 1; j >= 0; j--) {
3376                        if (priv->rx_irq[j] > 0) {
3377                                irq_set_affinity_hint(priv->rx_irq[j], NULL);
3378                                free_irq(priv->rx_irq[j], &priv->rx_queue[j]);
3379                        }
3380                }
3381
3382                if (priv->sfty_ue_irq > 0 && priv->sfty_ue_irq != dev->irq)
3383                        free_irq(priv->sfty_ue_irq, dev);
3384                fallthrough;
3385        case REQ_IRQ_ERR_SFTY_UE:
3386                if (priv->sfty_ce_irq > 0 && priv->sfty_ce_irq != dev->irq)
3387                        free_irq(priv->sfty_ce_irq, dev);
3388                fallthrough;
3389        case REQ_IRQ_ERR_SFTY_CE:
3390                if (priv->lpi_irq > 0 && priv->lpi_irq != dev->irq)
3391                        free_irq(priv->lpi_irq, dev);
3392                fallthrough;
3393        case REQ_IRQ_ERR_LPI:
3394                if (priv->wol_irq > 0 && priv->wol_irq != dev->irq)
3395                        free_irq(priv->wol_irq, dev);
3396                fallthrough;
3397        case REQ_IRQ_ERR_WOL:
3398                free_irq(dev->irq, dev);
3399                fallthrough;
3400        case REQ_IRQ_ERR_MAC:
3401        case REQ_IRQ_ERR_NO:
3402                /* If MAC IRQ request error, no more IRQ to free */
3403                break;
3404        }
3405}
3406
3407static int stmmac_request_irq_multi_msi(struct net_device *dev)
3408{
3409        struct stmmac_priv *priv = netdev_priv(dev);
3410        enum request_irq_err irq_err;
3411        cpumask_t cpu_mask;
3412        int irq_idx = 0;
3413        char *int_name;
3414        int ret;
3415        int i;
3416
3417        /* For common interrupt */
3418        int_name = priv->int_name_mac;
3419        sprintf(int_name, "%s:%s", dev->name, "mac");
3420        ret = request_irq(dev->irq, stmmac_mac_interrupt,
3421                          0, int_name, dev);
3422        if (unlikely(ret < 0)) {
3423                netdev_err(priv->dev,
3424                           "%s: alloc mac MSI %d (error: %d)\n",
3425                           __func__, dev->irq, ret);
3426                irq_err = REQ_IRQ_ERR_MAC;
3427                goto irq_error;
3428        }
3429
3430        /* Request the Wake IRQ in case of another line
3431         * is used for WoL
3432         */
3433        if (priv->wol_irq > 0 && priv->wol_irq != dev->irq) {
3434                int_name = priv->int_name_wol;
3435                sprintf(int_name, "%s:%s", dev->name, "wol");
3436                ret = request_irq(priv->wol_irq,
3437                                  stmmac_mac_interrupt,
3438                                  0, int_name, dev);
3439                if (unlikely(ret < 0)) {
3440                        netdev_err(priv->dev,
3441                                   "%s: alloc wol MSI %d (error: %d)\n",
3442                                   __func__, priv->wol_irq, ret);
3443                        irq_err = REQ_IRQ_ERR_WOL;
3444                        goto irq_error;
3445                }
3446        }
3447
3448        /* Request the LPI IRQ in case of another line
3449         * is used for LPI
3450         */
3451        if (priv->lpi_irq > 0 && priv->lpi_irq != dev->irq) {
3452                int_name = priv->int_name_lpi;
3453                sprintf(int_name, "%s:%s", dev->name, "lpi");
3454                ret = request_irq(priv->lpi_irq,
3455                                  stmmac_mac_interrupt,
3456                                  0, int_name, dev);
3457                if (unlikely(ret < 0)) {
3458                        netdev_err(priv->dev,
3459                                   "%s: alloc lpi MSI %d (error: %d)\n",
3460                                   __func__, priv->lpi_irq, ret);
3461                        irq_err = REQ_IRQ_ERR_LPI;
3462                        goto irq_error;
3463                }
3464        }
3465
3466        /* Request the Safety Feature Correctible Error line in
3467         * case of another line is used
3468         */
3469        if (priv->sfty_ce_irq > 0 && priv->sfty_ce_irq != dev->irq) {
3470                int_name = priv->int_name_sfty_ce;
3471                sprintf(int_name, "%s:%s", dev->name, "safety-ce");
3472                ret = request_irq(priv->sfty_ce_irq,
3473                                  stmmac_safety_interrupt,
3474                                  0, int_name, dev);
3475                if (unlikely(ret < 0)) {
3476                        netdev_err(priv->dev,
3477                                   "%s: alloc sfty ce MSI %d (error: %d)\n",
3478                                   __func__, priv->sfty_ce_irq, ret);
3479                        irq_err = REQ_IRQ_ERR_SFTY_CE;
3480                        goto irq_error;
3481                }
3482        }
3483
3484        /* Request the Safety Feature Uncorrectible Error line in
3485         * case of another line is used
3486         */
3487        if (priv->sfty_ue_irq > 0 && priv->sfty_ue_irq != dev->irq) {
3488                int_name = priv->int_name_sfty_ue;
3489                sprintf(int_name, "%s:%s", dev->name, "safety-ue");
3490                ret = request_irq(priv->sfty_ue_irq,
3491                                  stmmac_safety_interrupt,
3492                                  0, int_name, dev);
3493                if (unlikely(ret < 0)) {
3494                        netdev_err(priv->dev,
3495                                   "%s: alloc sfty ue MSI %d (error: %d)\n",
3496                                   __func__, priv->sfty_ue_irq, ret);
3497                        irq_err = REQ_IRQ_ERR_SFTY_UE;
3498                        goto irq_error;
3499                }
3500        }
3501
3502        /* Request Rx MSI irq */
3503        for (i = 0; i < priv->plat->rx_queues_to_use; i++) {
3504                if (priv->rx_irq[i] == 0)
3505                        continue;
3506
3507                int_name = priv->int_name_rx_irq[i];
3508                sprintf(int_name, "%s:%s-%d", dev->name, "rx", i);
3509                ret = request_irq(priv->rx_irq[i],
3510                                  stmmac_msi_intr_rx,
3511                                  0, int_name, &priv->rx_queue[i]);
3512                if (unlikely(ret < 0)) {
3513                        netdev_err(priv->dev,
3514                                   "%s: alloc rx-%d  MSI %d (error: %d)\n",
3515                                   __func__, i, priv->rx_irq[i], ret);
3516                        irq_err = REQ_IRQ_ERR_RX;
3517                        irq_idx = i;
3518                        goto irq_error;
3519                }
3520                cpumask_clear(&cpu_mask);
3521                cpumask_set_cpu(i % num_online_cpus(), &cpu_mask);
3522                irq_set_affinity_hint(priv->rx_irq[i], &cpu_mask);
3523        }
3524
3525        /* Request Tx MSI irq */
3526        for (i = 0; i < priv->plat->tx_queues_to_use; i++) {
3527                if (priv->tx_irq[i] == 0)
3528                        continue;
3529
3530                int_name = priv->int_name_tx_irq[i];
3531                sprintf(int_name, "%s:%s-%d", dev->name, "tx", i);
3532                ret = request_irq(priv->tx_irq[i],
3533                                  stmmac_msi_intr_tx,
3534                                  0, int_name, &priv->tx_queue[i]);
3535                if (unlikely(ret < 0)) {
3536                        netdev_err(priv->dev,
3537                                   "%s: alloc tx-%d  MSI %d (error: %d)\n",
3538                                   __func__, i, priv->tx_irq[i], ret);
3539                        irq_err = REQ_IRQ_ERR_TX;
3540                        irq_idx = i;
3541                        goto irq_error;
3542                }
3543                cpumask_clear(&cpu_mask);
3544                cpumask_set_cpu(i % num_online_cpus(), &cpu_mask);
3545                irq_set_affinity_hint(priv->tx_irq[i], &cpu_mask);
3546        }
3547
3548        return 0;
3549
3550irq_error:
3551        stmmac_free_irq(dev, irq_err, irq_idx);
3552        return ret;
3553}
3554
3555static int stmmac_request_irq_single(struct net_device *dev)
3556{
3557        struct stmmac_priv *priv = netdev_priv(dev);
3558        enum request_irq_err irq_err;
3559        int ret;
3560
3561        ret = request_irq(dev->irq, stmmac_interrupt,
3562                          IRQF_SHARED, dev->name, dev);
3563        if (unlikely(ret < 0)) {
3564                netdev_err(priv->dev,
3565                           "%s: ERROR: allocating the IRQ %d (error: %d)\n",
3566                           __func__, dev->irq, ret);
3567                irq_err = REQ_IRQ_ERR_MAC;
3568                goto irq_error;
3569        }
3570
3571        /* Request the Wake IRQ in case of another line
3572         * is used for WoL
3573         */
3574        if (priv->wol_irq > 0 && priv->wol_irq != dev->irq) {
3575                ret = request_irq(priv->wol_irq, stmmac_interrupt,
3576                                  IRQF_SHARED, dev->name, dev);
3577                if (unlikely(ret < 0)) {
3578                        netdev_err(priv->dev,
3579                                   "%s: ERROR: allocating the WoL IRQ %d (%d)\n",
3580                                   __func__, priv->wol_irq, ret);
3581                        irq_err = REQ_IRQ_ERR_WOL;
3582                        goto irq_error;
3583                }
3584        }
3585
3586        /* Request the IRQ lines */
3587        if (priv->lpi_irq > 0 && priv->lpi_irq != dev->irq) {
3588                ret = request_irq(priv->lpi_irq, stmmac_interrupt,
3589                                  IRQF_SHARED, dev->name, dev);
3590                if (unlikely(ret < 0)) {
3591                        netdev_err(priv->dev,
3592                                   "%s: ERROR: allocating the LPI IRQ %d (%d)\n",
3593                                   __func__, priv->lpi_irq, ret);
3594                        irq_err = REQ_IRQ_ERR_LPI;
3595                        goto irq_error;
3596                }
3597        }
3598
3599        return 0;
3600
3601irq_error:
3602        stmmac_free_irq(dev, irq_err, 0);
3603        return ret;
3604}
3605
3606static int stmmac_request_irq(struct net_device *dev)
3607{
3608        struct stmmac_priv *priv = netdev_priv(dev);
3609        int ret;
3610
3611        /* Request the IRQ lines */
3612        if (priv->plat->multi_msi_en)
3613                ret = stmmac_request_irq_multi_msi(dev);
3614        else
3615                ret = stmmac_request_irq_single(dev);
3616
3617        return ret;
3618}
3619
3620/**
3621 *  stmmac_open - open entry point of the driver
3622 *  @dev : pointer to the device structure.
3623 *  Description:
3624 *  This function is the open entry point of the driver.
3625 *  Return value:
3626 *  0 on success and an appropriate (-)ve integer as defined in errno.h
3627 *  file on failure.
3628 */
3629int stmmac_open(struct net_device *dev)
3630{
3631        struct stmmac_priv *priv = netdev_priv(dev);
3632        int mode = priv->plat->phy_interface;
3633        int bfsize = 0;
3634        u32 chan;
3635        int ret;
3636
3637        ret = pm_runtime_get_sync(priv->device);
3638        if (ret < 0) {
3639                pm_runtime_put_noidle(priv->device);
3640                return ret;
3641        }
3642
3643        if (priv->hw->pcs != STMMAC_PCS_TBI &&
3644            priv->hw->pcs != STMMAC_PCS_RTBI &&
3645            (!priv->hw->xpcs ||
3646             xpcs_get_an_mode(priv->hw->xpcs, mode) != DW_AN_C73)) {
3647                ret = stmmac_init_phy(dev);
3648                if (ret) {
3649                        netdev_err(priv->dev,
3650                                   "%s: Cannot attach to PHY (error: %d)\n",
3651                                   __func__, ret);
3652                        goto init_phy_error;
3653                }
3654        }
3655
3656        /* Extra statistics */
3657        memset(&priv->xstats, 0, sizeof(struct stmmac_extra_stats));
3658        priv->xstats.threshold = tc;
3659
3660        bfsize = stmmac_set_16kib_bfsize(priv, dev->mtu);
3661        if (bfsize < 0)
3662                bfsize = 0;
3663
3664        if (bfsize < BUF_SIZE_16KiB)
3665                bfsize = stmmac_set_bfsize(dev->mtu, priv->dma_buf_sz);
3666
3667        priv->dma_buf_sz = bfsize;
3668        buf_sz = bfsize;
3669
3670        priv->rx_copybreak = STMMAC_RX_COPYBREAK;
3671
3672        if (!priv->dma_tx_size)
3673                priv->dma_tx_size = DMA_DEFAULT_TX_SIZE;
3674        if (!priv->dma_rx_size)
3675                priv->dma_rx_size = DMA_DEFAULT_RX_SIZE;
3676
3677        /* Earlier check for TBS */
3678        for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++) {
3679                struct stmmac_tx_queue *tx_q = &priv->tx_queue[chan];
3680                int tbs_en = priv->plat->tx_queues_cfg[chan].tbs_en;
3681
3682                /* Setup per-TXQ tbs flag before TX descriptor alloc */
3683                tx_q->tbs |= tbs_en ? STMMAC_TBS_AVAIL : 0;
3684        }
3685
3686        ret = alloc_dma_desc_resources(priv);
3687        if (ret < 0) {
3688                netdev_err(priv->dev, "%s: DMA descriptors allocation failed\n",
3689                           __func__);
3690                goto dma_desc_error;
3691        }
3692
3693        ret = init_dma_desc_rings(dev, GFP_KERNEL);
3694        if (ret < 0) {
3695                netdev_err(priv->dev, "%s: DMA descriptors initialization failed\n",
3696                           __func__);
3697                goto init_error;
3698        }
3699
3700        ret = stmmac_hw_setup(dev, true);
3701        if (ret < 0) {
3702                netdev_err(priv->dev, "%s: Hw setup failed\n", __func__);
3703                goto init_error;
3704        }
3705
3706        stmmac_init_coalesce(priv);
3707
3708        phylink_start(priv->phylink);
3709        /* We may have called phylink_speed_down before */
3710        phylink_speed_up(priv->phylink);
3711
3712        ret = stmmac_request_irq(dev);
3713        if (ret)
3714                goto irq_error;
3715
3716        stmmac_enable_all_queues(priv);
3717        netif_tx_start_all_queues(priv->dev);
3718
3719        return 0;
3720
3721irq_error:
3722        phylink_stop(priv->phylink);
3723
3724        for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++)
3725                hrtimer_cancel(&priv->tx_queue[chan].txtimer);
3726
3727        stmmac_hw_teardown(dev);
3728init_error:
3729        free_dma_desc_resources(priv);
3730dma_desc_error:
3731        phylink_disconnect_phy(priv->phylink);
3732init_phy_error:
3733        pm_runtime_put(priv->device);
3734        return ret;
3735}
3736
3737static void stmmac_fpe_stop_wq(struct stmmac_priv *priv)
3738{
3739        set_bit(__FPE_REMOVING, &priv->fpe_task_state);
3740
3741        if (priv->fpe_wq)
3742                destroy_workqueue(priv->fpe_wq);
3743
3744        netdev_info(priv->dev, "FPE workqueue stop");
3745}
3746
3747/**
3748 *  stmmac_release - close entry point of the driver
3749 *  @dev : device pointer.
3750 *  Description:
3751 *  This is the stop entry point of the driver.
3752 */
3753int stmmac_release(struct net_device *dev)
3754{
3755        struct stmmac_priv *priv = netdev_priv(dev);
3756        u32 chan;
3757
3758        if (device_may_wakeup(priv->device))
3759                phylink_speed_down(priv->phylink, false);
3760        /* Stop and disconnect the PHY */
3761        phylink_stop(priv->phylink);
3762        phylink_disconnect_phy(priv->phylink);
3763
3764        stmmac_disable_all_queues(priv);
3765
3766        for (chan = 0; chan < priv->plat->tx_queues_to_use; chan++)
3767                hrtimer_cancel(&priv->tx_queue[chan].txtimer);
3768
3769        /* Free the IRQ lines */
3770        stmmac_free_irq(dev, REQ_IRQ_ERR_ALL, 0);
3771
3772        if (priv->eee_enabled) {
3773                priv->tx_path_in_lpi_mode = false;
3774                del_timer_sync(&priv->eee_ctrl_timer);
3775        }
3776
3777        /* Stop TX/RX DMA and clear the descriptors */
3778        stmmac_stop_all_dma(priv);
3779
3780        /* Release and free the Rx/Tx resources */
3781        free_dma_desc_resources(priv);
3782
3783        /* Disable the MAC Rx/Tx */
3784        stmmac_mac_set(priv, priv->ioaddr, false);
3785
3786        netif_carrier_off(dev);
3787
3788        stmmac_release_ptp(priv);
3789
3790        pm_runtime_put(priv->device);
3791
3792        if (priv->dma_cap.fpesel)
3793                stmmac_fpe_stop_wq(priv);
3794
3795        return 0;
3796}
3797
3798static bool stmmac_vlan_insert(struct stmmac_priv *priv, struct sk_buff *skb,
3799                               struct stmmac_tx_queue *tx_q)
3800{
3801        u16 tag = 0x0, inner_tag = 0x0;
3802        u32 inner_type = 0x0;
3803        struct dma_desc *p;
3804
3805        if (!priv->dma_cap.vlins)
3806                return false;
3807        if (!skb_vlan_tag_present(skb))
3808                return false;
3809        if (skb->vlan_proto == htons(ETH_P_8021AD)) {
3810                inner_tag = skb_vlan_tag_get(skb);
3811                inner_type = STMMAC_VLAN_INSERT;
3812        }
3813
3814        tag = skb_vlan_tag_get(skb);
3815
3816        if (tx_q->tbs & STMMAC_TBS_AVAIL)
3817                p = &tx_q->dma_entx[tx_q->cur_tx].basic;
3818        else
3819                p = &tx_q->dma_tx[tx_q->cur_tx];
3820