path: root/drivers/net/dsa/sja1105/sja1105_ptp.c

// SPDX-License-Identifier: GPL-2.0
/* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
 */
#include "sja1105.h"

/* The adjfine API clamps ppb between [-32,768,000, 32,768,000], and
 * therefore scaled_ppm between [-2,147,483,648, 2,147,483,647].
 * Set the maximum supported ppb to a round value smaller than the maximum.
 *
 * Percentually speaking, this is a +/- 0.032x adjustment of the
 * free-running counter (0.968x to 1.032x).
 */
#define SJA1105_MAX_ADJ_PPB		32000000
#define SJA1105_SIZE_PTP_CMD		4

/* Timestamps are in units of 8 ns clock ticks (equivalent to a fixed
 * 125 MHz clock) so the scale factor (MULT / SHIFT) needs to be 8.
 * Furthermore, wisely pick SHIFT as 28 bits, which translates
 * MULT into 2^31 (0x80000000).  This is the same value around which
 * the hardware PTPCLKRATE is centered, so the same ppb conversion
 * arithmetic can be reused.
 */
#define SJA1105_CC_SHIFT		28
#define SJA1105_CC_MULT			(8 << SJA1105_CC_SHIFT)

/* Having 33 bits of cycle counter left until a 64-bit overflow during delta
 * conversion, we multiply this by the 8 ns counter resolution and arrive at
 * a comfortable 68.71 second refresh interval until the delta would cause
 * an integer overflow, in absence of any other readout.
 * Approximate to 1 minute.
 */
#define SJA1105_REFRESH_INTERVAL	(HZ * 60)

/*            This range is actually +/- SJA1105_MAX_ADJ_PPB
 *            divided by 1000 (ppb -> ppm) and with a 16-bit
 *            "fractional" part (actually fixed point).
 *                                    |
 *                                    v
 * Convert scaled_ppm from the +/- ((10^6) << 16) range
 * into the +/- (1 << 31) range.
 *
 * This forgoes a "ppb" numeric representation (up to NSEC_PER_SEC)
 * and defines the scaling factor between scaled_ppm and the actual
 * frequency adjustments (both cycle counter and hardware).
 *
 *   ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16)
 *   simplifies to
 *   ptpclkrate = scaled_ppm * 2^9 / 5^6
 */
#define SJA1105_CC_MULT_NUM		(1 << 9)
#define SJA1105_CC_MULT_DEM		15625

#define ptp_to_sja1105(d) container_of((d), struct sja1105_private, ptp_caps)
#define cc_to_sja1105(d) container_of((d), struct sja1105_private, tstamp_cc)
#define dw_to_sja1105(d) container_of((d), struct sja1105_private, refresh_work)

struct sja1105_ptp_cmd {
	u64 resptp;       /* reset */
};

int sja1105_get_ts_info(struct dsa_switch *ds, int port,
			struct ethtool_ts_info *info)
{
	struct sja1105_private *priv = ds->priv;

	/* Called during cleanup */
	if (!priv->clock)
		return -ENODEV;

	info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
				SOF_TIMESTAMPING_RX_HARDWARE |
				SOF_TIMESTAMPING_RAW_HARDWARE;
	info->tx_types = (1 << HWTSTAMP_TX_OFF);
	info->rx_filters = (1 << HWTSTAMP_FILTER_NONE);
	info->phc_index = ptp_clock_index(priv->clock);
	return 0;
}
EXPORT_SYMBOL_GPL(sja1105_get_ts_info);

int sja1105et_ptp_cmd(const void *ctx, const void *data)
{
	const struct sja1105_ptp_cmd *cmd = data;
	const struct sja1105_private *priv = ctx;
	const struct sja1105_regs *regs = priv->info->regs;
	const int size = SJA1105_SIZE_PTP_CMD;
	u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
	/* No need to keep this as part of the structure */
	u64 valid = 1;

	sja1105_pack(buf, &valid,           31, 31, size);
	sja1105_pack(buf, &cmd->resptp,      2,  2, size);

	return sja1105_spi_send_packed_buf(priv, SPI_WRITE, regs->ptp_control,
					   buf, SJA1105_SIZE_PTP_CMD);
}
EXPORT_SYMBOL_GPL(sja1105et_ptp_cmd);

int sja1105pqrs_ptp_cmd(const void *ctx, const void *data)
{
	const struct sja1105_ptp_cmd *cmd = data;
	const struct sja1105_private *priv = ctx;
	const struct sja1105_regs *regs = priv->info->regs;
	const int size = SJA1105_SIZE_PTP_CMD;
	u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
	/* No need to keep this as part of the structure */
	u64 valid = 1;

	sja1105_pack(buf, &valid,           31, 31, size);
	sja1105_pack(buf, &cmd->resptp,      3,  3, size);

	return sja1105_spi_send_packed_buf(priv, SPI_WRITE, regs->ptp_control,
					   buf, SJA1105_SIZE_PTP_CMD);
}
EXPORT_SYMBOL_GPL(sja1105pqrs_ptp_cmd);

int sja1105_ptp_reset(struct sja1105_private *priv)
{
	struct dsa_switch *ds = priv->ds;
	struct sja1105_ptp_cmd cmd = {0};
	int rc;

	mutex_lock(&priv->ptp_lock);

	cmd.resptp = 1;
	dev_dbg(ds->dev, "Resetting PTP clock\n");
	rc = priv->info->ptp_cmd(priv, &cmd);

	timecounter_init(&priv->tstamp_tc, &priv->tstamp_cc,
			 ktime_to_ns(ktime_get_real()));

	mutex_unlock(&priv->ptp_lock);

	return rc;
}
EXPORT_SYMBOL_GPL(sja1105_ptp_reset);

