marvell/linux/drivers/net/ethernet/asr/emac_ptp.c

// SPDX-License-Identifier: GPL-2.0
/*
 * emac ptp driver
 *
 * Copyright (C) 2023 ASR Micro Limited
 *
 */

#include <linux/bitops.h>
#include <linux/clk.h>
#include <linux/etherdevice.h>
#include <linux/ethtool.h>
#include <linux/io.h>
#include <linux/kernel.h>
#include <linux/platform_device.h>
#include <linux/ptp_clock_kernel.h>
#include <linux/pps_kernel.h>
#include <linux/timer.h>
#include <linux/time64.h>
#include <linux/types.h>
#include "emac_eth.h"

#define to_emacpriv(_ptp) container_of(_ptp, struct emac_priv, ptp_clock_ops)

#define CIU_UTC_OUT_REG1	0x54
#define CIU_UTC_OUT_REG2	0x58
#define CIU_PPS_SOURCE		0xBC

#define INCVALUE_100MHZ		10
#define INCVALUE_SHIFT_HW	19
#define INCVALUE_SHIFT_SW	23
#define INCPERIOD		1

#ifndef NS_PER_SEC
#define NS_PER_SEC		1000000000ULL
#endif

//#define EMAC_PPS_DEBUG

/* Another drawback of scaling the incvalue by a large factor is the
 * 64-bit SYSTIM register overflows more quickly.  This is dealt with
 * by simply reading the clock before it overflows.
 *
 * Clock	ns bits	Overflows after
 * ~~~~~~	~~~~~~~	~~~~~~~~~~~~~~~
 * 100MHz	(64-19)bit	2^45 / 10^9 / 3600 = 9.77 hrs
 */
#define EMAC_SYSTIM_OVERFLOW_CNT	(1ULL << (64 - INCVALUE_SHIFT_HW))
#define EMAC_SYSTIM_OVERFLOW_SEC	((unsigned long)((EMAC_SYSTIM_OVERFLOW_CNT/NS_PER_SEC)/2))
#define EMAC_SYSTIM_OVERFLOW_PERIOD	(HZ * EMAC_SYSTIM_OVERFLOW_SEC)

static u64 emac_timer_cyc2ns(struct emac_priv *priv, u64 cycle_delta,
				u64 *frac, int backwards)
{
	struct timecounter *tc = &priv->tc;
	u64 delta = cycle_delta & tc->cc->mask;
	u64 nsec, adj, to_adj;
	int neg_adj;

	if (!cycle_delta)
		return 0;

	if (priv->hw_adj) {
		nsec = delta * tc->cc->mult;

		if (priv->frac_div > 0)
			nsec += div_u64(nsec, priv->frac_div);

		if (frac) {
			if (backwards)
				nsec -= *frac;
			else
				nsec += *frac;
			*frac = nsec & tc->mask;
		}

		nsec >>= tc->cc->shift;
	} else {
		nsec = delta * tc->cc->mult;

		if (priv->addend_adj < 0) {
			neg_adj = 1;
			adj = -priv->addend_adj;
		} else {
			neg_adj = 0;
			adj = priv->addend_adj;
		}

		to_adj = div_u64(nsec, priv->ptp_clk_inc) * adj;
		if (priv->frac_div > 0)
			nsec += div_u64(nsec, priv->frac_div);

		if (neg_adj)
			nsec -= to_adj >> tc->cc->shift;
		else
			nsec += to_adj >> tc->cc->shift;

		if (frac)
			*frac = 0;
	}

	return nsec;
}

static void emac_timecounter_init(struct emac_priv *priv, u64 ns)
{
	timecounter_init(&priv->tc, &priv->cc, ns);

	if (!priv->pps_info.enable_pps)
		return;

	priv->pps_info.utc_ns = ns;
}

static u64 emac_timecounter_read(struct emac_priv *priv)
{
	struct timecounter *tc = &priv->tc;
	u64 cycle_now, cycle_delta;
	u64 ns_offset;

	/* read cycle counter: */
	cycle_now = tc->cc->read(tc->cc);

	/* calculate the delta since the last timecounter_read_delta(): */
	cycle_delta = (cycle_now -  tc->cycle_last) & tc->cc->mask;

	/* convert to nanoseconds: */
	ns_offset = emac_timer_cyc2ns(priv, cycle_delta, &tc->frac, 0);

	/* update time stamp of timecounter_read_delta() call: */
	tc->cycle_last = cycle_now;

	ns_offset += tc->nsec;
	tc->nsec = ns_offset;
	return ns_offset;
}

