macb_ptp.c

  1// SPDX-License-Identifier: GPL-2.0-only
  2/*
  3 * 1588 PTP support for Cadence GEM device.
  4 *
  5 * Copyright (C) 2017 Cadence Design Systems - https://www.cadence.com
  6 *
  7 * Authors: Rafal Ozieblo <rafalo@cadence.com>
  8 *          Bartosz Folta <bfolta@cadence.com>
  9 */
 10#include <linux/kernel.h>
 11#include <linux/types.h>
 12#include <linux/clk.h>
 13#include <linux/device.h>
 14#include <linux/etherdevice.h>
 15#include <linux/platform_device.h>
 16#include <linux/time64.h>
 17#include <linux/ptp_classify.h>
 18#include <linux/if_ether.h>
 19#include <linux/if_vlan.h>
 20#include <linux/net_tstamp.h>
 21#include <linux/circ_buf.h>
 22#include <linux/spinlock.h>
 23
 24#include "macb.h"
 25
 26#define  GEM_PTP_TIMER_NAME "gem-ptp-timer"
 27
 28static struct macb_dma_desc_ptp *macb_ptp_desc(struct macb *bp,
 29					       struct macb_dma_desc *desc)
 30{
 31	if (!macb_dma_ptp(bp))
 32		return NULL;
 33
 34	if (macb_dma64(bp))
 35		return (struct macb_dma_desc_ptp *)
 36				((u8 *)desc + sizeof(struct macb_dma_desc)
 37				+ sizeof(struct macb_dma_desc_64));
 38	else
 39		return (struct macb_dma_desc_ptp *)
 40				((u8 *)desc + sizeof(struct macb_dma_desc));
 41}
 42
 43static int gem_tsu_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts,
 44			    struct ptp_system_timestamp *sts)
 45{
 46	struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
 47	unsigned long flags;
 48	long first, second;
 49	u32 secl, sech;
 50
 51	spin_lock_irqsave(&bp->tsu_clk_lock, flags);
 52	ptp_read_system_prets(sts);
 53	first = gem_readl(bp, TN);
 54	ptp_read_system_postts(sts);
 55	secl = gem_readl(bp, TSL);
 56	sech = gem_readl(bp, TSH);
 57	second = gem_readl(bp, TN);
 58
 59	/* test for nsec rollover */
 60	if (first > second) {
 61		/* if so, use later read & re-read seconds
 62		 * (assume all done within 1s)
 63		 */
 64		ptp_read_system_prets(sts);
 65		ts->tv_nsec = gem_readl(bp, TN);
 66		ptp_read_system_postts(sts);
 67		secl = gem_readl(bp, TSL);
 68		sech = gem_readl(bp, TSH);
 69	} else {
 70		ts->tv_nsec = first;
 71	}
 72
 73	spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
 74	ts->tv_sec = (((u64)sech << GEM_TSL_SIZE) | secl)
 75			& TSU_SEC_MAX_VAL;
 76	return 0;
 77}
 78
 79static int gem_tsu_set_time(struct ptp_clock_info *ptp,
 80			    const struct timespec64 *ts)
 81{
 82	struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
 83	unsigned long flags;
 84	u32 ns, sech, secl;
 85
 86	secl = (u32)ts->tv_sec;
 87	sech = (ts->tv_sec >> GEM_TSL_SIZE) & ((1 << GEM_TSH_SIZE) - 1);
 88	ns = ts->tv_nsec;
 89
 90	spin_lock_irqsave(&bp->tsu_clk_lock, flags);
 91
 92	/* TSH doesn't latch the time and no atomicity! */
 93	gem_writel(bp, TN, 0); /* clear to avoid overflow */
 94	gem_writel(bp, TSH, sech);
 95	/* write lower bits 2nd, for synchronized secs update */
 96	gem_writel(bp, TSL, secl);
 97	gem_writel(bp, TN, ns);
 98
 99	spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
100
101	return 0;
102}
103
104static int gem_tsu_incr_set(struct macb *bp, struct tsu_incr *incr_spec)
105{
106	unsigned long flags;
107
108	/* tsu_timer_incr register must be written after
109	 * the tsu_timer_incr_sub_ns register and the write operation
110	 * will cause the value written to the tsu_timer_incr_sub_ns register
111	 * to take effect.
112	 */
113	spin_lock_irqsave(&bp->tsu_clk_lock, flags);
114	/* RegBit[15:0] = Subns[23:8]; RegBit[31:24] = Subns[7:0] */
115	gem_writel(bp, TISUBN, GEM_BF(SUBNSINCRL, incr_spec->sub_ns) |
116		   GEM_BF(SUBNSINCRH, (incr_spec->sub_ns >>
117			  GEM_SUBNSINCRL_SIZE)));
118	gem_writel(bp, TI, GEM_BF(NSINCR, incr_spec->ns));
119	spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
120
121	return 0;
122}
123
124static int gem_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
125{
126	struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
127	struct tsu_incr incr_spec;
128	bool neg_adj = false;
129	u32 word;
130	u64 adj;
131
132	if (scaled_ppm < 0) {
133		neg_adj = true;
134		scaled_ppm = -scaled_ppm;
135	}
136
137	/* Adjustment is relative to base frequency */
138	incr_spec.sub_ns = bp->tsu_incr.sub_ns;
139	incr_spec.ns = bp->tsu_incr.ns;
140
141	/* scaling: unused(8bit) | ns(8bit) | fractions(16bit) */
142	word = ((u64)incr_spec.ns << GEM_SUBNSINCR_SIZE) + incr_spec.sub_ns;
143	adj = (u64)scaled_ppm * word;
144	/* Divide with rounding, equivalent to floating dividing:
145	 * (temp / USEC_PER_SEC) + 0.5
146	 */
147	adj += (USEC_PER_SEC >> 1);
148	adj >>= PPM_FRACTION; /* remove fractions */
149	adj = div_u64(adj, USEC_PER_SEC);
150	adj = neg_adj ? (word - adj) : (word + adj);
151
152	incr_spec.ns = (adj >> GEM_SUBNSINCR_SIZE)
153			& ((1 << GEM_NSINCR_SIZE) - 1);
154	incr_spec.sub_ns = adj & ((1 << GEM_SUBNSINCR_SIZE) - 1);
155	gem_tsu_incr_set(bp, &incr_spec);
156	return 0;
157}
158
159static int gem_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
160{
161	struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
162	struct timespec64 now, then = ns_to_timespec64(delta);
163	u32 adj, sign = 0;
164
165	if (delta < 0) {
166		sign = 1;
167		delta = -delta;
168	}
169
170	if (delta > TSU_NSEC_MAX_VAL) {
171		gem_tsu_get_time(&bp->ptp_clock_info, &now, NULL);
172		now = timespec64_add(now, then);
173
174		gem_tsu_set_time(&bp->ptp_clock_info,
175				 (const struct timespec64 *)&now);
176	} else {
177		adj = (sign << GEM_ADDSUB_OFFSET) | delta;
178
179		gem_writel(bp, TA, adj);
180	}
181
182	return 0;
183}
184
185static int gem_ptp_enable(struct ptp_clock_info *ptp,
186			  struct ptp_clock_request *rq, int on)
187{
188	return -EOPNOTSUPP;
189}
190
191static const struct ptp_clock_info gem_ptp_caps_template = {
192	.owner		= THIS_MODULE,
193	.name		= GEM_PTP_TIMER_NAME,
194	.max_adj	= 0,
195	.n_alarm	= 0,
196	.n_ext_ts	= 0,
197	.n_per_out	= 0,
198	.n_pins		= 0,
199	.pps		= 1,
200	.adjfine	= gem_ptp_adjfine,
201	.adjtime	= gem_ptp_adjtime,
202	.gettimex64	= gem_tsu_get_time,
203	.settime64	= gem_tsu_set_time,
204	.enable		= gem_ptp_enable,
205};
206
207static void gem_ptp_init_timer(struct macb *bp)
208{
209	u32 rem = 0;
210	u64 adj;
211
212	bp->tsu_incr.ns = div_u64_rem(NSEC_PER_SEC, bp->tsu_rate, &rem);
213	if (rem) {
214		adj = rem;
215		adj <<= GEM_SUBNSINCR_SIZE;
216		bp->tsu_incr.sub_ns = div_u64(adj, bp->tsu_rate);
217	} else {
218		bp->tsu_incr.sub_ns = 0;
219	}
220}
221
222static void gem_ptp_init_tsu(struct macb *bp)
223{
224	struct timespec64 ts;
225
226	/* 1. get current system time */
227	ts = ns_to_timespec64(ktime_to_ns(ktime_get_real()));
228
229	/* 2. set ptp timer */
230	gem_tsu_set_time(&bp->ptp_clock_info, &ts);
231
232	/* 3. set PTP timer increment value to BASE_INCREMENT */
233	gem_tsu_incr_set(bp, &bp->tsu_incr);
234
235	gem_writel(bp, TA, 0);
236}
237
238static void gem_ptp_clear_timer(struct macb *bp)
239{
240	bp->tsu_incr.sub_ns = 0;
241	bp->tsu_incr.ns = 0;
242
243	gem_writel(bp, TISUBN, GEM_BF(SUBNSINCR, 0));
244	gem_writel(bp, TI, GEM_BF(NSINCR, 0));
245	gem_writel(bp, TA, 0);
246}
247
248static int gem_hw_timestamp(struct macb *bp, u32 dma_desc_ts_1,
249			    u32 dma_desc_ts_2, struct timespec64 *ts)
250{
251	struct timespec64 tsu;
252
253	ts->tv_sec = (GEM_BFEXT(DMA_SECH, dma_desc_ts_2) << GEM_DMA_SECL_SIZE) |
254			GEM_BFEXT(DMA_SECL, dma_desc_ts_1);
255	ts->tv_nsec = GEM_BFEXT(DMA_NSEC, dma_desc_ts_1);
256
257	/* TSU overlapping workaround
258	 * The timestamp only contains lower few bits of seconds,
259	 * so add value from 1588 timer
260	 */
261	gem_tsu_get_time(&bp->ptp_clock_info, &tsu, NULL);
262
263	ts->tv_sec |= ((~GEM_DMA_SEC_MASK) & tsu.tv_sec);
264
265	/* If the top bit is set in the timestamp,
266	 * but not in 1588 timer, it has rolled over,
267	 * so subtract max size
268	 */
269	if ((ts->tv_sec & (GEM_DMA_SEC_TOP >> 1)) &&
270	    !(tsu.tv_sec & (GEM_DMA_SEC_TOP >> 1)))
271		ts->tv_sec -= GEM_DMA_SEC_TOP;
272
273	return 0;
274}
275
276void gem_ptp_rxstamp(struct macb *bp, struct sk_buff *skb,
277		     struct macb_dma_desc *desc)
278{
279	struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
280	struct macb_dma_desc_ptp *desc_ptp;
281	struct timespec64 ts;
282
283	if (GEM_BFEXT(DMA_RXVALID, desc->addr)) {
284		desc_ptp = macb_ptp_desc(bp, desc);
285		/* Unlikely but check */
286		if (!desc_ptp) {
287			dev_warn_ratelimited(&bp->pdev->dev,
288					     "Timestamp not supported in BD\n");
289			return;
290		}
291		gem_hw_timestamp(bp, desc_ptp->ts_1, desc_ptp->ts_2, &ts);
292		memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
293		shhwtstamps->hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
294	}
295}
296
297void gem_ptp_txstamp(struct macb *bp, struct sk_buff *skb,
298		     struct macb_dma_desc *desc)
299{
300	struct skb_shared_hwtstamps shhwtstamps;
301	struct macb_dma_desc_ptp *desc_ptp;
302	struct timespec64 ts;
303
304	if (!GEM_BFEXT(DMA_TXVALID, desc->ctrl)) {
305		dev_warn_ratelimited(&bp->pdev->dev,
306				     "Timestamp not set in TX BD as expected\n");
307		return;
308	}
309
310	desc_ptp = macb_ptp_desc(bp, desc);
311	/* Unlikely but check */
312	if (!desc_ptp) {
313		dev_warn_ratelimited(&bp->pdev->dev,
314				     "Timestamp not supported in BD\n");
315		return;
316	}
317
318	/* ensure ts_1/ts_2 is loaded after ctrl (TX_USED check) */
319	dma_rmb();
320	gem_hw_timestamp(bp, desc_ptp->ts_1, desc_ptp->ts_2, &ts);
321
322	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
323	shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
324	skb_tstamp_tx(skb, &shhwtstamps);
325}
326
327void gem_ptp_init(struct net_device *dev)
328{
329	struct macb *bp = netdev_priv(dev);
330
331	bp->ptp_clock_info = gem_ptp_caps_template;
332
333	/* nominal frequency and maximum adjustment in ppb */
334	bp->tsu_rate = bp->ptp_info->get_tsu_rate(bp);
335	bp->ptp_clock_info.max_adj = bp->ptp_info->get_ptp_max_adj();
336	gem_ptp_init_timer(bp);
337	bp->ptp_clock = ptp_clock_register(&bp->ptp_clock_info, &dev->dev);
338	if (IS_ERR(bp->ptp_clock)) {
339		pr_err("ptp clock register failed: %ld\n",
340			PTR_ERR(bp->ptp_clock));
341		bp->ptp_clock = NULL;
342		return;
343	} else if (bp->ptp_clock == NULL) {
344		pr_err("ptp clock register failed\n");
345		return;
346	}
347
348	spin_lock_init(&bp->tsu_clk_lock);
349
350	gem_ptp_init_tsu(bp);
351
352	dev_info(&bp->pdev->dev, "%s ptp clock registered.\n",
353		 GEM_PTP_TIMER_NAME);
354}
355
356void gem_ptp_remove(struct net_device *ndev)
357{
358	struct macb *bp = netdev_priv(ndev);
359
360	if (bp->ptp_clock) {
361		ptp_clock_unregister(bp->ptp_clock);
362		bp->ptp_clock = NULL;
363	}
364
365	gem_ptp_clear_timer(bp);
366
367	dev_info(&bp->pdev->dev, "%s ptp clock unregistered.\n",
368		 GEM_PTP_TIMER_NAME);
369}
370
371static int gem_ptp_set_ts_mode(struct macb *bp,
372			       enum macb_bd_control tx_bd_control,
373			       enum macb_bd_control rx_bd_control)
374{
375	gem_writel(bp, TXBDCTRL, GEM_BF(TXTSMODE, tx_bd_control));
376	gem_writel(bp, RXBDCTRL, GEM_BF(RXTSMODE, rx_bd_control));
377
378	return 0;
379}
380
381int gem_get_hwtst(struct net_device *dev,
382		  struct kernel_hwtstamp_config *tstamp_config)
383{
384	struct macb *bp = netdev_priv(dev);
385
386	*tstamp_config = bp->tstamp_config;
387	if (!macb_dma_ptp(bp))
388		return -EOPNOTSUPP;
389
390	return 0;
391}
392
393static void gem_ptp_set_one_step_sync(struct macb *bp, u8 enable)
394{
395	u32 reg_val;
396
397	reg_val = macb_readl(bp, NCR);
398
399	if (enable)
400		macb_writel(bp, NCR, reg_val | MACB_BIT(OSSMODE));
401	else
402		macb_writel(bp, NCR, reg_val & ~MACB_BIT(OSSMODE));
403}
404
405int gem_set_hwtst(struct net_device *dev,
406		  struct kernel_hwtstamp_config *tstamp_config,
407		  struct netlink_ext_ack *extack)
408{
409	enum macb_bd_control tx_bd_control = TSTAMP_DISABLED;
410	enum macb_bd_control rx_bd_control = TSTAMP_DISABLED;
411	struct macb *bp = netdev_priv(dev);
412	u32 regval;
413
414	if (!macb_dma_ptp(bp))
415		return -EOPNOTSUPP;
416
417	switch (tstamp_config->tx_type) {
418	case HWTSTAMP_TX_OFF:
419		break;
420	case HWTSTAMP_TX_ONESTEP_SYNC:
421		gem_ptp_set_one_step_sync(bp, 1);
422		tx_bd_control = TSTAMP_ALL_FRAMES;
423		break;
424	case HWTSTAMP_TX_ON:
425		gem_ptp_set_one_step_sync(bp, 0);
426		tx_bd_control = TSTAMP_ALL_FRAMES;
427		break;
428	default:
429		return -ERANGE;
430	}
431
432	switch (tstamp_config->rx_filter) {
433	case HWTSTAMP_FILTER_NONE:
434		break;
435	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
436		break;
437	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
438		break;
439	case HWTSTAMP_FILTER_PTP_V2_EVENT:
440	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
441	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
442	case HWTSTAMP_FILTER_PTP_V2_SYNC:
443	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
444	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
445	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
446	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
447	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
448		rx_bd_control =  TSTAMP_ALL_PTP_FRAMES;
449		tstamp_config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
450		regval = macb_readl(bp, NCR);
451		macb_writel(bp, NCR, (regval | MACB_BIT(SRTSM)));
452		break;
453	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
454	case HWTSTAMP_FILTER_ALL:
455		rx_bd_control = TSTAMP_ALL_FRAMES;
456		tstamp_config->rx_filter = HWTSTAMP_FILTER_ALL;
457		break;
458	default:
459		tstamp_config->rx_filter = HWTSTAMP_FILTER_NONE;
460		return -ERANGE;
461	}
462
463	bp->tstamp_config = *tstamp_config;
464
465	if (gem_ptp_set_ts_mode(bp, tx_bd_control, rx_bd_control) != 0)
466		return -ERANGE;
467
468	return 0;
469}
470