diff mbox series

[net-next,2/3] net: lan966x: Allow using PCH extension for PTP

Message ID 20240212173307.1124120-3-maxime.chevallier@bootlin.com (mailing list archive)
State Changes Requested
Delegated to: Netdev Maintainers
Headers show
Series Introduce support for USGMII Inband Extensions | expand

Checks

Context Check Description
netdev/series_format success Posting correctly formatted
netdev/tree_selection success Clearly marked for net-next
netdev/ynl success Generated files up to date; no warnings/errors; no diff in generated;
netdev/fixes_present success Fixes tag not required for -next series
netdev/header_inline success No static functions without inline keyword in header files
netdev/build_32bit success Errors and warnings before: 989 this patch: 989
netdev/build_tools success No tools touched, skip
netdev/cc_maintainers success CCed 7 of 7 maintainers
netdev/build_clang success Errors and warnings before: 1006 this patch: 1006
netdev/verify_signedoff success Signed-off-by tag matches author and committer
netdev/deprecated_api success None detected
netdev/check_selftest success No net selftest shell script
netdev/verify_fixes success No Fixes tag
netdev/build_allmodconfig_warn success Errors and warnings before: 1006 this patch: 1006
netdev/checkpatch warning WARNING: line length of 83 exceeds 80 columns WARNING: line length of 85 exceeds 80 columns WARNING: line length of 86 exceeds 80 columns WARNING: line length of 87 exceeds 80 columns WARNING: line length of 90 exceeds 80 columns
netdev/build_clang_rust success No Rust files in patch. Skipping build
netdev/kdoc success Errors and warnings before: 0 this patch: 0
netdev/source_inline success Was 0 now: 0

Commit Message

Maxime Chevallier Feb. 12, 2024, 5:33 p.m. UTC
This commit adds the logic to use the PCH mode for timestamp passing in
ingress traffic for PTP. The tricky part is that we need to configure
both the PHY and the MAC, since the MAC will extract the timetsamp
reported by the PHY from the preamble and set it into the SKB.

Note that with the PCH mode, we only get the RX timestamp that way, the
TX timestamps are still reported OOB by the PHY.

Note that only the nanoseconds part is extracted from the PCH extension,
since there's not enough room in the 4 bytes extension to pass a full
80bits timestamp. The seconds part of the timestamp still needs to get
retrieved from the PHY using an out-of-band mechanism.

Signed-off-by: Maxime Chevallier <maxime.chevallier@bootlin.com>
---
 .../ethernet/microchip/lan966x/lan966x_main.h |  1 +
 .../ethernet/microchip/lan966x/lan966x_port.c | 12 +++
 .../ethernet/microchip/lan966x/lan966x_ptp.c  | 94 +++++++++++++++++--
 .../ethernet/microchip/lan966x/lan966x_regs.h | 64 +++++++++++++
 4 files changed, 161 insertions(+), 10 deletions(-)

Comments

