linux/drivers/net/ethernet/engleder/tsnep_main.c

// SPDX-License-Identifier: GPL-2.0
/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */

/* TSN endpoint Ethernet MAC driver
 *
 * The TSN endpoint Ethernet MAC is a FPGA based network device for real-time
 * communication. It is designed for endpoints within TSN (Time Sensitive
 * Networking) networks; e.g., for PLCs in the industrial automation case.
 *
 * It supports multiple TX/RX queue pairs. The first TX/RX queue pair is used
 * by the driver.
 *
 * More information can be found here:
 * - www.embedded-experts.at/tsn
 * - www.engleder-embedded.com
 */

#include "tsnep.h"
#include "tsnep_hw.h"

#include <linux/module.h>
#include <linux/of.h>
#include <linux/of_net.h>
#include <linux/of_mdio.h>
#include <linux/interrupt.h>
#include <linux/etherdevice.h>
#include <linux/phy.h>
#include <linux/iopoll.h>
#include <linux/bpf.h>
#include <linux/bpf_trace.h>

#define TSNEP_RX_OFFSET (max(NET_SKB_PAD, XDP_PACKET_HEADROOM) + NET_IP_ALIGN)
#define TSNEP_HEADROOM ALIGN(TSNEP_RX_OFFSET, 4)
#define TSNEP_MAX_RX_BUF_SIZE (PAGE_SIZE - TSNEP_HEADROOM - \
			       SKB_DATA_ALIGN(sizeof(struct skb_shared_info)))

#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
#define DMA_ADDR_HIGH(dma_addr) ((u32)(((dma_addr) >> 32) & 0xFFFFFFFF))
#else
#define DMA_ADDR_HIGH(dma_addr) ((u32)(0))
#endif
#define DMA_ADDR_LOW(dma_addr) ((u32)((dma_addr) & 0xFFFFFFFF))

#define TSNEP_COALESCE_USECS_DEFAULT 64
#define TSNEP_COALESCE_USECS_MAX     ((ECM_INT_DELAY_MASK >> ECM_INT_DELAY_SHIFT) * \
				      ECM_INT_DELAY_BASE_US + ECM_INT_DELAY_BASE_US - 1)

#define TSNEP_TX_TYPE_SKB	BIT(0)
#define TSNEP_TX_TYPE_SKB_FRAG	BIT(1)
#define TSNEP_TX_TYPE_XDP_TX	BIT(2)
#define TSNEP_TX_TYPE_XDP_NDO	BIT(3)

#define TSNEP_XDP_TX		BIT(0)
#define TSNEP_XDP_REDIRECT	BIT(1)

static void tsnep_enable_irq(struct tsnep_adapter *adapter, u32 mask)
{
	iowrite32(mask, adapter->addr + ECM_INT_ENABLE);
}

static void tsnep_disable_irq(struct tsnep_adapter *adapter, u32 mask)
{
	mask |= ECM_INT_DISABLE;
	iowrite32(mask, adapter->addr + ECM_INT_ENABLE);
}

static irqreturn_t tsnep_irq(int irq, void *arg)
{
	struct tsnep_adapter *adapter = arg;
	u32 active = ioread32(adapter->addr + ECM_INT_ACTIVE);

	/* acknowledge interrupt */
	if (active != 0)
		iowrite32(active, adapter->addr + ECM_INT_ACKNOWLEDGE);

	/* handle link interrupt */
	if ((active & ECM_INT_LINK) != 0)
		phy_mac_interrupt(adapter->netdev->phydev);

	/* handle TX/RX queue 0 interrupt */
	if ((active & adapter->queue[0].irq_mask) != 0) {
		tsnep_disable_irq(adapter, adapter->queue[0].irq_mask);
		napi_schedule(&adapter->queue[0].napi);
	}

	return IRQ_HANDLED;
}

static irqreturn_t tsnep_irq_txrx(int irq, void *arg)
{
	struct tsnep_queue *queue = arg;

	/* handle TX/RX queue interrupt */
	tsnep_disable_irq(queue->adapter, queue->irq_mask);
	napi_schedule(&queue->napi);

	return IRQ_HANDLED;
}

int tsnep_set_irq_coalesce(struct tsnep_queue *queue, u32 usecs)
{
	if (usecs > TSNEP_COALESCE_USECS_MAX)
		return -ERANGE;

	usecs /= ECM_INT_DELAY_BASE_US;
	usecs <<= ECM_INT_DELAY_SHIFT;
	usecs &= ECM_INT_DELAY_MASK;

	queue->irq_delay &= ~ECM_INT_DELAY_MASK;
	queue->irq_delay |= usecs;
	iowrite8(queue->irq_delay, queue->irq_delay_addr);

	return 0;
}

u32 tsnep_get_irq_coalesce(struct tsnep_queue *queue)
{
	u32 usecs;

	usecs = (queue->irq_delay & ECM_INT_DELAY_MASK);
	usecs >>= ECM_INT_DELAY_SHIFT;
	usecs *= ECM_INT_DELAY_BASE_US;

	return usecs;
}

static int tsnep_mdiobus_read(struct mii_bus *bus, int addr, int regnum)
{
	struct tsnep_adapter *adapter = bus->priv;
	u32 md;
	int retval;

	md = ECM_MD_READ;
	if (!adapter->suppress_preamble)
		md |= ECM_MD_PREAMBLE;
	md |= (regnum << ECM_MD_ADDR_SHIFT) & ECM_MD_ADDR_MASK;
	md |= (addr << ECM_MD_PHY_ADDR_SHIFT) & ECM_MD_PHY_ADDR_MASK;
	iowrite32(md, adapter->addr + ECM_MD_CONTROL);
	retval = readl_poll_timeout_atomic(adapter->addr + ECM_MD_STATUS, md,
					   !(md & ECM_MD_BUSY), 16, 1000);
	if (retval != 0)
		return retval;

	return (md & ECM_MD_DATA_MASK) >> ECM_MD_DATA_SHIFT;
}

static int tsnep_mdiobus_write(struct mii_bus *bus, int addr, int regnum,
			       u16 val)
{
	struct tsnep_adapter *adapter = bus->priv;
	u32 md;
	int retval;

	md = ECM_MD_WRITE;
	if (!adapter->suppress_preamble)
		md |= ECM_MD_PREAMBLE;
	md |= (regnum << ECM_MD_ADDR_SHIFT) & ECM_MD_ADDR_MASK;
	md |= (addr << ECM_MD_PHY_ADDR_SHIFT) & ECM_MD_PHY_ADDR_MASK;
	md |= ((u32)val << ECM_MD_DATA_SHIFT) & ECM_MD_DATA_MASK;
	iowrite32(md, adapter->addr + ECM_MD_CONTROL);
	retval = readl_poll_timeout_atomic(adapter->addr + ECM_MD_STATUS, md,
					   !(md & ECM_MD_BUSY), 16, 1000);
	if (retval != 0)
		return retval;

	return 0;
}

static void tsnep_set_link_mode(struct tsnep_adapter *adapter)
{
	u32 mode;

	switch (adapter->phydev->speed) {
	case SPEED_100:
		mode = ECM_LINK_MODE_100;
		break;
	case SPEED_1000:
		mode = ECM_LINK_MODE_1000;
		break;
	default:
		mode = ECM_LINK_MODE_OFF;
		break;
	}
	iowrite32(mode, adapter->addr + ECM_STATUS);
}

static void tsnep_phy_link_status_change(struct net_device *netdev)
{
	struct tsnep_adapter *adapter = netdev_priv(netdev);
	struct phy_device *phydev = netdev->phydev;

	if (phydev->link)
		tsnep_set_link_mode(adapter);

	phy_print_status(netdev->phydev);
}

static int tsnep_phy_loopback(struct tsnep_adapter *adapter, bool enable)
{
	int retval;

	retval = phy_loopback(adapter->phydev, enable);

	/* PHY link state change is not signaled if loopback is enabled, it
	 * would delay a working loopback anyway, let's ensure that loopback
	 * is working immediately by setting link mode directly
	 */
	if (!retval && enable)
		tsnep_set_link_mode(adapter);

	return retval;
}

static int tsnep_phy_open(struct tsnep_adapter *adapter)
{
	struct phy_device *phydev;
	struct ethtool_eee ethtool_eee;
	int retval;

	retval = phy_connect_direct(adapter->netdev, adapter->phydev,
				    tsnep_phy_link_status_change,
				    adapter->phy_mode);
	if (retval)
		return retval;
	phydev = adapter->netdev->phydev;

	/* MAC supports only 100Mbps|1000Mbps full duplex
	 * SPE (Single Pair Ethernet) is also an option but not implemented yet
	 */
	phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_10baseT_Half_BIT);
	phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_10baseT_Full_BIT);
	phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_100baseT_Half_BIT);
	phy_remove_link_mode(phydev, ETHTOOL_LINK_MODE_1000baseT_Half_BIT);

	/* disable EEE autoneg, EEE not supported by TSNEP */
	memset(&ethtool_eee, 0, sizeof(ethtool_eee));
	phy_ethtool_set_eee(adapter->phydev, &ethtool_eee);

	adapter->phydev->irq = PHY_MAC_INTERRUPT;
	phy_start(adapter->phydev);

	return 0;
}

