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
  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 conduit 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");