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_ptp_data *ptp_data = &priv->ptp_data; 87 struct sja1105_general_params_entry *general_params; 88 struct sja1105_table *table; 89 int rc; 90 91 table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS]; 92 general_params = table->entries; 93 general_params->send_meta1 = on; 94 general_params->send_meta0 = on; 95 96 rc = sja1105_init_avb_params(priv, on); 97 if (rc < 0) 98 return rc; 99 100 /* Initialize the meta state machine to a known state */ 101 if (priv->tagger_data.stampable_skb) { 102 kfree_skb(priv->tagger_data.stampable_skb); 103 priv->tagger_data.stampable_skb = NULL; 104 } 105 ptp_cancel_worker_sync(ptp_data->clock); 106 skb_queue_purge(&ptp_data->skb_rxtstamp_queue); 107 108 return sja1105_static_config_reload(priv, SJA1105_RX_HWTSTAMPING); 109 } 110 111 int sja1105_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr) 112 { 113 struct sja1105_private *priv = ds->priv; 114 struct hwtstamp_config config; 115 bool rx_on; 116 int rc; 117 118 if (copy_from_user(&config, ifr->ifr_data, sizeof(config))) 119 return -EFAULT; 120 121 switch (config.tx_type) { 122 case HWTSTAMP_TX_OFF: 123 priv->ports[port].hwts_tx_en = false; 124 break; 125 case HWTSTAMP_TX_ON: 126 priv->ports[port].hwts_tx_en = true; 127 break; 128 default: 129 return -ERANGE; 130 } 131 132 switch (config.rx_filter) { 133 case HWTSTAMP_FILTER_NONE: 134 rx_on = false; 135 break; 136 default: 137 rx_on = true; 138 break; 139 } 140 141 if (rx_on != test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state)) { 142 clear_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state); 143 144 rc = sja1105_change_rxtstamping(priv, rx_on); 145 if (rc < 0) { 146 dev_err(ds->dev, 147 "Failed to change RX timestamping: %d\n", rc); 148 return rc; 149 } 150 if (rx_on) 151 set_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state); 152 } 153 154 if (copy_to_user(ifr->ifr_data, &config, sizeof(config))) 155 return -EFAULT; 156 return 0; 157 } 158 159 int sja1105_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr) 160 { 161 struct sja1105_private *priv = ds->priv; 162 struct hwtstamp_config config; 163 164 config.flags = 0; 165 if (priv->ports[port].hwts_tx_en) 166 config.tx_type = HWTSTAMP_TX_ON; 167 else 168 config.tx_type = HWTSTAMP_TX_OFF; 169 if (test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state)) 170 config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; 171 else 172 config.rx_filter = HWTSTAMP_FILTER_NONE; 173 174 return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? 175 -EFAULT : 0; 176 } 177 178 int sja1105_get_ts_info(struct dsa_switch *ds, int port, 179 struct ethtool_ts_info *info) 180 { 181 struct sja1105_private *priv = ds->priv; 182 struct sja1105_ptp_data *ptp_data = &priv->ptp_data; 183 184 /* Called during cleanup */ 185 if (!ptp_data->clock) 186 return -ENODEV; 187 188 info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE | 189 SOF_TIMESTAMPING_RX_HARDWARE | 190 SOF_TIMESTAMPING_RAW_HARDWARE; 191 info->tx_types = (1 << HWTSTAMP_TX_OFF) | 192 (1 << HWTSTAMP_TX_ON); 193 info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) | 194 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT); 195 info->phc_index = ptp_clock_index(ptp_data->clock); 196 return 0; 197 } 198 199 void sja1105et_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd, 200 enum packing_op op) 201 { 202 const int size = SJA1105_SIZE_PTP_CMD; 203 /* No need to keep this as part of the structure */ 204 u64 valid = 1; 205 206 sja1105_packing(buf, &valid, 31, 31, size, op); 207 sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op); 208 sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op); 209 sja1105_packing(buf, &cmd->resptp, 2, 2, size, op); 210 sja1105_packing(buf, &cmd->corrclk4ts, 1, 1, size, op); 211 sja1105_packing(buf, &cmd->ptpclkadd, 0, 0, size, op); 212 } 213 214 void sja1105pqrs_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd, 215 enum packing_op op) 216 { 217 const int size = SJA1105_SIZE_PTP_CMD; 218 /* No need to keep this as part of the structure */ 219 u64 valid = 1; 220 221 sja1105_packing(buf, &valid, 31, 31, size, op); 222 sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op); 223 sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op); 224 sja1105_packing(buf, &cmd->resptp, 3, 3, size, op); 225 sja1105_packing(buf, &cmd->corrclk4ts, 2, 2, size, op); 226 sja1105_packing(buf, &cmd->ptpclkadd, 0, 0, size, op); 227 } 228 229 int sja1105_ptp_commit(struct dsa_switch *ds, struct sja1105_ptp_cmd *cmd, 230 sja1105_spi_rw_mode_t rw) 231 { 232 const struct sja1105_private *priv = ds->priv; 233 const struct sja1105_regs *regs = priv->info->regs; 234 u8 buf[SJA1105_SIZE_PTP_CMD] = {0}; 235 int rc; 236 237 if (rw == SPI_WRITE) 238 priv->info->ptp_cmd_packing(buf, cmd, PACK); 239 240 rc = sja1105_xfer_buf(priv, rw, regs->ptp_control, buf, 241 SJA1105_SIZE_PTP_CMD); 242 243 if (rw == SPI_READ) 244 priv->info->ptp_cmd_packing(buf, cmd, UNPACK); 245 246 return rc; 247 } 248 249 /* The switch returns partial timestamps (24 bits for SJA1105 E/T, which wrap 250 * around in 0.135 seconds, and 32 bits for P/Q/R/S, wrapping around in 34.35 251 * seconds). 252 * 253 * This receives the RX or TX MAC timestamps, provided by hardware as 254 * the lower bits of the cycle counter, sampled at the time the timestamp was 255 * collected. 256 * 257 * To reconstruct into a full 64-bit-wide timestamp, the cycle counter is 258 * read and the high-order bits are filled in. 259 * 260 * Must be called within one wraparound period of the partial timestamp since 261 * it was generated by the MAC. 262 */ 263 static u64 sja1105_tstamp_reconstruct(struct dsa_switch *ds, u64 now, 264 u64 ts_partial) 265 { 266 struct sja1105_private *priv = ds->priv; 267 u64 partial_tstamp_mask = CYCLECOUNTER_MASK(priv->info->ptp_ts_bits); 268 u64 ts_reconstructed; 269 270 ts_reconstructed = (now & ~partial_tstamp_mask) | ts_partial; 271 272 /* Check lower bits of current cycle counter against the timestamp. 273 * If the current cycle counter is lower than the partial timestamp, 274 * then wraparound surely occurred and must be accounted for. 275 */ 276 if ((now & partial_tstamp_mask) <= ts_partial) 277 ts_reconstructed -= (partial_tstamp_mask + 1); 278 279 return ts_reconstructed; 280 } 281 282 /* Reads the SPI interface for an egress timestamp generated by the switch 283 * for frames sent using management routes. 284 * 285 * SJA1105 E/T layout of the 4-byte SPI payload: 286 * 287 * 31 23 15 7 0 288 * | | | | | 289 * +-----+-----+-----+ ^ 290 * ^ | 291 * | | 292 * 24-bit timestamp Update bit 293 * 294 * 295 * SJA1105 P/Q/R/S layout of the 8-byte SPI payload: 296 * 297 * 31 23 15 7 0 63 55 47 39 32 298 * | | | | | | | | | | 299 * ^ +-----+-----+-----+-----+ 300 * | ^ 301 * | | 302 * Update bit 32-bit timestamp 303 * 304 * Notice that the update bit is in the same place. 305 * To have common code for E/T and P/Q/R/S for reading the timestamp, 306 * we need to juggle with the offset and the bit indices. 307 */ 308 static int sja1105_ptpegr_ts_poll(struct dsa_switch *ds, int port, u64 *ts) 309 { 310 struct sja1105_private *priv = ds->priv; 311 const struct sja1105_regs *regs = priv->info->regs; 312 int tstamp_bit_start, tstamp_bit_end; 313 int timeout = 10; 314 u8 packed_buf[8]; 315 u64 update; 316 int rc; 317 318 do { 319 rc = sja1105_xfer_buf(priv, SPI_READ, regs->ptpegr_ts[port], 320 packed_buf, priv->info->ptpegr_ts_bytes); 321 if (rc < 0) 322 return rc; 323 324 sja1105_unpack(packed_buf, &update, 0, 0, 325 priv->info->ptpegr_ts_bytes); 326 if (update) 327 break; 328 329 usleep_range(10, 50); 330 } while (--timeout); 331 332 if (!timeout) 333 return -ETIMEDOUT; 334 335 /* Point the end bit to the second 32-bit word on P/Q/R/S, 336 * no-op on E/T. 337 */ 338 tstamp_bit_end = (priv->info->ptpegr_ts_bytes - 4) * 8; 339 /* Shift the 24-bit timestamp on E/T to be collected from 31:8. 340 * No-op on P/Q/R/S. 341 */ 342 tstamp_bit_end += 32 - priv->info->ptp_ts_bits; 343 tstamp_bit_start = tstamp_bit_end + priv->info->ptp_ts_bits - 1; 344 345 *ts = 0; 346 347 sja1105_unpack(packed_buf, ts, tstamp_bit_start, tstamp_bit_end, 348 priv->info->ptpegr_ts_bytes); 349 350 return 0; 351 } 352 353 /* Caller must hold ptp_data->lock */ 354 static int sja1105_ptpclkval_read(struct sja1105_private *priv, u64 *ticks, 355 struct ptp_system_timestamp *ptp_sts) 356 { 357 const struct sja1105_regs *regs = priv->info->regs; 358 359 return sja1105_xfer_u64(priv, SPI_READ, regs->ptpclkval, ticks, 360 ptp_sts); 361 } 362 363 /* Caller must hold ptp_data->lock */ 364 static int sja1105_ptpclkval_write(struct sja1105_private *priv, u64 ticks, 365 struct ptp_system_timestamp *ptp_sts) 366 { 367 const struct sja1105_regs *regs = priv->info->regs; 368 369 return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpclkval, &ticks, 370 ptp_sts); 371 } 372 373 static long sja1105_rxtstamp_work(struct ptp_clock_info *ptp) 374 { 375 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp); 376 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data); 377 struct dsa_switch *ds = priv->ds; 378 struct sk_buff *skb; 379 380 mutex_lock(&ptp_data->lock); 381 382 while ((skb = skb_dequeue(&ptp_data->skb_rxtstamp_queue)) != NULL) { 383 struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb); 384 u64 ticks, ts; 385 int rc; 386 387 rc = sja1105_ptpclkval_read(priv, &ticks, NULL); 388 if (rc < 0) { 389 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc); 390 kfree_skb(skb); 391 continue; 392 } 393 394 *shwt = (struct skb_shared_hwtstamps) {0}; 395 396 ts = SJA1105_SKB_CB(skb)->meta_tstamp; 397 ts = sja1105_tstamp_reconstruct(ds, ticks, ts); 398 399 shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts)); 400 netif_rx_ni(skb); 401 } 402 403 mutex_unlock(&ptp_data->lock); 404 405 /* Don't restart */ 406 return -1; 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_ptp_data *ptp_data = &priv->ptp_data; 415 416 if (!test_bit(SJA1105_HWTS_RX_EN, &priv->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(&ptp_data->skb_rxtstamp_queue, skb); 423 ptp_schedule_worker(ptp_data->clock, 0); 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 .do_aux_work = sja1105_rxtstamp_work, 632 .max_adj = SJA1105_MAX_ADJ_PPB, 633 }; 634 635 skb_queue_head_init(&ptp_data->skb_rxtstamp_queue); 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 ptp_cancel_worker_sync(ptp_data->clock); 657 skb_queue_purge(&ptp_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 port, 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, port, &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