1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
2 /*
3  * Microsemi SoCs FDMA driver
4  *
5  * Copyright (c) 2021 Microchip
6  *
7  * Page recycling code is mostly taken from gianfar driver.
8  */
9 
10 #include <linux/align.h>
11 #include <linux/bitops.h>
12 #include <linux/dmapool.h>
13 #include <linux/dsa/ocelot.h>
14 #include <linux/netdevice.h>
15 #include <linux/skbuff.h>
16 
17 #include "ocelot_fdma.h"
18 #include "ocelot_qs.h"
19 
20 DEFINE_STATIC_KEY_FALSE(ocelot_fdma_enabled);
21 
ocelot_fdma_writel(struct ocelot * ocelot,u32 reg,u32 data)22 static void ocelot_fdma_writel(struct ocelot *ocelot, u32 reg, u32 data)
23 {
24 	regmap_write(ocelot->targets[FDMA], reg, data);
25 }
26 
ocelot_fdma_readl(struct ocelot * ocelot,u32 reg)27 static u32 ocelot_fdma_readl(struct ocelot *ocelot, u32 reg)
28 {
29 	u32 retval;
30 
31 	regmap_read(ocelot->targets[FDMA], reg, &retval);
32 
33 	return retval;
34 }
35 
ocelot_fdma_idx_dma(dma_addr_t base,u16 idx)36 static dma_addr_t ocelot_fdma_idx_dma(dma_addr_t base, u16 idx)
37 {
38 	return base + idx * sizeof(struct ocelot_fdma_dcb);
39 }
40 
ocelot_fdma_dma_idx(dma_addr_t base,dma_addr_t dma)41 static u16 ocelot_fdma_dma_idx(dma_addr_t base, dma_addr_t dma)
42 {
43 	return (dma - base) / sizeof(struct ocelot_fdma_dcb);
44 }
45 
ocelot_fdma_idx_next(u16 idx,u16 ring_sz)46 static u16 ocelot_fdma_idx_next(u16 idx, u16 ring_sz)
47 {
48 	return unlikely(idx == ring_sz - 1) ? 0 : idx + 1;
49 }
50 
ocelot_fdma_idx_prev(u16 idx,u16 ring_sz)51 static u16 ocelot_fdma_idx_prev(u16 idx, u16 ring_sz)
52 {
53 	return unlikely(idx == 0) ? ring_sz - 1 : idx - 1;
54 }
55 
ocelot_fdma_rx_ring_free(struct ocelot_fdma * fdma)56 static int ocelot_fdma_rx_ring_free(struct ocelot_fdma *fdma)
57 {
58 	struct ocelot_fdma_rx_ring *rx_ring = &fdma->rx_ring;
59 
60 	if (rx_ring->next_to_use >= rx_ring->next_to_clean)
61 		return OCELOT_FDMA_RX_RING_SIZE -
62 		       (rx_ring->next_to_use - rx_ring->next_to_clean) - 1;
63 	else
64 		return rx_ring->next_to_clean - rx_ring->next_to_use - 1;
65 }
66 
ocelot_fdma_tx_ring_free(struct ocelot_fdma * fdma)67 static int ocelot_fdma_tx_ring_free(struct ocelot_fdma *fdma)
68 {
69 	struct ocelot_fdma_tx_ring *tx_ring = &fdma->tx_ring;
70 
71 	if (tx_ring->next_to_use >= tx_ring->next_to_clean)
72 		return OCELOT_FDMA_TX_RING_SIZE -
73 		       (tx_ring->next_to_use - tx_ring->next_to_clean) - 1;
74 	else
75 		return tx_ring->next_to_clean - tx_ring->next_to_use - 1;
76 }
77 
ocelot_fdma_tx_ring_empty(struct ocelot_fdma * fdma)78 static bool ocelot_fdma_tx_ring_empty(struct ocelot_fdma *fdma)
79 {
80 	struct ocelot_fdma_tx_ring *tx_ring = &fdma->tx_ring;
81 
82 	return tx_ring->next_to_clean == tx_ring->next_to_use;
83 }
84 
ocelot_fdma_activate_chan(struct ocelot * ocelot,dma_addr_t dma,int chan)85 static void ocelot_fdma_activate_chan(struct ocelot *ocelot, dma_addr_t dma,
86 				      int chan)
87 {
88 	ocelot_fdma_writel(ocelot, MSCC_FDMA_DCB_LLP(chan), dma);
89 	/* Barrier to force memory writes to DCB to be completed before starting
90 	 * the channel.
91 	 */
92 	wmb();
93 	ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_ACTIVATE, BIT(chan));
94 }
95 
ocelot_fdma_read_ch_safe(struct ocelot * ocelot)96 static u32 ocelot_fdma_read_ch_safe(struct ocelot *ocelot)
97 {
98 	return ocelot_fdma_readl(ocelot, MSCC_FDMA_CH_SAFE);
99 }
100 
ocelot_fdma_wait_chan_safe(struct ocelot * ocelot,int chan)101 static int ocelot_fdma_wait_chan_safe(struct ocelot *ocelot, int chan)
102 {
103 	u32 safe;
104 
105 	return readx_poll_timeout_atomic(ocelot_fdma_read_ch_safe, ocelot, safe,
106 					 safe & BIT(chan), 0,
107 					 OCELOT_FDMA_CH_SAFE_TIMEOUT_US);
108 }
109 
ocelot_fdma_dcb_set_data(struct ocelot_fdma_dcb * dcb,dma_addr_t dma_addr,size_t size)110 static void ocelot_fdma_dcb_set_data(struct ocelot_fdma_dcb *dcb,
111 				     dma_addr_t dma_addr,
112 				     size_t size)
113 {
114 	u32 offset = dma_addr & 0x3;
115 
116 	dcb->llp = 0;
117 	dcb->datap = ALIGN_DOWN(dma_addr, 4);
118 	dcb->datal = ALIGN_DOWN(size, 4);
119 	dcb->stat = MSCC_FDMA_DCB_STAT_BLOCKO(offset);
120 }
121 
ocelot_fdma_rx_alloc_page(struct ocelot * ocelot,struct ocelot_fdma_rx_buf * rxb)122 static bool ocelot_fdma_rx_alloc_page(struct ocelot *ocelot,
123 				      struct ocelot_fdma_rx_buf *rxb)
124 {
125 	dma_addr_t mapping;
126 	struct page *page;
127 
128 	page = dev_alloc_page();
129 	if (unlikely(!page))
130 		return false;
131 
132 	mapping = dma_map_page(ocelot->dev, page, 0, PAGE_SIZE,
133 			       DMA_FROM_DEVICE);
134 	if (unlikely(dma_mapping_error(ocelot->dev, mapping))) {
135 		__free_page(page);
136 		return false;
137 	}
138 
139 	rxb->page = page;
140 	rxb->page_offset = 0;
141 	rxb->dma_addr = mapping;
142 
143 	return true;
144 }
145 
ocelot_fdma_alloc_rx_buffs(struct ocelot * ocelot,u16 alloc_cnt)146 static int ocelot_fdma_alloc_rx_buffs(struct ocelot *ocelot, u16 alloc_cnt)
147 {
148 	struct ocelot_fdma *fdma = ocelot->fdma;
149 	struct ocelot_fdma_rx_ring *rx_ring;
150 	struct ocelot_fdma_rx_buf *rxb;
151 	struct ocelot_fdma_dcb *dcb;
152 	dma_addr_t dma_addr;
153 	int ret = 0;
154 	u16 idx;
155 
156 	rx_ring = &fdma->rx_ring;
157 	idx = rx_ring->next_to_use;
158 
159 	while (alloc_cnt--) {
160 		rxb = &rx_ring->bufs[idx];
161 		/* try reuse page */
162 		if (unlikely(!rxb->page)) {
163 			if (unlikely(!ocelot_fdma_rx_alloc_page(ocelot, rxb))) {
164 				dev_err_ratelimited(ocelot->dev,
165 						    "Failed to allocate rx\n");
166 				ret = -ENOMEM;
167 				break;
168 			}
169 		}
170 
171 		dcb = &rx_ring->dcbs[idx];
172 		dma_addr = rxb->dma_addr + rxb->page_offset;
173 		ocelot_fdma_dcb_set_data(dcb, dma_addr, OCELOT_FDMA_RXB_SIZE);
174 
175 		idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE);
176 		/* Chain the DCB to the next one */
177 		dcb->llp = ocelot_fdma_idx_dma(rx_ring->dcbs_dma, idx);
178 	}
179 
180 	rx_ring->next_to_use = idx;
181 	rx_ring->next_to_alloc = idx;
182 
183 	return ret;
184 }
185 
ocelot_fdma_tx_dcb_set_skb(struct ocelot * ocelot,struct ocelot_fdma_tx_buf * tx_buf,struct ocelot_fdma_dcb * dcb,struct sk_buff * skb)186 static bool ocelot_fdma_tx_dcb_set_skb(struct ocelot *ocelot,
187 				       struct ocelot_fdma_tx_buf *tx_buf,
188 				       struct ocelot_fdma_dcb *dcb,
189 				       struct sk_buff *skb)
190 {
191 	dma_addr_t mapping;
192 
193 	mapping = dma_map_single(ocelot->dev, skb->data, skb->len,
194 				 DMA_TO_DEVICE);
195 	if (unlikely(dma_mapping_error(ocelot->dev, mapping)))
196 		return false;
197 
198 	dma_unmap_addr_set(tx_buf, dma_addr, mapping);
199 
200 	ocelot_fdma_dcb_set_data(dcb, mapping, OCELOT_FDMA_RX_SIZE);
201 	tx_buf->skb = skb;
202 	dcb->stat |= MSCC_FDMA_DCB_STAT_BLOCKL(skb->len);
203 	dcb->stat |= MSCC_FDMA_DCB_STAT_SOF | MSCC_FDMA_DCB_STAT_EOF;
204 
205 	return true;
206 }
207 
ocelot_fdma_check_stop_rx(struct ocelot * ocelot)208 static bool ocelot_fdma_check_stop_rx(struct ocelot *ocelot)
209 {
210 	u32 llp;
211 
212 	/* Check if the FDMA hits the DCB with LLP == NULL */
213 	llp = ocelot_fdma_readl(ocelot, MSCC_FDMA_DCB_LLP(MSCC_FDMA_XTR_CHAN));
214 	if (unlikely(llp))
215 		return false;
216 
217 	ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_DISABLE,
218 			   BIT(MSCC_FDMA_XTR_CHAN));
219 
220 	return true;
221 }
222 
ocelot_fdma_rx_set_llp(struct ocelot_fdma_rx_ring * rx_ring)223 static void ocelot_fdma_rx_set_llp(struct ocelot_fdma_rx_ring *rx_ring)
224 {
225 	struct ocelot_fdma_dcb *dcb;
226 	unsigned int idx;
227 
228 	idx = ocelot_fdma_idx_prev(rx_ring->next_to_use,
229 				   OCELOT_FDMA_RX_RING_SIZE);
230 	dcb = &rx_ring->dcbs[idx];
231 	dcb->llp = 0;
232 }
233 
ocelot_fdma_rx_restart(struct ocelot * ocelot)234 static void ocelot_fdma_rx_restart(struct ocelot *ocelot)
235 {
236 	struct ocelot_fdma *fdma = ocelot->fdma;
237 	struct ocelot_fdma_rx_ring *rx_ring;
238 	const u8 chan = MSCC_FDMA_XTR_CHAN;
239 	dma_addr_t new_llp, dma_base;
240 	unsigned int idx;
241 	u32 llp_prev;
242 	int ret;
243 
244 	rx_ring = &fdma->rx_ring;
245 	ret = ocelot_fdma_wait_chan_safe(ocelot, chan);
246 	if (ret) {
247 		dev_err_ratelimited(ocelot->dev,
248 				    "Unable to stop RX channel\n");
249 		return;
250 	}
251 
252 	ocelot_fdma_rx_set_llp(rx_ring);
253 
254 	/* FDMA stopped on the last DCB that contained a NULL LLP, since
255 	 * we processed some DCBs in RX, there is free space, and  we must set
256 	 * DCB_LLP to point to the next DCB
257 	 */
258 	llp_prev = ocelot_fdma_readl(ocelot, MSCC_FDMA_DCB_LLP_PREV(chan));
259 	dma_base = rx_ring->dcbs_dma;
260 
261 	/* Get the next DMA addr located after LLP == NULL DCB */
262 	idx = ocelot_fdma_dma_idx(dma_base, llp_prev);
263 	idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE);
264 	new_llp = ocelot_fdma_idx_dma(dma_base, idx);
265 
266 	/* Finally reactivate the channel */
267 	ocelot_fdma_activate_chan(ocelot, new_llp, chan);
268 }
269 
ocelot_fdma_add_rx_frag(struct ocelot_fdma_rx_buf * rxb,u32 stat,struct sk_buff * skb,bool first)270 static bool ocelot_fdma_add_rx_frag(struct ocelot_fdma_rx_buf *rxb, u32 stat,
271 				    struct sk_buff *skb, bool first)
272 {
273 	int size = MSCC_FDMA_DCB_STAT_BLOCKL(stat);
274 	struct page *page = rxb->page;
275 
276 	if (likely(first)) {
277 		skb_put(skb, size);
278 	} else {
279 		skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page,
280 				rxb->page_offset, size, OCELOT_FDMA_RX_SIZE);
281 	}
282 
283 	/* Try to reuse page */
284 	if (unlikely(page_ref_count(page) != 1 || page_is_pfmemalloc(page)))
285 		return false;
286 
287 	/* Change offset to the other half */
288 	rxb->page_offset ^= OCELOT_FDMA_RX_SIZE;
289 
290 	page_ref_inc(page);
291 
292 	return true;
293 }
294 
ocelot_fdma_reuse_rx_page(struct ocelot * ocelot,struct ocelot_fdma_rx_buf * old_rxb)295 static void ocelot_fdma_reuse_rx_page(struct ocelot *ocelot,
296 				      struct ocelot_fdma_rx_buf *old_rxb)
297 {
298 	struct ocelot_fdma_rx_ring *rx_ring = &ocelot->fdma->rx_ring;
299 	struct ocelot_fdma_rx_buf *new_rxb;
300 
301 	new_rxb = &rx_ring->bufs[rx_ring->next_to_alloc];
302 	rx_ring->next_to_alloc = ocelot_fdma_idx_next(rx_ring->next_to_alloc,
303 						      OCELOT_FDMA_RX_RING_SIZE);
304 
305 	/* Copy page reference */
306 	*new_rxb = *old_rxb;
307 
308 	/* Sync for use by the device */
309 	dma_sync_single_range_for_device(ocelot->dev, old_rxb->dma_addr,
310 					 old_rxb->page_offset,
311 					 OCELOT_FDMA_RX_SIZE, DMA_FROM_DEVICE);
312 }
313 
ocelot_fdma_get_skb(struct ocelot * ocelot,u32 stat,struct ocelot_fdma_rx_buf * rxb,struct sk_buff * skb)314 static struct sk_buff *ocelot_fdma_get_skb(struct ocelot *ocelot, u32 stat,
315 					   struct ocelot_fdma_rx_buf *rxb,
316 					   struct sk_buff *skb)
317 {
318 	bool first = false;
319 
320 	/* Allocate skb head and data */
321 	if (likely(!skb)) {
322 		void *buff_addr = page_address(rxb->page) +
323 				  rxb->page_offset;
324 
325 		skb = build_skb(buff_addr, OCELOT_FDMA_SKBFRAG_SIZE);
326 		if (unlikely(!skb)) {
327 			dev_err_ratelimited(ocelot->dev,
328 					    "build_skb failed !\n");
329 			return NULL;
330 		}
331 		first = true;
332 	}
333 
334 	dma_sync_single_range_for_cpu(ocelot->dev, rxb->dma_addr,
335 				      rxb->page_offset, OCELOT_FDMA_RX_SIZE,
336 				      DMA_FROM_DEVICE);
337 
338 	if (ocelot_fdma_add_rx_frag(rxb, stat, skb, first)) {
339 		/* Reuse the free half of the page for the next_to_alloc DCB*/
340 		ocelot_fdma_reuse_rx_page(ocelot, rxb);
341 	} else {
342 		/* page cannot be reused, unmap it */
343 		dma_unmap_page(ocelot->dev, rxb->dma_addr, PAGE_SIZE,
344 			       DMA_FROM_DEVICE);
345 	}
346 
347 	/* clear rx buff content */
348 	rxb->page = NULL;
349 
350 	return skb;
351 }
352 
ocelot_fdma_receive_skb(struct ocelot * ocelot,struct sk_buff * skb)353 static bool ocelot_fdma_receive_skb(struct ocelot *ocelot, struct sk_buff *skb)
354 {
355 	struct net_device *ndev;
356 	void *xfh = skb->data;
357 	u64 timestamp;
358 	u64 src_port;
359 
360 	skb_pull(skb, OCELOT_TAG_LEN);
361 
362 	ocelot_xfh_get_src_port(xfh, &src_port);
363 	if (unlikely(src_port >= ocelot->num_phys_ports))
364 		return false;
365 
366 	ndev = ocelot_port_to_netdev(ocelot, src_port);
367 	if (unlikely(!ndev))
368 		return false;
369 
370 	if (pskb_trim(skb, skb->len - ETH_FCS_LEN))
371 		return false;
372 
373 	skb->dev = ndev;
374 	skb->protocol = eth_type_trans(skb, skb->dev);
375 	skb->dev->stats.rx_bytes += skb->len;
376 	skb->dev->stats.rx_packets++;
377 
378 	if (ocelot->ptp) {
379 		ocelot_xfh_get_rew_val(xfh, &timestamp);
380 		ocelot_ptp_rx_timestamp(ocelot, skb, timestamp);
381 	}
382 
383 	if (likely(!skb_defer_rx_timestamp(skb)))
384 		netif_receive_skb(skb);
385 
386 	return true;
387 }
388 
ocelot_fdma_rx_get(struct ocelot * ocelot,int budget)389 static int ocelot_fdma_rx_get(struct ocelot *ocelot, int budget)
390 {
391 	struct ocelot_fdma *fdma = ocelot->fdma;
392 	struct ocelot_fdma_rx_ring *rx_ring;
393 	struct ocelot_fdma_rx_buf *rxb;
394 	struct ocelot_fdma_dcb *dcb;
395 	struct sk_buff *skb;
396 	int work_done = 0;
397 	int cleaned_cnt;
398 	u32 stat;
399 	u16 idx;
400 
401 	cleaned_cnt = ocelot_fdma_rx_ring_free(fdma);
402 	rx_ring = &fdma->rx_ring;
403 	skb = rx_ring->skb;
404 
405 	while (budget--) {
406 		idx = rx_ring->next_to_clean;
407 		dcb = &rx_ring->dcbs[idx];
408 		stat = dcb->stat;
409 		if (MSCC_FDMA_DCB_STAT_BLOCKL(stat) == 0)
410 			break;
411 
412 		/* New packet is a start of frame but we already got a skb set,
413 		 * we probably lost an EOF packet, free skb
414 		 */
415 		if (unlikely(skb && (stat & MSCC_FDMA_DCB_STAT_SOF))) {
416 			dev_kfree_skb(skb);
417 			skb = NULL;
418 		}
419 
420 		rxb = &rx_ring->bufs[idx];
421 		/* Fetch next to clean buffer from the rx_ring */
422 		skb = ocelot_fdma_get_skb(ocelot, stat, rxb, skb);
423 		if (unlikely(!skb))
424 			break;
425 
426 		work_done++;
427 		cleaned_cnt++;
428 
429 		idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE);
430 		rx_ring->next_to_clean = idx;
431 
432 		if (unlikely(stat & MSCC_FDMA_DCB_STAT_ABORT ||
433 			     stat & MSCC_FDMA_DCB_STAT_PD)) {
434 			dev_err_ratelimited(ocelot->dev,
435 					    "DCB aborted or pruned\n");
436 			dev_kfree_skb(skb);
437 			skb = NULL;
438 			continue;
439 		}
440 
441 		/* We still need to process the other fragment of the packet
442 		 * before delivering it to the network stack
443 		 */
444 		if (!(stat & MSCC_FDMA_DCB_STAT_EOF))
445 			continue;
446 
447 		if (unlikely(!ocelot_fdma_receive_skb(ocelot, skb)))
448 			dev_kfree_skb(skb);
449 
450 		skb = NULL;
451 	}
452 
453 	rx_ring->skb = skb;
454 
455 	if (cleaned_cnt)
456 		ocelot_fdma_alloc_rx_buffs(ocelot, cleaned_cnt);
457 
458 	return work_done;
459 }
460 
ocelot_fdma_wakeup_netdev(struct ocelot * ocelot)461 static void ocelot_fdma_wakeup_netdev(struct ocelot *ocelot)
462 {
463 	struct ocelot_port_private *priv;
464 	struct ocelot_port *ocelot_port;
465 	struct net_device *dev;
466 	int port;
467 
468 	for (port = 0; port < ocelot->num_phys_ports; port++) {
469 		ocelot_port = ocelot->ports[port];
470 		if (!ocelot_port)
471 			continue;
472 		priv = container_of(ocelot_port, struct ocelot_port_private,
473 				    port);
474 		dev = priv->dev;
475 
476 		if (unlikely(netif_queue_stopped(dev)))
477 			netif_wake_queue(dev);
478 	}
479 }
480 
ocelot_fdma_tx_cleanup(struct ocelot * ocelot,int budget)481 static void ocelot_fdma_tx_cleanup(struct ocelot *ocelot, int budget)
482 {
483 	struct ocelot_fdma *fdma = ocelot->fdma;
484 	struct ocelot_fdma_tx_ring *tx_ring;
485 	struct ocelot_fdma_tx_buf *buf;
486 	unsigned int new_null_llp_idx;
487 	struct ocelot_fdma_dcb *dcb;
488 	bool end_of_list = false;
489 	struct sk_buff *skb;
490 	dma_addr_t dma;
491 	u32 dcb_llp;
492 	u16 ntc;
493 	int ret;
494 
495 	tx_ring = &fdma->tx_ring;
496 
497 	/* Purge the TX packets that have been sent up to the NULL llp or the
498 	 * end of done list.
499 	 */
500 	while (!ocelot_fdma_tx_ring_empty(fdma)) {
501 		ntc = tx_ring->next_to_clean;
502 		dcb = &tx_ring->dcbs[ntc];
503 		if (!(dcb->stat & MSCC_FDMA_DCB_STAT_PD))
504 			break;
505 
506 		buf = &tx_ring->bufs[ntc];
507 		skb = buf->skb;
508 		dma_unmap_single(ocelot->dev, dma_unmap_addr(buf, dma_addr),
509 				 skb->len, DMA_TO_DEVICE);
510 		napi_consume_skb(skb, budget);
511 		dcb_llp = dcb->llp;
512 
513 		/* Only update after accessing all dcb fields */
514 		tx_ring->next_to_clean = ocelot_fdma_idx_next(ntc,
515 							      OCELOT_FDMA_TX_RING_SIZE);
516 
517 		/* If we hit the NULL LLP, stop, we might need to reload FDMA */
518 		if (dcb_llp == 0) {
519 			end_of_list = true;
520 			break;
521 		}
522 	}
523 
524 	/* No need to try to wake if there were no TX cleaned_cnt up. */
525 	if (ocelot_fdma_tx_ring_free(fdma))
526 		ocelot_fdma_wakeup_netdev(ocelot);
527 
528 	/* If there is still some DCBs to be processed by the FDMA or if the
529 	 * pending list is empty, there is no need to restart the FDMA.
530 	 */
531 	if (!end_of_list || ocelot_fdma_tx_ring_empty(fdma))
532 		return;
533 
534 	ret = ocelot_fdma_wait_chan_safe(ocelot, MSCC_FDMA_INJ_CHAN);
535 	if (ret) {
536 		dev_warn(ocelot->dev,
537 			 "Failed to wait for TX channel to stop\n");
538 		return;
539 	}
540 
541 	/* Set NULL LLP to be the last DCB used */
542 	new_null_llp_idx = ocelot_fdma_idx_prev(tx_ring->next_to_use,
543 						OCELOT_FDMA_TX_RING_SIZE);
544 	dcb = &tx_ring->dcbs[new_null_llp_idx];
545 	dcb->llp = 0;
546 
547 	dma = ocelot_fdma_idx_dma(tx_ring->dcbs_dma, tx_ring->next_to_clean);
548 	ocelot_fdma_activate_chan(ocelot, dma, MSCC_FDMA_INJ_CHAN);
549 }
550 
ocelot_fdma_napi_poll(struct napi_struct * napi,int budget)551 static int ocelot_fdma_napi_poll(struct napi_struct *napi, int budget)
552 {
553 	struct ocelot_fdma *fdma = container_of(napi, struct ocelot_fdma, napi);
554 	struct ocelot *ocelot = fdma->ocelot;
555 	int work_done = 0;
556 	bool rx_stopped;
557 
558 	ocelot_fdma_tx_cleanup(ocelot, budget);
559 
560 	rx_stopped = ocelot_fdma_check_stop_rx(ocelot);
561 
562 	work_done = ocelot_fdma_rx_get(ocelot, budget);
563 
564 	if (rx_stopped)
565 		ocelot_fdma_rx_restart(ocelot);
566 
567 	if (work_done < budget) {
568 		napi_complete_done(&fdma->napi, work_done);
569 		ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA,
570 				   BIT(MSCC_FDMA_INJ_CHAN) |
571 				   BIT(MSCC_FDMA_XTR_CHAN));
572 	}
573 
574 	return work_done;
575 }
576 
ocelot_fdma_interrupt(int irq,void * dev_id)577 static irqreturn_t ocelot_fdma_interrupt(int irq, void *dev_id)
578 {
579 	u32 ident, llp, frm, err, err_code;
580 	struct ocelot *ocelot = dev_id;
581 
582 	ident = ocelot_fdma_readl(ocelot, MSCC_FDMA_INTR_IDENT);
583 	frm = ocelot_fdma_readl(ocelot, MSCC_FDMA_INTR_FRM);
584 	llp = ocelot_fdma_readl(ocelot, MSCC_FDMA_INTR_LLP);
585 
586 	ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_LLP, llp & ident);
587 	ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_FRM, frm & ident);
588 	if (frm || llp) {
589 		ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, 0);
590 		napi_schedule(&ocelot->fdma->napi);
591 	}
592 
593 	err = ocelot_fdma_readl(ocelot, MSCC_FDMA_EVT_ERR);
594 	if (unlikely(err)) {
595 		err_code = ocelot_fdma_readl(ocelot, MSCC_FDMA_EVT_ERR_CODE);
596 		dev_err_ratelimited(ocelot->dev,
597 				    "Error ! chans mask: %#x, code: %#x\n",
598 				    err, err_code);
599 
600 		ocelot_fdma_writel(ocelot, MSCC_FDMA_EVT_ERR, err);
601 		ocelot_fdma_writel(ocelot, MSCC_FDMA_EVT_ERR_CODE, err_code);
602 	}
603 
604 	return IRQ_HANDLED;
605 }
606 
ocelot_fdma_send_skb(struct ocelot * ocelot,struct ocelot_fdma * fdma,struct sk_buff * skb)607 static void ocelot_fdma_send_skb(struct ocelot *ocelot,
608 				 struct ocelot_fdma *fdma, struct sk_buff *skb)
609 {
610 	struct ocelot_fdma_tx_ring *tx_ring = &fdma->tx_ring;
611 	struct ocelot_fdma_tx_buf *tx_buf;
612 	struct ocelot_fdma_dcb *dcb;
613 	dma_addr_t dma;
614 	u16 next_idx;
615 
616 	dcb = &tx_ring->dcbs[tx_ring->next_to_use];
617 	tx_buf = &tx_ring->bufs[tx_ring->next_to_use];
618 	if (!ocelot_fdma_tx_dcb_set_skb(ocelot, tx_buf, dcb, skb)) {
619 		dev_kfree_skb_any(skb);
620 		return;
621 	}
622 
623 	next_idx = ocelot_fdma_idx_next(tx_ring->next_to_use,
624 					OCELOT_FDMA_TX_RING_SIZE);
625 	skb_tx_timestamp(skb);
626 
627 	/* If the FDMA TX chan is empty, then enqueue the DCB directly */
628 	if (ocelot_fdma_tx_ring_empty(fdma)) {
629 		dma = ocelot_fdma_idx_dma(tx_ring->dcbs_dma,
630 					  tx_ring->next_to_use);
631 		ocelot_fdma_activate_chan(ocelot, dma, MSCC_FDMA_INJ_CHAN);
632 	} else {
633 		/* Chain the DCBs */
634 		dcb->llp = ocelot_fdma_idx_dma(tx_ring->dcbs_dma, next_idx);
635 	}
636 
637 	tx_ring->next_to_use = next_idx;
638 }
639 
ocelot_fdma_prepare_skb(struct ocelot * ocelot,int port,u32 rew_op,struct sk_buff * skb,struct net_device * dev)640 static int ocelot_fdma_prepare_skb(struct ocelot *ocelot, int port, u32 rew_op,
641 				   struct sk_buff *skb, struct net_device *dev)
642 {
643 	int needed_headroom = max_t(int, OCELOT_TAG_LEN - skb_headroom(skb), 0);
644 	int needed_tailroom = max_t(int, ETH_FCS_LEN - skb_tailroom(skb), 0);
645 	void *ifh;
646 	int err;
647 
648 	if (unlikely(needed_headroom || needed_tailroom ||
649 		     skb_header_cloned(skb))) {
650 		err = pskb_expand_head(skb, needed_headroom, needed_tailroom,
651 				       GFP_ATOMIC);
652 		if (unlikely(err)) {
653 			dev_kfree_skb_any(skb);
654 			return 1;
655 		}
656 	}
657 
658 	err = skb_linearize(skb);
659 	if (err) {
660 		net_err_ratelimited("%s: skb_linearize error (%d)!\n",
661 				    dev->name, err);
662 		dev_kfree_skb_any(skb);
663 		return 1;
664 	}
665 
666 	ifh = skb_push(skb, OCELOT_TAG_LEN);
667 	skb_put(skb, ETH_FCS_LEN);
668 	memset(ifh, 0, OCELOT_TAG_LEN);
669 	ocelot_ifh_port_set(ifh, port, rew_op, skb_vlan_tag_get(skb));
670 
671 	return 0;
672 }
673 
ocelot_fdma_inject_frame(struct ocelot * ocelot,int port,u32 rew_op,struct sk_buff * skb,struct net_device * dev)674 int ocelot_fdma_inject_frame(struct ocelot *ocelot, int port, u32 rew_op,
675 			     struct sk_buff *skb, struct net_device *dev)
676 {
677 	struct ocelot_fdma *fdma = ocelot->fdma;
678 	int ret = NETDEV_TX_OK;
679 
680 	spin_lock(&fdma->tx_ring.xmit_lock);
681 
682 	if (ocelot_fdma_tx_ring_free(fdma) == 0) {
683 		netif_stop_queue(dev);
684 		ret = NETDEV_TX_BUSY;
685 		goto out;
686 	}
687 
688 	if (ocelot_fdma_prepare_skb(ocelot, port, rew_op, skb, dev))
689 		goto out;
690 
691 	ocelot_fdma_send_skb(ocelot, fdma, skb);
692 
693 out:
694 	spin_unlock(&fdma->tx_ring.xmit_lock);
695 
696 	return ret;
697 }
698 
ocelot_fdma_free_rx_ring(struct ocelot * ocelot)699 static void ocelot_fdma_free_rx_ring(struct ocelot *ocelot)
700 {
701 	struct ocelot_fdma *fdma = ocelot->fdma;
702 	struct ocelot_fdma_rx_ring *rx_ring;
703 	struct ocelot_fdma_rx_buf *rxb;
704 	u16 idx;
705 
706 	rx_ring = &fdma->rx_ring;
707 	idx = rx_ring->next_to_clean;
708 
709 	/* Free the pages held in the RX ring */
710 	while (idx != rx_ring->next_to_use) {
711 		rxb = &rx_ring->bufs[idx];
712 		dma_unmap_page(ocelot->dev, rxb->dma_addr, PAGE_SIZE,
713 			       DMA_FROM_DEVICE);
714 		__free_page(rxb->page);
715 		idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE);
716 	}
717 
718 	if (fdma->rx_ring.skb)
719 		dev_kfree_skb_any(fdma->rx_ring.skb);
720 }
721 
ocelot_fdma_free_tx_ring(struct ocelot * ocelot)722 static void ocelot_fdma_free_tx_ring(struct ocelot *ocelot)
723 {
724 	struct ocelot_fdma *fdma = ocelot->fdma;
725 	struct ocelot_fdma_tx_ring *tx_ring;
726 	struct ocelot_fdma_tx_buf *txb;
727 	struct sk_buff *skb;
728 	u16 idx;
729 
730 	tx_ring = &fdma->tx_ring;
731 	idx = tx_ring->next_to_clean;
732 
733 	while (idx != tx_ring->next_to_use) {
734 		txb = &tx_ring->bufs[idx];
735 		skb = txb->skb;
736 		dma_unmap_single(ocelot->dev, dma_unmap_addr(txb, dma_addr),
737 				 skb->len, DMA_TO_DEVICE);
738 		dev_kfree_skb_any(skb);
739 		idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_TX_RING_SIZE);
740 	}
741 }
742 
ocelot_fdma_rings_alloc(struct ocelot * ocelot)743 static int ocelot_fdma_rings_alloc(struct ocelot *ocelot)
744 {
745 	struct ocelot_fdma *fdma = ocelot->fdma;
746 	struct ocelot_fdma_dcb *dcbs;
747 	unsigned int adjust;
748 	dma_addr_t dcbs_dma;
749 	int ret;
750 
751 	/* Create a pool of consistent memory blocks for hardware descriptors */
752 	fdma->dcbs_base = dmam_alloc_coherent(ocelot->dev,
753 					      OCELOT_DCBS_HW_ALLOC_SIZE,
754 					      &fdma->dcbs_dma_base, GFP_KERNEL);
755 	if (!fdma->dcbs_base)
756 		return -ENOMEM;
757 
758 	/* DCBs must be aligned on a 32bit boundary */
759 	dcbs = fdma->dcbs_base;
760 	dcbs_dma = fdma->dcbs_dma_base;
761 	if (!IS_ALIGNED(dcbs_dma, 4)) {
762 		adjust = dcbs_dma & 0x3;
763 		dcbs_dma = ALIGN(dcbs_dma, 4);
764 		dcbs = (void *)dcbs + adjust;
765 	}
766 
767 	/* TX queue */
768 	fdma->tx_ring.dcbs = dcbs;
769 	fdma->tx_ring.dcbs_dma = dcbs_dma;
770 	spin_lock_init(&fdma->tx_ring.xmit_lock);
771 
772 	/* RX queue */
773 	fdma->rx_ring.dcbs = dcbs + OCELOT_FDMA_TX_RING_SIZE;
774 	fdma->rx_ring.dcbs_dma = dcbs_dma + OCELOT_FDMA_TX_DCB_SIZE;
775 	ret = ocelot_fdma_alloc_rx_buffs(ocelot,
776 					 ocelot_fdma_tx_ring_free(fdma));
777 	if (ret) {
778 		ocelot_fdma_free_rx_ring(ocelot);
779 		return ret;
780 	}
781 
782 	/* Set the last DCB LLP as NULL, this is normally done when restarting
783 	 * the RX chan, but this is for the first run
784 	 */
785 	ocelot_fdma_rx_set_llp(&fdma->rx_ring);
786 
787 	return 0;
788 }
789 
ocelot_fdma_netdev_init(struct ocelot * ocelot,struct net_device * dev)790 void ocelot_fdma_netdev_init(struct ocelot *ocelot, struct net_device *dev)
791 {
792 	struct ocelot_fdma *fdma = ocelot->fdma;
793 
794 	dev->needed_headroom = OCELOT_TAG_LEN;
795 	dev->needed_tailroom = ETH_FCS_LEN;
796 
797 	if (fdma->ndev)
798 		return;
799 
800 	fdma->ndev = dev;
801 	netif_napi_add_weight(dev, &fdma->napi, ocelot_fdma_napi_poll,
802 			      OCELOT_FDMA_WEIGHT);
803 }
804 
ocelot_fdma_netdev_deinit(struct ocelot * ocelot,struct net_device * dev)805 void ocelot_fdma_netdev_deinit(struct ocelot *ocelot, struct net_device *dev)
806 {
807 	struct ocelot_fdma *fdma = ocelot->fdma;
808 
809 	if (fdma->ndev == dev) {
810 		netif_napi_del(&fdma->napi);
811 		fdma->ndev = NULL;
812 	}
813 }
814 
ocelot_fdma_init(struct platform_device * pdev,struct ocelot * ocelot)815 void ocelot_fdma_init(struct platform_device *pdev, struct ocelot *ocelot)
816 {
817 	struct device *dev = ocelot->dev;
818 	struct ocelot_fdma *fdma;
819 	int ret;
820 
821 	fdma = devm_kzalloc(dev, sizeof(*fdma), GFP_KERNEL);
822 	if (!fdma)
823 		return;
824 
825 	ocelot->fdma = fdma;
826 	ocelot->dev->coherent_dma_mask = DMA_BIT_MASK(32);
827 
828 	ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, 0);
829 
830 	fdma->ocelot = ocelot;
831 	fdma->irq = platform_get_irq_byname(pdev, "fdma");
832 	ret = devm_request_irq(dev, fdma->irq, ocelot_fdma_interrupt, 0,
833 			       dev_name(dev), ocelot);
834 	if (ret)
835 		goto err_free_fdma;
836 
837 	ret = ocelot_fdma_rings_alloc(ocelot);
838 	if (ret)
839 		goto err_free_irq;
840 
841 	static_branch_enable(&ocelot_fdma_enabled);
842 
843 	return;
844 
845 err_free_irq:
846 	devm_free_irq(dev, fdma->irq, fdma);
847 err_free_fdma:
848 	devm_kfree(dev, fdma);
849 
850 	ocelot->fdma = NULL;
851 }
852 
ocelot_fdma_start(struct ocelot * ocelot)853 void ocelot_fdma_start(struct ocelot *ocelot)
854 {
855 	struct ocelot_fdma *fdma = ocelot->fdma;
856 
857 	/* Reconfigure for extraction and injection using DMA */
858 	ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_MODE(2), QS_INJ_GRP_CFG, 0);
859 	ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(0), QS_INJ_CTRL, 0);
860 
861 	ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_MODE(2), QS_XTR_GRP_CFG, 0);
862 
863 	ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_LLP, 0xffffffff);
864 	ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_FRM, 0xffffffff);
865 
866 	ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_LLP_ENA,
867 			   BIT(MSCC_FDMA_INJ_CHAN) | BIT(MSCC_FDMA_XTR_CHAN));
868 	ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_FRM_ENA,
869 			   BIT(MSCC_FDMA_XTR_CHAN));
870 	ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA,
871 			   BIT(MSCC_FDMA_INJ_CHAN) | BIT(MSCC_FDMA_XTR_CHAN));
872 
873 	napi_enable(&fdma->napi);
874 
875 	ocelot_fdma_activate_chan(ocelot, ocelot->fdma->rx_ring.dcbs_dma,
876 				  MSCC_FDMA_XTR_CHAN);
877 }
878 
ocelot_fdma_deinit(struct ocelot * ocelot)879 void ocelot_fdma_deinit(struct ocelot *ocelot)
880 {
881 	struct ocelot_fdma *fdma = ocelot->fdma;
882 
883 	ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, 0);
884 	ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_FORCEDIS,
885 			   BIT(MSCC_FDMA_XTR_CHAN));
886 	ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_FORCEDIS,
887 			   BIT(MSCC_FDMA_INJ_CHAN));
888 	napi_synchronize(&fdma->napi);
889 	napi_disable(&fdma->napi);
890 
891 	ocelot_fdma_free_rx_ring(ocelot);
892 	ocelot_fdma_free_tx_ring(ocelot);
893 }
894