#include "stmmac.h"
#include "stmmac_est.h"
#include "stmmac_ptp.h"
#define PTP_SAFE_TIME_OFFSET_NS 500000
static int stmmac_adjust_freq(struct ptp_clock_info *ptp, long scaled_ppm)
{
struct stmmac_priv *priv =
container_of(ptp, struct stmmac_priv, ptp_clock_ops);
unsigned long flags;
u32 addend;
addend = adjust_by_scaled_ppm(priv->default_addend, scaled_ppm);
write_lock_irqsave(&priv->ptp_lock, flags);
stmmac_config_addend(priv, priv->ptpaddr, addend);
write_unlock_irqrestore(&priv->ptp_lock, flags);
return 0;
}
static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
{
struct stmmac_priv *priv =
container_of(ptp, struct stmmac_priv, ptp_clock_ops);
unsigned long flags;
u32 sec, nsec;
u32 quotient, reminder;
int neg_adj = 0;
bool xmac;
xmac = dwmac_is_xmac(priv->plat->core_type);
if (delta < 0) {
neg_adj = 1;
delta = -delta;
}
quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
sec = quotient;
nsec = reminder;
mutex_lock(&priv->est_lock);
if (priv->est && priv->est->enable)
stmmac_est_configure(priv, priv, priv->est,
priv->plat->clk_ptp_rate, false);
mutex_unlock(&priv->est_lock);
write_lock_irqsave(&priv->ptp_lock, flags);
stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);
write_unlock_irqrestore(&priv->ptp_lock, flags);
mutex_lock(&priv->est_lock);
if (priv->est && priv->est->enable)
__stmmac_setup_est(priv);
mutex_unlock(&priv->est_lock);
return 0;
}
static int stmmac_get_timex(struct ptp_clock_info *ptp, struct timespec64 *ts,
struct ptp_system_timestamp *sts)
{
struct stmmac_priv *priv =
container_of(ptp, struct stmmac_priv, ptp_clock_ops);
unsigned long flags;
u64 ns = 0;
read_lock_irqsave(&priv->ptp_lock, flags);
stmmac_get_systime(priv, priv->ptpaddr, &ns, sts);
read_unlock_irqrestore(&priv->ptp_lock, flags);
*ts = ns_to_timespec64(ns);
return 0;
}
static int stmmac_set_time(struct ptp_clock_info *ptp,
const struct timespec64 *ts)
{
struct stmmac_priv *priv =
container_of(ptp, struct stmmac_priv, ptp_clock_ops);
unsigned long flags;
write_lock_irqsave(&priv->ptp_lock, flags);
stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
write_unlock_irqrestore(&priv->ptp_lock, flags);
return 0;
}
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;
int ret = -EOPNOTSUPP;
unsigned long flags;
u32 acr_value;
switch (rq->type) {
case PTP_CLK_REQ_PEROUT: {
struct timespec64 curr_time;
u64 target_ns = 0;
u64 ns = 0;
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;
if (!cfg->start.tv_sec && cfg->start.tv_nsec < PTP_SAFE_TIME_OFFSET_NS)
cfg->start.tv_nsec += PTP_SAFE_TIME_OFFSET_NS;
target_ns = cfg->start.tv_nsec + ((u64)cfg->start.tv_sec * NSEC_PER_SEC);
stmmac_get_systime(priv, priv->ptpaddr, &ns, NULL);
if (ns > TIME64_MAX - PTP_SAFE_TIME_OFFSET_NS)
return -EINVAL;
curr_time = ns_to_timespec64(ns);
if (target_ns < ns + PTP_SAFE_TIME_OFFSET_NS) {
cfg->start = timespec64_add_safe(cfg->start, curr_time);
if (cfg->start.tv_sec == TIME64_MAX)
return -EINVAL;
}
cfg->period.tv_sec = rq->perout.period.sec;
cfg->period.tv_nsec = rq->perout.period.nsec;
write_lock_irqsave(&priv->ptp_lock, flags);
ret = stmmac_flex_pps_config(priv, priv->ioaddr,
rq->perout.index, cfg, on,
priv->sub_second_inc,
priv->systime_flags);
write_unlock_irqrestore(&priv->ptp_lock, flags);
break;
}
case PTP_CLK_REQ_EXTTS: {
u8 channel;
mutex_lock(&priv->aux_ts_lock);
acr_value = readl(ptpaddr + PTP_ACR);
channel = ilog2(FIELD_GET(PTP_ACR_MASK, acr_value));
acr_value &= ~PTP_ACR_MASK;
if (on) {
if (FIELD_GET(PTP_ACR_MASK, acr_value)) {
netdev_err(priv->dev,
"Cannot enable auxiliary snapshot %d as auxiliary snapshot %d is already enabled",
rq->extts.index, channel);
mutex_unlock(&priv->aux_ts_lock);
return -EBUSY;
}
priv->plat->flags |= STMMAC_FLAG_EXT_SNAPSHOT_EN;
acr_value |= PTP_ACR_ATSEN(rq->extts.index);
acr_value |= PTP_ACR_ATSFC;
} else {
priv->plat->flags &= ~STMMAC_FLAG_EXT_SNAPSHOT_EN;
}
netdev_dbg(priv->dev, "Auxiliary Snapshot %d %s.\n",
rq->extts.index, on ? "enabled" : "disabled");
writel(acr_value, ptpaddr + PTP_ACR);
mutex_unlock(&priv->aux_ts_lock);
ret = readl_poll_timeout(ptpaddr + PTP_ACR, acr_value,
!(acr_value & PTP_ACR_ATSFC),
10, 10000);
break;
}
default:
break;
}
return ret;
}
static int stmmac_get_syncdevicetime(ktime_t *device,
struct system_counterval_t *system,
void *ctx)
{
struct stmmac_priv *priv = (struct stmmac_priv *)ctx;
return priv->plat->crosststamp(device, system, ctx);
}
static int stmmac_getcrosststamp(struct ptp_clock_info *ptp,
struct system_device_crosststamp *xtstamp)
{
struct stmmac_priv *priv =
container_of(ptp, struct stmmac_priv, ptp_clock_ops);
return get_device_system_crosststamp(stmmac_get_syncdevicetime,
priv, NULL, xtstamp);
}
const struct ptp_clock_info stmmac_ptp_clock_ops = {
.owner = THIS_MODULE,
.name = "stmmac ptp",
.max_adj = 62500000,
.n_alarm = 0,
.n_ext_ts = 0,
.n_per_out = 0,
.n_pins = 0,
.pps = 0,
.supported_extts_flags = PTP_RISING_EDGE | PTP_STRICT_FLAGS,
.adjfine = stmmac_adjust_freq,
.adjtime = stmmac_adjust_time,
.gettimex64 = stmmac_get_timex,
.settime64 = stmmac_set_time,
.enable = stmmac_enable,
};
const struct ptp_clock_info dwmac1000_ptp_clock_ops = {
.owner = THIS_MODULE,
.name = "stmmac ptp",
.max_adj = 62500000,
.n_alarm = 0,
.n_ext_ts = 1,
.n_per_out = 0,
.n_pins = 0,
.pps = 0,
.supported_extts_flags = PTP_RISING_EDGE | PTP_STRICT_FLAGS,
.adjfine = stmmac_adjust_freq,
.adjtime = stmmac_adjust_time,
.gettimex64 = stmmac_get_timex,
.settime64 = stmmac_set_time,
.enable = dwmac1000_ptp_enable,
};
void stmmac_ptp_register(struct stmmac_priv *priv)
{
unsigned int pps_out_num = priv->dma_cap.pps_out_num;
unsigned int n_ext_ts;
if (pps_out_num > STMMAC_PPS_MAX) {
dev_warn(priv->device,
"pps outputs (%u) exceeds driver maximum, limiting to %u\n",
pps_out_num, STMMAC_PPS_MAX);
pps_out_num = STMMAC_PPS_MAX;
}
priv->plat->cdc_error_adj = 0;
if (priv->plat->core_type == DWMAC_CORE_GMAC4)
priv->plat->cdc_error_adj = (2 * NSEC_PER_SEC) / priv->plat->clk_ptp_rate;
if (pps_out_num)
priv->ptp_clock_ops.n_per_out = pps_out_num;
n_ext_ts = priv->dma_cap.aux_snapshot_n;
if (n_ext_ts)
priv->ptp_clock_ops.n_ext_ts = n_ext_ts;
if (priv->plat->ptp_max_adj)
priv->ptp_clock_ops.max_adj = priv->plat->ptp_max_adj;
if (priv->plat->crosststamp)
priv->ptp_clock_ops.getcrosststamp = stmmac_getcrosststamp;
mutex_init(&priv->aux_ts_lock);
priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
priv->device);
if (IS_ERR(priv->ptp_clock)) {
netdev_err(priv->dev, "ptp_clock_register failed\n");
priv->ptp_clock = NULL;
}
if (priv->ptp_clock)
netdev_info(priv->dev, "registered PTP clock\n");
else
mutex_destroy(&priv->aux_ts_lock);
}
void stmmac_ptp_unregister(struct stmmac_priv *priv)
{
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->dev->name);
mutex_destroy(&priv->aux_ts_lock);
}
}