u64 emac_timer_cyc2time(struct emac_priv *priv, u64 cycle_tstamp)
{
	struct timecounter *tc = &priv->tc;
	u64 nsec, cycle_last, delta, frac;
	int backwards = 0;
	u64 delta_ns;

	if (priv->pps_info.enable_pps) {
		nsec = priv->pps_info.utc_ns;
		frac = 0;
		cycle_last = priv->pps_info.ppstime;
	} else {
		nsec = tc->nsec;
		frac = tc->frac;
		cycle_last = tc->cycle_last;
	}

	delta = (cycle_tstamp - cycle_last) & tc->cc->mask;
	if (delta > tc->cc->mask / 2) {
		delta = (cycle_last - cycle_tstamp) & tc->cc->mask;
		backwards = 1;
	}

	delta_ns = emac_timer_cyc2ns(priv, delta, &frac, backwards);
	if (backwards)
		nsec -= delta_ns;
	else
		nsec += delta_ns;

	return nsec;
}

void emac_hw_timestamp_config(struct emac_priv *priv, u32 enable,
                                u8 rx_ptp_type, u32 ptp_msg_id)
{
	void __iomem *ioaddr = priv->iobase;
	u32 val;

	if (enable) {
		/*
		 * enable tx/rx timestamp and config rx ptp type
		 */
		val = emac_rd(priv, PTP_1588_CTRL);
		val |= TX_TIMESTAMP_EN | RX_TIMESTAMP_EN;
		val &= ~RX_PTP_PKT_TYPE_MSK;
		val |= (rx_ptp_type << RX_PTP_PKT_TYPE_OFST) &
                        RX_PTP_PKT_TYPE_MSK;
		writel(val, ioaddr + PTP_1588_CTRL);

		/* config ptp message id */
		writel(ptp_msg_id, ioaddr + PTP_MSG_ID);

		/* config ptp ethernet type */
		writel(ETH_P_1588, ioaddr + PTP_ETH_TYPE);

		/* config ptp udp port */
		writel(PTP_EVENT_PORT, ioaddr + PTP_UDP_PORT);

	} else {
		val = emac_rd(priv, PTP_1588_CTRL);
		val &= ~(TX_TIMESTAMP_EN | RX_TIMESTAMP_EN);
		writel(val, ioaddr + PTP_1588_CTRL);
        }
}

u32 emac_hw_config_systime_increment(struct emac_priv *priv)
{
	void __iomem *ioaddr = priv->iobase;
	u32 ptp_clock = priv->ptp_clk_rate;
	u32 cycle_inc, cycle_mod;
	u32 val;

	/*
	 * set system time counter resolution as ns if ptp clock is 100Mhz,
	 * 10ns per clock cycle, so increment value should be 10, increment
	 * period should be 1
	 */
	cycle_inc = (NS_PER_SEC / ptp_clock) * INCPERIOD;
	cycle_mod = NS_PER_SEC % ptp_clock;
	priv->ptp_clk_inc = cycle_inc;

	/*
	* Assume ptp_clk_rate= 38.4M
	* Tns = 1000000000/38400000 = 26 + 1/24
	* nsec = (delta/26) * Tns = (delta/26) * (26 + 1/24)
	* nsec = delta + delta/(26*24)
	* frag = delta/div, div = 26*24
	*/
	if (cycle_mod > 0)
		priv->frac_div = (priv->ptp_clk_rate * cycle_inc / cycle_mod);
	else
		priv->frac_div = 0;

	if (priv->hw_adj) {
		priv->default_addend = cycle_inc << INCVALUE_SHIFT_HW;
		val = (priv->default_addend | (INCPERIOD << INCR_PERIOD_OFST));
	} else {
		priv->default_addend = cycle_inc << INCVALUE_SHIFT_SW;
		priv->addend_adj = 0;
		val = (cycle_inc | (INCPERIOD << INCR_PERIOD_OFST));
	}

	writel(val, ioaddr + PTP_INRC_ATTR);
	pr_info("default_addend=%d cycle_mod=%d cycle_inc=%d frac_div=%d\n",
		priv->default_addend, cycle_mod, cycle_inc, priv->frac_div);
	return 0;
}

