--- v6
+++ v3
@@ -8,16 +8,16 @@
---
drivers/net/ethernet/Kconfig | 1 +
drivers/net/ethernet/Makefile | 1 +
- drivers/net/ethernet/engleder/Kconfig | 38 +
- drivers/net/ethernet/engleder/Makefile | 10 +
- drivers/net/ethernet/engleder/tsnep.h | 190 +++
- drivers/net/ethernet/engleder/tsnep_ethtool.c | 293 ++++
+ drivers/net/ethernet/engleder/Kconfig | 29 +
+ drivers/net/ethernet/engleder/Makefile | 9 +
+ drivers/net/ethernet/engleder/tsnep.h | 171 +++
+ drivers/net/ethernet/engleder/tsnep_ethtool.c | 288 ++++
drivers/net/ethernet/engleder/tsnep_hw.h | 230 +++
- drivers/net/ethernet/engleder/tsnep_main.c | 1273 +++++++++++++++++
+ drivers/net/ethernet/engleder/tsnep_main.c | 1252 +++++++++++++++++
drivers/net/ethernet/engleder/tsnep_ptp.c | 221 +++
- .../net/ethernet/engleder/tsnep_selftests.c | 811 +++++++++++
- drivers/net/ethernet/engleder/tsnep_tc.c | 443 ++++++
- 11 files changed, 3511 insertions(+)
+ drivers/net/ethernet/engleder/tsnep_tc.c | 442 ++++++
+ drivers/net/ethernet/engleder/tsnep_test.c | 811 +++++++++++
+ 11 files changed, 3455 insertions(+)
create mode 100644 drivers/net/ethernet/engleder/Kconfig
create mode 100644 drivers/net/ethernet/engleder/Makefile
create mode 100644 drivers/net/ethernet/engleder/tsnep.h
@@ -25,8 +25,8 @@
create mode 100644 drivers/net/ethernet/engleder/tsnep_hw.h
create mode 100644 drivers/net/ethernet/engleder/tsnep_main.c
create mode 100644 drivers/net/ethernet/engleder/tsnep_ptp.c
- create mode 100644 drivers/net/ethernet/engleder/tsnep_selftests.c
create mode 100644 drivers/net/ethernet/engleder/tsnep_tc.c
+ create mode 100644 drivers/net/ethernet/engleder/tsnep_test.c
diff --git a/drivers/net/ethernet/Kconfig b/drivers/net/ethernet/Kconfig
index 4601b38f532a..027cbacca1c9 100644
@@ -54,10 +54,10 @@
obj-$(CONFIG_NET_VENDOR_FREESCALE) += freescale/
diff --git a/drivers/net/ethernet/engleder/Kconfig b/drivers/net/ethernet/engleder/Kconfig
new file mode 100644
-index 000000000000..614dcc65c634
+index 000000000000..26c2a8e0acc0
--- /dev/null
+++ b/drivers/net/ethernet/engleder/Kconfig
-@@ -0,0 +1,38 @@
+@@ -0,0 +1,29 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Engleder network device configuration
@@ -86,22 +86,13 @@
+ To compile this driver as a module, choose M here. The module will be
+ called tsnep.
+
-+config TSNEP_SELFTESTS
-+ bool "TSN endpoint self test support"
-+ default n
-+ depends on TSNEP
-+ help
-+ This enables self test support within the TSN endpoint driver.
-+
-+ If unsure, say N.
-+
+endif # NET_VENDOR_ENGLEDER
diff --git a/drivers/net/ethernet/engleder/Makefile b/drivers/net/ethernet/engleder/Makefile
new file mode 100644
-index 000000000000..cce2191cb889
+index 000000000000..fbaecbfb0944
--- /dev/null
+++ b/drivers/net/ethernet/engleder/Makefile
-@@ -0,0 +1,10 @@
+@@ -0,0 +1,9 @@
+# SPDX-License-Identifier: GPL-2.0
+#
+# Makefile for the Engleder Ethernet drivers
@@ -110,14 +101,13 @@
+obj-$(CONFIG_TSNEP) += tsnep.o
+
+tsnep-objs := tsnep_main.o tsnep_ethtool.o tsnep_ptp.o tsnep_tc.o \
-+ $(tsnep-y)
-+tsnep-$(CONFIG_TSNEP_SELFTESTS) += tsnep_selftests.o
++ tsnep_test.o
diff --git a/drivers/net/ethernet/engleder/tsnep.h b/drivers/net/ethernet/engleder/tsnep.h
new file mode 100644
-index 000000000000..d19fa175e3d9
+index 000000000000..aa7d51d80ff1
--- /dev/null
+++ b/drivers/net/ethernet/engleder/tsnep.h
-@@ -0,0 +1,190 @@
+@@ -0,0 +1,171 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
+
@@ -144,7 +134,7 @@
+#define TSNEP_QUEUES 1
+
+struct tsnep_gcl {
-+ void __iomem *addr;
++ void *addr;
+
+ u64 base_time;
+ u64 cycle_time;
@@ -168,13 +158,13 @@
+ u32 properties;
+
+ struct sk_buff *skb;
-+ size_t len;
+ DEFINE_DMA_UNMAP_ADDR(dma);
++ DEFINE_DMA_UNMAP_LEN(len);
+};
+
+struct tsnep_tx {
+ struct tsnep_adapter *adapter;
-+ void __iomem *addr;
++ void *addr;
+
+ void *page[TSNEP_RING_PAGE_COUNT];
+ dma_addr_t page_dma[TSNEP_RING_PAGE_COUNT];
@@ -200,13 +190,13 @@
+ u32 properties;
+
+ struct sk_buff *skb;
-+ size_t len;
+ DEFINE_DMA_UNMAP_ADDR(dma);
++ DEFINE_DMA_UNMAP_LEN(len);
+};
+
+struct tsnep_rx {
+ struct tsnep_adapter *adapter;
-+ void __iomem *addr;
++ void *addr;
+
+ void *page[TSNEP_RING_PAGE_COUNT];
+ dma_addr_t page_dma[TSNEP_RING_PAGE_COUNT];
@@ -244,7 +234,7 @@
+
+ struct platform_device *pdev;
+ struct device *dmadev;
-+ void __iomem *addr;
++ void *addr;
+ unsigned long size;
+ int irq;
+
@@ -281,39 +271,20 @@
+int tsnep_tc_setup(struct net_device *netdev, enum tc_setup_type type,
+ void *type_data);
+
-+#if IS_ENABLED(CONFIG_TSNEP_SELFTESTS)
+int tsnep_ethtool_get_test_count(void);
+void tsnep_ethtool_get_test_strings(u8 *data);
+void tsnep_ethtool_self_test(struct net_device *netdev,
+ struct ethtool_test *eth_test, u64 *data);
-+#else
-+static inline int tsnep_ethtool_get_test_count(void)
-+{
-+ return -EOPNOTSUPP;
-+}
-+
-+static inline void tsnep_ethtool_get_test_strings(u8 *data)
-+{
-+ /* not enabled */
-+}
-+
-+static inline void tsnep_ethtool_self_test(struct net_device *dev,
-+ struct ethtool_test *eth_test,
-+ u64 *data)
-+{
-+ /* not enabled */
-+}
-+#endif /* CONFIG_TSNEP_SELFTESTS */
+
+void tsnep_get_system_time(struct tsnep_adapter *adapter, u64 *time);
+
+#endif /* _TSNEP_H */
diff --git a/drivers/net/ethernet/engleder/tsnep_ethtool.c b/drivers/net/ethernet/engleder/tsnep_ethtool.c
new file mode 100644
-index 000000000000..e6760dc68ddd
+index 000000000000..f9abcaab1c7c
--- /dev/null
+++ b/drivers/net/ethernet/engleder/tsnep_ethtool.c
-@@ -0,0 +1,293 @@
+@@ -0,0 +1,288 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
+
@@ -401,16 +372,11 @@
+{
+ struct tsnep_adapter *adapter = netdev_priv(netdev);
+ int len;
-+ int num_additional_queues;
++ int num_queues;
+
+ len = TSNEP_MAC_SIZE;
-+
-+ /* first queue pair is within TSNEP_MAC_SIZE, only queues additional to
-+ * the first queue pair extend the register length by TSNEP_QUEUE_SIZE
-+ */
-+ num_additional_queues =
-+ max(adapter->num_tx_queues, adapter->num_rx_queues) - 1;
-+ len += TSNEP_QUEUE_SIZE * num_additional_queues;
++ num_queues = max(adapter->num_tx_queues, adapter->num_rx_queues);
++ len += TSNEP_QUEUE_SIZE * (num_queues - 1);
+
+ return len;
+}
@@ -609,7 +575,7 @@
+};
diff --git a/drivers/net/ethernet/engleder/tsnep_hw.h b/drivers/net/ethernet/engleder/tsnep_hw.h
new file mode 100644
-index 000000000000..71cc8577d640
+index 000000000000..1a9327a23510
--- /dev/null
+++ b/drivers/net/ethernet/engleder/tsnep_hw.h
@@ -0,0 +1,230 @@
@@ -790,11 +756,11 @@
+
+/* tsnep TX descriptor */
+struct tsnep_tx_desc {
-+ __le32 properties;
-+ __le32 more_properties;
-+ __le32 reserved[2];
-+ __le64 next;
-+ __le64 tx;
++ u32 properties;
++ u32 more_properties;
++ u32 reserved[2];
++ u64 next;
++ u64 tx;
+};
+
+#define TSNEP_TX_DESC_OWNER_MASK 0xE0000000
@@ -804,11 +770,11 @@
+
+/* tsnep TX descriptor writeback */
+struct tsnep_tx_desc_wb {
-+ __le32 properties;
-+ __le32 reserved1[3];
-+ __le64 timestamp;
-+ __le32 dma_delay;
-+ __le32 reserved2;
++ u32 properties;
++ u32 reserved1[3];
++ u64 timestamp;
++ u32 dma_delay;
++ u32 reserved2;
+};
+
+#define TSNEP_TX_DESC_UNDERRUN_ERROR_FLAG 0x00010000
@@ -820,24 +786,24 @@
+
+/* tsnep RX descriptor */
+struct tsnep_rx_desc {
-+ __le32 properties;
-+ __le32 reserved[3];
-+ __le64 next;
-+ __le64 rx;
++ u32 properties;
++ u32 reserved[3];
++ u64 next;
++ u64 rx;
+};
+
+#define TSNEP_RX_DESC_BUFFER_SIZE_MASK 0x00003FFC
+
+/* tsnep RX descriptor writeback */
+struct tsnep_rx_desc_wb {
-+ __le32 properties;
-+ __le32 reserved[7];
++ u32 properties;
++ u32 reserved[7];
+};
+
+/* tsnep RX inline meta */
+struct tsnep_rx_inline {
-+ __le64 reserved;
-+ __le64 timestamp;
++ u64 reserved;
++ u64 timestamp;
+};
+
+#define TSNEP_RX_INLINE_METADATA_SIZE (sizeof(struct tsnep_rx_inline))
@@ -845,10 +811,10 @@
+#endif /* _TSNEP_HW_H */
diff --git a/drivers/net/ethernet/engleder/tsnep_main.c b/drivers/net/ethernet/engleder/tsnep_main.c
new file mode 100644
-index 000000000000..c965744e9530
+index 000000000000..27e5e1055027
--- /dev/null
+++ b/drivers/net/ethernet/engleder/tsnep_main.c
-@@ -0,0 +1,1273 @@
+@@ -0,0 +1,1252 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
+
@@ -1082,7 +1048,7 @@
+ for (i = 0; i < TSNEP_RING_SIZE; i++) {
+ entry = &tx->entry[i];
+ next_entry = &tx->entry[(i + 1) % TSNEP_RING_SIZE];
-+ entry->desc->next = __cpu_to_le64(next_entry->desc_dma);
++ entry->desc->next = next_entry->desc_dma;
+ }
+
+ return 0;
@@ -1139,15 +1105,11 @@
+ TSNEP_DESC_OWNER_COUNTER_MASK;
+ if (entry->owner_user_flag)
+ entry->properties |= TSNEP_TX_DESC_OWNER_USER_FLAG;
-+ entry->desc->more_properties =
-+ __cpu_to_le32(entry->len & TSNEP_DESC_LENGTH_MASK);
-+
-+ /* descriptor properties shall be written last, because valid data is
-+ * signaled there
-+ */
++ entry->desc->more_properties = entry->len & TSNEP_DESC_LENGTH_MASK;
++
+ dma_wmb();
+
-+ entry->desc->properties = __cpu_to_le32(entry->properties);
++ entry->desc->properties = entry->properties;
+}
+
+static int tsnep_tx_desc_available(struct tsnep_tx *tx)
@@ -1182,10 +1144,10 @@
+ if (dma_mapping_error(dmadev, dma))
+ return -ENOMEM;
+
-+ entry->len = len;
++ dma_unmap_len_set(entry, len, len);
+ dma_unmap_addr_set(entry, dma, dma);
+
-+ entry->desc->tx = __cpu_to_le64(dma);
++ entry->desc->tx = dma;
+ }
+
+ return 0;
@@ -1200,7 +1162,7 @@
+ for (i = 0; i < count; i++) {
+ entry = &tx->entry[(tx->read + i) % TSNEP_RING_SIZE];
+
-+ if (entry->len) {
++ if (dma_unmap_len(entry, len)) {
+ if (i == 0)
+ dma_unmap_single(dmadev,
+ dma_unmap_addr(entry, dma),
@@ -1211,7 +1173,7 @@
+ dma_unmap_addr(entry, dma),
+ dma_unmap_len(entry, len),
+ DMA_TO_DEVICE);
-+ entry->len = 0;
++ dma_unmap_len_set(entry, len, 0);
+ }
+ }
+}
@@ -1265,12 +1227,13 @@
+ for (i = 0; i < count; i++)
+ tsnep_tx_activate(tx, (tx->write + i) % TSNEP_RING_SIZE,
+ i == (count - 1));
++ skb_tx_timestamp(skb);
++
++ /* entry->properties shall be valid before write pointer is
++ * incrememted
++ */
++ wmb();
+ tx->write = (tx->write + count) % TSNEP_RING_SIZE;
-+
-+ skb_tx_timestamp(skb);
-+
-+ /* descriptor properties shall be valid before hardware is notified */
-+ dma_wmb();
+
+ iowrite32(TSNEP_CONTROL_TX_ENABLE, tx->addr + TSNEP_CONTROL);
+
@@ -1301,14 +1264,11 @@
+ break;
+
+ entry = &tx->entry[tx->read];
-+ if ((__le32_to_cpu(entry->desc_wb->properties) &
++ if ((entry->desc_wb->properties &
+ TSNEP_TX_DESC_OWNER_MASK) !=
+ (entry->properties & TSNEP_TX_DESC_OWNER_MASK))
+ break;
+
-+ /* descriptor properties shall be read first, because valid data
-+ * is signaled there
-+ */
+ dma_rmb();
+
+ count = 1;
@@ -1318,20 +1278,23 @@
+ tsnep_tx_unmap(tx, count);
+
+ if ((skb_shinfo(entry->skb)->tx_flags & SKBTX_IN_PROGRESS) &&
-+ (__le32_to_cpu(entry->desc_wb->properties) &
++ (entry->desc_wb->properties &
+ TSNEP_DESC_EXTENDED_WRITEBACK_FLAG)) {
+ struct skb_shared_hwtstamps hwtstamps;
-+ u64 timestamp =
-+ __le64_to_cpu(entry->desc_wb->timestamp);
+
+ memset(&hwtstamps, 0, sizeof(hwtstamps));
-+ hwtstamps.hwtstamp = ns_to_ktime(timestamp);
++ hwtstamps.hwtstamp =
++ ns_to_ktime(entry->desc_wb->timestamp);
+
+ skb_tstamp_tx(entry->skb, &hwtstamps);
+ }
+
+ napi_consume_skb(entry->skb, budget);
+ entry->skb = NULL;
++
++ /* descriptor shall be free before read pointer is incremented
++ */
++ wmb();
+
+ tx->read = (tx->read + count) % TSNEP_RING_SIZE;
+
@@ -1348,7 +1311,7 @@
+ return (budget != 0);
+}
+
-+static int tsnep_tx_open(struct tsnep_adapter *adapter, void __iomem *addr,
++static int tsnep_tx_open(struct tsnep_adapter *adapter, void *addr,
+ struct tsnep_tx *tx)
+{
+ dma_addr_t dma;
@@ -1392,9 +1355,8 @@
+
+ for (i = 0; i < TSNEP_RING_SIZE; i++) {
+ entry = &rx->entry[i];
-+ if (dma_unmap_addr(entry, dma))
-+ dma_unmap_single(dmadev, dma_unmap_addr(entry, dma),
-+ dma_unmap_len(entry, len),
++ if (entry->dma)
++ dma_unmap_single(dmadev, entry->dma, entry->len,
+ DMA_FROM_DEVICE);
+ if (entry->skb)
+ dev_kfree_skb(entry->skb);
@@ -1434,9 +1396,9 @@
+ }
+
+ entry->skb = skb;
-+ entry->len = RX_SKB_LENGTH;
++ dma_unmap_len_set(entry, len, RX_SKB_LENGTH);
+ dma_unmap_addr_set(entry, dma, dma);
-+ entry->desc->rx = __cpu_to_le64(dma);
++ entry->desc->rx = dma;
+
+ return 0;
+}
@@ -1469,7 +1431,7 @@
+ for (i = 0; i < TSNEP_RING_SIZE; i++) {
+ entry = &rx->entry[i];
+ next_entry = &rx->entry[(i + 1) % TSNEP_RING_SIZE];
-+ entry->desc->next = __cpu_to_le64(next_entry->desc_dma);
++ entry->desc->next = next_entry->desc_dma;
+
+ retval = tsnep_rx_alloc_and_map_skb(rx, entry);
+ if (retval)
@@ -1502,12 +1464,9 @@
+ (rx->owner_counter << TSNEP_DESC_OWNER_COUNTER_SHIFT) &
+ TSNEP_DESC_OWNER_COUNTER_MASK;
+
-+ /* descriptor properties shall be written last, because valid data is
-+ * signaled there
-+ */
+ dma_wmb();
+
-+ entry->desc->properties = __cpu_to_le32(entry->properties);
++ entry->desc->properties = entry->properties;
+}
+
+static int tsnep_rx_poll(struct tsnep_rx *rx, struct napi_struct *napi,
@@ -1517,27 +1476,23 @@
+ int done = 0;
+ struct tsnep_rx_entry *entry;
+ struct sk_buff *skb;
-+ size_t len;
-+ dma_addr_t dma;
++ DEFINE_DMA_UNMAP_ADDR(dma);
++ DEFINE_DMA_UNMAP_LEN(len);
+ int length;
-+ bool enable = false;
+ int retval;
+
+ while (likely(done < budget)) {
+ entry = &rx->entry[rx->read];
-+ if ((__le32_to_cpu(entry->desc_wb->properties) &
++ if ((entry->desc_wb->properties &
+ TSNEP_DESC_OWNER_COUNTER_MASK) !=
+ (entry->properties & TSNEP_DESC_OWNER_COUNTER_MASK))
+ break;
+
-+ /* descriptor properties shall be read first, because valid data
-+ * is signaled there
-+ */
+ dma_rmb();
+
+ skb = entry->skb;
-+ len = dma_unmap_len(entry, len);
-+ dma = dma_unmap_addr(entry, dma);
++ dma = entry->dma;
++ len = entry->len;
+
+ /* forward skb only if allocation is successful, otherwise
+ * skb is reused and frame dropped
@@ -1546,7 +1501,7 @@
+ if (!retval) {
+ dma_unmap_single(dmadev, dma, len, DMA_FROM_DEVICE);
+
-+ length = __le32_to_cpu(entry->desc_wb->properties) &
++ length = entry->desc_wb->properties &
+ TSNEP_DESC_LENGTH_MASK;
+ skb_put(skb, length - ETH_FCS_LEN);
+ if (rx->adapter->hwtstamp_config.rx_filter ==
@@ -1555,11 +1510,10 @@
+ skb_hwtstamps(skb);
+ struct tsnep_rx_inline *rx_inline =
+ (struct tsnep_rx_inline *)skb->data;
-+ u64 timestamp =
-+ __le64_to_cpu(rx_inline->timestamp);
+
+ memset(hwtstamps, 0, sizeof(*hwtstamps));
-+ hwtstamps->hwtstamp = ns_to_ktime(timestamp);
++ hwtstamps->hwtstamp =
++ ns_to_ktime(rx_inline->timestamp);
+ }
+ skb_pull(skb, TSNEP_RX_INLINE_METADATA_SIZE);
+ skb->protocol = eth_type_trans(skb,
@@ -1577,25 +1531,15 @@
+ }
+
+ tsnep_rx_activate(rx, rx->read);
-+
-+ enable = true;
++ iowrite32(TSNEP_CONTROL_RX_ENABLE, rx->addr + TSNEP_CONTROL);
+
+ rx->read = (rx->read + 1) % TSNEP_RING_SIZE;
+ }
+
-+ if (enable) {
-+ /* descriptor properties shall be valid before hardware is
-+ * notified
-+ */
-+ dma_wmb();
-+
-+ iowrite32(TSNEP_CONTROL_RX_ENABLE, rx->addr + TSNEP_CONTROL);
-+ }
-+
+ return done;
+}
+
-+static int tsnep_rx_open(struct tsnep_adapter *adapter, void __iomem *addr,
++static int tsnep_rx_open(struct tsnep_adapter *adapter, void *addr,
+ struct tsnep_rx *rx)
+{
+ dma_addr_t dma;
@@ -1619,9 +1563,6 @@
+ for (i = 0; i < TSNEP_RING_SIZE; i++)
+ tsnep_rx_activate(rx, i);
+
-+ /* descriptor properties shall be valid before hardware is notified */
-+ dma_wmb();
-+
+ iowrite32(TSNEP_CONTROL_RX_ENABLE, rx->addr + TSNEP_CONTROL);
+
+ return 0;
@@ -1669,7 +1610,7 @@
+{
+ struct tsnep_adapter *adapter = netdev_priv(netdev);
+ int i;
-+ void __iomem *addr;
++ void *addr;
+ int tx_queue_index = 0;
+ int rx_queue_index = 0;
+ int retval;
@@ -1936,12 +1877,16 @@
+ snprintf(adapter->mdiobus->id, MII_BUS_ID_SIZE, "%s",
+ adapter->pdev->name);
+
-+ /* do not scan broadcast address */
-+ adapter->mdiobus->phy_mask = 0x0000001;
-+
-+ retval = of_mdiobus_register(adapter->mdiobus, np);
-+ if (np)
++ if (np) {
++ retval = of_mdiobus_register(adapter->mdiobus, np);
++
+ of_node_put(np);
++ } else {
++ /* do not scan broadcast address */
++ adapter->mdiobus->phy_mask = 0x0000001;
++
++ retval = mdiobus_register(adapter->mdiobus);
++ }
+out:
+
+ return retval;
@@ -2349,11 +2294,459 @@
+ netdev_info(adapter->netdev, "PHC removed\n");
+ }
+}
-diff --git a/drivers/net/ethernet/engleder/tsnep_selftests.c b/drivers/net/ethernet/engleder/tsnep_selftests.c
+diff --git a/drivers/net/ethernet/engleder/tsnep_tc.c b/drivers/net/ethernet/engleder/tsnep_tc.c
+new file mode 100644
+index 000000000000..2b88c1e2eb2c
+--- /dev/null
++++ b/drivers/net/ethernet/engleder/tsnep_tc.c
+@@ -0,0 +1,442 @@
++// SPDX-License-Identifier: GPL-2.0
++/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
++
++#include "tsnep.h"
++
++#include <net/pkt_sched.h>
++
++/* save one operation at the end for additional operation at list change */
++#define TSNEP_MAX_GCL_NUM (TSNEP_GCL_COUNT - 1)
++
++static int tsnep_validate_gcl(struct tc_taprio_qopt_offload *qopt)
++{
++ int i;
++ u64 cycle_time;
++
++ if (!qopt->cycle_time)
++ return -ERANGE;
++ if (qopt->num_entries > TSNEP_MAX_GCL_NUM)
++ return -EINVAL;
++ cycle_time = 0;
++ for (i = 0; i < qopt->num_entries; i++) {
++ if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
++ return -EINVAL;
++ if (qopt->entries[i].gate_mask & ~TSNEP_GCL_MASK)
++ return -EINVAL;
++ if (qopt->entries[i].interval < TSNEP_GCL_MIN_INTERVAL)
++ return -EINVAL;
++ cycle_time += qopt->entries[i].interval;
++ }
++ if (qopt->cycle_time != cycle_time)
++ return -EINVAL;
++ if (qopt->cycle_time_extension >= qopt->cycle_time)
++ return -EINVAL;
++
++ return 0;
++}
++
++static void tsnep_write_gcl_operation(struct tsnep_gcl *gcl, int index,
++ u32 properties, u32 interval, bool flush)
++{
++ void *addr = gcl->addr + sizeof(struct tsnep_gcl_operation) * index;
++
++ gcl->operation[index].properties = properties;
++ gcl->operation[index].interval = interval;
++
++ iowrite32(properties, addr);
++ iowrite32(interval, addr + sizeof(u32));
++
++ if (flush) {
++ /* flush write with read access */
++ ioread32(addr);
++ }
++}
++
++static u64 tsnep_change_duration(struct tsnep_gcl *gcl, int index)
++{
++ u64 duration;
++ int count;
++
++ /* change needs to be triggered one or two operations before start of
++ * new gate control list
++ * - change is triggered at start of operation (minimum one operation)
++ * - operation with adjusted interval is inserted on demand to exactly
++ * meet the start of the new gate control list (optional)
++ *
++ * additionally properties are read directly after start of previous
++ * operation
++ *
++ * therefore, three operations needs to be considered for the limit
++ */
++ duration = 0;
++ count = 3;
++ while (count) {
++ duration += gcl->operation[index].interval;
++
++ index--;
++ if (index < 0)
++ index = gcl->count - 1;
++
++ count--;
++ }
++
++ return duration;
++}
++
++static void tsnep_write_gcl(struct tsnep_gcl *gcl,
++ struct tc_taprio_qopt_offload *qopt)
++{
++ int i;
++ u32 properties;
++ u64 extend;
++ u64 cut;
++
++ gcl->base_time = ktime_to_ns(qopt->base_time);
++ gcl->cycle_time = qopt->cycle_time;
++ gcl->cycle_time_extension = qopt->cycle_time_extension;
++
++ for (i = 0; i < qopt->num_entries; i++) {
++ properties = qopt->entries[i].gate_mask;
++ if (i == (qopt->num_entries - 1))
++ properties |= TSNEP_GCL_LAST;
++
++ tsnep_write_gcl_operation(gcl, i, properties,
++ qopt->entries[i].interval, true);
++ }
++ gcl->count = qopt->num_entries;
++
++ /* calculate change limit; i.e., the time needed between enable and
++ * start of new gate control list
++ */
++
++ /* case 1: extend cycle time for change
++ * - change duration of last operation
++ * - cycle time extension
++ */
++ extend = tsnep_change_duration(gcl, gcl->count - 1);
++ extend += gcl->cycle_time_extension;
++
++ /* case 2: cut cycle time for change
++ * - maximum change duration
++ */
++ cut = 0;
++ for (i = 0; i < gcl->count; i++)
++ cut = max(cut, tsnep_change_duration(gcl, i));
++
++ /* use maximum, because the actual case (extend or cut) can be
++ * determined only after limit is known (chicken-and-egg problem)
++ */
++ gcl->change_limit = max(extend, cut);
++}
++
++static u64 tsnep_gcl_start_after(struct tsnep_gcl *gcl, u64 limit)
++{
++ u64 start = gcl->base_time;
++ u64 n;
++
++ if (start <= limit) {
++ n = div64_u64(limit - start, gcl->cycle_time);
++ start += (n + 1) * gcl->cycle_time;
++ }
++
++ return start;
++}
++
++static u64 tsnep_gcl_start_before(struct tsnep_gcl *gcl, u64 limit)
++{
++ u64 start = gcl->base_time;
++ u64 n;
++
++ n = div64_u64(limit - start, gcl->cycle_time);
++ start += n * gcl->cycle_time;
++ if (start == limit)
++ start -= gcl->cycle_time;
++
++ return start;
++}
++
++static u64 tsnep_set_gcl_change(struct tsnep_gcl *gcl, int index, u64 change,
++ bool insert)
++{
++ /* previous operation triggers change and properties are evaluated at
++ * start of operation
++ */
++ if (index == 0)
++ index = gcl->count - 1;
++ else
++ index = index - 1;
++ change -= gcl->operation[index].interval;
++
++ /* optionally change to new list with additional operation in between */
++ if (insert) {
++ void *addr = gcl->addr +
++ sizeof(struct tsnep_gcl_operation) * index;
++
++ gcl->operation[index].properties |= TSNEP_GCL_INSERT;
++ iowrite32(gcl->operation[index].properties, addr);
++ }
++
++ return change;
++}
++
++static void tsnep_clean_gcl(struct tsnep_gcl *gcl)
++{
++ int i;
++ u32 mask = TSNEP_GCL_LAST | TSNEP_GCL_MASK;
++ void *addr;
++
++ /* search for insert operation and reset properties */
++ for (i = 0; i < gcl->count; i++) {
++ if (gcl->operation[i].properties & ~mask) {
++ addr = gcl->addr +
++ sizeof(struct tsnep_gcl_operation) * i;
++
++ gcl->operation[i].properties &= mask;
++ iowrite32(gcl->operation[i].properties, addr);
++
++ break;
++ }
++ }
++}
++
++static u64 tsnep_insert_gcl_operation(struct tsnep_gcl *gcl, int ref,
++ u64 change, u32 interval)
++{
++ u32 properties;
++
++ properties = gcl->operation[ref].properties & TSNEP_GCL_MASK;
++ /* change to new list directly after inserted operation */
++ properties |= TSNEP_GCL_CHANGE;
++
++ /* last operation of list is reserved to insert operation */
++ tsnep_write_gcl_operation(gcl, TSNEP_GCL_COUNT - 1, properties,
++ interval, false);
++
++ return tsnep_set_gcl_change(gcl, ref, change, true);
++}
++
++static u64 tsnep_extend_gcl(struct tsnep_gcl *gcl, u64 start, u32 extension)
++{
++ int ref = gcl->count - 1;
++ u32 interval = gcl->operation[ref].interval + extension;
++
++ start -= gcl->operation[ref].interval;
++
++ return tsnep_insert_gcl_operation(gcl, ref, start, interval);
++}
++
++static u64 tsnep_cut_gcl(struct tsnep_gcl *gcl, u64 start, u64 cycle_time)
++{
++ u64 sum = 0;
++ int i;
++
++ /* find operation which shall be cutted */
++ for (i = 0; i < gcl->count; i++) {
++ u64 sum_tmp = sum + gcl->operation[i].interval;
++ u64 interval;
++
++ /* sum up operations as long as cycle time is not exceeded */
++ if (sum_tmp > cycle_time)
++ break;
++
++ /* remaining interval must be big enough for hardware */
++ interval = cycle_time - sum_tmp;
++ if (interval > 0 && interval < TSNEP_GCL_MIN_INTERVAL)
++ break;
++
++ sum = sum_tmp;
++ }
++ if (sum == cycle_time) {
++ /* no need to cut operation itself or whole cycle
++ * => change exactly at operation
++ */
++ return tsnep_set_gcl_change(gcl, i, start + sum, false);
++ }
++ return tsnep_insert_gcl_operation(gcl, i, start + sum,
++ cycle_time - sum);
++}
++
++static int tsnep_enable_gcl(struct tsnep_adapter *adapter,
++ struct tsnep_gcl *gcl, struct tsnep_gcl *curr)
++{
++ u64 system_time;
++ u64 timeout;
++ u64 limit;
++
++ /* estimate timeout limit after timeout enable, actually timeout limit
++ * in hardware will be earlier than estimate so we are on the safe side
++ */
++ tsnep_get_system_time(adapter, &system_time);
++ timeout = system_time + TSNEP_GC_TIMEOUT;
++
++ if (curr)
++ limit = timeout + curr->change_limit;
++ else
++ limit = timeout;
++
++ gcl->start_time = tsnep_gcl_start_after(gcl, limit);
++
++ /* gate control time register is only 32bit => time shall be in the near
++ * future (no driver support for far future implemented)
++ */
++ if ((gcl->start_time - system_time) >= U32_MAX)
++ return -EAGAIN;
++
++ if (curr) {
++ /* change gate control list */
++ u64 last;
++ u64 change;
++
++ last = tsnep_gcl_start_before(curr, gcl->start_time);
++ if ((last + curr->cycle_time) == gcl->start_time)
++ change = tsnep_cut_gcl(curr, last,
++ gcl->start_time - last);
++ else if (((gcl->start_time - last) <=
++ curr->cycle_time_extension) ||
++ ((gcl->start_time - last) <= TSNEP_GCL_MIN_INTERVAL))
++ change = tsnep_extend_gcl(curr, last,
++ gcl->start_time - last);
++ else
++ change = tsnep_cut_gcl(curr, last,
++ gcl->start_time - last);
++
++ WARN_ON(change <= timeout);
++ gcl->change = true;
++ iowrite32(change & 0xFFFFFFFF, adapter->addr + TSNEP_GC_CHANGE);
++ } else {
++ /* start gate control list */
++ WARN_ON(gcl->start_time <= timeout);
++ gcl->change = false;
++ iowrite32(gcl->start_time & 0xFFFFFFFF,
++ adapter->addr + TSNEP_GC_TIME);
++ }
++
++ return 0;
++}
++
++static int tsnep_taprio(struct tsnep_adapter *adapter,
++ struct tc_taprio_qopt_offload *qopt)
++{
++ struct tsnep_gcl *gcl;
++ struct tsnep_gcl *curr;
++ int retval;
++
++ if (!adapter->gate_control)
++ return -EOPNOTSUPP;
++
++ if (!qopt->enable) {
++ /* disable gate control if active */
++ mutex_lock(&adapter->gate_control_lock);
++
++ if (adapter->gate_control_active) {
++ iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
++ adapter->gate_control_active = false;
++ }
++
++ mutex_unlock(&adapter->gate_control_lock);
++
++ return 0;
++ }
++
++ retval = tsnep_validate_gcl(qopt);
++ if (retval)
++ return retval;
++
++ mutex_lock(&adapter->gate_control_lock);
++
++ gcl = &adapter->gcl[adapter->next_gcl];
++ tsnep_write_gcl(gcl, qopt);
++
++ /* select current gate control list if active */
++ if (adapter->gate_control_active) {
++ if (adapter->next_gcl == 0)
++ curr = &adapter->gcl[1];
++ else
++ curr = &adapter->gcl[0];
++ } else {
++ curr = NULL;
++ }
++
++ for (;;) {
++ /* start timeout which discards late enable, this helps ensuring
++ * that start/change time are in the future at enable
++ */
++ iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC);
++
++ retval = tsnep_enable_gcl(adapter, gcl, curr);
++ if (retval) {
++ mutex_unlock(&adapter->gate_control_lock);
++
++ return retval;
++ }
++
++ /* enable gate control list */
++ if (adapter->next_gcl == 0)
++ iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
++ else
++ iowrite8(TSNEP_GC_ENABLE_B, adapter->addr + TSNEP_GC);
++
++ /* done if timeout did not happen */
++ if (!(ioread32(adapter->addr + TSNEP_GC) &
++ TSNEP_GC_TIMEOUT_SIGNAL))
++ break;
++
++ /* timeout is acknowledged with any enable */
++ iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
++
++ if (curr)
++ tsnep_clean_gcl(curr);
++
++ /* retry because of timeout */
++ }
++
++ adapter->gate_control_active = true;
++
++ if (adapter->next_gcl == 0)
++ adapter->next_gcl = 1;
++ else
++ adapter->next_gcl = 0;
++
++ mutex_unlock(&adapter->gate_control_lock);
++
++ return 0;
++}
++
++int tsnep_tc_setup(struct net_device *netdev, enum tc_setup_type type,
++ void *type_data)
++{
++ struct tsnep_adapter *adapter = netdev_priv(netdev);
++
++ switch (type) {
++ case TC_SETUP_QDISC_TAPRIO:
++ return tsnep_taprio(adapter, type_data);
++ default:
++ return -EOPNOTSUPP;
++ }
++}
++
++int tsnep_tc_init(struct tsnep_adapter *adapter)
++{
++ if (!adapter->gate_control)
++ return 0;
++
++ /* open all gates */
++ iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
++ iowrite32(TSNEP_GC_OPEN | TSNEP_GC_NEXT_OPEN, adapter->addr + TSNEP_GC);
++
++ adapter->gcl[0].addr = adapter->addr + TSNEP_GCL_A;
++ adapter->gcl[1].addr = adapter->addr + TSNEP_GCL_B;
++
++ return 0;
++}
++
++void tsnep_tc_cleanup(struct tsnep_adapter *adapter)
++{
++ if (!adapter->gate_control)
++ return;
++
++ if (adapter->gate_control_active) {
++ iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
++ adapter->gate_control_active = false;
++ }
++}
+diff --git a/drivers/net/ethernet/engleder/tsnep_test.c b/drivers/net/ethernet/engleder/tsnep_test.c
new file mode 100644
index 000000000000..1581d6b22232
--- /dev/null
-+++ b/drivers/net/ethernet/engleder/tsnep_selftests.c
++++ b/drivers/net/ethernet/engleder/tsnep_test.c
@@ -0,0 +1,811 @@
+// SPDX-License-Identifier: GPL-2.0
+/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
@@ -3166,455 +3559,6 @@
+ data[TSNEP_TEST_TAPRIO_EXTENSION] = 1;
+ }
+}
-diff --git a/drivers/net/ethernet/engleder/tsnep_tc.c b/drivers/net/ethernet/engleder/tsnep_tc.c
-new file mode 100644
-index 000000000000..c4c6e1357317
---- /dev/null
-+++ b/drivers/net/ethernet/engleder/tsnep_tc.c
-@@ -0,0 +1,443 @@
-+// SPDX-License-Identifier: GPL-2.0
-+/* Copyright (C) 2021 Gerhard Engleder <gerhard@engleder-embedded.com> */
-+
-+#include "tsnep.h"
-+
-+#include <net/pkt_sched.h>
-+
-+/* save one operation at the end for additional operation at list change */
-+#define TSNEP_MAX_GCL_NUM (TSNEP_GCL_COUNT - 1)
-+
-+static int tsnep_validate_gcl(struct tc_taprio_qopt_offload *qopt)
-+{
-+ int i;
-+ u64 cycle_time;
-+
-+ if (!qopt->cycle_time)
-+ return -ERANGE;
-+ if (qopt->num_entries > TSNEP_MAX_GCL_NUM)
-+ return -EINVAL;
-+ cycle_time = 0;
-+ for (i = 0; i < qopt->num_entries; i++) {
-+ if (qopt->entries[i].command != TC_TAPRIO_CMD_SET_GATES)
-+ return -EINVAL;
-+ if (qopt->entries[i].gate_mask & ~TSNEP_GCL_MASK)
-+ return -EINVAL;
-+ if (qopt->entries[i].interval < TSNEP_GCL_MIN_INTERVAL)
-+ return -EINVAL;
-+ cycle_time += qopt->entries[i].interval;
-+ }
-+ if (qopt->cycle_time != cycle_time)
-+ return -EINVAL;
-+ if (qopt->cycle_time_extension >= qopt->cycle_time)
-+ return -EINVAL;
-+
-+ return 0;
-+}
-+
-+static void tsnep_write_gcl_operation(struct tsnep_gcl *gcl, int index,
-+ u32 properties, u32 interval, bool flush)
-+{
-+ void __iomem *addr = gcl->addr +
-+ sizeof(struct tsnep_gcl_operation) * index;
-+
-+ gcl->operation[index].properties = properties;
-+ gcl->operation[index].interval = interval;
-+
-+ iowrite32(properties, addr);
-+ iowrite32(interval, addr + sizeof(u32));
-+
-+ if (flush) {
-+ /* flush write with read access */
-+ ioread32(addr);
-+ }
-+}
-+
-+static u64 tsnep_change_duration(struct tsnep_gcl *gcl, int index)
-+{
-+ u64 duration;
-+ int count;
-+
-+ /* change needs to be triggered one or two operations before start of
-+ * new gate control list
-+ * - change is triggered at start of operation (minimum one operation)
-+ * - operation with adjusted interval is inserted on demand to exactly
-+ * meet the start of the new gate control list (optional)
-+ *
-+ * additionally properties are read directly after start of previous
-+ * operation
-+ *
-+ * therefore, three operations needs to be considered for the limit
-+ */
-+ duration = 0;
-+ count = 3;
-+ while (count) {
-+ duration += gcl->operation[index].interval;
-+
-+ index--;
-+ if (index < 0)
-+ index = gcl->count - 1;
-+
-+ count--;
-+ }
-+
-+ return duration;
-+}
-+
-+static void tsnep_write_gcl(struct tsnep_gcl *gcl,
-+ struct tc_taprio_qopt_offload *qopt)
-+{
-+ int i;
-+ u32 properties;
-+ u64 extend;
-+ u64 cut;
-+
-+ gcl->base_time = ktime_to_ns(qopt->base_time);
-+ gcl->cycle_time = qopt->cycle_time;
-+ gcl->cycle_time_extension = qopt->cycle_time_extension;
-+
-+ for (i = 0; i < qopt->num_entries; i++) {
-+ properties = qopt->entries[i].gate_mask;
-+ if (i == (qopt->num_entries - 1))
-+ properties |= TSNEP_GCL_LAST;
-+
-+ tsnep_write_gcl_operation(gcl, i, properties,
-+ qopt->entries[i].interval, true);
-+ }
-+ gcl->count = qopt->num_entries;
-+
-+ /* calculate change limit; i.e., the time needed between enable and
-+ * start of new gate control list
-+ */
-+
-+ /* case 1: extend cycle time for change
-+ * - change duration of last operation
-+ * - cycle time extension
-+ */
-+ extend = tsnep_change_duration(gcl, gcl->count - 1);
-+ extend += gcl->cycle_time_extension;
-+
-+ /* case 2: cut cycle time for change
-+ * - maximum change duration
-+ */
-+ cut = 0;
-+ for (i = 0; i < gcl->count; i++)
-+ cut = max(cut, tsnep_change_duration(gcl, i));
-+
-+ /* use maximum, because the actual case (extend or cut) can be
-+ * determined only after limit is known (chicken-and-egg problem)
-+ */
-+ gcl->change_limit = max(extend, cut);
-+}
-+
-+static u64 tsnep_gcl_start_after(struct tsnep_gcl *gcl, u64 limit)
-+{
-+ u64 start = gcl->base_time;
-+ u64 n;
-+
-+ if (start <= limit) {
-+ n = div64_u64(limit - start, gcl->cycle_time);
-+ start += (n + 1) * gcl->cycle_time;
-+ }
-+
-+ return start;
-+}
-+
-+static u64 tsnep_gcl_start_before(struct tsnep_gcl *gcl, u64 limit)
-+{
-+ u64 start = gcl->base_time;
-+ u64 n;
-+
-+ n = div64_u64(limit - start, gcl->cycle_time);
-+ start += n * gcl->cycle_time;
-+ if (start == limit)
-+ start -= gcl->cycle_time;
-+
-+ return start;
-+}
-+
-+static u64 tsnep_set_gcl_change(struct tsnep_gcl *gcl, int index, u64 change,
-+ bool insert)
-+{
-+ /* previous operation triggers change and properties are evaluated at
-+ * start of operation
-+ */
-+ if (index == 0)
-+ index = gcl->count - 1;
-+ else
-+ index = index - 1;
-+ change -= gcl->operation[index].interval;
-+
-+ /* optionally change to new list with additional operation in between */
-+ if (insert) {
-+ void __iomem *addr = gcl->addr +
-+ sizeof(struct tsnep_gcl_operation) * index;
-+
-+ gcl->operation[index].properties |= TSNEP_GCL_INSERT;
-+ iowrite32(gcl->operation[index].properties, addr);
-+ }
-+
-+ return change;
-+}
-+
-+static void tsnep_clean_gcl(struct tsnep_gcl *gcl)
-+{
-+ int i;
-+ u32 mask = TSNEP_GCL_LAST | TSNEP_GCL_MASK;
-+ void __iomem *addr;
-+
-+ /* search for insert operation and reset properties */
-+ for (i = 0; i < gcl->count; i++) {
-+ if (gcl->operation[i].properties & ~mask) {
-+ addr = gcl->addr +
-+ sizeof(struct tsnep_gcl_operation) * i;
-+
-+ gcl->operation[i].properties &= mask;
-+ iowrite32(gcl->operation[i].properties, addr);
-+
-+ break;
-+ }
-+ }
-+}
-+
-+static u64 tsnep_insert_gcl_operation(struct tsnep_gcl *gcl, int ref,
-+ u64 change, u32 interval)
-+{
-+ u32 properties;
-+
-+ properties = gcl->operation[ref].properties & TSNEP_GCL_MASK;
-+ /* change to new list directly after inserted operation */
-+ properties |= TSNEP_GCL_CHANGE;
-+
-+ /* last operation of list is reserved to insert operation */
-+ tsnep_write_gcl_operation(gcl, TSNEP_GCL_COUNT - 1, properties,
-+ interval, false);
-+
-+ return tsnep_set_gcl_change(gcl, ref, change, true);
-+}
-+
-+static u64 tsnep_extend_gcl(struct tsnep_gcl *gcl, u64 start, u32 extension)
-+{
-+ int ref = gcl->count - 1;
-+ u32 interval = gcl->operation[ref].interval + extension;
-+
-+ start -= gcl->operation[ref].interval;
-+
-+ return tsnep_insert_gcl_operation(gcl, ref, start, interval);
-+}
-+
-+static u64 tsnep_cut_gcl(struct tsnep_gcl *gcl, u64 start, u64 cycle_time)
-+{
-+ u64 sum = 0;
-+ int i;
-+
-+ /* find operation which shall be cutted */
-+ for (i = 0; i < gcl->count; i++) {
-+ u64 sum_tmp = sum + gcl->operation[i].interval;
-+ u64 interval;
-+
-+ /* sum up operations as long as cycle time is not exceeded */
-+ if (sum_tmp > cycle_time)
-+ break;
-+
-+ /* remaining interval must be big enough for hardware */
-+ interval = cycle_time - sum_tmp;
-+ if (interval > 0 && interval < TSNEP_GCL_MIN_INTERVAL)
-+ break;
-+
-+ sum = sum_tmp;
-+ }
-+ if (sum == cycle_time) {
-+ /* no need to cut operation itself or whole cycle
-+ * => change exactly at operation
-+ */
-+ return tsnep_set_gcl_change(gcl, i, start + sum, false);
-+ }
-+ return tsnep_insert_gcl_operation(gcl, i, start + sum,
-+ cycle_time - sum);
-+}
-+
-+static int tsnep_enable_gcl(struct tsnep_adapter *adapter,
-+ struct tsnep_gcl *gcl, struct tsnep_gcl *curr)
-+{
-+ u64 system_time;
-+ u64 timeout;
-+ u64 limit;
-+
-+ /* estimate timeout limit after timeout enable, actually timeout limit
-+ * in hardware will be earlier than estimate so we are on the safe side
-+ */
-+ tsnep_get_system_time(adapter, &system_time);
-+ timeout = system_time + TSNEP_GC_TIMEOUT;
-+
-+ if (curr)
-+ limit = timeout + curr->change_limit;
-+ else
-+ limit = timeout;
-+
-+ gcl->start_time = tsnep_gcl_start_after(gcl, limit);
-+
-+ /* gate control time register is only 32bit => time shall be in the near
-+ * future (no driver support for far future implemented)
-+ */
-+ if ((gcl->start_time - system_time) >= U32_MAX)
-+ return -EAGAIN;
-+
-+ if (curr) {
-+ /* change gate control list */
-+ u64 last;
-+ u64 change;
-+
-+ last = tsnep_gcl_start_before(curr, gcl->start_time);
-+ if ((last + curr->cycle_time) == gcl->start_time)
-+ change = tsnep_cut_gcl(curr, last,
-+ gcl->start_time - last);
-+ else if (((gcl->start_time - last) <=
-+ curr->cycle_time_extension) ||
-+ ((gcl->start_time - last) <= TSNEP_GCL_MIN_INTERVAL))
-+ change = tsnep_extend_gcl(curr, last,
-+ gcl->start_time - last);
-+ else
-+ change = tsnep_cut_gcl(curr, last,
-+ gcl->start_time - last);
-+
-+ WARN_ON(change <= timeout);
-+ gcl->change = true;
-+ iowrite32(change & 0xFFFFFFFF, adapter->addr + TSNEP_GC_CHANGE);
-+ } else {
-+ /* start gate control list */
-+ WARN_ON(gcl->start_time <= timeout);
-+ gcl->change = false;
-+ iowrite32(gcl->start_time & 0xFFFFFFFF,
-+ adapter->addr + TSNEP_GC_TIME);
-+ }
-+
-+ return 0;
-+}
-+
-+static int tsnep_taprio(struct tsnep_adapter *adapter,
-+ struct tc_taprio_qopt_offload *qopt)
-+{
-+ struct tsnep_gcl *gcl;
-+ struct tsnep_gcl *curr;
-+ int retval;
-+
-+ if (!adapter->gate_control)
-+ return -EOPNOTSUPP;
-+
-+ if (!qopt->enable) {
-+ /* disable gate control if active */
-+ mutex_lock(&adapter->gate_control_lock);
-+
-+ if (adapter->gate_control_active) {
-+ iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
-+ adapter->gate_control_active = false;
-+ }
-+
-+ mutex_unlock(&adapter->gate_control_lock);
-+
-+ return 0;
-+ }
-+
-+ retval = tsnep_validate_gcl(qopt);
-+ if (retval)
-+ return retval;
-+
-+ mutex_lock(&adapter->gate_control_lock);
-+
-+ gcl = &adapter->gcl[adapter->next_gcl];
-+ tsnep_write_gcl(gcl, qopt);
-+
-+ /* select current gate control list if active */
-+ if (adapter->gate_control_active) {
-+ if (adapter->next_gcl == 0)
-+ curr = &adapter->gcl[1];
-+ else
-+ curr = &adapter->gcl[0];
-+ } else {
-+ curr = NULL;
-+ }
-+
-+ for (;;) {
-+ /* start timeout which discards late enable, this helps ensuring
-+ * that start/change time are in the future at enable
-+ */
-+ iowrite8(TSNEP_GC_ENABLE_TIMEOUT, adapter->addr + TSNEP_GC);
-+
-+ retval = tsnep_enable_gcl(adapter, gcl, curr);
-+ if (retval) {
-+ mutex_unlock(&adapter->gate_control_lock);
-+
-+ return retval;
-+ }
-+
-+ /* enable gate control list */
-+ if (adapter->next_gcl == 0)
-+ iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
-+ else
-+ iowrite8(TSNEP_GC_ENABLE_B, adapter->addr + TSNEP_GC);
-+
-+ /* done if timeout did not happen */
-+ if (!(ioread32(adapter->addr + TSNEP_GC) &
-+ TSNEP_GC_TIMEOUT_SIGNAL))
-+ break;
-+
-+ /* timeout is acknowledged with any enable */
-+ iowrite8(TSNEP_GC_ENABLE_A, adapter->addr + TSNEP_GC);
-+
-+ if (curr)
-+ tsnep_clean_gcl(curr);
-+
-+ /* retry because of timeout */
-+ }
-+
-+ adapter->gate_control_active = true;
-+
-+ if (adapter->next_gcl == 0)
-+ adapter->next_gcl = 1;
-+ else
-+ adapter->next_gcl = 0;
-+
-+ mutex_unlock(&adapter->gate_control_lock);
-+
-+ return 0;
-+}
-+
-+int tsnep_tc_setup(struct net_device *netdev, enum tc_setup_type type,
-+ void *type_data)
-+{
-+ struct tsnep_adapter *adapter = netdev_priv(netdev);
-+
-+ switch (type) {
-+ case TC_SETUP_QDISC_TAPRIO:
-+ return tsnep_taprio(adapter, type_data);
-+ default:
-+ return -EOPNOTSUPP;
-+ }
-+}
-+
-+int tsnep_tc_init(struct tsnep_adapter *adapter)
-+{
-+ if (!adapter->gate_control)
-+ return 0;
-+
-+ /* open all gates */
-+ iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
-+ iowrite32(TSNEP_GC_OPEN | TSNEP_GC_NEXT_OPEN, adapter->addr + TSNEP_GC);
-+
-+ adapter->gcl[0].addr = adapter->addr + TSNEP_GCL_A;
-+ adapter->gcl[1].addr = adapter->addr + TSNEP_GCL_B;
-+
-+ return 0;
-+}
-+
-+void tsnep_tc_cleanup(struct tsnep_adapter *adapter)
-+{
-+ if (!adapter->gate_control)
-+ return;
-+
-+ if (adapter->gate_control_active) {
-+ iowrite8(TSNEP_GC_DISABLE, adapter->addr + TSNEP_GC);
-+ adapter->gate_control_active = false;
-+ }
-+}
--
2.20.1