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