stmmac_ptp.c

  1// SPDX-License-Identifier: GPL-2.0-only
  2/*******************************************************************************
  3  PTP 1588 clock using the STMMAC.
  4
  5  Copyright (C) 2013  Vayavya Labs Pvt Ltd
  6
  7
  8  Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
  9*******************************************************************************/
 10#include "stmmac.h"
 11#include "stmmac_ptp.h"
 12
 13#define PTP_SAFE_TIME_OFFSET_NS	500000
 14
 15/**
 16 * stmmac_adjust_freq
 17 *
 18 * @ptp: pointer to ptp_clock_info structure
 19 * @scaled_ppm: desired period change in scaled parts per million
 20 *
 21 * Description: this function will adjust the frequency of hardware clock.
 22 *
 23 * Scaled parts per million is ppm with a 16-bit binary fractional field.
 24 */
 25static int stmmac_adjust_freq(struct ptp_clock_info *ptp, long scaled_ppm)
 26{
 27	struct stmmac_priv *priv =
 28	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 29	unsigned long flags;
 30	u32 addend;
 31
 32	addend = adjust_by_scaled_ppm(priv->default_addend, scaled_ppm);
 33
 34	write_lock_irqsave(&priv->ptp_lock, flags);
 35	stmmac_config_addend(priv, priv->ptpaddr, addend);
 36	write_unlock_irqrestore(&priv->ptp_lock, flags);
 37
 38	return 0;
 39}
 40
 41/**
 42 * stmmac_adjust_time
 43 *
 44 * @ptp: pointer to ptp_clock_info structure
 45 * @delta: desired change in nanoseconds
 46 *
 47 * Description: this function will shift/adjust the hardware clock time.
 48 */
 49static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
 50{
 51	struct stmmac_priv *priv =
 52	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
 53	unsigned long flags;
 54	u32 sec, nsec;
 55	u32 quotient, reminder;
 56	int neg_adj = 0;
 57	bool xmac, est_rst = false;
 58	int ret;
 59
 60	xmac = dwmac_is_xmac(priv->plat->core_type);
 61
 62	if (delta < 0) {
 63		neg_adj = 1;
 64		delta = -delta;
 65	}
 66
 67	quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
 68	sec = quotient;
 69	nsec = reminder;
 70
 71	/* If EST is enabled, disabled it before adjust ptp time. */
 72	if (priv->est && priv->est->enable) {
 73		est_rst = true;
 74		mutex_lock(&priv->est_lock);
 75		priv->est->enable = false;
 76		stmmac_est_configure(priv, priv, priv->est,
 77				     priv->plat->clk_ptp_rate);
 78		mutex_unlock(&priv->est_lock);
 79	}
 80
 81	write_lock_irqsave(&priv->ptp_lock, flags);
 82	stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);
 83	write_unlock_irqrestore(&priv->ptp_lock, flags);
 84
 85	/* Calculate new basetime and re-configured EST after PTP time adjust. */
 86	if (est_rst) {
 87		struct timespec64 current_time, time;
 88		ktime_t current_time_ns, basetime;
 89		u64 cycle_time;
 90
 91		mutex_lock(&priv->est_lock);
 92		priv->ptp_clock_ops.gettime64(&priv->ptp_clock_ops, &current_time);
 93		current_time_ns = timespec64_to_ktime(current_time);
 94		time.tv_nsec = priv->est->btr_reserve[0];
 95		time.tv_sec = priv->est->btr_reserve[1];
 96		basetime = timespec64_to_ktime(time);
 97		cycle_time = (u64)priv->est->ctr[1] * NSEC_PER_SEC +
 98			     priv->est->ctr[0];
 99		time = stmmac_calc_tas_basetime(basetime,
100						current_time_ns,
101						cycle_time);
102
103		priv->est->btr[0] = (u32)time.tv_nsec;
104		priv->est->btr[1] = (u32)time.tv_sec;
105		priv->est->enable = true;
106		ret = stmmac_est_configure(priv, priv, priv->est,
107					   priv->plat->clk_ptp_rate);
108		mutex_unlock(&priv->est_lock);
109		if (ret)
110			netdev_err(priv->dev, "failed to configure EST\n");
111	}
112
113	return 0;
114}
115
116/**
117 * stmmac_get_time
118 *
119 * @ptp: pointer to ptp_clock_info structure
120 * @ts: pointer to hold time/result
121 *
122 * Description: this function will read the current time from the
123 * hardware clock and store it in @ts.
124 */
125static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
126{
127	struct stmmac_priv *priv =
128	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
129	unsigned long flags;
130	u64 ns = 0;
131
132	read_lock_irqsave(&priv->ptp_lock, flags);
133	stmmac_get_systime(priv, priv->ptpaddr, &ns);
134	read_unlock_irqrestore(&priv->ptp_lock, flags);
135
136	*ts = ns_to_timespec64(ns);
137
138	return 0;
139}
140
141/**
142 * stmmac_set_time
143 *
144 * @ptp: pointer to ptp_clock_info structure
145 * @ts: time value to set
146 *
147 * Description: this function will set the current time on the
148 * hardware clock.
149 */
150static int stmmac_set_time(struct ptp_clock_info *ptp,
151			   const struct timespec64 *ts)
152{
153	struct stmmac_priv *priv =
154	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
155	unsigned long flags;
156
157	write_lock_irqsave(&priv->ptp_lock, flags);
158	stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
159	write_unlock_irqrestore(&priv->ptp_lock, flags);
160
161	return 0;
162}
163
164static int stmmac_enable(struct ptp_clock_info *ptp,
165			 struct ptp_clock_request *rq, int on)
166{
167	struct stmmac_priv *priv =
168	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
169	void __iomem *ptpaddr = priv->ptpaddr;
170	struct stmmac_pps_cfg *cfg;
171	int ret = -EOPNOTSUPP;
172	unsigned long flags;
173	u32 acr_value;
174
175	switch (rq->type) {
176	case PTP_CLK_REQ_PEROUT: {
177		struct timespec64 curr_time;
178		u64 target_ns = 0;
179		u64 ns = 0;
180
181		/* Reject requests with unsupported flags */
182		if (rq->perout.flags)
183			return -EOPNOTSUPP;
184
185		cfg = &priv->pps[rq->perout.index];
186
187		cfg->start.tv_sec = rq->perout.start.sec;
188		cfg->start.tv_nsec = rq->perout.start.nsec;
189
190		/* A time set in the past won't trigger the start of the flexible PPS generation for
191		 * the GMAC5. For some reason it does for the GMAC4 but setting a time in the past
192		 * should be addressed anyway. Therefore, any value set it the past is considered as
193		 * an offset compared to the current MAC system time.
194		 * Be aware that an offset too low may not trigger flexible PPS generation
195		 * if time spent in this configuration makes the targeted time already outdated.
196		 * To address this, add a safe time offset.
197		 */
198		if (!cfg->start.tv_sec && cfg->start.tv_nsec < PTP_SAFE_TIME_OFFSET_NS)
199			cfg->start.tv_nsec += PTP_SAFE_TIME_OFFSET_NS;
200
201		target_ns = cfg->start.tv_nsec + ((u64)cfg->start.tv_sec * NSEC_PER_SEC);
202
203		stmmac_get_systime(priv, priv->ptpaddr, &ns);
204		if (ns > TIME64_MAX - PTP_SAFE_TIME_OFFSET_NS)
205			return -EINVAL;
206
207		curr_time = ns_to_timespec64(ns);
208		if (target_ns < ns + PTP_SAFE_TIME_OFFSET_NS) {
209			cfg->start = timespec64_add_safe(cfg->start, curr_time);
210			if (cfg->start.tv_sec == TIME64_MAX)
211				return -EINVAL;
212		}
213
214		cfg->period.tv_sec = rq->perout.period.sec;
215		cfg->period.tv_nsec = rq->perout.period.nsec;
216
217		write_lock_irqsave(&priv->ptp_lock, flags);
218		ret = stmmac_flex_pps_config(priv, priv->ioaddr,
219					     rq->perout.index, cfg, on,
220					     priv->sub_second_inc,
221					     priv->systime_flags);
222		write_unlock_irqrestore(&priv->ptp_lock, flags);
223		break;
224	}
225	case PTP_CLK_REQ_EXTTS: {
226		u8 channel;
227
228		mutex_lock(&priv->aux_ts_lock);
229		acr_value = readl(ptpaddr + PTP_ACR);
230		channel = ilog2(FIELD_GET(PTP_ACR_MASK, acr_value));
231		acr_value &= ~PTP_ACR_MASK;
232
233		if (on) {
234			if (FIELD_GET(PTP_ACR_MASK, acr_value)) {
235				netdev_err(priv->dev,
236					   "Cannot enable auxiliary snapshot %d as auxiliary snapshot %d is already enabled",
237					rq->extts.index, channel);
238				mutex_unlock(&priv->aux_ts_lock);
239				return -EBUSY;
240			}
241
242			priv->plat->flags |= STMMAC_FLAG_EXT_SNAPSHOT_EN;
243
244			/* Enable External snapshot trigger */
245			acr_value |= PTP_ACR_ATSEN(rq->extts.index);
246			acr_value |= PTP_ACR_ATSFC;
247		} else {
248			priv->plat->flags &= ~STMMAC_FLAG_EXT_SNAPSHOT_EN;
249		}
250		netdev_dbg(priv->dev, "Auxiliary Snapshot %d %s.\n",
251			   rq->extts.index, on ? "enabled" : "disabled");
252		writel(acr_value, ptpaddr + PTP_ACR);
253		mutex_unlock(&priv->aux_ts_lock);
254		/* wait for auxts fifo clear to finish */
255		ret = readl_poll_timeout(ptpaddr + PTP_ACR, acr_value,
256					 !(acr_value & PTP_ACR_ATSFC),
257					 10, 10000);
258		break;
259	}
260
261	default:
262		break;
263	}
264
265	return ret;
266}
267
268/**
269 * stmmac_get_syncdevicetime
270 * @device: current device time
271 * @system: system counter value read synchronously with device time
272 * @ctx: context provided by timekeeping code
273 * Description: Read device and system clock simultaneously and return the
274 * corrected clock values in ns.
275 **/
276static int stmmac_get_syncdevicetime(ktime_t *device,
277				     struct system_counterval_t *system,
278				     void *ctx)
279{
280	struct stmmac_priv *priv = (struct stmmac_priv *)ctx;
281
282	return priv->plat->crosststamp(device, system, ctx);
283}
284
285static int stmmac_getcrosststamp(struct ptp_clock_info *ptp,
286				 struct system_device_crosststamp *xtstamp)
287{
288	struct stmmac_priv *priv =
289		container_of(ptp, struct stmmac_priv, ptp_clock_ops);
290
291	return get_device_system_crosststamp(stmmac_get_syncdevicetime,
292					     priv, NULL, xtstamp);
293}
294
295/* structure describing a PTP hardware clock */
296const struct ptp_clock_info stmmac_ptp_clock_ops = {
297	.owner = THIS_MODULE,
298	.name = "stmmac ptp",
299	.max_adj = 62500000,
300	.n_alarm = 0,
301	.n_ext_ts = 0, /* will be overwritten in stmmac_ptp_register */
302	.n_per_out = 0, /* will be overwritten in stmmac_ptp_register */
303	.n_pins = 0,
304	.pps = 0,
305	.adjfine = stmmac_adjust_freq,
306	.adjtime = stmmac_adjust_time,
307	.gettime64 = stmmac_get_time,
308	.settime64 = stmmac_set_time,
309	.enable = stmmac_enable,
310};
311
312/* structure describing a PTP hardware clock */
313const struct ptp_clock_info dwmac1000_ptp_clock_ops = {
314	.owner = THIS_MODULE,
315	.name = "stmmac ptp",
316	.max_adj = 62500000,
317	.n_alarm = 0,
318	.n_ext_ts = 1,
319	.n_per_out = 0,
320	.n_pins = 0,
321	.pps = 0,
322	.adjfine = stmmac_adjust_freq,
323	.adjtime = stmmac_adjust_time,
324	.gettime64 = stmmac_get_time,
325	.settime64 = stmmac_set_time,
326	.enable = dwmac1000_ptp_enable,
327};
328
329/**
330 * stmmac_ptp_register
331 * @priv: driver private structure
332 * Description: this function will register the ptp clock driver
333 * to kernel. It also does some house keeping work.
334 */
335void stmmac_ptp_register(struct stmmac_priv *priv)
336{
337	int i;
338
339	for (i = 0; i < priv->dma_cap.pps_out_num; i++) {
340		if (i >= STMMAC_PPS_MAX)
341			break;
342		priv->pps[i].available = true;
343	}
344
345	/* Calculate the clock domain crossing (CDC) error if necessary */
346	priv->plat->cdc_error_adj = 0;
347	if (priv->plat->core_type == DWMAC_CORE_GMAC4)
348		priv->plat->cdc_error_adj = (2 * NSEC_PER_SEC) / priv->plat->clk_ptp_rate;
349
350	/* Update the ptp clock parameters based on feature discovery, when
351	 * available
352	 */
353	if (priv->dma_cap.pps_out_num)
354		priv->ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num;
355
356	if (priv->dma_cap.aux_snapshot_n)
357		priv->ptp_clock_ops.n_ext_ts = priv->dma_cap.aux_snapshot_n;
358
359	if (priv->plat->ptp_max_adj)
360		priv->ptp_clock_ops.max_adj = priv->plat->ptp_max_adj;
361
362	if (priv->plat->crosststamp)
363		priv->ptp_clock_ops.getcrosststamp = stmmac_getcrosststamp;
364
365	rwlock_init(&priv->ptp_lock);
366	mutex_init(&priv->aux_ts_lock);
367
368	priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
369					     priv->device);
370	if (IS_ERR(priv->ptp_clock)) {
371		netdev_err(priv->dev, "ptp_clock_register failed\n");
372		priv->ptp_clock = NULL;
373	}
374
375	if (priv->ptp_clock)
376		netdev_info(priv->dev, "registered PTP clock\n");
377	else
378		mutex_destroy(&priv->aux_ts_lock);
379}
380
381/**
382 * stmmac_ptp_unregister
383 * @priv: driver private structure
384 * Description: this function will remove/unregister the ptp clock driver
385 * from the kernel.
386 */
387void stmmac_ptp_unregister(struct stmmac_priv *priv)
388{
389	if (priv->ptp_clock) {
390		ptp_clock_unregister(priv->ptp_clock);
391		priv->ptp_clock = NULL;
392		pr_debug("Removed PTP HW clock successfully on %s\n",
393			 priv->dev->name);
394
395		mutex_destroy(&priv->aux_ts_lock);
396	}
397}