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