1	// SPDX-License-Identifier: GPL-2.0
2	/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
3
4	/* TSN endpoint Ethernet MAC driver
5	*
6	* The TSN endpoint Ethernet MAC is a FPGA based network device for real-time
7	* communication. It is designed for endpoints within TSN (Time Sensitive
8	* Networking) networks; e.g., for PLCs in the industrial automation case.
9	*
10	* It supports multiple TX/RX queue pairs. The first TX/RX queue pair is used
11	* by the driver.
12	*
13	* More information can be found here:
14	* - www.embedded-experts.at/tsn
15	* - www.engleder-embedded.com
16	*/
17
18	#include "tsnep.h"
19	#include "tsnep_hw.h"
20
21	#include <linux/module.h>
22	#include <linux/of.h>
23	#include <linux/of_net.h>
24	#include <linux/of_mdio.h>
25	#include <linux/interrupt.h>
26	#include <linux/etherdevice.h>
27	#include <linux/phy.h>
28	#include <linux/iopoll.h>
29	#include <linux/bpf.h>
30	#include <linux/bpf_trace.h>
31	#include <net/page_pool/helpers.h>
32	#include <net/xdp_sock_drv.h>
33
34	#define TSNEP_RX_OFFSET (max(NET_SKB_PAD, XDP_PACKET_HEADROOM) + NET_IP_ALIGN)
35	#define TSNEP_HEADROOM ALIGN(TSNEP_RX_OFFSET, 4)
36	#define TSNEP_MAX_RX_BUF_SIZE (PAGE_SIZE - TSNEP_HEADROOM - \
37	SKB_DATA_ALIGN(sizeof(struct skb_shared_info)))
38	/* XSK buffer shall store at least Q-in-Q frame */
39	#define TSNEP_XSK_RX_BUF_SIZE (ALIGN(TSNEP_RX_INLINE_METADATA_SIZE + \
40	ETH_FRAME_LEN + ETH_FCS_LEN + \
41	VLAN_HLEN * 2, 4))
42
43	#ifdef CONFIG_ARCH_DMA_ADDR_T_64BIT
44	#define DMA_ADDR_HIGH(dma_addr) ((u32)(((dma_addr) >> 32) & 0xFFFFFFFF))
45	#else
46	#define DMA_ADDR_HIGH(dma_addr) ((u32)(0))
47	#endif
48	#define DMA_ADDR_LOW(dma_addr) ((u32)((dma_addr) & 0xFFFFFFFF))
49
50	#define TSNEP_COALESCE_USECS_DEFAULT 64
51	#define TSNEP_COALESCE_USECS_MAX ((ECM_INT_DELAY_MASK >> ECM_INT_DELAY_SHIFT) * \
52	ECM_INT_DELAY_BASE_US + ECM_INT_DELAY_BASE_US - 1)
53
54	/* mapping type */
55	#define TSNEP_TX_TYPE_MAP BIT(0)
56	#define TSNEP_TX_TYPE_MAP_PAGE BIT(1)
57	#define TSNEP_TX_TYPE_INLINE BIT(2)
58	/* buffer type */
59	#define TSNEP_TX_TYPE_SKB BIT(8)
60	#define TSNEP_TX_TYPE_SKB_MAP (TSNEP_TX_TYPE_SKB | TSNEP_TX_TYPE_MAP)
61	#define TSNEP_TX_TYPE_SKB_INLINE (TSNEP_TX_TYPE_SKB | TSNEP_TX_TYPE_INLINE)
62	#define TSNEP_TX_TYPE_SKB_FRAG BIT(9)
63	#define TSNEP_TX_TYPE_SKB_FRAG_MAP_PAGE (TSNEP_TX_TYPE_SKB_FRAG | TSNEP_TX_TYPE_MAP_PAGE)
64	#define TSNEP_TX_TYPE_SKB_FRAG_INLINE (TSNEP_TX_TYPE_SKB_FRAG | TSNEP_TX_TYPE_INLINE)
65	#define TSNEP_TX_TYPE_XDP_TX BIT(10)
66	#define TSNEP_TX_TYPE_XDP_NDO BIT(11)
67	#define TSNEP_TX_TYPE_XDP_NDO_MAP_PAGE (TSNEP_TX_TYPE_XDP_NDO | TSNEP_TX_TYPE_MAP_PAGE)
68	#define TSNEP_TX_TYPE_XDP (TSNEP_TX_TYPE_XDP_TX | TSNEP_TX_TYPE_XDP_NDO)
69	#define TSNEP_TX_TYPE_XSK BIT(12)
70	#define TSNEP_TX_TYPE_TSTAMP BIT(13)
71	#define TSNEP_TX_TYPE_SKB_TSTAMP (TSNEP_TX_TYPE_SKB | TSNEP_TX_TYPE_TSTAMP)
72
73	#define TSNEP_XDP_TX BIT(0)
74	#define TSNEP_XDP_REDIRECT BIT(1)
75
76	static void tsnep_enable_irq(struct tsnep_adapter *adapter, u32 mask)
77	{
78	iowrite32(mask, adapter->addr + ECM_INT_ENABLE);
79	}
80
81	static void tsnep_disable_irq(struct tsnep_adapter *adapter, u32 mask)
82	{
83	mask |= ECM_INT_DISABLE;
84	iowrite32(mask, adapter->addr + ECM_INT_ENABLE);
85	}
86
87	static irqreturn_t tsnep_irq(int irq, void *arg)
88	{
89	struct tsnep_adapter *adapter = arg;
90	u32 active = ioread32(adapter->addr + ECM_INT_ACTIVE);
91
92	/* acknowledge interrupt */
93	if (active != 0)
94	iowrite32(active, adapter->addr + ECM_INT_ACKNOWLEDGE);
95
96	/* handle link interrupt */
97	if ((active & ECM_INT_LINK) != 0)
98	phy_mac_interrupt(phydev: adapter->netdev->phydev);
99
100	/* handle TX/RX queue 0 interrupt */
101	if ((active & adapter->queue[0].irq_mask) != 0) {
102	if (napi_schedule_prep(n: &adapter->queue[0].napi)) {
103	tsnep_disable_irq(adapter, mask: adapter->queue[0].irq_mask);
104	/* schedule after masking to avoid races */
105	__napi_schedule(n: &adapter->queue[0].napi);
106	}
107	}
108
109	return IRQ_HANDLED;
110	}
111
112	static irqreturn_t tsnep_irq_txrx(int irq, void *arg)
113	{
114	struct tsnep_queue *queue = arg;
115
116	/* handle TX/RX queue interrupt */
117	if (napi_schedule_prep(n: &queue->napi)) {
118	tsnep_disable_irq(adapter: queue->adapter, mask: queue->irq_mask);
119	/* schedule after masking to avoid races */
120	__napi_schedule(n: &queue->napi);
121	}
122
123	return IRQ_HANDLED;
124	}
125
126	int tsnep_set_irq_coalesce(struct tsnep_queue *queue, u32 usecs)
127	{
128	if (usecs > TSNEP_COALESCE_USECS_MAX)
129	return -ERANGE;
130
131	usecs /= ECM_INT_DELAY_BASE_US;
132	usecs <<= ECM_INT_DELAY_SHIFT;
133	usecs &= ECM_INT_DELAY_MASK;
134
135	queue->irq_delay &= ~ECM_INT_DELAY_MASK;
136	queue->irq_delay |= usecs;
137	iowrite8(queue->irq_delay, queue->irq_delay_addr);
138
139	return 0;
140	}
141
142	u32 tsnep_get_irq_coalesce(struct tsnep_queue *queue)
143	{
144	u32 usecs;
145
146	usecs = (queue->irq_delay & ECM_INT_DELAY_MASK);
147	usecs >>= ECM_INT_DELAY_SHIFT;
148	usecs *= ECM_INT_DELAY_BASE_US;
149
150	return usecs;
151	}
152
153	static int tsnep_mdiobus_read(struct mii_bus *bus, int addr, int regnum)
154	{
155	struct tsnep_adapter *adapter = bus->priv;
156	u32 md;
157	int retval;
158
159	md = ECM_MD_READ;
160	if (!adapter->suppress_preamble)
161	md |= ECM_MD_PREAMBLE;
162	md |= (regnum << ECM_MD_ADDR_SHIFT) & ECM_MD_ADDR_MASK;
163	md |= (addr << ECM_MD_PHY_ADDR_SHIFT) & ECM_MD_PHY_ADDR_MASK;
164	iowrite32(md, adapter->addr + ECM_MD_CONTROL);
165	retval = readl_poll_timeout_atomic(adapter->addr + ECM_MD_STATUS, md,
166	!(md & ECM_MD_BUSY), 16, 1000);
167	if (retval != 0)
168	return retval;
169
170	return (md & ECM_MD_DATA_MASK) >> ECM_MD_DATA_SHIFT;
171	}
172
173	static int tsnep_mdiobus_write(struct mii_bus *bus, int addr, int regnum,
174	u16 val)
175	{
176	struct tsnep_adapter *adapter = bus->priv;
177	u32 md;
178	int retval;
179
180	md = ECM_MD_WRITE;
181	if (!adapter->suppress_preamble)
182	md |= ECM_MD_PREAMBLE;
183	md |= (regnum << ECM_MD_ADDR_SHIFT) & ECM_MD_ADDR_MASK;
184	md |= (addr << ECM_MD_PHY_ADDR_SHIFT) & ECM_MD_PHY_ADDR_MASK;
185	md |= ((u32)val << ECM_MD_DATA_SHIFT) & ECM_MD_DATA_MASK;
186	iowrite32(md, adapter->addr + ECM_MD_CONTROL);
187	retval = readl_poll_timeout_atomic(adapter->addr + ECM_MD_STATUS, md,
188	!(md & ECM_MD_BUSY), 16, 1000);
189	if (retval != 0)
190	return retval;
191
192	return 0;
193	}
194
195	static void tsnep_set_link_mode(struct tsnep_adapter *adapter)
196	{
197	u32 mode;
198
199	switch (adapter->phydev->speed) {
200	case SPEED_100:
201	mode = ECM_LINK_MODE_100;
202	break;
203	case SPEED_1000:
204	mode = ECM_LINK_MODE_1000;
205	break;
206	default:
207	mode = ECM_LINK_MODE_OFF;
208	break;
209	}
210	iowrite32(mode, adapter->addr + ECM_STATUS);
211	}
212
213	static void tsnep_phy_link_status_change(struct net_device *netdev)
214	{
215	struct tsnep_adapter *adapter = netdev_priv(dev: netdev);
216	struct phy_device *phydev = netdev->phydev;
217
218	if (phydev->link)
219	tsnep_set_link_mode(adapter);
220
221	phy_print_status(phydev: netdev->phydev);
222	}
223
224	static int tsnep_phy_loopback(struct tsnep_adapter *adapter, bool enable)
225	{
226	int speed;
227
228	if (enable) {
229	if (adapter->phydev->autoneg == AUTONEG_DISABLE &&
230	adapter->phydev->speed == SPEED_100)
231	speed = SPEED_100;
232	else
233	speed = SPEED_1000;
234	} else {
235	speed = 0;
236	}
237
238	return phy_loopback(phydev: adapter->phydev, enable, speed);
239	}
240
241	static int tsnep_phy_open(struct tsnep_adapter *adapter)
242	{
243	struct phy_device *phydev;
244	struct ethtool_keee ethtool_keee;
245	int retval;
246
247	retval = phy_connect_direct(dev: adapter->netdev, phydev: adapter->phydev,
248	handler: tsnep_phy_link_status_change,
249	interface: adapter->phy_mode);
250	if (retval)
251	return retval;
252	phydev = adapter->netdev->phydev;
253
254	/* MAC supports only 100Mbps|1000Mbps full duplex
255	* SPE (Single Pair Ethernet) is also an option but not implemented yet
256	*/
257	phy_remove_link_mode(phydev, link_mode: ETHTOOL_LINK_MODE_10baseT_Half_BIT);
258	phy_remove_link_mode(phydev, link_mode: ETHTOOL_LINK_MODE_10baseT_Full_BIT);
259	phy_remove_link_mode(phydev, link_mode: ETHTOOL_LINK_MODE_100baseT_Half_BIT);
260	phy_remove_link_mode(phydev, link_mode: ETHTOOL_LINK_MODE_1000baseT_Half_BIT);
261
262	/* disable EEE autoneg, EEE not supported by TSNEP */
263	memset(&ethtool_keee, 0, sizeof(ethtool_keee));
264	phy_ethtool_set_eee(phydev: adapter->phydev, data: &ethtool_keee);
265
266	adapter->phydev->irq = PHY_MAC_INTERRUPT;
267	phy_start(phydev: adapter->phydev);
268
269	return 0;
270	}
271
272	static void tsnep_phy_close(struct tsnep_adapter *adapter)
273	{
274	phy_stop(phydev: adapter->netdev->phydev);
275	phy_disconnect(phydev: adapter->netdev->phydev);
276	}
277
278	static void tsnep_tx_ring_cleanup(struct tsnep_tx *tx)
279	{
280	struct device *dmadev = tx->adapter->dmadev;
281	int i;
282
283	memset(tx->entry, 0, sizeof(tx->entry));
284
285	for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
286	if (tx->page[i]) {
287	dma_free_coherent(dev: dmadev, PAGE_SIZE, cpu_addr: tx->page[i],
288	dma_handle: tx->page_dma[i]);
289	tx->page[i] = NULL;
290	tx->page_dma[i] = 0;
291	}
292	}
293	}
294
295	static int tsnep_tx_ring_create(struct tsnep_tx *tx)
296	{
297	struct device *dmadev = tx->adapter->dmadev;
298	struct tsnep_tx_entry *entry;
299	struct tsnep_tx_entry *next_entry;
300	int i, j;
301	int retval;
302
303	for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
304	tx->page[i] =
305	dma_alloc_coherent(dev: dmadev, PAGE_SIZE, dma_handle: &tx->page_dma[i],
306	GFP_KERNEL);
307	if (!tx->page[i]) {
308	retval = -ENOMEM;
309	goto alloc_failed;
310	}
311	for (j = 0; j < TSNEP_RING_ENTRIES_PER_PAGE; j++) {
312	entry = &tx->entry[TSNEP_RING_ENTRIES_PER_PAGE * i + j];
313	entry->desc_wb = (struct tsnep_tx_desc_wb *)
314	(((u8 *)tx->page[i]) + TSNEP_DESC_SIZE * j);
315	entry->desc = (struct tsnep_tx_desc *)
316	(((u8 *)entry->desc_wb) + TSNEP_DESC_OFFSET);
317	entry->desc_dma = tx->page_dma[i] + TSNEP_DESC_SIZE * j;
318	entry->owner_user_flag = false;
319	}
320	}
321	for (i = 0; i < TSNEP_RING_SIZE; i++) {
322	entry = &tx->entry[i];
323	next_entry = &tx->entry[(i + 1) & TSNEP_RING_MASK];
324	entry->desc->next = __cpu_to_le64(next_entry->desc_dma);
325	}
326
327	return 0;
328
329	alloc_failed:
330	tsnep_tx_ring_cleanup(tx);
331	return retval;
332	}
333
334	static void tsnep_tx_init(struct tsnep_tx *tx)
335	{
336	dma_addr_t dma;
337
338	dma = tx->entry[0].desc_dma | TSNEP_RESET_OWNER_COUNTER;
339	iowrite32(DMA_ADDR_LOW(dma), tx->addr + TSNEP_TX_DESC_ADDR_LOW);
340	iowrite32(DMA_ADDR_HIGH(dma), tx->addr + TSNEP_TX_DESC_ADDR_HIGH);
341	tx->write = 0;
342	tx->read = 0;
343	tx->owner_counter = 1;
344	tx->increment_owner_counter = TSNEP_RING_SIZE - 1;
345	}
346
347	static void tsnep_tx_enable(struct tsnep_tx *tx)
348	{
349	struct netdev_queue *nq;
350
351	nq = netdev_get_tx_queue(dev: tx->adapter->netdev, index: tx->queue_index);
352
353	__netif_tx_lock_bh(txq: nq);
354	netif_tx_wake_queue(dev_queue: nq);
355	__netif_tx_unlock_bh(txq: nq);
356	}
357
358	static void tsnep_tx_disable(struct tsnep_tx *tx, struct napi_struct *napi)
359	{
360	struct netdev_queue *nq;
361	u32 val;
362
363	nq = netdev_get_tx_queue(dev: tx->adapter->netdev, index: tx->queue_index);
364
365	__netif_tx_lock_bh(txq: nq);
366	netif_tx_stop_queue(dev_queue: nq);
367	__netif_tx_unlock_bh(txq: nq);
368
369	/* wait until TX is done in hardware */
370	readx_poll_timeout(ioread32, tx->addr + TSNEP_CONTROL, val,
371	((val & TSNEP_CONTROL_TX_ENABLE) == 0), 10000,
372	1000000);
373
374	/* wait until TX is also done in software */
375	while (READ_ONCE(tx->read) != tx->write) {
376	napi_schedule(n: napi);
377	napi_synchronize(n: napi);
378	}
379	}
380
381	static void tsnep_tx_activate(struct tsnep_tx *tx, int index, int length,
382	bool last)
383	{
384	struct tsnep_tx_entry *entry = &tx->entry[index];
385
386	entry->properties = 0;
387	/* xdpf and zc are union with skb */
388	if (entry->skb) {
389	entry->properties = length & TSNEP_DESC_LENGTH_MASK;
390	entry->properties |= TSNEP_DESC_INTERRUPT_FLAG;
391	if ((entry->type & TSNEP_TX_TYPE_SKB_TSTAMP) == TSNEP_TX_TYPE_SKB_TSTAMP)
392	entry->properties |= TSNEP_DESC_EXTENDED_WRITEBACK_FLAG;
393
394	/* toggle user flag to prevent false acknowledge
395	*
396	* Only the first fragment is acknowledged. For all other
397	* fragments no acknowledge is done and the last written owner
398	* counter stays in the writeback descriptor. Therefore, it is
399	* possible that the last written owner counter is identical to
400	* the new incremented owner counter and a false acknowledge is
401	* detected before the real acknowledge has been done by
402	* hardware.
403	*
404	* The user flag is used to prevent this situation. The user
405	* flag is copied to the writeback descriptor by the hardware
406	* and is used as additional acknowledge data. By toggeling the
407	* user flag only for the first fragment (which is
408	* acknowledged), it is guaranteed that the last acknowledge
409	* done for this descriptor has used a different user flag and
410	* cannot be detected as false acknowledge.
411	*/
412	entry->owner_user_flag = !entry->owner_user_flag;
413	}
414	if (last)
415	entry->properties |= TSNEP_TX_DESC_LAST_FRAGMENT_FLAG;
416	if (index == tx->increment_owner_counter) {
417	tx->owner_counter++;
418	if (tx->owner_counter == 4)
419	tx->owner_counter = 1;
420	tx->increment_owner_counter--;
421	if (tx->increment_owner_counter < 0)
422	tx->increment_owner_counter = TSNEP_RING_SIZE - 1;
423	}
424	entry->properties |=
425	(tx->owner_counter << TSNEP_DESC_OWNER_COUNTER_SHIFT) &
426	TSNEP_DESC_OWNER_COUNTER_MASK;
427	if (entry->owner_user_flag)
428	entry->properties |= TSNEP_TX_DESC_OWNER_USER_FLAG;
429	entry->desc->more_properties =
430	__cpu_to_le32(entry->len & TSNEP_DESC_LENGTH_MASK);
431	if (entry->type & TSNEP_TX_TYPE_INLINE)
432	entry->properties |= TSNEP_TX_DESC_DATA_AFTER_DESC_FLAG;
433
434	/* descriptor properties shall be written last, because valid data is
435	* signaled there
436	*/
437	dma_wmb();
438
439	entry->desc->properties = __cpu_to_le32(entry->properties);
440	}
441
442	static int tsnep_tx_desc_available(struct tsnep_tx *tx)
443	{
444	if (tx->read <= tx->write)
445	return TSNEP_RING_SIZE - tx->write + tx->read - 1;
446	else
447	return tx->read - tx->write - 1;
448	}
449
450	static int tsnep_tx_map_frag(skb_frag_t *frag, struct tsnep_tx_entry *entry,
451	struct device *dmadev, dma_addr_t *dma)
452	{
453	unsigned int len;
454	int mapped;
455
456	len = skb_frag_size(frag);
457	if (likely(len > TSNEP_DESC_SIZE_DATA_AFTER_INLINE)) {
458	*dma = skb_frag_dma_map(dmadev, frag, 0, len, DMA_TO_DEVICE);
459	if (dma_mapping_error(dev: dmadev, dma_addr: *dma))
460	return -ENOMEM;
461	entry->type = TSNEP_TX_TYPE_SKB_FRAG_MAP_PAGE;
462	mapped = 1;
463	} else {
464	void *fragdata = skb_frag_address_safe(frag);
465
466	if (likely(fragdata)) {
467	memcpy(&entry->desc->tx, fragdata, len);
468	} else {
469	struct page *page = skb_frag_page(frag);
470
471	fragdata = kmap_local_page(page);
472	memcpy(&entry->desc->tx, fragdata + skb_frag_off(frag),
473	len);
474	kunmap_local(fragdata);
475	}
476	entry->type = TSNEP_TX_TYPE_SKB_FRAG_INLINE;
477	mapped = 0;
478	}
479
480	return mapped;
481	}
482
483	static int tsnep_tx_map(struct sk_buff *skb, struct tsnep_tx *tx, int count,
484	bool do_tstamp)
485	{
486	struct device *dmadev = tx->adapter->dmadev;
487	struct tsnep_tx_entry *entry;
488	unsigned int len;
489	int map_len = 0;
490	dma_addr_t dma;
491	int i, mapped;
492
493	for (i = 0; i < count; i++) {
494	entry = &tx->entry[(tx->write + i) & TSNEP_RING_MASK];
495
496	if (!i) {
497	len = skb_headlen(skb);
498	if (likely(len > TSNEP_DESC_SIZE_DATA_AFTER_INLINE)) {
499	dma = dma_map_single(dmadev, skb->data, len,
500	DMA_TO_DEVICE);
501	if (dma_mapping_error(dev: dmadev, dma_addr: dma))
502	return -ENOMEM;
503	entry->type = TSNEP_TX_TYPE_SKB_MAP;
504	mapped = 1;
505	} else {
506	memcpy(&entry->desc->tx, skb->data, len);
507	entry->type = TSNEP_TX_TYPE_SKB_INLINE;
508	mapped = 0;
509	}
510
511	if (do_tstamp)
512	entry->type |= TSNEP_TX_TYPE_TSTAMP;
513	} else {
514	skb_frag_t *frag = &skb_shinfo(skb)->frags[i - 1];
515
516	len = skb_frag_size(frag);
517	mapped = tsnep_tx_map_frag(frag, entry, dmadev, dma: &dma);
518	if (mapped < 0)
519	return mapped;
520	}
521
522	entry->len = len;
523	if (likely(mapped)) {
524	dma_unmap_addr_set(entry, dma, dma);
525	entry->desc->tx = __cpu_to_le64(dma);
526	}
527
528	map_len += len;
529	}
530
531	return map_len;
532	}
533
534	static int tsnep_tx_unmap(struct tsnep_tx *tx, int index, int count)
535	{
536	struct device *dmadev = tx->adapter->dmadev;
537	struct tsnep_tx_entry *entry;
538	int map_len = 0;
539	int i;
540
541	for (i = 0; i < count; i++) {
542	entry = &tx->entry[(index + i) & TSNEP_RING_MASK];
543
544	if (entry->len) {
545	if (entry->type & TSNEP_TX_TYPE_MAP)
546	dma_unmap_single(dmadev,
547	dma_unmap_addr(entry, dma),
548	dma_unmap_len(entry, len),
549	DMA_TO_DEVICE);
550	else if (entry->type & TSNEP_TX_TYPE_MAP_PAGE)
551	dma_unmap_page(dmadev,
552	dma_unmap_addr(entry, dma),
553	dma_unmap_len(entry, len),
554	DMA_TO_DEVICE);
555	map_len += entry->len;
556	entry->len = 0;
557	}
558	}
559
560	return map_len;
561	}
562
563	static netdev_tx_t tsnep_xmit_frame_ring(struct sk_buff *skb,
564	struct tsnep_tx *tx)
565	{
566	struct tsnep_tx_entry *entry;
567	bool do_tstamp = false;
568	int count = 1;
569	int length;
570	int retval;
571	int i;
572
573	if (skb_shinfo(skb)->nr_frags > 0)
574	count += skb_shinfo(skb)->nr_frags;
575
576	if (tsnep_tx_desc_available(tx) < count) {
577	/* ring full, shall not happen because queue is stopped if full
578	* below
579	*/
580	netif_stop_subqueue(dev: tx->adapter->netdev, queue_index: tx->queue_index);
581
582	return NETDEV_TX_BUSY;
583	}
584
585	entry = &tx->entry[tx->write];
586	entry->skb = skb;
587
588	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
589	tx->adapter->hwtstamp_config.tx_type == HWTSTAMP_TX_ON) {
590	skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
591	do_tstamp = true;
592	}
593
594	retval = tsnep_tx_map(skb, tx, count, do_tstamp);
595	if (retval < 0) {
596	tsnep_tx_unmap(tx, index: tx->write, count);
597	dev_kfree_skb_any(skb: entry->skb);
598	entry->skb = NULL;
599
600	tx->dropped++;
601
602	return NETDEV_TX_OK;
603	}
604	length = retval;
605
606	for (i = 0; i < count; i++)
607	tsnep_tx_activate(tx, index: (tx->write + i) & TSNEP_RING_MASK, length,
608	last: i == count - 1);
609	tx->write = (tx->write + count) & TSNEP_RING_MASK;
610
611	skb_tx_timestamp(skb);
612
613	/* descriptor properties shall be valid before hardware is notified */
614	dma_wmb();
615
616	iowrite32(TSNEP_CONTROL_TX_ENABLE, tx->addr + TSNEP_CONTROL);
617
618	if (tsnep_tx_desc_available(tx) < (MAX_SKB_FRAGS + 1)) {
619	/* ring can get full with next frame */
620	netif_stop_subqueue(dev: tx->adapter->netdev, queue_index: tx->queue_index);
621	}
622
623	return NETDEV_TX_OK;
624	}
625
626	static int tsnep_xdp_tx_map(struct xdp_frame *xdpf, struct tsnep_tx *tx,
627	struct skb_shared_info *shinfo, int count, u32 type)
628	{
629	struct device *dmadev = tx->adapter->dmadev;
630	struct tsnep_tx_entry *entry;
631	struct page *page;
632	skb_frag_t *frag;
633	unsigned int len;
634	int map_len = 0;
635	dma_addr_t dma;
636	void *data;
637	int i;
638
639	frag = NULL;
640	len = xdpf->len;
641	for (i = 0; i < count; i++) {
642	entry = &tx->entry[(tx->write + i) & TSNEP_RING_MASK];
643	if (type & TSNEP_TX_TYPE_XDP_NDO) {
644	data = unlikely(frag) ? skb_frag_address(frag) :
645	xdpf->data;
646	dma = dma_map_single(dmadev, data, len, DMA_TO_DEVICE);
647	if (dma_mapping_error(dev: dmadev, dma_addr: dma))
648	return -ENOMEM;
649
650	entry->type = TSNEP_TX_TYPE_XDP_NDO_MAP_PAGE;
651	} else {
652	page = unlikely(frag) ? skb_frag_page(frag) :
653	virt_to_page(xdpf->data);
654	dma = page_pool_get_dma_addr(page);
655	if (unlikely(frag))
656	dma += skb_frag_off(frag);
657	else
658	dma += sizeof(*xdpf) + xdpf->headroom;
659	dma_sync_single_for_device(dev: dmadev, addr: dma, size: len,
660	dir: DMA_BIDIRECTIONAL);
661
662	entry->type = TSNEP_TX_TYPE_XDP_TX;
663	}
664
665	entry->len = len;
666	dma_unmap_addr_set(entry, dma, dma);
667
668	entry->desc->tx = __cpu_to_le64(dma);
669
670	map_len += len;
671
672	if (i + 1 < count) {
673	frag = &shinfo->frags[i];
674	len = skb_frag_size(frag);
675	}
676	}
677
678	return map_len;
679	}
680
681	/* This function requires __netif_tx_lock is held by the caller. */
682	static bool tsnep_xdp_xmit_frame_ring(struct xdp_frame *xdpf,
683	struct tsnep_tx *tx, u32 type)
684	{
685	struct skb_shared_info *shinfo = xdp_get_shared_info_from_frame(frame: xdpf);
686	struct tsnep_tx_entry *entry;
687	int count, length, retval, i;
688
689	count = 1;
690	if (unlikely(xdp_frame_has_frags(xdpf)))
691	count += shinfo->nr_frags;
692
693	/* ensure that TX ring is not filled up by XDP, always MAX_SKB_FRAGS
694	* will be available for normal TX path and queue is stopped there if
695	* necessary
696	*/
697	if (tsnep_tx_desc_available(tx) < (MAX_SKB_FRAGS + 1 + count))
698	return false;
699
700	entry = &tx->entry[tx->write];
701	entry->xdpf = xdpf;
702
703	retval = tsnep_xdp_tx_map(xdpf, tx, shinfo, count, type);
704	if (retval < 0) {
705	tsnep_tx_unmap(tx, index: tx->write, count);
706	entry->xdpf = NULL;
707
708	tx->dropped++;
709
710	return false;
711	}
712	length = retval;
713
714	for (i = 0; i < count; i++)
715	tsnep_tx_activate(tx, index: (tx->write + i) & TSNEP_RING_MASK, length,
716	last: i == count - 1);
717	tx->write = (tx->write + count) & TSNEP_RING_MASK;
718
719	/* descriptor properties shall be valid before hardware is notified */
720	dma_wmb();
721
722	return true;
723	}
724
725	static void tsnep_xdp_xmit_flush(struct tsnep_tx *tx)
726	{
727	iowrite32(TSNEP_CONTROL_TX_ENABLE, tx->addr + TSNEP_CONTROL);
728	}
729
730	static bool tsnep_xdp_xmit_back(struct tsnep_adapter *adapter,
731	struct xdp_buff *xdp,
732	struct netdev_queue *tx_nq, struct tsnep_tx *tx,
733	bool zc)
734	{
735	struct xdp_frame *xdpf = xdp_convert_buff_to_frame(xdp);
736	bool xmit;
737	u32 type;
738
739	if (unlikely(!xdpf))
740	return false;
741
742	/* no page pool for zero copy */
743	if (zc)
744	type = TSNEP_TX_TYPE_XDP_NDO;
745	else
746	type = TSNEP_TX_TYPE_XDP_TX;
747
748	__netif_tx_lock(txq: tx_nq, smp_processor_id());
749
750	xmit = tsnep_xdp_xmit_frame_ring(xdpf, tx, type);
751
752	/* Avoid transmit queue timeout since we share it with the slow path */
753	if (xmit)
754	txq_trans_cond_update(txq: tx_nq);
755
756	__netif_tx_unlock(txq: tx_nq);
757
758	return xmit;
759	}
760
761	static int tsnep_xdp_tx_map_zc(struct xdp_desc *xdpd, struct tsnep_tx *tx)
762	{
763	struct tsnep_tx_entry *entry;
764	dma_addr_t dma;
765
766	entry = &tx->entry[tx->write];
767	entry->zc = true;
768
769	dma = xsk_buff_raw_get_dma(pool: tx->xsk_pool, addr: xdpd->addr);
770	xsk_buff_raw_dma_sync_for_device(pool: tx->xsk_pool, dma, size: xdpd->len);
771
772	entry->type = TSNEP_TX_TYPE_XSK;
773	entry->len = xdpd->len;
774
775	entry->desc->tx = __cpu_to_le64(dma);
776
777	return xdpd->len;
778	}
779
780	static void tsnep_xdp_xmit_frame_ring_zc(struct xdp_desc *xdpd,
781	struct tsnep_tx *tx)
782	{
783	int length;
784
785	length = tsnep_xdp_tx_map_zc(xdpd, tx);
786
787	tsnep_tx_activate(tx, index: tx->write, length, last: true);
788	tx->write = (tx->write + 1) & TSNEP_RING_MASK;
789	}
790
791	static void tsnep_xdp_xmit_zc(struct tsnep_tx *tx)
792	{
793	int desc_available = tsnep_tx_desc_available(tx);
794	struct xdp_desc *descs = tx->xsk_pool->tx_descs;
795	int batch, i;
796
797	/* ensure that TX ring is not filled up by XDP, always MAX_SKB_FRAGS
798	* will be available for normal TX path and queue is stopped there if
799	* necessary
800	*/
801	if (desc_available <= (MAX_SKB_FRAGS + 1))
802	return;
803	desc_available -= MAX_SKB_FRAGS + 1;
804
805	batch = xsk_tx_peek_release_desc_batch(pool: tx->xsk_pool, max: desc_available);
806	for (i = 0; i < batch; i++)
807	tsnep_xdp_xmit_frame_ring_zc(xdpd: &descs[i], tx);
808
809	if (batch) {
810	/* descriptor properties shall be valid before hardware is
811	* notified
812	*/
813	dma_wmb();
814
815	tsnep_xdp_xmit_flush(tx);
816	}
817	}
818
819	static bool tsnep_tx_poll(struct tsnep_tx *tx, int napi_budget)
820	{
821	struct tsnep_tx_entry *entry;
822	struct netdev_queue *nq;
823	int xsk_frames = 0;
824	int budget = 128;
825	int length;
826	int count;
827
828	nq = netdev_get_tx_queue(dev: tx->adapter->netdev, index: tx->queue_index);
829	__netif_tx_lock(txq: nq, smp_processor_id());
830
831	do {
832	if (tx->read == tx->write)
833	break;
834
835	entry = &tx->entry[tx->read];
836	if ((__le32_to_cpu(entry->desc_wb->properties) &
837	TSNEP_TX_DESC_OWNER_MASK) !=
838	(entry->properties & TSNEP_TX_DESC_OWNER_MASK))
839	break;
840
841	/* descriptor properties shall be read first, because valid data
842	* is signaled there
843	*/
844	dma_rmb();
845
846	count = 1;
847	if ((entry->type & TSNEP_TX_TYPE_SKB) &&
848	skb_shinfo(entry->skb)->nr_frags > 0)
849	count += skb_shinfo(entry->skb)->nr_frags;
850	else if ((entry->type & TSNEP_TX_TYPE_XDP) &&
851	xdp_frame_has_frags(frame: entry->xdpf))
852	count += xdp_get_shared_info_from_frame(frame: entry->xdpf)->nr_frags;
853
854	length = tsnep_tx_unmap(tx, index: tx->read, count);
855
856	if (((entry->type & TSNEP_TX_TYPE_SKB_TSTAMP) == TSNEP_TX_TYPE_SKB_TSTAMP) &&
857	(__le32_to_cpu(entry->desc_wb->properties) &
858	TSNEP_DESC_EXTENDED_WRITEBACK_FLAG)) {
859	struct skb_shared_hwtstamps hwtstamps;
860	u64 timestamp;
861
862	if (entry->skb->sk &&
863	READ_ONCE(entry->skb->sk->sk_tsflags) & SOF_TIMESTAMPING_BIND_PHC)
864	timestamp =
865	__le64_to_cpu(entry->desc_wb->counter);
866	else
867	timestamp =
868	__le64_to_cpu(entry->desc_wb->timestamp);
869
870	memset(&hwtstamps, 0, sizeof(hwtstamps));
871	hwtstamps.hwtstamp = ns_to_ktime(ns: timestamp);
872
873	skb_tstamp_tx(orig_skb: entry->skb, hwtstamps: &hwtstamps);
874	}
875
876	if (entry->type & TSNEP_TX_TYPE_SKB)
877	napi_consume_skb(skb: entry->skb, budget: napi_budget);
878	else if (entry->type & TSNEP_TX_TYPE_XDP)
879	xdp_return_frame_rx_napi(xdpf: entry->xdpf);
880	else
881	xsk_frames++;
882	/* xdpf and zc are union with skb */
883	entry->skb = NULL;
884
885	tx->read = (tx->read + count) & TSNEP_RING_MASK;
886
887	tx->packets++;
888	tx->bytes += length + ETH_FCS_LEN;
889
890	budget--;
891	} while (likely(budget));
892
893	if (tx->xsk_pool) {
894	if (xsk_frames)
895	xsk_tx_completed(pool: tx->xsk_pool, nb_entries: xsk_frames);
896	if (xsk_uses_need_wakeup(pool: tx->xsk_pool))
897	xsk_set_tx_need_wakeup(pool: tx->xsk_pool);
898	tsnep_xdp_xmit_zc(tx);
899	}
900
901	if ((tsnep_tx_desc_available(tx) >= ((MAX_SKB_FRAGS + 1) * 2)) &&
902	netif_tx_queue_stopped(dev_queue: nq)) {
903	netif_tx_wake_queue(dev_queue: nq);
904	}
905
906	__netif_tx_unlock(txq: nq);
907
908	return budget != 0;
909	}
910
911	static bool tsnep_tx_pending(struct tsnep_tx *tx)
912	{
913	struct tsnep_tx_entry *entry;
914	struct netdev_queue *nq;
915	bool pending = false;
916
917	nq = netdev_get_tx_queue(dev: tx->adapter->netdev, index: tx->queue_index);
918	__netif_tx_lock(txq: nq, smp_processor_id());
919
920	if (tx->read != tx->write) {
921	entry = &tx->entry[tx->read];
922	if ((__le32_to_cpu(entry->desc_wb->properties) &
923	TSNEP_TX_DESC_OWNER_MASK) ==
924	(entry->properties & TSNEP_TX_DESC_OWNER_MASK))
925	pending = true;
926	}
927
928	__netif_tx_unlock(txq: nq);
929
930	return pending;
931	}
932
933	static int tsnep_tx_open(struct tsnep_tx *tx)
934	{
935	int retval;
936
937	retval = tsnep_tx_ring_create(tx);
938	if (retval)
939	return retval;
940
941	tsnep_tx_init(tx);
942
943	return 0;
944	}
945
946	static void tsnep_tx_close(struct tsnep_tx *tx)
947	{
948	tsnep_tx_ring_cleanup(tx);
949	}
950
951	static void tsnep_rx_ring_cleanup(struct tsnep_rx *rx)
952	{
953	struct device *dmadev = rx->adapter->dmadev;
954	struct tsnep_rx_entry *entry;
955	int i;
956
957	for (i = 0; i < TSNEP_RING_SIZE; i++) {
958	entry = &rx->entry[i];
959	if (!rx->xsk_pool && entry->page)
960	page_pool_put_full_page(pool: rx->page_pool, page: entry->page,
961	allow_direct: false);
962	if (rx->xsk_pool && entry->xdp)
963	xsk_buff_free(xdp: entry->xdp);
964	/* xdp is union with page */
965	entry->page = NULL;
966	}
967
968	if (rx->page_pool)
969	page_pool_destroy(pool: rx->page_pool);
970
971	memset(rx->entry, 0, sizeof(rx->entry));
972
973	for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
974	if (rx->page[i]) {
975	dma_free_coherent(dev: dmadev, PAGE_SIZE, cpu_addr: rx->page[i],
976	dma_handle: rx->page_dma[i]);
977	rx->page[i] = NULL;
978	rx->page_dma[i] = 0;
979	}
980	}
981	}
982
983	static int tsnep_rx_ring_create(struct tsnep_rx *rx)
984	{
985	struct device *dmadev = rx->adapter->dmadev;
986	struct tsnep_rx_entry *entry;
987	struct page_pool_params pp_params = { 0 };
988	struct tsnep_rx_entry *next_entry;
989	int i, j;
990	int retval;
991
992	for (i = 0; i < TSNEP_RING_PAGE_COUNT; i++) {
993	rx->page[i] =
994	dma_alloc_coherent(dev: dmadev, PAGE_SIZE, dma_handle: &rx->page_dma[i],
995	GFP_KERNEL);
996	if (!rx->page[i]) {
997	retval = -ENOMEM;
998	goto failed;
999	}
1000	for (j = 0; j < TSNEP_RING_ENTRIES_PER_PAGE; j++) {
1001	entry = &rx->entry[TSNEP_RING_ENTRIES_PER_PAGE * i + j];
1002	entry->desc_wb = (struct tsnep_rx_desc_wb *)
1003	(((u8 *)rx->page[i]) + TSNEP_DESC_SIZE * j);
1004	entry->desc = (struct tsnep_rx_desc *)
1005	(((u8 *)entry->desc_wb) + TSNEP_DESC_OFFSET);
1006	entry->desc_dma = rx->page_dma[i] + TSNEP_DESC_SIZE * j;
1007	}
1008	}
1009
1010	pp_params.flags = PP_FLAG_DMA_MAP | PP_FLAG_DMA_SYNC_DEV;
1011	pp_params.order = 0;
1012	pp_params.pool_size = TSNEP_RING_SIZE;
1013	pp_params.nid = dev_to_node(dev: dmadev);
1014	pp_params.dev = dmadev;
1015	pp_params.dma_dir = DMA_BIDIRECTIONAL;
1016	pp_params.max_len = TSNEP_MAX_RX_BUF_SIZE;
1017	pp_params.offset = TSNEP_RX_OFFSET;
1018	rx->page_pool = page_pool_create(params: &pp_params);
1019	if (IS_ERR(ptr: rx->page_pool)) {
1020	retval = PTR_ERR(ptr: rx->page_pool);
1021	rx->page_pool = NULL;
1022	goto failed;
1023	}
1024
1025	for (i = 0; i < TSNEP_RING_SIZE; i++) {
1026	entry = &rx->entry[i];
1027	next_entry = &rx->entry[(i + 1) & TSNEP_RING_MASK];
1028	entry->desc->next = __cpu_to_le64(next_entry->desc_dma);
1029	}
1030
1031	return 0;
1032
1033	failed:
1034	tsnep_rx_ring_cleanup(rx);
1035	return retval;
1036	}
1037
1038	static void tsnep_rx_init(struct tsnep_rx *rx)
1039	{
1040	dma_addr_t dma;
1041
1042	dma = rx->entry[0].desc_dma | TSNEP_RESET_OWNER_COUNTER;
1043	iowrite32(DMA_ADDR_LOW(dma), rx->addr + TSNEP_RX_DESC_ADDR_LOW);
1044	iowrite32(DMA_ADDR_HIGH(dma), rx->addr + TSNEP_RX_DESC_ADDR_HIGH);
1045	rx->write = 0;
1046	rx->read = 0;
1047	rx->owner_counter = 1;
1048	rx->increment_owner_counter = TSNEP_RING_SIZE - 1;
1049	}
1050
1051	static void tsnep_rx_enable(struct tsnep_rx *rx)
1052	{
1053	/* descriptor properties shall be valid before hardware is notified */
1054	dma_wmb();
1055
1056	iowrite32(TSNEP_CONTROL_RX_ENABLE, rx->addr + TSNEP_CONTROL);
1057	}
1058
1059	static void tsnep_rx_disable(struct tsnep_rx *rx)
1060	{
1061	u32 val;
1062
1063	iowrite32(TSNEP_CONTROL_RX_DISABLE, rx->addr + TSNEP_CONTROL);
1064	readx_poll_timeout(ioread32, rx->addr + TSNEP_CONTROL, val,
1065	((val & TSNEP_CONTROL_RX_ENABLE) == 0), 10000,
1066	1000000);
1067	}
1068
1069	static int tsnep_rx_desc_available(struct tsnep_rx *rx)
1070	{
1071	if (rx->read <= rx->write)
1072	return TSNEP_RING_SIZE - rx->write + rx->read - 1;
1073	else
1074	return rx->read - rx->write - 1;
1075	}
1076
1077	static void tsnep_rx_free_page_buffer(struct tsnep_rx *rx)
1078	{
1079	struct page **page;
1080
1081	/* last entry of page_buffer is always zero, because ring cannot be
1082	* filled completely
1083	*/
1084	page = rx->page_buffer;
1085	while (*page) {
1086	page_pool_put_full_page(pool: rx->page_pool, page: *page, allow_direct: false);
1087	*page = NULL;
1088	page++;
1089	}
1090	}
1091
1092	static int tsnep_rx_alloc_page_buffer(struct tsnep_rx *rx)
1093	{
1094	int i;
1095
1096	/* alloc for all ring entries except the last one, because ring cannot
1097	* be filled completely
1098	*/
1099	for (i = 0; i < TSNEP_RING_SIZE - 1; i++) {
1100	rx->page_buffer[i] = page_pool_dev_alloc_pages(pool: rx->page_pool);
1101	if (!rx->page_buffer[i]) {
1102	tsnep_rx_free_page_buffer(rx);
1103
1104	return -ENOMEM;
1105	}
1106	}
1107
1108	return 0;
1109	}
1110
1111	static void tsnep_rx_set_page(struct tsnep_rx *rx, struct tsnep_rx_entry *entry,
1112	struct page *page)
1113	{
1114	entry->page = page;
1115	entry->len = TSNEP_MAX_RX_BUF_SIZE;
1116	entry->dma = page_pool_get_dma_addr(page: entry->page);
1117	entry->desc->rx = __cpu_to_le64(entry->dma + TSNEP_RX_OFFSET);
1118	}
1119
1120	static int tsnep_rx_alloc_buffer(struct tsnep_rx *rx, int index)
1121	{
1122	struct tsnep_rx_entry *entry = &rx->entry[index];
1123	struct page *page;
1124
1125	page = page_pool_dev_alloc_pages(pool: rx->page_pool);
1126	if (unlikely(!page))
1127	return -ENOMEM;
1128	tsnep_rx_set_page(rx, entry, page);
1129
1130	return 0;
1131	}
1132
1133	static void tsnep_rx_reuse_buffer(struct tsnep_rx *rx, int index)
1134	{
1135	struct tsnep_rx_entry *entry = &rx->entry[index];
1136	struct tsnep_rx_entry *read = &rx->entry[rx->read];
1137
1138	tsnep_rx_set_page(rx, entry, page: read->page);
1139	read->page = NULL;
1140	}
1141
1142	static void tsnep_rx_activate(struct tsnep_rx *rx, int index)
1143	{
1144	struct tsnep_rx_entry *entry = &rx->entry[index];
1145
1146	/* TSNEP_MAX_RX_BUF_SIZE and TSNEP_XSK_RX_BUF_SIZE are multiple of 4 */
1147	entry->properties = entry->len & TSNEP_DESC_LENGTH_MASK;
1148	entry->properties |= TSNEP_DESC_INTERRUPT_FLAG;
1149	if (index == rx->increment_owner_counter) {
1150	rx->owner_counter++;
1151	if (rx->owner_counter == 4)
1152	rx->owner_counter = 1;
1153	rx->increment_owner_counter--;
1154	if (rx->increment_owner_counter < 0)
1155	rx->increment_owner_counter = TSNEP_RING_SIZE - 1;
1156	}
1157	entry->properties |=
1158	(rx->owner_counter << TSNEP_DESC_OWNER_COUNTER_SHIFT) &
1159	TSNEP_DESC_OWNER_COUNTER_MASK;
1160
1161	/* descriptor properties shall be written last, because valid data is
1162	* signaled there
1163	*/
1164	dma_wmb();
1165
1166	entry->desc->properties = __cpu_to_le32(entry->properties);
1167	}
1168
1169	static int tsnep_rx_alloc(struct tsnep_rx *rx, int count, bool reuse)
1170	{
1171	bool alloc_failed = false;
1172	int i, index;
1173
1174	for (i = 0; i < count && !alloc_failed; i++) {
1175	index = (rx->write + i) & TSNEP_RING_MASK;
1176
1177	if (unlikely(tsnep_rx_alloc_buffer(rx, index))) {
1178	rx->alloc_failed++;
1179	alloc_failed = true;
1180
1181	/* reuse only if no other allocation was successful */
1182	if (i == 0 && reuse)
1183	tsnep_rx_reuse_buffer(rx, index);
1184	else
1185	break;
1186	}
1187
1188	tsnep_rx_activate(rx, index);
1189	}
1190
1191	if (i)
1192	rx->write = (rx->write + i) & TSNEP_RING_MASK;
1193
1194	return i;
1195	}
1196
1197	static int tsnep_rx_refill(struct tsnep_rx *rx, int count, bool reuse)
1198	{
1199	int desc_refilled;
1200
1201	desc_refilled = tsnep_rx_alloc(rx, count, reuse);
1202	if (desc_refilled)
1203	tsnep_rx_enable(rx);
1204
1205	return desc_refilled;
1206	}
1207
1208	static void tsnep_rx_set_xdp(struct tsnep_rx *rx, struct tsnep_rx_entry *entry,
1209	struct xdp_buff *xdp)
1210	{
1211	entry->xdp = xdp;
1212	entry->len = TSNEP_XSK_RX_BUF_SIZE;
1213	entry->dma = xsk_buff_xdp_get_dma(xdp: entry->xdp);
1214	entry->desc->rx = __cpu_to_le64(entry->dma);
1215	}
1216
1217	static void tsnep_rx_reuse_buffer_zc(struct tsnep_rx *rx, int index)
1218	{
1219	struct tsnep_rx_entry *entry = &rx->entry[index];
1220	struct tsnep_rx_entry *read = &rx->entry[rx->read];
1221
1222	tsnep_rx_set_xdp(rx, entry, xdp: read->xdp);
1223	read->xdp = NULL;
1224	}
1225
1226	static int tsnep_rx_alloc_zc(struct tsnep_rx *rx, int count, bool reuse)
1227	{
1228	u32 allocated;
1229	int i;
1230
1231	allocated = xsk_buff_alloc_batch(pool: rx->xsk_pool, xdp: rx->xdp_batch, max: count);
1232	for (i = 0; i < allocated; i++) {
1233	int index = (rx->write + i) & TSNEP_RING_MASK;
1234	struct tsnep_rx_entry *entry = &rx->entry[index];
1235
1236	tsnep_rx_set_xdp(rx, entry, xdp: rx->xdp_batch[i]);
1237	tsnep_rx_activate(rx, index);
1238	}
1239	if (i == 0) {
1240	rx->alloc_failed++;
1241
1242	if (reuse) {
1243	tsnep_rx_reuse_buffer_zc(rx, index: rx->write);
1244	tsnep_rx_activate(rx, index: rx->write);
1245	}
1246	}
1247
1248	if (i)
1249	rx->write = (rx->write + i) & TSNEP_RING_MASK;
1250
1251	return i;
1252	}
1253
1254	static void tsnep_rx_free_zc(struct tsnep_rx *rx)
1255	{
1256	int i;
1257
1258	for (i = 0; i < TSNEP_RING_SIZE; i++) {
1259	struct tsnep_rx_entry *entry = &rx->entry[i];
1260
1261	if (entry->xdp)
1262	xsk_buff_free(xdp: entry->xdp);
1263	entry->xdp = NULL;
1264	}
1265	}
1266
1267	static int tsnep_rx_refill_zc(struct tsnep_rx *rx, int count, bool reuse)
1268	{
1269	int desc_refilled;
1270
1271	desc_refilled = tsnep_rx_alloc_zc(rx, count, reuse);
1272	if (desc_refilled)
1273	tsnep_rx_enable(rx);
1274
1275	return desc_refilled;
1276	}
1277
1278	static void tsnep_xsk_rx_need_wakeup(struct tsnep_rx *rx, int desc_available)
1279	{
1280	if (desc_available)
1281	xsk_set_rx_need_wakeup(pool: rx->xsk_pool);
1282	else
1283	xsk_clear_rx_need_wakeup(pool: rx->xsk_pool);
1284	}
1285
1286	static bool tsnep_xdp_run_prog(struct tsnep_rx *rx, struct bpf_prog *prog,
1287	struct xdp_buff *xdp, int *status,
1288	struct netdev_queue *tx_nq, struct tsnep_tx *tx)
1289	{
1290	unsigned int length;
1291	unsigned int sync;
1292	u32 act;
1293
1294	length = xdp->data_end - xdp->data_hard_start - XDP_PACKET_HEADROOM;
1295
1296	act = bpf_prog_run_xdp(prog, xdp);
1297	switch (act) {
1298	case XDP_PASS:
1299	return false;
1300	case XDP_TX:
1301	if (!tsnep_xdp_xmit_back(adapter: rx->adapter, xdp, tx_nq, tx, zc: false))
1302	goto out_failure;
1303	*status |= TSNEP_XDP_TX;
1304	return true;
1305	case XDP_REDIRECT:
1306	if (xdp_do_redirect(dev: rx->adapter->netdev, xdp, prog) < 0)
1307	goto out_failure;
1308	*status |= TSNEP_XDP_REDIRECT;
1309	return true;
1310	default:
1311	bpf_warn_invalid_xdp_action(dev: rx->adapter->netdev, prog, act);
1312	fallthrough;
1313	case XDP_ABORTED:
1314	out_failure:
1315	trace_xdp_exception(dev: rx->adapter->netdev, xdp: prog, act);
1316	fallthrough;
1317	case XDP_DROP:
1318	/* Due xdp_adjust_tail: DMA sync for_device cover max len CPU
1319	* touch
1320	*/
1321	sync = xdp->data_end - xdp->data_hard_start -
1322	XDP_PACKET_HEADROOM;
1323	sync = max(sync, length);
1324	page_pool_put_page(pool: rx->page_pool, page: virt_to_head_page(x: xdp->data),
1325	dma_sync_size: sync, allow_direct: true);
1326	return true;
1327	}
1328	}
1329
1330	static bool tsnep_xdp_run_prog_zc(struct tsnep_rx *rx, struct bpf_prog *prog,
1331	struct xdp_buff *xdp, int *status,
1332	struct netdev_queue *tx_nq,
1333	struct tsnep_tx *tx)
1334	{
1335	u32 act;
1336
1337	act = bpf_prog_run_xdp(prog, xdp);
1338
1339	/* XDP_REDIRECT is the main action for zero-copy */
1340	if (likely(act == XDP_REDIRECT)) {
1341	if (xdp_do_redirect(dev: rx->adapter->netdev, xdp, prog) < 0)
1342	goto out_failure;
1343	*status |= TSNEP_XDP_REDIRECT;
1344	return true;
1345	}
1346
1347	switch (act) {
1348	case XDP_PASS:
1349	return false;
1350	case XDP_TX:
1351	if (!tsnep_xdp_xmit_back(adapter: rx->adapter, xdp, tx_nq, tx, zc: true))
1352	goto out_failure;
1353	*status |= TSNEP_XDP_TX;
1354	return true;
1355	default:
1356	bpf_warn_invalid_xdp_action(dev: rx->adapter->netdev, prog, act);
1357	fallthrough;
1358	case XDP_ABORTED:
1359	out_failure:
1360	trace_xdp_exception(dev: rx->adapter->netdev, xdp: prog, act);
1361	fallthrough;
1362	case XDP_DROP:
1363	xsk_buff_free(xdp);
1364	return true;
1365	}
1366	}
1367
1368	static void tsnep_finalize_xdp(struct tsnep_adapter *adapter, int status,
1369	struct netdev_queue *tx_nq, struct tsnep_tx *tx)
1370	{
1371	if (status & TSNEP_XDP_TX) {
1372	__netif_tx_lock(txq: tx_nq, smp_processor_id());
1373	tsnep_xdp_xmit_flush(tx);
1374	__netif_tx_unlock(txq: tx_nq);
1375	}
1376
1377	if (status & TSNEP_XDP_REDIRECT)
1378	xdp_do_flush();
1379	}
1380
1381	static struct sk_buff *tsnep_build_skb(struct tsnep_rx *rx, struct page *page,
1382	int length)
1383	{
1384	struct sk_buff *skb;
1385
1386	skb = napi_build_skb(page_address(page), PAGE_SIZE);
1387	if (unlikely(!skb))
1388	return NULL;
1389
1390	/* update pointers within the skb to store the data */
1391	skb_reserve(skb, TSNEP_RX_OFFSET + TSNEP_RX_INLINE_METADATA_SIZE);
1392	__skb_put(skb, len: length - ETH_FCS_LEN);
1393
1394	if (rx->adapter->hwtstamp_config.rx_filter == HWTSTAMP_FILTER_ALL) {
1395	struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb);
1396	struct tsnep_rx_inline *rx_inline =
1397	(struct tsnep_rx_inline *)(page_address(page) +
1398	TSNEP_RX_OFFSET);
1399
1400	skb_shinfo(skb)->tx_flags |=
1401	SKBTX_HW_TSTAMP_NETDEV;
1402	memset(hwtstamps, 0, sizeof(*hwtstamps));
1403	hwtstamps->netdev_data = rx_inline;
1404	}
1405
1406	skb_record_rx_queue(skb, rx_queue: rx->queue_index);
1407	skb->protocol = eth_type_trans(skb, dev: rx->adapter->netdev);
1408
1409	return skb;
1410	}
1411
1412	static void tsnep_rx_page(struct tsnep_rx *rx, struct napi_struct *napi,
1413	struct page *page, int length)
1414	{
1415	struct sk_buff *skb;
1416
1417	skb = tsnep_build_skb(rx, page, length);
1418	if (skb) {
1419	skb_mark_for_recycle(skb);
1420
1421	rx->packets++;
1422	rx->bytes += length;
1423	if (skb->pkt_type == PACKET_MULTICAST)
1424	rx->multicast++;
1425
1426	napi_gro_receive(napi, skb);
1427	} else {
1428	page_pool_recycle_direct(pool: rx->page_pool, page);
1429
1430	rx->dropped++;
1431	}
1432	}
1433
1434	static int tsnep_rx_poll(struct tsnep_rx *rx, struct napi_struct *napi,
1435	int budget)
1436	{
1437	struct device *dmadev = rx->adapter->dmadev;
1438	enum dma_data_direction dma_dir;
1439	struct tsnep_rx_entry *entry;
1440	struct netdev_queue *tx_nq;
1441	struct bpf_prog *prog;
1442	struct xdp_buff xdp;
1443	struct tsnep_tx *tx;
1444	int desc_available;
1445	int xdp_status = 0;
1446	int done = 0;
1447	int length;
1448
1449	desc_available = tsnep_rx_desc_available(rx);
1450	dma_dir = page_pool_get_dma_dir(pool: rx->page_pool);
1451	prog = READ_ONCE(rx->adapter->xdp_prog);
1452	if (prog) {
1453	tx_nq = netdev_get_tx_queue(dev: rx->adapter->netdev,
1454	index: rx->tx_queue_index);
1455	tx = &rx->adapter->tx[rx->tx_queue_index];
1456
1457	xdp_init_buff(xdp: &xdp, PAGE_SIZE, rxq: &rx->xdp_rxq);
1458	}
1459
1460	while (likely(done < budget) && (rx->read != rx->write)) {
1461	entry = &rx->entry[rx->read];
1462	if ((__le32_to_cpu(entry->desc_wb->properties) &
1463	TSNEP_DESC_OWNER_COUNTER_MASK) !=
1464	(entry->properties & TSNEP_DESC_OWNER_COUNTER_MASK))
1465	break;
1466	done++;
1467
1468	if (desc_available >= TSNEP_RING_RX_REFILL) {
1469	bool reuse = desc_available >= TSNEP_RING_RX_REUSE;
1470
1471	desc_available -= tsnep_rx_refill(rx, count: desc_available,
1472	reuse);
1473	if (!entry->page) {
1474	/* buffer has been reused for refill to prevent
1475	* empty RX ring, thus buffer cannot be used for
1476	* RX processing
1477	*/
1478	rx->read = (rx->read + 1) & TSNEP_RING_MASK;
1479	desc_available++;
1480
1481	rx->dropped++;
1482
1483	continue;
1484	}
1485	}
1486
1487	/* descriptor properties shall be read first, because valid data
1488	* is signaled there
1489	*/
1490	dma_rmb();
1491
1492	prefetch(page_address(entry->page) + TSNEP_RX_OFFSET);
1493	length = __le32_to_cpu(entry->desc_wb->properties) &
1494	TSNEP_DESC_LENGTH_MASK;
1495	dma_sync_single_range_for_cpu(dev: dmadev, addr: entry->dma,
1496	TSNEP_RX_OFFSET, size: length, dir: dma_dir);
1497
1498	/* RX metadata with timestamps is in front of actual data,
1499	* subtract metadata size to get length of actual data and
1500	* consider metadata size as offset of actual data during RX
1501	* processing
1502	*/
1503	length -= TSNEP_RX_INLINE_METADATA_SIZE;
1504
1505	rx->read = (rx->read + 1) & TSNEP_RING_MASK;
1506	desc_available++;
1507
1508	if (prog) {
1509	bool consume;
1510
1511	xdp_prepare_buff(xdp: &xdp, page_address(entry->page),
1512	XDP_PACKET_HEADROOM + TSNEP_RX_INLINE_METADATA_SIZE,
1513	data_len: length - ETH_FCS_LEN, meta_valid: false);
1514
1515	consume = tsnep_xdp_run_prog(rx, prog, xdp: &xdp,
1516	status: &xdp_status, tx_nq, tx);
1517	if (consume) {
1518	rx->packets++;
1519	rx->bytes += length;
1520
1521	entry->page = NULL;
1522
1523	continue;
1524	}
1525	}
1526
1527	tsnep_rx_page(rx, napi, page: entry->page, length);
1528	entry->page = NULL;
1529	}
1530
1531	if (xdp_status)
1532	tsnep_finalize_xdp(adapter: rx->adapter, status: xdp_status, tx_nq, tx);
1533
1534	if (desc_available)
1535	tsnep_rx_refill(rx, count: desc_available, reuse: false);
1536
1537	return done;
1538	}
1539
1540	static int tsnep_rx_poll_zc(struct tsnep_rx *rx, struct napi_struct *napi,
1541	int budget)
1542	{
1543	struct tsnep_rx_entry *entry;
1544	struct netdev_queue *tx_nq;
1545	struct bpf_prog *prog;
1546	struct tsnep_tx *tx;
1547	int desc_available;
1548	int xdp_status = 0;
1549	struct page *page;
1550	int done = 0;
1551	int length;
1552
1553	desc_available = tsnep_rx_desc_available(rx);
1554	prog = READ_ONCE(rx->adapter->xdp_prog);
1555	if (prog) {
1556	tx_nq = netdev_get_tx_queue(dev: rx->adapter->netdev,
1557	index: rx->tx_queue_index);
1558	tx = &rx->adapter->tx[rx->tx_queue_index];
1559	}
1560
1561	while (likely(done < budget) && (rx->read != rx->write)) {
1562	entry = &rx->entry[rx->read];
1563	if ((__le32_to_cpu(entry->desc_wb->properties) &
1564	TSNEP_DESC_OWNER_COUNTER_MASK) !=
1565	(entry->properties & TSNEP_DESC_OWNER_COUNTER_MASK))
1566	break;
1567	done++;
1568
1569	if (desc_available >= TSNEP_RING_RX_REFILL) {
1570	bool reuse = desc_available >= TSNEP_RING_RX_REUSE;
1571
1572	desc_available -= tsnep_rx_refill_zc(rx, count: desc_available,
1573	reuse);
1574	if (!entry->xdp) {
1575	/* buffer has been reused for refill to prevent
1576	* empty RX ring, thus buffer cannot be used for
1577	* RX processing
1578	*/
1579	rx->read = (rx->read + 1) & TSNEP_RING_MASK;
1580	desc_available++;
1581
1582	rx->dropped++;
1583
1584	continue;
1585	}
1586	}
1587
1588	/* descriptor properties shall be read first, because valid data
1589	* is signaled there
1590	*/
1591	dma_rmb();
1592
1593	prefetch(entry->xdp->data);
1594	length = __le32_to_cpu(entry->desc_wb->properties) &
1595	TSNEP_DESC_LENGTH_MASK;
1596	xsk_buff_set_size(xdp: entry->xdp, size: length - ETH_FCS_LEN);
1597	xsk_buff_dma_sync_for_cpu(xdp: entry->xdp);
1598
1599	/* RX metadata with timestamps is in front of actual data,
1600	* subtract metadata size to get length of actual data and
1601	* consider metadata size as offset of actual data during RX
1602	* processing
1603	*/
1604	length -= TSNEP_RX_INLINE_METADATA_SIZE;
1605
1606	rx->read = (rx->read + 1) & TSNEP_RING_MASK;
1607	desc_available++;
1608
1609	if (prog) {
1610	bool consume;
1611
1612	entry->xdp->data += TSNEP_RX_INLINE_METADATA_SIZE;
1613	entry->xdp->data_meta += TSNEP_RX_INLINE_METADATA_SIZE;
1614
1615	consume = tsnep_xdp_run_prog_zc(rx, prog, xdp: entry->xdp,
1616	status: &xdp_status, tx_nq, tx);
1617	if (consume) {
1618	rx->packets++;
1619	rx->bytes += length;
1620
1621	entry->xdp = NULL;
1622
1623	continue;
1624	}
1625	}
1626
1627	page = page_pool_dev_alloc_pages(pool: rx->page_pool);
1628	if (page) {
1629	memcpy(page_address(page) + TSNEP_RX_OFFSET,
1630	entry->xdp->data - TSNEP_RX_INLINE_METADATA_SIZE,
1631	length + TSNEP_RX_INLINE_METADATA_SIZE);
1632	tsnep_rx_page(rx, napi, page, length);
1633	} else {
1634	rx->dropped++;
1635	}
1636	xsk_buff_free(xdp: entry->xdp);
1637	entry->xdp = NULL;
1638	}
1639
1640	if (xdp_status)
1641	tsnep_finalize_xdp(adapter: rx->adapter, status: xdp_status, tx_nq, tx);
1642
1643	if (desc_available)
1644	desc_available -= tsnep_rx_refill_zc(rx, count: desc_available, reuse: false);
1645
1646	if (xsk_uses_need_wakeup(pool: rx->xsk_pool)) {
1647	tsnep_xsk_rx_need_wakeup(rx, desc_available);
1648
1649	return done;
1650	}
1651
1652	return desc_available ? budget : done;
1653	}
1654
1655	static bool tsnep_rx_pending(struct tsnep_rx *rx)
1656	{
1657	struct tsnep_rx_entry *entry;
1658
1659	if (rx->read != rx->write) {
1660	entry = &rx->entry[rx->read];
1661	if ((__le32_to_cpu(entry->desc_wb->properties) &
1662	TSNEP_DESC_OWNER_COUNTER_MASK) ==
1663	(entry->properties & TSNEP_DESC_OWNER_COUNTER_MASK))
1664	return true;
1665	}
1666
1667	return false;
1668	}
1669
1670	static int tsnep_rx_open(struct tsnep_rx *rx)
1671	{
1672	int desc_available;
1673	int retval;
1674
1675	retval = tsnep_rx_ring_create(rx);
1676	if (retval)
1677	return retval;
1678
1679	tsnep_rx_init(rx);
1680
1681	desc_available = tsnep_rx_desc_available(rx);
1682	if (rx->xsk_pool)
1683	retval = tsnep_rx_alloc_zc(rx, count: desc_available, reuse: false);
1684	else
1685	retval = tsnep_rx_alloc(rx, count: desc_available, reuse: false);
1686	if (retval != desc_available) {
1687	retval = -ENOMEM;
1688
1689	goto alloc_failed;
1690	}
1691
1692	/* prealloc pages to prevent allocation failures when XSK pool is
1693	* disabled at runtime
1694	*/
1695	if (rx->xsk_pool) {
1696	retval = tsnep_rx_alloc_page_buffer(rx);
1697	if (retval)
1698	goto alloc_failed;
1699	}
1700
1701	return 0;
1702
1703	alloc_failed:
1704	tsnep_rx_ring_cleanup(rx);
1705	return retval;
1706	}
1707
1708	static void tsnep_rx_close(struct tsnep_rx *rx)
1709	{
1710	if (rx->xsk_pool)
1711	tsnep_rx_free_page_buffer(rx);
1712
1713	tsnep_rx_ring_cleanup(rx);
1714	}
1715
1716	static void tsnep_rx_reopen(struct tsnep_rx *rx)
1717	{
1718	struct page **page = rx->page_buffer;
1719	int i;
1720
1721	tsnep_rx_init(rx);
1722
1723	for (i = 0; i < TSNEP_RING_SIZE; i++) {
1724	struct tsnep_rx_entry *entry = &rx->entry[i];
1725
1726	/* defined initial values for properties are required for
1727	* correct owner counter checking
1728	*/
1729	entry->desc->properties = 0;
1730	entry->desc_wb->properties = 0;
1731
1732	/* prevent allocation failures by reusing kept pages */
1733	if (*page) {
1734	tsnep_rx_set_page(rx, entry, page: *page);
1735	tsnep_rx_activate(rx, index: rx->write);
1736	rx->write++;
1737
1738	*page = NULL;
1739	page++;
1740	}
1741	}
1742	}
1743
1744	static void tsnep_rx_reopen_xsk(struct tsnep_rx *rx)
1745	{
1746	struct page **page = rx->page_buffer;
1747	u32 allocated;
1748	int i;
1749
1750	tsnep_rx_init(rx);
1751
1752	/* alloc all ring entries except the last one, because ring cannot be
1753	* filled completely, as many buffers as possible is enough as wakeup is
1754	* done if new buffers are available
1755	*/
1756	allocated = xsk_buff_alloc_batch(pool: rx->xsk_pool, xdp: rx->xdp_batch,
1757	TSNEP_RING_SIZE - 1);
1758
1759	for (i = 0; i < TSNEP_RING_SIZE; i++) {
1760	struct tsnep_rx_entry *entry = &rx->entry[i];
1761
1762	/* keep pages to prevent allocation failures when xsk is
1763	* disabled
1764	*/
1765	if (entry->page) {
1766	*page = entry->page;
1767	entry->page = NULL;
1768
1769	page++;
1770	}
1771
1772	/* defined initial values for properties are required for
1773	* correct owner counter checking
1774	*/
1775	entry->desc->properties = 0;
1776	entry->desc_wb->properties = 0;
1777
1778	if (allocated) {
1779	tsnep_rx_set_xdp(rx, entry,
1780	xdp: rx->xdp_batch[allocated - 1]);
1781	tsnep_rx_activate(rx, index: rx->write);
1782	rx->write++;
1783
1784	allocated--;
1785	}
1786	}
1787
1788	/* set need wakeup flag immediately if ring is not filled completely,
1789	* first polling would be too late as need wakeup signalisation would
1790	* be delayed for an indefinite time
1791	*/
1792	if (xsk_uses_need_wakeup(pool: rx->xsk_pool))
1793	tsnep_xsk_rx_need_wakeup(rx, desc_available: tsnep_rx_desc_available(rx));
1794	}
1795
1796	static bool tsnep_pending(struct tsnep_queue *queue)
1797	{
1798	if (queue->tx && tsnep_tx_pending(tx: queue->tx))
1799	return true;
1800
1801	if (queue->rx && tsnep_rx_pending(rx: queue->rx))
1802	return true;
1803
1804	return false;
1805	}
1806
1807	static int tsnep_poll(struct napi_struct *napi, int budget)
1808	{
1809	struct tsnep_queue *queue = container_of(napi, struct tsnep_queue,
1810	napi);
1811	bool complete = true;
1812	int done = 0;
1813
1814	if (queue->tx)
1815	complete = tsnep_tx_poll(tx: queue->tx, napi_budget: budget);
1816
1817	/* handle case where we are called by netpoll with a budget of 0 */
1818	if (unlikely(budget <= 0))
1819	return budget;
1820
1821	if (queue->rx) {
1822	done = queue->rx->xsk_pool ?
1823	tsnep_rx_poll_zc(rx: queue->rx, napi, budget) :
1824	tsnep_rx_poll(rx: queue->rx, napi, budget);
1825	if (done >= budget)
1826	complete = false;
1827	}
1828
1829	/* if all work not completed, return budget and keep polling */
1830	if (!complete)
1831	return budget;
1832
1833	if (likely(napi_complete_done(napi, done))) {
1834	tsnep_enable_irq(adapter: queue->adapter, mask: queue->irq_mask);
1835
1836	/* reschedule if work is already pending, prevent rotten packets
1837	* which are transmitted or received after polling but before
1838	* interrupt enable
1839	*/
1840	if (tsnep_pending(queue)) {
1841	tsnep_disable_irq(adapter: queue->adapter, mask: queue->irq_mask);
1842	napi_schedule(n: napi);
1843	}
1844	}
1845
1846	return min(done, budget - 1);
1847	}
1848
1849	static int tsnep_request_irq(struct tsnep_queue *queue, bool first)
1850	{
1851	const char *name = netdev_name(dev: queue->adapter->netdev);
1852	irq_handler_t handler;
1853	void *dev;
1854	int retval;
1855
1856	if (first) {
1857	sprintf(buf: queue->name, fmt: "%s-mac", name);
1858	handler = tsnep_irq;
1859	dev = queue->adapter;
1860	} else {
1861	if (queue->tx && queue->rx)
1862	snprintf(buf: queue->name, size: sizeof(queue->name), fmt: "%s-txrx-%d",
1863	name, queue->rx->queue_index);
1864	else if (queue->tx)
1865	snprintf(buf: queue->name, size: sizeof(queue->name), fmt: "%s-tx-%d",
1866	name, queue->tx->queue_index);
1867	else
1868	snprintf(buf: queue->name, size: sizeof(queue->name), fmt: "%s-rx-%d",
1869	name, queue->rx->queue_index);
1870	handler = tsnep_irq_txrx;
1871	dev = queue;
1872	}
1873
1874	retval = request_irq(irq: queue->irq, handler, flags: 0, name: queue->name, dev);
1875	if (retval) {
1876	/* if name is empty, then interrupt won't be freed */
1877	memset(queue->name, 0, sizeof(queue->name));
1878	}
1879
1880	return retval;
1881	}
1882
1883	static void tsnep_free_irq(struct tsnep_queue *queue, bool first)
1884	{
1885	void *dev;
1886
1887	if (!strlen(queue->name))
1888	return;
1889
1890	if (first)
1891	dev = queue->adapter;
1892	else
1893	dev = queue;
1894
1895	free_irq(queue->irq, dev);
1896	memset(queue->name, 0, sizeof(queue->name));
1897	}
1898
1899	static void tsnep_queue_close(struct tsnep_queue *queue, bool first)
1900	{
1901	struct tsnep_rx *rx = queue->rx;
1902
1903	tsnep_free_irq(queue, first);
1904
1905	if (rx) {
1906	if (xdp_rxq_info_is_reg(xdp_rxq: &rx->xdp_rxq))
1907	xdp_rxq_info_unreg(xdp_rxq: &rx->xdp_rxq);
1908	if (xdp_rxq_info_is_reg(xdp_rxq: &rx->xdp_rxq_zc))
1909	xdp_rxq_info_unreg(xdp_rxq: &rx->xdp_rxq_zc);
1910	}
1911
1912	netif_napi_del(napi: &queue->napi);
1913	}
1914
1915	static int tsnep_queue_open(struct tsnep_adapter *adapter,
1916	struct tsnep_queue *queue, bool first)
1917	{
1918	struct tsnep_rx *rx = queue->rx;
1919	struct tsnep_tx *tx = queue->tx;
1920	int retval;
1921
1922	netif_napi_add(dev: adapter->netdev, napi: &queue->napi, poll: tsnep_poll);
1923
1924	if (rx) {
1925	/* choose TX queue for XDP_TX */
1926	if (tx)
1927	rx->tx_queue_index = tx->queue_index;
1928	else if (rx->queue_index < adapter->num_tx_queues)
1929	rx->tx_queue_index = rx->queue_index;
1930	else
1931	rx->tx_queue_index = 0;
1932
1933	/* prepare both memory models to eliminate possible registration
1934	* errors when memory model is switched between page pool and
1935	* XSK pool during runtime
1936	*/
1937	retval = xdp_rxq_info_reg(xdp_rxq: &rx->xdp_rxq, dev: adapter->netdev,
1938	queue_index: rx->queue_index, napi_id: queue->napi.napi_id);
1939	if (retval)
1940	goto failed;
1941	retval = xdp_rxq_info_reg_mem_model(xdp_rxq: &rx->xdp_rxq,
1942	type: MEM_TYPE_PAGE_POOL,
1943	allocator: rx->page_pool);
1944	if (retval)
1945	goto failed;
1946	retval = xdp_rxq_info_reg(xdp_rxq: &rx->xdp_rxq_zc, dev: adapter->netdev,
1947	queue_index: rx->queue_index, napi_id: queue->napi.napi_id);
1948	if (retval)
1949	goto failed;
1950	retval = xdp_rxq_info_reg_mem_model(xdp_rxq: &rx->xdp_rxq_zc,
1951	type: MEM_TYPE_XSK_BUFF_POOL,
1952	NULL);
1953	if (retval)
1954	goto failed;
1955	if (rx->xsk_pool)
1956	xsk_pool_set_rxq_info(pool: rx->xsk_pool, rxq: &rx->xdp_rxq_zc);
1957	}
1958
1959	retval = tsnep_request_irq(queue, first);
1960	if (retval) {
1961	netif_err(adapter, drv, adapter->netdev,
1962	"can't get assigned irq %d.\n", queue->irq);
1963	goto failed;
1964	}
1965
1966	return 0;
1967
1968	failed:
1969	tsnep_queue_close(queue, first);
1970
1971	return retval;
1972	}
1973
1974	static void tsnep_queue_enable(struct tsnep_queue *queue)
1975	{
1976	struct tsnep_adapter *adapter = queue->adapter;
1977
1978	netif_napi_set_irq(napi: &queue->napi, irq: queue->irq);
1979	napi_enable(n: &queue->napi);
1980	tsnep_enable_irq(adapter, mask: queue->irq_mask);
1981
1982	if (queue->tx) {
1983	netif_queue_set_napi(dev: adapter->netdev, queue_index: queue->tx->queue_index,
1984	type: NETDEV_QUEUE_TYPE_TX, napi: &queue->napi);
1985	tsnep_tx_enable(tx: queue->tx);
1986	}
1987
1988	if (queue->rx) {
1989	netif_queue_set_napi(dev: adapter->netdev, queue_index: queue->rx->queue_index,
1990	type: NETDEV_QUEUE_TYPE_RX, napi: &queue->napi);
1991	tsnep_rx_enable(rx: queue->rx);
1992	}
1993	}
1994
1995	static void tsnep_queue_disable(struct tsnep_queue *queue)
1996	{
1997	struct tsnep_adapter *adapter = queue->adapter;
1998
1999	if (queue->rx)
2000	netif_queue_set_napi(dev: adapter->netdev, queue_index: queue->rx->queue_index,
2001	type: NETDEV_QUEUE_TYPE_RX, NULL);
2002
2003	if (queue->tx) {
2004	tsnep_tx_disable(tx: queue->tx, napi: &queue->napi);
2005	netif_queue_set_napi(dev: adapter->netdev, queue_index: queue->tx->queue_index,
2006	type: NETDEV_QUEUE_TYPE_TX, NULL);
2007	}
2008
2009	napi_disable(n: &queue->napi);
2010	tsnep_disable_irq(adapter, mask: queue->irq_mask);
2011
2012	/* disable RX after NAPI polling has been disabled, because RX can be
2013	* enabled during NAPI polling
2014	*/
2015	if (queue->rx)
2016	tsnep_rx_disable(rx: queue->rx);
2017	}
2018
2019	static int tsnep_netdev_open(struct net_device *netdev)
2020	{
2021	struct tsnep_adapter *adapter = netdev_priv(dev: netdev);
2022	int i, retval;
2023
2024	for (i = 0; i < adapter->num_queues; i++) {
2025	if (adapter->queue[i].tx) {
2026	retval = tsnep_tx_open(tx: adapter->queue[i].tx);
2027	if (retval)
2028	goto failed;
2029	}
2030	if (adapter->queue[i].rx) {
2031	retval = tsnep_rx_open(rx: adapter->queue[i].rx);
2032	if (retval)
2033	goto failed;
2034	}
2035
2036	retval = tsnep_queue_open(adapter, queue: &adapter->queue[i], first: i == 0);
2037	if (retval)
2038	goto failed;
2039	}
2040
2041	retval = netif_set_real_num_tx_queues(dev: adapter->netdev,
2042	txq: adapter->num_tx_queues);
2043	if (retval)
2044	goto failed;
2045	retval = netif_set_real_num_rx_queues(dev: adapter->netdev,
2046	rxq: adapter->num_rx_queues);
2047	if (retval)
2048	goto failed;
2049
2050	tsnep_enable_irq(adapter, ECM_INT_LINK);
2051	retval = tsnep_phy_open(adapter);
2052	if (retval)
2053	goto phy_failed;
2054
2055	for (i = 0; i < adapter->num_queues; i++)
2056	tsnep_queue_enable(queue: &adapter->queue[i]);
2057
2058	return 0;
2059
2060	phy_failed:
2061	tsnep_disable_irq(adapter, ECM_INT_LINK);
2062	failed:
2063	for (i = 0; i < adapter->num_queues; i++) {
2064	tsnep_queue_close(queue: &adapter->queue[i], first: i == 0);
2065
2066	if (adapter->queue[i].rx)
2067	tsnep_rx_close(rx: adapter->queue[i].rx);
2068	if (adapter->queue[i].tx)
2069	tsnep_tx_close(tx: adapter->queue[i].tx);
2070	}
2071	return retval;
2072	}
2073
2074	static int tsnep_netdev_close(struct net_device *netdev)
2075	{
2076	struct tsnep_adapter *adapter = netdev_priv(dev: netdev);
2077	int i;
2078
2079	tsnep_disable_irq(adapter, ECM_INT_LINK);
2080	tsnep_phy_close(adapter);
2081
2082	for (i = 0; i < adapter->num_queues; i++) {
2083	tsnep_queue_disable(queue: &adapter->queue[i]);
2084
2085	tsnep_queue_close(queue: &adapter->queue[i], first: i == 0);
2086
2087	if (adapter->queue[i].rx)
2088	tsnep_rx_close(rx: adapter->queue[i].rx);
2089	if (adapter->queue[i].tx)
2090	tsnep_tx_close(tx: adapter->queue[i].tx);
2091	}
2092
2093	return 0;
2094	}
2095
2096	int tsnep_enable_xsk(struct tsnep_queue *queue, struct xsk_buff_pool *pool)
2097	{
2098	bool running = netif_running(dev: queue->adapter->netdev);
2099	u32 frame_size;
2100
2101	frame_size = xsk_pool_get_rx_frame_size(pool);
2102	if (frame_size < TSNEP_XSK_RX_BUF_SIZE)
2103	return -EOPNOTSUPP;
2104
2105	queue->rx->page_buffer = kcalloc(TSNEP_RING_SIZE,
2106	sizeof(*queue->rx->page_buffer),
2107	GFP_KERNEL);
2108	if (!queue->rx->page_buffer)
2109	return -ENOMEM;
2110	queue->rx->xdp_batch = kcalloc(TSNEP_RING_SIZE,
2111	sizeof(*queue->rx->xdp_batch),
2112	GFP_KERNEL);
2113	if (!queue->rx->xdp_batch) {
2114	kfree(objp: queue->rx->page_buffer);
2115	queue->rx->page_buffer = NULL;
2116
2117	return -ENOMEM;
2118	}
2119
2120	xsk_pool_set_rxq_info(pool, rxq: &queue->rx->xdp_rxq_zc);
2121
2122	if (running)
2123	tsnep_queue_disable(queue);
2124
2125	queue->tx->xsk_pool = pool;
2126	queue->rx->xsk_pool = pool;
2127
2128	if (running) {
2129	tsnep_rx_reopen_xsk(rx: queue->rx);
2130	tsnep_queue_enable(queue);
2131	}
2132
2133	return 0;
2134	}
2135
2136	void tsnep_disable_xsk(struct tsnep_queue *queue)
2137	{
2138	bool running = netif_running(dev: queue->adapter->netdev);
2139
2140	if (running)
2141	tsnep_queue_disable(queue);
2142
2143	tsnep_rx_free_zc(rx: queue->rx);
2144
2145	queue->rx->xsk_pool = NULL;
2146	queue->tx->xsk_pool = NULL;
2147
2148	if (running) {
2149	tsnep_rx_reopen(rx: queue->rx);
2150	tsnep_queue_enable(queue);
2151	}
2152
2153	kfree(objp: queue->rx->xdp_batch);
2154	queue->rx->xdp_batch = NULL;
2155	kfree(objp: queue->rx->page_buffer);
2156	queue->rx->page_buffer = NULL;
2157	}
2158
2159	static netdev_tx_t tsnep_netdev_xmit_frame(struct sk_buff *skb,
2160	struct net_device *netdev)
2161	{
2162	struct tsnep_adapter *adapter = netdev_priv(dev: netdev);
2163	u16 queue_mapping = skb_get_queue_mapping(skb);
2164
2165	if (queue_mapping >= adapter->num_tx_queues)
2166	queue_mapping = 0;
2167
2168	return tsnep_xmit_frame_ring(skb, tx: &adapter->tx[queue_mapping]);
2169	}
2170
2171	static int tsnep_netdev_ioctl(struct net_device *netdev, struct ifreq *ifr,
2172	int cmd)
2173	{
2174	if (!netif_running(dev: netdev))
2175	return -EINVAL;
2176	if (cmd == SIOCSHWTSTAMP || cmd == SIOCGHWTSTAMP)
2177	return tsnep_ptp_ioctl(netdev, ifr, cmd);
2178	return phy_mii_ioctl(phydev: netdev->phydev, ifr, cmd);
2179	}
2180
2181	static void tsnep_netdev_set_multicast(struct net_device *netdev)
2182	{
2183	struct tsnep_adapter *adapter = netdev_priv(dev: netdev);
2184
2185	u16 rx_filter = 0;
2186
2187	/* configured MAC address and broadcasts are never filtered */
2188	if (netdev->flags & IFF_PROMISC) {
2189	rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_MULTICASTS;
2190	rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_UNICASTS;
2191	} else if (!netdev_mc_empty(netdev) || (netdev->flags & IFF_ALLMULTI)) {
2192	rx_filter |= TSNEP_RX_FILTER_ACCEPT_ALL_MULTICASTS;
2193	}
2194	iowrite16(rx_filter, adapter->addr + TSNEP_RX_FILTER);
2195	}
2196
2197	static void tsnep_netdev_get_stats64(struct net_device *netdev,
2198	struct rtnl_link_stats64 *stats)
2199	{
2200	struct tsnep_adapter *adapter = netdev_priv(dev: netdev);
2201	u32 reg;
2202	u32 val;
2203	int i;
2204
2205	for (i = 0; i < adapter->num_tx_queues; i++) {
2206	stats->tx_packets += adapter->tx[i].packets;
2207	stats->tx_bytes += adapter->tx[i].bytes;
2208	stats->tx_dropped += adapter->tx[i].dropped;
2209	}
2210	for (i = 0; i < adapter->num_rx_queues; i++) {
2211	stats->rx_packets += adapter->rx[i].packets;
2212	stats->rx_bytes += adapter->rx[i].bytes;
2213	stats->rx_dropped += adapter->rx[i].dropped;
2214	stats->multicast += adapter->rx[i].multicast;
2215
2216	reg = ioread32(adapter->addr + TSNEP_QUEUE(i) +
2217	TSNEP_RX_STATISTIC);
2218	val = (reg & TSNEP_RX_STATISTIC_NO_DESC_MASK) >>
2219	TSNEP_RX_STATISTIC_NO_DESC_SHIFT;
2220	stats->rx_dropped += val;
2221	val = (reg & TSNEP_RX_STATISTIC_BUFFER_TOO_SMALL_MASK) >>
2222	TSNEP_RX_STATISTIC_BUFFER_TOO_SMALL_SHIFT;
2223	stats->rx_dropped += val;
2224	val = (reg & TSNEP_RX_STATISTIC_FIFO_OVERFLOW_MASK) >>
2225	TSNEP_RX_STATISTIC_FIFO_OVERFLOW_SHIFT;
2226	stats->rx_errors += val;
2227	stats->rx_fifo_errors += val;
2228	val = (reg & TSNEP_RX_STATISTIC_INVALID_FRAME_MASK) >>
2229	TSNEP_RX_STATISTIC_INVALID_FRAME_SHIFT;
2230	stats->rx_errors += val;
2231	stats->rx_frame_errors += val;
2232	}
2233
2234	reg = ioread32(adapter->addr + ECM_STAT);
2235	val = (reg & ECM_STAT_RX_ERR_MASK) >> ECM_STAT_RX_ERR_SHIFT;
2236	stats->rx_errors += val;
2237	val = (reg & ECM_STAT_INV_FRM_MASK) >> ECM_STAT_INV_FRM_SHIFT;
2238	stats->rx_errors += val;
2239	stats->rx_crc_errors += val;
2240	val = (reg & ECM_STAT_FWD_RX_ERR_MASK) >> ECM_STAT_FWD_RX_ERR_SHIFT;
2241	stats->rx_errors += val;
2242	}
2243
2244	static void tsnep_mac_set_address(struct tsnep_adapter *adapter, u8 *addr)
2245	{
2246	iowrite32(*(u32 *)addr, adapter->addr + TSNEP_MAC_ADDRESS_LOW);
2247	iowrite16(*(u16 *)(addr + sizeof(u32)),
2248	adapter->addr + TSNEP_MAC_ADDRESS_HIGH);
2249
2250	ether_addr_copy(dst: adapter->mac_address, src: addr);
2251	netif_info(adapter, drv, adapter->netdev, "MAC address set to %pM\n",
2252	addr);
2253	}
2254
2255	static int tsnep_netdev_set_mac_address(struct net_device *netdev, void *addr)
2256	{
2257	struct tsnep_adapter *adapter = netdev_priv(dev: netdev);
2258	struct sockaddr *sock_addr = addr;
2259	int retval;
2260
2261	retval = eth_prepare_mac_addr_change(dev: netdev, p: sock_addr);
2262	if (retval)
2263	return retval;
2264	eth_hw_addr_set(dev: netdev, addr: sock_addr->sa_data);
2265	tsnep_mac_set_address(adapter, addr: sock_addr->sa_data);
2266
2267	return 0;
2268	}
2269
2270	static int tsnep_netdev_set_features(struct net_device *netdev,
2271	netdev_features_t features)
2272	{
2273	struct tsnep_adapter *adapter = netdev_priv(dev: netdev);
2274	netdev_features_t changed = netdev->features ^ features;
2275	bool enable;
2276	int retval = 0;
2277
2278	if (changed & NETIF_F_LOOPBACK) {
2279	enable = !!(features & NETIF_F_LOOPBACK);
2280	retval = tsnep_phy_loopback(adapter, enable);
2281	}
2282
2283	return retval;
2284	}
2285
2286	static ktime_t tsnep_netdev_get_tstamp(struct net_device *netdev,
2287	const struct skb_shared_hwtstamps *hwtstamps,
2288	bool cycles)
2289	{
2290	struct tsnep_rx_inline *rx_inline = hwtstamps->netdev_data;
2291	u64 timestamp;
2292
2293	if (cycles)
2294	timestamp = __le64_to_cpu(rx_inline->counter);
2295	else
2296	timestamp = __le64_to_cpu(rx_inline->timestamp);
2297
2298	return ns_to_ktime(ns: timestamp);
2299	}
2300
2301	static int tsnep_netdev_bpf(struct net_device *dev, struct netdev_bpf *bpf)
2302	{
2303	struct tsnep_adapter *adapter = netdev_priv(dev);
2304
2305	switch (bpf->command) {
2306	case XDP_SETUP_PROG:
2307	return tsnep_xdp_setup_prog(adapter, prog: bpf->prog, extack: bpf->extack);
2308	case XDP_SETUP_XSK_POOL:
2309	return tsnep_xdp_setup_pool(adapter, pool: bpf->xsk.pool,
2310	queue_id: bpf->xsk.queue_id);
2311	default:
2312	return -EOPNOTSUPP;
2313	}
2314	}
2315
2316	static struct tsnep_tx *tsnep_xdp_get_tx(struct tsnep_adapter *adapter, u32 cpu)
2317	{
2318	if (cpu >= TSNEP_MAX_QUEUES)
2319	cpu &= TSNEP_MAX_QUEUES - 1;
2320
2321	while (cpu >= adapter->num_tx_queues)
2322	cpu -= adapter->num_tx_queues;
2323
2324	return &adapter->tx[cpu];
2325	}
2326
2327	static int tsnep_netdev_xdp_xmit(struct net_device *dev, int n,
2328	struct xdp_frame **xdp, u32 flags)
2329	{
2330	struct tsnep_adapter *adapter = netdev_priv(dev);
2331	u32 cpu = smp_processor_id();
2332	struct netdev_queue *nq;
2333	struct tsnep_tx *tx;
2334	int nxmit;
2335	bool xmit;
2336
2337	if (unlikely(flags & ~XDP_XMIT_FLAGS_MASK))
2338	return -EINVAL;
2339
2340	tx = tsnep_xdp_get_tx(adapter, cpu);
2341	nq = netdev_get_tx_queue(dev: adapter->netdev, index: tx->queue_index);
2342
2343	__netif_tx_lock(txq: nq, cpu);
2344
2345	for (nxmit = 0; nxmit < n; nxmit++) {
2346	xmit = tsnep_xdp_xmit_frame_ring(xdpf: xdp[nxmit], tx,
2347	TSNEP_TX_TYPE_XDP_NDO);
2348	if (!xmit)
2349	break;
2350
2351	/* avoid transmit queue timeout since we share it with the slow
2352	* path
2353	*/
2354	txq_trans_cond_update(txq: nq);
2355	}
2356
2357	if (flags & XDP_XMIT_FLUSH)
2358	tsnep_xdp_xmit_flush(tx);
2359
2360	__netif_tx_unlock(txq: nq);
2361
2362	return nxmit;
2363	}
2364
2365	static int tsnep_netdev_xsk_wakeup(struct net_device *dev, u32 queue_id,
2366	u32 flags)
2367	{
2368	struct tsnep_adapter *adapter = netdev_priv(dev);
2369	struct tsnep_queue *queue;
2370
2371	if (queue_id >= adapter->num_rx_queues ||
2372	queue_id >= adapter->num_tx_queues)
2373	return -EINVAL;
2374
2375	queue = &adapter->queue[queue_id];
2376
2377	if (!napi_if_scheduled_mark_missed(n: &queue->napi))
2378	napi_schedule(n: &queue->napi);
2379
2380	return 0;
2381	}
2382
2383	static const struct net_device_ops tsnep_netdev_ops = {
2384	.ndo_open = tsnep_netdev_open,
2385	.ndo_stop = tsnep_netdev_close,
2386	.ndo_start_xmit = tsnep_netdev_xmit_frame,
2387	.ndo_eth_ioctl = tsnep_netdev_ioctl,
2388	.ndo_set_rx_mode = tsnep_netdev_set_multicast,
2389	.ndo_get_stats64 = tsnep_netdev_get_stats64,
2390	.ndo_set_mac_address = tsnep_netdev_set_mac_address,
2391	.ndo_set_features = tsnep_netdev_set_features,
2392	.ndo_get_tstamp = tsnep_netdev_get_tstamp,
2393	.ndo_setup_tc = tsnep_tc_setup,
2394	.ndo_bpf = tsnep_netdev_bpf,
2395	.ndo_xdp_xmit = tsnep_netdev_xdp_xmit,
2396	.ndo_xsk_wakeup = tsnep_netdev_xsk_wakeup,
2397	};
2398
2399	static int tsnep_mac_init(struct tsnep_adapter *adapter)
2400	{
2401	int retval;
2402
2403	/* initialize RX filtering, at least configured MAC address and
2404	* broadcast are not filtered
2405	*/
2406	iowrite16(0, adapter->addr + TSNEP_RX_FILTER);
2407
2408	/* try to get MAC address in the following order:
2409	* - device tree
2410	* - valid MAC address already set
2411	* - MAC address register if valid
2412	* - random MAC address
2413	*/
2414	retval = of_get_mac_address(np: adapter->pdev->dev.of_node,
2415	mac: adapter->mac_address);
2416	if (retval == -EPROBE_DEFER)
2417	return retval;
2418	if (retval && !is_valid_ether_addr(addr: adapter->mac_address)) {
2419	*(u32 *)adapter->mac_address =
2420	ioread32(adapter->addr + TSNEP_MAC_ADDRESS_LOW);
2421	*(u16 *)(adapter->mac_address + sizeof(u32)) =
2422	ioread16(adapter->addr + TSNEP_MAC_ADDRESS_HIGH);
2423	if (!is_valid_ether_addr(addr: adapter->mac_address))
2424	eth_random_addr(addr: adapter->mac_address);
2425	}
2426
2427	tsnep_mac_set_address(adapter, addr: adapter->mac_address);
2428	eth_hw_addr_set(dev: adapter->netdev, addr: adapter->mac_address);
2429
2430	return 0;
2431	}
2432
2433	static int tsnep_mdio_init(struct tsnep_adapter *adapter)
2434	{
2435	struct device_node *np = adapter->pdev->dev.of_node;
2436	int retval;
2437
2438	if (np) {
2439	np = of_get_child_by_name(node: np, name: "mdio");
2440	if (!np)
2441	return 0;
2442
2443	adapter->suppress_preamble =
2444	of_property_read_bool(np, propname: "suppress-preamble");
2445	}
2446
2447	adapter->mdiobus = devm_mdiobus_alloc(dev: &adapter->pdev->dev);
2448	if (!adapter->mdiobus) {
2449	retval = -ENOMEM;
2450
2451	goto out;
2452	}
2453
2454	adapter->mdiobus->priv = (void *)adapter;
2455	adapter->mdiobus->parent = &adapter->pdev->dev;
2456	adapter->mdiobus->read = tsnep_mdiobus_read;
2457	adapter->mdiobus->write = tsnep_mdiobus_write;
2458	adapter->mdiobus->name = TSNEP "-mdiobus";
2459	snprintf(buf: adapter->mdiobus->id, MII_BUS_ID_SIZE, fmt: "%s",
2460	adapter->pdev->name);
2461
2462	/* do not scan broadcast address */
2463	adapter->mdiobus->phy_mask = 0x0000001;
2464
2465	retval = of_mdiobus_register(mdio: adapter->mdiobus, np);
2466
2467	out:
2468	of_node_put(node: np);
2469
2470	return retval;
2471	}
2472
2473	static int tsnep_phy_init(struct tsnep_adapter *adapter)
2474	{
2475	struct device_node *phy_node;
2476	int retval;
2477
2478	retval = of_get_phy_mode(np: adapter->pdev->dev.of_node,
2479	interface: &adapter->phy_mode);
2480	if (retval)
2481	adapter->phy_mode = PHY_INTERFACE_MODE_GMII;
2482
2483	phy_node = of_parse_phandle(np: adapter->pdev->dev.of_node, phandle_name: "phy-handle",
2484	index: 0);
2485	adapter->phydev = of_phy_find_device(phy_np: phy_node);
2486	of_node_put(node: phy_node);
2487	if (!adapter->phydev && adapter->mdiobus)
2488	adapter->phydev = phy_find_first(bus: adapter->mdiobus);
2489	if (!adapter->phydev)
2490	return -EIO;
2491
2492	return 0;
2493	}
2494
2495	static int tsnep_queue_init(struct tsnep_adapter *adapter, int queue_count)
2496	{
2497	u32 irq_mask = ECM_INT_TX_0 | ECM_INT_RX_0;
2498	char name[8];
2499	int i;
2500	int retval;
2501
2502	/* one TX/RX queue pair for netdev is mandatory */
2503	if (platform_irq_count(adapter->pdev) == 1)
2504	retval = platform_get_irq(adapter->pdev, 0);
2505	else
2506	retval = platform_get_irq_byname(adapter->pdev, "mac");
2507	if (retval < 0)
2508	return retval;
2509	adapter->num_tx_queues = 1;
2510	adapter->num_rx_queues = 1;
2511	adapter->num_queues = 1;
2512	adapter->queue[0].adapter = adapter;
2513	adapter->queue[0].irq = retval;
2514	adapter->queue[0].tx = &adapter->tx[0];
2515	adapter->queue[0].tx->adapter = adapter;
2516	adapter->queue[0].tx->addr = adapter->addr + TSNEP_QUEUE(0);
2517	adapter->queue[0].tx->queue_index = 0;
2518	adapter->queue[0].rx = &adapter->rx[0];
2519	adapter->queue[0].rx->adapter = adapter;
2520	adapter->queue[0].rx->addr = adapter->addr + TSNEP_QUEUE(0);
2521	adapter->queue[0].rx->queue_index = 0;
2522	adapter->queue[0].irq_mask = irq_mask;
2523	adapter->queue[0].irq_delay_addr = adapter->addr + ECM_INT_DELAY;
2524	retval = tsnep_set_irq_coalesce(queue: &adapter->queue[0],
2525	TSNEP_COALESCE_USECS_DEFAULT);
2526	if (retval < 0)
2527	return retval;
2528
2529	adapter->netdev->irq = adapter->queue[0].irq;
2530
2531	/* add additional TX/RX queue pairs only if dedicated interrupt is
2532	* available
2533	*/
2534	for (i = 1; i < queue_count; i++) {
2535	sprintf(buf: name, fmt: "txrx-%d", i);
2536	retval = platform_get_irq_byname_optional(dev: adapter->pdev, name);
2537	if (retval < 0)
2538	break;
2539
2540	adapter->num_tx_queues++;
2541	adapter->num_rx_queues++;
2542	adapter->num_queues++;
2543	adapter->queue[i].adapter = adapter;
2544	adapter->queue[i].irq = retval;
2545	adapter->queue[i].tx = &adapter->tx[i];
2546	adapter->queue[i].tx->adapter = adapter;
2547	adapter->queue[i].tx->addr = adapter->addr + TSNEP_QUEUE(i);
2548	adapter->queue[i].tx->queue_index = i;
2549	adapter->queue[i].rx = &adapter->rx[i];
2550	adapter->queue[i].rx->adapter = adapter;
2551	adapter->queue[i].rx->addr = adapter->addr + TSNEP_QUEUE(i);
2552	adapter->queue[i].rx->queue_index = i;
2553	adapter->queue[i].irq_mask =
2554	irq_mask << (ECM_INT_TXRX_SHIFT * i);
2555	adapter->queue[i].irq_delay_addr =
2556	adapter->addr + ECM_INT_DELAY + ECM_INT_DELAY_OFFSET * i;
2557	retval = tsnep_set_irq_coalesce(queue: &adapter->queue[i],
2558	TSNEP_COALESCE_USECS_DEFAULT);
2559	if (retval < 0)
2560	return retval;
2561	}
2562
2563	return 0;
2564	}
2565
2566	static int tsnep_probe(struct platform_device *pdev)
2567	{
2568	struct tsnep_adapter *adapter;
2569	struct net_device *netdev;
2570	struct resource *io;
2571	u32 type;
2572	int revision;
2573	int version;
2574	int queue_count;
2575	int retval;
2576
2577	netdev = devm_alloc_etherdev_mqs(dev: &pdev->dev,
2578	sizeof_priv: sizeof(struct tsnep_adapter),
2579	TSNEP_MAX_QUEUES, TSNEP_MAX_QUEUES);
2580	if (!netdev)
2581	return -ENODEV;
2582	SET_NETDEV_DEV(netdev, &pdev->dev);
2583	adapter = netdev_priv(dev: netdev);
2584	platform_set_drvdata(pdev, data: adapter);
2585	adapter->pdev = pdev;
2586	adapter->dmadev = &pdev->dev;
2587	adapter->netdev = netdev;
2588	adapter->msg_enable = NETIF_MSG_DRV | NETIF_MSG_PROBE |
2589	NETIF_MSG_LINK | NETIF_MSG_IFUP |
2590	NETIF_MSG_IFDOWN | NETIF_MSG_TX_QUEUED;
2591
2592	netdev->min_mtu = ETH_MIN_MTU;
2593	netdev->max_mtu = TSNEP_MAX_FRAME_SIZE;
2594
2595	mutex_init(&adapter->gate_control_lock);
2596	mutex_init(&adapter->rxnfc_lock);
2597	INIT_LIST_HEAD(list: &adapter->rxnfc_rules);
2598
2599	adapter->addr = devm_platform_get_and_ioremap_resource(pdev, index: 0, res: &io);
2600	if (IS_ERR(ptr: adapter->addr))
2601	return PTR_ERR(ptr: adapter->addr);
2602	netdev->mem_start = io->start;
2603	netdev->mem_end = io->end;
2604
2605	type = ioread32(adapter->addr + ECM_TYPE);
2606	revision = (type & ECM_REVISION_MASK) >> ECM_REVISION_SHIFT;
2607	version = (type & ECM_VERSION_MASK) >> ECM_VERSION_SHIFT;
2608	queue_count = (type & ECM_QUEUE_COUNT_MASK) >> ECM_QUEUE_COUNT_SHIFT;
2609	adapter->gate_control = type & ECM_GATE_CONTROL;
2610	adapter->rxnfc_max = TSNEP_RX_ASSIGN_ETHER_TYPE_COUNT;
2611
2612	tsnep_disable_irq(adapter, ECM_INT_ALL);
2613
2614	retval = tsnep_queue_init(adapter, queue_count);
2615	if (retval)
2616	return retval;
2617
2618	retval = dma_set_mask_and_coherent(dev: &adapter->pdev->dev,
2619	DMA_BIT_MASK(64));
2620	if (retval) {
2621	dev_err(&adapter->pdev->dev, "no usable DMA configuration.\n");
2622	return retval;
2623	}
2624
2625	retval = tsnep_mac_init(adapter);
2626	if (retval)
2627	return retval;
2628
2629	retval = tsnep_mdio_init(adapter);
2630	if (retval)
2631	goto mdio_init_failed;
2632
2633	retval = tsnep_phy_init(adapter);
2634	if (retval)
2635	goto phy_init_failed;
2636
2637	retval = tsnep_ptp_init(adapter);
2638	if (retval)
2639	goto ptp_init_failed;
2640
2641	retval = tsnep_tc_init(adapter);
2642	if (retval)
2643	goto tc_init_failed;
2644
2645	retval = tsnep_rxnfc_init(adapter);
2646	if (retval)
2647	goto rxnfc_init_failed;
2648
2649	netdev->netdev_ops = &tsnep_netdev_ops;
2650	netdev->ethtool_ops = &tsnep_ethtool_ops;
2651	netdev->features = NETIF_F_SG;
2652	netdev->hw_features = netdev->features | NETIF_F_LOOPBACK;
2653
2654	netdev->xdp_features = NETDEV_XDP_ACT_BASIC | NETDEV_XDP_ACT_REDIRECT |
2655	NETDEV_XDP_ACT_NDO_XMIT |
2656	NETDEV_XDP_ACT_NDO_XMIT_SG |
2657	NETDEV_XDP_ACT_XSK_ZEROCOPY;
2658
2659	/* carrier off reporting is important to ethtool even BEFORE open */
2660	netif_carrier_off(dev: netdev);
2661
2662	retval = register_netdev(dev: netdev);
2663	if (retval)
2664	goto register_failed;
2665
2666	dev_info(&adapter->pdev->dev, "device version %d.%02d\n", version,
2667	revision);
2668	if (adapter->gate_control)
2669	dev_info(&adapter->pdev->dev, "gate control detected\n");
2670
2671	return 0;
2672
2673	register_failed:
2674	tsnep_rxnfc_cleanup(adapter);
2675	rxnfc_init_failed:
2676	tsnep_tc_cleanup(adapter);
2677	tc_init_failed:
2678	tsnep_ptp_cleanup(adapter);
2679	ptp_init_failed:
2680	phy_init_failed:
2681	if (adapter->mdiobus)
2682	mdiobus_unregister(bus: adapter->mdiobus);
2683	mdio_init_failed:
2684	return retval;
2685	}
2686
2687	static void tsnep_remove(struct platform_device *pdev)
2688	{
2689	struct tsnep_adapter *adapter = platform_get_drvdata(pdev);
2690
2691	unregister_netdev(dev: adapter->netdev);
2692
2693	tsnep_rxnfc_cleanup(adapter);
2694
2695	tsnep_tc_cleanup(adapter);
2696
2697	tsnep_ptp_cleanup(adapter);
2698
2699	if (adapter->mdiobus)
2700	mdiobus_unregister(bus: adapter->mdiobus);
2701
2702	tsnep_disable_irq(adapter, ECM_INT_ALL);
2703	}
2704
2705	static const struct of_device_id tsnep_of_match[] = {
2706	{ .compatible = "engleder,tsnep", },
2707	{ },
2708	};
2709	MODULE_DEVICE_TABLE(of, tsnep_of_match);
2710
2711	static struct platform_driver tsnep_driver = {
2712	.driver = {
2713	.name = TSNEP,
2714	.of_match_table = tsnep_of_match,
2715	},
2716	.probe = tsnep_probe,
2717	.remove = tsnep_remove,
2718	};
2719	module_platform_driver(tsnep_driver);
2720
2721	MODULE_AUTHOR("Gerhard Engleder <gerhard@engleder-embedded.com>");
2722	MODULE_DESCRIPTION("TSN endpoint Ethernet MAC driver");
2723	MODULE_LICENSE("GPL");
2724