diff options
Diffstat (limited to 'drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c')
| -rw-r--r-- | drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c | 109 |
1 files changed, 82 insertions, 27 deletions
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c index 8b50f03056b7..bb110124f21e 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_hwtstamp.c @@ -18,9 +18,22 @@ #include "dwmac4.h" #include "stmmac.h" +#define STMMAC_HWTS_CFG_MASK (PTP_TCR_TSENA | PTP_TCR_TSCFUPDT | \ + PTP_TCR_TSINIT | PTP_TCR_TSUPDT | \ + PTP_TCR_TSCTRLSSR | PTP_TCR_SNAPTYPSEL_1 | \ + PTP_TCR_TSIPV4ENA | PTP_TCR_TSIPV6ENA | \ + PTP_TCR_TSEVNTENA | PTP_TCR_TSMSTRENA | \ + PTP_TCR_TSVER2ENA | PTP_TCR_TSIPENA | \ + PTP_TCR_TSTRIG | PTP_TCR_TSENALL) + static void config_hw_tstamping(void __iomem *ioaddr, u32 data) { - writel(data, ioaddr + PTP_TCR); + u32 regval = readl(ioaddr + PTP_TCR); + + regval &= ~STMMAC_HWTS_CFG_MASK; + regval |= data; + + writel(regval, ioaddr + PTP_TCR); } static void config_sub_second_increment(void __iomem *ioaddr, @@ -60,6 +73,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; @@ -80,7 +135,6 @@ static int init_systime(void __iomem *ioaddr, u32 sec, u32 nsec) static int config_addend(void __iomem *ioaddr, u32 addend) { u32 value; - int limit; writel(addend, ioaddr + PTP_TAR); /* issue command to update the addend value */ @@ -89,23 +143,15 @@ static int config_addend(void __iomem *ioaddr, u32 addend) writel(value, ioaddr + PTP_TCR); /* wait for present addend update to complete */ - limit = 10; - while (limit--) { - if (!(readl(ioaddr + PTP_TCR) & PTP_TCR_TSADDREG)) - break; - mdelay(10); - } - if (limit < 0) - return -EBUSY; - - return 0; + return readl_poll_timeout_atomic(ioaddr + PTP_TCR, value, + !(value & PTP_TCR_TSADDREG), + 10, 100000); } static int adjust_systime(void __iomem *ioaddr, u32 sec, u32 nsec, int add_sub, int gmac4) { u32 value; - int limit; if (add_sub) { /* If the new sec value needs to be subtracted with @@ -132,16 +178,9 @@ static int adjust_systime(void __iomem *ioaddr, u32 sec, u32 nsec, writel(value, ioaddr + PTP_TCR); /* wait for present system time adjust/update to complete */ - limit = 10; - while (limit--) { - if (!(readl(ioaddr + PTP_TCR) & PTP_TCR_TSUPDT)) - break; - mdelay(10); - } - if (limit < 0) - return -EBUSY; - - return 0; + return readl_poll_timeout_atomic(ioaddr + PTP_TCR, value, + !(value & PTP_TCR_TSUPDT), + 10, 100000); } static void get_systime(void __iomem *ioaddr, u64 *systime) @@ -167,7 +206,7 @@ static void get_ptptime(void __iomem *ptpaddr, u64 *ptp_time) u64 ns; ns = readl(ptpaddr + PTP_ATNR); - ns += readl(ptpaddr + PTP_ATSR) * NSEC_PER_SEC; + ns += (u64)readl(ptpaddr + PTP_ATSR) * NSEC_PER_SEC; *ptp_time = ns; } @@ -176,11 +215,12 @@ static void timestamp_interrupt(struct stmmac_priv *priv) { u32 num_snapshot, ts_status, tsync_int; struct ptp_clock_event event; + u32 acr_value, channel; unsigned long flags; u64 ptp_time; int i; - if (priv->plat->int_snapshot_en) { + if (priv->plat->flags & STMMAC_FLAG_INT_SNAPSHOT_EN) { wake_up(&priv->tstamp_busy_wait); return; } @@ -195,18 +235,21 @@ static void timestamp_interrupt(struct stmmac_priv *priv) */ ts_status = readl(priv->ioaddr + GMAC_TIMESTAMP_STATUS); - if (!priv->plat->ext_snapshot_en) + if (!(priv->plat->flags & STMMAC_FLAG_EXT_SNAPSHOT_EN)) return; num_snapshot = (ts_status & GMAC_TIMESTAMP_ATSNS_MASK) >> GMAC_TIMESTAMP_ATSNS_SHIFT; + acr_value = readl(priv->ptpaddr + PTP_ACR); + channel = ilog2(FIELD_GET(PTP_ACR_MASK, acr_value)); + for (i = 0; i < num_snapshot; i++) { read_lock_irqsave(&priv->ptp_lock, flags); get_ptptime(priv->ptpaddr, &ptp_time); read_unlock_irqrestore(&priv->ptp_lock, flags); event.type = PTP_CLOCK_EXTTS; - event.index = 0; + event.index = channel; event.timestamp = ptp_time; ptp_clock_event(priv->ptp_clock, &event); } @@ -221,4 +264,16 @@ const struct stmmac_hwtimestamp stmmac_ptp = { .get_systime = get_systime, .get_ptptime = get_ptptime, .timestamp_interrupt = timestamp_interrupt, + .hwtstamp_correct_latency = hwtstamp_correct_latency, +}; + +const struct stmmac_hwtimestamp dwmac1000_ptp = { + .config_hw_tstamping = config_hw_tstamping, + .init_systime = init_systime, + .config_sub_second_increment = config_sub_second_increment, + .config_addend = config_addend, + .adjust_systime = adjust_systime, + .get_systime = get_systime, + .get_ptptime = dwmac1000_get_ptptime, + .timestamp_interrupt = dwmac1000_timestamp_interrupt, }; |