drivers/ptp/ptp_idt82p33.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 //
   3 // Copyright (C) 2018 Integrated Device Technology, Inc
   4 //
   5
   6 #define pr_fmt(fmt) "IDT_82p33xxx: " fmt
   7
   8 #include <linux/firmware.h>
   9 #include <linux/platform_device.h>
  10 #include <linux/module.h>
  11 #include <linux/ptp_clock_kernel.h>
  12 #include <linux/delay.h>
  13 #include <linux/jiffies.h>
  14 #include <linux/kernel.h>
  15 #include <linux/timekeeping.h>
  16 #include <linux/bitops.h>
  17 #include <linux/of.h>
  18 #include <linux/mfd/rsmu.h>
  19 #include <linux/mfd/idt82p33_reg.h>
  20
  21 #include "ptp_private.h"
  22 #include "ptp_idt82p33.h"
  23
  24 MODULE_DESCRIPTION("Driver for IDT 82p33xxx clock devices");
  25 MODULE_AUTHOR("IDT support-1588 <IDT-support-1588@lm.renesas.com>");
  26 MODULE_VERSION("1.0");
  27 MODULE_LICENSE("GPL");
  28 MODULE_FIRMWARE(FW_FILENAME);
  29
  30 #define EXTTS_PERIOD_MS (95)
  31
  32 /* Module Parameters */
  33 static u32 phase_snap_threshold = SNAP_THRESHOLD_NS;
  34 module_param(phase_snap_threshold, uint, 0);
  35 MODULE_PARM_DESC(phase_snap_threshold,
  36 "threshold (10000ns by default) below which adjtime would use double dco");
  37
  38 static char *firmware;
  39 module_param(firmware, charp, 0);
  40
  41 static struct ptp_pin_desc pin_config[MAX_PHC_PLL][MAX_TRIG_CLK];
  42
  43 static inline int idt82p33_read(struct idt82p33 *idt82p33, u16 regaddr,
  44                                 u8 *buf, u16 count)
  45 {
  46         return regmap_bulk_read(idt82p33->regmap, regaddr, buf, count);
  47 }
  48
  49 static inline int idt82p33_write(struct idt82p33 *idt82p33, u16 regaddr,
  50                                  u8 *buf, u16 count)
  51 {
  52         return regmap_bulk_write(idt82p33->regmap, regaddr, buf, count);
  53 }
  54
  55 static void idt82p33_byte_array_to_timespec(struct timespec64 *ts,
  56                                             u8 buf[TOD_BYTE_COUNT])
  57 {
  58         time64_t sec;
  59         s32 nsec;
  60         u8 i;
  61
  62         nsec = buf[3];
  63         for (i = 0; i < 3; i++) {
  64                 nsec <<= 8;
  65                 nsec |= buf[2 - i];
  66         }
  67
  68         sec = buf[9];
  69         for (i = 0; i < 5; i++) {
  70                 sec <<= 8;
  71                 sec |= buf[8 - i];
  72         }
  73
  74         ts->tv_sec = sec;
  75         ts->tv_nsec = nsec;
  76 }
  77
  78 static void idt82p33_timespec_to_byte_array(struct timespec64 const *ts,
  79                                             u8 buf[TOD_BYTE_COUNT])
  80 {
  81         time64_t sec;
  82         s32 nsec;
  83         u8 i;
  84
  85         nsec = ts->tv_nsec;
  86         sec = ts->tv_sec;
  87
  88         for (i = 0; i < 4; i++) {
  89                 buf[i] = nsec & 0xff;
  90                 nsec >>= 8;
  91         }
  92
  93         for (i = 4; i < TOD_BYTE_COUNT; i++) {
  94                 buf[i] = sec & 0xff;
  95                 sec >>= 8;
  96         }
  97 }
  98
  99 static int idt82p33_dpll_set_mode(struct idt82p33_channel *channel,
 100                                   enum pll_mode mode)
 101 {
 102         struct idt82p33 *idt82p33 = channel->idt82p33;
 103         u8 dpll_mode;
 104         int err;
 105
 106         if (channel->pll_mode == mode)
 107                 return 0;
 108
 109         err = idt82p33_read(idt82p33, channel->dpll_mode_cnfg,
 110                             &dpll_mode, sizeof(dpll_mode));
 111         if (err)
 112                 return err;
 113
 114         dpll_mode &= ~(PLL_MODE_MASK << PLL_MODE_SHIFT);
 115
 116         dpll_mode |= (mode << PLL_MODE_SHIFT);
 117
 118         err = idt82p33_write(idt82p33, channel->dpll_mode_cnfg,
 119                              &dpll_mode, sizeof(dpll_mode));
 120         if (err)
 121                 return err;
 122
 123         channel->pll_mode = mode;
 124
 125         return 0;
 126 }
 127
 128 static int idt82p33_set_tod_trigger(struct idt82p33_channel *channel,
 129                                     u8 trigger, bool write)
 130 {
 131         struct idt82p33 *idt82p33 = channel->idt82p33;
 132         int err;
 133         u8 cfg;
 134
 135         if (trigger > WR_TRIG_SEL_MAX)
 136                 return -EINVAL;
 137
 138         err = idt82p33_read(idt82p33, channel->dpll_tod_trigger,
 139                             &cfg, sizeof(cfg));
 140
 141         if (err)
 142                 return err;
 143
 144         if (write == true)
 145                 trigger = (trigger << WRITE_TRIGGER_SHIFT) |
 146                           (cfg & READ_TRIGGER_MASK);
 147         else
 148                 trigger = (trigger << READ_TRIGGER_SHIFT) |
 149                           (cfg & WRITE_TRIGGER_MASK);
 150
 151         return idt82p33_write(idt82p33, channel->dpll_tod_trigger,
 152                               &trigger, sizeof(trigger));
 153 }
 154
 155 static int idt82p33_get_extts(struct idt82p33_channel *channel,
 156                               struct timespec64 *ts)
 157 {
 158         struct idt82p33 *idt82p33 = channel->idt82p33;
 159         u8 buf[TOD_BYTE_COUNT];
 160         int err;
 161
 162         err = idt82p33_read(idt82p33, channel->dpll_tod_sts, buf, sizeof(buf));
 163
 164         if (err)
 165                 return err;
 166
 167         /* Since trigger is not self clearing itself, we have to poll tod_sts */
 168         if (memcmp(buf, channel->extts_tod_sts, TOD_BYTE_COUNT) == 0)
 169                 return -EAGAIN;
 170
 171         memcpy(channel->extts_tod_sts, buf, TOD_BYTE_COUNT);
 172
 173         idt82p33_byte_array_to_timespec(ts, buf);
 174
 175         if (channel->discard_next_extts) {
 176                 channel->discard_next_extts = false;
 177                 return -EAGAIN;
 178         }
 179
 180         return 0;
 181 }
 182
 183 static int map_ref_to_tod_trig_sel(int ref, u8 *trigger)
 184 {
 185         int err = 0;
 186
 187         switch (ref) {
 188         case 0:
 189                 *trigger = HW_TOD_TRIG_SEL_IN12;
 190                 break;
 191         case 1:
 192                 *trigger = HW_TOD_TRIG_SEL_IN13;
 193                 break;
 194         case 2:
 195                 *trigger = HW_TOD_TRIG_SEL_IN14;
 196                 break;
 197         default:
 198                 err = -EINVAL;
 199         }
 200
 201         return err;
 202 }
 203
 204 static bool is_one_shot(u8 mask)
 205 {
 206         /* Treat single bit PLL masks as continuous trigger */
 207         if ((mask == 1) || (mask == 2))
 208                 return false;
 209         else
 210                 return true;
 211 }
 212
 213 static int arm_tod_read_with_trigger(struct idt82p33_channel *channel, u8 trigger)
 214 {
 215         struct idt82p33 *idt82p33 = channel->idt82p33;
 216         u8 buf[TOD_BYTE_COUNT];
 217         int err;
 218
 219         /* Remember the current tod_sts before setting the trigger */
 220         err = idt82p33_read(idt82p33, channel->dpll_tod_sts, buf, sizeof(buf));
 221
 222         if (err)
 223                 return err;
 224
 225         memcpy(channel->extts_tod_sts, buf, TOD_BYTE_COUNT);
 226
 227         err = idt82p33_set_tod_trigger(channel, trigger, false);
 228
 229         if (err)
 230                 dev_err(idt82p33->dev, "%s: err = %d", __func__, err);
 231
 232         return err;
 233 }
 234
 235 static int idt82p33_extts_enable(struct idt82p33_channel *channel,
 236                                  struct ptp_clock_request *rq, int on)
 237 {
 238         u8 index = rq->extts.index;
 239         struct idt82p33 *idt82p33;
 240         u8 mask = 1 << index;
 241         int err = 0;
 242         u8 old_mask;
 243         u8 trigger;
 244         int ref;
 245
 246         idt82p33  = channel->idt82p33;
 247         old_mask = idt82p33->extts_mask;
 248
 249         /* Reject requests with unsupported flags */
 250         if (rq->extts.flags & ~(PTP_ENABLE_FEATURE |
 251                                 PTP_RISING_EDGE |
 252                                 PTP_FALLING_EDGE |
 253                                 PTP_STRICT_FLAGS))
 254                 return -EOPNOTSUPP;
 255
 256         /* Reject requests to enable time stamping on falling edge */
 257         if ((rq->extts.flags & PTP_ENABLE_FEATURE) &&
 258             (rq->extts.flags & PTP_FALLING_EDGE))
 259                 return -EOPNOTSUPP;
 260
 261         if (index >= MAX_PHC_PLL)
 262                 return -EINVAL;
 263
 264         if (on) {
 265                 /* Return if it was already enabled */
 266                 if (idt82p33->extts_mask & mask)
 267                         return 0;
 268
 269                 /* Use the pin configured for the channel */
 270                 ref = ptp_find_pin(channel->ptp_clock, PTP_PF_EXTTS, channel->plln);
 271
 272                 if (ref < 0) {
 273                         dev_err(idt82p33->dev, "%s: No valid pin found for Pll%d!\n",
 274                                 __func__, channel->plln);
 275                         return -EBUSY;
 276                 }
 277
 278                 err = map_ref_to_tod_trig_sel(ref, &trigger);
 279
 280                 if (err) {
 281                         dev_err(idt82p33->dev,
 282                                 "%s: Unsupported ref %d!\n", __func__, ref);
 283                         return err;
 284                 }
 285
 286                 err = arm_tod_read_with_trigger(&idt82p33->channel[index], trigger);
 287
 288                 if (err == 0) {
 289                         idt82p33->extts_mask |= mask;
 290                         idt82p33->channel[index].tod_trigger = trigger;
 291                         idt82p33->event_channel[index] = channel;
 292                         idt82p33->extts_single_shot = is_one_shot(idt82p33->extts_mask);
 293
 294                         if (old_mask)
 295                                 return 0;
 296
 297                         schedule_delayed_work(&idt82p33->extts_work,
 298                                               msecs_to_jiffies(EXTTS_PERIOD_MS));
 299                 }
 300         } else {
 301                 idt82p33->extts_mask &= ~mask;
 302                 idt82p33->extts_single_shot = is_one_shot(idt82p33->extts_mask);
 303
 304                 if (idt82p33->extts_mask == 0)
 305                         cancel_delayed_work(&idt82p33->extts_work);
 306         }
 307
 308         return err;
 309 }
 310
 311 static int idt82p33_extts_check_channel(struct idt82p33 *idt82p33, u8 todn)
 312 {
 313         struct idt82p33_channel *event_channel;
 314         struct ptp_clock_event event;
 315         struct timespec64 ts;
 316         int err;
 317
 318         err = idt82p33_get_extts(&idt82p33->channel[todn], &ts);
 319         if (err == 0) {
 320                 event_channel = idt82p33->event_channel[todn];
 321                 event.type = PTP_CLOCK_EXTTS;
 322                 event.index = todn;
 323                 event.timestamp = timespec64_to_ns(&ts);
 324                 ptp_clock_event(event_channel->ptp_clock,
 325                                 &event);
 326         }
 327         return err;
 328 }
 329
 330 static u8 idt82p33_extts_enable_mask(struct idt82p33_channel *channel,
 331                                      u8 extts_mask, bool enable)
 332 {
 333         struct idt82p33 *idt82p33 = channel->idt82p33;
 334         u8 trigger = channel->tod_trigger;
 335         u8 mask;
 336         int err;
 337         int i;
 338
 339         if (extts_mask == 0)
 340                 return 0;
 341
 342         if (enable == false)
 343                 cancel_delayed_work_sync(&idt82p33->extts_work);
 344
 345         for (i = 0; i < MAX_PHC_PLL; i++) {
 346                 mask = 1 << i;
 347
 348                 if ((extts_mask & mask) == 0)
 349                         continue;
 350
 351                 if (enable) {
 352                         err = arm_tod_read_with_trigger(&idt82p33->channel[i], trigger);
 353                         if (err)
 354                                 dev_err(idt82p33->dev,
 355                                         "%s: Arm ToD read trigger failed, err = %d",
 356                                         __func__, err);
 357                 } else {
 358                         err = idt82p33_extts_check_channel(idt82p33, i);
 359                         if (err == 0 && idt82p33->extts_single_shot)
 360                                 /* trigger happened so we won't re-enable it */
 361                                 extts_mask &= ~mask;
 362                 }
 363         }
 364
 365         if (enable)
 366                 schedule_delayed_work(&idt82p33->extts_work,
 367                                       msecs_to_jiffies(EXTTS_PERIOD_MS));
 368
 369         return extts_mask;
 370 }
 371
 372 static int _idt82p33_gettime(struct idt82p33_channel *channel,
 373                              struct timespec64 *ts)
 374 {
 375         struct idt82p33 *idt82p33 = channel->idt82p33;
 376         u8 old_mask = idt82p33->extts_mask;
 377         u8 buf[TOD_BYTE_COUNT];
 378         u8 new_mask = 0;
 379         int err;
 380
 381         /* Disable extts */
 382         if (old_mask)
 383                 new_mask = idt82p33_extts_enable_mask(channel, old_mask, false);
 384
 385         err = idt82p33_set_tod_trigger(channel, HW_TOD_RD_TRIG_SEL_LSB_TOD_STS,
 386                                        false);
 387         if (err)
 388                 return err;
 389
 390         channel->discard_next_extts = true;
 391
 392         if (idt82p33->calculate_overhead_flag)
 393                 idt82p33->start_time = ktime_get_raw();
 394
 395         err = idt82p33_read(idt82p33, channel->dpll_tod_sts, buf, sizeof(buf));
 396
 397         if (err)
 398                 return err;
 399
 400         /* Re-enable extts */
 401         if (new_mask)
 402                 idt82p33_extts_enable_mask(channel, new_mask, true);
 403
 404         idt82p33_byte_array_to_timespec(ts, buf);
 405
 406         return 0;
 407 }
 408
 409 /*
 410  *   TOD Trigger:
 411  *   Bits[7:4] Write 0x9, MSB write
 412  *   Bits[3:0] Read 0x9, LSB read
 413  */
 414
 415 static int _idt82p33_settime(struct idt82p33_channel *channel,
 416                              struct timespec64 const *ts)
 417 {
 418         struct idt82p33 *idt82p33 = channel->idt82p33;
 419         struct timespec64 local_ts = *ts;
 420         char buf[TOD_BYTE_COUNT];
 421         s64 dynamic_overhead_ns;
 422         int err;
 423         u8 i;
 424
 425         err = idt82p33_set_tod_trigger(channel, HW_TOD_WR_TRIG_SEL_MSB_TOD_CNFG,
 426                                        true);
 427         if (err)
 428                 return err;
 429
 430         channel->discard_next_extts = true;
 431
 432         if (idt82p33->calculate_overhead_flag) {
 433                 dynamic_overhead_ns = ktime_to_ns(ktime_get_raw())
 434                                         - ktime_to_ns(idt82p33->start_time);
 435
 436                 timespec64_add_ns(&local_ts, dynamic_overhead_ns);
 437
 438                 idt82p33->calculate_overhead_flag = 0;
 439         }
 440
 441         idt82p33_timespec_to_byte_array(&local_ts, buf);
 442
 443         /*
 444          * Store the new time value.
 445          */
 446         for (i = 0; i < TOD_BYTE_COUNT; i++) {
 447                 err = idt82p33_write(idt82p33, channel->dpll_tod_cnfg + i,
 448                                      &buf[i], sizeof(buf[i]));
 449                 if (err)
 450                         return err;
 451         }
 452
 453         return err;
 454 }
 455
 456 static int _idt82p33_adjtime_immediate(struct idt82p33_channel *channel,
 457                                        s64 delta_ns)
 458 {
 459         struct idt82p33 *idt82p33 = channel->idt82p33;
 460         struct timespec64 ts;
 461         s64 now_ns;
 462         int err;
 463
 464         idt82p33->calculate_overhead_flag = 1;
 465
 466         err = _idt82p33_gettime(channel, &ts);
 467
 468         if (err)
 469                 return err;
 470
 471         now_ns = timespec64_to_ns(&ts);
 472         now_ns += delta_ns + idt82p33->tod_write_overhead_ns;
 473
 474         ts = ns_to_timespec64(now_ns);
 475
 476         err = _idt82p33_settime(channel, &ts);
 477
 478         return err;
 479 }
 480
 481 static int _idt82p33_adjtime_internal_triggered(struct idt82p33_channel *channel,
 482                                                 s64 delta_ns)
 483 {
 484         struct idt82p33 *idt82p33 = channel->idt82p33;
 485         char buf[TOD_BYTE_COUNT];
 486         struct timespec64 ts;
 487         const u8 delay_ns = 32;
 488         s32 remainder;
 489         s64 ns;
 490         int err;
 491
 492         err = _idt82p33_gettime(channel, &ts);
 493
 494         if (err)
 495                 return err;
 496
 497         if (ts.tv_nsec > (NSEC_PER_SEC - 5 * NSEC_PER_MSEC)) {
 498                 /*  Too close to miss next trigger, so skip it */
 499                 mdelay(6);
 500                 ns = (ts.tv_sec + 2) * NSEC_PER_SEC + delta_ns + delay_ns;
 501         } else
 502                 ns = (ts.tv_sec + 1) * NSEC_PER_SEC + delta_ns + delay_ns;
 503
 504         ts = ns_to_timespec64(ns);
 505         idt82p33_timespec_to_byte_array(&ts, buf);
 506
 507         /*
 508          * Store the new time value.
 509          */
 510         err = idt82p33_write(idt82p33, channel->dpll_tod_cnfg, buf, sizeof(buf));
 511         if (err)
 512                 return err;
 513
 514         /* Schedule to implement the workaround in one second */
 515         (void)div_s64_rem(delta_ns, NSEC_PER_SEC, &remainder);
 516         if (remainder != 0)
 517                 schedule_delayed_work(&channel->adjtime_work, HZ);
 518
 519         return idt82p33_set_tod_trigger(channel, HW_TOD_TRIG_SEL_TOD_PPS, true);
 520 }
 521
 522 static void idt82p33_adjtime_workaround(struct work_struct *work)
 523 {
 524         struct idt82p33_channel *channel = container_of(work,
 525                                                         struct idt82p33_channel,
 526                                                         adjtime_work.work);
 527         struct idt82p33 *idt82p33 = channel->idt82p33;
 528
 529         mutex_lock(idt82p33->lock);
 530         /* Workaround for TOD-to-output alignment issue */
 531         _idt82p33_adjtime_internal_triggered(channel, 0);
 532         mutex_unlock(idt82p33->lock);
 533 }
 534
 535 static int _idt82p33_adjfine(struct idt82p33_channel *channel, long scaled_ppm)
 536 {
 537         struct idt82p33 *idt82p33 = channel->idt82p33;
 538         unsigned char buf[5] = {0};
 539         int err, i;
 540         s64 fcw;
 541
 542         /*
 543          * Frequency Control Word unit is: 1.6861512 * 10^-10 ppm
 544          *
 545          * adjfreq:
 546          *       ppb * 10^14
 547          * FCW = -----------
 548          *         16861512
 549          *
 550          * adjfine:
 551          *       scaled_ppm * 5^12 * 10^5
 552          * FCW = ------------------------
 553          *            16861512 * 2^4
 554          */
 555
 556         fcw = scaled_ppm * 762939453125ULL;
 557         fcw = div_s64(fcw, 8430756LL);
 558
 559         for (i = 0; i < 5; i++) {
 560                 buf[i] = fcw & 0xff;
 561                 fcw >>= 8;
 562         }
 563
 564         err = idt82p33_dpll_set_mode(channel, PLL_MODE_DCO);
 565
 566         if (err)
 567                 return err;
 568
 569         err = idt82p33_write(idt82p33, channel->dpll_freq_cnfg,
 570                              buf, sizeof(buf));
 571
 572         return err;
 573 }
 574
 575 /* ppb = scaled_ppm * 125 / 2^13 */
 576 static s32 idt82p33_ddco_scaled_ppm(long current_ppm, s32 ddco_ppb)
 577 {
 578         s64 scaled_ppm = div_s64(((s64)ddco_ppb << 13), 125);
 579         s64 max_scaled_ppm = div_s64(((s64)DCO_MAX_PPB << 13), 125);
 580
 581         current_ppm += scaled_ppm;
 582
 583         if (current_ppm > max_scaled_ppm)
 584                 current_ppm = max_scaled_ppm;
 585         else if (current_ppm < -max_scaled_ppm)
 586                 current_ppm = -max_scaled_ppm;
 587
 588         return (s32)current_ppm;
 589 }
 590
 591 static int idt82p33_stop_ddco(struct idt82p33_channel *channel)
 592 {
 593         int err;
 594
 595         err = _idt82p33_adjfine(channel, channel->current_freq);
 596         if (err)
 597                 return err;
 598
 599         channel->ddco = false;
 600
 601         return 0;
 602 }
 603
 604 static int idt82p33_start_ddco(struct idt82p33_channel *channel, s32 delta_ns)
 605 {
 606         s32 current_ppm = channel->current_freq;
 607         u32 duration_ms = MSEC_PER_SEC;
 608         s32 ppb;
 609         int err;
 610
 611         /* If the ToD correction is less than 5 nanoseconds, then skip it.
 612          * The error introduced by the ToD adjustment procedure would be bigger
 613          * than the required ToD correction
 614          */
 615         if (abs(delta_ns) < DDCO_THRESHOLD_NS)
 616                 return 0;
 617
 618         /* For most cases, keep ddco duration 1 second */
 619         ppb = delta_ns;
 620         while (abs(ppb) > DCO_MAX_PPB) {
 621                 duration_ms *= 2;
 622                 ppb /= 2;
 623         }
 624
 625         err = _idt82p33_adjfine(channel,
 626                                 idt82p33_ddco_scaled_ppm(current_ppm, ppb));
 627         if (err)
 628                 return err;
 629
 630         /* schedule the worker to cancel ddco */
 631         ptp_schedule_worker(channel->ptp_clock,
 632                             msecs_to_jiffies(duration_ms) - 1);
 633         channel->ddco = true;
 634
 635         return 0;
 636 }
 637
 638 static int idt82p33_measure_one_byte_write_overhead(
 639                 struct idt82p33_channel *channel, s64 *overhead_ns)
 640 {
 641         struct idt82p33 *idt82p33 = channel->idt82p33;
 642         ktime_t start, stop;
 643         u8 trigger = 0;
 644         s64 total_ns;
 645         int err;
 646         u8 i;
 647
 648         total_ns = 0;
 649         *overhead_ns = 0;
 650
 651         for (i = 0; i < MAX_MEASURMENT_COUNT; i++) {
 652
 653                 start = ktime_get_raw();
 654
 655                 err = idt82p33_write(idt82p33, channel->dpll_tod_trigger,
 656                                      &trigger, sizeof(trigger));
 657
 658                 stop = ktime_get_raw();
 659
 660                 if (err)
 661                         return err;
 662
 663                 total_ns += ktime_to_ns(stop) - ktime_to_ns(start);
 664         }
 665
 666         *overhead_ns = div_s64(total_ns, MAX_MEASURMENT_COUNT);
 667
 668         return err;
 669 }
 670
 671 static int idt82p33_measure_one_byte_read_overhead(
 672                 struct idt82p33_channel *channel, s64 *overhead_ns)
 673 {
 674         struct idt82p33 *idt82p33 = channel->idt82p33;
 675         ktime_t start, stop;
 676         u8 trigger = 0;
 677         s64 total_ns;
 678         int err;
 679         u8 i;
 680
 681         total_ns = 0;
 682         *overhead_ns = 0;
 683
 684         for (i = 0; i < MAX_MEASURMENT_COUNT; i++) {
 685
 686                 start = ktime_get_raw();
 687
 688                 err = idt82p33_read(idt82p33, channel->dpll_tod_trigger,
 689                                     &trigger, sizeof(trigger));
 690
 691                 stop = ktime_get_raw();
 692
 693                 if (err)
 694                         return err;
 695
 696                 total_ns += ktime_to_ns(stop) - ktime_to_ns(start);
 697         }
 698
 699         *overhead_ns = div_s64(total_ns, MAX_MEASURMENT_COUNT);
 700
 701         return err;
 702 }
 703
 704 static int idt82p33_measure_tod_write_9_byte_overhead(
 705                 struct idt82p33_channel *channel)
 706 {
 707         struct idt82p33 *idt82p33 = channel->idt82p33;
 708         u8 buf[TOD_BYTE_COUNT];
 709         ktime_t start, stop;
 710         s64 total_ns;
 711         int err = 0;
 712         u8 i, j;
 713
 714         total_ns = 0;
 715         idt82p33->tod_write_overhead_ns = 0;
 716
 717         for (i = 0; i < MAX_MEASURMENT_COUNT; i++) {
 718
 719                 start = ktime_get_raw();
 720
 721                 /* Need one less byte for applicable overhead */
 722                 for (j = 0; j < (TOD_BYTE_COUNT - 1); j++) {
 723                         err = idt82p33_write(idt82p33,
 724                                              channel->dpll_tod_cnfg + i,
 725                                              &buf[i], sizeof(buf[i]));
 726                         if (err)
 727                                 return err;
 728                 }
 729
 730                 stop = ktime_get_raw();
 731
 732                 total_ns += ktime_to_ns(stop) - ktime_to_ns(start);
 733         }
 734
 735         idt82p33->tod_write_overhead_ns = div_s64(total_ns,
 736                                                   MAX_MEASURMENT_COUNT);
 737
 738         return err;
 739 }
 740
 741 static int idt82p33_measure_settime_gettime_gap_overhead(
 742                 struct idt82p33_channel *channel, s64 *overhead_ns)
 743 {
 744         struct timespec64 ts1 = {0, 0};
 745         struct timespec64 ts2;
 746         int err;
 747
 748         *overhead_ns = 0;
 749
 750         err = _idt82p33_settime(channel, &ts1);
 751
 752         if (err)
 753                 return err;
 754
 755         err = _idt82p33_gettime(channel, &ts2);
 756
 757         if (!err)
 758                 *overhead_ns = timespec64_to_ns(&ts2) - timespec64_to_ns(&ts1);
 759
 760         return err;
 761 }
 762
 763 static int idt82p33_measure_tod_write_overhead(struct idt82p33_channel *channel)
 764 {
 765         s64 trailing_overhead_ns, one_byte_write_ns, gap_ns, one_byte_read_ns;
 766         struct idt82p33 *idt82p33 = channel->idt82p33;
 767         int err;
 768
 769         idt82p33->tod_write_overhead_ns = 0;
 770
 771         err = idt82p33_measure_settime_gettime_gap_overhead(channel, &gap_ns);
 772
 773         if (err) {
 774                 dev_err(idt82p33->dev,
 775                         "Failed in %s with err %d!\n", __func__, err);
 776                 return err;
 777         }
 778
 779         err = idt82p33_measure_one_byte_write_overhead(channel,
 780                                                        &one_byte_write_ns);
 781
 782         if (err)
 783                 return err;
 784
 785         err = idt82p33_measure_one_byte_read_overhead(channel,
 786                                                       &one_byte_read_ns);
 787
 788         if (err)
 789                 return err;
 790
 791         err = idt82p33_measure_tod_write_9_byte_overhead(channel);
 792
 793         if (err)
 794                 return err;
 795
 796         trailing_overhead_ns = gap_ns - 2 * one_byte_write_ns
 797                                - one_byte_read_ns;
 798
 799         idt82p33->tod_write_overhead_ns -= trailing_overhead_ns;
 800
 801         return err;
 802 }
 803
 804 static int idt82p33_check_and_set_masks(struct idt82p33 *idt82p33,
 805                                         u8 page,
 806                                         u8 offset,
 807                                         u8 val)
 808 {
 809         int err = 0;
 810
 811         if (page == PLLMASK_ADDR_HI && offset == PLLMASK_ADDR_LO) {
 812                 if ((val & 0xfc) || !(val & 0x3)) {
 813                         dev_err(idt82p33->dev,
 814                                 "Invalid PLL mask 0x%x\n", val);
 815                         err = -EINVAL;
 816                 } else {
 817                         idt82p33->pll_mask = val;
 818                 }
 819         } else if (page == PLL0_OUTMASK_ADDR_HI &&
 820                 offset == PLL0_OUTMASK_ADDR_LO) {
 821                 idt82p33->channel[0].output_mask = val;
 822         } else if (page == PLL1_OUTMASK_ADDR_HI &&
 823                 offset == PLL1_OUTMASK_ADDR_LO) {
 824                 idt82p33->channel[1].output_mask = val;
 825         }
 826
 827         return err;
 828 }
 829
 830 static void idt82p33_display_masks(struct idt82p33 *idt82p33)
 831 {
 832         u8 mask, i;
 833
 834         dev_info(idt82p33->dev,
 835                  "pllmask = 0x%02x\n", idt82p33->pll_mask);
 836
 837         for (i = 0; i < MAX_PHC_PLL; i++) {
 838                 mask = 1 << i;
 839
 840                 if (mask & idt82p33->pll_mask)
 841                         dev_info(idt82p33->dev,
 842                                  "PLL%d output_mask = 0x%04x\n",
 843                                  i, idt82p33->channel[i].output_mask);
 844         }
 845 }
 846
 847 static int idt82p33_sync_tod(struct idt82p33_channel *channel, bool enable)
 848 {
 849         struct idt82p33 *idt82p33 = channel->idt82p33;
 850         u8 sync_cnfg;
 851         int err;
 852
 853         err = idt82p33_read(idt82p33, channel->dpll_sync_cnfg,
 854                             &sync_cnfg, sizeof(sync_cnfg));
 855         if (err)
 856                 return err;
 857
 858         sync_cnfg &= ~SYNC_TOD;
 859         if (enable)
 860                 sync_cnfg |= SYNC_TOD;
 861
 862         return idt82p33_write(idt82p33, channel->dpll_sync_cnfg,
 863                               &sync_cnfg, sizeof(sync_cnfg));
 864 }
 865
 866 static long idt82p33_work_handler(struct ptp_clock_info *ptp)
 867 {
 868         struct idt82p33_channel *channel =
 869                         container_of(ptp, struct idt82p33_channel, caps);
 870         struct idt82p33 *idt82p33 = channel->idt82p33;
 871
 872         mutex_lock(idt82p33->lock);
 873         (void)idt82p33_stop_ddco(channel);
 874         mutex_unlock(idt82p33->lock);
 875
 876         /* Return a negative value here to not reschedule */
 877         return -1;
 878 }
 879
 880 static int idt82p33_output_enable(struct idt82p33_channel *channel,
 881                                   bool enable, unsigned int outn)
 882 {
 883         struct idt82p33 *idt82p33 = channel->idt82p33;
 884         int err;
 885         u8 val;
 886
 887         err = idt82p33_read(idt82p33, OUT_MUX_CNFG(outn), &val, sizeof(val));
 888         if (err)
 889                 return err;
 890         if (enable)
 891                 val &= ~SQUELCH_ENABLE;
 892         else
 893                 val |= SQUELCH_ENABLE;
 894
 895         return idt82p33_write(idt82p33, OUT_MUX_CNFG(outn), &val, sizeof(val));
 896 }
 897
 898 static int idt82p33_perout_enable(struct idt82p33_channel *channel,
 899                                   bool enable,
 900                                   struct ptp_perout_request *perout)
 901 {
 902         /* Enable/disable individual output instead */
 903         return idt82p33_output_enable(channel, enable, perout->index);
 904 }
 905
 906 static int idt82p33_enable_tod(struct idt82p33_channel *channel)
 907 {
 908         struct idt82p33 *idt82p33 = channel->idt82p33;
 909         struct timespec64 ts = {0, 0};
 910         int err;
 911
 912         err = idt82p33_measure_tod_write_overhead(channel);
 913
 914         if (err) {
 915                 dev_err(idt82p33->dev,
 916                         "Failed in %s with err %d!\n", __func__, err);
 917                 return err;
 918         }
 919
 920         err = _idt82p33_settime(channel, &ts);
 921
 922         if (err)
 923                 return err;
 924
 925         return idt82p33_sync_tod(channel, true);
 926 }
 927
 928 static void idt82p33_ptp_clock_unregister_all(struct idt82p33 *idt82p33)
 929 {
 930         struct idt82p33_channel *channel;
 931         u8 i;
 932
 933         for (i = 0; i < MAX_PHC_PLL; i++) {
 934                 channel = &idt82p33->channel[i];
 935                 cancel_delayed_work_sync(&channel->adjtime_work);
 936                 if (channel->ptp_clock)
 937                         ptp_clock_unregister(channel->ptp_clock);
 938         }
 939 }
 940
 941
 942
 943 static int idt82p33_enable(struct ptp_clock_info *ptp,
 944                            struct ptp_clock_request *rq, int on)
 945 {
 946         struct idt82p33_channel *channel =
 947                         container_of(ptp, struct idt82p33_channel, caps);
 948         struct idt82p33 *idt82p33 = channel->idt82p33;
 949         int err = -EOPNOTSUPP;
 950
 951         mutex_lock(idt82p33->lock);
 952
 953         switch (rq->type) {
 954         case PTP_CLK_REQ_PEROUT:
 955                 if (!on)
 956                         err = idt82p33_perout_enable(channel, false,
 957                                                      &rq->perout);
 958                 /* Only accept a 1-PPS aligned to the second. */
 959                 else if (rq->perout.start.nsec || rq->perout.period.sec != 1 ||
 960                          rq->perout.period.nsec)
 961                         err = -ERANGE;
 962                 else
 963                         err = idt82p33_perout_enable(channel, true,
 964                                                      &rq->perout);
 965                 break;
 966         case PTP_CLK_REQ_EXTTS:
 967                 err = idt82p33_extts_enable(channel, rq, on);
 968                 break;
 969         default:
 970                 break;
 971         }
 972
 973         mutex_unlock(idt82p33->lock);
 974
 975         if (err)
 976                 dev_err(idt82p33->dev,
 977                         "Failed in %s with err %d!\n", __func__, err);
 978         return err;
 979 }
 980
 981 static s32 idt82p33_getmaxphase(__always_unused struct ptp_clock_info *ptp)
 982 {
 983         return WRITE_PHASE_OFFSET_LIMIT;
 984 }
 985
 986 static int idt82p33_adjwritephase(struct ptp_clock_info *ptp, s32 offset_ns)
 987 {
 988         struct idt82p33_channel *channel =
 989                 container_of(ptp, struct idt82p33_channel, caps);
 990         struct idt82p33 *idt82p33 = channel->idt82p33;
 991         s64 offset_regval;
 992         u8 val[4] = {0};
 993         int err;
 994
 995         /* Convert from phaseoffset_fs to register value */
 996         offset_regval = div_s64((s64)(-offset_ns) * 1000000000ll,
 997                                 IDT_T0DPLL_PHASE_RESOL);
 998
 999         val[0] = offset_regval & 0xFF;
1000         val[1] = (offset_regval >> 8) & 0xFF;
1001         val[2] = (offset_regval >> 16) & 0xFF;
1002         val[3] = (offset_regval >> 24) & 0x1F;
1003         val[3] |= PH_OFFSET_EN;
1004
1005         mutex_lock(idt82p33->lock);
1006
1007         err = idt82p33_dpll_set_mode(channel, PLL_MODE_WPH);
1008         if (err) {
1009                 dev_err(idt82p33->dev,
1010                         "Failed in %s with err %d!\n", __func__, err);
1011                 goto out;
1012         }
1013
1014         err = idt82p33_write(idt82p33, channel->dpll_phase_cnfg, val,
1015                              sizeof(val));
1016
1017 out:
1018         mutex_unlock(idt82p33->lock);
1019         return err;
1020 }
1021
1022 static int idt82p33_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
1023 {
1024         struct idt82p33_channel *channel =
1025                         container_of(ptp, struct idt82p33_channel, caps);
1026         struct idt82p33 *idt82p33 = channel->idt82p33;
1027         int err;
1028
1029         if (channel->ddco == true)
1030                 return 0;
1031
1032         if (scaled_ppm == channel->current_freq)
1033                 return 0;
1034
1035         mutex_lock(idt82p33->lock);
1036         err = _idt82p33_adjfine(channel, scaled_ppm);
1037
1038         if (err == 0)
1039                 channel->current_freq = scaled_ppm;
1040         mutex_unlock(idt82p33->lock);
1041
1042         if (err)
1043                 dev_err(idt82p33->dev,
1044                         "Failed in %s with err %d!\n", __func__, err);
1045         return err;
1046 }
1047
1048 static int idt82p33_adjtime(struct ptp_clock_info *ptp, s64 delta_ns)
1049 {
1050         struct idt82p33_channel *channel =
1051                         container_of(ptp, struct idt82p33_channel, caps);
1052         struct idt82p33 *idt82p33 = channel->idt82p33;
1053         int err;
1054
1055         if (channel->ddco == true)
1056                 return -EBUSY;
1057
1058         mutex_lock(idt82p33->lock);
1059
1060         if (abs(delta_ns) < phase_snap_threshold) {
1061                 err = idt82p33_start_ddco(channel, delta_ns);
1062                 mutex_unlock(idt82p33->lock);
1063                 return err;
1064         }
1065
1066         /* Use more accurate internal 1pps triggered write first */
1067         err = _idt82p33_adjtime_internal_triggered(channel, delta_ns);
1068         if (err && delta_ns > IMMEDIATE_SNAP_THRESHOLD_NS)
1069                 err = _idt82p33_adjtime_immediate(channel, delta_ns);
1070
1071         mutex_unlock(idt82p33->lock);
1072
1073         if (err)
1074                 dev_err(idt82p33->dev,
1075                         "Failed in %s with err %d!\n", __func__, err);
1076         return err;
1077 }
1078
1079 static int idt82p33_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
1080 {
1081         struct idt82p33_channel *channel =
1082                         container_of(ptp, struct idt82p33_channel, caps);
1083         struct idt82p33 *idt82p33 = channel->idt82p33;
1084         int err;
1085
1086         mutex_lock(idt82p33->lock);
1087         err = _idt82p33_gettime(channel, ts);
1088         mutex_unlock(idt82p33->lock);
1089
1090         if (err)
1091                 dev_err(idt82p33->dev,
1092                         "Failed in %s with err %d!\n", __func__, err);
1093         return err;
1094 }
1095
1096 static int idt82p33_settime(struct ptp_clock_info *ptp,
1097                             const struct timespec64 *ts)
1098 {
1099         struct idt82p33_channel *channel =
1100                         container_of(ptp, struct idt82p33_channel, caps);
1101         struct idt82p33 *idt82p33 = channel->idt82p33;
1102         int err;
1103
1104         mutex_lock(idt82p33->lock);
1105         err = _idt82p33_settime(channel, ts);
1106         mutex_unlock(idt82p33->lock);
1107
1108         if (err)
1109                 dev_err(idt82p33->dev,
1110                         "Failed in %s with err %d!\n", __func__, err);
1111         return err;
1112 }
1113
1114 static int idt82p33_channel_init(struct idt82p33 *idt82p33, u32 index)
1115 {
1116         struct idt82p33_channel *channel = &idt82p33->channel[index];
1117
1118         switch (index) {
1119         case 0:
1120                 channel->dpll_tod_cnfg = DPLL1_TOD_CNFG;
1121                 channel->dpll_tod_trigger = DPLL1_TOD_TRIGGER;
1122                 channel->dpll_tod_sts = DPLL1_TOD_STS;
1123                 channel->dpll_mode_cnfg = DPLL1_OPERATING_MODE_CNFG;
1124                 channel->dpll_freq_cnfg = DPLL1_HOLDOVER_FREQ_CNFG;
1125                 channel->dpll_phase_cnfg = DPLL1_PHASE_OFFSET_CNFG;
1126                 channel->dpll_sync_cnfg = DPLL1_SYNC_EDGE_CNFG;
1127                 channel->dpll_input_mode_cnfg = DPLL1_INPUT_MODE_CNFG;
1128                 break;
1129         case 1:
1130                 channel->dpll_tod_cnfg = DPLL2_TOD_CNFG;
1131                 channel->dpll_tod_trigger = DPLL2_TOD_TRIGGER;
1132                 channel->dpll_tod_sts = DPLL2_TOD_STS;
1133                 channel->dpll_mode_cnfg = DPLL2_OPERATING_MODE_CNFG;
1134                 channel->dpll_freq_cnfg = DPLL2_HOLDOVER_FREQ_CNFG;
1135                 channel->dpll_phase_cnfg = DPLL2_PHASE_OFFSET_CNFG;
1136                 channel->dpll_sync_cnfg = DPLL2_SYNC_EDGE_CNFG;
1137                 channel->dpll_input_mode_cnfg = DPLL2_INPUT_MODE_CNFG;
1138                 break;
1139         default:
1140                 return -EINVAL;
1141         }
1142
1143         channel->plln = index;
1144         channel->current_freq = 0;
1145         channel->idt82p33 = idt82p33;
1146         INIT_DELAYED_WORK(&channel->adjtime_work, idt82p33_adjtime_workaround);
1147
1148         return 0;
1149 }
1150
1151 static int idt82p33_verify_pin(struct ptp_clock_info *ptp, unsigned int pin,
1152                                enum ptp_pin_function func, unsigned int chan)
1153 {
1154         switch (func) {
1155         case PTP_PF_NONE:
1156         case PTP_PF_EXTTS:
1157                 break;
1158         case PTP_PF_PEROUT:
1159         case PTP_PF_PHYSYNC:
1160                 return -1;
1161         }
1162         return 0;
1163 }
1164
1165 static void idt82p33_caps_init(u32 index, struct ptp_clock_info *caps,
1166                                struct ptp_pin_desc *pin_cfg, u8 max_pins)
1167 {
1168         struct ptp_pin_desc *ppd;
1169         int i;
1170
1171         caps->owner = THIS_MODULE;
1172         caps->max_adj = DCO_MAX_PPB;
1173         caps->n_per_out = MAX_PER_OUT;
1174         caps->n_ext_ts = MAX_PHC_PLL,
1175         caps->n_pins = max_pins,
1176         caps->adjphase = idt82p33_adjwritephase,
1177         caps->getmaxphase = idt82p33_getmaxphase,
1178         caps->adjfine = idt82p33_adjfine;
1179         caps->adjtime = idt82p33_adjtime;
1180         caps->gettime64 = idt82p33_gettime;
1181         caps->settime64 = idt82p33_settime;
1182         caps->enable = idt82p33_enable;
1183         caps->verify = idt82p33_verify_pin;
1184         caps->do_aux_work = idt82p33_work_handler;
1185
1186         snprintf(caps->name, sizeof(caps->name), "IDT 82P33 PLL%u", index);
1187
1188         caps->pin_config = pin_cfg;
1189
1190         for (i = 0; i < max_pins; ++i) {
1191                 ppd = &pin_cfg[i];
1192
1193                 ppd->index = i;
1194                 ppd->func = PTP_PF_NONE;
1195                 ppd->chan = index;
1196                 snprintf(ppd->name, sizeof(ppd->name), "in%d", 12 + i);
1197         }
1198 }
1199
1200 static int idt82p33_enable_channel(struct idt82p33 *idt82p33, u32 index)
1201 {
1202         struct idt82p33_channel *channel;
1203         int err;
1204
1205         if (!(index < MAX_PHC_PLL))
1206                 return -EINVAL;
1207
1208         channel = &idt82p33->channel[index];
1209
1210         err = idt82p33_channel_init(idt82p33, index);
1211         if (err) {
1212                 dev_err(idt82p33->dev,
1213                         "Channel_init failed in %s with err %d!\n",
1214                         __func__, err);
1215                 return err;
1216         }
1217
1218         idt82p33_caps_init(index, &channel->caps,
1219                            pin_config[index], MAX_TRIG_CLK);
1220
1221         channel->ptp_clock = ptp_clock_register(&channel->caps, NULL);
1222
1223         if (IS_ERR(channel->ptp_clock)) {
1224                 err = PTR_ERR(channel->ptp_clock);
1225                 channel->ptp_clock = NULL;
1226                 return err;
1227         }
1228
1229         if (!channel->ptp_clock)
1230                 return -ENOTSUPP;
1231
1232         err = idt82p33_dpll_set_mode(channel, PLL_MODE_DCO);
1233         if (err) {
1234                 dev_err(idt82p33->dev,
1235                         "Dpll_set_mode failed in %s with err %d!\n",
1236                         __func__, err);
1237                 return err;
1238         }
1239
1240         err = idt82p33_enable_tod(channel);
1241         if (err) {
1242                 dev_err(idt82p33->dev,
1243                         "Enable_tod failed in %s with err %d!\n",
1244                         __func__, err);
1245                 return err;
1246         }
1247
1248         dev_info(idt82p33->dev, "PLL%d registered as ptp%d\n",
1249                  index, channel->ptp_clock->index);
1250
1251         return 0;
1252 }
1253
1254 static int idt82p33_reset(struct idt82p33 *idt82p33, bool cold)
1255 {
1256         int err;
1257         u8 cfg = SOFT_RESET_EN;
1258
1259         if (cold == true)
1260                 goto cold_reset;
1261
1262         err = idt82p33_read(idt82p33, REG_SOFT_RESET, &cfg, sizeof(cfg));
1263         if (err) {
1264                 dev_err(idt82p33->dev,
1265                         "Soft reset failed with err %d!\n", err);
1266                 return err;
1267         }
1268
1269         cfg |= SOFT_RESET_EN;
1270
1271 cold_reset:
1272         err = idt82p33_write(idt82p33, REG_SOFT_RESET, &cfg, sizeof(cfg));
1273         if (err)
1274                 dev_err(idt82p33->dev,
1275                         "Cold reset failed with err %d!\n", err);
1276         return err;
1277 }
1278
1279 static int idt82p33_load_firmware(struct idt82p33 *idt82p33)
1280 {
1281         char fname[128] = FW_FILENAME;
1282         const struct firmware *fw;
1283         struct idt82p33_fwrc *rec;
1284         u8 loaddr, page, val;
1285         int err;
1286         s32 len;
1287
1288         if (firmware) /* module parameter */
1289                 snprintf(fname, sizeof(fname), "%s", firmware);
1290
1291         dev_info(idt82p33->dev, "requesting firmware '%s'\n", fname);
1292
1293         err = request_firmware(&fw, fname, idt82p33->dev);
1294
1295         if (err) {
1296                 dev_err(idt82p33->dev,
1297                         "Failed in %s with err %d!\n", __func__, err);
1298                 return err;
1299         }
1300
1301         dev_dbg(idt82p33->dev, "firmware size %zu bytes\n", fw->size);
1302
1303         rec = (struct idt82p33_fwrc *) fw->data;
1304
1305         for (len = fw->size; len > 0; len -= sizeof(*rec)) {
1306
1307                 if (rec->reserved) {
1308                         dev_err(idt82p33->dev,
1309                                 "bad firmware, reserved field non-zero\n");
1310                         err = -EINVAL;
1311                 } else {
1312                         val = rec->value;
1313                         loaddr = rec->loaddr;
1314                         page = rec->hiaddr;
1315
1316                         rec++;
1317
1318                         err = idt82p33_check_and_set_masks(idt82p33, page,
1319                                                            loaddr, val);
1320                 }
1321
1322                 if (err == 0) {
1323                         /* Page size 128, last 4 bytes of page skipped */
1324                         if (loaddr > 0x7b)
1325                                 continue;
1326
1327                         err = idt82p33_write(idt82p33, REG_ADDR(page, loaddr),
1328                                              &val, sizeof(val));
1329                 }
1330
1331                 if (err)
1332                         goto out;
1333         }
1334
1335         idt82p33_display_masks(idt82p33);
1336 out:
1337         release_firmware(fw);
1338         return err;
1339 }
1340
1341 static void idt82p33_extts_check(struct work_struct *work)
1342 {
1343         struct idt82p33 *idt82p33 = container_of(work, struct idt82p33,
1344                                                  extts_work.work);
1345         struct idt82p33_channel *channel;
1346         int err;
1347         u8 mask;
1348         int i;
1349
1350         if (idt82p33->extts_mask == 0)
1351                 return;
1352
1353         mutex_lock(idt82p33->lock);
1354
1355         for (i = 0; i < MAX_PHC_PLL; i++) {
1356                 mask = 1 << i;
1357
1358                 if ((idt82p33->extts_mask & mask) == 0)
1359                         continue;
1360
1361                 err = idt82p33_extts_check_channel(idt82p33, i);
1362
1363                 if (err == 0) {
1364                         /* trigger clears itself, so clear the mask */
1365                         if (idt82p33->extts_single_shot) {
1366                                 idt82p33->extts_mask &= ~mask;
1367                         } else {
1368                                 /* Re-arm */
1369                                 channel = &idt82p33->channel[i];
1370                                 arm_tod_read_with_trigger(channel, channel->tod_trigger);
1371                         }
1372                 }
1373         }
1374
1375         if (idt82p33->extts_mask)
1376                 schedule_delayed_work(&idt82p33->extts_work,
1377                                       msecs_to_jiffies(EXTTS_PERIOD_MS));
1378
1379         mutex_unlock(idt82p33->lock);
1380 }
1381
1382 static int idt82p33_probe(struct platform_device *pdev)
1383 {
1384         struct rsmu_ddata *ddata = dev_get_drvdata(pdev->dev.parent);
1385         struct idt82p33 *idt82p33;
1386         int err;
1387         u8 i;
1388
1389         idt82p33 = devm_kzalloc(&pdev->dev,
1390                                 sizeof(struct idt82p33), GFP_KERNEL);
1391         if (!idt82p33)
1392                 return -ENOMEM;
1393
1394         idt82p33->dev = &pdev->dev;
1395         idt82p33->mfd = pdev->dev.parent;
1396         idt82p33->lock = &ddata->lock;
1397         idt82p33->regmap = ddata->regmap;
1398         idt82p33->tod_write_overhead_ns = 0;
1399         idt82p33->calculate_overhead_flag = 0;
1400         idt82p33->pll_mask = DEFAULT_PLL_MASK;
1401         idt82p33->channel[0].output_mask = DEFAULT_OUTPUT_MASK_PLL0;
1402         idt82p33->channel[1].output_mask = DEFAULT_OUTPUT_MASK_PLL1;
1403         idt82p33->extts_mask = 0;
1404         INIT_DELAYED_WORK(&idt82p33->extts_work, idt82p33_extts_check);
1405
1406         mutex_lock(idt82p33->lock);
1407
1408         /* cold reset before loading firmware */
1409         idt82p33_reset(idt82p33, true);
1410
1411         err = idt82p33_load_firmware(idt82p33);
1412         if (err)
1413                 dev_warn(idt82p33->dev,
1414                          "loading firmware failed with %d\n", err);
1415
1416         /* soft reset after loading firmware */
1417         idt82p33_reset(idt82p33, false);
1418
1419         if (idt82p33->pll_mask) {
1420                 for (i = 0; i < MAX_PHC_PLL; i++) {
1421                         if (idt82p33->pll_mask & (1 << i))
1422                                 err = idt82p33_enable_channel(idt82p33, i);
1423                         else
1424                                 err = idt82p33_channel_init(idt82p33, i);
1425                         if (err) {
1426                                 dev_err(idt82p33->dev,
1427                                         "Failed in %s with err %d!\n",
1428                                         __func__, err);
1429                                 break;
1430                         }
1431                 }
1432         } else {
1433                 dev_err(idt82p33->dev,
1434                         "no PLLs flagged as PHCs, nothing to do\n");
1435                 err = -ENODEV;
1436         }
1437
1438         mutex_unlock(idt82p33->lock);
1439
1440         if (err) {
1441                 idt82p33_ptp_clock_unregister_all(idt82p33);
1442                 return err;
1443         }
1444
1445         platform_set_drvdata(pdev, idt82p33);
1446
1447         return 0;
1448 }
1449
1450 static int idt82p33_remove(struct platform_device *pdev)
1451 {
1452         struct idt82p33 *idt82p33 = platform_get_drvdata(pdev);
1453
1454         cancel_delayed_work_sync(&idt82p33->extts_work);
1455
1456         idt82p33_ptp_clock_unregister_all(idt82p33);
1457
1458         return 0;
1459 }
1460
1461 static struct platform_driver idt82p33_driver = {
1462         .driver = {
1463                 .name = "82p33x1x-phc",
1464         },
1465         .probe = idt82p33_probe,
1466         .remove = idt82p33_remove,
1467 };
1468
1469 module_platform_driver(idt82p33_driver);