static void tsnep_phy_close(struct tsnep_adapter *adapter)
{
	phy_stop(adapter->netdev->phydev);
	phy_disconnect(adapter->netdev->phydev);
	adapter->netdev->phydev = NULL;
}

static void tsnep_tx_ring_cleanup(struct tsnep_tx *tx)
{
	struct device *dmadev = tx->adapter->dmadev;
	int i;

	memset(tx->entry, 0, sizeof(tx->entry));

	for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
		if (tx->page[i]) {
			dma_free_coherent(dmadev, PAGE_SIZE, tx->page[i],
					  tx->page_dma[i]);
			tx->page[i] = NULL;
			tx->page_dma[i] = 0;
		}
	}
}

static int tsnep_tx_ring_init(struct tsnep_tx *tx)
{
	struct device *dmadev = tx->adapter->dmadev;
	struct tsnep_tx_entry *entry;
	struct tsnep_tx_entry *next_entry;
	int i, j;
	int retval;

	for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
		tx->page[i] =
			dma_alloc_coherent(dmadev, PAGE_SIZE, &tx->page_dma[i],
					   GFP_KERNEL);
		if (!tx->page[i]) {
			retval = -ENOMEM;
			goto alloc_failed;
		}
		for (j = 0; j < TSNEP_RING_ENTRIES_PER_PAGE; j++) {
			entry = &tx->entry[TSNEP_RING_ENTRIES_PER_PAGE * i + j];
			entry->desc_wb = (struct tsnep_tx_desc_wb *)
				(((u8 *)tx->page[i]) + TSNEP_DESC_SIZE * j);
			entry->desc = (struct tsnep_tx_desc *)
				(((u8 *)entry->desc_wb) + TSNEP_DESC_OFFSET);
			entry->desc_dma = tx->page_dma[i] + TSNEP_DESC_SIZE * j;
		}
	}
	for (i = 0; i < TSNEP_RING_SIZE; i++) {
		entry = &tx->entry[i];
		next_entry = &tx->entry[(i + 1) % TSNEP_RING_SIZE];
		entry->desc->next = __cpu_to_le64(next_entry->desc_dma);
	}

	return 0;

alloc_failed:
	tsnep_tx_ring_cleanup(tx);
	return retval;
}

static void tsnep_tx_activate(struct tsnep_tx *tx, int index, int length,
			      bool last)
{
	struct tsnep_tx_entry *entry = &tx->entry[index];

	entry->properties = 0;
	/* xdpf is union with skb */
	if (entry->skb) {
		entry->properties = length & TSNEP_DESC_LENGTH_MASK;
		entry->properties |= TSNEP_DESC_INTERRUPT_FLAG;
		if ((entry->type & TSNEP_TX_TYPE_SKB) &&
		    (skb_shinfo(entry->skb)->tx_flags & SKBTX_IN_PROGRESS))
			entry->properties |= TSNEP_DESC_EXTENDED_WRITEBACK_FLAG;

		/* toggle user flag to prevent false acknowledge
		 *
		 * Only the first fragment is acknowledged. For all other
		 * fragments no acknowledge is done and the last written owner
		 * counter stays in the writeback descriptor. Therefore, it is
		 * possible that the last written owner counter is identical to
		 * the new incremented owner counter and a false acknowledge is
		 * detected before the real acknowledge has been done by
		 * hardware.
		 *
		 * The user flag is used to prevent this situation. The user
		 * flag is copied to the writeback descriptor by the hardware
		 * and is used as additional acknowledge data. By toggeling the
		 * user flag only for the first fragment (which is
		 * acknowledged), it is guaranteed that the last acknowledge
		 * done for this descriptor has used a different user flag and
		 * cannot be detected as false acknowledge.
		 */
		entry->owner_user_flag = !entry->owner_user_flag;
	}
	if (last)
		entry->properties |= TSNEP_TX_DESC_LAST_FRAGMENT_FLAG;
	if (index == tx->increment_owner_counter) {
		tx->owner_counter++;
		if (tx->owner_counter == 4)
			tx->owner_counter = 1;
		tx->increment_owner_counter--;
		if (tx->increment_owner_counter < 0)
			tx->increment_owner_counter = TSNEP_RING_SIZE - 1;
	}
	entry->properties |=
		(tx->owner_counter << TSNEP_DESC_OWNER_COUNTER_SHIFT) &
		TSNEP_DESC_OWNER_COUNTER_MASK;
	if (entry->owner_user_flag)
		entry->properties |= TSNEP_TX_DESC_OWNER_USER_FLAG;
	entry->desc->more_properties =
		__cpu_to_le32(entry->len & TSNEP_DESC_LENGTH_MASK);

	/* descriptor properties shall be written last, because valid data is
	 * signaled there
	 */
	dma_wmb();

	entry->desc->properties = __cpu_to_le32(entry->properties);
}

static int tsnep_tx_desc_available(struct tsnep_tx *tx)
{
	if (tx->read <= tx->write)
		return TSNEP_RING_SIZE - tx->write + tx->read - 1;
	else
		return tx->read - tx->write - 1;
}

static int tsnep_tx_map(struct sk_buff *skb, struct tsnep_tx *tx, int count)
{
	struct device *dmadev = tx->adapter->dmadev;
	struct tsnep_tx_entry *entry;
	unsigned int len;
	dma_addr_t dma;
	int map_len = 0;
	int i;

	for (i = 0; i < count; i++) {
		entry = &tx->entry[(tx->write + i) % TSNEP_RING_SIZE];

		if (!i) {
			len = skb_headlen(skb);
			dma = dma_map_single(dmadev, skb->data, len,
					     DMA_TO_DEVICE);

			entry->type = TSNEP_TX_TYPE_SKB;
		} else {
			len = skb_frag_size(&skb_shinfo(skb)->frags[i - 1]);
			dma = skb_frag_dma_map(dmadev,
					       &skb_shinfo(skb)->frags[i - 1],
					       0, len, DMA_TO_DEVICE);

			entry->type = TSNEP_TX_TYPE_SKB_FRAG;
		}
		if (dma_mapping_error(dmadev, dma))
			return -ENOMEM;

		entry->len = len;
		dma_unmap_addr_set(entry, dma, dma);

		entry->desc->tx = __cpu_to_le64(dma);

		map_len += len;
	}

	return map_len;
}

static int tsnep_tx_unmap(struct tsnep_tx *tx, int index, int count)
{
	struct device *dmadev = tx->adapter->dmadev;
	struct tsnep_tx_entry *entry;
	int map_len = 0;
	int i;

	for (i = 0; i < count; i++) {
		entry = &tx->entry[(index + i) % TSNEP_RING_SIZE];

		if (entry->len) {
			if (entry->type & TSNEP_TX_TYPE_SKB)
				dma_unmap_single(dmadev,
						 dma_unmap_addr(entry, dma),
						 dma_unmap_len(entry, len),
						 DMA_TO_DEVICE);
			else if (entry->type &
				 (TSNEP_TX_TYPE_SKB_FRAG | TSNEP_TX_TYPE_XDP_NDO))
				dma_unmap_page(dmadev,
					       dma_unmap_addr(entry, dma),
					       dma_unmap_len(entry, len),
					       DMA_TO_DEVICE);
			map_len += entry->len;
			entry->len = 0;
		}
	}

	return map_len;
}

static netdev_tx_t tsnep_xmit_frame_ring(struct sk_buff *skb,
					 struct tsnep_tx *tx)
{
	int count = 1;
	struct tsnep_tx_entry *entry;
	int length;
	int i;
	int retval;

	if (skb_shinfo(skb)->nr_frags > 0)
		count += skb_shinfo(skb)->nr_frags;

	if (tsnep_tx_desc_available(tx) < count) {
		/* ring full, shall not happen because queue is stopped if full
		 * below
		 */
		netif_stop_subqueue(tx->adapter->netdev, tx->queue_index);

		return NETDEV_TX_BUSY;
	}

	entry = &tx->entry[tx->write];
	entry->skb = skb;

	retval = tsnep_tx_map(skb, tx, count);
	if (retval < 0) {
		tsnep_tx_unmap(tx, tx->write, count);
		dev_kfree_skb_any(entry->skb);
		entry->skb = NULL;

		tx->dropped++;

		return NETDEV_TX_OK;
	}
	length = retval;