u64 emac_hw_get_systime(struct emac_priv *priv)
{
	void __iomem *ioaddr = priv->iobase;
	unsigned long flags;
	u64 systimel, systimeh;
	u64 systim;

	local_irq_save(flags);

	/* first read system time low register */
	systimel = readl(ioaddr + SYS_TIME_GET_LOW);
	systimeh = readl(ioaddr + SYS_TIME_GET_HI);
	systim = (systimeh << 32) | systimel;

	/* Add a dummy read to WA systimer jump issue */
	systimel = emac_rd(priv, SYS_TIME_ADJ_LOW);

	local_irq_restore(flags);

	return systim;
}

u64 emac_hw_get_ppstime(struct emac_priv *priv)
{
	void __iomem *ioaddr = priv->iobase;
	u64 ppstimel, ppstimeh;
	u64 ppstime;

	ppstimel = readl(ioaddr + PTP_PPS_TIME_L);
	ppstimeh = readl(ioaddr + PTP_PPS_TIME_H);
	ppstime = (ppstimeh << 32) | ppstimel;

	return ppstime;
}

u64 emac_hw_get_phc_time(struct emac_priv *priv)
{
	unsigned long flags;
	u64 cycles, ns;

	spin_lock_irqsave(&priv->ptp_lock, flags);

	cycles = emac_hw_get_systime(priv);
	ns = emac_timer_cyc2time(priv, cycles);

	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	return ns;
}

u64 emac_hw_get_tx_timestamp(struct emac_priv *priv)
{
	void __iomem *ioaddr = priv->iobase;
	unsigned long flags;
	u64 systimel, systimeh;
	u64 systim;
	u64 ns;

	/* first read system time low register */
	systimel = readl(ioaddr + TX_TIMESTAMP_LOW);
	systimeh = readl(ioaddr + TX_TIMESTAMP_HI);
	systim = (systimeh << 32) | systimel;

	spin_lock_irqsave(&priv->ptp_lock, flags);

	ns = emac_timer_cyc2time(priv, systim);

	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	return ns;
}

u64 emac_hw_get_rx_timestamp(struct emac_priv *priv)
{
	void __iomem *ioaddr = priv->iobase;
	unsigned long flags;
	u64 systimel, systimeh;
	u64 systim;
	u64 ns;

	/* first read system time low register */
	systimel = readl(ioaddr + RX_TIMESTAMP_LOW);
	systimeh = readl(ioaddr + RX_TIMESTAMP_HI);
	systim = (systimeh << 32) | systimel;

	spin_lock_irqsave(&priv->ptp_lock, flags);

	ns = emac_timer_cyc2time(priv, systim);

	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	return ns;
}

/**
 * emac_cyclecounter_read - read raw cycle counter (used by time counter)
 * @cc: cyclecounter structure
 **/
static u64 emac_cyclecounter_read(const struct cyclecounter *cc)
{
	struct emac_priv *priv = container_of(cc, struct emac_priv, cc);

	return emac_hw_get_systime(priv);
}

int emac_hw_init_systime(struct emac_priv *priv, u64 set_ns)
{
	unsigned long flags;

	spin_lock_irqsave(&priv->ptp_lock, flags);

	emac_timecounter_init(priv, set_ns);

	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	return 0;
}

struct emac_hw_ptp emac_hwptp = {
	.config_hw_tstamping = emac_hw_timestamp_config,
	.config_systime_increment = emac_hw_config_systime_increment,
	.init_systime = emac_hw_init_systime,
	.get_phc_time = emac_hw_get_phc_time,
	.get_tx_timestamp = emac_hw_get_tx_timestamp,
	.get_rx_timestamp = emac_hw_get_rx_timestamp,
};

/**
 * emac_adjust_freq
 *
 * @ptp: pointer to ptp_clock_info structure
 * @ppb: desired period change in parts ber billion
 *
 * Description: this function will adjust the frequency of hardware clock.
 */
