drivers/ptp/ptp_idt82p33.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 //
   3 // Copyright (C) 2018 Integrated Device Technology, Inc
   4 //
   5
   6 #define pr_fmt(fmt) "IDT_82p33xxx: " fmt
   7
   8 #include <linux/firmware.h>
   9 #include <linux/i2c.h>
  10 #include <linux/module.h>
  11 #include <linux/ptp_clock_kernel.h>
  12 #include <linux/delay.h>
  13 #include <linux/kernel.h>
  14 #include <linux/timekeeping.h>
  15 #include <linux/bitops.h>
  16
  17 #include "ptp_private.h"
  18 #include "ptp_idt82p33.h"
  19
  20 MODULE_DESCRIPTION("Driver for IDT 82p33xxx clock devices");
  21 MODULE_AUTHOR("IDT support-1588 <IDT-support-1588@lm.renesas.com>");
  22 MODULE_VERSION("1.0");
  23 MODULE_LICENSE("GPL");
  24 MODULE_FIRMWARE(FW_FILENAME);
  25
  26 /* Module Parameters */
  27 static u32 sync_tod_timeout = SYNC_TOD_TIMEOUT_SEC;
  28 module_param(sync_tod_timeout, uint, 0);
  29 MODULE_PARM_DESC(sync_tod_timeout,
  30 "duration in second to keep SYNC_TOD on (set to 0 to keep it always on)");
  31
  32 static u32 phase_snap_threshold = SNAP_THRESHOLD_NS;
  33 module_param(phase_snap_threshold, uint, 0);
  34 MODULE_PARM_DESC(phase_snap_threshold,
  35 "threshold (150000ns by default) below which adjtime would ignore");
  36
  37 static void idt82p33_byte_array_to_timespec(struct timespec64 *ts,
  38                                             u8 buf[TOD_BYTE_COUNT])
  39 {
  40         time64_t sec;
  41         s32 nsec;
  42         u8 i;
  43
  44         nsec = buf[3];
  45         for (i = 0; i < 3; i++) {
  46                 nsec <<= 8;
  47                 nsec |= buf[2 - i];
  48         }
  49
  50         sec = buf[9];
  51         for (i = 0; i < 5; i++) {
  52                 sec <<= 8;
  53                 sec |= buf[8 - i];
  54         }
  55
  56         ts->tv_sec = sec;
  57         ts->tv_nsec = nsec;
  58 }
  59
  60 static void idt82p33_timespec_to_byte_array(struct timespec64 const *ts,
  61                                             u8 buf[TOD_BYTE_COUNT])
  62 {
  63         time64_t sec;
  64         s32 nsec;
  65         u8 i;
  66
  67         nsec = ts->tv_nsec;
  68         sec = ts->tv_sec;
  69
  70         for (i = 0; i < 4; i++) {
  71                 buf[i] = nsec & 0xff;
  72                 nsec >>= 8;
  73         }
  74
  75         for (i = 4; i < TOD_BYTE_COUNT; i++) {
  76                 buf[i] = sec & 0xff;
  77                 sec >>= 8;
  78         }
  79 }
  80
  81 static int idt82p33_xfer_read(struct idt82p33 *idt82p33,
  82                               unsigned char regaddr,
  83                               unsigned char *buf,
  84                               unsigned int count)
  85 {
  86         struct i2c_client *client = idt82p33->client;
  87         struct i2c_msg msg[2];
  88         int cnt;
  89
  90         msg[0].addr = client->addr;
  91         msg[0].flags = 0;
  92         msg[0].len = 1;
  93         msg[0].buf = &regaddr;
  94
  95         msg[1].addr = client->addr;
  96         msg[1].flags = I2C_M_RD;
  97         msg[1].len = count;
  98         msg[1].buf = buf;
  99
 100         cnt = i2c_transfer(client->adapter, msg, 2);
 101         if (cnt < 0) {
 102                 dev_err(&client->dev, "i2c_transfer returned %d\n", cnt);
 103                 return cnt;
 104         } else if (cnt != 2) {
 105                 dev_err(&client->dev,
 106                         "i2c_transfer sent only %d of %d messages\n", cnt, 2);
 107                 return -EIO;
 108         }
 109         return 0;
 110 }
 111
 112 static int idt82p33_xfer_write(struct idt82p33 *idt82p33,
 113                                u8 regaddr,
 114                                u8 *buf,
 115                                u16 count)
 116 {
 117         struct i2c_client *client = idt82p33->client;
 118         /* we add 1 byte for device register */
 119         u8 msg[IDT82P33_MAX_WRITE_COUNT + 1];
 120         int err;
 121
 122         if (count > IDT82P33_MAX_WRITE_COUNT)
 123                 return -EINVAL;
 124
 125         msg[0] = regaddr;
 126         memcpy(&msg[1], buf, count);
 127
 128         err = i2c_master_send(client, msg, count + 1);
 129         if (err < 0) {
 130                 dev_err(&client->dev, "i2c_master_send returned %d\n", err);
 131                 return err;
 132         }
 133
 134         return 0;
 135 }
 136
 137 static int idt82p33_page_offset(struct idt82p33 *idt82p33, unsigned char val)
 138 {
 139         int err;
 140
 141         if (idt82p33->page_offset == val)
 142                 return 0;
 143
 144         err = idt82p33_xfer_write(idt82p33, PAGE_ADDR, &val, sizeof(val));
 145         if (err)
 146                 dev_err(&idt82p33->client->dev,
 147                         "failed to set page offset %d\n", val);
 148         else
 149                 idt82p33->page_offset = val;
 150
 151         return err;
 152 }
 153
 154 static int idt82p33_rdwr(struct idt82p33 *idt82p33, unsigned int regaddr,
 155                          unsigned char *buf, unsigned int count, bool write)
 156 {
 157         u8 offset, page;
 158         int err;
 159
 160         page = _PAGE(regaddr);
 161         offset = _OFFSET(regaddr);
 162
 163         err = idt82p33_page_offset(idt82p33, page);
 164         if (err)
 165                 return err;
 166
 167         if (write)
 168                 return idt82p33_xfer_write(idt82p33, offset, buf, count);
 169
 170         return idt82p33_xfer_read(idt82p33, offset, buf, count);
 171 }
 172
 173 static int idt82p33_read(struct idt82p33 *idt82p33, unsigned int regaddr,
 174                         unsigned char *buf, unsigned int count)
 175 {
 176         return idt82p33_rdwr(idt82p33, regaddr, buf, count, false);
 177 }
 178
 179 static int idt82p33_write(struct idt82p33 *idt82p33, unsigned int regaddr,
 180                         unsigned char *buf, unsigned int count)
 181 {
 182         return idt82p33_rdwr(idt82p33, regaddr, buf, count, true);
 183 }
 184
 185 static int idt82p33_dpll_set_mode(struct idt82p33_channel *channel,
 186                                   enum pll_mode mode)
 187 {
 188         struct idt82p33 *idt82p33 = channel->idt82p33;
 189         u8 dpll_mode;
 190         int err;
 191
 192         if (channel->pll_mode == mode)
 193                 return 0;
 194
 195         err = idt82p33_read(idt82p33, channel->dpll_mode_cnfg,
 196                             &dpll_mode, sizeof(dpll_mode));
 197         if (err)
 198                 return err;
 199
 200         dpll_mode &= ~(PLL_MODE_MASK << PLL_MODE_SHIFT);
 201
 202         dpll_mode |= (mode << PLL_MODE_SHIFT);
 203
 204         err = idt82p33_write(idt82p33, channel->dpll_mode_cnfg,
 205                              &dpll_mode, sizeof(dpll_mode));
 206         if (err)
 207                 return err;
 208
 209         channel->pll_mode = dpll_mode;
 210
 211         return 0;
 212 }
 213
 214 static int _idt82p33_gettime(struct idt82p33_channel *channel,
 215                              struct timespec64 *ts)
 216 {
 217         struct idt82p33 *idt82p33 = channel->idt82p33;
 218         u8 buf[TOD_BYTE_COUNT];
 219         u8 trigger;
 220         int err;
 221
 222         trigger = TOD_TRIGGER(HW_TOD_WR_TRIG_SEL_MSB_TOD_CNFG,
 223                               HW_TOD_RD_TRIG_SEL_LSB_TOD_STS);
 224
 225
 226         err = idt82p33_write(idt82p33, channel->dpll_tod_trigger,
 227                              &trigger, sizeof(trigger));
 228
 229         if (err)
 230                 return err;
 231
 232         if (idt82p33->calculate_overhead_flag)
 233                 idt82p33->start_time = ktime_get_raw();
 234
 235         err = idt82p33_read(idt82p33, channel->dpll_tod_sts, buf, sizeof(buf));
 236
 237         if (err)
 238                 return err;
 239
 240         idt82p33_byte_array_to_timespec(ts, buf);
 241
 242         return 0;
 243 }
 244
 245 /*
 246  *   TOD Trigger:
 247  *   Bits[7:4] Write 0x9, MSB write
 248  *   Bits[3:0] Read 0x9, LSB read
 249  */
 250
 251 static int _idt82p33_settime(struct idt82p33_channel *channel,
 252                              struct timespec64 const *ts)
 253 {
 254         struct idt82p33 *idt82p33 = channel->idt82p33;
 255         struct timespec64 local_ts = *ts;
 256         char buf[TOD_BYTE_COUNT];
 257         s64 dynamic_overhead_ns;
 258         unsigned char trigger;
 259         int err;
 260         u8 i;
 261
 262         trigger = TOD_TRIGGER(HW_TOD_WR_TRIG_SEL_MSB_TOD_CNFG,
 263                               HW_TOD_RD_TRIG_SEL_LSB_TOD_STS);
 264
 265         err = idt82p33_write(idt82p33, channel->dpll_tod_trigger,
 266                         &trigger, sizeof(trigger));
 267
 268         if (err)
 269                 return err;
 270
 271         if (idt82p33->calculate_overhead_flag) {
 272                 dynamic_overhead_ns = ktime_to_ns(ktime_get_raw())
 273                                         - ktime_to_ns(idt82p33->start_time);
 274
 275                 timespec64_add_ns(&local_ts, dynamic_overhead_ns);
 276
 277                 idt82p33->calculate_overhead_flag = 0;
 278         }
 279
 280         idt82p33_timespec_to_byte_array(&local_ts, buf);
 281
 282         /*
 283          * Store the new time value.
 284          */
 285         for (i = 0; i < TOD_BYTE_COUNT; i++) {
 286                 err = idt82p33_write(idt82p33, channel->dpll_tod_cnfg + i,
 287                                      &buf[i], sizeof(buf[i]));
 288                 if (err)
 289                         return err;
 290         }
 291
 292         return err;
 293 }
 294
 295 static int _idt82p33_adjtime(struct idt82p33_channel *channel, s64 delta_ns)
 296 {
 297         struct idt82p33 *idt82p33 = channel->idt82p33;
 298         struct timespec64 ts;
 299         s64 now_ns;
 300         int err;
 301
 302         idt82p33->calculate_overhead_flag = 1;
 303
 304         err = _idt82p33_gettime(channel, &ts);
 305
 306         if (err)
 307                 return err;
 308
 309         now_ns = timespec64_to_ns(&ts);
 310         now_ns += delta_ns + idt82p33->tod_write_overhead_ns;
 311
 312         ts = ns_to_timespec64(now_ns);
 313
 314         err = _idt82p33_settime(channel, &ts);
 315
 316         return err;
 317 }
 318
 319 static int _idt82p33_adjfine(struct idt82p33_channel *channel, long scaled_ppm)
 320 {
 321         struct idt82p33 *idt82p33 = channel->idt82p33;
 322         unsigned char buf[5] = {0};
 323         int err, i;
 324         s64 fcw;
 325
 326         if (scaled_ppm == channel->current_freq_ppb)
 327                 return 0;
 328
 329         /*
 330          * Frequency Control Word unit is: 1.68 * 10^-10 ppm
 331          *
 332          * adjfreq:
 333          *       ppb * 10^9
 334          * FCW = ----------
 335          *          168
 336          *
 337          * adjfine:
 338          *       scaled_ppm * 5^12
 339          * FCW = -------------
 340          *         168 * 2^4
 341          */
 342
 343         fcw = scaled_ppm * 244140625ULL;
 344         fcw = div_s64(fcw, 2688);
 345
 346         for (i = 0; i < 5; i++) {
 347                 buf[i] = fcw & 0xff;
 348                 fcw >>= 8;
 349         }
 350
 351         err = idt82p33_dpll_set_mode(channel, PLL_MODE_DCO);
 352
 353         if (err)
 354                 return err;
 355
 356         err = idt82p33_write(idt82p33, channel->dpll_freq_cnfg,
 357                              buf, sizeof(buf));
 358
 359         if (err == 0)
 360                 channel->current_freq_ppb = scaled_ppm;
 361
 362         return err;
 363 }
 364
 365 static int idt82p33_measure_one_byte_write_overhead(
 366                 struct idt82p33_channel *channel, s64 *overhead_ns)
 367 {
 368         struct idt82p33 *idt82p33 = channel->idt82p33;
 369         ktime_t start, stop;
 370         s64 total_ns;
 371         u8 trigger;
 372         int err;
 373         u8 i;
 374
 375         total_ns = 0;
 376         *overhead_ns = 0;
 377         trigger = TOD_TRIGGER(HW_TOD_WR_TRIG_SEL_MSB_TOD_CNFG,
 378                               HW_TOD_RD_TRIG_SEL_LSB_TOD_STS);
 379
 380         for (i = 0; i < MAX_MEASURMENT_COUNT; i++) {
 381
 382                 start = ktime_get_raw();
 383
 384                 err = idt82p33_write(idt82p33, channel->dpll_tod_trigger,
 385                                      &trigger, sizeof(trigger));
 386
 387                 stop = ktime_get_raw();
 388
 389                 if (err)
 390                         return err;
 391
 392                 total_ns += ktime_to_ns(stop) - ktime_to_ns(start);
 393         }
 394
 395         *overhead_ns = div_s64(total_ns, MAX_MEASURMENT_COUNT);
 396
 397         return err;
 398 }
 399
 400 static int idt82p33_measure_tod_write_9_byte_overhead(
 401                         struct idt82p33_channel *channel)
 402 {
 403         struct idt82p33 *idt82p33 = channel->idt82p33;
 404         u8 buf[TOD_BYTE_COUNT];
 405         ktime_t start, stop;
 406         s64 total_ns;
 407         int err = 0;
 408         u8 i, j;
 409
 410         total_ns = 0;
 411         idt82p33->tod_write_overhead_ns = 0;
 412
 413         for (i = 0; i < MAX_MEASURMENT_COUNT; i++) {
 414
 415                 start = ktime_get_raw();
 416
 417                 /* Need one less byte for applicable overhead */
 418                 for (j = 0; j < (TOD_BYTE_COUNT - 1); j++) {
 419                         err = idt82p33_write(idt82p33,
 420                                              channel->dpll_tod_cnfg + i,
 421                                              &buf[i], sizeof(buf[i]));
 422                         if (err)
 423                                 return err;
 424                 }
 425
 426                 stop = ktime_get_raw();
 427
 428                 total_ns += ktime_to_ns(stop) - ktime_to_ns(start);
 429         }
 430
 431         idt82p33->tod_write_overhead_ns = div_s64(total_ns,
 432                                                   MAX_MEASURMENT_COUNT);
 433
 434         return err;
 435 }
 436
 437 static int idt82p33_measure_settime_gettime_gap_overhead(
 438                 struct idt82p33_channel *channel, s64 *overhead_ns)
 439 {
 440         struct timespec64 ts1 = {0, 0};
 441         struct timespec64 ts2;
 442         int err;
 443
 444         *overhead_ns = 0;
 445
 446         err = _idt82p33_settime(channel, &ts1);
 447
 448         if (err)
 449                 return err;
 450
 451         err = _idt82p33_gettime(channel, &ts2);
 452
 453         if (!err)
 454                 *overhead_ns = timespec64_to_ns(&ts2) - timespec64_to_ns(&ts1);
 455
 456         return err;
 457 }
 458
 459 static int idt82p33_measure_tod_write_overhead(struct idt82p33_channel *channel)
 460 {
 461         s64 trailing_overhead_ns, one_byte_write_ns, gap_ns;
 462         struct idt82p33 *idt82p33 = channel->idt82p33;
 463         int err;
 464
 465         idt82p33->tod_write_overhead_ns = 0;
 466
 467         err = idt82p33_measure_settime_gettime_gap_overhead(channel, &gap_ns);
 468
 469         if (err) {
 470                 dev_err(&idt82p33->client->dev,
 471                         "Failed in %s with err %d!\n", __func__, err);
 472                 return err;
 473         }
 474
 475         err = idt82p33_measure_one_byte_write_overhead(channel,
 476                                                        &one_byte_write_ns);
 477
 478         if (err)
 479                 return err;
 480
 481         err = idt82p33_measure_tod_write_9_byte_overhead(channel);
 482
 483         if (err)
 484                 return err;
 485
 486         trailing_overhead_ns = gap_ns - (2 * one_byte_write_ns);
 487
 488         idt82p33->tod_write_overhead_ns -= trailing_overhead_ns;
 489
 490         return err;
 491 }
 492
 493 static int idt82p33_check_and_set_masks(struct idt82p33 *idt82p33,
 494                                         u8 page,
 495                                         u8 offset,
 496                                         u8 val)
 497 {
 498         int err = 0;
 499
 500         if (page == PLLMASK_ADDR_HI && offset == PLLMASK_ADDR_LO) {
 501                 if ((val & 0xfc) || !(val & 0x3)) {
 502                         dev_err(&idt82p33->client->dev,
 503                                 "Invalid PLL mask 0x%hhx\n", val);
 504                         err = -EINVAL;
 505                 } else {
 506                         idt82p33->pll_mask = val;
 507                 }
 508         } else if (page == PLL0_OUTMASK_ADDR_HI &&
 509                 offset == PLL0_OUTMASK_ADDR_LO) {
 510                 idt82p33->channel[0].output_mask = val;
 511         } else if (page == PLL1_OUTMASK_ADDR_HI &&
 512                 offset == PLL1_OUTMASK_ADDR_LO) {
 513                 idt82p33->channel[1].output_mask = val;
 514         }
 515
 516         return err;
 517 }
 518
 519 static void idt82p33_display_masks(struct idt82p33 *idt82p33)
 520 {
 521         u8 mask, i;
 522
 523         dev_info(&idt82p33->client->dev,
 524                  "pllmask = 0x%02x\n", idt82p33->pll_mask);
 525
 526         for (i = 0; i < MAX_PHC_PLL; i++) {
 527                 mask = 1 << i;
 528
 529                 if (mask & idt82p33->pll_mask)
 530                         dev_info(&idt82p33->client->dev,
 531                                  "PLL%d output_mask = 0x%04x\n",
 532                                  i, idt82p33->channel[i].output_mask);
 533         }
 534 }
 535
 536 static int idt82p33_sync_tod(struct idt82p33_channel *channel, bool enable)
 537 {
 538         struct idt82p33 *idt82p33 = channel->idt82p33;
 539         u8 sync_cnfg;
 540         int err;
 541
 542         /* Turn it off after sync_tod_timeout seconds */
 543         if (enable && sync_tod_timeout)
 544                 ptp_schedule_worker(channel->ptp_clock,
 545                                     sync_tod_timeout * HZ);
 546
 547         err = idt82p33_read(idt82p33, channel->dpll_sync_cnfg,
 548                             &sync_cnfg, sizeof(sync_cnfg));
 549         if (err)
 550                 return err;
 551
 552         sync_cnfg &= ~SYNC_TOD;
 553         if (enable)
 554                 sync_cnfg |= SYNC_TOD;
 555
 556         return idt82p33_write(idt82p33, channel->dpll_sync_cnfg,
 557                               &sync_cnfg, sizeof(sync_cnfg));
 558 }
 559
 560 static long idt82p33_sync_tod_work_handler(struct ptp_clock_info *ptp)
 561 {
 562         struct idt82p33_channel *channel =
 563                         container_of(ptp, struct idt82p33_channel, caps);
 564         struct idt82p33 *idt82p33 = channel->idt82p33;
 565
 566         mutex_lock(&idt82p33->reg_lock);
 567
 568         (void)idt82p33_sync_tod(channel, false);
 569
 570         mutex_unlock(&idt82p33->reg_lock);
 571
 572         /* Return a negative value here to not reschedule */
 573         return -1;
 574 }
 575
 576 static int idt82p33_output_enable(struct idt82p33_channel *channel,
 577                                   bool enable, unsigned int outn)
 578 {
 579         struct idt82p33 *idt82p33 = channel->idt82p33;
 580         int err;
 581         u8 val;
 582
 583         err = idt82p33_read(idt82p33, OUT_MUX_CNFG(outn), &val, sizeof(val));
 584         if (err)
 585                 return err;
 586         if (enable)
 587                 val &= ~SQUELCH_ENABLE;
 588         else
 589                 val |= SQUELCH_ENABLE;
 590
 591         return idt82p33_write(idt82p33, OUT_MUX_CNFG(outn), &val, sizeof(val));
 592 }
 593
 594 static int idt82p33_output_mask_enable(struct idt82p33_channel *channel,
 595                                        bool enable)
 596 {
 597         u16 mask;
 598         int err;
 599         u8 outn;
 600
 601         mask = channel->output_mask;
 602         outn = 0;
 603
 604         while (mask) {
 605                 if (mask & 0x1) {
 606                         err = idt82p33_output_enable(channel, enable, outn);
 607                         if (err)
 608                                 return err;
 609                 }
 610
 611                 mask >>= 0x1;
 612                 outn++;
 613         }
 614
 615         return 0;
 616 }
 617
 618 static int idt82p33_perout_enable(struct idt82p33_channel *channel,
 619                                   bool enable,
 620                                   struct ptp_perout_request *perout)
 621 {
 622         unsigned int flags = perout->flags;
 623
 624         /* Enable/disable output based on output_mask */
 625         if (flags == PEROUT_ENABLE_OUTPUT_MASK)
 626                 return idt82p33_output_mask_enable(channel, enable);
 627
 628         /* Enable/disable individual output instead */
 629         return idt82p33_output_enable(channel, enable, perout->index);
 630 }
 631
 632 static int idt82p33_enable_tod(struct idt82p33_channel *channel)
 633 {
 634         struct idt82p33 *idt82p33 = channel->idt82p33;
 635         struct timespec64 ts = {0, 0};
 636         int err;
 637         u8 val;
 638
 639         val = 0;
 640         err = idt82p33_write(idt82p33, channel->dpll_input_mode_cnfg,
 641                              &val, sizeof(val));
 642         if (err)
 643                 return err;
 644
 645         err = idt82p33_measure_tod_write_overhead(channel);
 646
 647         if (err) {
 648                 dev_err(&idt82p33->client->dev,
 649                         "Failed in %s with err %d!\n", __func__, err);
 650                 return err;
 651         }
 652
 653         err = _idt82p33_settime(channel, &ts);
 654
 655         if (err)
 656                 return err;
 657
 658         return idt82p33_sync_tod(channel, true);
 659 }
 660
 661 static void idt82p33_ptp_clock_unregister_all(struct idt82p33 *idt82p33)
 662 {
 663         struct idt82p33_channel *channel;
 664         u8 i;
 665
 666         for (i = 0; i < MAX_PHC_PLL; i++) {
 667
 668                 channel = &idt82p33->channel[i];
 669
 670                 if (channel->ptp_clock)
 671                         ptp_clock_unregister(channel->ptp_clock);
 672         }
 673 }
 674
 675 static int idt82p33_enable(struct ptp_clock_info *ptp,
 676                          struct ptp_clock_request *rq, int on)
 677 {
 678         struct idt82p33_channel *channel =
 679                         container_of(ptp, struct idt82p33_channel, caps);
 680         struct idt82p33 *idt82p33 = channel->idt82p33;
 681         int err;
 682
 683         err = -EOPNOTSUPP;
 684
 685         mutex_lock(&idt82p33->reg_lock);
 686
 687         if (rq->type == PTP_CLK_REQ_PEROUT) {
 688                 if (!on)
 689                         err = idt82p33_perout_enable(channel, false,
 690                                                      &rq->perout);
 691                 /* Only accept a 1-PPS aligned to the second. */
 692                 else if (rq->perout.start.nsec || rq->perout.period.sec != 1 ||
 693                     rq->perout.period.nsec) {
 694                         err = -ERANGE;
 695                 } else
 696                         err = idt82p33_perout_enable(channel, true,
 697                                                      &rq->perout);
 698         }
 699
 700         mutex_unlock(&idt82p33->reg_lock);
 701
 702         return err;
 703 }
 704
 705 static int idt82p33_adjwritephase(struct ptp_clock_info *ptp, s32 offset_ns)
 706 {
 707         struct idt82p33_channel *channel =
 708                 container_of(ptp, struct idt82p33_channel, caps);
 709         struct idt82p33 *idt82p33 = channel->idt82p33;
 710         s64 offset_regval, offset_fs;
 711         u8 val[4] = {0};
 712         int err;
 713
 714         offset_fs = (s64)(-offset_ns) * 1000000;
 715
 716         if (offset_fs > WRITE_PHASE_OFFSET_LIMIT)
 717                 offset_fs = WRITE_PHASE_OFFSET_LIMIT;
 718         else if (offset_fs < -WRITE_PHASE_OFFSET_LIMIT)
 719                 offset_fs = -WRITE_PHASE_OFFSET_LIMIT;
 720
 721         /* Convert from phaseoffset_fs to register value */
 722         offset_regval = div_s64(offset_fs * 1000, IDT_T0DPLL_PHASE_RESOL);
 723
 724         val[0] = offset_regval & 0xFF;
 725         val[1] = (offset_regval >> 8) & 0xFF;
 726         val[2] = (offset_regval >> 16) & 0xFF;
 727         val[3] = (offset_regval >> 24) & 0x1F;
 728         val[3] |= PH_OFFSET_EN;
 729
 730         mutex_lock(&idt82p33->reg_lock);
 731
 732         err = idt82p33_dpll_set_mode(channel, PLL_MODE_WPH);
 733         if (err) {
 734                 dev_err(&idt82p33->client->dev,
 735                         "Failed in %s with err %d!\n", __func__, err);
 736                 goto out;
 737         }
 738
 739         err = idt82p33_write(idt82p33, channel->dpll_phase_cnfg, val,
 740                              sizeof(val));
 741
 742 out:
 743         mutex_unlock(&idt82p33->reg_lock);
 744         return err;
 745 }
 746
 747 static int idt82p33_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 748 {
 749         struct idt82p33_channel *channel =
 750                         container_of(ptp, struct idt82p33_channel, caps);
 751         struct idt82p33 *idt82p33 = channel->idt82p33;
 752         int err;
 753
 754         mutex_lock(&idt82p33->reg_lock);
 755         err = _idt82p33_adjfine(channel, scaled_ppm);
 756         if (err)
 757                 dev_err(&idt82p33->client->dev,
 758                         "Failed in %s with err %d!\n", __func__, err);
 759         mutex_unlock(&idt82p33->reg_lock);
 760
 761         return err;
 762 }
 763
 764 static int idt82p33_adjtime(struct ptp_clock_info *ptp, s64 delta_ns)
 765 {
 766         struct idt82p33_channel *channel =
 767                         container_of(ptp, struct idt82p33_channel, caps);
 768         struct idt82p33 *idt82p33 = channel->idt82p33;
 769         int err;
 770
 771         mutex_lock(&idt82p33->reg_lock);
 772
 773         if (abs(delta_ns) < phase_snap_threshold) {
 774                 mutex_unlock(&idt82p33->reg_lock);
 775                 return 0;
 776         }
 777
 778         err = _idt82p33_adjtime(channel, delta_ns);
 779
 780         if (err) {
 781                 mutex_unlock(&idt82p33->reg_lock);
 782                 dev_err(&idt82p33->client->dev,
 783                         "Adjtime failed in %s with err %d!\n", __func__, err);
 784                 return err;
 785         }
 786
 787         err = idt82p33_sync_tod(channel, true);
 788         if (err)
 789                 dev_err(&idt82p33->client->dev,
 790                         "Sync_tod failed in %s with err %d!\n", __func__, err);
 791
 792         mutex_unlock(&idt82p33->reg_lock);
 793
 794         return err;
 795 }
 796
 797 static int idt82p33_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 798 {
 799         struct idt82p33_channel *channel =
 800                         container_of(ptp, struct idt82p33_channel, caps);
 801         struct idt82p33 *idt82p33 = channel->idt82p33;
 802         int err;
 803
 804         mutex_lock(&idt82p33->reg_lock);
 805         err = _idt82p33_gettime(channel, ts);
 806         if (err)
 807                 dev_err(&idt82p33->client->dev,
 808                         "Failed in %s with err %d!\n", __func__, err);
 809         mutex_unlock(&idt82p33->reg_lock);
 810
 811         return err;
 812 }
 813
 814 static int idt82p33_settime(struct ptp_clock_info *ptp,
 815                         const struct timespec64 *ts)
 816 {
 817         struct idt82p33_channel *channel =
 818                         container_of(ptp, struct idt82p33_channel, caps);
 819         struct idt82p33 *idt82p33 = channel->idt82p33;
 820         int err;
 821
 822         mutex_lock(&idt82p33->reg_lock);
 823         err = _idt82p33_settime(channel, ts);
 824         if (err)
 825                 dev_err(&idt82p33->client->dev,
 826                         "Failed in %s with err %d!\n", __func__, err);
 827         mutex_unlock(&idt82p33->reg_lock);
 828
 829         return err;
 830 }
 831
 832 static int idt82p33_channel_init(struct idt82p33_channel *channel, int index)
 833 {
 834         switch (index) {
 835         case 0:
 836                 channel->dpll_tod_cnfg = DPLL1_TOD_CNFG;
 837                 channel->dpll_tod_trigger = DPLL1_TOD_TRIGGER;
 838                 channel->dpll_tod_sts = DPLL1_TOD_STS;
 839                 channel->dpll_mode_cnfg = DPLL1_OPERATING_MODE_CNFG;
 840                 channel->dpll_freq_cnfg = DPLL1_HOLDOVER_FREQ_CNFG;
 841                 channel->dpll_phase_cnfg = DPLL1_PHASE_OFFSET_CNFG;
 842                 channel->dpll_sync_cnfg = DPLL1_SYNC_EDGE_CNFG;
 843                 channel->dpll_input_mode_cnfg = DPLL1_INPUT_MODE_CNFG;
 844                 break;
 845         case 1:
 846                 channel->dpll_tod_cnfg = DPLL2_TOD_CNFG;
 847                 channel->dpll_tod_trigger = DPLL2_TOD_TRIGGER;
 848                 channel->dpll_tod_sts = DPLL2_TOD_STS;
 849                 channel->dpll_mode_cnfg = DPLL2_OPERATING_MODE_CNFG;
 850                 channel->dpll_freq_cnfg = DPLL2_HOLDOVER_FREQ_CNFG;
 851                 channel->dpll_phase_cnfg = DPLL2_PHASE_OFFSET_CNFG;
 852                 channel->dpll_sync_cnfg = DPLL2_SYNC_EDGE_CNFG;
 853                 channel->dpll_input_mode_cnfg = DPLL2_INPUT_MODE_CNFG;
 854                 break;
 855         default:
 856                 return -EINVAL;
 857         }
 858
 859         channel->current_freq_ppb = 0;
 860
 861         return 0;
 862 }
 863
 864 static void idt82p33_caps_init(struct ptp_clock_info *caps)
 865 {
 866         caps->owner = THIS_MODULE;
 867         caps->max_adj = 92000;
 868         caps->n_per_out = 11;
 869         caps->adjphase = idt82p33_adjwritephase;
 870         caps->adjfine = idt82p33_adjfine;
 871         caps->adjtime = idt82p33_adjtime;
 872         caps->gettime64 = idt82p33_gettime;
 873         caps->settime64 = idt82p33_settime;
 874         caps->enable = idt82p33_enable;
 875         caps->do_aux_work = idt82p33_sync_tod_work_handler;
 876 }
 877
 878 static int idt82p33_enable_channel(struct idt82p33 *idt82p33, u32 index)
 879 {
 880         struct idt82p33_channel *channel;
 881         int err;
 882
 883         if (!(index < MAX_PHC_PLL))
 884                 return -EINVAL;
 885
 886         channel = &idt82p33->channel[index];
 887
 888         err = idt82p33_channel_init(channel, index);
 889         if (err) {
 890                 dev_err(&idt82p33->client->dev,
 891                         "Channel_init failed in %s with err %d!\n",
 892                         __func__, err);
 893                 return err;
 894         }
 895
 896         channel->idt82p33 = idt82p33;
 897
 898         idt82p33_caps_init(&channel->caps);
 899         snprintf(channel->caps.name, sizeof(channel->caps.name),
 900                  "IDT 82P33 PLL%u", index);
 901
 902         channel->ptp_clock = ptp_clock_register(&channel->caps, NULL);
 903
 904         if (IS_ERR(channel->ptp_clock)) {
 905                 err = PTR_ERR(channel->ptp_clock);
 906                 channel->ptp_clock = NULL;
 907                 return err;
 908         }
 909
 910         if (!channel->ptp_clock)
 911                 return -ENOTSUPP;
 912
 913         err = idt82p33_dpll_set_mode(channel, PLL_MODE_DCO);
 914         if (err) {
 915                 dev_err(&idt82p33->client->dev,
 916                         "Dpll_set_mode failed in %s with err %d!\n",
 917                         __func__, err);
 918                 return err;
 919         }
 920
 921         err = idt82p33_enable_tod(channel);
 922         if (err) {
 923                 dev_err(&idt82p33->client->dev,
 924                         "Enable_tod failed in %s with err %d!\n",
 925                         __func__, err);
 926                 return err;
 927         }
 928
 929         dev_info(&idt82p33->client->dev, "PLL%d registered as ptp%d\n",
 930                  index, channel->ptp_clock->index);
 931
 932         return 0;
 933 }
 934
 935 static int idt82p33_load_firmware(struct idt82p33 *idt82p33)
 936 {
 937         const struct firmware *fw;
 938         struct idt82p33_fwrc *rec;
 939         u8 loaddr, page, val;
 940         int err;
 941         s32 len;
 942
 943         dev_dbg(&idt82p33->client->dev,
 944                 "requesting firmware '%s'\n", FW_FILENAME);
 945
 946         err = request_firmware(&fw, FW_FILENAME, &idt82p33->client->dev);
 947
 948         if (err) {
 949                 dev_err(&idt82p33->client->dev,
 950                         "Failed in %s with err %d!\n", __func__, err);
 951                 return err;
 952         }
 953
 954         dev_dbg(&idt82p33->client->dev, "firmware size %zu bytes\n", fw->size);
 955
 956         rec = (struct idt82p33_fwrc *) fw->data;
 957
 958         for (len = fw->size; len > 0; len -= sizeof(*rec)) {
 959
 960                 if (rec->reserved) {
 961                         dev_err(&idt82p33->client->dev,
 962                                 "bad firmware, reserved field non-zero\n");
 963                         err = -EINVAL;
 964                 } else {
 965                         val = rec->value;
 966                         loaddr = rec->loaddr;
 967                         page = rec->hiaddr;
 968
 969                         rec++;
 970
 971                         err = idt82p33_check_and_set_masks(idt82p33, page,
 972                                                            loaddr, val);
 973                 }
 974
 975                 if (err == 0) {
 976                         /* maximum 8 pages  */
 977                         if (page >= PAGE_NUM)
 978                                 continue;
 979
 980                         /* Page size 128, last 4 bytes of page skipped */
 981                         if (((loaddr > 0x7b) && (loaddr <= 0x7f))
 982                              || loaddr > 0xfb)
 983                                 continue;
 984
 985                         err = idt82p33_write(idt82p33, _ADDR(page, loaddr),
 986                                              &val, sizeof(val));
 987                 }
 988
 989                 if (err)
 990                         goto out;
 991         }
 992
 993         idt82p33_display_masks(idt82p33);
 994 out:
 995         release_firmware(fw);
 996         return err;
 997 }
 998
 999