	if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)
		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;

	for (i = 0; i < count; i++)
		tsnep_tx_activate(tx, (tx->write + i) % TSNEP_RING_SIZE, length,
				  i == count - 1);
	tx->write = (tx->write + count) % TSNEP_RING_SIZE;

	skb_tx_timestamp(skb);

	/* descriptor properties shall be valid before hardware is notified */
	dma_wmb();

	iowrite32(TSNEP_CONTROL_TX_ENABLE, tx->addr + TSNEP_CONTROL);

	if (tsnep_tx_desc_available(tx) < (MAX_SKB_FRAGS + 1)) {
		/* ring can get full with next frame */
		netif_stop_subqueue(tx->adapter->netdev, tx->queue_index);
	}

	return NETDEV_TX_OK;
}

static int tsnep_xdp_tx_map(struct xdp_frame *xdpf, struct tsnep_tx *tx,
			    struct skb_shared_info *shinfo, int count, u32 type)
{
	struct device *dmadev = tx->adapter->dmadev;
	struct tsnep_tx_entry *entry;
	struct page *page;
	skb_frag_t *frag;
	unsigned int len;
	int map_len = 0;
	dma_addr_t dma;
	void *data;
	int i;

	frag = NULL;
	len = xdpf->len;
	for (i = 0; i < count; i++) {
		entry = &tx->entry[(tx->write + i) % TSNEP_RING_SIZE];
		if (type & TSNEP_TX_TYPE_XDP_NDO) {
			data = unlikely(frag) ? skb_frag_address(frag) :
						xdpf->data;
			dma = dma_map_single(dmadev, data, len, DMA_TO_DEVICE);
			if (dma_mapping_error(dmadev, dma))
				return -ENOMEM;

			entry->type = TSNEP_TX_TYPE_XDP_NDO;
		} else {
			page = unlikely(frag) ? skb_frag_page(frag) :
						virt_to_page(xdpf->data);
			dma = page_pool_get_dma_addr(page);
			if (unlikely(frag))
				dma += skb_frag_off(frag);
			else
				dma += sizeof(*xdpf) + xdpf->headroom;
			dma_sync_single_for_device(dmadev, dma, len,
						   DMA_BIDIRECTIONAL);

			entry->type = TSNEP_TX_TYPE_XDP_TX;
		}

		entry->len = len;
		dma_unmap_addr_set(entry, dma, dma);

		entry->desc->tx = __cpu_to_le64(dma);

		map_len += len;

		if (i + 1 < count) {
			frag = &shinfo->frags[i];
			len = skb_frag_size(frag);
		}
	}

	return map_len;
}

/* This function requires __netif_tx_lock is held by the caller. */
static bool tsnep_xdp_xmit_frame_ring(struct xdp_frame *xdpf,
				      struct tsnep_tx *tx, u32 type)
{
	struct skb_shared_info *shinfo = xdp_get_shared_info_from_frame(xdpf);
	struct tsnep_tx_entry *entry;
	int count, length, retval, i;

	count = 1;
	if (unlikely(xdp_frame_has_frags(xdpf)))
		count += shinfo->nr_frags;

	/* ensure that TX ring is not filled up by XDP, always MAX_SKB_FRAGS
	 * will be available for normal TX path and queue is stopped there if
	 * necessary
	 */
	if (tsnep_tx_desc_available(tx) < (MAX_SKB_FRAGS + 1 + count))
		return false;

	entry = &tx->entry[tx->write];
	entry->xdpf = xdpf;

	retval = tsnep_xdp_tx_map(xdpf, tx, shinfo, count, type);
	if (retval < 0) {
		tsnep_tx_unmap(tx, tx->write, count);
		entry->xdpf = NULL;

		tx->dropped++;

		return false;
	}
	length = retval;

	for (i = 0; i < count; i++)
		tsnep_tx_activate(tx, (tx->write + i) % TSNEP_RING_SIZE, length,
				  i == count - 1);
	tx->write = (tx->write + count) % TSNEP_RING_SIZE;

	/* descriptor properties shall be valid before hardware is notified */
	dma_wmb();

	return true;
}

static void tsnep_xdp_xmit_flush(struct tsnep_tx *tx)
{
	iowrite32(TSNEP_CONTROL_TX_ENABLE, tx->addr + TSNEP_CONTROL);
}

static bool tsnep_xdp_xmit_back(struct tsnep_adapter *adapter,
				struct xdp_buff *xdp,
				struct netdev_queue *tx_nq, struct tsnep_tx *tx)
{
	struct xdp_frame *xdpf = xdp_convert_buff_to_frame(xdp);
	bool xmit;

	if (unlikely(!xdpf))
		return false;

	__netif_tx_lock(tx_nq, smp_processor_id());

	xmit = tsnep_xdp_xmit_frame_ring(xdpf, tx, TSNEP_TX_TYPE_XDP_TX);

	/* Avoid transmit queue timeout since we share it with the slow path */
	if (xmit)
		txq_trans_cond_update(tx_nq);

	__netif_tx_unlock(tx_nq);

	return xmit;
}

static bool tsnep_tx_poll(struct tsnep_tx *tx, int napi_budget)
{
	struct tsnep_tx_entry *entry;
	struct netdev_queue *nq;
	int budget = 128;
	int length;
	int count;

	nq = netdev_get_tx_queue(tx->adapter->netdev, tx->queue_index);
	__netif_tx_lock(nq, smp_processor_id());

	do {
		if (tx->read == tx->write)
			break;

		entry = &tx->entry[tx->read];
		if ((__le32_to_cpu(entry->desc_wb->properties) &
		     TSNEP_TX_DESC_OWNER_MASK) !=
		    (entry->properties & TSNEP_TX_DESC_OWNER_MASK))
			break;

		/* descriptor properties shall be read first, because valid data
		 * is signaled there
		 */
		dma_rmb();

		count = 1;
		if ((entry->type & TSNEP_TX_TYPE_SKB) &&
		    skb_shinfo(entry->skb)->nr_frags > 0)
			count += skb_shinfo(entry->skb)->nr_frags;
		else if (!(entry->type & TSNEP_TX_TYPE_SKB) &&
			 xdp_frame_has_frags(entry->xdpf))
			count += xdp_get_shared_info_from_frame(entry->xdpf)->nr_frags;

		length = tsnep_tx_unmap(tx, tx->read, count);

		if ((entry->type & TSNEP_TX_TYPE_SKB) &&
		    (skb_shinfo(entry->skb)->tx_flags & SKBTX_IN_PROGRESS) &&
		    (__le32_to_cpu(entry->desc_wb->properties) &
		     TSNEP_DESC_EXTENDED_WRITEBACK_FLAG)) {
			struct skb_shared_hwtstamps hwtstamps;
			u64 timestamp;

			if (skb_shinfo(entry->skb)->tx_flags &
			    SKBTX_HW_TSTAMP_USE_CYCLES)
				timestamp =
					__le64_to_cpu(entry->desc_wb->counter);
			else
				timestamp =
					__le64_to_cpu(entry->desc_wb->timestamp);

			memset(&hwtstamps, 0, sizeof(hwtstamps));
			hwtstamps.hwtstamp = ns_to_ktime(timestamp);

			skb_tstamp_tx(entry->skb, &hwtstamps);
		}

		if (entry->type & TSNEP_TX_TYPE_SKB)
			napi_consume_skb(entry->skb, napi_budget);
		else
			xdp_return_frame_rx_napi(entry->xdpf);
		/* xdpf is union with skb */
		entry->skb = NULL;

		tx->read = (tx->read + count) % TSNEP_RING_SIZE;

		tx->packets++;
		tx->bytes += length + ETH_FCS_LEN;

		budget--;
	} while (likely(budget));

	if ((tsnep_tx_desc_available(tx) >= ((MAX_SKB_FRAGS + 1) * 2)) &&
	    netif_tx_queue_stopped(nq)) {
		netif_tx_wake_queue(nq);
	}

	__netif_tx_unlock(nq);

	return budget != 0;
}

static bool tsnep_tx_pending(struct tsnep_tx *tx)
{
	struct tsnep_tx_entry *entry;
	struct netdev_queue *nq;
	bool pending = false;

	nq = netdev_get_tx_queue(tx->adapter->netdev, tx->queue_index);
	__netif_tx_lock(nq, smp_processor_id());

	if (tx->read != tx->write) {
		entry = &tx->entry[tx->read];
		if ((__le32_to_cpu(entry->desc_wb->properties) &
		     TSNEP_TX_DESC_OWNER_MASK) ==
		    (entry->properties & TSNEP_TX_DESC_OWNER_MASK))
			pending = true;
	}

	__netif_tx_unlock(nq);

	return pending;
}