static int emac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb)
{
	struct emac_priv *priv = to_emacpriv(ptp);
	void __iomem *ioaddr = priv->iobase;
	unsigned long flags;
	int neg_adj = 0;
	u64 incvalue, adj;
	u32 addend;

	/* ppb means delta time each sample cycle, in nano second */
	if ((ppb > ptp->max_adj) || (ppb <= -1000000000))
		return -EINVAL;

	if (!ppb)
		return 0;

	if (ppb < 0) {
		neg_adj = 1;
		ppb = -ppb;
	}

	spin_lock_irqsave(&priv->ptp_lock, flags);

	incvalue = priv->default_addend;
	adj = incvalue;
	adj *= ppb;
	adj = div_u64(adj, 1000000000);
	if (priv->hw_adj) {
		addend = neg_adj ? (incvalue - adj) : (incvalue + adj);
		addend = (addend | (INCPERIOD << INCR_PERIOD_OFST));
		writel(addend, ioaddr + PTP_INRC_ATTR);
		priv->addend_adj = 0;
	} else {
		/* update tc->cycle_last since adj to be changed */
		emac_timecounter_read(priv);
		priv->addend_adj = neg_adj ? - adj : adj;
	}
#ifdef EMAC_PPS_DEBUG
	pr_info("emac_adjust_freq: ppb=%d adj=%s%lld\n", ppb,
		neg_adj ? "-" : "+" , adj);
#endif
	spin_unlock_irqrestore(&priv->ptp_lock, flags);
	return 0;
}

/**
 * emac_adjust_time
 *
 * @ptp: pointer to ptp_clock_info structure
 * @delta: desired change in nanoseconds
 *
 * Description: this function will shift/adjust the hardware clock time.
 */
static int emac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
{
	struct emac_priv *priv = to_emacpriv(ptp);
	unsigned long flags;	

	spin_lock_irqsave(&priv->ptp_lock, flags);

	if (priv->pps_info.enable_pps)
		priv->pps_info.utc_ns += delta;
	else
		timecounter_adjtime(&priv->tc, delta);

	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	return 0;
}

/**
 * emac_phc_get_time
 *
 * @ptp: pointer to ptp_clock_info structure
 * @ts: pointer to hold time/result
 *
 * Description: this function will read the current time from the
 * hardware clock and store it in @ts.
 */
static int emac_phc_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
{
	struct emac_priv *priv = to_emacpriv(ptp);
	unsigned long flags;
	u64 cycles, ns;

	spin_lock_irqsave(&priv->ptp_lock, flags);

	cycles = emac_hw_get_systime(priv);
	ns = emac_timer_cyc2time(priv, cycles);

	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	*ts = ns_to_timespec64(ns);
	return 0;
}

/**
 * emac_phc_set_time
 *
 * @ptp: pointer to ptp_clock_info structure
 * @ts: time value to set
 *
 * Description: this function will set the current time on the
 * hardware clock.
 */
static int emac_phc_set_time(struct ptp_clock_info *ptp,
			   const struct timespec64 *ts)
{
	struct emac_priv *priv = to_emacpriv(ptp);
	unsigned long flags;
	u64 ns;

	ns = timespec64_to_ns(ts);

	spin_lock_irqsave(&priv->ptp_lock, flags);

	if (priv->pps_info.enable_pps &&
	    priv->pps_info.pps_source == EMAC_PPS_GNSS) {
		u64 ppstime;

		ppstime = emac_hw_get_ppstime(priv);
		if (ppstime != priv->pps_info.ppstime) {
			pr_warn("New PPS come, the time is out-of-date\n");
			spin_unlock_irqrestore(&priv->ptp_lock, flags);
			return 0;
		}
	}

	emac_timecounter_init(priv, ns);

	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	return 0;
}

/* structure describing a PTP hardware clock */
static struct ptp_clock_info emac_ptp_clock_ops = {
	.owner = THIS_MODULE,
	.name = "emac_ptp_clock",
	.n_alarm = 0,
	.n_ext_ts = 0,
	.n_per_out = 0,
	.n_pins = 0,
	.pps = 0,
	.adjfreq = emac_adjust_freq,
	.adjtime = emac_adjust_time,
	.gettime64 = emac_phc_get_time,
	.settime64 = emac_phc_set_time,
};

