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 /*(DEBLOBBED)*/
  29
  30 /* Module Parameters */
  31 static u32 phase_snap_threshold = SNAP_THRESHOLD_NS;
  32 module_param(phase_snap_threshold, uint, 0);
  33 MODULE_PARM_DESC(phase_snap_threshold,
  34 "threshold (10000ns by default) below which adjtime would use double dco");
  35
  36 static char *firmware;
  37 module_param(firmware, charp, 0);
  38
  39 static inline int idt82p33_read(struct idt82p33 *idt82p33, u16 regaddr,
  40                                 u8 *buf, u16 count)
  41 {
  42         return regmap_bulk_read(idt82p33->regmap, regaddr, buf, count);
  43 }
  44
  45 static inline int idt82p33_write(struct idt82p33 *idt82p33, u16 regaddr,
  46                                  u8 *buf, u16 count)
  47 {
  48         return regmap_bulk_write(idt82p33->regmap, regaddr, buf, count);
  49 }
  50
  51 static void idt82p33_byte_array_to_timespec(struct timespec64 *ts,
  52                                             u8 buf[TOD_BYTE_COUNT])
  53 {
  54         time64_t sec;
  55         s32 nsec;
  56         u8 i;
  57
  58         nsec = buf[3];
  59         for (i = 0; i < 3; i++) {
  60                 nsec <<= 8;
  61                 nsec |= buf[2 - i];
  62         }
  63
  64         sec = buf[9];
  65         for (i = 0; i < 5; i++) {
  66                 sec <<= 8;
  67                 sec |= buf[8 - i];
  68         }
  69
  70         ts->tv_sec = sec;
  71         ts->tv_nsec = nsec;
  72 }
  73
  74 static void idt82p33_timespec_to_byte_array(struct timespec64 const *ts,
  75                                             u8 buf[TOD_BYTE_COUNT])
  76 {
  77         time64_t sec;
  78         s32 nsec;
  79         u8 i;
  80
  81         nsec = ts->tv_nsec;
  82         sec = ts->tv_sec;
  83
  84         for (i = 0; i < 4; i++) {
  85                 buf[i] = nsec & 0xff;
  86                 nsec >>= 8;
  87         }
  88
  89         for (i = 4; i < TOD_BYTE_COUNT; i++) {
  90                 buf[i] = sec & 0xff;
  91                 sec >>= 8;
  92         }
  93 }
  94
  95 static int idt82p33_dpll_set_mode(struct idt82p33_channel *channel,
  96                                   enum pll_mode mode)
  97 {
  98         struct idt82p33 *idt82p33 = channel->idt82p33;
  99         u8 dpll_mode;
 100         int err;
 101
 102         if (channel->pll_mode == mode)
 103                 return 0;
 104
 105         err = idt82p33_read(idt82p33, channel->dpll_mode_cnfg,
 106                             &dpll_mode, sizeof(dpll_mode));
 107         if (err)
 108                 return err;
 109
 110         dpll_mode &= ~(PLL_MODE_MASK << PLL_MODE_SHIFT);
 111
 112         dpll_mode |= (mode << PLL_MODE_SHIFT);
 113
 114         err = idt82p33_write(idt82p33, channel->dpll_mode_cnfg,
 115                              &dpll_mode, sizeof(dpll_mode));
 116         if (err)
 117                 return err;
 118
 119         channel->pll_mode = mode;
 120
 121         return 0;
 122 }
 123
 124 static int _idt82p33_gettime(struct idt82p33_channel *channel,
 125                              struct timespec64 *ts)
 126 {
 127         struct idt82p33 *idt82p33 = channel->idt82p33;
 128         u8 buf[TOD_BYTE_COUNT];
 129         u8 trigger;
 130         int err;
 131
 132         trigger = TOD_TRIGGER(HW_TOD_WR_TRIG_SEL_MSB_TOD_CNFG,
 133                               HW_TOD_RD_TRIG_SEL_LSB_TOD_STS);
 134
 135
 136         err = idt82p33_write(idt82p33, channel->dpll_tod_trigger,
 137                              &trigger, sizeof(trigger));
 138
 139         if (err)
 140                 return err;
 141
 142         if (idt82p33->calculate_overhead_flag)
 143                 idt82p33->start_time = ktime_get_raw();
 144
 145         err = idt82p33_read(idt82p33, channel->dpll_tod_sts, buf, sizeof(buf));
 146
 147         if (err)
 148                 return err;
 149
 150         idt82p33_byte_array_to_timespec(ts, buf);
 151
 152         return 0;
 153 }
 154
 155 /*
 156  *   TOD Trigger:
 157  *   Bits[7:4] Write 0x9, MSB write
 158  *   Bits[3:0] Read 0x9, LSB read
 159  */
 160
 161 static int _idt82p33_settime(struct idt82p33_channel *channel,
 162                              struct timespec64 const *ts)
 163 {
 164         struct idt82p33 *idt82p33 = channel->idt82p33;
 165         struct timespec64 local_ts = *ts;
 166         char buf[TOD_BYTE_COUNT];
 167         s64 dynamic_overhead_ns;
 168         unsigned char trigger;
 169         int err;
 170         u8 i;
 171
 172         trigger = TOD_TRIGGER(HW_TOD_WR_TRIG_SEL_MSB_TOD_CNFG,
 173                               HW_TOD_RD_TRIG_SEL_LSB_TOD_STS);
 174
 175         err = idt82p33_write(idt82p33, channel->dpll_tod_trigger,
 176                         &trigger, sizeof(trigger));
 177
 178         if (err)
 179                 return err;
 180
 181         if (idt82p33->calculate_overhead_flag) {
 182                 dynamic_overhead_ns = ktime_to_ns(ktime_get_raw())
 183                                         - ktime_to_ns(idt82p33->start_time);
 184
 185                 timespec64_add_ns(&local_ts, dynamic_overhead_ns);
 186
 187                 idt82p33->calculate_overhead_flag = 0;
 188         }
 189
 190         idt82p33_timespec_to_byte_array(&local_ts, buf);
 191
 192         /*
 193          * Store the new time value.
 194          */
 195         for (i = 0; i < TOD_BYTE_COUNT; i++) {
 196                 err = idt82p33_write(idt82p33, channel->dpll_tod_cnfg + i,
 197                                      &buf[i], sizeof(buf[i]));
 198                 if (err)
 199                         return err;
 200         }
 201
 202         return err;
 203 }
 204
 205 static int _idt82p33_adjtime(struct idt82p33_channel *channel, s64 delta_ns)
 206 {
 207         struct idt82p33 *idt82p33 = channel->idt82p33;
 208         struct timespec64 ts;
 209         s64 now_ns;
 210         int err;
 211
 212         idt82p33->calculate_overhead_flag = 1;
 213
 214         err = _idt82p33_gettime(channel, &ts);
 215
 216         if (err)
 217                 return err;
 218
 219         now_ns = timespec64_to_ns(&ts);
 220         now_ns += delta_ns + idt82p33->tod_write_overhead_ns;
 221
 222         ts = ns_to_timespec64(now_ns);
 223
 224         err = _idt82p33_settime(channel, &ts);
 225
 226         return err;
 227 }
 228
 229 static int _idt82p33_adjfine(struct idt82p33_channel *channel, long scaled_ppm)
 230 {
 231         struct idt82p33 *idt82p33 = channel->idt82p33;
 232         unsigned char buf[5] = {0};
 233         int err, i;
 234         s64 fcw;
 235
 236         if (scaled_ppm == channel->current_freq_ppb)
 237                 return 0;
 238
 239         /*
 240          * Frequency Control Word unit is: 1.68 * 10^-10 ppm
 241          *
 242          * adjfreq:
 243          *       ppb * 10^9
 244          * FCW = ----------
 245          *          168
 246          *
 247          * adjfine:
 248          *       scaled_ppm * 5^12
 249          * FCW = -------------
 250          *         168 * 2^4
 251          */
 252
 253         fcw = scaled_ppm * 244140625ULL;
 254         fcw = div_s64(fcw, 2688);
 255
 256         for (i = 0; i < 5; i++) {
 257                 buf[i] = fcw & 0xff;
 258                 fcw >>= 8;
 259         }
 260
 261         err = idt82p33_dpll_set_mode(channel, PLL_MODE_DCO);
 262
 263         if (err)
 264                 return err;
 265
 266         err = idt82p33_write(idt82p33, channel->dpll_freq_cnfg,
 267                              buf, sizeof(buf));
 268
 269         if (err == 0)
 270                 channel->current_freq_ppb = scaled_ppm;
 271
 272         return err;
 273 }
 274
 275 static int idt82p33_measure_one_byte_write_overhead(
 276                 struct idt82p33_channel *channel, s64 *overhead_ns)
 277 {
 278         struct idt82p33 *idt82p33 = channel->idt82p33;
 279         ktime_t start, stop;
 280         s64 total_ns;
 281         u8 trigger;
 282         int err;
 283         u8 i;
 284
 285         total_ns = 0;
 286         *overhead_ns = 0;
 287         trigger = TOD_TRIGGER(HW_TOD_WR_TRIG_SEL_MSB_TOD_CNFG,
 288                               HW_TOD_RD_TRIG_SEL_LSB_TOD_STS);
 289
 290         for (i = 0; i < MAX_MEASURMENT_COUNT; i++) {
 291
 292                 start = ktime_get_raw();
 293
 294                 err = idt82p33_write(idt82p33, channel->dpll_tod_trigger,
 295                                      &trigger, sizeof(trigger));
 296
 297                 stop = ktime_get_raw();
 298
 299                 if (err)
 300                         return err;
 301
 302                 total_ns += ktime_to_ns(stop) - ktime_to_ns(start);
 303         }
 304
 305         *overhead_ns = div_s64(total_ns, MAX_MEASURMENT_COUNT);
 306
 307         return err;
 308 }
 309
 310 static int idt82p33_measure_tod_write_9_byte_overhead(
 311                         struct idt82p33_channel *channel)
 312 {
 313         struct idt82p33 *idt82p33 = channel->idt82p33;
 314         u8 buf[TOD_BYTE_COUNT];
 315         ktime_t start, stop;
 316         s64 total_ns;
 317         int err = 0;
 318         u8 i, j;
 319
 320         total_ns = 0;
 321         idt82p33->tod_write_overhead_ns = 0;
 322
 323         for (i = 0; i < MAX_MEASURMENT_COUNT; i++) {
 324
 325                 start = ktime_get_raw();
 326
 327                 /* Need one less byte for applicable overhead */
 328                 for (j = 0; j < (TOD_BYTE_COUNT - 1); j++) {
 329                         err = idt82p33_write(idt82p33,
 330                                              channel->dpll_tod_cnfg + i,
 331                                              &buf[i], sizeof(buf[i]));
 332                         if (err)
 333                                 return err;
 334                 }
 335
 336                 stop = ktime_get_raw();
 337
 338                 total_ns += ktime_to_ns(stop) - ktime_to_ns(start);
 339         }
 340
 341         idt82p33->tod_write_overhead_ns = div_s64(total_ns,
 342                                                   MAX_MEASURMENT_COUNT);
 343
 344         return err;
 345 }
 346
 347 static int idt82p33_measure_settime_gettime_gap_overhead(
 348                 struct idt82p33_channel *channel, s64 *overhead_ns)
 349 {
 350         struct timespec64 ts1 = {0, 0};
 351         struct timespec64 ts2;
 352         int err;
 353
 354         *overhead_ns = 0;
 355
 356         err = _idt82p33_settime(channel, &ts1);
 357
 358         if (err)
 359                 return err;
 360
 361         err = _idt82p33_gettime(channel, &ts2);
 362
 363         if (!err)
 364                 *overhead_ns = timespec64_to_ns(&ts2) - timespec64_to_ns(&ts1);
 365
 366         return err;
 367 }
 368
 369 static int idt82p33_measure_tod_write_overhead(struct idt82p33_channel *channel)
 370 {
 371         s64 trailing_overhead_ns, one_byte_write_ns, gap_ns;
 372         struct idt82p33 *idt82p33 = channel->idt82p33;
 373         int err;
 374
 375         idt82p33->tod_write_overhead_ns = 0;
 376
 377         err = idt82p33_measure_settime_gettime_gap_overhead(channel, &gap_ns);
 378
 379         if (err) {
 380                 dev_err(idt82p33->dev,
 381                         "Failed in %s with err %d!\n", __func__, err);
 382                 return err;
 383         }
 384
 385         err = idt82p33_measure_one_byte_write_overhead(channel,
 386                                                        &one_byte_write_ns);
 387
 388         if (err)
 389                 return err;
 390
 391         err = idt82p33_measure_tod_write_9_byte_overhead(channel);
 392
 393         if (err)
 394                 return err;
 395
 396         trailing_overhead_ns = gap_ns - (2 * one_byte_write_ns);
 397
 398         idt82p33->tod_write_overhead_ns -= trailing_overhead_ns;
 399
 400         return err;
 401 }
 402
 403 static int idt82p33_check_and_set_masks(struct idt82p33 *idt82p33,
 404                                         u8 page,
 405                                         u8 offset,
 406                                         u8 val)
 407 {
 408         int err = 0;
 409
 410         if (page == PLLMASK_ADDR_HI && offset == PLLMASK_ADDR_LO) {
 411                 if ((val & 0xfc) || !(val & 0x3)) {
 412                         dev_err(idt82p33->dev,
 413                                 "Invalid PLL mask 0x%x\n", val);
 414                         err = -EINVAL;
 415                 } else {
 416                         idt82p33->pll_mask = val;
 417                 }
 418         } else if (page == PLL0_OUTMASK_ADDR_HI &&
 419                 offset == PLL0_OUTMASK_ADDR_LO) {
 420                 idt82p33->channel[0].output_mask = val;
 421         } else if (page == PLL1_OUTMASK_ADDR_HI &&
 422                 offset == PLL1_OUTMASK_ADDR_LO) {
 423                 idt82p33->channel[1].output_mask = val;
 424         }
 425
 426         return err;
 427 }
 428
 429 static void idt82p33_display_masks(struct idt82p33 *idt82p33)
 430 {
 431         u8 mask, i;
 432
 433         dev_info(idt82p33->dev,
 434                  "pllmask = 0x%02x\n", idt82p33->pll_mask);
 435
 436         for (i = 0; i < MAX_PHC_PLL; i++) {
 437                 mask = 1 << i;
 438
 439                 if (mask & idt82p33->pll_mask)
 440                         dev_info(idt82p33->dev,
 441                                  "PLL%d output_mask = 0x%04x\n",
 442                                  i, idt82p33->channel[i].output_mask);
 443         }
 444 }
 445
 446 static int idt82p33_sync_tod(struct idt82p33_channel *channel, bool enable)
 447 {
 448         struct idt82p33 *idt82p33 = channel->idt82p33;
 449         u8 sync_cnfg;
 450         int err;
 451
 452         err = idt82p33_read(idt82p33, channel->dpll_sync_cnfg,
 453                             &sync_cnfg, sizeof(sync_cnfg));
 454         if (err)
 455                 return err;
 456
 457         sync_cnfg &= ~SYNC_TOD;
 458         if (enable)
 459                 sync_cnfg |= SYNC_TOD;
 460
 461         return idt82p33_write(idt82p33, channel->dpll_sync_cnfg,
 462                               &sync_cnfg, sizeof(sync_cnfg));
 463 }
 464
 465 static int idt82p33_output_enable(struct idt82p33_channel *channel,
 466                                   bool enable, unsigned int outn)
 467 {
 468         struct idt82p33 *idt82p33 = channel->idt82p33;
 469         int err;
 470         u8 val;
 471
 472         err = idt82p33_read(idt82p33, OUT_MUX_CNFG(outn), &val, sizeof(val));
 473         if (err)
 474                 return err;
 475         if (enable)
 476                 val &= ~SQUELCH_ENABLE;
 477         else
 478                 val |= SQUELCH_ENABLE;
 479
 480         return idt82p33_write(idt82p33, OUT_MUX_CNFG(outn), &val, sizeof(val));
 481 }
 482
 483 static int idt82p33_output_mask_enable(struct idt82p33_channel *channel,
 484                                        bool enable)
 485 {
 486         u16 mask;
 487         int err;
 488         u8 outn;
 489
 490         mask = channel->output_mask;
 491         outn = 0;
 492
 493         while (mask) {
 494                 if (mask & 0x1) {
 495                         err = idt82p33_output_enable(channel, enable, outn);
 496                         if (err)
 497                                 return err;
 498                 }
 499
 500                 mask >>= 0x1;
 501                 outn++;
 502         }
 503
 504         return 0;
 505 }
 506
 507 static int idt82p33_perout_enable(struct idt82p33_channel *channel,
 508                                   bool enable,
 509                                   struct ptp_perout_request *perout)
 510 {
 511         unsigned int flags = perout->flags;
 512
 513         /* Enable/disable output based on output_mask */
 514         if (flags == PEROUT_ENABLE_OUTPUT_MASK)
 515                 return idt82p33_output_mask_enable(channel, enable);
 516
 517         /* Enable/disable individual output instead */
 518         return idt82p33_output_enable(channel, enable, perout->index);
 519 }
 520
 521 static int idt82p33_enable_tod(struct idt82p33_channel *channel)
 522 {
 523         struct idt82p33 *idt82p33 = channel->idt82p33;
 524         struct timespec64 ts = {0, 0};
 525         int err;
 526
 527         err = idt82p33_measure_tod_write_overhead(channel);
 528
 529         if (err) {
 530                 dev_err(idt82p33->dev,
 531                         "Failed in %s with err %d!\n", __func__, err);
 532                 return err;
 533         }
 534
 535         err = _idt82p33_settime(channel, &ts);
 536
 537         if (err)
 538                 return err;
 539
 540         return idt82p33_sync_tod(channel, true);
 541 }
 542
 543 static void idt82p33_ptp_clock_unregister_all(struct idt82p33 *idt82p33)
 544 {
 545         struct idt82p33_channel *channel;
 546         u8 i;
 547
 548         for (i = 0; i < MAX_PHC_PLL; i++) {
 549
 550                 channel = &idt82p33->channel[i];
 551
 552                 if (channel->ptp_clock)
 553                         ptp_clock_unregister(channel->ptp_clock);
 554         }
 555 }
 556
 557 static int idt82p33_enable(struct ptp_clock_info *ptp,
 558                            struct ptp_clock_request *rq, int on)
 559 {
 560         struct idt82p33_channel *channel =
 561                         container_of(ptp, struct idt82p33_channel, caps);
 562         struct idt82p33 *idt82p33 = channel->idt82p33;
 563         int err = -EOPNOTSUPP;
 564
 565         mutex_lock(idt82p33->lock);
 566
 567         if (rq->type == PTP_CLK_REQ_PEROUT) {
 568                 if (!on)
 569                         err = idt82p33_perout_enable(channel, false,
 570                                                      &rq->perout);
 571                 /* Only accept a 1-PPS aligned to the second. */
 572                 else if (rq->perout.start.nsec || rq->perout.period.sec != 1 ||
 573                          rq->perout.period.nsec)
 574                         err = -ERANGE;
 575                 else
 576                         err = idt82p33_perout_enable(channel, true,
 577                                                      &rq->perout);
 578         }
 579
 580         mutex_unlock(idt82p33->lock);
 581
 582         if (err)
 583                 dev_err(idt82p33->dev,
 584                         "Failed in %s with err %d!\n", __func__, err);
 585         return err;
 586 }
 587
 588 static int idt82p33_adjwritephase(struct ptp_clock_info *ptp, s32 offset_ns)
 589 {
 590         struct idt82p33_channel *channel =
 591                 container_of(ptp, struct idt82p33_channel, caps);
 592         struct idt82p33 *idt82p33 = channel->idt82p33;
 593         s64 offset_regval, offset_fs;
 594         u8 val[4] = {0};
 595         int err;
 596
 597         offset_fs = (s64)(-offset_ns) * 1000000;
 598
 599         if (offset_fs > WRITE_PHASE_OFFSET_LIMIT)
 600                 offset_fs = WRITE_PHASE_OFFSET_LIMIT;
 601         else if (offset_fs < -WRITE_PHASE_OFFSET_LIMIT)
 602                 offset_fs = -WRITE_PHASE_OFFSET_LIMIT;
 603
 604         /* Convert from phaseoffset_fs to register value */
 605         offset_regval = div_s64(offset_fs * 1000, IDT_T0DPLL_PHASE_RESOL);
 606
 607         val[0] = offset_regval & 0xFF;
 608         val[1] = (offset_regval >> 8) & 0xFF;
 609         val[2] = (offset_regval >> 16) & 0xFF;
 610         val[3] = (offset_regval >> 24) & 0x1F;
 611         val[3] |= PH_OFFSET_EN;
 612
 613         mutex_lock(idt82p33->lock);
 614
 615         err = idt82p33_dpll_set_mode(channel, PLL_MODE_WPH);
 616         if (err) {
 617                 dev_err(idt82p33->dev,
 618                         "Failed in %s with err %d!\n", __func__, err);
 619                 goto out;
 620         }
 621
 622         err = idt82p33_write(idt82p33, channel->dpll_phase_cnfg, val,
 623                              sizeof(val));
 624
 625 out:
 626         mutex_unlock(idt82p33->lock);
 627         return err;
 628 }
 629
 630 static int idt82p33_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 631 {
 632         struct idt82p33_channel *channel =
 633                         container_of(ptp, struct idt82p33_channel, caps);
 634         struct idt82p33 *idt82p33 = channel->idt82p33;
 635         int err;
 636
 637         mutex_lock(idt82p33->lock);
 638         err = _idt82p33_adjfine(channel, scaled_ppm);
 639         mutex_unlock(idt82p33->lock);
 640         if (err)
 641                 dev_err(idt82p33->dev,
 642                         "Failed in %s with err %d!\n", __func__, err);
 643
 644         return err;
 645 }
 646
 647 static int idt82p33_adjtime(struct ptp_clock_info *ptp, s64 delta_ns)
 648 {
 649         struct idt82p33_channel *channel =
 650                         container_of(ptp, struct idt82p33_channel, caps);
 651         struct idt82p33 *idt82p33 = channel->idt82p33;
 652         int err;
 653
 654         mutex_lock(idt82p33->lock);
 655
 656         if (abs(delta_ns) < phase_snap_threshold) {
 657                 mutex_unlock(idt82p33->lock);
 658                 return 0;
 659         }
 660
 661         err = _idt82p33_adjtime(channel, delta_ns);
 662
 663         mutex_unlock(idt82p33->lock);
 664
 665         if (err)
 666                 dev_err(idt82p33->dev,
 667                         "Failed in %s with err %d!\n", __func__, err);
 668         return err;
 669 }
 670
 671 static int idt82p33_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 672 {
 673         struct idt82p33_channel *channel =
 674                         container_of(ptp, struct idt82p33_channel, caps);
 675         struct idt82p33 *idt82p33 = channel->idt82p33;
 676         int err;
 677
 678         mutex_lock(idt82p33->lock);
 679         err = _idt82p33_gettime(channel, ts);
 680         mutex_unlock(idt82p33->lock);
 681
 682         if (err)
 683                 dev_err(idt82p33->dev,
 684                         "Failed in %s with err %d!\n", __func__, err);
 685         return err;
 686 }
 687
 688 static int idt82p33_settime(struct ptp_clock_info *ptp,
 689                             const struct timespec64 *ts)
 690 {
 691         struct idt82p33_channel *channel =
 692                         container_of(ptp, struct idt82p33_channel, caps);
 693         struct idt82p33 *idt82p33 = channel->idt82p33;
 694         int err;
 695
 696         mutex_lock(idt82p33->lock);
 697         err = _idt82p33_settime(channel, ts);
 698         mutex_unlock(idt82p33->lock);
 699
 700         if (err)
 701                 dev_err(idt82p33->dev,
 702                         "Failed in %s with err %d!\n", __func__, err);
 703         return err;
 704 }
 705
 706 static int idt82p33_channel_init(struct idt82p33_channel *channel, int index)
 707 {
 708         switch (index) {
 709         case 0:
 710                 channel->dpll_tod_cnfg = DPLL1_TOD_CNFG;
 711                 channel->dpll_tod_trigger = DPLL1_TOD_TRIGGER;
 712                 channel->dpll_tod_sts = DPLL1_TOD_STS;
 713                 channel->dpll_mode_cnfg = DPLL1_OPERATING_MODE_CNFG;
 714                 channel->dpll_freq_cnfg = DPLL1_HOLDOVER_FREQ_CNFG;
 715                 channel->dpll_phase_cnfg = DPLL1_PHASE_OFFSET_CNFG;
 716                 channel->dpll_sync_cnfg = DPLL1_SYNC_EDGE_CNFG;
 717                 channel->dpll_input_mode_cnfg = DPLL1_INPUT_MODE_CNFG;
 718                 break;
 719         case 1:
 720                 channel->dpll_tod_cnfg = DPLL2_TOD_CNFG;
 721                 channel->dpll_tod_trigger = DPLL2_TOD_TRIGGER;
 722                 channel->dpll_tod_sts = DPLL2_TOD_STS;
 723                 channel->dpll_mode_cnfg = DPLL2_OPERATING_MODE_CNFG;
 724                 channel->dpll_freq_cnfg = DPLL2_HOLDOVER_FREQ_CNFG;
 725                 channel->dpll_phase_cnfg = DPLL2_PHASE_OFFSET_CNFG;
 726                 channel->dpll_sync_cnfg = DPLL2_SYNC_EDGE_CNFG;
 727                 channel->dpll_input_mode_cnfg = DPLL2_INPUT_MODE_CNFG;
 728                 break;
 729         default:
 730                 return -EINVAL;
 731         }
 732
 733         channel->current_freq_ppb = 0;
 734
 735         return 0;
 736 }
 737
 738 static void idt82p33_caps_init(struct ptp_clock_info *caps)
 739 {
 740         caps->owner = THIS_MODULE;
 741         caps->max_adj = DCO_MAX_PPB;
 742         caps->n_per_out = 11;
 743         caps->adjphase = idt82p33_adjwritephase;
 744         caps->adjfine = idt82p33_adjfine;
 745         caps->adjtime = idt82p33_adjtime;
 746         caps->gettime64 = idt82p33_gettime;
 747         caps->settime64 = idt82p33_settime;
 748         caps->enable = idt82p33_enable;
 749 }
 750
 751 static int idt82p33_enable_channel(struct idt82p33 *idt82p33, u32 index)
 752 {
 753         struct idt82p33_channel *channel;
 754         int err;
 755
 756         if (!(index < MAX_PHC_PLL))
 757                 return -EINVAL;
 758
 759         channel = &idt82p33->channel[index];
 760
 761         err = idt82p33_channel_init(channel, index);
 762         if (err) {
 763                 dev_err(idt82p33->dev,
 764                         "Channel_init failed in %s with err %d!\n",
 765                         __func__, err);
 766                 return err;
 767         }
 768
 769         channel->idt82p33 = idt82p33;
 770
 771         idt82p33_caps_init(&channel->caps);
 772         snprintf(channel->caps.name, sizeof(channel->caps.name),
 773                  "IDT 82P33 PLL%u", index);
 774
 775         channel->ptp_clock = ptp_clock_register(&channel->caps, NULL);
 776
 777         if (IS_ERR(channel->ptp_clock)) {
 778                 err = PTR_ERR(channel->ptp_clock);
 779                 channel->ptp_clock = NULL;
 780                 return err;
 781         }
 782
 783         if (!channel->ptp_clock)
 784                 return -ENOTSUPP;
 785
 786         err = idt82p33_dpll_set_mode(channel, PLL_MODE_DCO);
 787         if (err) {
 788                 dev_err(idt82p33->dev,
 789                         "Dpll_set_mode failed in %s with err %d!\n",
 790                         __func__, err);
 791                 return err;
 792         }
 793
 794         err = idt82p33_enable_tod(channel);
 795         if (err) {
 796                 dev_err(idt82p33->dev,
 797                         "Enable_tod failed in %s with err %d!\n",
 798                         __func__, err);
 799                 return err;
 800         }
 801
 802         dev_info(idt82p33->dev, "PLL%d registered as ptp%d\n",
 803                  index, channel->ptp_clock->index);
 804
 805         return 0;
 806 }
 807
 808 static int idt82p33_load_firmware(struct idt82p33 *idt82p33)
 809 {
 810         const struct firmware *fw;
 811         struct idt82p33_fwrc *rec;
 812         u8 loaddr, page, val;
 813         int err;
 814         s32 len;
 815
 816         dev_dbg(idt82p33->dev, "requesting firmware '%s'\n", FW_FILENAME);
 817
 818         err = reject_firmware(&fw, FW_FILENAME, idt82p33->dev);
 819
 820         if (err) {
 821                 dev_err(idt82p33->dev,
 822                         "Failed in %s with err %d!\n", __func__, err);
 823                 return err;
 824         }
 825
 826         dev_dbg(idt82p33->dev, "firmware size %zu bytes\n", fw->size);
 827
 828         rec = (struct idt82p33_fwrc *) fw->data;
 829
 830         for (len = fw->size; len > 0; len -= sizeof(*rec)) {
 831
 832                 if (rec->reserved) {
 833                         dev_err(idt82p33->dev,
 834                                 "bad firmware, reserved field non-zero\n");
 835                         err = -EINVAL;
 836                 } else {
 837                         val = rec->value;
 838                         loaddr = rec->loaddr;
 839                         page = rec->hiaddr;
 840
 841                         rec++;
 842
 843                         err = idt82p33_check_and_set_masks(idt82p33, page,
 844                                                            loaddr, val);
 845                 }
 846
 847                 if (err == 0) {
 848                         /* Page size 128, last 4 bytes of page skipped */
 849                         if (loaddr > 0x7b)
 850                                 continue;
 851
 852                         err = idt82p33_write(idt82p33, REG_ADDR(page, loaddr),
 853                                              &val, sizeof(val));
 854                 }
 855
 856                 if (err)
 857                         goto out;
 858         }
 859
 860         idt82p33_display_masks(idt82p33);
 861 out:
 862         release_firmware(fw);
 863         return err;
 864 }
 865
 866
 867 static int idt82p33_probe(struct platform_device *pdev)
 868 {
 869         struct rsmu_ddata *ddata = dev_get_drvdata(pdev->dev.parent);
 870         struct idt82p33 *idt82p33;
 871         int err;
 872         u8 i;
 873
 874         idt82p33 = devm_kzalloc(&pdev->dev,
 875                                 sizeof(struct idt82p33), GFP_KERNEL);
 876         if (!idt82p33)
 877                 return -ENOMEM;
 878
 879         idt82p33->dev = &pdev->dev;
 880         idt82p33->mfd = pdev->dev.parent;
 881         idt82p33->lock = &ddata->lock;
 882         idt82p33->regmap = ddata->regmap;
 883         idt82p33->tod_write_overhead_ns = 0;
 884         idt82p33->calculate_overhead_flag = 0;
 885         idt82p33->pll_mask = DEFAULT_PLL_MASK;
 886         idt82p33->channel[0].output_mask = DEFAULT_OUTPUT_MASK_PLL0;
 887         idt82p33->channel[1].output_mask = DEFAULT_OUTPUT_MASK_PLL1;
 888
 889         mutex_lock(idt82p33->lock);
 890
 891         err = idt82p33_load_firmware(idt82p33);
 892
 893         if (err)
 894                 dev_warn(idt82p33->dev,
 895                          "loading firmware failed with %d\n", err);
 896
 897         if (idt82p33->pll_mask) {
 898                 for (i = 0; i < MAX_PHC_PLL; i++) {
 899                         if (idt82p33->pll_mask & (1 << i)) {
 900                                 err = idt82p33_enable_channel(idt82p33, i);
 901                                 if (err) {
 902                                         dev_err(idt82p33->dev,
 903                                                 "Failed in %s with err %d!\n",
 904                                                 __func__, err);
 905                                         break;
 906                                 }
 907                         }
 908                 }
 909         } else {
 910                 dev_err(idt82p33->dev,
 911                         "no PLLs flagged as PHCs, nothing to do\n");
 912                 err = -ENODEV;
 913         }
 914
 915         mutex_unlock(idt82p33->lock);
 916
 917         if (err) {
 918                 idt82p33_ptp_clock_unregister_all(idt82p33);
 919                 return err;
 920         }
 921
 922         platform_set_drvdata(pdev, idt82p33);
 923
 924         return 0;
 925 }
 926
 927 static int idt82p33_remove(struct platform_device *pdev)
 928 {
 929         struct idt82p33 *idt82p33 = platform_get_drvdata(pdev);
 930
 931         idt82p33_ptp_clock_unregister_all(idt82p33);
 932
 933         return 0;
 934 }
 935
 936 static struct platform_driver idt82p33_driver = {
 937         .driver = {
 938                 .name = "82p33x1x-phc",
 939         },
 940         .probe = idt82p33_probe,
 941         .remove = idt82p33_remove,
 942 };
 943
 944 module_platform_driver(idt82p33_driver);