Quelle  stmmac_hwtstamp.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0-only
/*******************************************************************************
  Copyright (C) 2013  Vayavya Labs Pvt Ltd

  This implements all the API for managing HW timestamp & PTP.


  Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
  Author: Giuseppe Cavallaro <peppe.cavallaro@st.com>
*******************************************************************************/

#include <linux/io.h>
#include <linux/iopoll.h>
#include <linux/delay.h>
#include <linux/ptp_clock_kernel.h>
#include "common.h"
#include "stmmac_ptp.h"
#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)
{
 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,
  u32 ptp_clock, int gmac4, u32 *ssinc)
{
 u32 value = readl(ioaddr + PTP_TCR);
 unsigned long data;
 u32 reg_value;

 /* For GMAC3.x, 4.x versions, in "fine adjustement mode" set sub-second
 * increment to twice the number of nanoseconds of a clock cycle.
 * The calculation of the default_addend value by the caller will set it
 * to mid-range = 2^31 when the remainder of this division is zero,
 * which will make the accumulator overflow once every 2 ptp_clock
 * cycles, adding twice the number of nanoseconds of a clock cycle :
 * 2000000000ULL / ptp_clock.
 */
 if (value & PTP_TCR_TSCFUPDT)
  data = (2000000000ULL / ptp_clock);
 else
  data = (1000000000ULL / ptp_clock);

 /* 0.465ns accuracy */
 if (!(value & PTP_TCR_TSCTRLSSR))
  data = (data * 1000) / 465;

 if (data > PTP_SSIR_SSINC_MAX)
  data = PTP_SSIR_SSINC_MAX;

 reg_value = data;
 if (gmac4)
  reg_value <<= GMAC4_PTP_SSIR_SSINC_SHIFT;

 writel(reg_value, ioaddr + PTP_SSIR);

 if (ssinc)
  *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;

 writel(sec, ioaddr + PTP_STSUR);
 writel(nsec, ioaddr + PTP_STNSUR);
 /* issue command to initialize the system time value */
 value = readl(ioaddr + PTP_TCR);
 value |= PTP_TCR_TSINIT;
 writel(value, ioaddr + PTP_TCR);

 /* wait for present system time initialize to complete */
 return readl_poll_timeout_atomic(ioaddr + PTP_TCR, value,
     !(value & PTP_TCR_TSINIT),
     10, 100000);
}

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 */
 value = readl(ioaddr + PTP_TCR);
 value |= PTP_TCR_TSADDREG;
 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;
}

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
 * the system time, then MAC_STSUR reg should be
 * programmed with (2^32 – <new_sec_value>)
 */
  if (gmac4)
   sec = -sec;

  value = readl(ioaddr + PTP_TCR);
  if (value & PTP_TCR_TSCTRLSSR)
   nsec = (PTP_DIGITAL_ROLLOVER_MODE - nsec);
  else
   nsec = (PTP_BINARY_ROLLOVER_MODE - nsec);
 }

 writel(sec, ioaddr + PTP_STSUR);
 value = (add_sub << PTP_STNSUR_ADDSUB_SHIFT) | nsec;
 writel(value, ioaddr + PTP_STNSUR);

 /* issue command to initialize the system time value */
 value = readl(ioaddr + PTP_TCR);
 value |= PTP_TCR_TSUPDT;
 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;
}

static void get_systime(void __iomem *ioaddr, u64 *systime)
{
 u64 ns, sec0, sec1;

 /* Get the TSS value */
 sec1 = readl_relaxed(ioaddr + PTP_STSR);
 do {
  sec0 = sec1;
  /* Get the TSSS value */
  ns = readl_relaxed(ioaddr + PTP_STNSR);
  /* Get the TSS value */
  sec1 = readl_relaxed(ioaddr + PTP_STSR);
 } while (sec0 != sec1);

 if (systime)
  *systime = ns + (sec1 * 1000000000ULL);
}

static void get_ptptime(void __iomem *ptpaddr, u64 *ptp_time)
{
 u64 ns;

 ns = readl(ptpaddr + PTP_ATNR);
 ns += (u64)readl(ptpaddr + PTP_ATSR) * NSEC_PER_SEC;

 *ptp_time = ns;
}

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->flags & STMMAC_FLAG_INT_SNAPSHOT_EN) {
  wake_up(&priv->tstamp_busy_wait);
  return;
 }

 tsync_int = readl(priv->ioaddr + GMAC_INT_STATUS) & GMAC_INT_TSIE;

 if (!tsync_int)
  return;

 /* Read timestamp status to clear interrupt from either external
 * timestamp or start/end of PPS.
 */
 ts_status = readl(priv->ioaddr + GMAC_TIMESTAMP_STATUS);

 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 = channel;
  event.timestamp = ptp_time;
  ptp_clock_event(priv->ptp_clock, &event);
 }
}

const struct stmmac_hwtimestamp stmmac_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 = 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,
};