static int tsnep_tx_open(struct tsnep_adapter *adapter, void __iomem *addr,
			 int queue_index, struct tsnep_tx *tx)
{
	dma_addr_t dma;
	int retval;

	memset(tx, 0, sizeof(*tx));
	tx->adapter = adapter;
	tx->addr = addr;
	tx->queue_index = queue_index;

	retval = tsnep_tx_ring_init(tx);
	if (retval)
		return retval;

	dma = tx->entry[0].desc_dma | TSNEP_RESET_OWNER_COUNTER;
	iowrite32(DMA_ADDR_LOW(dma), tx->addr + TSNEP_TX_DESC_ADDR_LOW);
	iowrite32(DMA_ADDR_HIGH(dma), tx->addr + TSNEP_TX_DESC_ADDR_HIGH);
	tx->owner_counter = 1;
	tx->increment_owner_counter = TSNEP_RING_SIZE - 1;

	return 0;
}

static void tsnep_tx_close(struct tsnep_tx *tx)
{
	u32 val;

	readx_poll_timeout(ioread32, tx->addr + TSNEP_CONTROL, val,
			   ((val & TSNEP_CONTROL_TX_ENABLE) == 0), 10000,
			   1000000);

	tsnep_tx_ring_cleanup(tx);
}

static void tsnep_rx_ring_cleanup(struct tsnep_rx *rx)
{
	struct device *dmadev = rx->adapter->dmadev;
	struct tsnep_rx_entry *entry;
	int i;

	for (i = 0; i < TSNEP_RING_SIZE; i++) {
		entry = &rx->entry[i];
		if (entry->page)
			page_pool_put_full_page(rx->page_pool, entry->page,
						false);
		entry->page = NULL;
	}

	if (rx->page_pool)
		page_pool_destroy(rx->page_pool);

	memset(rx->entry, 0, sizeof(rx->entry));

	for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
		if (rx->page[i]) {
			dma_free_coherent(dmadev, PAGE_SIZE, rx->page[i],
					  rx->page_dma[i]);
			rx->page[i] = NULL;
			rx->page_dma[i] = 0;
		}
	}
}

static int tsnep_rx_ring_init(struct tsnep_rx *rx)
{
	struct device *dmadev = rx->adapter->dmadev;
	struct tsnep_rx_entry *entry;
	struct page_pool_params pp_params = { 0 };
	struct tsnep_rx_entry *next_entry;
	int i, j;
	int retval;

	for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
		rx->page[i] =
			dma_alloc_coherent(dmadev, PAGE_SIZE, &rx->page_dma[i],
					   GFP_KERNEL);
		if (!rx->page[i]) {
			retval = -ENOMEM;
			goto failed;
		}
		for (j = 0; j < TSNEP_RING_ENTRIES_PER_PAGE; j++) {
			entry = &rx->entry[TSNEP_RING_ENTRIES_PER_PAGE * i + j];
			entry->desc_wb = (struct tsnep_rx_desc_wb *)
				(((u8 *)rx->page[i]) + TSNEP_DESC_SIZE * j);
			entry->desc = (struct tsnep_rx_desc *)
				(((u8 *)entry->desc_wb) + TSNEP_DESC_OFFSET);
			entry->desc_dma = rx->page_dma[i] + TSNEP_DESC_SIZE * j;
		}
	}

	pp_params.flags = PP_FLAG_DMA_MAP | PP_FLAG_DMA_SYNC_DEV;
	pp_params.order = 0;
	pp_params.pool_size = TSNEP_RING_SIZE;
	pp_params.nid = dev_to_node(dmadev);
	pp_params.dev = dmadev;
	pp_params.dma_dir = DMA_BIDIRECTIONAL;
	pp_params.max_len = TSNEP_MAX_RX_BUF_SIZE;
	pp_params.offset = TSNEP_RX_OFFSET;
	rx->page_pool = page_pool_create(&pp_params);
	if (IS_ERR(rx->page_pool)) {
		retval = PTR_ERR(rx->page_pool);
		rx->page_pool = NULL;
		goto failed;
	}

	for (i = 0; i < TSNEP_RING_SIZE; i++) {
		entry = &rx->entry[i];
		next_entry = &rx->entry[(i + 1) % TSNEP_RING_SIZE];
		entry->desc->next = __cpu_to_le64(next_entry->desc_dma);
	}

	return 0;

failed:
	tsnep_rx_ring_cleanup(rx);
	return retval;
}

static int tsnep_rx_desc_available(struct tsnep_rx *rx)
{
	if (rx->read <= rx->write)
		return TSNEP_RING_SIZE - rx->write + rx->read - 1;
	else
		return rx->read - rx->write - 1;
}

static void tsnep_rx_set_page(struct tsnep_rx *rx, struct tsnep_rx_entry *entry,
			      struct page *page)
{
	entry->page = page;
	entry->len = TSNEP_MAX_RX_BUF_SIZE;
	entry->dma = page_pool_get_dma_addr(entry->page);
	entry->desc->rx = __cpu_to_le64(entry->dma + TSNEP_RX_OFFSET);
}

static int tsnep_rx_alloc_buffer(struct tsnep_rx *rx, int index)
{
	struct tsnep_rx_entry *entry = &rx->entry[index];
	struct page *page;

	page = page_pool_dev_alloc_pages(rx->page_pool);
	if (unlikely(!page))
		return -ENOMEM;
	tsnep_rx_set_page(rx, entry, page);

	return 0;
}

static void tsnep_rx_reuse_buffer(struct tsnep_rx *rx, int index)
{
	struct tsnep_rx_entry *entry = &rx->entry[index];
	struct tsnep_rx_entry *read = &rx->entry[rx->read];

	tsnep_rx_set_page(rx, entry, read->page);
	read->page = NULL;
}

static void tsnep_rx_activate(struct tsnep_rx *rx, int index)
{
	struct tsnep_rx_entry *entry = &rx->entry[index];

	/* TSNEP_MAX_RX_BUF_SIZE is a multiple of 4 */
	entry->properties = entry->len & TSNEP_DESC_LENGTH_MASK;
	entry->properties |= TSNEP_DESC_INTERRUPT_FLAG;
	if (index == rx->increment_owner_counter) {
		rx->owner_counter++;
		if (rx->owner_counter == 4)
			rx->owner_counter = 1;
		rx->increment_owner_counter--;
		if (rx->increment_owner_counter < 0)
			rx->increment_owner_counter = TSNEP_RING_SIZE - 1;
	}
	entry->properties |=
		(rx->owner_counter << TSNEP_DESC_OWNER_COUNTER_SHIFT) &
		TSNEP_DESC_OWNER_COUNTER_MASK;

	/* descriptor properties shall be written last, because valid data is
	 * signaled there
	 */
	dma_wmb();

	entry->desc->properties = __cpu_to_le32(entry->properties);
}

static int tsnep_rx_refill(struct tsnep_rx *rx, int count, bool reuse)
{
	int index;
	bool alloc_failed = false;
	bool enable = false;
	int i;
	int retval;

	for (i = 0; i < count && !alloc_failed; i++) {
		index = (rx->write + i) % TSNEP_RING_SIZE;

		retval = tsnep_rx_alloc_buffer(rx, index);
		if (unlikely(retval)) {
			rx->alloc_failed++;
			alloc_failed = true;

			/* reuse only if no other allocation was successful */
			if (i == 0 && reuse)
				tsnep_rx_reuse_buffer(rx, index);
			else
				break;
		}

		tsnep_rx_activate(rx, index);

		enable = true;
	}

	if (enable) {
		rx->write = (rx->write + i) % TSNEP_RING_SIZE;

		/* descriptor properties shall be valid before hardware is
		 * notified
		 */
		dma_wmb();

		iowrite32(TSNEP_CONTROL_RX_ENABLE, rx->addr + TSNEP_CONTROL);
	}

	return i;
}

static bool tsnep_xdp_run_prog(struct tsnep_rx *rx, struct bpf_prog *prog,
			       struct xdp_buff *xdp, int *status,
			       struct netdev_queue *tx_nq, struct tsnep_tx *tx)
{
	unsigned int length;
	unsigned int sync;
	u32 act;

	length = xdp->data_end - xdp->data_hard_start - XDP_PACKET_HEADROOM;

	act = bpf_prog_run_xdp(prog, xdp);

	/* Due xdp_adjust_tail: DMA sync for_device cover max len CPU touch */
	sync = xdp->data_end - xdp->data_hard_start - XDP_PACKET_HEADROOM;
	sync = max(sync, length);

