Skip to content

Commit

Permalink
net: dsa: sja1105: Switch to hardware operations for PTP
Browse files Browse the repository at this point in the history 
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 <[email protected]>
Signed-off-by: David S. Miller <[email protected]>
  • Loading branch information
@vladimiroltean @davem330
vladimiroltean authored and davem330 committed Oct 18, 2019
1 parent d9496f3 commit 2fb079a
Show file tree
Hide file tree
Showing 4 changed files with 128 additions and 124 deletions.
3 changes: 1 addition & 2 deletions drivers/net/dsa/sja1105/sja1105.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,9 +33,8 @@ struct sja1105_regs {
u64 config;
u64 rmii_pll1;
u64 ptp_control;
u64 ptpclk;
u64 ptpclkval;
u64 ptpclkrate;
u64 ptptsclk;
u64 ptpegr_ts[SJA1105_NUM_PORTS];
u64 pad_mii_tx[SJA1105_NUM_PORTS];
u64 pad_mii_id[SJA1105_NUM_PORTS];
Expand Down
218 changes: 107 additions & 111 deletions drivers/net/dsa/sja1105/sja1105_ptp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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).
Expand All @@ -41,21 +23,23 @@
*
* 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
* ptpclkrate = scaled_ppm * 2^9 / 5^6
*/
#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)

Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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) \
Expand All @@ -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);
}

Expand Down Expand Up @@ -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;
Expand All @@ -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)
Expand All @@ -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",
Expand All @@ -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);
}
Expand All @@ -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;
}
Expand All @@ -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) {
Expand All @@ -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:
Expand Down
Loading

0 comments on commit 2fb079a

Please sign in to comment.