drivers/net/ethernet/engleder/tsnep_main.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
   3
   4 /* TSN endpoint Ethernet MAC driver
   5  *
   6  * The TSN endpoint Ethernet MAC is a FPGA based network device for real-time
   7  * communication. It is designed for endpoints within TSN (Time Sensitive
   8  * Networking) networks; e.g., for PLCs in the industrial automation case.
   9  *
  10  * It supports multiple TX/RX queue pairs. The first TX/RX queue pair is used
  11  * by the driver.
  12  *
  13  * More information can be found here:
  14  * - www.embedded-experts.at/tsn
  15  * - www.engleder-embedded.com
  16  */
  17
  18 #include "tsnep.h"
  19 #include "tsnep_hw.h"
  20
  21 #include <linux/module.h>
  22 #include <linux/of.h>
  23 #include <linux/of_net.h>
  24 #include <linux/of_mdio.h>
  25 #include <linux/interrupt.h>
  26 #include <linux/etherdevice.h>
  27 #include <linux/phy.h>
  28 #include <linux/iopoll.h>
  29 #include <linux/bpf.h>
  30 #include <linux/bpf_trace.h>
  31 #include <net/page_pool/helpers.h>
  32 #include <net/xdp_sock_drv.h>
  33
  34 #define TSNEP_RX_OFFSET (max(NET_SKB_PAD, XDP_PACKET_HEADROOM) + NET_IP_ALIGN)
  35 #define TSNEP_HEADROOM ALIGN(TSNEP_RX_OFFSET, 4)
  36 #define TSNEP_MAX_RX_BUF_SIZE (PAGE_SIZE - TSNEP_HEADROOM - \
  37                                SKB_DATA_ALIGN(sizeof(struct skb_shared_info)))
  38 /* XSK buffer shall store at least Q-in-Q frame */
  39 #define TSNEP_XSK_RX_BUF_SIZE (ALIGN(TSNEP_RX_INLINE_METADATA_SIZE + \
  40                                      ETH_FRAME_LEN + ETH_FCS_LEN + \
  41                                      VLAN_HLEN * 2, 4))
  42
  43 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
  44 #define DMA_ADDR_HIGH(dma_addr) ((u32)(((dma_addr) >> 32) & 0xFFFFFFFF))
  45 #else
  46 #define DMA_ADDR_HIGH(dma_addr) ((u32)(0))
  47 #endif
  48 #define DMA_ADDR_LOW(dma_addr) ((u32)((dma_addr) & 0xFFFFFFFF))
  49
  50 #define TSNEP_COALESCE_USECS_DEFAULT 64
  51 #define TSNEP_COALESCE_USECS_MAX     ((ECM_INT_DELAY_MASK >> ECM_INT_DELAY_SHIFT) * \
  52                                       ECM_INT_DELAY_BASE_US + ECM_INT_DELAY_BASE_US - 1)
  53
  54 #define TSNEP_TX_TYPE_SKB       BIT(0)
  55 #define TSNEP_TX_TYPE_SKB_FRAG  BIT(1)
  56 #define TSNEP_TX_TYPE_XDP_TX    BIT(2)
  57 #define TSNEP_TX_TYPE_XDP_NDO   BIT(3)
  58 #define TSNEP_TX_TYPE_XDP       (TSNEP_TX_TYPE_XDP_TX | TSNEP_TX_TYPE_XDP_NDO)
  59 #define TSNEP_TX_TYPE_XSK       BIT(4)
  60
  61 #define TSNEP_XDP_TX            BIT(0)
  62 #define TSNEP_XDP_REDIRECT      BIT(1)
  63
  64 static void tsnep_enable_irq(struct tsnep_adapter *adapter, u32 mask)
  65 {
  66         iowrite32(mask, adapter->addr + ECM_INT_ENABLE);
  67 }
  68
  69 static void tsnep_disable_irq(struct tsnep_adapter *adapter, u32 mask)
  70 {
  71         mask |= ECM_INT_DISABLE;
  72         iowrite32(mask, adapter->addr + ECM_INT_ENABLE);
  73 }
  74
  75 static irqreturn_t tsnep_irq(int irq, void *arg)
  76 {
  77         struct tsnep_adapter *adapter = arg;
  78         u32 active = ioread32(adapter->addr + ECM_INT_ACTIVE);
  79
  80         /* acknowledge interrupt */
  81         if (active != 0)
  82                 iowrite32(active, adapter->addr + ECM_INT_ACKNOWLEDGE);
  83
  84         /* handle link interrupt */
  85         if ((active & ECM_INT_LINK) != 0)
  86                 phy_mac_interrupt(adapter->netdev->phydev);
  87
  88         /* handle TX/RX queue 0 interrupt */
  89         if ((active & adapter->queue[0].irq_mask) != 0) {
  90                 tsnep_disable_irq(adapter, adapter->queue[0].irq_mask);
  91                 napi_schedule(&adapter->queue[0].napi);
  92         }
  93
  94         return IRQ_HANDLED;
  95 }
  96
  97 static irqreturn_t tsnep_irq_txrx(int irq, void *arg)
  98 {
  99         struct tsnep_queue *queue = arg;
 100
 101         /* handle TX/RX queue interrupt */
 102         tsnep_disable_irq(queue->adapter, queue->irq_mask);
 103         napi_schedule(&queue->napi);
 104
 105         return IRQ_HANDLED;
 106 }
 107
 108 int tsnep_set_irq_coalesce(struct tsnep_queue *queue, u32 usecs)
 109 {
 110         if (usecs > TSNEP_COALESCE_USECS_MAX)
 111                 return -ERANGE;
 112
 113         usecs /= ECM_INT_DELAY_BASE_US;
 114         usecs <<= ECM_INT_DELAY_SHIFT;
 115         usecs &= ECM_INT_DELAY_MASK;
 116
 117         queue->irq_delay &= ~ECM_INT_DELAY_MASK;
 118         queue->irq_delay |= usecs;
 119         iowrite8(queue->irq_delay, queue->irq_delay_addr);
 120
 121         return 0;
 122 }
 123
 124 u32 tsnep_get_irq_coalesce(struct tsnep_queue *queue)
 125 {
 126         u32 usecs;
 127
 128         usecs = (queue->irq_delay & ECM_INT_DELAY_MASK);
 129         usecs >>= ECM_INT_DELAY_SHIFT;
 130         usecs *= ECM_INT_DELAY_BASE_US;
 131
 132         return usecs;
 133 }
 134
 135 static int tsnep_mdiobus_read(struct mii_bus *bus, int addr, int regnum)
 136 {
 137         struct tsnep_adapter *adapter = bus->priv;
 138         u32 md;
 139         int retval;
 140
 141         md = ECM_MD_READ;
 142         if (!adapter->suppress_preamble)
 143                 md |= ECM_MD_PREAMBLE;
 144         md |= (regnum << ECM_MD_ADDR_SHIFT) & ECM_MD_ADDR_MASK;
 145         md |= (addr << ECM_MD_PHY_ADDR_SHIFT) & ECM_MD_PHY_ADDR_MASK;
 146         iowrite32(md, adapter->addr + ECM_MD_CONTROL);
 147         retval = readl_poll_timeout_atomic(adapter->addr + ECM_MD_STATUS, md,
 148                                            !(md & ECM_MD_BUSY), 16, 1000);
 149         if (retval != 0)
 150                 return retval;
 151
 152         return (md & ECM_MD_DATA_MASK) >> ECM_MD_DATA_SHIFT;
 153 }
 154
 155 static int tsnep_mdiobus_write(struct mii_bus *bus, int addr, int regnum,
 156                                u16 val)
 157 {
 158         struct tsnep_adapter *adapter = bus->priv;
 159         u32 md;
 160         int retval;
 161
 162         md = ECM_MD_WRITE;
 163         if (!adapter->suppress_preamble)
 164                 md |= ECM_MD_PREAMBLE;
 165         md |= (regnum << ECM_MD_ADDR_SHIFT) & ECM_MD_ADDR_MASK;
 166         md |= (addr << ECM_MD_PHY_ADDR_SHIFT) & ECM_MD_PHY_ADDR_MASK;
 167         md |= ((u32)val << ECM_MD_DATA_SHIFT) & ECM_MD_DATA_MASK;
 168         iowrite32(md, adapter->addr + ECM_MD_CONTROL);
 169         retval = readl_poll_timeout_atomic(adapter->addr + ECM_MD_STATUS, md,
 170                                            !(md & ECM_MD_BUSY), 16, 1000);
 171         if (retval != 0)
 172                 return retval;
 173
 174         return 0;
 175 }
 176
 177 static void tsnep_set_link_mode(struct tsnep_adapter *adapter)
 178 {
 179         u32 mode;
 180
 181         switch (adapter->phydev->speed) {
 182         case SPEED_100:
 183                 mode = ECM_LINK_MODE_100;
 184                 break;
 185         case SPEED_1000:
 186                 mode = ECM_LINK_MODE_1000;
 187                 break;
 188         default:
 189                 mode = ECM_LINK_MODE_OFF;
 190                 break;
 191         }
 192         iowrite32(mode, adapter->addr + ECM_STATUS);
 193 }
 194
 195 static void tsnep_phy_link_status_change(struct net_device *netdev)
 196 {
 197         struct tsnep_adapter *adapter = netdev_priv(netdev);
 198         struct phy_device *phydev = netdev->phydev;
 199
 200         if (phydev->link)
 201                 tsnep_set_link_mode(adapter);
 202
 203         phy_print_status(netdev->phydev);
 204 }
 205
 206 static int tsnep_phy_loopback(struct tsnep_adapter *adapter, bool enable)
 207 {
 208         int retval;
 209
 210         retval = phy_loopback(adapter->phydev, enable);
 211
 212         /* PHY link state change is not signaled if loopback is enabled, it
 213          * would delay a working loopback anyway, let's ensure that loopback
 214          * is working immediately by setting link mode directly
 215          */
 216         if (!retval && enable)
 217                 tsnep_set_link_mode(adapter);
 218
 219         return retval;
 220 }
 221
 222 static int tsnep_phy_open(struct tsnep_adapter *adapter)
 223 {
 224         struct phy_device *phydev;
 225         struct ethtool_eee ethtool_eee;
 226         int retval;
 227
 228         retval = phy_connect_direct(adapter->netdev, adapter->phydev,
 229                                     tsnep_phy_link_status_change,
 230                                     adapter->phy_mode);
 231         if (retval)
 232                 return retval;
 233         phydev = adapter->netdev->phydev;
 234
 235         /* MAC supports only 100Mbps|1000Mbps full duplex
 236          * SPE (Single Pair Ethernet) is also an option but not implemented yet
 237          */
 238         phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_10baseT_Half_BIT);
 239         phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_10baseT_Full_BIT);
 240         phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_100baseT_Half_BIT);
 241         phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_1000baseT_Half_BIT);
 242
 243         /* disable EEE autoneg, EEE not supported by TSNEP */
 244         memset(&ethtool_eee, 0, sizeof(ethtool_eee));
 245         phy_ethtool_set_eee(adapter->phydev, &ethtool_eee);
 246
 247         adapter->phydev->irq = PHY_MAC_INTERRUPT;
 248         phy_start(adapter->phydev);
 249
 250         return 0;
 251 }
 252
 253 static void tsnep_phy_close(struct tsnep_adapter *adapter)
 254 {
 255         phy_stop(adapter->netdev->phydev);
 256         phy_disconnect(adapter->netdev->phydev);
 257 }
 258
 259 static void tsnep_tx_ring_cleanup(struct tsnep_tx *tx)
 260 {
 261         struct device *dmadev = tx->adapter->dmadev;
 262         int i;
 263
 264         memset(tx->entry, 0, sizeof(tx->entry));
 265
 266         for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
 267                 if (tx->page[i]) {
 268                         dma_free_coherent(dmadev, PAGE_SIZE, tx->page[i],
 269                                           tx->page_dma[i]);
 270                         tx->page[i] = NULL;
 271                         tx->page_dma[i] = 0;
 272                 }
 273         }
 274 }
 275
 276 static int tsnep_tx_ring_create(struct tsnep_tx *tx)
 277 {
 278         struct device *dmadev = tx->adapter->dmadev;
 279         struct tsnep_tx_entry *entry;
 280         struct tsnep_tx_entry *next_entry;
 281         int i, j;
 282         int retval;
 283
 284         for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
 285                 tx->page[i] =
 286                         dma_alloc_coherent(dmadev, PAGE_SIZE, &tx->page_dma[i],
 287                                            GFP_KERNEL);
 288                 if (!tx->page[i]) {
 289                         retval = -ENOMEM;
 290                         goto alloc_failed;
 291                 }
 292                 for (j = 0; j < TSNEP_RING_ENTRIES_PER_PAGE; j++) {
 293                         entry = &tx->entry[TSNEP_RING_ENTRIES_PER_PAGE * i + j];
 294                         entry->desc_wb = (struct tsnep_tx_desc_wb *)
 295                                 (((u8 *)tx->page[i]) + TSNEP_DESC_SIZE * j);
 296                         entry->desc = (struct tsnep_tx_desc *)
 297                                 (((u8 *)entry->desc_wb) + TSNEP_DESC_OFFSET);
 298                         entry->desc_dma = tx->page_dma[i] + TSNEP_DESC_SIZE * j;
 299                         entry->owner_user_flag = false;
 300                 }
 301         }
 302         for (i = 0; i < TSNEP_RING_SIZE; i++) {
 303                 entry = &tx->entry[i];
 304                 next_entry = &tx->entry[(i + 1) & TSNEP_RING_MASK];
 305                 entry->desc->next = __cpu_to_le64(next_entry->desc_dma);
 306         }
 307
 308         return 0;
 309
 310 alloc_failed:
 311         tsnep_tx_ring_cleanup(tx);
 312         return retval;
 313 }
 314
 315 static void tsnep_tx_init(struct tsnep_tx *tx)
 316 {
 317         dma_addr_t dma;
 318
 319         dma = tx->entry[0].desc_dma | TSNEP_RESET_OWNER_COUNTER;
 320         iowrite32(DMA_ADDR_LOW(dma), tx->addr + TSNEP_TX_DESC_ADDR_LOW);
 321         iowrite32(DMA_ADDR_HIGH(dma), tx->addr + TSNEP_TX_DESC_ADDR_HIGH);
 322         tx->write = 0;
 323         tx->read = 0;
 324         tx->owner_counter = 1;
 325         tx->increment_owner_counter = TSNEP_RING_SIZE - 1;
 326 }
 327
 328 static void tsnep_tx_enable(struct tsnep_tx *tx)
 329 {
 330         struct netdev_queue *nq;
 331
 332         nq = netdev_get_tx_queue(tx->adapter->netdev, tx->queue_index);
 333
 334         __netif_tx_lock_bh(nq);
 335         netif_tx_wake_queue(nq);
 336         __netif_tx_unlock_bh(nq);
 337 }
 338
 339 static void tsnep_tx_disable(struct tsnep_tx *tx, struct napi_struct *napi)
 340 {
 341         struct netdev_queue *nq;
 342         u32 val;
 343
 344         nq = netdev_get_tx_queue(tx->adapter->netdev, tx->queue_index);
 345
 346         __netif_tx_lock_bh(nq);
 347         netif_tx_stop_queue(nq);
 348         __netif_tx_unlock_bh(nq);
 349
 350         /* wait until TX is done in hardware */
 351         readx_poll_timeout(ioread32, tx->addr + TSNEP_CONTROL, val,
 352                            ((val & TSNEP_CONTROL_TX_ENABLE) == 0), 10000,
 353                            1000000);
 354
 355         /* wait until TX is also done in software */
 356         while (READ_ONCE(tx->read) != tx->write) {
 357                 napi_schedule(napi);
 358                 napi_synchronize(napi);
 359         }
 360 }
 361
 362 static void tsnep_tx_activate(struct tsnep_tx *tx, int index, int length,
 363                               bool last)
 364 {
 365         struct tsnep_tx_entry *entry = &tx->entry[index];
 366
 367         entry->properties = 0;
 368         /* xdpf and zc are union with skb */
 369         if (entry->skb) {
 370                 entry->properties = length & TSNEP_DESC_LENGTH_MASK;
 371                 entry->properties |= TSNEP_DESC_INTERRUPT_FLAG;
 372                 if ((entry->type & TSNEP_TX_TYPE_SKB) &&
 373                     (skb_shinfo(entry->skb)->tx_flags & SKBTX_IN_PROGRESS))
 374                         entry->properties |= TSNEP_DESC_EXTENDED_WRITEBACK_FLAG;
 375
 376                 /* toggle user flag to prevent false acknowledge
 377                  *
 378                  * Only the first fragment is acknowledged. For all other
 379                  * fragments no acknowledge is done and the last written owner
 380                  * counter stays in the writeback descriptor. Therefore, it is
 381                  * possible that the last written owner counter is identical to
 382                  * the new incremented owner counter and a false acknowledge is
 383                  * detected before the real acknowledge has been done by
 384                  * hardware.
 385                  *
 386                  * The user flag is used to prevent this situation. The user
 387                  * flag is copied to the writeback descriptor by the hardware
 388                  * and is used as additional acknowledge data. By toggeling the
 389                  * user flag only for the first fragment (which is
 390                  * acknowledged), it is guaranteed that the last acknowledge
 391                  * done for this descriptor has used a different user flag and
 392                  * cannot be detected as false acknowledge.
 393                  */
 394                 entry->owner_user_flag = !entry->owner_user_flag;
 395         }
 396         if (last)
 397                 entry->properties |= TSNEP_TX_DESC_LAST_FRAGMENT_FLAG;
 398         if (index == tx->increment_owner_counter) {
 399                 tx->owner_counter++;
 400                 if (tx->owner_counter == 4)
 401                         tx->owner_counter = 1;
 402                 tx->increment_owner_counter--;
 403                 if (tx->increment_owner_counter < 0)
 404                         tx->increment_owner_counter = TSNEP_RING_SIZE - 1;
 405         }
 406         entry->properties |=
 407                 (tx->owner_counter << TSNEP_DESC_OWNER_COUNTER_SHIFT) &
 408                 TSNEP_DESC_OWNER_COUNTER_MASK;
 409         if (entry->owner_user_flag)
 410                 entry->properties |= TSNEP_TX_DESC_OWNER_USER_FLAG;
 411         entry->desc->more_properties =
 412                 __cpu_to_le32(entry->len & TSNEP_DESC_LENGTH_MASK);
 413
 414         /* descriptor properties shall be written last, because valid data is
 415          * signaled there
 416          */
 417         dma_wmb();
 418
 419         entry->desc->properties = __cpu_to_le32(entry->properties);
 420 }
 421
 422 static int tsnep_tx_desc_available(struct tsnep_tx *tx)
 423 {
 424         if (tx->read <= tx->write)
 425                 return TSNEP_RING_SIZE - tx->write + tx->read - 1;
 426         else
 427                 return tx->read - tx->write - 1;
 428 }
 429
 430 static int tsnep_tx_map(struct sk_buff *skb, struct tsnep_tx *tx, int count)
 431 {
 432         struct device *dmadev = tx->adapter->dmadev;
 433         struct tsnep_tx_entry *entry;
 434         unsigned int len;
 435         dma_addr_t dma;
 436         int map_len = 0;
 437         int i;
 438
 439         for (i = 0; i < count; i++) {
 440                 entry = &tx->entry[(tx->write + i) & TSNEP_RING_MASK];
 441
 442                 if (!i) {
 443                         len = skb_headlen(skb);
 444                         dma = dma_map_single(dmadev, skb->data, len,
 445                                              DMA_TO_DEVICE);
 446
 447                         entry->type = TSNEP_TX_TYPE_SKB;
 448                 } else {
 449                         len = skb_frag_size(&skb_shinfo(skb)->frags[i - 1]);
 450                         dma = skb_frag_dma_map(dmadev,
 451                                                &skb_shinfo(skb)->frags[i - 1],
 452                                                0, len, DMA_TO_DEVICE);
 453
 454                         entry->type = TSNEP_TX_TYPE_SKB_FRAG;
 455                 }
 456                 if (dma_mapping_error(dmadev, dma))
 457                         return -ENOMEM;
 458
 459                 entry->len = len;
 460                 dma_unmap_addr_set(entry, dma, dma);
 461
 462                 entry->desc->tx = __cpu_to_le64(dma);
 463
 464                 map_len += len;
 465         }
 466
 467         return map_len;
 468 }
 469
 470 static int tsnep_tx_unmap(struct tsnep_tx *tx, int index, int count)
 471 {
 472         struct device *dmadev = tx->adapter->dmadev;
 473         struct tsnep_tx_entry *entry;
 474         int map_len = 0;
 475         int i;
 476
 477         for (i = 0; i < count; i++) {
 478                 entry = &tx->entry[(index + i) & TSNEP_RING_MASK];
 479
 480                 if (entry->len) {
 481                         if (entry->type & TSNEP_TX_TYPE_SKB)
 482                                 dma_unmap_single(dmadev,
 483                                                  dma_unmap_addr(entry, dma),
 484                                                  dma_unmap_len(entry, len),
 485                                                  DMA_TO_DEVICE);
 486                         else if (entry->type &
 487                                  (TSNEP_TX_TYPE_SKB_FRAG | TSNEP_TX_TYPE_XDP_NDO))
 488                                 dma_unmap_page(dmadev,
 489                                                dma_unmap_addr(entry, dma),
 490                                                dma_unmap_len(entry, len),
 491                                                DMA_TO_DEVICE);
 492                         map_len += entry->len;
 493                         entry->len = 0;
 494                 }
 495         }
 496
 497         return map_len;
 498 }
 499
 500 static netdev_tx_t tsnep_xmit_frame_ring(struct sk_buff *skb,
 501                                          struct tsnep_tx *tx)
 502 {
 503         int count = 1;
 504         struct tsnep_tx_entry *entry;
 505         int length;
 506         int i;
 507         int retval;
 508
 509         if (skb_shinfo(skb)->nr_frags > 0)
 510                 count += skb_shinfo(skb)->nr_frags;
 511
 512         if (tsnep_tx_desc_available(tx) < count) {
 513                 /* ring full, shall not happen because queue is stopped if full
 514                  * below
 515                  */
 516                 netif_stop_subqueue(tx->adapter->netdev, tx->queue_index);
 517
 518                 return NETDEV_TX_BUSY;
 519         }
 520
 521         entry = &tx->entry[tx->write];
 522         entry->skb = skb;
 523
 524         retval = tsnep_tx_map(skb, tx, count);
 525         if (retval < 0) {
 526                 tsnep_tx_unmap(tx, tx->write, count);
 527                 dev_kfree_skb_any(entry->skb);
 528                 entry->skb = NULL;
 529
 530                 tx->dropped++;
 531
 532                 return NETDEV_TX_OK;
 533         }
 534         length = retval;
 535
 536         if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)
 537                 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 538
 539         for (i = 0; i < count; i++)
 540                 tsnep_tx_activate(tx, (tx->write + i) & TSNEP_RING_MASK, length,
 541                                   i == count - 1);
 542         tx->write = (tx->write + count) & TSNEP_RING_MASK;
 543
 544         skb_tx_timestamp(skb);
 545
 546         /* descriptor properties shall be valid before hardware is notified */
 547         dma_wmb();
 548
 549         iowrite32(TSNEP_CONTROL_TX_ENABLE, tx->addr + TSNEP_CONTROL);
 550
 551         if (tsnep_tx_desc_available(tx) < (MAX_SKB_FRAGS + 1)) {
 552                 /* ring can get full with next frame */
 553                 netif_stop_subqueue(tx->adapter->netdev, tx->queue_index);
 554         }
 555
 556         return NETDEV_TX_OK;
 557 }
 558
 559 static int tsnep_xdp_tx_map(struct xdp_frame *xdpf, struct tsnep_tx *tx,
 560                             struct skb_shared_info *shinfo, int count, u32 type)
 561 {
 562         struct device *dmadev = tx->adapter->dmadev;
 563         struct tsnep_tx_entry *entry;
 564         struct page *page;
 565         skb_frag_t *frag;
 566         unsigned int len;
 567         int map_len = 0;
 568         dma_addr_t dma;
 569         void *data;
 570         int i;
 571
 572         frag = NULL;
 573         len = xdpf->len;
 574         for (i = 0; i < count; i++) {
 575                 entry = &tx->entry[(tx->write + i) & TSNEP_RING_MASK];
 576                 if (type & TSNEP_TX_TYPE_XDP_NDO) {
 577                         data = unlikely(frag) ? skb_frag_address(frag) :
 578                                                 xdpf->data;
 579                         dma = dma_map_single(dmadev, data, len, DMA_TO_DEVICE);
 580                         if (dma_mapping_error(dmadev, dma))
 581                                 return -ENOMEM;
 582
 583                         entry->type = TSNEP_TX_TYPE_XDP_NDO;
 584                 } else {
 585                         page = unlikely(frag) ? skb_frag_page(frag) :
 586                                                 virt_to_page(xdpf->data);
 587                         dma = page_pool_get_dma_addr(page);
 588                         if (unlikely(frag))
 589                                 dma += skb_frag_off(frag);
 590                         else
 591                                 dma += sizeof(*xdpf) + xdpf->headroom;
 592                         dma_sync_single_for_device(dmadev, dma, len,
 593                                                    DMA_BIDIRECTIONAL);
 594
 595                         entry->type = TSNEP_TX_TYPE_XDP_TX;
 596                 }
 597
 598                 entry->len = len;
 599                 dma_unmap_addr_set(entry, dma, dma);
 600
 601                 entry->desc->tx = __cpu_to_le64(dma);
 602
 603                 map_len += len;
 604
 605                 if (i + 1 < count) {
 606                         frag = &shinfo->frags[i];
 607                         len = skb_frag_size(frag);
 608                 }
 609         }
 610
 611         return map_len;
 612 }
 613
 614 /* This function requires __netif_tx_lock is held by the caller. */
 615 static bool tsnep_xdp_xmit_frame_ring(struct xdp_frame *xdpf,
 616                                       struct tsnep_tx *tx, u32 type)
 617 {
 618         struct skb_shared_info *shinfo = xdp_get_shared_info_from_frame(xdpf);
 619         struct tsnep_tx_entry *entry;
 620         int count, length, retval, i;
 621
 622         count = 1;
 623         if (unlikely(xdp_frame_has_frags(xdpf)))
 624                 count += shinfo->nr_frags;
 625
 626         /* ensure that TX ring is not filled up by XDP, always MAX_SKB_FRAGS
 627          * will be available for normal TX path and queue is stopped there if
 628          * necessary
 629          */
 630         if (tsnep_tx_desc_available(tx) < (MAX_SKB_FRAGS + 1 + count))
 631                 return false;
 632
 633         entry = &tx->entry[tx->write];
 634         entry->xdpf = xdpf;
 635
 636         retval = tsnep_xdp_tx_map(xdpf, tx, shinfo, count, type);
 637         if (retval < 0) {
 638                 tsnep_tx_unmap(tx, tx->write, count);
 639                 entry->xdpf = NULL;
 640
 641                 tx->dropped++;
 642
 643                 return false;
 644         }
 645         length = retval;
 646
 647         for (i = 0; i < count; i++)
 648                 tsnep_tx_activate(tx, (tx->write + i) & TSNEP_RING_MASK, length,
 649                                   i == count - 1);
 650         tx->write = (tx->write + count) & TSNEP_RING_MASK;
 651
 652         /* descriptor properties shall be valid before hardware is notified */
 653         dma_wmb();
 654
 655         return true;
 656 }
 657
 658 static void tsnep_xdp_xmit_flush(struct tsnep_tx *tx)
 659 {
 660         iowrite32(TSNEP_CONTROL_TX_ENABLE, tx->addr + TSNEP_CONTROL);
 661 }
 662
 663 static bool tsnep_xdp_xmit_back(struct tsnep_adapter *adapter,
 664                                 struct xdp_buff *xdp,
 665                                 struct netdev_queue *tx_nq, struct tsnep_tx *tx)
 666 {
 667         struct xdp_frame *xdpf = xdp_convert_buff_to_frame(xdp);
 668         bool xmit;
 669
 670         if (unlikely(!xdpf))
 671                 return false;
 672
 673         __netif_tx_lock(tx_nq, smp_processor_id());
 674
 675         xmit = tsnep_xdp_xmit_frame_ring(xdpf, tx, TSNEP_TX_TYPE_XDP_TX);
 676
 677         /* Avoid transmit queue timeout since we share it with the slow path */
 678         if (xmit)
 679                 txq_trans_cond_update(tx_nq);
 680
 681         __netif_tx_unlock(tx_nq);
 682
 683         return xmit;
 684 }
 685
 686 static int tsnep_xdp_tx_map_zc(struct xdp_desc *xdpd, struct tsnep_tx *tx)
 687 {
 688         struct tsnep_tx_entry *entry;
 689         dma_addr_t dma;
 690
 691         entry = &tx->entry[tx->write];
 692         entry->zc = true;
 693
 694         dma = xsk_buff_raw_get_dma(tx->xsk_pool, xdpd->addr);
 695         xsk_buff_raw_dma_sync_for_device(tx->xsk_pool, dma, xdpd->len);
 696
 697         entry->type = TSNEP_TX_TYPE_XSK;
 698         entry->len = xdpd->len;
 699
 700         entry->desc->tx = __cpu_to_le64(dma);
 701
 702         return xdpd->len;
 703 }
 704
 705 static void tsnep_xdp_xmit_frame_ring_zc(struct xdp_desc *xdpd,
 706                                          struct tsnep_tx *tx)
 707 {
 708         int length;
 709
 710         length = tsnep_xdp_tx_map_zc(xdpd, tx);
 711
 712         tsnep_tx_activate(tx, tx->write, length, true);
 713         tx->write = (tx->write + 1) & TSNEP_RING_MASK;
 714 }
 715
 716 static void tsnep_xdp_xmit_zc(struct tsnep_tx *tx)
 717 {
 718         int desc_available = tsnep_tx_desc_available(tx);
 719         struct xdp_desc *descs = tx->xsk_pool->tx_descs;
 720         int batch, i;
 721
 722         /* ensure that TX ring is not filled up by XDP, always MAX_SKB_FRAGS
 723          * will be available for normal TX path and queue is stopped there if
 724          * necessary
 725          */
 726         if (desc_available <= (MAX_SKB_FRAGS + 1))
 727                 return;
 728         desc_available -= MAX_SKB_FRAGS + 1;
 729
 730         batch = xsk_tx_peek_release_desc_batch(tx->xsk_pool, desc_available);
 731         for (i = 0; i < batch; i++)
 732                 tsnep_xdp_xmit_frame_ring_zc(&descs[i], tx);
 733
 734         if (batch) {
 735                 /* descriptor properties shall be valid before hardware is
 736                  * notified
 737                  */
 738                 dma_wmb();
 739
 740                 tsnep_xdp_xmit_flush(tx);
 741         }
 742 }
 743
 744 static bool tsnep_tx_poll(struct tsnep_tx *tx, int napi_budget)
 745 {
 746         struct tsnep_tx_entry *entry;
 747         struct netdev_queue *nq;
 748         int xsk_frames = 0;
 749         int budget = 128;
 750         int length;
 751         int count;
 752
 753         nq = netdev_get_tx_queue(tx->adapter->netdev, tx->queue_index);
 754         __netif_tx_lock(nq, smp_processor_id());
 755
 756         do {
 757                 if (tx->read == tx->write)
 758                         break;
 759
 760                 entry = &tx->entry[tx->read];
 761                 if ((__le32_to_cpu(entry->desc_wb->properties) &
 762                      TSNEP_TX_DESC_OWNER_MASK) !=
 763                     (entry->properties & TSNEP_TX_DESC_OWNER_MASK))
 764                         break;
 765
 766                 /* descriptor properties shall be read first, because valid data
 767                  * is signaled there
 768                  */
 769                 dma_rmb();
 770
 771                 count = 1;
 772                 if ((entry->type & TSNEP_TX_TYPE_SKB) &&
 773                     skb_shinfo(entry->skb)->nr_frags > 0)
 774                         count += skb_shinfo(entry->skb)->nr_frags;
 775                 else if ((entry->type & TSNEP_TX_TYPE_XDP) &&
 776                          xdp_frame_has_frags(entry->xdpf))
 777                         count += xdp_get_shared_info_from_frame(entry->xdpf)->nr_frags;
 778
 779                 length = tsnep_tx_unmap(tx, tx->read, count);
 780
 781                 if ((entry->type & TSNEP_TX_TYPE_SKB) &&
 782                     (skb_shinfo(entry->skb)->tx_flags & SKBTX_IN_PROGRESS) &&
 783                     (__le32_to_cpu(entry->desc_wb->properties) &
 784                      TSNEP_DESC_EXTENDED_WRITEBACK_FLAG)) {
 785                         struct skb_shared_hwtstamps hwtstamps;
 786                         u64 timestamp;
 787
 788                         if (skb_shinfo(entry->skb)->tx_flags &
 789                             SKBTX_HW_TSTAMP_USE_CYCLES)
 790                                 timestamp =
 791                                         __le64_to_cpu(entry->desc_wb->counter);
 792                         else
 793                                 timestamp =
 794                                         __le64_to_cpu(entry->desc_wb->timestamp);
 795
 796                         memset(&hwtstamps, 0, sizeof(hwtstamps));
 797                         hwtstamps.hwtstamp = ns_to_ktime(timestamp);
 798
 799                         skb_tstamp_tx(entry->skb, &hwtstamps);
 800                 }
 801
 802                 if (entry->type & TSNEP_TX_TYPE_SKB)
 803                         napi_consume_skb(entry->skb, napi_budget);
 804                 else if (entry->type & TSNEP_TX_TYPE_XDP)
 805                         xdp_return_frame_rx_napi(entry->xdpf);
 806                 else
 807                         xsk_frames++;
 808                 /* xdpf and zc are union with skb */
 809                 entry->skb = NULL;
 810
 811                 tx->read = (tx->read + count) & TSNEP_RING_MASK;
 812
 813                 tx->packets++;
 814                 tx->bytes += length + ETH_FCS_LEN;
 815
 816                 budget--;
 817         } while (likely(budget));
 818
 819         if (tx->xsk_pool) {
 820                 if (xsk_frames)
 821                         xsk_tx_completed(tx->xsk_pool, xsk_frames);
 822                 if (xsk_uses_need_wakeup(tx->xsk_pool))
 823                         xsk_set_tx_need_wakeup(tx->xsk_pool);
 824                 tsnep_xdp_xmit_zc(tx);
 825         }
 826
 827         if ((tsnep_tx_desc_available(tx) >= ((MAX_SKB_FRAGS + 1) * 2)) &&
 828             netif_tx_queue_stopped(nq)) {
 829                 netif_tx_wake_queue(nq);
 830         }
 831
 832         __netif_tx_unlock(nq);
 833
 834         return budget != 0;
 835 }
 836
 837 static bool tsnep_tx_pending(struct tsnep_tx *tx)
 838 {
 839         struct tsnep_tx_entry *entry;
 840         struct netdev_queue *nq;
 841         bool pending = false;
 842
 843         nq = netdev_get_tx_queue(tx->adapter->netdev, tx->queue_index);
 844         __netif_tx_lock(nq, smp_processor_id());
 845
 846         if (tx->read != tx->write) {
 847                 entry = &tx->entry[tx->read];
 848                 if ((__le32_to_cpu(entry->desc_wb->properties) &
 849                      TSNEP_TX_DESC_OWNER_MASK) ==
 850                     (entry->properties & TSNEP_TX_DESC_OWNER_MASK))
 851                         pending = true;
 852         }
 853
 854         __netif_tx_unlock(nq);
 855
 856         return pending;
 857 }
 858
 859 static int tsnep_tx_open(struct tsnep_tx *tx)
 860 {
 861         int retval;
 862
 863         retval = tsnep_tx_ring_create(tx);
 864         if (retval)
 865                 return retval;
 866
 867         tsnep_tx_init(tx);
 868
 869         return 0;
 870 }
 871
 872 static void tsnep_tx_close(struct tsnep_tx *tx)
 873 {
 874         tsnep_tx_ring_cleanup(tx);
 875 }
 876
 877 static void tsnep_rx_ring_cleanup(struct tsnep_rx *rx)
 878 {
 879         struct device *dmadev = rx->adapter->dmadev;
 880         struct tsnep_rx_entry *entry;
 881         int i;
 882
 883         for (i = 0; i < TSNEP_RING_SIZE; i++) {
 884                 entry = &rx->entry[i];
 885                 if (!rx->xsk_pool && entry->page)
 886                         page_pool_put_full_page(rx->page_pool, entry->page,
 887                                                 false);
 888                 if (rx->xsk_pool && entry->xdp)
 889                         xsk_buff_free(entry->xdp);
 890                 /* xdp is union with page */
 891                 entry->page = NULL;
 892         }
 893
 894         if (rx->page_pool)
 895                 page_pool_destroy(rx->page_pool);
 896
 897         memset(rx->entry, 0, sizeof(rx->entry));
 898
 899         for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
 900                 if (rx->page[i]) {
 901                         dma_free_coherent(dmadev, PAGE_SIZE, rx->page[i],
 902                                           rx->page_dma[i]);
 903                         rx->page[i] = NULL;
 904                         rx->page_dma[i] = 0;
 905                 }
 906         }
 907 }
 908
 909 static int tsnep_rx_ring_create(struct tsnep_rx *rx)
 910 {
 911         struct device *dmadev = rx->adapter->dmadev;
 912         struct tsnep_rx_entry *entry;
 913         struct page_pool_params pp_params = { 0 };
 914         struct tsnep_rx_entry *next_entry;
 915         int i, j;
 916         int retval;
 917
 918         for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
 919                 rx->page[i] =
 920                         dma_alloc_coherent(dmadev, PAGE_SIZE, &rx->page_dma[i],
 921                                            GFP_KERNEL);
 922                 if (!rx->page[i]) {
 923                         retval = -ENOMEM;
 924                         goto failed;
 925                 }
 926                 for (j = 0; j < TSNEP_RING_ENTRIES_PER_PAGE; j++) {
 927                         entry = &rx->entry[TSNEP_RING_ENTRIES_PER_PAGE * i + j];
 928                         entry->desc_wb = (struct tsnep_rx_desc_wb *)
 929                                 (((u8 *)rx->page[i]) + TSNEP_DESC_SIZE * j);
 930                         entry->desc = (struct tsnep_rx_desc *)
 931                                 (((u8 *)entry->desc_wb) + TSNEP_DESC_OFFSET);
 932                         entry->desc_dma = rx->page_dma[i] + TSNEP_DESC_SIZE * j;
 933                 }
 934         }
 935
 936         pp_params.flags = PP_FLAG_DMA_MAP | PP_FLAG_DMA_SYNC_DEV;
 937         pp_params.order = 0;
 938         pp_params.pool_size = TSNEP_RING_SIZE;
 939         pp_params.nid = dev_to_node(dmadev);
 940         pp_params.dev = dmadev;
 941         pp_params.dma_dir = DMA_BIDIRECTIONAL;
 942         pp_params.max_len = TSNEP_MAX_RX_BUF_SIZE;
 943         pp_params.offset = TSNEP_RX_OFFSET;
 944         rx->page_pool = page_pool_create(&pp_params);
 945         if (IS_ERR(rx->page_pool)) {
 946                 retval = PTR_ERR(rx->page_pool);
 947                 rx->page_pool = NULL;
 948                 goto failed;
 949         }
 950
 951         for (i = 0; i < TSNEP_RING_SIZE; i++) {
 952                 entry = &rx->entry[i];
 953                 next_entry = &rx->entry[(i + 1) & TSNEP_RING_MASK];
 954                 entry->desc->next = __cpu_to_le64(next_entry->desc_dma);
 955         }
 956
 957         return 0;
 958
 959 failed:
 960         tsnep_rx_ring_cleanup(rx);
 961         return retval;
 962 }
 963
 964 static void tsnep_rx_init(struct tsnep_rx *rx)
 965 {
 966         dma_addr_t dma;
 967
 968         dma = rx->entry[0].desc_dma | TSNEP_RESET_OWNER_COUNTER;
 969         iowrite32(DMA_ADDR_LOW(dma), rx->addr + TSNEP_RX_DESC_ADDR_LOW);
 970         iowrite32(DMA_ADDR_HIGH(dma), rx->addr + TSNEP_RX_DESC_ADDR_HIGH);
 971         rx->write = 0;
 972         rx->read = 0;
 973         rx->owner_counter = 1;
 974         rx->increment_owner_counter = TSNEP_RING_SIZE - 1;
 975 }
 976
 977 static void tsnep_rx_enable(struct tsnep_rx *rx)
 978 {
 979         /* descriptor properties shall be valid before hardware is notified */
 980         dma_wmb();
 981
 982         iowrite32(TSNEP_CONTROL_RX_ENABLE, rx->addr + TSNEP_CONTROL);
 983 }
 984
 985 static void tsnep_rx_disable(struct tsnep_rx *rx)
 986 {
 987         u32 val;
 988
 989         iowrite32(TSNEP_CONTROL_RX_DISABLE, rx->addr + TSNEP_CONTROL);
 990         readx_poll_timeout(ioread32, rx->addr + TSNEP_CONTROL, val,
 991                            ((val & TSNEP_CONTROL_RX_ENABLE) == 0), 10000,
 992                            1000000);
 993 }
 994
 995 static int tsnep_rx_desc_available(struct tsnep_rx *rx)
 996 {
 997         if (rx->read <= rx->write)
 998                 return TSNEP_RING_SIZE - rx->write + rx->read - 1;
 999         else
1000                 return rx->read - rx->write - 1;
1001 }
1002
1003 static void tsnep_rx_free_page_buffer(struct tsnep_rx *rx)
1004 {
1005         struct page **page;
1006
1007         /* last entry of page_buffer is always zero, because ring cannot be
1008          * filled completely
1009          */
1010         page = rx->page_buffer;
1011         while (*page) {
1012                 page_pool_put_full_page(rx->page_pool, *page, false);
1013                 *page = NULL;
1014                 page++;
1015         }
1016 }
1017
1018 static int tsnep_rx_alloc_page_buffer(struct tsnep_rx *rx)
1019 {
1020         int i;
1021
1022         /* alloc for all ring entries except the last one, because ring cannot
1023          * be filled completely
1024          */
1025         for (i = 0; i < TSNEP_RING_SIZE - 1; i++) {
1026                 rx->page_buffer[i] = page_pool_dev_alloc_pages(rx->page_pool);
1027                 if (!rx->page_buffer[i]) {
1028                         tsnep_rx_free_page_buffer(rx);
1029
1030                         return -ENOMEM;
1031                 }
1032         }
1033
1034         return 0;
1035 }
1036
1037 static void tsnep_rx_set_page(struct tsnep_rx *rx, struct tsnep_rx_entry *entry,
1038                               struct page *page)
1039 {
1040         entry->page = page;
1041         entry->len = TSNEP_MAX_RX_BUF_SIZE;
1042         entry->dma = page_pool_get_dma_addr(entry->page);
1043         entry->desc->rx = __cpu_to_le64(entry->dma + TSNEP_RX_OFFSET);
1044 }
1045
1046 static int tsnep_rx_alloc_buffer(struct tsnep_rx *rx, int index)
1047 {
1048         struct tsnep_rx_entry *entry = &rx->entry[index];
1049         struct page *page;
1050
1051         page = page_pool_dev_alloc_pages(rx->page_pool);
1052         if (unlikely(!page))
1053                 return -ENOMEM;
1054         tsnep_rx_set_page(rx, entry, page);
1055
1056         return 0;
1057 }
1058
1059 static void tsnep_rx_reuse_buffer(struct tsnep_rx *rx, int index)
1060 {
1061         struct tsnep_rx_entry *entry = &rx->entry[index];
1062         struct tsnep_rx_entry *read = &rx->entry[rx->read];
1063
1064         tsnep_rx_set_page(rx, entry, read->page);
1065         read->page = NULL;
1066 }
1067
1068 static void tsnep_rx_activate(struct tsnep_rx *rx, int index)
1069 {
1070         struct tsnep_rx_entry *entry = &rx->entry[index];
1071
1072         /* TSNEP_MAX_RX_BUF_SIZE and TSNEP_XSK_RX_BUF_SIZE are multiple of 4 */
1073         entry->properties = entry->len & TSNEP_DESC_LENGTH_MASK;
1074         entry->properties |= TSNEP_DESC_INTERRUPT_FLAG;
1075         if (index == rx->increment_owner_counter) {
1076                 rx->owner_counter++;
1077                 if (rx->owner_counter == 4)
1078                         rx->owner_counter = 1;
1079                 rx->increment_owner_counter--;
1080                 if (rx->increment_owner_counter < 0)
1081                         rx->increment_owner_counter = TSNEP_RING_SIZE - 1;
1082         }
1083         entry->properties |=
1084                 (rx->owner_counter << TSNEP_DESC_OWNER_COUNTER_SHIFT) &
1085                 TSNEP_DESC_OWNER_COUNTER_MASK;
1086
1087         /* descriptor properties shall be written last, because valid data is
1088          * signaled there
1089          */
1090         dma_wmb();
1091
1092         entry->desc->properties = __cpu_to_le32(entry->properties);
1093 }
1094
1095 static int tsnep_rx_alloc(struct tsnep_rx *rx, int count, bool reuse)
1096 {
1097         bool alloc_failed = false;
1098         int i, index;
1099
1100         for (i = 0; i < count && !alloc_failed; i++) {
1101                 index = (rx->write + i) & TSNEP_RING_MASK;
1102
1103                 if (unlikely(tsnep_rx_alloc_buffer(rx, index))) {
1104                         rx->alloc_failed++;
1105                         alloc_failed = true;
1106
1107                         /* reuse only if no other allocation was successful */
1108                         if (i == 0 && reuse)
1109                                 tsnep_rx_reuse_buffer(rx, index);
1110                         else
1111                                 break;
1112                 }
1113
1114                 tsnep_rx_activate(rx, index);
1115         }
1116
1117         if (i)
1118                 rx->write = (rx->write + i) & TSNEP_RING_MASK;
1119
1120         return i;
1121 }
1122
1123 static int tsnep_rx_refill(struct tsnep_rx *rx, int count, bool reuse)
1124 {
1125         int desc_refilled;
1126
1127         desc_refilled = tsnep_rx_alloc(rx, count, reuse);
1128         if (desc_refilled)
1129                 tsnep_rx_enable(rx);
1130
1131         return desc_refilled;
1132 }
1133
1134 static void tsnep_rx_set_xdp(struct tsnep_rx *rx, struct tsnep_rx_entry *entry,
1135                              struct xdp_buff *xdp)
1136 {
1137         entry->xdp = xdp;
1138         entry->len = TSNEP_XSK_RX_BUF_SIZE;
1139         entry->dma = xsk_buff_xdp_get_dma(entry->xdp);
1140         entry->desc->rx = __cpu_to_le64(entry->dma);
1141 }
1142
1143 static void tsnep_rx_reuse_buffer_zc(struct tsnep_rx *rx, int index)
1144 {
1145         struct tsnep_rx_entry *entry = &rx->entry[index];
1146         struct tsnep_rx_entry *read = &rx->entry[rx->read];
1147
1148         tsnep_rx_set_xdp(rx, entry, read->xdp);
1149         read->xdp = NULL;
1150 }
1151
1152 static int tsnep_rx_alloc_zc(struct tsnep_rx *rx, int count, bool reuse)
1153 {
1154         u32 allocated;
1155         int i;
1156
1157         allocated = xsk_buff_alloc_batch(rx->xsk_pool, rx->xdp_batch, count);
1158         for (i = 0; i < allocated; i++) {
1159                 int index = (rx->write + i) & TSNEP_RING_MASK;
1160                 struct tsnep_rx_entry *entry = &rx->entry[index];
1161
1162                 tsnep_rx_set_xdp(rx, entry, rx->xdp_batch[i]);
1163                 tsnep_rx_activate(rx, index);
1164         }
1165         if (i == 0) {
1166                 rx->alloc_failed++;
1167
1168                 if (reuse) {
1169                         tsnep_rx_reuse_buffer_zc(rx, rx->write);
1170                         tsnep_rx_activate(rx, rx->write);
1171                 }
1172         }
1173
1174         if (i)
1175                 rx->write = (rx->write + i) & TSNEP_RING_MASK;
1176
1177         return i;
1178 }
1179
1180 static void tsnep_rx_free_zc(struct tsnep_rx *rx)
1181 {
1182         int i;
1183
1184         for (i = 0; i < TSNEP_RING_SIZE; i++) {
1185                 struct tsnep_rx_entry *entry = &rx->entry[i];
1186
1187                 if (entry->xdp)
1188                         xsk_buff_free(entry->xdp);
1189                 entry->xdp = NULL;
1190         }
1191 }
1192
1193 static int tsnep_rx_refill_zc(struct tsnep_rx *rx, int count, bool reuse)
1194 {
1195         int desc_refilled;
1196
1197         desc_refilled = tsnep_rx_alloc_zc(rx, count, reuse);
1198         if (desc_refilled)
1199                 tsnep_rx_enable(rx);
1200
1201         return desc_refilled;
1202 }
1203
1204 static bool tsnep_xdp_run_prog(struct tsnep_rx *rx, struct bpf_prog *prog,
1205                                struct xdp_buff *xdp, int *status,
1206                                struct netdev_queue *tx_nq, struct tsnep_tx *tx)
1207 {
1208         unsigned int length;
1209         unsigned int sync;
1210         u32 act;
1211
1212         length = xdp->data_end - xdp->data_hard_start - XDP_PACKET_HEADROOM;
1213
1214         act = bpf_prog_run_xdp(prog, xdp);
1215         switch (act) {
1216         case XDP_PASS:
1217                 return false;
1218         case XDP_TX:
1219                 if (!tsnep_xdp_xmit_back(rx->adapter, xdp, tx_nq, tx))
1220                         goto out_failure;
1221                 *status |= TSNEP_XDP_TX;
1222                 return true;
1223         case XDP_REDIRECT:
1224                 if (xdp_do_redirect(rx->adapter->netdev, xdp, prog) < 0)
1225                         goto out_failure;
1226                 *status |= TSNEP_XDP_REDIRECT;
1227                 return true;
1228         default:
1229                 bpf_warn_invalid_xdp_action(rx->adapter->netdev, prog, act);
1230                 fallthrough;
1231         case XDP_ABORTED:
1232 out_failure:
1233                 trace_xdp_exception(rx->adapter->netdev, prog, act);
1234                 fallthrough;
1235         case XDP_DROP:
1236                 /* Due xdp_adjust_tail: DMA sync for_device cover max len CPU
1237                  * touch
1238                  */
1239                 sync = xdp->data_end - xdp->data_hard_start -
1240                        XDP_PACKET_HEADROOM;
1241                 sync = max(sync, length);
1242                 page_pool_put_page(rx->page_pool, virt_to_head_page(xdp->data),
1243                                    sync, true);
1244                 return true;
1245         }
1246 }
1247
1248 static bool tsnep_xdp_run_prog_zc(struct tsnep_rx *rx, struct bpf_prog *prog,
1249                                   struct xdp_buff *xdp, int *status,
1250                                   struct netdev_queue *tx_nq,
1251                                   struct tsnep_tx *tx)
1252 {
1253         u32 act;
1254
1255         act = bpf_prog_run_xdp(prog, xdp);
1256
1257         /* XDP_REDIRECT is the main action for zero-copy */
1258         if (likely(act == XDP_REDIRECT)) {
1259                 if (xdp_do_redirect(rx->adapter->netdev, xdp, prog) < 0)
1260                         goto out_failure;
1261                 *status |= TSNEP_XDP_REDIRECT;
1262                 return true;
1263         }
1264
1265         switch (act) {
1266         case XDP_PASS:
1267                 return false;
1268         case XDP_TX:
1269                 if (!tsnep_xdp_xmit_back(rx->adapter, xdp, tx_nq, tx))
1270                         goto out_failure;
1271                 *status |= TSNEP_XDP_TX;
1272                 return true;
1273         default:
1274                 bpf_warn_invalid_xdp_action(rx->adapter->netdev, prog, act);
1275                 fallthrough;
1276         case XDP_ABORTED:
1277 out_failure:
1278                 trace_xdp_exception(rx->adapter->netdev, prog, act);
1279                 fallthrough;
1280         case XDP_DROP:
1281                 xsk_buff_free(xdp);
1282                 return true;
1283         }
1284 }
1285
1286 static void tsnep_finalize_xdp(struct tsnep_adapter *adapter, int status,
1287                                struct netdev_queue *tx_nq, struct tsnep_tx *tx)
1288 {
1289         if (status & TSNEP_XDP_TX) {
1290                 __netif_tx_lock(tx_nq, smp_processor_id());
1291                 tsnep_xdp_xmit_flush(tx);
1292                 __netif_tx_unlock(tx_nq);
1293         }
1294
1295         if (status & TSNEP_XDP_REDIRECT)
1296                 xdp_do_flush();
1297 }
1298
1299 static struct sk_buff *tsnep_build_skb(struct tsnep_rx *rx, struct page *page,
1300                                        int length)
1301 {
1302         struct sk_buff *skb;
1303
1304         skb = napi_build_skb(page_address(page), PAGE_SIZE);
1305         if (unlikely(!skb))
1306                 return NULL;
1307
1308         /* update pointers within the skb to store the data */
1309         skb_reserve(skb, TSNEP_RX_OFFSET + TSNEP_RX_INLINE_METADATA_SIZE);
1310         __skb_put(skb, length - ETH_FCS_LEN);
1311
1312         if (rx->adapter->hwtstamp_config.rx_filter == HWTSTAMP_FILTER_ALL) {
1313                 struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb);
1314                 struct tsnep_rx_inline *rx_inline =
1315                         (struct tsnep_rx_inline *)(page_address(page) +
1316                                                    TSNEP_RX_OFFSET);
1317
1318                 skb_shinfo(skb)->tx_flags |=
1319                         SKBTX_HW_TSTAMP_NETDEV;
1320                 memset(hwtstamps, 0, sizeof(*hwtstamps));
1321                 hwtstamps->netdev_data = rx_inline;
1322         }
1323
1324         skb_record_rx_queue(skb, rx->queue_index);
1325         skb->protocol = eth_type_trans(skb, rx->adapter->netdev);
1326
1327         return skb;
1328 }
1329
1330 static void tsnep_rx_page(struct tsnep_rx *rx, struct napi_struct *napi,
1331                           struct page *page, int length)
1332 {
1333         struct sk_buff *skb;
1334
1335         skb = tsnep_build_skb(rx, page, length);
1336         if (skb) {
1337                 skb_mark_for_recycle(skb);
1338
1339                 rx->packets++;
1340                 rx->bytes += length;
1341                 if (skb->pkt_type == PACKET_MULTICAST)
1342                         rx->multicast++;
1343
1344                 napi_gro_receive(napi, skb);
1345         } else {
1346                 page_pool_recycle_direct(rx->page_pool, page);
1347
1348                 rx->dropped++;
1349         }
1350 }
1351
1352 static int tsnep_rx_poll(struct tsnep_rx *rx, struct napi_struct *napi,
1353                          int budget)
1354 {
1355         struct device *dmadev = rx->adapter->dmadev;
1356         enum dma_data_direction dma_dir;
1357         struct tsnep_rx_entry *entry;
1358         struct netdev_queue *tx_nq;
1359         struct bpf_prog *prog;
1360         struct xdp_buff xdp;
1361         struct tsnep_tx *tx;
1362         int desc_available;
1363         int xdp_status = 0;
1364         int done = 0;
1365         int length;
1366
1367         desc_available = tsnep_rx_desc_available(rx);
1368         dma_dir = page_pool_get_dma_dir(rx->page_pool);
1369         prog = READ_ONCE(rx->adapter->xdp_prog);
1370         if (prog) {
1371                 tx_nq = netdev_get_tx_queue(rx->adapter->netdev,
1372                                             rx->tx_queue_index);
1373                 tx = &rx->adapter->tx[rx->tx_queue_index];
1374
1375                 xdp_init_buff(&xdp, PAGE_SIZE, &rx->xdp_rxq);
1376         }
1377
1378         while (likely(done < budget) && (rx->read != rx->write)) {
1379                 entry = &rx->entry[rx->read];
1380                 if ((__le32_to_cpu(entry->desc_wb->properties) &
1381                      TSNEP_DESC_OWNER_COUNTER_MASK) !=
1382                     (entry->properties & TSNEP_DESC_OWNER_COUNTER_MASK))
1383                         break;
1384                 done++;
1385
1386                 if (desc_available >= TSNEP_RING_RX_REFILL) {
1387                         bool reuse = desc_available >= TSNEP_RING_RX_REUSE;
1388
1389                         desc_available -= tsnep_rx_refill(rx, desc_available,
1390                                                           reuse);
1391                         if (!entry->page) {
1392                                 /* buffer has been reused for refill to prevent
1393                                  * empty RX ring, thus buffer cannot be used for
1394                                  * RX processing
1395                                  */
1396                                 rx->read = (rx->read + 1) & TSNEP_RING_MASK;
1397                                 desc_available++;
1398
1399                                 rx->dropped++;
1400
1401                                 continue;
1402                         }
1403                 }
1404
1405                 /* descriptor properties shall be read first, because valid data
1406                  * is signaled there
1407                  */
1408                 dma_rmb();
1409
1410                 prefetch(page_address(entry->page) + TSNEP_RX_OFFSET);
1411                 length = __le32_to_cpu(entry->desc_wb->properties) &
1412                          TSNEP_DESC_LENGTH_MASK;
1413                 dma_sync_single_range_for_cpu(dmadev, entry->dma,
1414                                               TSNEP_RX_OFFSET, length, dma_dir);
1415
1416                 /* RX metadata with timestamps is in front of actual data,
1417                  * subtract metadata size to get length of actual data and
1418                  * consider metadata size as offset of actual data during RX
1419                  * processing
1420                  */
1421                 length -= TSNEP_RX_INLINE_METADATA_SIZE;
1422
1423                 rx->read = (rx->read + 1) & TSNEP_RING_MASK;
1424                 desc_available++;
1425
1426                 if (prog) {
1427                         bool consume;
1428
1429                         xdp_prepare_buff(&xdp, page_address(entry->page),
1430                                          XDP_PACKET_HEADROOM + TSNEP_RX_INLINE_METADATA_SIZE,
1431                                          length, false);
1432
1433                         consume = tsnep_xdp_run_prog(rx, prog, &xdp,
1434                                                      &xdp_status, tx_nq, tx);
1435                         if (consume) {
1436                                 rx->packets++;
1437                                 rx->bytes += length;
1438
1439                                 entry->page = NULL;
1440
1441                                 continue;
1442                         }
1443                 }
1444
1445                 tsnep_rx_page(rx, napi, entry->page, length);
1446                 entry->page = NULL;
1447         }
1448
1449         if (xdp_status)
1450                 tsnep_finalize_xdp(rx->adapter, xdp_status, tx_nq, tx);
1451
1452         if (desc_available)
1453                 tsnep_rx_refill(rx, desc_available, false);
1454
1455         return done;
1456 }
1457
1458 static int tsnep_rx_poll_zc(struct tsnep_rx *rx, struct napi_struct *napi,
1459                             int budget)
1460 {
1461         struct tsnep_rx_entry *entry;
1462         struct netdev_queue *tx_nq;
1463         struct bpf_prog *prog;
1464         struct tsnep_tx *tx;
1465         int desc_available;
1466         int xdp_status = 0;
1467         struct page *page;
1468         int done = 0;
1469         int length;
1470
1471         desc_available = tsnep_rx_desc_available(rx);
1472         prog = READ_ONCE(rx->adapter->xdp_prog);
1473         if (prog) {
1474                 tx_nq = netdev_get_tx_queue(rx->adapter->netdev,
1475                                             rx->tx_queue_index);
1476                 tx = &rx->adapter->tx[rx->tx_queue_index];
1477         }
1478
1479         while (likely(done < budget) && (rx->read != rx->write)) {
1480                 entry = &rx->entry[rx->read];
1481                 if ((__le32_to_cpu(entry->desc_wb->properties) &
1482                      TSNEP_DESC_OWNER_COUNTER_MASK) !=
1483                     (entry->properties & TSNEP_DESC_OWNER_COUNTER_MASK))
1484                         break;
1485                 done++;
1486
1487                 if (desc_available >= TSNEP_RING_RX_REFILL) {
1488                         bool reuse = desc_available >= TSNEP_RING_RX_REUSE;
1489
1490                         desc_available -= tsnep_rx_refill_zc(rx, desc_available,
1491                                                              reuse);
1492                         if (!entry->xdp) {
1493                                 /* buffer has been reused for refill to prevent
1494                                  * empty RX ring, thus buffer cannot be used for
1495                                  * RX processing
1496                                  */
1497                                 rx->read = (rx->read + 1) & TSNEP_RING_MASK;
1498                                 desc_available++;
1499
1500                                 rx->dropped++;
1501
1502                                 continue;
1503                         }
1504                 }
1505
1506                 /* descriptor properties shall be read first, because valid data
1507                  * is signaled there
1508                  */
1509                 dma_rmb();
1510
1511                 prefetch(entry->xdp->data);
1512                 length = __le32_to_cpu(entry->desc_wb->properties) &
1513                          TSNEP_DESC_LENGTH_MASK;
1514                 xsk_buff_set_size(entry->xdp, length);
1515                 xsk_buff_dma_sync_for_cpu(entry->xdp, rx->xsk_pool);
1516
1517                 /* RX metadata with timestamps is in front of actual data,
1518                  * subtract metadata size to get length of actual data and
1519                  * consider metadata size as offset of actual data during RX
1520                  * processing
1521                  */
1522                 length -= TSNEP_RX_INLINE_METADATA_SIZE;
1523
1524                 rx->read = (rx->read + 1) & TSNEP_RING_MASK;
1525                 desc_available++;
1526
1527                 if (prog) {
1528                         bool consume;
1529
1530                         entry->xdp->data += TSNEP_RX_INLINE_METADATA_SIZE;
1531                         entry->xdp->data_meta += TSNEP_RX_INLINE_METADATA_SIZE;
1532
1533                         consume = tsnep_xdp_run_prog_zc(rx, prog, entry->xdp,
1534                                                         &xdp_status, tx_nq, tx);
1535                         if (consume) {
1536                                 rx->packets++;
1537                                 rx->bytes += length;
1538
1539                                 entry->xdp = NULL;
1540
1541                                 continue;
1542                         }
1543                 }
1544
1545                 page = page_pool_dev_alloc_pages(rx->page_pool);
1546                 if (page) {
1547                         memcpy(page_address(page) + TSNEP_RX_OFFSET,
1548                                entry->xdp->data - TSNEP_RX_INLINE_METADATA_SIZE,
1549                                length + TSNEP_RX_INLINE_METADATA_SIZE);
1550                         tsnep_rx_page(rx, napi, page, length);
1551                 } else {
1552                         rx->dropped++;
1553                 }
1554                 xsk_buff_free(entry->xdp);
1555                 entry->xdp = NULL;
1556         }
1557
1558         if (xdp_status)
1559                 tsnep_finalize_xdp(rx->adapter, xdp_status, tx_nq, tx);
1560
1561         if (desc_available)
1562                 desc_available -= tsnep_rx_refill_zc(rx, desc_available, false);
1563
1564         if (xsk_uses_need_wakeup(rx->xsk_pool)) {
1565                 if (desc_available)
1566                         xsk_set_rx_need_wakeup(rx->xsk_pool);
1567                 else
1568                         xsk_clear_rx_need_wakeup(rx->xsk_pool);
1569
1570                 return done;
1571         }
1572
1573         return desc_available ? budget : done;
1574 }
1575
1576 static bool tsnep_rx_pending(struct tsnep_rx *rx)
1577 {
1578         struct tsnep_rx_entry *entry;
1579
1580         if (rx->read != rx->write) {
1581                 entry = &rx->entry[rx->read];
1582                 if ((__le32_to_cpu(entry->desc_wb->properties) &
1583                      TSNEP_DESC_OWNER_COUNTER_MASK) ==
1584                     (entry->properties & TSNEP_DESC_OWNER_COUNTER_MASK))
1585                         return true;
1586         }
1587
1588         return false;
1589 }
1590
1591 static int tsnep_rx_open(struct tsnep_rx *rx)
1592 {
1593         int desc_available;
1594         int retval;
1595
1596         retval = tsnep_rx_ring_create(rx);
1597         if (retval)
1598                 return retval;
1599
1600         tsnep_rx_init(rx);
1601
1602         desc_available = tsnep_rx_desc_available(rx);
1603         if (rx->xsk_pool)
1604                 retval = tsnep_rx_alloc_zc(rx, desc_available, false);
1605         else
1606                 retval = tsnep_rx_alloc(rx, desc_available, false);
1607         if (retval != desc_available) {
1608                 retval = -ENOMEM;
1609
1610                 goto alloc_failed;
1611         }
1612
1613         /* prealloc pages to prevent allocation failures when XSK pool is
1614          * disabled at runtime
1615          */
1616         if (rx->xsk_pool) {
1617                 retval = tsnep_rx_alloc_page_buffer(rx);
1618                 if (retval)
1619                         goto alloc_failed;
1620         }
1621
1622         return 0;
1623
1624 alloc_failed:
1625         tsnep_rx_ring_cleanup(rx);
1626         return retval;
1627 }
1628
1629 static void tsnep_rx_close(struct tsnep_rx *rx)
1630 {
1631         if (rx->xsk_pool)
1632                 tsnep_rx_free_page_buffer(rx);
1633
1634         tsnep_rx_ring_cleanup(rx);
1635 }
1636
1637 static void tsnep_rx_reopen(struct tsnep_rx *rx)
1638 {
1639         struct page **page = rx->page_buffer;
1640         int i;
1641
1642         tsnep_rx_init(rx);
1643
1644         for (i = 0; i < TSNEP_RING_SIZE; i++) {
1645                 struct tsnep_rx_entry *entry = &rx->entry[i];
1646
1647                 /* defined initial values for properties are required for
1648                  * correct owner counter checking
1649                  */
1650                 entry->desc->properties = 0;
1651                 entry->desc_wb->properties = 0;
1652
1653                 /* prevent allocation failures by reusing kept pages */
1654                 if (*page) {
1655                         tsnep_rx_set_page(rx, entry, *page);
1656                         tsnep_rx_activate(rx, rx->write);
1657                         rx->write++;
1658
1659                         *page = NULL;
1660                         page++;
1661                 }
1662         }
1663 }
1664
1665 static void tsnep_rx_reopen_xsk(struct tsnep_rx *rx)
1666 {
1667         struct page **page = rx->page_buffer;
1668         u32 allocated;
1669         int i;
1670
1671         tsnep_rx_init(rx);
1672
1673         /* alloc all ring entries except the last one, because ring cannot be
1674          * filled completely, as many buffers as possible is enough as wakeup is
1675          * done if new buffers are available
1676          */
1677         allocated = xsk_buff_alloc_batch(rx->xsk_pool, rx->xdp_batch,
1678                                          TSNEP_RING_SIZE - 1);
1679
1680         for (i = 0; i < TSNEP_RING_SIZE; i++) {
1681                 struct tsnep_rx_entry *entry = &rx->entry[i];
1682
1683                 /* keep pages to prevent allocation failures when xsk is
1684                  * disabled
1685                  */
1686                 if (entry->page) {
1687                         *page = entry->page;
1688                         entry->page = NULL;
1689
1690                         page++;
1691                 }
1692
1693                 /* defined initial values for properties are required for
1694                  * correct owner counter checking
1695                  */
1696                 entry->desc->properties = 0;
1697                 entry->desc_wb->properties = 0;
1698
1699                 if (allocated) {
1700                         tsnep_rx_set_xdp(rx, entry,
1701                                          rx->xdp_batch[allocated - 1]);
1702                         tsnep_rx_activate(rx, rx->write);
1703                         rx->write++;
1704
1705                         allocated--;
1706                 }
1707         }
1708 }
1709
1710 static bool tsnep_pending(struct tsnep_queue *queue)
1711 {
1712         if (queue->tx && tsnep_tx_pending(queue->tx))
1713                 return true;
1714
1715         if (queue->rx && tsnep_rx_pending(queue->rx))
1716                 return true;
1717
1718         return false;
1719 }
1720
1721 static int tsnep_poll(struct napi_struct *napi, int budget)
1722 {
1723         struct tsnep_queue *queue = container_of(napi, struct tsnep_queue,
1724                                                  napi);
1725         bool complete = true;
1726         int done = 0;
1727
1728         if (queue->tx)
1729                 complete = tsnep_tx_poll(queue->tx, budget);
1730
1731         if (queue->rx) {
1732                 done = queue->rx->xsk_pool ?
1733                        tsnep_rx_poll_zc(queue->rx, napi, budget) :
1734                        tsnep_rx_poll(queue->rx, napi, budget);
1735                 if (done >= budget)
1736                         complete = false;
1737         }
1738
1739         /* if all work not completed, return budget and keep polling */
1740         if (!complete)
1741                 return budget;
1742
1743         if (likely(napi_complete_done(napi, done))) {
1744                 tsnep_enable_irq(queue->adapter, queue->irq_mask);
1745
1746                 /* reschedule if work is already pending, prevent rotten packets
1747                  * which are transmitted or received after polling but before
1748                  * interrupt enable
1749                  */
1750                 if (tsnep_pending(queue)) {
1751                         tsnep_disable_irq(queue->adapter, queue->irq_mask);
1752                         napi_schedule(napi);
1753                 }
1754         }
1755
1756         return min(done, budget - 1);
1757 }
1758
1759 static int tsnep_request_irq(struct tsnep_queue *queue, bool first)
1760 {
1761         const char *name = netdev_name(queue->adapter->netdev);
1762         irq_handler_t handler;
1763         void *dev;
1764         int retval;
1765
1766         if (first) {
1767                 sprintf(queue->name, "%s-mac", name);
1768                 handler = tsnep_irq;
1769                 dev = queue->adapter;
1770         } else {
1771                 if (queue->tx && queue->rx)
1772                         sprintf(queue->name, "%s-txrx-%d", name,
1773                                 queue->rx->queue_index);
1774                 else if (queue->tx)
1775                         sprintf(queue->name, "%s-tx-%d", name,
1776                                 queue->tx->queue_index);
1777                 else
1778                         sprintf(queue->name, "%s-rx-%d", name,
1779                                 queue->rx->queue_index);
1780                 handler = tsnep_irq_txrx;
1781                 dev = queue;
1782         }
1783
1784         retval = request_irq(queue->irq, handler, 0, queue->name, dev);
1785         if (retval) {
1786                 /* if name is empty, then interrupt won't be freed */
1787                 memset(queue->name, 0, sizeof(queue->name));
1788         }
1789
1790         return retval;
1791 }
1792
1793 static void tsnep_free_irq(struct tsnep_queue *queue, bool first)
1794 {
1795         void *dev;
1796
1797         if (!strlen(queue->name))
1798                 return;
1799
1800         if (first)
1801                 dev = queue->adapter;
1802         else
1803                 dev = queue;
1804
1805         free_irq(queue->irq, dev);
1806         memset(queue->name, 0, sizeof(queue->name));
1807 }
1808
1809 static void tsnep_queue_close(struct tsnep_queue *queue, bool first)
1810 {
1811         struct tsnep_rx *rx = queue->rx;
1812
1813         tsnep_free_irq(queue, first);
1814
1815         if (rx) {
1816                 if (xdp_rxq_info_is_reg(&rx->xdp_rxq))
1817                         xdp_rxq_info_unreg(&rx->xdp_rxq);
1818                 if (xdp_rxq_info_is_reg(&rx->xdp_rxq_zc))
1819                         xdp_rxq_info_unreg(&rx->xdp_rxq_zc);
1820         }
1821
1822         netif_napi_del(&queue->napi);
1823 }
1824
1825 static int tsnep_queue_open(struct tsnep_adapter *adapter,
1826                             struct tsnep_queue *queue, bool first)
1827 {
1828         struct tsnep_rx *rx = queue->rx;
1829         struct tsnep_tx *tx = queue->tx;
1830         int retval;
1831
1832         netif_napi_add(adapter->netdev, &queue->napi, tsnep_poll);
1833
1834         if (rx) {
1835                 /* choose TX queue for XDP_TX */
1836                 if (tx)
1837                         rx->tx_queue_index = tx->queue_index;
1838                 else if (rx->queue_index < adapter->num_tx_queues)
1839                         rx->tx_queue_index = rx->queue_index;
1840                 else
1841                         rx->tx_queue_index = 0;
1842
1843                 /* prepare both memory models to eliminate possible registration
1844                  * errors when memory model is switched between page pool and
1845                  * XSK pool during runtime
1846                  */
1847                 retval = xdp_rxq_info_reg(&rx->xdp_rxq, adapter->netdev,
1848                                           rx->queue_index, queue->napi.napi_id);
1849                 if (retval)
1850                         goto failed;
1851                 retval = xdp_rxq_info_reg_mem_model(&rx->xdp_rxq,
1852                                                     MEM_TYPE_PAGE_POOL,
1853                                                     rx->page_pool);
1854                 if (retval)
1855                         goto failed;
1856                 retval = xdp_rxq_info_reg(&rx->xdp_rxq_zc, adapter->netdev,
1857                                           rx->queue_index, queue->napi.napi_id);
1858                 if (retval)
1859                         goto failed;
1860                 retval = xdp_rxq_info_reg_mem_model(&rx->xdp_rxq_zc,
1861                                                     MEM_TYPE_XSK_BUFF_POOL,
1862                                                     NULL);
1863                 if (retval)
1864                         goto failed;
1865                 if (rx->xsk_pool)
1866                         xsk_pool_set_rxq_info(rx->xsk_pool, &rx->xdp_rxq_zc);
1867         }
1868
1869         retval = tsnep_request_irq(queue, first);
1870         if (retval) {
1871                 netif_err(adapter, drv, adapter->netdev,
1872                           "can't get assigned irq %d.\n", queue->irq);
1873                 goto failed;
1874         }
1875
1876         return 0;
1877
1878 failed:
1879         tsnep_queue_close(queue, first);
1880
1881         return retval;
1882 }
1883
1884 static void tsnep_queue_enable(struct tsnep_queue *queue)
1885 {
1886         napi_enable(&queue->napi);
1887         tsnep_enable_irq(queue->adapter, queue->irq_mask);
1888
1889         if (queue->tx)
1890                 tsnep_tx_enable(queue->tx);
1891
1892         if (queue->rx)
1893                 tsnep_rx_enable(queue->rx);
1894 }
1895
1896 static void tsnep_queue_disable(struct tsnep_queue *queue)
1897 {
1898         if (queue->tx)
1899                 tsnep_tx_disable(queue->tx, &queue->napi);
1900
1901         napi_disable(&queue->napi);
1902         tsnep_disable_irq(queue->adapter, queue->irq_mask);
1903
1904         /* disable RX after NAPI polling has been disabled, because RX can be
1905          * enabled during NAPI polling
1906          */
1907         if (queue->rx)
1908                 tsnep_rx_disable(queue->rx);
1909 }
1910
1911 static int tsnep_netdev_open(struct net_device *netdev)
1912 {
1913         struct tsnep_adapter *adapter = netdev_priv(netdev);
1914         int i, retval;
1915
1916         for (i = 0; i < adapter->num_queues; i++) {
1917                 if (adapter->queue[i].tx) {
1918                         retval = tsnep_tx_open(adapter->queue[i].tx);
1919                         if (retval)
1920                                 goto failed;
1921                 }
1922                 if (adapter->queue[i].rx) {
1923                         retval = tsnep_rx_open(adapter->queue[i].rx);
1924                         if (retval)
1925                                 goto failed;
1926                 }
1927
1928                 retval = tsnep_queue_open(adapter, &adapter->queue[i], i == 0);
1929                 if (retval)
1930                         goto failed;
1931         }
1932
1933         retval = netif_set_real_num_tx_queues(adapter->netdev,
1934                                               adapter->num_tx_queues);
1935         if (retval)
1936                 goto failed;
1937         retval = netif_set_real_num_rx_queues(adapter->netdev,
1938                                               adapter->num_rx_queues);
1939         if (retval)
1940                 goto failed;
1941
1942         tsnep_enable_irq(adapter, ECM_INT_LINK);
1943         retval = tsnep_phy_open(adapter);
1944         if (retval)
1945                 goto phy_failed;
1946
1947         for (i = 0; i < adapter->num_queues; i++)
1948                 tsnep_queue_enable(&adapter->queue[i]);
1949
1950         return 0;
1951
1952 phy_failed:
1953         tsnep_disable_irq(adapter, ECM_INT_LINK);
1954 failed:
1955         for (i = 0; i < adapter->num_queues; i++) {
1956                 tsnep_queue_close(&adapter->queue[i], i == 0);
1957
1958                 if (adapter->queue[i].rx)
1959                         tsnep_rx_close(adapter->queue[i].rx);
1960                 if (adapter->queue[i].tx)
1961                         tsnep_tx_close(adapter->queue[i].tx);
1962         }
1963         return retval;
1964 }
1965
1966 static int tsnep_netdev_close(struct net_device *netdev)
1967 {
1968         struct tsnep_adapter *adapter = netdev_priv(netdev);
1969         int i;
1970
1971         tsnep_disable_irq(adapter, ECM_INT_LINK);
1972         tsnep_phy_close(adapter);
1973
1974         for (i = 0; i < adapter->num_queues; i++) {
1975                 tsnep_queue_disable(&adapter->queue[i]);
1976
1977                 tsnep_queue_close(&adapter->queue[i], i == 0);
1978
1979                 if (adapter->queue[i].rx)
1980                         tsnep_rx_close(adapter->queue[i].rx);
1981                 if (adapter->queue[i].tx)
1982                         tsnep_tx_close(adapter->queue[i].tx);
1983         }
1984
1985         return 0;
1986 }
1987
1988 int tsnep_enable_xsk(struct tsnep_queue *queue, struct xsk_buff_pool *pool)
1989 {
1990         bool running = netif_running(queue->adapter->netdev);
1991         u32 frame_size;
1992
1993         frame_size = xsk_pool_get_rx_frame_size(pool);
1994         if (frame_size < TSNEP_XSK_RX_BUF_SIZE)
1995                 return -EOPNOTSUPP;
1996
1997         queue->rx->page_buffer = kcalloc(TSNEP_RING_SIZE,
1998                                          sizeof(*queue->rx->page_buffer),
1999                                          GFP_KERNEL);
2000         if (!queue->rx->page_buffer)
2001                 return -ENOMEM;
2002         queue->rx->xdp_batch = kcalloc(TSNEP_RING_SIZE,
2003                                        sizeof(*queue->rx->xdp_batch),
2004                                        GFP_KERNEL);
2005         if (!queue->rx->xdp_batch) {
2006                 kfree(queue->rx->page_buffer);
2007                 queue->rx->page_buffer = NULL;
2008
2009                 return -ENOMEM;
2010         }
2011
2012         xsk_pool_set_rxq_info(pool, &queue->rx->xdp_rxq_zc);
2013
2014         if (running)
2015                 tsnep_queue_disable(queue);
2016
2017         queue->tx->xsk_pool = pool;
2018         queue->rx->xsk_pool = pool;
2019
2020         if (running) {
2021                 tsnep_rx_reopen_xsk(queue->rx);
2022                 tsnep_queue_enable(queue);
2023         }
2024
2025         return 0;
2026 }
2027
2028 void tsnep_disable_xsk(struct tsnep_queue *queue)
2029 {
2030         bool running = netif_running(queue->adapter->netdev);
2031
2032         if (running)
2033                 tsnep_queue_disable(queue);
2034
2035         tsnep_rx_free_zc(queue->rx);
2036
2037         queue->rx->xsk_pool = NULL;
2038         queue->tx->xsk_pool = NULL;
2039
2040         if (running) {
2041                 tsnep_rx_reopen(queue->rx);
2042                 tsnep_queue_enable(queue);
2043         }
2044
2045         kfree(queue->rx->xdp_batch);
2046         queue->rx->xdp_batch = NULL;
2047         kfree(queue->rx->page_buffer);
2048         queue->rx->page_buffer = NULL;
2049 }
2050
2051 static netdev_tx_t tsnep_netdev_xmit_frame(struct sk_buff *skb,
2052                                            struct net_device *netdev)
2053 {
2054         struct tsnep_adapter *adapter = netdev_priv(netdev);
2055         u16 queue_mapping = skb_get_queue_mapping(skb);
2056
2057         if (queue_mapping >= adapter->num_tx_queues)
2058                 queue_mapping = 0;
2059
2060         return tsnep_xmit_frame_ring(skb, &adapter->tx[queue_mapping]);
2061 }
2062
2063 static int tsnep_netdev_ioctl(struct net_device *netdev, struct ifreq *ifr,
2064                               int cmd)
2065 {
2066         if (!netif_running(netdev))
2067                 return -EINVAL;
2068         if (cmd == SIOCSHWTSTAMP || cmd == SIOCGHWTSTAMP)
2069                 return tsnep_ptp_ioctl(netdev, ifr, cmd);
2070         return phy_mii_ioctl(netdev->phydev, ifr, cmd);
2071 }
2072
2073 static void tsnep_netdev_set_multicast(struct net_device *netdev)
2074 {
2075         struct tsnep_adapter *adapter = netdev_priv(netdev);
2076
2077         u16 rx_filter = 0;
2078
2079         /* configured MAC address and broadcasts are never filtered */
2080         if (netdev->flags & IFF_PROMISC) {
2081                 rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_MULTICASTS;
2082                 rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_UNICASTS;
2083         } else if (!netdev_mc_empty(netdev) || (netdev->flags & IFF_ALLMULTI)) {
2084                 rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_MULTICASTS;
2085         }
2086         iowrite16(rx_filter, adapter->addr + TSNEP_RX_FILTER);
2087 }
2088
2089 static void tsnep_netdev_get_stats64(struct net_device *netdev,
2090                                      struct rtnl_link_stats64 *stats)
2091 {
2092         struct tsnep_adapter *adapter = netdev_priv(netdev);
2093         u32 reg;
2094         u32 val;
2095         int i;
2096
2097         for (i = 0; i < adapter->num_tx_queues; i++) {
2098                 stats->tx_packets += adapter->tx[i].packets;
2099                 stats->tx_bytes += adapter->tx[i].bytes;
2100                 stats->tx_dropped += adapter->tx[i].dropped;
2101         }
2102         for (i = 0; i < adapter->num_rx_queues; i++) {
2103                 stats->rx_packets += adapter->rx[i].packets;
2104                 stats->rx_bytes += adapter->rx[i].bytes;
2105                 stats->rx_dropped += adapter->rx[i].dropped;
2106                 stats->multicast += adapter->rx[i].multicast;
2107
2108                 reg = ioread32(adapter->addr + TSNEP_QUEUE(i) +
2109                                TSNEP_RX_STATISTIC);
2110                 val = (reg & TSNEP_RX_STATISTIC_NO_DESC_MASK) >>
2111                       TSNEP_RX_STATISTIC_NO_DESC_SHIFT;
2112                 stats->rx_dropped += val;
2113                 val = (reg & TSNEP_RX_STATISTIC_BUFFER_TOO_SMALL_MASK) >>
2114                       TSNEP_RX_STATISTIC_BUFFER_TOO_SMALL_SHIFT;
2115                 stats->rx_dropped += val;
2116                 val = (reg & TSNEP_RX_STATISTIC_FIFO_OVERFLOW_MASK) >>
2117                       TSNEP_RX_STATISTIC_FIFO_OVERFLOW_SHIFT;
2118                 stats->rx_errors += val;
2119                 stats->rx_fifo_errors += val;
2120                 val = (reg & TSNEP_RX_STATISTIC_INVALID_FRAME_MASK) >>
2121                       TSNEP_RX_STATISTIC_INVALID_FRAME_SHIFT;
2122                 stats->rx_errors += val;
2123                 stats->rx_frame_errors += val;
2124         }
2125
2126         reg = ioread32(adapter->addr + ECM_STAT);
2127         val = (reg & ECM_STAT_RX_ERR_MASK) >> ECM_STAT_RX_ERR_SHIFT;
2128         stats->rx_errors += val;
2129         val = (reg & ECM_STAT_INV_FRM_MASK) >> ECM_STAT_INV_FRM_SHIFT;
2130         stats->rx_errors += val;
2131         stats->rx_crc_errors += val;
2132         val = (reg & ECM_STAT_FWD_RX_ERR_MASK) >> ECM_STAT_FWD_RX_ERR_SHIFT;
2133         stats->rx_errors += val;
2134 }
2135
2136 static void tsnep_mac_set_address(struct tsnep_adapter *adapter, u8 *addr)
2137 {
2138         iowrite32(*(u32 *)addr, adapter->addr + TSNEP_MAC_ADDRESS_LOW);
2139         iowrite16(*(u16 *)(addr + sizeof(u32)),
2140                   adapter->addr + TSNEP_MAC_ADDRESS_HIGH);
2141
2142         ether_addr_copy(adapter->mac_address, addr);
2143         netif_info(adapter, drv, adapter->netdev, "MAC address set to %pM\n",
2144                    addr);
2145 }
2146
2147 static int tsnep_netdev_set_mac_address(struct net_device *netdev, void *addr)
2148 {
2149         struct tsnep_adapter *adapter = netdev_priv(netdev);
2150         struct sockaddr *sock_addr = addr;
2151         int retval;
2152
2153         retval = eth_prepare_mac_addr_change(netdev, sock_addr);
2154         if (retval)
2155                 return retval;
2156         eth_hw_addr_set(netdev, sock_addr->sa_data);
2157         tsnep_mac_set_address(adapter, sock_addr->sa_data);
2158
2159         return 0;
2160 }
2161
2162 static int tsnep_netdev_set_features(struct net_device *netdev,
2163                                      netdev_features_t features)
2164 {
2165         struct tsnep_adapter *adapter = netdev_priv(netdev);
2166         netdev_features_t changed = netdev->features ^ features;
2167         bool enable;
2168         int retval = 0;
2169
2170         if (changed & NETIF_F_LOOPBACK) {
2171                 enable = !!(features & NETIF_F_LOOPBACK);
2172                 retval = tsnep_phy_loopback(adapter, enable);
2173         }
2174
2175         return retval;
2176 }
2177
2178 static ktime_t tsnep_netdev_get_tstamp(struct net_device *netdev,
2179                                        const struct skb_shared_hwtstamps *hwtstamps,
2180                                        bool cycles)
2181 {
2182         struct tsnep_rx_inline *rx_inline = hwtstamps->netdev_data;
2183         u64 timestamp;
2184
2185         if (cycles)
2186                 timestamp = __le64_to_cpu(rx_inline->counter);
2187         else
2188                 timestamp = __le64_to_cpu(rx_inline->timestamp);
2189
2190         return ns_to_ktime(timestamp);
2191 }
2192
2193 static int tsnep_netdev_bpf(struct net_device *dev, struct netdev_bpf *bpf)
2194 {
2195         struct tsnep_adapter *adapter = netdev_priv(dev);
2196
2197         switch (bpf->command) {
2198         case XDP_SETUP_PROG:
2199                 return tsnep_xdp_setup_prog(adapter, bpf->prog, bpf->extack);
2200         case XDP_SETUP_XSK_POOL:
2201                 return tsnep_xdp_setup_pool(adapter, bpf->xsk.pool,
2202                                             bpf->xsk.queue_id);
2203         default:
2204                 return -EOPNOTSUPP;
2205         }
2206 }
2207
2208 static struct tsnep_tx *tsnep_xdp_get_tx(struct tsnep_adapter *adapter, u32 cpu)
2209 {
2210         if (cpu >= TSNEP_MAX_QUEUES)
2211                 cpu &= TSNEP_MAX_QUEUES - 1;
2212
2213         while (cpu >= adapter->num_tx_queues)
2214                 cpu -= adapter->num_tx_queues;
2215
2216         return &adapter->tx[cpu];
2217 }
2218
2219 static int tsnep_netdev_xdp_xmit(struct net_device *dev, int n,
2220                                  struct xdp_frame **xdp, u32 flags)
2221 {
2222         struct tsnep_adapter *adapter = netdev_priv(dev);
2223         u32 cpu = smp_processor_id();
2224         struct netdev_queue *nq;
2225         struct tsnep_tx *tx;
2226         int nxmit;
2227         bool xmit;
2228
2229         if (unlikely(flags & ~XDP_XMIT_FLAGS_MASK))
2230                 return -EINVAL;
2231
2232         tx = tsnep_xdp_get_tx(adapter, cpu);
2233         nq = netdev_get_tx_queue(adapter->netdev, tx->queue_index);
2234
2235         __netif_tx_lock(nq, cpu);
2236
2237         for (nxmit = 0; nxmit < n; nxmit++) {
2238                 xmit = tsnep_xdp_xmit_frame_ring(xdp[nxmit], tx,
2239                                                  TSNEP_TX_TYPE_XDP_NDO);
2240                 if (!xmit)
2241                         break;
2242
2243                 /* avoid transmit queue timeout since we share it with the slow
2244                  * path
2245                  */
2246                 txq_trans_cond_update(nq);
2247         }
2248
2249         if (flags & XDP_XMIT_FLUSH)
2250                 tsnep_xdp_xmit_flush(tx);
2251
2252         __netif_tx_unlock(nq);
2253
2254         return nxmit;
2255 }
2256
2257 static int tsnep_netdev_xsk_wakeup(struct net_device *dev, u32 queue_id,
2258                                    u32 flags)
2259 {
2260         struct tsnep_adapter *adapter = netdev_priv(dev);
2261         struct tsnep_queue *queue;
2262
2263         if (queue_id >= adapter->num_rx_queues ||
2264             queue_id >= adapter->num_tx_queues)
2265                 return -EINVAL;
2266
2267         queue = &adapter->queue[queue_id];
2268
2269         if (!napi_if_scheduled_mark_missed(&queue->napi))
2270                 napi_schedule(&queue->napi);
2271
2272         return 0;
2273 }
2274
2275 static const struct net_device_ops tsnep_netdev_ops = {
2276         .ndo_open = tsnep_netdev_open,
2277         .ndo_stop = tsnep_netdev_close,
2278         .ndo_start_xmit = tsnep_netdev_xmit_frame,
2279         .ndo_eth_ioctl = tsnep_netdev_ioctl,
2280         .ndo_set_rx_mode = tsnep_netdev_set_multicast,
2281         .ndo_get_stats64 = tsnep_netdev_get_stats64,
2282         .ndo_set_mac_address = tsnep_netdev_set_mac_address,
2283         .ndo_set_features = tsnep_netdev_set_features,
2284         .ndo_get_tstamp = tsnep_netdev_get_tstamp,
2285         .ndo_setup_tc = tsnep_tc_setup,
2286         .ndo_bpf = tsnep_netdev_bpf,
2287         .ndo_xdp_xmit = tsnep_netdev_xdp_xmit,
2288         .ndo_xsk_wakeup = tsnep_netdev_xsk_wakeup,
2289 };
2290
2291 static int tsnep_mac_init(struct tsnep_adapter *adapter)
2292 {
2293         int retval;
2294
2295         /* initialize RX filtering, at least configured MAC address and
2296          * broadcast are not filtered
2297          */
2298         iowrite16(0, adapter->addr + TSNEP_RX_FILTER);
2299
2300         /* try to get MAC address in the following order:
2301          * - device tree
2302          * - valid MAC address already set
2303          * - MAC address register if valid
2304          * - random MAC address
2305          */
2306         retval = of_get_mac_address(adapter->pdev->dev.of_node,
2307                                     adapter->mac_address);
2308         if (retval == -EPROBE_DEFER)
2309                 return retval;
2310         if (retval && !is_valid_ether_addr(adapter->mac_address)) {
2311                 *(u32 *)adapter->mac_address =
2312                         ioread32(adapter->addr + TSNEP_MAC_ADDRESS_LOW);
2313                 *(u16 *)(adapter->mac_address + sizeof(u32)) =
2314                         ioread16(adapter->addr + TSNEP_MAC_ADDRESS_HIGH);
2315                 if (!is_valid_ether_addr(adapter->mac_address))
2316                         eth_random_addr(adapter->mac_address);
2317         }
2318
2319         tsnep_mac_set_address(adapter, adapter->mac_address);
2320         eth_hw_addr_set(adapter->netdev, adapter->mac_address);
2321
2322         return 0;
2323 }
2324
2325 static int tsnep_mdio_init(struct tsnep_adapter *adapter)
2326 {
2327         struct device_node *np = adapter->pdev->dev.of_node;
2328         int retval;
2329
2330         if (np) {
2331                 np = of_get_child_by_name(np, "mdio");
2332                 if (!np)
2333                         return 0;
2334
2335                 adapter->suppress_preamble =
2336                         of_property_read_bool(np, "suppress-preamble");
2337         }
2338
2339         adapter->mdiobus = devm_mdiobus_alloc(&adapter->pdev->dev);
2340         if (!adapter->mdiobus) {
2341                 retval = -ENOMEM;
2342
2343                 goto out;
2344         }
2345
2346         adapter->mdiobus->priv = (void *)adapter;
2347         adapter->mdiobus->parent = &adapter->pdev->dev;
2348         adapter->mdiobus->read = tsnep_mdiobus_read;
2349         adapter->mdiobus->write = tsnep_mdiobus_write;
2350         adapter->mdiobus->name = TSNEP "-mdiobus";
2351         snprintf(adapter->mdiobus->id, MII_BUS_ID_SIZE, "%s",
2352                  adapter->pdev->name);
2353
2354         /* do not scan broadcast address */
2355         adapter->mdiobus->phy_mask = 0x0000001;
2356
2357         retval = of_mdiobus_register(adapter->mdiobus, np);
2358
2359 out:
2360         of_node_put(np);
2361
2362         return retval;
2363 }
2364
2365 static int tsnep_phy_init(struct tsnep_adapter *adapter)
2366 {
2367         struct device_node *phy_node;
2368         int retval;
2369
2370         retval = of_get_phy_mode(adapter->pdev->dev.of_node,
2371                                  &adapter->phy_mode);
2372         if (retval)
2373                 adapter->phy_mode = PHY_INTERFACE_MODE_GMII;
2374
2375         phy_node = of_parse_phandle(adapter->pdev->dev.of_node, "phy-handle",
2376                                     0);
2377         adapter->phydev = of_phy_find_device(phy_node);
2378         of_node_put(phy_node);
2379         if (!adapter->phydev && adapter->mdiobus)
2380                 adapter->phydev = phy_find_first(adapter->mdiobus);
2381         if (!adapter->phydev)
2382                 return -EIO;
2383
2384         return 0;
2385 }
2386
2387 static int tsnep_queue_init(struct tsnep_adapter *adapter, int queue_count)
2388 {
2389         u32 irq_mask = ECM_INT_TX_0 | ECM_INT_RX_0;
2390         char name[8];
2391         int i;
2392         int retval;
2393
2394         /* one TX/RX queue pair for netdev is mandatory */
2395         if (platform_irq_count(adapter->pdev) == 1)
2396                 retval = platform_get_irq(adapter->pdev, 0);
2397         else
2398                 retval = platform_get_irq_byname(adapter->pdev, "mac");
2399         if (retval < 0)
2400                 return retval;
2401         adapter->num_tx_queues = 1;
2402         adapter->num_rx_queues = 1;
2403         adapter->num_queues = 1;
2404         adapter->queue[0].adapter = adapter;
2405         adapter->queue[0].irq = retval;
2406         adapter->queue[0].tx = &adapter->tx[0];
2407         adapter->queue[0].tx->adapter = adapter;
2408         adapter->queue[0].tx->addr = adapter->addr + TSNEP_QUEUE(0);
2409         adapter->queue[0].tx->queue_index = 0;
2410         adapter->queue[0].rx = &adapter->rx[0];
2411         adapter->queue[0].rx->adapter = adapter;
2412         adapter->queue[0].rx->addr = adapter->addr + TSNEP_QUEUE(0);
2413         adapter->queue[0].rx->queue_index = 0;
2414         adapter->queue[0].irq_mask = irq_mask;
2415         adapter->queue[0].irq_delay_addr = adapter->addr + ECM_INT_DELAY;
2416         retval = tsnep_set_irq_coalesce(&adapter->queue[0],
2417                                         TSNEP_COALESCE_USECS_DEFAULT);
2418         if (retval < 0)
2419                 return retval;
2420
2421         adapter->netdev->irq = adapter->queue[0].irq;
2422
2423         /* add additional TX/RX queue pairs only if dedicated interrupt is
2424          * available
2425          */
2426         for (i = 1; i < queue_count; i++) {
2427                 sprintf(name, "txrx-%d", i);
2428                 retval = platform_get_irq_byname_optional(adapter->pdev, name);
2429                 if (retval < 0)
2430                         break;
2431
2432                 adapter->num_tx_queues++;
2433                 adapter->num_rx_queues++;
2434                 adapter->num_queues++;
2435                 adapter->queue[i].adapter = adapter;
2436                 adapter->queue[i].irq = retval;
2437                 adapter->queue[i].tx = &adapter->tx[i];
2438                 adapter->queue[i].tx->adapter = adapter;
2439                 adapter->queue[i].tx->addr = adapter->addr + TSNEP_QUEUE(i);
2440                 adapter->queue[i].tx->queue_index = i;
2441                 adapter->queue[i].rx = &adapter->rx[i];
2442                 adapter->queue[i].rx->adapter = adapter;
2443                 adapter->queue[i].rx->addr = adapter->addr + TSNEP_QUEUE(i);
2444                 adapter->queue[i].rx->queue_index = i;
2445                 adapter->queue[i].irq_mask =
2446                         irq_mask << (ECM_INT_TXRX_SHIFT * i);
2447                 adapter->queue[i].irq_delay_addr =
2448                         adapter->addr + ECM_INT_DELAY + ECM_INT_DELAY_OFFSET * i;
2449                 retval = tsnep_set_irq_coalesce(&adapter->queue[i],
2450                                                 TSNEP_COALESCE_USECS_DEFAULT);
2451                 if (retval < 0)
2452                         return retval;
2453         }
2454
2455         return 0;
2456 }
2457
2458 static int tsnep_probe(struct platform_device *pdev)
2459 {
2460         struct tsnep_adapter *adapter;
2461         struct net_device *netdev;
2462         struct resource *io;
2463         u32 type;
2464         int revision;
2465         int version;
2466         int queue_count;
2467         int retval;
2468
2469         netdev = devm_alloc_etherdev_mqs(&pdev->dev,
2470                                          sizeof(struct tsnep_adapter),
2471                                          TSNEP_MAX_QUEUES, TSNEP_MAX_QUEUES);
2472         if (!netdev)
2473                 return -ENODEV;
2474         SET_NETDEV_DEV(netdev, &pdev->dev);
2475         adapter = netdev_priv(netdev);
2476         platform_set_drvdata(pdev, adapter);
2477         adapter->pdev = pdev;
2478         adapter->dmadev = &pdev->dev;
2479         adapter->netdev = netdev;
2480         adapter->msg_enable = NETIF_MSG_DRV | NETIF_MSG_PROBE |
2481                               NETIF_MSG_LINK | NETIF_MSG_IFUP |
2482                               NETIF_MSG_IFDOWN | NETIF_MSG_TX_QUEUED;
2483
2484         netdev->min_mtu = ETH_MIN_MTU;
2485         netdev->max_mtu = TSNEP_MAX_FRAME_SIZE;
2486
2487         mutex_init(&adapter->gate_control_lock);
2488         mutex_init(&adapter->rxnfc_lock);
2489         INIT_LIST_HEAD(&adapter->rxnfc_rules);
2490
2491         io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
2492         adapter->addr = devm_ioremap_resource(&pdev->dev, io);
2493         if (IS_ERR(adapter->addr))
2494                 return PTR_ERR(adapter->addr);
2495         netdev->mem_start = io->start;
2496         netdev->mem_end = io->end;
2497
2498         type = ioread32(adapter->addr + ECM_TYPE);
2499         revision = (type & ECM_REVISION_MASK) >> ECM_REVISION_SHIFT;
2500         version = (type & ECM_VERSION_MASK) >> ECM_VERSION_SHIFT;
2501         queue_count = (type & ECM_QUEUE_COUNT_MASK) >> ECM_QUEUE_COUNT_SHIFT;
2502         adapter->gate_control = type & ECM_GATE_CONTROL;
2503         adapter->rxnfc_max = TSNEP_RX_ASSIGN_ETHER_TYPE_COUNT;
2504
2505         tsnep_disable_irq(adapter, ECM_INT_ALL);
2506
2507         retval = tsnep_queue_init(adapter, queue_count);
2508         if (retval)
2509                 return retval;
2510
2511         retval = dma_set_mask_and_coherent(&adapter->pdev->dev,
2512                                            DMA_BIT_MASK(64));
2513         if (retval) {
2514                 dev_err(&adapter->pdev->dev, "no usable DMA configuration.\n");
2515                 return retval;
2516         }
2517
2518         retval = tsnep_mac_init(adapter);
2519         if (retval)
2520                 return retval;
2521
2522         retval = tsnep_mdio_init(adapter);
2523         if (retval)
2524                 goto mdio_init_failed;
2525
2526         retval = tsnep_phy_init(adapter);
2527         if (retval)
2528                 goto phy_init_failed;
2529
2530         retval = tsnep_ptp_init(adapter);
2531         if (retval)
2532                 goto ptp_init_failed;
2533
2534         retval = tsnep_tc_init(adapter);
2535         if (retval)
2536                 goto tc_init_failed;
2537
2538         retval = tsnep_rxnfc_init(adapter);
2539         if (retval)
2540                 goto rxnfc_init_failed;
2541
2542         netdev->netdev_ops = &tsnep_netdev_ops;
2543         netdev->ethtool_ops = &tsnep_ethtool_ops;
2544         netdev->features = NETIF_F_SG;
2545         netdev->hw_features = netdev->features | NETIF_F_LOOPBACK;
2546
2547         netdev->xdp_features = NETDEV_XDP_ACT_BASIC | NETDEV_XDP_ACT_REDIRECT |
2548                                NETDEV_XDP_ACT_NDO_XMIT |
2549                                NETDEV_XDP_ACT_NDO_XMIT_SG |
2550                                NETDEV_XDP_ACT_XSK_ZEROCOPY;
2551
2552         /* carrier off reporting is important to ethtool even BEFORE open */
2553         netif_carrier_off(netdev);
2554
2555         retval = register_netdev(netdev);
2556         if (retval)
2557                 goto register_failed;
2558
2559         dev_info(&adapter->pdev->dev, "device version %d.%02d\n", version,
2560                  revision);
2561         if (adapter->gate_control)
2562                 dev_info(&adapter->pdev->dev, "gate control detected\n");
2563
2564         return 0;
2565
2566 register_failed:
2567         tsnep_rxnfc_cleanup(adapter);
2568 rxnfc_init_failed:
2569         tsnep_tc_cleanup(adapter);
2570 tc_init_failed:
2571         tsnep_ptp_cleanup(adapter);
2572 ptp_init_failed:
2573 phy_init_failed:
2574         if (adapter->mdiobus)
2575                 mdiobus_unregister(adapter->mdiobus);
2576 mdio_init_failed:
2577         return retval;
2578 }
2579
2580 static int tsnep_remove(struct platform_device *pdev)
2581 {
2582         struct tsnep_adapter *adapter = platform_get_drvdata(pdev);
2583
2584         unregister_netdev(adapter->netdev);
2585
2586         tsnep_rxnfc_cleanup(adapter);
2587
2588         tsnep_tc_cleanup(adapter);
2589
2590         tsnep_ptp_cleanup(adapter);
2591
2592         if (adapter->mdiobus)
2593                 mdiobus_unregister(adapter->mdiobus);
2594
2595         tsnep_disable_irq(adapter, ECM_INT_ALL);
2596
2597         return 0;
2598 }
2599
2600 static const struct of_device_id tsnep_of_match[] = {
2601         { .compatible = "engleder,tsnep", },
2602 { },
2603 };
2604 MODULE_DEVICE_TABLE(of, tsnep_of_match);
2605
2606 static struct platform_driver tsnep_driver = {
2607         .driver = {
2608                 .name = TSNEP,
2609                 .of_match_table = tsnep_of_match,
2610         },
2611         .probe = tsnep_probe,
2612         .remove = tsnep_remove,
2613 };
2614 module_platform_driver(tsnep_driver);
2615
2616 MODULE_AUTHOR("Gerhard Engleder <gerhard@engleder-embedded.com>");
2617 MODULE_DESCRIPTION("TSN endpoint Ethernet MAC driver");
2618 MODULE_LICENSE("GPL");