drivers/ptp/ptp_fc3.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * PTP hardware clock driver for the FemtoClock3 family of timing and
   4  * synchronization devices.
   5  *
   6  * Copyright (C) 2023 Integrated Device Technology, Inc., a Renesas Company.
   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/string.h>
  17 #include <linux/of.h>
  18 #include <linux/bitfield.h>
  19 #include <linux/mfd/rsmu.h>
  20 #include <linux/mfd/idtRC38xxx_reg.h>
  21 #include <asm/unaligned.h>
  22
  23 #include "ptp_private.h"
  24 #include "ptp_fc3.h"
  25
  26 MODULE_DESCRIPTION("Driver for IDT FemtoClock3(TM) family");
  27 MODULE_AUTHOR("IDT support-1588 <[email protected]>");
  28 MODULE_VERSION("1.0");
  29 MODULE_LICENSE("GPL");
  30
  31 /*
  32  * The name of the firmware file to be loaded
  33  * over-rides any automatic selection
  34  */
  35 static char *firmware;
  36 module_param(firmware, charp, 0);
  37
  38 static s64 ns2counters(struct idtfc3 *idtfc3, s64 nsec, u32 *sub_ns)
  39 {
  40         s64 sync;
  41         s32 rem;
  42
  43         if (likely(nsec >= 0)) {
  44                 sync = div_u64_rem(nsec, idtfc3->ns_per_sync, &rem);
  45                 *sub_ns = rem;
  46         } else {
  47                 sync = -div_u64_rem(-nsec - 1, idtfc3->ns_per_sync, &rem) - 1;
  48                 *sub_ns = idtfc3->ns_per_sync - rem - 1;
  49         }
  50
  51         return sync * idtfc3->ns_per_sync;
  52 }
  53
  54 static s64 tdc_meas2offset(struct idtfc3 *idtfc3, u64 meas_read)
  55 {
  56         s64 coarse, fine;
  57
  58         fine = sign_extend64(FIELD_GET(FINE_MEAS_MASK, meas_read), 12);
  59         coarse = sign_extend64(FIELD_GET(COARSE_MEAS_MASK, meas_read), (39 - 13));
  60
  61         fine = div64_s64(fine * NSEC_PER_SEC, idtfc3->tdc_apll_freq * 62LL);
  62         coarse = div64_s64(coarse * NSEC_PER_SEC, idtfc3->time_ref_freq);
  63
  64         return coarse + fine;
  65 }
  66
  67 static s64 tdc_offset2phase(struct idtfc3 *idtfc3, s64 offset_ns)
  68 {
  69         if (offset_ns > idtfc3->ns_per_sync / 2)
  70                 offset_ns -= idtfc3->ns_per_sync;
  71
  72         return offset_ns * idtfc3->tdc_offset_sign;
  73 }
  74
  75 static int idtfc3_set_lpf_mode(struct idtfc3 *idtfc3, u8 mode)
  76 {
  77         int err;
  78
  79         if (mode >= LPF_INVALID)
  80                 return -EINVAL;
  81
  82         if (idtfc3->lpf_mode == mode)
  83                 return 0;
  84
  85         err = regmap_bulk_write(idtfc3->regmap, LPF_MODE_CNFG, &mode, sizeof(mode));
  86         if (err)
  87                 return err;
  88
  89         idtfc3->lpf_mode = mode;
  90
  91         return 0;
  92 }
  93
  94 static int idtfc3_enable_lpf(struct idtfc3 *idtfc3, bool enable)
  95 {
  96         u8 val;
  97         int err;
  98
  99         err = regmap_bulk_read(idtfc3->regmap, LPF_CTRL, &val, sizeof(val));
 100         if (err)
 101                 return err;
 102
 103         if (enable == true)
 104                 val |= LPF_EN;
 105         else
 106                 val &= ~LPF_EN;
 107
 108         return regmap_bulk_write(idtfc3->regmap, LPF_CTRL, &val, sizeof(val));
 109 }
 110
 111 static int idtfc3_get_time_ref_freq(struct idtfc3 *idtfc3)
 112 {
 113         int err;
 114         u8 buf[4];
 115         u8 time_ref_div;
 116         u8 time_clk_div;
 117
 118         err = regmap_bulk_read(idtfc3->regmap, TIME_CLOCK_MEAS_DIV_CNFG, buf, sizeof(buf));
 119         if (err)
 120                 return err;
 121         time_ref_div = FIELD_GET(TIME_REF_DIV_MASK, get_unaligned_le32(buf)) + 1;
 122
 123         err = regmap_bulk_read(idtfc3->regmap, TIME_CLOCK_COUNT, buf, 1);
 124         if (err)
 125                 return err;
 126         time_clk_div = (buf[0] & TIME_CLOCK_COUNT_MASK) + 1;
 127         idtfc3->time_ref_freq = idtfc3->hw_param.time_clk_freq *
 128                                 time_clk_div / time_ref_div;
 129
 130         return 0;
 131 }
 132
 133 static int idtfc3_get_tdc_offset_sign(struct idtfc3 *idtfc3)
 134 {
 135         int err;
 136         u8 buf[4];
 137         u32 val;
 138         u8 sig1, sig2;
 139
 140         err = regmap_bulk_read(idtfc3->regmap, TIME_CLOCK_TDC_FANOUT_CNFG, buf, sizeof(buf));
 141         if (err)
 142                 return err;
 143
 144         val = get_unaligned_le32(buf);
 145         if ((val & TIME_SYNC_TO_TDC_EN) != TIME_SYNC_TO_TDC_EN) {
 146                 dev_err(idtfc3->dev, "TIME_SYNC_TO_TDC_EN is off !!!");
 147                 return -EINVAL;
 148         }
 149
 150         sig1 = FIELD_GET(SIG1_MUX_SEL_MASK, val);
 151         sig2 = FIELD_GET(SIG2_MUX_SEL_MASK, val);
 152
 153         if ((sig1 == sig2) || ((sig1 != TIME_SYNC) && (sig2 != TIME_SYNC))) {
 154                 dev_err(idtfc3->dev, "Invalid tdc_mux_sel sig1=%d sig2=%d", sig1, sig2);
 155                 return -EINVAL;
 156         } else if (sig1 == TIME_SYNC) {
 157                 idtfc3->tdc_offset_sign = 1;
 158         } else if (sig2 == TIME_SYNC) {
 159                 idtfc3->tdc_offset_sign = -1;
 160         }
 161
 162         return 0;
 163 }
 164
 165 static int idtfc3_lpf_bw(struct idtfc3 *idtfc3, u8 shift, u8 mult)
 166 {
 167         u8 val = FIELD_PREP(LPF_BW_SHIFT, shift) | FIELD_PREP(LPF_BW_MULT, mult);
 168
 169         return regmap_bulk_write(idtfc3->regmap, LPF_BW_CNFG, &val, sizeof(val));
 170 }
 171
 172 static int idtfc3_enable_tdc(struct idtfc3 *idtfc3, bool enable, u8 meas_mode)
 173 {
 174         int err;
 175         u8 val = 0;
 176
 177         /* Disable TDC first */
 178         err = regmap_bulk_write(idtfc3->regmap, TIME_CLOCK_MEAS_CTRL, &val, sizeof(val));
 179         if (err)
 180                 return err;
 181
 182         if (enable == false)
 183                 return idtfc3_lpf_bw(idtfc3, LPF_BW_SHIFT_DEFAULT, LPF_BW_MULT_DEFAULT);
 184
 185         if (meas_mode >= MEAS_MODE_INVALID)
 186                 return -EINVAL;
 187
 188         /* Change TDC meas mode */
 189         err = regmap_bulk_write(idtfc3->regmap, TIME_CLOCK_MEAS_CNFG,
 190                                 &meas_mode, sizeof(meas_mode));
 191         if (err)
 192                 return err;
 193
 194         /* Enable TDC */
 195         val = TDC_MEAS_EN;
 196         if (meas_mode == CONTINUOUS)
 197                 val |= TDC_MEAS_START;
 198         err = regmap_bulk_write(idtfc3->regmap, TIME_CLOCK_MEAS_CTRL, &val, sizeof(val));
 199         if (err)
 200                 return err;
 201
 202         return idtfc3_lpf_bw(idtfc3, LPF_BW_SHIFT_1PPS, LPF_BW_MULT_DEFAULT);
 203 }
 204
 205 static bool get_tdc_meas(struct idtfc3 *idtfc3, s64 *offset_ns)
 206 {
 207         bool valid = false;
 208         u8 buf[9];
 209         u8 val;
 210         int err;
 211
 212         while (true) {
 213                 err = regmap_bulk_read(idtfc3->regmap, TDC_FIFO_STS,
 214                                        &val, sizeof(val));
 215                 if (err)
 216                         return false;
 217
 218                 if (val & FIFO_EMPTY)
 219                         break;
 220
 221                 err = regmap_bulk_read(idtfc3->regmap, TDC_FIFO_READ_REQ,
 222                                        &buf, sizeof(buf));
 223                 if (err)
 224                         return false;
 225
 226                 valid = true;
 227         }
 228
 229         if (valid)
 230                 *offset_ns = tdc_meas2offset(idtfc3, get_unaligned_le64(&buf[1]));
 231
 232         return valid;
 233 }
 234
 235 static int check_tdc_fifo_overrun(struct idtfc3 *idtfc3)
 236 {
 237         u8 val;
 238         int err;
 239
 240         /* Check if FIFO is overrun */
 241         err = regmap_bulk_read(idtfc3->regmap, TDC_FIFO_STS, &val, sizeof(val));
 242         if (err)
 243                 return err;
 244
 245         if (!(val & FIFO_FULL))
 246                 return 0;
 247
 248         dev_warn(idtfc3->dev, "TDC FIFO overrun !!!");
 249
 250         err = idtfc3_enable_tdc(idtfc3, true, CONTINUOUS);
 251         if (err)
 252                 return err;
 253
 254         return 0;
 255 }
 256
 257 static int get_tdc_meas_continuous(struct idtfc3 *idtfc3)
 258 {
 259         int err;
 260         s64 offset_ns;
 261         struct ptp_clock_event event;
 262
 263         err = check_tdc_fifo_overrun(idtfc3);
 264         if (err)
 265                 return err;
 266
 267         if (get_tdc_meas(idtfc3, &offset_ns) && offset_ns >= 0) {
 268                 event.index = 0;
 269                 event.offset = tdc_offset2phase(idtfc3, offset_ns);
 270                 event.type = PTP_CLOCK_EXTOFF;
 271                 ptp_clock_event(idtfc3->ptp_clock, &event);
 272         }
 273
 274         return 0;
 275 }
 276
 277 static int idtfc3_read_subcounter(struct idtfc3 *idtfc3)
 278 {
 279         u8 buf[5] = {0};
 280         int err;
 281
 282         err = regmap_bulk_read(idtfc3->regmap, TOD_COUNTER_READ_REQ,
 283                                &buf, sizeof(buf));
 284         if (err)
 285                 return err;
 286
 287         /* sync_counter_value is [31:82] and sub_sync_counter_value is [0:30] */
 288         return get_unaligned_le32(&buf[1]) & SUB_SYNC_COUNTER_MASK;
 289 }
 290
 291 static int idtfc3_tod_update_is_done(struct idtfc3 *idtfc3)
 292 {
 293         int err;
 294         u8 req;
 295
 296         err = read_poll_timeout_atomic(regmap_bulk_read, err, !req, USEC_PER_MSEC,
 297                                        idtfc3->tc_write_timeout, true, idtfc3->regmap,
 298                                        TOD_SYNC_LOAD_REQ_CTRL, &req, 1);
 299         if (err)
 300                 dev_err(idtfc3->dev, "TOD counter write timeout !!!");
 301
 302         return err;
 303 }
 304
 305 static int idtfc3_write_subcounter(struct idtfc3 *idtfc3, u32 counter)
 306 {
 307         u8 buf[18] = {0};
 308         int err;
 309
 310         /* sync_counter_value is [31:82] and sub_sync_counter_value is [0:30] */
 311         put_unaligned_le32(counter & SUB_SYNC_COUNTER_MASK, &buf[0]);
 312
 313         buf[16] = SUB_SYNC_LOAD_ENABLE | SYNC_LOAD_ENABLE;
 314         buf[17] = SYNC_LOAD_REQ;
 315
 316         err = regmap_bulk_write(idtfc3->regmap, TOD_SYNC_LOAD_VAL_CTRL,
 317                                 &buf, sizeof(buf));
 318         if (err)
 319                 return err;
 320
 321         return idtfc3_tod_update_is_done(idtfc3);
 322 }
 323
 324 static int idtfc3_timecounter_update(struct idtfc3 *idtfc3, u32 counter, s64 ns)
 325 {
 326         int err;
 327
 328         err = idtfc3_write_subcounter(idtfc3, counter);
 329         if (err)
 330                 return err;
 331
 332         /* Update time counter */
 333         idtfc3->ns = ns;
 334         idtfc3->last_counter = counter;
 335
 336         return 0;
 337 }
 338
 339 static int idtfc3_timecounter_read(struct idtfc3 *idtfc3)
 340 {
 341         int now, delta;
 342
 343         now = idtfc3_read_subcounter(idtfc3);
 344         if (now < 0)
 345                 return now;
 346
 347         /* calculate the delta since the last idtfc3_timecounter_read(): */
 348         if (now >= idtfc3->last_counter)
 349                 delta = now - idtfc3->last_counter;
 350         else
 351                 delta = idtfc3->sub_sync_count - idtfc3->last_counter + now;
 352
 353         /* Update time counter */
 354         idtfc3->ns += delta * idtfc3->ns_per_counter;
 355         idtfc3->last_counter = now;
 356
 357         return 0;
 358 }
 359
 360 static int _idtfc3_gettime(struct idtfc3 *idtfc3, struct timespec64 *ts)
 361 {
 362         int err;
 363
 364         err = idtfc3_timecounter_read(idtfc3);
 365         if (err)
 366                 return err;
 367
 368         *ts = ns_to_timespec64(idtfc3->ns);
 369
 370         return 0;
 371 }
 372
 373 static int idtfc3_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 374 {
 375         struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
 376         int err;
 377
 378         mutex_lock(idtfc3->lock);
 379         err = _idtfc3_gettime(idtfc3, ts);
 380         mutex_unlock(idtfc3->lock);
 381
 382         return err;
 383 }
 384
 385 static int _idtfc3_settime(struct idtfc3 *idtfc3, const struct timespec64 *ts)
 386 {
 387         s64 offset_ns, now_ns;
 388         u32 counter, sub_ns;
 389         int now;
 390
 391         if (timespec64_valid(ts) == false) {
 392                 dev_err(idtfc3->dev, "%s: invalid timespec", __func__);
 393                 return -EINVAL;
 394         }
 395
 396         now = idtfc3_read_subcounter(idtfc3);
 397         if (now < 0)
 398                 return now;
 399
 400         offset_ns = (idtfc3->sub_sync_count - now) * idtfc3->ns_per_counter;
 401         now_ns = timespec64_to_ns(ts);
 402         (void)ns2counters(idtfc3, offset_ns + now_ns, &sub_ns);
 403
 404         counter = sub_ns / idtfc3->ns_per_counter;
 405         return idtfc3_timecounter_update(idtfc3, counter, now_ns);
 406 }
 407
 408 static int idtfc3_settime(struct ptp_clock_info *ptp, const struct timespec64 *ts)
 409 {
 410         struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
 411         int err;
 412
 413         mutex_lock(idtfc3->lock);
 414         err = _idtfc3_settime(idtfc3, ts);
 415         mutex_unlock(idtfc3->lock);
 416
 417         return err;
 418 }
 419
 420 static int _idtfc3_adjtime(struct idtfc3 *idtfc3, s64 delta)
 421 {
 422         /*
 423          * The TOD counter can be synchronously loaded with any value,
 424          * to be loaded on the next Time Sync pulse
 425          */
 426         s64 sync_ns;
 427         u32 sub_ns;
 428         u32 counter;
 429
 430         if (idtfc3->ns + delta < 0) {
 431                 dev_err(idtfc3->dev, "%lld ns adj is too large", delta);
 432                 return -EINVAL;
 433         }
 434
 435         sync_ns = ns2counters(idtfc3, delta + idtfc3->ns_per_sync, &sub_ns);
 436
 437         counter = sub_ns / idtfc3->ns_per_counter;
 438         return idtfc3_timecounter_update(idtfc3, counter, idtfc3->ns + sync_ns +
 439                                                                         counter * idtfc3->ns_per_counter);
 440 }
 441
 442 static int idtfc3_adjtime(struct ptp_clock_info *ptp, s64 delta)
 443 {
 444         struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
 445         int err;
 446
 447         mutex_lock(idtfc3->lock);
 448         err = _idtfc3_adjtime(idtfc3, delta);
 449         mutex_unlock(idtfc3->lock);
 450
 451         return err;
 452 }
 453
 454 static int _idtfc3_adjphase(struct idtfc3 *idtfc3, s32 delta)
 455 {
 456         u8 buf[8] = {0};
 457         int err;
 458         s64 pcw;
 459
 460         err = idtfc3_set_lpf_mode(idtfc3, LPF_WP);
 461         if (err)
 462                 return err;
 463
 464         /*
 465          * Phase Control Word unit is: 10^9 / (TDC_APLL_FREQ * 124)
 466          *
 467          *       delta * TDC_APLL_FREQ * 124
 468          * PCW = ---------------------------
 469          *                  10^9
 470          *
 471          */
 472         pcw = div_s64((s64)delta * idtfc3->tdc_apll_freq * 124, NSEC_PER_SEC);
 473
 474         put_unaligned_le64(pcw, buf);
 475
 476         return regmap_bulk_write(idtfc3->regmap, LPF_WR_PHASE_CTRL, buf, sizeof(buf));
 477 }
 478
 479 static int idtfc3_adjphase(struct ptp_clock_info *ptp, s32 delta)
 480 {
 481         struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
 482         int err;
 483
 484         mutex_lock(idtfc3->lock);
 485         err = _idtfc3_adjphase(idtfc3, delta);
 486         mutex_unlock(idtfc3->lock);
 487
 488         return err;
 489 }
 490
 491 static int _idtfc3_adjfine(struct idtfc3 *idtfc3, long scaled_ppm)
 492 {
 493         u8 buf[8] = {0};
 494         int err;
 495         s64 fcw;
 496
 497         err = idtfc3_set_lpf_mode(idtfc3, LPF_WF);
 498         if (err)
 499                 return err;
 500
 501         /*
 502          * Frequency Control Word unit is: 2^-44 * 10^6 ppm
 503          *
 504          * adjfreq:
 505          *       ppb * 2^44
 506          * FCW = ----------
 507          *          10^9
 508          *
 509          * adjfine:
 510          *       ppm_16 * 2^28
 511          * FCW = -------------
 512          *           10^6
 513          */
 514         fcw = scaled_ppm * BIT(28);
 515         fcw = div_s64(fcw, 1000000);
 516
 517         put_unaligned_le64(fcw, buf);
 518
 519         return regmap_bulk_write(idtfc3->regmap, LPF_WR_FREQ_CTRL, buf, sizeof(buf));
 520 }
 521
 522 static int idtfc3_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 523 {
 524         struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
 525         int err;
 526
 527         mutex_lock(idtfc3->lock);
 528         err = _idtfc3_adjfine(idtfc3, scaled_ppm);
 529         mutex_unlock(idtfc3->lock);
 530
 531         return err;
 532 }
 533
 534 static int idtfc3_enable(struct ptp_clock_info *ptp,
 535                          struct ptp_clock_request *rq, int on)
 536 {
 537         struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
 538         int err = -EOPNOTSUPP;
 539
 540         mutex_lock(idtfc3->lock);
 541         switch (rq->type) {
 542         case PTP_CLK_REQ_PEROUT:
 543                 if (!on)
 544                         err = 0;
 545                 /* Only accept a 1-PPS aligned to the second. */
 546                 else if (rq->perout.start.nsec || rq->perout.period.sec != 1 ||
 547                          rq->perout.period.nsec)
 548                         err = -ERANGE;
 549                 else
 550                         err = 0;
 551                 break;
 552         case PTP_CLK_REQ_EXTTS:
 553                 if (on) {
 554                         /* Only accept requests for external phase offset */
 555                         if ((rq->extts.flags & PTP_EXT_OFFSET) != (PTP_EXT_OFFSET))
 556                                 err = -EOPNOTSUPP;
 557                         else
 558                                 err = idtfc3_enable_tdc(idtfc3, true, CONTINUOUS);
 559                 } else {
 560                         err = idtfc3_enable_tdc(idtfc3, false, MEAS_MODE_INVALID);
 561                 }
 562                 break;
 563         default:
 564                 break;
 565         }
 566         mutex_unlock(idtfc3->lock);
 567
 568         if (err)
 569                 dev_err(idtfc3->dev, "Failed in %s with err %d!", __func__, err);
 570
 571         return err;
 572 }
 573
 574 static long idtfc3_aux_work(struct ptp_clock_info *ptp)
 575 {
 576         struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
 577         static int tdc_get;
 578
 579         mutex_lock(idtfc3->lock);
 580         tdc_get %= TDC_GET_PERIOD;
 581         if ((tdc_get == 0) || (tdc_get == TDC_GET_PERIOD / 2))
 582                 idtfc3_timecounter_read(idtfc3);
 583         get_tdc_meas_continuous(idtfc3);
 584         tdc_get++;
 585         mutex_unlock(idtfc3->lock);
 586
 587         return idtfc3->tc_update_period;
 588 }
 589
 590 static const struct ptp_clock_info idtfc3_caps = {
 591         .owner          = THIS_MODULE,
 592         .max_adj        = MAX_FFO_PPB,
 593         .n_per_out      = 1,
 594         .n_ext_ts       = 1,
 595         .adjphase       = &idtfc3_adjphase,
 596         .adjfine        = &idtfc3_adjfine,
 597         .adjtime        = &idtfc3_adjtime,
 598         .gettime64      = &idtfc3_gettime,
 599         .settime64      = &idtfc3_settime,
 600         .enable         = &idtfc3_enable,
 601         .do_aux_work    = &idtfc3_aux_work,
 602 };
 603
 604 static int idtfc3_hw_calibrate(struct idtfc3 *idtfc3)
 605 {
 606         int err = 0;
 607         u8 val;
 608
 609         mdelay(10);
 610         /*
 611          * Toggle TDC_DAC_RECAL_REQ:
 612          * (1) set tdc_en to 1
 613          * (2) set tdc_dac_recal_req to 0
 614          * (3) set tdc_dac_recal_req to 1
 615          */
 616         val = TDC_EN;
 617         err = regmap_bulk_write(idtfc3->regmap, TDC_CTRL,
 618                                 &val, sizeof(val));
 619         if (err)
 620                 return err;
 621         val = TDC_EN | TDC_DAC_RECAL_REQ;
 622         err = regmap_bulk_write(idtfc3->regmap, TDC_CTRL,
 623                                 &val, sizeof(val));
 624         if (err)
 625                 return err;
 626         mdelay(10);
 627
 628         /*
 629          * Toggle APLL_REINIT:
 630          * (1) set apll_reinit to 0
 631          * (2) set apll_reinit to 1
 632          */
 633         val = 0;
 634         err = regmap_bulk_write(idtfc3->regmap, SOFT_RESET_CTRL,
 635                                 &val, sizeof(val));
 636         if (err)
 637                 return err;
 638         val = APLL_REINIT;
 639         err = regmap_bulk_write(idtfc3->regmap, SOFT_RESET_CTRL,
 640                                 &val, sizeof(val));
 641         if (err)
 642                 return err;
 643         mdelay(10);
 644
 645         return err;
 646 }
 647
 648 static int idtfc3_init_timecounter(struct idtfc3 *idtfc3)
 649 {
 650         int err;
 651         u32 period_ms;
 652
 653         period_ms = idtfc3->sub_sync_count * MSEC_PER_SEC /
 654                         idtfc3->hw_param.time_clk_freq;
 655
 656         idtfc3->tc_update_period = msecs_to_jiffies(period_ms / TDC_GET_PERIOD);
 657         idtfc3->tc_write_timeout = period_ms * USEC_PER_MSEC;
 658
 659         err = idtfc3_timecounter_update(idtfc3, 0, 0);
 660         if (err)
 661                 return err;
 662
 663         err = idtfc3_timecounter_read(idtfc3);
 664         if (err)
 665                 return err;
 666
 667         ptp_schedule_worker(idtfc3->ptp_clock, idtfc3->tc_update_period);
 668
 669         return 0;
 670 }
 671
 672 static int idtfc3_get_tdc_apll_freq(struct idtfc3 *idtfc3)
 673 {
 674         int err;
 675         u8 tdc_fb_div_int;
 676         u8 tdc_ref_div;
 677         struct idtfc3_hw_param *param = &idtfc3->hw_param;
 678
 679         err = regmap_bulk_read(idtfc3->regmap, TDC_REF_DIV_CNFG,
 680                                 &tdc_ref_div, sizeof(tdc_ref_div));
 681         if (err)
 682                 return err;
 683
 684         err = regmap_bulk_read(idtfc3->regmap, TDC_FB_DIV_INT_CNFG,
 685                                 &tdc_fb_div_int, sizeof(tdc_fb_div_int));
 686         if (err)
 687                 return err;
 688
 689         tdc_fb_div_int &= TDC_FB_DIV_INT_MASK;
 690         tdc_ref_div &= TDC_REF_DIV_CONFIG_MASK;
 691
 692         idtfc3->tdc_apll_freq = div_u64(param->xtal_freq * (u64)tdc_fb_div_int,
 693                                         1 << tdc_ref_div);
 694
 695         return 0;
 696 }
 697
 698 static int idtfc3_get_fod(struct idtfc3 *idtfc3)
 699 {
 700         int err;
 701         u8 fod;
 702
 703         err = regmap_bulk_read(idtfc3->regmap, TIME_CLOCK_SRC, &fod, sizeof(fod));
 704         if (err)
 705                 return err;
 706
 707         switch (fod) {
 708         case 0:
 709                 idtfc3->fod_n = FOD_0;
 710                 break;
 711         case 1:
 712                 idtfc3->fod_n = FOD_1;
 713                 break;
 714         case 2:
 715                 idtfc3->fod_n = FOD_2;
 716                 break;
 717         default:
 718                 return -EINVAL;
 719         }
 720
 721         return 0;
 722 }
 723
 724 static int idtfc3_get_sync_count(struct idtfc3 *idtfc3)
 725 {
 726         int err;
 727         u8 buf[4];
 728
 729         err = regmap_bulk_read(idtfc3->regmap, SUB_SYNC_GEN_CNFG, buf, sizeof(buf));
 730         if (err)
 731                 return err;
 732
 733         idtfc3->sub_sync_count = (get_unaligned_le32(buf) & SUB_SYNC_COUNTER_MASK) + 1;
 734         idtfc3->ns_per_counter = NSEC_PER_SEC / idtfc3->hw_param.time_clk_freq;
 735         idtfc3->ns_per_sync = idtfc3->sub_sync_count * idtfc3->ns_per_counter;
 736
 737         return 0;
 738 }
 739
 740 static int idtfc3_setup_hw_param(struct idtfc3 *idtfc3)
 741 {
 742         int err;
 743
 744         err = idtfc3_get_fod(idtfc3);
 745         if (err)
 746                 return err;
 747
 748         err = idtfc3_get_sync_count(idtfc3);
 749         if (err)
 750                 return err;
 751
 752         err = idtfc3_get_time_ref_freq(idtfc3);
 753         if (err)
 754                 return err;
 755
 756         return idtfc3_get_tdc_apll_freq(idtfc3);
 757 }
 758
 759 static int idtfc3_configure_hw(struct idtfc3 *idtfc3)
 760 {
 761         int err = 0;
 762
 763         err = idtfc3_hw_calibrate(idtfc3);
 764         if (err)
 765                 return err;
 766
 767         err = idtfc3_enable_lpf(idtfc3, true);
 768         if (err)
 769                 return err;
 770
 771         err = idtfc3_enable_tdc(idtfc3, false, MEAS_MODE_INVALID);
 772         if (err)
 773                 return err;
 774
 775         err = idtfc3_get_tdc_offset_sign(idtfc3);
 776         if (err)
 777                 return err;
 778
 779         return idtfc3_setup_hw_param(idtfc3);
 780 }
 781
 782 static int idtfc3_set_overhead(struct idtfc3 *idtfc3)
 783 {
 784         s64 current_ns = 0;
 785         s64 lowest_ns = 0;
 786         int err;
 787         u8 i;
 788         ktime_t start;
 789         ktime_t stop;
 790         ktime_t diff;
 791
 792         char buf[18] = {0};
 793
 794         for (i = 0; i < 5; i++) {
 795                 start = ktime_get_raw();
 796
 797                 err = regmap_bulk_write(idtfc3->regmap, TOD_SYNC_LOAD_VAL_CTRL,
 798                                         &buf, sizeof(buf));
 799                 if (err)
 800                         return err;
 801
 802                 stop = ktime_get_raw();
 803
 804                 diff = ktime_sub(stop, start);
 805
 806                 current_ns = ktime_to_ns(diff);
 807
 808                 if (i == 0) {
 809                         lowest_ns = current_ns;
 810                 } else {
 811                         if (current_ns < lowest_ns)
 812                                 lowest_ns = current_ns;
 813                 }
 814         }
 815
 816         idtfc3->tod_write_overhead = lowest_ns;
 817
 818         return err;
 819 }
 820
 821 static int idtfc3_enable_ptp(struct idtfc3 *idtfc3)
 822 {
 823         int err;
 824
 825         idtfc3->caps = idtfc3_caps;
 826         snprintf(idtfc3->caps.name, sizeof(idtfc3->caps.name), "IDT FC3W");
 827         idtfc3->ptp_clock = ptp_clock_register(&idtfc3->caps, NULL);
 828
 829         if (IS_ERR(idtfc3->ptp_clock)) {
 830                 err = PTR_ERR(idtfc3->ptp_clock);
 831                 idtfc3->ptp_clock = NULL;
 832                 return err;
 833         }
 834
 835         err = idtfc3_set_overhead(idtfc3);
 836         if (err)
 837                 return err;
 838
 839         err = idtfc3_init_timecounter(idtfc3);
 840         if (err)
 841                 return err;
 842
 843         dev_info(idtfc3->dev, "TIME_SYNC_CHANNEL registered as ptp%d",
 844                  idtfc3->ptp_clock->index);
 845
 846         return 0;
 847 }
 848
 849 static int idtfc3_load_firmware(struct idtfc3 *idtfc3)
 850 {
 851         char fname[128] = FW_FILENAME;
 852         const struct firmware *fw;
 853         struct idtfc3_fwrc *rec;
 854         u16 addr;
 855         u8 val;
 856         int err;
 857         s32 len;
 858
 859         idtfc3_default_hw_param(&idtfc3->hw_param);
 860
 861         if (firmware) /* module parameter */
 862                 snprintf(fname, sizeof(fname), "%s", firmware);
 863
 864         dev_info(idtfc3->dev, "requesting firmware '%s'\n", fname);
 865
 866         err = request_firmware(&fw, fname, idtfc3->dev);
 867
 868         if (err) {
 869                 dev_err(idtfc3->dev,
 870                         "requesting firmware failed with err %d!\n", err);
 871                 return err;
 872         }
 873
 874         dev_dbg(idtfc3->dev, "firmware size %zu bytes\n", fw->size);
 875
 876         rec = (struct idtfc3_fwrc *)fw->data;
 877
 878         for (len = fw->size; len > 0; len -= sizeof(*rec)) {
 879                 if (rec->reserved) {
 880                         dev_err(idtfc3->dev,
 881                                 "bad firmware, reserved field non-zero\n");
 882                         err = -EINVAL;
 883                 } else {
 884                         val = rec->value;
 885                         addr = rec->hiaddr << 8 | rec->loaddr;
 886
 887                         rec++;
 888
 889                         err = idtfc3_set_hw_param(&idtfc3->hw_param, addr, val);
 890                 }
 891
 892                 if (err != -EINVAL) {
 893                         err = 0;
 894
 895                         /* Max register */
 896                         if (addr >= 0xE88)
 897                                 continue;
 898
 899                         err = regmap_bulk_write(idtfc3->regmap, addr,
 900                                                 &val, sizeof(val));
 901                 }
 902
 903                 if (err)
 904                         goto out;
 905         }
 906
 907         err = idtfc3_configure_hw(idtfc3);
 908 out:
 909         release_firmware(fw);
 910         return err;
 911 }
 912
 913 static int idtfc3_read_device_id(struct idtfc3 *idtfc3, u16 *device_id)
 914 {
 915         int err;
 916         u8 buf[2] = {0};
 917
 918         err = regmap_bulk_read(idtfc3->regmap, DEVICE_ID,
 919                                &buf, sizeof(buf));
 920         if (err) {
 921                 dev_err(idtfc3->dev, "%s failed with %d", __func__, err);
 922                 return err;
 923         }
 924
 925         *device_id = get_unaligned_le16(buf);
 926
 927         return 0;
 928 }
 929
 930 static int idtfc3_check_device_compatibility(struct idtfc3 *idtfc3)
 931 {
 932         int err;
 933         u16 device_id;
 934
 935         err = idtfc3_read_device_id(idtfc3, &device_id);
 936         if (err)
 937                 return err;
 938
 939         if ((device_id & DEVICE_ID_MASK) == 0) {
 940                 dev_err(idtfc3->dev, "invalid device");
 941                 return -EINVAL;
 942         }
 943
 944         return 0;
 945 }
 946
 947 static int idtfc3_probe(struct platform_device *pdev)
 948 {
 949         struct rsmu_ddata *ddata = dev_get_drvdata(pdev->dev.parent);
 950         struct idtfc3 *idtfc3;
 951         int err;
 952
 953         idtfc3 = devm_kzalloc(&pdev->dev, sizeof(struct idtfc3), GFP_KERNEL);
 954
 955         if (!idtfc3)
 956                 return -ENOMEM;
 957
 958         idtfc3->dev = &pdev->dev;
 959         idtfc3->mfd = pdev->dev.parent;
 960         idtfc3->lock = &ddata->lock;
 961         idtfc3->regmap = ddata->regmap;
 962
 963         mutex_lock(idtfc3->lock);
 964
 965         err = idtfc3_check_device_compatibility(idtfc3);
 966         if (err) {
 967                 mutex_unlock(idtfc3->lock);
 968                 return err;
 969         }
 970
 971         err = idtfc3_load_firmware(idtfc3);
 972         if (err) {
 973                 if (err == -ENOENT) {
 974                         mutex_unlock(idtfc3->lock);
 975                         return -EPROBE_DEFER;
 976                 }
 977                 dev_warn(idtfc3->dev, "loading firmware failed with %d", err);
 978         }
 979
 980         err = idtfc3_enable_ptp(idtfc3);
 981         if (err) {
 982                 dev_err(idtfc3->dev, "idtfc3_enable_ptp failed with %d", err);
 983                 mutex_unlock(idtfc3->lock);
 984                 return err;
 985         }
 986
 987         mutex_unlock(idtfc3->lock);
 988
 989         if (err) {
 990                 ptp_clock_unregister(idtfc3->ptp_clock);
 991                 return err;
 992         }
 993
 994         platform_set_drvdata(pdev, idtfc3);
 995
 996         return 0;
 997 }
 998
 999 static void idtfc3_remove(struct platform_device *pdev)
1000 {
1001         struct idtfc3 *idtfc3 = platform_get_drvdata(pdev);
1002
1003         ptp_clock_unregister(idtfc3->ptp_clock);
1004 }
1005
1006 static struct platform_driver idtfc3_driver = {
1007         .driver = {
1008                 .name = "rc38xxx-phc",
1009         },
1010         .probe = idtfc3_probe,
1011         .remove_new = idtfc3_remove,
1012 };
1013
1014 module_platform_driver(idtfc3_driver);