1 // SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
2 /* Copyright (c) 2019-2020 Marvell International Ltd. All rights reserved */
3 
4 #include <linux/bitfield.h>
5 #include <linux/dmapool.h>
6 #include <linux/etherdevice.h>
7 #include <linux/if_vlan.h>
8 #include <linux/platform_device.h>
9 
10 #include "prestera_dsa.h"
11 #include "prestera.h"
12 #include "prestera_hw.h"
13 #include "prestera_rxtx.h"
14 #include "prestera_devlink.h"
15 
16 #define PRESTERA_SDMA_WAIT_MUL		10
17 
18 struct prestera_sdma_desc {
19 	__le32 word1;
20 	__le32 word2;
21 	__le32 buff;
22 	__le32 next;
23 } __packed __aligned(16);
24 
25 #define PRESTERA_SDMA_BUFF_SIZE_MAX	1544
26 
27 #define PRESTERA_SDMA_RX_DESC_PKT_LEN(desc) \
28 	((le32_to_cpu((desc)->word2) >> 16) & GENMASK(13, 0))
29 
30 #define PRESTERA_SDMA_RX_DESC_OWNER(desc) \
31 	((le32_to_cpu((desc)->word1) & BIT(31)) >> 31)
32 
33 #define PRESTERA_SDMA_RX_DESC_IS_RCVD(desc) \
34 	(PRESTERA_SDMA_RX_DESC_OWNER(desc) == PRESTERA_SDMA_RX_DESC_CPU_OWN)
35 
36 #define PRESTERA_SDMA_RX_DESC_CPU_OWN	0
37 #define PRESTERA_SDMA_RX_DESC_DMA_OWN	1
38 
39 #define PRESTERA_SDMA_RX_QUEUE_NUM	8
40 
41 #define PRESTERA_SDMA_RX_DESC_PER_Q	1000
42 
43 #define PRESTERA_SDMA_TX_DESC_PER_Q	1000
44 #define PRESTERA_SDMA_TX_MAX_BURST	64
45 
46 #define PRESTERA_SDMA_TX_DESC_OWNER(desc) \
47 	((le32_to_cpu((desc)->word1) & BIT(31)) >> 31)
48 
49 #define PRESTERA_SDMA_TX_DESC_CPU_OWN	0
50 #define PRESTERA_SDMA_TX_DESC_DMA_OWN	1U
51 
52 #define PRESTERA_SDMA_TX_DESC_IS_SENT(desc) \
53 	(PRESTERA_SDMA_TX_DESC_OWNER(desc) == PRESTERA_SDMA_TX_DESC_CPU_OWN)
54 
55 #define PRESTERA_SDMA_TX_DESC_LAST	BIT(20)
56 #define PRESTERA_SDMA_TX_DESC_FIRST	BIT(21)
57 #define PRESTERA_SDMA_TX_DESC_CALC_CRC	BIT(12)
58 
59 #define PRESTERA_SDMA_TX_DESC_SINGLE	\
60 	(PRESTERA_SDMA_TX_DESC_FIRST | PRESTERA_SDMA_TX_DESC_LAST)
61 
62 #define PRESTERA_SDMA_TX_DESC_INIT	\
63 	(PRESTERA_SDMA_TX_DESC_SINGLE | PRESTERA_SDMA_TX_DESC_CALC_CRC)
64 
65 #define PRESTERA_SDMA_RX_INTR_MASK_REG		0x2814
66 #define PRESTERA_SDMA_RX_QUEUE_STATUS_REG	0x2680
67 #define PRESTERA_SDMA_RX_QUEUE_DESC_REG(n)	(0x260C + (n) * 16)
68 
69 #define PRESTERA_SDMA_TX_QUEUE_DESC_REG		0x26C0
70 #define PRESTERA_SDMA_TX_QUEUE_START_REG	0x2868
71 
72 struct prestera_sdma_buf {
73 	struct prestera_sdma_desc *desc;
74 	dma_addr_t desc_dma;
75 	struct sk_buff *skb;
76 	dma_addr_t buf_dma;
77 	bool is_used;
78 };
79 
80 struct prestera_rx_ring {
81 	struct prestera_sdma_buf *bufs;
82 	int next_rx;
83 };
84 
85 struct prestera_tx_ring {
86 	struct prestera_sdma_buf *bufs;
87 	int next_tx;
88 	int max_burst;
89 	int burst;
90 };
91 
92 struct prestera_sdma {
93 	struct prestera_rx_ring rx_ring[PRESTERA_SDMA_RX_QUEUE_NUM];
94 	struct prestera_tx_ring tx_ring;
95 	struct prestera_switch *sw;
96 	struct dma_pool *desc_pool;
97 	struct work_struct tx_work;
98 	struct napi_struct rx_napi;
99 	struct net_device napi_dev;
100 	u32 map_addr;
101 	u64 dma_mask;
102 	/* protect SDMA with concurrent access from multiple CPUs */
103 	spinlock_t tx_lock;
104 };
105 
106 struct prestera_rxtx {
107 	struct prestera_sdma sdma;
108 };
109 
110 static int prestera_sdma_buf_init(struct prestera_sdma *sdma,
111 				  struct prestera_sdma_buf *buf)
112 {
113 	struct prestera_sdma_desc *desc;
114 	dma_addr_t dma;
115 
116 	desc = dma_pool_alloc(sdma->desc_pool, GFP_DMA | GFP_KERNEL, &dma);
117 	if (!desc)
118 		return -ENOMEM;
119 
120 	buf->buf_dma = DMA_MAPPING_ERROR;
121 	buf->desc_dma = dma;
122 	buf->desc = desc;
123 	buf->skb = NULL;
124 
125 	return 0;
126 }
127 
128 static u32 prestera_sdma_map(struct prestera_sdma *sdma, dma_addr_t pa)
129 {
130 	return sdma->map_addr + pa;
131 }
132 
133 static void prestera_sdma_rx_desc_init(struct prestera_sdma *sdma,
134 				       struct prestera_sdma_desc *desc,
135 				       dma_addr_t buf)
136 {
137 	u32 word = le32_to_cpu(desc->word2);
138 
139 	u32p_replace_bits(&word, PRESTERA_SDMA_BUFF_SIZE_MAX, GENMASK(15, 0));
140 	desc->word2 = cpu_to_le32(word);
141 
142 	desc->buff = cpu_to_le32(prestera_sdma_map(sdma, buf));
143 
144 	/* make sure buffer is set before reset the descriptor */
145 	wmb();
146 
147 	desc->word1 = cpu_to_le32(0xA0000000);
148 }
149 
150 static void prestera_sdma_rx_desc_set_next(struct prestera_sdma *sdma,
151 					   struct prestera_sdma_desc *desc,
152 					   dma_addr_t next)
153 {
154 	desc->next = cpu_to_le32(prestera_sdma_map(sdma, next));
155 }
156 
157 static int prestera_sdma_rx_skb_alloc(struct prestera_sdma *sdma,
158 				      struct prestera_sdma_buf *buf)
159 {
160 	struct device *dev = sdma->sw->dev->dev;
161 	struct sk_buff *skb;
162 	dma_addr_t dma;
163 
164 	skb = alloc_skb(PRESTERA_SDMA_BUFF_SIZE_MAX, GFP_DMA | GFP_ATOMIC);
165 	if (!skb)
166 		return -ENOMEM;
167 
168 	dma = dma_map_single(dev, skb->data, skb->len, DMA_FROM_DEVICE);
169 	if (dma_mapping_error(dev, dma))
170 		goto err_dma_map;
171 
172 	if (buf->skb)
173 		dma_unmap_single(dev, buf->buf_dma, buf->skb->len,
174 				 DMA_FROM_DEVICE);
175 
176 	buf->buf_dma = dma;
177 	buf->skb = skb;
178 
179 	return 0;
180 
181 err_dma_map:
182 	kfree_skb(skb);
183 
184 	return -ENOMEM;
185 }
186 
187 static struct sk_buff *prestera_sdma_rx_skb_get(struct prestera_sdma *sdma,
188 						struct prestera_sdma_buf *buf)
189 {
190 	dma_addr_t buf_dma = buf->buf_dma;
191 	struct sk_buff *skb = buf->skb;
192 	u32 len = skb->len;
193 	int err;
194 
195 	err = prestera_sdma_rx_skb_alloc(sdma, buf);
196 	if (err) {
197 		buf->buf_dma = buf_dma;
198 		buf->skb = skb;
199 
200 		skb = alloc_skb(skb->len, GFP_ATOMIC);
201 		if (skb) {
202 			skb_put(skb, len);
203 			skb_copy_from_linear_data(buf->skb, skb->data, len);
204 		}
205 	}
206 
207 	prestera_sdma_rx_desc_init(sdma, buf->desc, buf->buf_dma);
208 
209 	return skb;
210 }
211 
212 static int prestera_rxtx_process_skb(struct prestera_sdma *sdma,
213 				     struct sk_buff *skb)
214 {
215 	struct prestera_port *port;
216 	struct prestera_dsa dsa;
217 	u32 hw_port, dev_id;
218 	u8 cpu_code;
219 	int err;
220 
221 	skb_pull(skb, ETH_HLEN);
222 
223 	/* ethertype field is part of the dsa header */
224 	err = prestera_dsa_parse(&dsa, skb->data - ETH_TLEN);
225 	if (err)
226 		return err;
227 
228 	dev_id = dsa.hw_dev_num;
229 	hw_port = dsa.port_num;
230 
231 	port = prestera_port_find_by_hwid(sdma->sw, dev_id, hw_port);
232 	if (unlikely(!port)) {
233 		dev_warn_ratelimited(prestera_dev(sdma->sw), "received pkt for non-existent port(%u, %u)\n",
234 				     dev_id, hw_port);
235 		return -ENOENT;
236 	}
237 
238 	if (unlikely(!pskb_may_pull(skb, PRESTERA_DSA_HLEN)))
239 		return -EINVAL;
240 
241 	/* remove DSA tag and update checksum */
242 	skb_pull_rcsum(skb, PRESTERA_DSA_HLEN);
243 
244 	memmove(skb->data - ETH_HLEN, skb->data - ETH_HLEN - PRESTERA_DSA_HLEN,
245 		ETH_ALEN * 2);
246 
247 	skb_push(skb, ETH_HLEN);
248 
249 	skb->protocol = eth_type_trans(skb, port->dev);
250 
251 	if (dsa.vlan.is_tagged) {
252 		u16 tci = dsa.vlan.vid & VLAN_VID_MASK;
253 
254 		tci |= dsa.vlan.vpt << VLAN_PRIO_SHIFT;
255 		if (dsa.vlan.cfi_bit)
256 			tci |= VLAN_CFI_MASK;
257 
258 		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), tci);
259 	}
260 
261 	cpu_code = dsa.cpu_code;
262 	prestera_devlink_trap_report(port, skb, cpu_code);
263 
264 	return 0;
265 }
266 
267 static int prestera_sdma_next_rx_buf_idx(int buf_idx)
268 {
269 	return (buf_idx + 1) % PRESTERA_SDMA_RX_DESC_PER_Q;
270 }
271 
272 static int prestera_sdma_rx_poll(struct napi_struct *napi, int budget)
273 {
274 	int qnum = PRESTERA_SDMA_RX_QUEUE_NUM;
275 	unsigned int rxq_done_map = 0;
276 	struct prestera_sdma *sdma;
277 	struct list_head rx_list;
278 	unsigned int qmask;
279 	int pkts_done = 0;
280 	int q;
281 
282 	qnum = PRESTERA_SDMA_RX_QUEUE_NUM;
283 	qmask = GENMASK(qnum - 1, 0);
284 
285 	INIT_LIST_HEAD(&rx_list);
286 
287 	sdma = container_of(napi, struct prestera_sdma, rx_napi);
288 
289 	while (pkts_done < budget && rxq_done_map != qmask) {
290 		for (q = 0; q < qnum && pkts_done < budget; q++) {
291 			struct prestera_rx_ring *ring = &sdma->rx_ring[q];
292 			struct prestera_sdma_desc *desc;
293 			struct prestera_sdma_buf *buf;
294 			int buf_idx = ring->next_rx;
295 			struct sk_buff *skb;
296 
297 			buf = &ring->bufs[buf_idx];
298 			desc = buf->desc;
299 
300 			if (PRESTERA_SDMA_RX_DESC_IS_RCVD(desc)) {
301 				rxq_done_map &= ~BIT(q);
302 			} else {
303 				rxq_done_map |= BIT(q);
304 				continue;
305 			}
306 
307 			pkts_done++;
308 
309 			__skb_trim(buf->skb, PRESTERA_SDMA_RX_DESC_PKT_LEN(desc));
310 
311 			skb = prestera_sdma_rx_skb_get(sdma, buf);
312 			if (!skb)
313 				goto rx_next_buf;
314 
315 			if (unlikely(prestera_rxtx_process_skb(sdma, skb)))
316 				goto rx_next_buf;
317 
318 			list_add_tail(&skb->list, &rx_list);
319 rx_next_buf:
320 			ring->next_rx = prestera_sdma_next_rx_buf_idx(buf_idx);
321 		}
322 	}
323 
324 	if (pkts_done < budget && napi_complete_done(napi, pkts_done))
325 		prestera_write(sdma->sw, PRESTERA_SDMA_RX_INTR_MASK_REG,
326 			       GENMASK(9, 2));
327 
328 	netif_receive_skb_list(&rx_list);
329 
330 	return pkts_done;
331 }
332 
333 static void prestera_sdma_rx_fini(struct prestera_sdma *sdma)
334 {
335 	int qnum = PRESTERA_SDMA_RX_QUEUE_NUM;
336 	int q, b;
337 
338 	/* disable all rx queues */
339 	prestera_write(sdma->sw, PRESTERA_SDMA_RX_QUEUE_STATUS_REG,
340 		       GENMASK(15, 8));
341 
342 	for (q = 0; q < qnum; q++) {
343 		struct prestera_rx_ring *ring = &sdma->rx_ring[q];
344 
345 		if (!ring->bufs)
346 			break;
347 
348 		for (b = 0; b < PRESTERA_SDMA_RX_DESC_PER_Q; b++) {
349 			struct prestera_sdma_buf *buf = &ring->bufs[b];
350 
351 			if (buf->desc_dma)
352 				dma_pool_free(sdma->desc_pool, buf->desc,
353 					      buf->desc_dma);
354 
355 			if (!buf->skb)
356 				continue;
357 
358 			if (buf->buf_dma != DMA_MAPPING_ERROR)
359 				dma_unmap_single(sdma->sw->dev->dev,
360 						 buf->buf_dma, buf->skb->len,
361 						 DMA_FROM_DEVICE);
362 			kfree_skb(buf->skb);
363 		}
364 	}
365 }
366 
367 static int prestera_sdma_rx_init(struct prestera_sdma *sdma)
368 {
369 	int bnum = PRESTERA_SDMA_RX_DESC_PER_Q;
370 	int qnum = PRESTERA_SDMA_RX_QUEUE_NUM;
371 	int err;
372 	int q;
373 
374 	/* disable all rx queues */
375 	prestera_write(sdma->sw, PRESTERA_SDMA_RX_QUEUE_STATUS_REG,
376 		       GENMASK(15, 8));
377 
378 	for (q = 0; q < qnum; q++) {
379 		struct prestera_sdma_buf *head, *tail, *next, *prev;
380 		struct prestera_rx_ring *ring = &sdma->rx_ring[q];
381 
382 		ring->bufs = kmalloc_array(bnum, sizeof(*head), GFP_KERNEL);
383 		if (!ring->bufs)
384 			return -ENOMEM;
385 
386 		ring->next_rx = 0;
387 
388 		tail = &ring->bufs[bnum - 1];
389 		head = &ring->bufs[0];
390 		next = head;
391 		prev = next;
392 
393 		do {
394 			err = prestera_sdma_buf_init(sdma, next);
395 			if (err)
396 				return err;
397 
398 			err = prestera_sdma_rx_skb_alloc(sdma, next);
399 			if (err)
400 				return err;
401 
402 			prestera_sdma_rx_desc_init(sdma, next->desc,
403 						   next->buf_dma);
404 
405 			prestera_sdma_rx_desc_set_next(sdma, prev->desc,
406 						       next->desc_dma);
407 
408 			prev = next;
409 			next++;
410 		} while (prev != tail);
411 
412 		/* join tail with head to make a circular list */
413 		prestera_sdma_rx_desc_set_next(sdma, tail->desc, head->desc_dma);
414 
415 		prestera_write(sdma->sw, PRESTERA_SDMA_RX_QUEUE_DESC_REG(q),
416 			       prestera_sdma_map(sdma, head->desc_dma));
417 	}
418 
419 	/* make sure all rx descs are filled before enabling all rx queues */
420 	wmb();
421 
422 	prestera_write(sdma->sw, PRESTERA_SDMA_RX_QUEUE_STATUS_REG,
423 		       GENMASK(7, 0));
424 
425 	return 0;
426 }
427 
428 static void prestera_sdma_tx_desc_init(struct prestera_sdma *sdma,
429 				       struct prestera_sdma_desc *desc)
430 {
431 	desc->word1 = cpu_to_le32(PRESTERA_SDMA_TX_DESC_INIT);
432 	desc->word2 = 0;
433 }
434 
435 static void prestera_sdma_tx_desc_set_next(struct prestera_sdma *sdma,
436 					   struct prestera_sdma_desc *desc,
437 					   dma_addr_t next)
438 {
439 	desc->next = cpu_to_le32(prestera_sdma_map(sdma, next));
440 }
441 
442 static void prestera_sdma_tx_desc_set_buf(struct prestera_sdma *sdma,
443 					  struct prestera_sdma_desc *desc,
444 					  dma_addr_t buf, size_t len)
445 {
446 	u32 word = le32_to_cpu(desc->word2);
447 
448 	u32p_replace_bits(&word, len + ETH_FCS_LEN, GENMASK(30, 16));
449 
450 	desc->buff = cpu_to_le32(prestera_sdma_map(sdma, buf));
451 	desc->word2 = cpu_to_le32(word);
452 }
453 
454 static void prestera_sdma_tx_desc_xmit(struct prestera_sdma_desc *desc)
455 {
456 	u32 word = le32_to_cpu(desc->word1);
457 
458 	word |= PRESTERA_SDMA_TX_DESC_DMA_OWN << 31;
459 
460 	/* make sure everything is written before enable xmit */
461 	wmb();
462 
463 	desc->word1 = cpu_to_le32(word);
464 }
465 
466 static int prestera_sdma_tx_buf_map(struct prestera_sdma *sdma,
467 				    struct prestera_sdma_buf *buf,
468 				    struct sk_buff *skb)
469 {
470 	struct device *dma_dev = sdma->sw->dev->dev;
471 	dma_addr_t dma;
472 
473 	dma = dma_map_single(dma_dev, skb->data, skb->len, DMA_TO_DEVICE);
474 	if (dma_mapping_error(dma_dev, dma))
475 		return -ENOMEM;
476 
477 	buf->buf_dma = dma;
478 	buf->skb = skb;
479 
480 	return 0;
481 }
482 
483 static void prestera_sdma_tx_buf_unmap(struct prestera_sdma *sdma,
484 				       struct prestera_sdma_buf *buf)
485 {
486 	struct device *dma_dev = sdma->sw->dev->dev;
487 
488 	dma_unmap_single(dma_dev, buf->buf_dma, buf->skb->len, DMA_TO_DEVICE);
489 }
490 
491 static void prestera_sdma_tx_recycle_work_fn(struct work_struct *work)
492 {
493 	int bnum = PRESTERA_SDMA_TX_DESC_PER_Q;
494 	struct prestera_tx_ring *tx_ring;
495 	struct prestera_sdma *sdma;
496 	int b;
497 
498 	sdma = container_of(work, struct prestera_sdma, tx_work);
499 
500 	tx_ring = &sdma->tx_ring;
501 
502 	for (b = 0; b < bnum; b++) {
503 		struct prestera_sdma_buf *buf = &tx_ring->bufs[b];
504 
505 		if (!buf->is_used)
506 			continue;
507 
508 		if (!PRESTERA_SDMA_TX_DESC_IS_SENT(buf->desc))
509 			continue;
510 
511 		prestera_sdma_tx_buf_unmap(sdma, buf);
512 		dev_consume_skb_any(buf->skb);
513 		buf->skb = NULL;
514 
515 		/* make sure everything is cleaned up */
516 		wmb();
517 
518 		buf->is_used = false;
519 	}
520 }
521 
522 static int prestera_sdma_tx_init(struct prestera_sdma *sdma)
523 {
524 	struct prestera_sdma_buf *head, *tail, *next, *prev;
525 	struct prestera_tx_ring *tx_ring = &sdma->tx_ring;
526 	int bnum = PRESTERA_SDMA_TX_DESC_PER_Q;
527 	int err;
528 
529 	INIT_WORK(&sdma->tx_work, prestera_sdma_tx_recycle_work_fn);
530 	spin_lock_init(&sdma->tx_lock);
531 
532 	tx_ring->bufs = kmalloc_array(bnum, sizeof(*head), GFP_KERNEL);
533 	if (!tx_ring->bufs)
534 		return -ENOMEM;
535 
536 	tail = &tx_ring->bufs[bnum - 1];
537 	head = &tx_ring->bufs[0];
538 	next = head;
539 	prev = next;
540 
541 	tx_ring->max_burst = PRESTERA_SDMA_TX_MAX_BURST;
542 	tx_ring->burst = tx_ring->max_burst;
543 	tx_ring->next_tx = 0;
544 
545 	do {
546 		err = prestera_sdma_buf_init(sdma, next);
547 		if (err)
548 			return err;
549 
550 		next->is_used = false;
551 
552 		prestera_sdma_tx_desc_init(sdma, next->desc);
553 
554 		prestera_sdma_tx_desc_set_next(sdma, prev->desc,
555 					       next->desc_dma);
556 
557 		prev = next;
558 		next++;
559 	} while (prev != tail);
560 
561 	/* join tail with head to make a circular list */
562 	prestera_sdma_tx_desc_set_next(sdma, tail->desc, head->desc_dma);
563 
564 	/* make sure descriptors are written */
565 	wmb();
566 
567 	prestera_write(sdma->sw, PRESTERA_SDMA_TX_QUEUE_DESC_REG,
568 		       prestera_sdma_map(sdma, head->desc_dma));
569 
570 	return 0;
571 }
572 
573 static void prestera_sdma_tx_fini(struct prestera_sdma *sdma)
574 {
575 	struct prestera_tx_ring *ring = &sdma->tx_ring;
576 	int bnum = PRESTERA_SDMA_TX_DESC_PER_Q;
577 	int b;
578 
579 	cancel_work_sync(&sdma->tx_work);
580 
581 	if (!ring->bufs)
582 		return;
583 
584 	for (b = 0; b < bnum; b++) {
585 		struct prestera_sdma_buf *buf = &ring->bufs[b];
586 
587 		if (buf->desc)
588 			dma_pool_free(sdma->desc_pool, buf->desc,
589 				      buf->desc_dma);
590 
591 		if (!buf->skb)
592 			continue;
593 
594 		dma_unmap_single(sdma->sw->dev->dev, buf->buf_dma,
595 				 buf->skb->len, DMA_TO_DEVICE);
596 
597 		dev_consume_skb_any(buf->skb);
598 	}
599 }
600 
601 static void prestera_rxtx_handle_event(struct prestera_switch *sw,
602 				       struct prestera_event *evt,
603 				       void *arg)
604 {
605 	struct prestera_sdma *sdma = arg;
606 
607 	if (evt->id != PRESTERA_RXTX_EVENT_RCV_PKT)
608 		return;
609 
610 	prestera_write(sdma->sw, PRESTERA_SDMA_RX_INTR_MASK_REG, 0);
611 	napi_schedule(&sdma->rx_napi);
612 }
613 
614 static int prestera_sdma_switch_init(struct prestera_switch *sw)
615 {
616 	struct prestera_sdma *sdma = &sw->rxtx->sdma;
617 	struct device *dev = sw->dev->dev;
618 	struct prestera_rxtx_params p;
619 	int err;
620 
621 	p.use_sdma = true;
622 
623 	err = prestera_hw_rxtx_init(sw, &p);
624 	if (err) {
625 		dev_err(dev, "failed to init rxtx by hw\n");
626 		return err;
627 	}
628 
629 	sdma->dma_mask = dma_get_mask(dev);
630 	sdma->map_addr = p.map_addr;
631 	sdma->sw = sw;
632 
633 	sdma->desc_pool = dma_pool_create("desc_pool", dev,
634 					  sizeof(struct prestera_sdma_desc),
635 					  16, 0);
636 	if (!sdma->desc_pool)
637 		return -ENOMEM;
638 
639 	err = prestera_sdma_rx_init(sdma);
640 	if (err) {
641 		dev_err(dev, "failed to init rx ring\n");
642 		goto err_rx_init;
643 	}
644 
645 	err = prestera_sdma_tx_init(sdma);
646 	if (err) {
647 		dev_err(dev, "failed to init tx ring\n");
648 		goto err_tx_init;
649 	}
650 
651 	err = prestera_hw_event_handler_register(sw, PRESTERA_EVENT_TYPE_RXTX,
652 						 prestera_rxtx_handle_event,
653 						 sdma);
654 	if (err)
655 		goto err_evt_register;
656 
657 	init_dummy_netdev(&sdma->napi_dev);
658 
659 	netif_napi_add(&sdma->napi_dev, &sdma->rx_napi, prestera_sdma_rx_poll);
660 	napi_enable(&sdma->rx_napi);
661 
662 	return 0;
663 
664 err_evt_register:
665 err_tx_init:
666 	prestera_sdma_tx_fini(sdma);
667 err_rx_init:
668 	prestera_sdma_rx_fini(sdma);
669 
670 	dma_pool_destroy(sdma->desc_pool);
671 	return err;
672 }
673 
674 static void prestera_sdma_switch_fini(struct prestera_switch *sw)
675 {
676 	struct prestera_sdma *sdma = &sw->rxtx->sdma;
677 
678 	napi_disable(&sdma->rx_napi);
679 	netif_napi_del(&sdma->rx_napi);
680 	prestera_hw_event_handler_unregister(sw, PRESTERA_EVENT_TYPE_RXTX,
681 					     prestera_rxtx_handle_event);
682 	prestera_sdma_tx_fini(sdma);
683 	prestera_sdma_rx_fini(sdma);
684 	dma_pool_destroy(sdma->desc_pool);
685 }
686 
687 static bool prestera_sdma_is_ready(struct prestera_sdma *sdma)
688 {
689 	return !(prestera_read(sdma->sw, PRESTERA_SDMA_TX_QUEUE_START_REG) & 1);
690 }
691 
692 static int prestera_sdma_tx_wait(struct prestera_sdma *sdma,
693 				 struct prestera_tx_ring *tx_ring)
694 {
695 	int tx_wait_num = PRESTERA_SDMA_WAIT_MUL * tx_ring->max_burst;
696 
697 	do {
698 		if (prestera_sdma_is_ready(sdma))
699 			return 0;
700 
701 		udelay(1);
702 	} while (--tx_wait_num);
703 
704 	return -EBUSY;
705 }
706 
707 static void prestera_sdma_tx_start(struct prestera_sdma *sdma)
708 {
709 	prestera_write(sdma->sw, PRESTERA_SDMA_TX_QUEUE_START_REG, 1);
710 	schedule_work(&sdma->tx_work);
711 }
712 
713 static netdev_tx_t prestera_sdma_xmit(struct prestera_sdma *sdma,
714 				      struct sk_buff *skb)
715 {
716 	struct device *dma_dev = sdma->sw->dev->dev;
717 	struct net_device *dev = skb->dev;
718 	struct prestera_tx_ring *tx_ring;
719 	struct prestera_sdma_buf *buf;
720 	int err;
721 
722 	spin_lock(&sdma->tx_lock);
723 
724 	tx_ring = &sdma->tx_ring;
725 
726 	buf = &tx_ring->bufs[tx_ring->next_tx];
727 	if (buf->is_used) {
728 		schedule_work(&sdma->tx_work);
729 		goto drop_skb;
730 	}
731 
732 	if (unlikely(eth_skb_pad(skb)))
733 		goto drop_skb_nofree;
734 
735 	err = prestera_sdma_tx_buf_map(sdma, buf, skb);
736 	if (err)
737 		goto drop_skb;
738 
739 	prestera_sdma_tx_desc_set_buf(sdma, buf->desc, buf->buf_dma, skb->len);
740 
741 	dma_sync_single_for_device(dma_dev, buf->buf_dma, skb->len,
742 				   DMA_TO_DEVICE);
743 
744 	if (tx_ring->burst) {
745 		tx_ring->burst--;
746 	} else {
747 		tx_ring->burst = tx_ring->max_burst;
748 
749 		err = prestera_sdma_tx_wait(sdma, tx_ring);
750 		if (err)
751 			goto drop_skb_unmap;
752 	}
753 
754 	tx_ring->next_tx = (tx_ring->next_tx + 1) % PRESTERA_SDMA_TX_DESC_PER_Q;
755 	prestera_sdma_tx_desc_xmit(buf->desc);
756 	buf->is_used = true;
757 
758 	prestera_sdma_tx_start(sdma);
759 
760 	goto tx_done;
761 
762 drop_skb_unmap:
763 	prestera_sdma_tx_buf_unmap(sdma, buf);
764 drop_skb:
765 	dev_consume_skb_any(skb);
766 drop_skb_nofree:
767 	dev->stats.tx_dropped++;
768 tx_done:
769 	spin_unlock(&sdma->tx_lock);
770 	return NETDEV_TX_OK;
771 }
772 
773 int prestera_rxtx_switch_init(struct prestera_switch *sw)
774 {
775 	struct prestera_rxtx *rxtx;
776 	int err;
777 
778 	rxtx = kzalloc(sizeof(*rxtx), GFP_KERNEL);
779 	if (!rxtx)
780 		return -ENOMEM;
781 
782 	sw->rxtx = rxtx;
783 
784 	err = prestera_sdma_switch_init(sw);
785 	if (err)
786 		kfree(rxtx);
787 
788 	return err;
789 }
790 
791 void prestera_rxtx_switch_fini(struct prestera_switch *sw)
792 {
793 	prestera_sdma_switch_fini(sw);
794 	kfree(sw->rxtx);
795 }
796 
797 int prestera_rxtx_port_init(struct prestera_port *port)
798 {
799 	port->dev->needed_headroom = PRESTERA_DSA_HLEN;
800 	return 0;
801 }
802 
803 netdev_tx_t prestera_rxtx_xmit(struct prestera_port *port, struct sk_buff *skb)
804 {
805 	struct prestera_dsa dsa;
806 
807 	dsa.hw_dev_num = port->dev_id;
808 	dsa.port_num = port->hw_id;
809 
810 	if (skb_cow_head(skb, PRESTERA_DSA_HLEN) < 0)
811 		return NET_XMIT_DROP;
812 
813 	skb_push(skb, PRESTERA_DSA_HLEN);
814 	memmove(skb->data, skb->data + PRESTERA_DSA_HLEN, 2 * ETH_ALEN);
815 
816 	if (prestera_dsa_build(&dsa, skb->data + 2 * ETH_ALEN) != 0)
817 		return NET_XMIT_DROP;
818 
819 	return prestera_sdma_xmit(&port->sw->rxtx->sdma, skb);
820 }
821