Loading...
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
20DEFINE_STATIC_KEY_FALSE(ocelot_fdma_enabled);
21
22static void ocelot_fdma_writel(struct ocelot *ocelot, u32 reg, u32 data)
23{
24 regmap_write(ocelot->targets[FDMA], reg, data);
25}
26
27static 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
36static 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
41static 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
46static u16 ocelot_fdma_idx_next(u16 idx, u16 ring_sz)
47{
48 return unlikely(idx == ring_sz - 1) ? 0 : idx + 1;
49}
50
51static u16 ocelot_fdma_idx_prev(u16 idx, u16 ring_sz)
52{
53 return unlikely(idx == 0) ? ring_sz - 1 : idx - 1;
54}
55
56static 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
67static 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
78static 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
85static 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
96static u32 ocelot_fdma_read_ch_safe(struct ocelot *ocelot)
97{
98 return ocelot_fdma_readl(ocelot, MSCC_FDMA_CH_SAFE);
99}
100
101static 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
110static 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
122static 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
146static 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
186static 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
208static 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
223static 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
234static 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
270static 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
295static 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
314static 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
353static 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, ×tamp);
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
389static 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
461static 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
481static 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
551static 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
577static 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
607static 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
640static 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
674int 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
693out:
694 spin_unlock(&fdma->tx_ring.xmit_lock);
695
696 return ret;
697}
698
699static 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
722static 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
743static 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
790void 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
805void 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
815void 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
845err_free_irq:
846 devm_free_irq(dev, fdma->irq, fdma);
847err_free_fdma:
848 devm_kfree(dev, fdma);
849
850 ocelot->fdma = NULL;
851}
852
853void 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
879void 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}
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/of_platform.h>
16#include <linux/skbuff.h>
17
18#include "ocelot_fdma.h"
19#include "ocelot_qs.h"
20
21DEFINE_STATIC_KEY_FALSE(ocelot_fdma_enabled);
22
23static void ocelot_fdma_writel(struct ocelot *ocelot, u32 reg, u32 data)
24{
25 regmap_write(ocelot->targets[FDMA], reg, data);
26}
27
28static u32 ocelot_fdma_readl(struct ocelot *ocelot, u32 reg)
29{
30 u32 retval;
31
32 regmap_read(ocelot->targets[FDMA], reg, &retval);
33
34 return retval;
35}
36
37static dma_addr_t ocelot_fdma_idx_dma(dma_addr_t base, u16 idx)
38{
39 return base + idx * sizeof(struct ocelot_fdma_dcb);
40}
41
42static u16 ocelot_fdma_dma_idx(dma_addr_t base, dma_addr_t dma)
43{
44 return (dma - base) / sizeof(struct ocelot_fdma_dcb);
45}
46
47static u16 ocelot_fdma_idx_next(u16 idx, u16 ring_sz)
48{
49 return unlikely(idx == ring_sz - 1) ? 0 : idx + 1;
50}
51
52static u16 ocelot_fdma_idx_prev(u16 idx, u16 ring_sz)
53{
54 return unlikely(idx == 0) ? ring_sz - 1 : idx - 1;
55}
56
57static int ocelot_fdma_rx_ring_free(struct ocelot_fdma *fdma)
58{
59 struct ocelot_fdma_rx_ring *rx_ring = &fdma->rx_ring;
60
61 if (rx_ring->next_to_use >= rx_ring->next_to_clean)
62 return OCELOT_FDMA_RX_RING_SIZE -
63 (rx_ring->next_to_use - rx_ring->next_to_clean) - 1;
64 else
65 return rx_ring->next_to_clean - rx_ring->next_to_use - 1;
66}
67
68static int ocelot_fdma_tx_ring_free(struct ocelot_fdma *fdma)
69{
70 struct ocelot_fdma_tx_ring *tx_ring = &fdma->tx_ring;
71
72 if (tx_ring->next_to_use >= tx_ring->next_to_clean)
73 return OCELOT_FDMA_TX_RING_SIZE -
74 (tx_ring->next_to_use - tx_ring->next_to_clean) - 1;
75 else
76 return tx_ring->next_to_clean - tx_ring->next_to_use - 1;
77}
78
79static bool ocelot_fdma_tx_ring_empty(struct ocelot_fdma *fdma)
80{
81 struct ocelot_fdma_tx_ring *tx_ring = &fdma->tx_ring;
82
83 return tx_ring->next_to_clean == tx_ring->next_to_use;
84}
85
86static void ocelot_fdma_activate_chan(struct ocelot *ocelot, dma_addr_t dma,
87 int chan)
88{
89 ocelot_fdma_writel(ocelot, MSCC_FDMA_DCB_LLP(chan), dma);
90 /* Barrier to force memory writes to DCB to be completed before starting
91 * the channel.
92 */
93 wmb();
94 ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_ACTIVATE, BIT(chan));
95}
96
97static u32 ocelot_fdma_read_ch_safe(struct ocelot *ocelot)
98{
99 return ocelot_fdma_readl(ocelot, MSCC_FDMA_CH_SAFE);
100}
101
102static int ocelot_fdma_wait_chan_safe(struct ocelot *ocelot, int chan)
103{
104 u32 safe;
105
106 return readx_poll_timeout_atomic(ocelot_fdma_read_ch_safe, ocelot, safe,
107 safe & BIT(chan), 0,
108 OCELOT_FDMA_CH_SAFE_TIMEOUT_US);
109}
110
111static void ocelot_fdma_dcb_set_data(struct ocelot_fdma_dcb *dcb,
112 dma_addr_t dma_addr,
113 size_t size)
114{
115 u32 offset = dma_addr & 0x3;
116
117 dcb->llp = 0;
118 dcb->datap = ALIGN_DOWN(dma_addr, 4);
119 dcb->datal = ALIGN_DOWN(size, 4);
120 dcb->stat = MSCC_FDMA_DCB_STAT_BLOCKO(offset);
121}
122
123static bool ocelot_fdma_rx_alloc_page(struct ocelot *ocelot,
124 struct ocelot_fdma_rx_buf *rxb)
125{
126 dma_addr_t mapping;
127 struct page *page;
128
129 page = dev_alloc_page();
130 if (unlikely(!page))
131 return false;
132
133 mapping = dma_map_page(ocelot->dev, page, 0, PAGE_SIZE,
134 DMA_FROM_DEVICE);
135 if (unlikely(dma_mapping_error(ocelot->dev, mapping))) {
136 __free_page(page);
137 return false;
138 }
139
140 rxb->page = page;
141 rxb->page_offset = 0;
142 rxb->dma_addr = mapping;
143
144 return true;
145}
146
147static int ocelot_fdma_alloc_rx_buffs(struct ocelot *ocelot, u16 alloc_cnt)
148{
149 struct ocelot_fdma *fdma = ocelot->fdma;
150 struct ocelot_fdma_rx_ring *rx_ring;
151 struct ocelot_fdma_rx_buf *rxb;
152 struct ocelot_fdma_dcb *dcb;
153 dma_addr_t dma_addr;
154 int ret = 0;
155 u16 idx;
156
157 rx_ring = &fdma->rx_ring;
158 idx = rx_ring->next_to_use;
159
160 while (alloc_cnt--) {
161 rxb = &rx_ring->bufs[idx];
162 /* try reuse page */
163 if (unlikely(!rxb->page)) {
164 if (unlikely(!ocelot_fdma_rx_alloc_page(ocelot, rxb))) {
165 dev_err_ratelimited(ocelot->dev,
166 "Failed to allocate rx\n");
167 ret = -ENOMEM;
168 break;
169 }
170 }
171
172 dcb = &rx_ring->dcbs[idx];
173 dma_addr = rxb->dma_addr + rxb->page_offset;
174 ocelot_fdma_dcb_set_data(dcb, dma_addr, OCELOT_FDMA_RXB_SIZE);
175
176 idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE);
177 /* Chain the DCB to the next one */
178 dcb->llp = ocelot_fdma_idx_dma(rx_ring->dcbs_dma, idx);
179 }
180
181 rx_ring->next_to_use = idx;
182 rx_ring->next_to_alloc = idx;
183
184 return ret;
185}
186
187static bool ocelot_fdma_tx_dcb_set_skb(struct ocelot *ocelot,
188 struct ocelot_fdma_tx_buf *tx_buf,
189 struct ocelot_fdma_dcb *dcb,
190 struct sk_buff *skb)
191{
192 dma_addr_t mapping;
193
194 mapping = dma_map_single(ocelot->dev, skb->data, skb->len,
195 DMA_TO_DEVICE);
196 if (unlikely(dma_mapping_error(ocelot->dev, mapping)))
197 return false;
198
199 dma_unmap_addr_set(tx_buf, dma_addr, mapping);
200
201 ocelot_fdma_dcb_set_data(dcb, mapping, OCELOT_FDMA_RX_SIZE);
202 tx_buf->skb = skb;
203 dcb->stat |= MSCC_FDMA_DCB_STAT_BLOCKL(skb->len);
204 dcb->stat |= MSCC_FDMA_DCB_STAT_SOF | MSCC_FDMA_DCB_STAT_EOF;
205
206 return true;
207}
208
209static bool ocelot_fdma_check_stop_rx(struct ocelot *ocelot)
210{
211 u32 llp;
212
213 /* Check if the FDMA hits the DCB with LLP == NULL */
214 llp = ocelot_fdma_readl(ocelot, MSCC_FDMA_DCB_LLP(MSCC_FDMA_XTR_CHAN));
215 if (unlikely(llp))
216 return false;
217
218 ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_DISABLE,
219 BIT(MSCC_FDMA_XTR_CHAN));
220
221 return true;
222}
223
224static void ocelot_fdma_rx_set_llp(struct ocelot_fdma_rx_ring *rx_ring)
225{
226 struct ocelot_fdma_dcb *dcb;
227 unsigned int idx;
228
229 idx = ocelot_fdma_idx_prev(rx_ring->next_to_use,
230 OCELOT_FDMA_RX_RING_SIZE);
231 dcb = &rx_ring->dcbs[idx];
232 dcb->llp = 0;
233}
234
235static void ocelot_fdma_rx_restart(struct ocelot *ocelot)
236{
237 struct ocelot_fdma *fdma = ocelot->fdma;
238 struct ocelot_fdma_rx_ring *rx_ring;
239 const u8 chan = MSCC_FDMA_XTR_CHAN;
240 dma_addr_t new_llp, dma_base;
241 unsigned int idx;
242 u32 llp_prev;
243 int ret;
244
245 rx_ring = &fdma->rx_ring;
246 ret = ocelot_fdma_wait_chan_safe(ocelot, chan);
247 if (ret) {
248 dev_err_ratelimited(ocelot->dev,
249 "Unable to stop RX channel\n");
250 return;
251 }
252
253 ocelot_fdma_rx_set_llp(rx_ring);
254
255 /* FDMA stopped on the last DCB that contained a NULL LLP, since
256 * we processed some DCBs in RX, there is free space, and we must set
257 * DCB_LLP to point to the next DCB
258 */
259 llp_prev = ocelot_fdma_readl(ocelot, MSCC_FDMA_DCB_LLP_PREV(chan));
260 dma_base = rx_ring->dcbs_dma;
261
262 /* Get the next DMA addr located after LLP == NULL DCB */
263 idx = ocelot_fdma_dma_idx(dma_base, llp_prev);
264 idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE);
265 new_llp = ocelot_fdma_idx_dma(dma_base, idx);
266
267 /* Finally reactivate the channel */
268 ocelot_fdma_activate_chan(ocelot, new_llp, chan);
269}
270
271static bool ocelot_fdma_add_rx_frag(struct ocelot_fdma_rx_buf *rxb, u32 stat,
272 struct sk_buff *skb, bool first)
273{
274 int size = MSCC_FDMA_DCB_STAT_BLOCKL(stat);
275 struct page *page = rxb->page;
276
277 if (likely(first)) {
278 skb_put(skb, size);
279 } else {
280 skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page,
281 rxb->page_offset, size, OCELOT_FDMA_RX_SIZE);
282 }
283
284 /* Try to reuse page */
285 if (unlikely(page_ref_count(page) != 1 || page_is_pfmemalloc(page)))
286 return false;
287
288 /* Change offset to the other half */
289 rxb->page_offset ^= OCELOT_FDMA_RX_SIZE;
290
291 page_ref_inc(page);
292
293 return true;
294}
295
296static void ocelot_fdma_reuse_rx_page(struct ocelot *ocelot,
297 struct ocelot_fdma_rx_buf *old_rxb)
298{
299 struct ocelot_fdma_rx_ring *rx_ring = &ocelot->fdma->rx_ring;
300 struct ocelot_fdma_rx_buf *new_rxb;
301
302 new_rxb = &rx_ring->bufs[rx_ring->next_to_alloc];
303 rx_ring->next_to_alloc = ocelot_fdma_idx_next(rx_ring->next_to_alloc,
304 OCELOT_FDMA_RX_RING_SIZE);
305
306 /* Copy page reference */
307 *new_rxb = *old_rxb;
308
309 /* Sync for use by the device */
310 dma_sync_single_range_for_device(ocelot->dev, old_rxb->dma_addr,
311 old_rxb->page_offset,
312 OCELOT_FDMA_RX_SIZE, DMA_FROM_DEVICE);
313}
314
315static struct sk_buff *ocelot_fdma_get_skb(struct ocelot *ocelot, u32 stat,
316 struct ocelot_fdma_rx_buf *rxb,
317 struct sk_buff *skb)
318{
319 bool first = false;
320
321 /* Allocate skb head and data */
322 if (likely(!skb)) {
323 void *buff_addr = page_address(rxb->page) +
324 rxb->page_offset;
325
326 skb = build_skb(buff_addr, OCELOT_FDMA_SKBFRAG_SIZE);
327 if (unlikely(!skb)) {
328 dev_err_ratelimited(ocelot->dev,
329 "build_skb failed !\n");
330 return NULL;
331 }
332 first = true;
333 }
334
335 dma_sync_single_range_for_cpu(ocelot->dev, rxb->dma_addr,
336 rxb->page_offset, OCELOT_FDMA_RX_SIZE,
337 DMA_FROM_DEVICE);
338
339 if (ocelot_fdma_add_rx_frag(rxb, stat, skb, first)) {
340 /* Reuse the free half of the page for the next_to_alloc DCB*/
341 ocelot_fdma_reuse_rx_page(ocelot, rxb);
342 } else {
343 /* page cannot be reused, unmap it */
344 dma_unmap_page(ocelot->dev, rxb->dma_addr, PAGE_SIZE,
345 DMA_FROM_DEVICE);
346 }
347
348 /* clear rx buff content */
349 rxb->page = NULL;
350
351 return skb;
352}
353
354static bool ocelot_fdma_receive_skb(struct ocelot *ocelot, struct sk_buff *skb)
355{
356 struct net_device *ndev;
357 void *xfh = skb->data;
358 u64 timestamp;
359 u64 src_port;
360
361 skb_pull(skb, OCELOT_TAG_LEN);
362
363 ocelot_xfh_get_src_port(xfh, &src_port);
364 if (unlikely(src_port >= ocelot->num_phys_ports))
365 return false;
366
367 ndev = ocelot_port_to_netdev(ocelot, src_port);
368 if (unlikely(!ndev))
369 return false;
370
371 pskb_trim(skb, skb->len - ETH_FCS_LEN);
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, ×tamp);
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
389static 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
461static 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
481static 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
551static 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
577static 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
607static 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
640static 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
674int 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
693out:
694 spin_unlock(&fdma->tx_ring.xmit_lock);
695
696 return ret;
697}
698
699static 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
722static 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
743static 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
790void 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
805void 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
815void 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
845err_free_irq:
846 devm_free_irq(dev, fdma->irq, fdma);
847err_free_fdma:
848 devm_kfree(dev, fdma);
849
850 ocelot->fdma = NULL;
851}
852
853void 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
879void 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}