1000 static int idt82p33_probe(struct i2c_client *client,
1001                           const struct i2c_device_id *id)
1002 {
1003         struct idt82p33 *idt82p33;
1004         int err;
1005         u8 i;
1006
1007         (void)id;
1008
1009         idt82p33 = devm_kzalloc(&client->dev,
1010                                 sizeof(struct idt82p33), GFP_KERNEL);
1011         if (!idt82p33)
1012                 return -ENOMEM;
1013
1014         mutex_init(&idt82p33->reg_lock);
1015
1016         idt82p33->client = client;
1017         idt82p33->page_offset = 0xff;
1018         idt82p33->tod_write_overhead_ns = 0;
1019         idt82p33->calculate_overhead_flag = 0;
1020         idt82p33->pll_mask = DEFAULT_PLL_MASK;
1021         idt82p33->channel[0].output_mask = DEFAULT_OUTPUT_MASK_PLL0;
1022         idt82p33->channel[1].output_mask = DEFAULT_OUTPUT_MASK_PLL1;
1023
1024         mutex_lock(&idt82p33->reg_lock);
1025
1026         err = idt82p33_load_firmware(idt82p33);
1027
1028         if (err)
1029                 dev_warn(&idt82p33->client->dev,
1030                          "loading firmware failed with %d\n", err);
1031
1032         if (idt82p33->pll_mask) {
1033                 for (i = 0; i < MAX_PHC_PLL; i++) {
1034                         if (idt82p33->pll_mask & (1 << i)) {
1035                                 err = idt82p33_enable_channel(idt82p33, i);
1036                                 if (err) {
1037                                         dev_err(&idt82p33->client->dev,
1038                                                 "Failed in %s with err %d!\n",
1039                                                 __func__, err);
1040                                         break;
1041                                 }
1042                         }
1043                 }
1044         } else {
1045                 dev_err(&idt82p33->client->dev,
1046                         "no PLLs flagged as PHCs, nothing to do\n");
1047                 err = -ENODEV;
1048         }
1049
1050         mutex_unlock(&idt82p33->reg_lock);
1051
1052         if (err) {
1053                 idt82p33_ptp_clock_unregister_all(idt82p33);
1054                 return err;
1055         }
1056
1057         i2c_set_clientdata(client, idt82p33);
1058
1059         return 0;
1060 }
1061
1062 static int idt82p33_remove(struct i2c_client *client)
1063 {
1064         struct idt82p33 *idt82p33 = i2c_get_clientdata(client);
1065
1066         idt82p33_ptp_clock_unregister_all(idt82p33);
1067         mutex_destroy(&idt82p33->reg_lock);
1068
1069         return 0;
1070 }
1071
1072 #ifdef CONFIG_OF
1073 static const struct of_device_id idt82p33_dt_id[] = {
1074         { .compatible = "idt,82p33810" },
1075         { .compatible = "idt,82p33813" },
1076         { .compatible = "idt,82p33814" },
1077         { .compatible = "idt,82p33831" },
1078         { .compatible = "idt,82p33910" },
1079         { .compatible = "idt,82p33913" },
1080         { .compatible = "idt,82p33914" },
1081         { .compatible = "idt,82p33931" },
1082         {},
1083 };
1084 MODULE_DEVICE_TABLE(of, idt82p33_dt_id);
1085 #endif
1086
1087 static const struct i2c_device_id idt82p33_i2c_id[] = {
1088         { "idt82p33810", },
1089         { "idt82p33813", },
1090         { "idt82p33814", },
1091         { "idt82p33831", },
1092         { "idt82p33910", },
1093         { "idt82p33913", },
1094         { "idt82p33914", },
1095         { "idt82p33931", },
1096         {},
1097 };
1098 MODULE_DEVICE_TABLE(i2c, idt82p33_i2c_id);
1099
1100 static struct i2c_driver idt82p33_driver = {
1101         .driver = {
1102                 .of_match_table = of_match_ptr(idt82p33_dt_id),
1103                 .name           = "idt82p33",
1104         },
1105         .probe          = idt82p33_probe,
1106         .remove         = idt82p33_remove,
1107         .id_table       = idt82p33_i2c_id,
1108 };
1109
1110 module_i2c_driver(idt82p33_driver);