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 | 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(to: &config, from: ifr->ifr_data, n: 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(to: ifr->ifr_data, from: &config, n: sizeof(config))) |
90 | return -EFAULT; |
91 | return 0; |
92 | } |
93 | |
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(to: ifr->ifr_data, from: &config, n: sizeof(config)) ? |
110 | -EFAULT : 0; |
111 | } |
112 | |
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: ptp_data->clock); |
131 | return 0; |
132 | } |
133 | |
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, val: &valid, start: 31, end: 31, len: size, op); |
142 | sja1105_packing(buf, val: &cmd->ptpstrtsch, start: 30, end: 30, len: size, op); |
143 | sja1105_packing(buf, val: &cmd->ptpstopsch, start: 29, end: 29, len: size, op); |
144 | sja1105_packing(buf, val: &cmd->startptpcp, start: 28, end: 28, len: size, op); |
145 | sja1105_packing(buf, val: &cmd->stopptpcp, start: 27, end: 27, len: size, op); |
146 | sja1105_packing(buf, val: &cmd->resptp, start: 2, end: 2, len: size, op); |
147 | sja1105_packing(buf, val: &cmd->corrclk4ts, start: 1, end: 1, len: size, op); |
148 | sja1105_packing(buf, val: &cmd->ptpclkadd, start: 0, end: 0, len: size, op); |
149 | } |
150 | |
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, val: &valid, start: 31, end: 31, len: size, op); |
159 | sja1105_packing(buf, val: &cmd->ptpstrtsch, start: 30, end: 30, len: size, op); |
160 | sja1105_packing(buf, val: &cmd->ptpstopsch, start: 29, end: 29, len: size, op); |
161 | sja1105_packing(buf, val: &cmd->startptpcp, start: 28, end: 28, len: size, op); |
162 | sja1105_packing(buf, val: &cmd->stopptpcp, start: 27, end: 27, len: size, op); |
163 | sja1105_packing(buf, val: &cmd->resptp, start: 3, end: 3, len: size, op); |
164 | sja1105_packing(buf, val: &cmd->corrclk4ts, start: 2, end: 2, len: size, op); |
165 | sja1105_packing(buf, val: &cmd->ptpclkadd, start: 0, end: 0, len: size, op); |
166 | } |
167 | |
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, reg_addr: 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 | */ |
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 | */ |
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, rw: SPI_READ, reg_addr: regs->ptpegr_ts[port], |
259 | buf: packed_buf, len: priv->info->ptpegr_ts_bytes); |
260 | if (rc < 0) |
261 | return rc; |
262 | |
263 | sja1105_unpack(buf: packed_buf, val: &update, start: 0, end: 0, |
264 | len: priv->info->ptpegr_ts_bytes); |
265 | if (update) |
266 | break; |
267 | |
268 | usleep_range(min: 10, max: 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(buf: packed_buf, val: ts, start: tstamp_bit_start, end: tstamp_bit_end, |
287 | len: priv->info->ptpegr_ts_bytes); |
288 | |
289 | return 0; |
290 | } |
291 | |
292 | /* Caller must hold ptp_data->lock */ |
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, rw: SPI_READ, reg_addr: regs->ptpclkval, value: ticks, |
299 | ptp_sts); |
300 | } |
301 | |
302 | /* Caller must hold ptp_data->lock */ |
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, rw: SPI_WRITE, reg_addr: regs->ptpclkval, value: &ticks, |
309 | ptp_sts); |
310 | } |
311 | |
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, rw: SPI_READ, reg_addr: regs->ptpsyncts, value: &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(ns: sja1105_ticks_to_ns(ticks: ptpsyncts)); |
330 | ptp_clock_event(ptp: ptp_data->clock, event: &event); |
331 | |
332 | ptp_data->ptpsyncts = ptpsyncts; |
333 | } |
334 | } |
335 | |
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(list: &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: &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 | |
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(list: &ptp_data->skb_rxtstamp_queue, newsk: skb); |
387 | ptp_schedule_worker(ptp: ptp_data->clock, delay: 0); |
388 | return true; |
389 | } |
390 | |
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 */ |
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 | |
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(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, list: &ptp_data->skb_txtstamp_queue); |
432 | skb_match = skb; |
433 | |
434 | break; |
435 | } |
436 | |
437 | spin_unlock(lock: &ptp_data->skb_txtstamp_queue.lock); |
438 | |
439 | if (WARN_ON(!skb_match)) |
440 | return; |
441 | |
442 | shwt.hwtstamp = ns_to_ktime(ns: sja1105_ticks_to_ns(ticks: tstamp)); |
443 | skb_complete_tx_timestamp(skb: skb_match, hwtstamps: &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 | */ |
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(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(lock: &priv->ts_id_lock); |
468 | |
469 | skb_queue_tail(list: &ptp_data->skb_txtstamp_queue, newsk: 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 | */ |
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 | |
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: &cmd, rw: SPI_WRITE); |
507 | |
508 | sja1105_tas_clockstep(ds: priv->ds); |
509 | |
510 | mutex_unlock(lock: &ptp_data->lock); |
511 | |
512 | return rc; |
513 | } |
514 | |
515 | /* Caller must hold ptp_data->lock */ |
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: &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 | |
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(ds: priv->ds, ns: &now, ptp_sts); |
546 | *ts = ns_to_timespec64(nsec: now); |
547 | |
548 | mutex_unlock(lock: &ptp_data->lock); |
549 | |
550 | return rc; |
551 | } |
552 | |
553 | /* Caller must hold ptp_data->lock */ |
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(ds: priv->ds, cmd: &ptp_data->cmd, rw: SPI_WRITE); |
565 | } |
566 | |
567 | /* Write to PTPCLKVAL while PTPCLKADD is 0 */ |
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, mode: 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(ds: priv->ds); |
584 | |
585 | return rc; |
586 | } |
587 | |
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(ds: priv->ds, ns, NULL); |
599 | |
600 | mutex_unlock(lock: &ptp_data->lock); |
601 | |
602 | return rc; |
603 | } |
604 | |
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(dividend: 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, rw: SPI_WRITE, reg_addr: regs->ptpclkrate, value: &clkrate32, |
625 | NULL); |
626 | |
627 | sja1105_tas_adjfreq(ds: priv->ds); |
628 | |
629 | mutex_unlock(lock: &ptp_data->lock); |
630 | |
631 | return rc; |
632 | } |
633 | |
634 | /* Write to PTPCLKVAL while PTPCLKADD is 1 */ |
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(ns: delta); |
639 | int rc; |
640 | |
641 | rc = sja1105_ptp_mode_set(priv, mode: 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(ds: priv->ds); |
650 | |
651 | return rc; |
652 | } |
653 | |
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(ds: priv->ds, delta); |
663 | |
664 | mutex_unlock(lock: &ptp_data->lock); |
665 | |
666 | return rc; |
667 | } |
668 | |
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(timer: &ptp_data->extts_timer, expires); |
675 | } |
676 | |
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: ptp_data->clock, delay: 0); |
682 | |
683 | sja1105_ptp_extts_setup_timer(ptp_data); |
684 | } |
685 | |
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: BLK_IDX_AVB_PARAMS, index: 0, entry: avb, |
707 | keep: 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 | */ |
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, func: 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(ts: &pin_duration_ts); |
750 | u64 pin_start = timespec64_to_ns(ts: &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(ns: 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(ds: priv->ds, ns: &now, NULL); |
772 | if (rc < 0) |
773 | goto out; |
774 | |
775 | pin_start = future_base_time(base_time: pin_start, cycle_time: pin_duration, |
776 | now: now + 1ull * NSEC_PER_SEC); |
777 | pin_start = ns_to_sja1105_ticks(ns: pin_start); |
778 | |
779 | rc = sja1105_xfer_u64(priv, rw: SPI_WRITE, reg_addr: regs->ptppinst, |
780 | value: &pin_start, NULL); |
781 | if (rc < 0) |
782 | goto out; |
783 | |
784 | rc = sja1105_xfer_u32(priv, rw: SPI_WRITE, reg_addr: regs->ptppindur, |
785 | value: &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(ds: priv->ds, cmd: &cmd, rw: SPI_WRITE); |
796 | |
797 | out: |
798 | mutex_unlock(lock: &ptp_data->lock); |
799 | |
800 | return rc; |
801 | } |
802 | |
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, func: 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(ptp_data: &priv->ptp_data); |
834 | else |
835 | del_timer_sync(timer: &priv->ptp_data.extts_timer); |
836 | |
837 | return 0; |
838 | } |
839 | |
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, perout: &req->perout, on); |
849 | else if (req->type == PTP_CLK_REQ_EXTTS) |
850 | rc = sja1105_extts_enable(priv, extts: &req->extts, on); |
851 | |
852 | return rc; |
853 | } |
854 | |
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 | |
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(list: &ptp_data->skb_rxtstamp_queue); |
909 | /* Only used on SJA1110 */ |
910 | skb_queue_head_init(list: &ptp_data->skb_txtstamp_queue); |
911 | |
912 | ptp_data->clock = ptp_clock_register(info: &ptp_data->caps, parent: ds->dev); |
913 | if (IS_ERR_OR_NULL(ptr: ptp_data->clock)) |
914 | return PTR_ERR(ptr: 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 | |
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(ptr: ptp_data->clock)) |
930 | return; |
931 | |
932 | del_timer_sync(timer: &ptp_data->extts_timer); |
933 | ptp_cancel_worker_sync(ptp: ptp_data->clock); |
934 | skb_queue_purge(list: &ptp_data->skb_txtstamp_queue); |
935 | skb_queue_purge(list: &ptp_data->skb_rxtstamp_queue); |
936 | ptp_clock_unregister(ptp: ptp_data->clock); |
937 | ptp_data->clock = NULL; |
938 | } |
939 | |
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: &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: &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, now: ticks, ts_partial: ts); |
968 | |
969 | shwt.hwtstamp = ns_to_ktime(ns: sja1105_ticks_to_ns(ticks: ts)); |
970 | skb_complete_tx_timestamp(skb, hwtstamps: &shwt); |
971 | |
972 | out: |
973 | mutex_unlock(lock: &ptp_data->lock); |
974 | } |
975 | |