	switch (act) {
	case XDP_PASS:
		return false;
	case XDP_TX:
		if (!tsnep_xdp_xmit_back(rx->adapter, xdp, tx_nq, tx))
			goto out_failure;
		*status |= TSNEP_XDP_TX;
		return true;
	case XDP_REDIRECT:
		if (xdp_do_redirect(rx->adapter->netdev, xdp, prog) < 0)
			goto out_failure;
		*status |= TSNEP_XDP_REDIRECT;
		return true;
	default:
		bpf_warn_invalid_xdp_action(rx->adapter->netdev, prog, act);
		fallthrough;
	case XDP_ABORTED:
out_failure:
		trace_xdp_exception(rx->adapter->netdev, prog, act);
		fallthrough;
	case XDP_DROP:
		page_pool_put_page(rx->page_pool, virt_to_head_page(xdp->data),
				   sync, true);
		return true;
	}
}

static void tsnep_finalize_xdp(struct tsnep_adapter *adapter, int status,
			       struct netdev_queue *tx_nq, struct tsnep_tx *tx)
{
	if (status & TSNEP_XDP_TX) {
		__netif_tx_lock(tx_nq, smp_processor_id());
		tsnep_xdp_xmit_flush(tx);
		__netif_tx_unlock(tx_nq);
	}

	if (status & TSNEP_XDP_REDIRECT)
		xdp_do_flush();
}

static struct sk_buff *tsnep_build_skb(struct tsnep_rx *rx, struct page *page,
				       int length)
{
	struct sk_buff *skb;

	skb = napi_build_skb(page_address(page), PAGE_SIZE);
	if (unlikely(!skb))
		return NULL;

	/* update pointers within the skb to store the data */
	skb_reserve(skb, TSNEP_RX_OFFSET + TSNEP_RX_INLINE_METADATA_SIZE);
	__skb_put(skb, length - ETH_FCS_LEN);

	if (rx->adapter->hwtstamp_config.rx_filter == HWTSTAMP_FILTER_ALL) {
		struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb);
		struct tsnep_rx_inline *rx_inline =
			(struct tsnep_rx_inline *)(page_address(page) +
						   TSNEP_RX_OFFSET);

		skb_shinfo(skb)->tx_flags |=
			SKBTX_HW_TSTAMP_NETDEV;
		memset(hwtstamps, 0, sizeof(*hwtstamps));
		hwtstamps->netdev_data = rx_inline;
	}

	skb_record_rx_queue(skb, rx->queue_index);
	skb->protocol = eth_type_trans(skb, rx->adapter->netdev);

	return skb;
}

static int tsnep_rx_poll(struct tsnep_rx *rx, struct napi_struct *napi,
			 int budget)
{
	struct device *dmadev = rx->adapter->dmadev;
	enum dma_data_direction dma_dir;
	struct tsnep_rx_entry *entry;
	struct netdev_queue *tx_nq;
	struct bpf_prog *prog;
	struct xdp_buff xdp;
	struct sk_buff *skb;
	struct tsnep_tx *tx;
	int desc_available;
	int xdp_status = 0;
	int done = 0;
	int length;

	desc_available = tsnep_rx_desc_available(rx);
	dma_dir = page_pool_get_dma_dir(rx->page_pool);
	prog = READ_ONCE(rx->adapter->xdp_prog);
	if (prog) {
		tx_nq = netdev_get_tx_queue(rx->adapter->netdev,
					    rx->tx_queue_index);
		tx = &rx->adapter->tx[rx->tx_queue_index];

		xdp_init_buff(&xdp, PAGE_SIZE, &rx->xdp_rxq);
	}

	while (likely(done < budget) && (rx->read != rx->write)) {
		entry = &rx->entry[rx->read];
		if ((__le32_to_cpu(entry->desc_wb->properties) &
		     TSNEP_DESC_OWNER_COUNTER_MASK) !=
		    (entry->properties & TSNEP_DESC_OWNER_COUNTER_MASK))
			break;
		done++;

		if (desc_available >= TSNEP_RING_RX_REFILL) {
			bool reuse = desc_available >= TSNEP_RING_RX_REUSE;

			desc_available -= tsnep_rx_refill(rx, desc_available,
							  reuse);
			if (!entry->page) {
				/* buffer has been reused for refill to prevent
				 * empty RX ring, thus buffer cannot be used for
				 * RX processing
				 */
				rx->read = (rx->read + 1) % TSNEP_RING_SIZE;
				desc_available++;

				rx->dropped++;

				continue;
			}
		}

		/* descriptor properties shall be read first, because valid data
		 * is signaled there
		 */
		dma_rmb();

		prefetch(page_address(entry->page) + TSNEP_RX_OFFSET);
		length = __le32_to_cpu(entry->desc_wb->properties) &
			 TSNEP_DESC_LENGTH_MASK;
		dma_sync_single_range_for_cpu(dmadev, entry->dma,
					      TSNEP_RX_OFFSET, length, dma_dir);

		/* RX metadata with timestamps is in front of actual data,
		 * subtract metadata size to get length of actual data and
		 * consider metadata size as offset of actual data during RX
		 * processing
		 */
		length -= TSNEP_RX_INLINE_METADATA_SIZE;

		rx->read = (rx->read + 1) % TSNEP_RING_SIZE;
		desc_available++;

		if (prog) {
			bool consume;

			xdp_prepare_buff(&xdp, page_address(entry->page),
					 XDP_PACKET_HEADROOM + TSNEP_RX_INLINE_METADATA_SIZE,
					 length, false);

			consume = tsnep_xdp_run_prog(rx, prog, &xdp,
						     &xdp_status, tx_nq, tx);
			if (consume) {
				rx->packets++;
				rx->bytes += length;

				entry->page = NULL;

				continue;
			}
		}

		skb = tsnep_build_skb(rx, entry->page, length);
		if (skb) {
			page_pool_release_page(rx->page_pool, entry->page);

			rx->packets++;
			rx->bytes += length;
			if (skb->pkt_type == PACKET_MULTICAST)
				rx->multicast++;

			napi_gro_receive(napi, skb);
		} else {
			page_pool_recycle_direct(rx->page_pool, entry->page);

			rx->dropped++;
		}
		entry->page = NULL;
	}

	if (xdp_status)
		tsnep_finalize_xdp(rx->adapter, xdp_status, tx_nq, tx);

	if (desc_available)
		tsnep_rx_refill(rx, desc_available, false);

	return done;
}

static bool tsnep_rx_pending(struct tsnep_rx *rx)
{
	struct tsnep_rx_entry *entry;

	if (rx->read != rx->write) {
		entry = &rx->entry[rx->read];
		if ((__le32_to_cpu(entry->desc_wb->properties) &
		     TSNEP_DESC_OWNER_COUNTER_MASK) ==
		    (entry->properties & TSNEP_DESC_OWNER_COUNTER_MASK))
			return true;
	}

	return false;
}

static int tsnep_rx_open(struct tsnep_adapter *adapter, void __iomem *addr,
			 int queue_index, struct tsnep_rx *rx)
{
	dma_addr_t dma;
	int retval;

	memset(rx, 0, sizeof(*rx));
	rx->adapter = adapter;
	rx->addr = addr;
	rx->queue_index = queue_index;

	retval = tsnep_rx_ring_init(rx);
	if (retval)
		return retval;

	dma = rx->entry[0].desc_dma | TSNEP_RESET_OWNER_COUNTER;
	iowrite32(DMA_ADDR_LOW(dma), rx->addr + TSNEP_RX_DESC_ADDR_LOW);
	iowrite32(DMA_ADDR_HIGH(dma), rx->addr + TSNEP_RX_DESC_ADDR_HIGH);
	rx->owner_counter = 1;
	rx->increment_owner_counter = TSNEP_RING_SIZE - 1;

	tsnep_rx_refill(rx, tsnep_rx_desc_available(rx), false);

	return 0;
}

static void tsnep_rx_close(struct tsnep_rx *rx)
{
	u32 val;

	iowrite32(TSNEP_CONTROL_RX_DISABLE, rx->addr + TSNEP_CONTROL);
	readx_poll_timeout(ioread32, rx->addr + TSNEP_CONTROL, val,
			   ((val & TSNEP_CONTROL_RX_ENABLE) == 0), 10000,
			   1000000);

	tsnep_rx_ring_cleanup(rx);
}

static bool tsnep_pending(struct tsnep_queue *queue)
{
	if (queue->tx && tsnep_tx_pending(queue->tx))
		return true;

	if (queue->rx && tsnep_rx_pending(queue->rx))
		return true;

	return false;
}