#ifdef CONFIG_PPS
static s64 emac_read_bcode_utc(void)
{
	unsigned int year, mon, day, hour, min, sec;
	struct timespec64 utc;
	struct tm result;
	s64 utc_ns;
	u32 val;

	val = readl(CIU_VIRT_BASE + CIU_UTC_OUT_REG2);
	year = 2000 + ((val >> 16) & 0x7F);
	mon = 0;
	day = val & 0x1FF;

	val = readl(CIU_VIRT_BASE + CIU_UTC_OUT_REG1);
	hour = (val >> 16) & 0x1F;
	min = (val >> 8) & 0x3F;
	sec = val & 0x3F;

	utc.tv_sec = mktime64(year, mon, day, hour, min, sec);
	utc.tv_nsec = 0;
	utc_ns = timespec64_to_ns(&utc);
	time64_to_tm(utc.tv_sec, 0, &result);
#ifdef EMAC_PPS_DEBUG
	pr_info("UTC from bcode: %lldns\n", utc_ns);
	pr_info("%d-%d-%d %d:%d:%d ==> %ld-%d-%d %d:%d:%d\n",
		year, mon, day, hour, min, sec,
		(1900 + result.tm_year), (result.tm_mon + 1), result.tm_mday,
		result.tm_hour, result.tm_min, result.tm_sec);
#endif
	return utc_ns;
}

static inline int emac_lost_pps_interrupt(struct emac_priv *priv, u64 *ns,
					u64 interval)
{
	s64 offset = 0;

	*ns = 0;
	if (interval < (priv->pps_info.pps_cycle * NS_PER_SEC * 3)/2)
		return 0;

	*ns = div_u64(interval, NS_PER_SEC);
	*ns *= NS_PER_SEC;
	offset = interval - *ns;
	if (offset > NS_PER_SEC / 2)
		*ns += NS_PER_SEC;

	pr_warn("Lost PPS signal about %lldns\n", interval);
	return 1;
}

static inline void emac_pps_get_ts(struct emac_priv *priv,
			struct pps_event_time *ts, u64 pps_ns)
{
	struct emac_pps *info = &priv->pps_info;
	struct system_time_snapshot snap;
	u64 ns;

	ktime_get_snapshot(&snap);
	if (priv->pps_info.enable_pps) {
		ts->ts_real = ns_to_timespec64(info->utc_ns + pps_ns);
		if (info->pps_source == EMAC_PPS_BCODE) {
			info->utc_ns = emac_read_bcode_utc();
		} else {
			if (emac_lost_pps_interrupt(priv, &ns, pps_ns))
				info->utc_ns += ns;
			else
				info->utc_ns += info->pps_cycle * NS_PER_SEC;
		}
	} else {
		ts->ts_real = ktime_to_timespec64(snap.real);
	}

#ifdef CONFIG_NTP_PPS
	ts->ts_raw = ktime_to_timespec64(snap.raw);
#endif
}

static irqreturn_t emac_pps_irq(int irq, void *dev_id)
{
	struct emac_priv *priv = (struct emac_priv *)dev_id;
	struct ptp_clock *ptp = priv->ptp_clock;
	struct pps_device *pps;
	struct pps_event_time ts;
	u32 pps_cycle, pps_cnt;
	u64 ppstime, systime;
	u64 ppstime_ns;
	u64 pps_interval_ns, pps_interval, delay_ns;
	u32 status;

	status = emac_rd(priv, PTP_1588_IRQ_STS);
	if (!(status & PTP_PPS_VALID))
		return IRQ_NONE;

	ppstime = emac_hw_get_ppstime(priv);
	systime = emac_hw_get_systime(priv);
	pps_cnt = emac_rd(priv, PTP_PPS_COUNTER);
	pps_cycle = emac_rd(priv, PTP_PPS_VALUE);

	delay_ns = emac_timer_cyc2ns(priv, systime - ppstime, NULL, 0);
	pps_interval = ppstime - priv->pps_info.ppstime;
	pps_interval_ns = emac_timer_cyc2ns(priv, pps_interval, NULL, 0);
	ppstime_ns = emac_timer_cyc2ns(priv, ppstime, NULL, 0);

#ifdef EMAC_PPS_DEBUG
	pr_info("emac_pps_irq: pps_cnt=%d interrupt_delay=%lldns\n",
		pps_cnt, delay_ns);
	pr_info("emac_pps_irq: ppstime_ns=%lld interval=%lldns ppstime=%lld systime=%lld\n",
		ppstime_ns, pps_interval_ns, ppstime, systime);
#endif
	/* report pps event */
	pps = ptp_pps_device(ptp);
	if (pps) {
#ifdef EMAC_PPS_DEBUG
		s64 diff;

		diff = NS_PER_SEC * pps_cycle * (pps_cnt - priv->pps_info.ppscnt);
		diff = pps_interval_ns - diff;

		/*
		 * Stop when |diff| < cycle, show higher accuracy for
		 * frequency synchronization via PPS
		 */
		if (abs(diff) < priv->ptp_clk_inc)
			pps_interval_ns = NS_PER_SEC * pps_cycle;
		pr_info("emac_pps_irq: pps width diff %lldns\n", diff);
#endif
		emac_pps_get_ts(priv, &ts, pps_interval_ns);
		pps_event(pps, &ts, PPS_CAPTUREASSERT, dev_id);
	}

	priv->pps_info.ppstime = ppstime;
	priv->pps_info.ppscnt = pps_cnt;

	emac_wr(priv, PTP_1588_IRQ_STS, PTP_PPS_VALID);
	return IRQ_HANDLED;
}

