diff options
Diffstat (limited to 'drivers/net/ethernet/mscc/ocelot_fdma.c')
-rw-r--r-- | drivers/net/ethernet/mscc/ocelot_fdma.c | 893 |
1 files changed, 893 insertions, 0 deletions
diff --git a/drivers/net/ethernet/mscc/ocelot_fdma.c b/drivers/net/ethernet/mscc/ocelot_fdma.c new file mode 100644 index 000000000..8e3894cf5 --- /dev/null +++ b/drivers/net/ethernet/mscc/ocelot_fdma.c @@ -0,0 +1,893 @@ +// SPDX-License-Identifier: (GPL-2.0 OR MIT) +/* + * Microsemi SoCs FDMA driver + * + * Copyright (c) 2021 Microchip + * + * Page recycling code is mostly taken from gianfar driver. + */ + +#include <linux/align.h> +#include <linux/bitops.h> +#include <linux/dmapool.h> +#include <linux/dsa/ocelot.h> +#include <linux/netdevice.h> +#include <linux/of_platform.h> +#include <linux/skbuff.h> + +#include "ocelot_fdma.h" +#include "ocelot_qs.h" + +DEFINE_STATIC_KEY_FALSE(ocelot_fdma_enabled); + +static void ocelot_fdma_writel(struct ocelot *ocelot, u32 reg, u32 data) +{ + regmap_write(ocelot->targets[FDMA], reg, data); +} + +static u32 ocelot_fdma_readl(struct ocelot *ocelot, u32 reg) +{ + u32 retval; + + regmap_read(ocelot->targets[FDMA], reg, &retval); + + return retval; +} + +static dma_addr_t ocelot_fdma_idx_dma(dma_addr_t base, u16 idx) +{ + return base + idx * sizeof(struct ocelot_fdma_dcb); +} + +static u16 ocelot_fdma_dma_idx(dma_addr_t base, dma_addr_t dma) +{ + return (dma - base) / sizeof(struct ocelot_fdma_dcb); +} + +static u16 ocelot_fdma_idx_next(u16 idx, u16 ring_sz) +{ + return unlikely(idx == ring_sz - 1) ? 0 : idx + 1; +} + +static u16 ocelot_fdma_idx_prev(u16 idx, u16 ring_sz) +{ + return unlikely(idx == 0) ? ring_sz - 1 : idx - 1; +} + +static int ocelot_fdma_rx_ring_free(struct ocelot_fdma *fdma) +{ + struct ocelot_fdma_rx_ring *rx_ring = &fdma->rx_ring; + + if (rx_ring->next_to_use >= rx_ring->next_to_clean) + return OCELOT_FDMA_RX_RING_SIZE - + (rx_ring->next_to_use - rx_ring->next_to_clean) - 1; + else + return rx_ring->next_to_clean - rx_ring->next_to_use - 1; +} + +static int ocelot_fdma_tx_ring_free(struct ocelot_fdma *fdma) +{ + struct ocelot_fdma_tx_ring *tx_ring = &fdma->tx_ring; + + if (tx_ring->next_to_use >= tx_ring->next_to_clean) + return OCELOT_FDMA_TX_RING_SIZE - + (tx_ring->next_to_use - tx_ring->next_to_clean) - 1; + else + return tx_ring->next_to_clean - tx_ring->next_to_use - 1; +} + +static bool ocelot_fdma_tx_ring_empty(struct ocelot_fdma *fdma) +{ + struct ocelot_fdma_tx_ring *tx_ring = &fdma->tx_ring; + + return tx_ring->next_to_clean == tx_ring->next_to_use; +} + +static void ocelot_fdma_activate_chan(struct ocelot *ocelot, dma_addr_t dma, + int chan) +{ + ocelot_fdma_writel(ocelot, MSCC_FDMA_DCB_LLP(chan), dma); + /* Barrier to force memory writes to DCB to be completed before starting + * the channel. + */ + wmb(); + ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_ACTIVATE, BIT(chan)); +} + +static u32 ocelot_fdma_read_ch_safe(struct ocelot *ocelot) +{ + return ocelot_fdma_readl(ocelot, MSCC_FDMA_CH_SAFE); +} + +static int ocelot_fdma_wait_chan_safe(struct ocelot *ocelot, int chan) +{ + u32 safe; + + return readx_poll_timeout_atomic(ocelot_fdma_read_ch_safe, ocelot, safe, + safe & BIT(chan), 0, + OCELOT_FDMA_CH_SAFE_TIMEOUT_US); +} + +static void ocelot_fdma_dcb_set_data(struct ocelot_fdma_dcb *dcb, + dma_addr_t dma_addr, + size_t size) +{ + u32 offset = dma_addr & 0x3; + + dcb->llp = 0; + dcb->datap = ALIGN_DOWN(dma_addr, 4); + dcb->datal = ALIGN_DOWN(size, 4); + dcb->stat = MSCC_FDMA_DCB_STAT_BLOCKO(offset); +} + +static bool ocelot_fdma_rx_alloc_page(struct ocelot *ocelot, + struct ocelot_fdma_rx_buf *rxb) +{ + dma_addr_t mapping; + struct page *page; + + page = dev_alloc_page(); + if (unlikely(!page)) + return false; + + mapping = dma_map_page(ocelot->dev, page, 0, PAGE_SIZE, + DMA_FROM_DEVICE); + if (unlikely(dma_mapping_error(ocelot->dev, mapping))) { + __free_page(page); + return false; + } + + rxb->page = page; + rxb->page_offset = 0; + rxb->dma_addr = mapping; + + return true; +} + +static int ocelot_fdma_alloc_rx_buffs(struct ocelot *ocelot, u16 alloc_cnt) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + struct ocelot_fdma_rx_ring *rx_ring; + struct ocelot_fdma_rx_buf *rxb; + struct ocelot_fdma_dcb *dcb; + dma_addr_t dma_addr; + int ret = 0; + u16 idx; + + rx_ring = &fdma->rx_ring; + idx = rx_ring->next_to_use; + + while (alloc_cnt--) { + rxb = &rx_ring->bufs[idx]; + /* try reuse page */ + if (unlikely(!rxb->page)) { + if (unlikely(!ocelot_fdma_rx_alloc_page(ocelot, rxb))) { + dev_err_ratelimited(ocelot->dev, + "Failed to allocate rx\n"); + ret = -ENOMEM; + break; + } + } + + dcb = &rx_ring->dcbs[idx]; + dma_addr = rxb->dma_addr + rxb->page_offset; + ocelot_fdma_dcb_set_data(dcb, dma_addr, OCELOT_FDMA_RXB_SIZE); + + idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE); + /* Chain the DCB to the next one */ + dcb->llp = ocelot_fdma_idx_dma(rx_ring->dcbs_dma, idx); + } + + rx_ring->next_to_use = idx; + rx_ring->next_to_alloc = idx; + + return ret; +} + +static bool 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) +{ + dma_addr_t mapping; + + mapping = dma_map_single(ocelot->dev, skb->data, skb->len, + DMA_TO_DEVICE); + if (unlikely(dma_mapping_error(ocelot->dev, mapping))) + return false; + + dma_unmap_addr_set(tx_buf, dma_addr, mapping); + + ocelot_fdma_dcb_set_data(dcb, mapping, OCELOT_FDMA_RX_SIZE); + tx_buf->skb = skb; + dcb->stat |= MSCC_FDMA_DCB_STAT_BLOCKL(skb->len); + dcb->stat |= MSCC_FDMA_DCB_STAT_SOF | MSCC_FDMA_DCB_STAT_EOF; + + return true; +} + +static bool ocelot_fdma_check_stop_rx(struct ocelot *ocelot) +{ + u32 llp; + + /* Check if the FDMA hits the DCB with LLP == NULL */ + llp = ocelot_fdma_readl(ocelot, MSCC_FDMA_DCB_LLP(MSCC_FDMA_XTR_CHAN)); + if (unlikely(llp)) + return false; + + ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_DISABLE, + BIT(MSCC_FDMA_XTR_CHAN)); + + return true; +} + +static void ocelot_fdma_rx_set_llp(struct ocelot_fdma_rx_ring *rx_ring) +{ + struct ocelot_fdma_dcb *dcb; + unsigned int idx; + + idx = ocelot_fdma_idx_prev(rx_ring->next_to_use, + OCELOT_FDMA_RX_RING_SIZE); + dcb = &rx_ring->dcbs[idx]; + dcb->llp = 0; +} + +static void ocelot_fdma_rx_restart(struct ocelot *ocelot) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + struct ocelot_fdma_rx_ring *rx_ring; + const u8 chan = MSCC_FDMA_XTR_CHAN; + dma_addr_t new_llp, dma_base; + unsigned int idx; + u32 llp_prev; + int ret; + + rx_ring = &fdma->rx_ring; + ret = ocelot_fdma_wait_chan_safe(ocelot, chan); + if (ret) { + dev_err_ratelimited(ocelot->dev, + "Unable to stop RX channel\n"); + return; + } + + ocelot_fdma_rx_set_llp(rx_ring); + + /* FDMA stopped on the last DCB that contained a NULL LLP, since + * we processed some DCBs in RX, there is free space, and we must set + * DCB_LLP to point to the next DCB + */ + llp_prev = ocelot_fdma_readl(ocelot, MSCC_FDMA_DCB_LLP_PREV(chan)); + dma_base = rx_ring->dcbs_dma; + + /* Get the next DMA addr located after LLP == NULL DCB */ + idx = ocelot_fdma_dma_idx(dma_base, llp_prev); + idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE); + new_llp = ocelot_fdma_idx_dma(dma_base, idx); + + /* Finally reactivate the channel */ + ocelot_fdma_activate_chan(ocelot, new_llp, chan); +} + +static bool ocelot_fdma_add_rx_frag(struct ocelot_fdma_rx_buf *rxb, u32 stat, + struct sk_buff *skb, bool first) +{ + int size = MSCC_FDMA_DCB_STAT_BLOCKL(stat); + struct page *page = rxb->page; + + if (likely(first)) { + skb_put(skb, size); + } else { + skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page, + rxb->page_offset, size, OCELOT_FDMA_RX_SIZE); + } + + /* Try to reuse page */ + if (unlikely(page_ref_count(page) != 1 || page_is_pfmemalloc(page))) + return false; + + /* Change offset to the other half */ + rxb->page_offset ^= OCELOT_FDMA_RX_SIZE; + + page_ref_inc(page); + + return true; +} + +static void ocelot_fdma_reuse_rx_page(struct ocelot *ocelot, + struct ocelot_fdma_rx_buf *old_rxb) +{ + struct ocelot_fdma_rx_ring *rx_ring = &ocelot->fdma->rx_ring; + struct ocelot_fdma_rx_buf *new_rxb; + + new_rxb = &rx_ring->bufs[rx_ring->next_to_alloc]; + rx_ring->next_to_alloc = ocelot_fdma_idx_next(rx_ring->next_to_alloc, + OCELOT_FDMA_RX_RING_SIZE); + + /* Copy page reference */ + *new_rxb = *old_rxb; + + /* Sync for use by the device */ + dma_sync_single_range_for_device(ocelot->dev, old_rxb->dma_addr, + old_rxb->page_offset, + OCELOT_FDMA_RX_SIZE, DMA_FROM_DEVICE); +} + +static struct sk_buff *ocelot_fdma_get_skb(struct ocelot *ocelot, u32 stat, + struct ocelot_fdma_rx_buf *rxb, + struct sk_buff *skb) +{ + bool first = false; + + /* Allocate skb head and data */ + if (likely(!skb)) { + void *buff_addr = page_address(rxb->page) + + rxb->page_offset; + + skb = build_skb(buff_addr, OCELOT_FDMA_SKBFRAG_SIZE); + if (unlikely(!skb)) { + dev_err_ratelimited(ocelot->dev, + "build_skb failed !\n"); + return NULL; + } + first = true; + } + + dma_sync_single_range_for_cpu(ocelot->dev, rxb->dma_addr, + rxb->page_offset, OCELOT_FDMA_RX_SIZE, + DMA_FROM_DEVICE); + + if (ocelot_fdma_add_rx_frag(rxb, stat, skb, first)) { + /* Reuse the free half of the page for the next_to_alloc DCB*/ + ocelot_fdma_reuse_rx_page(ocelot, rxb); + } else { + /* page cannot be reused, unmap it */ + dma_unmap_page(ocelot->dev, rxb->dma_addr, PAGE_SIZE, + DMA_FROM_DEVICE); + } + + /* clear rx buff content */ + rxb->page = NULL; + + return skb; +} + +static bool ocelot_fdma_receive_skb(struct ocelot *ocelot, struct sk_buff *skb) +{ + struct net_device *ndev; + void *xfh = skb->data; + u64 timestamp; + u64 src_port; + + skb_pull(skb, OCELOT_TAG_LEN); + + ocelot_xfh_get_src_port(xfh, &src_port); + if (unlikely(src_port >= ocelot->num_phys_ports)) + return false; + + ndev = ocelot_port_to_netdev(ocelot, src_port); + if (unlikely(!ndev)) + return false; + + pskb_trim(skb, skb->len - ETH_FCS_LEN); + + skb->dev = ndev; + skb->protocol = eth_type_trans(skb, skb->dev); + skb->dev->stats.rx_bytes += skb->len; + skb->dev->stats.rx_packets++; + + if (ocelot->ptp) { + ocelot_xfh_get_rew_val(xfh, ×tamp); + ocelot_ptp_rx_timestamp(ocelot, skb, timestamp); + } + + if (likely(!skb_defer_rx_timestamp(skb))) + netif_receive_skb(skb); + + return true; +} + +static int ocelot_fdma_rx_get(struct ocelot *ocelot, int budget) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + struct ocelot_fdma_rx_ring *rx_ring; + struct ocelot_fdma_rx_buf *rxb; + struct ocelot_fdma_dcb *dcb; + struct sk_buff *skb; + int work_done = 0; + int cleaned_cnt; + u32 stat; + u16 idx; + + cleaned_cnt = ocelot_fdma_rx_ring_free(fdma); + rx_ring = &fdma->rx_ring; + skb = rx_ring->skb; + + while (budget--) { + idx = rx_ring->next_to_clean; + dcb = &rx_ring->dcbs[idx]; + stat = dcb->stat; + if (MSCC_FDMA_DCB_STAT_BLOCKL(stat) == 0) + break; + + /* New packet is a start of frame but we already got a skb set, + * we probably lost an EOF packet, free skb + */ + if (unlikely(skb && (stat & MSCC_FDMA_DCB_STAT_SOF))) { + dev_kfree_skb(skb); + skb = NULL; + } + + rxb = &rx_ring->bufs[idx]; + /* Fetch next to clean buffer from the rx_ring */ + skb = ocelot_fdma_get_skb(ocelot, stat, rxb, skb); + if (unlikely(!skb)) + break; + + work_done++; + cleaned_cnt++; + + idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE); + rx_ring->next_to_clean = idx; + + if (unlikely(stat & MSCC_FDMA_DCB_STAT_ABORT || + stat & MSCC_FDMA_DCB_STAT_PD)) { + dev_err_ratelimited(ocelot->dev, + "DCB aborted or pruned\n"); + dev_kfree_skb(skb); + skb = NULL; + continue; + } + + /* We still need to process the other fragment of the packet + * before delivering it to the network stack + */ + if (!(stat & MSCC_FDMA_DCB_STAT_EOF)) + continue; + + if (unlikely(!ocelot_fdma_receive_skb(ocelot, skb))) + dev_kfree_skb(skb); + + skb = NULL; + } + + rx_ring->skb = skb; + + if (cleaned_cnt) + ocelot_fdma_alloc_rx_buffs(ocelot, cleaned_cnt); + + return work_done; +} + +static void ocelot_fdma_wakeup_netdev(struct ocelot *ocelot) +{ + struct ocelot_port_private *priv; + struct ocelot_port *ocelot_port; + struct net_device *dev; + int port; + + for (port = 0; port < ocelot->num_phys_ports; port++) { + ocelot_port = ocelot->ports[port]; + if (!ocelot_port) + continue; + priv = container_of(ocelot_port, struct ocelot_port_private, + port); + dev = priv->dev; + + if (unlikely(netif_queue_stopped(dev))) + netif_wake_queue(dev); + } +} + +static void ocelot_fdma_tx_cleanup(struct ocelot *ocelot, int budget) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + struct ocelot_fdma_tx_ring *tx_ring; + struct ocelot_fdma_tx_buf *buf; + unsigned int new_null_llp_idx; + struct ocelot_fdma_dcb *dcb; + bool end_of_list = false; + struct sk_buff *skb; + dma_addr_t dma; + u32 dcb_llp; + u16 ntc; + int ret; + + tx_ring = &fdma->tx_ring; + + /* Purge the TX packets that have been sent up to the NULL llp or the + * end of done list. + */ + while (!ocelot_fdma_tx_ring_empty(fdma)) { + ntc = tx_ring->next_to_clean; + dcb = &tx_ring->dcbs[ntc]; + if (!(dcb->stat & MSCC_FDMA_DCB_STAT_PD)) + break; + + buf = &tx_ring->bufs[ntc]; + skb = buf->skb; + dma_unmap_single(ocelot->dev, dma_unmap_addr(buf, dma_addr), + skb->len, DMA_TO_DEVICE); + napi_consume_skb(skb, budget); + dcb_llp = dcb->llp; + + /* Only update after accessing all dcb fields */ + tx_ring->next_to_clean = ocelot_fdma_idx_next(ntc, + OCELOT_FDMA_TX_RING_SIZE); + + /* If we hit the NULL LLP, stop, we might need to reload FDMA */ + if (dcb_llp == 0) { + end_of_list = true; + break; + } + } + + /* No need to try to wake if there were no TX cleaned_cnt up. */ + if (ocelot_fdma_tx_ring_free(fdma)) + ocelot_fdma_wakeup_netdev(ocelot); + + /* If there is still some DCBs to be processed by the FDMA or if the + * pending list is empty, there is no need to restart the FDMA. + */ + if (!end_of_list || ocelot_fdma_tx_ring_empty(fdma)) + return; + + ret = ocelot_fdma_wait_chan_safe(ocelot, MSCC_FDMA_INJ_CHAN); + if (ret) { + dev_warn(ocelot->dev, + "Failed to wait for TX channel to stop\n"); + return; + } + + /* Set NULL LLP to be the last DCB used */ + new_null_llp_idx = ocelot_fdma_idx_prev(tx_ring->next_to_use, + OCELOT_FDMA_TX_RING_SIZE); + dcb = &tx_ring->dcbs[new_null_llp_idx]; + dcb->llp = 0; + + dma = ocelot_fdma_idx_dma(tx_ring->dcbs_dma, tx_ring->next_to_clean); + ocelot_fdma_activate_chan(ocelot, dma, MSCC_FDMA_INJ_CHAN); +} + +static int ocelot_fdma_napi_poll(struct napi_struct *napi, int budget) +{ + struct ocelot_fdma *fdma = container_of(napi, struct ocelot_fdma, napi); + struct ocelot *ocelot = fdma->ocelot; + int work_done = 0; + bool rx_stopped; + + ocelot_fdma_tx_cleanup(ocelot, budget); + + rx_stopped = ocelot_fdma_check_stop_rx(ocelot); + + work_done = ocelot_fdma_rx_get(ocelot, budget); + + if (rx_stopped) + ocelot_fdma_rx_restart(ocelot); + + if (work_done < budget) { + napi_complete_done(&fdma->napi, work_done); + ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, + BIT(MSCC_FDMA_INJ_CHAN) | + BIT(MSCC_FDMA_XTR_CHAN)); + } + + return work_done; +} + +static irqreturn_t ocelot_fdma_interrupt(int irq, void *dev_id) +{ + u32 ident, llp, frm, err, err_code; + struct ocelot *ocelot = dev_id; + + ident = ocelot_fdma_readl(ocelot, MSCC_FDMA_INTR_IDENT); + frm = ocelot_fdma_readl(ocelot, MSCC_FDMA_INTR_FRM); + llp = ocelot_fdma_readl(ocelot, MSCC_FDMA_INTR_LLP); + + ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_LLP, llp & ident); + ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_FRM, frm & ident); + if (frm || llp) { + ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, 0); + napi_schedule(&ocelot->fdma->napi); + } + + err = ocelot_fdma_readl(ocelot, MSCC_FDMA_EVT_ERR); + if (unlikely(err)) { + err_code = ocelot_fdma_readl(ocelot, MSCC_FDMA_EVT_ERR_CODE); + dev_err_ratelimited(ocelot->dev, + "Error ! chans mask: %#x, code: %#x\n", + err, err_code); + + ocelot_fdma_writel(ocelot, MSCC_FDMA_EVT_ERR, err); + ocelot_fdma_writel(ocelot, MSCC_FDMA_EVT_ERR_CODE, err_code); + } + + return IRQ_HANDLED; +} + +static void ocelot_fdma_send_skb(struct ocelot *ocelot, + struct ocelot_fdma *fdma, struct sk_buff *skb) +{ + struct ocelot_fdma_tx_ring *tx_ring = &fdma->tx_ring; + struct ocelot_fdma_tx_buf *tx_buf; + struct ocelot_fdma_dcb *dcb; + dma_addr_t dma; + u16 next_idx; + + dcb = &tx_ring->dcbs[tx_ring->next_to_use]; + tx_buf = &tx_ring->bufs[tx_ring->next_to_use]; + if (!ocelot_fdma_tx_dcb_set_skb(ocelot, tx_buf, dcb, skb)) { + dev_kfree_skb_any(skb); + return; + } + + next_idx = ocelot_fdma_idx_next(tx_ring->next_to_use, + OCELOT_FDMA_TX_RING_SIZE); + skb_tx_timestamp(skb); + + /* If the FDMA TX chan is empty, then enqueue the DCB directly */ + if (ocelot_fdma_tx_ring_empty(fdma)) { + dma = ocelot_fdma_idx_dma(tx_ring->dcbs_dma, + tx_ring->next_to_use); + ocelot_fdma_activate_chan(ocelot, dma, MSCC_FDMA_INJ_CHAN); + } else { + /* Chain the DCBs */ + dcb->llp = ocelot_fdma_idx_dma(tx_ring->dcbs_dma, next_idx); + } + + tx_ring->next_to_use = next_idx; +} + +static int ocelot_fdma_prepare_skb(struct ocelot *ocelot, int port, u32 rew_op, + struct sk_buff *skb, struct net_device *dev) +{ + int needed_headroom = max_t(int, OCELOT_TAG_LEN - skb_headroom(skb), 0); + int needed_tailroom = max_t(int, ETH_FCS_LEN - skb_tailroom(skb), 0); + void *ifh; + int err; + + if (unlikely(needed_headroom || needed_tailroom || + skb_header_cloned(skb))) { + err = pskb_expand_head(skb, needed_headroom, needed_tailroom, + GFP_ATOMIC); + if (unlikely(err)) { + dev_kfree_skb_any(skb); + return 1; + } + } + + err = skb_linearize(skb); + if (err) { + net_err_ratelimited("%s: skb_linearize error (%d)!\n", + dev->name, err); + dev_kfree_skb_any(skb); + return 1; + } + + ifh = skb_push(skb, OCELOT_TAG_LEN); + skb_put(skb, ETH_FCS_LEN); + memset(ifh, 0, OCELOT_TAG_LEN); + ocelot_ifh_port_set(ifh, port, rew_op, skb_vlan_tag_get(skb)); + + return 0; +} + +int ocelot_fdma_inject_frame(struct ocelot *ocelot, int port, u32 rew_op, + struct sk_buff *skb, struct net_device *dev) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + int ret = NETDEV_TX_OK; + + spin_lock(&fdma->tx_ring.xmit_lock); + + if (ocelot_fdma_tx_ring_free(fdma) == 0) { + netif_stop_queue(dev); + ret = NETDEV_TX_BUSY; + goto out; + } + + if (ocelot_fdma_prepare_skb(ocelot, port, rew_op, skb, dev)) + goto out; + + ocelot_fdma_send_skb(ocelot, fdma, skb); + +out: + spin_unlock(&fdma->tx_ring.xmit_lock); + + return ret; +} + +static void ocelot_fdma_free_rx_ring(struct ocelot *ocelot) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + struct ocelot_fdma_rx_ring *rx_ring; + struct ocelot_fdma_rx_buf *rxb; + u16 idx; + + rx_ring = &fdma->rx_ring; + idx = rx_ring->next_to_clean; + + /* Free the pages held in the RX ring */ + while (idx != rx_ring->next_to_use) { + rxb = &rx_ring->bufs[idx]; + dma_unmap_page(ocelot->dev, rxb->dma_addr, PAGE_SIZE, + DMA_FROM_DEVICE); + __free_page(rxb->page); + idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_RX_RING_SIZE); + } + + if (fdma->rx_ring.skb) + dev_kfree_skb_any(fdma->rx_ring.skb); +} + +static void ocelot_fdma_free_tx_ring(struct ocelot *ocelot) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + struct ocelot_fdma_tx_ring *tx_ring; + struct ocelot_fdma_tx_buf *txb; + struct sk_buff *skb; + u16 idx; + + tx_ring = &fdma->tx_ring; + idx = tx_ring->next_to_clean; + + while (idx != tx_ring->next_to_use) { + txb = &tx_ring->bufs[idx]; + skb = txb->skb; + dma_unmap_single(ocelot->dev, dma_unmap_addr(txb, dma_addr), + skb->len, DMA_TO_DEVICE); + dev_kfree_skb_any(skb); + idx = ocelot_fdma_idx_next(idx, OCELOT_FDMA_TX_RING_SIZE); + } +} + +static int ocelot_fdma_rings_alloc(struct ocelot *ocelot) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + struct ocelot_fdma_dcb *dcbs; + unsigned int adjust; + dma_addr_t dcbs_dma; + int ret; + + /* Create a pool of consistent memory blocks for hardware descriptors */ + fdma->dcbs_base = dmam_alloc_coherent(ocelot->dev, + OCELOT_DCBS_HW_ALLOC_SIZE, + &fdma->dcbs_dma_base, GFP_KERNEL); + if (!fdma->dcbs_base) + return -ENOMEM; + + /* DCBs must be aligned on a 32bit boundary */ + dcbs = fdma->dcbs_base; + dcbs_dma = fdma->dcbs_dma_base; + if (!IS_ALIGNED(dcbs_dma, 4)) { + adjust = dcbs_dma & 0x3; + dcbs_dma = ALIGN(dcbs_dma, 4); + dcbs = (void *)dcbs + adjust; + } + + /* TX queue */ + fdma->tx_ring.dcbs = dcbs; + fdma->tx_ring.dcbs_dma = dcbs_dma; + spin_lock_init(&fdma->tx_ring.xmit_lock); + + /* RX queue */ + fdma->rx_ring.dcbs = dcbs + OCELOT_FDMA_TX_RING_SIZE; + fdma->rx_ring.dcbs_dma = dcbs_dma + OCELOT_FDMA_TX_DCB_SIZE; + ret = ocelot_fdma_alloc_rx_buffs(ocelot, + ocelot_fdma_tx_ring_free(fdma)); + if (ret) { + ocelot_fdma_free_rx_ring(ocelot); + return ret; + } + + /* Set the last DCB LLP as NULL, this is normally done when restarting + * the RX chan, but this is for the first run + */ + ocelot_fdma_rx_set_llp(&fdma->rx_ring); + + return 0; +} + +void ocelot_fdma_netdev_init(struct ocelot *ocelot, struct net_device *dev) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + + dev->needed_headroom = OCELOT_TAG_LEN; + dev->needed_tailroom = ETH_FCS_LEN; + + if (fdma->ndev) + return; + + fdma->ndev = dev; + netif_napi_add_weight(dev, &fdma->napi, ocelot_fdma_napi_poll, + OCELOT_FDMA_WEIGHT); +} + +void ocelot_fdma_netdev_deinit(struct ocelot *ocelot, struct net_device *dev) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + + if (fdma->ndev == dev) { + netif_napi_del(&fdma->napi); + fdma->ndev = NULL; + } +} + +void ocelot_fdma_init(struct platform_device *pdev, struct ocelot *ocelot) +{ + struct device *dev = ocelot->dev; + struct ocelot_fdma *fdma; + int ret; + + fdma = devm_kzalloc(dev, sizeof(*fdma), GFP_KERNEL); + if (!fdma) + return; + + ocelot->fdma = fdma; + ocelot->dev->coherent_dma_mask = DMA_BIT_MASK(32); + + ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, 0); + + fdma->ocelot = ocelot; + fdma->irq = platform_get_irq_byname(pdev, "fdma"); + ret = devm_request_irq(dev, fdma->irq, ocelot_fdma_interrupt, 0, + dev_name(dev), ocelot); + if (ret) + goto err_free_fdma; + + ret = ocelot_fdma_rings_alloc(ocelot); + if (ret) + goto err_free_irq; + + static_branch_enable(&ocelot_fdma_enabled); + + return; + +err_free_irq: + devm_free_irq(dev, fdma->irq, fdma); +err_free_fdma: + devm_kfree(dev, fdma); + + ocelot->fdma = NULL; +} + +void ocelot_fdma_start(struct ocelot *ocelot) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + + /* Reconfigure for extraction and injection using DMA */ + ocelot_write_rix(ocelot, QS_INJ_GRP_CFG_MODE(2), QS_INJ_GRP_CFG, 0); + ocelot_write_rix(ocelot, QS_INJ_CTRL_GAP_SIZE(0), QS_INJ_CTRL, 0); + + ocelot_write_rix(ocelot, QS_XTR_GRP_CFG_MODE(2), QS_XTR_GRP_CFG, 0); + + ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_LLP, 0xffffffff); + ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_FRM, 0xffffffff); + + ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_LLP_ENA, + BIT(MSCC_FDMA_INJ_CHAN) | BIT(MSCC_FDMA_XTR_CHAN)); + ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_FRM_ENA, + BIT(MSCC_FDMA_XTR_CHAN)); + ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, + BIT(MSCC_FDMA_INJ_CHAN) | BIT(MSCC_FDMA_XTR_CHAN)); + + napi_enable(&fdma->napi); + + ocelot_fdma_activate_chan(ocelot, ocelot->fdma->rx_ring.dcbs_dma, + MSCC_FDMA_XTR_CHAN); +} + +void ocelot_fdma_deinit(struct ocelot *ocelot) +{ + struct ocelot_fdma *fdma = ocelot->fdma; + + ocelot_fdma_writel(ocelot, MSCC_FDMA_INTR_ENA, 0); + ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_FORCEDIS, + BIT(MSCC_FDMA_XTR_CHAN)); + ocelot_fdma_writel(ocelot, MSCC_FDMA_CH_FORCEDIS, + BIT(MSCC_FDMA_INJ_CHAN)); + napi_synchronize(&fdma->napi); + napi_disable(&fdma->napi); + + ocelot_fdma_free_rx_ring(ocelot); + ocelot_fdma_free_tx_ring(ocelot); +} |