1 // SPDX-License-Identifier: GPL-2.0
2 /* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
3  */
4 #include <linux/spi/spi.h>
5 #include "sja1105.h"
6 
7 /* The adjfine API clamps ppb between [-32,768,000, 32,768,000], and
8  * therefore scaled_ppm between [-2,147,483,648, 2,147,483,647].
9  * Set the maximum supported ppb to a round value smaller than the maximum.
10  *
11  * Percentually speaking, this is a +/- 0.032x adjustment of the
12  * free-running counter (0.968x to 1.032x).
13  */
14 #define SJA1105_MAX_ADJ_PPB		32000000
15 #define SJA1105_SIZE_PTP_CMD		4
16 
17 /* PTPSYNCTS has no interrupt or update mechanism, because the intended
18  * hardware use case is for the timestamp to be collected synchronously,
19  * immediately after the CAS_MASTER SJA1105 switch has performed a CASSYNC
20  * one-shot toggle (no return to level) on the PTP_CLK pin. When used as a
21  * generic extts source, the PTPSYNCTS register needs polling and a comparison
22  * with the old value. The polling interval is configured as the Nyquist rate
23  * of a signal with 50% duty cycle and 1Hz frequency, which is sadly all that
24  * this hardware can do (but may be enough for some setups). Anything of higher
25  * frequency than 1 Hz will be lost, since there is no timestamp FIFO.
26  */
27 #define SJA1105_EXTTS_INTERVAL		(HZ / 6)
28 
29 /*            This range is actually +/- SJA1105_MAX_ADJ_PPB
30  *            divided by 1000 (ppb -> ppm) and with a 16-bit
31  *            "fractional" part (actually fixed point).
32  *                                    |
33  *                                    v
34  * Convert scaled_ppm from the +/- ((10^6) << 16) range
35  * into the +/- (1 << 31) range.
36  *
37  * This forgoes a "ppb" numeric representation (up to NSEC_PER_SEC)
38  * and defines the scaling factor between scaled_ppm and the actual
39  * frequency adjustments of the PHC.
40  *
41  *   ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16)
42  *   simplifies to
43  *   ptpclkrate = scaled_ppm * 2^9 / 5^6
44  */
45 #define SJA1105_CC_MULT_NUM		(1 << 9)
46 #define SJA1105_CC_MULT_DEM		15625
47 #define SJA1105_CC_MULT			0x80000000
48 
49 enum sja1105_ptp_clk_mode {
50 	PTP_ADD_MODE = 1,
51 	PTP_SET_MODE = 0,
52 };
53 
54 #define extts_to_data(t) \
55 		container_of((t), struct sja1105_ptp_data, extts_timer)
56 #define ptp_caps_to_data(d) \
57 		container_of((d), struct sja1105_ptp_data, caps)
58 #define ptp_data_to_sja1105(d) \
59 		container_of((d), struct sja1105_private, ptp_data)
60 
sja1105_hwtstamp_set(struct dsa_switch * ds,int port,struct ifreq * ifr)61 int sja1105_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
62 {
63 	struct sja1105_private *priv = ds->priv;
64 	struct hwtstamp_config config;
65 
66 	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
67 		return -EFAULT;
68 
69 	switch (config.tx_type) {
70 	case HWTSTAMP_TX_OFF:
71 		priv->hwts_tx_en &= ~BIT(port);
72 		break;
73 	case HWTSTAMP_TX_ON:
74 		priv->hwts_tx_en |= BIT(port);
75 		break;
76 	default:
77 		return -ERANGE;
78 	}
79 
80 	switch (config.rx_filter) {
81 	case HWTSTAMP_FILTER_NONE:
82 		priv->hwts_rx_en &= ~BIT(port);
83 		break;
84 	default:
85 		priv->hwts_rx_en |= BIT(port);
86 		break;
87 	}
88 
89 	if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
90 		return -EFAULT;
91 	return 0;
92 }
93 
sja1105_hwtstamp_get(struct dsa_switch * ds,int port,struct ifreq * ifr)94 int sja1105_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr)
95 {
96 	struct sja1105_private *priv = ds->priv;
97 	struct hwtstamp_config config;
98 
99 	config.flags = 0;
100 	if (priv->hwts_tx_en & BIT(port))
101 		config.tx_type = HWTSTAMP_TX_ON;
102 	else
103 		config.tx_type = HWTSTAMP_TX_OFF;
104 	if (priv->hwts_rx_en & BIT(port))
105 		config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
106 	else
107 		config.rx_filter = HWTSTAMP_FILTER_NONE;
108 
109 	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
110 		-EFAULT : 0;
111 }
112 
sja1105_get_ts_info(struct dsa_switch * ds,int port,struct ethtool_ts_info * info)113 int sja1105_get_ts_info(struct dsa_switch *ds, int port,
114 			struct ethtool_ts_info *info)
115 {
116 	struct sja1105_private *priv = ds->priv;
117 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
118 
119 	/* Called during cleanup */
120 	if (!ptp_data->clock)
121 		return -ENODEV;
122 
123 	info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
124 				SOF_TIMESTAMPING_RX_HARDWARE |
125 				SOF_TIMESTAMPING_RAW_HARDWARE;
126 	info->tx_types = (1 << HWTSTAMP_TX_OFF) |
127 			 (1 << HWTSTAMP_TX_ON);
128 	info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
129 			   (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT);
130 	info->phc_index = ptp_clock_index(ptp_data->clock);
131 	return 0;
132 }
133 
sja1105et_ptp_cmd_packing(u8 * buf,struct sja1105_ptp_cmd * cmd,enum packing_op op)134 void sja1105et_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
135 			       enum packing_op op)
136 {
137 	const int size = SJA1105_SIZE_PTP_CMD;
138 	/* No need to keep this as part of the structure */
139 	u64 valid = 1;
140 
141 	sja1105_packing(buf, &valid,           31, 31, size, op);
142 	sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
143 	sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
144 	sja1105_packing(buf, &cmd->startptpcp, 28, 28, size, op);
145 	sja1105_packing(buf, &cmd->stopptpcp,  27, 27, size, op);
146 	sja1105_packing(buf, &cmd->resptp,      2,  2, size, op);
147 	sja1105_packing(buf, &cmd->corrclk4ts,  1,  1, size, op);
148 	sja1105_packing(buf, &cmd->ptpclkadd,   0,  0, size, op);
149 }
150 
sja1105pqrs_ptp_cmd_packing(u8 * buf,struct sja1105_ptp_cmd * cmd,enum packing_op op)151 void sja1105pqrs_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
152 				 enum packing_op op)
153 {
154 	const int size = SJA1105_SIZE_PTP_CMD;
155 	/* No need to keep this as part of the structure */
156 	u64 valid = 1;
157 
158 	sja1105_packing(buf, &valid,           31, 31, size, op);
159 	sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
160 	sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
161 	sja1105_packing(buf, &cmd->startptpcp, 28, 28, size, op);
162 	sja1105_packing(buf, &cmd->stopptpcp,  27, 27, size, op);
163 	sja1105_packing(buf, &cmd->resptp,      3,  3, size, op);
164 	sja1105_packing(buf, &cmd->corrclk4ts,  2,  2, size, op);
165 	sja1105_packing(buf, &cmd->ptpclkadd,   0,  0, size, op);
166 }
167 
sja1105_ptp_commit(struct dsa_switch * ds,struct sja1105_ptp_cmd * cmd,sja1105_spi_rw_mode_t rw)168 int sja1105_ptp_commit(struct dsa_switch *ds, struct sja1105_ptp_cmd *cmd,
169 		       sja1105_spi_rw_mode_t rw)
170 {
171 	const struct sja1105_private *priv = ds->priv;
172 	const struct sja1105_regs *regs = priv->info->regs;
173 	u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
174 	int rc;
175 
176 	if (rw == SPI_WRITE)
177 		priv->info->ptp_cmd_packing(buf, cmd, PACK);
178 
179 	rc = sja1105_xfer_buf(priv, rw, regs->ptp_control, buf,
180 			      SJA1105_SIZE_PTP_CMD);
181 
182 	if (rw == SPI_READ)
183 		priv->info->ptp_cmd_packing(buf, cmd, UNPACK);
184 
185 	return rc;
186 }
187 
188 /* The switch returns partial timestamps (24 bits for SJA1105 E/T, which wrap
189  * around in 0.135 seconds, and 32 bits for P/Q/R/S, wrapping around in 34.35
190  * seconds).
191  *
192  * This receives the RX or TX MAC timestamps, provided by hardware as
193  * the lower bits of the cycle counter, sampled at the time the timestamp was
194  * collected.
195  *
196  * To reconstruct into a full 64-bit-wide timestamp, the cycle counter is
197  * read and the high-order bits are filled in.
198  *
199  * Must be called within one wraparound period of the partial timestamp since
200  * it was generated by the MAC.
201  */
sja1105_tstamp_reconstruct(struct dsa_switch * ds,u64 now,u64 ts_partial)202 static u64 sja1105_tstamp_reconstruct(struct dsa_switch *ds, u64 now,
203 				      u64 ts_partial)
204 {
205 	struct sja1105_private *priv = ds->priv;
206 	u64 partial_tstamp_mask = CYCLECOUNTER_MASK(priv->info->ptp_ts_bits);
207 	u64 ts_reconstructed;
208 
209 	ts_reconstructed = (now & ~partial_tstamp_mask) | ts_partial;
210 
211 	/* Check lower bits of current cycle counter against the timestamp.
212 	 * If the current cycle counter is lower than the partial timestamp,
213 	 * then wraparound surely occurred and must be accounted for.
214 	 */
215 	if ((now & partial_tstamp_mask) <= ts_partial)
216 		ts_reconstructed -= (partial_tstamp_mask + 1);
217 
218 	return ts_reconstructed;
219 }
220 
221 /* Reads the SPI interface for an egress timestamp generated by the switch
222  * for frames sent using management routes.
223  *
224  * SJA1105 E/T layout of the 4-byte SPI payload:
225  *
226  * 31    23    15    7     0
227  * |     |     |     |     |
228  * +-----+-----+-----+     ^
229  *          ^              |
230  *          |              |
231  *  24-bit timestamp   Update bit
232  *
233  *
234  * SJA1105 P/Q/R/S layout of the 8-byte SPI payload:
235  *
236  * 31    23    15    7     0     63    55    47    39    32
237  * |     |     |     |     |     |     |     |     |     |
238  *                         ^     +-----+-----+-----+-----+
239  *                         |                 ^
240  *                         |                 |
241  *                    Update bit    32-bit timestamp
242  *
243  * Notice that the update bit is in the same place.
244  * To have common code for E/T and P/Q/R/S for reading the timestamp,
245  * we need to juggle with the offset and the bit indices.
246  */
sja1105_ptpegr_ts_poll(struct dsa_switch * ds,int port,u64 * ts)247 static int sja1105_ptpegr_ts_poll(struct dsa_switch *ds, int port, u64 *ts)
248 {
249 	struct sja1105_private *priv = ds->priv;
250 	const struct sja1105_regs *regs = priv->info->regs;
251 	int tstamp_bit_start, tstamp_bit_end;
252 	int timeout = 10;
253 	u8 packed_buf[8];
254 	u64 update;
255 	int rc;
256 
257 	do {
258 		rc = sja1105_xfer_buf(priv, SPI_READ, regs->ptpegr_ts[port],
259 				      packed_buf, priv->info->ptpegr_ts_bytes);
260 		if (rc < 0)
261 			return rc;
262 
263 		sja1105_unpack(packed_buf, &update, 0, 0,
264 			       priv->info->ptpegr_ts_bytes);
265 		if (update)
266 			break;
267 
268 		usleep_range(10, 50);
269 	} while (--timeout);
270 
271 	if (!timeout)
272 		return -ETIMEDOUT;
273 
274 	/* Point the end bit to the second 32-bit word on P/Q/R/S,
275 	 * no-op on E/T.
276 	 */
277 	tstamp_bit_end = (priv->info->ptpegr_ts_bytes - 4) * 8;
278 	/* Shift the 24-bit timestamp on E/T to be collected from 31:8.
279 	 * No-op on P/Q/R/S.
280 	 */
281 	tstamp_bit_end += 32 - priv->info->ptp_ts_bits;
282 	tstamp_bit_start = tstamp_bit_end + priv->info->ptp_ts_bits - 1;
283 
284 	*ts = 0;
285 
286 	sja1105_unpack(packed_buf, ts, tstamp_bit_start, tstamp_bit_end,
287 		       priv->info->ptpegr_ts_bytes);
288 
289 	return 0;
290 }
291 
292 /* Caller must hold ptp_data->lock */
sja1105_ptpclkval_read(struct sja1105_private * priv,u64 * ticks,struct ptp_system_timestamp * ptp_sts)293 static int sja1105_ptpclkval_read(struct sja1105_private *priv, u64 *ticks,
294 				  struct ptp_system_timestamp *ptp_sts)
295 {
296 	const struct sja1105_regs *regs = priv->info->regs;
297 
298 	return sja1105_xfer_u64(priv, SPI_READ, regs->ptpclkval, ticks,
299 				ptp_sts);
300 }
301 
302 /* Caller must hold ptp_data->lock */
sja1105_ptpclkval_write(struct sja1105_private * priv,u64 ticks,struct ptp_system_timestamp * ptp_sts)303 static int sja1105_ptpclkval_write(struct sja1105_private *priv, u64 ticks,
304 				   struct ptp_system_timestamp *ptp_sts)
305 {
306 	const struct sja1105_regs *regs = priv->info->regs;
307 
308 	return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpclkval, &ticks,
309 				ptp_sts);
310 }
311 
sja1105_extts_poll(struct sja1105_private * priv)312 static void sja1105_extts_poll(struct sja1105_private *priv)
313 {
314 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
315 	const struct sja1105_regs *regs = priv->info->regs;
316 	struct ptp_clock_event event;
317 	u64 ptpsyncts = 0;
318 	int rc;
319 
320 	rc = sja1105_xfer_u64(priv, SPI_READ, regs->ptpsyncts, &ptpsyncts,
321 			      NULL);
322 	if (rc < 0)
323 		dev_err_ratelimited(priv->ds->dev,
324 				    "Failed to read PTPSYNCTS: %d\n", rc);
325 
326 	if (ptpsyncts && ptp_data->ptpsyncts != ptpsyncts) {
327 		event.index = 0;
328 		event.type = PTP_CLOCK_EXTTS;
329 		event.timestamp = ns_to_ktime(sja1105_ticks_to_ns(ptpsyncts));
330 		ptp_clock_event(ptp_data->clock, &event);
331 
332 		ptp_data->ptpsyncts = ptpsyncts;
333 	}
334 }
335 
sja1105_rxtstamp_work(struct ptp_clock_info * ptp)336 static long sja1105_rxtstamp_work(struct ptp_clock_info *ptp)
337 {
338 	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
339 	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
340 	struct dsa_switch *ds = priv->ds;
341 	struct sk_buff *skb;
342 
343 	mutex_lock(&ptp_data->lock);
344 
345 	while ((skb = skb_dequeue(&ptp_data->skb_rxtstamp_queue)) != NULL) {
346 		struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
347 		u64 ticks, ts;
348 		int rc;
349 
350 		rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
351 		if (rc < 0) {
352 			dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
353 			kfree_skb(skb);
354 			continue;
355 		}
356 
357 		*shwt = (struct skb_shared_hwtstamps) {0};
358 
359 		ts = SJA1105_SKB_CB(skb)->tstamp;
360 		ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
361 
362 		shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
363 		netif_rx(skb);
364 	}
365 
366 	if (ptp_data->extts_enabled)
367 		sja1105_extts_poll(priv);
368 
369 	mutex_unlock(&ptp_data->lock);
370 
371 	/* Don't restart */
372 	return -1;
373 }
374 
sja1105_rxtstamp(struct dsa_switch * ds,int port,struct sk_buff * skb)375 bool sja1105_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
376 {
377 	struct sja1105_private *priv = ds->priv;
378 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
379 
380 	if (!(priv->hwts_rx_en & BIT(port)))
381 		return false;
382 
383 	/* We need to read the full PTP clock to reconstruct the Rx
384 	 * timestamp. For that we need a sleepable context.
385 	 */
386 	skb_queue_tail(&ptp_data->skb_rxtstamp_queue, skb);
387 	ptp_schedule_worker(ptp_data->clock, 0);
388 	return true;
389 }
390 
sja1110_rxtstamp(struct dsa_switch * ds,int port,struct sk_buff * skb)391 bool sja1110_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
392 {
393 	struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
394 	u64 ts = SJA1105_SKB_CB(skb)->tstamp;
395 
396 	*shwt = (struct skb_shared_hwtstamps) {0};
397 
398 	shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
399 
400 	/* Don't defer */
401 	return false;
402 }
403 
404 /* Called from dsa_skb_defer_rx_timestamp */
sja1105_port_rxtstamp(struct dsa_switch * ds,int port,struct sk_buff * skb,unsigned int type)405 bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
406 			   struct sk_buff *skb, unsigned int type)
407 {
408 	struct sja1105_private *priv = ds->priv;
409 
410 	return priv->info->rxtstamp(ds, port, skb);
411 }
412 
sja1110_process_meta_tstamp(struct dsa_switch * ds,int port,u8 ts_id,enum sja1110_meta_tstamp dir,u64 tstamp)413 void sja1110_process_meta_tstamp(struct dsa_switch *ds, int port, u8 ts_id,
414 				 enum sja1110_meta_tstamp dir, u64 tstamp)
415 {
416 	struct sja1105_private *priv = ds->priv;
417 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
418 	struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
419 	struct skb_shared_hwtstamps shwt = {0};
420 
421 	/* We don't care about RX timestamps on the CPU port */
422 	if (dir == SJA1110_META_TSTAMP_RX)
423 		return;
424 
425 	spin_lock(&ptp_data->skb_txtstamp_queue.lock);
426 
427 	skb_queue_walk_safe(&ptp_data->skb_txtstamp_queue, skb, skb_tmp) {
428 		if (SJA1105_SKB_CB(skb)->ts_id != ts_id)
429 			continue;
430 
431 		__skb_unlink(skb, &ptp_data->skb_txtstamp_queue);
432 		skb_match = skb;
433 
434 		break;
435 	}
436 
437 	spin_unlock(&ptp_data->skb_txtstamp_queue.lock);
438 
439 	if (WARN_ON(!skb_match))
440 		return;
441 
442 	shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(tstamp));
443 	skb_complete_tx_timestamp(skb_match, &shwt);
444 }
445 
446 /* In addition to cloning the skb which is done by the common
447  * sja1105_port_txtstamp, we need to generate a timestamp ID and save the
448  * packet to the TX timestamping queue.
449  */
sja1110_txtstamp(struct dsa_switch * ds,int port,struct sk_buff * skb)450 void sja1110_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
451 {
452 	struct sk_buff *clone = SJA1105_SKB_CB(skb)->clone;
453 	struct sja1105_private *priv = ds->priv;
454 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
455 	u8 ts_id;
456 
457 	skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
458 
459 	spin_lock(&priv->ts_id_lock);
460 
461 	ts_id = priv->ts_id;
462 	/* Deal automatically with 8-bit wraparound */
463 	priv->ts_id++;
464 
465 	SJA1105_SKB_CB(clone)->ts_id = ts_id;
466 
467 	spin_unlock(&priv->ts_id_lock);
468 
469 	skb_queue_tail(&ptp_data->skb_txtstamp_queue, clone);
470 }
471 
472 /* Called from dsa_skb_tx_timestamp. This callback is just to clone
473  * the skb and have it available in SJA1105_SKB_CB in the .port_deferred_xmit
474  * callback, where we will timestamp it synchronously.
475  */
sja1105_port_txtstamp(struct dsa_switch * ds,int port,struct sk_buff * skb)476 void sja1105_port_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
477 {
478 	struct sja1105_private *priv = ds->priv;
479 	struct sk_buff *clone;
480 
481 	if (!(priv->hwts_tx_en & BIT(port)))
482 		return;
483 
484 	clone = skb_clone_sk(skb);
485 	if (!clone)
486 		return;
487 
488 	SJA1105_SKB_CB(skb)->clone = clone;
489 
490 	if (priv->info->txtstamp)
491 		priv->info->txtstamp(ds, port, skb);
492 }
493 
sja1105_ptp_reset(struct dsa_switch * ds)494 static int sja1105_ptp_reset(struct dsa_switch *ds)
495 {
496 	struct sja1105_private *priv = ds->priv;
497 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
498 	struct sja1105_ptp_cmd cmd = ptp_data->cmd;
499 	int rc;
500 
501 	mutex_lock(&ptp_data->lock);
502 
503 	cmd.resptp = 1;
504 
505 	dev_dbg(ds->dev, "Resetting PTP clock\n");
506 	rc = sja1105_ptp_commit(ds, &cmd, SPI_WRITE);
507 
508 	sja1105_tas_clockstep(priv->ds);
509 
510 	mutex_unlock(&ptp_data->lock);
511 
512 	return rc;
513 }
514 
515 /* Caller must hold ptp_data->lock */
__sja1105_ptp_gettimex(struct dsa_switch * ds,u64 * ns,struct ptp_system_timestamp * ptp_sts)516 int __sja1105_ptp_gettimex(struct dsa_switch *ds, u64 *ns,
517 			   struct ptp_system_timestamp *ptp_sts)
518 {
519 	struct sja1105_private *priv = ds->priv;
520 	u64 ticks;
521 	int rc;
522 
523 	rc = sja1105_ptpclkval_read(priv, &ticks, ptp_sts);
524 	if (rc < 0) {
525 		dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
526 		return rc;
527 	}
528 
529 	*ns = sja1105_ticks_to_ns(ticks);
530 
531 	return 0;
532 }
533 
sja1105_ptp_gettimex(struct ptp_clock_info * ptp,struct timespec64 * ts,struct ptp_system_timestamp * ptp_sts)534 static int sja1105_ptp_gettimex(struct ptp_clock_info *ptp,
535 				struct timespec64 *ts,
536 				struct ptp_system_timestamp *ptp_sts)
537 {
538 	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
539 	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
540 	u64 now = 0;
541 	int rc;
542 
543 	mutex_lock(&ptp_data->lock);
544 
545 	rc = __sja1105_ptp_gettimex(priv->ds, &now, ptp_sts);
546 	*ts = ns_to_timespec64(now);
547 
548 	mutex_unlock(&ptp_data->lock);
549 
550 	return rc;
551 }
552 
553 /* Caller must hold ptp_data->lock */
sja1105_ptp_mode_set(struct sja1105_private * priv,enum sja1105_ptp_clk_mode mode)554 static int sja1105_ptp_mode_set(struct sja1105_private *priv,
555 				enum sja1105_ptp_clk_mode mode)
556 {
557 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
558 
559 	if (ptp_data->cmd.ptpclkadd == mode)
560 		return 0;
561 
562 	ptp_data->cmd.ptpclkadd = mode;
563 
564 	return sja1105_ptp_commit(priv->ds, &ptp_data->cmd, SPI_WRITE);
565 }
566 
567 /* Write to PTPCLKVAL while PTPCLKADD is 0 */
__sja1105_ptp_settime(struct dsa_switch * ds,u64 ns,struct ptp_system_timestamp * ptp_sts)568 int __sja1105_ptp_settime(struct dsa_switch *ds, u64 ns,
569 			  struct ptp_system_timestamp *ptp_sts)
570 {
571 	struct sja1105_private *priv = ds->priv;
572 	u64 ticks = ns_to_sja1105_ticks(ns);
573 	int rc;
574 
575 	rc = sja1105_ptp_mode_set(priv, PTP_SET_MODE);
576 	if (rc < 0) {
577 		dev_err(priv->ds->dev, "Failed to put PTPCLK in set mode\n");
578 		return rc;
579 	}
580 
581 	rc = sja1105_ptpclkval_write(priv, ticks, ptp_sts);
582 
583 	sja1105_tas_clockstep(priv->ds);
584 
585 	return rc;
586 }
587 
sja1105_ptp_settime(struct ptp_clock_info * ptp,const struct timespec64 * ts)588 static int sja1105_ptp_settime(struct ptp_clock_info *ptp,
589 			       const struct timespec64 *ts)
590 {
591 	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
592 	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
593 	u64 ns = timespec64_to_ns(ts);
594 	int rc;
595 
596 	mutex_lock(&ptp_data->lock);
597 
598 	rc = __sja1105_ptp_settime(priv->ds, ns, NULL);
599 
600 	mutex_unlock(&ptp_data->lock);
601 
602 	return rc;
603 }
604 
sja1105_ptp_adjfine(struct ptp_clock_info * ptp,long scaled_ppm)605 static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
606 {
607 	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
608 	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
609 	const struct sja1105_regs *regs = priv->info->regs;
610 	u32 clkrate32;
611 	s64 clkrate;
612 	int rc;
613 
614 	clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;
615 	clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);
616 
617 	/* Take a +/- value and re-center it around 2^31. */
618 	clkrate = SJA1105_CC_MULT + clkrate;
619 	WARN_ON(abs(clkrate) >= GENMASK_ULL(31, 0));
620 	clkrate32 = clkrate;
621 
622 	mutex_lock(&ptp_data->lock);
623 
624 	rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptpclkrate, &clkrate32,
625 			      NULL);
626 
627 	sja1105_tas_adjfreq(priv->ds);
628 
629 	mutex_unlock(&ptp_data->lock);
630 
631 	return rc;
632 }
633 
634 /* Write to PTPCLKVAL while PTPCLKADD is 1 */
__sja1105_ptp_adjtime(struct dsa_switch * ds,s64 delta)635 int __sja1105_ptp_adjtime(struct dsa_switch *ds, s64 delta)
636 {
637 	struct sja1105_private *priv = ds->priv;
638 	s64 ticks = ns_to_sja1105_ticks(delta);
639 	int rc;
640 
641 	rc = sja1105_ptp_mode_set(priv, PTP_ADD_MODE);
642 	if (rc < 0) {
643 		dev_err(priv->ds->dev, "Failed to put PTPCLK in add mode\n");
644 		return rc;
645 	}
646 
647 	rc = sja1105_ptpclkval_write(priv, ticks, NULL);
648 
649 	sja1105_tas_clockstep(priv->ds);
650 
651 	return rc;
652 }
653 
sja1105_ptp_adjtime(struct ptp_clock_info * ptp,s64 delta)654 static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
655 {
656 	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
657 	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
658 	int rc;
659 
660 	mutex_lock(&ptp_data->lock);
661 
662 	rc = __sja1105_ptp_adjtime(priv->ds, delta);
663 
664 	mutex_unlock(&ptp_data->lock);
665 
666 	return rc;
667 }
668 
sja1105_ptp_extts_setup_timer(struct sja1105_ptp_data * ptp_data)669 static void sja1105_ptp_extts_setup_timer(struct sja1105_ptp_data *ptp_data)
670 {
671 	unsigned long expires = ((jiffies / SJA1105_EXTTS_INTERVAL) + 1) *
672 				SJA1105_EXTTS_INTERVAL;
673 
674 	mod_timer(&ptp_data->extts_timer, expires);
675 }
676 
sja1105_ptp_extts_timer(struct timer_list * t)677 static void sja1105_ptp_extts_timer(struct timer_list *t)
678 {
679 	struct sja1105_ptp_data *ptp_data = extts_to_data(t);
680 
681 	ptp_schedule_worker(ptp_data->clock, 0);
682 
683 	sja1105_ptp_extts_setup_timer(ptp_data);
684 }
685 
sja1105_change_ptp_clk_pin_func(struct sja1105_private * priv,enum ptp_pin_function func)686 static int sja1105_change_ptp_clk_pin_func(struct sja1105_private *priv,
687 					   enum ptp_pin_function func)
688 {
689 	struct sja1105_avb_params_entry *avb;
690 	enum ptp_pin_function old_func;
691 
692 	avb = priv->static_config.tables[BLK_IDX_AVB_PARAMS].entries;
693 
694 	if (priv->info->device_id == SJA1105E_DEVICE_ID ||
695 	    priv->info->device_id == SJA1105T_DEVICE_ID ||
696 	    avb->cas_master)
697 		old_func = PTP_PF_PEROUT;
698 	else
699 		old_func = PTP_PF_EXTTS;
700 
701 	if (func == old_func)
702 		return 0;
703 
704 	avb->cas_master = (func == PTP_PF_PEROUT);
705 
706 	return sja1105_dynamic_config_write(priv, BLK_IDX_AVB_PARAMS, 0, avb,
707 					    true);
708 }
709 
710 /* The PTP_CLK pin may be configured to toggle with a 50% duty cycle and a
711  * frequency f:
712  *
713  *           NSEC_PER_SEC
714  * f = ----------------------
715  *     (PTPPINDUR * 8 ns) * 2
716  */
sja1105_per_out_enable(struct sja1105_private * priv,struct ptp_perout_request * perout,bool on)717 static int sja1105_per_out_enable(struct sja1105_private *priv,
718 				  struct ptp_perout_request *perout,
719 				  bool on)
720 {
721 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
722 	const struct sja1105_regs *regs = priv->info->regs;
723 	struct sja1105_ptp_cmd cmd = ptp_data->cmd;
724 	int rc;
725 
726 	/* We only support one channel */
727 	if (perout->index != 0)
728 		return -EOPNOTSUPP;
729 
730 	/* Reject requests with unsupported flags */
731 	if (perout->flags)
732 		return -EOPNOTSUPP;
733 
734 	mutex_lock(&ptp_data->lock);
735 
736 	rc = sja1105_change_ptp_clk_pin_func(priv, PTP_PF_PEROUT);
737 	if (rc)
738 		goto out;
739 
740 	if (on) {
741 		struct timespec64 pin_duration_ts = {
742 			.tv_sec = perout->period.sec,
743 			.tv_nsec = perout->period.nsec,
744 		};
745 		struct timespec64 pin_start_ts = {
746 			.tv_sec = perout->start.sec,
747 			.tv_nsec = perout->start.nsec,
748 		};
749 		u64 pin_duration = timespec64_to_ns(&pin_duration_ts);
750 		u64 pin_start = timespec64_to_ns(&pin_start_ts);
751 		u32 pin_duration32;
752 		u64 now;
753 
754 		/* ptppindur: 32 bit register which holds the interval between
755 		 * 2 edges on PTP_CLK. So check for truncation which happens
756 		 * at periods larger than around 68.7 seconds.
757 		 */
758 		pin_duration = ns_to_sja1105_ticks(pin_duration / 2);
759 		if (pin_duration > U32_MAX) {
760 			rc = -ERANGE;
761 			goto out;
762 		}
763 		pin_duration32 = pin_duration;
764 
765 		/* ptppins: 64 bit register which needs to hold a PTP time
766 		 * larger than the current time, otherwise the startptpcp
767 		 * command won't do anything. So advance the current time
768 		 * by a number of periods in a way that won't alter the
769 		 * phase offset.
770 		 */
771 		rc = __sja1105_ptp_gettimex(priv->ds, &now, NULL);
772 		if (rc < 0)
773 			goto out;
774 
775 		pin_start = future_base_time(pin_start, pin_duration,
776 					     now + 1ull * NSEC_PER_SEC);
777 		pin_start = ns_to_sja1105_ticks(pin_start);
778 
779 		rc = sja1105_xfer_u64(priv, SPI_WRITE, regs->ptppinst,
780 				      &pin_start, NULL);
781 		if (rc < 0)
782 			goto out;
783 
784 		rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptppindur,
785 				      &pin_duration32, NULL);
786 		if (rc < 0)
787 			goto out;
788 	}
789 
790 	if (on)
791 		cmd.startptpcp = true;
792 	else
793 		cmd.stopptpcp = true;
794 
795 	rc = sja1105_ptp_commit(priv->ds, &cmd, SPI_WRITE);
796 
797 out:
798 	mutex_unlock(&ptp_data->lock);
799 
800 	return rc;
801 }
802 
sja1105_extts_enable(struct sja1105_private * priv,struct ptp_extts_request * extts,bool on)803 static int sja1105_extts_enable(struct sja1105_private *priv,
804 				struct ptp_extts_request *extts,
805 				bool on)
806 {
807 	int rc;
808 
809 	/* We only support one channel */
810 	if (extts->index != 0)
811 		return -EOPNOTSUPP;
812 
813 	/* Reject requests with unsupported flags */
814 	if (extts->flags & ~(PTP_ENABLE_FEATURE |
815 			     PTP_RISING_EDGE |
816 			     PTP_FALLING_EDGE |
817 			     PTP_STRICT_FLAGS))
818 		return -EOPNOTSUPP;
819 
820 	/* We can only enable time stamping on both edges, sadly. */
821 	if ((extts->flags & PTP_STRICT_FLAGS) &&
822 	    (extts->flags & PTP_ENABLE_FEATURE) &&
823 	    (extts->flags & PTP_EXTTS_EDGES) != PTP_EXTTS_EDGES)
824 		return -EOPNOTSUPP;
825 
826 	rc = sja1105_change_ptp_clk_pin_func(priv, PTP_PF_EXTTS);
827 	if (rc)
828 		return rc;
829 
830 	priv->ptp_data.extts_enabled = on;
831 
832 	if (on)
833 		sja1105_ptp_extts_setup_timer(&priv->ptp_data);
834 	else
835 		del_timer_sync(&priv->ptp_data.extts_timer);
836 
837 	return 0;
838 }
839 
sja1105_ptp_enable(struct ptp_clock_info * ptp,struct ptp_clock_request * req,int on)840 static int sja1105_ptp_enable(struct ptp_clock_info *ptp,
841 			      struct ptp_clock_request *req, int on)
842 {
843 	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
844 	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
845 	int rc = -EOPNOTSUPP;
846 
847 	if (req->type == PTP_CLK_REQ_PEROUT)
848 		rc = sja1105_per_out_enable(priv, &req->perout, on);
849 	else if (req->type == PTP_CLK_REQ_EXTTS)
850 		rc = sja1105_extts_enable(priv, &req->extts, on);
851 
852 	return rc;
853 }
854 
sja1105_ptp_verify_pin(struct ptp_clock_info * ptp,unsigned int pin,enum ptp_pin_function func,unsigned int chan)855 static int sja1105_ptp_verify_pin(struct ptp_clock_info *ptp, unsigned int pin,
856 				  enum ptp_pin_function func, unsigned int chan)
857 {
858 	struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
859 	struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
860 
861 	if (chan != 0 || pin != 0)
862 		return -1;
863 
864 	switch (func) {
865 	case PTP_PF_NONE:
866 	case PTP_PF_PEROUT:
867 		break;
868 	case PTP_PF_EXTTS:
869 		if (priv->info->device_id == SJA1105E_DEVICE_ID ||
870 		    priv->info->device_id == SJA1105T_DEVICE_ID)
871 			return -1;
872 		break;
873 	default:
874 		return -1;
875 	}
876 	return 0;
877 }
878 
879 static struct ptp_pin_desc sja1105_ptp_pin = {
880 	.name = "ptp_clk",
881 	.index = 0,
882 	.func = PTP_PF_NONE,
883 };
884 
sja1105_ptp_clock_register(struct dsa_switch * ds)885 int sja1105_ptp_clock_register(struct dsa_switch *ds)
886 {
887 	struct sja1105_private *priv = ds->priv;
888 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
889 
890 	ptp_data->caps = (struct ptp_clock_info) {
891 		.owner		= THIS_MODULE,
892 		.name		= "SJA1105 PHC",
893 		.adjfine	= sja1105_ptp_adjfine,
894 		.adjtime	= sja1105_ptp_adjtime,
895 		.gettimex64	= sja1105_ptp_gettimex,
896 		.settime64	= sja1105_ptp_settime,
897 		.enable		= sja1105_ptp_enable,
898 		.verify		= sja1105_ptp_verify_pin,
899 		.do_aux_work	= sja1105_rxtstamp_work,
900 		.max_adj	= SJA1105_MAX_ADJ_PPB,
901 		.pin_config	= &sja1105_ptp_pin,
902 		.n_pins		= 1,
903 		.n_ext_ts	= 1,
904 		.n_per_out	= 1,
905 	};
906 
907 	/* Only used on SJA1105 */
908 	skb_queue_head_init(&ptp_data->skb_rxtstamp_queue);
909 	/* Only used on SJA1110 */
910 	skb_queue_head_init(&ptp_data->skb_txtstamp_queue);
911 
912 	ptp_data->clock = ptp_clock_register(&ptp_data->caps, ds->dev);
913 	if (IS_ERR_OR_NULL(ptp_data->clock))
914 		return PTR_ERR(ptp_data->clock);
915 
916 	ptp_data->cmd.corrclk4ts = true;
917 	ptp_data->cmd.ptpclkadd = PTP_SET_MODE;
918 
919 	timer_setup(&ptp_data->extts_timer, sja1105_ptp_extts_timer, 0);
920 
921 	return sja1105_ptp_reset(ds);
922 }
923 
sja1105_ptp_clock_unregister(struct dsa_switch * ds)924 void sja1105_ptp_clock_unregister(struct dsa_switch *ds)
925 {
926 	struct sja1105_private *priv = ds->priv;
927 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
928 
929 	if (IS_ERR_OR_NULL(ptp_data->clock))
930 		return;
931 
932 	del_timer_sync(&ptp_data->extts_timer);
933 	ptp_cancel_worker_sync(ptp_data->clock);
934 	skb_queue_purge(&ptp_data->skb_txtstamp_queue);
935 	skb_queue_purge(&ptp_data->skb_rxtstamp_queue);
936 	ptp_clock_unregister(ptp_data->clock);
937 	ptp_data->clock = NULL;
938 }
939 
sja1105_ptp_txtstamp_skb(struct dsa_switch * ds,int port,struct sk_buff * skb)940 void sja1105_ptp_txtstamp_skb(struct dsa_switch *ds, int port,
941 			      struct sk_buff *skb)
942 {
943 	struct sja1105_private *priv = ds->priv;
944 	struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
945 	struct skb_shared_hwtstamps shwt = {0};
946 	u64 ticks, ts;
947 	int rc;
948 
949 	skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
950 
951 	mutex_lock(&ptp_data->lock);
952 
953 	rc = sja1105_ptpegr_ts_poll(ds, port, &ts);
954 	if (rc < 0) {
955 		dev_err(ds->dev, "timed out polling for tstamp\n");
956 		kfree_skb(skb);
957 		goto out;
958 	}
959 
960 	rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
961 	if (rc < 0) {
962 		dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
963 		kfree_skb(skb);
964 		goto out;
965 	}
966 
967 	ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
968 
969 	shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
970 	skb_complete_tx_timestamp(skb, &shwt);
971 
972 out:
973 	mutex_unlock(&ptp_data->lock);
974 }
975