static int emac_ptp_config_pps(struct emac_priv *priv)
{
	unsigned long timeo = jiffies + msecs_to_jiffies(500);
	u64 ppstime;
	u32 pps_cnt;
	u32 reg;
	int ret;

	if (priv->irq_pps) {
		ret = request_irq(priv->irq_pps, emac_pps_irq,
				  IRQF_SHARED, "emac_pps", priv);
		if (ret) {
			pr_err("request irq_pps failed, ret=%d\\n", ret);
			return ret;
		}
	}

	priv->ptp_clock_ops.pps = 1;
	priv->pps_info.pps_cycle = 1;

	/* Config PPS source */
	reg = readl(CIU_VIRT_BASE + CIU_PPS_SOURCE);
	if (priv->pps_info.pps_source == EMAC_PPS_BCODE)
		reg &= ~BIT(0);
	else
		reg |= BIT(0);
	writel(reg, CIU_VIRT_BASE + CIU_PPS_SOURCE);

	/* Clear PPS register */
	pr_info("reset emac PPS\n");
	reg = emac_rd(priv, PTP_1588_CTRL);
	reg |= PPS_COUNTER_RESET;
	emac_wr(priv, PTP_1588_CTRL, reg);
	do {
		reg = emac_rd(priv, PTP_1588_CTRL);
		if (!(reg & PPS_COUNTER_RESET))
			break;
	} while (time_before(jiffies, timeo));

	if (reg & PPS_COUNTER_RESET)
		pr_err("reset PPS failed\n");

	/* Config PPS interrupt cycle */
	emac_wr(priv, PTP_PPS_VALUE, priv->pps_info.pps_cycle);

	/* Disable PPS interrupt */
	reg = emac_rd(priv, PTP_1588_IRQ_EN);
	reg &= ~PTP_PPS_VALID;
	emac_wr(priv, PTP_1588_IRQ_EN, reg);

	reg = emac_rd(priv, PTP_1588_CTRL);
	reg |= PPS_MODE_ENABLE | TX_TIMESTAMP_EN | RX_TIMESTAMP_EN;
	emac_wr(priv, PTP_1588_CTRL, reg);

	ppstime = emac_hw_get_ppstime(priv);
	pps_cnt = emac_rd(priv, PTP_PPS_COUNTER);
	pr_info("ppstime=%lld pps_cnt=%d\n", ppstime, pps_cnt);

	priv->pps_info.ppscnt = pps_cnt;
	priv->pps_info.ppstime = ppstime;
	priv->pps_info.utc_ns = emac_read_bcode_utc();

	/* Enable PPS interrupt */
	reg = emac_rd(priv, PTP_1588_IRQ_EN);
	reg |= PTP_PPS_VALID;
	emac_wr(priv, PTP_1588_IRQ_EN, reg);
	return 0;
}
#endif

