net: dsa: sja1105: Switch to hardware operations for PTP

Adjusting the hardware clock (PTPCLKVAL, PTPCLKADD, PTPCLKRATE) is a
requirement for the auxiliary PTP functionality of the switch
(TTEthernet, PPS input, PPS output).

Therefore we need to switch to using these registers to keep a
synchronized time in hardware, instead of the timecounter/cyclecounter
implementation, which is reliant on the free-running PTPTSCLK.

Signed-off-by: Vladimir Oltean <olteanv@gmail.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

1 files changed, 107 insertions, 111 deletions

diff --git a/drivers/net/dsa/sja1105/sja1105_ptp.c b/drivers/net/dsa/sja1105/sja1105_ptp.c
index b43096063cf4..783100397f8a 100644
--- a/drivers/net/dsa/sja1105/sja1105_ptp.c
+++ b/drivers/net/dsa/sja1105/sja1105_ptp.c
@@ -13,24 +13,6 @@
 #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).
@@ -41,7 +23,7 @@
  *
  * 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).
+ * frequency adjustments of the PHC.
  *
  *   ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16)
  *   simplifies to
@@ -49,13 +31,15 @@
  */
 #define SJA1105_CC_MULT_NUM		(1 << 9)
 #define SJA1105_CC_MULT_DEM		15625
+#define SJA1105_CC_MULT			0x80000000
+
+enum sja1105_ptp_clk_mode {
+	PTP_ADD_MODE = 1,
+	PTP_SET_MODE = 0,
+};
 
 #define ptp_caps_to_data(d) \
 		container_of((d), struct sja1105_ptp_data, caps)
-#define cc_to_ptp_data(d) \
-		container_of((d), struct sja1105_ptp_data, tstamp_cc)
-#define dw_to_ptp_data(d) \
-		container_of((d), struct sja1105_ptp_data, refresh_work)
 #define ptp_data_to_sja1105(d) \
 		container_of((d), struct sja1105_private, ptp_data)
 
@@ -220,6 +204,8 @@ int sja1105et_ptp_cmd(const struct dsa_switch *ds,
 
 	sja1105_pack(buf, &valid,           31, 31, size);
 	sja1105_pack(buf, &cmd->resptp,      2,  2, size);
+	sja1105_pack(buf, &cmd->corrclk4ts,  1,  1, size);
+	sja1105_pack(buf, &cmd->ptpclkadd,   0,  0, size);
 
 	return sja1105_xfer_buf(priv, SPI_WRITE, regs->ptp_control, buf,
 				SJA1105_SIZE_PTP_CMD);
@@ -237,6 +223,8 @@ int sja1105pqrs_ptp_cmd(const struct dsa_switch *ds,
 
 	sja1105_pack(buf, &valid,           31, 31, size);
 	sja1105_pack(buf, &cmd->resptp,      3,  3, size);
+	sja1105_pack(buf, &cmd->corrclk4ts,  2,  2, size);
+	sja1105_pack(buf, &cmd->ptpclkadd,   0,  0, size);
 
 	return sja1105_xfer_buf(priv, SPI_WRITE, regs->ptp_control, buf,
 				SJA1105_SIZE_PTP_CMD);
@@ -346,6 +334,22 @@ static int sja1105_ptpegr_ts_poll(struct dsa_switch *ds, int port, u64 *ts)
 	return 0;
 }
 
+/* Caller must hold ptp_data->lock */
+static int sja1105_ptpclkval_read(struct sja1105_private *priv, u64 *ticks)
+{
+	const struct sja1105_regs *regs = priv->info->regs;
+
+	return sja1105_xfer_u64(priv, SPI_READ, regs->ptpclkval, ticks);
+}
+
+/* Caller must hold ptp_data->lock */
+static int sja1105_ptpclkval_write(struct sja1105_private *priv, u64 ticks)
+{
+	const struct sja1105_regs *regs = priv->info->regs;
+
+	return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpclkval, &ticks);
+}
+
 #define rxtstamp_to_tagger(d) \
 	container_of((d), struct sja1105_tagger_data, rxtstamp_work)
 #define tagger_to_sja1105(d) \
