@@ -340,6 +340,12 @@ struct stmmac_priv {
int use_riwt;
int irq_wake;
rwlock_t ptp_lock;
+ /* Serialize PHC changes with a hardware reset; gettime uses ptp_lock. */
+ struct mutex ptp_mutex;
+ bool ptp_blocked;
+ long ptp_scaled_ppm;
+ u32 ptp_perout;
+ u32 ptp_extts;
/* Protects auxiliary snapshot registers from concurrent access. */
struct mutex aux_ts_lock;
wait_queue_head_t tstamp_busy_wait;
@@ -407,6 +413,7 @@ void stmmac_set_ethtool_ops(struct net_device *netdev);
void stmmac_ptp_register(struct stmmac_priv *priv);
void stmmac_ptp_unregister(struct stmmac_priv *priv);
+int stmmac_ptp_restore(struct stmmac_priv *priv);
int stmmac_xdp_open(struct net_device *dev);
void stmmac_xdp_release(struct net_device *dev);
int stmmac_get_phy_intf_sel(phy_interface_t interface);
@@ -607,6 +607,7 @@ static void stmmac_update_subsecond_increment(struct stmmac_priv *priv)
bool xmac = dwmac_is_xmac(priv->plat->core_type);
u32 sec_inc = 0;
u64 temp = 0;
+ u32 addend;
stmmac_config_hw_tstamping(priv, priv->ptpaddr, priv->systime_flags);
@@ -626,7 +627,8 @@ static void stmmac_update_subsecond_increment(struct stmmac_priv *priv)
*/
temp = (u64)(temp << 32);
priv->default_addend = div_u64(temp, priv->plat->clk_ptp_rate);
- stmmac_config_addend(priv, priv->ptpaddr, priv->default_addend);
+ addend = adjust_by_scaled_ppm(priv->default_addend, priv->ptp_scaled_ppm);
+ stmmac_config_addend(priv, priv->ptpaddr, addend);
}
/**
@@ -954,6 +956,11 @@ static int stmmac_setup_ptp(struct stmmac_priv *priv)
return 0;
}
+ priv->ptp_scaled_ppm = 0;
+ priv->ptp_perout = 0;
+ priv->ptp_extts = 0;
+ priv->ptp_blocked = false;
+
ret = clk_prepare_enable(priv->plat->clk_ptp_ref);
if (ret < 0) {
netdev_warn(priv->dev,
@@ -6329,7 +6336,8 @@ static void stmmac_common_interrupt(struct stmmac_priv *priv)
for (queue = 0; queue < queues_count; queue++)
stmmac_host_mtl_irq_status(priv, priv->hw, queue);
- stmmac_timestamp_interrupt(priv, priv);
+ if (!READ_ONCE(priv->ptp_blocked))
+ stmmac_timestamp_interrupt(priv, priv);
}
}
@@ -7762,6 +7770,14 @@ static int stmmac_dl_ts_coarse_set(struct devlink *dl, u32 id,
{
struct stmmac_devlink_priv *dl_priv = devlink_priv(dl);
struct stmmac_priv *priv = dl_priv->stmmac_priv;
+ unsigned long flags;
+
+ mutex_lock(&priv->ptp_mutex);
+ if (priv->ptp_blocked) {
+ mutex_unlock(&priv->ptp_mutex);
+ return -EBUSY;
+ }
+ write_lock_irqsave(&priv->ptp_lock, flags);
priv->tsfupdt_coarse = ctx->val.vbool;
@@ -7774,6 +7790,8 @@ static int stmmac_dl_ts_coarse_set(struct devlink *dl, u32 id,
* reconfigure the systime, subsecond increment and addend.
*/
stmmac_update_subsecond_increment(priv);
+ write_unlock_irqrestore(&priv->ptp_lock, flags);
+ mutex_unlock(&priv->ptp_mutex);
return 0;
}
@@ -8114,6 +8132,7 @@ static int __stmmac_dvr_probe(struct device *device,
mutex_init(&priv->lock);
mutex_init(&priv->est_lock);
rwlock_init(&priv->ptp_lock);
+ mutex_init(&priv->ptp_mutex);
stmmac_fpe_init(priv);
@@ -13,6 +13,16 @@
#define PTP_SAFE_TIME_OFFSET_NS 500000
+static int stmmac_ptp_begin(struct stmmac_priv *priv)
+{
+ mutex_lock(&priv->ptp_mutex);
+ if (priv->ptp_blocked) {
+ mutex_unlock(&priv->ptp_mutex);
+ return -EBUSY;
+ }
+ return 0;
+}
+
/**
* stmmac_adjust_freq
*
@@ -29,14 +39,21 @@ static int stmmac_adjust_freq(struct ptp_clock_info *ptp, long scaled_ppm)
container_of(ptp, struct stmmac_priv, ptp_clock_ops);
unsigned long flags;
u32 addend;
+ int ret;
- addend = adjust_by_scaled_ppm(priv->default_addend, scaled_ppm);
+ ret = stmmac_ptp_begin(priv);
+ if (ret)
+ return ret;
write_lock_irqsave(&priv->ptp_lock, flags);
- stmmac_config_addend(priv, priv->ptpaddr, addend);
+ addend = adjust_by_scaled_ppm(priv->default_addend, scaled_ppm);
+ ret = stmmac_config_addend(priv, priv->ptpaddr, addend);
+ if (!ret)
+ priv->ptp_scaled_ppm = scaled_ppm;
write_unlock_irqrestore(&priv->ptp_lock, flags);
+ mutex_unlock(&priv->ptp_mutex);
- return 0;
+ return ret;
}
/**
@@ -55,7 +72,12 @@ static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
u32 sec, nsec;
u32 quotient, reminder;
int neg_adj = 0;
- bool xmac, est_rst = false;
+ bool xmac;
+ int ret, err;
+
+ ret = stmmac_ptp_begin(priv);
+ if (ret)
+ return ret;
xmac = dwmac_is_xmac(priv->plat->core_type);
@@ -68,24 +90,32 @@ static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
sec = quotient;
nsec = reminder;
- /* If EST is enabled, disabled it before adjust ptp time. */
+ /* Keep the installed schedule stable across the entire clock step. */
mutex_lock(&priv->est_lock);
if (priv->est.enable) {
- est_rst = true;
- stmmac_est_configure(priv, priv, &priv->est,
- priv->plat->clk_ptp_rate, false);
+ ret = stmmac_est_configure(priv, priv, &priv->est,
+ priv->plat->clk_ptp_rate, false);
+ if (ret)
+ goto out_unlock;
}
- mutex_unlock(&priv->est_lock);
write_lock_irqsave(&priv->ptp_lock, flags);
- stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);
+ ret = stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);
write_unlock_irqrestore(&priv->ptp_lock, flags);
- /* Calculate new basetime and re-configured EST after PTP time adjust. */
- if (est_rst)
- stmmac_setup_est(priv);
+ /* Also try to restore EST after a failed clock update. Keep the first
+ * error, but do not report success if only schedule replay failed.
+ */
+ if (priv->est.enable) {
+ err = __stmmac_setup_est(priv);
+ if (!ret)
+ ret = err;
+ }
- return 0;
+out_unlock:
+ mutex_unlock(&priv->est_lock);
+ mutex_unlock(&priv->ptp_mutex);
+ return ret;
}
/**
@@ -103,14 +133,18 @@ static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
container_of(ptp, struct stmmac_priv, ptp_clock_ops);
unsigned long flags;
u64 ns = 0;
+ int ret = 0;
read_lock_irqsave(&priv->ptp_lock, flags);
- stmmac_get_systime(priv, priv->ptpaddr, &ns);
+ if (priv->ptp_blocked)
+ ret = -EBUSY;
+ else
+ stmmac_get_systime(priv, priv->ptpaddr, &ns);
read_unlock_irqrestore(&priv->ptp_lock, flags);
*ts = ns_to_timespec64(ns);
- return 0;
+ return ret;
}
/**
@@ -128,25 +162,34 @@ static int stmmac_set_time(struct ptp_clock_info *ptp,
struct stmmac_priv *priv =
container_of(ptp, struct stmmac_priv, ptp_clock_ops);
unsigned long flags;
+ int ret;
+
+ ret = stmmac_ptp_begin(priv);
+ if (ret)
+ return ret;
write_lock_irqsave(&priv->ptp_lock, flags);
- stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
+ ret = stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
write_unlock_irqrestore(&priv->ptp_lock, flags);
+ mutex_unlock(&priv->ptp_mutex);
- return 0;
+ return ret;
}
-static int stmmac_enable(struct ptp_clock_info *ptp,
- struct ptp_clock_request *rq, int on)
+static int __stmmac_enable(struct ptp_clock_info *ptp,
+ struct ptp_clock_request *rq, int on)
{
struct stmmac_priv *priv =
container_of(ptp, struct stmmac_priv, ptp_clock_ops);
void __iomem *ptpaddr = priv->ptpaddr;
- struct stmmac_pps_cfg *cfg;
+ struct stmmac_pps_cfg pps, saved, *cfg = &pps;
int ret = -EOPNOTSUPP;
unsigned long flags;
u32 acr_value;
+ if (priv->plat->core_type == DWMAC_CORE_GMAC)
+ return dwmac1000_ptp_enable(ptp, rq, on);
+
switch (rq->type) {
case PTP_CLK_REQ_PEROUT: {
struct timespec64 curr_time;
@@ -157,8 +200,6 @@ static int stmmac_enable(struct ptp_clock_info *ptp,
if (rq->perout.flags)
return -EOPNOTSUPP;
- cfg = &priv->pps[rq->perout.index];
-
cfg->start.tv_sec = rq->perout.start.sec;
cfg->start.tv_nsec = rq->perout.start.nsec;
@@ -188,6 +229,7 @@ static int stmmac_enable(struct ptp_clock_info *ptp,
cfg->period.tv_sec = rq->perout.period.sec;
cfg->period.tv_nsec = rq->perout.period.nsec;
+ saved = *cfg;
write_lock_irqsave(&priv->ptp_lock, flags);
ret = stmmac_flex_pps_config(priv, priv->ioaddr,
@@ -195,6 +237,9 @@ static int stmmac_enable(struct ptp_clock_info *ptp,
priv->sub_second_inc,
priv->systime_flags);
write_unlock_irqrestore(&priv->ptp_lock, flags);
+ /* Some cores convert cfg->start to binary rollover units. */
+ if (!ret)
+ priv->pps[rq->perout.index] = saved;
break;
}
case PTP_CLK_REQ_EXTTS: {
@@ -240,6 +285,64 @@ static int stmmac_enable(struct ptp_clock_info *ptp,
return ret;
}
+static int stmmac_enable(struct ptp_clock_info *ptp,
+ struct ptp_clock_request *rq, int on)
+{
+ struct stmmac_priv *priv =
+ container_of(ptp, struct stmmac_priv, ptp_clock_ops);
+ int ret;
+
+ ret = stmmac_ptp_begin(priv);
+ if (ret)
+ return ret;
+ ret = __stmmac_enable(ptp, rq, on);
+ if (!ret) {
+ if (rq->type == PTP_CLK_REQ_PEROUT) {
+ if (on)
+ priv->ptp_perout |= BIT(rq->perout.index);
+ else
+ priv->ptp_perout &= ~BIT(rq->perout.index);
+ } else if (rq->type == PTP_CLK_REQ_EXTTS) {
+ priv->ptp_extts = on ? BIT(rq->extts.index) : 0;
+ }
+ }
+ mutex_unlock(&priv->ptp_mutex);
+ return ret;
+}
+
+/* Called with ptp_mutex held and PHC access blocked across the MAC reset. */
+int stmmac_ptp_restore(struct stmmac_priv *priv)
+{
+ struct ptp_clock_request rq = { .type = PTP_CLK_REQ_EXTTS };
+ unsigned long flags;
+ u64 ns = 0, period;
+ u32 addend;
+ int i, ret;
+
+ write_lock_irqsave(&priv->ptp_lock, flags);
+ addend = adjust_by_scaled_ppm(priv->default_addend, priv->ptp_scaled_ppm);
+ ret = stmmac_config_addend(priv, priv->ptpaddr, addend);
+ for (i = 0; !ret && i < STMMAC_PPS_MAX; i++) {
+ struct stmmac_pps_cfg cfg = priv->pps[i];
+
+ if (!(priv->ptp_perout & BIT(i)))
+ continue;
+ stmmac_get_systime(priv, priv->ptpaddr, &ns);
+ period = timespec64_to_ns(&cfg.period);
+ /* Retain phase, but move an expired target into the future. */
+ cfg.start = stmmac_calc_tas_basetime(timespec64_to_ktime(cfg.start),
+ ns + PTP_SAFE_TIME_OFFSET_NS, period);
+ ret = stmmac_flex_pps_config(priv, priv->ioaddr, i, &cfg, true,
+ priv->sub_second_inc, priv->systime_flags);
+ }
+ write_unlock_irqrestore(&priv->ptp_lock, flags);
+ if (ret || !priv->ptp_extts)
+ return ret;
+
+ rq.extts.index = __ffs(priv->ptp_extts);
+ return __stmmac_enable(&priv->ptp_clock_ops, &rq, 1);
+}
+
/**
* stmmac_get_syncdevicetime
* @device: current device time
@@ -262,9 +365,15 @@ static int stmmac_getcrosststamp(struct ptp_clock_info *ptp,
{
struct stmmac_priv *priv =
container_of(ptp, struct stmmac_priv, ptp_clock_ops);
-
- return get_device_system_crosststamp(stmmac_get_syncdevicetime,
- priv, NULL, xtstamp);
+ int ret;
+
+ ret = stmmac_ptp_begin(priv);
+ if (ret)
+ return ret;
+ ret = get_device_system_crosststamp(stmmac_get_syncdevicetime,
+ priv, NULL, xtstamp);
+ mutex_unlock(&priv->ptp_mutex);
+ return ret;
}
/* structure describing a PTP hardware clock */
@@ -298,7 +407,7 @@ const struct ptp_clock_info dwmac1000_ptp_clock_ops = {
.adjtime = stmmac_adjust_time,
.gettime64 = stmmac_get_time,
.settime64 = stmmac_set_time,
- .enable = dwmac1000_ptp_enable,
+ .enable = stmmac_enable,
};
/**