drivers/ptp/ptp_clockmatrix.c

   1 // SPDX-License-Identifier: GPL-2.0+
   2 /*
   3  * PTP hardware clock driver for the IDT ClockMatrix(TM) family of timing and
   4  * synchronization devices.
   5  *
   6  * Copyright (C) 2019 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/mfd/rsmu.h>
  19 #include <linux/mfd/idt8a340_reg.h>
  20 #include <asm/unaligned.h>
  21
  22 #include "ptp_private.h"
  23 #include "ptp_clockmatrix.h"
  24
  25 MODULE_DESCRIPTION("Driver for IDT ClockMatrix(TM) family");
  26 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
  27 MODULE_AUTHOR("IDT support-1588 <IDT-support-1588@lm.renesas.com>");
  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 #define SETTIME_CORRECTION (0)
  39 #define EXTTS_PERIOD_MS (95)
  40
  41 static int _idtcm_adjfine(struct idtcm_channel *channel, long scaled_ppm);
  42
  43 static inline int idtcm_read(struct idtcm *idtcm,
  44                              u16 module,
  45                              u16 regaddr,
  46                              u8 *buf,
  47                              u16 count)
  48 {
  49         return regmap_bulk_read(idtcm->regmap, module + regaddr, buf, count);
  50 }
  51
  52 static inline int idtcm_write(struct idtcm *idtcm,
  53                               u16 module,
  54                               u16 regaddr,
  55                               u8 *buf,
  56                               u16 count)
  57 {
  58         return regmap_bulk_write(idtcm->regmap, module + regaddr, buf, count);
  59 }
  60
  61 static int contains_full_configuration(struct idtcm *idtcm,
  62                                        const struct firmware *fw)
  63 {
  64         struct idtcm_fwrc *rec = (struct idtcm_fwrc *)fw->data;
  65         u16 scratch = IDTCM_FW_REG(idtcm->fw_ver, V520, SCRATCH);
  66         s32 full_count;
  67         s32 count = 0;
  68         u16 regaddr;
  69         u8 loaddr;
  70         s32 len;
  71
  72         /* 4 bytes skipped every 0x80 */
  73         full_count = (scratch - GPIO_USER_CONTROL) -
  74                      ((scratch >> 7) - (GPIO_USER_CONTROL >> 7)) * 4;
  75
  76         /* If the firmware contains 'full configuration' SM_RESET can be used
  77          * to ensure proper configuration.
  78          *
  79          * Full configuration is defined as the number of programmable
  80          * bytes within the configuration range minus page offset addr range.
  81          */
  82         for (len = fw->size; len > 0; len -= sizeof(*rec)) {
  83                 regaddr = rec->hiaddr << 8;
  84                 regaddr |= rec->loaddr;
  85
  86                 loaddr = rec->loaddr;
  87
  88                 rec++;
  89
  90                 /* Top (status registers) and bottom are read-only */
  91                 if (regaddr < GPIO_USER_CONTROL || regaddr >= scratch)
  92                         continue;
  93
  94                 /* Page size 128, last 4 bytes of page skipped */
  95                 if ((loaddr > 0x7b && loaddr <= 0x7f) || loaddr > 0xfb)
  96                         continue;
  97
  98                 count++;
  99         }
 100
 101         return (count >= full_count);
 102 }
 103
 104 static int char_array_to_timespec(u8 *buf,
 105                                   u8 count,
 106                                   struct timespec64 *ts)
 107 {
 108         u8 i;
 109         u64 nsec;
 110         time64_t sec;
 111
 112         if (count < TOD_BYTE_COUNT)
 113                 return 1;
 114
 115         /* Sub-nanoseconds are in buf[0]. */
 116         nsec = buf[4];
 117         for (i = 0; i < 3; i++) {
 118                 nsec <<= 8;
 119                 nsec |= buf[3 - i];
 120         }
 121
 122         sec = buf[10];
 123         for (i = 0; i < 5; i++) {
 124                 sec <<= 8;
 125                 sec |= buf[9 - i];
 126         }
 127
 128         ts->tv_sec = sec;
 129         ts->tv_nsec = nsec;
 130
 131         return 0;
 132 }
 133
 134 static int timespec_to_char_array(struct timespec64 const *ts,
 135                                   u8 *buf,
 136                                   u8 count)
 137 {
 138         u8 i;
 139         s32 nsec;
 140         time64_t sec;
 141
 142         if (count < TOD_BYTE_COUNT)
 143                 return 1;
 144
 145         nsec = ts->tv_nsec;
 146         sec = ts->tv_sec;
 147
 148         /* Sub-nanoseconds are in buf[0]. */
 149         buf[0] = 0;
 150         for (i = 1; i < 5; i++) {
 151                 buf[i] = nsec & 0xff;
 152                 nsec >>= 8;
 153         }
 154
 155         for (i = 5; i < TOD_BYTE_COUNT; i++) {
 156
 157                 buf[i] = sec & 0xff;
 158                 sec >>= 8;
 159         }
 160
 161         return 0;
 162 }
 163
 164 static int idtcm_strverscmp(const char *version1, const char *version2)
 165 {
 166         u8 ver1[3], ver2[3];
 167         int i;
 168
 169         if (sscanf(version1, "%hhu.%hhu.%hhu",
 170                    &ver1[0], &ver1[1], &ver1[2]) != 3)
 171                 return -1;
 172         if (sscanf(version2, "%hhu.%hhu.%hhu",
 173                    &ver2[0], &ver2[1], &ver2[2]) != 3)
 174                 return -1;
 175
 176         for (i = 0; i < 3; i++) {
 177                 if (ver1[i] > ver2[i])
 178                         return 1;
 179                 if (ver1[i] < ver2[i])
 180                         return -1;
 181         }
 182
 183         return 0;
 184 }
 185
 186 static enum fw_version idtcm_fw_version(const char *version)
 187 {
 188         enum fw_version ver = V_DEFAULT;
 189
 190         if (idtcm_strverscmp(version, "4.8.7") >= 0)
 191                 ver = V487;
 192
 193         if (idtcm_strverscmp(version, "5.2.0") >= 0)
 194                 ver = V520;
 195
 196         return ver;
 197 }
 198
 199 static int clear_boot_status(struct idtcm *idtcm)
 200 {
 201         u8 buf[4] = {0};
 202
 203         return idtcm_write(idtcm, GENERAL_STATUS, BOOT_STATUS, buf, sizeof(buf));
 204 }
 205
 206 static int read_boot_status(struct idtcm *idtcm, u32 *status)
 207 {
 208         int err;
 209         u8 buf[4] = {0};
 210
 211         err = idtcm_read(idtcm, GENERAL_STATUS, BOOT_STATUS, buf, sizeof(buf));
 212
 213         *status = (buf[3] << 24) | (buf[2] << 16) | (buf[1] << 8) | buf[0];
 214
 215         return err;
 216 }
 217
 218 static int wait_for_boot_status_ready(struct idtcm *idtcm)
 219 {
 220         u32 status = 0;
 221         u8 i = 30;      /* 30 * 100ms = 3s */
 222         int err;
 223
 224         do {
 225                 err = read_boot_status(idtcm, &status);
 226                 if (err)
 227                         return err;
 228
 229                 if (status == 0xA0)
 230                         return 0;
 231
 232                 msleep(100);
 233                 i--;
 234
 235         } while (i);
 236
 237         dev_warn(idtcm->dev, "%s timed out", __func__);
 238
 239         return -EBUSY;
 240 }
 241
 242 static int _idtcm_set_scsr_read_trig(struct idtcm_channel *channel,
 243                                      enum scsr_read_trig_sel trig, u8 ref)
 244 {
 245         struct idtcm *idtcm = channel->idtcm;
 246         u16 tod_read_cmd = IDTCM_FW_REG(idtcm->fw_ver, V520, TOD_READ_PRIMARY_CMD);
 247         u8 val;
 248         int err;
 249
 250         if (trig == SCSR_TOD_READ_TRIG_SEL_REFCLK) {
 251                 err = idtcm_read(idtcm, channel->tod_read_primary,
 252                                  TOD_READ_PRIMARY_SEL_CFG_0, &val, sizeof(val));
 253                 if (err)
 254                         return err;
 255
 256                 val &= ~(WR_REF_INDEX_MASK << WR_REF_INDEX_SHIFT);
 257                 val |= (ref << WR_REF_INDEX_SHIFT);
 258
 259                 err = idtcm_write(idtcm, channel->tod_read_primary,
 260                                   TOD_READ_PRIMARY_SEL_CFG_0, &val, sizeof(val));
 261                 if (err)
 262                         return err;
 263         }
 264
 265         err = idtcm_read(idtcm, channel->tod_read_primary,
 266                          tod_read_cmd, &val, sizeof(val));
 267         if (err)
 268                 return err;
 269
 270         val &= ~(TOD_READ_TRIGGER_MASK << TOD_READ_TRIGGER_SHIFT);
 271         val |= (trig << TOD_READ_TRIGGER_SHIFT);
 272         val &= ~TOD_READ_TRIGGER_MODE; /* single shot */
 273
 274         err = idtcm_write(idtcm, channel->tod_read_primary,
 275                           tod_read_cmd, &val, sizeof(val));
 276         return err;
 277 }
 278
 279 static int idtcm_enable_extts(struct idtcm_channel *channel, u8 todn, u8 ref,
 280                               bool enable)
 281 {
 282         struct idtcm *idtcm = channel->idtcm;
 283         u8 old_mask = idtcm->extts_mask;
 284         u8 mask = 1 << todn;
 285         int err = 0;
 286
 287         if (todn >= MAX_TOD)
 288                 return -EINVAL;
 289
 290         if (enable) {
 291                 if (ref > 0xF) /* E_REF_CLK15 */
 292                         return -EINVAL;
 293                 if (idtcm->extts_mask & mask)
 294                         return 0;
 295                 err = _idtcm_set_scsr_read_trig(&idtcm->channel[todn],
 296                                                 SCSR_TOD_READ_TRIG_SEL_REFCLK,
 297                                                 ref);
 298                 if (err == 0) {
 299                         idtcm->extts_mask |= mask;
 300                         idtcm->event_channel[todn] = channel;
 301                         idtcm->channel[todn].refn = ref;
 302                 }
 303         } else
 304                 idtcm->extts_mask &= ~mask;
 305
 306         if (old_mask == 0 && idtcm->extts_mask)
 307                 schedule_delayed_work(&idtcm->extts_work,
 308                                       msecs_to_jiffies(EXTTS_PERIOD_MS));
 309
 310         return err;
 311 }
 312
 313 static int read_sys_apll_status(struct idtcm *idtcm, u8 *status)
 314 {
 315         return idtcm_read(idtcm, STATUS, DPLL_SYS_APLL_STATUS, status,
 316                           sizeof(u8));
 317 }
 318
 319 static int read_sys_dpll_status(struct idtcm *idtcm, u8 *status)
 320 {
 321         return idtcm_read(idtcm, STATUS, DPLL_SYS_STATUS, status, sizeof(u8));
 322 }
 323
 324 static int wait_for_sys_apll_dpll_lock(struct idtcm *idtcm)
 325 {
 326         unsigned long timeout = jiffies + msecs_to_jiffies(LOCK_TIMEOUT_MS);
 327         u8 apll = 0;
 328         u8 dpll = 0;
 329         int err;
 330
 331         do {
 332                 err = read_sys_apll_status(idtcm, &apll);
 333                 if (err)
 334                         return err;
 335
 336                 err = read_sys_dpll_status(idtcm, &dpll);
 337                 if (err)
 338                         return err;
 339
 340                 apll &= SYS_APLL_LOSS_LOCK_LIVE_MASK;
 341                 dpll &= DPLL_SYS_STATE_MASK;
 342
 343                 if (apll == SYS_APLL_LOSS_LOCK_LIVE_LOCKED &&
 344                     dpll == DPLL_STATE_LOCKED) {
 345                         return 0;
 346                 } else if (dpll == DPLL_STATE_FREERUN ||
 347                            dpll == DPLL_STATE_HOLDOVER ||
 348                            dpll == DPLL_STATE_OPEN_LOOP) {
 349                         dev_warn(idtcm->dev,
 350                                 "No wait state: DPLL_SYS_STATE %d", dpll);
 351                         return -EPERM;
 352                 }
 353
 354                 msleep(LOCK_POLL_INTERVAL_MS);
 355         } while (time_is_after_jiffies(timeout));
 356
 357         dev_warn(idtcm->dev,
 358                  "%d ms lock timeout: SYS APLL Loss Lock %d  SYS DPLL state %d",
 359                  LOCK_TIMEOUT_MS, apll, dpll);
 360
 361         return -ETIME;
 362 }
 363
 364 static void wait_for_chip_ready(struct idtcm *idtcm)
 365 {
 366         if (wait_for_boot_status_ready(idtcm))
 367                 dev_warn(idtcm->dev, "BOOT_STATUS != 0xA0");
 368
 369         if (wait_for_sys_apll_dpll_lock(idtcm))
 370                 dev_warn(idtcm->dev,
 371                          "Continuing while SYS APLL/DPLL is not locked");
 372 }
 373
 374 static int _idtcm_gettime(struct idtcm_channel *channel,
 375                           struct timespec64 *ts, u8 timeout)
 376 {
 377         struct idtcm *idtcm = channel->idtcm;
 378         u16 tod_read_cmd = IDTCM_FW_REG(idtcm->fw_ver, V520, TOD_READ_PRIMARY_CMD);
 379         u8 buf[TOD_BYTE_COUNT];
 380         u8 trigger;
 381         int err;
 382
 383         /* wait trigger to be 0 */
 384         do {
 385                 if (timeout-- == 0)
 386                         return -EIO;
 387
 388                 if (idtcm->calculate_overhead_flag)
 389                         idtcm->start_time = ktime_get_raw();
 390
 391                 err = idtcm_read(idtcm, channel->tod_read_primary,
 392                                  tod_read_cmd, &trigger,
 393                                  sizeof(trigger));
 394                 if (err)
 395                         return err;
 396         } while (trigger & TOD_READ_TRIGGER_MASK);
 397
 398         err = idtcm_read(idtcm, channel->tod_read_primary,
 399                          TOD_READ_PRIMARY, buf, sizeof(buf));
 400         if (err)
 401                 return err;
 402
 403         err = char_array_to_timespec(buf, sizeof(buf), ts);
 404
 405         return err;
 406 }
 407
 408 static int idtcm_extts_check_channel(struct idtcm *idtcm, u8 todn)
 409 {
 410         struct idtcm_channel *ptp_channel, *extts_channel;
 411         struct ptp_clock_event event;
 412         struct timespec64 ts;
 413         u32 dco_delay = 0;
 414         int err;
 415
 416         extts_channel = &idtcm->channel[todn];
 417         ptp_channel = idtcm->event_channel[todn];
 418         if (extts_channel == ptp_channel)
 419                 dco_delay = ptp_channel->dco_delay;
 420
 421         err = _idtcm_gettime(extts_channel, &ts, 1);
 422         if (err == 0) {
 423                 event.type = PTP_CLOCK_EXTTS;
 424                 event.index = todn;
 425                 event.timestamp = timespec64_to_ns(&ts) - dco_delay;
 426                 ptp_clock_event(ptp_channel->ptp_clock, &event);
 427         }
 428         return err;
 429 }
 430
 431 static u8 idtcm_enable_extts_mask(struct idtcm_channel *channel,
 432                                     u8 extts_mask, bool enable)
 433 {
 434         struct idtcm *idtcm = channel->idtcm;
 435         int i, err;
 436
 437         for (i = 0; i < MAX_TOD; i++) {
 438                 u8 mask = 1 << i;
 439                 u8 refn = idtcm->channel[i].refn;
 440
 441                 if (extts_mask & mask) {
 442                         /* check extts before disabling it */
 443                         if (enable == false) {
 444                                 err = idtcm_extts_check_channel(idtcm, i);
 445                                 /* trigger happened so we won't re-enable it */
 446                                 if (err == 0)
 447                                         extts_mask &= ~mask;
 448                         }
 449                         (void)idtcm_enable_extts(channel, i, refn, enable);
 450                 }
 451         }
 452
 453         return extts_mask;
 454 }
 455
 456 static int _idtcm_gettime_immediate(struct idtcm_channel *channel,
 457                                     struct timespec64 *ts)
 458 {
 459         struct idtcm *idtcm = channel->idtcm;
 460         u8 extts_mask = 0;
 461         int err;
 462
 463         /* Disable extts */
 464         if (idtcm->extts_mask) {
 465                 extts_mask = idtcm_enable_extts_mask(channel, idtcm->extts_mask,
 466                                                      false);
 467         }
 468
 469         err = _idtcm_set_scsr_read_trig(channel,
 470                                         SCSR_TOD_READ_TRIG_SEL_IMMEDIATE, 0);
 471         if (err == 0)
 472                 err = _idtcm_gettime(channel, ts, 10);
 473
 474         /* Re-enable extts */
 475         if (extts_mask)
 476                 idtcm_enable_extts_mask(channel, extts_mask, true);
 477
 478         return err;
 479 }
 480
 481 static int _sync_pll_output(struct idtcm *idtcm,
 482                             u8 pll,
 483                             u8 sync_src,
 484                             u8 qn,
 485                             u8 qn_plus_1)
 486 {
 487         int err;
 488         u8 val;
 489         u16 sync_ctrl0;
 490         u16 sync_ctrl1;
 491         u8 temp;
 492
 493         if (qn == 0 && qn_plus_1 == 0)
 494                 return 0;
 495
 496         switch (pll) {
 497         case 0:
 498                 sync_ctrl0 = HW_Q0_Q1_CH_SYNC_CTRL_0;
 499                 sync_ctrl1 = HW_Q0_Q1_CH_SYNC_CTRL_1;
 500                 break;
 501         case 1:
 502                 sync_ctrl0 = HW_Q2_Q3_CH_SYNC_CTRL_0;
 503                 sync_ctrl1 = HW_Q2_Q3_CH_SYNC_CTRL_1;
 504                 break;
 505         case 2:
 506                 sync_ctrl0 = HW_Q4_Q5_CH_SYNC_CTRL_0;
 507                 sync_ctrl1 = HW_Q4_Q5_CH_SYNC_CTRL_1;
 508                 break;
 509         case 3:
 510                 sync_ctrl0 = HW_Q6_Q7_CH_SYNC_CTRL_0;
 511                 sync_ctrl1 = HW_Q6_Q7_CH_SYNC_CTRL_1;
 512                 break;
 513         case 4:
 514                 sync_ctrl0 = HW_Q8_CH_SYNC_CTRL_0;
 515                 sync_ctrl1 = HW_Q8_CH_SYNC_CTRL_1;
 516                 break;
 517         case 5:
 518                 sync_ctrl0 = HW_Q9_CH_SYNC_CTRL_0;
 519                 sync_ctrl1 = HW_Q9_CH_SYNC_CTRL_1;
 520                 break;
 521         case 6:
 522                 sync_ctrl0 = HW_Q10_CH_SYNC_CTRL_0;
 523                 sync_ctrl1 = HW_Q10_CH_SYNC_CTRL_1;
 524                 break;
 525         case 7:
 526                 sync_ctrl0 = HW_Q11_CH_SYNC_CTRL_0;
 527                 sync_ctrl1 = HW_Q11_CH_SYNC_CTRL_1;
 528                 break;
 529         default:
 530                 return -EINVAL;
 531         }
 532
 533         val = SYNCTRL1_MASTER_SYNC_RST;
 534
 535         /* Place master sync in reset */
 536         err = idtcm_write(idtcm, 0, sync_ctrl1, &val, sizeof(val));
 537         if (err)
 538                 return err;
 539
 540         err = idtcm_write(idtcm, 0, sync_ctrl0, &sync_src, sizeof(sync_src));
 541         if (err)
 542                 return err;
 543
 544         /* Set sync trigger mask */
 545         val |= SYNCTRL1_FBDIV_FRAME_SYNC_TRIG | SYNCTRL1_FBDIV_SYNC_TRIG;
 546
 547         if (qn)
 548                 val |= SYNCTRL1_Q0_DIV_SYNC_TRIG;
 549
 550         if (qn_plus_1)
 551                 val |= SYNCTRL1_Q1_DIV_SYNC_TRIG;
 552
 553         err = idtcm_write(idtcm, 0, sync_ctrl1, &val, sizeof(val));
 554         if (err)
 555                 return err;
 556
 557         /* PLL5 can have OUT8 as second additional output. */
 558         if (pll == 5 && qn_plus_1 != 0) {
 559                 err = idtcm_read(idtcm, 0, HW_Q8_CTRL_SPARE,
 560                                  &temp, sizeof(temp));
 561                 if (err)
 562                         return err;
 563
 564                 temp &= ~(Q9_TO_Q8_SYNC_TRIG);
 565
 566                 err = idtcm_write(idtcm, 0, HW_Q8_CTRL_SPARE,
 567                                   &temp, sizeof(temp));
 568                 if (err)
 569                         return err;
 570
 571                 temp |= Q9_TO_Q8_SYNC_TRIG;
 572
 573                 err = idtcm_write(idtcm, 0, HW_Q8_CTRL_SPARE,
 574                                   &temp, sizeof(temp));
 575                 if (err)
 576                         return err;
 577         }
 578
 579         /* PLL6 can have OUT11 as second additional output. */
 580         if (pll == 6 && qn_plus_1 != 0) {
 581                 err = idtcm_read(idtcm, 0, HW_Q11_CTRL_SPARE,
 582                                  &temp, sizeof(temp));
 583                 if (err)
 584                         return err;
 585
 586                 temp &= ~(Q10_TO_Q11_SYNC_TRIG);
 587
 588                 err = idtcm_write(idtcm, 0, HW_Q11_CTRL_SPARE,
 589                                   &temp, sizeof(temp));
 590                 if (err)
 591                         return err;
 592
 593                 temp |= Q10_TO_Q11_SYNC_TRIG;
 594
 595                 err = idtcm_write(idtcm, 0, HW_Q11_CTRL_SPARE,
 596                                   &temp, sizeof(temp));
 597                 if (err)
 598                         return err;
 599         }
 600
 601         /* Place master sync out of reset */
 602         val &= ~(SYNCTRL1_MASTER_SYNC_RST);
 603         err = idtcm_write(idtcm, 0, sync_ctrl1, &val, sizeof(val));
 604
 605         return err;
 606 }
 607
 608 static int idtcm_sync_pps_output(struct idtcm_channel *channel)
 609 {
 610         struct idtcm *idtcm = channel->idtcm;
 611         u8 pll;
 612         u8 qn;
 613         u8 qn_plus_1;
 614         int err = 0;
 615         u8 out8_mux = 0;
 616         u8 out11_mux = 0;
 617         u8 temp;
 618         u16 output_mask = channel->output_mask;
 619
 620         err = idtcm_read(idtcm, 0, HW_Q8_CTRL_SPARE,
 621                          &temp, sizeof(temp));
 622         if (err)
 623                 return err;
 624
 625         if ((temp & Q9_TO_Q8_FANOUT_AND_CLOCK_SYNC_ENABLE_MASK) ==
 626             Q9_TO_Q8_FANOUT_AND_CLOCK_SYNC_ENABLE_MASK)
 627                 out8_mux = 1;
 628
 629         err = idtcm_read(idtcm, 0, HW_Q11_CTRL_SPARE,
 630                          &temp, sizeof(temp));
 631         if (err)
 632                 return err;
 633
 634         if ((temp & Q10_TO_Q11_FANOUT_AND_CLOCK_SYNC_ENABLE_MASK) ==
 635             Q10_TO_Q11_FANOUT_AND_CLOCK_SYNC_ENABLE_MASK)
 636                 out11_mux = 1;
 637
 638         for (pll = 0; pll < 8; pll++) {
 639                 qn = 0;
 640                 qn_plus_1 = 0;
 641
 642                 if (pll < 4) {
 643                         /* First 4 pll has 2 outputs */
 644                         qn = output_mask & 0x1;
 645                         output_mask = output_mask >> 1;
 646                         qn_plus_1 = output_mask & 0x1;
 647                         output_mask = output_mask >> 1;
 648                 } else if (pll == 4) {
 649                         if (out8_mux == 0) {
 650                                 qn = output_mask & 0x1;
 651                                 output_mask = output_mask >> 1;
 652                         }
 653                 } else if (pll == 5) {
 654                         if (out8_mux) {
 655                                 qn_plus_1 = output_mask & 0x1;
 656                                 output_mask = output_mask >> 1;
 657                         }
 658                         qn = output_mask & 0x1;
 659                         output_mask = output_mask >> 1;
 660                 } else if (pll == 6) {
 661                         qn = output_mask & 0x1;
 662                         output_mask = output_mask >> 1;
 663                         if (out11_mux) {
 664                                 qn_plus_1 = output_mask & 0x1;
 665                                 output_mask = output_mask >> 1;
 666                         }
 667                 } else if (pll == 7) {
 668                         if (out11_mux == 0) {
 669                                 qn = output_mask & 0x1;
 670                                 output_mask = output_mask >> 1;
 671                         }
 672                 }
 673
 674                 if (qn != 0 || qn_plus_1 != 0)
 675                         err = _sync_pll_output(idtcm, pll, channel->sync_src,
 676                                                qn, qn_plus_1);
 677
 678                 if (err)
 679                         return err;
 680         }
 681
 682         return err;
 683 }
 684
 685 static int _idtcm_set_dpll_hw_tod(struct idtcm_channel *channel,
 686                                   struct timespec64 const *ts,
 687                                   enum hw_tod_write_trig_sel wr_trig)
 688 {
 689         struct idtcm *idtcm = channel->idtcm;
 690         u8 buf[TOD_BYTE_COUNT];
 691         u8 cmd;
 692         int err;
 693         struct timespec64 local_ts = *ts;
 694         s64 total_overhead_ns;
 695
 696         /* Configure HW TOD write trigger. */
 697         err = idtcm_read(idtcm, channel->hw_dpll_n, HW_DPLL_TOD_CTRL_1,
 698                          &cmd, sizeof(cmd));
 699         if (err)
 700                 return err;
 701
 702         cmd &= ~(0x0f);
 703         cmd |= wr_trig | 0x08;
 704
 705         err = idtcm_write(idtcm, channel->hw_dpll_n, HW_DPLL_TOD_CTRL_1,
 706                           &cmd, sizeof(cmd));
 707         if (err)
 708                 return err;
 709
 710         if (wr_trig  != HW_TOD_WR_TRIG_SEL_MSB) {
 711                 err = timespec_to_char_array(&local_ts, buf, sizeof(buf));
 712                 if (err)
 713                         return err;
 714
 715                 err = idtcm_write(idtcm, channel->hw_dpll_n,
 716                                   HW_DPLL_TOD_OVR__0, buf, sizeof(buf));
 717                 if (err)
 718                         return err;
 719         }
 720
 721         /* ARM HW TOD write trigger. */
 722         cmd &= ~(0x08);
 723
 724         err = idtcm_write(idtcm, channel->hw_dpll_n, HW_DPLL_TOD_CTRL_1,
 725                           &cmd, sizeof(cmd));
 726
 727         if (wr_trig == HW_TOD_WR_TRIG_SEL_MSB) {
 728                 if (idtcm->calculate_overhead_flag) {
 729                         /* Assumption: I2C @ 400KHz */
 730                         ktime_t diff = ktime_sub(ktime_get_raw(),
 731                                                  idtcm->start_time);
 732                         total_overhead_ns =  ktime_to_ns(diff)
 733                                              + idtcm->tod_write_overhead_ns
 734                                              + SETTIME_CORRECTION;
 735
 736                         timespec64_add_ns(&local_ts, total_overhead_ns);
 737
 738                         idtcm->calculate_overhead_flag = 0;
 739                 }
 740
 741                 err = timespec_to_char_array(&local_ts, buf, sizeof(buf));
 742                 if (err)
 743                         return err;
 744
 745                 err = idtcm_write(idtcm, channel->hw_dpll_n,
 746                                   HW_DPLL_TOD_OVR__0, buf, sizeof(buf));
 747         }
 748
 749         return err;
 750 }
 751
 752 static int _idtcm_set_dpll_scsr_tod(struct idtcm_channel *channel,
 753                                     struct timespec64 const *ts,
 754                                     enum scsr_tod_write_trig_sel wr_trig,
 755                                     enum scsr_tod_write_type_sel wr_type)
 756 {
 757         struct idtcm *idtcm = channel->idtcm;
 758         unsigned char buf[TOD_BYTE_COUNT], cmd;
 759         struct timespec64 local_ts = *ts;
 760         int err, count = 0;
 761
 762         timespec64_add_ns(&local_ts, SETTIME_CORRECTION);
 763
 764         err = timespec_to_char_array(&local_ts, buf, sizeof(buf));
 765         if (err)
 766                 return err;
 767
 768         err = idtcm_write(idtcm, channel->tod_write, TOD_WRITE,
 769                           buf, sizeof(buf));
 770         if (err)
 771                 return err;
 772
 773         /* Trigger the write operation. */
 774         err = idtcm_read(idtcm, channel->tod_write, TOD_WRITE_CMD,
 775                          &cmd, sizeof(cmd));
 776         if (err)
 777                 return err;
 778
 779         cmd &= ~(TOD_WRITE_SELECTION_MASK << TOD_WRITE_SELECTION_SHIFT);
 780         cmd &= ~(TOD_WRITE_TYPE_MASK << TOD_WRITE_TYPE_SHIFT);
 781         cmd |= (wr_trig << TOD_WRITE_SELECTION_SHIFT);
 782         cmd |= (wr_type << TOD_WRITE_TYPE_SHIFT);
 783
 784         err = idtcm_write(idtcm, channel->tod_write, TOD_WRITE_CMD,
 785                            &cmd, sizeof(cmd));
 786         if (err)
 787                 return err;
 788
 789         /* Wait for the operation to complete. */
 790         while (1) {
 791                 /* pps trigger takes up to 1 sec to complete */
 792                 if (wr_trig == SCSR_TOD_WR_TRIG_SEL_TODPPS)
 793                         msleep(50);
 794
 795                 err = idtcm_read(idtcm, channel->tod_write, TOD_WRITE_CMD,
 796                                  &cmd, sizeof(cmd));
 797                 if (err)
 798                         return err;
 799
 800                 if ((cmd & TOD_WRITE_SELECTION_MASK) == 0)
 801                         break;
 802
 803                 if (++count > 20) {
 804                         dev_err(idtcm->dev,
 805                                 "Timed out waiting for the write counter");
 806                         return -EIO;
 807                 }
 808         }
 809
 810         return 0;
 811 }
 812
 813 static int get_output_base_addr(enum fw_version ver, u8 outn)
 814 {
 815         int base;
 816
 817         switch (outn) {
 818         case 0:
 819                 base = IDTCM_FW_REG(ver, V520, OUTPUT_0);
 820                 break;
 821         case 1:
 822                 base = IDTCM_FW_REG(ver, V520, OUTPUT_1);
 823                 break;
 824         case 2:
 825                 base = IDTCM_FW_REG(ver, V520, OUTPUT_2);
 826                 break;
 827         case 3:
 828                 base = IDTCM_FW_REG(ver, V520, OUTPUT_3);
 829                 break;
 830         case 4:
 831                 base = IDTCM_FW_REG(ver, V520, OUTPUT_4);
 832                 break;
 833         case 5:
 834                 base = IDTCM_FW_REG(ver, V520, OUTPUT_5);
 835                 break;
 836         case 6:
 837                 base = IDTCM_FW_REG(ver, V520, OUTPUT_6);
 838                 break;
 839         case 7:
 840                 base = IDTCM_FW_REG(ver, V520, OUTPUT_7);
 841                 break;
 842         case 8:
 843                 base = IDTCM_FW_REG(ver, V520, OUTPUT_8);
 844                 break;
 845         case 9:
 846                 base = IDTCM_FW_REG(ver, V520, OUTPUT_9);
 847                 break;
 848         case 10:
 849                 base = IDTCM_FW_REG(ver, V520, OUTPUT_10);
 850                 break;
 851         case 11:
 852                 base = IDTCM_FW_REG(ver, V520, OUTPUT_11);
 853                 break;
 854         default:
 855                 base = -EINVAL;
 856         }
 857
 858         return base;
 859 }
 860
 861 static int _idtcm_settime_deprecated(struct idtcm_channel *channel,
 862                                      struct timespec64 const *ts)
 863 {
 864         struct idtcm *idtcm = channel->idtcm;
 865         int err;
 866
 867         err = _idtcm_set_dpll_hw_tod(channel, ts, HW_TOD_WR_TRIG_SEL_MSB);
 868         if (err) {
 869                 dev_err(idtcm->dev,
 870                         "%s: Set HW ToD failed", __func__);
 871                 return err;
 872         }
 873
 874         return idtcm_sync_pps_output(channel);
 875 }
 876
 877 static int _idtcm_settime(struct idtcm_channel *channel,
 878                           struct timespec64 const *ts,
 879                           enum scsr_tod_write_type_sel wr_type)
 880 {
 881         return _idtcm_set_dpll_scsr_tod(channel, ts,
 882                                         SCSR_TOD_WR_TRIG_SEL_IMMEDIATE,
 883                                         wr_type);
 884 }
 885
 886 static int idtcm_set_phase_pull_in_offset(struct idtcm_channel *channel,
 887                                           s32 offset_ns)
 888 {
 889         int err;
 890         int i;
 891         struct idtcm *idtcm = channel->idtcm;
 892         u8 buf[4];
 893
 894         for (i = 0; i < 4; i++) {
 895                 buf[i] = 0xff & (offset_ns);
 896                 offset_ns >>= 8;
 897         }
 898
 899         err = idtcm_write(idtcm, channel->dpll_phase_pull_in, PULL_IN_OFFSET,
 900                           buf, sizeof(buf));
 901
 902         return err;
 903 }
 904
 905 static int idtcm_set_phase_pull_in_slope_limit(struct idtcm_channel *channel,
 906                                                u32 max_ffo_ppb)
 907 {
 908         int err;
 909         u8 i;
 910         struct idtcm *idtcm = channel->idtcm;
 911         u8 buf[3];
 912
 913         if (max_ffo_ppb & 0xff000000)
 914                 max_ffo_ppb = 0;
 915
 916         for (i = 0; i < 3; i++) {
 917                 buf[i] = 0xff & (max_ffo_ppb);
 918                 max_ffo_ppb >>= 8;
 919         }
 920
 921         err = idtcm_write(idtcm, channel->dpll_phase_pull_in,
 922                           PULL_IN_SLOPE_LIMIT, buf, sizeof(buf));
 923
 924         return err;
 925 }
 926
 927 static int idtcm_start_phase_pull_in(struct idtcm_channel *channel)
 928 {
 929         int err;
 930         struct idtcm *idtcm = channel->idtcm;
 931         u8 buf;
 932
 933         err = idtcm_read(idtcm, channel->dpll_phase_pull_in, PULL_IN_CTRL,
 934                          &buf, sizeof(buf));
 935         if (err)
 936                 return err;
 937
 938         if (buf == 0) {
 939                 buf = 0x01;
 940                 err = idtcm_write(idtcm, channel->dpll_phase_pull_in,
 941                                   PULL_IN_CTRL, &buf, sizeof(buf));
 942         } else {
 943                 err = -EBUSY;
 944         }
 945
 946         return err;
 947 }
 948
 949 static int do_phase_pull_in_fw(struct idtcm_channel *channel,
 950                                s32 offset_ns,
 951                                u32 max_ffo_ppb)
 952 {
 953         int err;
 954
 955         err = idtcm_set_phase_pull_in_offset(channel, -offset_ns);
 956         if (err)
 957                 return err;
 958
 959         err = idtcm_set_phase_pull_in_slope_limit(channel, max_ffo_ppb);
 960         if (err)
 961                 return err;
 962
 963         err = idtcm_start_phase_pull_in(channel);
 964
 965         return err;
 966 }
 967
 968 static int set_tod_write_overhead(struct idtcm_channel *channel)
 969 {
 970         struct idtcm *idtcm = channel->idtcm;
 971         s64 current_ns = 0;
 972         s64 lowest_ns = 0;
 973         int err;
 974         u8 i;
 975         ktime_t start;
 976         ktime_t stop;
 977         ktime_t diff;
 978
 979         char buf[TOD_BYTE_COUNT] = {0};
 980
 981         /* Set page offset */
 982         idtcm_write(idtcm, channel->hw_dpll_n, HW_DPLL_TOD_OVR__0,
 983                     buf, sizeof(buf));
 984
 985         for (i = 0; i < TOD_WRITE_OVERHEAD_COUNT_MAX; i++) {
 986                 start = ktime_get_raw();
 987
 988                 err = idtcm_write(idtcm, channel->hw_dpll_n,
 989                                   HW_DPLL_TOD_OVR__0, buf, sizeof(buf));
 990                 if (err)
 991                         return err;
 992
 993                 stop = ktime_get_raw();
 994
 995                 diff = ktime_sub(stop, start);
 996
 997                 current_ns = ktime_to_ns(diff);
 998
 999                 if (i == 0) {
1000                         lowest_ns = current_ns;
1001                 } else {
1002                         if (current_ns < lowest_ns)
1003                                 lowest_ns = current_ns;
1004                 }
1005         }
1006
1007         idtcm->tod_write_overhead_ns = lowest_ns;
1008
1009         return err;
1010 }
1011
1012 static int _idtcm_adjtime_deprecated(struct idtcm_channel *channel, s64 delta)
1013 {
1014         int err;
1015         struct idtcm *idtcm = channel->idtcm;
1016         struct timespec64 ts;
1017         s64 now;
1018
1019         if (abs(delta) < PHASE_PULL_IN_THRESHOLD_NS_DEPRECATED) {
1020                 err = channel->do_phase_pull_in(channel, delta, 0);
1021         } else {
1022                 idtcm->calculate_overhead_flag = 1;
1023
1024                 err = set_tod_write_overhead(channel);
1025                 if (err)
1026                         return err;
1027
1028                 err = _idtcm_gettime_immediate(channel, &ts);
1029                 if (err)
1030                         return err;
1031
1032                 now = timespec64_to_ns(&ts);
1033                 now += delta;
1034
1035                 ts = ns_to_timespec64(now);
1036
1037                 err = _idtcm_settime_deprecated(channel, &ts);
1038         }
1039
1040         return err;
1041 }
1042
1043 static int idtcm_state_machine_reset(struct idtcm *idtcm)
1044 {
1045         u8 byte = SM_RESET_CMD;
1046         u32 status = 0;
1047         int err;
1048         u8 i;
1049
1050         clear_boot_status(idtcm);
1051
1052         err = idtcm_write(idtcm, RESET_CTRL,
1053                           IDTCM_FW_REG(idtcm->fw_ver, V520, SM_RESET),
1054                           &byte, sizeof(byte));
1055
1056         if (!err) {
1057                 for (i = 0; i < 30; i++) {
1058                         msleep_interruptible(100);
1059                         read_boot_status(idtcm, &status);
1060
1061                         if (status == 0xA0) {
1062                                 dev_dbg(idtcm->dev,
1063                                         "SM_RESET completed in %d ms", i * 100);
1064                                 break;
1065                         }
1066                 }
1067
1068                 if (!status)
1069                         dev_err(idtcm->dev,
1070                                 "Timed out waiting for CM_RESET to complete");
1071         }
1072
1073         return err;
1074 }
1075
1076 static int idtcm_read_hw_rev_id(struct idtcm *idtcm, u8 *hw_rev_id)
1077 {
1078         return idtcm_read(idtcm, HW_REVISION, REV_ID, hw_rev_id, sizeof(u8));
1079 }
1080
1081 static int idtcm_read_product_id(struct idtcm *idtcm, u16 *product_id)
1082 {
1083         int err;
1084         u8 buf[2] = {0};
1085
1086         err = idtcm_read(idtcm, GENERAL_STATUS, PRODUCT_ID, buf, sizeof(buf));
1087
1088         *product_id = (buf[1] << 8) | buf[0];
1089
1090         return err;
1091 }
1092
1093 static int idtcm_read_major_release(struct idtcm *idtcm, u8 *major)
1094 {
1095         int err;
1096         u8 buf = 0;
1097
1098         err = idtcm_read(idtcm, GENERAL_STATUS, MAJ_REL, &buf, sizeof(buf));
1099
1100         *major = buf >> 1;
1101
1102         return err;
1103 }
1104
1105 static int idtcm_read_minor_release(struct idtcm *idtcm, u8 *minor)
1106 {
1107         return idtcm_read(idtcm, GENERAL_STATUS, MIN_REL, minor, sizeof(u8));
1108 }
1109
1110 static int idtcm_read_hotfix_release(struct idtcm *idtcm, u8 *hotfix)
1111 {
1112         return idtcm_read(idtcm,
1113                           GENERAL_STATUS,
1114                           HOTFIX_REL,
1115                           hotfix,
1116                           sizeof(u8));
1117 }
1118
1119 static int idtcm_read_otp_scsr_config_select(struct idtcm *idtcm,
1120                                              u8 *config_select)
1121 {
1122         return idtcm_read(idtcm, GENERAL_STATUS, OTP_SCSR_CONFIG_SELECT,
1123                           config_select, sizeof(u8));
1124 }
1125
1126 static int set_pll_output_mask(struct idtcm *idtcm, u16 addr, u8 val)
1127 {
1128         int err = 0;
1129
1130         switch (addr) {
1131         case TOD0_OUT_ALIGN_MASK_ADDR:
1132                 SET_U16_LSB(idtcm->channel[0].output_mask, val);
1133                 break;
1134         case TOD0_OUT_ALIGN_MASK_ADDR + 1:
1135                 SET_U16_MSB(idtcm->channel[0].output_mask, val);
1136                 break;
1137         case TOD1_OUT_ALIGN_MASK_ADDR:
1138                 SET_U16_LSB(idtcm->channel[1].output_mask, val);
1139                 break;
1140         case TOD1_OUT_ALIGN_MASK_ADDR + 1:
1141                 SET_U16_MSB(idtcm->channel[1].output_mask, val);
1142                 break;
1143         case TOD2_OUT_ALIGN_MASK_ADDR:
1144                 SET_U16_LSB(idtcm->channel[2].output_mask, val);
1145                 break;
1146         case TOD2_OUT_ALIGN_MASK_ADDR + 1:
1147                 SET_U16_MSB(idtcm->channel[2].output_mask, val);
1148                 break;
1149         case TOD3_OUT_ALIGN_MASK_ADDR:
1150                 SET_U16_LSB(idtcm->channel[3].output_mask, val);
1151                 break;
1152         case TOD3_OUT_ALIGN_MASK_ADDR + 1:
1153                 SET_U16_MSB(idtcm->channel[3].output_mask, val);
1154                 break;
1155         default:
1156                 err = -EFAULT; /* Bad address */;
1157                 break;
1158         }
1159
1160         return err;
1161 }
1162
1163 static int set_tod_ptp_pll(struct idtcm *idtcm, u8 index, u8 pll)
1164 {
1165         if (index >= MAX_TOD) {
1166                 dev_err(idtcm->dev, "ToD%d not supported", index);
1167                 return -EINVAL;
1168         }
1169
1170         if (pll >= MAX_PLL) {
1171                 dev_err(idtcm->dev, "Pll%d not supported", pll);
1172                 return -EINVAL;
1173         }
1174
1175         idtcm->channel[index].pll = pll;
1176
1177         return 0;
1178 }
1179
1180 static int check_and_set_masks(struct idtcm *idtcm,
1181                                u16 regaddr,
1182                                u8 val)
1183 {
1184         int err = 0;
1185
1186         switch (regaddr) {
1187         case TOD_MASK_ADDR:
1188                 if ((val & 0xf0) || !(val & 0x0f)) {
1189                         dev_err(idtcm->dev, "Invalid TOD mask 0x%02x", val);
1190                         err = -EINVAL;
1191                 } else {
1192                         idtcm->tod_mask = val;
1193                 }
1194                 break;
1195         case TOD0_PTP_PLL_ADDR:
1196                 err = set_tod_ptp_pll(idtcm, 0, val);
1197                 break;
1198         case TOD1_PTP_PLL_ADDR:
1199                 err = set_tod_ptp_pll(idtcm, 1, val);
1200                 break;
1201         case TOD2_PTP_PLL_ADDR:
1202                 err = set_tod_ptp_pll(idtcm, 2, val);
1203                 break;
1204         case TOD3_PTP_PLL_ADDR:
1205                 err = set_tod_ptp_pll(idtcm, 3, val);
1206                 break;
1207         default:
1208                 err = set_pll_output_mask(idtcm, regaddr, val);
1209                 break;
1210         }
1211
1212         return err;
1213 }
1214
1215 static void display_pll_and_masks(struct idtcm *idtcm)
1216 {
1217         u8 i;
1218         u8 mask;
1219
1220         dev_dbg(idtcm->dev, "tod_mask = 0x%02x", idtcm->tod_mask);
1221
1222         for (i = 0; i < MAX_TOD; i++) {
1223                 mask = 1 << i;
1224
1225                 if (mask & idtcm->tod_mask)
1226                         dev_dbg(idtcm->dev,
1227                                 "TOD%d pll = %d    output_mask = 0x%04x",
1228                                 i, idtcm->channel[i].pll,
1229                                 idtcm->channel[i].output_mask);
1230         }
1231 }
1232
1233 static int idtcm_load_firmware(struct idtcm *idtcm,
1234                                struct device *dev)
1235 {
1236         u16 scratch = IDTCM_FW_REG(idtcm->fw_ver, V520, SCRATCH);
1237         char fname[128] = FW_FILENAME;
1238         const struct firmware *fw;
1239         struct idtcm_fwrc *rec;
1240         u32 regaddr;
1241         int err;
1242         s32 len;
1243         u8 val;
1244         u8 loaddr;
1245
1246         if (firmware) /* module parameter */
1247                 snprintf(fname, sizeof(fname), "%s", firmware);
1248
1249         dev_info(idtcm->dev, "requesting firmware '%s'", fname);
1250
1251         err = request_firmware(&fw, fname, dev);
1252         if (err) {
1253                 dev_err(idtcm->dev,
1254                         "Failed at line %d in %s!", __LINE__, __func__);
1255                 return err;
1256         }
1257
1258         dev_dbg(idtcm->dev, "firmware size %zu bytes", fw->size);
1259
1260         rec = (struct idtcm_fwrc *) fw->data;
1261
1262         if (contains_full_configuration(idtcm, fw))
1263                 idtcm_state_machine_reset(idtcm);
1264
1265         for (len = fw->size; len > 0; len -= sizeof(*rec)) {
1266                 if (rec->reserved) {
1267                         dev_err(idtcm->dev,
1268                                 "bad firmware, reserved field non-zero");
1269                         err = -EINVAL;
1270                 } else {
1271                         regaddr = rec->hiaddr << 8;
1272                         regaddr |= rec->loaddr;
1273
1274                         val = rec->value;
1275                         loaddr = rec->loaddr;
1276
1277                         rec++;
1278
1279                         err = check_and_set_masks(idtcm, regaddr, val);
1280                 }
1281
1282                 if (err != -EINVAL) {
1283                         err = 0;
1284
1285                         /* Top (status registers) and bottom are read-only */
1286                         if (regaddr < GPIO_USER_CONTROL || regaddr >= scratch)
1287                                 continue;
1288
1289                         /* Page size 128, last 4 bytes of page skipped */
1290                         if ((loaddr > 0x7b && loaddr <= 0x7f) || loaddr > 0xfb)
1291                                 continue;
1292
1293                         err = idtcm_write(idtcm, regaddr, 0, &val, sizeof(val));
1294                 }
1295
1296                 if (err)
1297                         goto out;
1298         }
1299
1300         display_pll_and_masks(idtcm);
1301
1302 out:
1303         release_firmware(fw);
1304         return err;
1305 }
1306
1307 static int idtcm_output_enable(struct idtcm_channel *channel,
1308                                bool enable, unsigned int outn)
1309 {
1310         struct idtcm *idtcm = channel->idtcm;
1311         int base;
1312         int err;
1313         u8 val;
1314
1315         base = get_output_base_addr(idtcm->fw_ver, outn);
1316
1317         if (!(base > 0)) {
1318                 dev_err(idtcm->dev,
1319                         "%s - Unsupported out%d", __func__, outn);
1320                 return base;
1321         }
1322
1323         err = idtcm_read(idtcm, (u16)base, OUT_CTRL_1, &val, sizeof(val));
1324         if (err)
1325                 return err;
1326
1327         if (enable)
1328                 val |= SQUELCH_DISABLE;
1329         else
1330                 val &= ~SQUELCH_DISABLE;
1331
1332         return idtcm_write(idtcm, (u16)base, OUT_CTRL_1, &val, sizeof(val));
1333 }
1334
1335 static int idtcm_output_mask_enable(struct idtcm_channel *channel,
1336                                     bool enable)
1337 {
1338         u16 mask;
1339         int err;
1340         u8 outn;
1341
1342         mask = channel->output_mask;
1343         outn = 0;
1344
1345         while (mask) {
1346                 if (mask & 0x1) {
1347                         err = idtcm_output_enable(channel, enable, outn);
1348                         if (err)
1349                                 return err;
1350                 }
1351
1352                 mask >>= 0x1;
1353                 outn++;
1354         }
1355
1356         return 0;
1357 }
1358
1359 static int idtcm_perout_enable(struct idtcm_channel *channel,
1360                                struct ptp_perout_request *perout,
1361                                bool enable)
1362 {
1363         struct idtcm *idtcm = channel->idtcm;
1364         unsigned int flags = perout->flags;
1365         struct timespec64 ts = {0, 0};
1366         int err;
1367
1368         if (flags == PEROUT_ENABLE_OUTPUT_MASK)
1369                 err = idtcm_output_mask_enable(channel, enable);
1370         else
1371                 err = idtcm_output_enable(channel, enable, perout->index);
1372
1373         if (err) {
1374                 dev_err(idtcm->dev, "Unable to set output enable");
1375                 return err;
1376         }
1377
1378         /* Align output to internal 1 PPS */
1379         return _idtcm_settime(channel, &ts, SCSR_TOD_WR_TYPE_SEL_DELTA_PLUS);
1380 }
1381
1382 static int idtcm_get_pll_mode(struct idtcm_channel *channel,
1383                               enum pll_mode *mode)
1384 {
1385         struct idtcm *idtcm = channel->idtcm;
1386         int err;
1387         u8 dpll_mode;
1388
1389         err = idtcm_read(idtcm, channel->dpll_n,
1390                          IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_MODE),
1391                          &dpll_mode, sizeof(dpll_mode));
1392         if (err)
1393                 return err;
1394
1395         *mode = (dpll_mode >> PLL_MODE_SHIFT) & PLL_MODE_MASK;
1396
1397         return 0;
1398 }
1399
1400 static int idtcm_set_pll_mode(struct idtcm_channel *channel,
1401                               enum pll_mode mode)
1402 {
1403         struct idtcm *idtcm = channel->idtcm;
1404         int err;
1405         u8 dpll_mode;
1406
1407         err = idtcm_read(idtcm, channel->dpll_n,
1408                          IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_MODE),
1409                          &dpll_mode, sizeof(dpll_mode));
1410         if (err)
1411                 return err;
1412
1413         dpll_mode &= ~(PLL_MODE_MASK << PLL_MODE_SHIFT);
1414
1415         dpll_mode |= (mode << PLL_MODE_SHIFT);
1416
1417         err = idtcm_write(idtcm, channel->dpll_n,
1418                           IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_MODE),
1419                           &dpll_mode, sizeof(dpll_mode));
1420         return err;
1421 }
1422
1423 static int idtcm_get_manual_reference(struct idtcm_channel *channel,
1424                                       enum manual_reference *ref)
1425 {
1426         struct idtcm *idtcm = channel->idtcm;
1427         u8 dpll_manu_ref_cfg;
1428         int err;
1429
1430         err = idtcm_read(idtcm, channel->dpll_ctrl_n,
1431                          DPLL_CTRL_DPLL_MANU_REF_CFG,
1432                          &dpll_manu_ref_cfg, sizeof(dpll_manu_ref_cfg));
1433         if (err)
1434                 return err;
1435
1436         dpll_manu_ref_cfg &= (MANUAL_REFERENCE_MASK << MANUAL_REFERENCE_SHIFT);
1437
1438         *ref = dpll_manu_ref_cfg >> MANUAL_REFERENCE_SHIFT;
1439
1440         return 0;
1441 }
1442
1443 static int idtcm_set_manual_reference(struct idtcm_channel *channel,
1444                                       enum manual_reference ref)
1445 {
1446         struct idtcm *idtcm = channel->idtcm;
1447         u8 dpll_manu_ref_cfg;
1448         int err;
1449
1450         err = idtcm_read(idtcm, channel->dpll_ctrl_n,
1451                          DPLL_CTRL_DPLL_MANU_REF_CFG,
1452                          &dpll_manu_ref_cfg, sizeof(dpll_manu_ref_cfg));
1453         if (err)
1454                 return err;
1455
1456         dpll_manu_ref_cfg &= ~(MANUAL_REFERENCE_MASK << MANUAL_REFERENCE_SHIFT);
1457
1458         dpll_manu_ref_cfg |= (ref << MANUAL_REFERENCE_SHIFT);
1459
1460         err = idtcm_write(idtcm, channel->dpll_ctrl_n,
1461                           DPLL_CTRL_DPLL_MANU_REF_CFG,
1462                           &dpll_manu_ref_cfg, sizeof(dpll_manu_ref_cfg));
1463
1464         return err;
1465 }
1466
1467 static int configure_dpll_mode_write_frequency(struct idtcm_channel *channel)
1468 {
1469         struct idtcm *idtcm = channel->idtcm;
1470         int err;
1471
1472         err = idtcm_set_pll_mode(channel, PLL_MODE_WRITE_FREQUENCY);
1473
1474         if (err)
1475                 dev_err(idtcm->dev, "Failed to set pll mode to write frequency");
1476         else
1477                 channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
1478
1479         return err;
1480 }
1481
1482 static int configure_dpll_mode_write_phase(struct idtcm_channel *channel)
1483 {
1484         struct idtcm *idtcm = channel->idtcm;
1485         int err;
1486
1487         err = idtcm_set_pll_mode(channel, PLL_MODE_WRITE_PHASE);
1488
1489         if (err)
1490                 dev_err(idtcm->dev, "Failed to set pll mode to write phase");
1491         else
1492                 channel->mode = PTP_PLL_MODE_WRITE_PHASE;
1493
1494         return err;
1495 }
1496
1497 static int configure_manual_reference_write_frequency(struct idtcm_channel *channel)
1498 {
1499         struct idtcm *idtcm = channel->idtcm;
1500         int err;
1501
1502         err = idtcm_set_manual_reference(channel, MANU_REF_WRITE_FREQUENCY);
1503
1504         if (err)
1505                 dev_err(idtcm->dev, "Failed to set manual reference to write frequency");
1506         else
1507                 channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
1508
1509         return err;
1510 }
1511
1512 static int configure_manual_reference_write_phase(struct idtcm_channel *channel)
1513 {
1514         struct idtcm *idtcm = channel->idtcm;
1515         int err;
1516
1517         err = idtcm_set_manual_reference(channel, MANU_REF_WRITE_PHASE);
1518
1519         if (err)
1520                 dev_err(idtcm->dev, "Failed to set manual reference to write phase");
1521         else
1522                 channel->mode = PTP_PLL_MODE_WRITE_PHASE;
1523
1524         return err;
1525 }
1526
1527 static int idtcm_stop_phase_pull_in(struct idtcm_channel *channel)
1528 {
1529         int err;
1530
1531         err = _idtcm_adjfine(channel, channel->current_freq_scaled_ppm);
1532         if (err)
1533                 return err;
1534
1535         channel->phase_pull_in = false;
1536
1537         return 0;
1538 }
1539
1540 static long idtcm_work_handler(struct ptp_clock_info *ptp)
1541 {
1542         struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
1543         struct idtcm *idtcm = channel->idtcm;
1544
1545         mutex_lock(idtcm->lock);
1546
1547         (void)idtcm_stop_phase_pull_in(channel);
1548
1549         mutex_unlock(idtcm->lock);
1550
1551         /* Return a negative value here to not reschedule */
1552         return -1;
1553 }
1554
1555 static s32 phase_pull_in_scaled_ppm(s32 current_ppm, s32 phase_pull_in_ppb)
1556 {
1557         /* ppb = scaled_ppm * 125 / 2^13 */
1558         /* scaled_ppm = ppb * 2^13 / 125 */
1559
1560         s64 max_scaled_ppm = div_s64((s64)PHASE_PULL_IN_MAX_PPB << 13, 125);
1561         s64 scaled_ppm = div_s64((s64)phase_pull_in_ppb << 13, 125);
1562
1563         current_ppm += scaled_ppm;
1564
1565         if (current_ppm > max_scaled_ppm)
1566                 current_ppm = max_scaled_ppm;
1567         else if (current_ppm < -max_scaled_ppm)
1568                 current_ppm = -max_scaled_ppm;
1569
1570         return current_ppm;
1571 }
1572
1573 static int do_phase_pull_in_sw(struct idtcm_channel *channel,
1574                                s32 delta_ns,
1575                                u32 max_ffo_ppb)
1576 {
1577         s32 current_ppm = channel->current_freq_scaled_ppm;
1578         u32 duration_ms = MSEC_PER_SEC;
1579         s32 delta_ppm;
1580         s32 ppb;
1581         int err;
1582
1583         /* If the ToD correction is less than PHASE_PULL_IN_MIN_THRESHOLD_NS,
1584          * skip. The error introduced by the ToD adjustment procedure would
1585          * be bigger than the required ToD correction
1586          */
1587         if (abs(delta_ns) < PHASE_PULL_IN_MIN_THRESHOLD_NS)
1588                 return 0;
1589
1590         if (max_ffo_ppb == 0)
1591                 max_ffo_ppb = PHASE_PULL_IN_MAX_PPB;
1592
1593         /* For most cases, keep phase pull-in duration 1 second */
1594         ppb = delta_ns;
1595         while (abs(ppb) > max_ffo_ppb) {
1596                 duration_ms *= 2;
1597                 ppb /= 2;
1598         }
1599
1600         delta_ppm = phase_pull_in_scaled_ppm(current_ppm, ppb);
1601
1602         err = _idtcm_adjfine(channel, delta_ppm);
1603
1604         if (err)
1605                 return err;
1606
1607         /* schedule the worker to cancel phase pull-in */
1608         ptp_schedule_worker(channel->ptp_clock,
1609                             msecs_to_jiffies(duration_ms) - 1);
1610
1611         channel->phase_pull_in = true;
1612
1613         return 0;
1614 }
1615
1616 static int initialize_operating_mode_with_manual_reference(struct idtcm_channel *channel,
1617                                                            enum manual_reference ref)
1618 {
1619         struct idtcm *idtcm = channel->idtcm;
1620
1621         channel->mode = PTP_PLL_MODE_UNSUPPORTED;
1622         channel->configure_write_frequency = configure_manual_reference_write_frequency;
1623         channel->configure_write_phase = configure_manual_reference_write_phase;
1624         channel->do_phase_pull_in = do_phase_pull_in_sw;
1625
1626         switch (ref) {
1627         case MANU_REF_WRITE_PHASE:
1628                 channel->mode = PTP_PLL_MODE_WRITE_PHASE;
1629                 break;
1630         case MANU_REF_WRITE_FREQUENCY:
1631                 channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
1632                 break;
1633         default:
1634                 dev_warn(idtcm->dev,
1635                          "Unsupported MANUAL_REFERENCE: 0x%02x", ref);
1636         }
1637
1638         return 0;
1639 }
1640
1641 static int initialize_operating_mode_with_pll_mode(struct idtcm_channel *channel,
1642                                                    enum pll_mode mode)
1643 {
1644         struct idtcm *idtcm = channel->idtcm;
1645         int err = 0;
1646
1647         channel->mode = PTP_PLL_MODE_UNSUPPORTED;
1648         channel->configure_write_frequency = configure_dpll_mode_write_frequency;
1649         channel->configure_write_phase = configure_dpll_mode_write_phase;
1650         channel->do_phase_pull_in = do_phase_pull_in_fw;
1651
1652         switch (mode) {
1653         case  PLL_MODE_WRITE_PHASE:
1654                 channel->mode = PTP_PLL_MODE_WRITE_PHASE;
1655                 break;
1656         case PLL_MODE_WRITE_FREQUENCY:
1657                 channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
1658                 break;
1659         default:
1660                 dev_err(idtcm->dev,
1661                         "Unsupported PLL_MODE: 0x%02x", mode);
1662                 err = -EINVAL;
1663         }
1664
1665         return err;
1666 }
1667
1668 static int initialize_dco_operating_mode(struct idtcm_channel *channel)
1669 {
1670         enum manual_reference ref = MANU_REF_XO_DPLL;
1671         enum pll_mode mode = PLL_MODE_DISABLED;
1672         struct idtcm *idtcm = channel->idtcm;
1673         int err;
1674
1675         channel->mode = PTP_PLL_MODE_UNSUPPORTED;
1676
1677         err = idtcm_get_pll_mode(channel, &mode);
1678         if (err) {
1679                 dev_err(idtcm->dev, "Unable to read pll mode!");
1680                 return err;
1681         }
1682
1683         if (mode == PLL_MODE_PLL) {
1684                 err = idtcm_get_manual_reference(channel, &ref);
1685                 if (err) {
1686                         dev_err(idtcm->dev, "Unable to read manual reference!");
1687                         return err;
1688                 }
1689                 err = initialize_operating_mode_with_manual_reference(channel, ref);
1690         } else {
1691                 err = initialize_operating_mode_with_pll_mode(channel, mode);
1692         }
1693
1694         if (channel->mode == PTP_PLL_MODE_WRITE_PHASE)
1695                 channel->configure_write_frequency(channel);
1696
1697         return err;
1698 }
1699
1700 /* PTP Hardware Clock interface */
1701
1702 /*
1703  * Maximum absolute value for write phase offset in picoseconds
1704  *
1705  * Destination signed register is 32-bit register in resolution of 50ps
1706  *
1707  * 0x7fffffff * 50 =  2147483647 * 50 = 107374182350
1708  */
1709 static int _idtcm_adjphase(struct idtcm_channel *channel, s32 delta_ns)
1710 {
1711         struct idtcm *idtcm = channel->idtcm;
1712         int err;
1713         u8 i;
1714         u8 buf[4] = {0};
1715         s32 phase_50ps;
1716         s64 offset_ps;
1717
1718         if (channel->mode != PTP_PLL_MODE_WRITE_PHASE) {
1719                 err = channel->configure_write_phase(channel);
1720                 if (err)
1721                         return err;
1722         }
1723
1724         offset_ps = (s64)delta_ns * 1000;
1725
1726         /*
1727          * Check for 32-bit signed max * 50:
1728          *
1729          * 0x7fffffff * 50 =  2147483647 * 50 = 107374182350
1730          */
1731         if (offset_ps > MAX_ABS_WRITE_PHASE_PICOSECONDS)
1732                 offset_ps = MAX_ABS_WRITE_PHASE_PICOSECONDS;
1733         else if (offset_ps < -MAX_ABS_WRITE_PHASE_PICOSECONDS)
1734                 offset_ps = -MAX_ABS_WRITE_PHASE_PICOSECONDS;
1735
1736         phase_50ps = div_s64(offset_ps, 50);
1737
1738         for (i = 0; i < 4; i++) {
1739                 buf[i] = phase_50ps & 0xff;
1740                 phase_50ps >>= 8;
1741         }
1742
1743         err = idtcm_write(idtcm, channel->dpll_phase, DPLL_WR_PHASE,
1744                           buf, sizeof(buf));
1745
1746         return err;
1747 }
1748
1749 static int _idtcm_adjfine(struct idtcm_channel *channel, long scaled_ppm)
1750 {
1751         struct idtcm *idtcm = channel->idtcm;
1752         u8 i;
1753         int err;
1754         u8 buf[6] = {0};
1755         s64 fcw;
1756
1757         if (channel->mode  != PTP_PLL_MODE_WRITE_FREQUENCY) {
1758                 err = channel->configure_write_frequency(channel);
1759                 if (err)
1760                         return err;
1761         }
1762
1763         /*
1764          * Frequency Control Word unit is: 1.11 * 10^-10 ppm
1765          *
1766          * adjfreq:
1767          *       ppb * 10^9
1768          * FCW = ----------
1769          *          111
1770          *
1771          * adjfine:
1772          *       ppm_16 * 5^12
1773          * FCW = -------------
1774          *         111 * 2^4
1775          */
1776
1777         /* 2 ^ -53 = 1.1102230246251565404236316680908e-16 */
1778         fcw = scaled_ppm * 244140625ULL;
1779
1780         fcw = div_s64(fcw, 1776);
1781
1782         for (i = 0; i < 6; i++) {
1783                 buf[i] = fcw & 0xff;
1784                 fcw >>= 8;
1785         }
1786
1787         err = idtcm_write(idtcm, channel->dpll_freq, DPLL_WR_FREQ,
1788                           buf, sizeof(buf));
1789
1790         return err;
1791 }
1792
1793 static int idtcm_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
1794 {
1795         struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
1796         struct idtcm *idtcm = channel->idtcm;
1797         int err;
1798
1799         mutex_lock(idtcm->lock);
1800         err = _idtcm_gettime_immediate(channel, ts);
1801         mutex_unlock(idtcm->lock);
1802
1803         if (err)
1804                 dev_err(idtcm->dev, "Failed at line %d in %s!",
1805                         __LINE__, __func__);
1806
1807         return err;
1808 }
1809
1810 static int idtcm_settime_deprecated(struct ptp_clock_info *ptp,
1811                                     const struct timespec64 *ts)
1812 {
1813         struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
1814         struct idtcm *idtcm = channel->idtcm;
1815         int err;
1816
1817         mutex_lock(idtcm->lock);
1818         err = _idtcm_settime_deprecated(channel, ts);
1819         mutex_unlock(idtcm->lock);
1820
1821         if (err)
1822                 dev_err(idtcm->dev,
1823                         "Failed at line %d in %s!", __LINE__, __func__);
1824
1825         return err;
1826 }
1827
1828 static int idtcm_settime(struct ptp_clock_info *ptp,
1829                          const struct timespec64 *ts)
1830 {
1831         struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
1832         struct idtcm *idtcm = channel->idtcm;
1833         int err;
1834
1835         mutex_lock(idtcm->lock);
1836         err = _idtcm_settime(channel, ts, SCSR_TOD_WR_TYPE_SEL_ABSOLUTE);
1837         mutex_unlock(idtcm->lock);
1838
1839         if (err)
1840                 dev_err(idtcm->dev,
1841                         "Failed at line %d in %s!", __LINE__, __func__);
1842
1843         return err;
1844 }
1845
1846 static int idtcm_adjtime_deprecated(struct ptp_clock_info *ptp, s64 delta)
1847 {
1848         struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
1849         struct idtcm *idtcm = channel->idtcm;
1850         int err;
1851
1852         mutex_lock(idtcm->lock);
1853         err = _idtcm_adjtime_deprecated(channel, delta);
1854         mutex_unlock(idtcm->lock);
1855
1856         if (err)
1857                 dev_err(idtcm->dev,
1858                         "Failed at line %d in %s!", __LINE__, __func__);
1859
1860         return err;
1861 }
1862
1863 static int idtcm_adjtime(struct ptp_clock_info *ptp, s64 delta)
1864 {
1865         struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
1866         struct idtcm *idtcm = channel->idtcm;
1867         struct timespec64 ts;
1868         enum scsr_tod_write_type_sel type;
1869         int err;
1870
1871         if (channel->phase_pull_in == true)
1872                 return 0;
1873
1874         mutex_lock(idtcm->lock);
1875
1876         if (abs(delta) < PHASE_PULL_IN_THRESHOLD_NS) {
1877                 err = channel->do_phase_pull_in(channel, delta, 0);
1878         } else {
1879                 if (delta >= 0) {
1880                         ts = ns_to_timespec64(delta);
1881                         type = SCSR_TOD_WR_TYPE_SEL_DELTA_PLUS;
1882                 } else {
1883                         ts = ns_to_timespec64(-delta);
1884                         type = SCSR_TOD_WR_TYPE_SEL_DELTA_MINUS;
1885                 }
1886                 err = _idtcm_settime(channel, &ts, type);
1887         }
1888
1889         mutex_unlock(idtcm->lock);
1890
1891         if (err)
1892                 dev_err(idtcm->dev,
1893                         "Failed at line %d in %s!", __LINE__, __func__);
1894
1895         return err;
1896 }
1897
1898 static int idtcm_adjphase(struct ptp_clock_info *ptp, s32 delta)
1899 {
1900         struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
1901         struct idtcm *idtcm = channel->idtcm;
1902         int err;
1903
1904         mutex_lock(idtcm->lock);
1905         err = _idtcm_adjphase(channel, delta);
1906         mutex_unlock(idtcm->lock);
1907
1908         if (err)
1909                 dev_err(idtcm->dev,
1910                         "Failed at line %d in %s!", __LINE__, __func__);
1911
1912         return err;
1913 }
1914
1915 static int idtcm_adjfine(struct ptp_clock_info *ptp,  long scaled_ppm)
1916 {
1917         struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
1918         struct idtcm *idtcm = channel->idtcm;
1919         int err;
1920
1921         if (channel->phase_pull_in == true)
1922                 return 0;
1923
1924         if (scaled_ppm == channel->current_freq_scaled_ppm)
1925                 return 0;
1926
1927         mutex_lock(idtcm->lock);
1928         err = _idtcm_adjfine(channel, scaled_ppm);
1929         mutex_unlock(idtcm->lock);
1930
1931         if (err)
1932                 dev_err(idtcm->dev,
1933                         "Failed at line %d in %s!", __LINE__, __func__);
1934         else
1935                 channel->current_freq_scaled_ppm = scaled_ppm;
1936
1937         return err;
1938 }
1939
1940 static int idtcm_enable(struct ptp_clock_info *ptp,
1941                         struct ptp_clock_request *rq, int on)
1942 {
1943         struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
1944         struct idtcm *idtcm = channel->idtcm;
1945         int err = -EOPNOTSUPP;
1946
1947         mutex_lock(idtcm->lock);
1948
1949         switch (rq->type) {
1950         case PTP_CLK_REQ_PEROUT:
1951                 if (!on)
1952                         err = idtcm_perout_enable(channel, &rq->perout, false);
1953                 /* Only accept a 1-PPS aligned to the second. */
1954                 else if (rq->perout.start.nsec || rq->perout.period.sec != 1 ||
1955                          rq->perout.period.nsec)
1956                         err = -ERANGE;
1957                 else
1958                         err = idtcm_perout_enable(channel, &rq->perout, true);
1959                 break;
1960         case PTP_CLK_REQ_EXTTS:
1961                 err = idtcm_enable_extts(channel, rq->extts.index,
1962                                          rq->extts.rsv[0], on);
1963                 break;
1964         default:
1965                 break;
1966         }
1967
1968         mutex_unlock(idtcm->lock);
1969
1970         if (err)
1971                 dev_err(channel->idtcm->dev,
1972                         "Failed in %s with err %d!", __func__, err);
1973
1974         return err;
1975 }
1976
1977 static int idtcm_enable_tod(struct idtcm_channel *channel)
1978 {
1979         struct idtcm *idtcm = channel->idtcm;
1980         struct timespec64 ts = {0, 0};
1981         u16 tod_cfg = IDTCM_FW_REG(idtcm->fw_ver, V520, TOD_CFG);
1982         u8 cfg;
1983         int err;
1984
1985         /* STEELAI-366 - Temporary workaround for ts2phc compatibility */
1986         if (0) {
1987                 err = idtcm_output_mask_enable(channel, false);
1988                 if (err)
1989                         return err;
1990         }
1991
1992         /*
1993          * Start the TOD clock ticking.
1994          */
1995         err = idtcm_read(idtcm, channel->tod_n, tod_cfg, &cfg, sizeof(cfg));
1996         if (err)
1997                 return err;
1998
1999         cfg |= TOD_ENABLE;
2000
2001         err = idtcm_write(idtcm, channel->tod_n, tod_cfg, &cfg, sizeof(cfg));
2002         if (err)
2003                 return err;
2004
2005         if (idtcm->fw_ver < V487)
2006                 return _idtcm_settime_deprecated(channel, &ts);
2007         else
2008                 return _idtcm_settime(channel, &ts,
2009                                       SCSR_TOD_WR_TYPE_SEL_ABSOLUTE);
2010 }
2011
2012 static void idtcm_set_version_info(struct idtcm *idtcm)
2013 {
2014         u8 major;
2015         u8 minor;
2016         u8 hotfix;
2017         u16 product_id;
2018         u8 hw_rev_id;
2019         u8 config_select;
2020
2021         idtcm_read_major_release(idtcm, &major);
2022         idtcm_read_minor_release(idtcm, &minor);
2023         idtcm_read_hotfix_release(idtcm, &hotfix);
2024
2025         idtcm_read_product_id(idtcm, &product_id);
2026         idtcm_read_hw_rev_id(idtcm, &hw_rev_id);
2027
2028         idtcm_read_otp_scsr_config_select(idtcm, &config_select);
2029
2030         snprintf(idtcm->version, sizeof(idtcm->version), "%u.%u.%u",
2031                  major, minor, hotfix);
2032
2033         idtcm->fw_ver = idtcm_fw_version(idtcm->version);
2034
2035         dev_info(idtcm->dev,
2036                  "%d.%d.%d, Id: 0x%04x  HW Rev: %d  OTP Config Select: %d",
2037                  major, minor, hotfix,
2038                  product_id, hw_rev_id, config_select);
2039 }
2040
2041 static const struct ptp_clock_info idtcm_caps = {
2042         .owner          = THIS_MODULE,
2043         .max_adj        = 244000,
2044         .n_per_out      = 12,
2045         .n_ext_ts       = MAX_TOD,
2046         .adjphase       = &idtcm_adjphase,
2047         .adjfine        = &idtcm_adjfine,
2048         .adjtime        = &idtcm_adjtime,
2049         .gettime64      = &idtcm_gettime,
2050         .settime64      = &idtcm_settime,
2051         .enable         = &idtcm_enable,
2052         .do_aux_work    = &idtcm_work_handler,
2053 };
2054
2055 static const struct ptp_clock_info idtcm_caps_deprecated = {
2056         .owner          = THIS_MODULE,
2057         .max_adj        = 244000,
2058         .n_per_out      = 12,
2059         .n_ext_ts       = MAX_TOD,
2060         .adjphase       = &idtcm_adjphase,
2061         .adjfine        = &idtcm_adjfine,
2062         .adjtime        = &idtcm_adjtime_deprecated,
2063         .gettime64      = &idtcm_gettime,
2064         .settime64      = &idtcm_settime_deprecated,
2065         .enable         = &idtcm_enable,
2066         .do_aux_work    = &idtcm_work_handler,
2067 };
2068
2069 static int configure_channel_pll(struct idtcm_channel *channel)
2070 {
2071         struct idtcm *idtcm = channel->idtcm;
2072         int err = 0;
2073
2074         switch (channel->pll) {
2075         case 0:
2076                 channel->dpll_freq = DPLL_FREQ_0;
2077                 channel->dpll_n = DPLL_0;
2078                 channel->hw_dpll_n = HW_DPLL_0;
2079                 channel->dpll_phase = DPLL_PHASE_0;
2080                 channel->dpll_ctrl_n = DPLL_CTRL_0;
2081                 channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_0;
2082                 break;
2083         case 1:
2084                 channel->dpll_freq = DPLL_FREQ_1;
2085                 channel->dpll_n = DPLL_1;
2086                 channel->hw_dpll_n = HW_DPLL_1;
2087                 channel->dpll_phase = DPLL_PHASE_1;
2088                 channel->dpll_ctrl_n = DPLL_CTRL_1;
2089                 channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_1;
2090                 break;
2091         case 2:
2092                 channel->dpll_freq = DPLL_FREQ_2;
2093                 channel->dpll_n = IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_2);
2094                 channel->hw_dpll_n = HW_DPLL_2;
2095                 channel->dpll_phase = DPLL_PHASE_2;
2096                 channel->dpll_ctrl_n = DPLL_CTRL_2;
2097                 channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_2;
2098                 break;
2099         case 3:
2100                 channel->dpll_freq = DPLL_FREQ_3;
2101                 channel->dpll_n = DPLL_3;
2102                 channel->hw_dpll_n = HW_DPLL_3;
2103                 channel->dpll_phase = DPLL_PHASE_3;
2104                 channel->dpll_ctrl_n = DPLL_CTRL_3;
2105                 channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_3;
2106                 break;
2107         case 4:
2108                 channel->dpll_freq = DPLL_FREQ_4;
2109                 channel->dpll_n = IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_4);
2110                 channel->hw_dpll_n = HW_DPLL_4;
2111                 channel->dpll_phase = DPLL_PHASE_4;
2112                 channel->dpll_ctrl_n = DPLL_CTRL_4;
2113                 channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_4;
2114                 break;
2115         case 5:
2116                 channel->dpll_freq = DPLL_FREQ_5;
2117                 channel->dpll_n = DPLL_5;
2118                 channel->hw_dpll_n = HW_DPLL_5;
2119                 channel->dpll_phase = DPLL_PHASE_5;
2120                 channel->dpll_ctrl_n = DPLL_CTRL_5;
2121                 channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_5;
2122                 break;
2123         case 6:
2124                 channel->dpll_freq = DPLL_FREQ_6;
2125                 channel->dpll_n = IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_6);
2126                 channel->hw_dpll_n = HW_DPLL_6;
2127                 channel->dpll_phase = DPLL_PHASE_6;
2128                 channel->dpll_ctrl_n = DPLL_CTRL_6;
2129                 channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_6;
2130                 break;
2131         case 7:
2132                 channel->dpll_freq = DPLL_FREQ_7;
2133                 channel->dpll_n = DPLL_7;
2134                 channel->hw_dpll_n = HW_DPLL_7;
2135                 channel->dpll_phase = DPLL_PHASE_7;
2136                 channel->dpll_ctrl_n = DPLL_CTRL_7;
2137                 channel->dpll_phase_pull_in = DPLL_PHASE_PULL_IN_7;
2138                 break;
2139         default:
2140                 err = -EINVAL;
2141         }
2142
2143         return err;
2144 }
2145
2146 /*
2147  * Compensate for the PTP DCO input-to-output delay.
2148  * This delay is 18 FOD cycles.
2149  */
2150 static u32 idtcm_get_dco_delay(struct idtcm_channel *channel)
2151 {
2152         struct idtcm *idtcm = channel->idtcm;
2153         u8 mbuf[8] = {0};
2154         u8 nbuf[2] = {0};
2155         u32 fodFreq;
2156         int err;
2157         u64 m;
2158         u16 n;
2159
2160         err = idtcm_read(idtcm, channel->dpll_ctrl_n,
2161                          DPLL_CTRL_DPLL_FOD_FREQ, mbuf, 6);
2162         if (err)
2163                 return 0;
2164
2165         err = idtcm_read(idtcm, channel->dpll_ctrl_n,
2166                          DPLL_CTRL_DPLL_FOD_FREQ + 6, nbuf, 2);
2167         if (err)
2168                 return 0;
2169
2170         m = get_unaligned_le64(mbuf);
2171         n = get_unaligned_le16(nbuf);
2172
2173         if (n == 0)
2174                 n = 1;
2175
2176         fodFreq = (u32)div_u64(m, n);
2177         if (fodFreq >= 500000000)
2178                 return 18 * (u32)div_u64(NSEC_PER_SEC, fodFreq);
2179
2180         return 0;
2181 }
2182
2183 static int configure_channel_tod(struct idtcm_channel *channel, u32 index)
2184 {
2185         enum fw_version fw_ver = channel->idtcm->fw_ver;
2186
2187         /* Set tod addresses */
2188         switch (index) {
2189         case 0:
2190                 channel->tod_read_primary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_PRIMARY_0);
2191                 channel->tod_write = IDTCM_FW_REG(fw_ver, V520, TOD_WRITE_0);
2192                 channel->tod_n = IDTCM_FW_REG(fw_ver, V520, TOD_0);
2193                 channel->sync_src = SYNC_SOURCE_DPLL0_TOD_PPS;
2194                 break;
2195         case 1:
2196                 channel->tod_read_primary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_PRIMARY_1);
2197                 channel->tod_write = IDTCM_FW_REG(fw_ver, V520, TOD_WRITE_1);
2198                 channel->tod_n = IDTCM_FW_REG(fw_ver, V520, TOD_1);
2199                 channel->sync_src = SYNC_SOURCE_DPLL1_TOD_PPS;
2200                 break;
2201         case 2:
2202                 channel->tod_read_primary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_PRIMARY_2);
2203                 channel->tod_write = IDTCM_FW_REG(fw_ver, V520, TOD_WRITE_2);
2204                 channel->tod_n = IDTCM_FW_REG(fw_ver, V520, TOD_2);
2205                 channel->sync_src = SYNC_SOURCE_DPLL2_TOD_PPS;
2206                 break;
2207         case 3:
2208                 channel->tod_read_primary = IDTCM_FW_REG(fw_ver, V520, TOD_READ_PRIMARY_3);
2209                 channel->tod_write = IDTCM_FW_REG(fw_ver, V520, TOD_WRITE_3);
2210                 channel->tod_n = IDTCM_FW_REG(fw_ver, V520, TOD_3);
2211                 channel->sync_src = SYNC_SOURCE_DPLL3_TOD_PPS;
2212                 break;
2213         default:
2214                 return -EINVAL;
2215         }
2216
2217         return 0;
2218 }
2219
2220 static int idtcm_enable_channel(struct idtcm *idtcm, u32 index)
2221 {
2222         struct idtcm_channel *channel;
2223         int err;
2224
2225         if (!(index < MAX_TOD))
2226                 return -EINVAL;
2227
2228         channel = &idtcm->channel[index];
2229
2230         channel->idtcm = idtcm;
2231         channel->current_freq_scaled_ppm = 0;
2232
2233         /* Set pll addresses */
2234         err = configure_channel_pll(channel);
2235         if (err)
2236                 return err;
2237
2238         /* Set tod addresses */
2239         err = configure_channel_tod(channel, index);
2240         if (err)
2241                 return err;
2242
2243         if (idtcm->fw_ver < V487)
2244                 channel->caps = idtcm_caps_deprecated;
2245         else
2246                 channel->caps = idtcm_caps;
2247
2248         snprintf(channel->caps.name, sizeof(channel->caps.name),
2249                  "IDT CM TOD%u", index);
2250
2251         err = initialize_dco_operating_mode(channel);
2252         if (err)
2253                 return err;
2254
2255         err = idtcm_enable_tod(channel);
2256         if (err) {
2257                 dev_err(idtcm->dev,
2258                         "Failed at line %d in %s!", __LINE__, __func__);
2259                 return err;
2260         }
2261
2262         channel->dco_delay = idtcm_get_dco_delay(channel);
2263
2264         channel->ptp_clock = ptp_clock_register(&channel->caps, NULL);
2265
2266         if (IS_ERR(channel->ptp_clock)) {
2267                 err = PTR_ERR(channel->ptp_clock);
2268                 channel->ptp_clock = NULL;
2269                 return err;
2270         }
2271
2272         if (!channel->ptp_clock)
2273                 return -ENOTSUPP;
2274
2275         dev_info(idtcm->dev, "PLL%d registered as ptp%d",
2276                  index, channel->ptp_clock->index);
2277
2278         return 0;
2279 }
2280
2281 static int idtcm_enable_extts_channel(struct idtcm *idtcm, u32 index)
2282 {
2283         struct idtcm_channel *channel;
2284         int err;
2285
2286         if (!(index < MAX_TOD))
2287                 return -EINVAL;
2288
2289         channel = &idtcm->channel[index];
2290         channel->idtcm = idtcm;
2291
2292         /* Set tod addresses */
2293         err = configure_channel_tod(channel, index);
2294         if (err)
2295                 return err;
2296
2297         channel->idtcm = idtcm;
2298
2299         return 0;
2300 }
2301
2302 static void idtcm_extts_check(struct work_struct *work)
2303 {
2304         struct idtcm *idtcm = container_of(work, struct idtcm, extts_work.work);
2305         int err, i;
2306
2307         if (idtcm->extts_mask == 0)
2308                 return;
2309
2310         mutex_lock(idtcm->lock);
2311         for (i = 0; i < MAX_TOD; i++) {
2312                 u8 mask = 1 << i;
2313
2314                 if (idtcm->extts_mask & mask) {
2315                         err = idtcm_extts_check_channel(idtcm, i);
2316                         /* trigger clears itself, so clear the mask */
2317                         if (err == 0)
2318                                 idtcm->extts_mask &= ~mask;
2319                 }
2320         }
2321
2322         if (idtcm->extts_mask)
2323                 schedule_delayed_work(&idtcm->extts_work,
2324                                       msecs_to_jiffies(EXTTS_PERIOD_MS));
2325         mutex_unlock(idtcm->lock);
2326 }
2327
2328 static void ptp_clock_unregister_all(struct idtcm *idtcm)
2329 {
2330         u8 i;
2331         struct idtcm_channel *channel;
2332
2333         for (i = 0; i < MAX_TOD; i++) {
2334                 channel = &idtcm->channel[i];
2335                 if (channel->ptp_clock)
2336                         ptp_clock_unregister(channel->ptp_clock);
2337         }
2338 }
2339
2340 static void set_default_masks(struct idtcm *idtcm)
2341 {
2342         idtcm->tod_mask = DEFAULT_TOD_MASK;
2343         idtcm->extts_mask = 0;
2344
2345         idtcm->channel[0].pll = DEFAULT_TOD0_PTP_PLL;
2346         idtcm->channel[1].pll = DEFAULT_TOD1_PTP_PLL;
2347         idtcm->channel[2].pll = DEFAULT_TOD2_PTP_PLL;
2348         idtcm->channel[3].pll = DEFAULT_TOD3_PTP_PLL;
2349
2350         idtcm->channel[0].output_mask = DEFAULT_OUTPUT_MASK_PLL0;
2351         idtcm->channel[1].output_mask = DEFAULT_OUTPUT_MASK_PLL1;
2352         idtcm->channel[2].output_mask = DEFAULT_OUTPUT_MASK_PLL2;
2353         idtcm->channel[3].output_mask = DEFAULT_OUTPUT_MASK_PLL3;
2354 }
2355
2356 static int idtcm_probe(struct platform_device *pdev)
2357 {
2358         struct rsmu_ddata *ddata = dev_get_drvdata(pdev->dev.parent);
2359         struct idtcm *idtcm;
2360         int err;
2361         u8 i;
2362
2363         idtcm = devm_kzalloc(&pdev->dev, sizeof(struct idtcm), GFP_KERNEL);
2364
2365         if (!idtcm)
2366                 return -ENOMEM;
2367
2368         idtcm->dev = &pdev->dev;
2369         idtcm->mfd = pdev->dev.parent;
2370         idtcm->lock = &ddata->lock;
2371         idtcm->regmap = ddata->regmap;
2372         idtcm->calculate_overhead_flag = 0;
2373
2374         INIT_DELAYED_WORK(&idtcm->extts_work, idtcm_extts_check);
2375
2376         set_default_masks(idtcm);
2377
2378         mutex_lock(idtcm->lock);
2379
2380         idtcm_set_version_info(idtcm);
2381
2382         err = idtcm_load_firmware(idtcm, &pdev->dev);
2383
2384         if (err)
2385                 dev_warn(idtcm->dev, "loading firmware failed with %d", err);
2386
2387         wait_for_chip_ready(idtcm);
2388
2389         if (idtcm->tod_mask) {
2390                 for (i = 0; i < MAX_TOD; i++) {
2391                         if (idtcm->tod_mask & (1 << i))
2392                                 err = idtcm_enable_channel(idtcm, i);
2393                         else
2394                                 err = idtcm_enable_extts_channel(idtcm, i);
2395                         if (err) {
2396                                 dev_err(idtcm->dev,
2397                                         "idtcm_enable_channel %d failed!", i);
2398                                 break;
2399                         }
2400                 }
2401         } else {
2402                 dev_err(idtcm->dev,
2403                         "no PLLs flagged as PHCs, nothing to do");
2404                 err = -ENODEV;
2405         }
2406
2407         mutex_unlock(idtcm->lock);
2408
2409         if (err) {
2410                 ptp_clock_unregister_all(idtcm);
2411                 return err;
2412         }
2413
2414         platform_set_drvdata(pdev, idtcm);
2415
2416         return 0;
2417 }
2418
2419 static int idtcm_remove(struct platform_device *pdev)
2420 {
2421         struct idtcm *idtcm = platform_get_drvdata(pdev);
2422
2423         ptp_clock_unregister_all(idtcm);
2424
2425         cancel_delayed_work_sync(&idtcm->extts_work);
2426
2427         return 0;
2428 }
2429
2430 static struct platform_driver idtcm_driver = {
2431         .driver = {
2432                 .name = "8a3400x-phc",
2433         },
2434         .probe = idtcm_probe,
2435         .remove = idtcm_remove,
2436 };
2437
2438 module_platform_driver(idtcm_driver);