[RFC PATCH 8/8] net: xilinx: tsn: Add PTP packet transmission support
Srinivas Neeli
srinivas.neeli at amd.com
Wed Feb 18 21:49:11 PST 2026
Add support for PTP (Precision Time Protocol) packet transmission
and timestamping for Xilinx TSN Ethernet MAC. This implementation
provides hardware-assisted packet timestamping for IEEE 1588 PTP
synchronization.
Key features added:
- PTP TX/RX buffer management with 8 TX and 16 RX buffers
- Hardware timestamp extraction for transmitted and received packets
- Dedicated PTP packet queue and transmission path
- Interrupt-driven timestamp retrieval via work queues
- Support for 2-step PTP mode (HWTSTAMP_TX_ON)
- PTP packet filtering based on ETH_P_1588 ethertype
Signed-off-by: Srinivas Neeli <srinivas.neeli at amd.com>
---
drivers/net/ethernet/xilinx/tsn/Makefile | 2 +-
drivers/net/ethernet/xilinx/tsn/xilinx_tsn.h | 58 +++
.../net/ethernet/xilinx/tsn/xilinx_tsn_emac.c | 178 ++++++-
.../ethernet/xilinx/tsn/xilinx_tsn_ptp_xmit.c | 451 ++++++++++++++++++
4 files changed, 683 insertions(+), 6 deletions(-)
create mode 100644 drivers/net/ethernet/xilinx/tsn/xilinx_tsn_ptp_xmit.c
diff --git a/drivers/net/ethernet/xilinx/tsn/Makefile b/drivers/net/ethernet/xilinx/tsn/Makefile
index 0faa5233221b..a39cc7ca1533 100644
--- a/drivers/net/ethernet/xilinx/tsn/Makefile
+++ b/drivers/net/ethernet/xilinx/tsn/Makefile
@@ -1,2 +1,2 @@
obj-$(CONFIG_XILINX_TSN) :=xilinx_tsn.o
-xilinx_tsn-objs := xilinx_tsn_main.o xilinx_tsn_ep.o xilinx_tsn_emac.o xilinx_tsn_mdio.o xilinx_tsn_switch.o xilinx_tsn_ptp_clock.o
+xilinx_tsn-objs := xilinx_tsn_main.o xilinx_tsn_ep.o xilinx_tsn_emac.o xilinx_tsn_mdio.o xilinx_tsn_switch.o xilinx_tsn_ptp_clock.o xilinx_tsn_ptp_xmit.o
diff --git a/drivers/net/ethernet/xilinx/tsn/xilinx_tsn.h b/drivers/net/ethernet/xilinx/tsn/xilinx_tsn.h
index 0cce916825ea..c8339ecef2f6 100644
--- a/drivers/net/ethernet/xilinx/tsn/xilinx_tsn.h
+++ b/drivers/net/ethernet/xilinx/tsn/xilinx_tsn.h
@@ -137,6 +137,37 @@
/* PTP Timer Register Base Offset */
#define TSN_PTP_TIMER_OFFSET 0x12800
+/* PTP register offsets */
+#define PTP_TX_CONTROL_OFFSET 0x00012000
+#define PTP_RX_CONTROL_OFFSET 0x00012004
+
+/* PTP RX buffer configuration */
+#define PTP_RX_BASE_OFFSET 0x00010000
+#define PTP_RX_PACKET_FIELD_MASK 0x00000F00
+#define PTP_RX_PACKET_CLEAR 0x00000001
+
+/* PTP TX buffer configuration */
+#define PTP_TX_BUFFER_OFFSET(index) (0x00011000 + (index) * 0x100)
+#define PTP_TX_CMD_FIELD_LEN 8
+#define PTP_TX_CMD_1STEP_SHIFT BIT(16)
+#define PTP_TX_BUFFER_CMD2_FIELD 0x4
+
+/* PTP TX control and status masks */
+#define PTP_TX_FRAME_WAITING_MASK 0x0000ff00
+#define PTP_TX_FRAME_WAITING_SHIFT 8
+#define PTP_TX_PACKET_FIELD_MASK 0x00070000
+#define PTP_TX_PACKET_FIELD_SHIFT 16
+
+/* PTP timestamp and buffer definitions */
+#define PTP_HW_TSTAMP_SIZE 8 /* 64 bit timestamp */
+#define PTP_RX_HWBUF_SIZE 256
+#define PTP_RX_FRAME_SIZE 252
+#define PTP_HW_TSTAMP_OFFSET (PTP_RX_HWBUF_SIZE - PTP_HW_TSTAMP_SIZE)
+
+/* PTP message type definitions */
+#define PTP_MSG_TYPE_MASK BIT(3)
+#define PTP_TYPE_SYNC 0x0
+
/**
* struct tsn_ptp_timer - PTP timer private data
* @dev: Device pointer
@@ -175,6 +206,16 @@ struct tsn_ptp_timer {
* @mii_clk_div: MDIO clock divider value
* @emac_num: EMAC instance number (1 or 2)
* @irq: Interrupt number for this EMAC
+ * @ptp_rx_irq: PTP RX interrupt number
+ * @ptp_tx_irq: PTP TX interrupt number
+ * @ptp_txq: PTP TX packet queue for timestamping
+ * @ptp_tx_lock: Spinlock for PTP TX queue
+ * @tx_tstamp_work: Work structure for TX timestamp processing
+ * @ptp_rx_hw_pointer: Hardware pointer for PTP RX packets
+ * @ptp_rx_sw_pointer: Software pointer for PTP RX packets
+ * @ptp_ts_type: PTP timestamp type configuration
+ * @tstamp_config: Hardware timestamp config structure
+ * @current_rx_filter : Current rx filter
*/
struct tsn_emac {
struct net_device *ndev;
@@ -189,6 +230,16 @@ struct tsn_emac {
u8 mii_clk_div;
int emac_num;
int irq;
+ int ptp_rx_irq;
+ int ptp_tx_irq;
+ struct sk_buff_head ptp_txq;
+ spinlock_t ptp_tx_lock; /* Protect PTP TX queue */
+ struct work_struct tx_tstamp_work;
+ u8 ptp_rx_hw_pointer;
+ u8 ptp_rx_sw_pointer;
+ int ptp_ts_type;
+ struct hwtstamp_config tstamp_config;
+ int current_rx_filter;
};
/*
@@ -369,4 +420,11 @@ int tsn_switch_init(struct platform_device *pdev);
void tsn_switch_exit(struct platform_device *pdev);
int tsn_ptp_timer_init(struct tsn_emac *emac, struct device_node *emac_np);
void tsn_ptp_timer_exit(struct tsn_emac *emac);
+int tsn_ptp_get_irq_info(struct tsn_emac *emac, struct device_node *emac_np);
+int tsn_ptp_init_and_register_irqs(struct tsn_emac *emac);
+void tsn_ptp_unregister_irqs(struct tsn_emac *emac);
+int tsn_ptp_xmit(struct sk_buff *skb, struct tsn_emac *emac);
+void tsn_ptp_tx_tstamp(struct work_struct *work);
+irqreturn_t tsn_ptp_rx_irq(int irq, void *data);
+irqreturn_t tsn_ptp_tx_irq(int irq, void *data);
#endif /* XILINX_TSN_H */
diff --git a/drivers/net/ethernet/xilinx/tsn/xilinx_tsn_emac.c b/drivers/net/ethernet/xilinx/tsn/xilinx_tsn_emac.c
index b7d7ba0de717..4d0780a29fc9 100644
--- a/drivers/net/ethernet/xilinx/tsn/xilinx_tsn_emac.c
+++ b/drivers/net/ethernet/xilinx/tsn/xilinx_tsn_emac.c
@@ -61,16 +61,28 @@ static int emac_open(struct net_device *ndev)
{
struct tsn_emac *emac = netdev_priv(ndev);
struct phy_device *phydev = NULL;
+ int ret;
+
+ /* Register PTP interrupts */
+ ret = tsn_ptp_init_and_register_irqs(emac);
+ if (ret) {
+ dev_err(emac->common->dev,
+ "EMAC %d: Failed to register PTP interrupts: %d\n",
+ emac->emac_num, ret);
+ return ret;
+ }
if (emac->phy_node) {
phydev = of_phy_connect(emac->ndev, emac->phy_node,
tsn_adjust_link_tsn,
emac->phy_flags,
emac->phy_mode);
- if (!phydev)
+ if (!phydev) {
dev_err(emac->common->dev, "of_phy_connect() failed\n");
- else
- phy_start(phydev);
+ tsn_ptp_unregister_irqs(emac);
+ return -ENODEV;
+ }
+ phy_start(phydev);
}
return 0;
@@ -87,9 +99,13 @@ static int emac_open(struct net_device *ndev)
*/
static int emac_stop(struct net_device *ndev)
{
+ struct tsn_emac *emac = netdev_priv(ndev);
+
if (ndev->phydev)
phy_disconnect(ndev->phydev);
+ tsn_ptp_unregister_irqs(emac);
+
return 0;
}
@@ -120,16 +136,158 @@ static int emac_validate_addr(struct net_device *ndev)
static netdev_tx_t emac_start_xmit(struct sk_buff *skb, struct net_device *ndev)
{
struct tsn_emac *emac = netdev_priv(ndev);
+ u16 queue = skb_get_queue_mapping(skb);
+
+ if (queue == emac->common->num_priorities)
+ return tsn_ptp_xmit(skb, emac);
return tsn_start_xmit_dmaengine(emac->common, skb, ndev);
}
+/**
+ * emac_select_queue - select queue for packet transmission
+ * @ndev: Pointer to net_device structure
+ * @skb: socket buffer containing the packet
+ * @sb_dev: fallback device (not used)
+ *
+ * Return: Queue index for PTP packets or default queue
+ *
+ * This function selects the appropriate queue for packet transmission.
+ * PTP packets (ETH_P_1588) are directed to a dedicated PTP queue.
+ */
+static u16 emac_select_queue(struct net_device *ndev,
+ struct sk_buff *skb,
+ struct net_device *sb_dev)
+{
+ struct tsn_emac *emac = netdev_priv(ndev);
+ struct tsn_priv *common = emac->common;
+ struct ethhdr *hdr = (struct ethhdr *)skb->data;
+
+ /* PTP over Ethernet (Layer 2) */
+ if (hdr->h_proto == htons(ETH_P_1588))
+ return common->num_priorities;
+ return netdev_pick_tx(ndev, skb, sb_dev);
+}
+
+/**
+ * emac_set_timestamp_mode - sets up the hardware for the requested mode
+ * @emac: Pointer to TSN EMAC structure
+ * @config: the hwtstamp configuration requested
+ *
+ * Return: 0 on success, Negative value on errors
+ */
+static int emac_set_timestamp_mode(struct tsn_emac *emac,
+ struct hwtstamp_config *config)
+{
+ /* reserved for future extensions */
+ if (config->flags)
+ return -EINVAL;
+
+ if (config->tx_type < HWTSTAMP_TX_OFF ||
+ config->tx_type > HWTSTAMP_TX_ON)
+ return -ERANGE;
+
+ emac->ptp_ts_type = config->tx_type;
+
+ /* On RX always timestamp everything */
+ switch (config->rx_filter) {
+ case HWTSTAMP_FILTER_NONE:
+ emac->current_rx_filter = HWTSTAMP_FILTER_NONE;
+ break;
+ case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+ emac->current_rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
+ config->rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
+ break;
+ default:
+ return -ERANGE;
+ }
+ return 0;
+}
+
+/**
+ * emac_set_ts_config - user entry point for timestamp mode
+ * @emac: Pointer to TSN EMAC structure
+ * @ifr: ioctl data
+ *
+ * Set hardware to the requested more. If unsupported return an error
+ * with no changes. Otherwise, store the mode for future reference
+ *
+ * Return: 0 on success, Negative value on errors
+ */
+static int emac_set_ts_config(struct tsn_emac *emac, struct ifreq *ifr)
+{
+ struct hwtstamp_config config;
+ int err;
+
+ if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+ return -EFAULT;
+
+ err = emac_set_timestamp_mode(emac, &config);
+ if (err)
+ return err;
+
+ /* save these settings for future reference */
+ memcpy(&emac->tstamp_config, &config, sizeof(emac->tstamp_config));
+
+ return copy_to_user(ifr->ifr_data, &config,
+ sizeof(config)) ? -EFAULT : 0;
+}
+
+/**
+ * emac_get_ts_config - return the current timestamp configuration
+ * to the user
+ * @emac: pointer to TSN EMAC structure
+ * @ifr: ioctl data
+ *
+ * Return: 0 on success, Negative value on errors
+ */
+static int emac_get_ts_config(struct tsn_emac *emac, struct ifreq *ifr)
+{
+ struct hwtstamp_config *config = &emac->tstamp_config;
+
+ return copy_to_user(ifr->ifr_data, config,
+ sizeof(*config)) ? -EFAULT : 0;
+}
+
+/**
+ * emac_ioctl - Ioctl MII Interface
+ * @dev: Pointer to net_device structure
+ * @rq: ioctl request structure
+ * @cmd: ioctl command
+ *
+ * Return: 0 on success, Negative value on errors
+ */
+static int emac_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
+{
+ struct tsn_emac *emac = netdev_priv(dev);
+
+ if (!netif_running(dev))
+ return -EINVAL;
+
+ switch (cmd) {
+ case SIOCGMIIPHY:
+ case SIOCGMIIREG:
+ case SIOCSMIIREG:
+ if (!dev->phydev)
+ return -EOPNOTSUPP;
+ return phy_mii_ioctl(dev->phydev, rq, cmd);
+ case SIOCSHWTSTAMP:
+ return emac_set_ts_config(emac, rq);
+ case SIOCGHWTSTAMP:
+ return emac_get_ts_config(emac, rq);
+ default:
+ return -EOPNOTSUPP;
+ }
+}
+
static const struct net_device_ops emac_netdev_ops = {
.ndo_open = emac_open,
.ndo_stop = emac_stop,
.ndo_start_xmit = emac_start_xmit,
.ndo_set_mac_address = tsn_ndo_set_mac_address,
.ndo_validate_addr = emac_validate_addr,
+ .ndo_select_queue = emac_select_queue,
+ .ndo_eth_ioctl = emac_ioctl,
};
/**
@@ -171,7 +329,7 @@ static int emac_get_ts_info(struct net_device *ndev,
BIT(HWTSTAMP_TX_ON);
info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
- BIT(HWTSTAMP_FILTER_ALL);
+ BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT);
if (common->phc_index >= 0)
info->phc_index = common->phc_index;
@@ -225,7 +383,9 @@ int tsn_emac_init(struct platform_device *pdev)
goto err_cleanup_all;
}
- ndev = alloc_etherdev(sizeof(*emac));
+ ndev = alloc_etherdev_mqs(sizeof(*emac),
+ common->num_tx_queues + 1,
+ common->num_rx_queues);
if (!ndev) {
ret = -ENOMEM;
of_node_put(emac_np);
@@ -279,6 +439,14 @@ int tsn_emac_init(struct platform_device *pdev)
goto err_teardown_mdio;
}
}
+
+ ret = tsn_ptp_get_irq_info(emac, emac_np);
+ if (ret) {
+ dev_err(dev, "Failed to get PTP IRQ info for EMAC %d: %d\n",
+ emac->emac_num, ret);
+ goto err_remove_ptp;
+ }
+
ret = register_netdev(ndev);
if (ret) {
dev_err(dev, "Failed to register net device for MAC %d\n", mac_id);
diff --git a/drivers/net/ethernet/xilinx/tsn/xilinx_tsn_ptp_xmit.c b/drivers/net/ethernet/xilinx/tsn/xilinx_tsn_ptp_xmit.c
new file mode 100644
index 000000000000..0a2850ed42ad
--- /dev/null
+++ b/drivers/net/ethernet/xilinx/tsn/xilinx_tsn_ptp_xmit.c
@@ -0,0 +1,451 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Xilinx FPGA Xilinx TSN PTP transfer protocol module.
+ *
+ */
+
+#include "xilinx_tsn.h"
+
+/**
+ * ptp_iow - write to PTP register
+ * @emac: Pointer to TSN EMAC structure
+ * @off: Register offset
+ * @val: Value to write
+ *
+ * This function writes to PTP control registers.
+ */
+static inline void ptp_iow(struct tsn_emac *emac, off_t off, u32 val)
+{
+ iowrite32(val, emac->regs + off);
+}
+
+/**
+ * ptp_ior - read from PTP register
+ * @emac: Pointer to TSN EMAC structure
+ * @off: Register offset
+ *
+ * Return: Register value
+ *
+ * This function reads from PTP control registers.
+ */
+static inline u32 ptp_ior(struct tsn_emac *emac, u32 off)
+{
+ return ioread32(emac->regs + off);
+}
+
+/**
+ * memcpy_fromio_32 - copy ptp buffer from HW
+ * @emac: Pointer to TSN EMAC structure
+ * @offset: Offset in the PTP buffer
+ * @data: Destination buffer
+ * @len: Length to copy
+ *
+ * This functions copies the data from PTP buffer to destination data buffer
+ */
+static void memcpy_fromio_32(struct tsn_emac *emac,
+ unsigned long offset, u8 *data, size_t len)
+{
+ while (len >= 4) {
+ *(u32 *)data = ptp_ior(emac, offset);
+ len -= 4;
+ offset += 4;
+ data += 4;
+ }
+
+ if (len > 0) {
+ u32 leftover = ptp_ior(emac, offset);
+ u8 *src = (u8 *)&leftover;
+
+ while (len) {
+ *data++ = *src++;
+ len--;
+ }
+ }
+}
+
+/**
+ * memcpy_toio_32 - copy ptp buffer to HW
+ * @emac: Pointer to TSN EMAC structure
+ * @offset: Offset in the PTP buffer
+ * @data: Source data
+ * @len: Length to copy
+ *
+ * This functions copies the source data to destination ptp buffer
+ */
+static void memcpy_toio_32(struct tsn_emac *emac,
+ unsigned long offset, u8 *data, size_t len)
+{
+ while (len >= 4) {
+ ptp_iow(emac, offset, *(u32 *)data);
+ len -= 4;
+ offset += 4;
+ data += 4;
+ }
+
+ if (len > 0) {
+ u32 leftover = 0;
+ u8 *dest = (u8 *)&leftover;
+
+ while (len) {
+ *dest++ = *data++;
+ len--;
+ }
+ ptp_iow(emac, offset, leftover);
+ }
+}
+
+/**
+ * tsn_ptp_xmit - xmit skb using PTP HW
+ * @skb: sk_buff pointer that contains data to be Txed.
+ * @emac: Pointer to TSN EMAC structure.
+ *
+ * Return: NETDEV_TX_OK, on success
+ * NETDEV_TX_BUSY, if any of the descriptors are not free
+ *
+ * This function is called to transmit a PTP skb. The function uses
+ * the free PTP TX buffer entry and sends the frame
+ */
+int tsn_ptp_xmit(struct sk_buff *skb, struct tsn_emac *emac)
+{
+ u16 queue = skb_get_queue_mapping(skb);
+ u8 tx_frame_waiting;
+ u32 cmd1_field = 0;
+ u32 cmd2_field = 0;
+ u8 free_index;
+
+ tx_frame_waiting = (ptp_ior(emac, PTP_TX_CONTROL_OFFSET) &
+ PTP_TX_FRAME_WAITING_MASK) >>
+ PTP_TX_FRAME_WAITING_SHIFT;
+
+ /* we reached last frame */
+ if (tx_frame_waiting & (1 << 7)) {
+ netif_stop_subqueue(emac->ndev, queue);
+ emac->ndev->stats.tx_dropped++;
+ netdev_dbg(emac->ndev, "PTP TX buffers full: 0x%x\n", tx_frame_waiting);
+ return NETDEV_TX_BUSY;
+ }
+
+ /* go to next available slot */
+ free_index = fls(tx_frame_waiting);
+
+ cmd1_field |= skb->len;
+
+ ptp_iow(emac, PTP_TX_BUFFER_OFFSET(free_index), cmd1_field);
+ ptp_iow(emac, PTP_TX_BUFFER_OFFSET(free_index) +
+ PTP_TX_BUFFER_CMD2_FIELD, cmd2_field);
+ memcpy_toio_32(emac,
+ (PTP_TX_BUFFER_OFFSET(free_index) +
+ PTP_TX_CMD_FIELD_LEN),
+ skb->data, skb->len);
+
+ /* send the frame */
+ ptp_iow(emac, PTP_TX_CONTROL_OFFSET, (1 << free_index));
+
+ scoped_guard(spinlock_irq, &emac->ptp_tx_lock) {
+ skb->cb[0] = free_index;
+ skb_queue_tail(&emac->ptp_txq, skb);
+
+ if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)
+ skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+ }
+ skb_tx_timestamp(skb);
+
+ return NETDEV_TX_OK;
+}
+
+/**
+ * tsn_set_timestamp - timestamp skb with HW timestamp
+ * @emac: Pointer to TSN EMAC structure
+ * @hwtstamps: Pointer to skb timestamp structure
+ * @offset: offset of the timestamp in the PTP buffer
+ *
+ * Return: None.
+ *
+ */
+static void tsn_set_timestamp(struct tsn_emac *emac,
+ struct skb_shared_hwtstamps *hwtstamps,
+ unsigned int offset)
+{
+ u32 captured_ns;
+ u32 captured_sec;
+
+ captured_ns = ptp_ior(emac, offset + 4);
+ captured_sec = ptp_ior(emac, offset);
+
+ hwtstamps->hwtstamp = ktime_set(captured_sec, captured_ns);
+}
+
+/**
+ * tsn_ptp_recv - receive ptp buffer in skb from HW
+ * @ndev: Pointer to net_device structure.
+ *
+ * This function is called from the ptp rx isr. It allocates skb, and
+ * copies the ptp rx buffer data to it and calls netif_rx for further
+ * processing.
+ *
+ */
+static void tsn_ptp_recv(struct net_device *ndev)
+{
+ struct tsn_emac *emac = netdev_priv(ndev);
+ unsigned long ptp_frame_base_addr = 0;
+ struct sk_buff *skb;
+ u16 msg_len;
+ u8 msg_type;
+ u32 bytes = 0;
+ u32 packets = 0;
+
+ if (!ndev || !netif_running(ndev))
+ return;
+
+ while (((emac->ptp_rx_hw_pointer & 0xf) !=
+ (emac->ptp_rx_sw_pointer & 0xf))) {
+ skb = netdev_alloc_skb(ndev, PTP_RX_FRAME_SIZE);
+ if (!skb) {
+ ndev->stats.rx_dropped++;
+ emac->ptp_rx_sw_pointer += 1;
+ continue;
+ }
+ emac->ptp_rx_sw_pointer += 1;
+
+ ptp_frame_base_addr = PTP_RX_BASE_OFFSET +
+ ((emac->ptp_rx_sw_pointer & 0xf) *
+ PTP_RX_HWBUF_SIZE);
+
+ memcpy_fromio_32(emac, ptp_frame_base_addr, skb->data,
+ PTP_RX_FRAME_SIZE);
+
+ msg_type = *(u8 *)(skb->data + ETH_HLEN) & 0xf;
+ msg_len = *(u16 *)(skb->data + ETH_HLEN + 2);
+
+ skb_put(skb, ntohs(msg_len) + ETH_HLEN);
+
+ bytes += skb->len;
+ packets++;
+
+ skb->protocol = eth_type_trans(skb, ndev);
+ skb->ip_summed = CHECKSUM_UNNECESSARY;
+
+ /* timestamp only event messages */
+ if (!(msg_type & PTP_MSG_TYPE_MASK)) {
+ tsn_set_timestamp(emac, skb_hwtstamps(skb),
+ (ptp_frame_base_addr +
+ PTP_HW_TSTAMP_OFFSET));
+ }
+
+ netif_rx(skb);
+ }
+ ndev->stats.rx_packets += packets;
+ ndev->stats.rx_bytes += bytes;
+}
+
+/**
+ * tsn_ptp_rx_irq - PTP RX ISR handler
+ * @irq: irq number
+ * @data: net_device pointer
+ *
+ * Return: IRQ_HANDLED for all cases.
+ */
+irqreturn_t tsn_ptp_rx_irq(int irq, void *data)
+{
+ struct tsn_emac *emac = data;
+
+ emac->ptp_rx_hw_pointer = (ptp_ior(emac, PTP_RX_CONTROL_OFFSET)
+ & PTP_RX_PACKET_FIELD_MASK) >> 8;
+
+ tsn_ptp_recv(emac->ndev);
+
+ return IRQ_HANDLED;
+}
+
+/**
+ * tsn_ptp_tx_tstamp - timestamp skb on transmit path
+ * @work: Pointer to work_struct structure
+ *
+ * This adds TX timestamp to skb
+ */
+void tsn_ptp_tx_tstamp(struct work_struct *work)
+{
+ struct tsn_emac *emac = container_of(work, struct tsn_emac,
+ tx_tstamp_work);
+ struct net_device *ndev = emac->ndev;
+ struct skb_shared_hwtstamps hwtstamps;
+ struct sk_buff *skb;
+ unsigned long ts_reg_offset;
+ unsigned long flags;
+ u8 tx_packet;
+ u8 index;
+ u32 bytes = 0;
+ u32 packets = 0;
+
+ memset(&hwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
+
+ spin_lock_irqsave(&emac->ptp_tx_lock, flags);
+
+ tx_packet = (ptp_ior(emac, PTP_TX_CONTROL_OFFSET) &
+ PTP_TX_PACKET_FIELD_MASK) >>
+ PTP_TX_PACKET_FIELD_SHIFT;
+
+ while ((skb = __skb_dequeue(&emac->ptp_txq)) != NULL) {
+ index = skb->cb[0];
+
+ /* dequeued packet yet to be xmited? */
+ if (index > tx_packet) {
+ /* enqueue it back and break */
+ skb_queue_tail(&emac->ptp_txq, skb);
+ break;
+ }
+ /* time stamp reg offset */
+ ts_reg_offset = PTP_TX_BUFFER_OFFSET(index) +
+ PTP_HW_TSTAMP_OFFSET;
+
+ if (skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS) {
+ tsn_set_timestamp(emac, &hwtstamps, ts_reg_offset);
+ skb_tstamp_tx(skb, &hwtstamps);
+ }
+
+ bytes += skb->len;
+ packets++;
+ dev_kfree_skb_any(skb);
+ }
+ ndev->stats.tx_packets += packets;
+ ndev->stats.tx_bytes += bytes;
+
+ spin_unlock_irqrestore(&emac->ptp_tx_lock, flags);
+}
+
+/**
+ * tsn_ptp_tx_irq - PTP TX irq handler
+ * @irq: irq number
+ * @data: net_device pointer
+ *
+ * Return: IRQ_HANDLED for all cases.
+ *
+ */
+irqreturn_t tsn_ptp_tx_irq(int irq, void *data)
+{
+ struct tsn_emac *emac = data;
+
+ if (!emac || !emac->ndev || !emac->common)
+ return IRQ_HANDLED;
+
+ /* read ctrl register to clear the interrupt */
+ ptp_ior(emac, PTP_TX_CONTROL_OFFSET);
+
+ schedule_work(&emac->tx_tstamp_work);
+ if (__netif_subqueue_stopped(emac->ndev, emac->common->num_priorities))
+ netif_wake_subqueue(emac->ndev, emac->common->num_priorities);
+
+ return IRQ_HANDLED;
+}
+
+/**
+ * tsn_ptp_get_irq_info - Get PTP interrupt information from device tree
+ * @emac: Pointer to TSN EMAC structure
+ * @emac_np: Device tree node for EMAC
+ *
+ * Return: 0 on success, negative error code on failure
+ *
+ * This function retrieves PTP RX and TX interrupt numbers from device tree.
+ */
+int tsn_ptp_get_irq_info(struct tsn_emac *emac, struct device_node *emac_np)
+{
+ struct device *dev = emac->common->dev;
+
+ emac->ptp_rx_irq = of_irq_get_byname(emac_np, "interrupt_ptp_rx");
+ if (emac->ptp_rx_irq < 0) {
+ dev_err(dev,
+ "EMAC %d: Failed to get mandatory 'interrupt_ptp_rx': %d\n",
+ emac->emac_num, emac->ptp_rx_irq);
+ return emac->ptp_rx_irq;
+ }
+
+ emac->ptp_tx_irq = of_irq_get_byname(emac_np, "interrupt_ptp_tx");
+ if (emac->ptp_tx_irq < 0) {
+ dev_err(dev,
+ "EMAC %d: Failed to get mandatory 'interrupt_ptp_tx': %d\n",
+ emac->emac_num, emac->ptp_tx_irq);
+ return emac->ptp_tx_irq;
+ }
+
+ dev_info(dev, "EMAC %d: PTP IRQs - RX: %d, TX: %d\n",
+ emac->emac_num, emac->ptp_rx_irq, emac->ptp_tx_irq);
+
+ return 0;
+}
+
+/**
+ * tsn_ptp_init_and_register_irqs - Initialize PTP subsystem and register interrupts
+ * @emac: Pointer to TSN EMAC structure
+ *
+ * Return: 0 on success, negative error code on failure
+ *
+ * This function initializes the PTP packet handling subsystem and registers
+ * interrupt handlers for PTP RX and TX events.
+ */
+int tsn_ptp_init_and_register_irqs(struct tsn_emac *emac)
+{
+ struct device *dev = emac->common->dev;
+ int ret;
+
+ /* Initialize PTP TX queue and lock */
+ skb_queue_head_init(&emac->ptp_txq);
+ spin_lock_init(&emac->ptp_tx_lock);
+ INIT_WORK(&emac->tx_tstamp_work, tsn_ptp_tx_tstamp);
+
+ /* Initialize PTP RX pointers */
+ emac->current_rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
+ emac->ptp_ts_type = HWTSTAMP_TX_ON;
+ emac->ptp_rx_hw_pointer = 0;
+ emac->ptp_rx_sw_pointer = 0xff;
+
+ /* Clear PTP RX control register */
+ ptp_iow(emac, PTP_RX_CONTROL_OFFSET, PTP_RX_PACKET_CLEAR);
+
+ /* Register PTP RX interrupt */
+ ret = request_irq(emac->ptp_rx_irq, tsn_ptp_rx_irq, 0,
+ "tsn_ptp_rx", emac);
+ if (ret) {
+ dev_err(dev, "EMAC %d: Failed to register PTP RX IRQ %d: %d\n",
+ emac->emac_num, emac->ptp_rx_irq, ret);
+ return ret;
+ }
+
+ /* Register PTP TX interrupt */
+ ret = request_irq(emac->ptp_tx_irq, tsn_ptp_tx_irq, 0,
+ "tsn_ptp_tx", emac);
+ if (ret) {
+ dev_err(dev, "EMAC %d: Failed to register PTP TX IRQ %d: %d\n",
+ emac->emac_num, emac->ptp_tx_irq, ret);
+ free_irq(emac->ptp_rx_irq, emac);
+ return ret;
+ }
+
+ dev_info(dev, "EMAC %d: PTP interrupts registered\n", emac->emac_num);
+ return 0;
+}
+
+/**
+ * tsn_ptp_unregister_irqs - Unregister PTP interrupts
+ * @emac: Pointer to TSN EMAC structure
+ *
+ * This function unregisters PTP RX and TX interrupt handlers and cleans up
+ * PTP TX queue. Called during interface close.
+ */
+void tsn_ptp_unregister_irqs(struct tsn_emac *emac)
+{
+ struct sk_buff *skb;
+
+ if (emac->ptp_tx_irq > 0)
+ free_irq(emac->ptp_tx_irq, emac);
+
+ if (emac->ptp_rx_irq > 0)
+ free_irq(emac->ptp_rx_irq, emac);
+
+ cancel_work_sync(&emac->tx_tstamp_work);
+
+ while ((skb = skb_dequeue(&emac->ptp_txq)) != NULL)
+ dev_kfree_skb_any(skb);
+
+ dev_info(emac->common->dev, "EMAC %d: PTP interrupts unregistered\n",
+ emac->emac_num);
+}
--
2.25.1
More information about the linux-arm-kernel
mailing list