// SPDX-License-Identifier: GPL-2.0

/*
 * This module implements DC Mode Ext in software.
 * It is essentially an adaptation of the software implementation of
 * DC Mode.
 * As per include/directconnect_dp_api.h, DC Mode Ext is usually used
 * for the Lightning Mountain SoC.
 * As dsl_gfast_drv_dp-prx/src/drv_dsl_vrx618_dp.c from the VRX619 driver says,
 * Lightning Mountain is little-endian.
 *
 * Two different naming schemes are used: (1) the one from the documentation and
 * (2) the one from the DC DP code. They are equivalent, though:
 *   Documentation  |    Source Code
 *   ---------------|---------------
 *      TXIN ADD    |    soc2dev_enq
 *      TXIN SUB    |    soc2dev_deq
 *     TXOUT ADD    |    soc2dev_ret_enq
 *     TXOUT SUB    |    soc2dev_ret_deq
 *      RXIN ADD    |    dev2soc_ret_enq
 *      RXIN SUB    |    dev2soc_ret_deq
 *     RXOUT ADD    |    dev2soc_enq
 *     RXOUT SUB    |    dev2soc_deq
 *
 * Synchronization:
 *
 * 1. tx_skb_list and tx_active_buffers per-device data are accessed
 * by TXIN and TXOUT handling. The tx_lock spinlock protects that data.
 */

// #define DC_SW_DEBUG_ENABLED
// #define DC_SW_DEBUG_CONTENT

#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/netdevice.h>
#include <linux/hrtimer.h>
#include <linux/interrupt.h>
#include <linux/dma-mapping.h>
#include <linux/pci.h>
#include <linux/skbuff.h>
#include <linux/bitfield.h>

#include <directconnect_dp_dcmode_api.h>

/* the VRX619 may fail when handling packets that cross a 4k boundary */
#define BUFFER_FIXUP_SHIFT 12

static inline bool buffer_needs_fixup(void *start, size_t len)
{
	return  ((uintptr_t)start >> BUFFER_FIXUP_SHIFT) !=
	       (((uintptr_t)start + len - 1) >> BUFFER_FIXUP_SHIFT);
}

struct kmem_cache *dcdp_tx_copy_cache __ro_after_init;

static inline struct kmem_cache *
dcdp_tx_copy_cache_get(size_t size)
{
	if (size <= 2048)
		return dcdp_tx_copy_cache;
	else
		return NULL;
}

struct skb_cb {
	size_t size;
	dma_addr_t dma;
	void *copy;
};

static inline struct skb_cb *skb_cb(struct sk_buff *skb) {
	BUILD_BUG_ON(sizeof(struct skb_cb) > sizeof(skb->cb));
	BUILD_BUG_ON(__alignof__(struct skb_cb) > 8);

	return (struct skb_cb *)skb->cb;
}

/* BEGIN: ADAPTED FROM DSL DRIVER FOR STATISTICS. */
struct dsl_dp_plat_ctx_s;

#define DSL_DP_OUTQ_PNUM		2
#define DSL_DP_PTM_PRIO_Q_NUM		8

/* cast sk_buff pointer to control register and dereference it */
#define BYTE_OFFSET_MASK 0x7

/* END: ADAPTED FROM DSL DRIVER FOR STATISTICS. */

#define DCMODE_EXT_SW_MAX_SUBIF_PER_DEV 32
#define DCMODE_EXT_SW_SUBIFID_MASK      0x1F
#define DCMODE_EXT_SW_SUBIFID_OFFSET    8
#define DCMODE_EXT_SW_GET_SUBIFIDX(subif_id) \
            ((subif_id >> DCMODE_EXT_SW_SUBIFID_OFFSET) & DCMODE_EXT_SW_SUBIFID_MASK)
#define DCMODE_EXT_SW_SET_SUBIFIDX(subif_id) \
            ((subif_id & DCMODE_EXT_SW_SUBIFID_MASK) << DCMODE_EXT_SW_SUBIFID_OFFSET)
#define DCMODE_EXT_SW_MAX_DEV_NUM 16
#define HANDLE_RINGS_INTERVAL_NS (500 * 1000)

#define RX_BH_DESC_BUDGET 32
#define TX_BH_DESC_BUDGET 32

#define TX_DESC_RING_SIZE 128
#define RX_DESC_RING_SIZE 256

#define TXOUT_MAGIC 0x886359ac /* read from /dev/urandom */

#ifdef DC_SW_DEBUG_ENABLED
#define DC_SW_DEBUG(fmt, args...) pr_info("DC SW [%s:%d] " fmt, __func__, __LINE__, ##args)
#define DC_SW_DEBUG_RATELIMITED(fmt, args...) pr_info_ratelimited("DC SW [%s:%d] " fmt, __func__, __LINE__, ##args)
#else
static inline __printf(1, 2) void DC_SW_DEBUG(const char *fmt, ...) {}
static inline __printf(1, 2) void DC_SW_DEBUG_RATELIMITED(const char *fmt, ...) {}
#ifdef DC_SW_DEBUG_CONTENT
#error "DC_SW_DEBUG_CONTENT is active, but DC_SW_DEBUG_ENABLED is not. This combination is not supported."
#endif
#endif

/* Descriptor formats and counter types taken from SoC_Host_Descriptor_Formats.xlsx. */
/* This is both a TXIN and RXOUT buffer descriptor, since they are the same. */
struct desc_4dw_hw {
	__le32 dw0;
	__le32 dw1;
	__le32 dw2;
	__le32 dw3;
} __aligned(16);

struct desc_4dw {
	u32 dw0;
	u32 dw1;
	u32 dw2;
	u32 dw3;
};

#define DC_EXT_SW_DESC_DW0_STATION_ID	GENMASK(7, 0)
#define DC_EXT_SW_DESC_DW0_VAP_ID	GENMASK(14, 8)
#define DC_EXT_SW_DESC_DW0_MULTICAST	BIT(15)
#define DC_EXT_SW_DESC_DW0_SUBIF_ID	GENMASK(15, 0)
#define DC_EXT_SW_DESC_DW0_ETH_TYPE	GENMASK(17, 16)

#define DC_EXT_SW_DESC_DW1_ADDR_MSB	GENMASK(3, 0)
#define DC_EXT_SW_DESC_DW1_PMAC		BIT(7)
#define DC_EXT_SW_DESC_DW1_DEV_QOS	GENMASK(11, 8)
#define DC_EXT_SW_DESC_DW1_PRE_L2	GENMASK(13, 12)
#define DC_EXT_SW_DESC_DW1_COLOR	GENMASK(15, 14)
#define DC_EXT_SW_DESC_DW1_EP		GENMASK(23, 16)
#define DC_EXT_SW_DESC_DW1_FRAG_COUNT	GENMASK(27, 24)
#define DC_EXT_SW_DESC_DW1_CLASS	GENMASK(31, 28)

#define DC_EXT_SW_DESC_DW2_ADDR		GENMASK(31, 0)
#define DC_EXT_SW_DESC_DW2_ADDR_OFF	GENMASK(3, 0)
#define DC_EXT_SW_DESC_DW2_ADDR_PTR	GENMASK(31, 4)

#define DC_EXT_SW_DESC_DW3_SIZE		GENMASK(13, 0)
#define DC_EXT_SW_DESC_DW3_POLICY	GENMASK(23, 16)
#define DC_EXT_SW_DESC_DW3_POOL		GENMASK(27, 24)
#define DC_EXT_SW_DESC_DW3_EOP		BIT(28)
#define DC_EXT_SW_DESC_DW3_SOP		BIT(29)
#define DC_EXT_SW_DESC_DW3_OWN		BIT(31)

struct desc_2dw_hw {
	__le32 dw0;
	__le32 dw1;
} __aligned(8);

struct desc_2dw {
	u32 dw0;
	u32 dw1;
};

#define DC_EXT_SW_DESC_RET_DW0_ADDR	GENMASK(31, 0)