static int sja1105_ptp_gettime(struct ptp_clock_info *ptp,
			       struct timespec64 *ts)
{
	struct sja1105_private *priv = ptp_to_sja1105(ptp);
	u64 ns;

	mutex_lock(&priv->ptp_lock);
	ns = timecounter_read(&priv->tstamp_tc);
	mutex_unlock(&priv->ptp_lock);

	*ts = ns_to_timespec64(ns);

	return 0;
}

static int sja1105_ptp_settime(struct ptp_clock_info *ptp,
			       const struct timespec64 *ts)
{
	struct sja1105_private *priv = ptp_to_sja1105(ptp);
	u64 ns = timespec64_to_ns(ts);

	mutex_lock(&priv->ptp_lock);
	timecounter_init(&priv->tstamp_tc, &priv->tstamp_cc, ns);
	mutex_unlock(&priv->ptp_lock);

	return 0;
}

static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
{
	struct sja1105_private *priv = ptp_to_sja1105(ptp);
	s64 clkrate;

	clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;
	clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);

	mutex_lock(&priv->ptp_lock);

	/* Force a readout to update the timer *before* changing its frequency.
	 *
	 * This way, its corrected time curve can at all times be modeled
	 * as a linear "A * x + B" function, where:
	 *
	 * - B are past frequency adjustments and offset shifts, all
	 *   accumulated into the cycle_last variable.
	 *
	 * - A is the new frequency adjustments we're just about to set.
	 *
	 * Reading now makes B accumulate the correct amount of time,
	 * corrected at the old rate, before changing it.
	 *
	 * Hardware timestamps then become simple points on the curve and
	 * are approximated using the above function.  This is still better
	 * than letting the switch take the timestamps using the hardware
	 * rate-corrected clock (PTPCLKVAL) - the comparison in this case would
	 * be that we're shifting the ruler at the same time as we're taking
	 * measurements with it.
	 *
	 * The disadvantage is that it's possible to receive timestamps when
	 * a frequency adjustment took place in the near past.
	 * In this case they will be approximated using the new ppb value
	 * instead of a compound function made of two segments (one at the old
	 * and the other at the new rate) - introducing some inaccuracy.
	 */
	timecounter_read(&priv->tstamp_tc);

	priv->tstamp_cc.mult = SJA1105_CC_MULT + clkrate;

	mutex_unlock(&priv->ptp_lock);

	return 0;
}

static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
{
	struct sja1105_private *priv = ptp_to_sja1105(ptp);

	mutex_lock(&priv->ptp_lock);
	timecounter_adjtime(&priv->tstamp_tc, delta);
	mutex_unlock(&priv->ptp_lock);

	return 0;
}

static u64 sja1105_ptptsclk_read(const struct cyclecounter *cc)
{
	struct sja1105_private *priv = cc_to_sja1105(cc);
	const struct sja1105_regs *regs = priv->info->regs;
	u64 ptptsclk = 0;
	int rc;

	rc = sja1105_spi_send_int(priv, SPI_READ, regs->ptptsclk,
				  &ptptsclk, 8);
	if (rc < 0)
		dev_err_ratelimited(priv->ds->dev,
				    "failed to read ptp cycle counter: %d\n",
				    rc);
	return ptptsclk;
}

static void sja1105_ptp_overflow_check(struct work_struct *work)
{
	struct delayed_work *dw = to_delayed_work(work);
	struct sja1105_private *priv = dw_to_sja1105(dw);
	struct timespec64 ts;

	sja1105_ptp_gettime(&priv->ptp_caps, &ts);

	schedule_delayed_work(&priv->refresh_work, SJA1105_REFRESH_INTERVAL);
}

static const struct ptp_clock_info sja1105_ptp_caps = {
	.owner		= THIS_MODULE,
	.name		= "SJA1105 PHC",
	.adjfine	= sja1105_ptp_adjfine,
	.adjtime	= sja1105_ptp_adjtime,
	.gettime64	= sja1105_ptp_gettime,
	.settime64	= sja1105_ptp_settime,
	.max_adj	= SJA1105_MAX_ADJ_PPB,
};

int sja1105_ptp_clock_register(struct sja1105_private *priv)
{
	struct dsa_switch *ds = priv->ds;

	/* Set up the cycle counter */
	priv->tstamp_cc = (struct cyclecounter) {
		.read = sja1105_ptptsclk_read,
		.mask = CYCLECOUNTER_MASK(64),
		.shift = SJA1105_CC_SHIFT,
		.mult = SJA1105_CC_MULT,
	};
	mutex_init(&priv->ptp_lock);
	INIT_DELAYED_WORK(&priv->refresh_work, sja1105_ptp_overflow_check);

	schedule_delayed_work(&priv->refresh_work, SJA1105_REFRESH_INTERVAL);

	priv->ptp_caps = sja1105_ptp_caps;

	priv->clock = ptp_clock_register(&priv->ptp_caps, ds->dev);
	if (IS_ERR_OR_NULL(priv->clock))
		return PTR_ERR(priv->clock);

	return sja1105_ptp_reset(priv);
}
EXPORT_SYMBOL_GPL(sja1105_ptp_clock_register);

void sja1105_ptp_clock_unregister(struct sja1105_private *priv)
{
	if (IS_ERR_OR_NULL(priv->clock))
		return;

	ptp_clock_unregister(priv->clock);
	priv->clock = NULL;
}
EXPORT_SYMBOL_GPL(sja1105_ptp_clock_unregister);

MODULE_AUTHOR("Vladimir Oltean <olteanv@gmail.com>");
MODULE_DESCRIPTION("SJA1105 PHC Driver");
MODULE_LICENSE("GPL v2");