net: ocelot: add FDMA support (753a026c) · Commits · EulixOS / Software / Kernel

drivers/net/ethernet/mscc/Makefile

+1 −0

Original line number	Diff line number	Diff line
		@@ -12,5 +12,6 @@ mscc_ocelot_switch_lib-y := \
		mscc_ocelot_switch_lib-$(CONFIG_BRIDGE_MRP) += ocelot_mrp.o
		obj-$(CONFIG_MSCC_OCELOT_SWITCH) += mscc_ocelot.o
		mscc_ocelot-y := \
		ocelot_fdma.o \
		ocelot_vsc7514.o \
		ocelot_net.o

drivers/net/ethernet/mscc/ocelot_fdma.c

0 → 100644

+894 −0

Original line number	Diff line number	Diff line
		// 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 int ocelot_fdma_wait_chan_safe(struct ocelot *ocelot, int chan)
		{
		unsigned long timeout;
		u32 safe;

		timeout = jiffies + usecs_to_jiffies(OCELOT_FDMA_CH_SAFE_TIMEOUT_US);
		do {
		safe = ocelot_fdma_readl(ocelot, MSCC_FDMA_CH_SAFE);
		if (safe & BIT(chan))
		return 0;
		} while (time_after(jiffies, timeout));

		return -ETIMEDOUT;
		}

		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, &timestamp);
		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, 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(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);
		}

drivers/net/ethernet/mscc/ocelot_fdma.h

0 → 100644

+166 −0

File added.

Preview size limit exceeded, changes collapsed.

drivers/net/ethernet/mscc/ocelot_net.c

+21 −4

Original line number	Diff line number	Diff line
		@@ -15,6 +15,7 @@
		#include <net/pkt_cls.h>
		#include "ocelot.h"
		#include "ocelot_vcap.h"
		#include "ocelot_fdma.h"

		#define OCELOT_MAC_QUIRKS OCELOT_QUIRK_QSGMII_PORTS_MUST_BE_UP

		@@ -457,7 +458,8 @@ static netdev_tx_t ocelot_port_xmit(struct sk_buff skb, struct net_device dev)
		int port = priv->chip_port;
		u32 rew_op = 0;

		if (!ocelot_can_inject(ocelot, 0))
		if (!static_branch_unlikely(&ocelot_fdma_enabled) &&
		!ocelot_can_inject(ocelot, 0))
		return NETDEV_TX_BUSY;

		/* Check if timestamping is needed */
		@@ -475,9 +477,13 @@ static netdev_tx_t ocelot_port_xmit(struct sk_buff skb, struct net_device dev)
		rew_op = ocelot_ptp_rew_op(skb);
		}

		if (static_branch_unlikely(&ocelot_fdma_enabled)) {
		ocelot_fdma_inject_frame(ocelot, port, rew_op, skb, dev);
		} else {
		ocelot_port_inject_frame(ocelot, port, 0, rew_op, skb);

		kfree_skb(skb);
		consume_skb(skb);
		}

		return NETDEV_TX_OK;
		}
		@@ -1699,14 +1705,20 @@ int ocelot_probe_port(struct ocelot ocelot, int port, struct regmap target,
		if (err)
		goto out;

		if (ocelot->fdma)
		ocelot_fdma_netdev_init(ocelot, dev);

		err = register_netdev(dev);
		if (err) {
		dev_err(ocelot->dev, "register_netdev failed\n");
		goto out;
		goto out_fdma_deinit;
		}

		return 0;

		out_fdma_deinit:
		if (ocelot->fdma)
		ocelot_fdma_netdev_deinit(ocelot, dev);
		out:
		ocelot->ports[port] = NULL;
		free_netdev(dev);
		@@ -1719,9 +1731,14 @@ void ocelot_release_port(struct ocelot_port *ocelot_port)
		struct ocelot_port_private *priv = container_of(ocelot_port,
		struct ocelot_port_private,
		port);
		struct ocelot *ocelot = ocelot_port->ocelot;
		struct ocelot_fdma *fdma = ocelot->fdma;

		unregister_netdev(priv->dev);

		if (fdma)
		ocelot_fdma_netdev_deinit(ocelot, priv->dev);

		if (priv->phylink) {
		rtnl_lock();
		phylink_disconnect_phy(priv->phylink);

drivers/net/ethernet/mscc/ocelot_vsc7514.c

+10 −0

File changed.

Preview size limit exceeded, changes collapsed.