FILE
ksz_ptp.c
File preview with syntax highlighting.
linux-6.19.14/drivers/net/dsa/microchip/ksz_ptp.c
1// SPDX-License-Identifier: GPL-2.0 2/* Microchip KSZ PTP Implementation 3 * 4 * Copyright (C) 2020 ARRI Lighting 5 * Copyright (C) 2022 Microchip Technology Inc. 6 */ 7 8#include <linux/dsa/ksz_common.h> 9#include <linux/irq.h> 10#include <linux/irqdomain.h> 11#include <linux/kernel.h> 12#include <linux/ptp_classify.h> 13#include <linux/ptp_clock_kernel.h> 14 15#include "ksz_common.h" 16#include "ksz_ptp.h" 17#include "ksz_ptp_reg.h" 18 19#define ptp_caps_to_data(d) container_of((d), struct ksz_ptp_data, caps) 20#define ptp_data_to_ksz_dev(d) container_of((d), struct ksz_device, ptp_data) 21#define work_to_xmit_work(w) \ 22 container_of((w), struct ksz_deferred_xmit_work, work) 23 24/* Sub-nanoseconds-adj,max * sub-nanoseconds / 40ns * 1ns 25 * = (2^30-1) * (2 ^ 32) / 40 ns * 1 ns = 6249999 26 */ 27#define KSZ_MAX_DRIFT_CORR 6249999 28#define KSZ_MAX_PULSE_WIDTH 125000000LL 29 30#define KSZ_PTP_INC_NS 40ULL /* HW clock is incremented every 40 ns (by 40) */ 31#define KSZ_PTP_SUBNS_BITS 32 32 33#define KSZ_PTP_INT_START 13 34 35static int ksz_ptp_tou_gpio(struct ksz_device *dev) 36{ 37 int ret; 38 39 if (!is_lan937x(dev)) 40 return 0; 41 42 ret = ksz_rmw32(dev, REG_PTP_CTRL_STAT__4, GPIO_OUT, 43 GPIO_OUT); 44 if (ret) 45 return ret; 46 47 ret = ksz_rmw32(dev, REG_SW_GLOBAL_LED_OVR__4, LED_OVR_1 | LED_OVR_2, 48 LED_OVR_1 | LED_OVR_2); 49 if (ret) 50 return ret; 51 52 return ksz_rmw32(dev, REG_SW_GLOBAL_LED_SRC__4, 53 LED_SRC_PTP_GPIO_1 | LED_SRC_PTP_GPIO_2, 54 LED_SRC_PTP_GPIO_1 | LED_SRC_PTP_GPIO_2); 55} 56 57static int ksz_ptp_tou_reset(struct ksz_device *dev, u8 unit) 58{ 59 u32 data; 60 int ret; 61 62 /* Reset trigger unit (clears TRIGGER_EN, but not GPIOSTATx) */ 63 ret = ksz_rmw32(dev, REG_PTP_CTRL_STAT__4, TRIG_RESET, TRIG_RESET); 64 65 data = FIELD_PREP(TRIG_DONE_M, BIT(unit)); 66 ret = ksz_write32(dev, REG_PTP_TRIG_STATUS__4, data); 67 if (ret) 68 return ret; 69 70 data = FIELD_PREP(TRIG_INT_M, BIT(unit)); 71 ret = ksz_write32(dev, REG_PTP_INT_STATUS__4, data); 72 if (ret) 73 return ret; 74 75 /* Clear reset and set GPIO direction */ 76 return ksz_rmw32(dev, REG_PTP_CTRL_STAT__4, (TRIG_RESET | TRIG_ENABLE), 77 0); 78} 79 80static int ksz_ptp_tou_pulse_verify(u64 pulse_ns) 81{ 82 u32 data; 83 84 if (pulse_ns & 0x3) 85 return -EINVAL; 86 87 data = (pulse_ns / 8); 88 if (!FIELD_FIT(TRIG_PULSE_WIDTH_M, data)) 89 return -ERANGE; 90 91 return 0; 92} 93 94static int ksz_ptp_tou_target_time_set(struct ksz_device *dev, 95 struct timespec64 const *ts) 96{ 97 int ret; 98 99 /* Hardware has only 32 bit */ 100 if ((ts->tv_sec & 0xffffffff) != ts->tv_sec) 101 return -EINVAL; 102 103 ret = ksz_write32(dev, REG_TRIG_TARGET_NANOSEC, ts->tv_nsec); 104 if (ret) 105 return ret; 106 107 ret = ksz_write32(dev, REG_TRIG_TARGET_SEC, ts->tv_sec); 108 if (ret) 109 return ret; 110 111 return 0; 112} 113 114static int ksz_ptp_tou_start(struct ksz_device *dev, u8 unit) 115{ 116 u32 data; 117 int ret; 118 119 ret = ksz_rmw32(dev, REG_PTP_CTRL_STAT__4, TRIG_ENABLE, TRIG_ENABLE); 120 if (ret) 121 return ret; 122 123 /* Check error flag: 124 * - the ACTIVE flag is NOT cleared an error! 125 */ 126 ret = ksz_read32(dev, REG_PTP_TRIG_STATUS__4, &data); 127 if (ret) 128 return ret; 129 130 if (FIELD_GET(TRIG_ERROR_M, data) & (1 << unit)) { 131 dev_err(dev->dev, "%s: Trigger unit%d error!\n", __func__, 132 unit); 133 ret = -EIO; 134 /* Unit will be reset on next access */ 135 return ret; 136 } 137 138 return 0; 139} 140 141static int ksz_ptp_configure_perout(struct ksz_device *dev, 142 u32 cycle_width_ns, u32 pulse_width_ns, 143 struct timespec64 const *target_time, 144 u8 index) 145{ 146 u32 data; 147 int ret; 148 149 data = FIELD_PREP(TRIG_NOTIFY, 1) | 150 FIELD_PREP(TRIG_GPO_M, index) | 151 FIELD_PREP(TRIG_PATTERN_M, TRIG_POS_PERIOD); 152 ret = ksz_write32(dev, REG_TRIG_CTRL__4, data); 153 if (ret) 154 return ret; 155 156 ret = ksz_write32(dev, REG_TRIG_CYCLE_WIDTH, cycle_width_ns); 157 if (ret) 158 return ret; 159 160 /* Set cycle count 0 - Infinite */ 161 ret = ksz_rmw32(dev, REG_TRIG_CYCLE_CNT, TRIG_CYCLE_CNT_M, 0); 162 if (ret) 163 return ret; 164 165 data = (pulse_width_ns / 8); 166 ret = ksz_write32(dev, REG_TRIG_PULSE_WIDTH__4, data); 167 if (ret) 168 return ret; 169 170 ret = ksz_ptp_tou_target_time_set(dev, target_time); 171 if (ret) 172 return ret; 173 174 return 0; 175} 176 177static int ksz_ptp_enable_perout(struct ksz_device *dev, 178 struct ptp_perout_request const *request, 179 int on) 180{ 181 struct ksz_ptp_data *ptp_data = &dev->ptp_data; 182 u64 req_pulse_width_ns; 183 u64 cycle_width_ns; 184 u64 pulse_width_ns; 185 int pin = 0; 186 u32 data32; 187 int ret; 188 189 if (request->flags & ~PTP_PEROUT_DUTY_CYCLE) 190 return -EOPNOTSUPP; 191 192 if (ptp_data->tou_mode != KSZ_PTP_TOU_PEROUT && 193 ptp_data->tou_mode != KSZ_PTP_TOU_IDLE) 194 return -EBUSY; 195 196 pin = ptp_find_pin(ptp_data->clock, PTP_PF_PEROUT, request->index); 197 if (pin < 0) 198 return -EINVAL; 199 200 data32 = FIELD_PREP(PTP_GPIO_INDEX, pin) | 201 FIELD_PREP(PTP_TOU_INDEX, request->index); 202 ret = ksz_rmw32(dev, REG_PTP_UNIT_INDEX__4, 203 PTP_GPIO_INDEX | PTP_TOU_INDEX, data32); 204 if (ret) 205 return ret; 206 207 ret = ksz_ptp_tou_reset(dev, request->index); 208 if (ret) 209 return ret; 210 211 if (!on) { 212 ptp_data->tou_mode = KSZ_PTP_TOU_IDLE; 213 return 0; 214 } 215 216 ptp_data->perout_target_time_first.tv_sec = request->start.sec; 217 ptp_data->perout_target_time_first.tv_nsec = request->start.nsec; 218 219 ptp_data->perout_period.tv_sec = request->period.sec; 220 ptp_data->perout_period.tv_nsec = request->period.nsec; 221 222 cycle_width_ns = timespec64_to_ns(&ptp_data->perout_period); 223 if ((cycle_width_ns & TRIG_CYCLE_WIDTH_M) != cycle_width_ns) 224 return -EINVAL; 225 226 if (request->flags & PTP_PEROUT_DUTY_CYCLE) { 227 pulse_width_ns = request->on.sec * NSEC_PER_SEC + 228 request->on.nsec; 229 } else { 230 /* Use a duty cycle of 50%. Maximum pulse width supported by the 231 * hardware is a little bit more than 125 ms. 232 */ 233 req_pulse_width_ns = (request->period.sec * NSEC_PER_SEC + 234 request->period.nsec) / 2; 235 pulse_width_ns = min_t(u64, req_pulse_width_ns, 236 KSZ_MAX_PULSE_WIDTH); 237 } 238 239 ret = ksz_ptp_tou_pulse_verify(pulse_width_ns); 240 if (ret) 241 return ret; 242 243 ret = ksz_ptp_configure_perout(dev, cycle_width_ns, pulse_width_ns, 244 &ptp_data->perout_target_time_first, 245 pin); 246 if (ret) 247 return ret; 248 249 ret = ksz_ptp_tou_gpio(dev); 250 if (ret) 251 return ret; 252 253 ret = ksz_ptp_tou_start(dev, request->index); 254 if (ret) 255 return ret; 256 257 ptp_data->tou_mode = KSZ_PTP_TOU_PEROUT; 258 259 return 0; 260} 261 262static int ksz_ptp_enable_mode(struct ksz_device *dev) 263{ 264 struct ksz_tagger_data *tagger_data = ksz_tagger_data(dev->ds); 265 struct ksz_ptp_data *ptp_data = &dev->ptp_data; 266 struct ksz_port *prt; 267 struct dsa_port *dp; 268 bool tag_en = false; 269 270 dsa_switch_for_each_user_port(dp, dev->ds) { 271 prt = &dev->ports[dp->index]; 272 if (prt->hwts_tx_en || prt->hwts_rx_en) { 273 tag_en = true; 274 break; 275 } 276 } 277 278 if (tag_en) { 279 ptp_schedule_worker(ptp_data->clock, 0); 280 } else { 281 ptp_cancel_worker_sync(ptp_data->clock); 282 } 283 284 tagger_data->hwtstamp_set_state(dev->ds, tag_en); 285 286 return ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_ENABLE, 287 tag_en ? PTP_ENABLE : 0); 288} 289 290/* The function is return back the capability of timestamping feature when 291 * requested through ethtool -T <interface> utility 292 */ 293int ksz_get_ts_info(struct dsa_switch *ds, int port, struct kernel_ethtool_ts_info *ts) 294{ 295 struct ksz_device *dev = ds->priv; 296 struct ksz_ptp_data *ptp_data; 297 298 ptp_data = &dev->ptp_data; 299 300 if (!ptp_data->clock) 301 return -ENODEV; 302 303 ts->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE | 304 SOF_TIMESTAMPING_RX_HARDWARE | 305 SOF_TIMESTAMPING_RAW_HARDWARE; 306 307 ts->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ONESTEP_P2P); 308 309 if (is_lan937x(dev)) 310 ts->tx_types |= BIT(HWTSTAMP_TX_ON); 311 312 ts->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | 313 BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT) | 314 BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 315 BIT(HWTSTAMP_FILTER_PTP_V2_EVENT); 316 317 ts->phc_index = ptp_clock_index(ptp_data->clock); 318 319 return 0; 320} 321 322int ksz_hwtstamp_get(struct dsa_switch *ds, int port, 323 struct kernel_hwtstamp_config *config) 324{ 325 struct ksz_device *dev = ds->priv; 326 struct ksz_port *prt; 327 328 prt = &dev->ports[port]; 329 *config = prt->tstamp_config; 330 331 return 0; 332} 333 334static int ksz_set_hwtstamp_config(struct ksz_device *dev, 335 struct ksz_port *prt, 336 struct kernel_hwtstamp_config *config) 337{ 338 int ret; 339 340 if (config->flags) 341 return -EINVAL; 342 343 switch (config->tx_type) { 344 case HWTSTAMP_TX_OFF: 345 prt->ptpmsg_irq[KSZ_SYNC_MSG].ts_en = false; 346 prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = false; 347 prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = false; 348 prt->hwts_tx_en = false; 349 break; 350 case HWTSTAMP_TX_ONESTEP_P2P: 351 prt->ptpmsg_irq[KSZ_SYNC_MSG].ts_en = false; 352 prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = true; 353 prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = false; 354 prt->hwts_tx_en = true; 355 356 ret = ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_1STEP, PTP_1STEP); 357 if (ret) 358 return ret; 359 360 break; 361 case HWTSTAMP_TX_ON: 362 if (!is_lan937x(dev)) 363 return -ERANGE; 364 365 prt->ptpmsg_irq[KSZ_SYNC_MSG].ts_en = true; 366 prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = true; 367 prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = true; 368 prt->hwts_tx_en = true; 369 370 ret = ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_1STEP, 0); 371 if (ret) 372 return ret; 373 374 break; 375 default: 376 return -ERANGE; 377 } 378 379 switch (config->rx_filter) { 380 case HWTSTAMP_FILTER_NONE: 381 prt->hwts_rx_en = false; 382 break; 383 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 384 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 385 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT; 386 prt->hwts_rx_en = true; 387 break; 388 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 389 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 390 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; 391 prt->hwts_rx_en = true; 392 break; 393 case HWTSTAMP_FILTER_PTP_V2_EVENT: 394 case HWTSTAMP_FILTER_PTP_V2_SYNC: 395 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 396 prt->hwts_rx_en = true; 397 break; 398 default: 399 config->rx_filter = HWTSTAMP_FILTER_NONE; 400 return -ERANGE; 401 } 402 403 return ksz_ptp_enable_mode(dev); 404} 405 406int ksz_hwtstamp_set(struct dsa_switch *ds, int port, 407 struct kernel_hwtstamp_config *config, 408 struct netlink_ext_ack *extack) 409{ 410 struct ksz_device *dev = ds->priv; 411 struct ksz_port *prt; 412 int ret; 413 414 prt = &dev->ports[port]; 415 416 ret = ksz_set_hwtstamp_config(dev, prt, config); 417 if (ret) 418 return ret; 419 420 prt->tstamp_config = *config; 421 422 return 0; 423} 424 425static ktime_t ksz_tstamp_reconstruct(struct ksz_device *dev, ktime_t tstamp) 426{ 427 struct timespec64 ptp_clock_time; 428 struct ksz_ptp_data *ptp_data; 429 struct timespec64 diff; 430 struct timespec64 ts; 431 432 ptp_data = &dev->ptp_data; 433 ts = ktime_to_timespec64(tstamp); 434 435 spin_lock_bh(&ptp_data->clock_lock); 436 ptp_clock_time = ptp_data->clock_time; 437 spin_unlock_bh(&ptp_data->clock_lock); 438 439 /* calculate full time from partial time stamp */ 440 ts.tv_sec = (ptp_clock_time.tv_sec & ~3) | ts.tv_sec; 441 442 /* find nearest possible point in time */ 443 diff = timespec64_sub(ts, ptp_clock_time); 444 if (diff.tv_sec > 2) 445 ts.tv_sec -= 4; 446 else if (diff.tv_sec < -2) 447 ts.tv_sec += 4; 448 449 return timespec64_to_ktime(ts); 450} 451 452bool ksz_port_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb, 453 unsigned int type) 454{ 455 struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb); 456 struct ksz_device *dev = ds->priv; 457 struct ptp_header *ptp_hdr; 458 struct ksz_port *prt; 459 u8 ptp_msg_type; 460 ktime_t tstamp; 461 s64 correction; 462 463 prt = &dev->ports[port]; 464 465 tstamp = KSZ_SKB_CB(skb)->tstamp; 466 memset(hwtstamps, 0, sizeof(*hwtstamps)); 467 hwtstamps->hwtstamp = ksz_tstamp_reconstruct(dev, tstamp); 468 469 if (prt->tstamp_config.tx_type != HWTSTAMP_TX_ONESTEP_P2P) 470 goto out; 471 472 ptp_hdr = ptp_parse_header(skb, type); 473 if (!ptp_hdr) 474 goto out; 475 476 ptp_msg_type = ptp_get_msgtype(ptp_hdr, type); 477 if (ptp_msg_type != PTP_MSGTYPE_PDELAY_REQ) 478 goto out; 479 480 /* Only subtract the partial time stamp from the correction field. When 481 * the hardware adds the egress time stamp to the correction field of 482 * the PDelay_Resp message on tx, also only the partial time stamp will 483 * be added. 484 */ 485 correction = (s64)get_unaligned_be64(&ptp_hdr->correction); 486 correction -= ktime_to_ns(tstamp) << 16; 487 488 ptp_header_update_correction(skb, type, ptp_hdr, correction); 489 490out: 491 return false; 492} 493 494void ksz_port_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb) 495{ 496 struct ksz_device *dev = ds->priv; 497 struct ptp_header *hdr; 498 struct sk_buff *clone; 499 struct ksz_port *prt; 500 unsigned int type; 501 u8 ptp_msg_type; 502 503 prt = &dev->ports[port]; 504 505 if (!prt->hwts_tx_en) 506 return; 507 508 type = ptp_classify_raw(skb); 509 if (type == PTP_CLASS_NONE) 510 return; 511 512 hdr = ptp_parse_header(skb, type); 513 if (!hdr) 514 return; 515 516 ptp_msg_type = ptp_get_msgtype(hdr, type); 517 518 switch (ptp_msg_type) { 519 case PTP_MSGTYPE_SYNC: 520 if (prt->tstamp_config.tx_type == HWTSTAMP_TX_ONESTEP_P2P) 521 return; 522 break; 523 case PTP_MSGTYPE_PDELAY_REQ: 524 break; 525 case PTP_MSGTYPE_PDELAY_RESP: 526 if (prt->tstamp_config.tx_type == HWTSTAMP_TX_ONESTEP_P2P) { 527 KSZ_SKB_CB(skb)->ptp_type = type; 528 KSZ_SKB_CB(skb)->update_correction = true; 529 return; 530 } 531 break; 532 533 default: 534 return; 535 } 536 537 clone = skb_clone_sk(skb); 538 if (!clone) 539 return; 540 541 /* caching the value to be used in tag_ksz.c */ 542 KSZ_SKB_CB(skb)->clone = clone; 543} 544 545static void ksz_ptp_txtstamp_skb(struct ksz_device *dev, 546 struct ksz_port *prt, struct sk_buff *skb) 547{ 548 struct skb_shared_hwtstamps hwtstamps = {}; 549 int ret; 550 551 /* timeout must include DSA conduit to transmit data, tstamp latency, 552 * IRQ latency and time for reading the time stamp. 553 */ 554 ret = wait_for_completion_timeout(&prt->tstamp_msg_comp, 555 msecs_to_jiffies(100)); 556 if (!ret) 557 return; 558 559 hwtstamps.hwtstamp = prt->tstamp_msg; 560 skb_complete_tx_timestamp(skb, &hwtstamps); 561} 562 563void ksz_port_deferred_xmit(struct kthread_work *work) 564{ 565 struct ksz_deferred_xmit_work *xmit_work = work_to_xmit_work(work); 566 struct sk_buff *clone, *skb = xmit_work->skb; 567 struct dsa_switch *ds = xmit_work->dp->ds; 568 struct ksz_device *dev = ds->priv; 569 struct ksz_port *prt; 570 571 prt = &dev->ports[xmit_work->dp->index]; 572 573 clone = KSZ_SKB_CB(skb)->clone; 574 575 skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS; 576 577 reinit_completion(&prt->tstamp_msg_comp); 578 579 dsa_enqueue_skb(skb, skb->dev); 580 581 ksz_ptp_txtstamp_skb(dev, prt, clone); 582 583 kfree(xmit_work); 584} 585 586static int _ksz_ptp_gettime(struct ksz_device *dev, struct timespec64 *ts) 587{ 588 u32 nanoseconds; 589 u32 seconds; 590 u8 phase; 591 int ret; 592 593 /* Copy current PTP clock into shadow registers and read */ 594 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_READ_TIME, PTP_READ_TIME); 595 if (ret) 596 return ret; 597 598 ret = ksz_read8(dev, REG_PTP_RTC_SUB_NANOSEC__2, &phase); 599 if (ret) 600 return ret; 601 602 ret = ksz_read32(dev, REG_PTP_RTC_NANOSEC, &nanoseconds); 603 if (ret) 604 return ret; 605 606 ret = ksz_read32(dev, REG_PTP_RTC_SEC, &seconds); 607 if (ret) 608 return ret; 609 610 ts->tv_sec = seconds; 611 ts->tv_nsec = nanoseconds + phase * 8; 612 613 return 0; 614} 615 616static int ksz_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts) 617{ 618 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 619 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 620 int ret; 621 622 mutex_lock(&ptp_data->lock); 623 ret = _ksz_ptp_gettime(dev, ts); 624 mutex_unlock(&ptp_data->lock); 625 626 return ret; 627} 628 629static int ksz_ptp_restart_perout(struct ksz_device *dev) 630{ 631 struct ksz_ptp_data *ptp_data = &dev->ptp_data; 632 s64 now_ns, first_ns, period_ns, next_ns; 633 struct ptp_perout_request request; 634 struct timespec64 next; 635 struct timespec64 now; 636 unsigned int count; 637 int ret; 638 639 dev_info(dev->dev, "Restarting periodic output signal\n"); 640 641 ret = _ksz_ptp_gettime(dev, &now); 642 if (ret) 643 return ret; 644 645 now_ns = timespec64_to_ns(&now); 646 first_ns = timespec64_to_ns(&ptp_data->perout_target_time_first); 647 648 /* Calculate next perout event based on start time and period */ 649 period_ns = timespec64_to_ns(&ptp_data->perout_period); 650 651 if (first_ns < now_ns) { 652 count = div_u64(now_ns - first_ns, period_ns); 653 next_ns = first_ns + count * period_ns; 654 } else { 655 next_ns = first_ns; 656 } 657 658 /* Ensure 100 ms guard time prior next event */ 659 while (next_ns < now_ns + 100000000) 660 next_ns += period_ns; 661 662 /* Restart periodic output signal */ 663 next = ns_to_timespec64(next_ns); 664 request.start.sec = next.tv_sec; 665 request.start.nsec = next.tv_nsec; 666 request.period.sec = ptp_data->perout_period.tv_sec; 667 request.period.nsec = ptp_data->perout_period.tv_nsec; 668 request.index = 0; 669 request.flags = 0; 670 671 return ksz_ptp_enable_perout(dev, &request, 1); 672} 673 674static int ksz_ptp_settime(struct ptp_clock_info *ptp, 675 const struct timespec64 *ts) 676{ 677 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 678 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 679 int ret; 680 681 mutex_lock(&ptp_data->lock); 682 683 /* Write to shadow registers and Load PTP clock */ 684 ret = ksz_write16(dev, REG_PTP_RTC_SUB_NANOSEC__2, PTP_RTC_0NS); 685 if (ret) 686 goto unlock; 687 688 ret = ksz_write32(dev, REG_PTP_RTC_NANOSEC, ts->tv_nsec); 689 if (ret) 690 goto unlock; 691 692 ret = ksz_write32(dev, REG_PTP_RTC_SEC, ts->tv_sec); 693 if (ret) 694 goto unlock; 695 696 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_LOAD_TIME, PTP_LOAD_TIME); 697 if (ret) 698 goto unlock; 699 700 switch (ptp_data->tou_mode) { 701 case KSZ_PTP_TOU_IDLE: 702 break; 703 704 case KSZ_PTP_TOU_PEROUT: 705 ret = ksz_ptp_restart_perout(dev); 706 if (ret) 707 goto unlock; 708 709 break; 710 } 711 712 spin_lock_bh(&ptp_data->clock_lock); 713 ptp_data->clock_time = *ts; 714 spin_unlock_bh(&ptp_data->clock_lock); 715 716unlock: 717 mutex_unlock(&ptp_data->lock); 718 719 return ret; 720} 721 722static int ksz_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 723{ 724 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 725 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 726 u64 base, adj; 727 bool negative; 728 u32 data32; 729 int ret; 730 731 mutex_lock(&ptp_data->lock); 732 733 if (scaled_ppm) { 734 base = KSZ_PTP_INC_NS << KSZ_PTP_SUBNS_BITS; 735 negative = diff_by_scaled_ppm(base, scaled_ppm, &adj); 736 737 data32 = (u32)adj; 738 data32 &= PTP_SUBNANOSEC_M; 739 if (!negative) 740 data32 |= PTP_RATE_DIR; 741 742 ret = ksz_write32(dev, REG_PTP_SUBNANOSEC_RATE, data32); 743 if (ret) 744 goto unlock; 745 746 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ADJ_ENABLE, 747 PTP_CLK_ADJ_ENABLE); 748 if (ret) 749 goto unlock; 750 } else { 751 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ADJ_ENABLE, 0); 752 if (ret) 753 goto unlock; 754 } 755 756unlock: 757 mutex_unlock(&ptp_data->lock); 758 return ret; 759} 760 761static int ksz_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 762{ 763 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 764 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 765 struct timespec64 delta64 = ns_to_timespec64(delta); 766 s32 sec, nsec; 767 u16 data16; 768 int ret; 769 770 mutex_lock(&ptp_data->lock); 771 772 /* do not use ns_to_timespec64(), 773 * both sec and nsec are subtracted by hw 774 */ 775 sec = div_s64_rem(delta, NSEC_PER_SEC, &nsec); 776 777 ret = ksz_write32(dev, REG_PTP_RTC_NANOSEC, abs(nsec)); 778 if (ret) 779 goto unlock; 780 781 ret = ksz_write32(dev, REG_PTP_RTC_SEC, abs(sec)); 782 if (ret) 783 goto unlock; 784 785 ret = ksz_read16(dev, REG_PTP_CLK_CTRL, &data16); 786 if (ret) 787 goto unlock; 788 789 data16 |= PTP_STEP_ADJ; 790 791 /* PTP_STEP_DIR -- 0: subtract, 1: add */ 792 if (delta < 0) 793 data16 &= ~PTP_STEP_DIR; 794 else 795 data16 |= PTP_STEP_DIR; 796 797 ret = ksz_write16(dev, REG_PTP_CLK_CTRL, data16); 798 if (ret) 799 goto unlock; 800 801 switch (ptp_data->tou_mode) { 802 case KSZ_PTP_TOU_IDLE: 803 break; 804 805 case KSZ_PTP_TOU_PEROUT: 806 ret = ksz_ptp_restart_perout(dev); 807 if (ret) 808 goto unlock; 809 810 break; 811 } 812 813 spin_lock_bh(&ptp_data->clock_lock); 814 ptp_data->clock_time = timespec64_add(ptp_data->clock_time, delta64); 815 spin_unlock_bh(&ptp_data->clock_lock); 816 817unlock: 818 mutex_unlock(&ptp_data->lock); 819 return ret; 820} 821 822static int ksz_ptp_enable(struct ptp_clock_info *ptp, 823 struct ptp_clock_request *req, int on) 824{ 825 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 826 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 827 int ret; 828 829 switch (req->type) { 830 case PTP_CLK_REQ_PEROUT: 831 mutex_lock(&ptp_data->lock); 832 ret = ksz_ptp_enable_perout(dev, &req->perout, on); 833 mutex_unlock(&ptp_data->lock); 834 break; 835 default: 836 return -EOPNOTSUPP; 837 } 838 839 return ret; 840} 841 842static int ksz_ptp_verify_pin(struct ptp_clock_info *ptp, unsigned int pin, 843 enum ptp_pin_function func, unsigned int chan) 844{ 845 int ret = 0; 846 847 switch (func) { 848 case PTP_PF_NONE: 849 case PTP_PF_PEROUT: 850 break; 851 default: 852 ret = -1; 853 break; 854 } 855 856 return ret; 857} 858 859/* Function is pointer to the do_aux_work in the ptp_clock capability */ 860static long ksz_ptp_do_aux_work(struct ptp_clock_info *ptp) 861{ 862 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 863 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 864 struct timespec64 ts; 865 int ret; 866 867 mutex_lock(&ptp_data->lock); 868 ret = _ksz_ptp_gettime(dev, &ts); 869 if (ret) 870 goto out; 871 872 spin_lock_bh(&ptp_data->clock_lock); 873 ptp_data->clock_time = ts; 874 spin_unlock_bh(&ptp_data->clock_lock); 875 876out: 877 mutex_unlock(&ptp_data->lock); 878 879 return HZ; /* reschedule in 1 second */ 880} 881 882static int ksz_ptp_start_clock(struct ksz_device *dev) 883{ 884 struct ksz_ptp_data *ptp_data = &dev->ptp_data; 885 int ret; 886 887 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ENABLE, PTP_CLK_ENABLE); 888 if (ret) 889 return ret; 890 891 ptp_data->clock_time.tv_sec = 0; 892 ptp_data->clock_time.tv_nsec = 0; 893 894 return 0; 895} 896 897int ksz_ptp_clock_register(struct dsa_switch *ds) 898{ 899 struct ksz_device *dev = ds->priv; 900 struct ksz_ptp_data *ptp_data; 901 int ret; 902 u8 i; 903 904 ptp_data = &dev->ptp_data; 905 mutex_init(&ptp_data->lock); 906 spin_lock_init(&ptp_data->clock_lock); 907 908 ptp_data->caps.owner = THIS_MODULE; 909 snprintf(ptp_data->caps.name, 16, "Microchip Clock"); 910 ptp_data->caps.max_adj = KSZ_MAX_DRIFT_CORR; 911 ptp_data->caps.gettime64 = ksz_ptp_gettime; 912 ptp_data->caps.settime64 = ksz_ptp_settime; 913 ptp_data->caps.adjfine = ksz_ptp_adjfine; 914 ptp_data->caps.adjtime = ksz_ptp_adjtime; 915 ptp_data->caps.do_aux_work = ksz_ptp_do_aux_work; 916 ptp_data->caps.enable = ksz_ptp_enable; 917 ptp_data->caps.verify = ksz_ptp_verify_pin; 918 ptp_data->caps.n_pins = KSZ_PTP_N_GPIO; 919 ptp_data->caps.n_per_out = 3; 920 921 ret = ksz_ptp_start_clock(dev); 922 if (ret) 923 return ret; 924 925 for (i = 0; i < KSZ_PTP_N_GPIO; i++) { 926 struct ptp_pin_desc *ptp_pin = &ptp_data->pin_config[i]; 927 928 snprintf(ptp_pin->name, 929 sizeof(ptp_pin->name), "ksz_ptp_pin_%02d", i); 930 ptp_pin->index = i; 931 ptp_pin->func = PTP_PF_NONE; 932 } 933 934 ptp_data->caps.pin_config = ptp_data->pin_config; 935 936 /* Currently only P2P mode is supported. When 802_1AS bit is set, it 937 * forwards all PTP packets to host port and none to other ports. 938 */ 939 ret = ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_TC_P2P | PTP_802_1AS, 940 PTP_TC_P2P | PTP_802_1AS); 941 if (ret) 942 return ret; 943 944 ptp_data->clock = ptp_clock_register(&ptp_data->caps, dev->dev); 945 if (IS_ERR_OR_NULL(ptp_data->clock)) 946 return PTR_ERR(ptp_data->clock); 947 948 return 0; 949} 950 951void ksz_ptp_clock_unregister(struct dsa_switch *ds) 952{ 953 struct ksz_device *dev = ds->priv; 954 struct ksz_ptp_data *ptp_data; 955 956 ptp_data = &dev->ptp_data; 957 958 if (ptp_data->clock) 959 ptp_clock_unregister(ptp_data->clock); 960} 961 962static irqreturn_t ksz_ptp_msg_thread_fn(int irq, void *dev_id) 963{ 964 struct ksz_ptp_irq *ptpmsg_irq = dev_id; 965 struct ksz_device *dev; 966 struct ksz_port *port; 967 u32 tstamp_raw; 968 ktime_t tstamp; 969 int ret; 970 971 port = ptpmsg_irq->port; 972 dev = port->ksz_dev; 973 974 if (ptpmsg_irq->ts_en) { 975 ret = ksz_read32(dev, ptpmsg_irq->ts_reg, &tstamp_raw); 976 if (ret) 977 return IRQ_NONE; 978 979 tstamp = ksz_decode_tstamp(tstamp_raw); 980 981 port->tstamp_msg = ksz_tstamp_reconstruct(dev, tstamp); 982 983 complete(&port->tstamp_msg_comp); 984 } 985 986 return IRQ_HANDLED; 987} 988 989static irqreturn_t ksz_ptp_irq_thread_fn(int irq, void *dev_id) 990{ 991 struct ksz_irq *ptpirq = dev_id; 992 unsigned int nhandled = 0; 993 struct ksz_device *dev; 994 unsigned int sub_irq; 995 u16 data; 996 int ret; 997 u8 n; 998 999 dev = ptpirq->dev; 1000 1001 ret = ksz_read16(dev, ptpirq->reg_status, &data); 1002 if (ret) 1003 goto out; 1004 1005 /* Clear the interrupts W1C */ 1006 ret = ksz_write16(dev, ptpirq->reg_status, data); 1007 if (ret) 1008 return IRQ_NONE; 1009 1010 for (n = 0; n < ptpirq->nirqs; ++n) { 1011 if (data & BIT(n + KSZ_PTP_INT_START)) { 1012 sub_irq = irq_find_mapping(ptpirq->domain, n); 1013 handle_nested_irq(sub_irq); 1014 ++nhandled; 1015 } 1016 } 1017 1018out: 1019 return (nhandled > 0 ? IRQ_HANDLED : IRQ_NONE); 1020} 1021 1022static void ksz_ptp_irq_mask(struct irq_data *d) 1023{ 1024 struct ksz_irq *kirq = irq_data_get_irq_chip_data(d); 1025 1026 kirq->masked &= ~BIT(d->hwirq + KSZ_PTP_INT_START); 1027} 1028 1029static void ksz_ptp_irq_unmask(struct irq_data *d) 1030{ 1031 struct ksz_irq *kirq = irq_data_get_irq_chip_data(d); 1032 1033 kirq->masked |= BIT(d->hwirq + KSZ_PTP_INT_START); 1034} 1035 1036static void ksz_ptp_irq_bus_lock(struct irq_data *d) 1037{ 1038 struct ksz_irq *kirq = irq_data_get_irq_chip_data(d); 1039 1040 mutex_lock(&kirq->dev->lock_irq); 1041} 1042 1043static void ksz_ptp_irq_bus_sync_unlock(struct irq_data *d) 1044{ 1045 struct ksz_irq *kirq = irq_data_get_irq_chip_data(d); 1046 struct ksz_device *dev = kirq->dev; 1047 int ret; 1048 1049 ret = ksz_write16(dev, kirq->reg_mask, kirq->masked); 1050 if (ret) 1051 dev_err(dev->dev, "failed to change IRQ mask\n"); 1052 1053 mutex_unlock(&dev->lock_irq); 1054} 1055 1056static const struct irq_chip ksz_ptp_irq_chip = { 1057 .name = "ksz-irq", 1058 .irq_mask = ksz_ptp_irq_mask, 1059 .irq_unmask = ksz_ptp_irq_unmask, 1060 .irq_bus_lock = ksz_ptp_irq_bus_lock, 1061 .irq_bus_sync_unlock = ksz_ptp_irq_bus_sync_unlock, 1062}; 1063 1064static int ksz_ptp_irq_domain_map(struct irq_domain *d, 1065 unsigned int irq, irq_hw_number_t hwirq) 1066{ 1067 irq_set_chip_data(irq, d->host_data); 1068 irq_set_chip_and_handler(irq, &ksz_ptp_irq_chip, handle_level_irq); 1069 irq_set_noprobe(irq); 1070 1071 return 0; 1072} 1073 1074static const struct irq_domain_ops ksz_ptp_irq_domain_ops = { 1075 .map = ksz_ptp_irq_domain_map, 1076 .xlate = irq_domain_xlate_twocell, 1077}; 1078 1079static void ksz_ptp_msg_irq_free(struct ksz_port *port, u8 n) 1080{ 1081 struct ksz_ptp_irq *ptpmsg_irq; 1082 1083 ptpmsg_irq = &port->ptpmsg_irq[n]; 1084 1085 free_irq(ptpmsg_irq->num, ptpmsg_irq); 1086 irq_dispose_mapping(ptpmsg_irq->num); 1087} 1088 1089static int ksz_ptp_msg_irq_setup(struct ksz_port *port, u8 n) 1090{ 1091 u16 ts_reg[] = {REG_PTP_PORT_PDRESP_TS, REG_PTP_PORT_XDELAY_TS, 1092 REG_PTP_PORT_SYNC_TS}; 1093 static const char * const name[] = {"pdresp-msg", "xdreq-msg", 1094 "sync-msg"}; 1095 const struct ksz_dev_ops *ops = port->ksz_dev->dev_ops; 1096 struct ksz_irq *ptpirq = &port->ptpirq; 1097 struct ksz_ptp_irq *ptpmsg_irq; 1098 int ret; 1099 1100 ptpmsg_irq = &port->ptpmsg_irq[n]; 1101 ptpmsg_irq->num = irq_create_mapping(ptpirq->domain, n); 1102 if (!ptpmsg_irq->num) 1103 return -EINVAL; 1104 1105 ptpmsg_irq->port = port; 1106 ptpmsg_irq->ts_reg = ops->get_port_addr(port->num, ts_reg[n]); 1107 1108 strscpy(ptpmsg_irq->name, name[n]); 1109 1110 ret = request_threaded_irq(ptpmsg_irq->num, NULL, 1111 ksz_ptp_msg_thread_fn, IRQF_ONESHOT, 1112 ptpmsg_irq->name, ptpmsg_irq); 1113 if (ret) 1114 irq_dispose_mapping(ptpmsg_irq->num); 1115 1116 return ret; 1117} 1118 1119int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p) 1120{ 1121 struct ksz_device *dev = ds->priv; 1122 const struct ksz_dev_ops *ops = dev->dev_ops; 1123 struct ksz_port *port = &dev->ports[p]; 1124 struct ksz_irq *ptpirq = &port->ptpirq; 1125 int irq; 1126 int ret; 1127 1128 ptpirq->dev = dev; 1129 ptpirq->masked = 0; 1130 ptpirq->nirqs = 3; 1131 ptpirq->reg_mask = ops->get_port_addr(p, REG_PTP_PORT_TX_INT_ENABLE__2); 1132 ptpirq->reg_status = ops->get_port_addr(p, 1133 REG_PTP_PORT_TX_INT_STATUS__2); 1134 snprintf(ptpirq->name, sizeof(ptpirq->name), "ptp-irq-%d", p); 1135 1136 init_completion(&port->tstamp_msg_comp); 1137 1138 ptpirq->domain = irq_domain_create_linear(dev_fwnode(dev->dev), ptpirq->nirqs, 1139 &ksz_ptp_irq_domain_ops, ptpirq); 1140 if (!ptpirq->domain) 1141 return -ENOMEM; 1142 1143 ptpirq->irq_num = irq_find_mapping(port->pirq.domain, PORT_SRC_PTP_INT); 1144 if (!ptpirq->irq_num) { 1145 ret = -EINVAL; 1146 goto out; 1147 } 1148 1149 ret = request_threaded_irq(ptpirq->irq_num, NULL, ksz_ptp_irq_thread_fn, 1150 IRQF_ONESHOT, ptpirq->name, ptpirq); 1151 if (ret) 1152 goto out; 1153 1154 for (irq = 0; irq < ptpirq->nirqs; irq++) { 1155 ret = ksz_ptp_msg_irq_setup(port, irq); 1156 if (ret) 1157 goto out_ptp_msg; 1158 } 1159 1160 return 0; 1161 1162out_ptp_msg: 1163 free_irq(ptpirq->irq_num, ptpirq); 1164 while (irq--) { 1165 free_irq(port->ptpmsg_irq[irq].num, &port->ptpmsg_irq[irq]); 1166 irq_dispose_mapping(port->ptpmsg_irq[irq].num); 1167 } 1168out: 1169 irq_domain_remove(ptpirq->domain); 1170 1171 return ret; 1172} 1173 1174void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p) 1175{ 1176 struct ksz_device *dev = ds->priv; 1177 struct ksz_port *port = &dev->ports[p]; 1178 struct ksz_irq *ptpirq = &port->ptpirq; 1179 u8 n; 1180 1181 for (n = 0; n < ptpirq->nirqs; n++) 1182 ksz_ptp_msg_irq_free(port, n); 1183 1184 free_irq(ptpirq->irq_num, ptpirq); 1185 irq_dispose_mapping(ptpirq->irq_num); 1186 1187 irq_domain_remove(ptpirq->domain); 1188} 1189 1190MODULE_AUTHOR("Christian Eggers <ceggers@arri.de>"); 1191MODULE_AUTHOR("Arun Ramadoss <arun.ramadoss@microchip.com>"); 1192MODULE_DESCRIPTION("PTP support for KSZ switch"); 1193MODULE_LICENSE("GPL");
~/
Read-only file preview.