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