--- drivers/net/phy/marvell/marvell_ptp_rmk.c 2025-04-09 17:49:04.750370045 +0200 +++ drivers/net/phy/marvell/marvell_ptp.c 2025-04-09 17:45:54.003259596 +0200 @@ -38,10 +38,14 @@ #define MARVELL_PAGE_PTP_PORT_2 9 #define PTP2_DEP_STATUS 0 +#define MARVELL_PAGE_PTP_GLOBAL_CONFIG 14 +#define PTP_GLOBAL_CONFIG1 1 + struct marvell_ptp_cb { unsigned long timeout; u16 seq; }; + #define MARVELL_PTP_CB(skb) ((struct marvell_ptp_cb *)(skb)->cb) struct marvell_rxts { @@ -60,7 +64,7 @@ struct sk_buff *tx_skb; enum hwtstamp_tx_types tx_type; - struct mutex rx_mutex; + struct mutex rx_mutex; /* Protect rx_queue, rx_pend and rx_free */ struct list_head rx_free; struct list_head rx_pend; struct sk_buff_head rx_queue; @@ -79,17 +83,26 @@ u16 seq; }; -/* Read the status, timestamp and PTP common header sequence from the PHY. +/** + * marvell_read_tstamp - read the status, timestamp and sequence + * @phydev: Pointer to the phy_device structure + * @ts: Pointer to marvell_ts structure + * @page: Registers page number + * @reg: Register index + * + * Read the status, timestamp and PTP common header sequence from the PHY. * Apparently, reading these are atomic, but there is no mention how the * PHY treats this access as atomic. So, we set the DisTSOverwrite bit * when configuring the PHY. + * + * Return: 1 on success, 0 for invalid status, failure value on error */ static int marvell_read_tstamp(struct phy_device *phydev, struct marvell_ts *ts, - uint8_t page, uint8_t reg) + u8 page, u8 reg) { int oldpage; - int ret; + int ret = 0; /* Read status register */ oldpage = phy_select_page(phydev, page); @@ -127,56 +140,26 @@ /* Clear valid */ __phy_write(phydev, reg, 0); + + /* Return 1 as 0 is for invalid status */ + ret = 1; } restore: return phy_restore_page(phydev, oldpage, ret); } -/* Shouldn't the ptp/networking provide this? */ -static u8 *ptp_header(struct sk_buff *skb, int type) -{ - u8 *data = skb_mac_header(skb); - u8 *ptr = data; - - if (type & PTP_CLASS_VLAN) - ptr += VLAN_HLEN; - - switch (type & PTP_CLASS_PMASK) { - case PTP_CLASS_IPV4: - ptr += IPV4_HLEN(ptr) + UDP_HLEN; - break; - - case PTP_CLASS_IPV6: - ptr += IP6_HLEN + UDP_HLEN; - break; - - case PTP_CLASS_L2: - break; - - default: - return NULL; - } - - if (skb->len < ptr - data + 34) - return NULL; - - return ptr + ETH_HLEN; -} - -/* Extract the sequence ID */ -static u16 ptp_seqid(u8 *ptp_hdr) -{ - __be16 *seqp = (__be16 *)(ptp_hdr + OFF_PTP_SEQUENCE_ID); - - return be16_to_cpup(seqp); -} - static struct marvell_ptp *mii_ts_to_ptp(struct mii_timestamper *mii_ts) { return container_of(mii_ts, struct marvell_ptp, mii_ts); } -/* Deliver a skb with its timestamp back to the networking core */ +/** + * marvell_ptp_rx - Deliver skb with timestamp + * @skb: rx socket buffer + * @ns: Timestamp in ns + * + * Deliver a skb with its timestamp back to the networking core + */ static void marvell_ptp_rx(struct sk_buff *skb, u64 ns) { struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb); @@ -186,8 +169,14 @@ netif_rx(skb); } -/* Get a rx timestamp entry. Try the free list, and if that fails, +/** + * marvell_ptp_get_rxts - Get a rx timestamp entry + * @ptp: Pointer to marvell_ptp structure + * + * Get a rx timestamp entry. Try the free list, and if that fails, * steal the oldest off the pending list. + * + * Return: marvell_rxts list element or zero */ static struct marvell_rxts *marvell_ptp_get_rxts(struct marvell_ptp *ptp) { @@ -198,9 +187,15 @@ return list_last_entry(&ptp->rx_pend, struct marvell_rxts, node); } -/* Check for a rx timestamp entry, try to find the corresponding skb and +/** + * marvell_ptp_rx_ts - Check for a rx timestamp entry + * @ptp: Pointer to marvell_ptp structure + * + * Check for a rx timestamp entry, try to find the corresponding skb and * deliver it, otherwise add the rx timestamp to the queue of pending * timestamps. + * + * Return: 1 if found, 0 if no rx timestamp, -1 in case of timestamp overrun */ static int marvell_ptp_rx_ts(struct marvell_ptp *ptp) { @@ -251,28 +246,37 @@ return 1; } -/* Check whether the packet is suitable for timestamping, and if so, +/** + * marvell_ptp_rxtstamp - Validate rx packet + * @mii_ts: Pointer to mii_timestamper structure + * @skb: rx socket buffer + * @type: PTP class type + * + * Check whether the packet is suitable for timestamping, and if so, * try to find a pending timestamp for it. If no timestamp is found, * queue the packet with a timeout. + * + * Return: true if a ptp packet detected and rx filter is enabled false + * otherwise. */ static bool marvell_ptp_rxtstamp(struct mii_timestamper *mii_ts, struct sk_buff *skb, int type) { struct marvell_ptp *ptp = mii_ts_to_ptp(mii_ts); + struct ptp_header *ptp_hdr; struct marvell_rxts *rxts; bool found = false; - u8 *ptp_hdr; u16 seqid; u64 ns; if (ptp->rx_filter == HWTSTAMP_FILTER_NONE) return false; - ptp_hdr = ptp_header(skb, type); + ptp_hdr = ptp_parse_header(skb, type); if (!ptp_hdr) return false; - seqid = ptp_seqid(ptp_hdr); + seqid = ntohs(ptp_hdr->sequence_id); mutex_lock(&ptp->rx_mutex); @@ -308,13 +312,18 @@ */ marvell_ptp_rx(skb, ns); } else { - schedule_delayed_work(&ptp->ts_work, 2); + if (ptp->phydev->irq < 0) + schedule_delayed_work(&ptp->ts_work, 2); } return true; } -/* Move any expired skbs on to our own list, and then hand the contents of +/** + * marvell_ptp_rx_expire - Manage expired socket + * @ptp: Pointer to marvell_ptp structure + * + * Move any expired skbs on to our own list, and then hand the contents of * our list to netif_rx() - this avoids calling netif_rx() with our * mutex held. */ @@ -339,8 +348,14 @@ netif_rx(skb); } -/* Complete the transmit timestamping; this is called to read the transmit +/** + * marvell_ptp_txtstamp_complete - Complete tx timestamp + * @ptp: Pointer to marvell_ptp structure + * + * Complete the transmit timestamping. This is called to read the transmit * timestamp from the PHY, and report back the transmitted timestamp. + * + * Return: 1 on success, 0 on tx timestamp timeout case, -1 on error */ static int marvell_ptp_txtstamp_complete(struct marvell_ptp *ptp) { @@ -397,14 +412,22 @@ return -1; } -/* Check whether the skb will be timestamped on transmit; we only support +/** + * marvell_ptp_do_txtstamp - Prepared tx socket with timestamping + * @mii_ts: Pointer to mii_timestamper structure + * @skb: tx socket buffer + * @type: PTP class type + * + * Check whether the skb will be timestamped on transmit. We only support * a single outstanding skb. Add it if the slot is available. + * + * Return: True if packet can be transmitted false otherwise */ static bool marvell_ptp_do_txtstamp(struct mii_timestamper *mii_ts, struct sk_buff *skb, int type) { struct marvell_ptp *ptp = mii_ts_to_ptp(mii_ts); - u8 *ptp_hdr; + struct ptp_header *ptp_hdr; if (ptp->tx_type != HWTSTAMP_TX_ON) return false; @@ -412,15 +435,15 @@ if (!(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) return false; - ptp_hdr = ptp_header(skb, type); + ptp_hdr = ptp_parse_header(skb, type); if (!ptp_hdr) return false; - MARVELL_PTP_CB(skb)->seq = ptp_seqid(ptp_hdr); + MARVELL_PTP_CB(skb)->seq = ntohs(ptp_hdr->sequence_id); MARVELL_PTP_CB(skb)->timeout = jiffies + msecs_to_jiffies(TX_TIMEOUT_MS); - if (cmpxchg(&ptp->tx_skb, NULL, skb) != NULL) + if (cmpxchg(&ptp->tx_skb, NULL, skb)) return false; /* DP83640 marks the skb for hw timestamping. Since the MAC driver @@ -428,7 +451,8 @@ * itself, it may not set this flag. So, we need to do this here. */ skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; - schedule_delayed_work(&ptp->ts_work, 2); + if (ptp->phydev->irq < 0) + schedule_delayed_work(&ptp->ts_work, 2); return true; } @@ -441,34 +465,32 @@ } static int marvell_ptp_hwtstamp(struct mii_timestamper *mii_ts, - struct ifreq *ifreq) + struct kernel_hwtstamp_config *config, + struct netlink_ext_ack *extack) { struct marvell_ptp *ptp = mii_ts_to_ptp(mii_ts); - struct hwtstamp_config config; u16 cfg0 = PTP1_PORT_CONFIG_0_DISPTP; u16 cfg2 = 0; int err; - if (copy_from_user(&config, ifreq->ifr_data, sizeof(config))) - return -EFAULT; - - if (config.flags) + if (config->flags) return -EINVAL; - switch (config.tx_type) { + switch (config->tx_type) { case HWTSTAMP_TX_OFF: break; case HWTSTAMP_TX_ON: cfg0 = 0; - cfg2 |= PTP1_PORT_CONFIG_2_DEPINTEN; + if (ptp->phydev->irq >= 0) + cfg2 |= PTP1_PORT_CONFIG_2_DEPINTEN; break; default: return -ERANGE; } - switch (config.rx_filter) { + switch (config->rx_filter) { case HWTSTAMP_FILTER_NONE: break; @@ -488,9 +510,10 @@ * filter on 802.1AS using the transportSpecific field, but * that affects the transmit path too. */ - config.rx_filter = HWTSTAMP_FILTER_SOME; + config->rx_filter = HWTSTAMP_FILTER_SOME; cfg0 = 0; - cfg2 |= PTP1_PORT_CONFIG_2_ARRINTEN; + if (ptp->phydev->irq >= 0) + cfg2 |= PTP1_PORT_CONFIG_2_ARRINTEN; break; default: @@ -508,15 +531,14 @@ if (err) return err; - ptp->tx_type = config.tx_type; - ptp->rx_filter = config.rx_filter; + ptp->tx_type = config->tx_type; + ptp->rx_filter = config->rx_filter; - return copy_to_user(ifreq->ifr_data, &config, sizeof(config)) ? - -EFAULT : 0; + return 0; } static int marvell_ptp_ts_info(struct mii_timestamper *mii_ts, - struct ethtool_ts_info *ts_info) + struct kernel_ethtool_ts_info *ts_info) { struct marvell_ptp *ptp = mii_ts_to_ptp(mii_ts); @@ -559,6 +581,14 @@ if (err < 0) return err; + /* Timestamp only sync (MessageID 1) and delay_req (MessageID 2) + * PTP frames + */ + err = phy_write_paged(phydev, MARVELL_PAGE_PTP_GLOBAL_CONFIG, + PTP_GLOBAL_CONFIG1, 0x3); + if (err < 0) + return err; + /* Disable all interrupts */ phy_write_paged(phydev, MARVELL_PAGE_PTP_PORT_1, PTP1_PORT_CONFIG_2, 0); @@ -577,10 +607,11 @@ PTP1_PORT_CONFIG_2, 0); } -/* This function should be called from the PHY threaded interrupt - * handler to process any stored timestamps in a timely manner. - * The presence of an interrupt has an effect on how quickly a - * timestamp requiring received packet will be processed. +/** + * marvell_ptp_irq - PTP IRQ handler + * @phydev: Pointer to phy_device structure + * + * Return: IRQ_HANDLED/IRQ_NONE */ irqreturn_t marvell_ptp_irq(struct phy_device *phydev) { @@ -606,11 +637,11 @@ struct marvell_ptp *ptp = container_of(work, struct marvell_ptp, ts_work.work); - marvell_ptp_rx_ts(ptp); - if (ptp->tx_skb) marvell_ptp_txtstamp_complete(ptp); + marvell_ptp_rx_ts(ptp); + marvell_ptp_rx_expire(ptp); if (!skb_queue_empty(&ptp->rx_queue) || ptp->tx_skb) @@ -632,7 +663,9 @@ ptp->mii_ts.hwtstamp = marvell_ptp_hwtstamp; ptp->mii_ts.ts_info = marvell_ptp_ts_info; - INIT_DELAYED_WORK(&ptp->ts_work, marvell_ptp_worker); + /* No phy interrupt */ + if (phydev->irq < 0) + INIT_DELAYED_WORK(&ptp->ts_work, marvell_ptp_worker); mutex_init(&ptp->rx_mutex); INIT_LIST_HEAD(&ptp->rx_free); INIT_LIST_HEAD(&ptp->rx_pend); @@ -672,7 +705,8 @@ ptp = mii_ts_to_ptp(phydev->mii_ts); phydev->mii_ts = NULL; - cancel_delayed_work_sync(&ptp->ts_work); + if (phydev->irq < 0) + cancel_delayed_work_sync(&ptp->ts_work); /* Free or dequeue all pending skbs */ if (ptp->tx_skb) @@ -688,4 +722,4 @@ MODULE_AUTHOR("Russell King"); MODULE_DESCRIPTION("Marvell PHY PTP library"); -MODULE_LICENSE("GPL v2"); +MODULE_LICENSE("GPL");