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 35 static 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 57 static 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 80 static 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 94 static 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 114 static 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 141 static 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 177 static 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 262 static 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 int ret; 270 271 dsa_switch_for_each_user_port(dp, dev->ds) { 272 prt = &dev->ports[dp->index]; 273 if (prt->hwts_tx_en || prt->hwts_rx_en) { 274 tag_en = true; 275 break; 276 } 277 } 278 279 if (tag_en) { 280 ret = ptp_schedule_worker(ptp_data->clock, 0); 281 if (ret) 282 return ret; 283 } else { 284 ptp_cancel_worker_sync(ptp_data->clock); 285 } 286 287 tagger_data->hwtstamp_set_state(dev->ds, tag_en); 288 289 return ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_ENABLE, 290 tag_en ? PTP_ENABLE : 0); 291 } 292 293 /* The function is return back the capability of timestamping feature when 294 * requested through ethtool -T <interface> utility 295 */ 296 int ksz_get_ts_info(struct dsa_switch *ds, int port, struct ethtool_ts_info *ts) 297 { 298 struct ksz_device *dev = ds->priv; 299 struct ksz_ptp_data *ptp_data; 300 301 ptp_data = &dev->ptp_data; 302 303 if (!ptp_data->clock) 304 return -ENODEV; 305 306 ts->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE | 307 SOF_TIMESTAMPING_RX_HARDWARE | 308 SOF_TIMESTAMPING_RAW_HARDWARE; 309 310 ts->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ONESTEP_P2P); 311 312 if (is_lan937x(dev)) 313 ts->tx_types |= BIT(HWTSTAMP_TX_ON); 314 315 ts->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | 316 BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT) | 317 BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 318 BIT(HWTSTAMP_FILTER_PTP_V2_EVENT); 319 320 ts->phc_index = ptp_clock_index(ptp_data->clock); 321 322 return 0; 323 } 324 325 int ksz_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr) 326 { 327 struct ksz_device *dev = ds->priv; 328 struct hwtstamp_config *config; 329 struct ksz_port *prt; 330 331 prt = &dev->ports[port]; 332 config = &prt->tstamp_config; 333 334 return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ? 335 -EFAULT : 0; 336 } 337 338 static int ksz_set_hwtstamp_config(struct ksz_device *dev, 339 struct ksz_port *prt, 340 struct hwtstamp_config *config) 341 { 342 int ret; 343 344 if (config->flags) 345 return -EINVAL; 346 347 switch (config->tx_type) { 348 case HWTSTAMP_TX_OFF: 349 prt->ptpmsg_irq[KSZ_SYNC_MSG].ts_en = false; 350 prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = false; 351 prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = false; 352 prt->hwts_tx_en = false; 353 break; 354 case HWTSTAMP_TX_ONESTEP_P2P: 355 prt->ptpmsg_irq[KSZ_SYNC_MSG].ts_en = false; 356 prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = true; 357 prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = false; 358 prt->hwts_tx_en = true; 359 360 ret = ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_1STEP, PTP_1STEP); 361 if (ret) 362 return ret; 363 364 break; 365 case HWTSTAMP_TX_ON: 366 if (!is_lan937x(dev)) 367 return -ERANGE; 368 369 prt->ptpmsg_irq[KSZ_SYNC_MSG].ts_en = true; 370 prt->ptpmsg_irq[KSZ_XDREQ_MSG].ts_en = true; 371 prt->ptpmsg_irq[KSZ_PDRES_MSG].ts_en = true; 372 prt->hwts_tx_en = true; 373 374 ret = ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_1STEP, 0); 375 if (ret) 376 return ret; 377 378 break; 379 default: 380 return -ERANGE; 381 } 382 383 switch (config->rx_filter) { 384 case HWTSTAMP_FILTER_NONE: 385 prt->hwts_rx_en = false; 386 break; 387 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 388 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 389 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT; 390 prt->hwts_rx_en = true; 391 break; 392 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 393 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 394 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT; 395 prt->hwts_rx_en = true; 396 break; 397 case HWTSTAMP_FILTER_PTP_V2_EVENT: 398 case HWTSTAMP_FILTER_PTP_V2_SYNC: 399 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 400 prt->hwts_rx_en = true; 401 break; 402 default: 403 config->rx_filter = HWTSTAMP_FILTER_NONE; 404 return -ERANGE; 405 } 406 407 return ksz_ptp_enable_mode(dev); 408 } 409 410 int ksz_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr) 411 { 412 struct ksz_device *dev = ds->priv; 413 struct hwtstamp_config config; 414 struct ksz_port *prt; 415 int ret; 416 417 prt = &dev->ports[port]; 418 419 if (copy_from_user(&config, ifr->ifr_data, sizeof(config))) 420 return -EFAULT; 421 422 ret = ksz_set_hwtstamp_config(dev, prt, &config); 423 if (ret) 424 return ret; 425 426 memcpy(&prt->tstamp_config, &config, sizeof(config)); 427 428 if (copy_to_user(ifr->ifr_data, &config, sizeof(config))) 429 return -EFAULT; 430 431 return 0; 432 } 433 434 static ktime_t ksz_tstamp_reconstruct(struct ksz_device *dev, ktime_t tstamp) 435 { 436 struct timespec64 ptp_clock_time; 437 struct ksz_ptp_data *ptp_data; 438 struct timespec64 diff; 439 struct timespec64 ts; 440 441 ptp_data = &dev->ptp_data; 442 ts = ktime_to_timespec64(tstamp); 443 444 spin_lock_bh(&ptp_data->clock_lock); 445 ptp_clock_time = ptp_data->clock_time; 446 spin_unlock_bh(&ptp_data->clock_lock); 447 448 /* calculate full time from partial time stamp */ 449 ts.tv_sec = (ptp_clock_time.tv_sec & ~3) | ts.tv_sec; 450 451 /* find nearest possible point in time */ 452 diff = timespec64_sub(ts, ptp_clock_time); 453 if (diff.tv_sec > 2) 454 ts.tv_sec -= 4; 455 else if (diff.tv_sec < -2) 456 ts.tv_sec += 4; 457 458 return timespec64_to_ktime(ts); 459 } 460 461 bool ksz_port_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb, 462 unsigned int type) 463 { 464 struct skb_shared_hwtstamps *hwtstamps = skb_hwtstamps(skb); 465 struct ksz_device *dev = ds->priv; 466 struct ptp_header *ptp_hdr; 467 struct ksz_port *prt; 468 u8 ptp_msg_type; 469 ktime_t tstamp; 470 s64 correction; 471 472 prt = &dev->ports[port]; 473 474 tstamp = KSZ_SKB_CB(skb)->tstamp; 475 memset(hwtstamps, 0, sizeof(*hwtstamps)); 476 hwtstamps->hwtstamp = ksz_tstamp_reconstruct(dev, tstamp); 477 478 if (prt->tstamp_config.tx_type != HWTSTAMP_TX_ONESTEP_P2P) 479 goto out; 480 481 ptp_hdr = ptp_parse_header(skb, type); 482 if (!ptp_hdr) 483 goto out; 484 485 ptp_msg_type = ptp_get_msgtype(ptp_hdr, type); 486 if (ptp_msg_type != PTP_MSGTYPE_PDELAY_REQ) 487 goto out; 488 489 /* Only subtract the partial time stamp from the correction field. When 490 * the hardware adds the egress time stamp to the correction field of 491 * the PDelay_Resp message on tx, also only the partial time stamp will 492 * be added. 493 */ 494 correction = (s64)get_unaligned_be64(&ptp_hdr->correction); 495 correction -= ktime_to_ns(tstamp) << 16; 496 497 ptp_header_update_correction(skb, type, ptp_hdr, correction); 498 499 out: 500 return false; 501 } 502 503 void ksz_port_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb) 504 { 505 struct ksz_device *dev = ds->priv; 506 struct ptp_header *hdr; 507 struct sk_buff *clone; 508 struct ksz_port *prt; 509 unsigned int type; 510 u8 ptp_msg_type; 511 512 prt = &dev->ports[port]; 513 514 if (!prt->hwts_tx_en) 515 return; 516 517 type = ptp_classify_raw(skb); 518 if (type == PTP_CLASS_NONE) 519 return; 520 521 hdr = ptp_parse_header(skb, type); 522 if (!hdr) 523 return; 524 525 ptp_msg_type = ptp_get_msgtype(hdr, type); 526 527 switch (ptp_msg_type) { 528 case PTP_MSGTYPE_SYNC: 529 if (prt->tstamp_config.tx_type == HWTSTAMP_TX_ONESTEP_P2P) 530 return; 531 break; 532 case PTP_MSGTYPE_PDELAY_REQ: 533 break; 534 case PTP_MSGTYPE_PDELAY_RESP: 535 if (prt->tstamp_config.tx_type == HWTSTAMP_TX_ONESTEP_P2P) { 536 KSZ_SKB_CB(skb)->ptp_type = type; 537 KSZ_SKB_CB(skb)->update_correction = true; 538 return; 539 } 540 break; 541 542 default: 543 return; 544 } 545 546 clone = skb_clone_sk(skb); 547 if (!clone) 548 return; 549 550 /* caching the value to be used in tag_ksz.c */ 551 KSZ_SKB_CB(skb)->clone = clone; 552 } 553 554 static void ksz_ptp_txtstamp_skb(struct ksz_device *dev, 555 struct ksz_port *prt, struct sk_buff *skb) 556 { 557 struct skb_shared_hwtstamps hwtstamps = {}; 558 int ret; 559 560 /* timeout must include DSA master to transmit data, tstamp latency, 561 * IRQ latency and time for reading the time stamp. 562 */ 563 ret = wait_for_completion_timeout(&prt->tstamp_msg_comp, 564 msecs_to_jiffies(100)); 565 if (!ret) 566 return; 567 568 hwtstamps.hwtstamp = prt->tstamp_msg; 569 skb_complete_tx_timestamp(skb, &hwtstamps); 570 } 571 572 void ksz_port_deferred_xmit(struct kthread_work *work) 573 { 574 struct ksz_deferred_xmit_work *xmit_work = work_to_xmit_work(work); 575 struct sk_buff *clone, *skb = xmit_work->skb; 576 struct dsa_switch *ds = xmit_work->dp->ds; 577 struct ksz_device *dev = ds->priv; 578 struct ksz_port *prt; 579 580 prt = &dev->ports[xmit_work->dp->index]; 581 582 clone = KSZ_SKB_CB(skb)->clone; 583 584 skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS; 585 586 reinit_completion(&prt->tstamp_msg_comp); 587 588 dsa_enqueue_skb(skb, skb->dev); 589 590 ksz_ptp_txtstamp_skb(dev, prt, clone); 591 592 kfree(xmit_work); 593 } 594 595 static int _ksz_ptp_gettime(struct ksz_device *dev, struct timespec64 *ts) 596 { 597 u32 nanoseconds; 598 u32 seconds; 599 u8 phase; 600 int ret; 601 602 /* Copy current PTP clock into shadow registers and read */ 603 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_READ_TIME, PTP_READ_TIME); 604 if (ret) 605 return ret; 606 607 ret = ksz_read8(dev, REG_PTP_RTC_SUB_NANOSEC__2, &phase); 608 if (ret) 609 return ret; 610 611 ret = ksz_read32(dev, REG_PTP_RTC_NANOSEC, &nanoseconds); 612 if (ret) 613 return ret; 614 615 ret = ksz_read32(dev, REG_PTP_RTC_SEC, &seconds); 616 if (ret) 617 return ret; 618 619 ts->tv_sec = seconds; 620 ts->tv_nsec = nanoseconds + phase * 8; 621 622 return 0; 623 } 624 625 static int ksz_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts) 626 { 627 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 628 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 629 int ret; 630 631 mutex_lock(&ptp_data->lock); 632 ret = _ksz_ptp_gettime(dev, ts); 633 mutex_unlock(&ptp_data->lock); 634 635 return ret; 636 } 637 638 static int ksz_ptp_restart_perout(struct ksz_device *dev) 639 { 640 struct ksz_ptp_data *ptp_data = &dev->ptp_data; 641 s64 now_ns, first_ns, period_ns, next_ns; 642 struct ptp_perout_request request; 643 struct timespec64 next; 644 struct timespec64 now; 645 unsigned int count; 646 int ret; 647 648 dev_info(dev->dev, "Restarting periodic output signal\n"); 649 650 ret = _ksz_ptp_gettime(dev, &now); 651 if (ret) 652 return ret; 653 654 now_ns = timespec64_to_ns(&now); 655 first_ns = timespec64_to_ns(&ptp_data->perout_target_time_first); 656 657 /* Calculate next perout event based on start time and period */ 658 period_ns = timespec64_to_ns(&ptp_data->perout_period); 659 660 if (first_ns < now_ns) { 661 count = div_u64(now_ns - first_ns, period_ns); 662 next_ns = first_ns + count * period_ns; 663 } else { 664 next_ns = first_ns; 665 } 666 667 /* Ensure 100 ms guard time prior next event */ 668 while (next_ns < now_ns + 100000000) 669 next_ns += period_ns; 670 671 /* Restart periodic output signal */ 672 next = ns_to_timespec64(next_ns); 673 request.start.sec = next.tv_sec; 674 request.start.nsec = next.tv_nsec; 675 request.period.sec = ptp_data->perout_period.tv_sec; 676 request.period.nsec = ptp_data->perout_period.tv_nsec; 677 request.index = 0; 678 request.flags = 0; 679 680 return ksz_ptp_enable_perout(dev, &request, 1); 681 } 682 683 static int ksz_ptp_settime(struct ptp_clock_info *ptp, 684 const struct timespec64 *ts) 685 { 686 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 687 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 688 int ret; 689 690 mutex_lock(&ptp_data->lock); 691 692 /* Write to shadow registers and Load PTP clock */ 693 ret = ksz_write16(dev, REG_PTP_RTC_SUB_NANOSEC__2, PTP_RTC_0NS); 694 if (ret) 695 goto unlock; 696 697 ret = ksz_write32(dev, REG_PTP_RTC_NANOSEC, ts->tv_nsec); 698 if (ret) 699 goto unlock; 700 701 ret = ksz_write32(dev, REG_PTP_RTC_SEC, ts->tv_sec); 702 if (ret) 703 goto unlock; 704 705 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_LOAD_TIME, PTP_LOAD_TIME); 706 if (ret) 707 goto unlock; 708 709 switch (ptp_data->tou_mode) { 710 case KSZ_PTP_TOU_IDLE: 711 break; 712 713 case KSZ_PTP_TOU_PEROUT: 714 ret = ksz_ptp_restart_perout(dev); 715 if (ret) 716 goto unlock; 717 718 break; 719 } 720 721 spin_lock_bh(&ptp_data->clock_lock); 722 ptp_data->clock_time = *ts; 723 spin_unlock_bh(&ptp_data->clock_lock); 724 725 unlock: 726 mutex_unlock(&ptp_data->lock); 727 728 return ret; 729 } 730 731 static int ksz_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 732 { 733 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 734 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 735 u64 base, adj; 736 bool negative; 737 u32 data32; 738 int ret; 739 740 mutex_lock(&ptp_data->lock); 741 742 if (scaled_ppm) { 743 base = KSZ_PTP_INC_NS << KSZ_PTP_SUBNS_BITS; 744 negative = diff_by_scaled_ppm(base, scaled_ppm, &adj); 745 746 data32 = (u32)adj; 747 data32 &= PTP_SUBNANOSEC_M; 748 if (!negative) 749 data32 |= PTP_RATE_DIR; 750 751 ret = ksz_write32(dev, REG_PTP_SUBNANOSEC_RATE, data32); 752 if (ret) 753 goto unlock; 754 755 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ADJ_ENABLE, 756 PTP_CLK_ADJ_ENABLE); 757 if (ret) 758 goto unlock; 759 } else { 760 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ADJ_ENABLE, 0); 761 if (ret) 762 goto unlock; 763 } 764 765 unlock: 766 mutex_unlock(&ptp_data->lock); 767 return ret; 768 } 769 770 static int ksz_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 771 { 772 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 773 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 774 struct timespec64 delta64 = ns_to_timespec64(delta); 775 s32 sec, nsec; 776 u16 data16; 777 int ret; 778 779 mutex_lock(&ptp_data->lock); 780 781 /* do not use ns_to_timespec64(), 782 * both sec and nsec are subtracted by hw 783 */ 784 sec = div_s64_rem(delta, NSEC_PER_SEC, &nsec); 785 786 ret = ksz_write32(dev, REG_PTP_RTC_NANOSEC, abs(nsec)); 787 if (ret) 788 goto unlock; 789 790 ret = ksz_write32(dev, REG_PTP_RTC_SEC, abs(sec)); 791 if (ret) 792 goto unlock; 793 794 ret = ksz_read16(dev, REG_PTP_CLK_CTRL, &data16); 795 if (ret) 796 goto unlock; 797 798 data16 |= PTP_STEP_ADJ; 799 800 /* PTP_STEP_DIR -- 0: subtract, 1: add */ 801 if (delta < 0) 802 data16 &= ~PTP_STEP_DIR; 803 else 804 data16 |= PTP_STEP_DIR; 805 806 ret = ksz_write16(dev, REG_PTP_CLK_CTRL, data16); 807 if (ret) 808 goto unlock; 809 810 switch (ptp_data->tou_mode) { 811 case KSZ_PTP_TOU_IDLE: 812 break; 813 814 case KSZ_PTP_TOU_PEROUT: 815 ret = ksz_ptp_restart_perout(dev); 816 if (ret) 817 goto unlock; 818 819 break; 820 } 821 822 spin_lock_bh(&ptp_data->clock_lock); 823 ptp_data->clock_time = timespec64_add(ptp_data->clock_time, delta64); 824 spin_unlock_bh(&ptp_data->clock_lock); 825 826 unlock: 827 mutex_unlock(&ptp_data->lock); 828 return ret; 829 } 830 831 static int ksz_ptp_enable(struct ptp_clock_info *ptp, 832 struct ptp_clock_request *req, int on) 833 { 834 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 835 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 836 int ret; 837 838 switch (req->type) { 839 case PTP_CLK_REQ_PEROUT: 840 mutex_lock(&ptp_data->lock); 841 ret = ksz_ptp_enable_perout(dev, &req->perout, on); 842 mutex_unlock(&ptp_data->lock); 843 break; 844 default: 845 return -EOPNOTSUPP; 846 } 847 848 return ret; 849 } 850 851 static int ksz_ptp_verify_pin(struct ptp_clock_info *ptp, unsigned int pin, 852 enum ptp_pin_function func, unsigned int chan) 853 { 854 int ret = 0; 855 856 switch (func) { 857 case PTP_PF_NONE: 858 case PTP_PF_PEROUT: 859 break; 860 default: 861 ret = -1; 862 break; 863 } 864 865 return ret; 866 } 867 868 /* Function is pointer to the do_aux_work in the ptp_clock capability */ 869 static long ksz_ptp_do_aux_work(struct ptp_clock_info *ptp) 870 { 871 struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp); 872 struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data); 873 struct timespec64 ts; 874 int ret; 875 876 mutex_lock(&ptp_data->lock); 877 ret = _ksz_ptp_gettime(dev, &ts); 878 if (ret) 879 goto out; 880 881 spin_lock_bh(&ptp_data->clock_lock); 882 ptp_data->clock_time = ts; 883 spin_unlock_bh(&ptp_data->clock_lock); 884 885 out: 886 mutex_unlock(&ptp_data->lock); 887 888 return HZ; /* reschedule in 1 second */ 889 } 890 891 static int ksz_ptp_start_clock(struct ksz_device *dev) 892 { 893 struct ksz_ptp_data *ptp_data = &dev->ptp_data; 894 int ret; 895 896 ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ENABLE, PTP_CLK_ENABLE); 897 if (ret) 898 return ret; 899 900 ptp_data->clock_time.tv_sec = 0; 901 ptp_data->clock_time.tv_nsec = 0; 902 903 return 0; 904 } 905 906 int ksz_ptp_clock_register(struct dsa_switch *ds) 907 { 908 struct ksz_device *dev = ds->priv; 909 struct ksz_ptp_data *ptp_data; 910 int ret; 911 u8 i; 912 913 ptp_data = &dev->ptp_data; 914 mutex_init(&ptp_data->lock); 915 spin_lock_init(&ptp_data->clock_lock); 916 917 ptp_data->caps.owner = THIS_MODULE; 918 snprintf(ptp_data->caps.name, 16, "Microchip Clock"); 919 ptp_data->caps.max_adj = KSZ_MAX_DRIFT_CORR; 920 ptp_data->caps.gettime64 = ksz_ptp_gettime; 921 ptp_data->caps.settime64 = ksz_ptp_settime; 922 ptp_data->caps.adjfine = ksz_ptp_adjfine; 923 ptp_data->caps.adjtime = ksz_ptp_adjtime; 924 ptp_data->caps.do_aux_work = ksz_ptp_do_aux_work; 925 ptp_data->caps.enable = ksz_ptp_enable; 926 ptp_data->caps.verify = ksz_ptp_verify_pin; 927 ptp_data->caps.n_pins = KSZ_PTP_N_GPIO; 928 ptp_data->caps.n_per_out = 3; 929 930 ret = ksz_ptp_start_clock(dev); 931 if (ret) 932 return ret; 933 934 for (i = 0; i < KSZ_PTP_N_GPIO; i++) { 935 struct ptp_pin_desc *ptp_pin = &ptp_data->pin_config[i]; 936 937 snprintf(ptp_pin->name, 938 sizeof(ptp_pin->name), "ksz_ptp_pin_%02d", i); 939 ptp_pin->index = i; 940 ptp_pin->func = PTP_PF_NONE; 941 } 942 943 ptp_data->caps.pin_config = ptp_data->pin_config; 944 945 /* Currently only P2P mode is supported. When 802_1AS bit is set, it 946 * forwards all PTP packets to host port and none to other ports. 947 */ 948 ret = ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_TC_P2P | PTP_802_1AS, 949 PTP_TC_P2P | PTP_802_1AS); 950 if (ret) 951 return ret; 952 953 ptp_data->clock = ptp_clock_register(&ptp_data->caps, dev->dev); 954 if (IS_ERR_OR_NULL(ptp_data->clock)) 955 return PTR_ERR(ptp_data->clock); 956 957 return 0; 958 } 959 960 void ksz_ptp_clock_unregister(struct dsa_switch *ds) 961 { 962 struct ksz_device *dev = ds->priv; 963 struct ksz_ptp_data *ptp_data; 964 965 ptp_data = &dev->ptp_data; 966 967 if (ptp_data->clock) 968 ptp_clock_unregister(ptp_data->clock); 969 } 970 971 static irqreturn_t ksz_ptp_msg_thread_fn(int irq, void *dev_id) 972 { 973 struct ksz_ptp_irq *ptpmsg_irq = dev_id; 974 struct ksz_device *dev; 975 struct ksz_port *port; 976 u32 tstamp_raw; 977 ktime_t tstamp; 978 int ret; 979 980 port = ptpmsg_irq->port; 981 dev = port->ksz_dev; 982 983 if (ptpmsg_irq->ts_en) { 984 ret = ksz_read32(dev, ptpmsg_irq->ts_reg, &tstamp_raw); 985 if (ret) 986 return IRQ_NONE; 987 988 tstamp = ksz_decode_tstamp(tstamp_raw); 989 990 port->tstamp_msg = ksz_tstamp_reconstruct(dev, tstamp); 991 992 complete(&port->tstamp_msg_comp); 993 } 994 995 return IRQ_HANDLED; 996 } 997 998 static irqreturn_t ksz_ptp_irq_thread_fn(int irq, void *dev_id) 999 { 1000 struct ksz_irq *ptpirq = dev_id; 1001 unsigned int nhandled = 0; 1002 struct ksz_device *dev; 1003 unsigned int sub_irq; 1004 u16 data; 1005 int ret; 1006 u8 n; 1007 1008 dev = ptpirq->dev; 1009 1010 ret = ksz_read16(dev, ptpirq->reg_status, &data); 1011 if (ret) 1012 goto out; 1013 1014 /* Clear the interrupts W1C */ 1015 ret = ksz_write16(dev, ptpirq->reg_status, data); 1016 if (ret) 1017 return IRQ_NONE; 1018 1019 for (n = 0; n < ptpirq->nirqs; ++n) { 1020 if (data & BIT(n + KSZ_PTP_INT_START)) { 1021 sub_irq = irq_find_mapping(ptpirq->domain, n); 1022 handle_nested_irq(sub_irq); 1023 ++nhandled; 1024 } 1025 } 1026 1027 out: 1028 return (nhandled > 0 ? IRQ_HANDLED : IRQ_NONE); 1029 } 1030 1031 static void ksz_ptp_irq_mask(struct irq_data *d) 1032 { 1033 struct ksz_irq *kirq = irq_data_get_irq_chip_data(d); 1034 1035 kirq->masked &= ~BIT(d->hwirq + KSZ_PTP_INT_START); 1036 } 1037 1038 static void ksz_ptp_irq_unmask(struct irq_data *d) 1039 { 1040 struct ksz_irq *kirq = irq_data_get_irq_chip_data(d); 1041 1042 kirq->masked |= BIT(d->hwirq + KSZ_PTP_INT_START); 1043 } 1044 1045 static void ksz_ptp_irq_bus_lock(struct irq_data *d) 1046 { 1047 struct ksz_irq *kirq = irq_data_get_irq_chip_data(d); 1048 1049 mutex_lock(&kirq->dev->lock_irq); 1050 } 1051 1052 static void ksz_ptp_irq_bus_sync_unlock(struct irq_data *d) 1053 { 1054 struct ksz_irq *kirq = irq_data_get_irq_chip_data(d); 1055 struct ksz_device *dev = kirq->dev; 1056 int ret; 1057 1058 ret = ksz_write16(dev, kirq->reg_mask, kirq->masked); 1059 if (ret) 1060 dev_err(dev->dev, "failed to change IRQ mask\n"); 1061 1062 mutex_unlock(&dev->lock_irq); 1063 } 1064 1065 static const struct irq_chip ksz_ptp_irq_chip = { 1066 .name = "ksz-irq", 1067 .irq_mask = ksz_ptp_irq_mask, 1068 .irq_unmask = ksz_ptp_irq_unmask, 1069 .irq_bus_lock = ksz_ptp_irq_bus_lock, 1070 .irq_bus_sync_unlock = ksz_ptp_irq_bus_sync_unlock, 1071 }; 1072 1073 static int ksz_ptp_irq_domain_map(struct irq_domain *d, 1074 unsigned int irq, irq_hw_number_t hwirq) 1075 { 1076 irq_set_chip_data(irq, d->host_data); 1077 irq_set_chip_and_handler(irq, &ksz_ptp_irq_chip, handle_level_irq); 1078 irq_set_noprobe(irq); 1079 1080 return 0; 1081 } 1082 1083 static const struct irq_domain_ops ksz_ptp_irq_domain_ops = { 1084 .map = ksz_ptp_irq_domain_map, 1085 .xlate = irq_domain_xlate_twocell, 1086 }; 1087 1088 static void ksz_ptp_msg_irq_free(struct ksz_port *port, u8 n) 1089 { 1090 struct ksz_ptp_irq *ptpmsg_irq; 1091 1092 ptpmsg_irq = &port->ptpmsg_irq[n]; 1093 1094 free_irq(ptpmsg_irq->num, ptpmsg_irq); 1095 irq_dispose_mapping(ptpmsg_irq->num); 1096 } 1097 1098 static int ksz_ptp_msg_irq_setup(struct ksz_port *port, u8 n) 1099 { 1100 u16 ts_reg[] = {REG_PTP_PORT_PDRESP_TS, REG_PTP_PORT_XDELAY_TS, 1101 REG_PTP_PORT_SYNC_TS}; 1102 static const char * const name[] = {"pdresp-msg", "xdreq-msg", 1103 "sync-msg"}; 1104 const struct ksz_dev_ops *ops = port->ksz_dev->dev_ops; 1105 struct ksz_ptp_irq *ptpmsg_irq; 1106 1107 ptpmsg_irq = &port->ptpmsg_irq[n]; 1108 1109 ptpmsg_irq->port = port; 1110 ptpmsg_irq->ts_reg = ops->get_port_addr(port->num, ts_reg[n]); 1111 1112 snprintf(ptpmsg_irq->name, sizeof(ptpmsg_irq->name), name[n]); 1113 1114 ptpmsg_irq->num = irq_find_mapping(port->ptpirq.domain, n); 1115 if (ptpmsg_irq->num < 0) 1116 return ptpmsg_irq->num; 1117 1118 return request_threaded_irq(ptpmsg_irq->num, NULL, 1119 ksz_ptp_msg_thread_fn, IRQF_ONESHOT, 1120 ptpmsg_irq->name, ptpmsg_irq); 1121 } 1122 1123 int ksz_ptp_irq_setup(struct dsa_switch *ds, u8 p) 1124 { 1125 struct ksz_device *dev = ds->priv; 1126 const struct ksz_dev_ops *ops = dev->dev_ops; 1127 struct ksz_port *port = &dev->ports[p]; 1128 struct ksz_irq *ptpirq = &port->ptpirq; 1129 int irq; 1130 int ret; 1131 1132 ptpirq->dev = dev; 1133 ptpirq->masked = 0; 1134 ptpirq->nirqs = 3; 1135 ptpirq->reg_mask = ops->get_port_addr(p, REG_PTP_PORT_TX_INT_ENABLE__2); 1136 ptpirq->reg_status = ops->get_port_addr(p, 1137 REG_PTP_PORT_TX_INT_STATUS__2); 1138 snprintf(ptpirq->name, sizeof(ptpirq->name), "ptp-irq-%d", p); 1139 1140 init_completion(&port->tstamp_msg_comp); 1141 1142 ptpirq->domain = irq_domain_add_linear(dev->dev->of_node, ptpirq->nirqs, 1143 &ksz_ptp_irq_domain_ops, ptpirq); 1144 if (!ptpirq->domain) 1145 return -ENOMEM; 1146 1147 for (irq = 0; irq < ptpirq->nirqs; irq++) 1148 irq_create_mapping(ptpirq->domain, irq); 1149 1150 ptpirq->irq_num = irq_find_mapping(port->pirq.domain, PORT_SRC_PTP_INT); 1151 if (ptpirq->irq_num < 0) { 1152 ret = ptpirq->irq_num; 1153 goto out; 1154 } 1155 1156 ret = request_threaded_irq(ptpirq->irq_num, NULL, ksz_ptp_irq_thread_fn, 1157 IRQF_ONESHOT, ptpirq->name, ptpirq); 1158 if (ret) 1159 goto out; 1160 1161 for (irq = 0; irq < ptpirq->nirqs; irq++) { 1162 ret = ksz_ptp_msg_irq_setup(port, irq); 1163 if (ret) 1164 goto out_ptp_msg; 1165 } 1166 1167 return 0; 1168 1169 out_ptp_msg: 1170 free_irq(ptpirq->irq_num, ptpirq); 1171 while (irq--) 1172 free_irq(port->ptpmsg_irq[irq].num, &port->ptpmsg_irq[irq]); 1173 out: 1174 for (irq = 0; irq < ptpirq->nirqs; irq++) 1175 irq_dispose_mapping(port->ptpmsg_irq[irq].num); 1176 1177 irq_domain_remove(ptpirq->domain); 1178 1179 return ret; 1180 } 1181 1182 void ksz_ptp_irq_free(struct dsa_switch *ds, u8 p) 1183 { 1184 struct ksz_device *dev = ds->priv; 1185 struct ksz_port *port = &dev->ports[p]; 1186 struct ksz_irq *ptpirq = &port->ptpirq; 1187 u8 n; 1188 1189 for (n = 0; n < ptpirq->nirqs; n++) 1190 ksz_ptp_msg_irq_free(port, n); 1191 1192 free_irq(ptpirq->irq_num, ptpirq); 1193 irq_dispose_mapping(ptpirq->irq_num); 1194 1195 irq_domain_remove(ptpirq->domain); 1196 } 1197 1198 MODULE_AUTHOR("Christian Eggers <ceggers@arri.de>"); 1199 MODULE_AUTHOR("Arun Ramadoss <arun.ramadoss@microchip.com>"); 1200 MODULE_DESCRIPTION("PTP support for KSZ switch"); 1201 MODULE_LICENSE("GPL"); 1202