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