#define DC_EXT_SW_DESC_RET_DW1_POOL	GENMASK(3, 0)
#define DC_EXT_SW_DESC_RET_DW1_POLICY	GENMASK(14, 21) /* Tx-Out  only */
#define DC_EXT_SW_DESC_RET_DW1_ADDR_MSB	GENMASK(26, 23)

struct hostif_acc_counters {
	__le32 rxin_hd_add;
	__le32 txin_hd_add;
	__le32 rxout_hd_add;
	__le32 txout_hd_add;
	__le32 rsvd_add[6];
	__le32 rxin_hd_sub;
	__le32 txin_hd_sub;
	__le32 rxout_hd_sub;
	__le32 txout_hd_sub;
	__le32 rsvd_sub[6];
	__le32 rxin_hd_count;
	__le32 txin_hd_count;
	__le32 rxout_hd_count;
	__le32 txout_hd_count;
	__le32 rsvd_count[6];
};

struct _ring {
	const char *name;
	size_t entry_num;
	void *mem;
	dma_addr_t dma_addr;

	size_t head;
	u32 sw_count;
};

static inline void *
_ring_at(size_t entry_size, const struct _ring *ring, size_t at)
{
	return ring->mem + entry_size * at;
}

static inline void *
_ring_head(size_t entry_size, const struct _ring *ring)
{
	return _ring_at(entry_size, ring, ring->head);
}

static inline void
_ring_step(struct _ring *ring)
{
	ring->head = (ring->head + 1) % ring->entry_num;
	ring->sw_count++;
}

static int
_ring_init(size_t entry_size, struct _ring *ring, struct device *dev, const char *name,
	   size_t entry_num)
{
	size_t size;

	ring->name = name;
	ring->entry_num = entry_num;
	ring->head = 0;
	ring->sw_count = 0;

	size = entry_num * entry_size;
	ring->mem = dma_alloc_coherent(dev, size, &ring->dma_addr,
				       GFP_KERNEL | GFP_DMA | __GFP_ZERO);
	if (!ring->mem) {
		DC_DP_ERROR("%s: failed allocating DMA ring: out of memory.\n",
			    __func__);
		return -ENOMEM;
	}

	DC_SW_DEBUG("%s ring size: %u\n", name, ring->entry_num);

	return 0;
}

static void
_ring_init_dcdp_data(size_t entry_size, struct _ring *ring, struct dc_dp_ring *ring_data)
{
	ring_data->base = ring->mem;
	ring_data->phys_base = (void *)ring->dma_addr;
	ring_data->ring = ring;
	ring_data->desc_dwsz = entry_size / 4;

}

static inline size_t
_ring_get_entry_num(const struct _ring *ring)
{
	return ring->entry_num;
}

static inline u32
_ring_get_sw_count(const struct _ring *ring)
{
	return ring->sw_count;
}

static void
_ring_dump(size_t entry_size, void (*dump_at)(struct seq_file *seq, const void *at),
	   const struct _ring *ring, struct seq_file *seq)
{
	size_t head = READ_ONCE(ring->head), i;

	seq_printf(seq, "\t%s:\n\t\tphys. addr = %pad\n\t\tvirt. addr = %p\n\t\thead = %u\n",
		   ring->name, &ring->dma_addr, ring->mem, head);

	for (i = 0; i < ring->entry_num; ++i) {
		void *at = _ring_at(entry_size, ring, i);

		seq_puts(seq, "\t\t");
		dump_at(seq, at);
		if (i == head)
			seq_puts(seq, " <-");
		seq_puts(seq, "\n");
	}
}

static void
_ring_free(size_t entry_size, struct _ring *ring, struct device *dev)
{
	if (!ring)
		return;

	dma_free_coherent(dev, ring->entry_num * entry_size, ring->mem, ring->dma_addr);
}

