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