diff options
| -rw-r--r-- | drivers/net/ethernet/stmicro/stmmac/hwif.h | 3 | ||||
| -rw-r--r-- | drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c | 43 | ||||
| -rw-r--r-- | drivers/net/ethernet/stmicro/stmmac/stmmac_main.c | 6 | ||||
| -rw-r--r-- | drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h | 6 | ||||
| -rw-r--r-- | include/linux/stmmac.h | 1 |
5 files changed, 59 insertions, 0 deletions
diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h index 652af8f6e75f..238f17c50a1e 100644 --- a/drivers/net/ethernet/stmicro/stmmac/hwif.h +++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h @@ -532,6 +532,7 @@ struct stmmac_hwtimestamp { void (*get_systime) (void __iomem *ioaddr, u64 *systime); void (*get_ptptime)(void __iomem *ioaddr, u64 *ptp_time); void (*timestamp_interrupt)(struct stmmac_priv *priv); + void (*hwtstamp_correct_latency)(struct stmmac_priv *priv); }; #define stmmac_config_hw_tstamping(__priv, __args...) \ @@ -550,6 +551,8 @@ struct stmmac_hwtimestamp { stmmac_do_void_callback(__priv, ptp, get_ptptime, __args) #define stmmac_timestamp_interrupt(__priv, __args...) \ stmmac_do_void_callback(__priv, ptp, timestamp_interrupt, __args) +#define stmmac_hwtstamp_correct_latency(__priv, __args...) \ + stmmac_do_void_callback(__priv, ptp, hwtstamp_correct_latency, __args) struct stmmac_tx_queue; struct stmmac_rx_queue; diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c index fa2c3ba7e9fe..540f6a4ec0b8 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c @@ -60,6 +60,48 @@ static void config_sub_second_increment(void __iomem *ioaddr, *ssinc = data; } +static void hwtstamp_correct_latency(struct stmmac_priv *priv) +{ + void __iomem *ioaddr = priv->ptpaddr; + u32 reg_tsic, reg_tsicsns; + u32 reg_tsec, reg_tsecsns; + u64 scaled_ns; + u32 val; + + /* MAC-internal ingress latency */ + scaled_ns = readl(ioaddr + PTP_TS_INGR_LAT); + + /* See section 11.7.2.5.3.1 "Ingress Correction" on page 4001 of + * i.MX8MP Applications Processor Reference Manual Rev. 1, 06/2021 + */ + val = readl(ioaddr + PTP_TCR); + if (val & PTP_TCR_TSCTRLSSR) + /* nanoseconds field is in decimal format with granularity of 1ns/bit */ + scaled_ns = ((u64)NSEC_PER_SEC << 16) - scaled_ns; + else + /* nanoseconds field is in binary format with granularity of ~0.466ns/bit */ + scaled_ns = ((1ULL << 31) << 16) - + DIV_U64_ROUND_CLOSEST(scaled_ns * PSEC_PER_NSEC, 466U); + + reg_tsic = scaled_ns >> 16; + reg_tsicsns = scaled_ns & 0xff00; + + /* set bit 31 for 2's compliment */ + reg_tsic |= BIT(31); + + writel(reg_tsic, ioaddr + PTP_TS_INGR_CORR_NS); + writel(reg_tsicsns, ioaddr + PTP_TS_INGR_CORR_SNS); + + /* MAC-internal egress latency */ + scaled_ns = readl(ioaddr + PTP_TS_EGR_LAT); + + reg_tsec = scaled_ns >> 16; + reg_tsecsns = scaled_ns & 0xff00; + + writel(reg_tsec, ioaddr + PTP_TS_EGR_CORR_NS); + writel(reg_tsecsns, ioaddr + PTP_TS_EGR_CORR_SNS); +} + static int init_systime(void __iomem *ioaddr, u32 sec, u32 nsec) { u32 value; @@ -221,4 +263,5 @@ const struct stmmac_hwtimestamp stmmac_ptp = { .get_systime = get_systime, .get_ptptime = get_ptptime, .timestamp_interrupt = timestamp_interrupt, + .hwtstamp_correct_latency = hwtstamp_correct_latency, }; diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c index e1f1c034d325..99aa5360b3ff 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -909,6 +909,9 @@ static int stmmac_init_ptp(struct stmmac_priv *priv) priv->hwts_tx_en = 0; priv->hwts_rx_en = 0; + if (priv->plat->flags & STMMAC_FLAG_HWTSTAMP_CORRECT_LATENCY) + stmmac_hwtstamp_correct_latency(priv, priv); + return 0; } @@ -1094,6 +1097,9 @@ static void stmmac_mac_link_up(struct phylink_config *config, if (priv->dma_cap.fpesel) stmmac_fpe_link_state_handle(priv, true); + + if (priv->plat->flags & STMMAC_FLAG_HWTSTAMP_CORRECT_LATENCY) + stmmac_hwtstamp_correct_latency(priv, priv); } static const struct phylink_mac_ops stmmac_phylink_mac_ops = { diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h index bf619295d079..d1fe4b46f162 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h @@ -26,6 +26,12 @@ #define PTP_ACR 0x40 /* Auxiliary Control Reg */ #define PTP_ATNR 0x48 /* Auxiliary Timestamp - Nanoseconds Reg */ #define PTP_ATSR 0x4c /* Auxiliary Timestamp - Seconds Reg */ +#define PTP_TS_INGR_CORR_NS 0x58 /* Ingress timestamp correction nanoseconds */ +#define PTP_TS_EGR_CORR_NS 0x5C /* Egress timestamp correction nanoseconds*/ +#define PTP_TS_INGR_CORR_SNS 0x60 /* Ingress timestamp correction subnanoseconds */ +#define PTP_TS_EGR_CORR_SNS 0x64 /* Egress timestamp correction subnanoseconds */ +#define PTP_TS_INGR_LAT 0x68 /* MAC internal Ingress Latency */ +#define PTP_TS_EGR_LAT 0x6c /* MAC internal Egress Latency */ #define PTP_STNSUR_ADDSUB_SHIFT 31 #define PTP_DIGITAL_ROLLOVER_MODE 0x3B9ACA00 /* 10e9-1 ns */ diff --git a/include/linux/stmmac.h b/include/linux/stmmac.h index 3d0702510224..652404c03944 100644 --- a/include/linux/stmmac.h +++ b/include/linux/stmmac.h @@ -218,6 +218,7 @@ struct dwmac4_addrs { #define STMMAC_FLAG_INT_SNAPSHOT_EN BIT(9) #define STMMAC_FLAG_RX_CLK_RUNS_IN_LPI BIT(10) #define STMMAC_FLAG_EN_TX_LPI_CLOCKGATING BIT(11) +#define STMMAC_FLAG_HWTSTAMP_CORRECT_LATENCY BIT(12) struct plat_stmmacenet_data { int bus_id; |