1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Copyright (C) 2022 Meta Platforms Inc. 4 * Copyright (C) 2022 Jonathan Lemon <jonathan.lemon@gmail.com> 5 */ 6 7 #include <asm/unaligned.h> 8 #include <linux/mii.h> 9 #include <linux/phy.h> 10 #include <linux/ptp_classify.h> 11 #include <linux/ptp_clock_kernel.h> 12 #include <linux/net_tstamp.h> 13 #include <linux/netdevice.h> 14 #include <linux/workqueue.h> 15 16 #include "bcm-phy-lib.h" 17 18 /* IEEE 1588 Expansion registers */ 19 #define SLICE_CTRL 0x0810 20 #define SLICE_TX_EN BIT(0) 21 #define SLICE_RX_EN BIT(8) 22 #define TX_EVENT_MODE 0x0811 23 #define MODE_TX_UPDATE_CF BIT(0) 24 #define MODE_TX_REPLACE_TS_CF BIT(1) 25 #define MODE_TX_REPLACE_TS GENMASK(1, 0) 26 #define RX_EVENT_MODE 0x0819 27 #define MODE_RX_UPDATE_CF BIT(0) 28 #define MODE_RX_INSERT_TS_48 BIT(1) 29 #define MODE_RX_INSERT_TS_64 GENMASK(1, 0) 30 31 #define MODE_EVT_SHIFT_SYNC 0 32 #define MODE_EVT_SHIFT_DELAY_REQ 2 33 #define MODE_EVT_SHIFT_PDELAY_REQ 4 34 #define MODE_EVT_SHIFT_PDELAY_RESP 6 35 36 #define MODE_SEL_SHIFT_PORT 0 37 #define MODE_SEL_SHIFT_CPU 8 38 39 #define RX_MODE_SEL(sel, evt, act) \ 40 (((MODE_RX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel)) 41 42 #define TX_MODE_SEL(sel, evt, act) \ 43 (((MODE_TX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel)) 44 45 /* needs global TS capture first */ 46 #define TX_TS_CAPTURE 0x0821 47 #define TX_TS_CAP_EN BIT(0) 48 #define RX_TS_CAPTURE 0x0822 49 #define RX_TS_CAP_EN BIT(0) 50 51 #define TIME_CODE_0 0x0854 52 #define TIME_CODE_1 0x0855 53 #define TIME_CODE_2 0x0856 54 #define TIME_CODE_3 0x0857 55 #define TIME_CODE_4 0x0858 56 57 #define DPLL_SELECT 0x085b 58 #define DPLL_HB_MODE2 BIT(6) 59 60 #define SHADOW_CTRL 0x085c 61 #define SHADOW_LOAD 0x085d 62 #define TIME_CODE_LOAD BIT(10) 63 #define SYNC_OUT_LOAD BIT(9) 64 #define NCO_TIME_LOAD BIT(7) 65 #define FREQ_LOAD BIT(6) 66 #define INTR_MASK 0x085e 67 #define INTR_STATUS 0x085f 68 #define INTC_FSYNC BIT(0) 69 #define INTC_SOP BIT(1) 70 71 #define NCO_FREQ_LSB 0x0873 72 #define NCO_FREQ_MSB 0x0874 73 74 #define NCO_TIME_0 0x0875 75 #define NCO_TIME_1 0x0876 76 #define NCO_TIME_2_CTRL 0x0877 77 #define FREQ_MDIO_SEL BIT(14) 78 79 #define SYNC_OUT_0 0x0878 80 #define SYNC_OUT_1 0x0879 81 #define SYNC_OUT_2 0x087a 82 83 #define SYNOUT_TS_0 0x087c 84 #define SYNOUT_TS_1 0x087d 85 #define SYNOUT_TS_2 0x087e 86 87 #define NSE_CTRL 0x087f 88 #define NSE_GMODE_EN GENMASK(15, 14) 89 #define NSE_CAPTURE_EN BIT(13) 90 #define NSE_INIT BIT(12) 91 #define NSE_CPU_FRAMESYNC BIT(5) 92 #define NSE_FRAMESYNC_MASK GENMASK(5, 2) 93 #define NSE_PEROUT_EN BIT(1) 94 #define NSE_ONESHOT_EN BIT(0) 95 #define NSE_SYNC_OUT_MASK GENMASK(1, 0) 96 97 #define TS_READ_CTRL 0x0885 98 #define TS_READ_START BIT(0) 99 #define TS_READ_END BIT(1) 100 101 #define HB_REG_0 0x0886 102 #define HB_REG_1 0x0887 103 #define HB_REG_2 0x0888 104 #define HB_REG_3 0x08ec 105 #define HB_REG_4 0x08ed 106 #define HB_STAT_CTRL 0x088e 107 #define HB_READ_START BIT(10) 108 #define HB_READ_END BIT(11) 109 #define HB_READ_MASK GENMASK(11, 10) 110 111 #define TS_REG_0 0x0889 112 #define TS_REG_1 0x088a 113 #define TS_REG_2 0x088b 114 #define TS_REG_3 0x08c4 115 116 #define TS_INFO_0 0x088c 117 #define TS_INFO_1 0x088d 118 119 #define TIMECODE_CTRL 0x08c3 120 #define TX_TIMECODE_SEL GENMASK(7, 0) 121 #define RX_TIMECODE_SEL GENMASK(15, 8) 122 123 #define TIME_SYNC 0x0ff5 124 #define TIME_SYNC_EN BIT(0) 125 126 struct bcm_ptp_private { 127 struct phy_device *phydev; 128 struct mii_timestamper mii_ts; 129 struct ptp_clock *ptp_clock; 130 struct ptp_clock_info ptp_info; 131 struct mutex mutex; 132 struct sk_buff_head tx_queue; 133 int tx_type; 134 bool hwts_rx; 135 u16 nse_ctrl; 136 }; 137 138 struct bcm_ptp_skb_cb { 139 unsigned long timeout; 140 u16 seq_id; 141 u8 msgtype; 142 bool discard; 143 }; 144 145 struct bcm_ptp_capture { 146 ktime_t hwtstamp; 147 u16 seq_id; 148 u8 msgtype; 149 bool tx_dir; 150 }; 151 152 #define BCM_SKB_CB(skb) ((struct bcm_ptp_skb_cb *)(skb)->cb) 153 #define SKB_TS_TIMEOUT 10 /* jiffies */ 154 155 #define BCM_MAX_PULSE_8NS ((1U << 9) - 1) 156 #define BCM_MAX_PERIOD_8NS ((1U << 30) - 1) 157 158 #define BRCM_PHY_MODEL(phydev) \ 159 ((phydev)->drv->phy_id & (phydev)->drv->phy_id_mask) 160 161 static struct bcm_ptp_private *mii2priv(struct mii_timestamper *mii_ts) 162 { 163 return container_of(mii_ts, struct bcm_ptp_private, mii_ts); 164 } 165 166 static struct bcm_ptp_private *ptp2priv(struct ptp_clock_info *info) 167 { 168 return container_of(info, struct bcm_ptp_private, ptp_info); 169 } 170 171 static void bcm_ptp_get_framesync_ts(struct phy_device *phydev, 172 struct timespec64 *ts) 173 { 174 u16 hb[4]; 175 176 bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_START); 177 178 hb[0] = bcm_phy_read_exp(phydev, HB_REG_0); 179 hb[1] = bcm_phy_read_exp(phydev, HB_REG_1); 180 hb[2] = bcm_phy_read_exp(phydev, HB_REG_2); 181 hb[3] = bcm_phy_read_exp(phydev, HB_REG_3); 182 183 bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_END); 184 bcm_phy_write_exp(phydev, HB_STAT_CTRL, 0); 185 186 ts->tv_sec = (hb[3] << 16) | hb[2]; 187 ts->tv_nsec = (hb[1] << 16) | hb[0]; 188 } 189 190 static u16 bcm_ptp_framesync_disable(struct phy_device *phydev, u16 orig_ctrl) 191 { 192 u16 ctrl = orig_ctrl & ~(NSE_FRAMESYNC_MASK | NSE_CAPTURE_EN); 193 194 bcm_phy_write_exp(phydev, NSE_CTRL, ctrl); 195 196 return ctrl; 197 } 198 199 static void bcm_ptp_framesync_restore(struct phy_device *phydev, u16 orig_ctrl) 200 { 201 if (orig_ctrl & NSE_FRAMESYNC_MASK) 202 bcm_phy_write_exp(phydev, NSE_CTRL, orig_ctrl); 203 } 204 205 static void bcm_ptp_framesync(struct phy_device *phydev, u16 ctrl) 206 { 207 /* trigger framesync - must have 0->1 transition. */ 208 bcm_phy_write_exp(phydev, NSE_CTRL, ctrl | NSE_CPU_FRAMESYNC); 209 } 210 211 static int bcm_ptp_framesync_ts(struct phy_device *phydev, 212 struct ptp_system_timestamp *sts, 213 struct timespec64 *ts, 214 u16 orig_ctrl) 215 { 216 u16 ctrl, reg; 217 int i; 218 219 ctrl = bcm_ptp_framesync_disable(phydev, orig_ctrl); 220 221 ptp_read_system_prets(sts); 222 223 /* trigger framesync + capture */ 224 bcm_ptp_framesync(phydev, ctrl | NSE_CAPTURE_EN); 225 226 ptp_read_system_postts(sts); 227 228 /* poll for FSYNC interrupt from TS capture */ 229 for (i = 0; i < 10; i++) { 230 reg = bcm_phy_read_exp(phydev, INTR_STATUS); 231 if (reg & INTC_FSYNC) { 232 bcm_ptp_get_framesync_ts(phydev, ts); 233 break; 234 } 235 } 236 237 bcm_ptp_framesync_restore(phydev, orig_ctrl); 238 239 return reg & INTC_FSYNC ? 0 : -ETIMEDOUT; 240 } 241 242 static int bcm_ptp_gettimex(struct ptp_clock_info *info, 243 struct timespec64 *ts, 244 struct ptp_system_timestamp *sts) 245 { 246 struct bcm_ptp_private *priv = ptp2priv(info); 247 int err; 248 249 mutex_lock(&priv->mutex); 250 err = bcm_ptp_framesync_ts(priv->phydev, sts, ts, priv->nse_ctrl); 251 mutex_unlock(&priv->mutex); 252 253 return err; 254 } 255 256 static int bcm_ptp_settime_locked(struct bcm_ptp_private *priv, 257 const struct timespec64 *ts) 258 { 259 struct phy_device *phydev = priv->phydev; 260 u16 ctrl; 261 u64 ns; 262 263 ctrl = bcm_ptp_framesync_disable(phydev, priv->nse_ctrl); 264 265 /* set up time code */ 266 bcm_phy_write_exp(phydev, TIME_CODE_0, ts->tv_nsec); 267 bcm_phy_write_exp(phydev, TIME_CODE_1, ts->tv_nsec >> 16); 268 bcm_phy_write_exp(phydev, TIME_CODE_2, ts->tv_sec); 269 bcm_phy_write_exp(phydev, TIME_CODE_3, ts->tv_sec >> 16); 270 bcm_phy_write_exp(phydev, TIME_CODE_4, ts->tv_sec >> 32); 271 272 /* set NCO counter to match */ 273 ns = timespec64_to_ns(ts); 274 bcm_phy_write_exp(phydev, NCO_TIME_0, ns >> 4); 275 bcm_phy_write_exp(phydev, NCO_TIME_1, ns >> 20); 276 bcm_phy_write_exp(phydev, NCO_TIME_2_CTRL, (ns >> 36) & 0xfff); 277 278 /* set up load on next frame sync (auto-clears due to NSE_INIT) */ 279 bcm_phy_write_exp(phydev, SHADOW_LOAD, TIME_CODE_LOAD | NCO_TIME_LOAD); 280 281 /* must have NSE_INIT in order to write time code */ 282 bcm_ptp_framesync(phydev, ctrl | NSE_INIT); 283 284 bcm_ptp_framesync_restore(phydev, priv->nse_ctrl); 285 286 return 0; 287 } 288 289 static int bcm_ptp_settime(struct ptp_clock_info *info, 290 const struct timespec64 *ts) 291 { 292 struct bcm_ptp_private *priv = ptp2priv(info); 293 int err; 294 295 mutex_lock(&priv->mutex); 296 err = bcm_ptp_settime_locked(priv, ts); 297 mutex_unlock(&priv->mutex); 298 299 return err; 300 } 301 302 static int bcm_ptp_adjtime_locked(struct bcm_ptp_private *priv, 303 s64 delta_ns) 304 { 305 struct timespec64 ts; 306 int err; 307 s64 ns; 308 309 err = bcm_ptp_framesync_ts(priv->phydev, NULL, &ts, priv->nse_ctrl); 310 if (!err) { 311 ns = timespec64_to_ns(&ts) + delta_ns; 312 ts = ns_to_timespec64(ns); 313 err = bcm_ptp_settime_locked(priv, &ts); 314 } 315 return err; 316 } 317 318 static int bcm_ptp_adjtime(struct ptp_clock_info *info, s64 delta_ns) 319 { 320 struct bcm_ptp_private *priv = ptp2priv(info); 321 int err; 322 323 mutex_lock(&priv->mutex); 324 err = bcm_ptp_adjtime_locked(priv, delta_ns); 325 mutex_unlock(&priv->mutex); 326 327 return err; 328 } 329 330 /* A 125Mhz clock should adjust 8ns per pulse. 331 * The frequency adjustment base is 0x8000 0000, or 8*2^28. 332 * 333 * Frequency adjustment is 334 * adj = scaled_ppm * 8*2^28 / (10^6 * 2^16) 335 * which simplifies to: 336 * adj = scaled_ppm * 2^9 / 5^6 337 */ 338 static int bcm_ptp_adjfine(struct ptp_clock_info *info, long scaled_ppm) 339 { 340 struct bcm_ptp_private *priv = ptp2priv(info); 341 int neg_adj = 0; 342 u32 diff, freq; 343 u16 ctrl; 344 u64 adj; 345 346 if (scaled_ppm < 0) { 347 neg_adj = 1; 348 scaled_ppm = -scaled_ppm; 349 } 350 351 adj = scaled_ppm << 9; 352 diff = div_u64(adj, 15625); 353 freq = (8 << 28) + (neg_adj ? -diff : diff); 354 355 mutex_lock(&priv->mutex); 356 357 ctrl = bcm_ptp_framesync_disable(priv->phydev, priv->nse_ctrl); 358 359 bcm_phy_write_exp(priv->phydev, NCO_FREQ_LSB, freq); 360 bcm_phy_write_exp(priv->phydev, NCO_FREQ_MSB, freq >> 16); 361 362 bcm_phy_write_exp(priv->phydev, NCO_TIME_2_CTRL, FREQ_MDIO_SEL); 363 364 /* load on next framesync */ 365 bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, FREQ_LOAD); 366 367 bcm_ptp_framesync(priv->phydev, ctrl); 368 369 /* clear load */ 370 bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, 0); 371 372 bcm_ptp_framesync_restore(priv->phydev, priv->nse_ctrl); 373 374 mutex_unlock(&priv->mutex); 375 376 return 0; 377 } 378 379 static bool bcm_ptp_rxtstamp(struct mii_timestamper *mii_ts, 380 struct sk_buff *skb, int type) 381 { 382 struct bcm_ptp_private *priv = mii2priv(mii_ts); 383 struct skb_shared_hwtstamps *hwts; 384 struct ptp_header *header; 385 u32 sec, nsec; 386 u8 *data; 387 int off; 388 389 if (!priv->hwts_rx) 390 return false; 391 392 header = ptp_parse_header(skb, type); 393 if (!header) 394 return false; 395 396 data = (u8 *)(header + 1); 397 sec = get_unaligned_be32(data); 398 nsec = get_unaligned_be32(data + 4); 399 400 hwts = skb_hwtstamps(skb); 401 hwts->hwtstamp = ktime_set(sec, nsec); 402 403 off = data - skb->data + 8; 404 if (off < skb->len) { 405 memmove(data, data + 8, skb->len - off); 406 __pskb_trim(skb, skb->len - 8); 407 } 408 409 return false; 410 } 411 412 static bool bcm_ptp_get_tstamp(struct bcm_ptp_private *priv, 413 struct bcm_ptp_capture *capts) 414 { 415 struct phy_device *phydev = priv->phydev; 416 u16 ts[4], reg; 417 u32 sec, nsec; 418 419 mutex_lock(&priv->mutex); 420 421 reg = bcm_phy_read_exp(phydev, INTR_STATUS); 422 if ((reg & INTC_SOP) == 0) { 423 mutex_unlock(&priv->mutex); 424 return false; 425 } 426 427 bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_START); 428 429 ts[0] = bcm_phy_read_exp(phydev, TS_REG_0); 430 ts[1] = bcm_phy_read_exp(phydev, TS_REG_1); 431 ts[2] = bcm_phy_read_exp(phydev, TS_REG_2); 432 ts[3] = bcm_phy_read_exp(phydev, TS_REG_3); 433 434 /* not in be32 format for some reason */ 435 capts->seq_id = bcm_phy_read_exp(priv->phydev, TS_INFO_0); 436 437 reg = bcm_phy_read_exp(phydev, TS_INFO_1); 438 capts->msgtype = reg >> 12; 439 capts->tx_dir = !!(reg & BIT(11)); 440 441 bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_END); 442 bcm_phy_write_exp(phydev, TS_READ_CTRL, 0); 443 444 mutex_unlock(&priv->mutex); 445 446 sec = (ts[3] << 16) | ts[2]; 447 nsec = (ts[1] << 16) | ts[0]; 448 capts->hwtstamp = ktime_set(sec, nsec); 449 450 return true; 451 } 452 453 static void bcm_ptp_match_tstamp(struct bcm_ptp_private *priv, 454 struct bcm_ptp_capture *capts) 455 { 456 struct skb_shared_hwtstamps hwts; 457 struct sk_buff *skb, *ts_skb; 458 unsigned long flags; 459 bool first = false; 460 461 ts_skb = NULL; 462 spin_lock_irqsave(&priv->tx_queue.lock, flags); 463 skb_queue_walk(&priv->tx_queue, skb) { 464 if (BCM_SKB_CB(skb)->seq_id == capts->seq_id && 465 BCM_SKB_CB(skb)->msgtype == capts->msgtype) { 466 first = skb_queue_is_first(&priv->tx_queue, skb); 467 __skb_unlink(skb, &priv->tx_queue); 468 ts_skb = skb; 469 break; 470 } 471 } 472 spin_unlock_irqrestore(&priv->tx_queue.lock, flags); 473 474 /* TX captures one-step packets, discard them if needed. */ 475 if (ts_skb) { 476 if (BCM_SKB_CB(ts_skb)->discard) { 477 kfree_skb(ts_skb); 478 } else { 479 memset(&hwts, 0, sizeof(hwts)); 480 hwts.hwtstamp = capts->hwtstamp; 481 skb_complete_tx_timestamp(ts_skb, &hwts); 482 } 483 } 484 485 /* not first match, try and expire entries */ 486 if (!first) { 487 while ((skb = skb_dequeue(&priv->tx_queue))) { 488 if (!time_after(jiffies, BCM_SKB_CB(skb)->timeout)) { 489 skb_queue_head(&priv->tx_queue, skb); 490 break; 491 } 492 kfree_skb(skb); 493 } 494 } 495 } 496 497 static long bcm_ptp_do_aux_work(struct ptp_clock_info *info) 498 { 499 struct bcm_ptp_private *priv = ptp2priv(info); 500 struct bcm_ptp_capture capts; 501 bool reschedule = false; 502 503 while (!skb_queue_empty_lockless(&priv->tx_queue)) { 504 if (!bcm_ptp_get_tstamp(priv, &capts)) { 505 reschedule = true; 506 break; 507 } 508 bcm_ptp_match_tstamp(priv, &capts); 509 } 510 511 return reschedule ? 1 : -1; 512 } 513 514 static const struct ptp_clock_info bcm_ptp_clock_info = { 515 .owner = THIS_MODULE, 516 .name = KBUILD_MODNAME, 517 .max_adj = 100000000, 518 .gettimex64 = bcm_ptp_gettimex, 519 .settime64 = bcm_ptp_settime, 520 .adjtime = bcm_ptp_adjtime, 521 .adjfine = bcm_ptp_adjfine, 522 .do_aux_work = bcm_ptp_do_aux_work, 523 }; 524 525 static void bcm_ptp_txtstamp(struct mii_timestamper *mii_ts, 526 struct sk_buff *skb, int type) 527 { 528 struct bcm_ptp_private *priv = mii2priv(mii_ts); 529 struct ptp_header *hdr; 530 bool discard = false; 531 int msgtype; 532 533 hdr = ptp_parse_header(skb, type); 534 if (!hdr) 535 goto out; 536 msgtype = ptp_get_msgtype(hdr, type); 537 538 switch (priv->tx_type) { 539 case HWTSTAMP_TX_ONESTEP_P2P: 540 if (msgtype == PTP_MSGTYPE_PDELAY_RESP) 541 discard = true; 542 fallthrough; 543 case HWTSTAMP_TX_ONESTEP_SYNC: 544 if (msgtype == PTP_MSGTYPE_SYNC) 545 discard = true; 546 fallthrough; 547 case HWTSTAMP_TX_ON: 548 BCM_SKB_CB(skb)->timeout = jiffies + SKB_TS_TIMEOUT; 549 BCM_SKB_CB(skb)->seq_id = be16_to_cpu(hdr->sequence_id); 550 BCM_SKB_CB(skb)->msgtype = msgtype; 551 BCM_SKB_CB(skb)->discard = discard; 552 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; 553 skb_queue_tail(&priv->tx_queue, skb); 554 ptp_schedule_worker(priv->ptp_clock, 0); 555 return; 556 default: 557 break; 558 } 559 560 out: 561 kfree_skb(skb); 562 } 563 564 static int bcm_ptp_hwtstamp(struct mii_timestamper *mii_ts, 565 struct ifreq *ifr) 566 { 567 struct bcm_ptp_private *priv = mii2priv(mii_ts); 568 struct hwtstamp_config cfg; 569 u16 mode, ctrl; 570 571 if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg))) 572 return -EFAULT; 573 574 switch (cfg.rx_filter) { 575 case HWTSTAMP_FILTER_NONE: 576 priv->hwts_rx = false; 577 break; 578 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 579 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 580 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 581 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 582 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 583 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 584 case HWTSTAMP_FILTER_PTP_V2_EVENT: 585 case HWTSTAMP_FILTER_PTP_V2_SYNC: 586 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 587 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 588 priv->hwts_rx = true; 589 break; 590 default: 591 return -ERANGE; 592 } 593 594 priv->tx_type = cfg.tx_type; 595 596 ctrl = priv->hwts_rx ? SLICE_RX_EN : 0; 597 ctrl |= priv->tx_type != HWTSTAMP_TX_OFF ? SLICE_TX_EN : 0; 598 599 mode = TX_MODE_SEL(PORT, SYNC, REPLACE_TS) | 600 TX_MODE_SEL(PORT, DELAY_REQ, REPLACE_TS) | 601 TX_MODE_SEL(PORT, PDELAY_REQ, REPLACE_TS) | 602 TX_MODE_SEL(PORT, PDELAY_RESP, REPLACE_TS); 603 604 bcm_phy_write_exp(priv->phydev, TX_EVENT_MODE, mode); 605 606 mode = RX_MODE_SEL(PORT, SYNC, INSERT_TS_64) | 607 RX_MODE_SEL(PORT, DELAY_REQ, INSERT_TS_64) | 608 RX_MODE_SEL(PORT, PDELAY_REQ, INSERT_TS_64) | 609 RX_MODE_SEL(PORT, PDELAY_RESP, INSERT_TS_64); 610 611 bcm_phy_write_exp(priv->phydev, RX_EVENT_MODE, mode); 612 613 bcm_phy_write_exp(priv->phydev, SLICE_CTRL, ctrl); 614 615 if (ctrl & SLICE_TX_EN) 616 bcm_phy_write_exp(priv->phydev, TX_TS_CAPTURE, TX_TS_CAP_EN); 617 else 618 ptp_cancel_worker_sync(priv->ptp_clock); 619 620 /* purge existing data */ 621 skb_queue_purge(&priv->tx_queue); 622 623 return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0; 624 } 625 626 static int bcm_ptp_ts_info(struct mii_timestamper *mii_ts, 627 struct ethtool_ts_info *ts_info) 628 { 629 struct bcm_ptp_private *priv = mii2priv(mii_ts); 630 631 ts_info->phc_index = ptp_clock_index(priv->ptp_clock); 632 ts_info->so_timestamping = 633 SOF_TIMESTAMPING_TX_HARDWARE | 634 SOF_TIMESTAMPING_RX_HARDWARE | 635 SOF_TIMESTAMPING_RAW_HARDWARE; 636 ts_info->tx_types = 637 BIT(HWTSTAMP_TX_ON) | 638 BIT(HWTSTAMP_TX_OFF) | 639 BIT(HWTSTAMP_TX_ONESTEP_SYNC) | 640 BIT(HWTSTAMP_TX_ONESTEP_P2P); 641 ts_info->rx_filters = 642 BIT(HWTSTAMP_FILTER_NONE) | 643 BIT(HWTSTAMP_FILTER_PTP_V2_EVENT); 644 645 return 0; 646 } 647 648 void bcm_ptp_stop(struct bcm_ptp_private *priv) 649 { 650 ptp_cancel_worker_sync(priv->ptp_clock); 651 } 652 EXPORT_SYMBOL_GPL(bcm_ptp_stop); 653 654 void bcm_ptp_config_init(struct phy_device *phydev) 655 { 656 /* init network sync engine */ 657 bcm_phy_write_exp(phydev, NSE_CTRL, NSE_GMODE_EN | NSE_INIT); 658 659 /* enable time sync (TX/RX SOP capture) */ 660 bcm_phy_write_exp(phydev, TIME_SYNC, TIME_SYNC_EN); 661 662 /* use sec.nsec heartbeat capture */ 663 bcm_phy_write_exp(phydev, DPLL_SELECT, DPLL_HB_MODE2); 664 665 /* use 64 bit timecode for TX */ 666 bcm_phy_write_exp(phydev, TIMECODE_CTRL, TX_TIMECODE_SEL); 667 668 /* always allow FREQ_LOAD on framesync */ 669 bcm_phy_write_exp(phydev, SHADOW_CTRL, FREQ_LOAD); 670 } 671 EXPORT_SYMBOL_GPL(bcm_ptp_config_init); 672 673 static void bcm_ptp_init(struct bcm_ptp_private *priv) 674 { 675 priv->nse_ctrl = NSE_GMODE_EN; 676 677 mutex_init(&priv->mutex); 678 skb_queue_head_init(&priv->tx_queue); 679 680 priv->mii_ts.rxtstamp = bcm_ptp_rxtstamp; 681 priv->mii_ts.txtstamp = bcm_ptp_txtstamp; 682 priv->mii_ts.hwtstamp = bcm_ptp_hwtstamp; 683 priv->mii_ts.ts_info = bcm_ptp_ts_info; 684 685 priv->phydev->mii_ts = &priv->mii_ts; 686 } 687 688 struct bcm_ptp_private *bcm_ptp_probe(struct phy_device *phydev) 689 { 690 struct bcm_ptp_private *priv; 691 struct ptp_clock *clock; 692 693 switch (BRCM_PHY_MODEL(phydev)) { 694 case PHY_ID_BCM54210E: 695 break; 696 default: 697 return NULL; 698 } 699 700 priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL); 701 if (!priv) 702 return ERR_PTR(-ENOMEM); 703 704 priv->ptp_info = bcm_ptp_clock_info; 705 706 clock = ptp_clock_register(&priv->ptp_info, &phydev->mdio.dev); 707 if (IS_ERR(clock)) 708 return ERR_CAST(clock); 709 priv->ptp_clock = clock; 710 711 priv->phydev = phydev; 712 bcm_ptp_init(priv); 713 714 return priv; 715 } 716 EXPORT_SYMBOL_GPL(bcm_ptp_probe); 717 718 MODULE_LICENSE("GPL"); 719