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
  30 #define TSNEP_SKB_PAD (NET_SKB_PAD + NET_IP_ALIGN)
  31 #define TSNEP_HEADROOM ALIGN(TSNEP_SKB_PAD, 4)
  32 #define TSNEP_MAX_RX_BUF_SIZE (PAGE_SIZE - TSNEP_HEADROOM - \
  33                                SKB_DATA_ALIGN(sizeof(struct skb_shared_info)))
  34
  35 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
  36 #define DMA_ADDR_HIGH(dma_addr) ((u32)(((dma_addr) >> 32) & 0xFFFFFFFF))
  37 #else
  38 #define DMA_ADDR_HIGH(dma_addr) ((u32)(0))
  39 #endif
  40 #define DMA_ADDR_LOW(dma_addr) ((u32)((dma_addr) & 0xFFFFFFFF))
  41
  42 #define TSNEP_COALESCE_USECS_DEFAULT 64
  43 #define TSNEP_COALESCE_USECS_MAX     ((ECM_INT_DELAY_MASK >> ECM_INT_DELAY_SHIFT) * \
  44                                       ECM_INT_DELAY_BASE_US + ECM_INT_DELAY_BASE_US - 1)
  45
  46 static void tsnep_enable_irq(struct tsnep_adapter *adapter, u32 mask)
  47 {
  48         iowrite32(mask, adapter->addr + ECM_INT_ENABLE);
  49 }
  50
  51 static void tsnep_disable_irq(struct tsnep_adapter *adapter, u32 mask)
  52 {
  53         mask |= ECM_INT_DISABLE;
  54         iowrite32(mask, adapter->addr + ECM_INT_ENABLE);
  55 }
  56
  57 static irqreturn_t tsnep_irq(int irq, void *arg)
  58 {
  59         struct tsnep_adapter *adapter = arg;
  60         u32 active = ioread32(adapter->addr + ECM_INT_ACTIVE);
  61
  62         /* acknowledge interrupt */
  63         if (active != 0)
  64                 iowrite32(active, adapter->addr + ECM_INT_ACKNOWLEDGE);
  65
  66         /* handle link interrupt */
  67         if ((active & ECM_INT_LINK) != 0)
  68                 phy_mac_interrupt(adapter->netdev->phydev);
  69
  70         /* handle TX/RX queue 0 interrupt */
  71         if ((active & adapter->queue[0].irq_mask) != 0) {
  72                 tsnep_disable_irq(adapter, adapter->queue[0].irq_mask);
  73                 napi_schedule(&adapter->queue[0].napi);
  74         }
  75
  76         return IRQ_HANDLED;
  77 }
  78
  79 static irqreturn_t tsnep_irq_txrx(int irq, void *arg)
  80 {
  81         struct tsnep_queue *queue = arg;
  82
  83         /* handle TX/RX queue interrupt */
  84         tsnep_disable_irq(queue->adapter, queue->irq_mask);
  85         napi_schedule(&queue->napi);
  86
  87         return IRQ_HANDLED;
  88 }
  89
  90 int tsnep_set_irq_coalesce(struct tsnep_queue *queue, u32 usecs)
  91 {
  92         if (usecs > TSNEP_COALESCE_USECS_MAX)
  93                 return -ERANGE;
  94
  95         usecs /= ECM_INT_DELAY_BASE_US;
  96         usecs <<= ECM_INT_DELAY_SHIFT;
  97         usecs &= ECM_INT_DELAY_MASK;
  98
  99         queue->irq_delay &= ~ECM_INT_DELAY_MASK;
 100         queue->irq_delay |= usecs;
 101         iowrite8(queue->irq_delay, queue->irq_delay_addr);
 102
 103         return 0;
 104 }
 105
 106 u32 tsnep_get_irq_coalesce(struct tsnep_queue *queue)
 107 {
 108         u32 usecs;
 109
 110         usecs = (queue->irq_delay & ECM_INT_DELAY_MASK);
 111         usecs >>= ECM_INT_DELAY_SHIFT;
 112         usecs *= ECM_INT_DELAY_BASE_US;
 113
 114         return usecs;
 115 }
 116
 117 static int tsnep_mdiobus_read(struct mii_bus *bus, int addr, int regnum)
 118 {
 119         struct tsnep_adapter *adapter = bus->priv;
 120         u32 md;
 121         int retval;
 122
 123         if (regnum & MII_ADDR_C45)
 124                 return -EOPNOTSUPP;
 125
 126         md = ECM_MD_READ;
 127         if (!adapter->suppress_preamble)
 128                 md |= ECM_MD_PREAMBLE;
 129         md |= (regnum << ECM_MD_ADDR_SHIFT) & ECM_MD_ADDR_MASK;
 130         md |= (addr << ECM_MD_PHY_ADDR_SHIFT) & ECM_MD_PHY_ADDR_MASK;
 131         iowrite32(md, adapter->addr + ECM_MD_CONTROL);
 132         retval = readl_poll_timeout_atomic(adapter->addr + ECM_MD_STATUS, md,
 133                                            !(md & ECM_MD_BUSY), 16, 1000);
 134         if (retval != 0)
 135                 return retval;
 136
 137         return (md & ECM_MD_DATA_MASK) >> ECM_MD_DATA_SHIFT;
 138 }
 139
 140 static int tsnep_mdiobus_write(struct mii_bus *bus, int addr, int regnum,
 141                                u16 val)
 142 {
 143         struct tsnep_adapter *adapter = bus->priv;
 144         u32 md;
 145         int retval;
 146
 147         if (regnum & MII_ADDR_C45)
 148                 return -EOPNOTSUPP;
 149
 150         md = ECM_MD_WRITE;
 151         if (!adapter->suppress_preamble)
 152                 md |= ECM_MD_PREAMBLE;
 153         md |= (regnum << ECM_MD_ADDR_SHIFT) & ECM_MD_ADDR_MASK;
 154         md |= (addr << ECM_MD_PHY_ADDR_SHIFT) & ECM_MD_PHY_ADDR_MASK;
 155         md |= ((u32)val << ECM_MD_DATA_SHIFT) & ECM_MD_DATA_MASK;
 156         iowrite32(md, adapter->addr + ECM_MD_CONTROL);
 157         retval = readl_poll_timeout_atomic(adapter->addr + ECM_MD_STATUS, md,
 158                                            !(md & ECM_MD_BUSY), 16, 1000);
 159         if (retval != 0)
 160                 return retval;
 161
 162         return 0;
 163 }
 164
 165 static void tsnep_set_link_mode(struct tsnep_adapter *adapter)
 166 {
 167         u32 mode;
 168
 169         switch (adapter->phydev->speed) {
 170         case SPEED_100:
 171                 mode = ECM_LINK_MODE_100;
 172                 break;
 173         case SPEED_1000:
 174                 mode = ECM_LINK_MODE_1000;
 175                 break;
 176         default:
 177                 mode = ECM_LINK_MODE_OFF;
 178                 break;
 179         }
 180         iowrite32(mode, adapter->addr + ECM_STATUS);
 181 }
 182
 183 static void tsnep_phy_link_status_change(struct net_device *netdev)
 184 {
 185         struct tsnep_adapter *adapter = netdev_priv(netdev);
 186         struct phy_device *phydev = netdev->phydev;
 187
 188         if (phydev->link)
 189                 tsnep_set_link_mode(adapter);
 190
 191         phy_print_status(netdev->phydev);
 192 }
 193
 194 static int tsnep_phy_loopback(struct tsnep_adapter *adapter, bool enable)
 195 {
 196         int retval;
 197
 198         retval = phy_loopback(adapter->phydev, enable);
 199
 200         /* PHY link state change is not signaled if loopback is enabled, it
 201          * would delay a working loopback anyway, let's ensure that loopback
 202          * is working immediately by setting link mode directly
 203          */
 204         if (!retval && enable)
 205                 tsnep_set_link_mode(adapter);
 206
 207         return retval;
 208 }
 209
 210 static int tsnep_phy_open(struct tsnep_adapter *adapter)
 211 {
 212         struct phy_device *phydev;
 213         struct ethtool_eee ethtool_eee;
 214         int retval;
 215
 216         retval = phy_connect_direct(adapter->netdev, adapter->phydev,
 217                                     tsnep_phy_link_status_change,
 218                                     adapter->phy_mode);
 219         if (retval)
 220                 return retval;
 221         phydev = adapter->netdev->phydev;
 222
 223         /* MAC supports only 100Mbps|1000Mbps full duplex
 224          * SPE (Single Pair Ethernet) is also an option but not implemented yet
 225          */
 226         phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_10baseT_Half_BIT);
 227         phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_10baseT_Full_BIT);
 228         phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_100baseT_Half_BIT);
 229         phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_1000baseT_Half_BIT);
 230
 231         /* disable EEE autoneg, EEE not supported by TSNEP */
 232         memset(&ethtool_eee, 0, sizeof(ethtool_eee));
 233         phy_ethtool_set_eee(adapter->phydev, &ethtool_eee);
 234
 235         adapter->phydev->irq = PHY_MAC_INTERRUPT;
 236         phy_start(adapter->phydev);
 237
 238         return 0;
 239 }
 240
 241 static void tsnep_phy_close(struct tsnep_adapter *adapter)
 242 {
 243         phy_stop(adapter->netdev->phydev);
 244         phy_disconnect(adapter->netdev->phydev);
 245         adapter->netdev->phydev = NULL;
 246 }
 247
 248 static void tsnep_tx_ring_cleanup(struct tsnep_tx *tx)
 249 {
 250         struct device *dmadev = tx->adapter->dmadev;
 251         int i;
 252
 253         memset(tx->entry, 0, sizeof(tx->entry));
 254
 255         for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
 256                 if (tx->page[i]) {
 257                         dma_free_coherent(dmadev, PAGE_SIZE, tx->page[i],
 258                                           tx->page_dma[i]);
 259                         tx->page[i] = NULL;
 260                         tx->page_dma[i] = 0;
 261                 }
 262         }
 263 }
 264
 265 static int tsnep_tx_ring_init(struct tsnep_tx *tx)
 266 {
 267         struct device *dmadev = tx->adapter->dmadev;
 268         struct tsnep_tx_entry *entry;
 269         struct tsnep_tx_entry *next_entry;
 270         int i, j;
 271         int retval;
 272
 273         for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
 274                 tx->page[i] =
 275                         dma_alloc_coherent(dmadev, PAGE_SIZE, &tx->page_dma[i],
 276                                            GFP_KERNEL);
 277                 if (!tx->page[i]) {
 278                         retval = -ENOMEM;
 279                         goto alloc_failed;
 280                 }
 281                 for (j = 0; j < TSNEP_RING_ENTRIES_PER_PAGE; j++) {
 282                         entry = &tx->entry[TSNEP_RING_ENTRIES_PER_PAGE * i + j];
 283                         entry->desc_wb = (struct tsnep_tx_desc_wb *)
 284                                 (((u8 *)tx->page[i]) + TSNEP_DESC_SIZE * j);
 285                         entry->desc = (struct tsnep_tx_desc *)
 286                                 (((u8 *)entry->desc_wb) + TSNEP_DESC_OFFSET);
 287                         entry->desc_dma = tx->page_dma[i] + TSNEP_DESC_SIZE * j;
 288                 }
 289         }
 290         for (i = 0; i < TSNEP_RING_SIZE; i++) {
 291                 entry = &tx->entry[i];
 292                 next_entry = &tx->entry[(i + 1) % TSNEP_RING_SIZE];
 293                 entry->desc->next = __cpu_to_le64(next_entry->desc_dma);
 294         }
 295
 296         return 0;
 297
 298 alloc_failed:
 299         tsnep_tx_ring_cleanup(tx);
 300         return retval;
 301 }
 302
 303 static void tsnep_tx_activate(struct tsnep_tx *tx, int index, int length,
 304                               bool last)
 305 {
 306         struct tsnep_tx_entry *entry = &tx->entry[index];
 307
 308         entry->properties = 0;
 309         if (entry->skb) {
 310                 entry->properties = length & TSNEP_DESC_LENGTH_MASK;
 311                 entry->properties |= TSNEP_DESC_INTERRUPT_FLAG;
 312                 if (skb_shinfo(entry->skb)->tx_flags & SKBTX_IN_PROGRESS)
 313                         entry->properties |= TSNEP_DESC_EXTENDED_WRITEBACK_FLAG;
 314
 315                 /* toggle user flag to prevent false acknowledge
 316                  *
 317                  * Only the first fragment is acknowledged. For all other
 318                  * fragments no acknowledge is done and the last written owner
 319                  * counter stays in the writeback descriptor. Therefore, it is
 320                  * possible that the last written owner counter is identical to
 321                  * the new incremented owner counter and a false acknowledge is
 322                  * detected before the real acknowledge has been done by
 323                  * hardware.
 324                  *
 325                  * The user flag is used to prevent this situation. The user
 326                  * flag is copied to the writeback descriptor by the hardware
 327                  * and is used as additional acknowledge data. By toggeling the
 328                  * user flag only for the first fragment (which is
 329                  * acknowledged), it is guaranteed that the last acknowledge
 330                  * done for this descriptor has used a different user flag and
 331                  * cannot be detected as false acknowledge.
 332                  */
 333                 entry->owner_user_flag = !entry->owner_user_flag;
 334         }
 335         if (last)
 336                 entry->properties |= TSNEP_TX_DESC_LAST_FRAGMENT_FLAG;
 337         if (index == tx->increment_owner_counter) {
 338                 tx->owner_counter++;
 339                 if (tx->owner_counter == 4)
 340                         tx->owner_counter = 1;
 341                 tx->increment_owner_counter--;
 342                 if (tx->increment_owner_counter < 0)
 343                         tx->increment_owner_counter = TSNEP_RING_SIZE - 1;
 344         }
 345         entry->properties |=
 346                 (tx->owner_counter << TSNEP_DESC_OWNER_COUNTER_SHIFT) &
 347                 TSNEP_DESC_OWNER_COUNTER_MASK;
 348         if (entry->owner_user_flag)
 349                 entry->properties |= TSNEP_TX_DESC_OWNER_USER_FLAG;
 350         entry->desc->more_properties =
 351                 __cpu_to_le32(entry->len & TSNEP_DESC_LENGTH_MASK);
 352
 353         /* descriptor properties shall be written last, because valid data is
 354          * signaled there
 355          */
 356         dma_wmb();
 357
 358         entry->desc->properties = __cpu_to_le32(entry->properties);
 359 }
 360
 361 static int tsnep_tx_desc_available(struct tsnep_tx *tx)
 362 {
 363         if (tx->read <= tx->write)
 364                 return TSNEP_RING_SIZE - tx->write + tx->read - 1;
 365         else
 366                 return tx->read - tx->write - 1;
 367 }
 368
 369 static int tsnep_tx_map(struct sk_buff *skb, struct tsnep_tx *tx, int count)
 370 {
 371         struct device *dmadev = tx->adapter->dmadev;
 372         struct tsnep_tx_entry *entry;
 373         unsigned int len;
 374         dma_addr_t dma;
 375         int map_len = 0;
 376         int i;
 377
 378         for (i = 0; i < count; i++) {
 379                 entry = &tx->entry[(tx->write + i) % TSNEP_RING_SIZE];
 380
 381                 if (i == 0) {
 382                         len = skb_headlen(skb);
 383                         dma = dma_map_single(dmadev, skb->data, len,
 384                                              DMA_TO_DEVICE);
 385                 } else {
 386                         len = skb_frag_size(&skb_shinfo(skb)->frags[i - 1]);
 387                         dma = skb_frag_dma_map(dmadev,
 388                                                &skb_shinfo(skb)->frags[i - 1],
 389                                                0, len, DMA_TO_DEVICE);
 390                 }
 391                 if (dma_mapping_error(dmadev, dma))
 392                         return -ENOMEM;
 393
 394                 entry->len = len;
 395                 dma_unmap_addr_set(entry, dma, dma);
 396
 397                 entry->desc->tx = __cpu_to_le64(dma);
 398
 399                 map_len += len;
 400         }
 401
 402         return map_len;
 403 }
 404
 405 static int tsnep_tx_unmap(struct tsnep_tx *tx, int index, int count)
 406 {
 407         struct device *dmadev = tx->adapter->dmadev;
 408         struct tsnep_tx_entry *entry;
 409         int map_len = 0;
 410         int i;
 411
 412         for (i = 0; i < count; i++) {
 413                 entry = &tx->entry[(index + i) % TSNEP_RING_SIZE];
 414
 415                 if (entry->len) {
 416                         if (i == 0)
 417                                 dma_unmap_single(dmadev,
 418                                                  dma_unmap_addr(entry, dma),
 419                                                  dma_unmap_len(entry, len),
 420                                                  DMA_TO_DEVICE);
 421                         else
 422                                 dma_unmap_page(dmadev,
 423                                                dma_unmap_addr(entry, dma),
 424                                                dma_unmap_len(entry, len),
 425                                                DMA_TO_DEVICE);
 426                         map_len += entry->len;
 427                         entry->len = 0;
 428                 }
 429         }
 430
 431         return map_len;
 432 }
 433
 434 static netdev_tx_t tsnep_xmit_frame_ring(struct sk_buff *skb,
 435                                          struct tsnep_tx *tx)
 436 {
 437         unsigned long flags;
 438         int count = 1;
 439         struct tsnep_tx_entry *entry;
 440         int length;
 441         int i;
 442         int retval;
 443
 444         if (skb_shinfo(skb)->nr_frags > 0)
 445                 count += skb_shinfo(skb)->nr_frags;
 446
 447         spin_lock_irqsave(&tx->lock, flags);
 448
 449         if (tsnep_tx_desc_available(tx) < count) {
 450                 /* ring full, shall not happen because queue is stopped if full
 451                  * below
 452                  */
 453                 netif_stop_queue(tx->adapter->netdev);
 454
 455                 spin_unlock_irqrestore(&tx->lock, flags);
 456
 457                 return NETDEV_TX_BUSY;
 458         }
 459
 460         entry = &tx->entry[tx->write];
 461         entry->skb = skb;
 462
 463         retval = tsnep_tx_map(skb, tx, count);
 464         if (retval < 0) {
 465                 tsnep_tx_unmap(tx, tx->write, count);
 466                 dev_kfree_skb_any(entry->skb);
 467                 entry->skb = NULL;
 468
 469                 tx->dropped++;
 470
 471                 spin_unlock_irqrestore(&tx->lock, flags);
 472
 473                 netdev_err(tx->adapter->netdev, "TX DMA map failed\n");
 474
 475                 return NETDEV_TX_OK;
 476         }
 477         length = retval;
 478
 479         if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)
 480                 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 481
 482         for (i = 0; i < count; i++)
 483                 tsnep_tx_activate(tx, (tx->write + i) % TSNEP_RING_SIZE, length,
 484                                   i == (count - 1));
 485         tx->write = (tx->write + count) % TSNEP_RING_SIZE;
 486
 487         skb_tx_timestamp(skb);
 488
 489         /* descriptor properties shall be valid before hardware is notified */
 490         dma_wmb();
 491
 492         iowrite32(TSNEP_CONTROL_TX_ENABLE, tx->addr + TSNEP_CONTROL);
 493
 494         if (tsnep_tx_desc_available(tx) < (MAX_SKB_FRAGS + 1)) {
 495                 /* ring can get full with next frame */
 496                 netif_stop_queue(tx->adapter->netdev);
 497         }
 498
 499         spin_unlock_irqrestore(&tx->lock, flags);
 500
 501         return NETDEV_TX_OK;
 502 }
 503
 504 static bool tsnep_tx_poll(struct tsnep_tx *tx, int napi_budget)
 505 {
 506         unsigned long flags;
 507         int budget = 128;
 508         struct tsnep_tx_entry *entry;
 509         int count;
 510         int length;
 511
 512         spin_lock_irqsave(&tx->lock, flags);
 513
 514         do {
 515                 if (tx->read == tx->write)
 516                         break;
 517
 518                 entry = &tx->entry[tx->read];
 519                 if ((__le32_to_cpu(entry->desc_wb->properties) &
 520                      TSNEP_TX_DESC_OWNER_MASK) !=
 521                     (entry->properties & TSNEP_TX_DESC_OWNER_MASK))
 522                         break;
 523
 524                 /* descriptor properties shall be read first, because valid data
 525                  * is signaled there
 526                  */
 527                 dma_rmb();
 528
 529                 count = 1;
 530                 if (skb_shinfo(entry->skb)->nr_frags > 0)
 531                         count += skb_shinfo(entry->skb)->nr_frags;
 532
 533                 length = tsnep_tx_unmap(tx, tx->read, count);
 534
 535                 if ((skb_shinfo(entry->skb)->tx_flags & SKBTX_IN_PROGRESS) &&
 536                     (__le32_to_cpu(entry->desc_wb->properties) &
 537                      TSNEP_DESC_EXTENDED_WRITEBACK_FLAG)) {
 538                         struct skb_shared_hwtstamps hwtstamps;
 539                         u64 timestamp;
 540
 541                         if (skb_shinfo(entry->skb)->tx_flags &
 542                             SKBTX_HW_TSTAMP_USE_CYCLES)
 543                                 timestamp =
 544                                         __le64_to_cpu(entry->desc_wb->counter);
 545                         else
 546                                 timestamp =
 547                                         __le64_to_cpu(entry->desc_wb->timestamp);
 548
 549                         memset(&hwtstamps, 0, sizeof(hwtstamps));
 550                         hwtstamps.hwtstamp = ns_to_ktime(timestamp);
 551
 552                         skb_tstamp_tx(entry->skb, &hwtstamps);
 553                 }
 554
 555                 napi_consume_skb(entry->skb, budget);
 556                 entry->skb = NULL;
 557
 558                 tx->read = (tx->read + count) % TSNEP_RING_SIZE;
 559
 560                 tx->packets++;
 561                 tx->bytes += length + ETH_FCS_LEN;
 562
 563                 budget--;
 564         } while (likely(budget));
 565
 566         if ((tsnep_tx_desc_available(tx) >= ((MAX_SKB_FRAGS + 1) * 2)) &&
 567             netif_queue_stopped(tx->adapter->netdev)) {
 568                 netif_wake_queue(tx->adapter->netdev);
 569         }
 570
 571         spin_unlock_irqrestore(&tx->lock, flags);
 572
 573         return (budget != 0);
 574 }
 575
 576 static bool tsnep_tx_pending(struct tsnep_tx *tx)
 577 {
 578         unsigned long flags;
 579         struct tsnep_tx_entry *entry;
 580         bool pending = false;
 581
 582         spin_lock_irqsave(&tx->lock, flags);
 583
 584         if (tx->read != tx->write) {
 585                 entry = &tx->entry[tx->read];
 586                 if ((__le32_to_cpu(entry->desc_wb->properties) &
 587                      TSNEP_TX_DESC_OWNER_MASK) ==
 588                     (entry->properties & TSNEP_TX_DESC_OWNER_MASK))
 589                         pending = true;
 590         }
 591
 592         spin_unlock_irqrestore(&tx->lock, flags);
 593
 594         return pending;
 595 }
 596
 597 static int tsnep_tx_open(struct tsnep_adapter *adapter, void __iomem *addr,
 598                          int queue_index, struct tsnep_tx *tx)
 599 {
 600         dma_addr_t dma;
 601         int retval;
 602
 603         memset(tx, 0, sizeof(*tx));
 604         tx->adapter = adapter;
 605         tx->addr = addr;
 606         tx->queue_index = queue_index;
 607
 608         retval = tsnep_tx_ring_init(tx);
 609         if (retval)
 610                 return retval;
 611
 612         dma = tx->entry[0].desc_dma | TSNEP_RESET_OWNER_COUNTER;
 613         iowrite32(DMA_ADDR_LOW(dma), tx->addr + TSNEP_TX_DESC_ADDR_LOW);
 614         iowrite32(DMA_ADDR_HIGH(dma), tx->addr + TSNEP_TX_DESC_ADDR_HIGH);
 615         tx->owner_counter = 1;
 616         tx->increment_owner_counter = TSNEP_RING_SIZE - 1;
 617
 618         spin_lock_init(&tx->lock);
 619
 620         return 0;
 621 }
 622
 623 static void tsnep_tx_close(struct tsnep_tx *tx)
 624 {
 625         u32 val;
 626
 627         readx_poll_timeout(ioread32, tx->addr + TSNEP_CONTROL, val,
 628                            ((val & TSNEP_CONTROL_TX_ENABLE) == 0), 10000,
 629                            1000000);
 630
 631         tsnep_tx_ring_cleanup(tx);
 632 }
 633
 634 static void tsnep_rx_ring_cleanup(struct tsnep_rx *rx)
 635 {
 636         struct device *dmadev = rx->adapter->dmadev;
 637         struct tsnep_rx_entry *entry;
 638         int i;
 639
 640         for (i = 0; i < TSNEP_RING_SIZE; i++) {
 641                 entry = &rx->entry[i];
 642                 if (entry->page)
 643                         page_pool_put_full_page(rx->page_pool, entry->page,
 644                                                 false);
 645                 entry->page = NULL;
 646         }
 647
 648         if (rx->page_pool)
 649                 page_pool_destroy(rx->page_pool);
 650
 651         memset(rx->entry, 0, sizeof(rx->entry));
 652
 653         for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
 654                 if (rx->page[i]) {
 655                         dma_free_coherent(dmadev, PAGE_SIZE, rx->page[i],
 656                                           rx->page_dma[i]);
 657                         rx->page[i] = NULL;
 658                         rx->page_dma[i] = 0;
 659                 }
 660         }
 661 }
 662
 663 static int tsnep_rx_ring_init(struct tsnep_rx *rx)
 664 {
 665         struct device *dmadev = rx->adapter->dmadev;
 666         struct tsnep_rx_entry *entry;
 667         struct page_pool_params pp_params = { 0 };
 668         struct tsnep_rx_entry *next_entry;
 669         int i, j;
 670         int retval;
 671
 672         for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
 673                 rx->page[i] =
 674                         dma_alloc_coherent(dmadev, PAGE_SIZE, &rx->page_dma[i],
 675                                            GFP_KERNEL);
 676                 if (!rx->page[i]) {
 677                         retval = -ENOMEM;
 678                         goto failed;
 679                 }
 680                 for (j = 0; j < TSNEP_RING_ENTRIES_PER_PAGE; j++) {
 681                         entry = &rx->entry[TSNEP_RING_ENTRIES_PER_PAGE * i + j];
 682                         entry->desc_wb = (struct tsnep_rx_desc_wb *)
 683                                 (((u8 *)rx->page[i]) + TSNEP_DESC_SIZE * j);
 684                         entry->desc = (struct tsnep_rx_desc *)
 685                                 (((u8 *)entry->desc_wb) + TSNEP_DESC_OFFSET);
 686                         entry->desc_dma = rx->page_dma[i] + TSNEP_DESC_SIZE * j;
 687                 }
 688         }
 689
 690         pp_params.flags = PP_FLAG_DMA_MAP | PP_FLAG_DMA_SYNC_DEV;
 691         pp_params.order = 0;
 692         pp_params.pool_size = TSNEP_RING_SIZE;
 693         pp_params.nid = dev_to_node(dmadev);
 694         pp_params.dev = dmadev;
 695         pp_params.dma_dir = DMA_FROM_DEVICE;
 696         pp_params.max_len = TSNEP_MAX_RX_BUF_SIZE;
 697         pp_params.offset = TSNEP_SKB_PAD;
 698         rx->page_pool = page_pool_create(&pp_params);
 699         if (IS_ERR(rx->page_pool)) {
 700                 retval = PTR_ERR(rx->page_pool);
 701                 rx->page_pool = NULL;
 702                 goto failed;
 703         }
 704
 705         for (i = 0; i < TSNEP_RING_SIZE; i++) {
 706                 entry = &rx->entry[i];
 707                 next_entry = &rx->entry[(i + 1) % TSNEP_RING_SIZE];
 708                 entry->desc->next = __cpu_to_le64(next_entry->desc_dma);
 709         }
 710
 711         return 0;
 712
 713 failed:
 714         tsnep_rx_ring_cleanup(rx);
 715         return retval;
 716 }
 717
 718 static int tsnep_rx_desc_available(struct tsnep_rx *rx)
 719 {
 720         if (rx->read <= rx->write)
 721                 return TSNEP_RING_SIZE - rx->write + rx->read - 1;
 722         else
 723                 return rx->read - rx->write - 1;
 724 }
 725
 726 static void tsnep_rx_set_page(struct tsnep_rx *rx, struct tsnep_rx_entry *entry,
 727                               struct page *page)
 728 {
 729         entry->page = page;
 730         entry->len = TSNEP_MAX_RX_BUF_SIZE;
 731         entry->dma = page_pool_get_dma_addr(entry->page);
 732         entry->desc->rx = __cpu_to_le64(entry->dma + TSNEP_SKB_PAD);
 733 }
 734
 735 static int tsnep_rx_alloc_buffer(struct tsnep_rx *rx, int index)
 736 {
 737         struct tsnep_rx_entry *entry = &rx->entry[index];
 738         struct page *page;
 739
 740         page = page_pool_dev_alloc_pages(rx->page_pool);
 741         if (unlikely(!page))
 742                 return -ENOMEM;
 743         tsnep_rx_set_page(rx, entry, page);
 744
 745         return 0;
 746 }
 747
 748 static void tsnep_rx_reuse_buffer(struct tsnep_rx *rx, int index)
 749 {
 750         struct tsnep_rx_entry *entry = &rx->entry[index];
 751         struct tsnep_rx_entry *read = &rx->entry[rx->read];
 752
 753         tsnep_rx_set_page(rx, entry, read->page);
 754         read->page = NULL;
 755 }
 756
 757 static void tsnep_rx_activate(struct tsnep_rx *rx, int index)
 758 {
 759         struct tsnep_rx_entry *entry = &rx->entry[index];
 760
 761         /* TSNEP_MAX_RX_BUF_SIZE is a multiple of 4 */
 762         entry->properties = entry->len & TSNEP_DESC_LENGTH_MASK;
 763         entry->properties |= TSNEP_DESC_INTERRUPT_FLAG;
 764         if (index == rx->increment_owner_counter) {
 765                 rx->owner_counter++;
 766                 if (rx->owner_counter == 4)
 767                         rx->owner_counter = 1;
 768                 rx->increment_owner_counter--;
 769                 if (rx->increment_owner_counter < 0)
 770                         rx->increment_owner_counter = TSNEP_RING_SIZE - 1;
 771         }
 772         entry->properties |=
 773                 (rx->owner_counter << TSNEP_DESC_OWNER_COUNTER_SHIFT) &
 774                 TSNEP_DESC_OWNER_COUNTER_MASK;
 775
 776         /* descriptor properties shall be written last, because valid data is
 777          * signaled there
 778          */
 779         dma_wmb();
 780
 781         entry->desc->properties = __cpu_to_le32(entry->properties);
 782 }
 783
 784 static int tsnep_rx_refill(struct tsnep_rx *rx, int count, bool reuse)
 785 {
 786         int index;
 787         bool alloc_failed = false;
 788         bool enable = false;
 789         int i;
 790         int retval;
 791
 792         for (i = 0; i < count && !alloc_failed; i++) {
 793                 index = (rx->write + i) % TSNEP_RING_SIZE;
 794
 795                 retval = tsnep_rx_alloc_buffer(rx, index);
 796                 if (unlikely(retval)) {
 797                         rx->alloc_failed++;
 798                         alloc_failed = true;
 799
 800                         /* reuse only if no other allocation was successful */
 801                         if (i == 0 && reuse)
 802                                 tsnep_rx_reuse_buffer(rx, index);
 803                         else
 804                                 break;
 805                 }
 806
 807                 tsnep_rx_activate(rx, index);
 808
 809                 enable = true;
 810         }
 811
 812         if (enable) {
 813                 rx->write = (rx->write + i) % TSNEP_RING_SIZE;
 814
 815                 /* descriptor properties shall be valid before hardware is
 816                  * notified
 817                  */
 818                 dma_wmb();
 819
 820                 iowrite32(TSNEP_CONTROL_RX_ENABLE, rx->addr + TSNEP_CONTROL);
 821         }
 822
 823         return i;
 824 }
 825
 826 static struct sk_buff *tsnep_build_skb(struct tsnep_rx *rx, struct page *page,
 827                                        int length)
 828 {
 829         struct sk_buff *skb;
 830
 831         skb = napi_build_skb(page_address(page), PAGE_SIZE);
 832         if (unlikely(!skb))
 833                 return NULL;
 834
 835         /* update pointers within the skb to store the data */
 836         skb_reserve(skb, TSNEP_SKB_PAD + TSNEP_RX_INLINE_METADATA_SIZE);
 837         __skb_put(skb, length - TSNEP_RX_INLINE_METADATA_SIZE - ETH_FCS_LEN);
 838
 839         if (rx->adapter->hwtstamp_config.rx_filter == HWTSTAMP_FILTER_ALL) {
 840                 struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb);
 841                 struct tsnep_rx_inline *rx_inline =
 842                         (struct tsnep_rx_inline *)(page_address(page) +
 843                                                    TSNEP_SKB_PAD);
 844
 845                 skb_shinfo(skb)->tx_flags |=
 846                         SKBTX_HW_TSTAMP_NETDEV;
 847                 memset(hwtstamps, 0, sizeof(*hwtstamps));
 848                 hwtstamps->netdev_data = rx_inline;
 849         }
 850
 851         skb_record_rx_queue(skb, rx->queue_index);
 852         skb->protocol = eth_type_trans(skb, rx->adapter->netdev);
 853
 854         return skb;
 855 }
 856
 857 static int tsnep_rx_poll(struct tsnep_rx *rx, struct napi_struct *napi,
 858                          int budget)
 859 {
 860         struct device *dmadev = rx->adapter->dmadev;
 861         int desc_available;
 862         int done = 0;
 863         enum dma_data_direction dma_dir;
 864         struct tsnep_rx_entry *entry;
 865         struct sk_buff *skb;
 866         int length;
 867
 868         desc_available = tsnep_rx_desc_available(rx);
 869         dma_dir = page_pool_get_dma_dir(rx->page_pool);
 870
 871         while (likely(done < budget) && (rx->read != rx->write)) {
 872                 entry = &rx->entry[rx->read];
 873                 if ((__le32_to_cpu(entry->desc_wb->properties) &
 874                      TSNEP_DESC_OWNER_COUNTER_MASK) !=
 875                     (entry->properties & TSNEP_DESC_OWNER_COUNTER_MASK))
 876                         break;
 877                 done++;
 878
 879                 if (desc_available >= TSNEP_RING_RX_REFILL) {
 880                         bool reuse = desc_available >= TSNEP_RING_RX_REUSE;
 881
 882                         desc_available -= tsnep_rx_refill(rx, desc_available,
 883                                                           reuse);
 884                         if (!entry->page) {
 885                                 /* buffer has been reused for refill to prevent
 886                                  * empty RX ring, thus buffer cannot be used for
 887                                  * RX processing
 888                                  */
 889                                 rx->read = (rx->read + 1) % TSNEP_RING_SIZE;
 890                                 desc_available++;
 891
 892                                 rx->dropped++;
 893
 894                                 continue;
 895                         }
 896                 }
 897
 898                 /* descriptor properties shall be read first, because valid data
 899                  * is signaled there
 900                  */
 901                 dma_rmb();
 902
 903                 prefetch(page_address(entry->page) + TSNEP_SKB_PAD);
 904                 length = __le32_to_cpu(entry->desc_wb->properties) &
 905                          TSNEP_DESC_LENGTH_MASK;
 906                 dma_sync_single_range_for_cpu(dmadev, entry->dma, TSNEP_SKB_PAD,
 907                                               length, dma_dir);
 908
 909                 rx->read = (rx->read + 1) % TSNEP_RING_SIZE;
 910                 desc_available++;
 911
 912                 skb = tsnep_build_skb(rx, entry->page, length);
 913                 if (skb) {
 914                         page_pool_release_page(rx->page_pool, entry->page);
 915
 916                         rx->packets++;
 917                         rx->bytes += length - TSNEP_RX_INLINE_METADATA_SIZE;
 918                         if (skb->pkt_type == PACKET_MULTICAST)
 919                                 rx->multicast++;
 920
 921                         napi_gro_receive(napi, skb);
 922                 } else {
 923                         page_pool_recycle_direct(rx->page_pool, entry->page);
 924
 925                         rx->dropped++;
 926                 }
 927                 entry->page = NULL;
 928         }
 929
 930         if (desc_available)
 931                 tsnep_rx_refill(rx, desc_available, false);
 932
 933         return done;
 934 }
 935
 936 static bool tsnep_rx_pending(struct tsnep_rx *rx)
 937 {
 938         struct tsnep_rx_entry *entry;
 939
 940         if (rx->read != rx->write) {
 941                 entry = &rx->entry[rx->read];
 942                 if ((__le32_to_cpu(entry->desc_wb->properties) &
 943                      TSNEP_DESC_OWNER_COUNTER_MASK) ==
 944                     (entry->properties & TSNEP_DESC_OWNER_COUNTER_MASK))
 945                         return true;
 946         }
 947
 948         return false;
 949 }
 950
 951 static int tsnep_rx_open(struct tsnep_adapter *adapter, void __iomem *addr,
 952                          int queue_index, struct tsnep_rx *rx)
 953 {
 954         dma_addr_t dma;
 955         int retval;
 956
 957         memset(rx, 0, sizeof(*rx));
 958         rx->adapter = adapter;
 959         rx->addr = addr;
 960         rx->queue_index = queue_index;
 961
 962         retval = tsnep_rx_ring_init(rx);
 963         if (retval)
 964                 return retval;
 965
 966         dma = rx->entry[0].desc_dma | TSNEP_RESET_OWNER_COUNTER;
 967         iowrite32(DMA_ADDR_LOW(dma), rx->addr + TSNEP_RX_DESC_ADDR_LOW);
 968         iowrite32(DMA_ADDR_HIGH(dma), rx->addr + TSNEP_RX_DESC_ADDR_HIGH);
 969         rx->owner_counter = 1;
 970         rx->increment_owner_counter = TSNEP_RING_SIZE - 1;
 971
 972         tsnep_rx_refill(rx, tsnep_rx_desc_available(rx), false);
 973
 974         return 0;
 975 }
 976
 977 static void tsnep_rx_close(struct tsnep_rx *rx)
 978 {
 979         u32 val;
 980
 981         iowrite32(TSNEP_CONTROL_RX_DISABLE, rx->addr + TSNEP_CONTROL);
 982         readx_poll_timeout(ioread32, rx->addr + TSNEP_CONTROL, val,
 983                            ((val & TSNEP_CONTROL_RX_ENABLE) == 0), 10000,
 984                            1000000);
 985
 986         tsnep_rx_ring_cleanup(rx);
 987 }
 988
 989 static bool tsnep_pending(struct tsnep_queue *queue)
 990 {
 991         if (queue->tx && tsnep_tx_pending(queue->tx))
 992                 return true;
 993
 994         if (queue->rx && tsnep_rx_pending(queue->rx))
 995                 return true;
 996
 997         return false;
 998 }
 999