static int tsnep_poll(struct napi_struct *napi, int budget)
{
	struct tsnep_queue *queue = container_of(napi, struct tsnep_queue,
						 napi);
	bool complete = true;
	int done = 0;

	if (queue->tx)
		complete = tsnep_tx_poll(queue->tx, budget);

	if (queue->rx) {
		done = tsnep_rx_poll(queue->rx, napi, budget);
		if (done >= budget)
			complete = false;
	}

	/* if all work not completed, return budget and keep polling */
	if (!complete)
		return budget;

	if (likely(napi_complete_done(napi, done))) {
		tsnep_enable_irq(queue->adapter, queue->irq_mask);

		/* reschedule if work is already pending, prevent rotten packets
		 * which are transmitted or received after polling but before
		 * interrupt enable
		 */
		if (tsnep_pending(queue)) {
			tsnep_disable_irq(queue->adapter, queue->irq_mask);
			napi_schedule(napi);
		}
	}

	return min(done, budget - 1);
}

static int tsnep_request_irq(struct tsnep_queue *queue, bool first)
{
	const char *name = netdev_name(queue->adapter->netdev);
	irq_handler_t handler;
	void *dev;
	int retval;

	if (first) {
		sprintf(queue->name, "%s-mac", name);
		handler = tsnep_irq;
		dev = queue->adapter;
	} else {
		if (queue->tx && queue->rx)
			sprintf(queue->name, "%s-txrx-%d", name,
				queue->rx->queue_index);
		else if (queue->tx)
			sprintf(queue->name, "%s-tx-%d", name,
				queue->tx->queue_index);
		else
			sprintf(queue->name, "%s-rx-%d", name,
				queue->rx->queue_index);
		handler = tsnep_irq_txrx;
		dev = queue;
	}

	retval = request_irq(queue->irq, handler, 0, queue->name, dev);
	if (retval) {
		/* if name is empty, then interrupt won't be freed */
		memset(queue->name, 0, sizeof(queue->name));
	}

	return retval;
}

static void tsnep_free_irq(struct tsnep_queue *queue, bool first)
{
	void *dev;

	if (!strlen(queue->name))
		return;

	if (first)
		dev = queue->adapter;
	else
		dev = queue;

	free_irq(queue->irq, dev);
	memset(queue->name, 0, sizeof(queue->name));
}

static void tsnep_queue_close(struct tsnep_queue *queue, bool first)
{
	struct tsnep_rx *rx = queue->rx;

	tsnep_free_irq(queue, first);

	if (rx && xdp_rxq_info_is_reg(&rx->xdp_rxq))
		xdp_rxq_info_unreg(&rx->xdp_rxq);

	netif_napi_del(&queue->napi);
}

static int tsnep_queue_open(struct tsnep_adapter *adapter,
			    struct tsnep_queue *queue, bool first)
{
	struct tsnep_rx *rx = queue->rx;
	struct tsnep_tx *tx = queue->tx;
	int retval;

	queue->adapter = adapter;

	netif_napi_add(adapter->netdev, &queue->napi, tsnep_poll);

	if (rx) {
		/* choose TX queue for XDP_TX */
		if (tx)
			rx->tx_queue_index = tx->queue_index;
		else if (rx->queue_index < adapter->num_tx_queues)
			rx->tx_queue_index = rx->queue_index;
		else
			rx->tx_queue_index = 0;

		retval = xdp_rxq_info_reg(&rx->xdp_rxq, adapter->netdev,
					  rx->queue_index, queue->napi.napi_id);
		if (retval)
			goto failed;
		retval = xdp_rxq_info_reg_mem_model(&rx->xdp_rxq,
						    MEM_TYPE_PAGE_POOL,
						    rx->page_pool);
		if (retval)
			goto failed;
	}

	retval = tsnep_request_irq(queue, first);
	if (retval) {
		netif_err(adapter, drv, adapter->netdev,
			  "can't get assigned irq %d.\n", queue->irq);
		goto failed;
	}

	return 0;

failed:
	tsnep_queue_close(queue, first);

	return retval;
}

static int tsnep_netdev_open(struct net_device *netdev)
{
	struct tsnep_adapter *adapter = netdev_priv(netdev);
	int tx_queue_index = 0;
	int rx_queue_index = 0;
	void __iomem *addr;
	int i, retval;

	for (i = 0; i < adapter->num_queues; i++) {
		if (adapter->queue[i].tx) {
			addr = adapter->addr + TSNEP_QUEUE(tx_queue_index);
			retval = tsnep_tx_open(adapter, addr, tx_queue_index,
					       adapter->queue[i].tx);
			if (retval)
				goto failed;
			tx_queue_index++;
		}
		if (adapter->queue[i].rx) {
			addr = adapter->addr + TSNEP_QUEUE(rx_queue_index);
			retval = tsnep_rx_open(adapter, addr, rx_queue_index,
					       adapter->queue[i].rx);
			if (retval)
				goto failed;
			rx_queue_index++;
		}

		retval = tsnep_queue_open(adapter, &adapter->queue[i], i == 0);
		if (retval)
			goto failed;
	}

	retval = netif_set_real_num_tx_queues(adapter->netdev,
					      adapter->num_tx_queues);
	if (retval)
		goto failed;
	retval = netif_set_real_num_rx_queues(adapter->netdev,
					      adapter->num_rx_queues);
	if (retval)
		goto failed;

	tsnep_enable_irq(adapter, ECM_INT_LINK);
	retval = tsnep_phy_open(adapter);
	if (retval)
		goto phy_failed;

	for (i = 0; i < adapter->num_queues; i++) {
		napi_enable(&adapter->queue[i].napi);

		tsnep_enable_irq(adapter, adapter->queue[i].irq_mask);
	}

	return 0;

phy_failed:
	tsnep_disable_irq(adapter, ECM_INT_LINK);
failed:
	for (i = 0; i < adapter->num_queues; i++) {
		tsnep_queue_close(&adapter->queue[i], i == 0);

		if (adapter->queue[i].rx)
			tsnep_rx_close(adapter->queue[i].rx);
		if (adapter->queue[i].tx)
			tsnep_tx_close(adapter->queue[i].tx);
	}
	return retval;
}

static int tsnep_netdev_close(struct net_device *netdev)
{
	struct tsnep_adapter *adapter = netdev_priv(netdev);
	int i;

	tsnep_disable_irq(adapter, ECM_INT_LINK);
	tsnep_phy_close(adapter);

	for (i = 0; i < adapter->num_queues; i++) {
		tsnep_disable_irq(adapter, adapter->queue[i].irq_mask);

		napi_disable(&adapter->queue[i].napi);

		tsnep_queue_close(&adapter->queue[i], i == 0);

		if (adapter->queue[i].rx)
			tsnep_rx_close(adapter->queue[i].rx);
		if (adapter->queue[i].tx)
			tsnep_tx_close(adapter->queue[i].tx);
	}

	return 0;
}

static netdev_tx_t tsnep_netdev_xmit_frame(struct sk_buff *skb,
					   struct net_device *netdev)
{
	struct tsnep_adapter *adapter = netdev_priv(netdev);
	u16 queue_mapping = skb_get_queue_mapping(skb);

	if (queue_mapping >= adapter->num_tx_queues)
		queue_mapping = 0;

	return tsnep_xmit_frame_ring(skb, &adapter->tx[queue_mapping]);
}

static int tsnep_netdev_ioctl(struct net_device *netdev, struct ifreq *ifr,
			      int cmd)
{
	if (!netif_running(netdev))
		return -EINVAL;
	if (cmd == SIOCSHWTSTAMP || cmd == SIOCGHWTSTAMP)
		return tsnep_ptp_ioctl(netdev, ifr, cmd);
	return phy_mii_ioctl(netdev->phydev, ifr, cmd);
}

static void tsnep_netdev_set_multicast(struct net_device *netdev)
{
	struct tsnep_adapter *adapter = netdev_priv(netdev);

	u16 rx_filter = 0;

	/* configured MAC address and broadcasts are never filtered */
	if (netdev->flags & IFF_PROMISC) {
		rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_MULTICASTS;
		rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_UNICASTS;
	} else if (!netdev_mc_empty(netdev) || (netdev->flags & IFF_ALLMULTI)) {
		rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_MULTICASTS;
	}
	iowrite16(rx_filter, adapter->addr + TSNEP_RX_FILTER);
}

static void tsnep_netdev_get_stats64(struct net_device *netdev,
				     struct rtnl_link_stats64 *stats)
{
	struct tsnep_adapter *adapter = netdev_priv(netdev);
	u32 reg;
	u32 val;
	int i;

