drivers/net/ethernet/cadence/macb_main.c

   1 /*
   2  * Cadence MACB/GEM Ethernet Controller driver
   3  *
   4  * Copyright (C) 2004-2006 Atmel Corporation
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License version 2 as
   8  * published by the Free Software Foundation.
   9  */
  10
  11 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  12 #include <linux/clk.h>
  13 #include <linux/crc32.h>
  14 #include <linux/module.h>
  15 #include <linux/moduleparam.h>
  16 #include <linux/kernel.h>
  17 #include <linux/types.h>
  18 #include <linux/circ_buf.h>
  19 #include <linux/slab.h>
  20 #include <linux/init.h>
  21 #include <linux/io.h>
  22 #include <linux/gpio.h>
  23 #include <linux/gpio/consumer.h>
  24 #include <linux/interrupt.h>
  25 #include <linux/netdevice.h>
  26 #include <linux/etherdevice.h>
  27 #include <linux/dma-mapping.h>
  28 #include <linux/platform_data/macb.h>
  29 #include <linux/platform_device.h>
  30 #include <linux/phy.h>
  31 #include <linux/of.h>
  32 #include <linux/of_device.h>
  33 #include <linux/of_gpio.h>
  34 #include <linux/of_mdio.h>
  35 #include <linux/of_net.h>
  36 #include <linux/ip.h>
  37 #include <linux/udp.h>
  38 #include <linux/tcp.h>
  39 #include "macb.h"
  40
  41 #define MACB_RX_BUFFER_SIZE     128
  42 #define RX_BUFFER_MULTIPLE      64  /* bytes */
  43
  44 #define DEFAULT_RX_RING_SIZE    512 /* must be power of 2 */
  45 #define MIN_RX_RING_SIZE        64
  46 #define MAX_RX_RING_SIZE        8192
  47 #define RX_RING_BYTES(bp)       (macb_dma_desc_get_size(bp)     \
  48                                  * (bp)->rx_ring_size)
  49
  50 #define DEFAULT_TX_RING_SIZE    512 /* must be power of 2 */
  51 #define MIN_TX_RING_SIZE        64
  52 #define MAX_TX_RING_SIZE        4096
  53 #define TX_RING_BYTES(bp)       (macb_dma_desc_get_size(bp)     \
  54                                  * (bp)->tx_ring_size)
  55
  56 /* level of occupied TX descriptors under which we wake up TX process */
  57 #define MACB_TX_WAKEUP_THRESH(bp)       (3 * (bp)->tx_ring_size / 4)
  58
  59 #define MACB_RX_INT_FLAGS       (MACB_BIT(RCOMP) | MACB_BIT(ISR_ROVR))
  60 #define MACB_TX_ERR_FLAGS       (MACB_BIT(ISR_TUND)                     \
  61                                         | MACB_BIT(ISR_RLE)             \
  62                                         | MACB_BIT(TXERR))
  63 #define MACB_TX_INT_FLAGS       (MACB_TX_ERR_FLAGS | MACB_BIT(TCOMP)    \
  64                                         | MACB_BIT(TXUBR))
  65
  66 /* Max length of transmit frame must be a multiple of 8 bytes */
  67 #define MACB_TX_LEN_ALIGN       8
  68 #define MACB_MAX_TX_LEN         ((unsigned int)((1 << MACB_TX_FRMLEN_SIZE) - 1) & ~((unsigned int)(MACB_TX_LEN_ALIGN - 1)))
  69 #define GEM_MAX_TX_LEN          ((unsigned int)((1 << GEM_TX_FRMLEN_SIZE) - 1) & ~((unsigned int)(MACB_TX_LEN_ALIGN - 1)))
  70
  71 #define GEM_MTU_MIN_SIZE        ETH_MIN_MTU
  72 #define MACB_NETIF_LSO          NETIF_F_TSO
  73
  74 #define MACB_WOL_HAS_MAGIC_PACKET       (0x1 << 0)
  75 #define MACB_WOL_ENABLED                (0x1 << 1)
  76
  77 /* Graceful stop timeouts in us. We should allow up to
  78  * 1 frame time (10 Mbits/s, full-duplex, ignoring collisions)
  79  */
  80 #define MACB_HALT_TIMEOUT       1230
  81
  82 /* DMA buffer descriptor might be different size
  83  * depends on hardware configuration:
  84  *
  85  * 1. dma address width 32 bits:
  86  *    word 1: 32 bit address of Data Buffer
  87  *    word 2: control
  88  *
  89  * 2. dma address width 64 bits:
  90  *    word 1: 32 bit address of Data Buffer
  91  *    word 2: control
  92  *    word 3: upper 32 bit address of Data Buffer
  93  *    word 4: unused
  94  *
  95  * 3. dma address width 32 bits with hardware timestamping:
  96  *    word 1: 32 bit address of Data Buffer
  97  *    word 2: control
  98  *    word 3: timestamp word 1
  99  *    word 4: timestamp word 2
 100  *
 101  * 4. dma address width 64 bits with hardware timestamping:
 102  *    word 1: 32 bit address of Data Buffer
 103  *    word 2: control
 104  *    word 3: upper 32 bit address of Data Buffer
 105  *    word 4: unused
 106  *    word 5: timestamp word 1
 107  *    word 6: timestamp word 2
 108  */
 109 static unsigned int macb_dma_desc_get_size(struct macb *bp)
 110 {
 111 #ifdef MACB_EXT_DESC
 112         unsigned int desc_size;
 113
 114         switch (bp->hw_dma_cap) {
 115         case HW_DMA_CAP_64B:
 116                 desc_size = sizeof(struct macb_dma_desc)
 117                         + sizeof(struct macb_dma_desc_64);
 118                 break;
 119         case HW_DMA_CAP_PTP:
 120                 desc_size = sizeof(struct macb_dma_desc)
 121                         + sizeof(struct macb_dma_desc_ptp);
 122                 break;
 123         case HW_DMA_CAP_64B_PTP:
 124                 desc_size = sizeof(struct macb_dma_desc)
 125                         + sizeof(struct macb_dma_desc_64)
 126                         + sizeof(struct macb_dma_desc_ptp);
 127                 break;
 128         default:
 129                 desc_size = sizeof(struct macb_dma_desc);
 130         }
 131         return desc_size;
 132 #endif
 133         return sizeof(struct macb_dma_desc);
 134 }
 135
 136 static unsigned int macb_adj_dma_desc_idx(struct macb *bp, unsigned int desc_idx)
 137 {
 138 #ifdef MACB_EXT_DESC
 139         switch (bp->hw_dma_cap) {
 140         case HW_DMA_CAP_64B:
 141         case HW_DMA_CAP_PTP:
 142                 desc_idx <<= 1;
 143                 break;
 144         case HW_DMA_CAP_64B_PTP:
 145                 desc_idx *= 3;
 146                 break;
 147         default:
 148                 break;
 149         }
 150 #endif
 151         return desc_idx;
 152 }
 153
 154 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
 155 static struct macb_dma_desc_64 *macb_64b_desc(struct macb *bp, struct macb_dma_desc *desc)
 156 {
 157         if (bp->hw_dma_cap & HW_DMA_CAP_64B)
 158                 return (struct macb_dma_desc_64 *)((void *)desc + sizeof(struct macb_dma_desc));
 159         return NULL;
 160 }
 161 #endif
 162
 163 /* Ring buffer accessors */
 164 static unsigned int macb_tx_ring_wrap(struct macb *bp, unsigned int index)
 165 {
 166         return index & (bp->tx_ring_size - 1);
 167 }
 168
 169 static struct macb_dma_desc *macb_tx_desc(struct macb_queue *queue,
 170                                           unsigned int index)
 171 {
 172         index = macb_tx_ring_wrap(queue->bp, index);
 173         index = macb_adj_dma_desc_idx(queue->bp, index);
 174         return &queue->tx_ring[index];
 175 }
 176
 177 static struct macb_tx_skb *macb_tx_skb(struct macb_queue *queue,
 178                                        unsigned int index)
 179 {
 180         return &queue->tx_skb[macb_tx_ring_wrap(queue->bp, index)];
 181 }
 182
 183 static dma_addr_t macb_tx_dma(struct macb_queue *queue, unsigned int index)
 184 {
 185         dma_addr_t offset;
 186
 187         offset = macb_tx_ring_wrap(queue->bp, index) *
 188                         macb_dma_desc_get_size(queue->bp);
 189
 190         return queue->tx_ring_dma + offset;
 191 }
 192
 193 static unsigned int macb_rx_ring_wrap(struct macb *bp, unsigned int index)
 194 {
 195         return index & (bp->rx_ring_size - 1);
 196 }
 197
 198 static struct macb_dma_desc *macb_rx_desc(struct macb_queue *queue, unsigned int index)
 199 {
 200         index = macb_rx_ring_wrap(queue->bp, index);
 201         index = macb_adj_dma_desc_idx(queue->bp, index);
 202         return &queue->rx_ring[index];
 203 }
 204
 205 static void *macb_rx_buffer(struct macb_queue *queue, unsigned int index)
 206 {
 207         return queue->rx_buffers + queue->bp->rx_buffer_size *
 208                macb_rx_ring_wrap(queue->bp, index);
 209 }
 210
 211 /* I/O accessors */
 212 static u32 hw_readl_native(struct macb *bp, int offset)
 213 {
 214         return __raw_readl(bp->regs + offset);
 215 }
 216
 217 static void hw_writel_native(struct macb *bp, int offset, u32 value)
 218 {
 219         __raw_writel(value, bp->regs + offset);
 220 }
 221
 222 static u32 hw_readl(struct macb *bp, int offset)
 223 {
 224         return readl_relaxed(bp->regs + offset);
 225 }
 226
 227 static void hw_writel(struct macb *bp, int offset, u32 value)
 228 {
 229         writel_relaxed(value, bp->regs + offset);
 230 }
 231
 232 /* Find the CPU endianness by using the loopback bit of NCR register. When the
 233  * CPU is in big endian we need to program swapped mode for management
 234  * descriptor access.
 235  */
 236 static bool hw_is_native_io(void __iomem *addr)
 237 {
 238         u32 value = MACB_BIT(LLB);
 239
 240         __raw_writel(value, addr + MACB_NCR);
 241         value = __raw_readl(addr + MACB_NCR);
 242
 243         /* Write 0 back to disable everything */
 244         __raw_writel(0, addr + MACB_NCR);
 245
 246         return value == MACB_BIT(LLB);
 247 }
 248
 249 static bool hw_is_gem(void __iomem *addr, bool native_io)
 250 {
 251         u32 id;
 252
 253         if (native_io)
 254                 id = __raw_readl(addr + MACB_MID);
 255         else
 256                 id = readl_relaxed(addr + MACB_MID);
 257
 258         return MACB_BFEXT(IDNUM, id) >= 0x2;
 259 }
 260
 261 static void macb_set_hwaddr(struct macb *bp)
 262 {
 263         u32 bottom;
 264         u16 top;
 265
 266         bottom = cpu_to_le32(*((u32 *)bp->dev->dev_addr));
 267         macb_or_gem_writel(bp, SA1B, bottom);
 268         top = cpu_to_le16(*((u16 *)(bp->dev->dev_addr + 4)));
 269         macb_or_gem_writel(bp, SA1T, top);
 270
 271         /* Clear unused address register sets */
 272         macb_or_gem_writel(bp, SA2B, 0);
 273         macb_or_gem_writel(bp, SA2T, 0);
 274         macb_or_gem_writel(bp, SA3B, 0);
 275         macb_or_gem_writel(bp, SA3T, 0);
 276         macb_or_gem_writel(bp, SA4B, 0);
 277         macb_or_gem_writel(bp, SA4T, 0);
 278 }
 279
 280 static void macb_get_hwaddr(struct macb *bp)
 281 {
 282         struct macb_platform_data *pdata;
 283         u32 bottom;
 284         u16 top;
 285         u8 addr[6];
 286         int i;
 287
 288         pdata = dev_get_platdata(&bp->pdev->dev);
 289
 290         /* Check all 4 address register for valid address */
 291         for (i = 0; i < 4; i++) {
 292                 bottom = macb_or_gem_readl(bp, SA1B + i * 8);
 293                 top = macb_or_gem_readl(bp, SA1T + i * 8);
 294
 295                 if (pdata && pdata->rev_eth_addr) {
 296                         addr[5] = bottom & 0xff;
 297                         addr[4] = (bottom >> 8) & 0xff;
 298                         addr[3] = (bottom >> 16) & 0xff;
 299                         addr[2] = (bottom >> 24) & 0xff;
 300                         addr[1] = top & 0xff;
 301                         addr[0] = (top & 0xff00) >> 8;
 302                 } else {
 303                         addr[0] = bottom & 0xff;
 304                         addr[1] = (bottom >> 8) & 0xff;
 305                         addr[2] = (bottom >> 16) & 0xff;
 306                         addr[3] = (bottom >> 24) & 0xff;
 307                         addr[4] = top & 0xff;
 308                         addr[5] = (top >> 8) & 0xff;
 309                 }
 310
 311                 if (is_valid_ether_addr(addr)) {
 312                         memcpy(bp->dev->dev_addr, addr, sizeof(addr));
 313                         return;
 314                 }
 315         }
 316
 317         dev_info(&bp->pdev->dev, "invalid hw address, using random\n");
 318         eth_hw_addr_random(bp->dev);
 319 }
 320
 321 static int macb_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 322 {
 323         struct macb *bp = bus->priv;
 324         int value;
 325
 326         macb_writel(bp, MAN, (MACB_BF(SOF, MACB_MAN_SOF)
 327                               | MACB_BF(RW, MACB_MAN_READ)
 328                               | MACB_BF(PHYA, mii_id)
 329                               | MACB_BF(REGA, regnum)
 330                               | MACB_BF(CODE, MACB_MAN_CODE)));
 331
 332         /* wait for end of transfer */
 333         while (!MACB_BFEXT(IDLE, macb_readl(bp, NSR)))
 334                 cpu_relax();
 335
 336         value = MACB_BFEXT(DATA, macb_readl(bp, MAN));
 337
 338         return value;
 339 }
 340
 341 static int macb_mdio_write(struct mii_bus *bus, int mii_id, int regnum,
 342                            u16 value)
 343 {
 344         struct macb *bp = bus->priv;
 345
 346         macb_writel(bp, MAN, (MACB_BF(SOF, MACB_MAN_SOF)
 347                               | MACB_BF(RW, MACB_MAN_WRITE)
 348                               | MACB_BF(PHYA, mii_id)
 349                               | MACB_BF(REGA, regnum)
 350                               | MACB_BF(CODE, MACB_MAN_CODE)
 351                               | MACB_BF(DATA, value)));
 352
 353         /* wait for end of transfer */
 354         while (!MACB_BFEXT(IDLE, macb_readl(bp, NSR)))
 355                 cpu_relax();
 356
 357         return 0;
 358 }
 359
 360 /**
 361  * macb_set_tx_clk() - Set a clock to a new frequency
 362  * @clk         Pointer to the clock to change
 363  * @rate        New frequency in Hz
 364  * @dev         Pointer to the struct net_device
 365  */
 366 static void macb_set_tx_clk(struct clk *clk, int speed, struct net_device *dev)
 367 {
 368         long ferr, rate, rate_rounded;
 369
 370         if (!clk)
 371                 return;
 372
 373         switch (speed) {
 374         case SPEED_10:
 375                 rate = 2500000;
 376                 break;
 377         case SPEED_100:
 378                 rate = 25000000;
 379                 break;
 380         case SPEED_1000:
 381                 rate = 125000000;
 382                 break;
 383         default:
 384                 return;
 385         }
 386
 387         rate_rounded = clk_round_rate(clk, rate);
 388         if (rate_rounded < 0)
 389                 return;
 390
 391         /* RGMII allows 50 ppm frequency error. Test and warn if this limit
 392          * is not satisfied.
 393          */
 394         ferr = abs(rate_rounded - rate);
 395         ferr = DIV_ROUND_UP(ferr, rate / 100000);
 396         if (ferr > 5)
 397                 netdev_warn(dev, "unable to generate target frequency: %ld Hz\n",
 398                             rate);
 399
 400         if (clk_set_rate(clk, rate_rounded))
 401                 netdev_err(dev, "adjusting tx_clk failed.\n");
 402 }
 403
 404 static void macb_handle_link_change(struct net_device *dev)
 405 {
 406         struct macb *bp = netdev_priv(dev);
 407         struct phy_device *phydev = dev->phydev;
 408         unsigned long flags;
 409         int status_change = 0;
 410
 411         spin_lock_irqsave(&bp->lock, flags);
 412
 413         if (phydev->link) {
 414                 if ((bp->speed != phydev->speed) ||
 415                     (bp->duplex != phydev->duplex)) {
 416                         u32 reg;
 417
 418                         reg = macb_readl(bp, NCFGR);
 419                         reg &= ~(MACB_BIT(SPD) | MACB_BIT(FD));
 420                         if (macb_is_gem(bp))
 421                                 reg &= ~GEM_BIT(GBE);
 422
 423                         if (phydev->duplex)
 424                                 reg |= MACB_BIT(FD);
 425                         if (phydev->speed == SPEED_100)
 426                                 reg |= MACB_BIT(SPD);
 427                         if (phydev->speed == SPEED_1000 &&
 428                             bp->caps & MACB_CAPS_GIGABIT_MODE_AVAILABLE)
 429                                 reg |= GEM_BIT(GBE);
 430
 431                         macb_or_gem_writel(bp, NCFGR, reg);
 432
 433                         bp->speed = phydev->speed;
 434                         bp->duplex = phydev->duplex;
 435                         status_change = 1;
 436                 }
 437         }
 438
 439         if (phydev->link != bp->link) {
 440                 if (!phydev->link) {
 441                         bp->speed = 0;
 442                         bp->duplex = -1;
 443                 }
 444                 bp->link = phydev->link;
 445
 446                 status_change = 1;
 447         }
 448
 449         spin_unlock_irqrestore(&bp->lock, flags);
 450
 451         if (status_change) {
 452                 if (phydev->link) {
 453                         /* Update the TX clock rate if and only if the link is
 454                          * up and there has been a link change.
 455                          */
 456                         macb_set_tx_clk(bp->tx_clk, phydev->speed, dev);
 457
 458                         netif_carrier_on(dev);
 459                         netdev_info(dev, "link up (%d/%s)\n",
 460                                     phydev->speed,
 461                                     phydev->duplex == DUPLEX_FULL ?
 462                                     "Full" : "Half");
 463                 } else {
 464                         netif_carrier_off(dev);
 465                         netdev_info(dev, "link down\n");
 466                 }
 467         }
 468 }
 469
 470 /* based on au1000_eth. c*/
 471 static int macb_mii_probe(struct net_device *dev)
 472 {
 473         struct macb *bp = netdev_priv(dev);
 474         struct macb_platform_data *pdata;
 475         struct phy_device *phydev;
 476         struct device_node *np;
 477         int phy_irq, ret, i;
 478
 479         pdata = dev_get_platdata(&bp->pdev->dev);
 480         np = bp->pdev->dev.of_node;
 481         ret = 0;
 482
 483         if (np) {
 484                 if (of_phy_is_fixed_link(np)) {
 485                         bp->phy_node = of_node_get(np);
 486                 } else {
 487                         bp->phy_node = of_parse_phandle(np, "phy-handle", 0);
 488                         /* fallback to standard phy registration if no
 489                          * phy-handle was found nor any phy found during
 490                          * dt phy registration
 491                          */
 492                         if (!bp->phy_node && !phy_find_first(bp->mii_bus)) {
 493                                 for (i = 0; i < PHY_MAX_ADDR; i++) {
 494                                         struct phy_device *phydev;
 495
 496                                         phydev = mdiobus_scan(bp->mii_bus, i);
 497                                         if (IS_ERR(phydev) &&
 498                                             PTR_ERR(phydev) != -ENODEV) {
 499                                                 ret = PTR_ERR(phydev);
 500                                                 break;
 501                                         }
 502                                 }
 503
 504                                 if (ret)
 505                                         return -ENODEV;
 506                         }
 507                 }
 508         }
 509
 510         if (bp->phy_node) {
 511                 phydev = of_phy_connect(dev, bp->phy_node,
 512                                         &macb_handle_link_change, 0,
 513                                         bp->phy_interface);
 514                 if (!phydev)
 515                         return -ENODEV;
 516         } else {
 517                 phydev = phy_find_first(bp->mii_bus);
 518                 if (!phydev) {
 519                         netdev_err(dev, "no PHY found\n");
 520                         return -ENXIO;
 521                 }
 522
 523                 if (pdata) {
 524                         if (gpio_is_valid(pdata->phy_irq_pin)) {
 525                                 ret = devm_gpio_request(&bp->pdev->dev,
 526                                                         pdata->phy_irq_pin, "phy int");
 527                                 if (!ret) {
 528                                         phy_irq = gpio_to_irq(pdata->phy_irq_pin);
 529                                         phydev->irq = (phy_irq < 0) ? PHY_POLL : phy_irq;
 530                                 }
 531                         } else {
 532                                 phydev->irq = PHY_POLL;
 533                         }
 534                 }
 535
 536                 /* attach the mac to the phy */
 537                 ret = phy_connect_direct(dev, phydev, &macb_handle_link_change,
 538                                          bp->phy_interface);
 539                 if (ret) {
 540                         netdev_err(dev, "Could not attach to PHY\n");
 541                         return ret;
 542                 }
 543         }
 544
 545         /* mask with MAC supported features */
 546         if (macb_is_gem(bp) && bp->caps & MACB_CAPS_GIGABIT_MODE_AVAILABLE)
 547                 phy_set_max_speed(phydev, SPEED_1000);
 548         else
 549                 phy_set_max_speed(phydev, SPEED_100);
 550
 551         if (bp->caps & MACB_CAPS_NO_GIGABIT_HALF)
 552                 phy_remove_link_mode(phydev,
 553                                      ETHTOOL_LINK_MODE_1000baseT_Half_BIT);
 554
 555         bp->link = 0;
 556         bp->speed = 0;
 557         bp->duplex = -1;
 558
 559         return 0;
 560 }
 561
 562 static int macb_mii_init(struct macb *bp)
 563 {
 564         struct macb_platform_data *pdata;
 565         struct device_node *np;
 566         int err = -ENXIO;
 567
 568         /* Enable management port */
 569         macb_writel(bp, NCR, MACB_BIT(MPE));
 570
 571         bp->mii_bus = mdiobus_alloc();
 572         if (!bp->mii_bus) {
 573                 err = -ENOMEM;
 574                 goto err_out;
 575         }
 576
 577         bp->mii_bus->name = "MACB_mii_bus";
 578         bp->mii_bus->read = &macb_mdio_read;
 579         bp->mii_bus->write = &macb_mdio_write;
 580         snprintf(bp->mii_bus->id, MII_BUS_ID_SIZE, "%s-%x",
 581                  bp->pdev->name, bp->pdev->id);
 582         bp->mii_bus->priv = bp;
 583         bp->mii_bus->parent = &bp->pdev->dev;
 584         pdata = dev_get_platdata(&bp->pdev->dev);
 585
 586         dev_set_drvdata(&bp->dev->dev, bp->mii_bus);
 587
 588         np = bp->pdev->dev.of_node;
 589         if (np && of_phy_is_fixed_link(np)) {
 590                 if (of_phy_register_fixed_link(np) < 0) {
 591                         dev_err(&bp->pdev->dev,
 592                                 "broken fixed-link specification %pOF\n", np);
 593                         goto err_out_free_mdiobus;
 594                 }
 595
 596                 err = mdiobus_register(bp->mii_bus);
 597         } else {
 598                 if (pdata)
 599                         bp->mii_bus->phy_mask = pdata->phy_mask;
 600
 601                 err = of_mdiobus_register(bp->mii_bus, np);
 602         }
 603
 604         if (err)
 605                 goto err_out_free_fixed_link;
 606
 607         err = macb_mii_probe(bp->dev);
 608         if (err)
 609                 goto err_out_unregister_bus;
 610
 611         return 0;
 612
 613 err_out_unregister_bus:
 614         mdiobus_unregister(bp->mii_bus);
 615 err_out_free_fixed_link:
 616         if (np && of_phy_is_fixed_link(np))
 617                 of_phy_deregister_fixed_link(np);
 618 err_out_free_mdiobus:
 619         of_node_put(bp->phy_node);
 620         mdiobus_free(bp->mii_bus);
 621 err_out:
 622         return err;
 623 }
 624
 625 static void macb_update_stats(struct macb *bp)
 626 {
 627         u32 *p = &bp->hw_stats.macb.rx_pause_frames;
 628         u32 *end = &bp->hw_stats.macb.tx_pause_frames + 1;
 629         int offset = MACB_PFR;
 630
 631         WARN_ON((unsigned long)(end - p - 1) != (MACB_TPF - MACB_PFR) / 4);
 632
 633         for (; p < end; p++, offset += 4)
 634                 *p += bp->macb_reg_readl(bp, offset);
 635 }
 636
 637 static int macb_halt_tx(struct macb *bp)
 638 {
 639         unsigned long   halt_time, timeout;
 640         u32             status;
 641
 642         macb_writel(bp, NCR, macb_readl(bp, NCR) | MACB_BIT(THALT));
 643
 644         timeout = jiffies + usecs_to_jiffies(MACB_HALT_TIMEOUT);
 645         do {
 646                 halt_time = jiffies;
 647                 status = macb_readl(bp, TSR);
 648                 if (!(status & MACB_BIT(TGO)))
 649                         return 0;
 650
 651                 udelay(250);
 652         } while (time_before(halt_time, timeout));
 653
 654         return -ETIMEDOUT;
 655 }
 656
 657 static void macb_tx_unmap(struct macb *bp, struct macb_tx_skb *tx_skb)
 658 {
 659         if (tx_skb->mapping) {
 660                 if (tx_skb->mapped_as_page)
 661                         dma_unmap_page(&bp->pdev->dev, tx_skb->mapping,
 662                                        tx_skb->size, DMA_TO_DEVICE);
 663                 else
 664                         dma_unmap_single(&bp->pdev->dev, tx_skb->mapping,
 665                                          tx_skb->size, DMA_TO_DEVICE);
 666                 tx_skb->mapping = 0;
 667         }
 668
 669         if (tx_skb->skb) {
 670                 dev_kfree_skb_any(tx_skb->skb);
 671                 tx_skb->skb = NULL;
 672         }
 673 }
 674
 675 static void macb_set_addr(struct macb *bp, struct macb_dma_desc *desc, dma_addr_t addr)
 676 {
 677 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
 678         struct macb_dma_desc_64 *desc_64;
 679
 680         if (bp->hw_dma_cap & HW_DMA_CAP_64B) {
 681                 desc_64 = macb_64b_desc(bp, desc);
 682                 desc_64->addrh = upper_32_bits(addr);
 683                 /* The low bits of RX address contain the RX_USED bit, clearing
 684                  * of which allows packet RX. Make sure the high bits are also
 685                  * visible to HW at that point.
 686                  */
 687                 dma_wmb();
 688         }
 689 #endif
 690         desc->addr = lower_32_bits(addr);
 691 }
 692
 693 static dma_addr_t macb_get_addr(struct macb *bp, struct macb_dma_desc *desc)
 694 {
 695         dma_addr_t addr = 0;
 696 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
 697         struct macb_dma_desc_64 *desc_64;
 698
 699         if (bp->hw_dma_cap & HW_DMA_CAP_64B) {
 700                 desc_64 = macb_64b_desc(bp, desc);
 701                 addr = ((u64)(desc_64->addrh) << 32);
 702         }
 703 #endif
 704         addr |= MACB_BF(RX_WADDR, MACB_BFEXT(RX_WADDR, desc->addr));
 705         return addr;
 706 }
 707
 708 static void macb_tx_error_task(struct work_struct *work)
 709 {
 710         struct macb_queue       *queue = container_of(work, struct macb_queue,
 711                                                       tx_error_task);
 712         struct macb             *bp = queue->bp;
 713         struct macb_tx_skb      *tx_skb;
 714         struct macb_dma_desc    *desc;
 715         struct sk_buff          *skb;
 716         unsigned int            tail;
 717         unsigned long           flags;
 718
 719         netdev_vdbg(bp->dev, "macb_tx_error_task: q = %u, t = %u, h = %u\n",
 720                     (unsigned int)(queue - bp->queues),
 721                     queue->tx_tail, queue->tx_head);
 722
 723         /* Prevent the queue IRQ handlers from running: each of them may call
 724          * macb_tx_interrupt(), which in turn may call netif_wake_subqueue().
 725          * As explained below, we have to halt the transmission before updating
 726          * TBQP registers so we call netif_tx_stop_all_queues() to notify the
 727          * network engine about the macb/gem being halted.
 728          */
 729         spin_lock_irqsave(&bp->lock, flags);
 730
 731         /* Make sure nobody is trying to queue up new packets */
 732         netif_tx_stop_all_queues(bp->dev);
 733
 734         /* Stop transmission now
 735          * (in case we have just queued new packets)
 736          * macb/gem must be halted to write TBQP register
 737          */
 738         if (macb_halt_tx(bp))
 739                 /* Just complain for now, reinitializing TX path can be good */
 740                 netdev_err(bp->dev, "BUG: halt tx timed out\n");
 741
 742         /* Treat frames in TX queue including the ones that caused the error.
 743          * Free transmit buffers in upper layer.
 744          */
 745         for (tail = queue->tx_tail; tail != queue->tx_head; tail++) {
 746                 u32     ctrl;
 747
 748                 desc = macb_tx_desc(queue, tail);
 749                 ctrl = desc->ctrl;
 750                 tx_skb = macb_tx_skb(queue, tail);
 751                 skb = tx_skb->skb;
 752
 753                 if (ctrl & MACB_BIT(TX_USED)) {
 754                         /* skb is set for the last buffer of the frame */
 755                         while (!skb) {
 756                                 macb_tx_unmap(bp, tx_skb);
 757                                 tail++;
 758                                 tx_skb = macb_tx_skb(queue, tail);
 759                                 skb = tx_skb->skb;
 760                         }
 761
 762                         /* ctrl still refers to the first buffer descriptor
 763                          * since it's the only one written back by the hardware
 764                          */
 765                         if (!(ctrl & MACB_BIT(TX_BUF_EXHAUSTED))) {
 766                                 netdev_vdbg(bp->dev, "txerr skb %u (data %p) TX complete\n",
 767                                             macb_tx_ring_wrap(bp, tail),
 768                                             skb->data);
 769                                 bp->dev->stats.tx_packets++;
 770                                 queue->stats.tx_packets++;
 771                                 bp->dev->stats.tx_bytes += skb->len;
 772                                 queue->stats.tx_bytes += skb->len;
 773                         }
 774                 } else {
 775                         /* "Buffers exhausted mid-frame" errors may only happen
 776                          * if the driver is buggy, so complain loudly about
 777                          * those. Statistics are updated by hardware.
 778                          */
 779                         if (ctrl & MACB_BIT(TX_BUF_EXHAUSTED))
 780                                 netdev_err(bp->dev,
 781                                            "BUG: TX buffers exhausted mid-frame\n");
 782
 783                         desc->ctrl = ctrl | MACB_BIT(TX_USED);
 784                 }
 785
 786                 macb_tx_unmap(bp, tx_skb);
 787         }
 788
 789         /* Set end of TX queue */
 790         desc = macb_tx_desc(queue, 0);
 791         macb_set_addr(bp, desc, 0);
 792         desc->ctrl = MACB_BIT(TX_USED);
 793
 794         /* Make descriptor updates visible to hardware */
 795         wmb();
 796
 797         /* Reinitialize the TX desc queue */
 798         queue_writel(queue, TBQP, lower_32_bits(queue->tx_ring_dma));
 799 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
 800         if (bp->hw_dma_cap & HW_DMA_CAP_64B)
 801                 queue_writel(queue, TBQPH, upper_32_bits(queue->tx_ring_dma));
 802 #endif
 803         /* Make TX ring reflect state of hardware */
 804         queue->tx_head = 0;
 805         queue->tx_tail = 0;
 806
 807         /* Housework before enabling TX IRQ */
 808         macb_writel(bp, TSR, macb_readl(bp, TSR));
 809         queue_writel(queue, IER, MACB_TX_INT_FLAGS);
 810
 811         /* Now we are ready to start transmission again */
 812         netif_tx_start_all_queues(bp->dev);
 813         macb_writel(bp, NCR, macb_readl(bp, NCR) | MACB_BIT(TSTART));
 814
 815         spin_unlock_irqrestore(&bp->lock, flags);
 816 }
 817
 818 static void macb_tx_interrupt(struct macb_queue *queue)
 819 {
 820         unsigned int tail;
 821         unsigned int head;
 822         u32 status;
 823         struct macb *bp = queue->bp;
 824         u16 queue_index = queue - bp->queues;
 825
 826         status = macb_readl(bp, TSR);
 827         macb_writel(bp, TSR, status);
 828
 829         if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
 830                 queue_writel(queue, ISR, MACB_BIT(TCOMP));
 831
 832         netdev_vdbg(bp->dev, "macb_tx_interrupt status = 0x%03lx\n",
 833                     (unsigned long)status);
 834
 835         head = queue->tx_head;
 836         for (tail = queue->tx_tail; tail != head; tail++) {
 837                 struct macb_tx_skb      *tx_skb;
 838                 struct sk_buff          *skb;
 839                 struct macb_dma_desc    *desc;
 840                 u32                     ctrl;
 841
 842                 desc = macb_tx_desc(queue, tail);
 843
 844                 /* Make hw descriptor updates visible to CPU */
 845                 rmb();
 846
 847                 ctrl = desc->ctrl;
 848
 849                 /* TX_USED bit is only set by hardware on the very first buffer
 850                  * descriptor of the transmitted frame.
 851                  */
 852                 if (!(ctrl & MACB_BIT(TX_USED)))
 853                         break;
 854
 855                 /* Process all buffers of the current transmitted frame */
 856                 for (;; tail++) {
 857                         tx_skb = macb_tx_skb(queue, tail);
 858                         skb = tx_skb->skb;
 859
 860                         /* First, update TX stats if needed */
 861                         if (skb) {
 862                                 if (gem_ptp_do_txstamp(queue, skb, desc) == 0) {
 863                                         /* skb now belongs to timestamp buffer
 864                                          * and will be removed later
 865                                          */
 866                                         tx_skb->skb = NULL;
 867                                 }
 868                                 netdev_vdbg(bp->dev, "skb %u (data %p) TX complete\n",
 869                                             macb_tx_ring_wrap(bp, tail),
 870                                             skb->data);
 871                                 bp->dev->stats.tx_packets++;
 872                                 queue->stats.tx_packets++;
 873                                 bp->dev->stats.tx_bytes += skb->len;
 874                                 queue->stats.tx_bytes += skb->len;
 875                         }
 876
 877                         /* Now we can safely release resources */
 878                         macb_tx_unmap(bp, tx_skb);
 879
 880                         /* skb is set only for the last buffer of the frame.
 881                          * WARNING: at this point skb has been freed by
 882                          * macb_tx_unmap().
 883                          */
 884                         if (skb)
 885                                 break;
 886                 }
 887         }
 888
 889         queue->tx_tail = tail;
 890         if (__netif_subqueue_stopped(bp->dev, queue_index) &&
 891             CIRC_CNT(queue->tx_head, queue->tx_tail,
 892                      bp->tx_ring_size) <= MACB_TX_WAKEUP_THRESH(bp))
 893                 netif_wake_subqueue(bp->dev, queue_index);
 894 }
 895
 896 static void gem_rx_refill(struct macb_queue *queue)
 897 {
 898         unsigned int            entry;
 899         struct sk_buff          *skb;
 900         dma_addr_t              paddr;
 901         struct macb *bp = queue->bp;
 902         struct macb_dma_desc *desc;
 903
 904         while (CIRC_SPACE(queue->rx_prepared_head, queue->rx_tail,
 905                         bp->rx_ring_size) > 0) {
 906                 entry = macb_rx_ring_wrap(bp, queue->rx_prepared_head);
 907
 908                 /* Make hw descriptor updates visible to CPU */
 909                 rmb();
 910
 911                 queue->rx_prepared_head++;
 912                 desc = macb_rx_desc(queue, entry);
 913
 914                 if (!queue->rx_skbuff[entry]) {
 915                         /* allocate sk_buff for this free entry in ring */
 916                         skb = netdev_alloc_skb(bp->dev, bp->rx_buffer_size);
 917                         if (unlikely(!skb)) {
 918                                 netdev_err(bp->dev,
 919                                            "Unable to allocate sk_buff\n");
 920                                 break;
 921                         }
 922
 923                         /* now fill corresponding descriptor entry */
 924                         paddr = dma_map_single(&bp->pdev->dev, skb->data,
 925                                                bp->rx_buffer_size,
 926                                                DMA_FROM_DEVICE);
 927                         if (dma_mapping_error(&bp->pdev->dev, paddr)) {
 928                                 dev_kfree_skb(skb);
 929                                 break;
 930                         }
 931
 932                         queue->rx_skbuff[entry] = skb;
 933
 934                         if (entry == bp->rx_ring_size - 1)
 935                                 paddr |= MACB_BIT(RX_WRAP);
 936                         desc->ctrl = 0;
 937                         /* Setting addr clears RX_USED and allows reception,
 938                          * make sure ctrl is cleared first to avoid a race.
 939                          */
 940                         dma_wmb();
 941                         macb_set_addr(bp, desc, paddr);
 942
 943                         /* properly align Ethernet header */
 944                         skb_reserve(skb, NET_IP_ALIGN);
 945                 } else {
 946                         desc->ctrl = 0;
 947                         dma_wmb();
 948                         desc->addr &= ~MACB_BIT(RX_USED);
 949                 }
 950         }
 951
 952         /* Make descriptor updates visible to hardware */
 953         wmb();
 954
 955         netdev_vdbg(bp->dev, "rx ring: queue: %p, prepared head %d, tail %d\n",
 956                         queue, queue->rx_prepared_head, queue->rx_tail);
 957 }
 958
 959 /* Mark DMA descriptors from begin up to and not including end as unused */
 960 static void discard_partial_frame(struct macb_queue *queue, unsigned int begin,
 961                                   unsigned int end)
 962 {
 963         unsigned int frag;
 964
 965         for (frag = begin; frag != end; frag++) {
 966                 struct macb_dma_desc *desc = macb_rx_desc(queue, frag);
 967
 968                 desc->addr &= ~MACB_BIT(RX_USED);
 969         }
 970
 971         /* Make descriptor updates visible to hardware */
 972         wmb();
 973
 974         /* When this happens, the hardware stats registers for
 975          * whatever caused this is updated, so we don't have to record
 976          * anything.
 977          */
 978 }
 979
 980 static int gem_rx(struct macb_queue *queue, int budget)
 981 {
 982         struct macb *bp = queue->bp;
 983         unsigned int            len;
 984         unsigned int            entry;
 985         struct sk_buff          *skb;
 986         struct macb_dma_desc    *desc;
 987         int                     count = 0;
 988
 989         while (count < budget) {
 990                 u32 ctrl;
 991                 dma_addr_t addr;
 992                 bool rxused;
 993
 994                 entry = macb_rx_ring_wrap(bp, queue->rx_tail);
 995                 desc = macb_rx_desc(queue, entry);
 996
 997                 /* Make hw descriptor updates visible to CPU */
 998                 rmb();
 999
1000                 rxused = (desc->addr & MACB_BIT(RX_USED)) ? true : false;
1001                 addr = macb_get_addr(bp, desc);
1002
1003                 if (!rxused)
1004                         break;
1005
1006                 /* Ensure ctrl is at least as up-to-date as rxused */
1007                 dma_rmb();
1008
1009                 ctrl = desc->ctrl;
1010
1011                 queue->rx_tail++;
1012                 count++;
1013
1014                 if (!(ctrl & MACB_BIT(RX_SOF) && ctrl & MACB_BIT(RX_EOF))) {
1015                         netdev_err(bp->dev,
1016                                    "not whole frame pointed by descriptor\n");
1017                         bp->dev->stats.rx_dropped++;
1018                         queue->stats.rx_dropped++;
1019                         break;
1020                 }
1021                 skb = queue->rx_skbuff[entry];
1022                 if (unlikely(!skb)) {
1023                         netdev_err(bp->dev,
1024                                    "inconsistent Rx descriptor chain\n");
1025                         bp->dev->stats.rx_dropped++;
1026                         queue->stats.rx_dropped++;
1027                         break;
1028                 }
1029                 /* now everything is ready for receiving packet */
1030                 queue->rx_skbuff[entry] = NULL;
1031                 len = ctrl & bp->rx_frm_len_mask;
1032
1033                 netdev_vdbg(bp->dev, "gem_rx %u (len %u)\n", entry, len);
1034
1035                 skb_put(skb, len);
1036                 dma_unmap_single(&bp->pdev->dev, addr,
1037                                  bp->rx_buffer_size, DMA_FROM_DEVICE);
1038
1039                 skb->protocol = eth_type_trans(skb, bp->dev);
1040                 skb_checksum_none_assert(skb);
1041                 if (bp->dev->features & NETIF_F_RXCSUM &&
1042                     !(bp->dev->flags & IFF_PROMISC) &&
1043                     GEM_BFEXT(RX_CSUM, ctrl) & GEM_RX_CSUM_CHECKED_MASK)
1044                         skb->ip_summed = CHECKSUM_UNNECESSARY;
1045
1046                 bp->dev->stats.rx_packets++;
1047                 queue->stats.rx_packets++;
1048                 bp->dev->stats.rx_bytes += skb->len;
1049                 queue->stats.rx_bytes += skb->len;
1050
1051                 gem_ptp_do_rxstamp(bp, skb, desc);
1052
1053 #if defined(DEBUG) && defined(VERBOSE_DEBUG)
1054                 netdev_vdbg(bp->dev, "received skb of length %u, csum: %08x\n",
1055                             skb->len, skb->csum);
1056                 print_hex_dump(KERN_DEBUG, " mac: ", DUMP_PREFIX_ADDRESS, 16, 1,
1057                                skb_mac_header(skb), 16, true);
1058                 print_hex_dump(KERN_DEBUG, "data: ", DUMP_PREFIX_ADDRESS, 16, 1,
1059                                skb->data, 32, true);
1060 #endif
1061
1062                 netif_receive_skb(skb);
1063         }
1064
1065         gem_rx_refill(queue);
1066
1067         return count;
1068 }
1069
1070 static int macb_rx_frame(struct macb_queue *queue, unsigned int first_frag,
1071                          unsigned int last_frag)
1072 {
1073         unsigned int len;
1074         unsigned int frag;
1075         unsigned int offset;
1076         struct sk_buff *skb;
1077         struct macb_dma_desc *desc;
1078         struct macb *bp = queue->bp;
1079
1080         desc = macb_rx_desc(queue, last_frag);
1081         len = desc->ctrl & bp->rx_frm_len_mask;
1082
1083         netdev_vdbg(bp->dev, "macb_rx_frame frags %u - %u (len %u)\n",
1084                 macb_rx_ring_wrap(bp, first_frag),
1085                 macb_rx_ring_wrap(bp, last_frag), len);
1086
1087         /* The ethernet header starts NET_IP_ALIGN bytes into the
1088          * first buffer. Since the header is 14 bytes, this makes the
1089          * payload word-aligned.
1090          *
1091          * Instead of calling skb_reserve(NET_IP_ALIGN), we just copy
1092          * the two padding bytes into the skb so that we avoid hitting
1093          * the slowpath in memcpy(), and pull them off afterwards.
1094          */
1095         skb = netdev_alloc_skb(bp->dev, len + NET_IP_ALIGN);
1096         if (!skb) {
1097                 bp->dev->stats.rx_dropped++;
1098                 for (frag = first_frag; ; frag++) {
1099                         desc = macb_rx_desc(queue, frag);
1100                         desc->addr &= ~MACB_BIT(RX_USED);
1101                         if (frag == last_frag)
1102                                 break;
1103                 }
1104
1105                 /* Make descriptor updates visible to hardware */
1106                 wmb();
1107
1108                 return 1;
1109         }
1110
1111         offset = 0;
1112         len += NET_IP_ALIGN;
1113         skb_checksum_none_assert(skb);
1114         skb_put(skb, len);
1115
1116         for (frag = first_frag; ; frag++) {
1117                 unsigned int frag_len = bp->rx_buffer_size;
1118
1119                 if (offset + frag_len > len) {
1120                         if (unlikely(frag != last_frag)) {
1121                                 dev_kfree_skb_any(skb);
1122                                 return -1;
1123                         }
1124                         frag_len = len - offset;
1125                 }
1126                 skb_copy_to_linear_data_offset(skb, offset,
1127                                                macb_rx_buffer(queue, frag),
1128                                                frag_len);
1129                 offset += bp->rx_buffer_size;
1130                 desc = macb_rx_desc(queue, frag);
1131                 desc->addr &= ~MACB_BIT(RX_USED);
1132
1133                 if (frag == last_frag)
1134                         break;
1135         }
1136
1137         /* Make descriptor updates visible to hardware */
1138         wmb();
1139
1140         __skb_pull(skb, NET_IP_ALIGN);
1141         skb->protocol = eth_type_trans(skb, bp->dev);
1142
1143         bp->dev->stats.rx_packets++;
1144         bp->dev->stats.rx_bytes += skb->len;
1145         netdev_vdbg(bp->dev, "received skb of length %u, csum: %08x\n",
1146                     skb->len, skb->csum);
1147         netif_receive_skb(skb);
1148
1149         return 0;
1150 }
1151
1152 static inline void macb_init_rx_ring(struct macb_queue *queue)
1153 {
1154         struct macb *bp = queue->bp;
1155         dma_addr_t addr;
1156         struct macb_dma_desc *desc = NULL;
1157         int i;
1158
1159         addr = queue->rx_buffers_dma;
1160         for (i = 0; i < bp->rx_ring_size; i++) {
1161                 desc = macb_rx_desc(queue, i);
1162                 macb_set_addr(bp, desc, addr);
1163                 desc->ctrl = 0;
1164                 addr += bp->rx_buffer_size;
1165         }
1166         desc->addr |= MACB_BIT(RX_WRAP);
1167         queue->rx_tail = 0;
1168 }
1169
1170 static int macb_rx(struct macb_queue *queue, int budget)
1171 {
1172         struct macb *bp = queue->bp;
1173         bool reset_rx_queue = false;
1174         int received = 0;
1175         unsigned int tail;
1176         int first_frag = -1;
1177
1178         for (tail = queue->rx_tail; budget > 0; tail++) {
1179                 struct macb_dma_desc *desc = macb_rx_desc(queue, tail);
1180                 u32 ctrl;
1181
1182                 /* Make hw descriptor updates visible to CPU */
1183                 rmb();
1184
1185                 if (!(desc->addr & MACB_BIT(RX_USED)))
1186                         break;
1187
1188                 /* Ensure ctrl is at least as up-to-date as addr */
1189                 dma_rmb();
1190
1191                 ctrl = desc->ctrl;
1192
1193                 if (ctrl & MACB_BIT(RX_SOF)) {
1194                         if (first_frag != -1)
1195                                 discard_partial_frame(queue, first_frag, tail);
1196                         first_frag = tail;
1197                 }
1198
1199                 if (ctrl & MACB_BIT(RX_EOF)) {
1200                         int dropped;
1201
1202                         if (unlikely(first_frag == -1)) {
1203                                 reset_rx_queue = true;
1204                                 continue;
1205                         }
1206
1207                         dropped = macb_rx_frame(queue, first_frag, tail);
1208                         first_frag = -1;
1209                         if (unlikely(dropped < 0)) {
1210                                 reset_rx_queue = true;
1211                                 continue;
1212                         }
1213                         if (!dropped) {
1214                                 received++;
1215                                 budget--;
1216                         }
1217                 }
1218         }
1219
1220         if (unlikely(reset_rx_queue)) {
1221                 unsigned long flags;
1222                 u32 ctrl;
1223
1224                 netdev_err(bp->dev, "RX queue corruption: reset it\n");
1225
1226                 spin_lock_irqsave(&bp->lock, flags);
1227
1228                 ctrl = macb_readl(bp, NCR);
1229                 macb_writel(bp, NCR, ctrl & ~MACB_BIT(RE));
1230
1231                 macb_init_rx_ring(queue);
1232                 queue_writel(queue, RBQP, queue->rx_ring_dma);
1233
1234                 macb_writel(bp, NCR, ctrl | MACB_BIT(RE));
1235
1236                 spin_unlock_irqrestore(&bp->lock, flags);
1237                 return received;
1238         }
1239
1240         if (first_frag != -1)
1241                 queue->rx_tail = first_frag;
1242         else
1243                 queue->rx_tail = tail;
1244
1245         return received;
1246 }
1247
1248 static int macb_poll(struct napi_struct *napi, int budget)
1249 {
1250         struct macb_queue *queue = container_of(napi, struct macb_queue, napi);
1251         struct macb *bp = queue->bp;
1252         int work_done;
1253         u32 status;
1254
1255         status = macb_readl(bp, RSR);
1256         macb_writel(bp, RSR, status);
1257
1258         netdev_vdbg(bp->dev, "poll: status = %08lx, budget = %d\n",
1259                     (unsigned long)status, budget);
1260
1261         work_done = bp->macbgem_ops.mog_rx(queue, budget);
1262         if (work_done < budget) {
1263                 napi_complete_done(napi, work_done);
1264
1265                 /* Packets received while interrupts were disabled */
1266                 status = macb_readl(bp, RSR);
1267                 if (status) {
1268                         if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
1269                                 queue_writel(queue, ISR, MACB_BIT(RCOMP));
1270                         napi_reschedule(napi);
1271                 } else {
1272                         queue_writel(queue, IER, bp->rx_intr_mask);
1273                 }
1274         }
1275
1276         /* TODO: Handle errors */
1277
1278         return work_done;
1279 }
1280
1281 static void macb_hresp_error_task(unsigned long data)
1282 {
1283         struct macb *bp = (struct macb *)data;
1284         struct net_device *dev = bp->dev;
1285         struct macb_queue *queue = bp->queues;
1286         unsigned int q;
1287         u32 ctrl;
1288
1289         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
1290                 queue_writel(queue, IDR, bp->rx_intr_mask |
1291                                          MACB_TX_INT_FLAGS |
1292                                          MACB_BIT(HRESP));
1293         }
1294         ctrl = macb_readl(bp, NCR);
1295         ctrl &= ~(MACB_BIT(RE) | MACB_BIT(TE));
1296         macb_writel(bp, NCR, ctrl);
1297
1298         netif_tx_stop_all_queues(dev);
1299         netif_carrier_off(dev);
1300
1301         bp->macbgem_ops.mog_init_rings(bp);
1302
1303         /* Initialize TX and RX buffers */
1304         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
1305                 queue_writel(queue, RBQP, lower_32_bits(queue->rx_ring_dma));
1306 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
1307                 if (bp->hw_dma_cap & HW_DMA_CAP_64B)
1308                         queue_writel(queue, RBQPH,
1309                                      upper_32_bits(queue->rx_ring_dma));
1310 #endif
1311                 queue_writel(queue, TBQP, lower_32_bits(queue->tx_ring_dma));
1312 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
1313                 if (bp->hw_dma_cap & HW_DMA_CAP_64B)
1314                         queue_writel(queue, TBQPH,
1315                                      upper_32_bits(queue->tx_ring_dma));
1316 #endif
1317
1318                 /* Enable interrupts */
1319                 queue_writel(queue, IER,
1320                              bp->rx_intr_mask |
1321                              MACB_TX_INT_FLAGS |
1322                              MACB_BIT(HRESP));
1323         }
1324
1325         ctrl |= MACB_BIT(RE) | MACB_BIT(TE);
1326         macb_writel(bp, NCR, ctrl);
1327
1328         netif_carrier_on(dev);
1329         netif_tx_start_all_queues(dev);
1330 }
1331
1332 static void macb_tx_restart(struct macb_queue *queue)
1333 {
1334         unsigned int head = queue->tx_head;
1335         unsigned int tail = queue->tx_tail;
1336         struct macb *bp = queue->bp;
1337
1338         if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
1339                 queue_writel(queue, ISR, MACB_BIT(TXUBR));
1340
1341         if (head == tail)
1342                 return;
1343
1344         macb_writel(bp, NCR, macb_readl(bp, NCR) | MACB_BIT(TSTART));
1345 }
1346
1347 static irqreturn_t macb_interrupt(int irq, void *dev_id)
1348 {
1349         struct macb_queue *queue = dev_id;
1350         struct macb *bp = queue->bp;
1351         struct net_device *dev = bp->dev;
1352         u32 status, ctrl;
1353
1354         status = queue_readl(queue, ISR);
1355
1356         if (unlikely(!status))
1357                 return IRQ_NONE;
1358
1359         spin_lock(&bp->lock);
1360
1361         while (status) {
1362                 /* close possible race with dev_close */
1363                 if (unlikely(!netif_running(dev))) {
1364                         queue_writel(queue, IDR, -1);
1365                         if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
1366                                 queue_writel(queue, ISR, -1);
1367                         break;
1368                 }
1369
1370                 netdev_vdbg(bp->dev, "queue = %u, isr = 0x%08lx\n",
1371                             (unsigned int)(queue - bp->queues),
1372                             (unsigned long)status);
1373
1374                 if (status & bp->rx_intr_mask) {
1375                         /* There's no point taking any more interrupts
1376                          * until we have processed the buffers. The
1377                          * scheduling call may fail if the poll routine
1378                          * is already scheduled, so disable interrupts
1379                          * now.
1380                          */
1381                         queue_writel(queue, IDR, bp->rx_intr_mask);
1382                         if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
1383                                 queue_writel(queue, ISR, MACB_BIT(RCOMP));
1384
1385                         if (napi_schedule_prep(&queue->napi)) {
1386                                 netdev_vdbg(bp->dev, "scheduling RX softirq\n");
1387                                 __napi_schedule(&queue->napi);
1388                         }
1389                 }
1390
1391                 if (unlikely(status & (MACB_TX_ERR_FLAGS))) {
1392                         queue_writel(queue, IDR, MACB_TX_INT_FLAGS);
1393                         schedule_work(&queue->tx_error_task);
1394
1395                         if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
1396                                 queue_writel(queue, ISR, MACB_TX_ERR_FLAGS);
1397
1398                         break;
1399                 }
1400
1401                 if (status & MACB_BIT(TCOMP))
1402                         macb_tx_interrupt(queue);
1403
1404                 if (status & MACB_BIT(TXUBR))
1405                         macb_tx_restart(queue);
1406
1407                 /* Link change detection isn't possible with RMII, so we'll
1408                  * add that if/when we get our hands on a full-blown MII PHY.
1409                  */
1410
1411                 /* There is a hardware issue under heavy load where DMA can
1412                  * stop, this causes endless "used buffer descriptor read"
1413                  * interrupts but it can be cleared by re-enabling RX. See
1414                  * the at91rm9200 manual, section 41.3.1 or the Zynq manual
1415                  * section 16.7.4 for details. RXUBR is only enabled for
1416                  * these two versions.
1417                  */
1418                 if (status & MACB_BIT(RXUBR)) {
1419                         ctrl = macb_readl(bp, NCR);
1420                         macb_writel(bp, NCR, ctrl & ~MACB_BIT(RE));
1421                         wmb();
1422                         macb_writel(bp, NCR, ctrl | MACB_BIT(RE));
1423
1424                         if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
1425                                 queue_writel(queue, ISR, MACB_BIT(RXUBR));
1426                 }
1427
1428                 if (status & MACB_BIT(ISR_ROVR)) {
1429                         /* We missed at least one packet */
1430                         if (macb_is_gem(bp))
1431                                 bp->hw_stats.gem.rx_overruns++;
1432                         else
1433                                 bp->hw_stats.macb.rx_overruns++;
1434
1435                         if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
1436                                 queue_writel(queue, ISR, MACB_BIT(ISR_ROVR));
1437                 }
1438
1439                 if (status & MACB_BIT(HRESP)) {
1440                         tasklet_schedule(&bp->hresp_err_tasklet);
1441                         netdev_err(dev, "DMA bus error: HRESP not OK\n");
1442
1443                         if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
1444                                 queue_writel(queue, ISR, MACB_BIT(HRESP));
1445                 }
1446                 status = queue_readl(queue, ISR);
1447         }
1448
1449         spin_unlock(&bp->lock);
1450
1451         return IRQ_HANDLED;
1452 }
1453
1454 #ifdef CONFIG_NET_POLL_CONTROLLER
1455 /* Polling receive - used by netconsole and other diagnostic tools
1456  * to allow network i/o with interrupts disabled.
1457  */
1458 static void macb_poll_controller(struct net_device *dev)
1459 {
1460         struct macb *bp = netdev_priv(dev);
1461         struct macb_queue *queue;
1462         unsigned long flags;
1463         unsigned int q;
1464
1465         local_irq_save(flags);
1466         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue)
1467                 macb_interrupt(dev->irq, queue);
1468         local_irq_restore(flags);
1469 }
1470 #endif
1471
1472 static unsigned int macb_tx_map(struct macb *bp,
1473                                 struct macb_queue *queue,
1474                                 struct sk_buff *skb,
1475                                 unsigned int hdrlen)
1476 {
1477         dma_addr_t mapping;
1478         unsigned int len, entry, i, tx_head = queue->tx_head;
1479         struct macb_tx_skb *tx_skb = NULL;
1480         struct macb_dma_desc *desc;
1481         unsigned int offset, size, count = 0;
1482         unsigned int f, nr_frags = skb_shinfo(skb)->nr_frags;
1483         unsigned int eof = 1, mss_mfs = 0;
1484         u32 ctrl, lso_ctrl = 0, seq_ctrl = 0;
1485
1486         /* LSO */
1487         if (skb_shinfo(skb)->gso_size != 0) {
1488                 if (ip_hdr(skb)->protocol == IPPROTO_UDP)
1489                         /* UDP - UFO */
1490                         lso_ctrl = MACB_LSO_UFO_ENABLE;
1491                 else
1492                         /* TCP - TSO */
1493                         lso_ctrl = MACB_LSO_TSO_ENABLE;
1494         }
1495
1496         /* First, map non-paged data */
1497         len = skb_headlen(skb);
1498
1499         /* first buffer length */
1500         size = hdrlen;
1501
1502         offset = 0;
1503         while (len) {
1504                 entry = macb_tx_ring_wrap(bp, tx_head);
1505                 tx_skb = &queue->tx_skb[entry];
1506
1507                 mapping = dma_map_single(&bp->pdev->dev,
1508                                          skb->data + offset,
1509                                          size, DMA_TO_DEVICE);
1510                 if (dma_mapping_error(&bp->pdev->dev, mapping))
1511                         goto dma_error;
1512
1513                 /* Save info to properly release resources */
1514                 tx_skb->skb = NULL;
1515                 tx_skb->mapping = mapping;
1516                 tx_skb->size = size;
1517                 tx_skb->mapped_as_page = false;
1518
1519                 len -= size;
1520                 offset += size;
1521                 count++;
1522                 tx_head++;
1523
1524                 size = min(len, bp->max_tx_length);
1525         }
1526
1527         /* Then, map paged data from fragments */
1528         for (f = 0; f < nr_frags; f++) {
1529                 const skb_frag_t *frag = &skb_shinfo(skb)->frags[f];
1530
1531                 len = skb_frag_size(frag);
1532                 offset = 0;
1533                 while (len) {
1534                         size = min(len, bp->max_tx_length);
1535                         entry = macb_tx_ring_wrap(bp, tx_head);
1536                         tx_skb = &queue->tx_skb[entry];
1537
1538                         mapping = skb_frag_dma_map(&bp->pdev->dev, frag,
1539                                                    offset, size, DMA_TO_DEVICE);
1540                         if (dma_mapping_error(&bp->pdev->dev, mapping))
1541                                 goto dma_error;
1542
1543                         /* Save info to properly release resources */
1544                         tx_skb->skb = NULL;
1545                         tx_skb->mapping = mapping;
1546                         tx_skb->size = size;
1547                         tx_skb->mapped_as_page = true;
1548
1549                         len -= size;
1550                         offset += size;
1551                         count++;
1552                         tx_head++;
1553                 }
1554         }
1555
1556         /* Should never happen */
1557         if (unlikely(!tx_skb)) {
1558                 netdev_err(bp->dev, "BUG! empty skb!\n");
1559                 return 0;
1560         }
1561
1562         /* This is the last buffer of the frame: save socket buffer */
1563         tx_skb->skb = skb;
1564
1565         /* Update TX ring: update buffer descriptors in reverse order
1566          * to avoid race condition
1567          */
1568
1569         /* Set 'TX_USED' bit in buffer descriptor at tx_head position
1570          * to set the end of TX queue
1571          */
1572         i = tx_head;
1573         entry = macb_tx_ring_wrap(bp, i);
1574         ctrl = MACB_BIT(TX_USED);
1575         desc = macb_tx_desc(queue, entry);
1576         desc->ctrl = ctrl;
1577
1578         if (lso_ctrl) {
1579                 if (lso_ctrl == MACB_LSO_UFO_ENABLE)
1580                         /* include header and FCS in value given to h/w */
1581                         mss_mfs = skb_shinfo(skb)->gso_size +
1582                                         skb_transport_offset(skb) +
1583                                         ETH_FCS_LEN;
1584                 else /* TSO */ {
1585                         mss_mfs = skb_shinfo(skb)->gso_size;
1586                         /* TCP Sequence Number Source Select
1587                          * can be set only for TSO
1588                          */
1589                         seq_ctrl = 0;
1590                 }
1591         }
1592
1593         do {
1594                 i--;
1595                 entry = macb_tx_ring_wrap(bp, i);
1596                 tx_skb = &queue->tx_skb[entry];
1597                 desc = macb_tx_desc(queue, entry);
1598
1599                 ctrl = (u32)tx_skb->size;
1600                 if (eof) {
1601                         ctrl |= MACB_BIT(TX_LAST);
1602                         eof = 0;
1603                 }
1604                 if (unlikely(entry == (bp->tx_ring_size - 1)))
1605                         ctrl |= MACB_BIT(TX_WRAP);
1606
1607                 /* First descriptor is header descriptor */
1608                 if (i == queue->tx_head) {
1609                         ctrl |= MACB_BF(TX_LSO, lso_ctrl);
1610                         ctrl |= MACB_BF(TX_TCP_SEQ_SRC, seq_ctrl);
1611                         if ((bp->dev->features & NETIF_F_HW_CSUM) &&
1612                             skb->ip_summed != CHECKSUM_PARTIAL && !lso_ctrl)
1613                                 ctrl |= MACB_BIT(TX_NOCRC);
1614                 } else
1615                         /* Only set MSS/MFS on payload descriptors
1616                          * (second or later descriptor)
1617                          */
1618                         ctrl |= MACB_BF(MSS_MFS, mss_mfs);
1619
1620                 /* Set TX buffer descriptor */
1621                 macb_set_addr(bp, desc, tx_skb->mapping);
1622                 /* desc->addr must be visible to hardware before clearing
1623                  * 'TX_USED' bit in desc->ctrl.
1624                  */
1625                 wmb();
1626                 desc->ctrl = ctrl;
1627         } while (i != queue->tx_head);
1628
1629         queue->tx_head = tx_head;
1630
1631         return count;
1632
1633 dma_error:
1634         netdev_err(bp->dev, "TX DMA map failed\n");
1635
1636         for (i = queue->tx_head; i != tx_head; i++) {
1637                 tx_skb = macb_tx_skb(queue, i);
1638
1639                 macb_tx_unmap(bp, tx_skb);
1640         }
1641
1642         return 0;
1643 }
1644
1645 static netdev_features_t macb_features_check(struct sk_buff *skb,
1646                                              struct net_device *dev,
1647                                              netdev_features_t features)
1648 {
1649         unsigned int nr_frags, f;
1650         unsigned int hdrlen;
1651
1652         /* Validate LSO compatibility */
1653
1654         /* there is only one buffer */
1655         if (!skb_is_nonlinear(skb))
1656                 return features;
1657
1658         /* length of header */
1659         hdrlen = skb_transport_offset(skb);
1660         if (ip_hdr(skb)->protocol == IPPROTO_TCP)
1661                 hdrlen += tcp_hdrlen(skb);
1662
1663         /* For LSO:
1664          * When software supplies two or more payload buffers all payload buffers
1665          * apart from the last must be a multiple of 8 bytes in size.
1666          */
1667         if (!IS_ALIGNED(skb_headlen(skb) - hdrlen, MACB_TX_LEN_ALIGN))
1668                 return features & ~MACB_NETIF_LSO;
1669
1670         nr_frags = skb_shinfo(skb)->nr_frags;
1671         /* No need to check last fragment */
1672         nr_frags--;
1673         for (f = 0; f < nr_frags; f++) {
1674                 const skb_frag_t *frag = &skb_shinfo(skb)->frags[f];
1675
1676                 if (!IS_ALIGNED(skb_frag_size(frag), MACB_TX_LEN_ALIGN))
1677                         return features & ~MACB_NETIF_LSO;
1678         }
1679         return features;
1680 }
1681
1682 static inline int macb_clear_csum(struct sk_buff *skb)
1683 {
1684         /* no change for packets without checksum offloading */
1685         if (skb->ip_summed != CHECKSUM_PARTIAL)
1686                 return 0;
1687
1688         /* make sure we can modify the header */
1689         if (unlikely(skb_cow_head(skb, 0)))
1690                 return -1;
1691
1692         /* initialize checksum field
1693          * This is required - at least for Zynq, which otherwise calculates
1694          * wrong UDP header checksums for UDP packets with UDP data len <=2
1695          */
1696         *(__sum16 *)(skb_checksum_start(skb) + skb->csum_offset) = 0;
1697         return 0;
1698 }
1699
1700 static int macb_pad_and_fcs(struct sk_buff **skb, struct net_device *ndev)
1701 {
1702         bool cloned = skb_cloned(*skb) || skb_header_cloned(*skb);
1703         int padlen = ETH_ZLEN - (*skb)->len;
1704         int headroom = skb_headroom(*skb);
1705         int tailroom = skb_tailroom(*skb);
1706         struct sk_buff *nskb;
1707         u32 fcs;
1708
1709         if (!(ndev->features & NETIF_F_HW_CSUM) ||
1710             !((*skb)->ip_summed != CHECKSUM_PARTIAL) ||
1711             skb_shinfo(*skb)->gso_size) /* Not available for GSO */
1712                 return 0;
1713
1714         if (padlen <= 0) {
1715                 /* FCS could be appeded to tailroom. */
1716                 if (tailroom >= ETH_FCS_LEN)
1717                         goto add_fcs;
1718                 /* FCS could be appeded by moving data to headroom. */
1719                 else if (!cloned && headroom + tailroom >= ETH_FCS_LEN)
1720                         padlen = 0;
1721                 /* No room for FCS, need to reallocate skb. */
1722                 else
1723                         padlen = ETH_FCS_LEN;
1724         } else {
1725                 /* Add room for FCS. */
1726                 padlen += ETH_FCS_LEN;
1727         }
1728
1729         if (!cloned && headroom + tailroom >= padlen) {
1730                 (*skb)->data = memmove((*skb)->head, (*skb)->data, (*skb)->len);
1731                 skb_set_tail_pointer(*skb, (*skb)->len);
1732         } else {
1733                 nskb = skb_copy_expand(*skb, 0, padlen, GFP_ATOMIC);
1734                 if (!nskb)
1735                         return -ENOMEM;
1736
1737                 dev_kfree_skb_any(*skb);
1738                 *skb = nskb;
1739         }
1740
1741         if (padlen > ETH_FCS_LEN)
1742                 skb_put_zero(*skb, padlen - ETH_FCS_LEN);
1743
1744 add_fcs:
1745         /* set FCS to packet */
1746         fcs = crc32_le(~0, (*skb)->data, (*skb)->len);
1747         fcs = ~fcs;
1748
1749         skb_put_u8(*skb, fcs            & 0xff);
1750         skb_put_u8(*skb, (fcs >> 8)     & 0xff);
1751         skb_put_u8(*skb, (fcs >> 16)    & 0xff);
1752         skb_put_u8(*skb, (fcs >> 24)    & 0xff);
1753
1754         return 0;
1755 }
1756
1757 static netdev_tx_t macb_start_xmit(struct sk_buff *skb, struct net_device *dev)
1758 {
1759         u16 queue_index = skb_get_queue_mapping(skb);
1760         struct macb *bp = netdev_priv(dev);
1761         struct macb_queue *queue = &bp->queues[queue_index];
1762         unsigned long flags;
1763         unsigned int desc_cnt, nr_frags, frag_size, f;
1764         unsigned int hdrlen;
1765         bool is_lso, is_udp = 0;
1766         netdev_tx_t ret = NETDEV_TX_OK;
1767
1768         if (macb_clear_csum(skb)) {
1769                 dev_kfree_skb_any(skb);
1770                 return ret;
1771         }
1772
1773         if (macb_pad_and_fcs(&skb, dev)) {
1774                 dev_kfree_skb_any(skb);
1775                 return ret;
1776         }
1777
1778         is_lso = (skb_shinfo(skb)->gso_size != 0);
1779
1780         if (is_lso) {
1781                 is_udp = !!(ip_hdr(skb)->protocol == IPPROTO_UDP);
1782
1783                 /* length of headers */
1784                 if (is_udp)
1785                         /* only queue eth + ip headers separately for UDP */
1786                         hdrlen = skb_transport_offset(skb);
1787                 else
1788                         hdrlen = skb_transport_offset(skb) + tcp_hdrlen(skb);
1789                 if (skb_headlen(skb) < hdrlen) {
1790                         netdev_err(bp->dev, "Error - LSO headers fragmented!!!\n");
1791                         /* if this is required, would need to copy to single buffer */
1792                         return NETDEV_TX_BUSY;
1793                 }
1794         } else
1795                 hdrlen = min(skb_headlen(skb), bp->max_tx_length);
1796
1797 #if defined(DEBUG) && defined(VERBOSE_DEBUG)
1798         netdev_vdbg(bp->dev,
1799                     "start_xmit: queue %hu len %u head %p data %p tail %p end %p\n",
1800                     queue_index, skb->len, skb->head, skb->data,
1801                     skb_tail_pointer(skb), skb_end_pointer(skb));
1802         print_hex_dump(KERN_DEBUG, "data: ", DUMP_PREFIX_OFFSET, 16, 1,
1803                        skb->data, 16, true);
1804 #endif
1805
1806         /* Count how many TX buffer descriptors are needed to send this
1807          * socket buffer: skb fragments of jumbo frames may need to be
1808          * split into many buffer descriptors.
1809          */
1810         if (is_lso && (skb_headlen(skb) > hdrlen))
1811                 /* extra header descriptor if also payload in first buffer */
1812                 desc_cnt = DIV_ROUND_UP((skb_headlen(skb) - hdrlen), bp->max_tx_length) + 1;
1813         else
1814                 desc_cnt = DIV_ROUND_UP(skb_headlen(skb), bp->max_tx_length);
1815         nr_frags = skb_shinfo(skb)->nr_frags;
1816         for (f = 0; f < nr_frags; f++) {
1817                 frag_size = skb_frag_size(&skb_shinfo(skb)->frags[f]);
1818                 desc_cnt += DIV_ROUND_UP(frag_size, bp->max_tx_length);
1819         }
1820
1821         spin_lock_irqsave(&bp->lock, flags);
1822
1823         /* This is a hard error, log it. */
1824         if (CIRC_SPACE(queue->tx_head, queue->tx_tail,
1825                        bp->tx_ring_size) < desc_cnt) {
1826                 netif_stop_subqueue(dev, queue_index);
1827                 spin_unlock_irqrestore(&bp->lock, flags);
1828                 netdev_dbg(bp->dev, "tx_head = %u, tx_tail = %u\n",
1829                            queue->tx_head, queue->tx_tail);
1830                 return NETDEV_TX_BUSY;
1831         }
1832
1833         /* Map socket buffer for DMA transfer */
1834         if (!macb_tx_map(bp, queue, skb, hdrlen)) {
1835                 dev_kfree_skb_any(skb);
1836                 goto unlock;
1837         }
1838
1839         /* Make newly initialized descriptor visible to hardware */
1840         wmb();
1841         skb_tx_timestamp(skb);
1842
1843         macb_writel(bp, NCR, macb_readl(bp, NCR) | MACB_BIT(TSTART));
1844
1845         if (CIRC_SPACE(queue->tx_head, queue->tx_tail, bp->tx_ring_size) < 1)
1846                 netif_stop_subqueue(dev, queue_index);
1847
1848 unlock:
1849         spin_unlock_irqrestore(&bp->lock, flags);
1850
1851         return ret;
1852 }
1853
1854 static void macb_init_rx_buffer_size(struct macb *bp, size_t size)
1855 {
1856         if (!macb_is_gem(bp)) {
1857                 bp->rx_buffer_size = MACB_RX_BUFFER_SIZE;
1858         } else {
1859                 bp->rx_buffer_size = size;
1860
1861                 if (bp->rx_buffer_size % RX_BUFFER_MULTIPLE) {
1862                         netdev_dbg(bp->dev,
1863                                    "RX buffer must be multiple of %d bytes, expanding\n",
1864                                    RX_BUFFER_MULTIPLE);
1865                         bp->rx_buffer_size =
1866                                 roundup(bp->rx_buffer_size, RX_BUFFER_MULTIPLE);
1867                 }
1868         }
1869
1870         netdev_dbg(bp->dev, "mtu [%u] rx_buffer_size [%zu]\n",
1871                    bp->dev->mtu, bp->rx_buffer_size);
1872 }
1873
1874 static void gem_free_rx_buffers(struct macb *bp)
1875 {
1876         struct sk_buff          *skb;
1877         struct macb_dma_desc    *desc;
1878         struct macb_queue *queue;
1879         dma_addr_t              addr;
1880         unsigned int q;
1881         int i;
1882
1883         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
1884                 if (!queue->rx_skbuff)
1885                         continue;
1886
1887                 for (i = 0; i < bp->rx_ring_size; i++) {
1888                         skb = queue->rx_skbuff[i];
1889
1890                         if (!skb)
1891                                 continue;
1892
1893                         desc = macb_rx_desc(queue, i);
1894                         addr = macb_get_addr(bp, desc);
1895
1896                         dma_unmap_single(&bp->pdev->dev, addr, bp->rx_buffer_size,
1897                                         DMA_FROM_DEVICE);
1898                         dev_kfree_skb_any(skb);
1899                         skb = NULL;
1900                 }
1901
1902                 kfree(queue->rx_skbuff);
1903                 queue->rx_skbuff = NULL;
1904         }
1905 }
1906
1907 static void macb_free_rx_buffers(struct macb *bp)
1908 {
1909         struct macb_queue *queue = &bp->queues[0];
1910
1911         if (queue->rx_buffers) {
1912                 dma_free_coherent(&bp->pdev->dev,
1913                                   bp->rx_ring_size * bp->rx_buffer_size,
1914                                   queue->rx_buffers, queue->rx_buffers_dma);
1915                 queue->rx_buffers = NULL;
1916         }
1917 }
1918
1919 static void macb_free_consistent(struct macb *bp)
1920 {
1921         struct macb_queue *queue;
1922         unsigned int q;
1923         int size;
1924
1925         bp->macbgem_ops.mog_free_rx_buffers(bp);
1926
1927         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
1928                 kfree(queue->tx_skb);
1929                 queue->tx_skb = NULL;
1930                 if (queue->tx_ring) {
1931                         size = TX_RING_BYTES(bp) + bp->tx_bd_rd_prefetch;
1932                         dma_free_coherent(&bp->pdev->dev, size,
1933                                           queue->tx_ring, queue->tx_ring_dma);
1934                         queue->tx_ring = NULL;
1935                 }
1936                 if (queue->rx_ring) {
1937                         size = RX_RING_BYTES(bp) + bp->rx_bd_rd_prefetch;
1938                         dma_free_coherent(&bp->pdev->dev, size,
1939                                           queue->rx_ring, queue->rx_ring_dma);
1940                         queue->rx_ring = NULL;
1941                 }
1942         }
1943 }
1944
1945 static int gem_alloc_rx_buffers(struct macb *bp)
1946 {
1947         struct macb_queue *queue;
1948         unsigned int q;
1949         int size;
1950
1951         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
1952                 size = bp->rx_ring_size * sizeof(struct sk_buff *);
1953                 queue->rx_skbuff = kzalloc(size, GFP_KERNEL);
1954                 if (!queue->rx_skbuff)
1955                         return -ENOMEM;
1956                 else
1957                         netdev_dbg(bp->dev,
1958                                    "Allocated %d RX struct sk_buff entries at %p\n",
1959                                    bp->rx_ring_size, queue->rx_skbuff);
1960         }
1961         return 0;
1962 }
1963
1964 static int macb_alloc_rx_buffers(struct macb *bp)
1965 {
1966         struct macb_queue *queue = &bp->queues[0];
1967         int size;
1968
1969         size = bp->rx_ring_size * bp->rx_buffer_size;
1970         queue->rx_buffers = dma_alloc_coherent(&bp->pdev->dev, size,
1971                                             &queue->rx_buffers_dma, GFP_KERNEL);
1972         if (!queue->rx_buffers)
1973                 return -ENOMEM;
1974
1975         netdev_dbg(bp->dev,
1976                    "Allocated RX buffers of %d bytes at %08lx (mapped %p)\n",
1977                    size, (unsigned long)queue->rx_buffers_dma, queue->rx_buffers);
1978         return 0;
1979 }
1980
1981 static int macb_alloc_consistent(struct macb *bp)
1982 {
1983         struct macb_queue *queue;
1984         unsigned int q;
1985         int size;
1986
1987         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
1988                 size = TX_RING_BYTES(bp) + bp->tx_bd_rd_prefetch;
1989                 queue->tx_ring = dma_alloc_coherent(&bp->pdev->dev, size,
1990                                                     &queue->tx_ring_dma,
1991                                                     GFP_KERNEL);
1992                 if (!queue->tx_ring)
1993                         goto out_err;
1994                 netdev_dbg(bp->dev,
1995                            "Allocated TX ring for queue %u of %d bytes at %08lx (mapped %p)\n",
1996                            q, size, (unsigned long)queue->tx_ring_dma,
1997                            queue->tx_ring);
1998
1999                 size = bp->tx_ring_size * sizeof(struct macb_tx_skb);
2000                 queue->tx_skb = kmalloc(size, GFP_KERNEL);
2001                 if (!queue->tx_skb)
2002                         goto out_err;
2003
2004                 size = RX_RING_BYTES(bp) + bp->rx_bd_rd_prefetch;
2005                 queue->rx_ring = dma_alloc_coherent(&bp->pdev->dev, size,
2006                                                  &queue->rx_ring_dma, GFP_KERNEL);
2007                 if (!queue->rx_ring)
2008                         goto out_err;
2009                 netdev_dbg(bp->dev,
2010                            "Allocated RX ring of %d bytes at %08lx (mapped %p)\n",
2011                            size, (unsigned long)queue->rx_ring_dma, queue->rx_ring);
2012         }
2013         if (bp->macbgem_ops.mog_alloc_rx_buffers(bp))
2014                 goto out_err;
2015
2016         return 0;
2017
2018 out_err:
2019         macb_free_consistent(bp);
2020         return -ENOMEM;
2021 }
2022
2023 static void gem_init_rings(struct macb *bp)
2024 {
2025         struct macb_queue *queue;
2026         struct macb_dma_desc *desc = NULL;
2027         unsigned int q;
2028         int i;
2029
2030         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
2031                 for (i = 0; i < bp->tx_ring_size; i++) {
2032                         desc = macb_tx_desc(queue, i);
2033                         macb_set_addr(bp, desc, 0);
2034                         desc->ctrl = MACB_BIT(TX_USED);
2035                 }
2036                 desc->ctrl |= MACB_BIT(TX_WRAP);
2037                 queue->tx_head = 0;
2038                 queue->tx_tail = 0;
2039
2040                 queue->rx_tail = 0;
2041                 queue->rx_prepared_head = 0;
2042
2043                 gem_rx_refill(queue);
2044         }
2045
2046 }
2047
2048 static void macb_init_rings(struct macb *bp)
2049 {
2050         int i;
2051         struct macb_dma_desc *desc = NULL;
2052
2053         macb_init_rx_ring(&bp->queues[0]);
2054
2055         for (i = 0; i < bp->tx_ring_size; i++) {
2056                 desc = macb_tx_desc(&bp->queues[0], i);
2057                 macb_set_addr(bp, desc, 0);
2058                 desc->ctrl = MACB_BIT(TX_USED);
2059         }
2060         bp->queues[0].tx_head = 0;
2061         bp->queues[0].tx_tail = 0;
2062         desc->ctrl |= MACB_BIT(TX_WRAP);
2063 }
2064
2065 static void macb_reset_hw(struct macb *bp)
2066 {
2067         struct macb_queue *queue;
2068         unsigned int q;
2069         u32 ctrl = macb_readl(bp, NCR);
2070
2071         /* Disable RX and TX (XXX: Should we halt the transmission
2072          * more gracefully?)
2073          */
2074         ctrl &= ~(MACB_BIT(RE) | MACB_BIT(TE));
2075
2076         /* Clear the stats registers (XXX: Update stats first?) */
2077         ctrl |= MACB_BIT(CLRSTAT);
2078
2079         macb_writel(bp, NCR, ctrl);
2080
2081         /* Clear all status flags */
2082         macb_writel(bp, TSR, -1);
2083         macb_writel(bp, RSR, -1);
2084
2085         /* Disable all interrupts */
2086         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
2087                 queue_writel(queue, IDR, -1);
2088                 queue_readl(queue, ISR);
2089                 if (bp->caps & MACB_CAPS_ISR_CLEAR_ON_WRITE)
2090                         queue_writel(queue, ISR, -1);
2091         }
2092 }
2093
2094 static u32 gem_mdc_clk_div(struct macb *bp)
2095 {
2096         u32 config;
2097         unsigned long pclk_hz = clk_get_rate(bp->pclk);
2098
2099         if (pclk_hz <= 20000000)
2100                 config = GEM_BF(CLK, GEM_CLK_DIV8);
2101         else if (pclk_hz <= 40000000)
2102                 config = GEM_BF(CLK, GEM_CLK_DIV16);
2103         else if (pclk_hz <= 80000000)
2104                 config = GEM_BF(CLK, GEM_CLK_DIV32);
2105         else if (pclk_hz <= 120000000)
2106                 config = GEM_BF(CLK, GEM_CLK_DIV48);
2107         else if (pclk_hz <= 160000000)
2108                 config = GEM_BF(CLK, GEM_CLK_DIV64);
2109         else
2110                 config = GEM_BF(CLK, GEM_CLK_DIV96);
2111
2112         return config;
2113 }
2114
2115 static u32 macb_mdc_clk_div(struct macb *bp)
2116 {
2117         u32 config;
2118         unsigned long pclk_hz;
2119
2120         if (macb_is_gem(bp))
2121                 return gem_mdc_clk_div(bp);
2122
2123         pclk_hz = clk_get_rate(bp->pclk);
2124         if (pclk_hz <= 20000000)
2125                 config = MACB_BF(CLK, MACB_CLK_DIV8);
2126         else if (pclk_hz <= 40000000)
2127                 config = MACB_BF(CLK, MACB_CLK_DIV16);
2128         else if (pclk_hz <= 80000000)
2129                 config = MACB_BF(CLK, MACB_CLK_DIV32);
2130         else
2131                 config = MACB_BF(CLK, MACB_CLK_DIV64);
2132
2133         return config;
2134 }
2135
2136 /* Get the DMA bus width field of the network configuration register that we
2137  * should program.  We find the width from decoding the design configuration
2138  * register to find the maximum supported data bus width.
2139  */
2140 static u32 macb_dbw(struct macb *bp)
2141 {
2142         if (!macb_is_gem(bp))
2143                 return 0;
2144
2145         switch (GEM_BFEXT(DBWDEF, gem_readl(bp, DCFG1))) {
2146         case 4:
2147                 return GEM_BF(DBW, GEM_DBW128);
2148         case 2:
2149                 return GEM_BF(DBW, GEM_DBW64);
2150         case 1:
2151         default:
2152                 return GEM_BF(DBW, GEM_DBW32);
2153         }
2154 }
2155
2156 /* Configure the receive DMA engine
2157  * - use the correct receive buffer size
2158  * - set best burst length for DMA operations
2159  *   (if not supported by FIFO, it will fallback to default)
2160  * - set both rx/tx packet buffers to full memory size
2161  * These are configurable parameters for GEM.
2162  */
2163 static void macb_configure_dma(struct macb *bp)
2164 {
2165         struct macb_queue *queue;
2166         u32 buffer_size;
2167         unsigned int q;
2168         u32 dmacfg;
2169
2170         buffer_size = bp->rx_buffer_size / RX_BUFFER_MULTIPLE;
2171         if (macb_is_gem(bp)) {
2172                 dmacfg = gem_readl(bp, DMACFG) & ~GEM_BF(RXBS, -1L);
2173                 for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
2174                         if (q)
2175                                 queue_writel(queue, RBQS, buffer_size);
2176                         else
2177                                 dmacfg |= GEM_BF(RXBS, buffer_size);
2178                 }
2179                 if (bp->dma_burst_length)
2180                         dmacfg = GEM_BFINS(FBLDO, bp->dma_burst_length, dmacfg);
2181                 dmacfg |= GEM_BIT(TXPBMS) | GEM_BF(RXBMS, -1L);
2182                 dmacfg &= ~GEM_BIT(ENDIA_PKT);
2183
2184                 if (bp->native_io)
2185                         dmacfg &= ~GEM_BIT(ENDIA_DESC);
2186                 else
2187                         dmacfg |= GEM_BIT(ENDIA_DESC); /* CPU in big endian */
2188
2189                 if (bp->dev->features & NETIF_F_HW_CSUM)
2190                         dmacfg |= GEM_BIT(TXCOEN);
2191                 else
2192                         dmacfg &= ~GEM_BIT(TXCOEN);
2193
2194                 dmacfg &= ~GEM_BIT(ADDR64);
2195 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
2196                 if (bp->hw_dma_cap & HW_DMA_CAP_64B)
2197                         dmacfg |= GEM_BIT(ADDR64);
2198 #endif
2199 #ifdef CONFIG_MACB_USE_HWSTAMP
2200                 if (bp->hw_dma_cap & HW_DMA_CAP_PTP)
2201                         dmacfg |= GEM_BIT(RXEXT) | GEM_BIT(TXEXT);
2202 #endif
2203                 netdev_dbg(bp->dev, "Cadence configure DMA with 0x%08x\n",
2204                            dmacfg);
2205                 gem_writel(bp, DMACFG, dmacfg);
2206         }
2207 }
2208
2209 static void macb_init_hw(struct macb *bp)
2210 {
2211         struct macb_queue *queue;
2212         unsigned int q;
2213
2214         u32 config;
2215
2216         macb_reset_hw(bp);
2217         macb_set_hwaddr(bp);
2218
2219         config = macb_mdc_clk_div(bp);
2220         if (bp->phy_interface == PHY_INTERFACE_MODE_SGMII)
2221                 config |= GEM_BIT(SGMIIEN) | GEM_BIT(PCSSEL);
2222         config |= MACB_BF(RBOF, NET_IP_ALIGN);  /* Make eth data aligned */
2223         config |= MACB_BIT(PAE);                /* PAuse Enable */
2224         config |= MACB_BIT(DRFCS);              /* Discard Rx FCS */
2225         if (bp->caps & MACB_CAPS_JUMBO)
2226                 config |= MACB_BIT(JFRAME);     /* Enable jumbo frames */
2227         else
2228                 config |= MACB_BIT(BIG);        /* Receive oversized frames */
2229         if (bp->dev->flags & IFF_PROMISC)
2230                 config |= MACB_BIT(CAF);        /* Copy All Frames */
2231         else if (macb_is_gem(bp) && bp->dev->features & NETIF_F_RXCSUM)
2232                 config |= GEM_BIT(RXCOEN);
2233         if (!(bp->dev->flags & IFF_BROADCAST))
2234                 config |= MACB_BIT(NBC);        /* No BroadCast */
2235         config |= macb_dbw(bp);
2236         macb_writel(bp, NCFGR, config);
2237         if ((bp->caps & MACB_CAPS_JUMBO) && bp->jumbo_max_len)
2238                 gem_writel(bp, JML, bp->jumbo_max_len);
2239         bp->speed = SPEED_10;
2240         bp->duplex = DUPLEX_HALF;
2241         bp->rx_frm_len_mask = MACB_RX_FRMLEN_MASK;
2242         if (bp->caps & MACB_CAPS_JUMBO)
2243                 bp->rx_frm_len_mask = MACB_RX_JFRMLEN_MASK;
2244
2245         macb_configure_dma(bp);
2246
2247         /* Initialize TX and RX buffers */
2248         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
2249                 queue_writel(queue, RBQP, lower_32_bits(queue->rx_ring_dma));
2250 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
2251                 if (bp->hw_dma_cap & HW_DMA_CAP_64B)
2252                         queue_writel(queue, RBQPH, upper_32_bits(queue->rx_ring_dma));
2253 #endif
2254                 queue_writel(queue, TBQP, lower_32_bits(queue->tx_ring_dma));
2255 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
2256                 if (bp->hw_dma_cap & HW_DMA_CAP_64B)
2257                         queue_writel(queue, TBQPH, upper_32_bits(queue->tx_ring_dma));
2258 #endif
2259
2260                 /* Enable interrupts */
2261                 queue_writel(queue, IER,
2262                              bp->rx_intr_mask |
2263                              MACB_TX_INT_FLAGS |
2264                              MACB_BIT(HRESP));
2265         }
2266
2267         /* Enable TX and RX */
2268         macb_writel(bp, NCR, macb_readl(bp, NCR) | MACB_BIT(RE) | MACB_BIT(TE));
2269 }
2270
2271 /* The hash address register is 64 bits long and takes up two
2272  * locations in the memory map.  The least significant bits are stored
2273  * in EMAC_HSL and the most significant bits in EMAC_HSH.
2274  *
2275  * The unicast hash enable and the multicast hash enable bits in the
2276  * network configuration register enable the reception of hash matched
2277  * frames. The destination address is reduced to a 6 bit index into
2278  * the 64 bit hash register using the following hash function.  The
2279  * hash function is an exclusive or of every sixth bit of the
2280  * destination address.
2281  *
2282  * hi[5] = da[5] ^ da[11] ^ da[17] ^ da[23] ^ da[29] ^ da[35] ^ da[41] ^ da[47]
2283  * hi[4] = da[4] ^ da[10] ^ da[16] ^ da[22] ^ da[28] ^ da[34] ^ da[40] ^ da[46]
2284  * hi[3] = da[3] ^ da[09] ^ da[15] ^ da[21] ^ da[27] ^ da[33] ^ da[39] ^ da[45]
2285  * hi[2] = da[2] ^ da[08] ^ da[14] ^ da[20] ^ da[26] ^ da[32] ^ da[38] ^ da[44]
2286  * hi[1] = da[1] ^ da[07] ^ da[13] ^ da[19] ^ da[25] ^ da[31] ^ da[37] ^ da[43]
2287  * hi[0] = da[0] ^ da[06] ^ da[12] ^ da[18] ^ da[24] ^ da[30] ^ da[36] ^ da[42]
2288  *
2289  * da[0] represents the least significant bit of the first byte
2290  * received, that is, the multicast/unicast indicator, and da[47]
2291  * represents the most significant bit of the last byte received.  If
2292  * the hash index, hi[n], points to a bit that is set in the hash
2293  * register then the frame will be matched according to whether the
2294  * frame is multicast or unicast.  A multicast match will be signalled
2295  * if the multicast hash enable bit is set, da[0] is 1 and the hash
2296  * index points to a bit set in the hash register.  A unicast match
2297  * will be signalled if the unicast hash enable bit is set, da[0] is 0
2298  * and the hash index points to a bit set in the hash register.  To
2299  * receive all multicast frames, the hash register should be set with
2300  * all ones and the multicast hash enable bit should be set in the
2301  * network configuration register.
2302  */
2303
2304 static inline int hash_bit_value(int bitnr, __u8 *addr)
2305 {
2306         if (addr[bitnr / 8] & (1 << (bitnr % 8)))
2307                 return 1;
2308         return 0;
2309 }
2310
2311 /* Return the hash index value for the specified address. */
2312 static int hash_get_index(__u8 *addr)
2313 {
2314         int i, j, bitval;
2315         int hash_index = 0;
2316
2317         for (j = 0; j < 6; j++) {
2318                 for (i = 0, bitval = 0; i < 8; i++)
2319                         bitval ^= hash_bit_value(i * 6 + j, addr);
2320
2321                 hash_index |= (bitval << j);
2322         }
2323
2324         return hash_index;
2325 }
2326
2327 /* Add multicast addresses to the internal multicast-hash table. */
2328 static void macb_sethashtable(struct net_device *dev)
2329 {
2330         struct netdev_hw_addr *ha;
2331         unsigned long mc_filter[2];
2332         unsigned int bitnr;
2333         struct macb *bp = netdev_priv(dev);
2334
2335         mc_filter[0] = 0;
2336         mc_filter[1] = 0;
2337
2338         netdev_for_each_mc_addr(ha, dev) {
2339                 bitnr = hash_get_index(ha->addr);
2340                 mc_filter[bitnr >> 5] |= 1 << (bitnr & 31);
2341         }
2342
2343         macb_or_gem_writel(bp, HRB, mc_filter[0]);
2344         macb_or_gem_writel(bp, HRT, mc_filter[1]);
2345 }
2346
2347 /* Enable/Disable promiscuous and multicast modes. */
2348 static void macb_set_rx_mode(struct net_device *dev)
2349 {
2350         unsigned long cfg;
2351         struct macb *bp = netdev_priv(dev);
2352
2353         cfg = macb_readl(bp, NCFGR);
2354
2355         if (dev->flags & IFF_PROMISC) {
2356                 /* Enable promiscuous mode */
2357                 cfg |= MACB_BIT(CAF);
2358
2359                 /* Disable RX checksum offload */
2360                 if (macb_is_gem(bp))
2361                         cfg &= ~GEM_BIT(RXCOEN);
2362         } else {
2363                 /* Disable promiscuous mode */
2364                 cfg &= ~MACB_BIT(CAF);
2365
2366                 /* Enable RX checksum offload only if requested */
2367                 if (macb_is_gem(bp) && dev->features & NETIF_F_RXCSUM)
2368                         cfg |= GEM_BIT(RXCOEN);
2369         }
2370
2371         if (dev->flags & IFF_ALLMULTI) {
2372                 /* Enable all multicast mode */
2373                 macb_or_gem_writel(bp, HRB, -1);
2374                 macb_or_gem_writel(bp, HRT, -1);
2375                 cfg |= MACB_BIT(NCFGR_MTI);
2376         } else if (!netdev_mc_empty(dev)) {
2377                 /* Enable specific multicasts */
2378                 macb_sethashtable(dev);
2379                 cfg |= MACB_BIT(NCFGR_MTI);
2380         } else if (dev->flags & (~IFF_ALLMULTI)) {
2381                 /* Disable all multicast mode */
2382                 macb_or_gem_writel(bp, HRB, 0);
2383                 macb_or_gem_writel(bp, HRT, 0);
2384                 cfg &= ~MACB_BIT(NCFGR_MTI);
2385         }
2386
2387         macb_writel(bp, NCFGR, cfg);
2388 }
2389
2390 static int macb_open(struct net_device *dev)
2391 {
2392         struct macb *bp = netdev_priv(dev);
2393         size_t bufsz = dev->mtu + ETH_HLEN + ETH_FCS_LEN + NET_IP_ALIGN;
2394         struct macb_queue *queue;
2395         unsigned int q;
2396         int err;
2397
2398         netdev_dbg(bp->dev, "open\n");
2399
2400         /* carrier starts down */
2401         netif_carrier_off(dev);
2402
2403         /* if the phy is not yet register, retry later*/
2404         if (!dev->phydev)
2405                 return -EAGAIN;
2406
2407         /* RX buffers initialization */
2408         macb_init_rx_buffer_size(bp, bufsz);
2409
2410         err = macb_alloc_consistent(bp);
2411         if (err) {
2412                 netdev_err(dev, "Unable to allocate DMA memory (error %d)\n",
2413                            err);
2414                 return err;
2415         }
2416
2417         bp->macbgem_ops.mog_init_rings(bp);
2418         macb_init_hw(bp);
2419
2420         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue)
2421                 napi_enable(&queue->napi);
2422
2423         /* schedule a link state check */
2424         phy_start(dev->phydev);
2425
2426         netif_tx_start_all_queues(dev);
2427
2428         if (bp->ptp_info)
2429                 bp->ptp_info->ptp_init(dev);
2430
2431         return 0;
2432 }
2433
2434 static int macb_close(struct net_device *dev)
2435 {
2436         struct macb *bp = netdev_priv(dev);
2437         struct macb_queue *queue;
2438         unsigned long flags;
2439         unsigned int q;
2440
2441         netif_tx_stop_all_queues(dev);
2442
2443         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue)
2444                 napi_disable(&queue->napi);
2445
2446         if (dev->phydev)
2447                 phy_stop(dev->phydev);
2448
2449         spin_lock_irqsave(&bp->lock, flags);
2450         macb_reset_hw(bp);
2451         netif_carrier_off(dev);
2452         spin_unlock_irqrestore(&bp->lock, flags);
2453
2454         macb_free_consistent(bp);
2455
2456         if (bp->ptp_info)
2457                 bp->ptp_info->ptp_remove(dev);
2458
2459         return 0;
2460 }
2461
2462 static int macb_change_mtu(struct net_device *dev, int new_mtu)
2463 {
2464         if (netif_running(dev))
2465                 return -EBUSY;
2466
2467         dev->mtu = new_mtu;
2468
2469         return 0;
2470 }
2471
2472 static void gem_update_stats(struct macb *bp)
2473 {
2474         struct macb_queue *queue;
2475         unsigned int i, q, idx;
2476         unsigned long *stat;
2477
2478         u32 *p = &bp->hw_stats.gem.tx_octets_31_0;
2479
2480         for (i = 0; i < GEM_STATS_LEN; ++i, ++p) {
2481                 u32 offset = gem_statistics[i].offset;
2482                 u64 val = bp->macb_reg_readl(bp, offset);
2483
2484                 bp->ethtool_stats[i] += val;
2485                 *p += val;
2486
2487                 if (offset == GEM_OCTTXL || offset == GEM_OCTRXL) {
2488                         /* Add GEM_OCTTXH, GEM_OCTRXH */
2489                         val = bp->macb_reg_readl(bp, offset + 4);
2490                         bp->ethtool_stats[i] += ((u64)val) << 32;
2491                         *(++p) += val;
2492                 }
2493         }
2494
2495         idx = GEM_STATS_LEN;
2496         for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue)
2497                 for (i = 0, stat = &queue->stats.first; i < QUEUE_STATS_LEN; ++i, ++stat)
2498                         bp->ethtool_stats[idx++] = *stat;
2499 }
2500
2501 static struct net_device_stats *gem_get_stats(struct macb *bp)
2502 {
2503         struct gem_stats *hwstat = &bp->hw_stats.gem;
2504         struct net_device_stats *nstat = &bp->dev->stats;
2505
2506         gem_update_stats(bp);
2507
2508         nstat->rx_errors = (hwstat->rx_frame_check_sequence_errors +
2509                             hwstat->rx_alignment_errors +
2510                             hwstat->rx_resource_errors +
2511                             hwstat->rx_overruns +
2512                             hwstat->rx_oversize_frames +
2513                             hwstat->rx_jabbers +
2514                             hwstat->rx_undersized_frames +
2515                             hwstat->rx_length_field_frame_errors);
2516         nstat->tx_errors = (hwstat->tx_late_collisions +
2517                             hwstat->tx_excessive_collisions +
2518                             hwstat->tx_underrun +
2519                             hwstat->tx_carrier_sense_errors);
2520         nstat->multicast = hwstat->rx_multicast_frames;
2521         nstat->collisions = (hwstat->tx_single_collision_frames +
2522                              hwstat->tx_multiple_collision_frames +
2523                              hwstat->tx_excessive_collisions);
2524         nstat->rx_length_errors = (hwstat->rx_oversize_frames +
2525                                    hwstat->rx_jabbers +
2526                                    hwstat->rx_undersized_frames +
2527                                    hwstat->rx_length_field_frame_errors);
2528         nstat->rx_over_errors = hwstat->rx_resource_errors;
2529         nstat->rx_crc_errors = hwstat->rx_frame_check_sequence_errors;
2530         nstat->rx_frame_errors = hwstat->rx_alignment_errors;
2531         nstat->rx_fifo_errors = hwstat->rx_overruns;
2532         nstat->tx_aborted_errors = hwstat->tx_excessive_collisions;
2533         nstat->tx_carrier_errors = hwstat->tx_carrier_sense_errors;
2534         nstat->tx_fifo_errors = hwstat->tx_underrun;
2535
2536         return nstat;
2537 }
2538
2539 static void gem_get_ethtool_stats(struct net_device *dev,
2540                                   struct ethtool_stats *stats, u64 *data)
2541 {
2542         struct macb *bp;
2543
2544         bp = netdev_priv(dev);
2545         gem_update_stats(bp);
2546         memcpy(data, &bp->ethtool_stats, sizeof(u64)
2547                         * (GEM_STATS_LEN + QUEUE_STATS_LEN * MACB_MAX_QUEUES));
2548 }
2549
2550 static int gem_get_sset_count(struct net_device *dev, int sset)
2551 {
2552         struct macb *bp = netdev_priv(dev);
2553
2554         switch (sset) {
2555         case ETH_SS_STATS:
2556                 return GEM_STATS_LEN + bp->num_queues * QUEUE_STATS_LEN;
2557         default:
2558                 return -EOPNOTSUPP;
2559         }
2560 }
2561
2562 static void gem_get_ethtool_strings(struct net_device *dev, u32 sset, u8 *p)
2563 {
2564         char stat_string[ETH_GSTRING_LEN];
2565         struct macb *bp = netdev_priv(dev);
2566         struct macb_queue *queue;
2567         unsigned int i;
2568         unsigned int q;
2569
2570         switch (sset) {
2571         case ETH_SS_STATS:
2572                 for (i = 0; i < GEM_STATS_LEN; i++, p += ETH_GSTRING_LEN)
2573                         memcpy(p, gem_statistics[i].stat_string,
2574                                ETH_GSTRING_LEN);
2575
2576                 for (q = 0, queue = bp->queues; q < bp->num_queues; ++q, ++queue) {
2577                         for (i = 0; i < QUEUE_STATS_LEN; i++, p += ETH_GSTRING_LEN) {
2578                                 snprintf(stat_string, ETH_GSTRING_LEN, "q%d_%s",
2579                                                 q, queue_statistics[i].stat_string);
2580                                 memcpy(p, stat_string, ETH_GSTRING_LEN);
2581                         }
2582                 }
2583                 break;
2584         }
2585 }
2586
2587 static struct net_device_stats *macb_get_stats(struct net_device *dev)
2588 {
2589         struct macb *bp = netdev_priv(dev);
2590         struct net_device_stats *nstat = &bp->dev->stats;
2591         struct macb_stats *hwstat = &bp->hw_stats.macb;
2592
2593         if (macb_is_gem(bp))
2594                 return gem_get_stats(bp);
2595
2596         /* read stats from hardware */
2597         macb_update_stats(bp);
2598
2599         /* Convert HW stats into netdevice stats */
2600         nstat->rx_errors = (hwstat->rx_fcs_errors +
2601                             hwstat->rx_align_errors +
2602                             hwstat->rx_resource_errors +
2603                             hwstat->rx_overruns +
2604                             hwstat->rx_oversize_pkts +
2605                             hwstat->rx_jabbers +
2606                             hwstat->rx_undersize_pkts +
2607                             hwstat->rx_length_mismatch);
2608         nstat->tx_errors = (hwstat->tx_late_cols +
2609                             hwstat->tx_excessive_cols +
2610                             hwstat->tx_underruns +
2611                             hwstat->tx_carrier_errors +
2612                             hwstat->sqe_test_errors);
2613         nstat->collisions = (hwstat->tx_single_cols +
2614                              hwstat->tx_multiple_cols +
2615                              hwstat->tx_excessive_cols);
2616         nstat->rx_length_errors = (hwstat->rx_oversize_pkts +
2617                                    hwstat->rx_jabbers +
2618                                    hwstat->rx_undersize_pkts +
2619                                    hwstat->rx_length_mismatch);
2620         nstat->rx_over_errors = hwstat->rx_resource_errors +
2621                                    hwstat->rx_overruns;
2622         nstat->rx_crc_errors = hwstat->rx_fcs_errors;
2623         nstat->rx_frame_errors = hwstat->rx_align_errors;
2624         nstat->rx_fifo_errors = hwstat->rx_overruns;
2625         /* XXX: What does "missed" mean? */
2626         nstat->tx_aborted_errors = hwstat->tx_excessive_cols;
2627         nstat->tx_carrier_errors = hwstat->tx_carrier_errors;
2628         nstat->tx_fifo_errors = hwstat->tx_underruns;
2629         /* Don't know about heartbeat or window errors... */
2630
2631         return nstat;
2632 }
2633
2634 static int macb_get_regs_len(struct net_device *netdev)
2635 {
2636         return MACB_GREGS_NBR * sizeof(u32);
2637 }
2638
2639 static void macb_get_regs(struct net_device *dev, struct ethtool_regs *regs,
2640                           void *p)
2641 {
2642         struct macb *bp = netdev_priv(dev);
2643         unsigned int tail, head;
2644         u32 *regs_buff = p;
2645
2646         regs->version = (macb_readl(bp, MID) & ((1 << MACB_REV_SIZE) - 1))
2647                         | MACB_GREGS_VERSION;
2648
2649         tail = macb_tx_ring_wrap(bp, bp->queues[0].tx_tail);
2650         head = macb_tx_ring_wrap(bp, bp->queues[0].tx_head);
2651
2652         regs_buff[0]  = macb_readl(bp, NCR);
2653         regs_buff[1]  = macb_or_gem_readl(bp, NCFGR);
2654         regs_buff[2]  = macb_readl(bp, NSR);
2655         regs_buff[3]  = macb_readl(bp, TSR);
2656         regs_buff[4]  = macb_readl(bp, RBQP);
2657         regs_buff[5]  = macb_readl(bp, TBQP);
2658         regs_buff[6]  = macb_readl(bp, RSR);
2659         regs_buff[7]  = macb_readl(bp, IMR);
2660
2661         regs_buff[8]  = tail;
2662         regs_buff[9]  = head;
2663         regs_buff[10] = macb_tx_dma(&bp->queues[0], tail);
2664         regs_buff[11] = macb_tx_dma(&bp->queues[0], head);
2665
2666         if (!(bp->caps & MACB_CAPS_USRIO_DISABLED))
2667                 regs_buff[12] = macb_or_gem_readl(bp, USRIO);
2668         if (macb_is_gem(bp))
2669                 regs_buff[13] = gem_readl(bp, DMACFG);
2670 }
2671
2672 static void macb_get_wol(struct net_device *netdev, struct ethtool_wolinfo *wol)
2673 {
2674         struct macb *bp = netdev_priv(netdev);
2675
2676         wol->supported = 0;
2677         wol->wolopts = 0;
2678
2679         if (bp->wol & MACB_WOL_HAS_MAGIC_PACKET) {
2680                 wol->supported = WAKE_MAGIC;
2681
2682                 if (bp->wol & MACB_WOL_ENABLED)
2683                         wol->wolopts |= WAKE_MAGIC;
2684         }
2685 }
2686
2687 static int macb_set_wol(struct net_device *netdev, struct ethtool_wolinfo *wol)
2688 {
2689         struct macb *bp = netdev_priv(netdev);
2690
2691         if (!(bp->wol & MACB_WOL_HAS_MAGIC_PACKET) ||
2692             (wol->wolopts & ~WAKE_MAGIC))
2693                 return -EOPNOTSUPP;
2694
2695         if (wol->wolopts & WAKE_MAGIC)
2696                 bp->wol |= MACB_WOL_ENABLED;
2697         else
2698                 bp->wol &= ~MACB_WOL_ENABLED;
2699
2700         device_set_wakeup_enable(&bp->pdev->dev, bp->wol & MACB_WOL_ENABLED);
2701
2702         return 0;
2703 }
2704
2705 static void macb_get_ringparam(struct net_device *netdev,
2706                                struct ethtool_ringparam *ring)
2707 {
2708         struct macb *bp = netdev_priv(netdev);
2709
2710         ring->rx_max_pending = MAX_RX_RING_SIZE;
2711         ring->tx_max_pending = MAX_TX_RING_SIZE;
2712
2713         ring->rx_pending = bp->rx_ring_size;
2714         ring->tx_pending = bp->tx_ring_size;
2715 }
2716
2717 static int macb_set_ringparam(struct net_device *netdev,
2718                               struct ethtool_ringparam *ring)
2719 {
2720         struct macb *bp = netdev_priv(netdev);
2721         u32 new_rx_size, new_tx_size;
2722         unsigned int reset = 0;
2723
2724         if ((ring->rx_mini_pending) || (ring->rx_jumbo_pending))
2725                 return -EINVAL;
2726
2727         new_rx_size = clamp_t(u32, ring->rx_pending,
2728                               MIN_RX_RING_SIZE, MAX_RX_RING_SIZE);
2729         new_rx_size = roundup_pow_of_two(new_rx_size);
2730
2731         new_tx_size = clamp_t(u32, ring->tx_pending,
2732                               MIN_TX_RING_SIZE, MAX_TX_RING_SIZE);
2733         new_tx_size = roundup_pow_of_two(new_tx_size);
2734
2735         if ((new_tx_size == bp->tx_ring_size) &&
2736             (new_rx_size == bp->rx_ring_size)) {
2737                 /* nothing to do */
2738                 return 0;
2739         }
2740
2741         if (netif_running(bp->dev)) {
2742                 reset = 1;
2743                 macb_close(bp->dev);
2744         }
2745
2746         bp->rx_ring_size = new_rx_size;
2747         bp->tx_ring_size = new_tx_size;
2748
2749         if (reset)
2750                 macb_open(bp->dev);
2751
2752         return 0;
2753 }
2754
2755 #ifdef CONFIG_MACB_USE_HWSTAMP
2756 static unsigned int gem_get_tsu_rate(struct macb *bp)
2757 {
2758         struct clk *tsu_clk;
2759         unsigned int tsu_rate;
2760
2761         tsu_clk = devm_clk_get(&bp->pdev->dev, "tsu_clk");
2762         if (!IS_ERR(tsu_clk))
2763                 tsu_rate = clk_get_rate(tsu_clk);
2764         /* try pclk instead */
2765         else if (!IS_ERR(bp->pclk)) {
2766                 tsu_clk = bp->pclk;
2767                 tsu_rate = clk_get_rate(tsu_clk);
2768         } else
2769                 return -ENOTSUPP;
2770         return tsu_rate;
2771 }
2772
2773 static s32 gem_get_ptp_max_adj(void)
2774 {
2775         return 64000000;
2776 }
2777
2778 static int gem_get_ts_info(struct net_device *dev,
2779                            struct ethtool_ts_info *info)
2780 {
2781         struct macb *bp = netdev_priv(dev);
2782
2783         if ((bp->hw_dma_cap & HW_DMA_CAP_PTP) == 0) {
2784                 ethtool_op_get_ts_info(dev, info);
2785                 return 0;
2786         }
2787
2788         info->so_timestamping =
2789                 SOF_TIMESTAMPING_TX_SOFTWARE |
2790                 SOF_TIMESTAMPING_RX_SOFTWARE |
2791                 SOF_TIMESTAMPING_SOFTWARE |
2792                 SOF_TIMESTAMPING_TX_HARDWARE |
2793                 SOF_TIMESTAMPING_RX_HARDWARE |
2794                 SOF_TIMESTAMPING_RAW_HARDWARE;
2795         info->tx_types =
2796                 (1 << HWTSTAMP_TX_ONESTEP_SYNC) |
2797                 (1 << HWTSTAMP_TX_OFF) |
2798                 (1 << HWTSTAMP_TX_ON);
2799         info->rx_filters =
2800                 (1 << HWTSTAMP_FILTER_NONE) |
2801                 (1 << HWTSTAMP_FILTER_ALL);
2802
2803         info->phc_index = bp->ptp_clock ? ptp_clock_index(bp->ptp_clock) : -1;
2804
2805         return 0;
2806 }
2807
2808 static struct macb_ptp_info gem_ptp_info = {
2809         .ptp_init        = gem_ptp_init,
2810         .ptp_remove      = gem_ptp_remove,
2811         .get_ptp_max_adj = gem_get_ptp_max_adj,
2812         .get_tsu_rate    = gem_get_tsu_rate,
2813         .get_ts_info     = gem_get_ts_info,
2814         .get_hwtst       = gem_get_hwtst,
2815         .set_hwtst       = gem_set_hwtst,
2816 };
2817 #endif
2818
2819 static int macb_get_ts_info(struct net_device *netdev,
2820                             struct ethtool_ts_info *info)
2821 {
2822         struct macb *bp = netdev_priv(netdev);
2823
2824         if (bp->ptp_info)
2825                 return bp->ptp_info->get_ts_info(netdev, info);
2826
2827         return ethtool_op_get_ts_info(netdev, info);
2828 }
2829
2830 static void gem_enable_flow_filters(struct macb *bp, bool enable)
2831 {
2832         struct ethtool_rx_fs_item *item;
2833         u32 t2_scr;
2834         int num_t2_scr;
2835
2836         num_t2_scr = GEM_BFEXT(T2SCR, gem_readl(bp, DCFG8));
2837
2838         list_for_each_entry(item, &bp->rx_fs_list.list, list) {
2839                 struct ethtool_rx_flow_spec *fs = &item->fs;
2840                 struct ethtool_tcpip4_spec *tp4sp_m;
2841
2842                 if (fs->location >= num_t2_scr)
2843                         continue;
2844
2845                 t2_scr = gem_readl_n(bp, SCRT2, fs->location);
2846
2847                 /* enable/disable screener regs for the flow entry */
2848                 t2_scr = GEM_BFINS(ETHTEN, enable, t2_scr);
2849
2850                 /* only enable fields with no masking */
2851                 tp4sp_m = &(fs->m_u.tcp_ip4_spec);
2852
2853                 if (enable && (tp4sp_m->ip4src == 0xFFFFFFFF))
2854                         t2_scr = GEM_BFINS(CMPAEN, 1, t2_scr);
2855                 else
2856                         t2_scr = GEM_BFINS(CMPAEN, 0, t2_scr);
2857
2858                 if (enable && (tp4sp_m->ip4dst == 0xFFFFFFFF))
2859                         t2_scr = GEM_BFINS(CMPBEN, 1, t2_scr);
2860                 else
2861                         t2_scr = GEM_BFINS(CMPBEN, 0, t2_scr);
2862
2863                 if (enable && ((tp4sp_m->psrc == 0xFFFF) || (tp4sp_m->pdst == 0xFFFF)))
2864                         t2_scr = GEM_BFINS(CMPCEN, 1, t2_scr);
2865                 else
2866                         t2_scr = GEM_BFINS(CMPCEN, 0, t2_scr);
2867
2868                 gem_writel_n(bp, SCRT2, fs->location, t2_scr);
2869         }
2870 }
2871
2872 static void gem_prog_cmp_regs(struct macb *bp, struct ethtool_rx_flow_spec *fs)
2873 {
2874         struct ethtool_tcpip4_spec *tp4sp_v, *tp4sp_m;
2875         uint16_t index = fs->location;
2876         u32 w0, w1, t2_scr;
2877         bool cmp_a = false;
2878         bool cmp_b = false;
2879         bool cmp_c = false;
2880
2881         tp4sp_v = &(fs->h_u.tcp_ip4_spec);
2882         tp4sp_m = &(fs->m_u.tcp_ip4_spec);
2883
2884         /* ignore field if any masking set */
2885         if (tp4sp_m->ip4src == 0xFFFFFFFF) {
2886                 /* 1st compare reg - IP source address */
2887                 w0 = 0;
2888                 w1 = 0;
2889                 w0 = tp4sp_v->ip4src;
2890                 w1 = GEM_BFINS(T2DISMSK, 1, w1); /* 32-bit compare */
2891                 w1 = GEM_BFINS(T2CMPOFST, GEM_T2COMPOFST_ETYPE, w1);
2892                 w1 = GEM_BFINS(T2OFST, ETYPE_SRCIP_OFFSET, w1);
2893                 gem_writel_n(bp, T2CMPW0, T2CMP_OFST(GEM_IP4SRC_CMP(index)), w0);
2894                 gem_writel_n(bp, T2CMPW1, T2CMP_OFST(GEM_IP4SRC_CMP(index)), w1);
2895                 cmp_a = true;
2896         }
2897
2898         /* ignore field if any masking set */
2899         if (tp4sp_m->ip4dst == 0xFFFFFFFF) {
2900                 /* 2nd compare reg - IP destination address */
2901                 w0 = 0;
2902                 w1 = 0;
2903                 w0 = tp4sp_v->ip4dst;
2904                 w1 = GEM_BFINS(T2DISMSK, 1, w1); /* 32-bit compare */
2905                 w1 = GEM_BFINS(T2CMPOFST, GEM_T2COMPOFST_ETYPE, w1);
2906                 w1 = GEM_BFINS(T2OFST, ETYPE_DSTIP_OFFSET, w1);
2907                 gem_writel_n(bp, T2CMPW0, T2CMP_OFST(GEM_IP4DST_CMP(index)), w0);
2908                 gem_writel_n(bp, T2CMPW1, T2CMP_OFST(GEM_IP4DST_CMP(index)), w1);
2909                 cmp_b = true;
2910         }
2911
2912         /* ignore both port fields if masking set in both */
2913         if ((tp4sp_m->psrc == 0xFFFF) || (tp4sp_m->pdst == 0xFFFF)) {
2914                 /* 3rd compare reg - source port, destination port */
2915                 w0 = 0;
2916                 w1 = 0;
2917                 w1 = GEM_BFINS(T2CMPOFST, GEM_T2COMPOFST_IPHDR, w1);
2918                 if (tp4sp_m->psrc == tp4sp_m->pdst) {
2919                         w0 = GEM_BFINS(T2MASK, tp4sp_v->psrc, w0);
2920                         w0 = GEM_BFINS(T2CMP, tp4sp_v->pdst, w0);
2921                         w1 = GEM_BFINS(T2DISMSK, 1, w1); /* 32-bit compare */
2922                         w1 = GEM_BFINS(T2OFST, IPHDR_SRCPORT_OFFSET, w1);
2923                 } else {
2924                         /* only one port definition */
2925                         w1 = GEM_BFINS(T2DISMSK, 0, w1); /* 16-bit compare */
2926                         w0 = GEM_BFINS(T2MASK, 0xFFFF, w0);
2927                         if (tp4sp_m->psrc == 0xFFFF) { /* src port */
2928                                 w0 = GEM_BFINS(T2CMP, tp4sp_v->psrc, w0);
2929                                 w1 = GEM_BFINS(T2OFST, IPHDR_SRCPORT_OFFSET, w1);
2930                         } else { /* dst port */
2931                                 w0 = GEM_BFINS(T2CMP, tp4sp_v->pdst, w0);
2932                                 w1 = GEM_BFINS(T2OFST, IPHDR_DSTPORT_OFFSET, w1);
2933                         }
2934                 }
2935                 gem_writel_n(bp, T2CMPW0, T2CMP_OFST(GEM_PORT_CMP(index)), w0);
2936                 gem_writel_n(bp, T2CMPW1, T2CMP_OFST(GEM_PORT_CMP(index)), w1);
2937                 cmp_c = true;
2938         }
2939
2940         t2_scr = 0;
2941         t2_scr = GEM_BFINS(QUEUE, (fs->ring_cookie) & 0xFF, t2_scr);
2942         t2_scr = GEM_BFINS(ETHT2IDX, SCRT2_ETHT, t2_scr);
2943         if (cmp_a)
2944                 t2_scr = GEM_BFINS(CMPA, GEM_IP4SRC_CMP(index), t2_scr);
2945         if (cmp_b)
2946                 t2_scr = GEM_BFINS(CMPB, GEM_IP4DST_CMP(index), t2_scr);
2947         if (cmp_c)
2948                 t2_scr = GEM_BFINS(CMPC, GEM_PORT_CMP(index), t2_scr);
2949         gem_writel_n(bp, SCRT2, index, t2_scr);
2950 }
2951
2952 static int gem_add_flow_filter(struct net_device *netdev,
2953                 struct ethtool_rxnfc *cmd)
2954 {
2955         struct macb *bp = netdev_priv(netdev);
2956         struct ethtool_rx_flow_spec *fs = &cmd->fs;
2957         struct ethtool_rx_fs_item *item, *newfs;
2958         unsigned long flags;
2959         int ret = -EINVAL;
2960         bool added = false;
2961
2962         newfs = kmalloc(sizeof(*newfs), GFP_KERNEL);
2963         if (newfs == NULL)
2964                 return -ENOMEM;
2965         memcpy(&newfs->fs, fs, sizeof(newfs->fs));
2966
2967         netdev_dbg(netdev,
2968                         "Adding flow filter entry,type=%u,queue=%u,loc=%u,src=%08X,dst=%08X,ps=%u,pd=%u\n",
2969                         fs->flow_type, (int)fs->ring_cookie, fs->location,
2970                         htonl(fs->h_u.tcp_ip4_spec.ip4src),
2971                         htonl(fs->h_u.tcp_ip4_spec.ip4dst),
2972                         htons(fs->h_u.tcp_ip4_spec.psrc), htons(fs->h_u.tcp_ip4_spec.pdst));
2973
2974         spin_lock_irqsave(&bp->rx_fs_lock, flags);
2975
2976         /* find correct place to add in list */
2977         list_for_each_entry(item, &bp->rx_fs_list.list, list) {
2978                 if (item->fs.location > newfs->fs.location) {
2979                         list_add_tail(&newfs->list, &item->list);
2980                         added = true;
2981                         break;
2982                 } else if (item->fs.location == fs->location) {
2983                         netdev_err(netdev, "Rule not added: location %d not free!\n",
2984                                         fs->location);
2985                         ret = -EBUSY;
2986                         goto err;
2987                 }
2988         }
2989         if (!added)
2990                 list_add_tail(&newfs->list, &bp->rx_fs_list.list);
2991
2992         gem_prog_cmp_regs(bp, fs);
2993         bp->rx_fs_list.count++;
2994         /* enable filtering if NTUPLE on */
2995         if (netdev->features & NETIF_F_NTUPLE)
2996                 gem_enable_flow_filters(bp, 1);
2997
2998         spin_unlock_irqrestore(&bp->rx_fs_lock, flags);
2999         return 0;
3000
3001 err:
3002         spin_unlock_irqrestore(&bp->rx_fs_lock, flags);
3003         kfree(newfs);
3004         return ret;
3005 }
3006
3007 static int gem_del_flow_filter(struct net_device *netdev,
3008                 struct ethtool_rxnfc *cmd)
3009 {
3010         struct macb *bp = netdev_priv(netdev);
3011         struct ethtool_rx_fs_item *item;
3012         struct ethtool_rx_flow_spec *fs;
3013         unsigned long flags;
3014
3015         spin_lock_irqsave(&bp->rx_fs_lock, flags);
3016
3017         list_for_each_entry(item, &bp->rx_fs_list.list, list) {
3018                 if (item->fs.location == cmd->fs.location) {
3019                         /* disable screener regs for the flow entry */
3020                         fs = &(item->fs);
3021                         netdev_dbg(netdev,
3022                                         "Deleting flow filter entry,type=%u,queue=%u,loc=%u,src=%08X,dst=%08X,ps=%u,pd=%u\n",
3023                                         fs->flow_type, (int)fs->ring_cookie, fs->location,
3024                                         htonl(fs->h_u.tcp_ip4_spec.ip4src),
3025                                         htonl(fs->h_u.tcp_ip4_spec.ip4dst),
3026                                         htons(fs->h_u.tcp_ip4_spec.psrc),
3027                                         htons(fs->h_u.tcp_ip4_spec.pdst));
3028
3029                         gem_writel_n(bp, SCRT2, fs->location, 0);
3030
3031                         list_del(&item->list);
3032                         bp->rx_fs_list.count--;
3033                         spin_unlock_irqrestore(&bp->rx_fs_lock, flags);
3034                         kfree(item);
3035                         return 0;
3036                 }
3037         }
3038
3039         spin_unlock_irqrestore(&bp->rx_fs_lock, flags);
3040         return -EINVAL;
3041 }
3042
3043 static int gem_get_flow_entry(struct net_device *netdev,
3044                 struct ethtool_rxnfc *cmd)
3045 {
3046         struct macb *bp = netdev_priv(netdev);
3047         struct ethtool_rx_fs_item *item;
3048
3049         list_for_each_entry(item, &bp->rx_fs_list.list, list) {
3050                 if (item->fs.location == cmd->fs.location) {
3051                         memcpy(&cmd->fs, &item->fs, sizeof(cmd->fs));
3052                         return 0;
3053                 }
3054         }
3055         return -EINVAL;
3056 }
3057
3058 static int gem_get_all_flow_entries(struct net_device *netdev,
3059                 struct ethtool_rxnfc *cmd, u32 *rule_locs)
3060 {
3061         struct macb *bp = netdev_priv(netdev);
3062         struct ethtool_rx_fs_item *item;
3063         uint32_t cnt = 0;
3064
3065         list_for_each_entry(item, &bp->rx_fs_list.list, list) {
3066                 if (cnt == cmd->rule_cnt)
3067                         return -EMSGSIZE;
3068                 rule_locs[cnt] = item->fs.location;
3069                 cnt++;
3070         }
3071         cmd->data = bp->max_tuples;
3072         cmd->rule_cnt = cnt;
3073
3074         return 0;
3075 }
3076
3077 static int gem_get_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *cmd,
3078                 u32 *rule_locs)
3079 {
3080         struct macb *bp = netdev_priv(netdev);
3081         int ret = 0;
3082
3083         switch (cmd->cmd) {
3084         case ETHTOOL_GRXRINGS:
3085                 cmd->data = bp->num_queues;
3086                 break;
3087         case ETHTOOL_GRXCLSRLCNT:
3088                 cmd->rule_cnt = bp->rx_fs_list.count;
3089                 break;
3090         case ETHTOOL_GRXCLSRULE:
3091                 ret = gem_get_flow_entry(netdev, cmd);
3092                 break;
3093         case ETHTOOL_GRXCLSRLALL:
3094                 ret = gem_get_all_flow_entries(netdev, cmd, rule_locs);
3095                 break;
3096         default:
3097                 netdev_err(netdev,
3098                           "Command parameter %d is not supported\n", cmd->cmd);
3099                 ret = -EOPNOTSUPP;
3100         }
3101
3102         return ret;
3103 }
3104
3105 static int gem_set_rxnfc(struct net_device *netdev, struct ethtool_rxnfc *cmd)
3106 {
3107         struct macb *bp = netdev_priv(netdev);
3108         int ret;
3109
3110         switch (cmd->cmd) {
3111         case ETHTOOL_SRXCLSRLINS:
3112                 if ((cmd->fs.location >= bp->max_tuples)
3113                                 || (cmd->fs.ring_cookie >= bp->num_queues)) {
3114                         ret = -EINVAL;
3115                         break;
3116                 }
3117                 ret = gem_add_flow_filter(netdev, cmd);
3118                 break;
3119         case ETHTOOL_SRXCLSRLDEL:
3120                 ret = gem_del_flow_filter(netdev, cmd);
3121                 break;
3122         default:
3123                 netdev_err(netdev,
3124                           "Command parameter %d is not supported\n", cmd->cmd);
3125                 ret = -EOPNOTSUPP;
3126         }
3127
3128         return ret;
3129 }
3130
3131 static const struct ethtool_ops macb_ethtool_ops = {
3132         .get_regs_len           = macb_get_regs_len,
3133         .get_regs               = macb_get_regs,
3134         .get_link               = ethtool_op_get_link,
3135         .get_ts_info            = ethtool_op_get_ts_info,
3136         .get_wol                = macb_get_wol,
3137         .set_wol                = macb_set_wol,
3138         .get_link_ksettings     = phy_ethtool_get_link_ksettings,
3139         .set_link_ksettings     = phy_ethtool_set_link_ksettings,
3140         .get_ringparam          = macb_get_ringparam,
3141         .set_ringparam          = macb_set_ringparam,
3142 };
3143
3144 static const struct ethtool_ops gem_ethtool_ops = {
3145         .get_regs_len           = macb_get_regs_len,
3146         .get_regs               = macb_get_regs,
3147         .get_link               = ethtool_op_get_link,
3148         .get_ts_info            = macb_get_ts_info,
3149         .get_ethtool_stats      = gem_get_ethtool_stats,
3150         .get_strings            = gem_get_ethtool_strings,
3151         .get_sset_count         = gem_get_sset_count,
3152         .get_link_ksettings     = phy_ethtool_get_link_ksettings,
3153         .set_link_ksettings     = phy_ethtool_set_link_ksettings,
3154         .get_ringparam          = macb_get_ringparam,
3155         .set_ringparam          = macb_set_ringparam,
3156         .get_rxnfc                      = gem_get_rxnfc,
3157         .set_rxnfc                      = gem_set_rxnfc,
3158 };
3159
3160 static int macb_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
3161 {
3162         struct phy_device *phydev = dev->phydev;
3163         struct macb *bp = netdev_priv(dev);
3164
3165         if (!netif_running(dev))
3166                 return -EINVAL;
3167
3168         if (!phydev)
3169                 return -ENODEV;
3170
3171         if (!bp->ptp_info)
3172                 return phy_mii_ioctl(phydev, rq, cmd);
3173
3174         switch (cmd) {
3175         case SIOCSHWTSTAMP:
3176                 return bp->ptp_info->set_hwtst(dev, rq, cmd);
3177         case SIOCGHWTSTAMP:
3178                 return bp->ptp_info->get_hwtst(dev, rq);
3179         default:
3180                 return phy_mii_ioctl(phydev, rq, cmd);
3181         }
3182 }
3183
3184 static int macb_set_features(struct net_device *netdev,
3185                              netdev_features_t features)
3186 {
3187         struct macb *bp = netdev_priv(netdev);
3188         netdev_features_t changed = features ^ netdev->features;
3189
3190         /* TX checksum offload */
3191         if ((changed & NETIF_F_HW_CSUM) && macb_is_gem(bp)) {
3192                 u32 dmacfg;
3193
3194                 dmacfg = gem_readl(bp, DMACFG);
3195                 if (features & NETIF_F_HW_CSUM)
3196                         dmacfg |= GEM_BIT(TXCOEN);
3197                 else
3198                         dmacfg &= ~GEM_BIT(TXCOEN);
3199                 gem_writel(bp, DMACFG, dmacfg);
3200         }
3201
3202         /* RX checksum offload */
3203         if ((changed & NETIF_F_RXCSUM) && macb_is_gem(bp)) {
3204                 u32 netcfg;
3205
3206                 netcfg = gem_readl(bp, NCFGR);
3207                 if (features & NETIF_F_RXCSUM &&
3208                     !(netdev->flags & IFF_PROMISC))
3209                         netcfg |= GEM_BIT(RXCOEN);
3210                 else
3211                         netcfg &= ~GEM_BIT(RXCOEN);
3212                 gem_writel(bp, NCFGR, netcfg);
3213         }
3214
3215         /* RX Flow Filters */
3216         if ((changed & NETIF_F_NTUPLE) && macb_is_gem(bp)) {
3217                 bool turn_on = features & NETIF_F_NTUPLE;
3218
3219                 gem_enable_flow_filters(bp, turn_on);
3220         }
3221         return 0;
3222 }
3223
3224 static const struct net_device_ops macb_netdev_ops = {
3225         .ndo_open               = macb_open,
3226         .ndo_stop               = macb_close,
3227         .ndo_start_xmit         = macb_start_xmit,
3228         .ndo_set_rx_mode        = macb_set_rx_mode,
3229         .ndo_get_stats          = macb_get_stats,
3230         .ndo_do_ioctl           = macb_ioctl,
3231         .ndo_validate_addr      = eth_validate_addr,
3232         .ndo_change_mtu         = macb_change_mtu,
3233         .ndo_set_mac_address    = eth_mac_addr,
3234 #ifdef CONFIG_NET_POLL_CONTROLLER
3235         .ndo_poll_controller    = macb_poll_controller,
3236 #endif
3237         .ndo_set_features       = macb_set_features,
3238         .ndo_features_check     = macb_features_check,
3239 };
3240
3241 /* Configure peripheral capabilities according to device tree
3242  * and integration options used
3243  */
3244 static void macb_configure_caps(struct macb *bp,
3245                                 const struct macb_config *dt_conf)
3246 {
3247         u32 dcfg;
3248
3249         if (dt_conf)
3250                 bp->caps = dt_conf->caps;
3251
3252         if (hw_is_gem(bp->regs, bp->native_io)) {
3253                 bp->caps |= MACB_CAPS_MACB_IS_GEM;
3254
3255                 dcfg = gem_readl(bp, DCFG1);
3256                 if (GEM_BFEXT(IRQCOR, dcfg) == 0)
3257                         bp->caps |= MACB_CAPS_ISR_CLEAR_ON_WRITE;
3258                 dcfg = gem_readl(bp, DCFG2);
3259                 if ((dcfg & (GEM_BIT(RX_PKT_BUFF) | GEM_BIT(TX_PKT_BUFF))) == 0)
3260                         bp->caps |= MACB_CAPS_FIFO_MODE;
3261 #ifdef CONFIG_MACB_USE_HWSTAMP
3262                 if (gem_has_ptp(bp)) {
3263                         if (!GEM_BFEXT(TSU, gem_readl(bp, DCFG5)))
3264                                 pr_err("GEM doesn't support hardware ptp.\n");
3265                         else {
3266                                 bp->hw_dma_cap |= HW_DMA_CAP_PTP;
3267                                 bp->ptp_info = &gem_ptp_info;
3268                         }
3269                 }
3270 #endif
3271         }
3272
3273         dev_dbg(&bp->pdev->dev, "Cadence caps 0x%08x\n", bp->caps);
3274 }
3275
3276 static void macb_probe_queues(void __iomem *mem,
3277                               bool native_io,
3278                               unsigned int *queue_mask,
3279                               unsigned int *num_queues)
3280 {
3281         unsigned int hw_q;
3282
3283         *queue_mask = 0x1;
3284         *num_queues = 1;
3285
3286         /* is it macb or gem ?
3287          *
3288          * We need to read directly from the hardware here because
3289          * we are early in the probe process and don't have the
3290          * MACB_CAPS_MACB_IS_GEM flag positioned
3291          */
3292         if (!hw_is_gem(mem, native_io))
3293                 return;
3294
3295         /* bit 0 is never set but queue 0 always exists */
3296         *queue_mask = readl_relaxed(mem + GEM_DCFG6) & 0xff;
3297
3298         *queue_mask |= 0x1;
3299
3300         for (hw_q = 1; hw_q < MACB_MAX_QUEUES; ++hw_q)
3301                 if (*queue_mask & (1 << hw_q))
3302                         (*num_queues)++;
3303 }
3304
3305 static int macb_clk_init(struct platform_device *pdev, struct clk **pclk,
3306                          struct clk **hclk, struct clk **tx_clk,
3307                          struct clk **rx_clk)
3308 {
3309         struct macb_platform_data *pdata;
3310         int err;
3311
3312         pdata = dev_get_platdata(&pdev->dev);
3313         if (pdata) {
3314                 *pclk = pdata->pclk;
3315                 *hclk = pdata->hclk;
3316         } else {
3317                 *pclk = devm_clk_get(&pdev->dev, "pclk");
3318                 *hclk = devm_clk_get(&pdev->dev, "hclk");
3319         }
3320
3321         if (IS_ERR(*pclk)) {
3322                 err = PTR_ERR(*pclk);
3323                 dev_err(&pdev->dev, "failed to get macb_clk (%u)\n", err);
3324                 return err;
3325         }
3326
3327         if (IS_ERR(*hclk)) {
3328                 err = PTR_ERR(*hclk);
3329                 dev_err(&pdev->dev, "failed to get hclk (%u)\n", err);
3330                 return err;
3331         }
3332
3333         *tx_clk = devm_clk_get(&pdev->dev, "tx_clk");
3334         if (IS_ERR(*tx_clk))
3335                 *tx_clk = NULL;
3336
3337         *rx_clk = devm_clk_get(&pdev->dev, "rx_clk");
3338         if (IS_ERR(*rx_clk))
3339                 *rx_clk = NULL;
3340
3341         err = clk_prepare_enable(*pclk);
3342         if (err) {
3343                 dev_err(&pdev->dev, "failed to enable pclk (%u)\n", err);
3344                 return err;
3345         }
3346
3347         err = clk_prepare_enable(*hclk);
3348         if (err) {
3349                 dev_err(&pdev->dev, "failed to enable hclk (%u)\n", err);
3350                 goto err_disable_pclk;
3351         }
3352
3353         err = clk_prepare_enable(*tx_clk);
3354         if (err) {
3355                 dev_err(&pdev->dev, "failed to enable tx_clk (%u)\n", err);
3356                 goto err_disable_hclk;
3357         }
3358
3359         err = clk_prepare_enable(*rx_clk);
3360         if (err) {
3361                 dev_err(&pdev->dev, "failed to enable rx_clk (%u)\n", err);
3362                 goto err_disable_txclk;
3363         }
3364
3365         return 0;
3366
3367 err_disable_txclk:
3368         clk_disable_unprepare(*tx_clk);
3369
3370 err_disable_hclk:
3371         clk_disable_unprepare(*hclk);
3372
3373 err_disable_pclk:
3374         clk_disable_unprepare(*pclk);
3375
3376         return err;
3377 }
3378
3379 static int macb_init(struct platform_device *pdev)
3380 {
3381         struct net_device *dev = platform_get_drvdata(pdev);
3382         unsigned int hw_q, q;
3383         struct macb *bp = netdev_priv(dev);
3384         struct macb_queue *queue;
3385         int err;
3386         u32 val, reg;
3387
3388         bp->tx_ring_size = DEFAULT_TX_RING_SIZE;
3389         bp->rx_ring_size = DEFAULT_RX_RING_SIZE;
3390
3391         /* set the queue register mapping once for all: queue0 has a special
3392          * register mapping but we don't want to test the queue index then
3393          * compute the corresponding register offset at run time.
3394          */
3395         for (hw_q = 0, q = 0; hw_q < MACB_MAX_QUEUES; ++hw_q) {
3396                 if (!(bp->queue_mask & (1 << hw_q)))
3397                         continue;
3398
3399                 queue = &bp->queues[q];
3400                 queue->bp = bp;
3401                 netif_napi_add(dev, &queue->napi, macb_poll, 64);
3402                 if (hw_q) {
3403                         queue->ISR  = GEM_ISR(hw_q - 1);
3404                         queue->IER  = GEM_IER(hw_q - 1);
3405                         queue->IDR  = GEM_IDR(hw_q - 1);
3406                         queue->IMR  = GEM_IMR(hw_q - 1);
3407                         queue->TBQP = GEM_TBQP(hw_q - 1);
3408                         queue->RBQP = GEM_RBQP(hw_q - 1);
3409                         queue->RBQS = GEM_RBQS(hw_q - 1);
3410 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
3411                         if (bp->hw_dma_cap & HW_DMA_CAP_64B) {
3412                                 queue->TBQPH = GEM_TBQPH(hw_q - 1);
3413                                 queue->RBQPH = GEM_RBQPH(hw_q - 1);
3414                         }
3415 #endif
3416                 } else {
3417                         /* queue0 uses legacy registers */
3418                         queue->ISR  = MACB_ISR;
3419                         queue->IER  = MACB_IER;
3420                         queue->IDR  = MACB_IDR;
3421                         queue->IMR  = MACB_IMR;
3422                         queue->TBQP = MACB_TBQP;
3423                         queue->RBQP = MACB_RBQP;
3424 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
3425                         if (bp->hw_dma_cap & HW_DMA_CAP_64B) {
3426                                 queue->TBQPH = MACB_TBQPH;
3427                                 queue->RBQPH = MACB_RBQPH;
3428                         }
3429 #endif
3430                 }
3431
3432                 /* get irq: here we use the linux queue index, not the hardware
3433                  * queue index. the queue irq definitions in the device tree
3434                  * must remove the optional gaps that could exist in the
3435                  * hardware queue mask.
3436                  */
3437                 queue->irq = platform_get_irq(pdev, q);
3438                 err = devm_request_irq(&pdev->dev, queue->irq, macb_interrupt,
3439                                        IRQF_SHARED, dev->name, queue);
3440                 if (err) {
3441                         dev_err(&pdev->dev,
3442                                 "Unable to request IRQ %d (error %d)\n",
3443                                 queue->irq, err);
3444                         return err;
3445                 }
3446
3447                 INIT_WORK(&queue->tx_error_task, macb_tx_error_task);
3448                 q++;
3449         }
3450
3451         dev->netdev_ops = &macb_netdev_ops;
3452
3453         /* setup appropriated routines according to adapter type */
3454         if (macb_is_gem(bp)) {
3455                 bp->max_tx_length = GEM_MAX_TX_LEN;
3456                 bp->macbgem_ops.mog_alloc_rx_buffers = gem_alloc_rx_buffers;
3457                 bp->macbgem_ops.mog_free_rx_buffers = gem_free_rx_buffers;
3458                 bp->macbgem_ops.mog_init_rings = gem_init_rings;
3459                 bp->macbgem_ops.mog_rx = gem_rx;
3460                 dev->ethtool_ops = &gem_ethtool_ops;
3461         } else {
3462                 bp->max_tx_length = MACB_MAX_TX_LEN;
3463                 bp->macbgem_ops.mog_alloc_rx_buffers = macb_alloc_rx_buffers;
3464                 bp->macbgem_ops.mog_free_rx_buffers = macb_free_rx_buffers;
3465                 bp->macbgem_ops.mog_init_rings = macb_init_rings;
3466                 bp->macbgem_ops.mog_rx = macb_rx;
3467                 dev->ethtool_ops = &macb_ethtool_ops;
3468         }
3469
3470         /* Set features */
3471         dev->hw_features = NETIF_F_SG;
3472
3473         /* Check LSO capability */
3474         if (GEM_BFEXT(PBUF_LSO, gem_readl(bp, DCFG6)))
3475                 dev->hw_features |= MACB_NETIF_LSO;
3476
3477         /* Checksum offload is only available on gem with packet buffer */
3478         if (macb_is_gem(bp) && !(bp->caps & MACB_CAPS_FIFO_MODE))
3479                 dev->hw_features |= NETIF_F_HW_CSUM | NETIF_F_RXCSUM;
3480         if (bp->caps & MACB_CAPS_SG_DISABLED)
3481                 dev->hw_features &= ~NETIF_F_SG;
3482         dev->features = dev->hw_features;
3483
3484         /* Check RX Flow Filters support.
3485          * Max Rx flows set by availability of screeners & compare regs:
3486          * each 4-tuple define requires 1 T2 screener reg + 3 compare regs
3487          */
3488         reg = gem_readl(bp, DCFG8);
3489         bp->max_tuples = min((GEM_BFEXT(SCR2CMP, reg) / 3),
3490                         GEM_BFEXT(T2SCR, reg));
3491         if (bp->max_tuples > 0) {
3492                 /* also needs one ethtype match to check IPv4 */
3493                 if (GEM_BFEXT(SCR2ETH, reg) > 0) {
3494                         /* program this reg now */
3495                         reg = 0;
3496                         reg = GEM_BFINS(ETHTCMP, (uint16_t)ETH_P_IP, reg);
3497                         gem_writel_n(bp, ETHT, SCRT2_ETHT, reg);
3498                         /* Filtering is supported in hw but don't enable it in kernel now */
3499                         dev->hw_features |= NETIF_F_NTUPLE;
3500                         /* init Rx flow definitions */
3501                         INIT_LIST_HEAD(&bp->rx_fs_list.list);
3502                         bp->rx_fs_list.count = 0;
3503                         spin_lock_init(&bp->rx_fs_lock);
3504                 } else
3505                         bp->max_tuples = 0;
3506         }
3507
3508         if (!(bp->caps & MACB_CAPS_USRIO_DISABLED)) {
3509                 val = 0;
3510                 if (bp->phy_interface == PHY_INTERFACE_MODE_RGMII)
3511                         val = GEM_BIT(RGMII);
3512                 else if (bp->phy_interface == PHY_INTERFACE_MODE_RMII &&
3513                          (bp->caps & MACB_CAPS_USRIO_DEFAULT_IS_MII_GMII))
3514                         val = MACB_BIT(RMII);
3515                 else if (!(bp->caps & MACB_CAPS_USRIO_DEFAULT_IS_MII_GMII))
3516                         val = MACB_BIT(MII);
3517
3518                 if (bp->caps & MACB_CAPS_USRIO_HAS_CLKEN)
3519                         val |= MACB_BIT(CLKEN);
3520
3521                 macb_or_gem_writel(bp, USRIO, val);
3522         }
3523
3524         /* Set MII management clock divider */
3525         val = macb_mdc_clk_div(bp);
3526         val |= macb_dbw(bp);
3527         if (bp->phy_interface == PHY_INTERFACE_MODE_SGMII)
3528                 val |= GEM_BIT(SGMIIEN) | GEM_BIT(PCSSEL);
3529         macb_writel(bp, NCFGR, val);
3530
3531         return 0;
3532 }
3533
3534 #if defined(CONFIG_OF)
3535 /* 1518 rounded up */
3536 #define AT91ETHER_MAX_RBUFF_SZ  0x600
3537 /* max number of receive buffers */
3538 #define AT91ETHER_MAX_RX_DESCR  9
3539
3540 /* Initialize and start the Receiver and Transmit subsystems */
3541 static int at91ether_start(struct net_device *dev)
3542 {
3543         struct macb *lp = netdev_priv(dev);
3544         struct macb_queue *q = &lp->queues[0];
3545         struct macb_dma_desc *desc;
3546         dma_addr_t addr;
3547         u32 ctl;
3548         int i;
3549
3550         q->rx_ring = dma_alloc_coherent(&lp->pdev->dev,
3551                                          (AT91ETHER_MAX_RX_DESCR *
3552                                           macb_dma_desc_get_size(lp)),
3553                                          &q->rx_ring_dma, GFP_KERNEL);
3554         if (!q->rx_ring)
3555                 return -ENOMEM;
3556
3557         q->rx_buffers = dma_alloc_coherent(&lp->pdev->dev,
3558                                             AT91ETHER_MAX_RX_DESCR *
3559                                             AT91ETHER_MAX_RBUFF_SZ,
3560                                             &q->rx_buffers_dma, GFP_KERNEL);
3561         if (!q->rx_buffers) {
3562                 dma_free_coherent(&lp->pdev->dev,
3563                                   AT91ETHER_MAX_RX_DESCR *
3564                                   macb_dma_desc_get_size(lp),
3565                                   q->rx_ring, q->rx_ring_dma);
3566                 q->rx_ring = NULL;
3567                 return -ENOMEM;
3568         }
3569
3570         addr = q->rx_buffers_dma;
3571         for (i = 0; i < AT91ETHER_MAX_RX_DESCR; i++) {
3572                 desc = macb_rx_desc(q, i);
3573                 macb_set_addr(lp, desc, addr);
3574                 desc->ctrl = 0;
3575                 addr += AT91ETHER_MAX_RBUFF_SZ;
3576         }
3577
3578         /* Set the Wrap bit on the last descriptor */
3579         desc->addr |= MACB_BIT(RX_WRAP);
3580
3581         /* Reset buffer index */
3582         q->rx_tail = 0;
3583
3584         /* Program address of descriptor list in Rx Buffer Queue register */
3585         macb_writel(lp, RBQP, q->rx_ring_dma);
3586
3587         /* Enable Receive and Transmit */
3588         ctl = macb_readl(lp, NCR);
3589         macb_writel(lp, NCR, ctl | MACB_BIT(RE) | MACB_BIT(TE));
3590
3591         return 0;
3592 }
3593
3594 /* Open the ethernet interface */
3595 static int at91ether_open(struct net_device *dev)
3596 {
3597         struct macb *lp = netdev_priv(dev);
3598         u32 ctl;
3599         int ret;
3600
3601         /* Clear internal statistics */
3602         ctl = macb_readl(lp, NCR);
3603         macb_writel(lp, NCR, ctl | MACB_BIT(CLRSTAT));
3604
3605         macb_set_hwaddr(lp);
3606
3607         ret = at91ether_start(dev);
3608         if (ret)
3609                 return ret;
3610
3611         /* Enable MAC interrupts */
3612         macb_writel(lp, IER, MACB_BIT(RCOMP)    |
3613                              MACB_BIT(RXUBR)    |
3614                              MACB_BIT(ISR_TUND) |
3615                              MACB_BIT(ISR_RLE)  |
3616                              MACB_BIT(TCOMP)    |
3617                              MACB_BIT(ISR_ROVR) |
3618                              MACB_BIT(HRESP));
3619
3620         /* schedule a link state check */
3621         phy_start(dev->phydev);
3622
3623         netif_start_queue(dev);
3624
3625         return 0;
3626 }
3627
3628 /* Close the interface */
3629 static int at91ether_close(struct net_device *dev)
3630 {
3631         struct macb *lp = netdev_priv(dev);
3632         struct macb_queue *q = &lp->queues[0];
3633         u32 ctl;
3634
3635         /* Disable Receiver and Transmitter */
3636         ctl = macb_readl(lp, NCR);
3637         macb_writel(lp, NCR, ctl & ~(MACB_BIT(TE) | MACB_BIT(RE)));
3638
3639         /* Disable MAC interrupts */
3640         macb_writel(lp, IDR, MACB_BIT(RCOMP)    |
3641                              MACB_BIT(RXUBR)    |
3642                              MACB_BIT(ISR_TUND) |
3643                              MACB_BIT(ISR_RLE)  |
3644                              MACB_BIT(TCOMP)    |
3645                              MACB_BIT(ISR_ROVR) |
3646                              MACB_BIT(HRESP));
3647
3648         netif_stop_queue(dev);
3649
3650         dma_free_coherent(&lp->pdev->dev,
3651                           AT91ETHER_MAX_RX_DESCR *
3652                           macb_dma_desc_get_size(lp),
3653                           q->rx_ring, q->rx_ring_dma);
3654         q->rx_ring = NULL;
3655
3656         dma_free_coherent(&lp->pdev->dev,
3657                           AT91ETHER_MAX_RX_DESCR * AT91ETHER_MAX_RBUFF_SZ,
3658                           q->rx_buffers, q->rx_buffers_dma);
3659         q->rx_buffers = NULL;
3660
3661         return 0;
3662 }
3663
3664 /* Transmit packet */
3665 static netdev_tx_t at91ether_start_xmit(struct sk_buff *skb,
3666                                         struct net_device *dev)
3667 {
3668         struct macb *lp = netdev_priv(dev);
3669
3670         if (macb_readl(lp, TSR) & MACB_BIT(RM9200_BNQ)) {
3671                 netif_stop_queue(dev);
3672
3673                 /* Store packet information (to free when Tx completed) */
3674                 lp->skb = skb;
3675                 lp->skb_length = skb->len;
3676                 lp->skb_physaddr = dma_map_single(NULL, skb->data, skb->len,
3677                                                         DMA_TO_DEVICE);
3678                 if (dma_mapping_error(NULL, lp->skb_physaddr)) {
3679                         dev_kfree_skb_any(skb);
3680                         dev->stats.tx_dropped++;
3681                         netdev_err(dev, "%s: DMA mapping error\n", __func__);
3682                         return NETDEV_TX_OK;
3683                 }
3684
3685                 /* Set address of the data in the Transmit Address register */
3686                 macb_writel(lp, TAR, lp->skb_physaddr);
3687                 /* Set length of the packet in the Transmit Control register */
3688                 macb_writel(lp, TCR, skb->len);
3689
3690         } else {
3691                 netdev_err(dev, "%s called, but device is busy!\n", __func__);
3692                 return NETDEV_TX_BUSY;
3693         }
3694
3695         return NETDEV_TX_OK;
3696 }
3697
3698 /* Extract received frame from buffer descriptors and sent to upper layers.
3699  * (Called from interrupt context)
3700  */
3701 static void at91ether_rx(struct net_device *dev)
3702 {
3703         struct macb *lp = netdev_priv(dev);
3704         struct macb_queue *q = &lp->queues[0];
3705         struct macb_dma_desc *desc;
3706         unsigned char *p_recv;
3707         struct sk_buff *skb;
3708         unsigned int pktlen;
3709
3710         desc = macb_rx_desc(q, q->rx_tail);
3711         while (desc->addr & MACB_BIT(RX_USED)) {
3712                 p_recv = q->rx_buffers + q->rx_tail * AT91ETHER_MAX_RBUFF_SZ;
3713                 pktlen = MACB_BF(RX_FRMLEN, desc->ctrl);
3714                 skb = netdev_alloc_skb(dev, pktlen + 2);
3715                 if (skb) {
3716                         skb_reserve(skb, 2);
3717                         skb_put_data(skb, p_recv, pktlen);
3718
3719                         skb->protocol = eth_type_trans(skb, dev);
3720                         dev->stats.rx_packets++;
3721                         dev->stats.rx_bytes += pktlen;
3722                         netif_rx(skb);
3723                 } else {
3724                         dev->stats.rx_dropped++;
3725                 }
3726
3727                 if (desc->ctrl & MACB_BIT(RX_MHASH_MATCH))
3728                         dev->stats.multicast++;
3729
3730                 /* reset ownership bit */
3731                 desc->addr &= ~MACB_BIT(RX_USED);
3732
3733                 /* wrap after last buffer */
3734                 if (q->rx_tail == AT91ETHER_MAX_RX_DESCR - 1)
3735                         q->rx_tail = 0;
3736                 else
3737                         q->rx_tail++;
3738
3739                 desc = macb_rx_desc(q, q->rx_tail);
3740         }
3741 }
3742
3743 /* MAC interrupt handler */
3744 static irqreturn_t at91ether_interrupt(int irq, void *dev_id)
3745 {
3746         struct net_device *dev = dev_id;
3747         struct macb *lp = netdev_priv(dev);
3748         u32 intstatus, ctl;
3749
3750         /* MAC Interrupt Status register indicates what interrupts are pending.
3751          * It is automatically cleared once read.
3752          */
3753         intstatus = macb_readl(lp, ISR);
3754
3755         /* Receive complete */
3756         if (intstatus & MACB_BIT(RCOMP))
3757                 at91ether_rx(dev);
3758
3759         /* Transmit complete */
3760         if (intstatus & MACB_BIT(TCOMP)) {
3761                 /* The TCOM bit is set even if the transmission failed */
3762                 if (intstatus & (MACB_BIT(ISR_TUND) | MACB_BIT(ISR_RLE)))
3763                         dev->stats.tx_errors++;
3764
3765                 if (lp->skb) {
3766                         dev_kfree_skb_irq(lp->skb);
3767                         lp->skb = NULL;
3768                         dma_unmap_single(NULL, lp->skb_physaddr,
3769                                          lp->skb_length, DMA_TO_DEVICE);
3770                         dev->stats.tx_packets++;
3771                         dev->stats.tx_bytes += lp->skb_length;
3772                 }
3773                 netif_wake_queue(dev);
3774         }
3775
3776         /* Work-around for EMAC Errata section 41.3.1 */
3777         if (intstatus & MACB_BIT(RXUBR)) {
3778                 ctl = macb_readl(lp, NCR);
3779                 macb_writel(lp, NCR, ctl & ~MACB_BIT(RE));
3780                 wmb();
3781                 macb_writel(lp, NCR, ctl | MACB_BIT(RE));
3782         }
3783
3784         if (intstatus & MACB_BIT(ISR_ROVR))
3785                 netdev_err(dev, "ROVR error\n");
3786
3787         return IRQ_HANDLED;
3788 }
3789
3790 #ifdef CONFIG_NET_POLL_CONTROLLER
3791 static void at91ether_poll_controller(struct net_device *dev)
3792 {
3793         unsigned long flags;
3794
3795         local_irq_save(flags);
3796         at91ether_interrupt(dev->irq, dev);
3797         local_irq_restore(flags);
3798 }
3799 #endif
3800
3801 static const struct net_device_ops at91ether_netdev_ops = {
3802         .ndo_open               = at91ether_open,
3803         .ndo_stop               = at91ether_close,
3804         .ndo_start_xmit         = at91ether_start_xmit,
3805         .ndo_get_stats          = macb_get_stats,
3806         .ndo_set_rx_mode        = macb_set_rx_mode,
3807         .ndo_set_mac_address    = eth_mac_addr,
3808         .ndo_do_ioctl           = macb_ioctl,
3809         .ndo_validate_addr      = eth_validate_addr,
3810 #ifdef CONFIG_NET_POLL_CONTROLLER
3811         .ndo_poll_controller    = at91ether_poll_controller,
3812 #endif
3813 };
3814
3815 static int at91ether_clk_init(struct platform_device *pdev, struct clk **pclk,
3816                               struct clk **hclk, struct clk **tx_clk,
3817                               struct clk **rx_clk)
3818 {
3819         int err;
3820
3821         *hclk = NULL;
3822         *tx_clk = NULL;
3823         *rx_clk = NULL;
3824
3825         *pclk = devm_clk_get(&pdev->dev, "ether_clk");
3826         if (IS_ERR(*pclk))
3827                 return PTR_ERR(*pclk);
3828
3829         err = clk_prepare_enable(*pclk);
3830         if (err) {
3831                 dev_err(&pdev->dev, "failed to enable pclk (%u)\n", err);
3832                 return err;
3833         }
3834
3835         return 0;
3836 }
3837
3838 static int at91ether_init(struct platform_device *pdev)
3839 {
3840         struct net_device *dev = platform_get_drvdata(pdev);
3841         struct macb *bp = netdev_priv(dev);
3842         int err;
3843         u32 reg;
3844
3845         bp->queues[0].bp = bp;
3846
3847         dev->netdev_ops = &at91ether_netdev_ops;
3848         dev->ethtool_ops = &macb_ethtool_ops;
3849
3850         err = devm_request_irq(&pdev->dev, dev->irq, at91ether_interrupt,
3851                                0, dev->name, dev);
3852         if (err)
3853                 return err;
3854
3855         macb_writel(bp, NCR, 0);
3856
3857         reg = MACB_BF(CLK, MACB_CLK_DIV32) | MACB_BIT(BIG);
3858         if (bp->phy_interface == PHY_INTERFACE_MODE_RMII)
3859                 reg |= MACB_BIT(RM9200_RMII);
3860
3861         macb_writel(bp, NCFGR, reg);
3862
3863         return 0;
3864 }
3865
3866 static const struct macb_config at91sam9260_config = {
3867         .caps = MACB_CAPS_USRIO_HAS_CLKEN | MACB_CAPS_USRIO_DEFAULT_IS_MII_GMII,
3868         .clk_init = macb_clk_init,
3869         .init = macb_init,
3870 };
3871
3872 static const struct macb_config sama5d3macb_config = {
3873         .caps = MACB_CAPS_SG_DISABLED
3874               | MACB_CAPS_USRIO_HAS_CLKEN | MACB_CAPS_USRIO_DEFAULT_IS_MII_GMII,
3875         .clk_init = macb_clk_init,
3876         .init = macb_init,
3877 };
3878
3879 static const struct macb_config pc302gem_config = {
3880         .caps = MACB_CAPS_SG_DISABLED | MACB_CAPS_GIGABIT_MODE_AVAILABLE,
3881         .dma_burst_length = 16,
3882         .clk_init = macb_clk_init,
3883         .init = macb_init,
3884 };
3885
3886 static const struct macb_config sama5d2_config = {
3887         .caps = MACB_CAPS_USRIO_DEFAULT_IS_MII_GMII,
3888         .dma_burst_length = 16,
3889         .clk_init = macb_clk_init,
3890         .init = macb_init,
3891 };
3892
3893 static const struct macb_config sama5d3_config = {
3894         .caps = MACB_CAPS_SG_DISABLED | MACB_CAPS_GIGABIT_MODE_AVAILABLE
3895               | MACB_CAPS_USRIO_DEFAULT_IS_MII_GMII | MACB_CAPS_JUMBO,
3896         .dma_burst_length = 16,
3897         .clk_init = macb_clk_init,
3898         .init = macb_init,
3899         .jumbo_max_len = 10240,
3900 };
3901
3902 static const struct macb_config sama5d4_config = {
3903         .caps = MACB_CAPS_USRIO_DEFAULT_IS_MII_GMII,
3904         .dma_burst_length = 4,
3905         .clk_init = macb_clk_init,
3906         .init = macb_init,
3907 };
3908
3909 static const struct macb_config emac_config = {
3910         .caps = MACB_CAPS_NEEDS_RSTONUBR,
3911         .clk_init = at91ether_clk_init,
3912         .init = at91ether_init,
3913 };
3914
3915 static const struct macb_config np4_config = {
3916         .caps = MACB_CAPS_USRIO_DISABLED,
3917         .clk_init = macb_clk_init,
3918         .init = macb_init,
3919 };
3920
3921 static const struct macb_config zynqmp_config = {
3922         .caps = MACB_CAPS_GIGABIT_MODE_AVAILABLE |
3923                         MACB_CAPS_JUMBO |
3924                         MACB_CAPS_GEM_HAS_PTP | MACB_CAPS_BD_RD_PREFETCH,
3925         .dma_burst_length = 16,
3926         .clk_init = macb_clk_init,
3927         .init = macb_init,
3928         .jumbo_max_len = 10240,
3929 };
3930
3931 static const struct macb_config zynq_config = {
3932         .caps = MACB_CAPS_GIGABIT_MODE_AVAILABLE | MACB_CAPS_NO_GIGABIT_HALF |
3933                 MACB_CAPS_NEEDS_RSTONUBR,
3934         .dma_burst_length = 16,
3935         .clk_init = macb_clk_init,
3936         .init = macb_init,
3937 };
3938
3939 static const struct of_device_id macb_dt_ids[] = {
3940         { .compatible = "cdns,at32ap7000-macb" },
3941         { .compatible = "cdns,at91sam9260-macb", .data = &at91sam9260_config },
3942         { .compatible = "cdns,macb" },
3943         { .compatible = "cdns,np4-macb", .data = &np4_config },
3944         { .compatible = "cdns,pc302-gem", .data = &pc302gem_config },
3945         { .compatible = "cdns,gem", .data = &pc302gem_config },
3946         { .compatible = "atmel,sama5d2-gem", .data = &sama5d2_config },
3947         { .compatible = "atmel,sama5d3-gem", .data = &sama5d3_config },
3948         { .compatible = "atmel,sama5d3-macb", .data = &sama5d3macb_config },
3949         { .compatible = "atmel,sama5d4-gem", .data = &sama5d4_config },
3950         { .compatible = "cdns,at91rm9200-emac", .data = &emac_config },
3951         { .compatible = "cdns,emac", .data = &emac_config },
3952         { .compatible = "cdns,zynqmp-gem", .data = &zynqmp_config},
3953         { .compatible = "cdns,zynq-gem", .data = &zynq_config },
3954         { /* sentinel */ }
3955 };
3956 MODULE_DEVICE_TABLE(of, macb_dt_ids);
3957 #endif /* CONFIG_OF */
3958
3959 static const struct macb_config default_gem_config = {
3960         .caps = MACB_CAPS_GIGABIT_MODE_AVAILABLE |
3961                         MACB_CAPS_JUMBO |
3962                         MACB_CAPS_GEM_HAS_PTP,
3963         .dma_burst_length = 16,
3964         .clk_init = macb_clk_init,
3965         .init = macb_init,
3966         .jumbo_max_len = 10240,
3967 };
3968
3969 static int macb_probe(struct platform_device *pdev)
3970 {
3971         const struct macb_config *macb_config = &default_gem_config;
3972         int (*clk_init)(struct platform_device *, struct clk **,
3973                         struct clk **, struct clk **,  struct clk **)
3974                                               = macb_config->clk_init;
3975         int (*init)(struct platform_device *) = macb_config->init;
3976         struct device_node *np = pdev->dev.of_node;
3977         struct clk *pclk, *hclk = NULL, *tx_clk = NULL, *rx_clk = NULL;
3978         unsigned int queue_mask, num_queues;
3979         struct macb_platform_data *pdata;
3980         bool native_io;
3981         struct phy_device *phydev;
3982         struct net_device *dev;
3983         struct resource *regs;
3984         void __iomem *mem;
3985         const char *mac;
3986         struct macb *bp;
3987         int err, val;
3988
3989         regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
3990         mem = devm_ioremap_resource(&pdev->dev, regs);
3991         if (IS_ERR(mem))
3992                 return PTR_ERR(mem);
3993
3994         if (np) {
3995                 const struct of_device_id *match;
3996
3997                 match = of_match_node(macb_dt_ids, np);
3998                 if (match && match->data) {
3999                         macb_config = match->data;
4000                         clk_init = macb_config->clk_init;
4001                         init = macb_config->init;
4002                 }
4003         }
4004
4005         err = clk_init(pdev, &pclk, &hclk, &tx_clk, &rx_clk);
4006         if (err)
4007                 return err;
4008
4009         native_io = hw_is_native_io(mem);
4010
4011         macb_probe_queues(mem, native_io, &queue_mask, &num_queues);
4012         dev = alloc_etherdev_mq(sizeof(*bp), num_queues);
4013         if (!dev) {
4014                 err = -ENOMEM;
4015                 goto err_disable_clocks;
4016         }
4017
4018         dev->base_addr = regs->start;
4019
4020         SET_NETDEV_DEV(dev, &pdev->dev);
4021
4022         bp = netdev_priv(dev);
4023         bp->pdev = pdev;
4024         bp->dev = dev;
4025         bp->regs = mem;
4026         bp->native_io = native_io;
4027         if (native_io) {
4028                 bp->macb_reg_readl = hw_readl_native;
4029                 bp->macb_reg_writel = hw_writel_native;
4030         } else {
4031                 bp->macb_reg_readl = hw_readl;
4032                 bp->macb_reg_writel = hw_writel;
4033         }
4034         bp->num_queues = num_queues;
4035         bp->queue_mask = queue_mask;
4036         if (macb_config)
4037                 bp->dma_burst_length = macb_config->dma_burst_length;
4038         bp->pclk = pclk;
4039         bp->hclk = hclk;
4040         bp->tx_clk = tx_clk;
4041         bp->rx_clk = rx_clk;
4042         if (macb_config)
4043                 bp->jumbo_max_len = macb_config->jumbo_max_len;
4044
4045         bp->wol = 0;
4046         if (of_get_property(np, "magic-packet", NULL))
4047                 bp->wol |= MACB_WOL_HAS_MAGIC_PACKET;
4048         device_init_wakeup(&pdev->dev, bp->wol & MACB_WOL_HAS_MAGIC_PACKET);
4049
4050         spin_lock_init(&bp->lock);
4051
4052         /* setup capabilities */
4053         macb_configure_caps(bp, macb_config);
4054
4055 #ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
4056         if (GEM_BFEXT(DAW64, gem_readl(bp, DCFG6))) {
4057                 dma_set_mask(&pdev->dev, DMA_BIT_MASK(44));
4058                 bp->hw_dma_cap |= HW_DMA_CAP_64B;
4059         }
4060 #endif
4061         platform_set_drvdata(pdev, dev);
4062
4063         dev->irq = platform_get_irq(pdev, 0);
4064         if (dev->irq < 0) {
4065                 err = dev->irq;
4066                 goto err_out_free_netdev;
4067         }
4068
4069         /* MTU range: 68 - 1500 or 10240 */
4070         dev->min_mtu = GEM_MTU_MIN_SIZE;
4071         if (bp->caps & MACB_CAPS_JUMBO)
4072                 dev->max_mtu = gem_readl(bp, JML) - ETH_HLEN - ETH_FCS_LEN;
4073         else
4074                 dev->max_mtu = ETH_DATA_LEN;
4075
4076         if (bp->caps & MACB_CAPS_BD_RD_PREFETCH) {
4077                 val = GEM_BFEXT(RXBD_RDBUFF, gem_readl(bp, DCFG10));
4078                 if (val)
4079                         bp->rx_bd_rd_prefetch = (2 << (val - 1)) *
4080                                                 macb_dma_desc_get_size(bp);
4081
4082                 val = GEM_BFEXT(TXBD_RDBUFF, gem_readl(bp, DCFG10));
4083                 if (val)
4084                         bp->tx_bd_rd_prefetch = (2 << (val - 1)) *
4085                                                 macb_dma_desc_get_size(bp);
4086         }
4087
4088         bp->rx_intr_mask = MACB_RX_INT_FLAGS;
4089         if (bp->caps & MACB_CAPS_NEEDS_RSTONUBR)
4090                 bp->rx_intr_mask |= MACB_BIT(RXUBR);
4091
4092         mac = of_get_mac_address(np);
4093         if (mac) {
4094                 ether_addr_copy(bp->dev->dev_addr, mac);
4095         } else {
4096                 err = nvmem_get_mac_address(&pdev->dev, bp->dev->dev_addr);
4097                 if (err) {
4098                         if (err == -EPROBE_DEFER)
4099                                 goto err_out_free_netdev;
4100                         macb_get_hwaddr(bp);
4101                 }
4102         }
4103
4104         err = of_get_phy_mode(np);
4105         if (err < 0) {
4106                 pdata = dev_get_platdata(&pdev->dev);
4107                 if (pdata && pdata->is_rmii)
4108                         bp->phy_interface = PHY_INTERFACE_MODE_RMII;
4109                 else
4110                         bp->phy_interface = PHY_INTERFACE_MODE_MII;
4111         } else {
4112                 bp->phy_interface = err;
4113         }
4114
4115         /* IP specific init */
4116         err = init(pdev);
4117         if (err)
4118                 goto err_out_free_netdev;
4119
4120         err = macb_mii_init(bp);
4121         if (err)
4122                 goto err_out_free_netdev;
4123
4124         phydev = dev->phydev;
4125
4126         netif_carrier_off(dev);
4127
4128         err = register_netdev(dev);
4129         if (err) {
4130                 dev_err(&pdev->dev, "Cannot register net device, aborting.\n");
4131                 goto err_out_unregister_mdio;
4132         }
4133
4134         tasklet_init(&bp->hresp_err_tasklet, macb_hresp_error_task,
4135                      (unsigned long)bp);
4136
4137         phy_attached_info(phydev);
4138
4139         netdev_info(dev, "Cadence %s rev 0x%08x at 0x%08lx irq %d (%pM)\n",
4140                     macb_is_gem(bp) ? "GEM" : "MACB", macb_readl(bp, MID),
4141                     dev->base_addr, dev->irq, dev->dev_addr);
4142
4143         return 0;
4144
4145 err_out_unregister_mdio:
4146         phy_disconnect(dev->phydev);
4147         mdiobus_unregister(bp->mii_bus);
4148         of_node_put(bp->phy_node);
4149         if (np && of_phy_is_fixed_link(np))
4150                 of_phy_deregister_fixed_link(np);
4151         mdiobus_free(bp->mii_bus);
4152
4153 err_out_free_netdev:
4154         free_netdev(dev);
4155
4156 err_disable_clocks:
4157         clk_disable_unprepare(tx_clk);
4158         clk_disable_unprepare(hclk);
4159         clk_disable_unprepare(pclk);
4160         clk_disable_unprepare(rx_clk);
4161
4162         return err;
4163 }
4164
4165 static int macb_remove(struct platform_device *pdev)
4166 {
4167         struct net_device *dev;
4168         struct macb *bp;
4169         struct device_node *np = pdev->dev.of_node;
4170
4171         dev = platform_get_drvdata(pdev);
4172
4173         if (dev) {
4174                 bp = netdev_priv(dev);
4175                 if (dev->phydev)
4176                         phy_disconnect(dev->phydev);
4177                 mdiobus_unregister(bp->mii_bus);
4178                 if (np && of_phy_is_fixed_link(np))
4179                         of_phy_deregister_fixed_link(np);
4180                 dev->phydev = NULL;
4181                 mdiobus_free(bp->mii_bus);
4182
4183                 unregister_netdev(dev);
4184                 clk_disable_unprepare(bp->tx_clk);
4185                 clk_disable_unprepare(bp->hclk);
4186                 clk_disable_unprepare(bp->pclk);
4187                 clk_disable_unprepare(bp->rx_clk);
4188                 of_node_put(bp->phy_node);
4189                 free_netdev(dev);
4190         }
4191
4192         return 0;
4193 }
4194
4195 static int __maybe_unused macb_suspend(struct device *dev)
4196 {
4197         struct net_device *netdev = dev_get_drvdata(dev);
4198         struct macb *bp = netdev_priv(netdev);
4199
4200         netif_carrier_off(netdev);
4201         netif_device_detach(netdev);
4202
4203         if (bp->wol & MACB_WOL_ENABLED) {
4204                 macb_writel(bp, IER, MACB_BIT(WOL));
4205                 macb_writel(bp, WOL, MACB_BIT(MAG));
4206                 enable_irq_wake(bp->queues[0].irq);
4207         } else {
4208                 clk_disable_unprepare(bp->tx_clk);
4209                 clk_disable_unprepare(bp->hclk);
4210                 clk_disable_unprepare(bp->pclk);
4211                 clk_disable_unprepare(bp->rx_clk);
4212         }
4213
4214         return 0;
4215 }
4216
4217 static int __maybe_unused macb_resume(struct device *dev)
4218 {
4219         struct net_device *netdev = dev_get_drvdata(dev);
4220         struct macb *bp = netdev_priv(netdev);
4221
4222         if (bp->wol & MACB_WOL_ENABLED) {
4223                 macb_writel(bp, IDR, MACB_BIT(WOL));
4224                 macb_writel(bp, WOL, 0);
4225                 disable_irq_wake(bp->queues[0].irq);
4226         } else {
4227                 clk_prepare_enable(bp->pclk);
4228                 clk_prepare_enable(bp->hclk);
4229                 clk_prepare_enable(bp->tx_clk);
4230                 clk_prepare_enable(bp->rx_clk);
4231         }
4232
4233         netif_device_attach(netdev);
4234
4235         return 0;
4236 }
4237
4238 static SIMPLE_DEV_PM_OPS(macb_pm_ops, macb_suspend, macb_resume);
4239
4240 static struct platform_driver macb_driver = {
4241         .probe          = macb_probe,
4242         .remove         = macb_remove,
4243         .driver         = {
4244                 .name           = "macb",
4245                 .of_match_table = of_match_ptr(macb_dt_ids),
4246                 .pm     = &macb_pm_ops,
4247         },
4248 };
4249
4250 module_platform_driver(macb_driver);
4251
4252 MODULE_LICENSE("GPL");
4253 MODULE_DESCRIPTION("Cadence MACB/GEM Ethernet driver");
4254 MODULE_AUTHOR("Haavard Skinnemoen (Atmel)");
4255 MODULE_ALIAS("platform:macb");