Horatiu Vultur Feb. 13, 2024, 10:31 a.m. UTC | #1
The 02/12/2024 18:33, Maxime Chevallier wrote:
> [Some people who received this message don't often get email from maxime.chevallier@bootlin.com. Learn why this is important at https://aka.ms/LearnAboutSenderIdentification ]

Hi Maxime,

I have tried your patches on pcb8291, which is a lan966x without PHYs
that support timestamping. And on this platform this patch breaks up the
things. Because it should just do the timestamping the MAC in that case,
but with this patch it doesn't get any time.
The same issue can be reproduced on pcb8280 and then disable PHY
timestamping, or change the lan8814 not to support HW timestamping.

Please see bellow the reason why.

> 
> +/* Enable or disable PCH timestamp transmission. This uses the USGMII PCH
> + * extensions to transmit the timestamps in the frame preamble.
> + */
> +static void lan966x_ptp_pch_configure(struct lan966x_port *port, bool *enable)
> +{
> +       struct phy_device *phydev = port->dev->phydev;
> +       int ret;
> +
> +       if (!phydev)
> +               *enable = false;
> +
> +       if (*enable) {
> +               /* If we cannot enable inband PCH mode, we fallback to classic
> +                * timestamping
> +                */
> +               if (phy_inband_ext_available(phydev, PHY_INBAND_EXT_PCH_TIMESTAMP)) {
> +                       ret = phy_inband_ext_enable(phydev, PHY_INBAND_EXT_PCH_TIMESTAMP);
> +                       if (ret)
> +                               *enable = false;
> +               } else {
> +                       *enable = false;
> +               }
> +       } else {
> +               phy_inband_ext_disable(phydev, PHY_INBAND_EXT_PCH_TIMESTAMP);
> +       }
> +
> +       lan_rmw(SYS_PCH_CFG_PCH_SUB_PORT_ID_SET(port->chip_port % 4) |
> +               SYS_PCH_CFG_PCH_TX_MODE_SET(*enable) |
> +               SYS_PCH_CFG_PCH_RX_MODE_SET(*enable),
> +               SYS_PCH_CFG_PCH_SUB_PORT_ID |
> +               SYS_PCH_CFG_PCH_TX_MODE |
> +               SYS_PCH_CFG_PCH_RX_MODE,
> +               port->lan966x, SYS_PCH_CFG(port->chip_port));
> +}
> +
>  int lan966x_ptp_hwtstamp_set(struct lan966x_port *port,
>                              struct kernel_hwtstamp_config *cfg,
>                              struct netlink_ext_ack *extack)
>  {
>         struct lan966x *lan966x = port->lan966x;
> +       bool timestamp_in_pch = false;
>         struct lan966x_phc *phc;
> 
>         switch (cfg->tx_type) {
> @@ -303,10 +339,18 @@ int lan966x_ptp_hwtstamp_set(struct lan966x_port *port,
>                 return -ERANGE;
>         }
> 
> +       if (cfg->source == HWTSTAMP_SOURCE_PHYLIB &&
> +           cfg->tx_type == HWTSTAMP_TX_ON &&
> +           port->config.portmode == PHY_INTERFACE_MODE_QUSGMII)
> +               timestamp_in_pch = true;
> +
> +       lan966x_ptp_pch_configure(port, &timestamp_in_pch);
> +
>         /* Commit back the result & save it */
>         mutex_lock(&lan966x->ptp_lock);
>         phc = &lan966x->phc[LAN966X_PHC_PORT];
>         phc->hwtstamp_config = *cfg;
> +       phc->pch = timestamp_in_pch;

Here we figure out if pch is enabled or not. If the cfg->source is not
PHYLIB or the interface is not QUSGMII then timestamp_in_pch will stay
false.

>         mutex_unlock(&lan966x->ptp_lock);
> 
>         return 0;
> @@ -397,6 +441,7 @@ int lan966x_ptp_txtstamp_request(struct lan966x_port *port,
>         LAN966X_SKB_CB(skb)->jiffies = jiffies;
> 
>         lan966x->ptp_skbs++;
> +

I think this is just a small style change. So maybe it shouldn't be in
here.

>         port->ts_id++;
>         if (port->ts_id == LAN966X_MAX_PTP_ID)
>                 port->ts_id = 0;
> @@ -500,6 +545,27 @@ irqreturn_t lan966x_ptp_irq_handler(int irq, void *args)
>                 /* Read RX timestamping to get the ID */
>                 id = lan_rd(lan966x, PTP_TWOSTEP_STAMP);
> 
> +               /* If PCH is enabled, there is a "feature" that also the MAC
> +                * will generate an interrupt for transmitted frames. This
> +                * interrupt should be ignored, so clear the allocated resources
> +                * and try to get the next timestamp. Maybe should clean the
> +                * resources on the TX side?
> +                */
> +               if (phy_inband_ext_enabled(port->dev->phydev,
> +                                          PHY_INBAND_EXT_PCH_TIMESTAMP)) {
> +                       spin_lock(&lan966x->ptp_ts_id_lock);
> +                       lan966x->ptp_skbs--;
> +                       spin_unlock(&lan966x->ptp_ts_id_lock);
> +
> +                       dev_kfree_skb_any(skb_match);
> +
> +                       lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1),
> +                               PTP_TWOSTEP_CTRL_NXT,
> +                               lan966x, PTP_TWOSTEP_CTRL);
> +
> +                       continue;
> +               }
> +
>                 spin_lock_irqsave(&port->tx_skbs.lock, flags);
>                 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
>                         if (LAN966X_SKB_CB(skb)->ts_id != id)
> @@ -1088,19 +1154,27 @@ void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb,
>         struct timespec64 ts;
>         u64 full_ts_in_ns;
> 
> +       phc = &lan966x->phc[LAN966X_PHC_PORT];
> +
>         if (!lan966x->ptp ||
> -           !lan966x->ports[src_port]->ptp_rx_cmd)
> +           !lan966x->ports[src_port]->ptp_rx_cmd ||
> +           !phc->pch)

And here because phc->pch is false, it would just return.
Meaning that it would never be able to get the time.
I presume that this check should not be modified.

>                 return;
> 
> -       phc = &lan966x->phc[LAN966X_PHC_PORT];
> -       lan966x_ptp_gettime64(&phc->info, &ts);
> -
> -       /* Drop the sub-ns precision */
> -       timestamp = timestamp >> 2;
> -       if (ts.tv_nsec < timestamp)
> -               ts.tv_sec--;
> -       ts.tv_nsec = timestamp;
> -       full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
> +       if (phc->pch) {
> +               /* Drop the sub-ns precision */
> +               timestamp = timestamp >> 2;
> +               full_ts_in_ns = lower_32_bits(timestamp);
> +       } else {
> +               lan966x_ptp_gettime64(&phc->info, &ts);
> +
> +               /* Drop the sub-ns precision */
> +               timestamp = timestamp >> 2;
> +               if (ts.tv_nsec < timestamp)
> +                       ts.tv_sec--;
> +               ts.tv_nsec = timestamp;
> +               full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
> +       }
Maxime Chevallier Feb. 13, 2024, 3:57 p.m. UTC | #2
Hello Horatiu,

On Tue, 13 Feb 2024 11:31:56 +0100
Horatiu Vultur <horatiu.vultur@microchip.com> wrote:

> The 02/12/2024 18:33, Maxime Chevallier wrote:
> > [Some people who received this message don't often get email from maxime.chevallier@bootlin.com. Learn why this is important at https://aka.ms/LearnAboutSenderIdentification ]  
> 
> Hi Maxime,
> 
> I have tried your patches on pcb8291, which is a lan966x without PHYs
> that support timestamping. And on this platform this patch breaks up the
> things. Because it should just do the timestamping the MAC in that case,
> but with this patch it doesn't get any time.
> The same issue can be reproduced on pcb8280 and then disable PHY
> timestamping, or change the lan8814 not to support HW timestamping.
> 
> Please see bellow the reason why.

You are entirely correct and I apparently messed-up my series as these
changes were implemented locally and somehow lost in the rebase. Indeed
this codes doesn't work at all... I'll resend that, thanks a lot for
the test and sorry !

> 
> > 
> > +/* Enable or disable PCH timestamp transmission. This uses the USGMII PCH
> > + * extensions to transmit the timestamps in the frame preamble.
> > + */
> > +static void lan966x_ptp_pch_configure(struct lan966x_port *port, bool *enable)
> > +{
> > +       struct phy_device *phydev = port->dev->phydev;
> > +       int ret;
> > +
> > +       if (!phydev)
> > +               *enable = false;
> > +
> > +       if (*enable) {
> > +               /* If we cannot enable inband PCH mode, we fallback to classic
> > +                * timestamping
> > +                */
> > +               if (phy_inband_ext_available(phydev, PHY_INBAND_EXT_PCH_TIMESTAMP)) {
> > +                       ret = phy_inband_ext_enable(phydev, PHY_INBAND_EXT_PCH_TIMESTAMP);
> > +                       if (ret)
> > +                               *enable = false;
> > +               } else {
> > +                       *enable = false;
> > +               }
> > +       } else {
> > +               phy_inband_ext_disable(phydev, PHY_INBAND_EXT_PCH_TIMESTAMP);
> > +       }
> > +
> > +       lan_rmw(SYS_PCH_CFG_PCH_SUB_PORT_ID_SET(port->chip_port % 4) |
> > +               SYS_PCH_CFG_PCH_TX_MODE_SET(*enable) |
> > +               SYS_PCH_CFG_PCH_RX_MODE_SET(*enable),
> > +               SYS_PCH_CFG_PCH_SUB_PORT_ID |
> > +               SYS_PCH_CFG_PCH_TX_MODE |
> > +               SYS_PCH_CFG_PCH_RX_MODE,
> > +               port->lan966x, SYS_PCH_CFG(port->chip_port));
> > +}
> > +
> >  int lan966x_ptp_hwtstamp_set(struct lan966x_port *port,
> >                              struct kernel_hwtstamp_config *cfg,
> >                              struct netlink_ext_ack *extack)
> >  {
> >         struct lan966x *lan966x = port->lan966x;
> > +       bool timestamp_in_pch = false;
> >         struct lan966x_phc *phc;
> > 
> >         switch (cfg->tx_type) {
> > @@ -303,10 +339,18 @@ int lan966x_ptp_hwtstamp_set(struct lan966x_port *port,
> >                 return -ERANGE;
> >         }
> > 
> > +       if (cfg->source == HWTSTAMP_SOURCE_PHYLIB &&
> > +           cfg->tx_type == HWTSTAMP_TX_ON &&
> > +           port->config.portmode == PHY_INTERFACE_MODE_QUSGMII)
> > +               timestamp_in_pch = true;
> > +
> > +       lan966x_ptp_pch_configure(port, &timestamp_in_pch);
> > +
> >         /* Commit back the result & save it */
> >         mutex_lock(&lan966x->ptp_lock);
> >         phc = &lan966x->phc[LAN966X_PHC_PORT];
> >         phc->hwtstamp_config = *cfg;
> > +       phc->pch = timestamp_in_pch;  
> 
> Here we figure out if pch is enabled or not. If the cfg->source is not
> PHYLIB or the interface is not QUSGMII then timestamp_in_pch will stay
> false.
> 
> >         mutex_unlock(&lan966x->ptp_lock);
> > 
> >         return 0;
> > @@ -397,6 +441,7 @@ int lan966x_ptp_txtstamp_request(struct lan966x_port *port,
> >         LAN966X_SKB_CB(skb)->jiffies = jiffies;
> > 
> >         lan966x->ptp_skbs++;
> > +  
> 
> I think this is just a small style change. So maybe it shouldn't be in
> here.
> 
> >         port->ts_id++;
> >         if (port->ts_id == LAN966X_MAX_PTP_ID)
> >                 port->ts_id = 0;
> > @@ -500,6 +545,27 @@ irqreturn_t lan966x_ptp_irq_handler(int irq, void *args)
> >                 /* Read RX timestamping to get the ID */
> >                 id = lan_rd(lan966x, PTP_TWOSTEP_STAMP);
> > 
> > +               /* If PCH is enabled, there is a "feature" that also the MAC
> > +                * will generate an interrupt for transmitted frames. This
> > +                * interrupt should be ignored, so clear the allocated resources
> > +                * and try to get the next timestamp. Maybe should clean the
> > +                * resources on the TX side?
> > +                */
> > +               if (phy_inband_ext_enabled(port->dev->phydev,
> > +                                          PHY_INBAND_EXT_PCH_TIMESTAMP)) {
> > +                       spin_lock(&lan966x->ptp_ts_id_lock);
> > +                       lan966x->ptp_skbs--;
> > +                       spin_unlock(&lan966x->ptp_ts_id_lock);
> > +
> > +                       dev_kfree_skb_any(skb_match);
> > +
> > +                       lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1),
> > +                               PTP_TWOSTEP_CTRL_NXT,
> > +                               lan966x, PTP_TWOSTEP_CTRL);
> > +
> > +                       continue;
> > +               }
> > +
> >                 spin_lock_irqsave(&port->tx_skbs.lock, flags);
> >                 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
> >                         if (LAN966X_SKB_CB(skb)->ts_id != id)
> > @@ -1088,19 +1154,27 @@ void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb,
> >         struct timespec64 ts;
> >         u64 full_ts_in_ns;
> > 
> > +       phc = &lan966x->phc[LAN966X_PHC_PORT];
> > +
> >         if (!lan966x->ptp ||
> > -           !lan966x->ports[src_port]->ptp_rx_cmd)
> > +           !lan966x->ports[src_port]->ptp_rx_cmd ||
> > +           !phc->pch)  
> 
> And here because phc->pch is false, it would just return.
> Meaning that it would never be able to get the time.
> I presume that this check should not be modified.

Dammit you are right and I had these modifications locally, but
apparently I messed my rebase and lost that... 

> 
> >                 return;
> > 
> > -       phc = &lan966x->phc[LAN966X_PHC_PORT];
> > -       lan966x_ptp_gettime64(&phc->info, &ts);
> > -
> > -       /* Drop the sub-ns precision */
> > -       timestamp = timestamp >> 2;
> > -       if (ts.tv_nsec < timestamp)
> > -               ts.tv_sec--;
> > -       ts.tv_nsec = timestamp;
> > -       full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
> > +       if (phc->pch) {
> > +               /* Drop the sub-ns precision */
> > +               timestamp = timestamp >> 2;
> > +               full_ts_in_ns = lower_32_bits(timestamp);
> > +       } else {
> > +               lan966x_ptp_gettime64(&phc->info, &ts);
> > +
> > +               /* Drop the sub-ns precision */
> > +               timestamp = timestamp >> 2;
> > +               if (ts.tv_nsec < timestamp)
> > +                       ts.tv_sec--;
> > +               ts.tv_nsec = timestamp;
> > +               full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
> > +       }  
>  
> 


Thanks for the review and analysis Horatiu, and sorry for this hiccup !

Maxime
diff mbox series

Patch

diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_main.h b/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
index caa9e0533c96..0aca85be02ac 100644
--- a/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
+++ b/drivers/net/ethernet/microchip/lan966x/lan966x_main.h
@@ -302,6 +302,7 @@  struct lan966x_phc {
 	struct kernel_hwtstamp_config hwtstamp_config;
 	struct lan966x *lan966x;
 	u8 index;
+	bool pch;
 };
 
 struct lan966x_skb_cb {
diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_port.c b/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
index 2e83bbb9477e..cf432d797ce6 100644
--- a/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
+++ b/drivers/net/ethernet/microchip/lan966x/lan966x_port.c
@@ -363,6 +363,18 @@  int lan966x_port_pcs_set(struct lan966x_port *port,
 		DEV_PCS1G_MODE_CFG_SAVE_PREAMBLE_ENA,
 		lan966x, DEV_PCS1G_MODE_CFG(port->chip_port));
 
+	if (full_preamble) {
+		lan_rmw(DEV_ENABLE_CONFIG_MM_TX_ENA_SET(1) |
+			DEV_ENABLE_CONFIG_MM_RX_ENA_SET(1),
+			DEV_ENABLE_CONFIG_MM_TX_ENA |
+			DEV_ENABLE_CONFIG_MM_RX_ENA,
+			lan966x, DEV_ENABLE_CONFIG(port->chip_port));
+
+		lan_rmw(SYS_PTP_MODE_CFG_PTP_MODE_VAL_SET(1),
+			SYS_PTP_MODE_CFG_PTP_MODE_VAL,
+			lan966x, SYS_PTP_MODE_CFG(port->chip_port, 0));
+	}
+
 	/* Enable PCS */
 	lan_wr(DEV_PCS1G_CFG_PCS_ENA_SET(1),
 	       lan966x, DEV_PCS1G_CFG(port->chip_port));
diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_ptp.c b/drivers/net/ethernet/microchip/lan966x/lan966x_ptp.c
index 63905bb5a63a..4d8c865eda90 100644
--- a/drivers/net/ethernet/microchip/lan966x/lan966x_ptp.c
+++ b/drivers/net/ethernet/microchip/lan966x/lan966x_ptp.c
@@ -257,11 +257,47 @@  int lan966x_ptp_setup_traps(struct lan966x_port *port,
 		return lan966x_ptp_add_traps(port);
 }
 
+/* Enable or disable PCH timestamp transmission. This uses the USGMII PCH
+ * extensions to transmit the timestamps in the frame preamble.
+ */
+static void lan966x_ptp_pch_configure(struct lan966x_port *port, bool *enable)
+{
+	struct phy_device *phydev = port->dev->phydev;
+	int ret;
+
+	if (!phydev)
+		*enable = false;
+
+	if (*enable) {
+		/* If we cannot enable inband PCH mode, we fallback to classic
+		 * timestamping
+		 */
+		if (phy_inband_ext_available(phydev, PHY_INBAND_EXT_PCH_TIMESTAMP)) {
+			ret = phy_inband_ext_enable(phydev, PHY_INBAND_EXT_PCH_TIMESTAMP);
+			if (ret)
+				*enable = false;
+		} else {
+			*enable = false;
+		}
+	} else {
+		phy_inband_ext_disable(phydev, PHY_INBAND_EXT_PCH_TIMESTAMP);
+	}
+
+	lan_rmw(SYS_PCH_CFG_PCH_SUB_PORT_ID_SET(port->chip_port % 4) |
+		SYS_PCH_CFG_PCH_TX_MODE_SET(*enable) |
+		SYS_PCH_CFG_PCH_RX_MODE_SET(*enable),
+		SYS_PCH_CFG_PCH_SUB_PORT_ID |
+		SYS_PCH_CFG_PCH_TX_MODE |
+		SYS_PCH_CFG_PCH_RX_MODE,
+		port->lan966x, SYS_PCH_CFG(port->chip_port));
+}
+
 int lan966x_ptp_hwtstamp_set(struct lan966x_port *port,
 			     struct kernel_hwtstamp_config *cfg,
 			     struct netlink_ext_ack *extack)
 {
 	struct lan966x *lan966x = port->lan966x;
+	bool timestamp_in_pch = false;
 	struct lan966x_phc *phc;
 
 	switch (cfg->tx_type) {
@@ -303,10 +339,18 @@  int lan966x_ptp_hwtstamp_set(struct lan966x_port *port,
 		return -ERANGE;
 	}
 
+	if (cfg->source == HWTSTAMP_SOURCE_PHYLIB &&
+	    cfg->tx_type == HWTSTAMP_TX_ON &&
+	    port->config.portmode == PHY_INTERFACE_MODE_QUSGMII)
+		timestamp_in_pch = true;
+
+	lan966x_ptp_pch_configure(port, &timestamp_in_pch);
+
 	/* Commit back the result & save it */
 	mutex_lock(&lan966x->ptp_lock);
 	phc = &lan966x->phc[LAN966X_PHC_PORT];
 	phc->hwtstamp_config = *cfg;
+	phc->pch = timestamp_in_pch;
 	mutex_unlock(&lan966x->ptp_lock);
 
 	return 0;
@@ -397,6 +441,7 @@  int lan966x_ptp_txtstamp_request(struct lan966x_port *port,
 	LAN966X_SKB_CB(skb)->jiffies = jiffies;
 
 	lan966x->ptp_skbs++;
+
 	port->ts_id++;
 	if (port->ts_id == LAN966X_MAX_PTP_ID)
 		port->ts_id = 0;
@@ -500,6 +545,27 @@  irqreturn_t lan966x_ptp_irq_handler(int irq, void *args)
 		/* Read RX timestamping to get the ID */
 		id = lan_rd(lan966x, PTP_TWOSTEP_STAMP);
 
+		/* If PCH is enabled, there is a "feature" that also the MAC
+		 * will generate an interrupt for transmitted frames. This
+		 * interrupt should be ignored, so clear the allocated resources
+		 * and try to get the next timestamp. Maybe should clean the
+		 * resources on the TX side?
+		 */
+		if (phy_inband_ext_enabled(port->dev->phydev,
+					   PHY_INBAND_EXT_PCH_TIMESTAMP)) {
+			spin_lock(&lan966x->ptp_ts_id_lock);
+			lan966x->ptp_skbs--;
+			spin_unlock(&lan966x->ptp_ts_id_lock);
+
+			dev_kfree_skb_any(skb_match);
+
+			lan_rmw(PTP_TWOSTEP_CTRL_NXT_SET(1),
+				PTP_TWOSTEP_CTRL_NXT,
+				lan966x, PTP_TWOSTEP_CTRL);
+
+			continue;
+		}
+
 		spin_lock_irqsave(&port->tx_skbs.lock, flags);
 		skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
 			if (LAN966X_SKB_CB(skb)->ts_id != id)
@@ -1088,19 +1154,27 @@  void lan966x_ptp_rxtstamp(struct lan966x *lan966x, struct sk_buff *skb,
 	struct timespec64 ts;
 	u64 full_ts_in_ns;
 
+	phc = &lan966x->phc[LAN966X_PHC_PORT];
+
 	if (!lan966x->ptp ||
-	    !lan966x->ports[src_port]->ptp_rx_cmd)
+	    !lan966x->ports[src_port]->ptp_rx_cmd ||
+	    !phc->pch)
 		return;
 
-	phc = &lan966x->phc[LAN966X_PHC_PORT];
-	lan966x_ptp_gettime64(&phc->info, &ts);
-
-	/* Drop the sub-ns precision */
-	timestamp = timestamp >> 2;
-	if (ts.tv_nsec < timestamp)
-		ts.tv_sec--;
-	ts.tv_nsec = timestamp;
-	full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
+	if (phc->pch) {
+		/* Drop the sub-ns precision */
+		timestamp = timestamp >> 2;
+		full_ts_in_ns = lower_32_bits(timestamp);
+	} else {
+		lan966x_ptp_gettime64(&phc->info, &ts);
+
+		/* Drop the sub-ns precision */
+		timestamp = timestamp >> 2;
+		if (ts.tv_nsec < timestamp)
+			ts.tv_sec--;
+		ts.tv_nsec = timestamp;
+		full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
+	}
 
 	shhwtstamps = skb_hwtstamps(skb);
 	shhwtstamps->hwtstamp = full_ts_in_ns;
diff --git a/drivers/net/ethernet/microchip/lan966x/lan966x_regs.h b/drivers/net/ethernet/microchip/lan966x/lan966x_regs.h
index 4b553927d2e0..bf4df025925e 100644
--- a/drivers/net/ethernet/microchip/lan966x/lan966x_regs.h
+++ b/drivers/net/ethernet/microchip/lan966x/lan966x_regs.h
@@ -937,6 +937,27 @@  enum lan966x_target {
 #define DEV_PCS1G_STICKY_LINK_DOWN_STICKY_GET(x)\
 	FIELD_GET(DEV_PCS1G_STICKY_LINK_DOWN_STICKY, x)
 
+/*      DEV:MM_CONFIG:ENABLE_CONFIG */
+#define DEV_ENABLE_CONFIG(t)      __REG(TARGET_DEV, t, 8, 156, 0, 1, 8, 0, 0, 1, 4)
+
+#define DEV_ENABLE_CONFIG_KEEP_S_AFTER_D         BIT(8)
+#define DEV_ENABLE_CONFIG_KEEP_S_AFTER_D_SET(x)\
+	FIELD_PREP(DEV_ENABLE_CONFIG_KEEP_S_AFTER_D, x)
+#define DEV_ENABLE_CONFIG_KEEP_S_AFTER_D_GET(x)\
+	FIELD_GET(DEV_ENABLE_CONFIG_KEEP_S_AFTER_D, x)
+
+#define DEV_ENABLE_CONFIG_MM_TX_ENA              BIT(4)
+#define DEV_ENABLE_CONFIG_MM_TX_ENA_SET(x)\
+	FIELD_PREP(DEV_ENABLE_CONFIG_MM_TX_ENA, x)
+#define DEV_ENABLE_CONFIG_MM_TX_ENA_GET(x)\
+	FIELD_GET(DEV_ENABLE_CONFIG_MM_TX_ENA, x)
+
+#define DEV_ENABLE_CONFIG_MM_RX_ENA              BIT(0)
+#define DEV_ENABLE_CONFIG_MM_RX_ENA_SET(x)\
+	FIELD_PREP(DEV_ENABLE_CONFIG_MM_RX_ENA, x)
+#define DEV_ENABLE_CONFIG_MM_RX_ENA_GET(x)\
+	FIELD_GET(DEV_ENABLE_CONFIG_MM_RX_ENA, x)
+
 /*      FDMA:FDMA:FDMA_CH_ACTIVATE */
 #define FDMA_CH_ACTIVATE          __REG(TARGET_FDMA, 0, 1, 8, 0, 1, 428, 0, 0, 1, 4)
 
@@ -1184,6 +1205,15 @@  enum lan966x_target {
 #define PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(x)\
 	FIELD_GET(PTP_TWOSTEP_STAMP_STAMP_NSEC, x)
 
+/*      SYS:PTPPORT:PTP_MODE_CFG */
+#define SYS_PTP_MODE_CFG(g, r)    __REG(TARGET_SYS, 0, 1, 4452, g, 10, 28, 20, r, 2, 4)
+
+#define SYS_PTP_MODE_CFG_PTP_MODE_VAL            GENMASK(1, 0)
+#define SYS_PTP_MODE_CFG_PTP_MODE_VAL_SET(x)\
+	FIELD_PREP(SYS_PTP_MODE_CFG_PTP_MODE_VAL, x)
+#define SYS_PTP_MODE_CFG_PTP_MODE_VAL_GET(x)\
+	FIELD_GET(SYS_PTP_MODE_CFG_PTP_MODE_VAL, x)
+
 /*      DEVCPU_QS:XTR:XTR_GRP_CFG */
 #define QS_XTR_GRP_CFG(r)         __REG(TARGET_QS, 0, 1, 0, 0, 1, 36, 0, r, 2, 4)
 
@@ -1623,6 +1653,20 @@  enum lan966x_target {
 	FIELD_PREP(REW_STAT_CFG_STAT_MODE, x)
 #define REW_STAT_CFG_STAT_MODE_GET(x)\
 	FIELD_GET(REW_STAT_CFG_STAT_MODE, x)
+/*      REW:PORT:PTP_MISC_CFG */
+#define REW_PTP_MISC_CFG(g)       __REG(TARGET_REW, 0, 1, 0, g, 10, 128, 80, 0, 1, 4)
+
+#define REW_PTP_MISC_CFG_PTP_RSRV_MOVEBACK       BIT(2)
+#define REW_PTP_MISC_CFG_PTP_RSRV_MOVEBACK_SET(x)\
+	FIELD_PREP(REW_PTP_MISC_CFG_PTP_RSRV_MOVEBACK, x)
+#define REW_PTP_MISC_CFG_PTP_RSRV_MOVEBACK_GET(x)\
+	FIELD_GET(REW_PTP_MISC_CFG_PTP_RSRV_MOVEBACK, x)
+
+#define REW_PTP_MISC_CFG_PTP_SIGNATURE_SEL       GENMASK(1, 0)
+#define REW_PTP_MISC_CFG_PTP_SIGNATURE_SEL_SET(x)\
+	FIELD_PREP(REW_PTP_MISC_CFG_PTP_SIGNATURE_SEL, x)
+#define REW_PTP_MISC_CFG_PTP_SIGNATURE_SEL_GET(x)\
+	FIELD_GET(REW_PTP_MISC_CFG_PTP_SIGNATURE_SEL, x)
 
 /*      SYS:SYSTEM:RESET_CFG */
 #define SYS_RESET_CFG             __REG(TARGET_SYS, 0, 1, 4128, 0, 1, 168, 0, 0, 1, 4)
@@ -1884,5 +1928,25 @@  enum lan966x_target {
 
 /*      VCAP:VCAP_CONST:IF_CNT */
 #define VCAP_IF_CNT(t)            __REG(TARGET_VCAP, t, 3, 924, 0, 1, 40, 36, 0, 1, 4)
+/*      SYS:PTPPORT:PCH_CFG */
+#define SYS_PCH_CFG(g)            __REG(TARGET_SYS, 0, 1, 4452, g, 10, 28, 0, 0, 1, 4)
+
+#define SYS_PCH_CFG_PCH_SUB_PORT_ID		GENMASK(10, 7)
+#define SYS_PCH_CFG_PCH_SUB_PORT_ID_SET(x)\
+	FIELD_PREP(SYS_PCH_CFG_PCH_SUB_PORT_ID, x)
+#define SYS_PCH_CFG_PCH_SUB_PORT_ID_GET(x)\
+	FIELD_GET(SYS_PCH_CFG_PCH_SUB_PORT_ID, x)
+
+#define SYS_PCH_CFG_PCH_TX_MODE			GENMASK(6, 5)
+#define SYS_PCH_CFG_PCH_TX_MODE_SET(x)\
+	FIELD_PREP(SYS_PCH_CFG_PCH_TX_MODE, x)
+#define SYS_PCH_CFG_PCH_TX_MODE_GET(x)\
+	FIELD_GET(SYS_MAC_FC_CFG_PAUSE_VAL_CFG, x)
+
+#define SYS_PCH_CFG_PCH_RX_MODE			GENMASK(4, 2)
+#define SYS_PCH_CFG_PCH_RX_MODE_SET(x)\
+	FIELD_PREP(SYS_PCH_CFG_PCH_RX_MODE, x)
+#define SYS_PCH_CFG_PCH_RX_MODE_GET(x)\
+	FIELD_GET(SYS_PCH_CFG_PCH_RX_MODE, x)
 
 #endif /* _LAN966X_REGS_H_ */