342 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*******************************************************************************3 PTP 1588 clock using the STMMAC.4 5 Copyright (C) 2013 Vayavya Labs Pvt Ltd6 7 8 Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>9*******************************************************************************/10#include "stmmac.h"11#include "stmmac_ptp.h"12#include "dwmac4.h"13 14/**15 * stmmac_adjust_freq16 *17 * @ptp: pointer to ptp_clock_info structure18 * @scaled_ppm: desired period change in scaled parts per million19 *20 * Description: this function will adjust the frequency of hardware clock.21 *22 * Scaled parts per million is ppm with a 16-bit binary fractional field.23 */24static int stmmac_adjust_freq(struct ptp_clock_info *ptp, long scaled_ppm)25{26 struct stmmac_priv *priv =27 container_of(ptp, struct stmmac_priv, ptp_clock_ops);28 unsigned long flags;29 u32 addend;30 31 addend = adjust_by_scaled_ppm(priv->default_addend, scaled_ppm);32 33 write_lock_irqsave(&priv->ptp_lock, flags);34 stmmac_config_addend(priv, priv->ptpaddr, addend);35 write_unlock_irqrestore(&priv->ptp_lock, flags);36 37 return 0;38}39 40/**41 * stmmac_adjust_time42 *43 * @ptp: pointer to ptp_clock_info structure44 * @delta: desired change in nanoseconds45 *46 * Description: this function will shift/adjust the hardware clock time.47 */48static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)49{50 struct stmmac_priv *priv =51 container_of(ptp, struct stmmac_priv, ptp_clock_ops);52 unsigned long flags;53 u32 sec, nsec;54 u32 quotient, reminder;55 int neg_adj = 0;56 bool xmac, est_rst = false;57 int ret;58 59 xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac;60 61 if (delta < 0) {62 neg_adj = 1;63 delta = -delta;64 }65 66 quotient = div_u64_rem(delta, 1000000000ULL, &reminder);67 sec = quotient;68 nsec = reminder;69 70 /* If EST is enabled, disabled it before adjust ptp time. */71 if (priv->est && priv->est->enable) {72 est_rst = true;73 mutex_lock(&priv->est_lock);74 priv->est->enable = false;75 stmmac_est_configure(priv, priv, priv->est,76 priv->plat->clk_ptp_rate);77 mutex_unlock(&priv->est_lock);78 }79 80 write_lock_irqsave(&priv->ptp_lock, flags);81 stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);82 write_unlock_irqrestore(&priv->ptp_lock, flags);83 84 /* Calculate new basetime and re-configured EST after PTP time adjust. */85 if (est_rst) {86 struct timespec64 current_time, time;87 ktime_t current_time_ns, basetime;88 u64 cycle_time;89 90 mutex_lock(&priv->est_lock);91 priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, ¤t_time);92 current_time_ns = timespec64_to_ktime(current_time);93 time.tv_nsec = priv->est->btr_reserve[0];94 time.tv_sec = priv->est->btr_reserve[1];95 basetime = timespec64_to_ktime(time);96 cycle_time = (u64)priv->est->ctr[1] * NSEC_PER_SEC +97 priv->est->ctr[0];98 time = stmmac_calc_tas_basetime(basetime,99 current_time_ns,100 cycle_time);101 102 priv->est->btr[0] = (u32)time.tv_nsec;103 priv->est->btr[1] = (u32)time.tv_sec;104 priv->est->enable = true;105 ret = stmmac_est_configure(priv, priv, priv->est,106 priv->plat->clk_ptp_rate);107 mutex_unlock(&priv->est_lock);108 if (ret)109 netdev_err(priv->dev, "failed to configure EST\n");110 }111 112 return 0;113}114 115/**116 * stmmac_get_time117 *118 * @ptp: pointer to ptp_clock_info structure119 * @ts: pointer to hold time/result120 *121 * Description: this function will read the current time from the122 * hardware clock and store it in @ts.123 */124static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)125{126 struct stmmac_priv *priv =127 container_of(ptp, struct stmmac_priv, ptp_clock_ops);128 unsigned long flags;129 u64 ns = 0;130 131 read_lock_irqsave(&priv->ptp_lock, flags);132 stmmac_get_systime(priv, priv->ptpaddr, &ns);133 read_unlock_irqrestore(&priv->ptp_lock, flags);134 135 *ts = ns_to_timespec64(ns);136 137 return 0;138}139 140/**141 * stmmac_set_time142 *143 * @ptp: pointer to ptp_clock_info structure144 * @ts: time value to set145 *146 * Description: this function will set the current time on the147 * hardware clock.148 */149static int stmmac_set_time(struct ptp_clock_info *ptp,150 const struct timespec64 *ts)151{152 struct stmmac_priv *priv =153 container_of(ptp, struct stmmac_priv, ptp_clock_ops);154 unsigned long flags;155 156 write_lock_irqsave(&priv->ptp_lock, flags);157 stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);158 write_unlock_irqrestore(&priv->ptp_lock, flags);159 160 return 0;161}162 163static int stmmac_enable(struct ptp_clock_info *ptp,164 struct ptp_clock_request *rq, int on)165{166 struct stmmac_priv *priv =167 container_of(ptp, struct stmmac_priv, ptp_clock_ops);168 void __iomem *ptpaddr = priv->ptpaddr;169 struct stmmac_pps_cfg *cfg;170 int ret = -EOPNOTSUPP;171 unsigned long flags;172 u32 acr_value;173 174 switch (rq->type) {175 case PTP_CLK_REQ_PEROUT:176 /* Reject requests with unsupported flags */177 if (rq->perout.flags)178 return -EOPNOTSUPP;179 180 cfg = &priv->pps[rq->perout.index];181 182 cfg->start.tv_sec = rq->perout.start.sec;183 cfg->start.tv_nsec = rq->perout.start.nsec;184 cfg->period.tv_sec = rq->perout.period.sec;185 cfg->period.tv_nsec = rq->perout.period.nsec;186 187 write_lock_irqsave(&priv->ptp_lock, flags);188 ret = stmmac_flex_pps_config(priv, priv->ioaddr,189 rq->perout.index, cfg, on,190 priv->sub_second_inc,191 priv->systime_flags);192 write_unlock_irqrestore(&priv->ptp_lock, flags);193 break;194 case PTP_CLK_REQ_EXTTS: {195 u8 channel;196 197 mutex_lock(&priv->aux_ts_lock);198 acr_value = readl(ptpaddr + PTP_ACR);199 channel = ilog2(FIELD_GET(PTP_ACR_MASK, acr_value));200 acr_value &= ~PTP_ACR_MASK;201 202 if (on) {203 if (FIELD_GET(PTP_ACR_MASK, acr_value)) {204 netdev_err(priv->dev,205 "Cannot enable auxiliary snapshot %d as auxiliary snapshot %d is already enabled",206 rq->extts.index, channel);207 mutex_unlock(&priv->aux_ts_lock);208 return -EBUSY;209 }210 211 priv->plat->flags |= STMMAC_FLAG_EXT_SNAPSHOT_EN;212 213 /* Enable External snapshot trigger */214 acr_value |= PTP_ACR_ATSEN(rq->extts.index);215 acr_value |= PTP_ACR_ATSFC;216 } else {217 priv->plat->flags &= ~STMMAC_FLAG_EXT_SNAPSHOT_EN;218 }219 netdev_dbg(priv->dev, "Auxiliary Snapshot %d %s.\n",220 rq->extts.index, on ? "enabled" : "disabled");221 writel(acr_value, ptpaddr + PTP_ACR);222 mutex_unlock(&priv->aux_ts_lock);223 /* wait for auxts fifo clear to finish */224 ret = readl_poll_timeout(ptpaddr + PTP_ACR, acr_value,225 !(acr_value & PTP_ACR_ATSFC),226 10, 10000);227 break;228 }229 230 default:231 break;232 }233 234 return ret;235}236 237/**238 * stmmac_get_syncdevicetime239 * @device: current device time240 * @system: system counter value read synchronously with device time241 * @ctx: context provided by timekeeping code242 * Description: Read device and system clock simultaneously and return the243 * corrected clock values in ns.244 **/245static int stmmac_get_syncdevicetime(ktime_t *device,246 struct system_counterval_t *system,247 void *ctx)248{249 struct stmmac_priv *priv = (struct stmmac_priv *)ctx;250 251 if (priv->plat->crosststamp)252 return priv->plat->crosststamp(device, system, ctx);253 else254 return -EOPNOTSUPP;255}256 257static int stmmac_getcrosststamp(struct ptp_clock_info *ptp,258 struct system_device_crosststamp *xtstamp)259{260 struct stmmac_priv *priv =261 container_of(ptp, struct stmmac_priv, ptp_clock_ops);262 263 return get_device_system_crosststamp(stmmac_get_syncdevicetime,264 priv, NULL, xtstamp);265}266 267/* structure describing a PTP hardware clock */268static struct ptp_clock_info stmmac_ptp_clock_ops = {269 .owner = THIS_MODULE,270 .name = "stmmac ptp",271 .max_adj = 62500000,272 .n_alarm = 0,273 .n_ext_ts = 0, /* will be overwritten in stmmac_ptp_register */274 .n_per_out = 0, /* will be overwritten in stmmac_ptp_register */275 .n_pins = 0,276 .pps = 0,277 .adjfine = stmmac_adjust_freq,278 .adjtime = stmmac_adjust_time,279 .gettime64 = stmmac_get_time,280 .settime64 = stmmac_set_time,281 .enable = stmmac_enable,282 .getcrosststamp = stmmac_getcrosststamp,283};284 285/**286 * stmmac_ptp_register287 * @priv: driver private structure288 * Description: this function will register the ptp clock driver289 * to kernel. It also does some house keeping work.290 */291void stmmac_ptp_register(struct stmmac_priv *priv)292{293 int i;294 295 for (i = 0; i < priv->dma_cap.pps_out_num; i++) {296 if (i >= STMMAC_PPS_MAX)297 break;298 priv->pps[i].available = true;299 }300 301 if (priv->plat->ptp_max_adj)302 stmmac_ptp_clock_ops.max_adj = priv->plat->ptp_max_adj;303 304 /* Calculate the clock domain crossing (CDC) error if necessary */305 priv->plat->cdc_error_adj = 0;306 if (priv->plat->has_gmac4 && priv->plat->clk_ptp_rate)307 priv->plat->cdc_error_adj = (2 * NSEC_PER_SEC) / priv->plat->clk_ptp_rate;308 309 stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num;310 stmmac_ptp_clock_ops.n_ext_ts = priv->dma_cap.aux_snapshot_n;311 312 rwlock_init(&priv->ptp_lock);313 mutex_init(&priv->aux_ts_lock);314 priv->ptp_clock_ops = stmmac_ptp_clock_ops;315 316 priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,317 priv->device);318 if (IS_ERR(priv->ptp_clock)) {319 netdev_err(priv->dev, "ptp_clock_register failed\n");320 priv->ptp_clock = NULL;321 } else if (priv->ptp_clock)322 netdev_info(priv->dev, "registered PTP clock\n");323}324 325/**326 * stmmac_ptp_unregister327 * @priv: driver private structure328 * Description: this function will remove/unregister the ptp clock driver329 * from the kernel.330 */331void stmmac_ptp_unregister(struct stmmac_priv *priv)332{333 if (priv->ptp_clock) {334 ptp_clock_unregister(priv->ptp_clock);335 priv->ptp_clock = NULL;336 pr_debug("Removed PTP HW clock successfully on %s\n",337 priv->dev->name);338 }339 340 mutex_destroy(&priv->aux_ts_lock);341}342