static void emac_systim_overflow_work(struct work_struct *work)
{
	struct emac_priv *priv = container_of(work, struct emac_priv,
						     systim_overflow_work.work);
	struct timespec64 ts;
	u64 ns;

	/* Update the timecounter */
	ns = emac_timecounter_read(priv);

	ts = ns_to_timespec64(ns);
	pr_debug("SYSTIM overflow check at %lld.%09lu\n",
	      (long long) ts.tv_sec, ts.tv_nsec);

#ifdef CONFIG_PPS
	if (priv->pps_info.enable_pps) {
		u64 systime, ppstime, interval_ns;
		unsigned long flags;

		spin_lock_irqsave(&priv->ptp_lock, flags);

		systime = emac_hw_get_systime(priv);
		ppstime = priv->pps_info.ppstime;
		interval_ns = emac_timer_cyc2ns(priv, systime - ppstime, NULL, 0);
		if (emac_lost_pps_interrupt(priv, &ns, interval_ns)) {
			struct timecounter *tc = &priv->tc;

			priv->pps_info.utc_ns += ns;
			priv->pps_info.ppstime +=
				div_u64(ns << tc->cc->shift, tc->cc->mult);
			pr_warn("PPS overflow: add %lld seconds to UTC\n", ns);
		}

		spin_unlock_irqrestore(&priv->ptp_lock, flags);
	}
#endif
	schedule_delayed_work(&priv->systim_overflow_work,
			     EMAC_SYSTIM_OVERFLOW_PERIOD);
}

/**
 * emac_ptp_register
 * @priv: driver private structure
 * Description: this function will register the ptp clock driver
 * to kernel. It also does some house keeping work.
 */
void emac_ptp_register(struct emac_priv *priv)
{
	unsigned long flags;
	int max_adj;

	priv->cc.read = emac_cyclecounter_read;
	priv->cc.mask = CYCLECOUNTER_MASK(64);
	priv->cc.mult = 1;
	if (priv->hw_adj)
		priv->cc.shift = INCVALUE_SHIFT_HW;
	else
		priv->cc.shift = INCVALUE_SHIFT_SW;

	spin_lock_init(&priv->ptp_lock);
	priv->ptp_clock_ops = emac_ptp_clock_ops;

	max_adj = (1 << (24 - INCVALUE_SHIFT_HW)) - 10;
	max_adj = max_adj * priv->ptp_clk_rate * INCPERIOD;
	if (max_adj < 0) {
		netdev_err(priv->ndev, "ptp increment too large\n");
		max_adj = 1000000000;
	}
	priv->ptp_clock_ops.max_adj = min(max_adj, 1000000000);

	INIT_DELAYED_WORK(&priv->systim_overflow_work,
			  emac_systim_overflow_work);
	schedule_delayed_work(&priv->systim_overflow_work,
			      EMAC_SYSTIM_OVERFLOW_PERIOD);
#ifdef CONFIG_PPS
	if (priv->pps_info.enable_pps) {
		emac_hw_config_systime_increment(priv);
		emac_ptp_config_pps(priv);
		netdev_info(priv->ndev, "PPS enabled\n");
	} else {
		priv->ptp_clock_ops.pps = 0;
	}
#endif
	priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
					     NULL);
	if (IS_ERR(priv->ptp_clock)) {
		netdev_err(priv->ndev, "ptp_clock_register failed\n");
		priv->ptp_clock = NULL;
	} else if (priv->ptp_clock)
		netdev_info(priv->ndev, "registered PTP clock\n");
	else
		netdev_info(priv->ndev, "PTP_1588_CLOCK maybe not enabled\n");

	spin_lock_irqsave(&priv->ptp_lock, flags);
	emac_timecounter_init(priv, ktime_to_ns(ktime_get_real()));
	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	priv->hwptp = &emac_hwptp;
	pr_info("ptp max_adj:%u, overflow timeout:%ld minutes\n",
		priv->ptp_clock_ops.max_adj, EMAC_SYSTIM_OVERFLOW_SEC / 60);
}

/**
 * emac_ptp_unregister
 * @priv: driver private structure
 * Description: this function will remove/unregister the ptp clock driver
 * from the kernel.
 */
void emac_ptp_unregister(struct emac_priv *priv)
{
	cancel_delayed_work_sync(&priv->systim_overflow_work);

	if (priv->ptp_clock) {
		ptp_clock_unregister(priv->ptp_clock);
		priv->ptp_clock = NULL;
		pr_debug("Removed PTP HW clock successfully on %s\n",
			 priv->ndev->name);
	}

	if (priv->irq_pps)
		free_irq(priv->irq_pps, priv);

	priv->hwptp = NULL;
}