	for (i = 0; i < adapter->num_tx_queues; i++) {
		stats->tx_packets += adapter->tx[i].packets;
		stats->tx_bytes += adapter->tx[i].bytes;
		stats->tx_dropped += adapter->tx[i].dropped;
	}
	for (i = 0; i < adapter->num_rx_queues; i++) {
		stats->rx_packets += adapter->rx[i].packets;
		stats->rx_bytes += adapter->rx[i].bytes;
		stats->rx_dropped += adapter->rx[i].dropped;
		stats->multicast += adapter->rx[i].multicast;

		reg = ioread32(adapter->addr + TSNEP_QUEUE(i) +
			       TSNEP_RX_STATISTIC);
		val = (reg & TSNEP_RX_STATISTIC_NO_DESC_MASK) >>
		      TSNEP_RX_STATISTIC_NO_DESC_SHIFT;
		stats->rx_dropped += val;
		val = (reg & TSNEP_RX_STATISTIC_BUFFER_TOO_SMALL_MASK) >>
		      TSNEP_RX_STATISTIC_BUFFER_TOO_SMALL_SHIFT;
		stats->rx_dropped += val;
		val = (reg & TSNEP_RX_STATISTIC_FIFO_OVERFLOW_MASK) >>
		      TSNEP_RX_STATISTIC_FIFO_OVERFLOW_SHIFT;
		stats->rx_errors += val;
		stats->rx_fifo_errors += val;
		val = (reg & TSNEP_RX_STATISTIC_INVALID_FRAME_MASK) >>
		      TSNEP_RX_STATISTIC_INVALID_FRAME_SHIFT;
		stats->rx_errors += val;
		stats->rx_frame_errors += val;
	}

	reg = ioread32(adapter->addr + ECM_STAT);
	val = (reg & ECM_STAT_RX_ERR_MASK) >> ECM_STAT_RX_ERR_SHIFT;
	stats->rx_errors += val;
	val = (reg & ECM_STAT_INV_FRM_MASK) >> ECM_STAT_INV_FRM_SHIFT;
	stats->rx_errors += val;
	stats->rx_crc_errors += val;
	val = (reg & ECM_STAT_FWD_RX_ERR_MASK) >> ECM_STAT_FWD_RX_ERR_SHIFT;
	stats->rx_errors += val;
}

static void tsnep_mac_set_address(struct tsnep_adapter *adapter, u8 *addr)
{
	iowrite32(*(u32 *)addr, adapter->addr + TSNEP_MAC_ADDRESS_LOW);
	iowrite16(*(u16 *)(addr + sizeof(u32)),
		  adapter->addr + TSNEP_MAC_ADDRESS_HIGH);

	ether_addr_copy(adapter->mac_address, addr);
	netif_info(adapter, drv, adapter->netdev, "MAC address set to %pM\n",
		   addr);
}

static int tsnep_netdev_set_mac_address(struct net_device *netdev, void *addr)
{
	struct tsnep_adapter *adapter = netdev_priv(netdev);
	struct sockaddr *sock_addr = addr;
	int retval;

	retval = eth_prepare_mac_addr_change(netdev, sock_addr);
	if (retval)
		return retval;
	eth_hw_addr_set(netdev, sock_addr->sa_data);
	tsnep_mac_set_address(adapter, sock_addr->sa_data);

	return 0;
}

static int tsnep_netdev_set_features(struct net_device *netdev,
				     netdev_features_t features)
{
	struct tsnep_adapter *adapter = netdev_priv(netdev);
	netdev_features_t changed = netdev->features ^ features;
	bool enable;
	int retval = 0;

	if (changed & NETIF_F_LOOPBACK) {
		enable = !!(features & NETIF_F_LOOPBACK);
		retval = tsnep_phy_loopback(adapter, enable);
	}

	return retval;
}

static ktime_t tsnep_netdev_get_tstamp(struct net_device *netdev,
				       const struct skb_shared_hwtstamps *hwtstamps,
				       bool cycles)
{
	struct tsnep_rx_inline *rx_inline = hwtstamps->netdev_data;
	u64 timestamp;

	if (cycles)
		timestamp = __le64_to_cpu(rx_inline->counter);
	else
		timestamp = __le64_to_cpu(rx_inline->timestamp);

	return ns_to_ktime(timestamp);
}

static int tsnep_netdev_bpf(struct net_device *dev, struct netdev_bpf *bpf)
{
	struct tsnep_adapter *adapter = netdev_priv(dev);

	switch (bpf->command) {
	case XDP_SETUP_PROG:
		return tsnep_xdp_setup_prog(adapter, bpf->prog, bpf->extack);
	default:
		return -EOPNOTSUPP;
	}
}

static struct tsnep_tx *tsnep_xdp_get_tx(struct tsnep_adapter *adapter, u32 cpu)
{
	if (cpu >= TSNEP_MAX_QUEUES)
		cpu &= TSNEP_MAX_QUEUES - 1;

	while (cpu >= adapter->num_tx_queues)
		cpu -= adapter->num_tx_queues;

	return &adapter->tx[cpu];
}

static int tsnep_netdev_xdp_xmit(struct net_device *dev, int n,
				 struct xdp_frame **xdp, u32 flags)
{
	struct tsnep_adapter *adapter = netdev_priv(dev);
	u32 cpu = smp_processor_id();
	struct netdev_queue *nq;
	struct tsnep_tx *tx;
	int nxmit;
	bool xmit;

	if (unlikely(flags & ~XDP_XMIT_FLAGS_MASK))
		return -EINVAL;

	tx = tsnep_xdp_get_tx(adapter, cpu);
	nq = netdev_get_tx_queue(adapter->netdev, tx->queue_index);

	__netif_tx_lock(nq, cpu);

	for (nxmit = 0; nxmit < n; nxmit++) {
		xmit = tsnep_xdp_xmit_frame_ring(xdp[nxmit], tx,
						 TSNEP_TX_TYPE_XDP_NDO);
		if (!xmit)
			break;

		/* avoid transmit queue timeout since we share it with the slow
		 * path
		 */
		txq_trans_cond_update(nq);
	}

	if (flags & XDP_XMIT_FLUSH)
		tsnep_xdp_xmit_flush(tx);

	__netif_tx_unlock(nq);

	return nxmit;
}

static const struct net_device_ops tsnep_netdev_ops = {
	.ndo_open = tsnep_netdev_open,
	.ndo_stop = tsnep_netdev_close,
	.ndo_start_xmit = tsnep_netdev_xmit_frame,
	.ndo_eth_ioctl = tsnep_netdev_ioctl,
	.ndo_set_rx_mode = tsnep_netdev_set_multicast,
	.ndo_get_stats64 = tsnep_netdev_get_stats64,
	.ndo_set_mac_address = tsnep_netdev_set_mac_address,
	.ndo_set_features = tsnep_netdev_set_features,
	.ndo_get_tstamp = tsnep_netdev_get_tstamp,
	.ndo_setup_tc = tsnep_tc_setup,
	.ndo_bpf = tsnep_netdev_bpf,
	.ndo_xdp_xmit = tsnep_netdev_xdp_xmit,
};

static int tsnep_mac_init(struct tsnep_adapter *adapter)
{
	int retval;

	/* initialize RX filtering, at least configured MAC address and
	 * broadcast are not filtered
	 */
	iowrite16(0, adapter->addr + TSNEP_RX_FILTER);

	/* try to get MAC address in the following order:
	 * - device tree
	 * - valid MAC address already set
	 * - MAC address register if valid
	 * - random MAC address
	 */
	retval = of_get_mac_address(adapter->pdev->dev.of_node,
				    adapter->mac_address);
	if (retval == -EPROBE_DEFER)
		return retval;
	if (retval && !is_valid_ether_addr(adapter->mac_address)) {
		*(u32 *)adapter->mac_address =
			ioread32(adapter->addr + TSNEP_MAC_ADDRESS_LOW);
		*(u16 *)(adapter->mac_address + sizeof(u32)) =
			ioread16(adapter->addr + TSNEP_MAC_ADDRESS_HIGH);
		if (!is_valid_ether_addr(adapter->mac_address))
			eth_random_addr(adapter->mac_address);
	}

	tsnep_mac_set_address(adapter, adapter->mac_address);
	eth_hw_addr_set(adapter->netdev, adapter->mac_address);

	return 0;
}

static int tsnep_mdio_init(struct tsnep_adapter *adapter)
{
	struct device_node *np = adapter->pdev->dev.of_node;
	int retval;

	if (np) {
		np = of_get_child_by_name(np, "mdio");
		if (!np)
			return 0;

		adapter->suppress_preamble =
			of_property_read_bool(np, "suppress-preamble");
	}

	adapter->mdiobus = devm_mdiobus_alloc(&adapter->pdev->dev);
	if (!adapter->mdiobus) {
		retval = -ENOMEM;

		goto out;
	}

	adapter->mdiobus->priv = (void *)adapter;
	adapter->mdiobus->parent = &adapter->pdev->dev;
	adapter->mdiobus->read = tsnep_mdiobus_read;
	adapter->mdiobus->write = tsnep_mdiobus_write;
	adapter->mdiobus->name = TSNEP "-mdiobus";
	snprintf(adapter->mdiobus->id, MII_BUS_ID_SIZE, "%s",
		 adapter->pdev->name);

	/* do not scan broadcast address */
	adapter->mdiobus->phy_mask = 0x0000001;

	retval = of_mdiobus_register(adapter->mdiobus, np);

out:
	of_node_put(np);

	return retval;
}