/* type safe aliases */
#define RING_FOR_TYPE(name, type, type_sw) \
	struct ring_ ## name { \
		struct _ring untyped; \
	}; \
	\
	static inline type * \
	ring_ ## name ## _at(const struct ring_ ## name *ring, size_t at) \
	{ \
		return _ring_at(sizeof(type), &ring->untyped, at); \
	} \
	\
	static inline type * \
	ring_ ## name ## _head(const struct ring_ ## name *ring) \
	{ \
		return _ring_head(sizeof(type), &ring->untyped); \
	} \
	\
	static inline void \
	ring_ ## name ## _step(struct ring_ ## name *ring) \
	{ \
		return _ring_step(&ring->untyped); \
	} \
	\
	static inline int \
	ring_ ## name ## _init(struct ring_ ## name *ring, struct device *dev, const char *name, \
			       size_t entry_num) \
	{ \
		return _ring_init(sizeof(type), &ring->untyped, dev, name, entry_num); \
	} \
	\
	static inline void \
	ring_ ## name ## _init_dcdp_data(struct ring_ ## name *ring, struct dc_dp_ring *ring_data) \
	{ \
		_ring_init_dcdp_data(sizeof(type), &ring->untyped, ring_data); \
	} \
	\
	static inline size_t \
	ring_ ## name ## _get_entry_num(const struct ring_ ## name *ring) \
	{ \
		return _ring_get_entry_num(&ring->untyped); \
	} \
	\
	static inline u32 \
	ring_ ## name ## _get_sw_count(const struct ring_ ## name *ring) \
	{ \
		return _ring_get_sw_count(&ring->untyped); \
	} \
	\
	static inline void \
	ring_ ## name ## _free(struct ring_ ## name *ring, struct device *dev) \
	{ \
		_ring_free(sizeof(type), &ring->untyped, dev); \
	} \
	\
	static void \
	ring_ ## name ## _dump_at(struct seq_file *seq, const type *at); \
	\
	static bool \
	ring_ ## name ## _read(const struct ring_ ## name *ring, type_sw *dst); \
	\
	static void \
	ring_ ## name ## _return(struct ring_ ## name *ring); \
	\
	static void \
	ring_ ## name ## _write(const struct ring_ ## name *ring, const type_sw *src); \
	\
	static void \
	_ring_ ## name ## _dump_at(struct seq_file *seq, const void *at) \
	{ \
		ring_ ## name ## _dump_at(seq, at); \
	} \
	\
	static void \
	ring_ ## name ## _dump(const struct ring_ ## name *ring, struct seq_file *seq) \
	{ \
		_ring_dump(sizeof(type), &_ring_ ## name ## _dump_at, &ring->untyped, seq); \
	} \
	\
	static bool \
	ring_ ## name ## _try_get(struct ring_ ## name *ring, type_sw *dst) \
	{ \
		if (!ring_ ## name ## _read(ring, dst)) \
			return false; \
		ring_ ## name ## _return(ring); \
		return true; \
	} \
	\
	static void \
	ring_ ## name ## _put(struct ring_ ## name *ring, const type_sw *src) \
	{ \
		ring_ ## name ## _write(ring, src); \
		ring_ ## name ## _step(ring); \
	}


RING_FOR_TYPE(desc_4dw, struct desc_4dw_hw, struct desc_4dw)
RING_FOR_TYPE(desc_2dw, struct desc_2dw_hw, struct desc_2dw)

static bool
ring_desc_4dw_read(const struct ring_desc_4dw *ring, struct desc_4dw *dst)
{
	const struct desc_4dw_hw *src = ring_desc_4dw_head(ring);

	dst->dw3 = le32_to_cpu(src->dw3);
	if (FIELD_GET(DC_EXT_SW_DESC_DW3_OWN, dst->dw3))
		return false;
	dma_rmb(); /* ensure the desc is read after the own bit was correct */
	dst->dw0 = le32_to_cpu(src->dw0);
	dst->dw1 = le32_to_cpu(src->dw1);
	dst->dw2 = le32_to_cpu(src->dw2);
	return true;
}

static void
ring_desc_4dw_return(struct ring_desc_4dw *ring)
{
	struct desc_4dw_hw *head = ring_desc_4dw_head(ring);

	head->dw3 |= cpu_to_le32(FIELD_PREP(DC_EXT_SW_DESC_DW3_OWN, 1));
	ring_desc_4dw_step(ring);
}

static void
ring_desc_4dw_write(const struct ring_desc_4dw *ring, const struct desc_4dw *src)
{
	struct desc_4dw_hw *dst = ring_desc_4dw_head(ring);

	dst->dw0 = cpu_to_le32(src->dw0);
	dst->dw1 = cpu_to_le32(src->dw1);
	dst->dw2 = cpu_to_le32(src->dw2);
	dma_wmb(); /* ensure the own bit is written after the content */
	dst->dw3 = cpu_to_le32(src->dw3);
}

static void
ring_desc_4dw_dump_at(struct seq_file *seq, const struct desc_4dw_hw *at)
{
	seq_printf(seq, "0x%08x,0x%08x,0x%08x,0x%08x",
		   le32_to_cpu(at->dw0),
		   le32_to_cpu(at->dw1),
		   le32_to_cpu(at->dw2),
		   le32_to_cpu(at->dw3));
}

static bool
ring_desc_2dw_read(const struct ring_desc_2dw *ring, struct desc_2dw *dst)
{
	const struct desc_2dw_hw *src = ring_desc_2dw_head(ring);

	dst->dw1 = le32_to_cpu(src->dw1);
	if (dst->dw1 != TXOUT_MAGIC)
		return false;
	dma_rmb(); /* ensure the desc is read after the magic value was correct */
	dst->dw0 = le32_to_cpu(src->dw0);
	return true;
}

static void
ring_desc_2dw_return(struct ring_desc_2dw *ring)
{
	struct desc_2dw_hw *head = ring_desc_2dw_head(ring);

	head->dw1 = 0;
	ring_desc_2dw_step(ring);
}

static void
ring_desc_2dw_write(const struct ring_desc_2dw *ring, const struct desc_2dw *src)
{
	struct desc_2dw_hw *dst = ring_desc_2dw_head(ring);

	dst->dw0 = le32_to_cpu(src->dw0);
	dst->dw1 = le32_to_cpu(src->dw1);
}

static void
ring_desc_2dw_dump_at(struct seq_file *seq, const struct desc_2dw_hw *at)
{
	seq_printf(seq, "0x%08x,0x%08x",
		   le32_to_cpu(at->dw0),
		   le32_to_cpu(at->dw1));
}

struct dcmode_ext_sw_subif_info {
	int used;
	uint32_t subif_id;
	struct net_device *netif;
};

struct dcmode_ext_sw_dev_ctx {
	int used;

	/* skbs enqueued in TXIN, but not yet returned via TXOUT */
	struct sk_buff_head tx_skb_list;
	/* skbs enqueued in RXIN, but not yet returned via RXOUT */
	struct sk_buff_head rx_skb_list;

	struct ring_desc_2dw rxin;
	struct ring_desc_4dw rxout;
	struct ring_desc_4dw txin;
	struct ring_desc_2dw txout;

	struct dcmode_ext_sw_subif_info subif_info[DCMODE_EXT_SW_MAX_SUBIF_PER_DEV];
	struct hostif_acc_counters __iomem *hostif_acc_counters;

	uint32_t port_id;
	struct device *hwdev;
	struct net_device *netdev;

	struct hrtimer tx_timer;
	struct hrtimer rx_timer;

	struct tasklet_struct rx_tasklet;
	struct tasklet_struct txout_tasklet;

	/* protects tx_skb_list and tx_active_buffers */
	spinlock_t tx_lock;

	/* number of rx or tx descriptors returned from the VRX, where the counter was updated,
	 * but the ownership was not yet set to the CPU in the descriptor
	 */
	unsigned long rx_wrong_own_bit;
	unsigned long tx_wrong_own_magic;
	/* number of 4k-crossing TX SKBs, which needed to be copied */
	unsigned long tx_copied;
	/* number of TX pkts dropped because the ring was full */
	unsigned long tx_ring_full_drop;

	/* number of unreturned RX buffers */
	size_t rx_active_buffers;
	/* number of unreturned TX buffers: inc by xmit, dec by txout_bh */
	size_t tx_active_buffers;
	struct rtnl_link_stats64 stats64;
	size_t rx_buf_alloc_retry;
	size_t rx_buf_alloc_retry_err_cnt;
	size_t rx_retry_alloc_ratelimit;
};

static int num_devices;
static struct dcmode_ext_sw_dev_ctx dcmode_ext_sw_devices[DCMODE_EXT_SW_MAX_DEV_NUM];

static int32_t dcmode_ext_sw_get_netif_stats(void *ctx,
					     struct net_device *netif,
					     struct dp_subif *subif_id,
					     struct rtnl_link_stats64 *if_stats,
					     uint32_t flags)
{
	struct dcmode_ext_sw_dev_ctx *dev_ctx = (struct dcmode_ext_sw_dev_ctx *)ctx;

	if_stats = &dev_ctx->stats64;
	return 0;
}

static struct sk_buff *extract_skb_by_dma_addr(struct sk_buff_head *skb_head,
						dma_addr_t dma_addr)
{
	struct sk_buff *skb_next;

	dma_addr &= ~BYTE_OFFSET_MASK;
	skb_queue_walk(skb_head, skb_next) {
		if ((skb_cb(skb_next)->dma & ~BYTE_OFFSET_MASK) == dma_addr) {
			skb_unlink(skb_next, skb_head);
			return skb_next;
		}
	}
	return NULL;
}

static inline struct sk_buff *alloc_and_map_skb(struct device *dev, struct net_device *netdev)
{
	struct sk_buff *skb;
	dma_addr_t dma_addr;

	skb = netdev_alloc_skb(netdev, DC_DP_PKT_BUF_SIZE_DEFAULT);
	if (!skb) {
		DC_DP_ERROR("%s: failed to allocate skb buffer: out of memory.\n",
			    __func__);
		return NULL;
	}
	skb_reserve(skb, NET_SKB_PAD);

	dma_addr = dma_map_single(dev, skb->data, skb_availroom(skb), PCI_DMA_FROMDEVICE);
	if (dma_mapping_error(dev, dma_addr)) {
		kfree_skb(skb);
		DC_DP_ERROR("%s: DMA mapping failed.\n", __func__);
		return NULL;
	}

	*skb_cb(skb) = (struct skb_cb){ .dma = dma_addr };
	return skb;
}

static void dcdp_skb_queue_purge(struct sk_buff_head *list, struct device *dev)
{
	struct sk_buff *skb;
	while ((skb = skb_dequeue(list)) != NULL) {
		dma_unmap_single(dev, skb_cb(skb)->dma,
			 skb_headlen(skb), PCI_DMA_TODEVICE);
		kfree_skb(skb);
	}
}

static int
dcmode_ext_sw_prepare_rx(struct dcmode_ext_sw_dev_ctx *dev_ctx, struct dc_dp_res *resources,
			 struct dc_dp_dev *devspec)
{
	struct device *dev = dev_ctx->hwdev;
	int ret = 0;
	size_t i;

	/* Set up channel and ring for reception. */
	ret = ring_desc_2dw_init(&dev_ctx->rxin, dev, "RXIN", resources->rings.dev2soc_ret.size);
	if (ret)
		goto out;

	for (i = 0; i < ring_desc_4dw_get_entry_num(&dev_ctx->rxout); ++i)
		ring_desc_4dw_return(&dev_ctx->rxout);

	/* Set up channel and ring for buffer return. */
	ret = ring_desc_4dw_init(&dev_ctx->rxout, dev, "RXOUT", resources->rings.dev2soc.size);
	if (ret)
		goto cleanup_rxin;

	ring_desc_2dw_init_dcdp_data(&dev_ctx->rxin, &resources->rings.dev2soc_ret);
	ring_desc_4dw_init_dcdp_data(&dev_ctx->rxout, &resources->rings.dev2soc);

	resources->dccntr->dev_write_dccntr_mode = 0; /* not used by VRX619 driver, so just zero */

	skb_queue_head_init(&dev_ctx->rx_skb_list);

	/*
	 * Now that the buffer manager is set up, we allocate the number of buffers
	 * that the ACA device driver wants to have.
	 */
	for (i = 0; i < resources->rings.dev2soc_ret.size; ++i) {
		struct desc_2dw desc;
		struct sk_buff *skb;

		skb = alloc_and_map_skb(dev, dev_ctx->netdev);
		if (!skb) {
			DC_DP_ERROR("%s: Alloc and map of skb failed\n",
					__func__);
			ret = -ENOMEM;
			goto cleanup_skbs;
		}

		desc.dw0 = skb_cb(skb)->dma;
		desc.dw1 = 0;
		ring_desc_2dw_put(&dev_ctx->rxin, &desc);

		skb_queue_tail(&dev_ctx->rx_skb_list, skb);
	}

	dev_ctx->rx_active_buffers = resources->rings.dev2soc_ret.size;

	/* Return results to the calling device driver. */
	resources->num_bufpools = 0;
	resources->buflist = NULL;	/* ignored by VRX619 driver on LGM */

	devspec->dc_rx_ring_used = DC_DP_RING_SW_MODE1_EXT;

	/* Tell the ACA device by writing RXIN ADD (incremental!). */
	iowrite32(resources->rings.dev2soc_ret.size, &dev_ctx->hostif_acc_counters->rxin_hd_add);

	goto out;

cleanup_skbs:
	dcdp_skb_queue_purge(&dev_ctx->rx_skb_list, dev);
	ring_desc_4dw_free(&dev_ctx->rxout, dev);
cleanup_rxin:
	ring_desc_2dw_free(&dev_ctx->rxin, dev);
out:
	return ret;
}

static void dcmode_ext_sw_deinit_rx(struct dcmode_ext_sw_dev_ctx *dev_ctx)
{
	struct device *dev = dev_ctx->hwdev;

	dcdp_skb_queue_purge(&dev_ctx->rx_skb_list, dev);
	ring_desc_4dw_free(&dev_ctx->rxout, dev);
	ring_desc_2dw_free(&dev_ctx->rxin, dev);
}

static int
dcmode_ext_sw_prepare_tx(struct dcmode_ext_sw_dev_ctx *dev_ctx, struct dc_dp_res *resources,
			 struct dc_dp_dev *devspec)
{
	struct device *dev = dev_ctx->hwdev;
	int ret = 0;

	/* Set up channel and ring for reception. */
	ret = ring_desc_4dw_init(&dev_ctx->txin, dev, "TXIN", resources->rings.soc2dev.size);
	if (ret)
		goto cleanup_tx_copy;

	/* Set up channel and ring for buffer return. */
	ret = ring_desc_2dw_init(&dev_ctx->txout, dev, "TXOUT", resources->rings.soc2dev_ret.size);
	if (ret)
		goto cleanup_txin;

	ring_desc_4dw_init_dcdp_data(&dev_ctx->txin, &resources->rings.soc2dev);
	ring_desc_2dw_init_dcdp_data(&dev_ctx->txout, &resources->rings.soc2dev_ret);

	skb_queue_head_init(&dev_ctx->tx_skb_list);

	/* Return results to the calling device driver. */
	resources->tx_num_bufpools = 0;
	resources->tx_buflist = NULL;	/* ignored by VRX619 driver */
	resources->tx_desc_start_idx = 0;

	devspec->dc_tx_ring_used = DC_DP_RING_SW_MODE1_EXT;

	return 0;

cleanup_txin:
	ring_desc_4dw_free(&dev_ctx->txin, dev);
cleanup_tx_copy:
	return ret;
}

static void dcmode_ext_sw_deinit_tx(struct dcmode_ext_sw_dev_ctx *dev_ctx)
{
	struct device *dev = dev_ctx->hwdev;

	dcdp_skb_queue_purge(&dev_ctx->tx_skb_list, dev);
	ring_desc_2dw_free(&dev_ctx->txout, dev);
	ring_desc_4dw_free(&dev_ctx->txin, dev);
}

static enum hrtimer_restart handle_ring_sw_rx(struct hrtimer *rx_timer)
{
	struct dcmode_ext_sw_dev_ctx *dev_ctx = container_of(rx_timer,
							typeof(*dev_ctx),
							rx_timer);

	if (ioread32(&dev_ctx->hostif_acc_counters->rxout_hd_count)
	    || ((dev_ctx->rx_buf_alloc_retry > 0) && (dev_ctx->rx_retry_alloc_ratelimit > 100)))
	{
		dev_ctx->rx_retry_alloc_ratelimit = 0;
		tasklet_schedule(&dev_ctx->rx_tasklet);
		return HRTIMER_NORESTART;
	}
	if (dev_ctx->rx_buf_alloc_retry > 0)
		dev_ctx->rx_retry_alloc_ratelimit++;
	hrtimer_forward_now(&dev_ctx->rx_timer, ns_to_ktime(HANDLE_RINGS_INTERVAL_NS));
	return HRTIMER_RESTART;
}

static enum hrtimer_restart handle_ring_sw_txout(struct hrtimer *tx_timer)
{
	struct dcmode_ext_sw_dev_ctx *dev_ctx = container_of(tx_timer,
							typeof(*dev_ctx),
							tx_timer);

	if (ioread32(&dev_ctx->hostif_acc_counters->txout_hd_count)) {
		tasklet_schedule(&dev_ctx->txout_tasklet);
		return HRTIMER_NORESTART;
	}
	hrtimer_forward_now(&dev_ctx->tx_timer, ns_to_ktime(HANDLE_RINGS_INTERVAL_NS));
	return HRTIMER_RESTART;
}

static int handle_rxout_descs(struct dcmode_ext_sw_dev_ctx *dev_ctx,
				uint32_t *p_descs_handled)
{
	struct sk_buff *rx_skb;
	struct net_device *netdev;
	struct dp_subif subif = { .port_id = dev_ctx->port_id };
	uint32_t descs_waiting, descs_handled = 0;
	uint32_t rx_budget = RX_BH_DESC_BUDGET;
	struct device *dev = dev_ctx->hwdev;

	descs_waiting = ioread32(&dev_ctx->hostif_acc_counters->rxout_hd_count);
	while (descs_waiting && rx_budget--) {
		struct desc_4dw cur_rx_desc;
		dma_addr_t desc_addr;
		size_t desc_len;

		/* Read whole desc from rxout ring.*/
		if (!ring_desc_4dw_try_get(&dev_ctx->rxout, &cur_rx_desc)) {
			dev_ctx->rx_wrong_own_bit++;
			DC_SW_DEBUG("%s: own bit was set!\n", __func__);
			break;
		}
		descs_waiting--;
		descs_handled++;
		dev_ctx->rx_active_buffers--;

		DC_SW_DEBUG("%08x,%08x,%08x,%08x\n",
			cur_rx_desc.dw0,
			cur_rx_desc.dw1,
			cur_rx_desc.dw2,
			cur_rx_desc.dw3
		);

		desc_addr = FIELD_GET(DC_EXT_SW_DESC_DW2_ADDR, cur_rx_desc.dw2);
		desc_len = FIELD_GET(DC_EXT_SW_DESC_DW3_SIZE, cur_rx_desc.dw3);

		/* Extract skbs that have been returned. */
		rx_skb = extract_skb_by_dma_addr(&dev_ctx->rx_skb_list, desc_addr);
		if (!rx_skb) {
			DC_DP_ERROR("%s: no buffer found at %pad. Descriptor dropped.\n",
				    __func__, &desc_addr);
			continue;
		}

		dma_unmap_single(dev, skb_cb(rx_skb)->dma, skb_availroom(rx_skb),
				 PCI_DMA_FROMDEVICE);

		if (((DC_EXT_SW_DESC_DW3_SOP | DC_EXT_SW_DESC_DW3_EOP) & cur_rx_desc.dw3) !=
		     (DC_EXT_SW_DESC_DW3_SOP | DC_EXT_SW_DESC_DW3_EOP)) {
			/* The VRX can't send fragmented packets.
			 * If we receive a fragment, it's an error.
			 */
			DC_DP_ERROR("%s: Packet fragmented. Packet seems broken and will be dropped.\n",
				    __func__);
			dev_ctx->stats64.rx_dropped++;
			kfree_skb(rx_skb);
			continue;
		}

		/* __skb_trim trims unconditionally, unlike skb_trim. */
		__skb_trim(rx_skb, desc_len);

		DC_SW_DEBUG("Received packet (%zu@%pad)\n", desc_len, &desc_addr);
#ifdef DC_SW_DEBUG_CONTENT
		print_hex_dump(KERN_INFO, "rx> ", DUMP_PREFIX_OFFSET, 32, 1, rx_skb->data,
			       skb_headlen(rx_skb), true);
#endif

		/* Leave further skb handling to higher levels. */
		subif.subif = FIELD_GET(DC_EXT_SW_DESC_DW0_SUBIF_ID, cur_rx_desc.dw0);
		netdev = dcmode_ext_sw_devices[dev_ctx->port_id]
				.subif_info[(subif.subif >> 8) & 0xf].netif;
		dc_dp_rx(dev_ctx->netdev ?: netdev, NULL, &subif, rx_skb, skb_headlen(rx_skb), 0);
	}

	iowrite32(descs_handled, &dev_ctx->hostif_acc_counters->rxout_hd_sub);
	*p_descs_handled = descs_handled;
	return descs_waiting;
}

static int alloc_new_rxin_buffer(struct dcmode_ext_sw_dev_ctx *dev_ctx,
					uint32_t buffer_num)
{
	struct device *dev = dev_ctx->hwdev;
	struct desc_2dw desc;
	struct sk_buff *rx_skb;
	int alloc_success = 0;

	buffer_num = min(buffer_num, ring_desc_2dw_get_entry_num(&dev_ctx->rxin) -
			 ioread32(&dev_ctx->hostif_acc_counters->rxin_hd_count));

	while (buffer_num--) {
		/* Alloc new skb into list. */
		rx_skb = alloc_and_map_skb(dev, dev_ctx->netdev);
		if (!rx_skb) {
			DC_DP_ERROR("%s: Alloc and map of skb failed\n",
					__func__);
			continue;
		}

		/* Return descriptor of new sk buffer. */
		desc.dw0 = FIELD_PREP(DC_EXT_SW_DESC_RET_DW0_ADDR, skb_cb(rx_skb)->dma);
		desc.dw1 = 0;

		ring_desc_2dw_put(&dev_ctx->rxin, &desc);

		skb_queue_tail(&dev_ctx->rx_skb_list, rx_skb);
		alloc_success++;
	}

	dev_ctx->rx_active_buffers += alloc_success;

	return alloc_success;
}

static void rx_bh(unsigned long raw_dev_ctx)
{
	struct dcmode_ext_sw_dev_ctx *dev_ctx = (struct dcmode_ext_sw_dev_ctx *) raw_dev_ctx;
	size_t new_buffer_num, handled_descs_num;
	uint32_t descs_waiting;

	descs_waiting = handle_rxout_descs(dev_ctx, &handled_descs_num);
	new_buffer_num = alloc_new_rxin_buffer(dev_ctx, handled_descs_num
						+ dev_ctx->rx_buf_alloc_retry);
	dev_ctx->rx_buf_alloc_retry = handled_descs_num + dev_ctx->rx_buf_alloc_retry
					- new_buffer_num;
	dev_ctx->rx_buf_alloc_retry_err_cnt += dev_ctx->rx_buf_alloc_retry;

	/* Tell the ACA device by writing RXIN ADD (incremental!). */
	iowrite32(new_buffer_num, &dev_ctx->hostif_acc_counters->rxin_hd_add);

	/* reschedule task or restart poll timer */
	/* Intentionally check only for descriptors that were enqueued before
	 * we started working on the ring. Newer descriptors have to wait for
	 * the timer. */
	if (descs_waiting)
		tasklet_schedule(&dev_ctx->rx_tasklet);
	else
		hrtimer_start(&dev_ctx->rx_timer,
				ns_to_ktime(HANDLE_RINGS_INTERVAL_NS),
				HRTIMER_MODE_REL);
}

static void txout_bh(unsigned long raw_dev_ctx)
{
	struct sk_buff *tx_skb;

	struct dcmode_ext_sw_dev_ctx *dev_ctx = (struct dcmode_ext_sw_dev_ctx *) raw_dev_ctx;
	struct device *dev = dev_ctx->hwdev;

	dma_addr_t desc_dma_addr;
	uint32_t handled_success = 0;
	uint32_t tx_budget;
	uint32_t descs_handled = 0;
	uint32_t descs_waiting = ioread32(&dev_ctx->hostif_acc_counters->txout_hd_count);

	tx_budget = TX_BH_DESC_BUDGET;
	while (descs_waiting && tx_budget--) {
		struct desc_2dw cur_tx_desc;

		if (!ring_desc_2dw_try_get(&dev_ctx->txout, &cur_tx_desc)) {
			dev_ctx->tx_wrong_own_magic++;
			DC_SW_DEBUG("%s: got 0x%08x instead of magic value 0x%08x!\n", __func__, cur_tx_desc.dw1, TXOUT_MAGIC);
			break;
		}
		descs_waiting--;
		descs_handled++;

		DC_SW_DEBUG("%08x,%08x\n",
			cur_tx_desc.dw0,
			cur_tx_desc.dw1
		);

		spin_lock(&dev_ctx->tx_lock);
		--dev_ctx->tx_active_buffers;

		/* Find buffer we gave to the hardware. */
		desc_dma_addr = (dma_addr_t) FIELD_GET(DC_EXT_SW_DESC_RET_DW0_ADDR,
						       cur_tx_desc.dw0);
		tx_skb = extract_skb_by_dma_addr(
					&dev_ctx->tx_skb_list,
					desc_dma_addr);
		spin_unlock(&dev_ctx->tx_lock);

		if (!tx_skb) {
			DC_DP_ERROR("%s: didn't find skb with dma addr: %08x\n",
				__func__, desc_dma_addr);
			dev_ctx->stats64.tx_errors++;
			continue;
		}
		dma_unmap_single(dev, skb_cb(tx_skb)->dma,
			 skb_headlen(tx_skb), PCI_DMA_TODEVICE);
		if (skb_cb(tx_skb)->copy)
			kmem_cache_free(dcdp_tx_copy_cache_get(skb_cb(tx_skb)->size), skb_cb(tx_skb)->copy);
		dev_consume_skb_irq(tx_skb);
		handled_success++;
	}

	iowrite32(descs_handled, &dev_ctx->hostif_acc_counters->txout_hd_sub);

	if (handled_success > 0)
		DC_SW_DEBUG("%zu returned TX descriptors effectively read.\n", handled_success);

	spin_lock(&dev_ctx->tx_lock);

	/* Remove leftover skb's. Their descriptor got likely lost or corrupted. */
	if (!dev_ctx->tx_active_buffers && !skb_queue_empty(&dev_ctx->tx_skb_list)) {
		DC_DP_ERROR("Ook! No skbs in transmit, but skb queue not empty; purging queue.\n");
		dcdp_skb_queue_purge(&dev_ctx->tx_skb_list, dev);
	}

	/* Wake up netstack queue, now that the TXIN ring has space */
	if (dev_ctx->netdev && netif_queue_stopped(dev_ctx->netdev) &&
	    (dev_ctx->tx_active_buffers < ring_desc_4dw_get_entry_num(&dev_ctx->txin))) {
		DC_SW_DEBUG("%s: wake up netif queue: %s\n",
			   __func__, dev_ctx->netdev->name);
		netif_wake_queue(dev_ctx->netdev);
	}
	spin_unlock(&dev_ctx->tx_lock);

	/* reschedule task or restart poll timer */
	/* Intentionally check only for descriptors that were enqueued before
	 * we started working on the ring. Newer descriptors have to wait for
	 * the timer. */
	if (descs_waiting)
		tasklet_schedule(&dev_ctx->txout_tasklet);
	else
		hrtimer_start(&dev_ctx->tx_timer,
				ns_to_ktime(HANDLE_RINGS_INTERVAL_NS),
				HRTIMER_MODE_REL);
}

static int32_t
dcmode_ext_sw_register_dev(void *ctx, struct module *owner, struct device *hwdev,
			   uint32_t port_id, struct net_device *netdev,
			   struct dc_dp_res *resources, struct dc_dp_dev *devspec)
{
	int32_t ret = DC_DP_SUCCESS;
	struct dcmode_ext_sw_dev_ctx *dev_ctx = ctx;

	/* Allocate device. */
	if (!dcmode_ext_sw_devices[port_id].used) {
		dev_ctx = &dcmode_ext_sw_devices[port_id];
		memset(dev_ctx, 0, sizeof(struct dcmode_ext_sw_dev_ctx));
		dev_ctx->used = 1;
	} else {
		DC_DP_ERROR("%s: port %u already in use.\n", __func__, port_id);
		ret = DC_DP_FAILURE;
		goto out;
	}

	/* Overwrite VRX619 ring sizes. */
	/* equal sizes for TXIN and TXOUT are strongly recommended! */
	resources->rings.soc2dev.size = TX_DESC_RING_SIZE;
	resources->rings.soc2dev_ret.size = TX_DESC_RING_SIZE;

	/* equal sizes for RXIN and RXOUT are strongly recommended! */
	resources->rings.dev2soc.size = RX_DESC_RING_SIZE;
	resources->rings.dev2soc_ret.size = RX_DESC_RING_SIZE;

	/* Hack: this uses knowledge about the register layout of the VRX host
	 * interface.
	 */
	dev_ctx->hostif_acc_counters =
		ioremap((uint32_t)resources->dccntr->dev2soc_ret_enq_phys_base,
			sizeof(*dev_ctx->hostif_acc_counters));

	dev_ctx->port_id = port_id;
	dev_ctx->netdev = netdev;

	if (hwdev) {
		dev_ctx->hwdev = hwdev;
	} else if (netdev) {
		dev_ctx->hwdev = &netdev->dev;
		ret = dma_set_coherent_mask(dev_ctx->hwdev, DMA_BIT_MASK(32));
		if (ret) {
			DC_DP_ERROR("%s: failed setting device DMA mask: %d\n", __func__, ret);
			return DC_DP_FAILURE;
		}
	} else {
		DC_DP_ERROR("%s: no hardware device was passed\n", __func__);
		return DC_DP_FAILURE;
	}

	ret = dcmode_ext_sw_prepare_rx(dev_ctx, resources, devspec);
	if (ret)
		goto cleanup_dev_ctx;
	ret = dcmode_ext_sw_prepare_tx(dev_ctx, resources, devspec);
	if (ret)
		goto cleanup_rx;

	resources->rings.txout_temp_dw3 = TXOUT_MAGIC;
	resources->rings.rxout_temp_dw3 = 0;

	devspec->dc_accel_used = DC_DP_ACCEL_NO_OFFLOAD;

	spin_lock_init(&dev_ctx->tx_lock);

	ret = dc_dp_register_dcmode_device(owner, port_id, netdev, dev_ctx, 0);
	if (ret) {
		DC_DP_ERROR("%s: failed to register device to DC common layer.\n",
			    __func__);
		goto cleanup_tx;
	}
	++num_devices;

	tasklet_init(&dev_ctx->rx_tasklet, rx_bh, (unsigned long) dev_ctx);
	tasklet_init(&dev_ctx->txout_tasklet, txout_bh, (unsigned long) dev_ctx);

	hrtimer_init(&dev_ctx->rx_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
	hrtimer_init(&dev_ctx->tx_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);

	dev_ctx->rx_timer.function = handle_ring_sw_rx;
	dev_ctx->tx_timer.function = handle_ring_sw_txout;

	hrtimer_start(&dev_ctx->rx_timer,
			ns_to_ktime(HANDLE_RINGS_INTERVAL_NS),
			HRTIMER_MODE_REL);

	hrtimer_start(&dev_ctx->tx_timer,
			ns_to_ktime(HANDLE_RINGS_INTERVAL_NS),
			HRTIMER_MODE_REL);

	DC_SW_DEBUG("new device registered.\n");
	goto out;
cleanup_tx:
	dcmode_ext_sw_deinit_tx(dev_ctx);
cleanup_rx:
	dcmode_ext_sw_deinit_rx(dev_ctx);
cleanup_dev_ctx:
	dev_ctx->used = 0;
out:
	return ret;
}

static int32_t
dcmode_ext_sw_deregister_dev(void *ctx, struct module *owner, uint32_t port_id,
			     struct net_device *netdev)
{
	struct dcmode_ext_sw_dev_ctx *dev_ctx = ctx;

	dc_dp_register_dcmode_device(owner, port_id, netdev, dev_ctx,
					DC_DP_DCMODE_DEV_DEREGISTER);

	/* "try" to not wait if function is running */
	hrtimer_try_to_cancel(&dev_ctx->tx_timer);
	hrtimer_try_to_cancel(&dev_ctx->rx_timer);

	tasklet_disable_nosync(&dev_ctx->rx_tasklet);
	tasklet_disable_nosync(&dev_ctx->txout_tasklet);

	dcmode_ext_sw_deinit_tx(dev_ctx);
	dcmode_ext_sw_deinit_rx(dev_ctx);
	dev_ctx->used = 0;

	return 0;
}

static int32_t
dcmode_ext_sw_register_dev_hwdev(void *ctx, struct module *owner, struct device *hwdev,
				 uint32_t port_id, struct net_device *netdev,
				 struct dc_dp_cb *datapathcb, struct dc_dp_res *resources,
				 struct dc_dp_dev *devspec, int32_t ref_port_id,
				 uint32_t alloc_flags, uint32_t flags)
{
	if (flags & DC_DP_F_DEREGISTER)
		return dcmode_ext_sw_deregister_dev(ctx, owner, port_id, netdev);
	else
		return dcmode_ext_sw_register_dev(ctx, owner, hwdev, port_id, netdev, resources,
						  devspec);
}

static int32_t dcmode_ext_sw_deregister_subif(struct dcmode_ext_sw_dev_ctx *dev_ctx,
					      struct dp_subif *subif)
{
	struct dcmode_ext_sw_subif_info *subif_info = NULL;
	int32_t subif_idx = DCMODE_EXT_SW_GET_SUBIFIDX(subif->subif);

	subif_info = &dev_ctx->subif_info[subif_idx];
	if (!subif_info->used) {
		DC_DP_ERROR("%s: subif ID %d not in use.\n", __func__,
				subif->subif);
		return DC_DP_FAILURE;
	}
	subif_info->subif_id = 0;
	subif_info->netif = NULL;
	subif_info->used = 0;

	return DC_DP_SUCCESS;
}

static int32_t dcmode_ext_sw_register_subif(void *ctx, struct module *owner,
					    struct net_device *dev, const uint8_t *subif_name,
					    struct dp_subif *subif, uint32_t flags)
{
	struct dcmode_ext_sw_dev_ctx *dev_ctx = ctx;
	struct dcmode_ext_sw_subif_info *subif_info = NULL;
	int32_t subif_idx;

	if (flags & DC_DP_F_DEREGISTER)
		return dcmode_ext_sw_deregister_subif(dev_ctx, subif);

	if (subif->subif < 0) { /* dynamic mode */
		for (subif_idx = 0; subif_idx < ARRAY_SIZE(dev_ctx->subif_info); ++subif_idx)
			if (!dev_ctx->subif_info[subif_idx].used) {
				subif_info = &dev_ctx->subif_info[subif_idx];
				break;
			}

		if (!subif_info) {
			DC_DP_ERROR("%s: no free subif found.\n", __func__);
			return DC_DP_FAILURE;
		}
	} else {
		subif_idx = DCMODE_EXT_SW_GET_SUBIFIDX(subif->subif);
		subif_info = &dev_ctx->subif_info[subif_idx];
		if (subif_info->used) {
			DC_DP_ERROR("%s: subif ID %d already in use.\n",
					__func__, subif->subif);
			return DC_DP_FAILURE;
		}
	}

	subif_info->used = 1;
	subif_info->subif_id = DCMODE_EXT_SW_SET_SUBIFIDX(subif_idx);
	subif->subif = subif_info->subif_id;
	subif_info->netif = dev;

	DC_SW_DEBUG("new sub-interface with ID %d registered.\n", subif->subif);

	return DC_DP_SUCCESS;
}

/* FCS is at the moment turn off for Alder/Maple. */
//#define SW_FCS

#ifdef SW_FCS
static void write_fcs(void *fcs_dest, void *data, size_t len)
{
	/*
	 * ether_crc_le uses init=true, refin=true (therefore *_le), refout=true, xorout=false
	 * (added manually).
	 *
	 * xorout is added manually (the tilde). It calculates the crc32 over the data +
	 * four zero bytes, so no need to add this manually.
	 * See https://zlib.net/crc_v3.txt for the functioning of CRC.
	 * TODO: for some reason, byte endianness is swapped in Ethernet. Why?
	 */

	uint32_t fcs = ~ether_crc_le(len, data);

	/* This is to enforce that the checksum is little-endian. */
	fcs = cpu_to_le32(fcs);

	memcpy(fcs_dest, &fcs, sizeof(fcs));
}
#endif

static int32_t dcmode_ext_sw_xmit(void *ctx, struct net_device *netdev,
	struct dp_subif *rx_subif, struct dp_subif *tx_subif,
	struct sk_buff *skb_orig, int skb_len, uint32_t flags)
{
	struct dcmode_ext_sw_dev_ctx *dev_ctx = ctx;
	struct device *dev = dev_ctx->hwdev;
	struct desc_4dw desc;
	int ret;
	dma_addr_t dma_addr;
	struct sk_buff *tx_skb;
	size_t size;
	struct kmem_cache *cache;
	void *copy = NULL;

	if (skb_get_queue_mapping(skb_orig)) {
		pr_warn("DCDP SW mode: Received a packet on queue %u, but only queue 0 is supported.\n",
			skb_get_queue_mapping(skb_orig));
		netif_tx_stop_queue(netdev_get_tx_queue(netdev, skb_get_queue_mapping(skb_orig)));
		kfree_skb(skb_orig);
		return DC_DP_SUCCESS;
	}

#ifdef SW_FCS
	tx_skb = skb_copy_expand(skb_orig, NET_IP_ALIGN, 4, GFP_ATOMIC);
	consume_skb(skb_orig);
#else
	tx_skb = skb_orig;
	if (skb_linearize(tx_skb) < 0) {
		kfree_skb(skb_orig);
		return -ENOMEM;
	}

#endif

	/* Ensure minimum Ethernet frame size */
	if (tx_skb->len < ETH_ZLEN) {
		ret = skb_padto(tx_skb, ETH_ZLEN);
		if (ret)
			return ret;
		skb_put(tx_skb, ETH_ZLEN - tx_skb->len);
	}

#ifdef SW_FCS
	skb_put(tx_skb, sizeof(uint32_t));
	write_fcs(tx_skb->tail - sizeof(uint32_t), tx_skb->data,
		  skb_headlen(tx_skb) - sizeof(uint32_t));

#endif

	DC_SW_DEBUG("Sending packet (%zu@%p)\n", skb_headlen(tx_skb), &tx_skb->data);
#ifdef DC_SW_DEBUG_CONTENT
	print_hex_dump(KERN_INFO, "tx> ", DUMP_PREFIX_OFFSET, 32, 1, tx_skb->data,
		       skb_headlen(tx_skb), true);
#endif
	DC_SW_DEBUG("tx_active_buffers=%d\n", dev_ctx->tx_active_buffers);

	size = skb_headlen(tx_skb);
	if (buffer_needs_fixup(tx_skb->data, size)) {
		cache = dcdp_tx_copy_cache_get(size);
		if (cache)
			copy = kmem_cache_alloc(cache, GFP_DMA | GFP_ATOMIC);
		if (!copy) {
			dev_err(dev,
				"DCDP SW: failed to allocate a copy buffer when SKB (size %zu) crossed a 4k boundary\n",
				size);
			kfree_skb(tx_skb);
			return DC_DP_SUCCESS;
		}
		memcpy(copy, tx_skb->data, size);
	}

	dma_addr = dma_map_single(dev, copy ?: tx_skb->data, size, PCI_DMA_TODEVICE);
	if (dma_mapping_error(dev, dma_addr)) {
		DC_DP_ERROR("%s: DMA mapping failed.\n", __func__);
		if (copy)
			kmem_cache_free(cache, copy);
		return DC_DP_FAILURE;
	}

	spin_lock_irq(&dev_ctx->tx_lock);
	dev_ctx->tx_active_buffers++;

	/* Guard against TXIN overflow (i.e., TXOUT cannot keep up with TXIN). */
	if (dev_ctx->netdev &&
	    dev_ctx->tx_active_buffers >= ring_desc_4dw_get_entry_num(&dev_ctx->txin)) {
		DC_SW_DEBUG("%s: netif (%s) queue stopped because TXIN ran full\n",
				__func__, dev_ctx->netdev->name);
		netif_stop_queue(dev_ctx->netdev);
	}

	if (unlikely(dev_ctx->tx_active_buffers > ring_desc_4dw_get_entry_num(&dev_ctx->txin))) {
		dev_ctx->tx_active_buffers--;
		spin_unlock_irq(&dev_ctx->tx_lock);
		if (dev_ctx->netdev) {
			pr_warn("DCDP SW mode: Tried to send a packet on a stopped queue. Stopping it again.\n");
			netif_stop_queue(dev_ctx->netdev);
		} else {
			pr_warn_ratelimited("DCDP SW mode: Tried to send a packet, but too many buffer are in flight already, and no netdev was provided. Dropping it.\n");
			dev_ctx->tx_ring_full_drop++;
		}
		dma_unmap_single(dev, dma_addr, size, PCI_DMA_TODEVICE);
		kfree_skb(skb_orig);
		return DC_DP_SUCCESS;
	}

	DC_SW_DEBUG("Mapped packet as (%zu@%pad)\n", skb_headlen(tx_skb), &dma_addr);
	DC_SW_DEBUG("tx_active_buffers=%d\n", dev_ctx->tx_active_buffers);

	if (copy) {
		dev_ctx->tx_copied++;
		DC_SW_DEBUG_RATELIMITED("%s: crossing a page boundary, copied %u@%p (total data size: %u) to %zu@%p (happened in %lu/%u cases)",
					__func__, size, tx_skb->data, skb_end_offset(tx_skb),
					size, copy, dev_ctx->tx_copied, ring_desc_4dw_get_sw_count(&dev_ctx->txin) + 1);
	}

	*skb_cb(tx_skb) = (struct skb_cb){
		.dma = dma_addr,
		.size = size,
		.copy = copy,
	};
	skb_queue_tail(&dev_ctx->tx_skb_list, tx_skb);

	/* Fill the descriptor. */
	desc.dw0 = FIELD_PREP(DC_EXT_SW_DESC_DW0_SUBIF_ID, tx_subif->subif);
	desc.dw1 = FIELD_PREP(DC_EXT_SW_DESC_DW1_EP, tx_subif->port_id) |
		   FIELD_PREP(DC_EXT_SW_DESC_DW1_CLASS, tx_skb->priority);
	desc.dw2 = FIELD_PREP(DC_EXT_SW_DESC_DW2_ADDR, dma_addr);
	desc.dw3 = FIELD_PREP(DC_EXT_SW_DESC_DW3_SIZE, skb_headlen(tx_skb)) |
		   FIELD_PREP(DC_EXT_SW_DESC_DW3_SOP, 1) |
		   FIELD_PREP(DC_EXT_SW_DESC_DW3_EOP, 1) |
		   FIELD_PREP(DC_EXT_SW_DESC_DW3_OWN, 1);

	ring_desc_4dw_put(&dev_ctx->txin, &desc);

	/* Finally notify ACA device by writing to TXIN ADD counter (incremental!). */
	iowrite32(1, &dev_ctx->hostif_acc_counters->txin_hd_add);
	spin_unlock_irq(&dev_ctx->tx_lock);

	return DC_DP_SUCCESS;
}

static void dcmode_ext_sw_dump_proc(void *ctx, struct seq_file *seq)
{

	struct dcmode_ext_sw_dev_ctx *dev_ctx = ctx;

	seq_printf(seq, "    Device:\t\t\t\t\t%s\n", dev_name(dev_ctx->hwdev));
	seq_puts(seq, "	Counters:\n");
	seq_printf(seq, "\tSW: TXIN:\t\t\t%10u (0x%08x)\n\tSW: TXOUT:\t\t\t%10u (0x%08x)\n\tSW: RXIN:\t\t\t%10u (0x%08x)\n\tSW: RXOUT:\t\t\t%10u (0x%08x)\n",
		   ring_desc_4dw_get_sw_count(&dev_ctx->txin),
		   ring_desc_4dw_get_sw_count(&dev_ctx->txin),
		   ring_desc_2dw_get_sw_count(&dev_ctx->txout),
		   ring_desc_2dw_get_sw_count(&dev_ctx->txout),
		   ring_desc_2dw_get_sw_count(&dev_ctx->rxin),
		   ring_desc_2dw_get_sw_count(&dev_ctx->rxin),
		   ring_desc_4dw_get_sw_count(&dev_ctx->rxout),
		   ring_desc_4dw_get_sw_count(&dev_ctx->rxout));
	seq_printf(seq, "\tHW: TXIN HD COUNT (ADD | SUB):\t%10u\t(%10u | %10u)\n",
		   ioread32(&dev_ctx->hostif_acc_counters->txin_hd_count),
		   ioread32(&dev_ctx->hostif_acc_counters->txin_hd_add),
		   ioread32(&dev_ctx->hostif_acc_counters->txin_hd_sub));
	seq_printf(seq, "\tHW: TXOUT HD COUNT (ADD | SUB):\t%10u\t(%10u | %10u)\n",
		   ioread32(&dev_ctx->hostif_acc_counters->txout_hd_count),
		   ioread32(&dev_ctx->hostif_acc_counters->txout_hd_add),
		   ioread32(&dev_ctx->hostif_acc_counters->txout_hd_sub));
	seq_printf(seq, "\tHW: RXIN HD COUNT (ADD | SUB):\t%10u\t(%10u | %10u)\n",
		   ioread32(&dev_ctx->hostif_acc_counters->rxin_hd_count),
		   ioread32(&dev_ctx->hostif_acc_counters->rxin_hd_add),
		   ioread32(&dev_ctx->hostif_acc_counters->rxin_hd_sub));
	seq_printf(seq, "\tHW: RXOUT HD COUNT (ADD | SUB):\t%10u\t(%10u | %10u)\n",
		   ioread32(&dev_ctx->hostif_acc_counters->rxout_hd_count),
		   ioread32(&dev_ctx->hostif_acc_counters->rxout_hd_add),
		   ioread32(&dev_ctx->hostif_acc_counters->rxout_hd_sub));
	seq_printf(seq, "\trx_active_buffers:\t\t%10zu\n",
		   dev_ctx->rx_active_buffers);
	seq_printf(seq, "\ttx_active_buffers:\t\t%10zu\n",
		   dev_ctx->tx_active_buffers);
	seq_printf(seq, "\trx_wrong_own_bit:\t\t%10lu\n", dev_ctx->rx_wrong_own_bit);
	seq_printf(seq, "\ttx_wrong_own_magic:\t\t%10lu\n", dev_ctx->tx_wrong_own_magic);
	seq_printf(seq, "\ttx_copied:\t\t\t%10lu\n", dev_ctx->tx_copied);
	seq_printf(seq, "\ttx_ring_full_drop:\t\t%10lu\n",
		   dev_ctx->tx_ring_full_drop);
	seq_printf(seq, "\trx_buf_alloc_retry:\t\t%10u\n", dev_ctx->rx_buf_alloc_retry);
	seq_printf(seq, "\trx_buf_alloc_retry_err_cnt:\t%10u\n",
		   dev_ctx->rx_buf_alloc_retry_err_cnt);
	seq_puts(seq, "\n");

	ring_desc_2dw_dump(&dev_ctx->rxin, seq);
	ring_desc_4dw_dump(&dev_ctx->rxout, seq);
	ring_desc_4dw_dump(&dev_ctx->txin, seq);
	ring_desc_2dw_dump(&dev_ctx->txout, seq);
}

static struct dc_dp_dcmode_ops dcmode_ext_sw_ops = {
	.register_dev_hwdev = dcmode_ext_sw_register_dev_hwdev,
	.register_subif = dcmode_ext_sw_register_subif,
	.xmit = dcmode_ext_sw_xmit,
	.handle_ring_sw = NULL,	/* handled by a periodic timer */
	.dump_proc = dcmode_ext_sw_dump_proc,
	.get_netif_stats = dcmode_ext_sw_get_netif_stats,
};

static struct dc_dp_dcmode dcmode_ext_sw = {
	/*
	 * To not modify the VRX619 driver, which expects a hardware DC mode,
	 * we use DC_DP_F_DCMODE_HW instead of DC_DP_F_DCMODE_SW.
	 */
	.dcmode_cap = DC_DP_F_DCMODE_HW | DC_DP_F_DCMODE_0,
	.dcmode_ops = &dcmode_ext_sw_ops
};

static __init int dcmode_ext_sw_init(void)
{
	int ret = -ENOMEM;

	DC_SW_DEBUG("initializing...\n");

	dcdp_tx_copy_cache = kmem_cache_create("dcdp-tx-copy", 2048, 2048, 0, NULL);
	if (!dcdp_tx_copy_cache)
		goto fail;

	ret = dc_dp_register_dcmode(&dcmode_ext_sw, 0);
	if (ret)
		goto fail_tx_copy_cache;

	return 0;

fail_tx_copy_cache:
	kmem_cache_destroy(dcdp_tx_copy_cache);
fail:
	return ret;
}

static __exit void dcmode_ext_sw_exit(void)
{
	size_t i;

	for (i = 0; i < ARRAY_SIZE(dcmode_ext_sw_devices); ++i) {
		if (!dcmode_ext_sw_devices[i].used)
			continue;
		dcmode_ext_sw_deregister_dev(dcmode_ext_sw_devices + i, THIS_MODULE,
					     dcmode_ext_sw_devices[i].port_id,
					     dcmode_ext_sw_devices[i].netdev);
	}

	dc_dp_register_dcmode(&dcmode_ext_sw, DC_DP_F_DCMODE_DEREGISTER);
	kmem_cache_destroy(dcdp_tx_copy_cache);
	DC_SW_DEBUG("exiting...\n");
}

module_init(dcmode_ext_sw_init);
module_exit(dcmode_ext_sw_exit);

MODULE_DESCRIPTION("DC Mode Ext support in software");
MODULE_LICENSE("GPL");