1000 static int tsnep_poll(struct napi_struct *napi, int budget)
1001 {
1002         struct tsnep_queue *queue = container_of(napi, struct tsnep_queue,
1003                                                  napi);
1004         bool complete = true;
1005         int done = 0;
1006
1007         if (queue->tx)
1008                 complete = tsnep_tx_poll(queue->tx, budget);
1009
1010         if (queue->rx) {
1011                 done = tsnep_rx_poll(queue->rx, napi, budget);
1012                 if (done >= budget)
1013                         complete = false;
1014         }
1015
1016         /* if all work not completed, return budget and keep polling */
1017         if (!complete)
1018                 return budget;
1019
1020         if (likely(napi_complete_done(napi, done))) {
1021                 tsnep_enable_irq(queue->adapter, queue->irq_mask);
1022
1023                 /* reschedule if work is already pending, prevent rotten packets
1024                  * which are transmitted or received after polling but before
1025                  * interrupt enable
1026                  */
1027                 if (tsnep_pending(queue)) {
1028                         tsnep_disable_irq(queue->adapter, queue->irq_mask);
1029                         napi_schedule(napi);
1030                 }
1031         }
1032
1033         return min(done, budget - 1);
1034 }
1035
1036 static int tsnep_request_irq(struct tsnep_queue *queue, bool first)
1037 {
1038         const char *name = netdev_name(queue->adapter->netdev);
1039         irq_handler_t handler;
1040         void *dev;
1041         int retval;
1042
1043         if (first) {
1044                 sprintf(queue->name, "%s-mac", name);
1045                 handler = tsnep_irq;
1046                 dev = queue->adapter;
1047         } else {
1048                 if (queue->tx && queue->rx)
1049                         sprintf(queue->name, "%s-txrx-%d", name,
1050                                 queue->rx->queue_index);
1051                 else if (queue->tx)
1052                         sprintf(queue->name, "%s-tx-%d", name,
1053                                 queue->tx->queue_index);
1054                 else
1055                         sprintf(queue->name, "%s-rx-%d", name,
1056                                 queue->rx->queue_index);
1057                 handler = tsnep_irq_txrx;
1058                 dev = queue;
1059         }
1060
1061         retval = request_irq(queue->irq, handler, 0, queue->name, dev);
1062         if (retval) {
1063                 /* if name is empty, then interrupt won't be freed */
1064                 memset(queue->name, 0, sizeof(queue->name));
1065         }
1066
1067         return retval;
1068 }
1069
1070 static void tsnep_free_irq(struct tsnep_queue *queue, bool first)
1071 {
1072         void *dev;
1073
1074         if (!strlen(queue->name))
1075                 return;
1076
1077         if (first)
1078                 dev = queue->adapter;
1079         else
1080                 dev = queue;
1081
1082         free_irq(queue->irq, dev);
1083         memset(queue->name, 0, sizeof(queue->name));
1084 }
1085
1086 static int tsnep_netdev_open(struct net_device *netdev)
1087 {
1088         struct tsnep_adapter *adapter = netdev_priv(netdev);
1089         int i;
1090         void __iomem *addr;
1091         int tx_queue_index = 0;
1092         int rx_queue_index = 0;
1093         int retval;
1094
1095         for (i = 0; i < adapter->num_queues; i++) {
1096                 adapter->queue[i].adapter = adapter;
1097                 if (adapter->queue[i].tx) {
1098                         addr = adapter->addr + TSNEP_QUEUE(tx_queue_index);
1099                         retval = tsnep_tx_open(adapter, addr, tx_queue_index,
1100                                                adapter->queue[i].tx);
1101                         if (retval)
1102                                 goto failed;
1103                         tx_queue_index++;
1104                 }
1105                 if (adapter->queue[i].rx) {
1106                         addr = adapter->addr + TSNEP_QUEUE(rx_queue_index);
1107                         retval = tsnep_rx_open(adapter, addr,
1108                                                rx_queue_index,
1109                                                adapter->queue[i].rx);
1110                         if (retval)
1111                                 goto failed;
1112                         rx_queue_index++;
1113                 }
1114
1115                 retval = tsnep_request_irq(&adapter->queue[i], i == 0);
1116                 if (retval) {
1117                         netif_err(adapter, drv, adapter->netdev,
1118                                   "can't get assigned irq %d.\n",
1119                                   adapter->queue[i].irq);
1120                         goto failed;
1121                 }
1122         }
1123
1124         retval = netif_set_real_num_tx_queues(adapter->netdev,
1125                                               adapter->num_tx_queues);
1126         if (retval)
1127                 goto failed;
1128         retval = netif_set_real_num_rx_queues(adapter->netdev,
1129                                               adapter->num_rx_queues);
1130         if (retval)
1131                 goto failed;
1132
1133         tsnep_enable_irq(adapter, ECM_INT_LINK);
1134         retval = tsnep_phy_open(adapter);
1135         if (retval)
1136                 goto phy_failed;
1137
1138         for (i = 0; i < adapter->num_queues; i++) {
1139                 netif_napi_add(adapter->netdev, &adapter->queue[i].napi,
1140                                tsnep_poll);
1141                 napi_enable(&adapter->queue[i].napi);
1142
1143                 tsnep_enable_irq(adapter, adapter->queue[i].irq_mask);
1144         }
1145
1146         return 0;
1147
1148 phy_failed:
1149         tsnep_disable_irq(adapter, ECM_INT_LINK);
1150         tsnep_phy_close(adapter);
1151 failed:
1152         for (i = 0; i < adapter->num_queues; i++) {
1153                 tsnep_free_irq(&adapter->queue[i], i == 0);
1154
1155                 if (adapter->queue[i].rx)
1156                         tsnep_rx_close(adapter->queue[i].rx);
1157                 if (adapter->queue[i].tx)
1158                         tsnep_tx_close(adapter->queue[i].tx);
1159         }
1160         return retval;
1161 }
1162
1163 static int tsnep_netdev_close(struct net_device *netdev)
1164 {
1165         struct tsnep_adapter *adapter = netdev_priv(netdev);
1166         int i;
1167
1168         tsnep_disable_irq(adapter, ECM_INT_LINK);
1169         tsnep_phy_close(adapter);
1170
1171         for (i = 0; i < adapter->num_queues; i++) {
1172                 tsnep_disable_irq(adapter, adapter->queue[i].irq_mask);
1173
1174                 napi_disable(&adapter->queue[i].napi);
1175                 netif_napi_del(&adapter->queue[i].napi);
1176
1177                 tsnep_free_irq(&adapter->queue[i], i == 0);
1178
1179                 if (adapter->queue[i].rx)
1180                         tsnep_rx_close(adapter->queue[i].rx);
1181                 if (adapter->queue[i].tx)
1182                         tsnep_tx_close(adapter->queue[i].tx);
1183         }
1184
1185         return 0;
1186 }
1187
1188 static netdev_tx_t tsnep_netdev_xmit_frame(struct sk_buff *skb,
1189                                            struct net_device *netdev)
1190 {
1191         struct tsnep_adapter *adapter = netdev_priv(netdev);
1192         u16 queue_mapping = skb_get_queue_mapping(skb);
1193
1194         if (queue_mapping >= adapter->num_tx_queues)
1195                 queue_mapping = 0;
1196
1197         return tsnep_xmit_frame_ring(skb, &adapter->tx[queue_mapping]);
1198 }
1199
1200 static int tsnep_netdev_ioctl(struct net_device *netdev, struct ifreq *ifr,
1201                               int cmd)
1202 {
1203         if (!netif_running(netdev))
1204                 return -EINVAL;
1205         if (cmd == SIOCSHWTSTAMP || cmd == SIOCGHWTSTAMP)
1206                 return tsnep_ptp_ioctl(netdev, ifr, cmd);
1207         return phy_mii_ioctl(netdev->phydev, ifr, cmd);
1208 }
1209
1210 static void tsnep_netdev_set_multicast(struct net_device *netdev)
1211 {
1212         struct tsnep_adapter *adapter = netdev_priv(netdev);
1213
1214         u16 rx_filter = 0;
1215
1216         /* configured MAC address and broadcasts are never filtered */
1217         if (netdev->flags & IFF_PROMISC) {
1218                 rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_MULTICASTS;
1219                 rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_UNICASTS;
1220         } else if (!netdev_mc_empty(netdev) || (netdev->flags & IFF_ALLMULTI)) {
1221                 rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_MULTICASTS;
1222         }
1223         iowrite16(rx_filter, adapter->addr + TSNEP_RX_FILTER);
1224 }
1225
1226 static void tsnep_netdev_get_stats64(struct net_device *netdev,
1227                                      struct rtnl_link_stats64 *stats)
1228 {
1229         struct tsnep_adapter *adapter = netdev_priv(netdev);
1230         u32 reg;
1231         u32 val;
1232         int i;
1233
1234         for (i = 0; i < adapter->num_tx_queues; i++) {
1235                 stats->tx_packets += adapter->tx[i].packets;
1236                 stats->tx_bytes += adapter->tx[i].bytes;
1237                 stats->tx_dropped += adapter->tx[i].dropped;
1238         }
1239         for (i = 0; i < adapter->num_rx_queues; i++) {
1240                 stats->rx_packets += adapter->rx[i].packets;
1241                 stats->rx_bytes += adapter->rx[i].bytes;
1242                 stats->rx_dropped += adapter->rx[i].dropped;
1243                 stats->multicast += adapter->rx[i].multicast;
1244
1245                 reg = ioread32(adapter->addr + TSNEP_QUEUE(i) +
1246                                TSNEP_RX_STATISTIC);
1247                 val = (reg & TSNEP_RX_STATISTIC_NO_DESC_MASK) >>
1248                       TSNEP_RX_STATISTIC_NO_DESC_SHIFT;
1249                 stats->rx_dropped += val;
1250                 val = (reg & TSNEP_RX_STATISTIC_BUFFER_TOO_SMALL_MASK) >>
1251                       TSNEP_RX_STATISTIC_BUFFER_TOO_SMALL_SHIFT;
1252                 stats->rx_dropped += val;
1253                 val = (reg & TSNEP_RX_STATISTIC_FIFO_OVERFLOW_MASK) >>
1254                       TSNEP_RX_STATISTIC_FIFO_OVERFLOW_SHIFT;
1255                 stats->rx_errors += val;
1256                 stats->rx_fifo_errors += val;
1257                 val = (reg & TSNEP_RX_STATISTIC_INVALID_FRAME_MASK) >>
1258                       TSNEP_RX_STATISTIC_INVALID_FRAME_SHIFT;
1259                 stats->rx_errors += val;
1260                 stats->rx_frame_errors += val;
1261         }
1262
1263         reg = ioread32(adapter->addr + ECM_STAT);
1264         val = (reg & ECM_STAT_RX_ERR_MASK) >> ECM_STAT_RX_ERR_SHIFT;
1265         stats->rx_errors += val;
1266         val = (reg & ECM_STAT_INV_FRM_MASK) >> ECM_STAT_INV_FRM_SHIFT;
1267         stats->rx_errors += val;
1268         stats->rx_crc_errors += val;
1269         val = (reg & ECM_STAT_FWD_RX_ERR_MASK) >> ECM_STAT_FWD_RX_ERR_SHIFT;
1270         stats->rx_errors += val;
1271 }
1272
1273 static void tsnep_mac_set_address(struct tsnep_adapter *adapter, u8 *addr)
1274 {
1275         iowrite32(*(u32 *)addr, adapter->addr + TSNEP_MAC_ADDRESS_LOW);
1276         iowrite16(*(u16 *)(addr + sizeof(u32)),
1277                   adapter->addr + TSNEP_MAC_ADDRESS_HIGH);
1278
1279         ether_addr_copy(adapter->mac_address, addr);
1280         netif_info(adapter, drv, adapter->netdev, "MAC address set to %pM\n",
1281                    addr);
1282 }
1283
1284 static int tsnep_netdev_set_mac_address(struct net_device *netdev, void *addr)
1285 {
1286         struct tsnep_adapter *adapter = netdev_priv(netdev);
1287         struct sockaddr *sock_addr = addr;
1288         int retval;
1289
1290         retval = eth_prepare_mac_addr_change(netdev, sock_addr);
1291         if (retval)
1292                 return retval;
1293         eth_hw_addr_set(netdev, sock_addr->sa_data);
1294         tsnep_mac_set_address(adapter, sock_addr->sa_data);
1295
1296         return 0;
1297 }
1298
1299 static int tsnep_netdev_set_features(struct net_device *netdev,
1300                                      netdev_features_t features)
1301 {
1302         struct tsnep_adapter *adapter = netdev_priv(netdev);
1303         netdev_features_t changed = netdev->features ^ features;
1304         bool enable;
1305         int retval = 0;
1306
1307         if (changed & NETIF_F_LOOPBACK) {
1308                 enable = !!(features & NETIF_F_LOOPBACK);
1309                 retval = tsnep_phy_loopback(adapter, enable);
1310         }
1311
1312         return retval;
1313 }
1314
1315 static ktime_t tsnep_netdev_get_tstamp(struct net_device *netdev,
1316                                        const struct skb_shared_hwtstamps *hwtstamps,
1317                                        bool cycles)
1318 {
1319         struct tsnep_rx_inline *rx_inline = hwtstamps->netdev_data;
1320         u64 timestamp;
1321
1322         if (cycles)
1323                 timestamp = __le64_to_cpu(rx_inline->counter);
1324         else
1325                 timestamp = __le64_to_cpu(rx_inline->timestamp);
1326
1327         return ns_to_ktime(timestamp);
1328 }
1329
1330 static const struct net_device_ops tsnep_netdev_ops = {
1331         .ndo_open = tsnep_netdev_open,
1332         .ndo_stop = tsnep_netdev_close,
1333         .ndo_start_xmit = tsnep_netdev_xmit_frame,
1334         .ndo_eth_ioctl = tsnep_netdev_ioctl,
1335         .ndo_set_rx_mode = tsnep_netdev_set_multicast,
1336         .ndo_get_stats64 = tsnep_netdev_get_stats64,
1337         .ndo_set_mac_address = tsnep_netdev_set_mac_address,
1338         .ndo_set_features = tsnep_netdev_set_features,
1339         .ndo_get_tstamp = tsnep_netdev_get_tstamp,
1340         .ndo_setup_tc = tsnep_tc_setup,
1341 };
1342
1343 static int tsnep_mac_init(struct tsnep_adapter *adapter)
1344 {
1345         int retval;
1346
1347         /* initialize RX filtering, at least configured MAC address and
1348          * broadcast are not filtered
1349          */
1350         iowrite16(0, adapter->addr + TSNEP_RX_FILTER);
1351
1352         /* try to get MAC address in the following order:
1353          * - device tree
1354          * - valid MAC address already set
1355          * - MAC address register if valid
1356          * - random MAC address
1357          */
1358         retval = of_get_mac_address(adapter->pdev->dev.of_node,
1359                                     adapter->mac_address);
1360         if (retval == -EPROBE_DEFER)
1361                 return retval;
1362         if (retval && !is_valid_ether_addr(adapter->mac_address)) {
1363                 *(u32 *)adapter->mac_address =
1364                         ioread32(adapter->addr + TSNEP_MAC_ADDRESS_LOW);
1365                 *(u16 *)(adapter->mac_address + sizeof(u32)) =
1366                         ioread16(adapter->addr + TSNEP_MAC_ADDRESS_HIGH);
1367                 if (!is_valid_ether_addr(adapter->mac_address))
1368                         eth_random_addr(adapter->mac_address);
1369         }
1370
1371         tsnep_mac_set_address(adapter, adapter->mac_address);
1372         eth_hw_addr_set(adapter->netdev, adapter->mac_address);
1373
1374         return 0;
1375 }
1376
1377 static int tsnep_mdio_init(struct tsnep_adapter *adapter)
1378 {
1379         struct device_node *np = adapter->pdev->dev.of_node;
1380         int retval;
1381
1382         if (np) {
1383                 np = of_get_child_by_name(np, "mdio");
1384                 if (!np)
1385                         return 0;
1386
1387                 adapter->suppress_preamble =
1388                         of_property_read_bool(np, "suppress-preamble");
1389         }
1390
1391         adapter->mdiobus = devm_mdiobus_alloc(&adapter->pdev->dev);
1392         if (!adapter->mdiobus) {
1393                 retval = -ENOMEM;
1394
1395                 goto out;
1396         }
1397
1398         adapter->mdiobus->priv = (void *)adapter;
1399         adapter->mdiobus->parent = &adapter->pdev->dev;
1400         adapter->mdiobus->read = tsnep_mdiobus_read;
1401         adapter->mdiobus->write = tsnep_mdiobus_write;
1402         adapter->mdiobus->name = TSNEP "-mdiobus";
1403         snprintf(adapter->mdiobus->id, MII_BUS_ID_SIZE, "%s",
1404                  adapter->pdev->name);
1405
1406         /* do not scan broadcast address */
1407         adapter->mdiobus->phy_mask = 0x0000001;
1408
1409         retval = of_mdiobus_register(adapter->mdiobus, np);
1410
1411 out:
1412         of_node_put(np);
1413
1414         return retval;
1415 }
1416
1417 static int tsnep_phy_init(struct tsnep_adapter *adapter)
1418 {
1419         struct device_node *phy_node;
1420         int retval;
1421
1422         retval = of_get_phy_mode(adapter->pdev->dev.of_node,
1423                                  &adapter->phy_mode);
1424         if (retval)
1425                 adapter->phy_mode = PHY_INTERFACE_MODE_GMII;
1426
1427         phy_node = of_parse_phandle(adapter->pdev->dev.of_node, "phy-handle",
1428                                     0);
1429         adapter->phydev = of_phy_find_device(phy_node);
1430         of_node_put(phy_node);
1431         if (!adapter->phydev && adapter->mdiobus)
1432                 adapter->phydev = phy_find_first(adapter->mdiobus);
1433         if (!adapter->phydev)
1434                 return -EIO;
1435
1436         return 0;
1437 }
1438
1439 static int tsnep_queue_init(struct tsnep_adapter *adapter, int queue_count)
1440 {
1441         u32 irq_mask = ECM_INT_TX_0 | ECM_INT_RX_0;
1442         char name[8];
1443         int i;
1444         int retval;
1445
1446         /* one TX/RX queue pair for netdev is mandatory */
1447         if (platform_irq_count(adapter->pdev) == 1)
1448                 retval = platform_get_irq(adapter->pdev, 0);
1449         else
1450                 retval = platform_get_irq_byname(adapter->pdev, "mac");
1451         if (retval < 0)
1452                 return retval;
1453         adapter->num_tx_queues = 1;
1454         adapter->num_rx_queues = 1;
1455         adapter->num_queues = 1;
1456         adapter->queue[0].irq = retval;
1457         adapter->queue[0].tx = &adapter->tx[0];
1458         adapter->queue[0].rx = &adapter->rx[0];
1459         adapter->queue[0].irq_mask = irq_mask;
1460         adapter->queue[0].irq_delay_addr = adapter->addr + ECM_INT_DELAY;
1461         retval = tsnep_set_irq_coalesce(&adapter->queue[0],
1462                                         TSNEP_COALESCE_USECS_DEFAULT);
1463         if (retval < 0)
1464                 return retval;
1465
1466         adapter->netdev->irq = adapter->queue[0].irq;
1467
1468         /* add additional TX/RX queue pairs only if dedicated interrupt is
1469          * available
1470          */
1471         for (i = 1; i < queue_count; i++) {
1472                 sprintf(name, "txrx-%d", i);
1473                 retval = platform_get_irq_byname_optional(adapter->pdev, name);
1474                 if (retval < 0)
1475                         break;
1476
1477                 adapter->num_tx_queues++;
1478                 adapter->num_rx_queues++;
1479                 adapter->num_queues++;
1480                 adapter->queue[i].irq = retval;
1481                 adapter->queue[i].tx = &adapter->tx[i];
1482                 adapter->queue[i].rx = &adapter->rx[i];
1483                 adapter->queue[i].irq_mask =
1484                         irq_mask << (ECM_INT_TXRX_SHIFT * i);
1485                 adapter->queue[i].irq_delay_addr =
1486                         adapter->addr + ECM_INT_DELAY + ECM_INT_DELAY_OFFSET * i;
1487                 retval = tsnep_set_irq_coalesce(&adapter->queue[i],
1488                                                 TSNEP_COALESCE_USECS_DEFAULT);
1489                 if (retval < 0)
1490                         return retval;
1491         }
1492
1493         return 0;
1494 }
1495
1496 static int tsnep_probe(struct platform_device *pdev)
1497 {
1498         struct tsnep_adapter *adapter;
1499         struct net_device *netdev;
1500         struct resource *io;
1501         u32 type;
1502         int revision;
1503         int version;
1504         int queue_count;
1505         int retval;
1506
1507         netdev = devm_alloc_etherdev_mqs(&pdev->dev,
1508                                          sizeof(struct tsnep_adapter),
1509                                          TSNEP_MAX_QUEUES, TSNEP_MAX_QUEUES);
1510         if (!netdev)
1511                 return -ENODEV;
1512         SET_NETDEV_DEV(netdev, &pdev->dev);
1513         adapter = netdev_priv(netdev);
1514         platform_set_drvdata(pdev, adapter);
1515         adapter->pdev = pdev;
1516         adapter->dmadev = &pdev->dev;
1517         adapter->netdev = netdev;
1518         adapter->msg_enable = NETIF_MSG_DRV | NETIF_MSG_PROBE |
1519                               NETIF_MSG_LINK | NETIF_MSG_IFUP |
1520                               NETIF_MSG_IFDOWN | NETIF_MSG_TX_QUEUED;
1521
1522         netdev->min_mtu = ETH_MIN_MTU;
1523         netdev->max_mtu = TSNEP_MAX_FRAME_SIZE;
1524
1525         mutex_init(&adapter->gate_control_lock);
1526         mutex_init(&adapter->rxnfc_lock);
1527         INIT_LIST_HEAD(&adapter->rxnfc_rules);
1528
1529         io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1530         adapter->addr = devm_ioremap_resource(&pdev->dev, io);
1531         if (IS_ERR(adapter->addr))
1532                 return PTR_ERR(adapter->addr);
1533         netdev->mem_start = io->start;
1534         netdev->mem_end = io->end;
1535
1536         type = ioread32(adapter->addr + ECM_TYPE);
1537         revision = (type & ECM_REVISION_MASK) >> ECM_REVISION_SHIFT;
1538         version = (type & ECM_VERSION_MASK) >> ECM_VERSION_SHIFT;
1539         queue_count = (type & ECM_QUEUE_COUNT_MASK) >> ECM_QUEUE_COUNT_SHIFT;
1540         adapter->gate_control = type & ECM_GATE_CONTROL;
1541         adapter->rxnfc_max = TSNEP_RX_ASSIGN_ETHER_TYPE_COUNT;
1542
1543         tsnep_disable_irq(adapter, ECM_INT_ALL);
1544
1545         retval = tsnep_queue_init(adapter, queue_count);
1546         if (retval)
1547                 return retval;
1548
1549         retval = dma_set_mask_and_coherent(&adapter->pdev->dev,
1550                                            DMA_BIT_MASK(64));
1551         if (retval) {
1552                 dev_err(&adapter->pdev->dev, "no usable DMA configuration.\n");
1553                 return retval;
1554         }
1555
1556         retval = tsnep_mac_init(adapter);
1557         if (retval)
1558                 return retval;
1559
1560         retval = tsnep_mdio_init(adapter);
1561         if (retval)
1562                 goto mdio_init_failed;
1563
1564         retval = tsnep_phy_init(adapter);
1565         if (retval)
1566                 goto phy_init_failed;
1567
1568         retval = tsnep_ptp_init(adapter);
1569         if (retval)
1570                 goto ptp_init_failed;
1571
1572         retval = tsnep_tc_init(adapter);
1573         if (retval)
1574                 goto tc_init_failed;
1575
1576         retval = tsnep_rxnfc_init(adapter);
1577         if (retval)
1578                 goto rxnfc_init_failed;
1579
1580         netdev->netdev_ops = &tsnep_netdev_ops;
1581         netdev->ethtool_ops = &tsnep_ethtool_ops;
1582         netdev->features = NETIF_F_SG;
1583         netdev->hw_features = netdev->features | NETIF_F_LOOPBACK;
1584
1585         /* carrier off reporting is important to ethtool even BEFORE open */
1586         netif_carrier_off(netdev);
1587
1588         retval = register_netdev(netdev);
1589         if (retval)
1590                 goto register_failed;
1591
1592         dev_info(&adapter->pdev->dev, "device version %d.%02d\n", version,
1593                  revision);
1594         if (adapter->gate_control)
1595                 dev_info(&adapter->pdev->dev, "gate control detected\n");
1596
1597         return 0;
1598
1599 register_failed:
1600         tsnep_rxnfc_cleanup(adapter);
1601 rxnfc_init_failed:
1602         tsnep_tc_cleanup(adapter);
1603 tc_init_failed:
1604         tsnep_ptp_cleanup(adapter);
1605 ptp_init_failed:
1606 phy_init_failed:
1607         if (adapter->mdiobus)
1608                 mdiobus_unregister(adapter->mdiobus);
1609 mdio_init_failed:
1610         return retval;
1611 }
1612
1613 static int tsnep_remove(struct platform_device *pdev)
1614 {
1615         struct tsnep_adapter *adapter = platform_get_drvdata(pdev);
1616
1617         unregister_netdev(adapter->netdev);
1618
1619         tsnep_rxnfc_cleanup(adapter);
1620
1621         tsnep_tc_cleanup(adapter);
1622
1623         tsnep_ptp_cleanup(adapter);
1624
1625         if (adapter->mdiobus)
1626                 mdiobus_unregister(adapter->mdiobus);
1627
1628         tsnep_disable_irq(adapter, ECM_INT_ALL);
1629
1630         return 0;
1631 }
1632
1633 static const struct of_device_id tsnep_of_match[] = {
1634         { .compatible = "engleder,tsnep", },
1635 { },
1636 };
1637 MODULE_DEVICE_TABLE(of, tsnep_of_match);
1638
1639 static struct platform_driver tsnep_driver = {
1640         .driver = {
1641                 .name = TSNEP,
1642                 .of_match_table = tsnep_of_match,
1643         },
1644         .probe = tsnep_probe,
1645         .remove = tsnep_remove,
1646 };
1647 module_platform_driver(tsnep_driver);
1648
1649 MODULE_AUTHOR("Gerhard Engleder <gerhard@engleder-embedded.com>");
1650 MODULE_DESCRIPTION("TSN endpoint Ethernet MAC driver");
1651 MODULE_LICENSE("GPL");