static int tsnep_phy_init(struct tsnep_adapter *adapter)
{
	struct device_node *phy_node;
	int retval;

	retval = of_get_phy_mode(adapter->pdev->dev.of_node,
				 &adapter->phy_mode);
	if (retval)
		adapter->phy_mode = PHY_INTERFACE_MODE_GMII;

	phy_node = of_parse_phandle(adapter->pdev->dev.of_node, "phy-handle",
				    0);
	adapter->phydev = of_phy_find_device(phy_node);
	of_node_put(phy_node);
	if (!adapter->phydev && adapter->mdiobus)
		adapter->phydev = phy_find_first(adapter->mdiobus);
	if (!adapter->phydev)
		return -EIO;

	return 0;
}

static int tsnep_queue_init(struct tsnep_adapter *adapter, int queue_count)
{
	u32 irq_mask = ECM_INT_TX_0 | ECM_INT_RX_0;
	char name[8];
	int i;
	int retval;

	/* one TX/RX queue pair for netdev is mandatory */
	if (platform_irq_count(adapter->pdev) == 1)
		retval = platform_get_irq(adapter->pdev, 0);
	else
		retval = platform_get_irq_byname(adapter->pdev, "mac");
	if (retval < 0)
		return retval;
	adapter->num_tx_queues = 1;
	adapter->num_rx_queues = 1;
	adapter->num_queues = 1;
	adapter->queue[0].irq = retval;
	adapter->queue[0].tx = &adapter->tx[0];
	adapter->queue[0].rx = &adapter->rx[0];
	adapter->queue[0].irq_mask = irq_mask;
	adapter->queue[0].irq_delay_addr = adapter->addr + ECM_INT_DELAY;
	retval = tsnep_set_irq_coalesce(&adapter->queue[0],
					TSNEP_COALESCE_USECS_DEFAULT);
	if (retval < 0)
		return retval;

	adapter->netdev->irq = adapter->queue[0].irq;

	/* add additional TX/RX queue pairs only if dedicated interrupt is
	 * available
	 */
	for (i = 1; i < queue_count; i++) {
		sprintf(name, "txrx-%d", i);
		retval = platform_get_irq_byname_optional(adapter->pdev, name);
		if (retval < 0)
			break;

		adapter->num_tx_queues++;
		adapter->num_rx_queues++;
		adapter->num_queues++;
		adapter->queue[i].irq = retval;
		adapter->queue[i].tx = &adapter->tx[i];
		adapter->queue[i].rx = &adapter->rx[i];
		adapter->queue[i].irq_mask =
			irq_mask << (ECM_INT_TXRX_SHIFT * i);
		adapter->queue[i].irq_delay_addr =
			adapter->addr + ECM_INT_DELAY + ECM_INT_DELAY_OFFSET * i;
		retval = tsnep_set_irq_coalesce(&adapter->queue[i],
						TSNEP_COALESCE_USECS_DEFAULT);
		if (retval < 0)
			return retval;
	}

	return 0;
}

static int tsnep_probe(struct platform_device *pdev)
{
	struct tsnep_adapter *adapter;
	struct net_device *netdev;
	struct resource *io;
	u32 type;
	int revision;
	int version;
	int queue_count;
	int retval;

	netdev = devm_alloc_etherdev_mqs(&pdev->dev,
					 sizeof(struct tsnep_adapter),
					 TSNEP_MAX_QUEUES, TSNEP_MAX_QUEUES);
	if (!netdev)
		return -ENODEV;
	SET_NETDEV_DEV(netdev, &pdev->dev);
	adapter = netdev_priv(netdev);
	platform_set_drvdata(pdev, adapter);
	adapter->pdev = pdev;
	adapter->dmadev = &pdev->dev;
	adapter->netdev = netdev;
	adapter->msg_enable = NETIF_MSG_DRV | NETIF_MSG_PROBE |
			      NETIF_MSG_LINK | NETIF_MSG_IFUP |
			      NETIF_MSG_IFDOWN | NETIF_MSG_TX_QUEUED;

	netdev->min_mtu = ETH_MIN_MTU;
	netdev->max_mtu = TSNEP_MAX_FRAME_SIZE;

	mutex_init(&adapter->gate_control_lock);
	mutex_init(&adapter->rxnfc_lock);
	INIT_LIST_HEAD(&adapter->rxnfc_rules);

	io = platform_get_resource(pdev, IORESOURCE_MEM, 0);
	adapter->addr = devm_ioremap_resource(&pdev->dev, io);
	if (IS_ERR(adapter->addr))
		return PTR_ERR(adapter->addr);
	netdev->mem_start = io->start;
	netdev->mem_end = io->end;

	type = ioread32(adapter->addr + ECM_TYPE);
	revision = (type & ECM_REVISION_MASK) >> ECM_REVISION_SHIFT;
	version = (type & ECM_VERSION_MASK) >> ECM_VERSION_SHIFT;
	queue_count = (type & ECM_QUEUE_COUNT_MASK) >> ECM_QUEUE_COUNT_SHIFT;
	adapter->gate_control = type & ECM_GATE_CONTROL;
	adapter->rxnfc_max = TSNEP_RX_ASSIGN_ETHER_TYPE_COUNT;

	tsnep_disable_irq(adapter, ECM_INT_ALL);

	retval = tsnep_queue_init(adapter, queue_count);
	if (retval)
		return retval;

	retval = dma_set_mask_and_coherent(&adapter->pdev->dev,
					   DMA_BIT_MASK(64));
	if (retval) {
		dev_err(&adapter->pdev->dev, "no usable DMA configuration.\n");
		return retval;
	}

	retval = tsnep_mac_init(adapter);
	if (retval)
		return retval;

	retval = tsnep_mdio_init(adapter);
	if (retval)
		goto mdio_init_failed;

	retval = tsnep_phy_init(adapter);
	if (retval)
		goto phy_init_failed;

	retval = tsnep_ptp_init(adapter);
	if (retval)
		goto ptp_init_failed;

	retval = tsnep_tc_init(adapter);
	if (retval)
		goto tc_init_failed;

	retval = tsnep_rxnfc_init(adapter);
	if (retval)
		goto rxnfc_init_failed;

	netdev->netdev_ops = &tsnep_netdev_ops;
	netdev->ethtool_ops = &tsnep_ethtool_ops;
	netdev->features = NETIF_F_SG;
	netdev->hw_features = netdev->features | NETIF_F_LOOPBACK;

	netdev->xdp_features = NETDEV_XDP_ACT_BASIC | NETDEV_XDP_ACT_REDIRECT |
			       NETDEV_XDP_ACT_NDO_XMIT |
			       NETDEV_XDP_ACT_NDO_XMIT_SG;

	/* carrier off reporting is important to ethtool even BEFORE open */
	netif_carrier_off(netdev);

	retval = register_netdev(netdev);
	if (retval)
		goto register_failed;

	dev_info(&adapter->pdev->dev, "device version %d.%02d\n", version,
		 revision);
	if (adapter->gate_control)
		dev_info(&adapter->pdev->dev, "gate control detected\n");

	return 0;

register_failed:
	tsnep_rxnfc_cleanup(adapter);
rxnfc_init_failed:
	tsnep_tc_cleanup(adapter);
tc_init_failed:
	tsnep_ptp_cleanup(adapter);
ptp_init_failed:
phy_init_failed:
	if (adapter->mdiobus)
		mdiobus_unregister(adapter->mdiobus);
mdio_init_failed:
	return retval;
}

static int tsnep_remove(struct platform_device *pdev)
{
	struct tsnep_adapter *adapter = platform_get_drvdata(pdev);

	unregister_netdev(adapter->netdev);

	tsnep_rxnfc_cleanup(adapter);

	tsnep_tc_cleanup(adapter);

	tsnep_ptp_cleanup(adapter);

	if (adapter->mdiobus)
		mdiobus_unregister(adapter->mdiobus);

	tsnep_disable_irq(adapter, ECM_INT_ALL);

	return 0;
}

static const struct of_device_id tsnep_of_match[] = {
	{ .compatible = "engleder,tsnep", },
{ },
};
MODULE_DEVICE_TABLE(of, tsnep_of_match);

static struct platform_driver tsnep_driver = {
	.driver = {
		.name = TSNEP,
		.of_match_table = tsnep_of_match,
	},
	.probe = tsnep_probe,
	.remove = tsnep_remove,
};
module_platform_driver(tsnep_driver);

MODULE_AUTHOR("Gerhard Engleder <gerhard@engleder-embedded.com>");
MODULE_DESCRIPTION("TSN endpoint Ethernet MAC driver");
MODULE_LICENSE("GPL");