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 /* This range is actually +/- SJA1105_MAX_ADJ_PPB 18 * divided by 1000 (ppb -> ppm) and with a 16-bit 19 * "fractional" part (actually fixed point). 20 * | 21 * v 22 * Convert scaled_ppm from the +/- ((10^6) << 16) range 23 * into the +/- (1 << 31) range. 24 * 25 * This forgoes a "ppb" numeric representation (up to NSEC_PER_SEC) 26 * and defines the scaling factor between scaled_ppm and the actual 27 * frequency adjustments of the PHC. 28 * 29 * ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16) 30 * simplifies to 31 * ptpclkrate = scaled_ppm * 2^9 / 5^6 32 */ 33 #define SJA1105_CC_MULT_NUM (1 << 9) 34 #define SJA1105_CC_MULT_DEM 15625 35 #define SJA1105_CC_MULT 0x80000000 36 37 enum sja1105_ptp_clk_mode { 38 PTP_ADD_MODE = 1, 39 PTP_SET_MODE = 0, 40 }; 41 42 #define ptp_caps_to_data(d) \ 43 container_of((d), struct sja1105_ptp_data, caps) 44 #define ptp_data_to_sja1105(d) \ 45 container_of((d), struct sja1105_private, ptp_data) 46 47 static int sja1105_init_avb_params(struct sja1105_private *priv, 48 bool on) 49 { 50 struct sja1105_avb_params_entry *avb; 51 struct sja1105_table *table; 52 53 table = &priv->static_config.tables[BLK_IDX_AVB_PARAMS]; 54 55 /* Discard previous AVB Parameters Table */ 56 if (table->entry_count) { 57 kfree(table->entries); 58 table->entry_count = 0; 59 } 60 61 /* Configure the reception of meta frames only if requested */ 62 if (!on) 63 return 0; 64 65 table->entries = kcalloc(SJA1105_MAX_AVB_PARAMS_COUNT, 66 table->ops->unpacked_entry_size, GFP_KERNEL); 67 if (!table->entries) 68 return -ENOMEM; 69 70 table->entry_count = SJA1105_MAX_AVB_PARAMS_COUNT; 71 72 avb = table->entries; 73 74 avb->destmeta = SJA1105_META_DMAC; 75 avb->srcmeta = SJA1105_META_SMAC; 76 77 return 0; 78 } 79 80 /* Must be called only with priv->tagger_data.state bit 81 * SJA1105_HWTS_RX_EN cleared 82 */ 83 static int sja1105_change_rxtstamping(struct sja1105_private *priv, 84 bool on) 85 { 86 struct sja1105_general_params_entry *general_params; 87 struct sja1105_table *table; 88 int rc; 89 90 table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS]; 91 general_params = table->entries; 92 general_params->send_meta1 = on; 93 general_params->send_meta0 = on; 94 95 rc = sja1105_init_avb_params(priv, on); 96 if (rc < 0) 97 return rc; 98 99 /* Initialize the meta state machine to a known state */ 100 if (priv->tagger_data.stampable_skb) { 101 kfree_skb(priv->tagger_data.stampable_skb); 102 priv->tagger_data.stampable_skb = NULL; 103 } 104 105 return sja1105_static_config_reload(priv, SJA1105_RX_HWTSTAMPING); 106 } 107 108 int sja1105_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr) 109 { 110 struct sja1105_private *priv = ds->priv; 111 struct hwtstamp_config config; 112 bool rx_on; 113 int rc; 114 115 if (copy_from_user(&config, ifr->ifr_data, sizeof(config))) 116 return -EFAULT; 117 118 switch (config.tx_type) { 119 case HWTSTAMP_TX_OFF: 120 priv->ports[port].hwts_tx_en = false; 121 break; 122 case HWTSTAMP_TX_ON: 123 priv->ports[port].hwts_tx_en = true; 124 break; 125 default: 126 return -ERANGE; 127 } 128 129 switch (config.rx_filter) { 130 case HWTSTAMP_FILTER_NONE: 131 rx_on = false; 132 break; 133 default: 134 rx_on = true; 135 break; 136 } 137 138 if (rx_on != test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state)) { 139 clear_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state); 140 141 rc = sja1105_change_rxtstamping(priv, rx_on); 142 if (rc < 0) { 143 dev_err(ds->dev, 144 "Failed to change RX timestamping: %d\n", rc); 145 return rc; 146 } 147 if (rx_on) 148 set_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state); 149 } 150 151 if (copy_to_user(ifr->ifr_data, &config, sizeof(config))) 152 return -EFAULT; 153 return 0; 154 } 155 156 int sja1105_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr) 157 { 158 struct sja1105_private *priv = ds->priv; 159 struct hwtstamp_config config; 160 161 config.flags = 0; 162 if (priv->ports[port].hwts_tx_en) 163 config.tx_type = HWTSTAMP_TX_ON; 164 else 165 config.tx_type = HWTSTAMP_TX_OFF; 166 if (test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state)) 167 config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; 168 else 169 config.rx_filter = HWTSTAMP_FILTER_NONE; 170 171 return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? 172 -EFAULT : 0; 173 } 174 175 int sja1105_get_ts_info(struct dsa_switch *ds, int port, 176 struct ethtool_ts_info *info) 177 { 178 struct sja1105_private *priv = ds->priv; 179 struct sja1105_ptp_data *ptp_data = &priv->ptp_data; 180 181 /* Called during cleanup */ 182 if (!ptp_data->clock) 183 return -ENODEV; 184 185 info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE | 186 SOF_TIMESTAMPING_RX_HARDWARE | 187 SOF_TIMESTAMPING_RAW_HARDWARE; 188 info->tx_types = (1 << HWTSTAMP_TX_OFF) | 189 (1 << HWTSTAMP_TX_ON); 190 info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) | 191 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT); 192 info->phc_index = ptp_clock_index(ptp_data->clock); 193 return 0; 194 } 195 196 void sja1105et_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd, 197 enum packing_op op) 198 { 199 const int size = SJA1105_SIZE_PTP_CMD; 200 /* No need to keep this as part of the structure */ 201 u64 valid = 1; 202 203 sja1105_packing(buf, &valid, 31, 31, size, op); 204 sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op); 205 sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op); 206 sja1105_packing(buf, &cmd->resptp, 2, 2, size, op); 207 sja1105_packing(buf, &cmd->corrclk4ts, 1, 1, size, op); 208 sja1105_packing(buf, &cmd->ptpclkadd, 0, 0, size, op); 209 } 210 211 void sja1105pqrs_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd, 212 enum packing_op op) 213 { 214 const int size = SJA1105_SIZE_PTP_CMD; 215 /* No need to keep this as part of the structure */ 216 u64 valid = 1; 217 218 sja1105_packing(buf, &valid, 31, 31, size, op); 219 sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op); 220 sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op); 221 sja1105_packing(buf, &cmd->resptp, 3, 3, size, op); 222 sja1105_packing(buf, &cmd->corrclk4ts, 2, 2, size, op); 223 sja1105_packing(buf, &cmd->ptpclkadd, 0, 0, size, op); 224 } 225 226 int sja1105_ptp_commit(struct dsa_switch *ds, struct sja1105_ptp_cmd *cmd, 227 sja1105_spi_rw_mode_t rw) 228 { 229 const struct sja1105_private *priv = ds->priv; 230 const struct sja1105_regs *regs = priv->info->regs; 231 u8 buf[SJA1105_SIZE_PTP_CMD] = {0}; 232 int rc; 233 234 if (rw == SPI_WRITE) 235 priv->info->ptp_cmd_packing(buf, cmd, PACK); 236 237 rc = sja1105_xfer_buf(priv, SPI_WRITE, regs->ptp_control, buf, 238 SJA1105_SIZE_PTP_CMD); 239 240 if (rw == SPI_READ) 241 priv->info->ptp_cmd_packing(buf, cmd, UNPACK); 242 243 return rc; 244 } 245 246 /* The switch returns partial timestamps (24 bits for SJA1105 E/T, which wrap 247 * around in 0.135 seconds, and 32 bits for P/Q/R/S, wrapping around in 34.35 248 * seconds). 249 * 250 * This receives the RX or TX MAC timestamps, provided by hardware as 251 * the lower bits of the cycle counter, sampled at the time the timestamp was 252 * collected. 253 * 254 * To reconstruct into a full 64-bit-wide timestamp, the cycle counter is 255 * read and the high-order bits are filled in. 256 * 257 * Must be called within one wraparound period of the partial timestamp since 258 * it was generated by the MAC. 259 */ 260 static u64 sja1105_tstamp_reconstruct(struct dsa_switch *ds, u64 now, 261 u64 ts_partial) 262 { 263 struct sja1105_private *priv = ds->priv; 264 u64 partial_tstamp_mask = CYCLECOUNTER_MASK(priv->info->ptp_ts_bits); 265 u64 ts_reconstructed; 266 267 ts_reconstructed = (now & ~partial_tstamp_mask) | ts_partial; 268 269 /* Check lower bits of current cycle counter against the timestamp. 270 * If the current cycle counter is lower than the partial timestamp, 271 * then wraparound surely occurred and must be accounted for. 272 */ 273 if ((now & partial_tstamp_mask) <= ts_partial) 274 ts_reconstructed -= (partial_tstamp_mask + 1); 275 276 return ts_reconstructed; 277 } 278 279 /* Reads the SPI interface for an egress timestamp generated by the switch 280 * for frames sent using management routes. 281 * 282 * SJA1105 E/T layout of the 4-byte SPI payload: 283 * 284 * 31 23 15 7 0 285 * | | | | | 286 * +-----+-----+-----+ ^ 287 * ^ | 288 * | | 289 * 24-bit timestamp Update bit 290 * 291 * 292 * SJA1105 P/Q/R/S layout of the 8-byte SPI payload: 293 * 294 * 31 23 15 7 0 63 55 47 39 32 295 * | | | | | | | | | | 296 * ^ +-----+-----+-----+-----+ 297 * | ^ 298 * | | 299 * Update bit 32-bit timestamp 300 * 301 * Notice that the update bit is in the same place. 302 * To have common code for E/T and P/Q/R/S for reading the timestamp, 303 * we need to juggle with the offset and the bit indices. 304 */ 305 static int sja1105_ptpegr_ts_poll(struct dsa_switch *ds, int port, u64 *ts) 306 { 307 struct sja1105_private *priv = ds->priv; 308 const struct sja1105_regs *regs = priv->info->regs; 309 int tstamp_bit_start, tstamp_bit_end; 310 int timeout = 10; 311 u8 packed_buf[8]; 312 u64 update; 313 int rc; 314 315 do { 316 rc = sja1105_xfer_buf(priv, SPI_READ, regs->ptpegr_ts[port], 317 packed_buf, priv->info->ptpegr_ts_bytes); 318 if (rc < 0) 319 return rc; 320 321 sja1105_unpack(packed_buf, &update, 0, 0, 322 priv->info->ptpegr_ts_bytes); 323 if (update) 324 break; 325 326 usleep_range(10, 50); 327 } while (--timeout); 328 329 if (!timeout) 330 return -ETIMEDOUT; 331 332 /* Point the end bit to the second 32-bit word on P/Q/R/S, 333 * no-op on E/T. 334 */ 335 tstamp_bit_end = (priv->info->ptpegr_ts_bytes - 4) * 8; 336 /* Shift the 24-bit timestamp on E/T to be collected from 31:8. 337 * No-op on P/Q/R/S. 338 */ 339 tstamp_bit_end += 32 - priv->info->ptp_ts_bits; 340 tstamp_bit_start = tstamp_bit_end + priv->info->ptp_ts_bits - 1; 341 342 *ts = 0; 343 344 sja1105_unpack(packed_buf, ts, tstamp_bit_start, tstamp_bit_end, 345 priv->info->ptpegr_ts_bytes); 346 347 return 0; 348 } 349 350 /* Caller must hold ptp_data->lock */ 351 static int sja1105_ptpclkval_read(struct sja1105_private *priv, u64 *ticks, 352 struct ptp_system_timestamp *ptp_sts) 353 { 354 const struct sja1105_regs *regs = priv->info->regs; 355 356 return sja1105_xfer_u64(priv, SPI_READ, regs->ptpclkval, ticks, 357 ptp_sts); 358 } 359 360 /* Caller must hold ptp_data->lock */ 361 static int sja1105_ptpclkval_write(struct sja1105_private *priv, u64 ticks, 362 struct ptp_system_timestamp *ptp_sts) 363 { 364 const struct sja1105_regs *regs = priv->info->regs; 365 366 return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpclkval, &ticks, 367 ptp_sts); 368 } 369 370 #define rxtstamp_to_tagger(d) \ 371 container_of((d), struct sja1105_tagger_data, rxtstamp_work) 372 #define tagger_to_sja1105(d) \ 373 container_of((d), struct sja1105_private, tagger_data) 374 375 static void sja1105_rxtstamp_work(struct work_struct *work) 376 { 377 struct sja1105_tagger_data *tagger_data = rxtstamp_to_tagger(work); 378 struct sja1105_private *priv = tagger_to_sja1105(tagger_data); 379 struct sja1105_ptp_data *ptp_data = &priv->ptp_data; 380 struct dsa_switch *ds = priv->ds; 381 struct sk_buff *skb; 382 383 mutex_lock(&ptp_data->lock); 384 385 while ((skb = skb_dequeue(&tagger_data->skb_rxtstamp_queue)) != NULL) { 386 struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb); 387 u64 ticks, ts; 388 int rc; 389 390 rc = sja1105_ptpclkval_read(priv, &ticks, NULL); 391 if (rc < 0) { 392 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc); 393 kfree_skb(skb); 394 continue; 395 } 396 397 *shwt = (struct skb_shared_hwtstamps) {0}; 398 399 ts = SJA1105_SKB_CB(skb)->meta_tstamp; 400 ts = sja1105_tstamp_reconstruct(ds, ticks, ts); 401 402 shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts)); 403 netif_rx_ni(skb); 404 } 405 406 mutex_unlock(&ptp_data->lock); 407 } 408 409 /* Called from dsa_skb_defer_rx_timestamp */ 410 bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port, 411 struct sk_buff *skb, unsigned int type) 412 { 413 struct sja1105_private *priv = ds->priv; 414 struct sja1105_tagger_data *tagger_data = &priv->tagger_data; 415 416 if (!test_bit(SJA1105_HWTS_RX_EN, &tagger_data->state)) 417 return false; 418 419 /* We need to read the full PTP clock to reconstruct the Rx 420 * timestamp. For that we need a sleepable context. 421 */ 422 skb_queue_tail(&tagger_data->skb_rxtstamp_queue, skb); 423 schedule_work(&tagger_data->rxtstamp_work); 424 return true; 425 } 426 427 /* Called from dsa_skb_tx_timestamp. This callback is just to make DSA clone 428 * the skb and have it available in DSA_SKB_CB in the .port_deferred_xmit 429 * callback, where we will timestamp it synchronously. 430 */ 431 bool sja1105_port_txtstamp(struct dsa_switch *ds, int port, 432 struct sk_buff *skb, unsigned int type) 433 { 434 struct sja1105_private *priv = ds->priv; 435 struct sja1105_port *sp = &priv->ports[port]; 436 437 if (!sp->hwts_tx_en) 438 return false; 439 440 return true; 441 } 442 443 static int sja1105_ptp_reset(struct dsa_switch *ds) 444 { 445 struct sja1105_private *priv = ds->priv; 446 struct sja1105_ptp_data *ptp_data = &priv->ptp_data; 447 struct sja1105_ptp_cmd cmd = ptp_data->cmd; 448 int rc; 449 450 mutex_lock(&ptp_data->lock); 451 452 cmd.resptp = 1; 453 454 dev_dbg(ds->dev, "Resetting PTP clock\n"); 455 rc = sja1105_ptp_commit(ds, &cmd, SPI_WRITE); 456 457 sja1105_tas_clockstep(priv->ds); 458 459 mutex_unlock(&ptp_data->lock); 460 461 return rc; 462 } 463 464 /* Caller must hold ptp_data->lock */ 465 int __sja1105_ptp_gettimex(struct dsa_switch *ds, u64 *ns, 466 struct ptp_system_timestamp *ptp_sts) 467 { 468 struct sja1105_private *priv = ds->priv; 469 u64 ticks; 470 int rc; 471 472 rc = sja1105_ptpclkval_read(priv, &ticks, ptp_sts); 473 if (rc < 0) { 474 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc); 475 return rc; 476 } 477 478 *ns = sja1105_ticks_to_ns(ticks); 479 480 return 0; 481 } 482 483 static int sja1105_ptp_gettimex(struct ptp_clock_info *ptp, 484 struct timespec64 *ts, 485 struct ptp_system_timestamp *ptp_sts) 486 { 487 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp); 488 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data); 489 u64 now = 0; 490 int rc; 491 492 mutex_lock(&ptp_data->lock); 493 494 rc = __sja1105_ptp_gettimex(priv->ds, &now, ptp_sts); 495 *ts = ns_to_timespec64(now); 496 497 mutex_unlock(&ptp_data->lock); 498 499 return rc; 500 } 501 502 /* Caller must hold ptp_data->lock */ 503 static int sja1105_ptp_mode_set(struct sja1105_private *priv, 504 enum sja1105_ptp_clk_mode mode) 505 { 506 struct sja1105_ptp_data *ptp_data = &priv->ptp_data; 507 508 if (ptp_data->cmd.ptpclkadd == mode) 509 return 0; 510 511 ptp_data->cmd.ptpclkadd = mode; 512 513 return sja1105_ptp_commit(priv->ds, &ptp_data->cmd, SPI_WRITE); 514 } 515 516 /* Write to PTPCLKVAL while PTPCLKADD is 0 */ 517 int __sja1105_ptp_settime(struct dsa_switch *ds, u64 ns, 518 struct ptp_system_timestamp *ptp_sts) 519 { 520 struct sja1105_private *priv = ds->priv; 521 u64 ticks = ns_to_sja1105_ticks(ns); 522 int rc; 523 524 rc = sja1105_ptp_mode_set(priv, PTP_SET_MODE); 525 if (rc < 0) { 526 dev_err(priv->ds->dev, "Failed to put PTPCLK in set mode\n"); 527 return rc; 528 } 529 530 rc = sja1105_ptpclkval_write(priv, ticks, ptp_sts); 531 532 sja1105_tas_clockstep(priv->ds); 533 534 return rc; 535 } 536 537 static int sja1105_ptp_settime(struct ptp_clock_info *ptp, 538 const struct timespec64 *ts) 539 { 540 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp); 541 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data); 542 u64 ns = timespec64_to_ns(ts); 543 int rc; 544 545 mutex_lock(&ptp_data->lock); 546 547 rc = __sja1105_ptp_settime(priv->ds, ns, NULL); 548 549 mutex_unlock(&ptp_data->lock); 550 551 return rc; 552 } 553 554 static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 555 { 556 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp); 557 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data); 558 const struct sja1105_regs *regs = priv->info->regs; 559 u32 clkrate32; 560 s64 clkrate; 561 int rc; 562 563 clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM; 564 clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM); 565 566 /* Take a +/- value and re-center it around 2^31. */ 567 clkrate = SJA1105_CC_MULT + clkrate; 568 WARN_ON(abs(clkrate) >= GENMASK_ULL(31, 0)); 569 clkrate32 = clkrate; 570 571 mutex_lock(&ptp_data->lock); 572 573 rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptpclkrate, &clkrate32, 574 NULL); 575 576 sja1105_tas_adjfreq(priv->ds); 577 578 mutex_unlock(&ptp_data->lock); 579 580 return rc; 581 } 582 583 /* Write to PTPCLKVAL while PTPCLKADD is 1 */ 584 int __sja1105_ptp_adjtime(struct dsa_switch *ds, s64 delta) 585 { 586 struct sja1105_private *priv = ds->priv; 587 s64 ticks = ns_to_sja1105_ticks(delta); 588 int rc; 589 590 rc = sja1105_ptp_mode_set(priv, PTP_ADD_MODE); 591 if (rc < 0) { 592 dev_err(priv->ds->dev, "Failed to put PTPCLK in add mode\n"); 593 return rc; 594 } 595 596 rc = sja1105_ptpclkval_write(priv, ticks, NULL); 597 598 sja1105_tas_clockstep(priv->ds); 599 600 return rc; 601 } 602 603 static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 604 { 605 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp); 606 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data); 607 int rc; 608 609 mutex_lock(&ptp_data->lock); 610 611 rc = __sja1105_ptp_adjtime(priv->ds, delta); 612 613 mutex_unlock(&ptp_data->lock); 614 615 return rc; 616 } 617 618 int sja1105_ptp_clock_register(struct dsa_switch *ds) 619 { 620 struct sja1105_private *priv = ds->priv; 621 struct sja1105_tagger_data *tagger_data = &priv->tagger_data; 622 struct sja1105_ptp_data *ptp_data = &priv->ptp_data; 623 624 ptp_data->caps = (struct ptp_clock_info) { 625 .owner = THIS_MODULE, 626 .name = "SJA1105 PHC", 627 .adjfine = sja1105_ptp_adjfine, 628 .adjtime = sja1105_ptp_adjtime, 629 .gettimex64 = sja1105_ptp_gettimex, 630 .settime64 = sja1105_ptp_settime, 631 .max_adj = SJA1105_MAX_ADJ_PPB, 632 }; 633 634 skb_queue_head_init(&tagger_data->skb_rxtstamp_queue); 635 INIT_WORK(&tagger_data->rxtstamp_work, sja1105_rxtstamp_work); 636 spin_lock_init(&tagger_data->meta_lock); 637 638 ptp_data->clock = ptp_clock_register(&ptp_data->caps, ds->dev); 639 if (IS_ERR_OR_NULL(ptp_data->clock)) 640 return PTR_ERR(ptp_data->clock); 641 642 ptp_data->cmd.corrclk4ts = true; 643 ptp_data->cmd.ptpclkadd = PTP_SET_MODE; 644 645 return sja1105_ptp_reset(ds); 646 } 647 648 void sja1105_ptp_clock_unregister(struct dsa_switch *ds) 649 { 650 struct sja1105_private *priv = ds->priv; 651 struct sja1105_ptp_data *ptp_data = &priv->ptp_data; 652 653 if (IS_ERR_OR_NULL(ptp_data->clock)) 654 return; 655 656 cancel_work_sync(&priv->tagger_data.rxtstamp_work); 657 skb_queue_purge(&priv->tagger_data.skb_rxtstamp_queue); 658 ptp_clock_unregister(ptp_data->clock); 659 ptp_data->clock = NULL; 660 } 661 662 void sja1105_ptp_txtstamp_skb(struct dsa_switch *ds, int slot, 663 struct sk_buff *skb) 664 { 665 struct sja1105_private *priv = ds->priv; 666 struct sja1105_ptp_data *ptp_data = &priv->ptp_data; 667 struct skb_shared_hwtstamps shwt = {0}; 668 u64 ticks, ts; 669 int rc; 670 671 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; 672 673 mutex_lock(&ptp_data->lock); 674 675 rc = sja1105_ptpclkval_read(priv, &ticks, NULL); 676 if (rc < 0) { 677 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc); 678 kfree_skb(skb); 679 goto out; 680 } 681 682 rc = sja1105_ptpegr_ts_poll(ds, slot, &ts); 683 if (rc < 0) { 684 dev_err(ds->dev, "timed out polling for tstamp\n"); 685 kfree_skb(skb); 686 goto out; 687 } 688 689 ts = sja1105_tstamp_reconstruct(ds, ticks, ts); 690 691 shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts)); 692 skb_complete_tx_timestamp(skb, &shwt); 693 694 out: 695 mutex_unlock(&ptp_data->lock); 696 } 697