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_ */
