1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Driver for Marvell PPv2 network controller for Armada 375 SoC.
4  *
5  * Copyright (C) 2014 Marvell
6  *
7  * Marcin Wojtas <mw@semihalf.com>
8  */
9 
10 #include <linux/acpi.h>
11 #include <linux/kernel.h>
12 #include <linux/netdevice.h>
13 #include <linux/etherdevice.h>
14 #include <linux/platform_device.h>
15 #include <linux/skbuff.h>
16 #include <linux/inetdevice.h>
17 #include <linux/mbus.h>
18 #include <linux/module.h>
19 #include <linux/mfd/syscon.h>
20 #include <linux/interrupt.h>
21 #include <linux/cpumask.h>
22 #include <linux/of.h>
23 #include <linux/of_irq.h>
24 #include <linux/of_mdio.h>
25 #include <linux/of_net.h>
26 #include <linux/of_address.h>
27 #include <linux/of_device.h>
28 #include <linux/phy.h>
29 #include <linux/phylink.h>
30 #include <linux/phy/phy.h>
31 #include <linux/ptp_classify.h>
32 #include <linux/clk.h>
33 #include <linux/hrtimer.h>
34 #include <linux/ktime.h>
35 #include <linux/regmap.h>
36 #include <uapi/linux/ppp_defs.h>
37 #include <net/ip.h>
38 #include <net/ipv6.h>
39 #include <net/tso.h>
40 #include <linux/bpf_trace.h>
41 
42 #include "mvpp2.h"
43 #include "mvpp2_prs.h"
44 #include "mvpp2_cls.h"
45 
46 enum mvpp2_bm_pool_log_num {
47 	MVPP2_BM_SHORT,
48 	MVPP2_BM_LONG,
49 	MVPP2_BM_JUMBO,
50 	MVPP2_BM_POOLS_NUM
51 };
52 
53 static struct {
54 	int pkt_size;
55 	int buf_num;
56 } mvpp2_pools[MVPP2_BM_POOLS_NUM];
57 
58 /* The prototype is added here to be used in start_dev when using ACPI. This
59  * will be removed once phylink is used for all modes (dt+ACPI).
60  */
61 static void mvpp2_acpi_start(struct mvpp2_port *port);
62 
63 /* Queue modes */
64 #define MVPP2_QDIST_SINGLE_MODE	0
65 #define MVPP2_QDIST_MULTI_MODE	1
66 
67 static int queue_mode = MVPP2_QDIST_MULTI_MODE;
68 
69 module_param(queue_mode, int, 0444);
70 MODULE_PARM_DESC(queue_mode, "Set queue_mode (single=0, multi=1)");
71 
72 /* Utility/helper methods */
73 
74 void mvpp2_write(struct mvpp2 *priv, u32 offset, u32 data)
75 {
76 	writel(data, priv->swth_base[0] + offset);
77 }
78 
79 u32 mvpp2_read(struct mvpp2 *priv, u32 offset)
80 {
81 	return readl(priv->swth_base[0] + offset);
82 }
83 
84 static u32 mvpp2_read_relaxed(struct mvpp2 *priv, u32 offset)
85 {
86 	return readl_relaxed(priv->swth_base[0] + offset);
87 }
88 
89 static inline u32 mvpp2_cpu_to_thread(struct mvpp2 *priv, int cpu)
90 {
91 	return cpu % priv->nthreads;
92 }
93 
94 static struct page_pool *
95 mvpp2_create_page_pool(struct device *dev, int num, int len,
96 		       enum dma_data_direction dma_dir)
97 {
98 	struct page_pool_params pp_params = {
99 		/* internal DMA mapping in page_pool */
100 		.flags = PP_FLAG_DMA_MAP | PP_FLAG_DMA_SYNC_DEV,
101 		.pool_size = num,
102 		.nid = NUMA_NO_NODE,
103 		.dev = dev,
104 		.dma_dir = dma_dir,
105 		.offset = MVPP2_SKB_HEADROOM,
106 		.max_len = len,
107 	};
108 
109 	return page_pool_create(&pp_params);
110 }
111 
112 /* These accessors should be used to access:
113  *
114  * - per-thread registers, where each thread has its own copy of the
115  *   register.
116  *
117  *   MVPP2_BM_VIRT_ALLOC_REG
118  *   MVPP2_BM_ADDR_HIGH_ALLOC
119  *   MVPP22_BM_ADDR_HIGH_RLS_REG
120  *   MVPP2_BM_VIRT_RLS_REG
121  *   MVPP2_ISR_RX_TX_CAUSE_REG
122  *   MVPP2_ISR_RX_TX_MASK_REG
123  *   MVPP2_TXQ_NUM_REG
124  *   MVPP2_AGGR_TXQ_UPDATE_REG
125  *   MVPP2_TXQ_RSVD_REQ_REG
126  *   MVPP2_TXQ_RSVD_RSLT_REG
127  *   MVPP2_TXQ_SENT_REG
128  *   MVPP2_RXQ_NUM_REG
129  *
130  * - global registers that must be accessed through a specific thread
131  *   window, because they are related to an access to a per-thread
132  *   register
133  *
134  *   MVPP2_BM_PHY_ALLOC_REG    (related to MVPP2_BM_VIRT_ALLOC_REG)
135  *   MVPP2_BM_PHY_RLS_REG      (related to MVPP2_BM_VIRT_RLS_REG)
136  *   MVPP2_RXQ_THRESH_REG      (related to MVPP2_RXQ_NUM_REG)
137  *   MVPP2_RXQ_DESC_ADDR_REG   (related to MVPP2_RXQ_NUM_REG)
138  *   MVPP2_RXQ_DESC_SIZE_REG   (related to MVPP2_RXQ_NUM_REG)
139  *   MVPP2_RXQ_INDEX_REG       (related to MVPP2_RXQ_NUM_REG)
140  *   MVPP2_TXQ_PENDING_REG     (related to MVPP2_TXQ_NUM_REG)
141  *   MVPP2_TXQ_DESC_ADDR_REG   (related to MVPP2_TXQ_NUM_REG)
142  *   MVPP2_TXQ_DESC_SIZE_REG   (related to MVPP2_TXQ_NUM_REG)
143  *   MVPP2_TXQ_INDEX_REG       (related to MVPP2_TXQ_NUM_REG)
144  *   MVPP2_TXQ_PENDING_REG     (related to MVPP2_TXQ_NUM_REG)
145  *   MVPP2_TXQ_PREF_BUF_REG    (related to MVPP2_TXQ_NUM_REG)
146  *   MVPP2_TXQ_PREF_BUF_REG    (related to MVPP2_TXQ_NUM_REG)
147  */
148 static void mvpp2_thread_write(struct mvpp2 *priv, unsigned int thread,
149 			       u32 offset, u32 data)
150 {
151 	writel(data, priv->swth_base[thread] + offset);
152 }
153 
154 static u32 mvpp2_thread_read(struct mvpp2 *priv, unsigned int thread,
155 			     u32 offset)
156 {
157 	return readl(priv->swth_base[thread] + offset);
158 }
159 
160 static void mvpp2_thread_write_relaxed(struct mvpp2 *priv, unsigned int thread,
161 				       u32 offset, u32 data)
162 {
163 	writel_relaxed(data, priv->swth_base[thread] + offset);
164 }
165 
166 static u32 mvpp2_thread_read_relaxed(struct mvpp2 *priv, unsigned int thread,
167 				     u32 offset)
168 {
169 	return readl_relaxed(priv->swth_base[thread] + offset);
170 }
171 
172 static dma_addr_t mvpp2_txdesc_dma_addr_get(struct mvpp2_port *port,
173 					    struct mvpp2_tx_desc *tx_desc)
174 {
175 	if (port->priv->hw_version == MVPP21)
176 		return le32_to_cpu(tx_desc->pp21.buf_dma_addr);
177 	else
178 		return le64_to_cpu(tx_desc->pp22.buf_dma_addr_ptp) &
179 		       MVPP2_DESC_DMA_MASK;
180 }
181 
182 static void mvpp2_txdesc_dma_addr_set(struct mvpp2_port *port,
183 				      struct mvpp2_tx_desc *tx_desc,
184 				      dma_addr_t dma_addr)
185 {
186 	dma_addr_t addr, offset;
187 
188 	addr = dma_addr & ~MVPP2_TX_DESC_ALIGN;
189 	offset = dma_addr & MVPP2_TX_DESC_ALIGN;
190 
191 	if (port->priv->hw_version == MVPP21) {
192 		tx_desc->pp21.buf_dma_addr = cpu_to_le32(addr);
193 		tx_desc->pp21.packet_offset = offset;
194 	} else {
195 		__le64 val = cpu_to_le64(addr);
196 
197 		tx_desc->pp22.buf_dma_addr_ptp &= ~cpu_to_le64(MVPP2_DESC_DMA_MASK);
198 		tx_desc->pp22.buf_dma_addr_ptp |= val;
199 		tx_desc->pp22.packet_offset = offset;
200 	}
201 }
202 
203 static size_t mvpp2_txdesc_size_get(struct mvpp2_port *port,
204 				    struct mvpp2_tx_desc *tx_desc)
205 {
206 	if (port->priv->hw_version == MVPP21)
207 		return le16_to_cpu(tx_desc->pp21.data_size);
208 	else
209 		return le16_to_cpu(tx_desc->pp22.data_size);
210 }
211 
212 static void mvpp2_txdesc_size_set(struct mvpp2_port *port,
213 				  struct mvpp2_tx_desc *tx_desc,
214 				  size_t size)
215 {
216 	if (port->priv->hw_version == MVPP21)
217 		tx_desc->pp21.data_size = cpu_to_le16(size);
218 	else
219 		tx_desc->pp22.data_size = cpu_to_le16(size);
220 }
221 
222 static void mvpp2_txdesc_txq_set(struct mvpp2_port *port,
223 				 struct mvpp2_tx_desc *tx_desc,
224 				 unsigned int txq)
225 {
226 	if (port->priv->hw_version == MVPP21)
227 		tx_desc->pp21.phys_txq = txq;
228 	else
229 		tx_desc->pp22.phys_txq = txq;
230 }
231 
232 static void mvpp2_txdesc_cmd_set(struct mvpp2_port *port,
233 				 struct mvpp2_tx_desc *tx_desc,
234 				 unsigned int command)
235 {
236 	if (port->priv->hw_version == MVPP21)
237 		tx_desc->pp21.command = cpu_to_le32(command);
238 	else
239 		tx_desc->pp22.command = cpu_to_le32(command);
240 }
241 
242 static unsigned int mvpp2_txdesc_offset_get(struct mvpp2_port *port,
243 					    struct mvpp2_tx_desc *tx_desc)
244 {
245 	if (port->priv->hw_version == MVPP21)
246 		return tx_desc->pp21.packet_offset;
247 	else
248 		return tx_desc->pp22.packet_offset;
249 }
250 
251 static dma_addr_t mvpp2_rxdesc_dma_addr_get(struct mvpp2_port *port,
252 					    struct mvpp2_rx_desc *rx_desc)
253 {
254 	if (port->priv->hw_version == MVPP21)
255 		return le32_to_cpu(rx_desc->pp21.buf_dma_addr);
256 	else
257 		return le64_to_cpu(rx_desc->pp22.buf_dma_addr_key_hash) &
258 		       MVPP2_DESC_DMA_MASK;
259 }
260 
261 static unsigned long mvpp2_rxdesc_cookie_get(struct mvpp2_port *port,
262 					     struct mvpp2_rx_desc *rx_desc)
263 {
264 	if (port->priv->hw_version == MVPP21)
265 		return le32_to_cpu(rx_desc->pp21.buf_cookie);
266 	else
267 		return le64_to_cpu(rx_desc->pp22.buf_cookie_misc) &
268 		       MVPP2_DESC_DMA_MASK;
269 }
270 
271 static size_t mvpp2_rxdesc_size_get(struct mvpp2_port *port,
272 				    struct mvpp2_rx_desc *rx_desc)
273 {
274 	if (port->priv->hw_version == MVPP21)
275 		return le16_to_cpu(rx_desc->pp21.data_size);
276 	else
277 		return le16_to_cpu(rx_desc->pp22.data_size);
278 }
279 
280 static u32 mvpp2_rxdesc_status_get(struct mvpp2_port *port,
281 				   struct mvpp2_rx_desc *rx_desc)
282 {
283 	if (port->priv->hw_version == MVPP21)
284 		return le32_to_cpu(rx_desc->pp21.status);
285 	else
286 		return le32_to_cpu(rx_desc->pp22.status);
287 }
288 
289 static void mvpp2_txq_inc_get(struct mvpp2_txq_pcpu *txq_pcpu)
290 {
291 	txq_pcpu->txq_get_index++;
292 	if (txq_pcpu->txq_get_index == txq_pcpu->size)
293 		txq_pcpu->txq_get_index = 0;
294 }
295 
296 static void mvpp2_txq_inc_put(struct mvpp2_port *port,
297 			      struct mvpp2_txq_pcpu *txq_pcpu,
298 			      void *data,
299 			      struct mvpp2_tx_desc *tx_desc,
300 			      enum mvpp2_tx_buf_type buf_type)
301 {
302 	struct mvpp2_txq_pcpu_buf *tx_buf =
303 		txq_pcpu->buffs + txq_pcpu->txq_put_index;
304 	tx_buf->type = buf_type;
305 	if (buf_type == MVPP2_TYPE_SKB)
306 		tx_buf->skb = data;
307 	else
308 		tx_buf->xdpf = data;
309 	tx_buf->size = mvpp2_txdesc_size_get(port, tx_desc);
310 	tx_buf->dma = mvpp2_txdesc_dma_addr_get(port, tx_desc) +
311 		mvpp2_txdesc_offset_get(port, tx_desc);
312 	txq_pcpu->txq_put_index++;
313 	if (txq_pcpu->txq_put_index == txq_pcpu->size)
314 		txq_pcpu->txq_put_index = 0;
315 }
316 
317 /* Get number of maximum RXQ */
318 static int mvpp2_get_nrxqs(struct mvpp2 *priv)
319 {
320 	unsigned int nrxqs;
321 
322 	if (priv->hw_version == MVPP22 && queue_mode == MVPP2_QDIST_SINGLE_MODE)
323 		return 1;
324 
325 	/* According to the PPv2.2 datasheet and our experiments on
326 	 * PPv2.1, RX queues have an allocation granularity of 4 (when
327 	 * more than a single one on PPv2.2).
328 	 * Round up to nearest multiple of 4.
329 	 */
330 	nrxqs = (num_possible_cpus() + 3) & ~0x3;
331 	if (nrxqs > MVPP2_PORT_MAX_RXQ)
332 		nrxqs = MVPP2_PORT_MAX_RXQ;
333 
334 	return nrxqs;
335 }
336 
337 /* Get number of physical egress port */
338 static inline int mvpp2_egress_port(struct mvpp2_port *port)
339 {
340 	return MVPP2_MAX_TCONT + port->id;
341 }
342 
343 /* Get number of physical TXQ */
344 static inline int mvpp2_txq_phys(int port, int txq)
345 {
346 	return (MVPP2_MAX_TCONT + port) * MVPP2_MAX_TXQ + txq;
347 }
348 
349 /* Returns a struct page if page_pool is set, otherwise a buffer */
350 static void *mvpp2_frag_alloc(const struct mvpp2_bm_pool *pool,
351 			      struct page_pool *page_pool)
352 {
353 	if (page_pool)
354 		return page_pool_dev_alloc_pages(page_pool);
355 
356 	if (likely(pool->frag_size <= PAGE_SIZE))
357 		return netdev_alloc_frag(pool->frag_size);
358 
359 	return kmalloc(pool->frag_size, GFP_ATOMIC);
360 }
361 
362 static void mvpp2_frag_free(const struct mvpp2_bm_pool *pool,
363 			    struct page_pool *page_pool, void *data)
364 {
365 	if (page_pool)
366 		page_pool_put_full_page(page_pool, virt_to_head_page(data), false);
367 	else if (likely(pool->frag_size <= PAGE_SIZE))
368 		skb_free_frag(data);
369 	else
370 		kfree(data);
371 }
372 
373 /* Buffer Manager configuration routines */
374 
375 /* Create pool */
376 static int mvpp2_bm_pool_create(struct device *dev, struct mvpp2 *priv,
377 				struct mvpp2_bm_pool *bm_pool, int size)
378 {
379 	u32 val;
380 
381 	/* Number of buffer pointers must be a multiple of 16, as per
382 	 * hardware constraints
383 	 */
384 	if (!IS_ALIGNED(size, 16))
385 		return -EINVAL;
386 
387 	/* PPv2.1 needs 8 bytes per buffer pointer, PPv2.2 needs 16
388 	 * bytes per buffer pointer
389 	 */
390 	if (priv->hw_version == MVPP21)
391 		bm_pool->size_bytes = 2 * sizeof(u32) * size;
392 	else
393 		bm_pool->size_bytes = 2 * sizeof(u64) * size;
394 
395 	bm_pool->virt_addr = dma_alloc_coherent(dev, bm_pool->size_bytes,
396 						&bm_pool->dma_addr,
397 						GFP_KERNEL);
398 	if (!bm_pool->virt_addr)
399 		return -ENOMEM;
400 
401 	if (!IS_ALIGNED((unsigned long)bm_pool->virt_addr,
402 			MVPP2_BM_POOL_PTR_ALIGN)) {
403 		dma_free_coherent(dev, bm_pool->size_bytes,
404 				  bm_pool->virt_addr, bm_pool->dma_addr);
405 		dev_err(dev, "BM pool %d is not %d bytes aligned\n",
406 			bm_pool->id, MVPP2_BM_POOL_PTR_ALIGN);
407 		return -ENOMEM;
408 	}
409 
410 	mvpp2_write(priv, MVPP2_BM_POOL_BASE_REG(bm_pool->id),
411 		    lower_32_bits(bm_pool->dma_addr));
412 	mvpp2_write(priv, MVPP2_BM_POOL_SIZE_REG(bm_pool->id), size);
413 
414 	val = mvpp2_read(priv, MVPP2_BM_POOL_CTRL_REG(bm_pool->id));
415 	val |= MVPP2_BM_START_MASK;
416 	mvpp2_write(priv, MVPP2_BM_POOL_CTRL_REG(bm_pool->id), val);
417 
418 	bm_pool->size = size;
419 	bm_pool->pkt_size = 0;
420 	bm_pool->buf_num = 0;
421 
422 	return 0;
423 }
424 
425 /* Set pool buffer size */
426 static void mvpp2_bm_pool_bufsize_set(struct mvpp2 *priv,
427 				      struct mvpp2_bm_pool *bm_pool,
428 				      int buf_size)
429 {
430 	u32 val;
431 
432 	bm_pool->buf_size = buf_size;
433 
434 	val = ALIGN(buf_size, 1 << MVPP2_POOL_BUF_SIZE_OFFSET);
435 	mvpp2_write(priv, MVPP2_POOL_BUF_SIZE_REG(bm_pool->id), val);
436 }
437 
438 static void mvpp2_bm_bufs_get_addrs(struct device *dev, struct mvpp2 *priv,
439 				    struct mvpp2_bm_pool *bm_pool,
440 				    dma_addr_t *dma_addr,
441 				    phys_addr_t *phys_addr)
442 {
443 	unsigned int thread = mvpp2_cpu_to_thread(priv, get_cpu());
444 
445 	*dma_addr = mvpp2_thread_read(priv, thread,
446 				      MVPP2_BM_PHY_ALLOC_REG(bm_pool->id));
447 	*phys_addr = mvpp2_thread_read(priv, thread, MVPP2_BM_VIRT_ALLOC_REG);
448 
449 	if (priv->hw_version == MVPP22) {
450 		u32 val;
451 		u32 dma_addr_highbits, phys_addr_highbits;
452 
453 		val = mvpp2_thread_read(priv, thread, MVPP22_BM_ADDR_HIGH_ALLOC);
454 		dma_addr_highbits = (val & MVPP22_BM_ADDR_HIGH_PHYS_MASK);
455 		phys_addr_highbits = (val & MVPP22_BM_ADDR_HIGH_VIRT_MASK) >>
456 			MVPP22_BM_ADDR_HIGH_VIRT_SHIFT;
457 
458 		if (sizeof(dma_addr_t) == 8)
459 			*dma_addr |= (u64)dma_addr_highbits << 32;
460 
461 		if (sizeof(phys_addr_t) == 8)
462 			*phys_addr |= (u64)phys_addr_highbits << 32;
463 	}
464 
465 	put_cpu();
466 }
467 
468 /* Free all buffers from the pool */
469 static void mvpp2_bm_bufs_free(struct device *dev, struct mvpp2 *priv,
470 			       struct mvpp2_bm_pool *bm_pool, int buf_num)
471 {
472 	struct page_pool *pp = NULL;
473 	int i;
474 
475 	if (buf_num > bm_pool->buf_num) {
476 		WARN(1, "Pool does not have so many bufs pool(%d) bufs(%d)\n",
477 		     bm_pool->id, buf_num);
478 		buf_num = bm_pool->buf_num;
479 	}
480 
481 	if (priv->percpu_pools)
482 		pp = priv->page_pool[bm_pool->id];
483 
484 	for (i = 0; i < buf_num; i++) {
485 		dma_addr_t buf_dma_addr;
486 		phys_addr_t buf_phys_addr;
487 		void *data;
488 
489 		mvpp2_bm_bufs_get_addrs(dev, priv, bm_pool,
490 					&buf_dma_addr, &buf_phys_addr);
491 
492 		if (!pp)
493 			dma_unmap_single(dev, buf_dma_addr,
494 					 bm_pool->buf_size, DMA_FROM_DEVICE);
495 
496 		data = (void *)phys_to_virt(buf_phys_addr);
497 		if (!data)
498 			break;
499 
500 		mvpp2_frag_free(bm_pool, pp, data);
501 	}
502 
503 	/* Update BM driver with number of buffers removed from pool */
504 	bm_pool->buf_num -= i;
505 }
506 
507 /* Check number of buffers in BM pool */
508 static int mvpp2_check_hw_buf_num(struct mvpp2 *priv, struct mvpp2_bm_pool *bm_pool)
509 {
510 	int buf_num = 0;
511 
512 	buf_num += mvpp2_read(priv, MVPP2_BM_POOL_PTRS_NUM_REG(bm_pool->id)) &
513 				    MVPP22_BM_POOL_PTRS_NUM_MASK;
514 	buf_num += mvpp2_read(priv, MVPP2_BM_BPPI_PTRS_NUM_REG(bm_pool->id)) &
515 				    MVPP2_BM_BPPI_PTR_NUM_MASK;
516 
517 	/* HW has one buffer ready which is not reflected in the counters */
518 	if (buf_num)
519 		buf_num += 1;
520 
521 	return buf_num;
522 }
523 
524 /* Cleanup pool */
525 static int mvpp2_bm_pool_destroy(struct device *dev, struct mvpp2 *priv,
526 				 struct mvpp2_bm_pool *bm_pool)
527 {
528 	int buf_num;
529 	u32 val;
530 
531 	buf_num = mvpp2_check_hw_buf_num(priv, bm_pool);
532 	mvpp2_bm_bufs_free(dev, priv, bm_pool, buf_num);
533 
534 	/* Check buffer counters after free */
535 	buf_num = mvpp2_check_hw_buf_num(priv, bm_pool);
536 	if (buf_num) {
537 		WARN(1, "cannot free all buffers in pool %d, buf_num left %d\n",
538 		     bm_pool->id, bm_pool->buf_num);
539 		return 0;
540 	}
541 
542 	val = mvpp2_read(priv, MVPP2_BM_POOL_CTRL_REG(bm_pool->id));
543 	val |= MVPP2_BM_STOP_MASK;
544 	mvpp2_write(priv, MVPP2_BM_POOL_CTRL_REG(bm_pool->id), val);
545 
546 	if (priv->percpu_pools) {
547 		page_pool_destroy(priv->page_pool[bm_pool->id]);
548 		priv->page_pool[bm_pool->id] = NULL;
549 	}
550 
551 	dma_free_coherent(dev, bm_pool->size_bytes,
552 			  bm_pool->virt_addr,
553 			  bm_pool->dma_addr);
554 	return 0;
555 }
556 
557 static int mvpp2_bm_pools_init(struct device *dev, struct mvpp2 *priv)
558 {
559 	int i, err, size, poolnum = MVPP2_BM_POOLS_NUM;
560 	struct mvpp2_bm_pool *bm_pool;
561 
562 	if (priv->percpu_pools)
563 		poolnum = mvpp2_get_nrxqs(priv) * 2;
564 
565 	/* Create all pools with maximum size */
566 	size = MVPP2_BM_POOL_SIZE_MAX;
567 	for (i = 0; i < poolnum; i++) {
568 		bm_pool = &priv->bm_pools[i];
569 		bm_pool->id = i;
570 		err = mvpp2_bm_pool_create(dev, priv, bm_pool, size);
571 		if (err)
572 			goto err_unroll_pools;
573 		mvpp2_bm_pool_bufsize_set(priv, bm_pool, 0);
574 	}
575 	return 0;
576 
577 err_unroll_pools:
578 	dev_err(dev, "failed to create BM pool %d, size %d\n", i, size);
579 	for (i = i - 1; i >= 0; i--)
580 		mvpp2_bm_pool_destroy(dev, priv, &priv->bm_pools[i]);
581 	return err;
582 }
583 
584 static int mvpp2_bm_init(struct device *dev, struct mvpp2 *priv)
585 {
586 	enum dma_data_direction dma_dir = DMA_FROM_DEVICE;
587 	int i, err, poolnum = MVPP2_BM_POOLS_NUM;
588 	struct mvpp2_port *port;
589 
590 	if (priv->percpu_pools) {
591 		for (i = 0; i < priv->port_count; i++) {
592 			port = priv->port_list[i];
593 			if (port->xdp_prog) {
594 				dma_dir = DMA_BIDIRECTIONAL;
595 				break;
596 			}
597 		}
598 
599 		poolnum = mvpp2_get_nrxqs(priv) * 2;
600 		for (i = 0; i < poolnum; i++) {
601 			/* the pool in use */
602 			int pn = i / (poolnum / 2);
603 
604 			priv->page_pool[i] =
605 				mvpp2_create_page_pool(dev,
606 						       mvpp2_pools[pn].buf_num,
607 						       mvpp2_pools[pn].pkt_size,
608 						       dma_dir);
609 			if (IS_ERR(priv->page_pool[i])) {
610 				int j;
611 
612 				for (j = 0; j < i; j++) {
613 					page_pool_destroy(priv->page_pool[j]);
614 					priv->page_pool[j] = NULL;
615 				}
616 				return PTR_ERR(priv->page_pool[i]);
617 			}
618 		}
619 	}
620 
621 	dev_info(dev, "using %d %s buffers\n", poolnum,
622 		 priv->percpu_pools ? "per-cpu" : "shared");
623 
624 	for (i = 0; i < poolnum; i++) {
625 		/* Mask BM all interrupts */
626 		mvpp2_write(priv, MVPP2_BM_INTR_MASK_REG(i), 0);
627 		/* Clear BM cause register */
628 		mvpp2_write(priv, MVPP2_BM_INTR_CAUSE_REG(i), 0);
629 	}
630 
631 	/* Allocate and initialize BM pools */
632 	priv->bm_pools = devm_kcalloc(dev, poolnum,
633 				      sizeof(*priv->bm_pools), GFP_KERNEL);
634 	if (!priv->bm_pools)
635 		return -ENOMEM;
636 
637 	err = mvpp2_bm_pools_init(dev, priv);
638 	if (err < 0)
639 		return err;
640 	return 0;
641 }
642 
643 static void mvpp2_setup_bm_pool(void)
644 {
645 	/* Short pool */
646 	mvpp2_pools[MVPP2_BM_SHORT].buf_num  = MVPP2_BM_SHORT_BUF_NUM;
647 	mvpp2_pools[MVPP2_BM_SHORT].pkt_size = MVPP2_BM_SHORT_PKT_SIZE;
648 
649 	/* Long pool */
650 	mvpp2_pools[MVPP2_BM_LONG].buf_num  = MVPP2_BM_LONG_BUF_NUM;
651 	mvpp2_pools[MVPP2_BM_LONG].pkt_size = MVPP2_BM_LONG_PKT_SIZE;
652 
653 	/* Jumbo pool */
654 	mvpp2_pools[MVPP2_BM_JUMBO].buf_num  = MVPP2_BM_JUMBO_BUF_NUM;
655 	mvpp2_pools[MVPP2_BM_JUMBO].pkt_size = MVPP2_BM_JUMBO_PKT_SIZE;
656 }
657 
658 /* Attach long pool to rxq */
659 static void mvpp2_rxq_long_pool_set(struct mvpp2_port *port,
660 				    int lrxq, int long_pool)
661 {
662 	u32 val, mask;
663 	int prxq;
664 
665 	/* Get queue physical ID */
666 	prxq = port->rxqs[lrxq]->id;
667 
668 	if (port->priv->hw_version == MVPP21)
669 		mask = MVPP21_RXQ_POOL_LONG_MASK;
670 	else
671 		mask = MVPP22_RXQ_POOL_LONG_MASK;
672 
673 	val = mvpp2_read(port->priv, MVPP2_RXQ_CONFIG_REG(prxq));
674 	val &= ~mask;
675 	val |= (long_pool << MVPP2_RXQ_POOL_LONG_OFFS) & mask;
676 	mvpp2_write(port->priv, MVPP2_RXQ_CONFIG_REG(prxq), val);
677 }
678 
679 /* Attach short pool to rxq */
680 static void mvpp2_rxq_short_pool_set(struct mvpp2_port *port,
681 				     int lrxq, int short_pool)
682 {
683 	u32 val, mask;
684 	int prxq;
685 
686 	/* Get queue physical ID */
687 	prxq = port->rxqs[lrxq]->id;
688 
689 	if (port->priv->hw_version == MVPP21)
690 		mask = MVPP21_RXQ_POOL_SHORT_MASK;
691 	else
692 		mask = MVPP22_RXQ_POOL_SHORT_MASK;
693 
694 	val = mvpp2_read(port->priv, MVPP2_RXQ_CONFIG_REG(prxq));
695 	val &= ~mask;
696 	val |= (short_pool << MVPP2_RXQ_POOL_SHORT_OFFS) & mask;
697 	mvpp2_write(port->priv, MVPP2_RXQ_CONFIG_REG(prxq), val);
698 }
699 
700 static void *mvpp2_buf_alloc(struct mvpp2_port *port,
701 			     struct mvpp2_bm_pool *bm_pool,
702 			     struct page_pool *page_pool,
703 			     dma_addr_t *buf_dma_addr,
704 			     phys_addr_t *buf_phys_addr,
705 			     gfp_t gfp_mask)
706 {
707 	dma_addr_t dma_addr;
708 	struct page *page;
709 	void *data;
710 
711 	data = mvpp2_frag_alloc(bm_pool, page_pool);
712 	if (!data)
713 		return NULL;
714 
715 	if (page_pool) {
716 		page = (struct page *)data;
717 		dma_addr = page_pool_get_dma_addr(page);
718 		data = page_to_virt(page);
719 	} else {
720 		dma_addr = dma_map_single(port->dev->dev.parent, data,
721 					  MVPP2_RX_BUF_SIZE(bm_pool->pkt_size),
722 					  DMA_FROM_DEVICE);
723 		if (unlikely(dma_mapping_error(port->dev->dev.parent, dma_addr))) {
724 			mvpp2_frag_free(bm_pool, NULL, data);
725 			return NULL;
726 		}
727 	}
728 	*buf_dma_addr = dma_addr;
729 	*buf_phys_addr = virt_to_phys(data);
730 
731 	return data;
732 }
733 
734 /* Release buffer to BM */
735 static inline void mvpp2_bm_pool_put(struct mvpp2_port *port, int pool,
736 				     dma_addr_t buf_dma_addr,
737 				     phys_addr_t buf_phys_addr)
738 {
739 	unsigned int thread = mvpp2_cpu_to_thread(port->priv, get_cpu());
740 	unsigned long flags = 0;
741 
742 	if (test_bit(thread, &port->priv->lock_map))
743 		spin_lock_irqsave(&port->bm_lock[thread], flags);
744 
745 	if (port->priv->hw_version == MVPP22) {
746 		u32 val = 0;
747 
748 		if (sizeof(dma_addr_t) == 8)
749 			val |= upper_32_bits(buf_dma_addr) &
750 				MVPP22_BM_ADDR_HIGH_PHYS_RLS_MASK;
751 
752 		if (sizeof(phys_addr_t) == 8)
753 			val |= (upper_32_bits(buf_phys_addr)
754 				<< MVPP22_BM_ADDR_HIGH_VIRT_RLS_SHIFT) &
755 				MVPP22_BM_ADDR_HIGH_VIRT_RLS_MASK;
756 
757 		mvpp2_thread_write_relaxed(port->priv, thread,
758 					   MVPP22_BM_ADDR_HIGH_RLS_REG, val);
759 	}
760 
761 	/* MVPP2_BM_VIRT_RLS_REG is not interpreted by HW, and simply
762 	 * returned in the "cookie" field of the RX
763 	 * descriptor. Instead of storing the virtual address, we
764 	 * store the physical address
765 	 */
766 	mvpp2_thread_write_relaxed(port->priv, thread,
767 				   MVPP2_BM_VIRT_RLS_REG, buf_phys_addr);
768 	mvpp2_thread_write_relaxed(port->priv, thread,
769 				   MVPP2_BM_PHY_RLS_REG(pool), buf_dma_addr);
770 
771 	if (test_bit(thread, &port->priv->lock_map))
772 		spin_unlock_irqrestore(&port->bm_lock[thread], flags);
773 
774 	put_cpu();
775 }
776 
777 /* Allocate buffers for the pool */
778 static int mvpp2_bm_bufs_add(struct mvpp2_port *port,
779 			     struct mvpp2_bm_pool *bm_pool, int buf_num)
780 {
781 	int i, buf_size, total_size;
782 	dma_addr_t dma_addr;
783 	phys_addr_t phys_addr;
784 	struct page_pool *pp = NULL;
785 	void *buf;
786 
787 	if (port->priv->percpu_pools &&
788 	    bm_pool->pkt_size > MVPP2_BM_LONG_PKT_SIZE) {
789 		netdev_err(port->dev,
790 			   "attempted to use jumbo frames with per-cpu pools");
791 		return 0;
792 	}
793 
794 	buf_size = MVPP2_RX_BUF_SIZE(bm_pool->pkt_size);
795 	total_size = MVPP2_RX_TOTAL_SIZE(buf_size);
796 
797 	if (buf_num < 0 ||
798 	    (buf_num + bm_pool->buf_num > bm_pool->size)) {
799 		netdev_err(port->dev,
800 			   "cannot allocate %d buffers for pool %d\n",
801 			   buf_num, bm_pool->id);
802 		return 0;
803 	}
804 
805 	if (port->priv->percpu_pools)
806 		pp = port->priv->page_pool[bm_pool->id];
807 	for (i = 0; i < buf_num; i++) {
808 		buf = mvpp2_buf_alloc(port, bm_pool, pp, &dma_addr,
809 				      &phys_addr, GFP_KERNEL);
810 		if (!buf)
811 			break;
812 
813 		mvpp2_bm_pool_put(port, bm_pool->id, dma_addr,
814 				  phys_addr);
815 	}
816 
817 	/* Update BM driver with number of buffers added to pool */
818 	bm_pool->buf_num += i;
819 
820 	netdev_dbg(port->dev,
821 		   "pool %d: pkt_size=%4d, buf_size=%4d, total_size=%4d\n",
822 		   bm_pool->id, bm_pool->pkt_size, buf_size, total_size);
823 
824 	netdev_dbg(port->dev,
825 		   "pool %d: %d of %d buffers added\n",
826 		   bm_pool->id, i, buf_num);
827 	return i;
828 }
829 
830 /* Notify the driver that BM pool is being used as specific type and return the
831  * pool pointer on success
832  */
833 static struct mvpp2_bm_pool *
834 mvpp2_bm_pool_use(struct mvpp2_port *port, unsigned pool, int pkt_size)
835 {
836 	struct mvpp2_bm_pool *new_pool = &port->priv->bm_pools[pool];
837 	int num;
838 
839 	if ((port->priv->percpu_pools && pool > mvpp2_get_nrxqs(port->priv) * 2) ||
840 	    (!port->priv->percpu_pools && pool >= MVPP2_BM_POOLS_NUM)) {
841 		netdev_err(port->dev, "Invalid pool %d\n", pool);
842 		return NULL;
843 	}
844 
845 	/* Allocate buffers in case BM pool is used as long pool, but packet
846 	 * size doesn't match MTU or BM pool hasn't being used yet
847 	 */
848 	if (new_pool->pkt_size == 0) {
849 		int pkts_num;
850 
851 		/* Set default buffer number or free all the buffers in case
852 		 * the pool is not empty
853 		 */
854 		pkts_num = new_pool->buf_num;
855 		if (pkts_num == 0) {
856 			if (port->priv->percpu_pools) {
857 				if (pool < port->nrxqs)
858 					pkts_num = mvpp2_pools[MVPP2_BM_SHORT].buf_num;
859 				else
860 					pkts_num = mvpp2_pools[MVPP2_BM_LONG].buf_num;
861 			} else {
862 				pkts_num = mvpp2_pools[pool].buf_num;
863 			}
864 		} else {
865 			mvpp2_bm_bufs_free(port->dev->dev.parent,
866 					   port->priv, new_pool, pkts_num);
867 		}
868 
869 		new_pool->pkt_size = pkt_size;
870 		new_pool->frag_size =
871 			SKB_DATA_ALIGN(MVPP2_RX_BUF_SIZE(pkt_size)) +
872 			MVPP2_SKB_SHINFO_SIZE;
873 
874 		/* Allocate buffers for this pool */
875 		num = mvpp2_bm_bufs_add(port, new_pool, pkts_num);
876 		if (num != pkts_num) {
877 			WARN(1, "pool %d: %d of %d allocated\n",
878 			     new_pool->id, num, pkts_num);
879 			return NULL;
880 		}
881 	}
882 
883 	mvpp2_bm_pool_bufsize_set(port->priv, new_pool,
884 				  MVPP2_RX_BUF_SIZE(new_pool->pkt_size));
885 
886 	return new_pool;
887 }
888 
889 static struct mvpp2_bm_pool *
890 mvpp2_bm_pool_use_percpu(struct mvpp2_port *port, int type,
891 			 unsigned int pool, int pkt_size)
892 {
893 	struct mvpp2_bm_pool *new_pool = &port->priv->bm_pools[pool];
894 	int num;
895 
896 	if (pool > port->nrxqs * 2) {
897 		netdev_err(port->dev, "Invalid pool %d\n", pool);
898 		return NULL;
899 	}
900 
901 	/* Allocate buffers in case BM pool is used as long pool, but packet
902 	 * size doesn't match MTU or BM pool hasn't being used yet
903 	 */
904 	if (new_pool->pkt_size == 0) {
905 		int pkts_num;
906 
907 		/* Set default buffer number or free all the buffers in case
908 		 * the pool is not empty
909 		 */
910 		pkts_num = new_pool->buf_num;
911 		if (pkts_num == 0)
912 			pkts_num = mvpp2_pools[type].buf_num;
913 		else
914 			mvpp2_bm_bufs_free(port->dev->dev.parent,
915 					   port->priv, new_pool, pkts_num);
916 
917 		new_pool->pkt_size = pkt_size;
918 		new_pool->frag_size =
919 			SKB_DATA_ALIGN(MVPP2_RX_BUF_SIZE(pkt_size)) +
920 			MVPP2_SKB_SHINFO_SIZE;
921 
922 		/* Allocate buffers for this pool */
923 		num = mvpp2_bm_bufs_add(port, new_pool, pkts_num);
924 		if (num != pkts_num) {
925 			WARN(1, "pool %d: %d of %d allocated\n",
926 			     new_pool->id, num, pkts_num);
927 			return NULL;
928 		}
929 	}
930 
931 	mvpp2_bm_pool_bufsize_set(port->priv, new_pool,
932 				  MVPP2_RX_BUF_SIZE(new_pool->pkt_size));
933 
934 	return new_pool;
935 }
936 
937 /* Initialize pools for swf, shared buffers variant */
938 static int mvpp2_swf_bm_pool_init_shared(struct mvpp2_port *port)
939 {
940 	enum mvpp2_bm_pool_log_num long_log_pool, short_log_pool;
941 	int rxq;
942 
943 	/* If port pkt_size is higher than 1518B:
944 	 * HW Long pool - SW Jumbo pool, HW Short pool - SW Long pool
945 	 * else: HW Long pool - SW Long pool, HW Short pool - SW Short pool
946 	 */
947 	if (port->pkt_size > MVPP2_BM_LONG_PKT_SIZE) {
948 		long_log_pool = MVPP2_BM_JUMBO;
949 		short_log_pool = MVPP2_BM_LONG;
950 	} else {
951 		long_log_pool = MVPP2_BM_LONG;
952 		short_log_pool = MVPP2_BM_SHORT;
953 	}
954 
955 	if (!port->pool_long) {
956 		port->pool_long =
957 			mvpp2_bm_pool_use(port, long_log_pool,
958 					  mvpp2_pools[long_log_pool].pkt_size);
959 		if (!port->pool_long)
960 			return -ENOMEM;
961 
962 		port->pool_long->port_map |= BIT(port->id);
963 
964 		for (rxq = 0; rxq < port->nrxqs; rxq++)
965 			mvpp2_rxq_long_pool_set(port, rxq, port->pool_long->id);
966 	}
967 
968 	if (!port->pool_short) {
969 		port->pool_short =
970 			mvpp2_bm_pool_use(port, short_log_pool,
971 					  mvpp2_pools[short_log_pool].pkt_size);
972 		if (!port->pool_short)
973 			return -ENOMEM;
974 
975 		port->pool_short->port_map |= BIT(port->id);
976 
977 		for (rxq = 0; rxq < port->nrxqs; rxq++)
978 			mvpp2_rxq_short_pool_set(port, rxq,
979 						 port->pool_short->id);
980 	}
981 
982 	return 0;
983 }
984 
985 /* Initialize pools for swf, percpu buffers variant */
986 static int mvpp2_swf_bm_pool_init_percpu(struct mvpp2_port *port)
987 {
988 	struct mvpp2_bm_pool *bm_pool;
989 	int i;
990 
991 	for (i = 0; i < port->nrxqs; i++) {
992 		bm_pool = mvpp2_bm_pool_use_percpu(port, MVPP2_BM_SHORT, i,
993 						   mvpp2_pools[MVPP2_BM_SHORT].pkt_size);
994 		if (!bm_pool)
995 			return -ENOMEM;
996 
997 		bm_pool->port_map |= BIT(port->id);
998 		mvpp2_rxq_short_pool_set(port, i, bm_pool->id);
999 	}
1000 
1001 	for (i = 0; i < port->nrxqs; i++) {
1002 		bm_pool = mvpp2_bm_pool_use_percpu(port, MVPP2_BM_LONG, i + port->nrxqs,
1003 						   mvpp2_pools[MVPP2_BM_LONG].pkt_size);
1004 		if (!bm_pool)
1005 			return -ENOMEM;
1006 
1007 		bm_pool->port_map |= BIT(port->id);
1008 		mvpp2_rxq_long_pool_set(port, i, bm_pool->id);
1009 	}
1010 
1011 	port->pool_long = NULL;
1012 	port->pool_short = NULL;
1013 
1014 	return 0;
1015 }
1016 
1017 static int mvpp2_swf_bm_pool_init(struct mvpp2_port *port)
1018 {
1019 	if (port->priv->percpu_pools)
1020 		return mvpp2_swf_bm_pool_init_percpu(port);
1021 	else
1022 		return mvpp2_swf_bm_pool_init_shared(port);
1023 }
1024 
1025 static void mvpp2_set_hw_csum(struct mvpp2_port *port,
1026 			      enum mvpp2_bm_pool_log_num new_long_pool)
1027 {
1028 	const netdev_features_t csums = NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM;
1029 
1030 	/* Update L4 checksum when jumbo enable/disable on port.
1031 	 * Only port 0 supports hardware checksum offload due to
1032 	 * the Tx FIFO size limitation.
1033 	 * Also, don't set NETIF_F_HW_CSUM because L3_offset in TX descriptor
1034 	 * has 7 bits, so the maximum L3 offset is 128.
1035 	 */
1036 	if (new_long_pool == MVPP2_BM_JUMBO && port->id != 0) {
1037 		port->dev->features &= ~csums;
1038 		port->dev->hw_features &= ~csums;
1039 	} else {
1040 		port->dev->features |= csums;
1041 		port->dev->hw_features |= csums;
1042 	}
1043 }
1044 
1045 static int mvpp2_bm_update_mtu(struct net_device *dev, int mtu)
1046 {
1047 	struct mvpp2_port *port = netdev_priv(dev);
1048 	enum mvpp2_bm_pool_log_num new_long_pool;
1049 	int pkt_size = MVPP2_RX_PKT_SIZE(mtu);
1050 
1051 	if (port->priv->percpu_pools)
1052 		goto out_set;
1053 
1054 	/* If port MTU is higher than 1518B:
1055 	 * HW Long pool - SW Jumbo pool, HW Short pool - SW Long pool
1056 	 * else: HW Long pool - SW Long pool, HW Short pool - SW Short pool
1057 	 */
1058 	if (pkt_size > MVPP2_BM_LONG_PKT_SIZE)
1059 		new_long_pool = MVPP2_BM_JUMBO;
1060 	else
1061 		new_long_pool = MVPP2_BM_LONG;
1062 
1063 	if (new_long_pool != port->pool_long->id) {
1064 		/* Remove port from old short & long pool */
1065 		port->pool_long = mvpp2_bm_pool_use(port, port->pool_long->id,
1066 						    port->pool_long->pkt_size);
1067 		port->pool_long->port_map &= ~BIT(port->id);
1068 		port->pool_long = NULL;
1069 
1070 		port->pool_short = mvpp2_bm_pool_use(port, port->pool_short->id,
1071 						     port->pool_short->pkt_size);
1072 		port->pool_short->port_map &= ~BIT(port->id);
1073 		port->pool_short = NULL;
1074 
1075 		port->pkt_size =  pkt_size;
1076 
1077 		/* Add port to new short & long pool */
1078 		mvpp2_swf_bm_pool_init(port);
1079 
1080 		mvpp2_set_hw_csum(port, new_long_pool);
1081 	}
1082 
1083 out_set:
1084 	dev->mtu = mtu;
1085 	dev->wanted_features = dev->features;
1086 
1087 	netdev_update_features(dev);
1088 	return 0;
1089 }
1090 
1091 static inline void mvpp2_interrupts_enable(struct mvpp2_port *port)
1092 {
1093 	int i, sw_thread_mask = 0;
1094 
1095 	for (i = 0; i < port->nqvecs; i++)
1096 		sw_thread_mask |= port->qvecs[i].sw_thread_mask;
1097 
1098 	mvpp2_write(port->priv, MVPP2_ISR_ENABLE_REG(port->id),
1099 		    MVPP2_ISR_ENABLE_INTERRUPT(sw_thread_mask));
1100 }
1101 
1102 static inline void mvpp2_interrupts_disable(struct mvpp2_port *port)
1103 {
1104 	int i, sw_thread_mask = 0;
1105 
1106 	for (i = 0; i < port->nqvecs; i++)
1107 		sw_thread_mask |= port->qvecs[i].sw_thread_mask;
1108 
1109 	mvpp2_write(port->priv, MVPP2_ISR_ENABLE_REG(port->id),
1110 		    MVPP2_ISR_DISABLE_INTERRUPT(sw_thread_mask));
1111 }
1112 
1113 static inline void mvpp2_qvec_interrupt_enable(struct mvpp2_queue_vector *qvec)
1114 {
1115 	struct mvpp2_port *port = qvec->port;
1116 
1117 	mvpp2_write(port->priv, MVPP2_ISR_ENABLE_REG(port->id),
1118 		    MVPP2_ISR_ENABLE_INTERRUPT(qvec->sw_thread_mask));
1119 }
1120 
1121 static inline void mvpp2_qvec_interrupt_disable(struct mvpp2_queue_vector *qvec)
1122 {
1123 	struct mvpp2_port *port = qvec->port;
1124 
1125 	mvpp2_write(port->priv, MVPP2_ISR_ENABLE_REG(port->id),
1126 		    MVPP2_ISR_DISABLE_INTERRUPT(qvec->sw_thread_mask));
1127 }
1128 
1129 /* Mask the current thread's Rx/Tx interrupts
1130  * Called by on_each_cpu(), guaranteed to run with migration disabled,
1131  * using smp_processor_id() is OK.
1132  */
1133 static void mvpp2_interrupts_mask(void *arg)
1134 {
1135 	struct mvpp2_port *port = arg;
1136 
1137 	/* If the thread isn't used, don't do anything */
1138 	if (smp_processor_id() > port->priv->nthreads)
1139 		return;
1140 
1141 	mvpp2_thread_write(port->priv,
1142 			   mvpp2_cpu_to_thread(port->priv, smp_processor_id()),
1143 			   MVPP2_ISR_RX_TX_MASK_REG(port->id), 0);
1144 }
1145 
1146 /* Unmask the current thread's Rx/Tx interrupts.
1147  * Called by on_each_cpu(), guaranteed to run with migration disabled,
1148  * using smp_processor_id() is OK.
1149  */
1150 static void mvpp2_interrupts_unmask(void *arg)
1151 {
1152 	struct mvpp2_port *port = arg;
1153 	u32 val;
1154 
1155 	/* If the thread isn't used, don't do anything */
1156 	if (smp_processor_id() > port->priv->nthreads)
1157 		return;
1158 
1159 	val = MVPP2_CAUSE_MISC_SUM_MASK |
1160 		MVPP2_CAUSE_RXQ_OCCUP_DESC_ALL_MASK(port->priv->hw_version);
1161 	if (port->has_tx_irqs)
1162 		val |= MVPP2_CAUSE_TXQ_OCCUP_DESC_ALL_MASK;
1163 
1164 	mvpp2_thread_write(port->priv,
1165 			   mvpp2_cpu_to_thread(port->priv, smp_processor_id()),
1166 			   MVPP2_ISR_RX_TX_MASK_REG(port->id), val);
1167 }
1168 
1169 static void
1170 mvpp2_shared_interrupt_mask_unmask(struct mvpp2_port *port, bool mask)
1171 {
1172 	u32 val;
1173 	int i;
1174 
1175 	if (port->priv->hw_version != MVPP22)
1176 		return;
1177 
1178 	if (mask)
1179 		val = 0;
1180 	else
1181 		val = MVPP2_CAUSE_RXQ_OCCUP_DESC_ALL_MASK(MVPP22);
1182 
1183 	for (i = 0; i < port->nqvecs; i++) {
1184 		struct mvpp2_queue_vector *v = port->qvecs + i;
1185 
1186 		if (v->type != MVPP2_QUEUE_VECTOR_SHARED)
1187 			continue;
1188 
1189 		mvpp2_thread_write(port->priv, v->sw_thread_id,
1190 				   MVPP2_ISR_RX_TX_MASK_REG(port->id), val);
1191 	}
1192 }
1193 
1194 /* Only GOP port 0 has an XLG MAC */
1195 static bool mvpp2_port_supports_xlg(struct mvpp2_port *port)
1196 {
1197 	return port->gop_id == 0;
1198 }
1199 
1200 static bool mvpp2_port_supports_rgmii(struct mvpp2_port *port)
1201 {
1202 	return !(port->priv->hw_version == MVPP22 && port->gop_id == 0);
1203 }
1204 
1205 /* Port configuration routines */
1206 static bool mvpp2_is_xlg(phy_interface_t interface)
1207 {
1208 	return interface == PHY_INTERFACE_MODE_10GBASER ||
1209 	       interface == PHY_INTERFACE_MODE_XAUI;
1210 }
1211 
1212 static void mvpp2_modify(void __iomem *ptr, u32 mask, u32 set)
1213 {
1214 	u32 old, val;
1215 
1216 	old = val = readl(ptr);
1217 	val &= ~mask;
1218 	val |= set;
1219 	if (old != val)
1220 		writel(val, ptr);
1221 }
1222 
1223 static void mvpp22_gop_init_rgmii(struct mvpp2_port *port)
1224 {
1225 	struct mvpp2 *priv = port->priv;
1226 	u32 val;
1227 
1228 	regmap_read(priv->sysctrl_base, GENCONF_PORT_CTRL0, &val);
1229 	val |= GENCONF_PORT_CTRL0_BUS_WIDTH_SELECT;
1230 	regmap_write(priv->sysctrl_base, GENCONF_PORT_CTRL0, val);
1231 
1232 	regmap_read(priv->sysctrl_base, GENCONF_CTRL0, &val);
1233 	if (port->gop_id == 2)
1234 		val |= GENCONF_CTRL0_PORT0_RGMII | GENCONF_CTRL0_PORT1_RGMII;
1235 	else if (port->gop_id == 3)
1236 		val |= GENCONF_CTRL0_PORT1_RGMII_MII;
1237 	regmap_write(priv->sysctrl_base, GENCONF_CTRL0, val);
1238 }
1239 
1240 static void mvpp22_gop_init_sgmii(struct mvpp2_port *port)
1241 {
1242 	struct mvpp2 *priv = port->priv;
1243 	u32 val;
1244 
1245 	regmap_read(priv->sysctrl_base, GENCONF_PORT_CTRL0, &val);
1246 	val |= GENCONF_PORT_CTRL0_BUS_WIDTH_SELECT |
1247 	       GENCONF_PORT_CTRL0_RX_DATA_SAMPLE;
1248 	regmap_write(priv->sysctrl_base, GENCONF_PORT_CTRL0, val);
1249 
1250 	if (port->gop_id > 1) {
1251 		regmap_read(priv->sysctrl_base, GENCONF_CTRL0, &val);
1252 		if (port->gop_id == 2)
1253 			val &= ~GENCONF_CTRL0_PORT0_RGMII;
1254 		else if (port->gop_id == 3)
1255 			val &= ~GENCONF_CTRL0_PORT1_RGMII_MII;
1256 		regmap_write(priv->sysctrl_base, GENCONF_CTRL0, val);
1257 	}
1258 }
1259 
1260 static void mvpp22_gop_init_10gkr(struct mvpp2_port *port)
1261 {
1262 	struct mvpp2 *priv = port->priv;
1263 	void __iomem *mpcs = priv->iface_base + MVPP22_MPCS_BASE(port->gop_id);
1264 	void __iomem *xpcs = priv->iface_base + MVPP22_XPCS_BASE(port->gop_id);
1265 	u32 val;
1266 
1267 	val = readl(xpcs + MVPP22_XPCS_CFG0);
1268 	val &= ~(MVPP22_XPCS_CFG0_PCS_MODE(0x3) |
1269 		 MVPP22_XPCS_CFG0_ACTIVE_LANE(0x3));
1270 	val |= MVPP22_XPCS_CFG0_ACTIVE_LANE(2);
1271 	writel(val, xpcs + MVPP22_XPCS_CFG0);
1272 
1273 	val = readl(mpcs + MVPP22_MPCS_CTRL);
1274 	val &= ~MVPP22_MPCS_CTRL_FWD_ERR_CONN;
1275 	writel(val, mpcs + MVPP22_MPCS_CTRL);
1276 
1277 	val = readl(mpcs + MVPP22_MPCS_CLK_RESET);
1278 	val &= ~MVPP22_MPCS_CLK_RESET_DIV_RATIO(0x7);
1279 	val |= MVPP22_MPCS_CLK_RESET_DIV_RATIO(1);
1280 	writel(val, mpcs + MVPP22_MPCS_CLK_RESET);
1281 }
1282 
1283 static int mvpp22_gop_init(struct mvpp2_port *port)
1284 {
1285 	struct mvpp2 *priv = port->priv;
1286 	u32 val;
1287 
1288 	if (!priv->sysctrl_base)
1289 		return 0;
1290 
1291 	switch (port->phy_interface) {
1292 	case PHY_INTERFACE_MODE_RGMII:
1293 	case PHY_INTERFACE_MODE_RGMII_ID:
1294 	case PHY_INTERFACE_MODE_RGMII_RXID:
1295 	case PHY_INTERFACE_MODE_RGMII_TXID:
1296 		if (!mvpp2_port_supports_rgmii(port))
1297 			goto invalid_conf;
1298 		mvpp22_gop_init_rgmii(port);
1299 		break;
1300 	case PHY_INTERFACE_MODE_SGMII:
1301 	case PHY_INTERFACE_MODE_1000BASEX:
1302 	case PHY_INTERFACE_MODE_2500BASEX:
1303 		mvpp22_gop_init_sgmii(port);
1304 		break;
1305 	case PHY_INTERFACE_MODE_10GBASER:
1306 		if (!mvpp2_port_supports_xlg(port))
1307 			goto invalid_conf;
1308 		mvpp22_gop_init_10gkr(port);
1309 		break;
1310 	default:
1311 		goto unsupported_conf;
1312 	}
1313 
1314 	regmap_read(priv->sysctrl_base, GENCONF_PORT_CTRL1, &val);
1315 	val |= GENCONF_PORT_CTRL1_RESET(port->gop_id) |
1316 	       GENCONF_PORT_CTRL1_EN(port->gop_id);
1317 	regmap_write(priv->sysctrl_base, GENCONF_PORT_CTRL1, val);
1318 
1319 	regmap_read(priv->sysctrl_base, GENCONF_PORT_CTRL0, &val);
1320 	val |= GENCONF_PORT_CTRL0_CLK_DIV_PHASE_CLR;
1321 	regmap_write(priv->sysctrl_base, GENCONF_PORT_CTRL0, val);
1322 
1323 	regmap_read(priv->sysctrl_base, GENCONF_SOFT_RESET1, &val);
1324 	val |= GENCONF_SOFT_RESET1_GOP;
1325 	regmap_write(priv->sysctrl_base, GENCONF_SOFT_RESET1, val);
1326 
1327 unsupported_conf:
1328 	return 0;
1329 
1330 invalid_conf:
1331 	netdev_err(port->dev, "Invalid port configuration\n");
1332 	return -EINVAL;
1333 }
1334 
1335 static void mvpp22_gop_unmask_irq(struct mvpp2_port *port)
1336 {
1337 	u32 val;
1338 
1339 	if (phy_interface_mode_is_rgmii(port->phy_interface) ||
1340 	    phy_interface_mode_is_8023z(port->phy_interface) ||
1341 	    port->phy_interface == PHY_INTERFACE_MODE_SGMII) {
1342 		/* Enable the GMAC link status irq for this port */
1343 		val = readl(port->base + MVPP22_GMAC_INT_SUM_MASK);
1344 		val |= MVPP22_GMAC_INT_SUM_MASK_LINK_STAT;
1345 		writel(val, port->base + MVPP22_GMAC_INT_SUM_MASK);
1346 	}
1347 
1348 	if (mvpp2_port_supports_xlg(port)) {
1349 		/* Enable the XLG/GIG irqs for this port */
1350 		val = readl(port->base + MVPP22_XLG_EXT_INT_MASK);
1351 		if (mvpp2_is_xlg(port->phy_interface))
1352 			val |= MVPP22_XLG_EXT_INT_MASK_XLG;
1353 		else
1354 			val |= MVPP22_XLG_EXT_INT_MASK_GIG;
1355 		writel(val, port->base + MVPP22_XLG_EXT_INT_MASK);
1356 	}
1357 }
1358 
1359 static void mvpp22_gop_mask_irq(struct mvpp2_port *port)
1360 {
1361 	u32 val;
1362 
1363 	if (mvpp2_port_supports_xlg(port)) {
1364 		val = readl(port->base + MVPP22_XLG_EXT_INT_MASK);
1365 		val &= ~(MVPP22_XLG_EXT_INT_MASK_XLG |
1366 			 MVPP22_XLG_EXT_INT_MASK_GIG);
1367 		writel(val, port->base + MVPP22_XLG_EXT_INT_MASK);
1368 	}
1369 
1370 	if (phy_interface_mode_is_rgmii(port->phy_interface) ||
1371 	    phy_interface_mode_is_8023z(port->phy_interface) ||
1372 	    port->phy_interface == PHY_INTERFACE_MODE_SGMII) {
1373 		val = readl(port->base + MVPP22_GMAC_INT_SUM_MASK);
1374 		val &= ~MVPP22_GMAC_INT_SUM_MASK_LINK_STAT;
1375 		writel(val, port->base + MVPP22_GMAC_INT_SUM_MASK);
1376 	}
1377 }
1378 
1379 static void mvpp22_gop_setup_irq(struct mvpp2_port *port)
1380 {
1381 	u32 val;
1382 
1383 	mvpp2_modify(port->base + MVPP22_GMAC_INT_SUM_MASK,
1384 		     MVPP22_GMAC_INT_SUM_MASK_PTP,
1385 		     MVPP22_GMAC_INT_SUM_MASK_PTP);
1386 
1387 	if (port->phylink ||
1388 	    phy_interface_mode_is_rgmii(port->phy_interface) ||
1389 	    phy_interface_mode_is_8023z(port->phy_interface) ||
1390 	    port->phy_interface == PHY_INTERFACE_MODE_SGMII) {
1391 		val = readl(port->base + MVPP22_GMAC_INT_MASK);
1392 		val |= MVPP22_GMAC_INT_MASK_LINK_STAT;
1393 		writel(val, port->base + MVPP22_GMAC_INT_MASK);
1394 	}
1395 
1396 	if (mvpp2_port_supports_xlg(port)) {
1397 		val = readl(port->base + MVPP22_XLG_INT_MASK);
1398 		val |= MVPP22_XLG_INT_MASK_LINK;
1399 		writel(val, port->base + MVPP22_XLG_INT_MASK);
1400 
1401 		mvpp2_modify(port->base + MVPP22_XLG_EXT_INT_MASK,
1402 			     MVPP22_XLG_EXT_INT_MASK_PTP,
1403 			     MVPP22_XLG_EXT_INT_MASK_PTP);
1404 	}
1405 
1406 	mvpp22_gop_unmask_irq(port);
1407 }
1408 
1409 /* Sets the PHY mode of the COMPHY (which configures the serdes lanes).
1410  *
1411  * The PHY mode used by the PPv2 driver comes from the network subsystem, while
1412  * the one given to the COMPHY comes from the generic PHY subsystem. Hence they
1413  * differ.
1414  *
1415  * The COMPHY configures the serdes lanes regardless of the actual use of the
1416  * lanes by the physical layer. This is why configurations like
1417  * "PPv2 (2500BaseX) - COMPHY (2500SGMII)" are valid.
1418  */
1419 static int mvpp22_comphy_init(struct mvpp2_port *port)
1420 {
1421 	int ret;
1422 
1423 	if (!port->comphy)
1424 		return 0;
1425 
1426 	ret = phy_set_mode_ext(port->comphy, PHY_MODE_ETHERNET,
1427 			       port->phy_interface);
1428 	if (ret)
1429 		return ret;
1430 
1431 	return phy_power_on(port->comphy);
1432 }
1433 
1434 static void mvpp2_port_enable(struct mvpp2_port *port)
1435 {
1436 	u32 val;
1437 
1438 	if (mvpp2_port_supports_xlg(port) &&
1439 	    mvpp2_is_xlg(port->phy_interface)) {
1440 		val = readl(port->base + MVPP22_XLG_CTRL0_REG);
1441 		val |= MVPP22_XLG_CTRL0_PORT_EN;
1442 		val &= ~MVPP22_XLG_CTRL0_MIB_CNT_DIS;
1443 		writel(val, port->base + MVPP22_XLG_CTRL0_REG);
1444 	} else {
1445 		val = readl(port->base + MVPP2_GMAC_CTRL_0_REG);
1446 		val |= MVPP2_GMAC_PORT_EN_MASK;
1447 		val |= MVPP2_GMAC_MIB_CNTR_EN_MASK;
1448 		writel(val, port->base + MVPP2_GMAC_CTRL_0_REG);
1449 	}
1450 }
1451 
1452 static void mvpp2_port_disable(struct mvpp2_port *port)
1453 {
1454 	u32 val;
1455 
1456 	if (mvpp2_port_supports_xlg(port) &&
1457 	    mvpp2_is_xlg(port->phy_interface)) {
1458 		val = readl(port->base + MVPP22_XLG_CTRL0_REG);
1459 		val &= ~MVPP22_XLG_CTRL0_PORT_EN;
1460 		writel(val, port->base + MVPP22_XLG_CTRL0_REG);
1461 	}
1462 
1463 	val = readl(port->base + MVPP2_GMAC_CTRL_0_REG);
1464 	val &= ~(MVPP2_GMAC_PORT_EN_MASK);
1465 	writel(val, port->base + MVPP2_GMAC_CTRL_0_REG);
1466 }
1467 
1468 /* Set IEEE 802.3x Flow Control Xon Packet Transmission Mode */
1469 static void mvpp2_port_periodic_xon_disable(struct mvpp2_port *port)
1470 {
1471 	u32 val;
1472 
1473 	val = readl(port->base + MVPP2_GMAC_CTRL_1_REG) &
1474 		    ~MVPP2_GMAC_PERIODIC_XON_EN_MASK;
1475 	writel(val, port->base + MVPP2_GMAC_CTRL_1_REG);
1476 }
1477 
1478 /* Configure loopback port */
1479 static void mvpp2_port_loopback_set(struct mvpp2_port *port,
1480 				    const struct phylink_link_state *state)
1481 {
1482 	u32 val;
1483 
1484 	val = readl(port->base + MVPP2_GMAC_CTRL_1_REG);
1485 
1486 	if (state->speed == 1000)
1487 		val |= MVPP2_GMAC_GMII_LB_EN_MASK;
1488 	else
1489 		val &= ~MVPP2_GMAC_GMII_LB_EN_MASK;
1490 
1491 	if (phy_interface_mode_is_8023z(state->interface) ||
1492 	    state->interface == PHY_INTERFACE_MODE_SGMII)
1493 		val |= MVPP2_GMAC_PCS_LB_EN_MASK;
1494 	else
1495 		val &= ~MVPP2_GMAC_PCS_LB_EN_MASK;
1496 
1497 	writel(val, port->base + MVPP2_GMAC_CTRL_1_REG);
1498 }
1499 
1500 enum {
1501 	ETHTOOL_XDP_REDIRECT,
1502 	ETHTOOL_XDP_PASS,
1503 	ETHTOOL_XDP_DROP,
1504 	ETHTOOL_XDP_TX,
1505 	ETHTOOL_XDP_TX_ERR,
1506 	ETHTOOL_XDP_XMIT,
1507 	ETHTOOL_XDP_XMIT_ERR,
1508 };
1509 
1510 struct mvpp2_ethtool_counter {
1511 	unsigned int offset;
1512 	const char string[ETH_GSTRING_LEN];
1513 	bool reg_is_64b;
1514 };
1515 
1516 static u64 mvpp2_read_count(struct mvpp2_port *port,
1517 			    const struct mvpp2_ethtool_counter *counter)
1518 {
1519 	u64 val;
1520 
1521 	val = readl(port->stats_base + counter->offset);
1522 	if (counter->reg_is_64b)
1523 		val += (u64)readl(port->stats_base + counter->offset + 4) << 32;
1524 
1525 	return val;
1526 }
1527 
1528 /* Some counters are accessed indirectly by first writing an index to
1529  * MVPP2_CTRS_IDX. The index can represent various resources depending on the
1530  * register we access, it can be a hit counter for some classification tables,
1531  * a counter specific to a rxq, a txq or a buffer pool.
1532  */
1533 static u32 mvpp2_read_index(struct mvpp2 *priv, u32 index, u32 reg)
1534 {
1535 	mvpp2_write(priv, MVPP2_CTRS_IDX, index);
1536 	return mvpp2_read(priv, reg);
1537 }
1538 
1539 /* Due to the fact that software statistics and hardware statistics are, by
1540  * design, incremented at different moments in the chain of packet processing,
1541  * it is very likely that incoming packets could have been dropped after being
1542  * counted by hardware but before reaching software statistics (most probably
1543  * multicast packets), and in the oppposite way, during transmission, FCS bytes
1544  * are added in between as well as TSO skb will be split and header bytes added.
1545  * Hence, statistics gathered from userspace with ifconfig (software) and
1546  * ethtool (hardware) cannot be compared.
1547  */
1548 static const struct mvpp2_ethtool_counter mvpp2_ethtool_mib_regs[] = {
1549 	{ MVPP2_MIB_GOOD_OCTETS_RCVD, "good_octets_received", true },
1550 	{ MVPP2_MIB_BAD_OCTETS_RCVD, "bad_octets_received" },
1551 	{ MVPP2_MIB_CRC_ERRORS_SENT, "crc_errors_sent" },
1552 	{ MVPP2_MIB_UNICAST_FRAMES_RCVD, "unicast_frames_received" },
1553 	{ MVPP2_MIB_BROADCAST_FRAMES_RCVD, "broadcast_frames_received" },
1554 	{ MVPP2_MIB_MULTICAST_FRAMES_RCVD, "multicast_frames_received" },
1555 	{ MVPP2_MIB_FRAMES_64_OCTETS, "frames_64_octets" },
1556 	{ MVPP2_MIB_FRAMES_65_TO_127_OCTETS, "frames_65_to_127_octet" },
1557 	{ MVPP2_MIB_FRAMES_128_TO_255_OCTETS, "frames_128_to_255_octet" },
1558 	{ MVPP2_MIB_FRAMES_256_TO_511_OCTETS, "frames_256_to_511_octet" },
1559 	{ MVPP2_MIB_FRAMES_512_TO_1023_OCTETS, "frames_512_to_1023_octet" },
1560 	{ MVPP2_MIB_FRAMES_1024_TO_MAX_OCTETS, "frames_1024_to_max_octet" },
1561 	{ MVPP2_MIB_GOOD_OCTETS_SENT, "good_octets_sent", true },
1562 	{ MVPP2_MIB_UNICAST_FRAMES_SENT, "unicast_frames_sent" },
1563 	{ MVPP2_MIB_MULTICAST_FRAMES_SENT, "multicast_frames_sent" },
1564 	{ MVPP2_MIB_BROADCAST_FRAMES_SENT, "broadcast_frames_sent" },
1565 	{ MVPP2_MIB_FC_SENT, "fc_sent" },
1566 	{ MVPP2_MIB_FC_RCVD, "fc_received" },
1567 	{ MVPP2_MIB_RX_FIFO_OVERRUN, "rx_fifo_overrun" },
1568 	{ MVPP2_MIB_UNDERSIZE_RCVD, "undersize_received" },
1569 	{ MVPP2_MIB_FRAGMENTS_RCVD, "fragments_received" },
1570 	{ MVPP2_MIB_OVERSIZE_RCVD, "oversize_received" },
1571 	{ MVPP2_MIB_JABBER_RCVD, "jabber_received" },
1572 	{ MVPP2_MIB_MAC_RCV_ERROR, "mac_receive_error" },
1573 	{ MVPP2_MIB_BAD_CRC_EVENT, "bad_crc_event" },
1574 	{ MVPP2_MIB_COLLISION, "collision" },
1575 	{ MVPP2_MIB_LATE_COLLISION, "late_collision" },
1576 };
1577 
1578 static const struct mvpp2_ethtool_counter mvpp2_ethtool_port_regs[] = {
1579 	{ MVPP2_OVERRUN_ETH_DROP, "rx_fifo_or_parser_overrun_drops" },
1580 	{ MVPP2_CLS_ETH_DROP, "rx_classifier_drops" },
1581 };
1582 
1583 static const struct mvpp2_ethtool_counter mvpp2_ethtool_txq_regs[] = {
1584 	{ MVPP2_TX_DESC_ENQ_CTR, "txq_%d_desc_enqueue" },
1585 	{ MVPP2_TX_DESC_ENQ_TO_DDR_CTR, "txq_%d_desc_enqueue_to_ddr" },
1586 	{ MVPP2_TX_BUFF_ENQ_TO_DDR_CTR, "txq_%d_buff_euqueue_to_ddr" },
1587 	{ MVPP2_TX_DESC_ENQ_HW_FWD_CTR, "txq_%d_desc_hardware_forwarded" },
1588 	{ MVPP2_TX_PKTS_DEQ_CTR, "txq_%d_packets_dequeued" },
1589 	{ MVPP2_TX_PKTS_FULL_QUEUE_DROP_CTR, "txq_%d_queue_full_drops" },
1590 	{ MVPP2_TX_PKTS_EARLY_DROP_CTR, "txq_%d_packets_early_drops" },
1591 	{ MVPP2_TX_PKTS_BM_DROP_CTR, "txq_%d_packets_bm_drops" },
1592 	{ MVPP2_TX_PKTS_BM_MC_DROP_CTR, "txq_%d_packets_rep_bm_drops" },
1593 };
1594 
1595 static const struct mvpp2_ethtool_counter mvpp2_ethtool_rxq_regs[] = {
1596 	{ MVPP2_RX_DESC_ENQ_CTR, "rxq_%d_desc_enqueue" },
1597 	{ MVPP2_RX_PKTS_FULL_QUEUE_DROP_CTR, "rxq_%d_queue_full_drops" },
1598 	{ MVPP2_RX_PKTS_EARLY_DROP_CTR, "rxq_%d_packets_early_drops" },
1599 	{ MVPP2_RX_PKTS_BM_DROP_CTR, "rxq_%d_packets_bm_drops" },
1600 };
1601 
1602 static const struct mvpp2_ethtool_counter mvpp2_ethtool_xdp[] = {
1603 	{ ETHTOOL_XDP_REDIRECT, "rx_xdp_redirect", },
1604 	{ ETHTOOL_XDP_PASS, "rx_xdp_pass", },
1605 	{ ETHTOOL_XDP_DROP, "rx_xdp_drop", },
1606 	{ ETHTOOL_XDP_TX, "rx_xdp_tx", },
1607 	{ ETHTOOL_XDP_TX_ERR, "rx_xdp_tx_errors", },
1608 	{ ETHTOOL_XDP_XMIT, "tx_xdp_xmit", },
1609 	{ ETHTOOL_XDP_XMIT_ERR, "tx_xdp_xmit_errors", },
1610 };
1611 
1612 #define MVPP2_N_ETHTOOL_STATS(ntxqs, nrxqs)	(ARRAY_SIZE(mvpp2_ethtool_mib_regs) + \
1613 						 ARRAY_SIZE(mvpp2_ethtool_port_regs) + \
1614 						 (ARRAY_SIZE(mvpp2_ethtool_txq_regs) * (ntxqs)) + \
1615 						 (ARRAY_SIZE(mvpp2_ethtool_rxq_regs) * (nrxqs)) + \
1616 						 ARRAY_SIZE(mvpp2_ethtool_xdp))
1617 
1618 static void mvpp2_ethtool_get_strings(struct net_device *netdev, u32 sset,
1619 				      u8 *data)
1620 {
1621 	struct mvpp2_port *port = netdev_priv(netdev);
1622 	int i, q;
1623 
1624 	if (sset != ETH_SS_STATS)
1625 		return;
1626 
1627 	for (i = 0; i < ARRAY_SIZE(mvpp2_ethtool_mib_regs); i++) {
1628 		strscpy(data, mvpp2_ethtool_mib_regs[i].string,
1629 			ETH_GSTRING_LEN);
1630 		data += ETH_GSTRING_LEN;
1631 	}
1632 
1633 	for (i = 0; i < ARRAY_SIZE(mvpp2_ethtool_port_regs); i++) {
1634 		strscpy(data, mvpp2_ethtool_port_regs[i].string,
1635 			ETH_GSTRING_LEN);
1636 		data += ETH_GSTRING_LEN;
1637 	}
1638 
1639 	for (q = 0; q < port->ntxqs; q++) {
1640 		for (i = 0; i < ARRAY_SIZE(mvpp2_ethtool_txq_regs); i++) {
1641 			snprintf(data, ETH_GSTRING_LEN,
1642 				 mvpp2_ethtool_txq_regs[i].string, q);
1643 			data += ETH_GSTRING_LEN;
1644 		}
1645 	}
1646 
1647 	for (q = 0; q < port->nrxqs; q++) {
1648 		for (i = 0; i < ARRAY_SIZE(mvpp2_ethtool_rxq_regs); i++) {
1649 			snprintf(data, ETH_GSTRING_LEN,
1650 				 mvpp2_ethtool_rxq_regs[i].string,
1651 				 q);
1652 			data += ETH_GSTRING_LEN;
1653 		}
1654 	}
1655 
1656 	for (i = 0; i < ARRAY_SIZE(mvpp2_ethtool_xdp); i++) {
1657 		strscpy(data, mvpp2_ethtool_xdp[i].string,
1658 			ETH_GSTRING_LEN);
1659 		data += ETH_GSTRING_LEN;
1660 	}
1661 }
1662 
1663 static void
1664 mvpp2_get_xdp_stats(struct mvpp2_port *port, struct mvpp2_pcpu_stats *xdp_stats)
1665 {
1666 	unsigned int start;
1667 	unsigned int cpu;
1668 
1669 	/* Gather XDP Statistics */
1670 	for_each_possible_cpu(cpu) {
1671 		struct mvpp2_pcpu_stats *cpu_stats;
1672 		u64	xdp_redirect;
1673 		u64	xdp_pass;
1674 		u64	xdp_drop;
1675 		u64	xdp_xmit;
1676 		u64	xdp_xmit_err;
1677 		u64	xdp_tx;
1678 		u64	xdp_tx_err;
1679 
1680 		cpu_stats = per_cpu_ptr(port->stats, cpu);
1681 		do {
1682 			start = u64_stats_fetch_begin_irq(&cpu_stats->syncp);
1683 			xdp_redirect = cpu_stats->xdp_redirect;
1684 			xdp_pass   = cpu_stats->xdp_pass;
1685 			xdp_drop = cpu_stats->xdp_drop;
1686 			xdp_xmit   = cpu_stats->xdp_xmit;
1687 			xdp_xmit_err   = cpu_stats->xdp_xmit_err;
1688 			xdp_tx   = cpu_stats->xdp_tx;
1689 			xdp_tx_err   = cpu_stats->xdp_tx_err;
1690 		} while (u64_stats_fetch_retry_irq(&cpu_stats->syncp, start));
1691 
1692 		xdp_stats->xdp_redirect += xdp_redirect;
1693 		xdp_stats->xdp_pass   += xdp_pass;
1694 		xdp_stats->xdp_drop += xdp_drop;
1695 		xdp_stats->xdp_xmit   += xdp_xmit;
1696 		xdp_stats->xdp_xmit_err   += xdp_xmit_err;
1697 		xdp_stats->xdp_tx   += xdp_tx;
1698 		xdp_stats->xdp_tx_err   += xdp_tx_err;
1699 	}
1700 }
1701 
1702 static void mvpp2_read_stats(struct mvpp2_port *port)
1703 {
1704 	struct mvpp2_pcpu_stats xdp_stats = {};
1705 	const struct mvpp2_ethtool_counter *s;
1706 	u64 *pstats;
1707 	int i, q;
1708 
1709 	pstats = port->ethtool_stats;
1710 
1711 	for (i = 0; i < ARRAY_SIZE(mvpp2_ethtool_mib_regs); i++)
1712 		*pstats++ += mvpp2_read_count(port, &mvpp2_ethtool_mib_regs[i]);
1713 
1714 	for (i = 0; i < ARRAY_SIZE(mvpp2_ethtool_port_regs); i++)
1715 		*pstats++ += mvpp2_read(port->priv,
1716 					mvpp2_ethtool_port_regs[i].offset +
1717 					4 * port->id);
1718 
1719 	for (q = 0; q < port->ntxqs; q++)
1720 		for (i = 0; i < ARRAY_SIZE(mvpp2_ethtool_txq_regs); i++)
1721 			*pstats++ += mvpp2_read_index(port->priv,
1722 						      MVPP22_CTRS_TX_CTR(port->id, q),
1723 						      mvpp2_ethtool_txq_regs[i].offset);
1724 
1725 	/* Rxqs are numbered from 0 from the user standpoint, but not from the
1726 	 * driver's. We need to add the  port->first_rxq offset.
1727 	 */
1728 	for (q = 0; q < port->nrxqs; q++)
1729 		for (i = 0; i < ARRAY_SIZE(mvpp2_ethtool_rxq_regs); i++)
1730 			*pstats++ += mvpp2_read_index(port->priv,
1731 						      port->first_rxq + q,
1732 						      mvpp2_ethtool_rxq_regs[i].offset);
1733 
1734 	/* Gather XDP Statistics */
1735 	mvpp2_get_xdp_stats(port, &xdp_stats);
1736 
1737 	for (i = 0, s = mvpp2_ethtool_xdp;
1738 		 s < mvpp2_ethtool_xdp + ARRAY_SIZE(mvpp2_ethtool_xdp);
1739 	     s++, i++) {
1740 		switch (s->offset) {
1741 		case ETHTOOL_XDP_REDIRECT:
1742 			*pstats++ = xdp_stats.xdp_redirect;
1743 			break;
1744 		case ETHTOOL_XDP_PASS:
1745 			*pstats++ = xdp_stats.xdp_pass;
1746 			break;
1747 		case ETHTOOL_XDP_DROP:
1748 			*pstats++ = xdp_stats.xdp_drop;
1749 			break;
1750 		case ETHTOOL_XDP_TX:
1751 			*pstats++ = xdp_stats.xdp_tx;
1752 			break;
1753 		case ETHTOOL_XDP_TX_ERR:
1754 			*pstats++ = xdp_stats.xdp_tx_err;
1755 			break;
1756 		case ETHTOOL_XDP_XMIT:
1757 			*pstats++ = xdp_stats.xdp_xmit;
1758 			break;
1759 		case ETHTOOL_XDP_XMIT_ERR:
1760 			*pstats++ = xdp_stats.xdp_xmit_err;
1761 			break;
1762 		}
1763 	}
1764 }
1765 
1766 static void mvpp2_gather_hw_statistics(struct work_struct *work)
1767 {
1768 	struct delayed_work *del_work = to_delayed_work(work);
1769 	struct mvpp2_port *port = container_of(del_work, struct mvpp2_port,
1770 					       stats_work);
1771 
1772 	mutex_lock(&port->gather_stats_lock);
1773 
1774 	mvpp2_read_stats(port);
1775 
1776 	/* No need to read again the counters right after this function if it
1777 	 * was called asynchronously by the user (ie. use of ethtool).
1778 	 */
1779 	cancel_delayed_work(&port->stats_work);
1780 	queue_delayed_work(port->priv->stats_queue, &port->stats_work,
1781 			   MVPP2_MIB_COUNTERS_STATS_DELAY);
1782 
1783 	mutex_unlock(&port->gather_stats_lock);
1784 }
1785 
1786 static void mvpp2_ethtool_get_stats(struct net_device *dev,
1787 				    struct ethtool_stats *stats, u64 *data)
1788 {
1789 	struct mvpp2_port *port = netdev_priv(dev);
1790 
1791 	/* Update statistics for the given port, then take the lock to avoid
1792 	 * concurrent accesses on the ethtool_stats structure during its copy.
1793 	 */
1794 	mvpp2_gather_hw_statistics(&port->stats_work.work);
1795 
1796 	mutex_lock(&port->gather_stats_lock);
1797 	memcpy(data, port->ethtool_stats,
1798 	       sizeof(u64) * MVPP2_N_ETHTOOL_STATS(port->ntxqs, port->nrxqs));
1799 	mutex_unlock(&port->gather_stats_lock);
1800 }
1801 
1802 static int mvpp2_ethtool_get_sset_count(struct net_device *dev, int sset)
1803 {
1804 	struct mvpp2_port *port = netdev_priv(dev);
1805 
1806 	if (sset == ETH_SS_STATS)
1807 		return MVPP2_N_ETHTOOL_STATS(port->ntxqs, port->nrxqs);
1808 
1809 	return -EOPNOTSUPP;
1810 }
1811 
1812 static void mvpp2_mac_reset_assert(struct mvpp2_port *port)
1813 {
1814 	u32 val;
1815 
1816 	val = readl(port->base + MVPP2_GMAC_CTRL_2_REG) |
1817 	      MVPP2_GMAC_PORT_RESET_MASK;
1818 	writel(val, port->base + MVPP2_GMAC_CTRL_2_REG);
1819 
1820 	if (port->priv->hw_version == MVPP22 && port->gop_id == 0) {
1821 		val = readl(port->base + MVPP22_XLG_CTRL0_REG) &
1822 		      ~MVPP22_XLG_CTRL0_MAC_RESET_DIS;
1823 		writel(val, port->base + MVPP22_XLG_CTRL0_REG);
1824 	}
1825 }
1826 
1827 static void mvpp22_pcs_reset_assert(struct mvpp2_port *port)
1828 {
1829 	struct mvpp2 *priv = port->priv;
1830 	void __iomem *mpcs, *xpcs;
1831 	u32 val;
1832 
1833 	if (port->priv->hw_version != MVPP22 || port->gop_id != 0)
1834 		return;
1835 
1836 	mpcs = priv->iface_base + MVPP22_MPCS_BASE(port->gop_id);
1837 	xpcs = priv->iface_base + MVPP22_XPCS_BASE(port->gop_id);
1838 
1839 	val = readl(mpcs + MVPP22_MPCS_CLK_RESET);
1840 	val &= ~(MAC_CLK_RESET_MAC | MAC_CLK_RESET_SD_RX | MAC_CLK_RESET_SD_TX);
1841 	val |= MVPP22_MPCS_CLK_RESET_DIV_SET;
1842 	writel(val, mpcs + MVPP22_MPCS_CLK_RESET);
1843 
1844 	val = readl(xpcs + MVPP22_XPCS_CFG0);
1845 	writel(val & ~MVPP22_XPCS_CFG0_RESET_DIS, xpcs + MVPP22_XPCS_CFG0);
1846 }
1847 
1848 static void mvpp22_pcs_reset_deassert(struct mvpp2_port *port)
1849 {
1850 	struct mvpp2 *priv = port->priv;
1851 	void __iomem *mpcs, *xpcs;
1852 	u32 val;
1853 
1854 	if (port->priv->hw_version != MVPP22 || port->gop_id != 0)
1855 		return;
1856 
1857 	mpcs = priv->iface_base + MVPP22_MPCS_BASE(port->gop_id);
1858 	xpcs = priv->iface_base + MVPP22_XPCS_BASE(port->gop_id);
1859 
1860 	switch (port->phy_interface) {
1861 	case PHY_INTERFACE_MODE_10GBASER:
1862 		val = readl(mpcs + MVPP22_MPCS_CLK_RESET);
1863 		val |= MAC_CLK_RESET_MAC | MAC_CLK_RESET_SD_RX |
1864 		       MAC_CLK_RESET_SD_TX;
1865 		val &= ~MVPP22_MPCS_CLK_RESET_DIV_SET;
1866 		writel(val, mpcs + MVPP22_MPCS_CLK_RESET);
1867 		break;
1868 	case PHY_INTERFACE_MODE_XAUI:
1869 	case PHY_INTERFACE_MODE_RXAUI:
1870 		val = readl(xpcs + MVPP22_XPCS_CFG0);
1871 		writel(val | MVPP22_XPCS_CFG0_RESET_DIS, xpcs + MVPP22_XPCS_CFG0);
1872 		break;
1873 	default:
1874 		break;
1875 	}
1876 }
1877 
1878 /* Change maximum receive size of the port */
1879 static inline void mvpp2_gmac_max_rx_size_set(struct mvpp2_port *port)
1880 {
1881 	u32 val;
1882 
1883 	val = readl(port->base + MVPP2_GMAC_CTRL_0_REG);
1884 	val &= ~MVPP2_GMAC_MAX_RX_SIZE_MASK;
1885 	val |= (((port->pkt_size - MVPP2_MH_SIZE) / 2) <<
1886 		    MVPP2_GMAC_MAX_RX_SIZE_OFFS);
1887 	writel(val, port->base + MVPP2_GMAC_CTRL_0_REG);
1888 }
1889 
1890 /* Change maximum receive size of the port */
1891 static inline void mvpp2_xlg_max_rx_size_set(struct mvpp2_port *port)
1892 {
1893 	u32 val;
1894 
1895 	val =  readl(port->base + MVPP22_XLG_CTRL1_REG);
1896 	val &= ~MVPP22_XLG_CTRL1_FRAMESIZELIMIT_MASK;
1897 	val |= ((port->pkt_size - MVPP2_MH_SIZE) / 2) <<
1898 	       MVPP22_XLG_CTRL1_FRAMESIZELIMIT_OFFS;
1899 	writel(val, port->base + MVPP22_XLG_CTRL1_REG);
1900 }
1901 
1902 /* Set defaults to the MVPP2 port */
1903 static void mvpp2_defaults_set(struct mvpp2_port *port)
1904 {
1905 	int tx_port_num, val, queue, lrxq;
1906 
1907 	if (port->priv->hw_version == MVPP21) {
1908 		/* Update TX FIFO MIN Threshold */
1909 		val = readl(port->base + MVPP2_GMAC_PORT_FIFO_CFG_1_REG);
1910 		val &= ~MVPP2_GMAC_TX_FIFO_MIN_TH_ALL_MASK;
1911 		/* Min. TX threshold must be less than minimal packet length */
1912 		val |= MVPP2_GMAC_TX_FIFO_MIN_TH_MASK(64 - 4 - 2);
1913 		writel(val, port->base + MVPP2_GMAC_PORT_FIFO_CFG_1_REG);
1914 	}
1915 
1916 	/* Disable Legacy WRR, Disable EJP, Release from reset */
1917 	tx_port_num = mvpp2_egress_port(port);
1918 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_PORT_INDEX_REG,
1919 		    tx_port_num);
1920 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_CMD_1_REG, 0);
1921 
1922 	/* Set TXQ scheduling to Round-Robin */
1923 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_FIXED_PRIO_REG, 0);
1924 
1925 	/* Close bandwidth for all queues */
1926 	for (queue = 0; queue < MVPP2_MAX_TXQ; queue++)
1927 		mvpp2_write(port->priv,
1928 			    MVPP2_TXQ_SCHED_TOKEN_CNTR_REG(queue), 0);
1929 
1930 	/* Set refill period to 1 usec, refill tokens
1931 	 * and bucket size to maximum
1932 	 */
1933 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_PERIOD_REG,
1934 		    port->priv->tclk / USEC_PER_SEC);
1935 	val = mvpp2_read(port->priv, MVPP2_TXP_SCHED_REFILL_REG);
1936 	val &= ~MVPP2_TXP_REFILL_PERIOD_ALL_MASK;
1937 	val |= MVPP2_TXP_REFILL_PERIOD_MASK(1);
1938 	val |= MVPP2_TXP_REFILL_TOKENS_ALL_MASK;
1939 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_REFILL_REG, val);
1940 	val = MVPP2_TXP_TOKEN_SIZE_MAX;
1941 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_TOKEN_SIZE_REG, val);
1942 
1943 	/* Set MaximumLowLatencyPacketSize value to 256 */
1944 	mvpp2_write(port->priv, MVPP2_RX_CTRL_REG(port->id),
1945 		    MVPP2_RX_USE_PSEUDO_FOR_CSUM_MASK |
1946 		    MVPP2_RX_LOW_LATENCY_PKT_SIZE(256));
1947 
1948 	/* Enable Rx cache snoop */
1949 	for (lrxq = 0; lrxq < port->nrxqs; lrxq++) {
1950 		queue = port->rxqs[lrxq]->id;
1951 		val = mvpp2_read(port->priv, MVPP2_RXQ_CONFIG_REG(queue));
1952 		val |= MVPP2_SNOOP_PKT_SIZE_MASK |
1953 			   MVPP2_SNOOP_BUF_HDR_MASK;
1954 		mvpp2_write(port->priv, MVPP2_RXQ_CONFIG_REG(queue), val);
1955 	}
1956 
1957 	/* At default, mask all interrupts to all present cpus */
1958 	mvpp2_interrupts_disable(port);
1959 }
1960 
1961 /* Enable/disable receiving packets */
1962 static void mvpp2_ingress_enable(struct mvpp2_port *port)
1963 {
1964 	u32 val;
1965 	int lrxq, queue;
1966 
1967 	for (lrxq = 0; lrxq < port->nrxqs; lrxq++) {
1968 		queue = port->rxqs[lrxq]->id;
1969 		val = mvpp2_read(port->priv, MVPP2_RXQ_CONFIG_REG(queue));
1970 		val &= ~MVPP2_RXQ_DISABLE_MASK;
1971 		mvpp2_write(port->priv, MVPP2_RXQ_CONFIG_REG(queue), val);
1972 	}
1973 }
1974 
1975 static void mvpp2_ingress_disable(struct mvpp2_port *port)
1976 {
1977 	u32 val;
1978 	int lrxq, queue;
1979 
1980 	for (lrxq = 0; lrxq < port->nrxqs; lrxq++) {
1981 		queue = port->rxqs[lrxq]->id;
1982 		val = mvpp2_read(port->priv, MVPP2_RXQ_CONFIG_REG(queue));
1983 		val |= MVPP2_RXQ_DISABLE_MASK;
1984 		mvpp2_write(port->priv, MVPP2_RXQ_CONFIG_REG(queue), val);
1985 	}
1986 }
1987 
1988 /* Enable transmit via physical egress queue
1989  * - HW starts take descriptors from DRAM
1990  */
1991 static void mvpp2_egress_enable(struct mvpp2_port *port)
1992 {
1993 	u32 qmap;
1994 	int queue;
1995 	int tx_port_num = mvpp2_egress_port(port);
1996 
1997 	/* Enable all initialized TXs. */
1998 	qmap = 0;
1999 	for (queue = 0; queue < port->ntxqs; queue++) {
2000 		struct mvpp2_tx_queue *txq = port->txqs[queue];
2001 
2002 		if (txq->descs)
2003 			qmap |= (1 << queue);
2004 	}
2005 
2006 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_PORT_INDEX_REG, tx_port_num);
2007 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_Q_CMD_REG, qmap);
2008 }
2009 
2010 /* Disable transmit via physical egress queue
2011  * - HW doesn't take descriptors from DRAM
2012  */
2013 static void mvpp2_egress_disable(struct mvpp2_port *port)
2014 {
2015 	u32 reg_data;
2016 	int delay;
2017 	int tx_port_num = mvpp2_egress_port(port);
2018 
2019 	/* Issue stop command for active channels only */
2020 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_PORT_INDEX_REG, tx_port_num);
2021 	reg_data = (mvpp2_read(port->priv, MVPP2_TXP_SCHED_Q_CMD_REG)) &
2022 		    MVPP2_TXP_SCHED_ENQ_MASK;
2023 	if (reg_data != 0)
2024 		mvpp2_write(port->priv, MVPP2_TXP_SCHED_Q_CMD_REG,
2025 			    (reg_data << MVPP2_TXP_SCHED_DISQ_OFFSET));
2026 
2027 	/* Wait for all Tx activity to terminate. */
2028 	delay = 0;
2029 	do {
2030 		if (delay >= MVPP2_TX_DISABLE_TIMEOUT_MSEC) {
2031 			netdev_warn(port->dev,
2032 				    "Tx stop timed out, status=0x%08x\n",
2033 				    reg_data);
2034 			break;
2035 		}
2036 		mdelay(1);
2037 		delay++;
2038 
2039 		/* Check port TX Command register that all
2040 		 * Tx queues are stopped
2041 		 */
2042 		reg_data = mvpp2_read(port->priv, MVPP2_TXP_SCHED_Q_CMD_REG);
2043 	} while (reg_data & MVPP2_TXP_SCHED_ENQ_MASK);
2044 }
2045 
2046 /* Rx descriptors helper methods */
2047 
2048 /* Get number of Rx descriptors occupied by received packets */
2049 static inline int
2050 mvpp2_rxq_received(struct mvpp2_port *port, int rxq_id)
2051 {
2052 	u32 val = mvpp2_read(port->priv, MVPP2_RXQ_STATUS_REG(rxq_id));
2053 
2054 	return val & MVPP2_RXQ_OCCUPIED_MASK;
2055 }
2056 
2057 /* Update Rx queue status with the number of occupied and available
2058  * Rx descriptor slots.
2059  */
2060 static inline void
2061 mvpp2_rxq_status_update(struct mvpp2_port *port, int rxq_id,
2062 			int used_count, int free_count)
2063 {
2064 	/* Decrement the number of used descriptors and increment count
2065 	 * increment the number of free descriptors.
2066 	 */
2067 	u32 val = used_count | (free_count << MVPP2_RXQ_NUM_NEW_OFFSET);
2068 
2069 	mvpp2_write(port->priv, MVPP2_RXQ_STATUS_UPDATE_REG(rxq_id), val);
2070 }
2071 
2072 /* Get pointer to next RX descriptor to be processed by SW */
2073 static inline struct mvpp2_rx_desc *
2074 mvpp2_rxq_next_desc_get(struct mvpp2_rx_queue *rxq)
2075 {
2076 	int rx_desc = rxq->next_desc_to_proc;
2077 
2078 	rxq->next_desc_to_proc = MVPP2_QUEUE_NEXT_DESC(rxq, rx_desc);
2079 	prefetch(rxq->descs + rxq->next_desc_to_proc);
2080 	return rxq->descs + rx_desc;
2081 }
2082 
2083 /* Set rx queue offset */
2084 static void mvpp2_rxq_offset_set(struct mvpp2_port *port,
2085 				 int prxq, int offset)
2086 {
2087 	u32 val;
2088 
2089 	/* Convert offset from bytes to units of 32 bytes */
2090 	offset = offset >> 5;
2091 
2092 	val = mvpp2_read(port->priv, MVPP2_RXQ_CONFIG_REG(prxq));
2093 	val &= ~MVPP2_RXQ_PACKET_OFFSET_MASK;
2094 
2095 	/* Offset is in */
2096 	val |= ((offset << MVPP2_RXQ_PACKET_OFFSET_OFFS) &
2097 		    MVPP2_RXQ_PACKET_OFFSET_MASK);
2098 
2099 	mvpp2_write(port->priv, MVPP2_RXQ_CONFIG_REG(prxq), val);
2100 }
2101 
2102 /* Tx descriptors helper methods */
2103 
2104 /* Get pointer to next Tx descriptor to be processed (send) by HW */
2105 static struct mvpp2_tx_desc *
2106 mvpp2_txq_next_desc_get(struct mvpp2_tx_queue *txq)
2107 {
2108 	int tx_desc = txq->next_desc_to_proc;
2109 
2110 	txq->next_desc_to_proc = MVPP2_QUEUE_NEXT_DESC(txq, tx_desc);
2111 	return txq->descs + tx_desc;
2112 }
2113 
2114 /* Update HW with number of aggregated Tx descriptors to be sent
2115  *
2116  * Called only from mvpp2_tx(), so migration is disabled, using
2117  * smp_processor_id() is OK.
2118  */
2119 static void mvpp2_aggr_txq_pend_desc_add(struct mvpp2_port *port, int pending)
2120 {
2121 	/* aggregated access - relevant TXQ number is written in TX desc */
2122 	mvpp2_thread_write(port->priv,
2123 			   mvpp2_cpu_to_thread(port->priv, smp_processor_id()),
2124 			   MVPP2_AGGR_TXQ_UPDATE_REG, pending);
2125 }
2126 
2127 /* Check if there are enough free descriptors in aggregated txq.
2128  * If not, update the number of occupied descriptors and repeat the check.
2129  *
2130  * Called only from mvpp2_tx(), so migration is disabled, using
2131  * smp_processor_id() is OK.
2132  */
2133 static int mvpp2_aggr_desc_num_check(struct mvpp2_port *port,
2134 				     struct mvpp2_tx_queue *aggr_txq, int num)
2135 {
2136 	if ((aggr_txq->count + num) > MVPP2_AGGR_TXQ_SIZE) {
2137 		/* Update number of occupied aggregated Tx descriptors */
2138 		unsigned int thread =
2139 			mvpp2_cpu_to_thread(port->priv, smp_processor_id());
2140 		u32 val = mvpp2_read_relaxed(port->priv,
2141 					     MVPP2_AGGR_TXQ_STATUS_REG(thread));
2142 
2143 		aggr_txq->count = val & MVPP2_AGGR_TXQ_PENDING_MASK;
2144 
2145 		if ((aggr_txq->count + num) > MVPP2_AGGR_TXQ_SIZE)
2146 			return -ENOMEM;
2147 	}
2148 	return 0;
2149 }
2150 
2151 /* Reserved Tx descriptors allocation request
2152  *
2153  * Called only from mvpp2_txq_reserved_desc_num_proc(), itself called
2154  * only by mvpp2_tx(), so migration is disabled, using
2155  * smp_processor_id() is OK.
2156  */
2157 static int mvpp2_txq_alloc_reserved_desc(struct mvpp2_port *port,
2158 					 struct mvpp2_tx_queue *txq, int num)
2159 {
2160 	unsigned int thread = mvpp2_cpu_to_thread(port->priv, smp_processor_id());
2161 	struct mvpp2 *priv = port->priv;
2162 	u32 val;
2163 
2164 	val = (txq->id << MVPP2_TXQ_RSVD_REQ_Q_OFFSET) | num;
2165 	mvpp2_thread_write_relaxed(priv, thread, MVPP2_TXQ_RSVD_REQ_REG, val);
2166 
2167 	val = mvpp2_thread_read_relaxed(priv, thread, MVPP2_TXQ_RSVD_RSLT_REG);
2168 
2169 	return val & MVPP2_TXQ_RSVD_RSLT_MASK;
2170 }
2171 
2172 /* Check if there are enough reserved descriptors for transmission.
2173  * If not, request chunk of reserved descriptors and check again.
2174  */
2175 static int mvpp2_txq_reserved_desc_num_proc(struct mvpp2_port *port,
2176 					    struct mvpp2_tx_queue *txq,
2177 					    struct mvpp2_txq_pcpu *txq_pcpu,
2178 					    int num)
2179 {
2180 	int req, desc_count;
2181 	unsigned int thread;
2182 
2183 	if (txq_pcpu->reserved_num >= num)
2184 		return 0;
2185 
2186 	/* Not enough descriptors reserved! Update the reserved descriptor
2187 	 * count and check again.
2188 	 */
2189 
2190 	desc_count = 0;
2191 	/* Compute total of used descriptors */
2192 	for (thread = 0; thread < port->priv->nthreads; thread++) {
2193 		struct mvpp2_txq_pcpu *txq_pcpu_aux;
2194 
2195 		txq_pcpu_aux = per_cpu_ptr(txq->pcpu, thread);
2196 		desc_count += txq_pcpu_aux->count;
2197 		desc_count += txq_pcpu_aux->reserved_num;
2198 	}
2199 
2200 	req = max(MVPP2_CPU_DESC_CHUNK, num - txq_pcpu->reserved_num);
2201 	desc_count += req;
2202 
2203 	if (desc_count >
2204 	   (txq->size - (MVPP2_MAX_THREADS * MVPP2_CPU_DESC_CHUNK)))
2205 		return -ENOMEM;
2206 
2207 	txq_pcpu->reserved_num += mvpp2_txq_alloc_reserved_desc(port, txq, req);
2208 
2209 	/* OK, the descriptor could have been updated: check again. */
2210 	if (txq_pcpu->reserved_num < num)
2211 		return -ENOMEM;
2212 	return 0;
2213 }
2214 
2215 /* Release the last allocated Tx descriptor. Useful to handle DMA
2216  * mapping failures in the Tx path.
2217  */
2218 static void mvpp2_txq_desc_put(struct mvpp2_tx_queue *txq)
2219 {
2220 	if (txq->next_desc_to_proc == 0)
2221 		txq->next_desc_to_proc = txq->last_desc - 1;
2222 	else
2223 		txq->next_desc_to_proc--;
2224 }
2225 
2226 /* Set Tx descriptors fields relevant for CSUM calculation */
2227 static u32 mvpp2_txq_desc_csum(int l3_offs, __be16 l3_proto,
2228 			       int ip_hdr_len, int l4_proto)
2229 {
2230 	u32 command;
2231 
2232 	/* fields: L3_offset, IP_hdrlen, L3_type, G_IPv4_chk,
2233 	 * G_L4_chk, L4_type required only for checksum calculation
2234 	 */
2235 	command = (l3_offs << MVPP2_TXD_L3_OFF_SHIFT);
2236 	command |= (ip_hdr_len << MVPP2_TXD_IP_HLEN_SHIFT);
2237 	command |= MVPP2_TXD_IP_CSUM_DISABLE;
2238 
2239 	if (l3_proto == htons(ETH_P_IP)) {
2240 		command &= ~MVPP2_TXD_IP_CSUM_DISABLE;	/* enable IPv4 csum */
2241 		command &= ~MVPP2_TXD_L3_IP6;		/* enable IPv4 */
2242 	} else {
2243 		command |= MVPP2_TXD_L3_IP6;		/* enable IPv6 */
2244 	}
2245 
2246 	if (l4_proto == IPPROTO_TCP) {
2247 		command &= ~MVPP2_TXD_L4_UDP;		/* enable TCP */
2248 		command &= ~MVPP2_TXD_L4_CSUM_FRAG;	/* generate L4 csum */
2249 	} else if (l4_proto == IPPROTO_UDP) {
2250 		command |= MVPP2_TXD_L4_UDP;		/* enable UDP */
2251 		command &= ~MVPP2_TXD_L4_CSUM_FRAG;	/* generate L4 csum */
2252 	} else {
2253 		command |= MVPP2_TXD_L4_CSUM_NOT;
2254 	}
2255 
2256 	return command;
2257 }
2258 
2259 /* Get number of sent descriptors and decrement counter.
2260  * The number of sent descriptors is returned.
2261  * Per-thread access
2262  *
2263  * Called only from mvpp2_txq_done(), called from mvpp2_tx()
2264  * (migration disabled) and from the TX completion tasklet (migration
2265  * disabled) so using smp_processor_id() is OK.
2266  */
2267 static inline int mvpp2_txq_sent_desc_proc(struct mvpp2_port *port,
2268 					   struct mvpp2_tx_queue *txq)
2269 {
2270 	u32 val;
2271 
2272 	/* Reading status reg resets transmitted descriptor counter */
2273 	val = mvpp2_thread_read_relaxed(port->priv,
2274 					mvpp2_cpu_to_thread(port->priv, smp_processor_id()),
2275 					MVPP2_TXQ_SENT_REG(txq->id));
2276 
2277 	return (val & MVPP2_TRANSMITTED_COUNT_MASK) >>
2278 		MVPP2_TRANSMITTED_COUNT_OFFSET;
2279 }
2280 
2281 /* Called through on_each_cpu(), so runs on all CPUs, with migration
2282  * disabled, therefore using smp_processor_id() is OK.
2283  */
2284 static void mvpp2_txq_sent_counter_clear(void *arg)
2285 {
2286 	struct mvpp2_port *port = arg;
2287 	int queue;
2288 
2289 	/* If the thread isn't used, don't do anything */
2290 	if (smp_processor_id() > port->priv->nthreads)
2291 		return;
2292 
2293 	for (queue = 0; queue < port->ntxqs; queue++) {
2294 		int id = port->txqs[queue]->id;
2295 
2296 		mvpp2_thread_read(port->priv,
2297 				  mvpp2_cpu_to_thread(port->priv, smp_processor_id()),
2298 				  MVPP2_TXQ_SENT_REG(id));
2299 	}
2300 }
2301 
2302 /* Set max sizes for Tx queues */
2303 static void mvpp2_txp_max_tx_size_set(struct mvpp2_port *port)
2304 {
2305 	u32	val, size, mtu;
2306 	int	txq, tx_port_num;
2307 
2308 	mtu = port->pkt_size * 8;
2309 	if (mtu > MVPP2_TXP_MTU_MAX)
2310 		mtu = MVPP2_TXP_MTU_MAX;
2311 
2312 	/* WA for wrong Token bucket update: Set MTU value = 3*real MTU value */
2313 	mtu = 3 * mtu;
2314 
2315 	/* Indirect access to registers */
2316 	tx_port_num = mvpp2_egress_port(port);
2317 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_PORT_INDEX_REG, tx_port_num);
2318 
2319 	/* Set MTU */
2320 	val = mvpp2_read(port->priv, MVPP2_TXP_SCHED_MTU_REG);
2321 	val &= ~MVPP2_TXP_MTU_MAX;
2322 	val |= mtu;
2323 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_MTU_REG, val);
2324 
2325 	/* TXP token size and all TXQs token size must be larger that MTU */
2326 	val = mvpp2_read(port->priv, MVPP2_TXP_SCHED_TOKEN_SIZE_REG);
2327 	size = val & MVPP2_TXP_TOKEN_SIZE_MAX;
2328 	if (size < mtu) {
2329 		size = mtu;
2330 		val &= ~MVPP2_TXP_TOKEN_SIZE_MAX;
2331 		val |= size;
2332 		mvpp2_write(port->priv, MVPP2_TXP_SCHED_TOKEN_SIZE_REG, val);
2333 	}
2334 
2335 	for (txq = 0; txq < port->ntxqs; txq++) {
2336 		val = mvpp2_read(port->priv,
2337 				 MVPP2_TXQ_SCHED_TOKEN_SIZE_REG(txq));
2338 		size = val & MVPP2_TXQ_TOKEN_SIZE_MAX;
2339 
2340 		if (size < mtu) {
2341 			size = mtu;
2342 			val &= ~MVPP2_TXQ_TOKEN_SIZE_MAX;
2343 			val |= size;
2344 			mvpp2_write(port->priv,
2345 				    MVPP2_TXQ_SCHED_TOKEN_SIZE_REG(txq),
2346 				    val);
2347 		}
2348 	}
2349 }
2350 
2351 /* Set the number of packets that will be received before Rx interrupt
2352  * will be generated by HW.
2353  */
2354 static void mvpp2_rx_pkts_coal_set(struct mvpp2_port *port,
2355 				   struct mvpp2_rx_queue *rxq)
2356 {
2357 	unsigned int thread = mvpp2_cpu_to_thread(port->priv, get_cpu());
2358 
2359 	if (rxq->pkts_coal > MVPP2_OCCUPIED_THRESH_MASK)
2360 		rxq->pkts_coal = MVPP2_OCCUPIED_THRESH_MASK;
2361 
2362 	mvpp2_thread_write(port->priv, thread, MVPP2_RXQ_NUM_REG, rxq->id);
2363 	mvpp2_thread_write(port->priv, thread, MVPP2_RXQ_THRESH_REG,
2364 			   rxq->pkts_coal);
2365 
2366 	put_cpu();
2367 }
2368 
2369 /* For some reason in the LSP this is done on each CPU. Why ? */
2370 static void mvpp2_tx_pkts_coal_set(struct mvpp2_port *port,
2371 				   struct mvpp2_tx_queue *txq)
2372 {
2373 	unsigned int thread = mvpp2_cpu_to_thread(port->priv, get_cpu());
2374 	u32 val;
2375 
2376 	if (txq->done_pkts_coal > MVPP2_TXQ_THRESH_MASK)
2377 		txq->done_pkts_coal = MVPP2_TXQ_THRESH_MASK;
2378 
2379 	val = (txq->done_pkts_coal << MVPP2_TXQ_THRESH_OFFSET);
2380 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_NUM_REG, txq->id);
2381 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_THRESH_REG, val);
2382 
2383 	put_cpu();
2384 }
2385 
2386 static u32 mvpp2_usec_to_cycles(u32 usec, unsigned long clk_hz)
2387 {
2388 	u64 tmp = (u64)clk_hz * usec;
2389 
2390 	do_div(tmp, USEC_PER_SEC);
2391 
2392 	return tmp > U32_MAX ? U32_MAX : tmp;
2393 }
2394 
2395 static u32 mvpp2_cycles_to_usec(u32 cycles, unsigned long clk_hz)
2396 {
2397 	u64 tmp = (u64)cycles * USEC_PER_SEC;
2398 
2399 	do_div(tmp, clk_hz);
2400 
2401 	return tmp > U32_MAX ? U32_MAX : tmp;
2402 }
2403 
2404 /* Set the time delay in usec before Rx interrupt */
2405 static void mvpp2_rx_time_coal_set(struct mvpp2_port *port,
2406 				   struct mvpp2_rx_queue *rxq)
2407 {
2408 	unsigned long freq = port->priv->tclk;
2409 	u32 val = mvpp2_usec_to_cycles(rxq->time_coal, freq);
2410 
2411 	if (val > MVPP2_MAX_ISR_RX_THRESHOLD) {
2412 		rxq->time_coal =
2413 			mvpp2_cycles_to_usec(MVPP2_MAX_ISR_RX_THRESHOLD, freq);
2414 
2415 		/* re-evaluate to get actual register value */
2416 		val = mvpp2_usec_to_cycles(rxq->time_coal, freq);
2417 	}
2418 
2419 	mvpp2_write(port->priv, MVPP2_ISR_RX_THRESHOLD_REG(rxq->id), val);
2420 }
2421 
2422 static void mvpp2_tx_time_coal_set(struct mvpp2_port *port)
2423 {
2424 	unsigned long freq = port->priv->tclk;
2425 	u32 val = mvpp2_usec_to_cycles(port->tx_time_coal, freq);
2426 
2427 	if (val > MVPP2_MAX_ISR_TX_THRESHOLD) {
2428 		port->tx_time_coal =
2429 			mvpp2_cycles_to_usec(MVPP2_MAX_ISR_TX_THRESHOLD, freq);
2430 
2431 		/* re-evaluate to get actual register value */
2432 		val = mvpp2_usec_to_cycles(port->tx_time_coal, freq);
2433 	}
2434 
2435 	mvpp2_write(port->priv, MVPP2_ISR_TX_THRESHOLD_REG(port->id), val);
2436 }
2437 
2438 /* Free Tx queue skbuffs */
2439 static void mvpp2_txq_bufs_free(struct mvpp2_port *port,
2440 				struct mvpp2_tx_queue *txq,
2441 				struct mvpp2_txq_pcpu *txq_pcpu, int num)
2442 {
2443 	int i;
2444 
2445 	for (i = 0; i < num; i++) {
2446 		struct mvpp2_txq_pcpu_buf *tx_buf =
2447 			txq_pcpu->buffs + txq_pcpu->txq_get_index;
2448 
2449 		if (!IS_TSO_HEADER(txq_pcpu, tx_buf->dma) &&
2450 		    tx_buf->type != MVPP2_TYPE_XDP_TX)
2451 			dma_unmap_single(port->dev->dev.parent, tx_buf->dma,
2452 					 tx_buf->size, DMA_TO_DEVICE);
2453 		if (tx_buf->type == MVPP2_TYPE_SKB && tx_buf->skb)
2454 			dev_kfree_skb_any(tx_buf->skb);
2455 		else if (tx_buf->type == MVPP2_TYPE_XDP_TX ||
2456 			 tx_buf->type == MVPP2_TYPE_XDP_NDO)
2457 			xdp_return_frame(tx_buf->xdpf);
2458 
2459 		mvpp2_txq_inc_get(txq_pcpu);
2460 	}
2461 }
2462 
2463 static inline struct mvpp2_rx_queue *mvpp2_get_rx_queue(struct mvpp2_port *port,
2464 							u32 cause)
2465 {
2466 	int queue = fls(cause) - 1;
2467 
2468 	return port->rxqs[queue];
2469 }
2470 
2471 static inline struct mvpp2_tx_queue *mvpp2_get_tx_queue(struct mvpp2_port *port,
2472 							u32 cause)
2473 {
2474 	int queue = fls(cause) - 1;
2475 
2476 	return port->txqs[queue];
2477 }
2478 
2479 /* Handle end of transmission */
2480 static void mvpp2_txq_done(struct mvpp2_port *port, struct mvpp2_tx_queue *txq,
2481 			   struct mvpp2_txq_pcpu *txq_pcpu)
2482 {
2483 	struct netdev_queue *nq = netdev_get_tx_queue(port->dev, txq->log_id);
2484 	int tx_done;
2485 
2486 	if (txq_pcpu->thread != mvpp2_cpu_to_thread(port->priv, smp_processor_id()))
2487 		netdev_err(port->dev, "wrong cpu on the end of Tx processing\n");
2488 
2489 	tx_done = mvpp2_txq_sent_desc_proc(port, txq);
2490 	if (!tx_done)
2491 		return;
2492 	mvpp2_txq_bufs_free(port, txq, txq_pcpu, tx_done);
2493 
2494 	txq_pcpu->count -= tx_done;
2495 
2496 	if (netif_tx_queue_stopped(nq))
2497 		if (txq_pcpu->count <= txq_pcpu->wake_threshold)
2498 			netif_tx_wake_queue(nq);
2499 }
2500 
2501 static unsigned int mvpp2_tx_done(struct mvpp2_port *port, u32 cause,
2502 				  unsigned int thread)
2503 {
2504 	struct mvpp2_tx_queue *txq;
2505 	struct mvpp2_txq_pcpu *txq_pcpu;
2506 	unsigned int tx_todo = 0;
2507 
2508 	while (cause) {
2509 		txq = mvpp2_get_tx_queue(port, cause);
2510 		if (!txq)
2511 			break;
2512 
2513 		txq_pcpu = per_cpu_ptr(txq->pcpu, thread);
2514 
2515 		if (txq_pcpu->count) {
2516 			mvpp2_txq_done(port, txq, txq_pcpu);
2517 			tx_todo += txq_pcpu->count;
2518 		}
2519 
2520 		cause &= ~(1 << txq->log_id);
2521 	}
2522 	return tx_todo;
2523 }
2524 
2525 /* Rx/Tx queue initialization/cleanup methods */
2526 
2527 /* Allocate and initialize descriptors for aggr TXQ */
2528 static int mvpp2_aggr_txq_init(struct platform_device *pdev,
2529 			       struct mvpp2_tx_queue *aggr_txq,
2530 			       unsigned int thread, struct mvpp2 *priv)
2531 {
2532 	u32 txq_dma;
2533 
2534 	/* Allocate memory for TX descriptors */
2535 	aggr_txq->descs = dma_alloc_coherent(&pdev->dev,
2536 					     MVPP2_AGGR_TXQ_SIZE * MVPP2_DESC_ALIGNED_SIZE,
2537 					     &aggr_txq->descs_dma, GFP_KERNEL);
2538 	if (!aggr_txq->descs)
2539 		return -ENOMEM;
2540 
2541 	aggr_txq->last_desc = MVPP2_AGGR_TXQ_SIZE - 1;
2542 
2543 	/* Aggr TXQ no reset WA */
2544 	aggr_txq->next_desc_to_proc = mvpp2_read(priv,
2545 						 MVPP2_AGGR_TXQ_INDEX_REG(thread));
2546 
2547 	/* Set Tx descriptors queue starting address indirect
2548 	 * access
2549 	 */
2550 	if (priv->hw_version == MVPP21)
2551 		txq_dma = aggr_txq->descs_dma;
2552 	else
2553 		txq_dma = aggr_txq->descs_dma >>
2554 			MVPP22_AGGR_TXQ_DESC_ADDR_OFFS;
2555 
2556 	mvpp2_write(priv, MVPP2_AGGR_TXQ_DESC_ADDR_REG(thread), txq_dma);
2557 	mvpp2_write(priv, MVPP2_AGGR_TXQ_DESC_SIZE_REG(thread),
2558 		    MVPP2_AGGR_TXQ_SIZE);
2559 
2560 	return 0;
2561 }
2562 
2563 /* Create a specified Rx queue */
2564 static int mvpp2_rxq_init(struct mvpp2_port *port,
2565 			  struct mvpp2_rx_queue *rxq)
2566 {
2567 	struct mvpp2 *priv = port->priv;
2568 	unsigned int thread;
2569 	u32 rxq_dma;
2570 	int err;
2571 
2572 	rxq->size = port->rx_ring_size;
2573 
2574 	/* Allocate memory for RX descriptors */
2575 	rxq->descs = dma_alloc_coherent(port->dev->dev.parent,
2576 					rxq->size * MVPP2_DESC_ALIGNED_SIZE,
2577 					&rxq->descs_dma, GFP_KERNEL);
2578 	if (!rxq->descs)
2579 		return -ENOMEM;
2580 
2581 	rxq->last_desc = rxq->size - 1;
2582 
2583 	/* Zero occupied and non-occupied counters - direct access */
2584 	mvpp2_write(port->priv, MVPP2_RXQ_STATUS_REG(rxq->id), 0);
2585 
2586 	/* Set Rx descriptors queue starting address - indirect access */
2587 	thread = mvpp2_cpu_to_thread(port->priv, get_cpu());
2588 	mvpp2_thread_write(port->priv, thread, MVPP2_RXQ_NUM_REG, rxq->id);
2589 	if (port->priv->hw_version == MVPP21)
2590 		rxq_dma = rxq->descs_dma;
2591 	else
2592 		rxq_dma = rxq->descs_dma >> MVPP22_DESC_ADDR_OFFS;
2593 	mvpp2_thread_write(port->priv, thread, MVPP2_RXQ_DESC_ADDR_REG, rxq_dma);
2594 	mvpp2_thread_write(port->priv, thread, MVPP2_RXQ_DESC_SIZE_REG, rxq->size);
2595 	mvpp2_thread_write(port->priv, thread, MVPP2_RXQ_INDEX_REG, 0);
2596 	put_cpu();
2597 
2598 	/* Set Offset */
2599 	mvpp2_rxq_offset_set(port, rxq->id, MVPP2_SKB_HEADROOM);
2600 
2601 	/* Set coalescing pkts and time */
2602 	mvpp2_rx_pkts_coal_set(port, rxq);
2603 	mvpp2_rx_time_coal_set(port, rxq);
2604 
2605 	/* Add number of descriptors ready for receiving packets */
2606 	mvpp2_rxq_status_update(port, rxq->id, 0, rxq->size);
2607 
2608 	if (priv->percpu_pools) {
2609 		err = xdp_rxq_info_reg(&rxq->xdp_rxq_short, port->dev, rxq->id);
2610 		if (err < 0)
2611 			goto err_free_dma;
2612 
2613 		err = xdp_rxq_info_reg(&rxq->xdp_rxq_long, port->dev, rxq->id);
2614 		if (err < 0)
2615 			goto err_unregister_rxq_short;
2616 
2617 		/* Every RXQ has a pool for short and another for long packets */
2618 		err = xdp_rxq_info_reg_mem_model(&rxq->xdp_rxq_short,
2619 						 MEM_TYPE_PAGE_POOL,
2620 						 priv->page_pool[rxq->logic_rxq]);
2621 		if (err < 0)
2622 			goto err_unregister_rxq_long;
2623 
2624 		err = xdp_rxq_info_reg_mem_model(&rxq->xdp_rxq_long,
2625 						 MEM_TYPE_PAGE_POOL,
2626 						 priv->page_pool[rxq->logic_rxq +
2627 								 port->nrxqs]);
2628 		if (err < 0)
2629 			goto err_unregister_mem_rxq_short;
2630 	}
2631 
2632 	return 0;
2633 
2634 err_unregister_mem_rxq_short:
2635 	xdp_rxq_info_unreg_mem_model(&rxq->xdp_rxq_short);
2636 err_unregister_rxq_long:
2637 	xdp_rxq_info_unreg(&rxq->xdp_rxq_long);
2638 err_unregister_rxq_short:
2639 	xdp_rxq_info_unreg(&rxq->xdp_rxq_short);
2640 err_free_dma:
2641 	dma_free_coherent(port->dev->dev.parent,
2642 			  rxq->size * MVPP2_DESC_ALIGNED_SIZE,
2643 			  rxq->descs, rxq->descs_dma);
2644 	return err;
2645 }
2646 
2647 /* Push packets received by the RXQ to BM pool */
2648 static void mvpp2_rxq_drop_pkts(struct mvpp2_port *port,
2649 				struct mvpp2_rx_queue *rxq)
2650 {
2651 	int rx_received, i;
2652 
2653 	rx_received = mvpp2_rxq_received(port, rxq->id);
2654 	if (!rx_received)
2655 		return;
2656 
2657 	for (i = 0; i < rx_received; i++) {
2658 		struct mvpp2_rx_desc *rx_desc = mvpp2_rxq_next_desc_get(rxq);
2659 		u32 status = mvpp2_rxdesc_status_get(port, rx_desc);
2660 		int pool;
2661 
2662 		pool = (status & MVPP2_RXD_BM_POOL_ID_MASK) >>
2663 			MVPP2_RXD_BM_POOL_ID_OFFS;
2664 
2665 		mvpp2_bm_pool_put(port, pool,
2666 				  mvpp2_rxdesc_dma_addr_get(port, rx_desc),
2667 				  mvpp2_rxdesc_cookie_get(port, rx_desc));
2668 	}
2669 	mvpp2_rxq_status_update(port, rxq->id, rx_received, rx_received);
2670 }
2671 
2672 /* Cleanup Rx queue */
2673 static void mvpp2_rxq_deinit(struct mvpp2_port *port,
2674 			     struct mvpp2_rx_queue *rxq)
2675 {
2676 	unsigned int thread;
2677 
2678 	if (xdp_rxq_info_is_reg(&rxq->xdp_rxq_short))
2679 		xdp_rxq_info_unreg(&rxq->xdp_rxq_short);
2680 
2681 	if (xdp_rxq_info_is_reg(&rxq->xdp_rxq_long))
2682 		xdp_rxq_info_unreg(&rxq->xdp_rxq_long);
2683 
2684 	mvpp2_rxq_drop_pkts(port, rxq);
2685 
2686 	if (rxq->descs)
2687 		dma_free_coherent(port->dev->dev.parent,
2688 				  rxq->size * MVPP2_DESC_ALIGNED_SIZE,
2689 				  rxq->descs,
2690 				  rxq->descs_dma);
2691 
2692 	rxq->descs             = NULL;
2693 	rxq->last_desc         = 0;
2694 	rxq->next_desc_to_proc = 0;
2695 	rxq->descs_dma         = 0;
2696 
2697 	/* Clear Rx descriptors queue starting address and size;
2698 	 * free descriptor number
2699 	 */
2700 	mvpp2_write(port->priv, MVPP2_RXQ_STATUS_REG(rxq->id), 0);
2701 	thread = mvpp2_cpu_to_thread(port->priv, get_cpu());
2702 	mvpp2_thread_write(port->priv, thread, MVPP2_RXQ_NUM_REG, rxq->id);
2703 	mvpp2_thread_write(port->priv, thread, MVPP2_RXQ_DESC_ADDR_REG, 0);
2704 	mvpp2_thread_write(port->priv, thread, MVPP2_RXQ_DESC_SIZE_REG, 0);
2705 	put_cpu();
2706 }
2707 
2708 /* Create and initialize a Tx queue */
2709 static int mvpp2_txq_init(struct mvpp2_port *port,
2710 			  struct mvpp2_tx_queue *txq)
2711 {
2712 	u32 val;
2713 	unsigned int thread;
2714 	int desc, desc_per_txq, tx_port_num;
2715 	struct mvpp2_txq_pcpu *txq_pcpu;
2716 
2717 	txq->size = port->tx_ring_size;
2718 
2719 	/* Allocate memory for Tx descriptors */
2720 	txq->descs = dma_alloc_coherent(port->dev->dev.parent,
2721 				txq->size * MVPP2_DESC_ALIGNED_SIZE,
2722 				&txq->descs_dma, GFP_KERNEL);
2723 	if (!txq->descs)
2724 		return -ENOMEM;
2725 
2726 	txq->last_desc = txq->size - 1;
2727 
2728 	/* Set Tx descriptors queue starting address - indirect access */
2729 	thread = mvpp2_cpu_to_thread(port->priv, get_cpu());
2730 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_NUM_REG, txq->id);
2731 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_DESC_ADDR_REG,
2732 			   txq->descs_dma);
2733 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_DESC_SIZE_REG,
2734 			   txq->size & MVPP2_TXQ_DESC_SIZE_MASK);
2735 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_INDEX_REG, 0);
2736 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_RSVD_CLR_REG,
2737 			   txq->id << MVPP2_TXQ_RSVD_CLR_OFFSET);
2738 	val = mvpp2_thread_read(port->priv, thread, MVPP2_TXQ_PENDING_REG);
2739 	val &= ~MVPP2_TXQ_PENDING_MASK;
2740 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_PENDING_REG, val);
2741 
2742 	/* Calculate base address in prefetch buffer. We reserve 16 descriptors
2743 	 * for each existing TXQ.
2744 	 * TCONTS for PON port must be continuous from 0 to MVPP2_MAX_TCONT
2745 	 * GBE ports assumed to be continuous from 0 to MVPP2_MAX_PORTS
2746 	 */
2747 	desc_per_txq = 16;
2748 	desc = (port->id * MVPP2_MAX_TXQ * desc_per_txq) +
2749 	       (txq->log_id * desc_per_txq);
2750 
2751 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_PREF_BUF_REG,
2752 			   MVPP2_PREF_BUF_PTR(desc) | MVPP2_PREF_BUF_SIZE_16 |
2753 			   MVPP2_PREF_BUF_THRESH(desc_per_txq / 2));
2754 	put_cpu();
2755 
2756 	/* WRR / EJP configuration - indirect access */
2757 	tx_port_num = mvpp2_egress_port(port);
2758 	mvpp2_write(port->priv, MVPP2_TXP_SCHED_PORT_INDEX_REG, tx_port_num);
2759 
2760 	val = mvpp2_read(port->priv, MVPP2_TXQ_SCHED_REFILL_REG(txq->log_id));
2761 	val &= ~MVPP2_TXQ_REFILL_PERIOD_ALL_MASK;
2762 	val |= MVPP2_TXQ_REFILL_PERIOD_MASK(1);
2763 	val |= MVPP2_TXQ_REFILL_TOKENS_ALL_MASK;
2764 	mvpp2_write(port->priv, MVPP2_TXQ_SCHED_REFILL_REG(txq->log_id), val);
2765 
2766 	val = MVPP2_TXQ_TOKEN_SIZE_MAX;
2767 	mvpp2_write(port->priv, MVPP2_TXQ_SCHED_TOKEN_SIZE_REG(txq->log_id),
2768 		    val);
2769 
2770 	for (thread = 0; thread < port->priv->nthreads; thread++) {
2771 		txq_pcpu = per_cpu_ptr(txq->pcpu, thread);
2772 		txq_pcpu->size = txq->size;
2773 		txq_pcpu->buffs = kmalloc_array(txq_pcpu->size,
2774 						sizeof(*txq_pcpu->buffs),
2775 						GFP_KERNEL);
2776 		if (!txq_pcpu->buffs)
2777 			return -ENOMEM;
2778 
2779 		txq_pcpu->count = 0;
2780 		txq_pcpu->reserved_num = 0;
2781 		txq_pcpu->txq_put_index = 0;
2782 		txq_pcpu->txq_get_index = 0;
2783 		txq_pcpu->tso_headers = NULL;
2784 
2785 		txq_pcpu->stop_threshold = txq->size - MVPP2_MAX_SKB_DESCS;
2786 		txq_pcpu->wake_threshold = txq_pcpu->stop_threshold / 2;
2787 
2788 		txq_pcpu->tso_headers =
2789 			dma_alloc_coherent(port->dev->dev.parent,
2790 					   txq_pcpu->size * TSO_HEADER_SIZE,
2791 					   &txq_pcpu->tso_headers_dma,
2792 					   GFP_KERNEL);
2793 		if (!txq_pcpu->tso_headers)
2794 			return -ENOMEM;
2795 	}
2796 
2797 	return 0;
2798 }
2799 
2800 /* Free allocated TXQ resources */
2801 static void mvpp2_txq_deinit(struct mvpp2_port *port,
2802 			     struct mvpp2_tx_queue *txq)
2803 {
2804 	struct mvpp2_txq_pcpu *txq_pcpu;
2805 	unsigned int thread;
2806 
2807 	for (thread = 0; thread < port->priv->nthreads; thread++) {
2808 		txq_pcpu = per_cpu_ptr(txq->pcpu, thread);
2809 		kfree(txq_pcpu->buffs);
2810 
2811 		if (txq_pcpu->tso_headers)
2812 			dma_free_coherent(port->dev->dev.parent,
2813 					  txq_pcpu->size * TSO_HEADER_SIZE,
2814 					  txq_pcpu->tso_headers,
2815 					  txq_pcpu->tso_headers_dma);
2816 
2817 		txq_pcpu->tso_headers = NULL;
2818 	}
2819 
2820 	if (txq->descs)
2821 		dma_free_coherent(port->dev->dev.parent,
2822 				  txq->size * MVPP2_DESC_ALIGNED_SIZE,
2823 				  txq->descs, txq->descs_dma);
2824 
2825 	txq->descs             = NULL;
2826 	txq->last_desc         = 0;
2827 	txq->next_desc_to_proc = 0;
2828 	txq->descs_dma         = 0;
2829 
2830 	/* Set minimum bandwidth for disabled TXQs */
2831 	mvpp2_write(port->priv, MVPP2_TXQ_SCHED_TOKEN_CNTR_REG(txq->log_id), 0);
2832 
2833 	/* Set Tx descriptors queue starting address and size */
2834 	thread = mvpp2_cpu_to_thread(port->priv, get_cpu());
2835 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_NUM_REG, txq->id);
2836 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_DESC_ADDR_REG, 0);
2837 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_DESC_SIZE_REG, 0);
2838 	put_cpu();
2839 }
2840 
2841 /* Cleanup Tx ports */
2842 static void mvpp2_txq_clean(struct mvpp2_port *port, struct mvpp2_tx_queue *txq)
2843 {
2844 	struct mvpp2_txq_pcpu *txq_pcpu;
2845 	int delay, pending;
2846 	unsigned int thread = mvpp2_cpu_to_thread(port->priv, get_cpu());
2847 	u32 val;
2848 
2849 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_NUM_REG, txq->id);
2850 	val = mvpp2_thread_read(port->priv, thread, MVPP2_TXQ_PREF_BUF_REG);
2851 	val |= MVPP2_TXQ_DRAIN_EN_MASK;
2852 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_PREF_BUF_REG, val);
2853 
2854 	/* The napi queue has been stopped so wait for all packets
2855 	 * to be transmitted.
2856 	 */
2857 	delay = 0;
2858 	do {
2859 		if (delay >= MVPP2_TX_PENDING_TIMEOUT_MSEC) {
2860 			netdev_warn(port->dev,
2861 				    "port %d: cleaning queue %d timed out\n",
2862 				    port->id, txq->log_id);
2863 			break;
2864 		}
2865 		mdelay(1);
2866 		delay++;
2867 
2868 		pending = mvpp2_thread_read(port->priv, thread,
2869 					    MVPP2_TXQ_PENDING_REG);
2870 		pending &= MVPP2_TXQ_PENDING_MASK;
2871 	} while (pending);
2872 
2873 	val &= ~MVPP2_TXQ_DRAIN_EN_MASK;
2874 	mvpp2_thread_write(port->priv, thread, MVPP2_TXQ_PREF_BUF_REG, val);
2875 	put_cpu();
2876 
2877 	for (thread = 0; thread < port->priv->nthreads; thread++) {
2878 		txq_pcpu = per_cpu_ptr(txq->pcpu, thread);
2879 
2880 		/* Release all packets */
2881 		mvpp2_txq_bufs_free(port, txq, txq_pcpu, txq_pcpu->count);
2882 
2883 		/* Reset queue */
2884 		txq_pcpu->count = 0;
2885 		txq_pcpu->txq_put_index = 0;
2886 		txq_pcpu->txq_get_index = 0;
2887 	}
2888 }
2889 
2890 /* Cleanup all Tx queues */
2891 static void mvpp2_cleanup_txqs(struct mvpp2_port *port)
2892 {
2893 	struct mvpp2_tx_queue *txq;
2894 	int queue;
2895 	u32 val;
2896 
2897 	val = mvpp2_read(port->priv, MVPP2_TX_PORT_FLUSH_REG);
2898 
2899 	/* Reset Tx ports and delete Tx queues */
2900 	val |= MVPP2_TX_PORT_FLUSH_MASK(port->id);
2901 	mvpp2_write(port->priv, MVPP2_TX_PORT_FLUSH_REG, val);
2902 
2903 	for (queue = 0; queue < port->ntxqs; queue++) {
2904 		txq = port->txqs[queue];
2905 		mvpp2_txq_clean(port, txq);
2906 		mvpp2_txq_deinit(port, txq);
2907 	}
2908 
2909 	on_each_cpu(mvpp2_txq_sent_counter_clear, port, 1);
2910 
2911 	val &= ~MVPP2_TX_PORT_FLUSH_MASK(port->id);
2912 	mvpp2_write(port->priv, MVPP2_TX_PORT_FLUSH_REG, val);
2913 }
2914 
2915 /* Cleanup all Rx queues */
2916 static void mvpp2_cleanup_rxqs(struct mvpp2_port *port)
2917 {
2918 	int queue;
2919 
2920 	for (queue = 0; queue < port->nrxqs; queue++)
2921 		mvpp2_rxq_deinit(port, port->rxqs[queue]);
2922 }
2923 
2924 /* Init all Rx queues for port */
2925 static int mvpp2_setup_rxqs(struct mvpp2_port *port)
2926 {
2927 	int queue, err;
2928 
2929 	for (queue = 0; queue < port->nrxqs; queue++) {
2930 		err = mvpp2_rxq_init(port, port->rxqs[queue]);
2931 		if (err)
2932 			goto err_cleanup;
2933 	}
2934 	return 0;
2935 
2936 err_cleanup:
2937 	mvpp2_cleanup_rxqs(port);
2938 	return err;
2939 }
2940 
2941 /* Init all tx queues for port */
2942 static int mvpp2_setup_txqs(struct mvpp2_port *port)
2943 {
2944 	struct mvpp2_tx_queue *txq;
2945 	int queue, err;
2946 
2947 	for (queue = 0; queue < port->ntxqs; queue++) {
2948 		txq = port->txqs[queue];
2949 		err = mvpp2_txq_init(port, txq);
2950 		if (err)
2951 			goto err_cleanup;
2952 
2953 		/* Assign this queue to a CPU */
2954 		if (queue < num_possible_cpus())
2955 			netif_set_xps_queue(port->dev, cpumask_of(queue), queue);
2956 	}
2957 
2958 	if (port->has_tx_irqs) {
2959 		mvpp2_tx_time_coal_set(port);
2960 		for (queue = 0; queue < port->ntxqs; queue++) {
2961 			txq = port->txqs[queue];
2962 			mvpp2_tx_pkts_coal_set(port, txq);
2963 		}
2964 	}
2965 
2966 	on_each_cpu(mvpp2_txq_sent_counter_clear, port, 1);
2967 	return 0;
2968 
2969 err_cleanup:
2970 	mvpp2_cleanup_txqs(port);
2971 	return err;
2972 }
2973 
2974 /* The callback for per-port interrupt */
2975 static irqreturn_t mvpp2_isr(int irq, void *dev_id)
2976 {
2977 	struct mvpp2_queue_vector *qv = dev_id;
2978 
2979 	mvpp2_qvec_interrupt_disable(qv);
2980 
2981 	napi_schedule(&qv->napi);
2982 
2983 	return IRQ_HANDLED;
2984 }
2985 
2986 static void mvpp2_isr_handle_ptp_queue(struct mvpp2_port *port, int nq)
2987 {
2988 	struct skb_shared_hwtstamps shhwtstamps;
2989 	struct mvpp2_hwtstamp_queue *queue;
2990 	struct sk_buff *skb;
2991 	void __iomem *ptp_q;
2992 	unsigned int id;
2993 	u32 r0, r1, r2;
2994 
2995 	ptp_q = port->priv->iface_base + MVPP22_PTP_BASE(port->gop_id);
2996 	if (nq)
2997 		ptp_q += MVPP22_PTP_TX_Q1_R0 - MVPP22_PTP_TX_Q0_R0;
2998 
2999 	queue = &port->tx_hwtstamp_queue[nq];
3000 
3001 	while (1) {
3002 		r0 = readl_relaxed(ptp_q + MVPP22_PTP_TX_Q0_R0) & 0xffff;
3003 		if (!r0)
3004 			break;
3005 
3006 		r1 = readl_relaxed(ptp_q + MVPP22_PTP_TX_Q0_R1) & 0xffff;
3007 		r2 = readl_relaxed(ptp_q + MVPP22_PTP_TX_Q0_R2) & 0xffff;
3008 
3009 		id = (r0 >> 1) & 31;
3010 
3011 		skb = queue->skb[id];
3012 		queue->skb[id] = NULL;
3013 		if (skb) {
3014 			u32 ts = r2 << 19 | r1 << 3 | r0 >> 13;
3015 
3016 			mvpp22_tai_tstamp(port->priv->tai, ts, &shhwtstamps);
3017 			skb_tstamp_tx(skb, &shhwtstamps);
3018 			dev_kfree_skb_any(skb);
3019 		}
3020 	}
3021 }
3022 
3023 static void mvpp2_isr_handle_ptp(struct mvpp2_port *port)
3024 {
3025 	void __iomem *ptp;
3026 	u32 val;
3027 
3028 	ptp = port->priv->iface_base + MVPP22_PTP_BASE(port->gop_id);
3029 	val = readl(ptp + MVPP22_PTP_INT_CAUSE);
3030 	if (val & MVPP22_PTP_INT_CAUSE_QUEUE0)
3031 		mvpp2_isr_handle_ptp_queue(port, 0);
3032 	if (val & MVPP22_PTP_INT_CAUSE_QUEUE1)
3033 		mvpp2_isr_handle_ptp_queue(port, 1);
3034 }
3035 
3036 static void mvpp2_isr_handle_link(struct mvpp2_port *port, bool link)
3037 {
3038 	struct net_device *dev = port->dev;
3039 
3040 	if (port->phylink) {
3041 		phylink_mac_change(port->phylink, link);
3042 		return;
3043 	}
3044 
3045 	if (!netif_running(dev))
3046 		return;
3047 
3048 	if (link) {
3049 		mvpp2_interrupts_enable(port);
3050 
3051 		mvpp2_egress_enable(port);
3052 		mvpp2_ingress_enable(port);
3053 		netif_carrier_on(dev);
3054 		netif_tx_wake_all_queues(dev);
3055 	} else {
3056 		netif_tx_stop_all_queues(dev);
3057 		netif_carrier_off(dev);
3058 		mvpp2_ingress_disable(port);
3059 		mvpp2_egress_disable(port);
3060 
3061 		mvpp2_interrupts_disable(port);
3062 	}
3063 }
3064 
3065 static void mvpp2_isr_handle_xlg(struct mvpp2_port *port)
3066 {
3067 	bool link;
3068 	u32 val;
3069 
3070 	val = readl(port->base + MVPP22_XLG_INT_STAT);
3071 	if (val & MVPP22_XLG_INT_STAT_LINK) {
3072 		val = readl(port->base + MVPP22_XLG_STATUS);
3073 		link = (val & MVPP22_XLG_STATUS_LINK_UP);
3074 		mvpp2_isr_handle_link(port, link);
3075 	}
3076 }
3077 
3078 static void mvpp2_isr_handle_gmac_internal(struct mvpp2_port *port)
3079 {
3080 	bool link;
3081 	u32 val;
3082 
3083 	if (phy_interface_mode_is_rgmii(port->phy_interface) ||
3084 	    phy_interface_mode_is_8023z(port->phy_interface) ||
3085 	    port->phy_interface == PHY_INTERFACE_MODE_SGMII) {
3086 		val = readl(port->base + MVPP22_GMAC_INT_STAT);
3087 		if (val & MVPP22_GMAC_INT_STAT_LINK) {
3088 			val = readl(port->base + MVPP2_GMAC_STATUS0);
3089 			link = (val & MVPP2_GMAC_STATUS0_LINK_UP);
3090 			mvpp2_isr_handle_link(port, link);
3091 		}
3092 	}
3093 }
3094 
3095 /* Per-port interrupt for link status changes */
3096 static irqreturn_t mvpp2_port_isr(int irq, void *dev_id)
3097 {
3098 	struct mvpp2_port *port = (struct mvpp2_port *)dev_id;
3099 	u32 val;
3100 
3101 	mvpp22_gop_mask_irq(port);
3102 
3103 	if (mvpp2_port_supports_xlg(port) &&
3104 	    mvpp2_is_xlg(port->phy_interface)) {
3105 		/* Check the external status register */
3106 		val = readl(port->base + MVPP22_XLG_EXT_INT_STAT);
3107 		if (val & MVPP22_XLG_EXT_INT_STAT_XLG)
3108 			mvpp2_isr_handle_xlg(port);
3109 		if (val & MVPP22_XLG_EXT_INT_STAT_PTP)
3110 			mvpp2_isr_handle_ptp(port);
3111 	} else {
3112 		/* If it's not the XLG, we must be using the GMAC.
3113 		 * Check the summary status.
3114 		 */
3115 		val = readl(port->base + MVPP22_GMAC_INT_SUM_STAT);
3116 		if (val & MVPP22_GMAC_INT_SUM_STAT_INTERNAL)
3117 			mvpp2_isr_handle_gmac_internal(port);
3118 		if (val & MVPP22_GMAC_INT_SUM_STAT_PTP)
3119 			mvpp2_isr_handle_ptp(port);
3120 	}
3121 
3122 	mvpp22_gop_unmask_irq(port);
3123 	return IRQ_HANDLED;
3124 }
3125 
3126 static enum hrtimer_restart mvpp2_hr_timer_cb(struct hrtimer *timer)
3127 {
3128 	struct net_device *dev;
3129 	struct mvpp2_port *port;
3130 	struct mvpp2_port_pcpu *port_pcpu;
3131 	unsigned int tx_todo, cause;
3132 
3133 	port_pcpu = container_of(timer, struct mvpp2_port_pcpu, tx_done_timer);
3134 	dev = port_pcpu->dev;
3135 
3136 	if (!netif_running(dev))
3137 		return HRTIMER_NORESTART;
3138 
3139 	port_pcpu->timer_scheduled = false;
3140 	port = netdev_priv(dev);
3141 
3142 	/* Process all the Tx queues */
3143 	cause = (1 << port->ntxqs) - 1;
3144 	tx_todo = mvpp2_tx_done(port, cause,
3145 				mvpp2_cpu_to_thread(port->priv, smp_processor_id()));
3146 
3147 	/* Set the timer in case not all the packets were processed */
3148 	if (tx_todo && !port_pcpu->timer_scheduled) {
3149 		port_pcpu->timer_scheduled = true;
3150 		hrtimer_forward_now(&port_pcpu->tx_done_timer,
3151 				    MVPP2_TXDONE_HRTIMER_PERIOD_NS);
3152 
3153 		return HRTIMER_RESTART;
3154 	}
3155 	return HRTIMER_NORESTART;
3156 }
3157 
3158 /* Main RX/TX processing routines */
3159 
3160 /* Display more error info */
3161 static void mvpp2_rx_error(struct mvpp2_port *port,
3162 			   struct mvpp2_rx_desc *rx_desc)
3163 {
3164 	u32 status = mvpp2_rxdesc_status_get(port, rx_desc);
3165 	size_t sz = mvpp2_rxdesc_size_get(port, rx_desc);
3166 	char *err_str = NULL;
3167 
3168 	switch (status & MVPP2_RXD_ERR_CODE_MASK) {
3169 	case MVPP2_RXD_ERR_CRC:
3170 		err_str = "crc";
3171 		break;
3172 	case MVPP2_RXD_ERR_OVERRUN:
3173 		err_str = "overrun";
3174 		break;
3175 	case MVPP2_RXD_ERR_RESOURCE:
3176 		err_str = "resource";
3177 		break;
3178 	}
3179 	if (err_str && net_ratelimit())
3180 		netdev_err(port->dev,
3181 			   "bad rx status %08x (%s error), size=%zu\n",
3182 			   status, err_str, sz);
3183 }
3184 
3185 /* Handle RX checksum offload */
3186 static void mvpp2_rx_csum(struct mvpp2_port *port, u32 status,
3187 			  struct sk_buff *skb)
3188 {
3189 	if (((status & MVPP2_RXD_L3_IP4) &&
3190 	     !(status & MVPP2_RXD_IP4_HEADER_ERR)) ||
3191 	    (status & MVPP2_RXD_L3_IP6))
3192 		if (((status & MVPP2_RXD_L4_UDP) ||
3193 		     (status & MVPP2_RXD_L4_TCP)) &&
3194 		     (status & MVPP2_RXD_L4_CSUM_OK)) {
3195 			skb->csum = 0;
3196 			skb->ip_summed = CHECKSUM_UNNECESSARY;
3197 			return;
3198 		}
3199 
3200 	skb->ip_summed = CHECKSUM_NONE;
3201 }
3202 
3203 /* Allocate a new skb and add it to BM pool */
3204 static int mvpp2_rx_refill(struct mvpp2_port *port,
3205 			   struct mvpp2_bm_pool *bm_pool,
3206 			   struct page_pool *page_pool, int pool)
3207 {
3208 	dma_addr_t dma_addr;
3209 	phys_addr_t phys_addr;
3210 	void *buf;
3211 
3212 	buf = mvpp2_buf_alloc(port, bm_pool, page_pool,
3213 			      &dma_addr, &phys_addr, GFP_ATOMIC);
3214 	if (!buf)
3215 		return -ENOMEM;
3216 
3217 	mvpp2_bm_pool_put(port, pool, dma_addr, phys_addr);
3218 
3219 	return 0;
3220 }
3221 
3222 /* Handle tx checksum */
3223 static u32 mvpp2_skb_tx_csum(struct mvpp2_port *port, struct sk_buff *skb)
3224 {
3225 	if (skb->ip_summed == CHECKSUM_PARTIAL) {
3226 		int ip_hdr_len = 0;
3227 		u8 l4_proto;
3228 		__be16 l3_proto = vlan_get_protocol(skb);
3229 
3230 		if (l3_proto == htons(ETH_P_IP)) {
3231 			struct iphdr *ip4h = ip_hdr(skb);
3232 
3233 			/* Calculate IPv4 checksum and L4 checksum */
3234 			ip_hdr_len = ip4h->ihl;
3235 			l4_proto = ip4h->protocol;
3236 		} else if (l3_proto == htons(ETH_P_IPV6)) {
3237 			struct ipv6hdr *ip6h = ipv6_hdr(skb);
3238 
3239 			/* Read l4_protocol from one of IPv6 extra headers */
3240 			if (skb_network_header_len(skb) > 0)
3241 				ip_hdr_len = (skb_network_header_len(skb) >> 2);
3242 			l4_proto = ip6h->nexthdr;
3243 		} else {
3244 			return MVPP2_TXD_L4_CSUM_NOT;
3245 		}
3246 
3247 		return mvpp2_txq_desc_csum(skb_network_offset(skb),
3248 					   l3_proto, ip_hdr_len, l4_proto);
3249 	}
3250 
3251 	return MVPP2_TXD_L4_CSUM_NOT | MVPP2_TXD_IP_CSUM_DISABLE;
3252 }
3253 
3254 static void mvpp2_xdp_finish_tx(struct mvpp2_port *port, u16 txq_id, int nxmit, int nxmit_byte)
3255 {
3256 	unsigned int thread = mvpp2_cpu_to_thread(port->priv, smp_processor_id());
3257 	struct mvpp2_tx_queue *aggr_txq;
3258 	struct mvpp2_txq_pcpu *txq_pcpu;
3259 	struct mvpp2_tx_queue *txq;
3260 	struct netdev_queue *nq;
3261 
3262 	txq = port->txqs[txq_id];
3263 	txq_pcpu = per_cpu_ptr(txq->pcpu, thread);
3264 	nq = netdev_get_tx_queue(port->dev, txq_id);
3265 	aggr_txq = &port->priv->aggr_txqs[thread];
3266 
3267 	txq_pcpu->reserved_num -= nxmit;
3268 	txq_pcpu->count += nxmit;
3269 	aggr_txq->count += nxmit;
3270 
3271 	/* Enable transmit */
3272 	wmb();
3273 	mvpp2_aggr_txq_pend_desc_add(port, nxmit);
3274 
3275 	if (txq_pcpu->count >= txq_pcpu->stop_threshold)
3276 		netif_tx_stop_queue(nq);
3277 
3278 	/* Finalize TX processing */
3279 	if (!port->has_tx_irqs && txq_pcpu->count >= txq->done_pkts_coal)
3280 		mvpp2_txq_done(port, txq, txq_pcpu);
3281 }
3282 
3283 static int
3284 mvpp2_xdp_submit_frame(struct mvpp2_port *port, u16 txq_id,
3285 		       struct xdp_frame *xdpf, bool dma_map)
3286 {
3287 	unsigned int thread = mvpp2_cpu_to_thread(port->priv, smp_processor_id());
3288 	u32 tx_cmd = MVPP2_TXD_L4_CSUM_NOT | MVPP2_TXD_IP_CSUM_DISABLE |
3289 		     MVPP2_TXD_F_DESC | MVPP2_TXD_L_DESC;
3290 	enum mvpp2_tx_buf_type buf_type;
3291 	struct mvpp2_txq_pcpu *txq_pcpu;
3292 	struct mvpp2_tx_queue *aggr_txq;
3293 	struct mvpp2_tx_desc *tx_desc;
3294 	struct mvpp2_tx_queue *txq;
3295 	int ret = MVPP2_XDP_TX;
3296 	dma_addr_t dma_addr;
3297 
3298 	txq = port->txqs[txq_id];
3299 	txq_pcpu = per_cpu_ptr(txq->pcpu, thread);
3300 	aggr_txq = &port->priv->aggr_txqs[thread];
3301 
3302 	/* Check number of available descriptors */
3303 	if (mvpp2_aggr_desc_num_check(port, aggr_txq, 1) ||
3304 	    mvpp2_txq_reserved_desc_num_proc(port, txq, txq_pcpu, 1)) {
3305 		ret = MVPP2_XDP_DROPPED;
3306 		goto out;
3307 	}
3308 
3309 	/* Get a descriptor for the first part of the packet */
3310 	tx_desc = mvpp2_txq_next_desc_get(aggr_txq);
3311 	mvpp2_txdesc_txq_set(port, tx_desc, txq->id);
3312 	mvpp2_txdesc_size_set(port, tx_desc, xdpf->len);
3313 
3314 	if (dma_map) {
3315 		/* XDP_REDIRECT or AF_XDP */
3316 		dma_addr = dma_map_single(port->dev->dev.parent, xdpf->data,
3317 					  xdpf->len, DMA_TO_DEVICE);
3318 
3319 		if (unlikely(dma_mapping_error(port->dev->dev.parent, dma_addr))) {
3320 			mvpp2_txq_desc_put(txq);
3321 			ret = MVPP2_XDP_DROPPED;
3322 			goto out;
3323 		}
3324 
3325 		buf_type = MVPP2_TYPE_XDP_NDO;
3326 	} else {
3327 		/* XDP_TX */
3328 		struct page *page = virt_to_page(xdpf->data);
3329 
3330 		dma_addr = page_pool_get_dma_addr(page) +
3331 			   sizeof(*xdpf) + xdpf->headroom;
3332 		dma_sync_single_for_device(port->dev->dev.parent, dma_addr,
3333 					   xdpf->len, DMA_BIDIRECTIONAL);
3334 
3335 		buf_type = MVPP2_TYPE_XDP_TX;
3336 	}
3337 
3338 	mvpp2_txdesc_dma_addr_set(port, tx_desc, dma_addr);
3339 
3340 	mvpp2_txdesc_cmd_set(port, tx_desc, tx_cmd);
3341 	mvpp2_txq_inc_put(port, txq_pcpu, xdpf, tx_desc, buf_type);
3342 
3343 out:
3344 	return ret;
3345 }
3346 
3347 static int
3348 mvpp2_xdp_xmit_back(struct mvpp2_port *port, struct xdp_buff *xdp)
3349 {
3350 	struct mvpp2_pcpu_stats *stats = this_cpu_ptr(port->stats);
3351 	struct xdp_frame *xdpf;
3352 	u16 txq_id;
3353 	int ret;
3354 
3355 	xdpf = xdp_convert_buff_to_frame(xdp);
3356 	if (unlikely(!xdpf))
3357 		return MVPP2_XDP_DROPPED;
3358 
3359 	/* The first of the TX queues are used for XPS,
3360 	 * the second half for XDP_TX
3361 	 */
3362 	txq_id = mvpp2_cpu_to_thread(port->priv, smp_processor_id()) + (port->ntxqs / 2);
3363 
3364 	ret = mvpp2_xdp_submit_frame(port, txq_id, xdpf, false);
3365 	if (ret == MVPP2_XDP_TX) {
3366 		u64_stats_update_begin(&stats->syncp);
3367 		stats->tx_bytes += xdpf->len;
3368 		stats->tx_packets++;
3369 		stats->xdp_tx++;
3370 		u64_stats_update_end(&stats->syncp);
3371 
3372 		mvpp2_xdp_finish_tx(port, txq_id, 1, xdpf->len);
3373 	} else {
3374 		u64_stats_update_begin(&stats->syncp);
3375 		stats->xdp_tx_err++;
3376 		u64_stats_update_end(&stats->syncp);
3377 	}
3378 
3379 	return ret;
3380 }
3381 
3382 static int
3383 mvpp2_xdp_xmit(struct net_device *dev, int num_frame,
3384 	       struct xdp_frame **frames, u32 flags)
3385 {
3386 	struct mvpp2_port *port = netdev_priv(dev);
3387 	int i, nxmit_byte = 0, nxmit = num_frame;
3388 	struct mvpp2_pcpu_stats *stats;
3389 	u16 txq_id;
3390 	u32 ret;
3391 
3392 	if (unlikely(test_bit(0, &port->state)))
3393 		return -ENETDOWN;
3394 
3395 	if (unlikely(flags & ~XDP_XMIT_FLAGS_MASK))
3396 		return -EINVAL;
3397 
3398 	/* The first of the TX queues are used for XPS,
3399 	 * the second half for XDP_TX
3400 	 */
3401 	txq_id = mvpp2_cpu_to_thread(port->priv, smp_processor_id()) + (port->ntxqs / 2);
3402 
3403 	for (i = 0; i < num_frame; i++) {
3404 		ret = mvpp2_xdp_submit_frame(port, txq_id, frames[i], true);
3405 		if (ret == MVPP2_XDP_TX) {
3406 			nxmit_byte += frames[i]->len;
3407 		} else {
3408 			xdp_return_frame_rx_napi(frames[i]);
3409 			nxmit--;
3410 		}
3411 	}
3412 
3413 	if (likely(nxmit > 0))
3414 		mvpp2_xdp_finish_tx(port, txq_id, nxmit, nxmit_byte);
3415 
3416 	stats = this_cpu_ptr(port->stats);
3417 	u64_stats_update_begin(&stats->syncp);
3418 	stats->tx_bytes += nxmit_byte;
3419 	stats->tx_packets += nxmit;
3420 	stats->xdp_xmit += nxmit;
3421 	stats->xdp_xmit_err += num_frame - nxmit;
3422 	u64_stats_update_end(&stats->syncp);
3423 
3424 	return nxmit;
3425 }
3426 
3427 static int
3428 mvpp2_run_xdp(struct mvpp2_port *port, struct mvpp2_rx_queue *rxq,
3429 	      struct bpf_prog *prog, struct xdp_buff *xdp,
3430 	      struct page_pool *pp, struct mvpp2_pcpu_stats *stats)
3431 {
3432 	unsigned int len, sync, err;
3433 	struct page *page;
3434 	u32 ret, act;
3435 
3436 	len = xdp->data_end - xdp->data_hard_start - MVPP2_SKB_HEADROOM;
3437 	act = bpf_prog_run_xdp(prog, xdp);
3438 
3439 	/* Due xdp_adjust_tail: DMA sync for_device cover max len CPU touch */
3440 	sync = xdp->data_end - xdp->data_hard_start - MVPP2_SKB_HEADROOM;
3441 	sync = max(sync, len);
3442 
3443 	switch (act) {
3444 	case XDP_PASS:
3445 		stats->xdp_pass++;
3446 		ret = MVPP2_XDP_PASS;
3447 		break;
3448 	case XDP_REDIRECT:
3449 		err = xdp_do_redirect(port->dev, xdp, prog);
3450 		if (unlikely(err)) {
3451 			ret = MVPP2_XDP_DROPPED;
3452 			page = virt_to_head_page(xdp->data);
3453 			page_pool_put_page(pp, page, sync, true);
3454 		} else {
3455 			ret = MVPP2_XDP_REDIR;
3456 			stats->xdp_redirect++;
3457 		}
3458 		break;
3459 	case XDP_TX:
3460 		ret = mvpp2_xdp_xmit_back(port, xdp);
3461 		if (ret != MVPP2_XDP_TX) {
3462 			page = virt_to_head_page(xdp->data);
3463 			page_pool_put_page(pp, page, sync, true);
3464 		}
3465 		break;
3466 	default:
3467 		bpf_warn_invalid_xdp_action(act);
3468 		fallthrough;
3469 	case XDP_ABORTED:
3470 		trace_xdp_exception(port->dev, prog, act);
3471 		fallthrough;
3472 	case XDP_DROP:
3473 		page = virt_to_head_page(xdp->data);
3474 		page_pool_put_page(pp, page, sync, true);
3475 		ret = MVPP2_XDP_DROPPED;
3476 		stats->xdp_drop++;
3477 		break;
3478 	}
3479 
3480 	return ret;
3481 }
3482 
3483 /* Main rx processing */
3484 static int mvpp2_rx(struct mvpp2_port *port, struct napi_struct *napi,
3485 		    int rx_todo, struct mvpp2_rx_queue *rxq)
3486 {
3487 	struct net_device *dev = port->dev;
3488 	struct mvpp2_pcpu_stats ps = {};
3489 	enum dma_data_direction dma_dir;
3490 	struct bpf_prog *xdp_prog;
3491 	struct xdp_buff xdp;
3492 	int rx_received;
3493 	int rx_done = 0;
3494 	u32 xdp_ret = 0;
3495 
3496 	rcu_read_lock();
3497 
3498 	xdp_prog = READ_ONCE(port->xdp_prog);
3499 
3500 	/* Get number of received packets and clamp the to-do */
3501 	rx_received = mvpp2_rxq_received(port, rxq->id);
3502 	if (rx_todo > rx_received)
3503 		rx_todo = rx_received;
3504 
3505 	while (rx_done < rx_todo) {
3506 		struct mvpp2_rx_desc *rx_desc = mvpp2_rxq_next_desc_get(rxq);
3507 		struct mvpp2_bm_pool *bm_pool;
3508 		struct page_pool *pp = NULL;
3509 		struct sk_buff *skb;
3510 		unsigned int frag_size;
3511 		dma_addr_t dma_addr;
3512 		phys_addr_t phys_addr;
3513 		u32 rx_status, timestamp;
3514 		int pool, rx_bytes, err, ret;
3515 		void *data;
3516 
3517 		rx_done++;
3518 		rx_status = mvpp2_rxdesc_status_get(port, rx_desc);
3519 		rx_bytes = mvpp2_rxdesc_size_get(port, rx_desc);
3520 		rx_bytes -= MVPP2_MH_SIZE;
3521 		dma_addr = mvpp2_rxdesc_dma_addr_get(port, rx_desc);
3522 		phys_addr = mvpp2_rxdesc_cookie_get(port, rx_desc);
3523 		data = (void *)phys_to_virt(phys_addr);
3524 
3525 		pool = (rx_status & MVPP2_RXD_BM_POOL_ID_MASK) >>
3526 			MVPP2_RXD_BM_POOL_ID_OFFS;
3527 		bm_pool = &port->priv->bm_pools[pool];
3528 
3529 		/* In case of an error, release the requested buffer pointer
3530 		 * to the Buffer Manager. This request process is controlled
3531 		 * by the hardware, and the information about the buffer is
3532 		 * comprised by the RX descriptor.
3533 		 */
3534 		if (rx_status & MVPP2_RXD_ERR_SUMMARY)
3535 			goto err_drop_frame;
3536 
3537 		if (port->priv->percpu_pools) {
3538 			pp = port->priv->page_pool[pool];
3539 			dma_dir = page_pool_get_dma_dir(pp);
3540 		} else {
3541 			dma_dir = DMA_FROM_DEVICE;
3542 		}
3543 
3544 		dma_sync_single_for_cpu(dev->dev.parent, dma_addr,
3545 					rx_bytes + MVPP2_MH_SIZE,
3546 					dma_dir);
3547 
3548 		/* Prefetch header */
3549 		prefetch(data);
3550 
3551 		if (bm_pool->frag_size > PAGE_SIZE)
3552 			frag_size = 0;
3553 		else
3554 			frag_size = bm_pool->frag_size;
3555 
3556 		if (xdp_prog) {
3557 			xdp.data_hard_start = data;
3558 			xdp.data = data + MVPP2_MH_SIZE + MVPP2_SKB_HEADROOM;
3559 			xdp.data_end = xdp.data + rx_bytes;
3560 			xdp.frame_sz = PAGE_SIZE;
3561 
3562 			if (bm_pool->pkt_size == MVPP2_BM_SHORT_PKT_SIZE)
3563 				xdp.rxq = &rxq->xdp_rxq_short;
3564 			else
3565 				xdp.rxq = &rxq->xdp_rxq_long;
3566 
3567 			xdp_set_data_meta_invalid(&xdp);
3568 
3569 			ret = mvpp2_run_xdp(port, rxq, xdp_prog, &xdp, pp, &ps);
3570 
3571 			if (ret) {
3572 				xdp_ret |= ret;
3573 				err = mvpp2_rx_refill(port, bm_pool, pp, pool);
3574 				if (err) {
3575 					netdev_err(port->dev, "failed to refill BM pools\n");
3576 					goto err_drop_frame;
3577 				}
3578 
3579 				ps.rx_packets++;
3580 				ps.rx_bytes += rx_bytes;
3581 				continue;
3582 			}
3583 		}
3584 
3585 		skb = build_skb(data, frag_size);
3586 		if (!skb) {
3587 			netdev_warn(port->dev, "skb build failed\n");
3588 			goto err_drop_frame;
3589 		}
3590 
3591 		/* If we have RX hardware timestamping enabled, grab the
3592 		 * timestamp from the queue and convert.
3593 		 */
3594 		if (mvpp22_rx_hwtstamping(port)) {
3595 			timestamp = le32_to_cpu(rx_desc->pp22.timestamp);
3596 			mvpp22_tai_tstamp(port->priv->tai, timestamp,
3597 					 skb_hwtstamps(skb));
3598 		}
3599 
3600 		err = mvpp2_rx_refill(port, bm_pool, pp, pool);
3601 		if (err) {
3602 			netdev_err(port->dev, "failed to refill BM pools\n");
3603 			dev_kfree_skb_any(skb);
3604 			goto err_drop_frame;
3605 		}
3606 
3607 		if (pp)
3608 			page_pool_release_page(pp, virt_to_page(data));
3609 		else
3610 			dma_unmap_single_attrs(dev->dev.parent, dma_addr,
3611 					       bm_pool->buf_size, DMA_FROM_DEVICE,
3612 					       DMA_ATTR_SKIP_CPU_SYNC);
3613 
3614 		ps.rx_packets++;
3615 		ps.rx_bytes += rx_bytes;
3616 
3617 		skb_reserve(skb, MVPP2_MH_SIZE + MVPP2_SKB_HEADROOM);
3618 		skb_put(skb, rx_bytes);
3619 		skb->protocol = eth_type_trans(skb, dev);
3620 		mvpp2_rx_csum(port, rx_status, skb);
3621 
3622 		napi_gro_receive(napi, skb);
3623 		continue;
3624 
3625 err_drop_frame:
3626 		dev->stats.rx_errors++;
3627 		mvpp2_rx_error(port, rx_desc);
3628 		/* Return the buffer to the pool */
3629 		mvpp2_bm_pool_put(port, pool, dma_addr, phys_addr);
3630 	}
3631 
3632 	rcu_read_unlock();
3633 
3634 	if (xdp_ret & MVPP2_XDP_REDIR)
3635 		xdp_do_flush_map();
3636 
3637 	if (ps.rx_packets) {
3638 		struct mvpp2_pcpu_stats *stats = this_cpu_ptr(port->stats);
3639 
3640 		u64_stats_update_begin(&stats->syncp);
3641 		stats->rx_packets += ps.rx_packets;
3642 		stats->rx_bytes   += ps.rx_bytes;
3643 		/* xdp */
3644 		stats->xdp_redirect += ps.xdp_redirect;
3645 		stats->xdp_pass += ps.xdp_pass;
3646 		stats->xdp_drop += ps.xdp_drop;
3647 		u64_stats_update_end(&stats->syncp);
3648 	}
3649 
3650 	/* Update Rx queue management counters */
3651 	wmb();
3652 	mvpp2_rxq_status_update(port, rxq->id, rx_done, rx_done);
3653 
3654 	return rx_todo;
3655 }
3656 
3657 static inline void
3658 tx_desc_unmap_put(struct mvpp2_port *port, struct mvpp2_tx_queue *txq,
3659 		  struct mvpp2_tx_desc *desc)
3660 {
3661 	unsigned int thread = mvpp2_cpu_to_thread(port->priv, smp_processor_id());
3662 	struct mvpp2_txq_pcpu *txq_pcpu = per_cpu_ptr(txq->pcpu, thread);
3663 
3664 	dma_addr_t buf_dma_addr =
3665 		mvpp2_txdesc_dma_addr_get(port, desc);
3666 	size_t buf_sz =
3667 		mvpp2_txdesc_size_get(port, desc);
3668 	if (!IS_TSO_HEADER(txq_pcpu, buf_dma_addr))
3669 		dma_unmap_single(port->dev->dev.parent, buf_dma_addr,
3670 				 buf_sz, DMA_TO_DEVICE);
3671 	mvpp2_txq_desc_put(txq);
3672 }
3673 
3674 static void mvpp2_txdesc_clear_ptp(struct mvpp2_port *port,
3675 				   struct mvpp2_tx_desc *desc)
3676 {
3677 	/* We only need to clear the low bits */
3678 	if (port->priv->hw_version != MVPP21)
3679 		desc->pp22.ptp_descriptor &=
3680 			cpu_to_le32(~MVPP22_PTP_DESC_MASK_LOW);
3681 }
3682 
3683 static bool mvpp2_tx_hw_tstamp(struct mvpp2_port *port,
3684 			       struct mvpp2_tx_desc *tx_desc,
3685 			       struct sk_buff *skb)
3686 {
3687 	struct mvpp2_hwtstamp_queue *queue;
3688 	unsigned int mtype, type, i;
3689 	struct ptp_header *hdr;
3690 	u64 ptpdesc;
3691 
3692 	if (port->priv->hw_version == MVPP21 ||
3693 	    port->tx_hwtstamp_type == HWTSTAMP_TX_OFF)
3694 		return false;
3695 
3696 	type = ptp_classify_raw(skb);
3697 	if (!type)
3698 		return false;
3699 
3700 	hdr = ptp_parse_header(skb, type);
3701 	if (!hdr)
3702 		return false;
3703 
3704 	skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
3705 
3706 	ptpdesc = MVPP22_PTP_MACTIMESTAMPINGEN |
3707 		  MVPP22_PTP_ACTION_CAPTURE;
3708 	queue = &port->tx_hwtstamp_queue[0];
3709 
3710 	switch (type & PTP_CLASS_VMASK) {
3711 	case PTP_CLASS_V1:
3712 		ptpdesc |= MVPP22_PTP_PACKETFORMAT(MVPP22_PTP_PKT_FMT_PTPV1);
3713 		break;
3714 
3715 	case PTP_CLASS_V2:
3716 		ptpdesc |= MVPP22_PTP_PACKETFORMAT(MVPP22_PTP_PKT_FMT_PTPV2);
3717 		mtype = hdr->tsmt & 15;
3718 		/* Direct PTP Sync messages to queue 1 */
3719 		if (mtype == 0) {
3720 			ptpdesc |= MVPP22_PTP_TIMESTAMPQUEUESELECT;
3721 			queue = &port->tx_hwtstamp_queue[1];
3722 		}
3723 		break;
3724 	}
3725 
3726 	/* Take a reference on the skb and insert into our queue */
3727 	i = queue->next;
3728 	queue->next = (i + 1) & 31;
3729 	if (queue->skb[i])
3730 		dev_kfree_skb_any(queue->skb[i]);
3731 	queue->skb[i] = skb_get(skb);
3732 
3733 	ptpdesc |= MVPP22_PTP_TIMESTAMPENTRYID(i);
3734 
3735 	/*
3736 	 * 3:0		- PTPAction
3737 	 * 6:4		- PTPPacketFormat
3738 	 * 7		- PTP_CF_WraparoundCheckEn
3739 	 * 9:8		- IngressTimestampSeconds[1:0]
3740 	 * 10		- Reserved
3741 	 * 11		- MACTimestampingEn
3742 	 * 17:12	- PTP_TimestampQueueEntryID[5:0]
3743 	 * 18		- PTPTimestampQueueSelect
3744 	 * 19		- UDPChecksumUpdateEn
3745 	 * 27:20	- TimestampOffset
3746 	 *			PTP, NTPTransmit, OWAMP/TWAMP - L3 to PTP header
3747 	 *			NTPTs, Y.1731 - L3 to timestamp entry
3748 	 * 35:28	- UDP Checksum Offset
3749 	 *
3750 	 * stored in tx descriptor bits 75:64 (11:0) and 191:168 (35:12)
3751 	 */
3752 	tx_desc->pp22.ptp_descriptor &=
3753 		cpu_to_le32(~MVPP22_PTP_DESC_MASK_LOW);
3754 	tx_desc->pp22.ptp_descriptor |=
3755 		cpu_to_le32(ptpdesc & MVPP22_PTP_DESC_MASK_LOW);
3756 	tx_desc->pp22.buf_dma_addr_ptp &= cpu_to_le64(~0xffffff0000000000ULL);
3757 	tx_desc->pp22.buf_dma_addr_ptp |= cpu_to_le64((ptpdesc >> 12) << 40);
3758 
3759 	return true;
3760 }
3761 
3762 /* Handle tx fragmentation processing */
3763 static int mvpp2_tx_frag_process(struct mvpp2_port *port, struct sk_buff *skb,
3764 				 struct mvpp2_tx_queue *aggr_txq,
3765 				 struct mvpp2_tx_queue *txq)
3766 {
3767 	unsigned int thread = mvpp2_cpu_to_thread(port->priv, smp_processor_id());
3768 	struct mvpp2_txq_pcpu *txq_pcpu = per_cpu_ptr(txq->pcpu, thread);
3769 	struct mvpp2_tx_desc *tx_desc;
3770 	int i;
3771 	dma_addr_t buf_dma_addr;
3772 
3773 	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
3774 		skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
3775 		void *addr = skb_frag_address(frag);
3776 
3777 		tx_desc = mvpp2_txq_next_desc_get(aggr_txq);
3778 		mvpp2_txdesc_clear_ptp(port, tx_desc);
3779 		mvpp2_txdesc_txq_set(port, tx_desc, txq->id);
3780 		mvpp2_txdesc_size_set(port, tx_desc, skb_frag_size(frag));
3781 
3782 		buf_dma_addr = dma_map_single(port->dev->dev.parent, addr,
3783 					      skb_frag_size(frag),
3784 					      DMA_TO_DEVICE);
3785 		if (dma_mapping_error(port->dev->dev.parent, buf_dma_addr)) {
3786 			mvpp2_txq_desc_put(txq);
3787 			goto cleanup;
3788 		}
3789 
3790 		mvpp2_txdesc_dma_addr_set(port, tx_desc, buf_dma_addr);
3791 
3792 		if (i == (skb_shinfo(skb)->nr_frags - 1)) {
3793 			/* Last descriptor */
3794 			mvpp2_txdesc_cmd_set(port, tx_desc,
3795 					     MVPP2_TXD_L_DESC);
3796 			mvpp2_txq_inc_put(port, txq_pcpu, skb, tx_desc, MVPP2_TYPE_SKB);
3797 		} else {
3798 			/* Descriptor in the middle: Not First, Not Last */
3799 			mvpp2_txdesc_cmd_set(port, tx_desc, 0);
3800 			mvpp2_txq_inc_put(port, txq_pcpu, NULL, tx_desc, MVPP2_TYPE_SKB);
3801 		}
3802 	}
3803 
3804 	return 0;
3805 cleanup:
3806 	/* Release all descriptors that were used to map fragments of
3807 	 * this packet, as well as the corresponding DMA mappings
3808 	 */
3809 	for (i = i - 1; i >= 0; i--) {
3810 		tx_desc = txq->descs + i;
3811 		tx_desc_unmap_put(port, txq, tx_desc);
3812 	}
3813 
3814 	return -ENOMEM;
3815 }
3816 
3817 static inline void mvpp2_tso_put_hdr(struct sk_buff *skb,
3818 				     struct net_device *dev,
3819 				     struct mvpp2_tx_queue *txq,
3820 				     struct mvpp2_tx_queue *aggr_txq,
3821 				     struct mvpp2_txq_pcpu *txq_pcpu,
3822 				     int hdr_sz)
3823 {
3824 	struct mvpp2_port *port = netdev_priv(dev);
3825 	struct mvpp2_tx_desc *tx_desc = mvpp2_txq_next_desc_get(aggr_txq);
3826 	dma_addr_t addr;
3827 
3828 	mvpp2_txdesc_clear_ptp(port, tx_desc);
3829 	mvpp2_txdesc_txq_set(port, tx_desc, txq->id);
3830 	mvpp2_txdesc_size_set(port, tx_desc, hdr_sz);
3831 
3832 	addr = txq_pcpu->tso_headers_dma +
3833 	       txq_pcpu->txq_put_index * TSO_HEADER_SIZE;
3834 	mvpp2_txdesc_dma_addr_set(port, tx_desc, addr);
3835 
3836 	mvpp2_txdesc_cmd_set(port, tx_desc, mvpp2_skb_tx_csum(port, skb) |
3837 					    MVPP2_TXD_F_DESC |
3838 					    MVPP2_TXD_PADDING_DISABLE);
3839 	mvpp2_txq_inc_put(port, txq_pcpu, NULL, tx_desc, MVPP2_TYPE_SKB);
3840 }
3841 
3842 static inline int mvpp2_tso_put_data(struct sk_buff *skb,
3843 				     struct net_device *dev, struct tso_t *tso,
3844 				     struct mvpp2_tx_queue *txq,
3845 				     struct mvpp2_tx_queue *aggr_txq,
3846 				     struct mvpp2_txq_pcpu *txq_pcpu,
3847 				     int sz, bool left, bool last)
3848 {
3849 	struct mvpp2_port *port = netdev_priv(dev);
3850 	struct mvpp2_tx_desc *tx_desc = mvpp2_txq_next_desc_get(aggr_txq);
3851 	dma_addr_t buf_dma_addr;
3852 
3853 	mvpp2_txdesc_clear_ptp(port, tx_desc);
3854 	mvpp2_txdesc_txq_set(port, tx_desc, txq->id);
3855 	mvpp2_txdesc_size_set(port, tx_desc, sz);
3856 
3857 	buf_dma_addr = dma_map_single(dev->dev.parent, tso->data, sz,
3858 				      DMA_TO_DEVICE);
3859 	if (unlikely(dma_mapping_error(dev->dev.parent, buf_dma_addr))) {
3860 		mvpp2_txq_desc_put(txq);
3861 		return -ENOMEM;
3862 	}
3863 
3864 	mvpp2_txdesc_dma_addr_set(port, tx_desc, buf_dma_addr);
3865 
3866 	if (!left) {
3867 		mvpp2_txdesc_cmd_set(port, tx_desc, MVPP2_TXD_L_DESC);
3868 		if (last) {
3869 			mvpp2_txq_inc_put(port, txq_pcpu, skb, tx_desc, MVPP2_TYPE_SKB);
3870 			return 0;
3871 		}
3872 	} else {
3873 		mvpp2_txdesc_cmd_set(port, tx_desc, 0);
3874 	}
3875 
3876 	mvpp2_txq_inc_put(port, txq_pcpu, NULL, tx_desc, MVPP2_TYPE_SKB);
3877 	return 0;
3878 }
3879 
3880 static int mvpp2_tx_tso(struct sk_buff *skb, struct net_device *dev,
3881 			struct mvpp2_tx_queue *txq,
3882 			struct mvpp2_tx_queue *aggr_txq,
3883 			struct mvpp2_txq_pcpu *txq_pcpu)
3884 {
3885 	struct mvpp2_port *port = netdev_priv(dev);
3886 	int hdr_sz, i, len, descs = 0;
3887 	struct tso_t tso;
3888 
3889 	/* Check number of available descriptors */
3890 	if (mvpp2_aggr_desc_num_check(port, aggr_txq, tso_count_descs(skb)) ||
3891 	    mvpp2_txq_reserved_desc_num_proc(port, txq, txq_pcpu,
3892 					     tso_count_descs(skb)))
3893 		return 0;
3894 
3895 	hdr_sz = tso_start(skb, &tso);
3896 
3897 	len = skb->len - hdr_sz;
3898 	while (len > 0) {
3899 		int left = min_t(int, skb_shinfo(skb)->gso_size, len);
3900 		char *hdr = txq_pcpu->tso_headers +
3901 			    txq_pcpu->txq_put_index * TSO_HEADER_SIZE;
3902 
3903 		len -= left;
3904 		descs++;
3905 
3906 		tso_build_hdr(skb, hdr, &tso, left, len == 0);
3907 		mvpp2_tso_put_hdr(skb, dev, txq, aggr_txq, txq_pcpu, hdr_sz);
3908 
3909 		while (left > 0) {
3910 			int sz = min_t(int, tso.size, left);
3911 			left -= sz;
3912 			descs++;
3913 
3914 			if (mvpp2_tso_put_data(skb, dev, &tso, txq, aggr_txq,
3915 					       txq_pcpu, sz, left, len == 0))
3916 				goto release;
3917 			tso_build_data(skb, &tso, sz);
3918 		}
3919 	}
3920 
3921 	return descs;
3922 
3923 release:
3924 	for (i = descs - 1; i >= 0; i--) {
3925 		struct mvpp2_tx_desc *tx_desc = txq->descs + i;
3926 		tx_desc_unmap_put(port, txq, tx_desc);
3927 	}
3928 	return 0;
3929 }
3930 
3931 /* Main tx processing */
3932 static netdev_tx_t mvpp2_tx(struct sk_buff *skb, struct net_device *dev)
3933 {
3934 	struct mvpp2_port *port = netdev_priv(dev);
3935 	struct mvpp2_tx_queue *txq, *aggr_txq;
3936 	struct mvpp2_txq_pcpu *txq_pcpu;
3937 	struct mvpp2_tx_desc *tx_desc;
3938 	dma_addr_t buf_dma_addr;
3939 	unsigned long flags = 0;
3940 	unsigned int thread;
3941 	int frags = 0;
3942 	u16 txq_id;
3943 	u32 tx_cmd;
3944 
3945 	thread = mvpp2_cpu_to_thread(port->priv, smp_processor_id());
3946 
3947 	txq_id = skb_get_queue_mapping(skb);
3948 	txq = port->txqs[txq_id];
3949 	txq_pcpu = per_cpu_ptr(txq->pcpu, thread);
3950 	aggr_txq = &port->priv->aggr_txqs[thread];
3951 
3952 	if (test_bit(thread, &port->priv->lock_map))
3953 		spin_lock_irqsave(&port->tx_lock[thread], flags);
3954 
3955 	if (skb_is_gso(skb)) {
3956 		frags = mvpp2_tx_tso(skb, dev, txq, aggr_txq, txq_pcpu);
3957 		goto out;
3958 	}
3959 	frags = skb_shinfo(skb)->nr_frags + 1;
3960 
3961 	/* Check number of available descriptors */
3962 	if (mvpp2_aggr_desc_num_check(port, aggr_txq, frags) ||
3963 	    mvpp2_txq_reserved_desc_num_proc(port, txq, txq_pcpu, frags)) {
3964 		frags = 0;
3965 		goto out;
3966 	}
3967 
3968 	/* Get a descriptor for the first part of the packet */
3969 	tx_desc = mvpp2_txq_next_desc_get(aggr_txq);
3970 	if (!(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) ||
3971 	    !mvpp2_tx_hw_tstamp(port, tx_desc, skb))
3972 		mvpp2_txdesc_clear_ptp(port, tx_desc);
3973 	mvpp2_txdesc_txq_set(port, tx_desc, txq->id);
3974 	mvpp2_txdesc_size_set(port, tx_desc, skb_headlen(skb));
3975 
3976 	buf_dma_addr = dma_map_single(dev->dev.parent, skb->data,
3977 				      skb_headlen(skb), DMA_TO_DEVICE);
3978 	if (unlikely(dma_mapping_error(dev->dev.parent, buf_dma_addr))) {
3979 		mvpp2_txq_desc_put(txq);
3980 		frags = 0;
3981 		goto out;
3982 	}
3983 
3984 	mvpp2_txdesc_dma_addr_set(port, tx_desc, buf_dma_addr);
3985 
3986 	tx_cmd = mvpp2_skb_tx_csum(port, skb);
3987 
3988 	if (frags == 1) {
3989 		/* First and Last descriptor */
3990 		tx_cmd |= MVPP2_TXD_F_DESC | MVPP2_TXD_L_DESC;
3991 		mvpp2_txdesc_cmd_set(port, tx_desc, tx_cmd);
3992 		mvpp2_txq_inc_put(port, txq_pcpu, skb, tx_desc, MVPP2_TYPE_SKB);
3993 	} else {
3994 		/* First but not Last */
3995 		tx_cmd |= MVPP2_TXD_F_DESC | MVPP2_TXD_PADDING_DISABLE;
3996 		mvpp2_txdesc_cmd_set(port, tx_desc, tx_cmd);
3997 		mvpp2_txq_inc_put(port, txq_pcpu, NULL, tx_desc, MVPP2_TYPE_SKB);
3998 
3999 		/* Continue with other skb fragments */
4000 		if (mvpp2_tx_frag_process(port, skb, aggr_txq, txq)) {
4001 			tx_desc_unmap_put(port, txq, tx_desc);
4002 			frags = 0;
4003 		}
4004 	}
4005 
4006 out:
4007 	if (frags > 0) {
4008 		struct mvpp2_pcpu_stats *stats = per_cpu_ptr(port->stats, thread);
4009 		struct netdev_queue *nq = netdev_get_tx_queue(dev, txq_id);
4010 
4011 		txq_pcpu->reserved_num -= frags;
4012 		txq_pcpu->count += frags;
4013 		aggr_txq->count += frags;
4014 
4015 		/* Enable transmit */
4016 		wmb();
4017 		mvpp2_aggr_txq_pend_desc_add(port, frags);
4018 
4019 		if (txq_pcpu->count >= txq_pcpu->stop_threshold)
4020 			netif_tx_stop_queue(nq);
4021 
4022 		u64_stats_update_begin(&stats->syncp);
4023 		stats->tx_packets++;
4024 		stats->tx_bytes += skb->len;
4025 		u64_stats_update_end(&stats->syncp);
4026 	} else {
4027 		dev->stats.tx_dropped++;
4028 		dev_kfree_skb_any(skb);
4029 	}
4030 
4031 	/* Finalize TX processing */
4032 	if (!port->has_tx_irqs && txq_pcpu->count >= txq->done_pkts_coal)
4033 		mvpp2_txq_done(port, txq, txq_pcpu);
4034 
4035 	/* Set the timer in case not all frags were processed */
4036 	if (!port->has_tx_irqs && txq_pcpu->count <= frags &&
4037 	    txq_pcpu->count > 0) {
4038 		struct mvpp2_port_pcpu *port_pcpu = per_cpu_ptr(port->pcpu, thread);
4039 
4040 		if (!port_pcpu->timer_scheduled) {
4041 			port_pcpu->timer_scheduled = true;
4042 			hrtimer_start(&port_pcpu->tx_done_timer,
4043 				      MVPP2_TXDONE_HRTIMER_PERIOD_NS,
4044 				      HRTIMER_MODE_REL_PINNED_SOFT);
4045 		}
4046 	}
4047 
4048 	if (test_bit(thread, &port->priv->lock_map))
4049 		spin_unlock_irqrestore(&port->tx_lock[thread], flags);
4050 
4051 	return NETDEV_TX_OK;
4052 }
4053 
4054 static inline void mvpp2_cause_error(struct net_device *dev, int cause)
4055 {
4056 	if (cause & MVPP2_CAUSE_FCS_ERR_MASK)
4057 		netdev_err(dev, "FCS error\n");
4058 	if (cause & MVPP2_CAUSE_RX_FIFO_OVERRUN_MASK)
4059 		netdev_err(dev, "rx fifo overrun error\n");
4060 	if (cause & MVPP2_CAUSE_TX_FIFO_UNDERRUN_MASK)
4061 		netdev_err(dev, "tx fifo underrun error\n");
4062 }
4063 
4064 static int mvpp2_poll(struct napi_struct *napi, int budget)
4065 {
4066 	u32 cause_rx_tx, cause_rx, cause_tx, cause_misc;
4067 	int rx_done = 0;
4068 	struct mvpp2_port *port = netdev_priv(napi->dev);
4069 	struct mvpp2_queue_vector *qv;
4070 	unsigned int thread = mvpp2_cpu_to_thread(port->priv, smp_processor_id());
4071 
4072 	qv = container_of(napi, struct mvpp2_queue_vector, napi);
4073 
4074 	/* Rx/Tx cause register
4075 	 *
4076 	 * Bits 0-15: each bit indicates received packets on the Rx queue
4077 	 * (bit 0 is for Rx queue 0).
4078 	 *
4079 	 * Bits 16-23: each bit indicates transmitted packets on the Tx queue
4080 	 * (bit 16 is for Tx queue 0).
4081 	 *
4082 	 * Each CPU has its own Rx/Tx cause register
4083 	 */
4084 	cause_rx_tx = mvpp2_thread_read_relaxed(port->priv, qv->sw_thread_id,
4085 						MVPP2_ISR_RX_TX_CAUSE_REG(port->id));
4086 
4087 	cause_misc = cause_rx_tx & MVPP2_CAUSE_MISC_SUM_MASK;
4088 	if (cause_misc) {
4089 		mvpp2_cause_error(port->dev, cause_misc);
4090 
4091 		/* Clear the cause register */
4092 		mvpp2_write(port->priv, MVPP2_ISR_MISC_CAUSE_REG, 0);
4093 		mvpp2_thread_write(port->priv, thread,
4094 				   MVPP2_ISR_RX_TX_CAUSE_REG(port->id),
4095 				   cause_rx_tx & ~MVPP2_CAUSE_MISC_SUM_MASK);
4096 	}
4097 
4098 	if (port->has_tx_irqs) {
4099 		cause_tx = cause_rx_tx & MVPP2_CAUSE_TXQ_OCCUP_DESC_ALL_MASK;
4100 		if (cause_tx) {
4101 			cause_tx >>= MVPP2_CAUSE_TXQ_OCCUP_DESC_ALL_OFFSET;
4102 			mvpp2_tx_done(port, cause_tx, qv->sw_thread_id);
4103 		}
4104 	}
4105 
4106 	/* Process RX packets */
4107 	cause_rx = cause_rx_tx &
4108 		   MVPP2_CAUSE_RXQ_OCCUP_DESC_ALL_MASK(port->priv->hw_version);
4109 	cause_rx <<= qv->first_rxq;
4110 	cause_rx |= qv->pending_cause_rx;
4111 	while (cause_rx && budget > 0) {
4112 		int count;
4113 		struct mvpp2_rx_queue *rxq;
4114 
4115 		rxq = mvpp2_get_rx_queue(port, cause_rx);
4116 		if (!rxq)
4117 			break;
4118 
4119 		count = mvpp2_rx(port, napi, budget, rxq);
4120 		rx_done += count;
4121 		budget -= count;
4122 		if (budget > 0) {
4123 			/* Clear the bit associated to this Rx queue
4124 			 * so that next iteration will continue from
4125 			 * the next Rx queue.
4126 			 */
4127 			cause_rx &= ~(1 << rxq->logic_rxq);
4128 		}
4129 	}
4130 
4131 	if (budget > 0) {
4132 		cause_rx = 0;
4133 		napi_complete_done(napi, rx_done);
4134 
4135 		mvpp2_qvec_interrupt_enable(qv);
4136 	}
4137 	qv->pending_cause_rx = cause_rx;
4138 	return rx_done;
4139 }
4140 
4141 static void mvpp22_mode_reconfigure(struct mvpp2_port *port)
4142 {
4143 	u32 ctrl3;
4144 
4145 	/* Set the GMAC & XLG MAC in reset */
4146 	mvpp2_mac_reset_assert(port);
4147 
4148 	/* Set the MPCS and XPCS in reset */
4149 	mvpp22_pcs_reset_assert(port);
4150 
4151 	/* comphy reconfiguration */
4152 	mvpp22_comphy_init(port);
4153 
4154 	/* gop reconfiguration */
4155 	mvpp22_gop_init(port);
4156 
4157 	mvpp22_pcs_reset_deassert(port);
4158 
4159 	if (mvpp2_port_supports_xlg(port)) {
4160 		ctrl3 = readl(port->base + MVPP22_XLG_CTRL3_REG);
4161 		ctrl3 &= ~MVPP22_XLG_CTRL3_MACMODESELECT_MASK;
4162 
4163 		if (mvpp2_is_xlg(port->phy_interface))
4164 			ctrl3 |= MVPP22_XLG_CTRL3_MACMODESELECT_10G;
4165 		else
4166 			ctrl3 |= MVPP22_XLG_CTRL3_MACMODESELECT_GMAC;
4167 
4168 		writel(ctrl3, port->base + MVPP22_XLG_CTRL3_REG);
4169 	}
4170 
4171 	if (mvpp2_port_supports_xlg(port) && mvpp2_is_xlg(port->phy_interface))
4172 		mvpp2_xlg_max_rx_size_set(port);
4173 	else
4174 		mvpp2_gmac_max_rx_size_set(port);
4175 }
4176 
4177 /* Set hw internals when starting port */
4178 static void mvpp2_start_dev(struct mvpp2_port *port)
4179 {
4180 	int i;
4181 
4182 	mvpp2_txp_max_tx_size_set(port);
4183 
4184 	for (i = 0; i < port->nqvecs; i++)
4185 		napi_enable(&port->qvecs[i].napi);
4186 
4187 	/* Enable interrupts on all threads */
4188 	mvpp2_interrupts_enable(port);
4189 
4190 	if (port->priv->hw_version == MVPP22)
4191 		mvpp22_mode_reconfigure(port);
4192 
4193 	if (port->phylink) {
4194 		phylink_start(port->phylink);
4195 	} else {
4196 		mvpp2_acpi_start(port);
4197 	}
4198 
4199 	netif_tx_start_all_queues(port->dev);
4200 
4201 	clear_bit(0, &port->state);
4202 }
4203 
4204 /* Set hw internals when stopping port */
4205 static void mvpp2_stop_dev(struct mvpp2_port *port)
4206 {
4207 	int i;
4208 
4209 	set_bit(0, &port->state);
4210 
4211 	/* Disable interrupts on all threads */
4212 	mvpp2_interrupts_disable(port);
4213 
4214 	for (i = 0; i < port->nqvecs; i++)
4215 		napi_disable(&port->qvecs[i].napi);
4216 
4217 	if (port->phylink)
4218 		phylink_stop(port->phylink);
4219 	phy_power_off(port->comphy);
4220 }
4221 
4222 static int mvpp2_check_ringparam_valid(struct net_device *dev,
4223 				       struct ethtool_ringparam *ring)
4224 {
4225 	u16 new_rx_pending = ring->rx_pending;
4226 	u16 new_tx_pending = ring->tx_pending;
4227 
4228 	if (ring->rx_pending == 0 || ring->tx_pending == 0)
4229 		return -EINVAL;
4230 
4231 	if (ring->rx_pending > MVPP2_MAX_RXD_MAX)
4232 		new_rx_pending = MVPP2_MAX_RXD_MAX;
4233 	else if (!IS_ALIGNED(ring->rx_pending, 16))
4234 		new_rx_pending = ALIGN(ring->rx_pending, 16);
4235 
4236 	if (ring->tx_pending > MVPP2_MAX_TXD_MAX)
4237 		new_tx_pending = MVPP2_MAX_TXD_MAX;
4238 	else if (!IS_ALIGNED(ring->tx_pending, 32))
4239 		new_tx_pending = ALIGN(ring->tx_pending, 32);
4240 
4241 	/* The Tx ring size cannot be smaller than the minimum number of
4242 	 * descriptors needed for TSO.
4243 	 */
4244 	if (new_tx_pending < MVPP2_MAX_SKB_DESCS)
4245 		new_tx_pending = ALIGN(MVPP2_MAX_SKB_DESCS, 32);
4246 
4247 	if (ring->rx_pending != new_rx_pending) {
4248 		netdev_info(dev, "illegal Rx ring size value %d, round to %d\n",
4249 			    ring->rx_pending, new_rx_pending);
4250 		ring->rx_pending = new_rx_pending;
4251 	}
4252 
4253 	if (ring->tx_pending != new_tx_pending) {
4254 		netdev_info(dev, "illegal Tx ring size value %d, round to %d\n",
4255 			    ring->tx_pending, new_tx_pending);
4256 		ring->tx_pending = new_tx_pending;
4257 	}
4258 
4259 	return 0;
4260 }
4261 
4262 static void mvpp21_get_mac_address(struct mvpp2_port *port, unsigned char *addr)
4263 {
4264 	u32 mac_addr_l, mac_addr_m, mac_addr_h;
4265 
4266 	mac_addr_l = readl(port->base + MVPP2_GMAC_CTRL_1_REG);
4267 	mac_addr_m = readl(port->priv->lms_base + MVPP2_SRC_ADDR_MIDDLE);
4268 	mac_addr_h = readl(port->priv->lms_base + MVPP2_SRC_ADDR_HIGH);
4269 	addr[0] = (mac_addr_h >> 24) & 0xFF;
4270 	addr[1] = (mac_addr_h >> 16) & 0xFF;
4271 	addr[2] = (mac_addr_h >> 8) & 0xFF;
4272 	addr[3] = mac_addr_h & 0xFF;
4273 	addr[4] = mac_addr_m & 0xFF;
4274 	addr[5] = (mac_addr_l >> MVPP2_GMAC_SA_LOW_OFFS) & 0xFF;
4275 }
4276 
4277 static int mvpp2_irqs_init(struct mvpp2_port *port)
4278 {
4279 	int err, i;
4280 
4281 	for (i = 0; i < port->nqvecs; i++) {
4282 		struct mvpp2_queue_vector *qv = port->qvecs + i;
4283 
4284 		if (qv->type == MVPP2_QUEUE_VECTOR_PRIVATE) {
4285 			qv->mask = kzalloc(cpumask_size(), GFP_KERNEL);
4286 			if (!qv->mask) {
4287 				err = -ENOMEM;
4288 				goto err;
4289 			}
4290 
4291 			irq_set_status_flags(qv->irq, IRQ_NO_BALANCING);
4292 		}
4293 
4294 		err = request_irq(qv->irq, mvpp2_isr, 0, port->dev->name, qv);
4295 		if (err)
4296 			goto err;
4297 
4298 		if (qv->type == MVPP2_QUEUE_VECTOR_PRIVATE) {
4299 			unsigned int cpu;
4300 
4301 			for_each_present_cpu(cpu) {
4302 				if (mvpp2_cpu_to_thread(port->priv, cpu) ==
4303 				    qv->sw_thread_id)
4304 					cpumask_set_cpu(cpu, qv->mask);
4305 			}
4306 
4307 			irq_set_affinity_hint(qv->irq, qv->mask);
4308 		}
4309 	}
4310 
4311 	return 0;
4312 err:
4313 	for (i = 0; i < port->nqvecs; i++) {
4314 		struct mvpp2_queue_vector *qv = port->qvecs + i;
4315 
4316 		irq_set_affinity_hint(qv->irq, NULL);
4317 		kfree(qv->mask);
4318 		qv->mask = NULL;
4319 		free_irq(qv->irq, qv);
4320 	}
4321 
4322 	return err;
4323 }
4324 
4325 static void mvpp2_irqs_deinit(struct mvpp2_port *port)
4326 {
4327 	int i;
4328 
4329 	for (i = 0; i < port->nqvecs; i++) {
4330 		struct mvpp2_queue_vector *qv = port->qvecs + i;
4331 
4332 		irq_set_affinity_hint(qv->irq, NULL);
4333 		kfree(qv->mask);
4334 		qv->mask = NULL;
4335 		irq_clear_status_flags(qv->irq, IRQ_NO_BALANCING);
4336 		free_irq(qv->irq, qv);
4337 	}
4338 }
4339 
4340 static bool mvpp22_rss_is_supported(void)
4341 {
4342 	return queue_mode == MVPP2_QDIST_MULTI_MODE;
4343 }
4344 
4345 static int mvpp2_open(struct net_device *dev)
4346 {
4347 	struct mvpp2_port *port = netdev_priv(dev);
4348 	struct mvpp2 *priv = port->priv;
4349 	unsigned char mac_bcast[ETH_ALEN] = {
4350 			0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
4351 	bool valid = false;
4352 	int err;
4353 
4354 	err = mvpp2_prs_mac_da_accept(port, mac_bcast, true);
4355 	if (err) {
4356 		netdev_err(dev, "mvpp2_prs_mac_da_accept BC failed\n");
4357 		return err;
4358 	}
4359 	err = mvpp2_prs_mac_da_accept(port, dev->dev_addr, true);
4360 	if (err) {
4361 		netdev_err(dev, "mvpp2_prs_mac_da_accept own addr failed\n");
4362 		return err;
4363 	}
4364 	err = mvpp2_prs_tag_mode_set(port->priv, port->id, MVPP2_TAG_TYPE_MH);
4365 	if (err) {
4366 		netdev_err(dev, "mvpp2_prs_tag_mode_set failed\n");
4367 		return err;
4368 	}
4369 	err = mvpp2_prs_def_flow(port);
4370 	if (err) {
4371 		netdev_err(dev, "mvpp2_prs_def_flow failed\n");
4372 		return err;
4373 	}
4374 
4375 	/* Allocate the Rx/Tx queues */
4376 	err = mvpp2_setup_rxqs(port);
4377 	if (err) {
4378 		netdev_err(port->dev, "cannot allocate Rx queues\n");
4379 		return err;
4380 	}
4381 
4382 	err = mvpp2_setup_txqs(port);
4383 	if (err) {
4384 		netdev_err(port->dev, "cannot allocate Tx queues\n");
4385 		goto err_cleanup_rxqs;
4386 	}
4387 
4388 	err = mvpp2_irqs_init(port);
4389 	if (err) {
4390 		netdev_err(port->dev, "cannot init IRQs\n");
4391 		goto err_cleanup_txqs;
4392 	}
4393 
4394 	/* Phylink isn't supported yet in ACPI mode */
4395 	if (port->of_node) {
4396 		err = phylink_of_phy_connect(port->phylink, port->of_node, 0);
4397 		if (err) {
4398 			netdev_err(port->dev, "could not attach PHY (%d)\n",
4399 				   err);
4400 			goto err_free_irq;
4401 		}
4402 
4403 		valid = true;
4404 	}
4405 
4406 	if (priv->hw_version == MVPP22 && port->port_irq) {
4407 		err = request_irq(port->port_irq, mvpp2_port_isr, 0,
4408 				  dev->name, port);
4409 		if (err) {
4410 			netdev_err(port->dev,
4411 				   "cannot request port link/ptp IRQ %d\n",
4412 				   port->port_irq);
4413 			goto err_free_irq;
4414 		}
4415 
4416 		mvpp22_gop_setup_irq(port);
4417 
4418 		/* In default link is down */
4419 		netif_carrier_off(port->dev);
4420 
4421 		valid = true;
4422 	} else {
4423 		port->port_irq = 0;
4424 	}
4425 
4426 	if (!valid) {
4427 		netdev_err(port->dev,
4428 			   "invalid configuration: no dt or link IRQ");
4429 		err = -ENOENT;
4430 		goto err_free_irq;
4431 	}
4432 
4433 	/* Unmask interrupts on all CPUs */
4434 	on_each_cpu(mvpp2_interrupts_unmask, port, 1);
4435 	mvpp2_shared_interrupt_mask_unmask(port, false);
4436 
4437 	mvpp2_start_dev(port);
4438 
4439 	/* Start hardware statistics gathering */
4440 	queue_delayed_work(priv->stats_queue, &port->stats_work,
4441 			   MVPP2_MIB_COUNTERS_STATS_DELAY);
4442 
4443 	return 0;
4444 
4445 err_free_irq:
4446 	mvpp2_irqs_deinit(port);
4447 err_cleanup_txqs:
4448 	mvpp2_cleanup_txqs(port);
4449 err_cleanup_rxqs:
4450 	mvpp2_cleanup_rxqs(port);
4451 	return err;
4452 }
4453 
4454 static int mvpp2_stop(struct net_device *dev)
4455 {
4456 	struct mvpp2_port *port = netdev_priv(dev);
4457 	struct mvpp2_port_pcpu *port_pcpu;
4458 	unsigned int thread;
4459 
4460 	mvpp2_stop_dev(port);
4461 
4462 	/* Mask interrupts on all threads */
4463 	on_each_cpu(mvpp2_interrupts_mask, port, 1);
4464 	mvpp2_shared_interrupt_mask_unmask(port, true);
4465 
4466 	if (port->phylink)
4467 		phylink_disconnect_phy(port->phylink);
4468 	if (port->port_irq)
4469 		free_irq(port->port_irq, port);
4470 
4471 	mvpp2_irqs_deinit(port);
4472 	if (!port->has_tx_irqs) {
4473 		for (thread = 0; thread < port->priv->nthreads; thread++) {
4474 			port_pcpu = per_cpu_ptr(port->pcpu, thread);
4475 
4476 			hrtimer_cancel(&port_pcpu->tx_done_timer);
4477 			port_pcpu->timer_scheduled = false;
4478 		}
4479 	}
4480 	mvpp2_cleanup_rxqs(port);
4481 	mvpp2_cleanup_txqs(port);
4482 
4483 	cancel_delayed_work_sync(&port->stats_work);
4484 
4485 	mvpp2_mac_reset_assert(port);
4486 	mvpp22_pcs_reset_assert(port);
4487 
4488 	return 0;
4489 }
4490 
4491 static int mvpp2_prs_mac_da_accept_list(struct mvpp2_port *port,
4492 					struct netdev_hw_addr_list *list)
4493 {
4494 	struct netdev_hw_addr *ha;
4495 	int ret;
4496 
4497 	netdev_hw_addr_list_for_each(ha, list) {
4498 		ret = mvpp2_prs_mac_da_accept(port, ha->addr, true);
4499 		if (ret)
4500 			return ret;
4501 	}
4502 
4503 	return 0;
4504 }
4505 
4506 static void mvpp2_set_rx_promisc(struct mvpp2_port *port, bool enable)
4507 {
4508 	if (!enable && (port->dev->features & NETIF_F_HW_VLAN_CTAG_FILTER))
4509 		mvpp2_prs_vid_enable_filtering(port);
4510 	else
4511 		mvpp2_prs_vid_disable_filtering(port);
4512 
4513 	mvpp2_prs_mac_promisc_set(port->priv, port->id,
4514 				  MVPP2_PRS_L2_UNI_CAST, enable);
4515 
4516 	mvpp2_prs_mac_promisc_set(port->priv, port->id,
4517 				  MVPP2_PRS_L2_MULTI_CAST, enable);
4518 }
4519 
4520 static void mvpp2_set_rx_mode(struct net_device *dev)
4521 {
4522 	struct mvpp2_port *port = netdev_priv(dev);
4523 
4524 	/* Clear the whole UC and MC list */
4525 	mvpp2_prs_mac_del_all(port);
4526 
4527 	if (dev->flags & IFF_PROMISC) {
4528 		mvpp2_set_rx_promisc(port, true);
4529 		return;
4530 	}
4531 
4532 	mvpp2_set_rx_promisc(port, false);
4533 
4534 	if (netdev_uc_count(dev) > MVPP2_PRS_MAC_UC_FILT_MAX ||
4535 	    mvpp2_prs_mac_da_accept_list(port, &dev->uc))
4536 		mvpp2_prs_mac_promisc_set(port->priv, port->id,
4537 					  MVPP2_PRS_L2_UNI_CAST, true);
4538 
4539 	if (dev->flags & IFF_ALLMULTI) {
4540 		mvpp2_prs_mac_promisc_set(port->priv, port->id,
4541 					  MVPP2_PRS_L2_MULTI_CAST, true);
4542 		return;
4543 	}
4544 
4545 	if (netdev_mc_count(dev) > MVPP2_PRS_MAC_MC_FILT_MAX ||
4546 	    mvpp2_prs_mac_da_accept_list(port, &dev->mc))
4547 		mvpp2_prs_mac_promisc_set(port->priv, port->id,
4548 					  MVPP2_PRS_L2_MULTI_CAST, true);
4549 }
4550 
4551 static int mvpp2_set_mac_address(struct net_device *dev, void *p)
4552 {
4553 	const struct sockaddr *addr = p;
4554 	int err;
4555 
4556 	if (!is_valid_ether_addr(addr->sa_data))
4557 		return -EADDRNOTAVAIL;
4558 
4559 	err = mvpp2_prs_update_mac_da(dev, addr->sa_data);
4560 	if (err) {
4561 		/* Reconfigure parser accept the original MAC address */
4562 		mvpp2_prs_update_mac_da(dev, dev->dev_addr);
4563 		netdev_err(dev, "failed to change MAC address\n");
4564 	}
4565 	return err;
4566 }
4567 
4568 /* Shut down all the ports, reconfigure the pools as percpu or shared,
4569  * then bring up again all ports.
4570  */
4571 static int mvpp2_bm_switch_buffers(struct mvpp2 *priv, bool percpu)
4572 {
4573 	int numbufs = MVPP2_BM_POOLS_NUM, i;
4574 	struct mvpp2_port *port = NULL;
4575 	bool status[MVPP2_MAX_PORTS];
4576 
4577 	for (i = 0; i < priv->port_count; i++) {
4578 		port = priv->port_list[i];
4579 		status[i] = netif_running(port->dev);
4580 		if (status[i])
4581 			mvpp2_stop(port->dev);
4582 	}
4583 
4584 	/* nrxqs is the same for all ports */
4585 	if (priv->percpu_pools)
4586 		numbufs = port->nrxqs * 2;
4587 
4588 	for (i = 0; i < numbufs; i++)
4589 		mvpp2_bm_pool_destroy(port->dev->dev.parent, priv, &priv->bm_pools[i]);
4590 
4591 	devm_kfree(port->dev->dev.parent, priv->bm_pools);
4592 	priv->percpu_pools = percpu;
4593 	mvpp2_bm_init(port->dev->dev.parent, priv);
4594 
4595 	for (i = 0; i < priv->port_count; i++) {
4596 		port = priv->port_list[i];
4597 		mvpp2_swf_bm_pool_init(port);
4598 		if (status[i])
4599 			mvpp2_open(port->dev);
4600 	}
4601 
4602 	return 0;
4603 }
4604 
4605 static int mvpp2_change_mtu(struct net_device *dev, int mtu)
4606 {
4607 	struct mvpp2_port *port = netdev_priv(dev);
4608 	bool running = netif_running(dev);
4609 	struct mvpp2 *priv = port->priv;
4610 	int err;
4611 
4612 	if (!IS_ALIGNED(MVPP2_RX_PKT_SIZE(mtu), 8)) {
4613 		netdev_info(dev, "illegal MTU value %d, round to %d\n", mtu,
4614 			    ALIGN(MVPP2_RX_PKT_SIZE(mtu), 8));
4615 		mtu = ALIGN(MVPP2_RX_PKT_SIZE(mtu), 8);
4616 	}
4617 
4618 	if (MVPP2_RX_PKT_SIZE(mtu) > MVPP2_BM_LONG_PKT_SIZE) {
4619 		if (port->xdp_prog) {
4620 			netdev_err(dev, "Jumbo frames are not supported with XDP\n");
4621 			return -EINVAL;
4622 		}
4623 		if (priv->percpu_pools) {
4624 			netdev_warn(dev, "mtu %d too high, switching to shared buffers", mtu);
4625 			mvpp2_bm_switch_buffers(priv, false);
4626 		}
4627 	} else {
4628 		bool jumbo = false;
4629 		int i;
4630 
4631 		for (i = 0; i < priv->port_count; i++)
4632 			if (priv->port_list[i] != port &&
4633 			    MVPP2_RX_PKT_SIZE(priv->port_list[i]->dev->mtu) >
4634 			    MVPP2_BM_LONG_PKT_SIZE) {
4635 				jumbo = true;
4636 				break;
4637 			}
4638 
4639 		/* No port is using jumbo frames */
4640 		if (!jumbo) {
4641 			dev_info(port->dev->dev.parent,
4642 				 "all ports have a low MTU, switching to per-cpu buffers");
4643 			mvpp2_bm_switch_buffers(priv, true);
4644 		}
4645 	}
4646 
4647 	if (running)
4648 		mvpp2_stop_dev(port);
4649 
4650 	err = mvpp2_bm_update_mtu(dev, mtu);
4651 	if (err) {
4652 		netdev_err(dev, "failed to change MTU\n");
4653 		/* Reconfigure BM to the original MTU */
4654 		mvpp2_bm_update_mtu(dev, dev->mtu);
4655 	} else {
4656 		port->pkt_size =  MVPP2_RX_PKT_SIZE(mtu);
4657 	}
4658 
4659 	if (running) {
4660 		mvpp2_start_dev(port);
4661 		mvpp2_egress_enable(port);
4662 		mvpp2_ingress_enable(port);
4663 	}
4664 
4665 	return err;
4666 }
4667 
4668 static int mvpp2_check_pagepool_dma(struct mvpp2_port *port)
4669 {
4670 	enum dma_data_direction dma_dir = DMA_FROM_DEVICE;
4671 	struct mvpp2 *priv = port->priv;
4672 	int err = -1, i;
4673 
4674 	if (!priv->percpu_pools)
4675 		return err;
4676 
4677 	if (!priv->page_pool[0])
4678 		return -ENOMEM;
4679 
4680 	for (i = 0; i < priv->port_count; i++) {
4681 		port = priv->port_list[i];
4682 		if (port->xdp_prog) {
4683 			dma_dir = DMA_BIDIRECTIONAL;
4684 			break;
4685 		}
4686 	}
4687 
4688 	/* All pools are equal in terms of DMA direction */
4689 	if (priv->page_pool[0]->p.dma_dir != dma_dir)
4690 		err = mvpp2_bm_switch_buffers(priv, true);
4691 
4692 	return err;
4693 }
4694 
4695 static void
4696 mvpp2_get_stats64(struct net_device *dev, struct rtnl_link_stats64 *stats)
4697 {
4698 	struct mvpp2_port *port = netdev_priv(dev);
4699 	unsigned int start;
4700 	unsigned int cpu;
4701 
4702 	for_each_possible_cpu(cpu) {
4703 		struct mvpp2_pcpu_stats *cpu_stats;
4704 		u64 rx_packets;
4705 		u64 rx_bytes;
4706 		u64 tx_packets;
4707 		u64 tx_bytes;
4708 
4709 		cpu_stats = per_cpu_ptr(port->stats, cpu);
4710 		do {
4711 			start = u64_stats_fetch_begin_irq(&cpu_stats->syncp);
4712 			rx_packets = cpu_stats->rx_packets;
4713 			rx_bytes   = cpu_stats->rx_bytes;
4714 			tx_packets = cpu_stats->tx_packets;
4715 			tx_bytes   = cpu_stats->tx_bytes;
4716 		} while (u64_stats_fetch_retry_irq(&cpu_stats->syncp, start));
4717 
4718 		stats->rx_packets += rx_packets;
4719 		stats->rx_bytes   += rx_bytes;
4720 		stats->tx_packets += tx_packets;
4721 		stats->tx_bytes   += tx_bytes;
4722 	}
4723 
4724 	stats->rx_errors	= dev->stats.rx_errors;
4725 	stats->rx_dropped	= dev->stats.rx_dropped;
4726 	stats->tx_dropped	= dev->stats.tx_dropped;
4727 }
4728 
4729 static int mvpp2_set_ts_config(struct mvpp2_port *port, struct ifreq *ifr)
4730 {
4731 	struct hwtstamp_config config;
4732 	void __iomem *ptp;
4733 	u32 gcr, int_mask;
4734 
4735 	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
4736 		return -EFAULT;
4737 
4738 	if (config.flags)
4739 		return -EINVAL;
4740 
4741 	if (config.tx_type != HWTSTAMP_TX_OFF &&
4742 	    config.tx_type != HWTSTAMP_TX_ON)
4743 		return -ERANGE;
4744 
4745 	ptp = port->priv->iface_base + MVPP22_PTP_BASE(port->gop_id);
4746 
4747 	int_mask = gcr = 0;
4748 	if (config.tx_type != HWTSTAMP_TX_OFF) {
4749 		gcr |= MVPP22_PTP_GCR_TSU_ENABLE | MVPP22_PTP_GCR_TX_RESET;
4750 		int_mask |= MVPP22_PTP_INT_MASK_QUEUE1 |
4751 			    MVPP22_PTP_INT_MASK_QUEUE0;
4752 	}
4753 
4754 	/* It seems we must also release the TX reset when enabling the TSU */
4755 	if (config.rx_filter != HWTSTAMP_FILTER_NONE)
4756 		gcr |= MVPP22_PTP_GCR_TSU_ENABLE | MVPP22_PTP_GCR_RX_RESET |
4757 		       MVPP22_PTP_GCR_TX_RESET;
4758 
4759 	if (gcr & MVPP22_PTP_GCR_TSU_ENABLE)
4760 		mvpp22_tai_start(port->priv->tai);
4761 
4762 	if (config.rx_filter != HWTSTAMP_FILTER_NONE) {
4763 		config.rx_filter = HWTSTAMP_FILTER_ALL;
4764 		mvpp2_modify(ptp + MVPP22_PTP_GCR,
4765 			     MVPP22_PTP_GCR_RX_RESET |
4766 			     MVPP22_PTP_GCR_TX_RESET |
4767 			     MVPP22_PTP_GCR_TSU_ENABLE, gcr);
4768 		port->rx_hwtstamp = true;
4769 	} else {
4770 		port->rx_hwtstamp = false;
4771 		mvpp2_modify(ptp + MVPP22_PTP_GCR,
4772 			     MVPP22_PTP_GCR_RX_RESET |
4773 			     MVPP22_PTP_GCR_TX_RESET |
4774 			     MVPP22_PTP_GCR_TSU_ENABLE, gcr);
4775 	}
4776 
4777 	mvpp2_modify(ptp + MVPP22_PTP_INT_MASK,
4778 		     MVPP22_PTP_INT_MASK_QUEUE1 |
4779 		     MVPP22_PTP_INT_MASK_QUEUE0, int_mask);
4780 
4781 	if (!(gcr & MVPP22_PTP_GCR_TSU_ENABLE))
4782 		mvpp22_tai_stop(port->priv->tai);
4783 
4784 	port->tx_hwtstamp_type = config.tx_type;
4785 
4786 	if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
4787 		return -EFAULT;
4788 
4789 	return 0;
4790 }
4791 
4792 static int mvpp2_get_ts_config(struct mvpp2_port *port, struct ifreq *ifr)
4793 {
4794 	struct hwtstamp_config config;
4795 
4796 	memset(&config, 0, sizeof(config));
4797 
4798 	config.tx_type = port->tx_hwtstamp_type;
4799 	config.rx_filter = port->rx_hwtstamp ?
4800 		HWTSTAMP_FILTER_ALL : HWTSTAMP_FILTER_NONE;
4801 
4802 	if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
4803 		return -EFAULT;
4804 
4805 	return 0;
4806 }
4807 
4808 static int mvpp2_ethtool_get_ts_info(struct net_device *dev,
4809 				     struct ethtool_ts_info *info)
4810 {
4811 	struct mvpp2_port *port = netdev_priv(dev);
4812 
4813 	if (!port->hwtstamp)
4814 		return -EOPNOTSUPP;
4815 
4816 	info->phc_index = mvpp22_tai_ptp_clock_index(port->priv->tai);
4817 	info->so_timestamping = SOF_TIMESTAMPING_TX_SOFTWARE |
4818 				SOF_TIMESTAMPING_RX_SOFTWARE |
4819 				SOF_TIMESTAMPING_SOFTWARE |
4820 				SOF_TIMESTAMPING_TX_HARDWARE |
4821 				SOF_TIMESTAMPING_RX_HARDWARE |
4822 				SOF_TIMESTAMPING_RAW_HARDWARE;
4823 	info->tx_types = BIT(HWTSTAMP_TX_OFF) |
4824 			 BIT(HWTSTAMP_TX_ON);
4825 	info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
4826 			   BIT(HWTSTAMP_FILTER_ALL);
4827 
4828 	return 0;
4829 }
4830 
4831 static int mvpp2_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
4832 {
4833 	struct mvpp2_port *port = netdev_priv(dev);
4834 
4835 	switch (cmd) {
4836 	case SIOCSHWTSTAMP:
4837 		if (port->hwtstamp)
4838 			return mvpp2_set_ts_config(port, ifr);
4839 		break;
4840 
4841 	case SIOCGHWTSTAMP:
4842 		if (port->hwtstamp)
4843 			return mvpp2_get_ts_config(port, ifr);
4844 		break;
4845 	}
4846 
4847 	if (!port->phylink)
4848 		return -ENOTSUPP;
4849 
4850 	return phylink_mii_ioctl(port->phylink, ifr, cmd);
4851 }
4852 
4853 static int mvpp2_vlan_rx_add_vid(struct net_device *dev, __be16 proto, u16 vid)
4854 {
4855 	struct mvpp2_port *port = netdev_priv(dev);
4856 	int ret;
4857 
4858 	ret = mvpp2_prs_vid_entry_add(port, vid);
4859 	if (ret)
4860 		netdev_err(dev, "rx-vlan-filter offloading cannot accept more than %d VIDs per port\n",
4861 			   MVPP2_PRS_VLAN_FILT_MAX - 1);
4862 	return ret;
4863 }
4864 
4865 static int mvpp2_vlan_rx_kill_vid(struct net_device *dev, __be16 proto, u16 vid)
4866 {
4867 	struct mvpp2_port *port = netdev_priv(dev);
4868 
4869 	mvpp2_prs_vid_entry_remove(port, vid);
4870 	return 0;
4871 }
4872 
4873 static int mvpp2_set_features(struct net_device *dev,
4874 			      netdev_features_t features)
4875 {
4876 	netdev_features_t changed = dev->features ^ features;
4877 	struct mvpp2_port *port = netdev_priv(dev);
4878 
4879 	if (changed & NETIF_F_HW_VLAN_CTAG_FILTER) {
4880 		if (features & NETIF_F_HW_VLAN_CTAG_FILTER) {
4881 			mvpp2_prs_vid_enable_filtering(port);
4882 		} else {
4883 			/* Invalidate all registered VID filters for this
4884 			 * port
4885 			 */
4886 			mvpp2_prs_vid_remove_all(port);
4887 
4888 			mvpp2_prs_vid_disable_filtering(port);
4889 		}
4890 	}
4891 
4892 	if (changed & NETIF_F_RXHASH) {
4893 		if (features & NETIF_F_RXHASH)
4894 			mvpp22_port_rss_enable(port);
4895 		else
4896 			mvpp22_port_rss_disable(port);
4897 	}
4898 
4899 	return 0;
4900 }
4901 
4902 static int mvpp2_xdp_setup(struct mvpp2_port *port, struct netdev_bpf *bpf)
4903 {
4904 	struct bpf_prog *prog = bpf->prog, *old_prog;
4905 	bool running = netif_running(port->dev);
4906 	bool reset = !prog != !port->xdp_prog;
4907 
4908 	if (port->dev->mtu > ETH_DATA_LEN) {
4909 		NL_SET_ERR_MSG_MOD(bpf->extack, "XDP is not supported with jumbo frames enabled");
4910 		return -EOPNOTSUPP;
4911 	}
4912 
4913 	if (!port->priv->percpu_pools) {
4914 		NL_SET_ERR_MSG_MOD(bpf->extack, "Per CPU Pools required for XDP");
4915 		return -EOPNOTSUPP;
4916 	}
4917 
4918 	if (port->ntxqs < num_possible_cpus() * 2) {
4919 		NL_SET_ERR_MSG_MOD(bpf->extack, "XDP_TX needs two TX queues per CPU");
4920 		return -EOPNOTSUPP;
4921 	}
4922 
4923 	/* device is up and bpf is added/removed, must setup the RX queues */
4924 	if (running && reset)
4925 		mvpp2_stop(port->dev);
4926 
4927 	old_prog = xchg(&port->xdp_prog, prog);
4928 	if (old_prog)
4929 		bpf_prog_put(old_prog);
4930 
4931 	/* bpf is just replaced, RXQ and MTU are already setup */
4932 	if (!reset)
4933 		return 0;
4934 
4935 	/* device was up, restore the link */
4936 	if (running)
4937 		mvpp2_open(port->dev);
4938 
4939 	/* Check Page Pool DMA Direction */
4940 	mvpp2_check_pagepool_dma(port);
4941 
4942 	return 0;
4943 }
4944 
4945 static int mvpp2_xdp(struct net_device *dev, struct netdev_bpf *xdp)
4946 {
4947 	struct mvpp2_port *port = netdev_priv(dev);
4948 
4949 	switch (xdp->command) {
4950 	case XDP_SETUP_PROG:
4951 		return mvpp2_xdp_setup(port, xdp);
4952 	default:
4953 		return -EINVAL;
4954 	}
4955 }
4956 
4957 /* Ethtool methods */
4958 
4959 static int mvpp2_ethtool_nway_reset(struct net_device *dev)
4960 {
4961 	struct mvpp2_port *port = netdev_priv(dev);
4962 
4963 	if (!port->phylink)
4964 		return -ENOTSUPP;
4965 
4966 	return phylink_ethtool_nway_reset(port->phylink);
4967 }
4968 
4969 /* Set interrupt coalescing for ethtools */
4970 static int mvpp2_ethtool_set_coalesce(struct net_device *dev,
4971 				      struct ethtool_coalesce *c)
4972 {
4973 	struct mvpp2_port *port = netdev_priv(dev);
4974 	int queue;
4975 
4976 	for (queue = 0; queue < port->nrxqs; queue++) {
4977 		struct mvpp2_rx_queue *rxq = port->rxqs[queue];
4978 
4979 		rxq->time_coal = c->rx_coalesce_usecs;
4980 		rxq->pkts_coal = c->rx_max_coalesced_frames;
4981 		mvpp2_rx_pkts_coal_set(port, rxq);
4982 		mvpp2_rx_time_coal_set(port, rxq);
4983 	}
4984 
4985 	if (port->has_tx_irqs) {
4986 		port->tx_time_coal = c->tx_coalesce_usecs;
4987 		mvpp2_tx_time_coal_set(port);
4988 	}
4989 
4990 	for (queue = 0; queue < port->ntxqs; queue++) {
4991 		struct mvpp2_tx_queue *txq = port->txqs[queue];
4992 
4993 		txq->done_pkts_coal = c->tx_max_coalesced_frames;
4994 
4995 		if (port->has_tx_irqs)
4996 			mvpp2_tx_pkts_coal_set(port, txq);
4997 	}
4998 
4999 	return 0;
5000 }
5001 
5002 /* get coalescing for ethtools */
5003 static int mvpp2_ethtool_get_coalesce(struct net_device *dev,
5004 				      struct ethtool_coalesce *c)
5005 {
5006 	struct mvpp2_port *port = netdev_priv(dev);
5007 
5008 	c->rx_coalesce_usecs       = port->rxqs[0]->time_coal;
5009 	c->rx_max_coalesced_frames = port->rxqs[0]->pkts_coal;
5010 	c->tx_max_coalesced_frames = port->txqs[0]->done_pkts_coal;
5011 	c->tx_coalesce_usecs       = port->tx_time_coal;
5012 	return 0;
5013 }
5014 
5015 static void mvpp2_ethtool_get_drvinfo(struct net_device *dev,
5016 				      struct ethtool_drvinfo *drvinfo)
5017 {
5018 	strlcpy(drvinfo->driver, MVPP2_DRIVER_NAME,
5019 		sizeof(drvinfo->driver));
5020 	strlcpy(drvinfo->version, MVPP2_DRIVER_VERSION,
5021 		sizeof(drvinfo->version));
5022 	strlcpy(drvinfo->bus_info, dev_name(&dev->dev),
5023 		sizeof(drvinfo->bus_info));
5024 }
5025 
5026 static void mvpp2_ethtool_get_ringparam(struct net_device *dev,
5027 					struct ethtool_ringparam *ring)
5028 {
5029 	struct mvpp2_port *port = netdev_priv(dev);
5030 
5031 	ring->rx_max_pending = MVPP2_MAX_RXD_MAX;
5032 	ring->tx_max_pending = MVPP2_MAX_TXD_MAX;
5033 	ring->rx_pending = port->rx_ring_size;
5034 	ring->tx_pending = port->tx_ring_size;
5035 }
5036 
5037 static int mvpp2_ethtool_set_ringparam(struct net_device *dev,
5038 				       struct ethtool_ringparam *ring)
5039 {
5040 	struct mvpp2_port *port = netdev_priv(dev);
5041 	u16 prev_rx_ring_size = port->rx_ring_size;
5042 	u16 prev_tx_ring_size = port->tx_ring_size;
5043 	int err;
5044 
5045 	err = mvpp2_check_ringparam_valid(dev, ring);
5046 	if (err)
5047 		return err;
5048 
5049 	if (!netif_running(dev)) {
5050 		port->rx_ring_size = ring->rx_pending;
5051 		port->tx_ring_size = ring->tx_pending;
5052 		return 0;
5053 	}
5054 
5055 	/* The interface is running, so we have to force a
5056 	 * reallocation of the queues
5057 	 */
5058 	mvpp2_stop_dev(port);
5059 	mvpp2_cleanup_rxqs(port);
5060 	mvpp2_cleanup_txqs(port);
5061 
5062 	port->rx_ring_size = ring->rx_pending;
5063 	port->tx_ring_size = ring->tx_pending;
5064 
5065 	err = mvpp2_setup_rxqs(port);
5066 	if (err) {
5067 		/* Reallocate Rx queues with the original ring size */
5068 		port->rx_ring_size = prev_rx_ring_size;
5069 		ring->rx_pending = prev_rx_ring_size;
5070 		err = mvpp2_setup_rxqs(port);
5071 		if (err)
5072 			goto err_out;
5073 	}
5074 	err = mvpp2_setup_txqs(port);
5075 	if (err) {
5076 		/* Reallocate Tx queues with the original ring size */
5077 		port->tx_ring_size = prev_tx_ring_size;
5078 		ring->tx_pending = prev_tx_ring_size;
5079 		err = mvpp2_setup_txqs(port);
5080 		if (err)
5081 			goto err_clean_rxqs;
5082 	}
5083 
5084 	mvpp2_start_dev(port);
5085 	mvpp2_egress_enable(port);
5086 	mvpp2_ingress_enable(port);
5087 
5088 	return 0;
5089 
5090 err_clean_rxqs:
5091 	mvpp2_cleanup_rxqs(port);
5092 err_out:
5093 	netdev_err(dev, "failed to change ring parameters");
5094 	return err;
5095 }
5096 
5097 static void mvpp2_ethtool_get_pause_param(struct net_device *dev,
5098 					  struct ethtool_pauseparam *pause)
5099 {
5100 	struct mvpp2_port *port = netdev_priv(dev);
5101 
5102 	if (!port->phylink)
5103 		return;
5104 
5105 	phylink_ethtool_get_pauseparam(port->phylink, pause);
5106 }
5107 
5108 static int mvpp2_ethtool_set_pause_param(struct net_device *dev,
5109 					 struct ethtool_pauseparam *pause)
5110 {
5111 	struct mvpp2_port *port = netdev_priv(dev);
5112 
5113 	if (!port->phylink)
5114 		return -ENOTSUPP;
5115 
5116 	return phylink_ethtool_set_pauseparam(port->phylink, pause);
5117 }
5118 
5119 static int mvpp2_ethtool_get_link_ksettings(struct net_device *dev,
5120 					    struct ethtool_link_ksettings *cmd)
5121 {
5122 	struct mvpp2_port *port = netdev_priv(dev);
5123 
5124 	if (!port->phylink)
5125 		return -ENOTSUPP;
5126 
5127 	return phylink_ethtool_ksettings_get(port->phylink, cmd);
5128 }
5129 
5130 static int mvpp2_ethtool_set_link_ksettings(struct net_device *dev,
5131 					    const struct ethtool_link_ksettings *cmd)
5132 {
5133 	struct mvpp2_port *port = netdev_priv(dev);
5134 
5135 	if (!port->phylink)
5136 		return -ENOTSUPP;
5137 
5138 	return phylink_ethtool_ksettings_set(port->phylink, cmd);
5139 }
5140 
5141 static int mvpp2_ethtool_get_rxnfc(struct net_device *dev,
5142 				   struct ethtool_rxnfc *info, u32 *rules)
5143 {
5144 	struct mvpp2_port *port = netdev_priv(dev);
5145 	int ret = 0, i, loc = 0;
5146 
5147 	if (!mvpp22_rss_is_supported())
5148 		return -EOPNOTSUPP;
5149 
5150 	switch (info->cmd) {
5151 	case ETHTOOL_GRXFH:
5152 		ret = mvpp2_ethtool_rxfh_get(port, info);
5153 		break;
5154 	case ETHTOOL_GRXRINGS:
5155 		info->data = port->nrxqs;
5156 		break;
5157 	case ETHTOOL_GRXCLSRLCNT:
5158 		info->rule_cnt = port->n_rfs_rules;
5159 		break;
5160 	case ETHTOOL_GRXCLSRULE:
5161 		ret = mvpp2_ethtool_cls_rule_get(port, info);
5162 		break;
5163 	case ETHTOOL_GRXCLSRLALL:
5164 		for (i = 0; i < MVPP2_N_RFS_ENTRIES_PER_FLOW; i++) {
5165 			if (port->rfs_rules[i])
5166 				rules[loc++] = i;
5167 		}
5168 		break;
5169 	default:
5170 		return -ENOTSUPP;
5171 	}
5172 
5173 	return ret;
5174 }
5175 
5176 static int mvpp2_ethtool_set_rxnfc(struct net_device *dev,
5177 				   struct ethtool_rxnfc *info)
5178 {
5179 	struct mvpp2_port *port = netdev_priv(dev);
5180 	int ret = 0;
5181 
5182 	if (!mvpp22_rss_is_supported())
5183 		return -EOPNOTSUPP;
5184 
5185 	switch (info->cmd) {
5186 	case ETHTOOL_SRXFH:
5187 		ret = mvpp2_ethtool_rxfh_set(port, info);
5188 		break;
5189 	case ETHTOOL_SRXCLSRLINS:
5190 		ret = mvpp2_ethtool_cls_rule_ins(port, info);
5191 		break;
5192 	case ETHTOOL_SRXCLSRLDEL:
5193 		ret = mvpp2_ethtool_cls_rule_del(port, info);
5194 		break;
5195 	default:
5196 		return -EOPNOTSUPP;
5197 	}
5198 	return ret;
5199 }
5200 
5201 static u32 mvpp2_ethtool_get_rxfh_indir_size(struct net_device *dev)
5202 {
5203 	return mvpp22_rss_is_supported() ? MVPP22_RSS_TABLE_ENTRIES : 0;
5204 }
5205 
5206 static int mvpp2_ethtool_get_rxfh(struct net_device *dev, u32 *indir, u8 *key,
5207 				  u8 *hfunc)
5208 {
5209 	struct mvpp2_port *port = netdev_priv(dev);
5210 	int ret = 0;
5211 
5212 	if (!mvpp22_rss_is_supported())
5213 		return -EOPNOTSUPP;
5214 
5215 	if (indir)
5216 		ret = mvpp22_port_rss_ctx_indir_get(port, 0, indir);
5217 
5218 	if (hfunc)
5219 		*hfunc = ETH_RSS_HASH_CRC32;
5220 
5221 	return ret;
5222 }
5223 
5224 static int mvpp2_ethtool_set_rxfh(struct net_device *dev, const u32 *indir,
5225 				  const u8 *key, const u8 hfunc)
5226 {
5227 	struct mvpp2_port *port = netdev_priv(dev);
5228 	int ret = 0;
5229 
5230 	if (!mvpp22_rss_is_supported())
5231 		return -EOPNOTSUPP;
5232 
5233 	if (hfunc != ETH_RSS_HASH_NO_CHANGE && hfunc != ETH_RSS_HASH_CRC32)
5234 		return -EOPNOTSUPP;
5235 
5236 	if (key)
5237 		return -EOPNOTSUPP;
5238 
5239 	if (indir)
5240 		ret = mvpp22_port_rss_ctx_indir_set(port, 0, indir);
5241 
5242 	return ret;
5243 }
5244 
5245 static int mvpp2_ethtool_get_rxfh_context(struct net_device *dev, u32 *indir,
5246 					  u8 *key, u8 *hfunc, u32 rss_context)
5247 {
5248 	struct mvpp2_port *port = netdev_priv(dev);
5249 	int ret = 0;
5250 
5251 	if (!mvpp22_rss_is_supported())
5252 		return -EOPNOTSUPP;
5253 	if (rss_context >= MVPP22_N_RSS_TABLES)
5254 		return -EINVAL;
5255 
5256 	if (hfunc)
5257 		*hfunc = ETH_RSS_HASH_CRC32;
5258 
5259 	if (indir)
5260 		ret = mvpp22_port_rss_ctx_indir_get(port, rss_context, indir);
5261 
5262 	return ret;
5263 }
5264 
5265 static int mvpp2_ethtool_set_rxfh_context(struct net_device *dev,
5266 					  const u32 *indir, const u8 *key,
5267 					  const u8 hfunc, u32 *rss_context,
5268 					  bool delete)
5269 {
5270 	struct mvpp2_port *port = netdev_priv(dev);
5271 	int ret;
5272 
5273 	if (!mvpp22_rss_is_supported())
5274 		return -EOPNOTSUPP;
5275 
5276 	if (hfunc != ETH_RSS_HASH_NO_CHANGE && hfunc != ETH_RSS_HASH_CRC32)
5277 		return -EOPNOTSUPP;
5278 
5279 	if (key)
5280 		return -EOPNOTSUPP;
5281 
5282 	if (delete)
5283 		return mvpp22_port_rss_ctx_delete(port, *rss_context);
5284 
5285 	if (*rss_context == ETH_RXFH_CONTEXT_ALLOC) {
5286 		ret = mvpp22_port_rss_ctx_create(port, rss_context);
5287 		if (ret)
5288 			return ret;
5289 	}
5290 
5291 	return mvpp22_port_rss_ctx_indir_set(port, *rss_context, indir);
5292 }
5293 /* Device ops */
5294 
5295 static const struct net_device_ops mvpp2_netdev_ops = {
5296 	.ndo_open		= mvpp2_open,
5297 	.ndo_stop		= mvpp2_stop,
5298 	.ndo_start_xmit		= mvpp2_tx,
5299 	.ndo_set_rx_mode	= mvpp2_set_rx_mode,
5300 	.ndo_set_mac_address	= mvpp2_set_mac_address,
5301 	.ndo_change_mtu		= mvpp2_change_mtu,
5302 	.ndo_get_stats64	= mvpp2_get_stats64,
5303 	.ndo_do_ioctl		= mvpp2_ioctl,
5304 	.ndo_vlan_rx_add_vid	= mvpp2_vlan_rx_add_vid,
5305 	.ndo_vlan_rx_kill_vid	= mvpp2_vlan_rx_kill_vid,
5306 	.ndo_set_features	= mvpp2_set_features,
5307 	.ndo_bpf		= mvpp2_xdp,
5308 	.ndo_xdp_xmit		= mvpp2_xdp_xmit,
5309 };
5310 
5311 static const struct ethtool_ops mvpp2_eth_tool_ops = {
5312 	.supported_coalesce_params = ETHTOOL_COALESCE_USECS |
5313 				     ETHTOOL_COALESCE_MAX_FRAMES,
5314 	.nway_reset		= mvpp2_ethtool_nway_reset,
5315 	.get_link		= ethtool_op_get_link,
5316 	.get_ts_info		= mvpp2_ethtool_get_ts_info,
5317 	.set_coalesce		= mvpp2_ethtool_set_coalesce,
5318 	.get_coalesce		= mvpp2_ethtool_get_coalesce,
5319 	.get_drvinfo		= mvpp2_ethtool_get_drvinfo,
5320 	.get_ringparam		= mvpp2_ethtool_get_ringparam,
5321 	.set_ringparam		= mvpp2_ethtool_set_ringparam,
5322 	.get_strings		= mvpp2_ethtool_get_strings,
5323 	.get_ethtool_stats	= mvpp2_ethtool_get_stats,
5324 	.get_sset_count		= mvpp2_ethtool_get_sset_count,
5325 	.get_pauseparam		= mvpp2_ethtool_get_pause_param,
5326 	.set_pauseparam		= mvpp2_ethtool_set_pause_param,
5327 	.get_link_ksettings	= mvpp2_ethtool_get_link_ksettings,
5328 	.set_link_ksettings	= mvpp2_ethtool_set_link_ksettings,
5329 	.get_rxnfc		= mvpp2_ethtool_get_rxnfc,
5330 	.set_rxnfc		= mvpp2_ethtool_set_rxnfc,
5331 	.get_rxfh_indir_size	= mvpp2_ethtool_get_rxfh_indir_size,
5332 	.get_rxfh		= mvpp2_ethtool_get_rxfh,
5333 	.set_rxfh		= mvpp2_ethtool_set_rxfh,
5334 	.get_rxfh_context	= mvpp2_ethtool_get_rxfh_context,
5335 	.set_rxfh_context	= mvpp2_ethtool_set_rxfh_context,
5336 };
5337 
5338 /* Used for PPv2.1, or PPv2.2 with the old Device Tree binding that
5339  * had a single IRQ defined per-port.
5340  */
5341 static int mvpp2_simple_queue_vectors_init(struct mvpp2_port *port,
5342 					   struct device_node *port_node)
5343 {
5344 	struct mvpp2_queue_vector *v = &port->qvecs[0];
5345 
5346 	v->first_rxq = 0;
5347 	v->nrxqs = port->nrxqs;
5348 	v->type = MVPP2_QUEUE_VECTOR_SHARED;
5349 	v->sw_thread_id = 0;
5350 	v->sw_thread_mask = *cpumask_bits(cpu_online_mask);
5351 	v->port = port;
5352 	v->irq = irq_of_parse_and_map(port_node, 0);
5353 	if (v->irq <= 0)
5354 		return -EINVAL;
5355 	netif_napi_add(port->dev, &v->napi, mvpp2_poll,
5356 		       NAPI_POLL_WEIGHT);
5357 
5358 	port->nqvecs = 1;
5359 
5360 	return 0;
5361 }
5362 
5363 static int mvpp2_multi_queue_vectors_init(struct mvpp2_port *port,
5364 					  struct device_node *port_node)
5365 {
5366 	struct mvpp2 *priv = port->priv;
5367 	struct mvpp2_queue_vector *v;
5368 	int i, ret;
5369 
5370 	switch (queue_mode) {
5371 	case MVPP2_QDIST_SINGLE_MODE:
5372 		port->nqvecs = priv->nthreads + 1;
5373 		break;
5374 	case MVPP2_QDIST_MULTI_MODE:
5375 		port->nqvecs = priv->nthreads;
5376 		break;
5377 	}
5378 
5379 	for (i = 0; i < port->nqvecs; i++) {
5380 		char irqname[16];
5381 
5382 		v = port->qvecs + i;
5383 
5384 		v->port = port;
5385 		v->type = MVPP2_QUEUE_VECTOR_PRIVATE;
5386 		v->sw_thread_id = i;
5387 		v->sw_thread_mask = BIT(i);
5388 
5389 		if (port->flags & MVPP2_F_DT_COMPAT)
5390 			snprintf(irqname, sizeof(irqname), "tx-cpu%d", i);
5391 		else
5392 			snprintf(irqname, sizeof(irqname), "hif%d", i);
5393 
5394 		if (queue_mode == MVPP2_QDIST_MULTI_MODE) {
5395 			v->first_rxq = i;
5396 			v->nrxqs = 1;
5397 		} else if (queue_mode == MVPP2_QDIST_SINGLE_MODE &&
5398 			   i == (port->nqvecs - 1)) {
5399 			v->first_rxq = 0;
5400 			v->nrxqs = port->nrxqs;
5401 			v->type = MVPP2_QUEUE_VECTOR_SHARED;
5402 
5403 			if (port->flags & MVPP2_F_DT_COMPAT)
5404 				strncpy(irqname, "rx-shared", sizeof(irqname));
5405 		}
5406 
5407 		if (port_node)
5408 			v->irq = of_irq_get_byname(port_node, irqname);
5409 		else
5410 			v->irq = fwnode_irq_get(port->fwnode, i);
5411 		if (v->irq <= 0) {
5412 			ret = -EINVAL;
5413 			goto err;
5414 		}
5415 
5416 		netif_napi_add(port->dev, &v->napi, mvpp2_poll,
5417 			       NAPI_POLL_WEIGHT);
5418 	}
5419 
5420 	return 0;
5421 
5422 err:
5423 	for (i = 0; i < port->nqvecs; i++)
5424 		irq_dispose_mapping(port->qvecs[i].irq);
5425 	return ret;
5426 }
5427 
5428 static int mvpp2_queue_vectors_init(struct mvpp2_port *port,
5429 				    struct device_node *port_node)
5430 {
5431 	if (port->has_tx_irqs)
5432 		return mvpp2_multi_queue_vectors_init(port, port_node);
5433 	else
5434 		return mvpp2_simple_queue_vectors_init(port, port_node);
5435 }
5436 
5437 static void mvpp2_queue_vectors_deinit(struct mvpp2_port *port)
5438 {
5439 	int i;
5440 
5441 	for (i = 0; i < port->nqvecs; i++)
5442 		irq_dispose_mapping(port->qvecs[i].irq);
5443 }
5444 
5445 /* Configure Rx queue group interrupt for this port */
5446 static void mvpp2_rx_irqs_setup(struct mvpp2_port *port)
5447 {
5448 	struct mvpp2 *priv = port->priv;
5449 	u32 val;
5450 	int i;
5451 
5452 	if (priv->hw_version == MVPP21) {
5453 		mvpp2_write(priv, MVPP21_ISR_RXQ_GROUP_REG(port->id),
5454 			    port->nrxqs);
5455 		return;
5456 	}
5457 
5458 	/* Handle the more complicated PPv2.2 case */
5459 	for (i = 0; i < port->nqvecs; i++) {
5460 		struct mvpp2_queue_vector *qv = port->qvecs + i;
5461 
5462 		if (!qv->nrxqs)
5463 			continue;
5464 
5465 		val = qv->sw_thread_id;
5466 		val |= port->id << MVPP22_ISR_RXQ_GROUP_INDEX_GROUP_OFFSET;
5467 		mvpp2_write(priv, MVPP22_ISR_RXQ_GROUP_INDEX_REG, val);
5468 
5469 		val = qv->first_rxq;
5470 		val |= qv->nrxqs << MVPP22_ISR_RXQ_SUB_GROUP_SIZE_OFFSET;
5471 		mvpp2_write(priv, MVPP22_ISR_RXQ_SUB_GROUP_CONFIG_REG, val);
5472 	}
5473 }
5474 
5475 /* Initialize port HW */
5476 static int mvpp2_port_init(struct mvpp2_port *port)
5477 {
5478 	struct device *dev = port->dev->dev.parent;
5479 	struct mvpp2 *priv = port->priv;
5480 	struct mvpp2_txq_pcpu *txq_pcpu;
5481 	unsigned int thread;
5482 	int queue, err;
5483 
5484 	/* Checks for hardware constraints */
5485 	if (port->first_rxq + port->nrxqs >
5486 	    MVPP2_MAX_PORTS * priv->max_port_rxqs)
5487 		return -EINVAL;
5488 
5489 	if (port->nrxqs > priv->max_port_rxqs || port->ntxqs > MVPP2_MAX_TXQ)
5490 		return -EINVAL;
5491 
5492 	/* Disable port */
5493 	mvpp2_egress_disable(port);
5494 	mvpp2_port_disable(port);
5495 
5496 	port->tx_time_coal = MVPP2_TXDONE_COAL_USEC;
5497 
5498 	port->txqs = devm_kcalloc(dev, port->ntxqs, sizeof(*port->txqs),
5499 				  GFP_KERNEL);
5500 	if (!port->txqs)
5501 		return -ENOMEM;
5502 
5503 	/* Associate physical Tx queues to this port and initialize.
5504 	 * The mapping is predefined.
5505 	 */
5506 	for (queue = 0; queue < port->ntxqs; queue++) {
5507 		int queue_phy_id = mvpp2_txq_phys(port->id, queue);
5508 		struct mvpp2_tx_queue *txq;
5509 
5510 		txq = devm_kzalloc(dev, sizeof(*txq), GFP_KERNEL);
5511 		if (!txq) {
5512 			err = -ENOMEM;
5513 			goto err_free_percpu;
5514 		}
5515 
5516 		txq->pcpu = alloc_percpu(struct mvpp2_txq_pcpu);
5517 		if (!txq->pcpu) {
5518 			err = -ENOMEM;
5519 			goto err_free_percpu;
5520 		}
5521 
5522 		txq->id = queue_phy_id;
5523 		txq->log_id = queue;
5524 		txq->done_pkts_coal = MVPP2_TXDONE_COAL_PKTS_THRESH;
5525 		for (thread = 0; thread < priv->nthreads; thread++) {
5526 			txq_pcpu = per_cpu_ptr(txq->pcpu, thread);
5527 			txq_pcpu->thread = thread;
5528 		}
5529 
5530 		port->txqs[queue] = txq;
5531 	}
5532 
5533 	port->rxqs = devm_kcalloc(dev, port->nrxqs, sizeof(*port->rxqs),
5534 				  GFP_KERNEL);
5535 	if (!port->rxqs) {
5536 		err = -ENOMEM;
5537 		goto err_free_percpu;
5538 	}
5539 
5540 	/* Allocate and initialize Rx queue for this port */
5541 	for (queue = 0; queue < port->nrxqs; queue++) {
5542 		struct mvpp2_rx_queue *rxq;
5543 
5544 		/* Map physical Rx queue to port's logical Rx queue */
5545 		rxq = devm_kzalloc(dev, sizeof(*rxq), GFP_KERNEL);
5546 		if (!rxq) {
5547 			err = -ENOMEM;
5548 			goto err_free_percpu;
5549 		}
5550 		/* Map this Rx queue to a physical queue */
5551 		rxq->id = port->first_rxq + queue;
5552 		rxq->port = port->id;
5553 		rxq->logic_rxq = queue;
5554 
5555 		port->rxqs[queue] = rxq;
5556 	}
5557 
5558 	mvpp2_rx_irqs_setup(port);
5559 
5560 	/* Create Rx descriptor rings */
5561 	for (queue = 0; queue < port->nrxqs; queue++) {
5562 		struct mvpp2_rx_queue *rxq = port->rxqs[queue];
5563 
5564 		rxq->size = port->rx_ring_size;
5565 		rxq->pkts_coal = MVPP2_RX_COAL_PKTS;
5566 		rxq->time_coal = MVPP2_RX_COAL_USEC;
5567 	}
5568 
5569 	mvpp2_ingress_disable(port);
5570 
5571 	/* Port default configuration */
5572 	mvpp2_defaults_set(port);
5573 
5574 	/* Port's classifier configuration */
5575 	mvpp2_cls_oversize_rxq_set(port);
5576 	mvpp2_cls_port_config(port);
5577 
5578 	if (mvpp22_rss_is_supported())
5579 		mvpp22_port_rss_init(port);
5580 
5581 	/* Provide an initial Rx packet size */
5582 	port->pkt_size = MVPP2_RX_PKT_SIZE(port->dev->mtu);
5583 
5584 	/* Initialize pools for swf */
5585 	err = mvpp2_swf_bm_pool_init(port);
5586 	if (err)
5587 		goto err_free_percpu;
5588 
5589 	/* Clear all port stats */
5590 	mvpp2_read_stats(port);
5591 	memset(port->ethtool_stats, 0,
5592 	       MVPP2_N_ETHTOOL_STATS(port->ntxqs, port->nrxqs) * sizeof(u64));
5593 
5594 	return 0;
5595 
5596 err_free_percpu:
5597 	for (queue = 0; queue < port->ntxqs; queue++) {
5598 		if (!port->txqs[queue])
5599 			continue;
5600 		free_percpu(port->txqs[queue]->pcpu);
5601 	}
5602 	return err;
5603 }
5604 
5605 static bool mvpp22_port_has_legacy_tx_irqs(struct device_node *port_node,
5606 					   unsigned long *flags)
5607 {
5608 	char *irqs[5] = { "rx-shared", "tx-cpu0", "tx-cpu1", "tx-cpu2",
5609 			  "tx-cpu3" };
5610 	int i;
5611 
5612 	for (i = 0; i < 5; i++)
5613 		if (of_property_match_string(port_node, "interrupt-names",
5614 					     irqs[i]) < 0)
5615 			return false;
5616 
5617 	*flags |= MVPP2_F_DT_COMPAT;
5618 	return true;
5619 }
5620 
5621 /* Checks if the port dt description has the required Tx interrupts:
5622  * - PPv2.1: there are no such interrupts.
5623  * - PPv2.2:
5624  *   - The old DTs have: "rx-shared", "tx-cpuX" with X in [0...3]
5625  *   - The new ones have: "hifX" with X in [0..8]
5626  *
5627  * All those variants are supported to keep the backward compatibility.
5628  */
5629 static bool mvpp2_port_has_irqs(struct mvpp2 *priv,
5630 				struct device_node *port_node,
5631 				unsigned long *flags)
5632 {
5633 	char name[5];
5634 	int i;
5635 
5636 	/* ACPI */
5637 	if (!port_node)
5638 		return true;
5639 
5640 	if (priv->hw_version == MVPP21)
5641 		return false;
5642 
5643 	if (mvpp22_port_has_legacy_tx_irqs(port_node, flags))
5644 		return true;
5645 
5646 	for (i = 0; i < MVPP2_MAX_THREADS; i++) {
5647 		snprintf(name, 5, "hif%d", i);
5648 		if (of_property_match_string(port_node, "interrupt-names",
5649 					     name) < 0)
5650 			return false;
5651 	}
5652 
5653 	return true;
5654 }
5655 
5656 static void mvpp2_port_copy_mac_addr(struct net_device *dev, struct mvpp2 *priv,
5657 				     struct fwnode_handle *fwnode,
5658 				     char **mac_from)
5659 {
5660 	struct mvpp2_port *port = netdev_priv(dev);
5661 	char hw_mac_addr[ETH_ALEN] = {0};
5662 	char fw_mac_addr[ETH_ALEN];
5663 
5664 	if (fwnode_get_mac_address(fwnode, fw_mac_addr, ETH_ALEN)) {
5665 		*mac_from = "firmware node";
5666 		ether_addr_copy(dev->dev_addr, fw_mac_addr);
5667 		return;
5668 	}
5669 
5670 	if (priv->hw_version == MVPP21) {
5671 		mvpp21_get_mac_address(port, hw_mac_addr);
5672 		if (is_valid_ether_addr(hw_mac_addr)) {
5673 			*mac_from = "hardware";
5674 			ether_addr_copy(dev->dev_addr, hw_mac_addr);
5675 			return;
5676 		}
5677 	}
5678 
5679 	*mac_from = "random";
5680 	eth_hw_addr_random(dev);
5681 }
5682 
5683 static struct mvpp2_port *mvpp2_phylink_to_port(struct phylink_config *config)
5684 {
5685 	return container_of(config, struct mvpp2_port, phylink_config);
5686 }
5687 
5688 static struct mvpp2_port *mvpp2_pcs_to_port(struct phylink_pcs *pcs)
5689 {
5690 	return container_of(pcs, struct mvpp2_port, phylink_pcs);
5691 }
5692 
5693 static void mvpp2_xlg_pcs_get_state(struct phylink_pcs *pcs,
5694 				    struct phylink_link_state *state)
5695 {
5696 	struct mvpp2_port *port = mvpp2_pcs_to_port(pcs);
5697 	u32 val;
5698 
5699 	state->speed = SPEED_10000;
5700 	state->duplex = 1;
5701 	state->an_complete = 1;
5702 
5703 	val = readl(port->base + MVPP22_XLG_STATUS);
5704 	state->link = !!(val & MVPP22_XLG_STATUS_LINK_UP);
5705 
5706 	state->pause = 0;
5707 	val = readl(port->base + MVPP22_XLG_CTRL0_REG);
5708 	if (val & MVPP22_XLG_CTRL0_TX_FLOW_CTRL_EN)
5709 		state->pause |= MLO_PAUSE_TX;
5710 	if (val & MVPP22_XLG_CTRL0_RX_FLOW_CTRL_EN)
5711 		state->pause |= MLO_PAUSE_RX;
5712 }
5713 
5714 static int mvpp2_xlg_pcs_config(struct phylink_pcs *pcs,
5715 				unsigned int mode,
5716 				phy_interface_t interface,
5717 				const unsigned long *advertising,
5718 				bool permit_pause_to_mac)
5719 {
5720 	return 0;
5721 }
5722 
5723 static const struct phylink_pcs_ops mvpp2_phylink_xlg_pcs_ops = {
5724 	.pcs_get_state = mvpp2_xlg_pcs_get_state,
5725 	.pcs_config = mvpp2_xlg_pcs_config,
5726 };
5727 
5728 static void mvpp2_gmac_pcs_get_state(struct phylink_pcs *pcs,
5729 				     struct phylink_link_state *state)
5730 {
5731 	struct mvpp2_port *port = mvpp2_pcs_to_port(pcs);
5732 	u32 val;
5733 
5734 	val = readl(port->base + MVPP2_GMAC_STATUS0);
5735 
5736 	state->an_complete = !!(val & MVPP2_GMAC_STATUS0_AN_COMPLETE);
5737 	state->link = !!(val & MVPP2_GMAC_STATUS0_LINK_UP);
5738 	state->duplex = !!(val & MVPP2_GMAC_STATUS0_FULL_DUPLEX);
5739 
5740 	switch (port->phy_interface) {
5741 	case PHY_INTERFACE_MODE_1000BASEX:
5742 		state->speed = SPEED_1000;
5743 		break;
5744 	case PHY_INTERFACE_MODE_2500BASEX:
5745 		state->speed = SPEED_2500;
5746 		break;
5747 	default:
5748 		if (val & MVPP2_GMAC_STATUS0_GMII_SPEED)
5749 			state->speed = SPEED_1000;
5750 		else if (val & MVPP2_GMAC_STATUS0_MII_SPEED)
5751 			state->speed = SPEED_100;
5752 		else
5753 			state->speed = SPEED_10;
5754 	}
5755 
5756 	state->pause = 0;
5757 	if (val & MVPP2_GMAC_STATUS0_RX_PAUSE)
5758 		state->pause |= MLO_PAUSE_RX;
5759 	if (val & MVPP2_GMAC_STATUS0_TX_PAUSE)
5760 		state->pause |= MLO_PAUSE_TX;
5761 }
5762 
5763 static int mvpp2_gmac_pcs_config(struct phylink_pcs *pcs, unsigned int mode,
5764 				 phy_interface_t interface,
5765 				 const unsigned long *advertising,
5766 				 bool permit_pause_to_mac)
5767 {
5768 	struct mvpp2_port *port = mvpp2_pcs_to_port(pcs);
5769 	u32 mask, val, an, old_an, changed;
5770 
5771 	mask = MVPP2_GMAC_IN_BAND_AUTONEG_BYPASS |
5772 	       MVPP2_GMAC_IN_BAND_AUTONEG |
5773 	       MVPP2_GMAC_AN_SPEED_EN |
5774 	       MVPP2_GMAC_FLOW_CTRL_AUTONEG |
5775 	       MVPP2_GMAC_AN_DUPLEX_EN;
5776 
5777 	if (phylink_autoneg_inband(mode)) {
5778 		mask |= MVPP2_GMAC_CONFIG_MII_SPEED |
5779 			MVPP2_GMAC_CONFIG_GMII_SPEED |
5780 			MVPP2_GMAC_CONFIG_FULL_DUPLEX;
5781 		val = MVPP2_GMAC_IN_BAND_AUTONEG;
5782 
5783 		if (interface == PHY_INTERFACE_MODE_SGMII) {
5784 			/* SGMII mode receives the speed and duplex from PHY */
5785 			val |= MVPP2_GMAC_AN_SPEED_EN |
5786 			       MVPP2_GMAC_AN_DUPLEX_EN;
5787 		} else {
5788 			/* 802.3z mode has fixed speed and duplex */
5789 			val |= MVPP2_GMAC_CONFIG_GMII_SPEED |
5790 			       MVPP2_GMAC_CONFIG_FULL_DUPLEX;
5791 
5792 			/* The FLOW_CTRL_AUTONEG bit selects either the hardware
5793 			 * automatically or the bits in MVPP22_GMAC_CTRL_4_REG
5794 			 * manually controls the GMAC pause modes.
5795 			 */
5796 			if (permit_pause_to_mac)
5797 				val |= MVPP2_GMAC_FLOW_CTRL_AUTONEG;
5798 
5799 			/* Configure advertisement bits */
5800 			mask |= MVPP2_GMAC_FC_ADV_EN | MVPP2_GMAC_FC_ADV_ASM_EN;
5801 			if (phylink_test(advertising, Pause))
5802 				val |= MVPP2_GMAC_FC_ADV_EN;
5803 			if (phylink_test(advertising, Asym_Pause))
5804 				val |= MVPP2_GMAC_FC_ADV_ASM_EN;
5805 		}
5806 	} else {
5807 		val = 0;
5808 	}
5809 
5810 	old_an = an = readl(port->base + MVPP2_GMAC_AUTONEG_CONFIG);
5811 	an = (an & ~mask) | val;
5812 	changed = an ^ old_an;
5813 	if (changed)
5814 		writel(an, port->base + MVPP2_GMAC_AUTONEG_CONFIG);
5815 
5816 	/* We are only interested in the advertisement bits changing */
5817 	return changed & (MVPP2_GMAC_FC_ADV_EN | MVPP2_GMAC_FC_ADV_ASM_EN);
5818 }
5819 
5820 static void mvpp2_gmac_pcs_an_restart(struct phylink_pcs *pcs)
5821 {
5822 	struct mvpp2_port *port = mvpp2_pcs_to_port(pcs);
5823 	u32 val = readl(port->base + MVPP2_GMAC_AUTONEG_CONFIG);
5824 
5825 	writel(val | MVPP2_GMAC_IN_BAND_RESTART_AN,
5826 	       port->base + MVPP2_GMAC_AUTONEG_CONFIG);
5827 	writel(val & ~MVPP2_GMAC_IN_BAND_RESTART_AN,
5828 	       port->base + MVPP2_GMAC_AUTONEG_CONFIG);
5829 }
5830 
5831 static const struct phylink_pcs_ops mvpp2_phylink_gmac_pcs_ops = {
5832 	.pcs_get_state = mvpp2_gmac_pcs_get_state,
5833 	.pcs_config = mvpp2_gmac_pcs_config,
5834 	.pcs_an_restart = mvpp2_gmac_pcs_an_restart,
5835 };
5836 
5837 static void mvpp2_phylink_validate(struct phylink_config *config,
5838 				   unsigned long *supported,
5839 				   struct phylink_link_state *state)
5840 {
5841 	struct mvpp2_port *port = mvpp2_phylink_to_port(config);
5842 	__ETHTOOL_DECLARE_LINK_MODE_MASK(mask) = { 0, };
5843 
5844 	/* Invalid combinations */
5845 	switch (state->interface) {
5846 	case PHY_INTERFACE_MODE_10GBASER:
5847 	case PHY_INTERFACE_MODE_XAUI:
5848 		if (!mvpp2_port_supports_xlg(port))
5849 			goto empty_set;
5850 		break;
5851 	case PHY_INTERFACE_MODE_RGMII:
5852 	case PHY_INTERFACE_MODE_RGMII_ID:
5853 	case PHY_INTERFACE_MODE_RGMII_RXID:
5854 	case PHY_INTERFACE_MODE_RGMII_TXID:
5855 		if (!mvpp2_port_supports_rgmii(port))
5856 			goto empty_set;
5857 		break;
5858 	default:
5859 		break;
5860 	}
5861 
5862 	phylink_set(mask, Autoneg);
5863 	phylink_set_port_modes(mask);
5864 	phylink_set(mask, Pause);
5865 	phylink_set(mask, Asym_Pause);
5866 
5867 	switch (state->interface) {
5868 	case PHY_INTERFACE_MODE_10GBASER:
5869 	case PHY_INTERFACE_MODE_XAUI:
5870 	case PHY_INTERFACE_MODE_NA:
5871 		if (mvpp2_port_supports_xlg(port)) {
5872 			phylink_set(mask, 10000baseT_Full);
5873 			phylink_set(mask, 10000baseCR_Full);
5874 			phylink_set(mask, 10000baseSR_Full);
5875 			phylink_set(mask, 10000baseLR_Full);
5876 			phylink_set(mask, 10000baseLRM_Full);
5877 			phylink_set(mask, 10000baseER_Full);
5878 			phylink_set(mask, 10000baseKR_Full);
5879 		}
5880 		if (state->interface != PHY_INTERFACE_MODE_NA)
5881 			break;
5882 		fallthrough;
5883 	case PHY_INTERFACE_MODE_RGMII:
5884 	case PHY_INTERFACE_MODE_RGMII_ID:
5885 	case PHY_INTERFACE_MODE_RGMII_RXID:
5886 	case PHY_INTERFACE_MODE_RGMII_TXID:
5887 	case PHY_INTERFACE_MODE_SGMII:
5888 		phylink_set(mask, 10baseT_Half);
5889 		phylink_set(mask, 10baseT_Full);
5890 		phylink_set(mask, 100baseT_Half);
5891 		phylink_set(mask, 100baseT_Full);
5892 		phylink_set(mask, 1000baseT_Full);
5893 		phylink_set(mask, 1000baseX_Full);
5894 		if (state->interface != PHY_INTERFACE_MODE_NA)
5895 			break;
5896 		fallthrough;
5897 	case PHY_INTERFACE_MODE_1000BASEX:
5898 	case PHY_INTERFACE_MODE_2500BASEX:
5899 		if (port->comphy ||
5900 		    state->interface != PHY_INTERFACE_MODE_2500BASEX) {
5901 			phylink_set(mask, 1000baseT_Full);
5902 			phylink_set(mask, 1000baseX_Full);
5903 		}
5904 		if (port->comphy ||
5905 		    state->interface == PHY_INTERFACE_MODE_2500BASEX) {
5906 			phylink_set(mask, 2500baseT_Full);
5907 			phylink_set(mask, 2500baseX_Full);
5908 		}
5909 		break;
5910 	default:
5911 		goto empty_set;
5912 	}
5913 
5914 	bitmap_and(supported, supported, mask, __ETHTOOL_LINK_MODE_MASK_NBITS);
5915 	bitmap_and(state->advertising, state->advertising, mask,
5916 		   __ETHTOOL_LINK_MODE_MASK_NBITS);
5917 
5918 	phylink_helper_basex_speed(state);
5919 	return;
5920 
5921 empty_set:
5922 	bitmap_zero(supported, __ETHTOOL_LINK_MODE_MASK_NBITS);
5923 }
5924 
5925 static void mvpp2_xlg_config(struct mvpp2_port *port, unsigned int mode,
5926 			     const struct phylink_link_state *state)
5927 {
5928 	u32 val;
5929 
5930 	mvpp2_modify(port->base + MVPP22_XLG_CTRL0_REG,
5931 		     MVPP22_XLG_CTRL0_MAC_RESET_DIS,
5932 		     MVPP22_XLG_CTRL0_MAC_RESET_DIS);
5933 	mvpp2_modify(port->base + MVPP22_XLG_CTRL4_REG,
5934 		     MVPP22_XLG_CTRL4_MACMODSELECT_GMAC |
5935 		     MVPP22_XLG_CTRL4_EN_IDLE_CHECK |
5936 		     MVPP22_XLG_CTRL4_FWD_FC | MVPP22_XLG_CTRL4_FWD_PFC,
5937 		     MVPP22_XLG_CTRL4_FWD_FC | MVPP22_XLG_CTRL4_FWD_PFC);
5938 
5939 	/* Wait for reset to deassert */
5940 	do {
5941 		val = readl(port->base + MVPP22_XLG_CTRL0_REG);
5942 	} while (!(val & MVPP22_XLG_CTRL0_MAC_RESET_DIS));
5943 }
5944 
5945 static void mvpp2_gmac_config(struct mvpp2_port *port, unsigned int mode,
5946 			      const struct phylink_link_state *state)
5947 {
5948 	u32 old_ctrl0, ctrl0;
5949 	u32 old_ctrl2, ctrl2;
5950 	u32 old_ctrl4, ctrl4;
5951 
5952 	old_ctrl0 = ctrl0 = readl(port->base + MVPP2_GMAC_CTRL_0_REG);
5953 	old_ctrl2 = ctrl2 = readl(port->base + MVPP2_GMAC_CTRL_2_REG);
5954 	old_ctrl4 = ctrl4 = readl(port->base + MVPP22_GMAC_CTRL_4_REG);
5955 
5956 	ctrl0 &= ~MVPP2_GMAC_PORT_TYPE_MASK;
5957 	ctrl2 &= ~(MVPP2_GMAC_INBAND_AN_MASK | MVPP2_GMAC_PCS_ENABLE_MASK);
5958 
5959 	/* Configure port type */
5960 	if (phy_interface_mode_is_8023z(state->interface)) {
5961 		ctrl2 |= MVPP2_GMAC_PCS_ENABLE_MASK;
5962 		ctrl4 &= ~MVPP22_CTRL4_EXT_PIN_GMII_SEL;
5963 		ctrl4 |= MVPP22_CTRL4_SYNC_BYPASS_DIS |
5964 			 MVPP22_CTRL4_DP_CLK_SEL |
5965 			 MVPP22_CTRL4_QSGMII_BYPASS_ACTIVE;
5966 	} else if (state->interface == PHY_INTERFACE_MODE_SGMII) {
5967 		ctrl2 |= MVPP2_GMAC_PCS_ENABLE_MASK | MVPP2_GMAC_INBAND_AN_MASK;
5968 		ctrl4 &= ~MVPP22_CTRL4_EXT_PIN_GMII_SEL;
5969 		ctrl4 |= MVPP22_CTRL4_SYNC_BYPASS_DIS |
5970 			 MVPP22_CTRL4_DP_CLK_SEL |
5971 			 MVPP22_CTRL4_QSGMII_BYPASS_ACTIVE;
5972 	} else if (phy_interface_mode_is_rgmii(state->interface)) {
5973 		ctrl4 &= ~MVPP22_CTRL4_DP_CLK_SEL;
5974 		ctrl4 |= MVPP22_CTRL4_EXT_PIN_GMII_SEL |
5975 			 MVPP22_CTRL4_SYNC_BYPASS_DIS |
5976 			 MVPP22_CTRL4_QSGMII_BYPASS_ACTIVE;
5977 	}
5978 
5979 	/* Configure negotiation style */
5980 	if (!phylink_autoneg_inband(mode)) {
5981 		/* Phy or fixed speed - no in-band AN, nothing to do, leave the
5982 		 * configured speed, duplex and flow control as-is.
5983 		 */
5984 	} else if (state->interface == PHY_INTERFACE_MODE_SGMII) {
5985 		/* SGMII in-band mode receives the speed and duplex from
5986 		 * the PHY. Flow control information is not received. */
5987 	} else if (phy_interface_mode_is_8023z(state->interface)) {
5988 		/* 1000BaseX and 2500BaseX ports cannot negotiate speed nor can
5989 		 * they negotiate duplex: they are always operating with a fixed
5990 		 * speed of 1000/2500Mbps in full duplex, so force 1000/2500
5991 		 * speed and full duplex here.
5992 		 */
5993 		ctrl0 |= MVPP2_GMAC_PORT_TYPE_MASK;
5994 	}
5995 
5996 	if (old_ctrl0 != ctrl0)
5997 		writel(ctrl0, port->base + MVPP2_GMAC_CTRL_0_REG);
5998 	if (old_ctrl2 != ctrl2)
5999 		writel(ctrl2, port->base + MVPP2_GMAC_CTRL_2_REG);
6000 	if (old_ctrl4 != ctrl4)
6001 		writel(ctrl4, port->base + MVPP22_GMAC_CTRL_4_REG);
6002 }
6003 
6004 static int mvpp2__mac_prepare(struct phylink_config *config, unsigned int mode,
6005 			      phy_interface_t interface)
6006 {
6007 	struct mvpp2_port *port = mvpp2_phylink_to_port(config);
6008 
6009 	/* Check for invalid configuration */
6010 	if (mvpp2_is_xlg(interface) && port->gop_id != 0) {
6011 		netdev_err(port->dev, "Invalid mode on %s\n", port->dev->name);
6012 		return -EINVAL;
6013 	}
6014 
6015 	if (port->phy_interface != interface ||
6016 	    phylink_autoneg_inband(mode)) {
6017 		/* Force the link down when changing the interface or if in
6018 		 * in-band mode to ensure we do not change the configuration
6019 		 * while the hardware is indicating link is up. We force both
6020 		 * XLG and GMAC down to ensure that they're both in a known
6021 		 * state.
6022 		 */
6023 		mvpp2_modify(port->base + MVPP2_GMAC_AUTONEG_CONFIG,
6024 			     MVPP2_GMAC_FORCE_LINK_PASS |
6025 			     MVPP2_GMAC_FORCE_LINK_DOWN,
6026 			     MVPP2_GMAC_FORCE_LINK_DOWN);
6027 
6028 		if (mvpp2_port_supports_xlg(port))
6029 			mvpp2_modify(port->base + MVPP22_XLG_CTRL0_REG,
6030 				     MVPP22_XLG_CTRL0_FORCE_LINK_PASS |
6031 				     MVPP22_XLG_CTRL0_FORCE_LINK_DOWN,
6032 				     MVPP22_XLG_CTRL0_FORCE_LINK_DOWN);
6033 	}
6034 
6035 	/* Make sure the port is disabled when reconfiguring the mode */
6036 	mvpp2_port_disable(port);
6037 
6038 	if (port->phy_interface != interface) {
6039 		/* Place GMAC into reset */
6040 		mvpp2_modify(port->base + MVPP2_GMAC_CTRL_2_REG,
6041 			     MVPP2_GMAC_PORT_RESET_MASK,
6042 			     MVPP2_GMAC_PORT_RESET_MASK);
6043 
6044 		if (port->priv->hw_version == MVPP22) {
6045 			mvpp22_gop_mask_irq(port);
6046 
6047 			phy_power_off(port->comphy);
6048 		}
6049 	}
6050 
6051 	/* Select the appropriate PCS operations depending on the
6052 	 * configured interface mode. We will only switch to a mode
6053 	 * that the validate() checks have already passed.
6054 	 */
6055 	if (mvpp2_is_xlg(interface))
6056 		port->phylink_pcs.ops = &mvpp2_phylink_xlg_pcs_ops;
6057 	else
6058 		port->phylink_pcs.ops = &mvpp2_phylink_gmac_pcs_ops;
6059 
6060 	return 0;
6061 }
6062 
6063 static int mvpp2_mac_prepare(struct phylink_config *config, unsigned int mode,
6064 			     phy_interface_t interface)
6065 {
6066 	struct mvpp2_port *port = mvpp2_phylink_to_port(config);
6067 	int ret;
6068 
6069 	ret = mvpp2__mac_prepare(config, mode, interface);
6070 	if (ret == 0)
6071 		phylink_set_pcs(port->phylink, &port->phylink_pcs);
6072 
6073 	return ret;
6074 }
6075 
6076 static void mvpp2_mac_config(struct phylink_config *config, unsigned int mode,
6077 			     const struct phylink_link_state *state)
6078 {
6079 	struct mvpp2_port *port = mvpp2_phylink_to_port(config);
6080 
6081 	/* mac (re)configuration */
6082 	if (mvpp2_is_xlg(state->interface))
6083 		mvpp2_xlg_config(port, mode, state);
6084 	else if (phy_interface_mode_is_rgmii(state->interface) ||
6085 		 phy_interface_mode_is_8023z(state->interface) ||
6086 		 state->interface == PHY_INTERFACE_MODE_SGMII)
6087 		mvpp2_gmac_config(port, mode, state);
6088 
6089 	if (port->priv->hw_version == MVPP21 && port->flags & MVPP2_F_LOOPBACK)
6090 		mvpp2_port_loopback_set(port, state);
6091 }
6092 
6093 static int mvpp2_mac_finish(struct phylink_config *config, unsigned int mode,
6094 			    phy_interface_t interface)
6095 {
6096 	struct mvpp2_port *port = mvpp2_phylink_to_port(config);
6097 
6098 	if (port->priv->hw_version == MVPP22 &&
6099 	    port->phy_interface != interface) {
6100 		port->phy_interface = interface;
6101 
6102 		/* Reconfigure the serdes lanes */
6103 		mvpp22_mode_reconfigure(port);
6104 
6105 		/* Unmask interrupts */
6106 		mvpp22_gop_unmask_irq(port);
6107 	}
6108 
6109 	if (!mvpp2_is_xlg(interface)) {
6110 		/* Release GMAC reset and wait */
6111 		mvpp2_modify(port->base + MVPP2_GMAC_CTRL_2_REG,
6112 			     MVPP2_GMAC_PORT_RESET_MASK, 0);
6113 
6114 		while (readl(port->base + MVPP2_GMAC_CTRL_2_REG) &
6115 		       MVPP2_GMAC_PORT_RESET_MASK)
6116 			continue;
6117 	}
6118 
6119 	mvpp2_port_enable(port);
6120 
6121 	/* Allow the link to come up if in in-band mode, otherwise the
6122 	 * link is forced via mac_link_down()/mac_link_up()
6123 	 */
6124 	if (phylink_autoneg_inband(mode)) {
6125 		if (mvpp2_is_xlg(interface))
6126 			mvpp2_modify(port->base + MVPP22_XLG_CTRL0_REG,
6127 				     MVPP22_XLG_CTRL0_FORCE_LINK_PASS |
6128 				     MVPP22_XLG_CTRL0_FORCE_LINK_DOWN, 0);
6129 		else
6130 			mvpp2_modify(port->base + MVPP2_GMAC_AUTONEG_CONFIG,
6131 				     MVPP2_GMAC_FORCE_LINK_PASS |
6132 				     MVPP2_GMAC_FORCE_LINK_DOWN, 0);
6133 	}
6134 
6135 	return 0;
6136 }
6137 
6138 static void mvpp2_mac_link_up(struct phylink_config *config,
6139 			      struct phy_device *phy,
6140 			      unsigned int mode, phy_interface_t interface,
6141 			      int speed, int duplex,
6142 			      bool tx_pause, bool rx_pause)
6143 {
6144 	struct mvpp2_port *port = mvpp2_phylink_to_port(config);
6145 	u32 val;
6146 
6147 	if (mvpp2_is_xlg(interface)) {
6148 		if (!phylink_autoneg_inband(mode)) {
6149 			val = MVPP22_XLG_CTRL0_FORCE_LINK_PASS;
6150 			if (tx_pause)
6151 				val |= MVPP22_XLG_CTRL0_TX_FLOW_CTRL_EN;
6152 			if (rx_pause)
6153 				val |= MVPP22_XLG_CTRL0_RX_FLOW_CTRL_EN;
6154 
6155 			mvpp2_modify(port->base + MVPP22_XLG_CTRL0_REG,
6156 				     MVPP22_XLG_CTRL0_FORCE_LINK_DOWN |
6157 				     MVPP22_XLG_CTRL0_FORCE_LINK_PASS |
6158 				     MVPP22_XLG_CTRL0_TX_FLOW_CTRL_EN |
6159 				     MVPP22_XLG_CTRL0_RX_FLOW_CTRL_EN, val);
6160 		}
6161 	} else {
6162 		if (!phylink_autoneg_inband(mode)) {
6163 			val = MVPP2_GMAC_FORCE_LINK_PASS;
6164 
6165 			if (speed == SPEED_1000 || speed == SPEED_2500)
6166 				val |= MVPP2_GMAC_CONFIG_GMII_SPEED;
6167 			else if (speed == SPEED_100)
6168 				val |= MVPP2_GMAC_CONFIG_MII_SPEED;
6169 
6170 			if (duplex == DUPLEX_FULL)
6171 				val |= MVPP2_GMAC_CONFIG_FULL_DUPLEX;
6172 
6173 			mvpp2_modify(port->base + MVPP2_GMAC_AUTONEG_CONFIG,
6174 				     MVPP2_GMAC_FORCE_LINK_DOWN |
6175 				     MVPP2_GMAC_FORCE_LINK_PASS |
6176 				     MVPP2_GMAC_CONFIG_MII_SPEED |
6177 				     MVPP2_GMAC_CONFIG_GMII_SPEED |
6178 				     MVPP2_GMAC_CONFIG_FULL_DUPLEX, val);
6179 		}
6180 
6181 		/* We can always update the flow control enable bits;
6182 		 * these will only be effective if flow control AN
6183 		 * (MVPP2_GMAC_FLOW_CTRL_AUTONEG) is disabled.
6184 		 */
6185 		val = 0;
6186 		if (tx_pause)
6187 			val |= MVPP22_CTRL4_TX_FC_EN;
6188 		if (rx_pause)
6189 			val |= MVPP22_CTRL4_RX_FC_EN;
6190 
6191 		mvpp2_modify(port->base + MVPP22_GMAC_CTRL_4_REG,
6192 			     MVPP22_CTRL4_RX_FC_EN | MVPP22_CTRL4_TX_FC_EN,
6193 			     val);
6194 	}
6195 
6196 	mvpp2_port_enable(port);
6197 
6198 	mvpp2_egress_enable(port);
6199 	mvpp2_ingress_enable(port);
6200 	netif_tx_wake_all_queues(port->dev);
6201 }
6202 
6203 static void mvpp2_mac_link_down(struct phylink_config *config,
6204 				unsigned int mode, phy_interface_t interface)
6205 {
6206 	struct mvpp2_port *port = mvpp2_phylink_to_port(config);
6207 	u32 val;
6208 
6209 	if (!phylink_autoneg_inband(mode)) {
6210 		if (mvpp2_is_xlg(interface)) {
6211 			val = readl(port->base + MVPP22_XLG_CTRL0_REG);
6212 			val &= ~MVPP22_XLG_CTRL0_FORCE_LINK_PASS;
6213 			val |= MVPP22_XLG_CTRL0_FORCE_LINK_DOWN;
6214 			writel(val, port->base + MVPP22_XLG_CTRL0_REG);
6215 		} else {
6216 			val = readl(port->base + MVPP2_GMAC_AUTONEG_CONFIG);
6217 			val &= ~MVPP2_GMAC_FORCE_LINK_PASS;
6218 			val |= MVPP2_GMAC_FORCE_LINK_DOWN;
6219 			writel(val, port->base + MVPP2_GMAC_AUTONEG_CONFIG);
6220 		}
6221 	}
6222 
6223 	netif_tx_stop_all_queues(port->dev);
6224 	mvpp2_egress_disable(port);
6225 	mvpp2_ingress_disable(port);
6226 
6227 	mvpp2_port_disable(port);
6228 }
6229 
6230 static const struct phylink_mac_ops mvpp2_phylink_ops = {
6231 	.validate = mvpp2_phylink_validate,
6232 	.mac_prepare = mvpp2_mac_prepare,
6233 	.mac_config = mvpp2_mac_config,
6234 	.mac_finish = mvpp2_mac_finish,
6235 	.mac_link_up = mvpp2_mac_link_up,
6236 	.mac_link_down = mvpp2_mac_link_down,
6237 };
6238 
6239 /* Work-around for ACPI */
6240 static void mvpp2_acpi_start(struct mvpp2_port *port)
6241 {
6242 	/* Phylink isn't used as of now for ACPI, so the MAC has to be
6243 	 * configured manually when the interface is started. This will
6244 	 * be removed as soon as the phylink ACPI support lands in.
6245 	 */
6246 	struct phylink_link_state state = {
6247 		.interface = port->phy_interface,
6248 	};
6249 	mvpp2__mac_prepare(&port->phylink_config, MLO_AN_INBAND,
6250 			   port->phy_interface);
6251 	mvpp2_mac_config(&port->phylink_config, MLO_AN_INBAND, &state);
6252 	port->phylink_pcs.ops->pcs_config(&port->phylink_pcs, MLO_AN_INBAND,
6253 					  port->phy_interface,
6254 					  state.advertising, false);
6255 	mvpp2_mac_finish(&port->phylink_config, MLO_AN_INBAND,
6256 			 port->phy_interface);
6257 	mvpp2_mac_link_up(&port->phylink_config, NULL,
6258 			  MLO_AN_INBAND, port->phy_interface,
6259 			  SPEED_UNKNOWN, DUPLEX_UNKNOWN, false, false);
6260 }
6261 
6262 /* Ports initialization */
6263 static int mvpp2_port_probe(struct platform_device *pdev,
6264 			    struct fwnode_handle *port_fwnode,
6265 			    struct mvpp2 *priv)
6266 {
6267 	struct phy *comphy = NULL;
6268 	struct mvpp2_port *port;
6269 	struct mvpp2_port_pcpu *port_pcpu;
6270 	struct device_node *port_node = to_of_node(port_fwnode);
6271 	netdev_features_t features;
6272 	struct net_device *dev;
6273 	struct phylink *phylink;
6274 	char *mac_from = "";
6275 	unsigned int ntxqs, nrxqs, thread;
6276 	unsigned long flags = 0;
6277 	bool has_tx_irqs;
6278 	u32 id;
6279 	int phy_mode;
6280 	int err, i;
6281 
6282 	has_tx_irqs = mvpp2_port_has_irqs(priv, port_node, &flags);
6283 	if (!has_tx_irqs && queue_mode == MVPP2_QDIST_MULTI_MODE) {
6284 		dev_err(&pdev->dev,
6285 			"not enough IRQs to support multi queue mode\n");
6286 		return -EINVAL;
6287 	}
6288 
6289 	ntxqs = MVPP2_MAX_TXQ;
6290 	nrxqs = mvpp2_get_nrxqs(priv);
6291 
6292 	dev = alloc_etherdev_mqs(sizeof(*port), ntxqs, nrxqs);
6293 	if (!dev)
6294 		return -ENOMEM;
6295 
6296 	phy_mode = fwnode_get_phy_mode(port_fwnode);
6297 	if (phy_mode < 0) {
6298 		dev_err(&pdev->dev, "incorrect phy mode\n");
6299 		err = phy_mode;
6300 		goto err_free_netdev;
6301 	}
6302 
6303 	/*
6304 	 * Rewrite 10GBASE-KR to 10GBASE-R for compatibility with existing DT.
6305 	 * Existing usage of 10GBASE-KR is not correct; no backplane
6306 	 * negotiation is done, and this driver does not actually support
6307 	 * 10GBASE-KR.
6308 	 */
6309 	if (phy_mode == PHY_INTERFACE_MODE_10GKR)
6310 		phy_mode = PHY_INTERFACE_MODE_10GBASER;
6311 
6312 	if (port_node) {
6313 		comphy = devm_of_phy_get(&pdev->dev, port_node, NULL);
6314 		if (IS_ERR(comphy)) {
6315 			if (PTR_ERR(comphy) == -EPROBE_DEFER) {
6316 				err = -EPROBE_DEFER;
6317 				goto err_free_netdev;
6318 			}
6319 			comphy = NULL;
6320 		}
6321 	}
6322 
6323 	if (fwnode_property_read_u32(port_fwnode, "port-id", &id)) {
6324 		err = -EINVAL;
6325 		dev_err(&pdev->dev, "missing port-id value\n");
6326 		goto err_free_netdev;
6327 	}
6328 
6329 	dev->tx_queue_len = MVPP2_MAX_TXD_MAX;
6330 	dev->watchdog_timeo = 5 * HZ;
6331 	dev->netdev_ops = &mvpp2_netdev_ops;
6332 	dev->ethtool_ops = &mvpp2_eth_tool_ops;
6333 
6334 	port = netdev_priv(dev);
6335 	port->dev = dev;
6336 	port->fwnode = port_fwnode;
6337 	port->has_phy = !!of_find_property(port_node, "phy", NULL);
6338 	port->ntxqs = ntxqs;
6339 	port->nrxqs = nrxqs;
6340 	port->priv = priv;
6341 	port->has_tx_irqs = has_tx_irqs;
6342 	port->flags = flags;
6343 
6344 	err = mvpp2_queue_vectors_init(port, port_node);
6345 	if (err)
6346 		goto err_free_netdev;
6347 
6348 	if (port_node)
6349 		port->port_irq = of_irq_get_byname(port_node, "link");
6350 	else
6351 		port->port_irq = fwnode_irq_get(port_fwnode, port->nqvecs + 1);
6352 	if (port->port_irq == -EPROBE_DEFER) {
6353 		err = -EPROBE_DEFER;
6354 		goto err_deinit_qvecs;
6355 	}
6356 	if (port->port_irq <= 0)
6357 		/* the link irq is optional */
6358 		port->port_irq = 0;
6359 
6360 	if (fwnode_property_read_bool(port_fwnode, "marvell,loopback"))
6361 		port->flags |= MVPP2_F_LOOPBACK;
6362 
6363 	port->id = id;
6364 	if (priv->hw_version == MVPP21)
6365 		port->first_rxq = port->id * port->nrxqs;
6366 	else
6367 		port->first_rxq = port->id * priv->max_port_rxqs;
6368 
6369 	port->of_node = port_node;
6370 	port->phy_interface = phy_mode;
6371 	port->comphy = comphy;
6372 
6373 	if (priv->hw_version == MVPP21) {
6374 		port->base = devm_platform_ioremap_resource(pdev, 2 + id);
6375 		if (IS_ERR(port->base)) {
6376 			err = PTR_ERR(port->base);
6377 			goto err_free_irq;
6378 		}
6379 
6380 		port->stats_base = port->priv->lms_base +
6381 				   MVPP21_MIB_COUNTERS_OFFSET +
6382 				   port->gop_id * MVPP21_MIB_COUNTERS_PORT_SZ;
6383 	} else {
6384 		if (fwnode_property_read_u32(port_fwnode, "gop-port-id",
6385 					     &port->gop_id)) {
6386 			err = -EINVAL;
6387 			dev_err(&pdev->dev, "missing gop-port-id value\n");
6388 			goto err_deinit_qvecs;
6389 		}
6390 
6391 		port->base = priv->iface_base + MVPP22_GMAC_BASE(port->gop_id);
6392 		port->stats_base = port->priv->iface_base +
6393 				   MVPP22_MIB_COUNTERS_OFFSET +
6394 				   port->gop_id * MVPP22_MIB_COUNTERS_PORT_SZ;
6395 
6396 		/* We may want a property to describe whether we should use
6397 		 * MAC hardware timestamping.
6398 		 */
6399 		if (priv->tai)
6400 			port->hwtstamp = true;
6401 	}
6402 
6403 	/* Alloc per-cpu and ethtool stats */
6404 	port->stats = netdev_alloc_pcpu_stats(struct mvpp2_pcpu_stats);
6405 	if (!port->stats) {
6406 		err = -ENOMEM;
6407 		goto err_free_irq;
6408 	}
6409 
6410 	port->ethtool_stats = devm_kcalloc(&pdev->dev,
6411 					   MVPP2_N_ETHTOOL_STATS(ntxqs, nrxqs),
6412 					   sizeof(u64), GFP_KERNEL);
6413 	if (!port->ethtool_stats) {
6414 		err = -ENOMEM;
6415 		goto err_free_stats;
6416 	}
6417 
6418 	mutex_init(&port->gather_stats_lock);
6419 	INIT_DELAYED_WORK(&port->stats_work, mvpp2_gather_hw_statistics);
6420 
6421 	mvpp2_port_copy_mac_addr(dev, priv, port_fwnode, &mac_from);
6422 
6423 	port->tx_ring_size = MVPP2_MAX_TXD_DFLT;
6424 	port->rx_ring_size = MVPP2_MAX_RXD_DFLT;
6425 	SET_NETDEV_DEV(dev, &pdev->dev);
6426 
6427 	err = mvpp2_port_init(port);
6428 	if (err < 0) {
6429 		dev_err(&pdev->dev, "failed to init port %d\n", id);
6430 		goto err_free_stats;
6431 	}
6432 
6433 	mvpp2_port_periodic_xon_disable(port);
6434 
6435 	mvpp2_mac_reset_assert(port);
6436 	mvpp22_pcs_reset_assert(port);
6437 
6438 	port->pcpu = alloc_percpu(struct mvpp2_port_pcpu);
6439 	if (!port->pcpu) {
6440 		err = -ENOMEM;
6441 		goto err_free_txq_pcpu;
6442 	}
6443 
6444 	if (!port->has_tx_irqs) {
6445 		for (thread = 0; thread < priv->nthreads; thread++) {
6446 			port_pcpu = per_cpu_ptr(port->pcpu, thread);
6447 
6448 			hrtimer_init(&port_pcpu->tx_done_timer, CLOCK_MONOTONIC,
6449 				     HRTIMER_MODE_REL_PINNED_SOFT);
6450 			port_pcpu->tx_done_timer.function = mvpp2_hr_timer_cb;
6451 			port_pcpu->timer_scheduled = false;
6452 			port_pcpu->dev = dev;
6453 		}
6454 	}
6455 
6456 	features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_IPV6_CSUM |
6457 		   NETIF_F_TSO;
6458 	dev->features = features | NETIF_F_RXCSUM;
6459 	dev->hw_features |= features | NETIF_F_RXCSUM | NETIF_F_GRO |
6460 			    NETIF_F_HW_VLAN_CTAG_FILTER;
6461 
6462 	if (mvpp22_rss_is_supported()) {
6463 		dev->hw_features |= NETIF_F_RXHASH;
6464 		dev->features |= NETIF_F_NTUPLE;
6465 	}
6466 
6467 	if (!port->priv->percpu_pools)
6468 		mvpp2_set_hw_csum(port, port->pool_long->id);
6469 
6470 	dev->vlan_features |= features;
6471 	dev->gso_max_segs = MVPP2_MAX_TSO_SEGS;
6472 	dev->priv_flags |= IFF_UNICAST_FLT;
6473 
6474 	/* MTU range: 68 - 9704 */
6475 	dev->min_mtu = ETH_MIN_MTU;
6476 	/* 9704 == 9728 - 20 and rounding to 8 */
6477 	dev->max_mtu = MVPP2_BM_JUMBO_PKT_SIZE;
6478 	dev->dev.of_node = port_node;
6479 
6480 	/* Phylink isn't used w/ ACPI as of now */
6481 	if (port_node) {
6482 		port->phylink_config.dev = &dev->dev;
6483 		port->phylink_config.type = PHYLINK_NETDEV;
6484 
6485 		phylink = phylink_create(&port->phylink_config, port_fwnode,
6486 					 phy_mode, &mvpp2_phylink_ops);
6487 		if (IS_ERR(phylink)) {
6488 			err = PTR_ERR(phylink);
6489 			goto err_free_port_pcpu;
6490 		}
6491 		port->phylink = phylink;
6492 	} else {
6493 		port->phylink = NULL;
6494 	}
6495 
6496 	/* Cycle the comphy to power it down, saving 270mW per port -
6497 	 * don't worry about an error powering it up. When the comphy
6498 	 * driver does this, we can remove this code.
6499 	 */
6500 	if (port->comphy) {
6501 		err = mvpp22_comphy_init(port);
6502 		if (err == 0)
6503 			phy_power_off(port->comphy);
6504 	}
6505 
6506 	err = register_netdev(dev);
6507 	if (err < 0) {
6508 		dev_err(&pdev->dev, "failed to register netdev\n");
6509 		goto err_phylink;
6510 	}
6511 	netdev_info(dev, "Using %s mac address %pM\n", mac_from, dev->dev_addr);
6512 
6513 	priv->port_list[priv->port_count++] = port;
6514 
6515 	return 0;
6516 
6517 err_phylink:
6518 	if (port->phylink)
6519 		phylink_destroy(port->phylink);
6520 err_free_port_pcpu:
6521 	free_percpu(port->pcpu);
6522 err_free_txq_pcpu:
6523 	for (i = 0; i < port->ntxqs; i++)
6524 		free_percpu(port->txqs[i]->pcpu);
6525 err_free_stats:
6526 	free_percpu(port->stats);
6527 err_free_irq:
6528 	if (port->port_irq)
6529 		irq_dispose_mapping(port->port_irq);
6530 err_deinit_qvecs:
6531 	mvpp2_queue_vectors_deinit(port);
6532 err_free_netdev:
6533 	free_netdev(dev);
6534 	return err;
6535 }
6536 
6537 /* Ports removal routine */
6538 static void mvpp2_port_remove(struct mvpp2_port *port)
6539 {
6540 	int i;
6541 
6542 	unregister_netdev(port->dev);
6543 	if (port->phylink)
6544 		phylink_destroy(port->phylink);
6545 	free_percpu(port->pcpu);
6546 	free_percpu(port->stats);
6547 	for (i = 0; i < port->ntxqs; i++)
6548 		free_percpu(port->txqs[i]->pcpu);
6549 	mvpp2_queue_vectors_deinit(port);
6550 	if (port->port_irq)
6551 		irq_dispose_mapping(port->port_irq);
6552 	free_netdev(port->dev);
6553 }
6554 
6555 /* Initialize decoding windows */
6556 static void mvpp2_conf_mbus_windows(const struct mbus_dram_target_info *dram,
6557 				    struct mvpp2 *priv)
6558 {
6559 	u32 win_enable;
6560 	int i;
6561 
6562 	for (i = 0; i < 6; i++) {
6563 		mvpp2_write(priv, MVPP2_WIN_BASE(i), 0);
6564 		mvpp2_write(priv, MVPP2_WIN_SIZE(i), 0);
6565 
6566 		if (i < 4)
6567 			mvpp2_write(priv, MVPP2_WIN_REMAP(i), 0);
6568 	}
6569 
6570 	win_enable = 0;
6571 
6572 	for (i = 0; i < dram->num_cs; i++) {
6573 		const struct mbus_dram_window *cs = dram->cs + i;
6574 
6575 		mvpp2_write(priv, MVPP2_WIN_BASE(i),
6576 			    (cs->base & 0xffff0000) | (cs->mbus_attr << 8) |
6577 			    dram->mbus_dram_target_id);
6578 
6579 		mvpp2_write(priv, MVPP2_WIN_SIZE(i),
6580 			    (cs->size - 1) & 0xffff0000);
6581 
6582 		win_enable |= (1 << i);
6583 	}
6584 
6585 	mvpp2_write(priv, MVPP2_BASE_ADDR_ENABLE, win_enable);
6586 }
6587 
6588 /* Initialize Rx FIFO's */
6589 static void mvpp2_rx_fifo_init(struct mvpp2 *priv)
6590 {
6591 	int port;
6592 
6593 	for (port = 0; port < MVPP2_MAX_PORTS; port++) {
6594 		mvpp2_write(priv, MVPP2_RX_DATA_FIFO_SIZE_REG(port),
6595 			    MVPP2_RX_FIFO_PORT_DATA_SIZE_4KB);
6596 		mvpp2_write(priv, MVPP2_RX_ATTR_FIFO_SIZE_REG(port),
6597 			    MVPP2_RX_FIFO_PORT_ATTR_SIZE_4KB);
6598 	}
6599 
6600 	mvpp2_write(priv, MVPP2_RX_MIN_PKT_SIZE_REG,
6601 		    MVPP2_RX_FIFO_PORT_MIN_PKT);
6602 	mvpp2_write(priv, MVPP2_RX_FIFO_INIT_REG, 0x1);
6603 }
6604 
6605 static void mvpp22_rx_fifo_init(struct mvpp2 *priv)
6606 {
6607 	int port;
6608 
6609 	/* The FIFO size parameters are set depending on the maximum speed a
6610 	 * given port can handle:
6611 	 * - Port 0: 10Gbps
6612 	 * - Port 1: 2.5Gbps
6613 	 * - Ports 2 and 3: 1Gbps
6614 	 */
6615 
6616 	mvpp2_write(priv, MVPP2_RX_DATA_FIFO_SIZE_REG(0),
6617 		    MVPP2_RX_FIFO_PORT_DATA_SIZE_32KB);
6618 	mvpp2_write(priv, MVPP2_RX_ATTR_FIFO_SIZE_REG(0),
6619 		    MVPP2_RX_FIFO_PORT_ATTR_SIZE_32KB);
6620 
6621 	mvpp2_write(priv, MVPP2_RX_DATA_FIFO_SIZE_REG(1),
6622 		    MVPP2_RX_FIFO_PORT_DATA_SIZE_8KB);
6623 	mvpp2_write(priv, MVPP2_RX_ATTR_FIFO_SIZE_REG(1),
6624 		    MVPP2_RX_FIFO_PORT_ATTR_SIZE_8KB);
6625 
6626 	for (port = 2; port < MVPP2_MAX_PORTS; port++) {
6627 		mvpp2_write(priv, MVPP2_RX_DATA_FIFO_SIZE_REG(port),
6628 			    MVPP2_RX_FIFO_PORT_DATA_SIZE_4KB);
6629 		mvpp2_write(priv, MVPP2_RX_ATTR_FIFO_SIZE_REG(port),
6630 			    MVPP2_RX_FIFO_PORT_ATTR_SIZE_4KB);
6631 	}
6632 
6633 	mvpp2_write(priv, MVPP2_RX_MIN_PKT_SIZE_REG,
6634 		    MVPP2_RX_FIFO_PORT_MIN_PKT);
6635 	mvpp2_write(priv, MVPP2_RX_FIFO_INIT_REG, 0x1);
6636 }
6637 
6638 /* Initialize Tx FIFO's: the total FIFO size is 19kB on PPv2.2 and 10G
6639  * interfaces must have a Tx FIFO size of 10kB. As only port 0 can do 10G,
6640  * configure its Tx FIFO size to 10kB and the others ports Tx FIFO size to 3kB.
6641  */
6642 static void mvpp22_tx_fifo_init(struct mvpp2 *priv)
6643 {
6644 	int port, size, thrs;
6645 
6646 	for (port = 0; port < MVPP2_MAX_PORTS; port++) {
6647 		if (port == 0) {
6648 			size = MVPP22_TX_FIFO_DATA_SIZE_10KB;
6649 			thrs = MVPP2_TX_FIFO_THRESHOLD_10KB;
6650 		} else {
6651 			size = MVPP22_TX_FIFO_DATA_SIZE_3KB;
6652 			thrs = MVPP2_TX_FIFO_THRESHOLD_3KB;
6653 		}
6654 		mvpp2_write(priv, MVPP22_TX_FIFO_SIZE_REG(port), size);
6655 		mvpp2_write(priv, MVPP22_TX_FIFO_THRESH_REG(port), thrs);
6656 	}
6657 }
6658 
6659 static void mvpp2_axi_init(struct mvpp2 *priv)
6660 {
6661 	u32 val, rdval, wrval;
6662 
6663 	mvpp2_write(priv, MVPP22_BM_ADDR_HIGH_RLS_REG, 0x0);
6664 
6665 	/* AXI Bridge Configuration */
6666 
6667 	rdval = MVPP22_AXI_CODE_CACHE_RD_CACHE
6668 		<< MVPP22_AXI_ATTR_CACHE_OFFS;
6669 	rdval |= MVPP22_AXI_CODE_DOMAIN_OUTER_DOM
6670 		<< MVPP22_AXI_ATTR_DOMAIN_OFFS;
6671 
6672 	wrval = MVPP22_AXI_CODE_CACHE_WR_CACHE
6673 		<< MVPP22_AXI_ATTR_CACHE_OFFS;
6674 	wrval |= MVPP22_AXI_CODE_DOMAIN_OUTER_DOM
6675 		<< MVPP22_AXI_ATTR_DOMAIN_OFFS;
6676 
6677 	/* BM */
6678 	mvpp2_write(priv, MVPP22_AXI_BM_WR_ATTR_REG, wrval);
6679 	mvpp2_write(priv, MVPP22_AXI_BM_RD_ATTR_REG, rdval);
6680 
6681 	/* Descriptors */
6682 	mvpp2_write(priv, MVPP22_AXI_AGGRQ_DESCR_RD_ATTR_REG, rdval);
6683 	mvpp2_write(priv, MVPP22_AXI_TXQ_DESCR_WR_ATTR_REG, wrval);
6684 	mvpp2_write(priv, MVPP22_AXI_TXQ_DESCR_RD_ATTR_REG, rdval);
6685 	mvpp2_write(priv, MVPP22_AXI_RXQ_DESCR_WR_ATTR_REG, wrval);
6686 
6687 	/* Buffer Data */
6688 	mvpp2_write(priv, MVPP22_AXI_TX_DATA_RD_ATTR_REG, rdval);
6689 	mvpp2_write(priv, MVPP22_AXI_RX_DATA_WR_ATTR_REG, wrval);
6690 
6691 	val = MVPP22_AXI_CODE_CACHE_NON_CACHE
6692 		<< MVPP22_AXI_CODE_CACHE_OFFS;
6693 	val |= MVPP22_AXI_CODE_DOMAIN_SYSTEM
6694 		<< MVPP22_AXI_CODE_DOMAIN_OFFS;
6695 	mvpp2_write(priv, MVPP22_AXI_RD_NORMAL_CODE_REG, val);
6696 	mvpp2_write(priv, MVPP22_AXI_WR_NORMAL_CODE_REG, val);
6697 
6698 	val = MVPP22_AXI_CODE_CACHE_RD_CACHE
6699 		<< MVPP22_AXI_CODE_CACHE_OFFS;
6700 	val |= MVPP22_AXI_CODE_DOMAIN_OUTER_DOM
6701 		<< MVPP22_AXI_CODE_DOMAIN_OFFS;
6702 
6703 	mvpp2_write(priv, MVPP22_AXI_RD_SNOOP_CODE_REG, val);
6704 
6705 	val = MVPP22_AXI_CODE_CACHE_WR_CACHE
6706 		<< MVPP22_AXI_CODE_CACHE_OFFS;
6707 	val |= MVPP22_AXI_CODE_DOMAIN_OUTER_DOM
6708 		<< MVPP22_AXI_CODE_DOMAIN_OFFS;
6709 
6710 	mvpp2_write(priv, MVPP22_AXI_WR_SNOOP_CODE_REG, val);
6711 }
6712 
6713 /* Initialize network controller common part HW */
6714 static int mvpp2_init(struct platform_device *pdev, struct mvpp2 *priv)
6715 {
6716 	const struct mbus_dram_target_info *dram_target_info;
6717 	int err, i;
6718 	u32 val;
6719 
6720 	/* MBUS windows configuration */
6721 	dram_target_info = mv_mbus_dram_info();
6722 	if (dram_target_info)
6723 		mvpp2_conf_mbus_windows(dram_target_info, priv);
6724 
6725 	if (priv->hw_version == MVPP22)
6726 		mvpp2_axi_init(priv);
6727 
6728 	/* Disable HW PHY polling */
6729 	if (priv->hw_version == MVPP21) {
6730 		val = readl(priv->lms_base + MVPP2_PHY_AN_CFG0_REG);
6731 		val |= MVPP2_PHY_AN_STOP_SMI0_MASK;
6732 		writel(val, priv->lms_base + MVPP2_PHY_AN_CFG0_REG);
6733 	} else {
6734 		val = readl(priv->iface_base + MVPP22_SMI_MISC_CFG_REG);
6735 		val &= ~MVPP22_SMI_POLLING_EN;
6736 		writel(val, priv->iface_base + MVPP22_SMI_MISC_CFG_REG);
6737 	}
6738 
6739 	/* Allocate and initialize aggregated TXQs */
6740 	priv->aggr_txqs = devm_kcalloc(&pdev->dev, MVPP2_MAX_THREADS,
6741 				       sizeof(*priv->aggr_txqs),
6742 				       GFP_KERNEL);
6743 	if (!priv->aggr_txqs)
6744 		return -ENOMEM;
6745 
6746 	for (i = 0; i < MVPP2_MAX_THREADS; i++) {
6747 		priv->aggr_txqs[i].id = i;
6748 		priv->aggr_txqs[i].size = MVPP2_AGGR_TXQ_SIZE;
6749 		err = mvpp2_aggr_txq_init(pdev, &priv->aggr_txqs[i], i, priv);
6750 		if (err < 0)
6751 			return err;
6752 	}
6753 
6754 	/* Fifo Init */
6755 	if (priv->hw_version == MVPP21) {
6756 		mvpp2_rx_fifo_init(priv);
6757 	} else {
6758 		mvpp22_rx_fifo_init(priv);
6759 		mvpp22_tx_fifo_init(priv);
6760 	}
6761 
6762 	if (priv->hw_version == MVPP21)
6763 		writel(MVPP2_EXT_GLOBAL_CTRL_DEFAULT,
6764 		       priv->lms_base + MVPP2_MNG_EXTENDED_GLOBAL_CTRL_REG);
6765 
6766 	/* Allow cache snoop when transmiting packets */
6767 	mvpp2_write(priv, MVPP2_TX_SNOOP_REG, 0x1);
6768 
6769 	/* Buffer Manager initialization */
6770 	err = mvpp2_bm_init(&pdev->dev, priv);
6771 	if (err < 0)
6772 		return err;
6773 
6774 	/* Parser default initialization */
6775 	err = mvpp2_prs_default_init(pdev, priv);
6776 	if (err < 0)
6777 		return err;
6778 
6779 	/* Classifier default initialization */
6780 	mvpp2_cls_init(priv);
6781 
6782 	return 0;
6783 }
6784 
6785 static int mvpp2_probe(struct platform_device *pdev)
6786 {
6787 	const struct acpi_device_id *acpi_id;
6788 	struct fwnode_handle *fwnode = pdev->dev.fwnode;
6789 	struct fwnode_handle *port_fwnode;
6790 	struct mvpp2 *priv;
6791 	struct resource *res;
6792 	void __iomem *base;
6793 	int i, shared;
6794 	int err;
6795 
6796 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
6797 	if (!priv)
6798 		return -ENOMEM;
6799 
6800 	if (has_acpi_companion(&pdev->dev)) {
6801 		acpi_id = acpi_match_device(pdev->dev.driver->acpi_match_table,
6802 					    &pdev->dev);
6803 		if (!acpi_id)
6804 			return -EINVAL;
6805 		priv->hw_version = (unsigned long)acpi_id->driver_data;
6806 	} else {
6807 		priv->hw_version =
6808 			(unsigned long)of_device_get_match_data(&pdev->dev);
6809 	}
6810 
6811 	/* multi queue mode isn't supported on PPV2.1, fallback to single
6812 	 * mode
6813 	 */
6814 	if (priv->hw_version == MVPP21)
6815 		queue_mode = MVPP2_QDIST_SINGLE_MODE;
6816 
6817 	base = devm_platform_ioremap_resource(pdev, 0);
6818 	if (IS_ERR(base))
6819 		return PTR_ERR(base);
6820 
6821 	if (priv->hw_version == MVPP21) {
6822 		priv->lms_base = devm_platform_ioremap_resource(pdev, 1);
6823 		if (IS_ERR(priv->lms_base))
6824 			return PTR_ERR(priv->lms_base);
6825 	} else {
6826 		res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
6827 		if (has_acpi_companion(&pdev->dev)) {
6828 			/* In case the MDIO memory region is declared in
6829 			 * the ACPI, it can already appear as 'in-use'
6830 			 * in the OS. Because it is overlapped by second
6831 			 * region of the network controller, make
6832 			 * sure it is released, before requesting it again.
6833 			 * The care is taken by mvpp2 driver to avoid
6834 			 * concurrent access to this memory region.
6835 			 */
6836 			release_resource(res);
6837 		}
6838 		priv->iface_base = devm_ioremap_resource(&pdev->dev, res);
6839 		if (IS_ERR(priv->iface_base))
6840 			return PTR_ERR(priv->iface_base);
6841 	}
6842 
6843 	if (priv->hw_version == MVPP22 && dev_of_node(&pdev->dev)) {
6844 		priv->sysctrl_base =
6845 			syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
6846 							"marvell,system-controller");
6847 		if (IS_ERR(priv->sysctrl_base))
6848 			/* The system controller regmap is optional for dt
6849 			 * compatibility reasons. When not provided, the
6850 			 * configuration of the GoP relies on the
6851 			 * firmware/bootloader.
6852 			 */
6853 			priv->sysctrl_base = NULL;
6854 	}
6855 
6856 	if (priv->hw_version == MVPP22 &&
6857 	    mvpp2_get_nrxqs(priv) * 2 <= MVPP2_BM_MAX_POOLS)
6858 		priv->percpu_pools = 1;
6859 
6860 	mvpp2_setup_bm_pool();
6861 
6862 
6863 	priv->nthreads = min_t(unsigned int, num_present_cpus(),
6864 			       MVPP2_MAX_THREADS);
6865 
6866 	shared = num_present_cpus() - priv->nthreads;
6867 	if (shared > 0)
6868 		bitmap_fill(&priv->lock_map,
6869 			    min_t(int, shared, MVPP2_MAX_THREADS));
6870 
6871 	for (i = 0; i < MVPP2_MAX_THREADS; i++) {
6872 		u32 addr_space_sz;
6873 
6874 		addr_space_sz = (priv->hw_version == MVPP21 ?
6875 				 MVPP21_ADDR_SPACE_SZ : MVPP22_ADDR_SPACE_SZ);
6876 		priv->swth_base[i] = base + i * addr_space_sz;
6877 	}
6878 
6879 	if (priv->hw_version == MVPP21)
6880 		priv->max_port_rxqs = 8;
6881 	else
6882 		priv->max_port_rxqs = 32;
6883 
6884 	if (dev_of_node(&pdev->dev)) {
6885 		priv->pp_clk = devm_clk_get(&pdev->dev, "pp_clk");
6886 		if (IS_ERR(priv->pp_clk))
6887 			return PTR_ERR(priv->pp_clk);
6888 		err = clk_prepare_enable(priv->pp_clk);
6889 		if (err < 0)
6890 			return err;
6891 
6892 		priv->gop_clk = devm_clk_get(&pdev->dev, "gop_clk");
6893 		if (IS_ERR(priv->gop_clk)) {
6894 			err = PTR_ERR(priv->gop_clk);
6895 			goto err_pp_clk;
6896 		}
6897 		err = clk_prepare_enable(priv->gop_clk);
6898 		if (err < 0)
6899 			goto err_pp_clk;
6900 
6901 		if (priv->hw_version == MVPP22) {
6902 			priv->mg_clk = devm_clk_get(&pdev->dev, "mg_clk");
6903 			if (IS_ERR(priv->mg_clk)) {
6904 				err = PTR_ERR(priv->mg_clk);
6905 				goto err_gop_clk;
6906 			}
6907 
6908 			err = clk_prepare_enable(priv->mg_clk);
6909 			if (err < 0)
6910 				goto err_gop_clk;
6911 
6912 			priv->mg_core_clk = devm_clk_get(&pdev->dev, "mg_core_clk");
6913 			if (IS_ERR(priv->mg_core_clk)) {
6914 				priv->mg_core_clk = NULL;
6915 			} else {
6916 				err = clk_prepare_enable(priv->mg_core_clk);
6917 				if (err < 0)
6918 					goto err_mg_clk;
6919 			}
6920 		}
6921 
6922 		priv->axi_clk = devm_clk_get(&pdev->dev, "axi_clk");
6923 		if (IS_ERR(priv->axi_clk)) {
6924 			err = PTR_ERR(priv->axi_clk);
6925 			if (err == -EPROBE_DEFER)
6926 				goto err_mg_core_clk;
6927 			priv->axi_clk = NULL;
6928 		} else {
6929 			err = clk_prepare_enable(priv->axi_clk);
6930 			if (err < 0)
6931 				goto err_mg_core_clk;
6932 		}
6933 
6934 		/* Get system's tclk rate */
6935 		priv->tclk = clk_get_rate(priv->pp_clk);
6936 	} else if (device_property_read_u32(&pdev->dev, "clock-frequency",
6937 					    &priv->tclk)) {
6938 		dev_err(&pdev->dev, "missing clock-frequency value\n");
6939 		return -EINVAL;
6940 	}
6941 
6942 	if (priv->hw_version == MVPP22) {
6943 		err = dma_set_mask(&pdev->dev, MVPP2_DESC_DMA_MASK);
6944 		if (err)
6945 			goto err_axi_clk;
6946 		/* Sadly, the BM pools all share the same register to
6947 		 * store the high 32 bits of their address. So they
6948 		 * must all have the same high 32 bits, which forces
6949 		 * us to restrict coherent memory to DMA_BIT_MASK(32).
6950 		 */
6951 		err = dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
6952 		if (err)
6953 			goto err_axi_clk;
6954 	}
6955 
6956 	/* Initialize network controller */
6957 	err = mvpp2_init(pdev, priv);
6958 	if (err < 0) {
6959 		dev_err(&pdev->dev, "failed to initialize controller\n");
6960 		goto err_axi_clk;
6961 	}
6962 
6963 	err = mvpp22_tai_probe(&pdev->dev, priv);
6964 	if (err < 0)
6965 		goto err_axi_clk;
6966 
6967 	/* Initialize ports */
6968 	fwnode_for_each_available_child_node(fwnode, port_fwnode) {
6969 		err = mvpp2_port_probe(pdev, port_fwnode, priv);
6970 		if (err < 0)
6971 			goto err_port_probe;
6972 	}
6973 
6974 	if (priv->port_count == 0) {
6975 		dev_err(&pdev->dev, "no ports enabled\n");
6976 		err = -ENODEV;
6977 		goto err_axi_clk;
6978 	}
6979 
6980 	/* Statistics must be gathered regularly because some of them (like
6981 	 * packets counters) are 32-bit registers and could overflow quite
6982 	 * quickly. For instance, a 10Gb link used at full bandwidth with the
6983 	 * smallest packets (64B) will overflow a 32-bit counter in less than
6984 	 * 30 seconds. Then, use a workqueue to fill 64-bit counters.
6985 	 */
6986 	snprintf(priv->queue_name, sizeof(priv->queue_name),
6987 		 "stats-wq-%s%s", netdev_name(priv->port_list[0]->dev),
6988 		 priv->port_count > 1 ? "+" : "");
6989 	priv->stats_queue = create_singlethread_workqueue(priv->queue_name);
6990 	if (!priv->stats_queue) {
6991 		err = -ENOMEM;
6992 		goto err_port_probe;
6993 	}
6994 
6995 	mvpp2_dbgfs_init(priv, pdev->name);
6996 
6997 	platform_set_drvdata(pdev, priv);
6998 	return 0;
6999 
7000 err_port_probe:
7001 	i = 0;
7002 	fwnode_for_each_available_child_node(fwnode, port_fwnode) {
7003 		if (priv->port_list[i])
7004 			mvpp2_port_remove(priv->port_list[i]);
7005 		i++;
7006 	}
7007 err_axi_clk:
7008 	clk_disable_unprepare(priv->axi_clk);
7009 
7010 err_mg_core_clk:
7011 	if (priv->hw_version == MVPP22)
7012 		clk_disable_unprepare(priv->mg_core_clk);
7013 err_mg_clk:
7014 	if (priv->hw_version == MVPP22)
7015 		clk_disable_unprepare(priv->mg_clk);
7016 err_gop_clk:
7017 	clk_disable_unprepare(priv->gop_clk);
7018 err_pp_clk:
7019 	clk_disable_unprepare(priv->pp_clk);
7020 	return err;
7021 }
7022 
7023 static int mvpp2_remove(struct platform_device *pdev)
7024 {
7025 	struct mvpp2 *priv = platform_get_drvdata(pdev);
7026 	struct fwnode_handle *fwnode = pdev->dev.fwnode;
7027 	int i = 0, poolnum = MVPP2_BM_POOLS_NUM;
7028 	struct fwnode_handle *port_fwnode;
7029 
7030 	mvpp2_dbgfs_cleanup(priv);
7031 
7032 	fwnode_for_each_available_child_node(fwnode, port_fwnode) {
7033 		if (priv->port_list[i]) {
7034 			mutex_destroy(&priv->port_list[i]->gather_stats_lock);
7035 			mvpp2_port_remove(priv->port_list[i]);
7036 		}
7037 		i++;
7038 	}
7039 
7040 	destroy_workqueue(priv->stats_queue);
7041 
7042 	if (priv->percpu_pools)
7043 		poolnum = mvpp2_get_nrxqs(priv) * 2;
7044 
7045 	for (i = 0; i < poolnum; i++) {
7046 		struct mvpp2_bm_pool *bm_pool = &priv->bm_pools[i];
7047 
7048 		mvpp2_bm_pool_destroy(&pdev->dev, priv, bm_pool);
7049 	}
7050 
7051 	for (i = 0; i < MVPP2_MAX_THREADS; i++) {
7052 		struct mvpp2_tx_queue *aggr_txq = &priv->aggr_txqs[i];
7053 
7054 		dma_free_coherent(&pdev->dev,
7055 				  MVPP2_AGGR_TXQ_SIZE * MVPP2_DESC_ALIGNED_SIZE,
7056 				  aggr_txq->descs,
7057 				  aggr_txq->descs_dma);
7058 	}
7059 
7060 	if (is_acpi_node(port_fwnode))
7061 		return 0;
7062 
7063 	clk_disable_unprepare(priv->axi_clk);
7064 	clk_disable_unprepare(priv->mg_core_clk);
7065 	clk_disable_unprepare(priv->mg_clk);
7066 	clk_disable_unprepare(priv->pp_clk);
7067 	clk_disable_unprepare(priv->gop_clk);
7068 
7069 	return 0;
7070 }
7071 
7072 static const struct of_device_id mvpp2_match[] = {
7073 	{
7074 		.compatible = "marvell,armada-375-pp2",
7075 		.data = (void *)MVPP21,
7076 	},
7077 	{
7078 		.compatible = "marvell,armada-7k-pp22",
7079 		.data = (void *)MVPP22,
7080 	},
7081 	{ }
7082 };
7083 MODULE_DEVICE_TABLE(of, mvpp2_match);
7084 
7085 #ifdef CONFIG_ACPI
7086 static const struct acpi_device_id mvpp2_acpi_match[] = {
7087 	{ "MRVL0110", MVPP22 },
7088 	{ },
7089 };
7090 MODULE_DEVICE_TABLE(acpi, mvpp2_acpi_match);
7091 #endif
7092 
7093 static struct platform_driver mvpp2_driver = {
7094 	.probe = mvpp2_probe,
7095 	.remove = mvpp2_remove,
7096 	.driver = {
7097 		.name = MVPP2_DRIVER_NAME,
7098 		.of_match_table = mvpp2_match,
7099 		.acpi_match_table = ACPI_PTR(mvpp2_acpi_match),
7100 	},
7101 };
7102 
7103 module_platform_driver(mvpp2_driver);
7104 
7105 MODULE_DESCRIPTION("Marvell PPv2 Ethernet Driver - www.marvell.com");
7106 MODULE_AUTHOR("Marcin Wojtas <mw@semihalf.com>");
7107 MODULE_LICENSE("GPL v2");
7108