@@ -363,17 +367,22 @@ static void sja1105_rxtstamp_work(struct work_struct *work)
 
 	while ((skb = skb_dequeue(&tagger_data->skb_rxtstamp_queue)) != NULL) {
 		struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
-		u64 now, ts;
+		u64 ticks, ts;
+		int rc;
 
-		now = ptp_data->tstamp_cc.read(&ptp_data->tstamp_cc);
+		rc = sja1105_ptpclkval_read(priv, &ticks);
+		if (rc < 0) {
+			dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
+			kfree_skb(skb);
+			continue;
+		}
 
 		*shwt = (struct skb_shared_hwtstamps) {0};
 
 		ts = SJA1105_SKB_CB(skb)->meta_tstamp;
-		ts = sja1105_tstamp_reconstruct(ds, now, ts);
-		ts = timecounter_cyc2time(&ptp_data->tstamp_tc, ts);
+		ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
 
-		shwt->hwtstamp = ns_to_ktime(ts);
+		shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
 		netif_rx_ni(skb);
 	}
 
@@ -427,9 +436,6 @@ int sja1105_ptp_reset(struct dsa_switch *ds)
 	dev_dbg(ds->dev, "Resetting PTP clock\n");
 	rc = priv->info->ptp_cmd(ds, &cmd);
 
-	timecounter_init(&ptp_data->tstamp_tc, &ptp_data->tstamp_cc,
-			 ktime_to_ns(ktime_get_real()));
-
 	mutex_unlock(&ptp_data->lock);
 
 	return rc;
@@ -439,112 +445,106 @@ static int sja1105_ptp_gettime(struct ptp_clock_info *ptp,
 			       struct timespec64 *ts)
 {
 	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
-	u64 ns;
+	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
+	u64 ticks = 0;
+	int rc;
 
 	mutex_lock(&ptp_data->lock);
-	ns = timecounter_read(&ptp_data->tstamp_tc);
+
+	rc = sja1105_ptpclkval_read(priv, &ticks);
+	*ts = ns_to_timespec64(sja1105_ticks_to_ns(ticks));
+
 	mutex_unlock(&ptp_data->lock);
 
-	*ts = ns_to_timespec64(ns);
+	return rc;
+}
 
-	return 0;
+/* Caller must hold ptp_data->lock */
+static int sja1105_ptp_mode_set(struct sja1105_private *priv,
+				enum sja1105_ptp_clk_mode mode)
+{
+	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
+
+	if (ptp_data->cmd.ptpclkadd == mode)
+		return 0;
+
+	ptp_data->cmd.ptpclkadd = mode;
+
+	return priv->info->ptp_cmd(priv->ds, &ptp_data->cmd);
 }
 
+/* Write to PTPCLKVAL while PTPCLKADD is 0 */
 static int sja1105_ptp_settime(struct ptp_clock_info *ptp,
 			       const struct timespec64 *ts)
 {
 	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
-	u64 ns = timespec64_to_ns(ts);
+	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
+	u64 ticks = ns_to_sja1105_ticks(timespec64_to_ns(ts));
+	int rc;
 
 	mutex_lock(&ptp_data->lock);
-	timecounter_init(&ptp_data->tstamp_tc, &ptp_data->tstamp_cc, ns);
+
+	rc = sja1105_ptp_mode_set(priv, PTP_SET_MODE);
+	if (rc < 0) {
+		dev_err(priv->ds->dev, "Failed to put PTPCLK in set mode\n");
+		goto out;
+	}
+
+	rc = sja1105_ptpclkval_write(priv, ticks);
+out:
 	mutex_unlock(&ptp_data->lock);
 
-	return 0;
+	return rc;
 }
 
 static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 {
 	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
+	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
+	const struct sja1105_regs *regs = priv->info->regs;
+	u32 clkrate32;
 	s64 clkrate;
+	int rc;
 
 	clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;
 	clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);
 
-	mutex_lock(&ptp_data->lock);
+	/* Take a +/- value and re-center it around 2^31. */
+	clkrate = SJA1105_CC_MULT + clkrate;
+	WARN_ON(abs(clkrate) >= GENMASK_ULL(31, 0));
+	clkrate32 = clkrate;
 
-	/* 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(&ptp_data->tstamp_tc);
+	mutex_lock(&ptp_data->lock);
 
-	ptp_data->tstamp_cc.mult = SJA1105_CC_MULT + clkrate;
+	rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptpclkrate, &clkrate32);
 
 	mutex_unlock(&ptp_data->lock);
 
-	return 0;
+	return rc;
 }
 
+/* Write to PTPCLKVAL while PTPCLKADD is 1 */
 static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 {
 	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
-
-	mutex_lock(&ptp_data->lock);
-	timecounter_adjtime(&ptp_data->tstamp_tc, delta);
-	mutex_unlock(&ptp_data->lock);
-
-	return 0;
-}
-
-static u64 sja1105_ptptsclk_read(const struct cyclecounter *cc)
-{
-	struct sja1105_ptp_data *ptp_data = cc_to_ptp_data(cc);
 	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
-	const struct sja1105_regs *regs = priv->info->regs;
-	u64 ptptsclk = 0;
+	s64 ticks = ns_to_sja1105_ticks(delta);
 	int rc;
 
-	rc = sja1105_xfer_u64(priv, SPI_READ, regs->ptptsclk, &ptptsclk);
-	if (rc < 0)
-		dev_err_ratelimited(priv->ds->dev,
-				    "failed to read ptp cycle counter: %d\n",
-				    rc);
-	return ptptsclk;
-}
+	mutex_lock(&ptp_data->lock);
 
-static void sja1105_ptp_overflow_check(struct work_struct *work)
-{
-	struct delayed_work *dw = to_delayed_work(work);
-	struct sja1105_ptp_data *ptp_data = dw_to_ptp_data(dw);
-	struct timespec64 ts;
+	rc = sja1105_ptp_mode_set(priv, PTP_ADD_MODE);
+	if (rc < 0) {
+		dev_err(priv->ds->dev, "Failed to put PTPCLK in add mode\n");
+		goto out;
+	}
 
-	sja1105_ptp_gettime(&ptp_data->caps, &ts);
+	rc = sja1105_ptpclkval_write(priv, ticks);
 
-	schedule_delayed_work(&ptp_data->refresh_work,
-			      SJA1105_REFRESH_INTERVAL);
+out:
+	mutex_unlock(&ptp_data->lock);
+
+	return rc;
 }
 
 int sja1105_ptp_clock_register(struct dsa_switch *ds)
@@ -553,13 +553,6 @@ int sja1105_ptp_clock_register(struct dsa_switch *ds)
 	struct sja1105_tagger_data *tagger_data = &priv->tagger_data;
 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 
-	/* Set up the cycle counter */
-	ptp_data->tstamp_cc = (struct cyclecounter) {
-		.read		= sja1105_ptptsclk_read,
-		.mask		= CYCLECOUNTER_MASK(64),
-		.shift		= SJA1105_CC_SHIFT,
-		.mult		= SJA1105_CC_MULT,
-	};
 	ptp_data->caps = (struct ptp_clock_info) {
 		.owner		= THIS_MODULE,
 		.name		= "SJA1105 PHC",
@@ -578,8 +571,8 @@ int sja1105_ptp_clock_register(struct dsa_switch *ds)
 	if (IS_ERR_OR_NULL(ptp_data->clock))
 		return PTR_ERR(ptp_data->clock);
 
-	INIT_DELAYED_WORK(&ptp_data->refresh_work, sja1105_ptp_overflow_check);
-	schedule_delayed_work(&ptp_data->refresh_work, SJA1105_REFRESH_INTERVAL);
+	ptp_data->cmd.corrclk4ts = true;
+	ptp_data->cmd.ptpclkadd = PTP_SET_MODE;
 
 	return sja1105_ptp_reset(ds);
 }
@@ -594,7 +587,6 @@ void sja1105_ptp_clock_unregister(struct dsa_switch *ds)
 
 	cancel_work_sync(&priv->tagger_data.rxtstamp_work);
 	skb_queue_purge(&priv->tagger_data.skb_rxtstamp_queue);
-	cancel_delayed_work_sync(&ptp_data->refresh_work);
 	ptp_clock_unregister(ptp_data->clock);
 	ptp_data->clock = NULL;
 }
@@ -605,14 +597,19 @@ void sja1105_ptp_txtstamp_skb(struct dsa_switch *ds, int slot,
 	struct sja1105_private *priv = ds->priv;
 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
 	struct skb_shared_hwtstamps shwt = {0};
-	u64 now, ts;
+	u64 ticks, ts;
 	int rc;
 
 	skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 
 	mutex_lock(&ptp_data->lock);
 
-	now = ptp_data->tstamp_cc.read(&ptp_data->tstamp_cc);
+	rc = sja1105_ptpclkval_read(priv, &ticks);
+	if (rc < 0) {
+		dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
+		kfree_skb(skb);
+		goto out;
+	}
 
 	rc = sja1105_ptpegr_ts_poll(ds, slot, &ts);
 	if (rc < 0) {
@@ -621,10 +618,9 @@ void sja1105_ptp_txtstamp_skb(struct dsa_switch *ds, int slot,
 		goto out;
 	}
 
-	ts = sja1105_tstamp_reconstruct(ds, now, ts);
-	ts = timecounter_cyc2time(&ptp_data->tstamp_tc, ts);
+	ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
 
-	shwt.hwtstamp = ns_to_ktime(ts);
+	shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
 	skb_complete_tx_timestamp(skb, &shwt);
 
 out: