drivers/net/ethernet/microchip/lan743x_ptp.c

   1 /* SPDX-License-Identifier: GPL-2.0+ */
   2 /* Copyright (C) 2018 Microchip Technology Inc. */
   3
   4 #include <linux/netdevice.h>
   5 #include "lan743x_main.h"
   6
   7 #include <linux/ptp_clock_kernel.h>
   8 #include <linux/module.h>
   9 #include <linux/pci.h>
  10 #include <linux/net_tstamp.h>
  11
  12 #include "lan743x_ptp.h"
  13
  14 #define LAN743X_NUMBER_OF_GPIO                  (12)
  15 #define LAN743X_PTP_MAX_FREQ_ADJ_IN_PPB         (31249999)
  16 #define LAN743X_PTP_MAX_FINE_ADJ_IN_SCALED_PPM  (2047999934)
  17
  18 static bool lan743x_ptp_is_enabled(struct lan743x_adapter *adapter);
  19 static void lan743x_ptp_enable(struct lan743x_adapter *adapter);
  20 static void lan743x_ptp_disable(struct lan743x_adapter *adapter);
  21 static void lan743x_ptp_reset(struct lan743x_adapter *adapter);
  22 static void lan743x_ptp_clock_set(struct lan743x_adapter *adapter,
  23                                   u32 seconds, u32 nano_seconds,
  24                                   u32 sub_nano_seconds);
  25
  26 int lan743x_gpio_init(struct lan743x_adapter *adapter)
  27 {
  28         struct lan743x_gpio *gpio = &adapter->gpio;
  29
  30         spin_lock_init(&gpio->gpio_lock);
  31
  32         gpio->gpio_cfg0 = 0; /* set all direction to input, data = 0 */
  33         gpio->gpio_cfg1 = 0x0FFF0000;/* disable all gpio, set to open drain */
  34         gpio->gpio_cfg2 = 0;/* set all to 1588 low polarity level */
  35         gpio->gpio_cfg3 = 0;/* disable all 1588 output */
  36         lan743x_csr_write(adapter, GPIO_CFG0, gpio->gpio_cfg0);
  37         lan743x_csr_write(adapter, GPIO_CFG1, gpio->gpio_cfg1);
  38         lan743x_csr_write(adapter, GPIO_CFG2, gpio->gpio_cfg2);
  39         lan743x_csr_write(adapter, GPIO_CFG3, gpio->gpio_cfg3);
  40
  41         return 0;
  42 }
  43
  44 static void lan743x_ptp_wait_till_cmd_done(struct lan743x_adapter *adapter,
  45                                            u32 bit_mask)
  46 {
  47         int timeout = 1000;
  48         u32 data = 0;
  49
  50         while (timeout &&
  51                (data = (lan743x_csr_read(adapter, PTP_CMD_CTL) &
  52                bit_mask))) {
  53                 usleep_range(1000, 20000);
  54                 timeout--;
  55         }
  56         if (data) {
  57                 netif_err(adapter, drv, adapter->netdev,
  58                           "timeout waiting for cmd to be done, cmd = 0x%08X\n",
  59                           bit_mask);
  60         }
  61 }
  62
  63 static void lan743x_ptp_tx_ts_enqueue_ts(struct lan743x_adapter *adapter,
  64                                          u32 seconds, u32 nano_seconds,
  65                                          u32 header)
  66 {
  67         struct lan743x_ptp *ptp = &adapter->ptp;
  68
  69         spin_lock_bh(&ptp->tx_ts_lock);
  70         if (ptp->tx_ts_queue_size < LAN743X_PTP_NUMBER_OF_TX_TIMESTAMPS) {
  71                 ptp->tx_ts_seconds_queue[ptp->tx_ts_queue_size] = seconds;
  72                 ptp->tx_ts_nseconds_queue[ptp->tx_ts_queue_size] = nano_seconds;
  73                 ptp->tx_ts_header_queue[ptp->tx_ts_queue_size] = header;
  74                 ptp->tx_ts_queue_size++;
  75         } else {
  76                 netif_err(adapter, drv, adapter->netdev,
  77                           "tx ts queue overflow\n");
  78         }
  79         spin_unlock_bh(&ptp->tx_ts_lock);
  80 }
  81
  82 static void lan743x_ptp_tx_ts_complete(struct lan743x_adapter *adapter)
  83 {
  84         struct lan743x_ptp *ptp = &adapter->ptp;
  85         struct skb_shared_hwtstamps tstamps;
  86         u32 header, nseconds, seconds;
  87         bool ignore_sync = false;
  88         struct sk_buff *skb;
  89         int c, i;
  90
  91         spin_lock_bh(&ptp->tx_ts_lock);
  92         c = ptp->tx_ts_skb_queue_size;
  93
  94         if (c > ptp->tx_ts_queue_size)
  95                 c = ptp->tx_ts_queue_size;
  96         if (c <= 0)
  97                 goto done;
  98
  99         for (i = 0; i < c; i++) {
 100                 ignore_sync = ((ptp->tx_ts_ignore_sync_queue &
 101                                 BIT(i)) != 0);
 102                 skb = ptp->tx_ts_skb_queue[i];
 103                 nseconds = ptp->tx_ts_nseconds_queue[i];
 104                 seconds = ptp->tx_ts_seconds_queue[i];
 105                 header = ptp->tx_ts_header_queue[i];
 106
 107                 memset(&tstamps, 0, sizeof(tstamps));
 108                 tstamps.hwtstamp = ktime_set(seconds, nseconds);
 109                 if (!ignore_sync ||
 110                     ((header & PTP_TX_MSG_HEADER_MSG_TYPE_) !=
 111                     PTP_TX_MSG_HEADER_MSG_TYPE_SYNC_))
 112                         skb_tstamp_tx(skb, &tstamps);
 113
 114                 dev_kfree_skb(skb);
 115
 116                 ptp->tx_ts_skb_queue[i] = NULL;
 117                 ptp->tx_ts_seconds_queue[i] = 0;
 118                 ptp->tx_ts_nseconds_queue[i] = 0;
 119                 ptp->tx_ts_header_queue[i] = 0;
 120         }
 121
 122         /* shift queue */
 123         ptp->tx_ts_ignore_sync_queue >>= c;
 124         for (i = c; i < LAN743X_PTP_NUMBER_OF_TX_TIMESTAMPS; i++) {
 125                 ptp->tx_ts_skb_queue[i - c] = ptp->tx_ts_skb_queue[i];
 126                 ptp->tx_ts_seconds_queue[i - c] = ptp->tx_ts_seconds_queue[i];
 127                 ptp->tx_ts_nseconds_queue[i - c] = ptp->tx_ts_nseconds_queue[i];
 128                 ptp->tx_ts_header_queue[i - c] = ptp->tx_ts_header_queue[i];
 129
 130                 ptp->tx_ts_skb_queue[i] = NULL;
 131                 ptp->tx_ts_seconds_queue[i] = 0;
 132                 ptp->tx_ts_nseconds_queue[i] = 0;
 133                 ptp->tx_ts_header_queue[i] = 0;
 134         }
 135         ptp->tx_ts_skb_queue_size -= c;
 136         ptp->tx_ts_queue_size -= c;
 137 done:
 138         ptp->pending_tx_timestamps -= c;
 139         spin_unlock_bh(&ptp->tx_ts_lock);
 140 }
 141
 142 static int lan743x_ptp_reserve_event_ch(struct lan743x_adapter *adapter)
 143 {
 144         struct lan743x_ptp *ptp = &adapter->ptp;
 145         int result = -ENODEV;
 146         int index = 0;
 147
 148         mutex_lock(&ptp->command_lock);
 149         for (index = 0; index < LAN743X_PTP_NUMBER_OF_EVENT_CHANNELS; index++) {
 150                 if (!(test_bit(index, &ptp->used_event_ch))) {
 151                         ptp->used_event_ch |= BIT(index);
 152                         result = index;
 153                         break;
 154                 }
 155         }
 156         mutex_unlock(&ptp->command_lock);
 157         return result;
 158 }
 159
 160 static void lan743x_ptp_release_event_ch(struct lan743x_adapter *adapter,
 161                                          int event_channel)
 162 {
 163         struct lan743x_ptp *ptp = &adapter->ptp;
 164
 165         mutex_lock(&ptp->command_lock);
 166         if (test_bit(event_channel, &ptp->used_event_ch)) {
 167                 ptp->used_event_ch &= ~BIT(event_channel);
 168         } else {
 169                 netif_warn(adapter, drv, adapter->netdev,
 170                            "attempted release on a not used event_channel = %d\n",
 171                            event_channel);
 172         }
 173         mutex_unlock(&ptp->command_lock);
 174 }
 175
 176 static void lan743x_ptp_clock_get(struct lan743x_adapter *adapter,
 177                                   u32 *seconds, u32 *nano_seconds,
 178                                   u32 *sub_nano_seconds);
 179 static void lan743x_ptp_clock_step(struct lan743x_adapter *adapter,
 180                                    s64 time_step_ns);
 181
 182 static int lan743x_gpio_rsrv_ptp_out(struct lan743x_adapter *adapter,
 183                                      int bit, int ptp_channel)
 184 {
 185         struct lan743x_gpio *gpio = &adapter->gpio;
 186         unsigned long irq_flags = 0;
 187         int bit_mask = BIT(bit);
 188         int ret = -EBUSY;
 189
 190         spin_lock_irqsave(&gpio->gpio_lock, irq_flags);
 191
 192         if (!(gpio->used_bits & bit_mask)) {
 193                 gpio->used_bits |= bit_mask;
 194                 gpio->output_bits |= bit_mask;
 195                 gpio->ptp_bits |= bit_mask;
 196
 197                 /* set as output, and zero initial value */
 198                 gpio->gpio_cfg0 |= GPIO_CFG0_GPIO_DIR_BIT_(bit);
 199                 gpio->gpio_cfg0 &= ~GPIO_CFG0_GPIO_DATA_BIT_(bit);
 200                 lan743x_csr_write(adapter, GPIO_CFG0, gpio->gpio_cfg0);
 201
 202                 /* enable gpio, and set buffer type to push pull */
 203                 gpio->gpio_cfg1 &= ~GPIO_CFG1_GPIOEN_BIT_(bit);
 204                 gpio->gpio_cfg1 |= GPIO_CFG1_GPIOBUF_BIT_(bit);
 205                 lan743x_csr_write(adapter, GPIO_CFG1, gpio->gpio_cfg1);
 206
 207                 /* set 1588 polarity to high */
 208                 gpio->gpio_cfg2 |= GPIO_CFG2_1588_POL_BIT_(bit);
 209                 lan743x_csr_write(adapter, GPIO_CFG2, gpio->gpio_cfg2);
 210
 211                 if (!ptp_channel) {
 212                         /* use channel A */
 213                         gpio->gpio_cfg3 &= ~GPIO_CFG3_1588_CH_SEL_BIT_(bit);
 214                 } else {
 215                         /* use channel B */
 216                         gpio->gpio_cfg3 |= GPIO_CFG3_1588_CH_SEL_BIT_(bit);
 217                 }
 218                 gpio->gpio_cfg3 |= GPIO_CFG3_1588_OE_BIT_(bit);
 219                 lan743x_csr_write(adapter, GPIO_CFG3, gpio->gpio_cfg3);
 220
 221                 ret = bit;
 222         }
 223         spin_unlock_irqrestore(&gpio->gpio_lock, irq_flags);
 224         return ret;
 225 }
 226
 227 static void lan743x_gpio_release(struct lan743x_adapter *adapter, int bit)
 228 {
 229         struct lan743x_gpio *gpio = &adapter->gpio;
 230         unsigned long irq_flags = 0;
 231         int bit_mask = BIT(bit);
 232
 233         spin_lock_irqsave(&gpio->gpio_lock, irq_flags);
 234         if (gpio->used_bits & bit_mask) {
 235                 gpio->used_bits &= ~bit_mask;
 236                 if (gpio->output_bits & bit_mask) {
 237                         gpio->output_bits &= ~bit_mask;
 238
 239                         if (gpio->ptp_bits & bit_mask) {
 240                                 gpio->ptp_bits &= ~bit_mask;
 241                                 /* disable ptp output */
 242                                 gpio->gpio_cfg3 &= ~GPIO_CFG3_1588_OE_BIT_(bit);
 243                                 lan743x_csr_write(adapter, GPIO_CFG3,
 244                                                   gpio->gpio_cfg3);
 245                         }
 246                         /* release gpio output */
 247
 248                         /* disable gpio */
 249                         gpio->gpio_cfg1 |= GPIO_CFG1_GPIOEN_BIT_(bit);
 250                         gpio->gpio_cfg1 &= ~GPIO_CFG1_GPIOBUF_BIT_(bit);
 251                         lan743x_csr_write(adapter, GPIO_CFG1, gpio->gpio_cfg1);
 252
 253                         /* reset back to input */
 254                         gpio->gpio_cfg0 &= ~GPIO_CFG0_GPIO_DIR_BIT_(bit);
 255                         gpio->gpio_cfg0 &= ~GPIO_CFG0_GPIO_DATA_BIT_(bit);
 256                         lan743x_csr_write(adapter, GPIO_CFG0, gpio->gpio_cfg0);
 257                 }
 258         }
 259         spin_unlock_irqrestore(&gpio->gpio_lock, irq_flags);
 260 }
 261
 262 static int lan743x_ptpci_adjfine(struct ptp_clock_info *ptpci, long scaled_ppm)
 263 {
 264         struct lan743x_ptp *ptp =
 265                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 266         struct lan743x_adapter *adapter =
 267                 container_of(ptp, struct lan743x_adapter, ptp);
 268         u32 lan743x_rate_adj = 0;
 269         bool positive = true;
 270         u64 u64_delta = 0;
 271
 272         if ((scaled_ppm < (-LAN743X_PTP_MAX_FINE_ADJ_IN_SCALED_PPM)) ||
 273             scaled_ppm > LAN743X_PTP_MAX_FINE_ADJ_IN_SCALED_PPM) {
 274                 return -EINVAL;
 275         }
 276         if (scaled_ppm > 0) {
 277                 u64_delta = (u64)scaled_ppm;
 278                 positive = true;
 279         } else {
 280                 u64_delta = (u64)(-scaled_ppm);
 281                 positive = false;
 282         }
 283         u64_delta = (u64_delta << 19);
 284         lan743x_rate_adj = div_u64(u64_delta, 1000000);
 285
 286         if (positive)
 287                 lan743x_rate_adj |= PTP_CLOCK_RATE_ADJ_DIR_;
 288
 289         lan743x_csr_write(adapter, PTP_CLOCK_RATE_ADJ,
 290                           lan743x_rate_adj);
 291
 292         return 0;
 293 }
 294
 295 static int lan743x_ptpci_adjfreq(struct ptp_clock_info *ptpci, s32 delta_ppb)
 296 {
 297         struct lan743x_ptp *ptp =
 298                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 299         struct lan743x_adapter *adapter =
 300                 container_of(ptp, struct lan743x_adapter, ptp);
 301         u32 lan743x_rate_adj = 0;
 302         bool positive = true;
 303         u32 u32_delta = 0;
 304         u64 u64_delta = 0;
 305
 306         if ((delta_ppb < (-LAN743X_PTP_MAX_FREQ_ADJ_IN_PPB)) ||
 307             delta_ppb > LAN743X_PTP_MAX_FREQ_ADJ_IN_PPB) {
 308                 return -EINVAL;
 309         }
 310         if (delta_ppb > 0) {
 311                 u32_delta = (u32)delta_ppb;
 312                 positive = true;
 313         } else {
 314                 u32_delta = (u32)(-delta_ppb);
 315                 positive = false;
 316         }
 317         u64_delta = (((u64)u32_delta) << 35);
 318         lan743x_rate_adj = div_u64(u64_delta, 1000000000);
 319
 320         if (positive)
 321                 lan743x_rate_adj |= PTP_CLOCK_RATE_ADJ_DIR_;
 322
 323         lan743x_csr_write(adapter, PTP_CLOCK_RATE_ADJ,
 324                           lan743x_rate_adj);
 325
 326         return 0;
 327 }
 328
 329 static int lan743x_ptpci_adjtime(struct ptp_clock_info *ptpci, s64 delta)
 330 {
 331         struct lan743x_ptp *ptp =
 332                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 333         struct lan743x_adapter *adapter =
 334                 container_of(ptp, struct lan743x_adapter, ptp);
 335
 336         lan743x_ptp_clock_step(adapter, delta);
 337
 338         return 0;
 339 }
 340
 341 static int lan743x_ptpci_gettime64(struct ptp_clock_info *ptpci,
 342                                    struct timespec64 *ts)
 343 {
 344         struct lan743x_ptp *ptp =
 345                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 346         struct lan743x_adapter *adapter =
 347                 container_of(ptp, struct lan743x_adapter, ptp);
 348         u32 nano_seconds = 0;
 349         u32 seconds = 0;
 350
 351         lan743x_ptp_clock_get(adapter, &seconds, &nano_seconds, NULL);
 352         ts->tv_sec = seconds;
 353         ts->tv_nsec = nano_seconds;
 354
 355         return 0;
 356 }
 357
 358 static int lan743x_ptpci_settime64(struct ptp_clock_info *ptpci,
 359                                    const struct timespec64 *ts)
 360 {
 361         struct lan743x_ptp *ptp =
 362                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 363         struct lan743x_adapter *adapter =
 364                 container_of(ptp, struct lan743x_adapter, ptp);
 365         u32 nano_seconds = 0;
 366         u32 seconds = 0;
 367
 368         if (ts) {
 369                 if (ts->tv_sec > 0xFFFFFFFFLL ||
 370                     ts->tv_sec < 0) {
 371                         netif_warn(adapter, drv, adapter->netdev,
 372                                    "ts->tv_sec out of range, %lld\n",
 373                                    ts->tv_sec);
 374                         return -ERANGE;
 375                 }
 376                 if (ts->tv_nsec >= 1000000000L ||
 377                     ts->tv_nsec < 0) {
 378                         netif_warn(adapter, drv, adapter->netdev,
 379                                    "ts->tv_nsec out of range, %ld\n",
 380                                    ts->tv_nsec);
 381                         return -ERANGE;
 382                 }
 383                 seconds = ts->tv_sec;
 384                 nano_seconds = ts->tv_nsec;
 385                 lan743x_ptp_clock_set(adapter, seconds, nano_seconds, 0);
 386         } else {
 387                 netif_warn(adapter, drv, adapter->netdev, "ts == NULL\n");
 388                 return -EINVAL;
 389         }
 390
 391         return 0;
 392 }
 393
 394 static void lan743x_ptp_perout_off(struct lan743x_adapter *adapter)
 395 {
 396         struct lan743x_ptp *ptp = &adapter->ptp;
 397         u32 general_config = 0;
 398
 399         if (ptp->perout_gpio_bit >= 0) {
 400                 lan743x_gpio_release(adapter, ptp->perout_gpio_bit);
 401                 ptp->perout_gpio_bit = -1;
 402         }
 403
 404         if (ptp->perout_event_ch >= 0) {
 405                 /* set target to far in the future, effectively disabling it */
 406                 lan743x_csr_write(adapter,
 407                                   PTP_CLOCK_TARGET_SEC_X(ptp->perout_event_ch),
 408                                   0xFFFF0000);
 409                 lan743x_csr_write(adapter,
 410                                   PTP_CLOCK_TARGET_NS_X(ptp->perout_event_ch),
 411                                   0);
 412
 413                 general_config = lan743x_csr_read(adapter, PTP_GENERAL_CONFIG);
 414                 general_config |= PTP_GENERAL_CONFIG_RELOAD_ADD_X_
 415                                   (ptp->perout_event_ch);
 416                 lan743x_csr_write(adapter, PTP_GENERAL_CONFIG, general_config);
 417                 lan743x_ptp_release_event_ch(adapter, ptp->perout_event_ch);
 418                 ptp->perout_event_ch = -1;
 419         }
 420 }
 421
 422 static int lan743x_ptp_perout(struct lan743x_adapter *adapter, int on,
 423                               struct ptp_perout_request *perout)
 424 {
 425         struct lan743x_ptp *ptp = &adapter->ptp;
 426         u32 period_sec = 0, period_nsec = 0;
 427         u32 start_sec = 0, start_nsec = 0;
 428         u32 general_config = 0;
 429         int pulse_width = 0;
 430         int perout_bit = 0;
 431
 432         if (!on) {
 433                 lan743x_ptp_perout_off(adapter);
 434                 return 0;
 435         }
 436
 437         if (ptp->perout_event_ch >= 0 ||
 438             ptp->perout_gpio_bit >= 0) {
 439                 /* already on, turn off first */
 440                 lan743x_ptp_perout_off(adapter);
 441         }
 442
 443         ptp->perout_event_ch = lan743x_ptp_reserve_event_ch(adapter);
 444         if (ptp->perout_event_ch < 0) {
 445                 netif_warn(adapter, drv, adapter->netdev,
 446                            "Failed to reserve event channel for PEROUT\n");
 447                 goto failed;
 448         }
 449
 450         switch (adapter->csr.id_rev & ID_REV_ID_MASK_) {
 451         case ID_REV_ID_LAN7430_:
 452                 perout_bit = 2;/* GPIO 2 is preferred on EVB LAN7430 */
 453                 break;
 454         case ID_REV_ID_LAN7431_:
 455                 perout_bit = 4;/* GPIO 4 is preferred on EVB LAN7431 */
 456                 break;
 457         }
 458
 459         ptp->perout_gpio_bit = lan743x_gpio_rsrv_ptp_out(adapter,
 460                                                          perout_bit,
 461                                                          ptp->perout_event_ch);
 462
 463         if (ptp->perout_gpio_bit < 0) {
 464                 netif_warn(adapter, drv, adapter->netdev,
 465                            "Failed to reserve gpio %d for PEROUT\n",
 466                            perout_bit);
 467                 goto failed;
 468         }
 469
 470         start_sec = perout->start.sec;
 471         start_sec += perout->start.nsec / 1000000000;
 472         start_nsec = perout->start.nsec % 1000000000;
 473
 474         period_sec = perout->period.sec;
 475         period_sec += perout->period.nsec / 1000000000;
 476         period_nsec = perout->period.nsec % 1000000000;
 477
 478         if (period_sec == 0) {
 479                 if (period_nsec >= 400000000) {
 480                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_200MS_;
 481                 } else if (period_nsec >= 20000000) {
 482                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_10MS_;
 483                 } else if (period_nsec >= 2000000) {
 484                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_1MS_;
 485                 } else if (period_nsec >= 200000) {
 486                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_100US_;
 487                 } else if (period_nsec >= 20000) {
 488                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_10US_;
 489                 } else if (period_nsec >= 200) {
 490                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_100NS_;
 491                 } else {
 492                         netif_warn(adapter, drv, adapter->netdev,
 493                                    "perout period too small, minimum is 200nS\n");
 494                         goto failed;
 495                 }
 496         } else {
 497                 pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_200MS_;
 498         }
 499
 500         /* turn off by setting target far in future */
 501         lan743x_csr_write(adapter,
 502                           PTP_CLOCK_TARGET_SEC_X(ptp->perout_event_ch),
 503                           0xFFFF0000);
 504         lan743x_csr_write(adapter,
 505                           PTP_CLOCK_TARGET_NS_X(ptp->perout_event_ch), 0);
 506
 507         /* Configure to pulse every period */
 508         general_config = lan743x_csr_read(adapter, PTP_GENERAL_CONFIG);
 509         general_config &= ~(PTP_GENERAL_CONFIG_CLOCK_EVENT_X_MASK_
 510                           (ptp->perout_event_ch));
 511         general_config |= PTP_GENERAL_CONFIG_CLOCK_EVENT_X_SET_
 512                           (ptp->perout_event_ch, pulse_width);
 513         general_config &= ~PTP_GENERAL_CONFIG_RELOAD_ADD_X_
 514                           (ptp->perout_event_ch);
 515         lan743x_csr_write(adapter, PTP_GENERAL_CONFIG, general_config);
 516
 517         /* set the reload to one toggle cycle */
 518         lan743x_csr_write(adapter,
 519                           PTP_CLOCK_TARGET_RELOAD_SEC_X(ptp->perout_event_ch),
 520                           period_sec);
 521         lan743x_csr_write(adapter,
 522                           PTP_CLOCK_TARGET_RELOAD_NS_X(ptp->perout_event_ch),
 523                           period_nsec);
 524
 525         /* set the start time */
 526         lan743x_csr_write(adapter,
 527                           PTP_CLOCK_TARGET_SEC_X(ptp->perout_event_ch),
 528                           start_sec);
 529         lan743x_csr_write(adapter,
 530                           PTP_CLOCK_TARGET_NS_X(ptp->perout_event_ch),
 531                           start_nsec);
 532
 533         return 0;
 534
 535 failed:
 536         lan743x_ptp_perout_off(adapter);
 537         return -ENODEV;
 538 }
 539
 540 static int lan743x_ptpci_enable(struct ptp_clock_info *ptpci,
 541                                 struct ptp_clock_request *request, int on)
 542 {
 543         struct lan743x_ptp *ptp =
 544                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 545         struct lan743x_adapter *adapter =
 546                 container_of(ptp, struct lan743x_adapter, ptp);
 547
 548         if (request) {
 549                 switch (request->type) {
 550                 case PTP_CLK_REQ_EXTTS:
 551                         return -EINVAL;
 552                 case PTP_CLK_REQ_PEROUT:
 553                         if (request->perout.index == 0)
 554                                 return lan743x_ptp_perout(adapter, on,
 555                                                           &request->perout);
 556                         return -EINVAL;
 557                 case PTP_CLK_REQ_PPS:
 558                         return -EINVAL;
 559                 default:
 560                         netif_err(adapter, drv, adapter->netdev,
 561                                   "request->type == %d, Unknown\n",
 562                                   request->type);
 563                         break;
 564                 }
 565         } else {
 566                 netif_err(adapter, drv, adapter->netdev, "request == NULL\n");
 567         }
 568         return 0;
 569 }
 570
 571 static long lan743x_ptpci_do_aux_work(struct ptp_clock_info *ptpci)
 572 {
 573         struct lan743x_ptp *ptp =
 574                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 575         struct lan743x_adapter *adapter =
 576                 container_of(ptp, struct lan743x_adapter, ptp);
 577         u32 cap_info, cause, header, nsec, seconds;
 578         bool new_timestamp_available = false;
 579         int count = 0;
 580
 581         while ((count < 100) &&
 582                (lan743x_csr_read(adapter, PTP_INT_STS) & PTP_INT_BIT_TX_TS_)) {
 583                 count++;
 584                 cap_info = lan743x_csr_read(adapter, PTP_CAP_INFO);
 585
 586                 if (PTP_CAP_INFO_TX_TS_CNT_GET_(cap_info) > 0) {
 587                         seconds = lan743x_csr_read(adapter,
 588                                                    PTP_TX_EGRESS_SEC);
 589                         nsec = lan743x_csr_read(adapter, PTP_TX_EGRESS_NS);
 590                         cause = (nsec &
 591                                  PTP_TX_EGRESS_NS_CAPTURE_CAUSE_MASK_);
 592                         header = lan743x_csr_read(adapter,
 593                                                   PTP_TX_MSG_HEADER);
 594
 595                         if (cause == PTP_TX_EGRESS_NS_CAPTURE_CAUSE_SW_) {
 596                                 nsec &= PTP_TX_EGRESS_NS_TS_NS_MASK_;
 597                                 lan743x_ptp_tx_ts_enqueue_ts(adapter,
 598                                                              seconds, nsec,
 599                                                              header);
 600                                 new_timestamp_available = true;
 601                         } else if (cause ==
 602                                 PTP_TX_EGRESS_NS_CAPTURE_CAUSE_AUTO_) {
 603                                 netif_err(adapter, drv, adapter->netdev,
 604                                           "Auto capture cause not supported\n");
 605                         } else {
 606                                 netif_warn(adapter, drv, adapter->netdev,
 607                                            "unknown tx timestamp capture cause\n");
 608                         }
 609                 } else {
 610                         netif_warn(adapter, drv, adapter->netdev,
 611                                    "TX TS INT but no TX TS CNT\n");
 612                 }
 613                 lan743x_csr_write(adapter, PTP_INT_STS, PTP_INT_BIT_TX_TS_);
 614         }
 615
 616         if (new_timestamp_available)
 617                 lan743x_ptp_tx_ts_complete(adapter);
 618
 619         lan743x_csr_write(adapter, INT_EN_SET, INT_BIT_1588_);
 620
 621         return -1;
 622 }
 623
 624 static void lan743x_ptp_clock_get(struct lan743x_adapter *adapter,
 625                                   u32 *seconds, u32 *nano_seconds,
 626                                   u32 *sub_nano_seconds)
 627 {
 628         struct lan743x_ptp *ptp = &adapter->ptp;
 629
 630         mutex_lock(&ptp->command_lock);
 631
 632         lan743x_csr_write(adapter, PTP_CMD_CTL, PTP_CMD_CTL_PTP_CLOCK_READ_);
 633         lan743x_ptp_wait_till_cmd_done(adapter, PTP_CMD_CTL_PTP_CLOCK_READ_);
 634
 635         if (seconds)
 636                 (*seconds) = lan743x_csr_read(adapter, PTP_CLOCK_SEC);
 637
 638         if (nano_seconds)
 639                 (*nano_seconds) = lan743x_csr_read(adapter, PTP_CLOCK_NS);
 640
 641         if (sub_nano_seconds)
 642                 (*sub_nano_seconds) =
 643                 lan743x_csr_read(adapter, PTP_CLOCK_SUBNS);
 644
 645         mutex_unlock(&ptp->command_lock);
 646 }
 647
 648 static void lan743x_ptp_clock_step(struct lan743x_adapter *adapter,
 649                                    s64 time_step_ns)
 650 {
 651         struct lan743x_ptp *ptp = &adapter->ptp;
 652         u32 nano_seconds_step = 0;
 653         u64 abs_time_step_ns = 0;
 654         u32 unsigned_seconds = 0;
 655         u32 nano_seconds = 0;
 656         u32 remainder = 0;
 657         s32 seconds = 0;
 658
 659         if (time_step_ns >  15000000000LL) {
 660                 /* convert to clock set */
 661                 lan743x_ptp_clock_get(adapter, &unsigned_seconds,
 662                                       &nano_seconds, NULL);
 663                 unsigned_seconds += div_u64_rem(time_step_ns, 1000000000LL,
 664                                                 &remainder);
 665                 nano_seconds += remainder;
 666                 if (nano_seconds >= 1000000000) {
 667                         unsigned_seconds++;
 668                         nano_seconds -= 1000000000;
 669                 }
 670                 lan743x_ptp_clock_set(adapter, unsigned_seconds,
 671                                       nano_seconds, 0);
 672                 return;
 673         } else if (time_step_ns < -15000000000LL) {
 674                 /* convert to clock set */
 675                 time_step_ns = -time_step_ns;
 676
 677                 lan743x_ptp_clock_get(adapter, &unsigned_seconds,
 678                                       &nano_seconds, NULL);
 679                 unsigned_seconds -= div_u64_rem(time_step_ns, 1000000000LL,
 680                                                 &remainder);
 681                 nano_seconds_step = remainder;
 682                 if (nano_seconds < nano_seconds_step) {
 683                         unsigned_seconds--;
 684                         nano_seconds += 1000000000;
 685                 }
 686                 nano_seconds -= nano_seconds_step;
 687                 lan743x_ptp_clock_set(adapter, unsigned_seconds,
 688                                       nano_seconds, 0);
 689                 return;
 690         }
 691
 692         /* do clock step */
 693         if (time_step_ns >= 0) {
 694                 abs_time_step_ns = (u64)(time_step_ns);
 695                 seconds = (s32)div_u64_rem(abs_time_step_ns, 1000000000,
 696                                            &remainder);
 697                 nano_seconds = (u32)remainder;
 698         } else {
 699                 abs_time_step_ns = (u64)(-time_step_ns);
 700                 seconds = -((s32)div_u64_rem(abs_time_step_ns, 1000000000,
 701                                              &remainder));
 702                 nano_seconds = (u32)remainder;
 703                 if (nano_seconds > 0) {
 704                         /* subtracting nano seconds is not allowed
 705                          * convert to subtracting from seconds,
 706                          * and adding to nanoseconds
 707                          */
 708                         seconds--;
 709                         nano_seconds = (1000000000 - nano_seconds);
 710                 }
 711         }
 712
 713         if (nano_seconds > 0) {
 714                 /* add 8 ns to cover the likely normal increment */
 715                 nano_seconds += 8;
 716         }
 717
 718         if (nano_seconds >= 1000000000) {
 719                 /* carry into seconds */
 720                 seconds++;
 721                 nano_seconds -= 1000000000;
 722         }
 723
 724         while (seconds) {
 725                 mutex_lock(&ptp->command_lock);
 726                 if (seconds > 0) {
 727                         u32 adjustment_value = (u32)seconds;
 728
 729                         if (adjustment_value > 0xF)
 730                                 adjustment_value = 0xF;
 731                         lan743x_csr_write(adapter, PTP_CLOCK_STEP_ADJ,
 732                                           PTP_CLOCK_STEP_ADJ_DIR_ |
 733                                           adjustment_value);
 734                         seconds -= ((s32)adjustment_value);
 735                 } else {
 736                         u32 adjustment_value = (u32)(-seconds);
 737
 738                         if (adjustment_value > 0xF)
 739                                 adjustment_value = 0xF;
 740                         lan743x_csr_write(adapter, PTP_CLOCK_STEP_ADJ,
 741                                           adjustment_value);
 742                         seconds += ((s32)adjustment_value);
 743                 }
 744                 lan743x_csr_write(adapter, PTP_CMD_CTL,
 745                                   PTP_CMD_CTL_PTP_CLOCK_STEP_SEC_);
 746                 lan743x_ptp_wait_till_cmd_done(adapter,
 747                                                PTP_CMD_CTL_PTP_CLOCK_STEP_SEC_);
 748                 mutex_unlock(&ptp->command_lock);
 749         }
 750         if (nano_seconds) {
 751                 mutex_lock(&ptp->command_lock);
 752                 lan743x_csr_write(adapter, PTP_CLOCK_STEP_ADJ,
 753                                   PTP_CLOCK_STEP_ADJ_DIR_ |
 754                                   (nano_seconds &
 755                                   PTP_CLOCK_STEP_ADJ_VALUE_MASK_));
 756                 lan743x_csr_write(adapter, PTP_CMD_CTL,
 757                                   PTP_CMD_CTL_PTP_CLK_STP_NSEC_);
 758                 lan743x_ptp_wait_till_cmd_done(adapter,
 759                                                PTP_CMD_CTL_PTP_CLK_STP_NSEC_);
 760                 mutex_unlock(&ptp->command_lock);
 761         }
 762 }
 763
 764 void lan743x_ptp_isr(void *context)
 765 {
 766         struct lan743x_adapter *adapter = (struct lan743x_adapter *)context;
 767         struct lan743x_ptp *ptp = NULL;
 768         int enable_flag = 1;
 769         u32 ptp_int_sts = 0;
 770
 771         ptp = &adapter->ptp;
 772
 773         lan743x_csr_write(adapter, INT_EN_CLR, INT_BIT_1588_);
 774
 775         ptp_int_sts = lan743x_csr_read(adapter, PTP_INT_STS);
 776         ptp_int_sts &= lan743x_csr_read(adapter, PTP_INT_EN_SET);
 777
 778         if (ptp_int_sts & PTP_INT_BIT_TX_TS_) {
 779                 ptp_schedule_worker(ptp->ptp_clock, 0);
 780                 enable_flag = 0;/* tasklet will re-enable later */
 781         }
 782         if (ptp_int_sts & PTP_INT_BIT_TX_SWTS_ERR_) {
 783                 netif_err(adapter, drv, adapter->netdev,
 784                           "PTP TX Software Timestamp Error\n");
 785                 /* clear int status bit */
 786                 lan743x_csr_write(adapter, PTP_INT_STS,
 787                                   PTP_INT_BIT_TX_SWTS_ERR_);
 788         }
 789         if (ptp_int_sts & PTP_INT_BIT_TIMER_B_) {
 790                 /* clear int status bit */
 791                 lan743x_csr_write(adapter, PTP_INT_STS,
 792                                   PTP_INT_BIT_TIMER_B_);
 793         }
 794         if (ptp_int_sts & PTP_INT_BIT_TIMER_A_) {
 795                 /* clear int status bit */
 796                 lan743x_csr_write(adapter, PTP_INT_STS,
 797                                   PTP_INT_BIT_TIMER_A_);
 798         }
 799
 800         if (enable_flag) {
 801                 /* re-enable isr */
 802                 lan743x_csr_write(adapter, INT_EN_SET, INT_BIT_1588_);
 803         }
 804 }
 805
 806 static void lan743x_ptp_tx_ts_enqueue_skb(struct lan743x_adapter *adapter,
 807                                           struct sk_buff *skb, bool ignore_sync)
 808 {
 809         struct lan743x_ptp *ptp = &adapter->ptp;
 810
 811         spin_lock_bh(&ptp->tx_ts_lock);
 812         if (ptp->tx_ts_skb_queue_size < LAN743X_PTP_NUMBER_OF_TX_TIMESTAMPS) {
 813                 ptp->tx_ts_skb_queue[ptp->tx_ts_skb_queue_size] = skb;
 814                 if (ignore_sync)
 815                         ptp->tx_ts_ignore_sync_queue |=
 816                                 BIT(ptp->tx_ts_skb_queue_size);
 817                 ptp->tx_ts_skb_queue_size++;
 818         } else {
 819                 /* this should never happen, so long as the tx channel
 820                  * calls and honors the result from
 821                  * lan743x_ptp_request_tx_timestamp
 822                  */
 823                 netif_err(adapter, drv, adapter->netdev,
 824                           "tx ts skb queue overflow\n");
 825                 dev_kfree_skb(skb);
 826         }
 827         spin_unlock_bh(&ptp->tx_ts_lock);
 828 }
 829
 830 static void lan743x_ptp_sync_to_system_clock(struct lan743x_adapter *adapter)
 831 {
 832         struct timespec64 ts;
 833
 834         ktime_get_clocktai_ts64(&ts);
 835
 836         lan743x_ptp_clock_set(adapter, ts.tv_sec, ts.tv_nsec, 0);
 837 }
 838
 839 void lan743x_ptp_update_latency(struct lan743x_adapter *adapter,
 840                                 u32 link_speed)
 841 {
 842         switch (link_speed) {
 843         case 10:
 844                 lan743x_csr_write(adapter, PTP_LATENCY,
 845                                   PTP_LATENCY_TX_SET_(0) |
 846                                   PTP_LATENCY_RX_SET_(0));
 847                 break;
 848         case 100:
 849                 lan743x_csr_write(adapter, PTP_LATENCY,
 850                                   PTP_LATENCY_TX_SET_(181) |
 851                                   PTP_LATENCY_RX_SET_(594));
 852                 break;
 853         case 1000:
 854                 lan743x_csr_write(adapter, PTP_LATENCY,
 855                                   PTP_LATENCY_TX_SET_(30) |
 856                                   PTP_LATENCY_RX_SET_(525));
 857                 break;
 858         }
 859 }
 860
 861 int lan743x_ptp_init(struct lan743x_adapter *adapter)
 862 {
 863         struct lan743x_ptp *ptp = &adapter->ptp;
 864
 865         mutex_init(&ptp->command_lock);
 866         spin_lock_init(&ptp->tx_ts_lock);
 867         ptp->used_event_ch = 0;
 868         ptp->perout_event_ch = -1;
 869         ptp->perout_gpio_bit = -1;
 870         return 0;
 871 }
 872
 873 int lan743x_ptp_open(struct lan743x_adapter *adapter)
 874 {
 875         struct lan743x_ptp *ptp = &adapter->ptp;
 876         int ret = -ENODEV;
 877         u32 temp;
 878
 879         lan743x_ptp_reset(adapter);
 880         lan743x_ptp_sync_to_system_clock(adapter);
 881         temp = lan743x_csr_read(adapter, PTP_TX_MOD2);
 882         temp |= PTP_TX_MOD2_TX_PTP_CLR_UDPV4_CHKSUM_;
 883         lan743x_csr_write(adapter, PTP_TX_MOD2, temp);
 884         lan743x_ptp_enable(adapter);
 885         lan743x_csr_write(adapter, INT_EN_SET, INT_BIT_1588_);
 886         lan743x_csr_write(adapter, PTP_INT_EN_SET,
 887                           PTP_INT_BIT_TX_SWTS_ERR_ | PTP_INT_BIT_TX_TS_);
 888         ptp->flags |= PTP_FLAG_ISR_ENABLED;
 889
 890         if (!IS_ENABLED(CONFIG_PTP_1588_CLOCK))
 891                 return 0;
 892
 893         snprintf(ptp->pin_config[0].name, 32, "lan743x_ptp_pin_0");
 894         ptp->pin_config[0].index = 0;
 895         ptp->pin_config[0].func = PTP_PF_PEROUT;
 896         ptp->pin_config[0].chan = 0;
 897
 898         ptp->ptp_clock_info.owner = THIS_MODULE;
 899         snprintf(ptp->ptp_clock_info.name, 16, "%pm",
 900                  adapter->netdev->dev_addr);
 901         ptp->ptp_clock_info.max_adj = LAN743X_PTP_MAX_FREQ_ADJ_IN_PPB;
 902         ptp->ptp_clock_info.n_alarm = 0;
 903         ptp->ptp_clock_info.n_ext_ts = 0;
 904         ptp->ptp_clock_info.n_per_out = 1;
 905         ptp->ptp_clock_info.n_pins = 0;
 906         ptp->ptp_clock_info.pps = 0;
 907         ptp->ptp_clock_info.pin_config = NULL;
 908         ptp->ptp_clock_info.adjfine = lan743x_ptpci_adjfine;
 909         ptp->ptp_clock_info.adjfreq = lan743x_ptpci_adjfreq;
 910         ptp->ptp_clock_info.adjtime = lan743x_ptpci_adjtime;
 911         ptp->ptp_clock_info.gettime64 = lan743x_ptpci_gettime64;
 912         ptp->ptp_clock_info.getcrosststamp = NULL;
 913         ptp->ptp_clock_info.settime64 = lan743x_ptpci_settime64;
 914         ptp->ptp_clock_info.enable = lan743x_ptpci_enable;
 915         ptp->ptp_clock_info.do_aux_work = lan743x_ptpci_do_aux_work;
 916         ptp->ptp_clock_info.verify = NULL;
 917
 918         ptp->ptp_clock = ptp_clock_register(&ptp->ptp_clock_info,
 919                                             &adapter->pdev->dev);
 920
 921         if (IS_ERR(ptp->ptp_clock)) {
 922                 netif_err(adapter, ifup, adapter->netdev,
 923                           "ptp_clock_register failed\n");
 924                 goto done;
 925         }
 926         ptp->flags |= PTP_FLAG_PTP_CLOCK_REGISTERED;
 927         netif_info(adapter, ifup, adapter->netdev,
 928                    "successfully registered ptp clock\n");
 929
 930         return 0;
 931 done:
 932         lan743x_ptp_close(adapter);
 933         return ret;
 934 }
 935
 936 void lan743x_ptp_close(struct lan743x_adapter *adapter)
 937 {
 938         struct lan743x_ptp *ptp = &adapter->ptp;
 939         int index;
 940
 941         if (IS_ENABLED(CONFIG_PTP_1588_CLOCK) &&
 942             ptp->flags & PTP_FLAG_PTP_CLOCK_REGISTERED) {
 943                 ptp_clock_unregister(ptp->ptp_clock);
 944                 ptp->ptp_clock = NULL;
 945                 ptp->flags &= ~PTP_FLAG_PTP_CLOCK_REGISTERED;
 946                 netif_info(adapter, drv, adapter->netdev,
 947                            "ptp clock unregister\n");
 948         }
 949
 950         if (ptp->flags & PTP_FLAG_ISR_ENABLED) {
 951                 lan743x_csr_write(adapter, PTP_INT_EN_CLR,
 952                                   PTP_INT_BIT_TX_SWTS_ERR_ |
 953                                   PTP_INT_BIT_TX_TS_);
 954                 lan743x_csr_write(adapter, INT_EN_CLR, INT_BIT_1588_);
 955                 ptp->flags &= ~PTP_FLAG_ISR_ENABLED;
 956         }
 957
 958         /* clean up pending timestamp requests */
 959         lan743x_ptp_tx_ts_complete(adapter);
 960         spin_lock_bh(&ptp->tx_ts_lock);
 961         for (index = 0;
 962                 index < LAN743X_PTP_NUMBER_OF_TX_TIMESTAMPS;
 963                 index++) {
 964                 struct sk_buff *skb = ptp->tx_ts_skb_queue[index];
 965
 966                 if (skb)
 967                         dev_kfree_skb(skb);
 968                 ptp->tx_ts_skb_queue[index] = NULL;
 969                 ptp->tx_ts_seconds_queue[index] = 0;
 970                 ptp->tx_ts_nseconds_queue[index] = 0;
 971         }
 972         ptp->tx_ts_skb_queue_size = 0;
 973         ptp->tx_ts_queue_size = 0;
 974         ptp->pending_tx_timestamps = 0;
 975         spin_unlock_bh(&ptp->tx_ts_lock);
 976
 977         lan743x_ptp_disable(adapter);
 978 }
 979
 980 void lan743x_ptp_set_sync_ts_insert(struct lan743x_adapter *adapter,
 981                                     bool ts_insert_enable)
 982 {
 983         u32 ptp_tx_mod = lan743x_csr_read(adapter, PTP_TX_MOD);
 984
 985         if (ts_insert_enable)
 986                 ptp_tx_mod |= PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_;
 987         else
 988                 ptp_tx_mod &= ~PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_;
 989
 990         lan743x_csr_write(adapter, PTP_TX_MOD, ptp_tx_mod);
 991 }
 992
 993 static bool lan743x_ptp_is_enabled(struct lan743x_adapter *adapter)
 994 {
 995         if (lan743x_csr_read(adapter, PTP_CMD_CTL) & PTP_CMD_CTL_PTP_ENABLE_)
 996                 return true;
 997         return false;
 998 }
 999
1000 static void lan743x_ptp_enable(struct lan743x_adapter *adapter)
1001 {
1002         struct lan743x_ptp *ptp = &adapter->ptp;
1003
1004         mutex_lock(&ptp->command_lock);
1005
1006         if (lan743x_ptp_is_enabled(adapter)) {
1007                 netif_warn(adapter, drv, adapter->netdev,
1008                            "PTP already enabled\n");
1009                 goto done;
1010         }
1011         lan743x_csr_write(adapter, PTP_CMD_CTL, PTP_CMD_CTL_PTP_ENABLE_);
1012 done:
1013         mutex_unlock(&ptp->command_lock);
1014 }
1015
1016 static void lan743x_ptp_disable(struct lan743x_adapter *adapter)
1017 {
1018         struct lan743x_ptp *ptp = &adapter->ptp;
1019
1020         mutex_lock(&ptp->command_lock);
1021         if (!lan743x_ptp_is_enabled(adapter)) {
1022                 netif_warn(adapter, drv, adapter->netdev,
1023                            "PTP already disabled\n");
1024                 goto done;
1025         }
1026         lan743x_csr_write(adapter, PTP_CMD_CTL, PTP_CMD_CTL_PTP_DISABLE_);
1027         lan743x_ptp_wait_till_cmd_done(adapter, PTP_CMD_CTL_PTP_ENABLE_);
1028 done:
1029         mutex_unlock(&ptp->command_lock);
1030 }
1031
1032 static void lan743x_ptp_reset(struct lan743x_adapter *adapter)
1033 {
1034         struct lan743x_ptp *ptp = &adapter->ptp;
1035
1036         mutex_lock(&ptp->command_lock);
1037
1038         if (lan743x_ptp_is_enabled(adapter)) {
1039                 netif_err(adapter, drv, adapter->netdev,
1040                           "Attempting reset while enabled\n");
1041                 goto done;
1042         }
1043
1044         lan743x_csr_write(adapter, PTP_CMD_CTL, PTP_CMD_CTL_PTP_RESET_);
1045         lan743x_ptp_wait_till_cmd_done(adapter, PTP_CMD_CTL_PTP_RESET_);
1046 done:
1047         mutex_unlock(&ptp->command_lock);
1048 }
1049
1050 static void lan743x_ptp_clock_set(struct lan743x_adapter *adapter,
1051                                   u32 seconds, u32 nano_seconds,
1052                                   u32 sub_nano_seconds)
1053 {
1054         struct lan743x_ptp *ptp = &adapter->ptp;
1055
1056         mutex_lock(&ptp->command_lock);
1057
1058         lan743x_csr_write(adapter, PTP_CLOCK_SEC, seconds);
1059         lan743x_csr_write(adapter, PTP_CLOCK_NS, nano_seconds);
1060         lan743x_csr_write(adapter, PTP_CLOCK_SUBNS, sub_nano_seconds);
1061
1062         lan743x_csr_write(adapter, PTP_CMD_CTL, PTP_CMD_CTL_PTP_CLOCK_LOAD_);
1063         lan743x_ptp_wait_till_cmd_done(adapter, PTP_CMD_CTL_PTP_CLOCK_LOAD_);
1064         mutex_unlock(&ptp->command_lock);
1065 }
1066
1067 bool lan743x_ptp_request_tx_timestamp(struct lan743x_adapter *adapter)
1068 {
1069         struct lan743x_ptp *ptp = &adapter->ptp;
1070         bool result = false;
1071
1072         spin_lock_bh(&ptp->tx_ts_lock);
1073         if (ptp->pending_tx_timestamps < LAN743X_PTP_NUMBER_OF_TX_TIMESTAMPS) {
1074                 /* request granted */
1075                 ptp->pending_tx_timestamps++;
1076                 result = true;
1077         }
1078         spin_unlock_bh(&ptp->tx_ts_lock);
1079         return result;
1080 }
1081
1082 void lan743x_ptp_unrequest_tx_timestamp(struct lan743x_adapter *adapter)
1083 {
1084         struct lan743x_ptp *ptp = &adapter->ptp;
1085
1086         spin_lock_bh(&ptp->tx_ts_lock);
1087         if (ptp->pending_tx_timestamps > 0)
1088                 ptp->pending_tx_timestamps--;
1089         else
1090                 netif_err(adapter, drv, adapter->netdev,
1091                           "unrequest failed, pending_tx_timestamps==0\n");
1092         spin_unlock_bh(&ptp->tx_ts_lock);
1093 }
1094
1095 void lan743x_ptp_tx_timestamp_skb(struct lan743x_adapter *adapter,
1096                                   struct sk_buff *skb, bool ignore_sync)
1097 {
1098         lan743x_ptp_tx_ts_enqueue_skb(adapter, skb, ignore_sync);
1099
1100         lan743x_ptp_tx_ts_complete(adapter);
1101 }
1102
1103 int lan743x_ptp_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
1104 {
1105         struct lan743x_adapter *adapter = netdev_priv(netdev);
1106         struct hwtstamp_config config;
1107         int ret = 0;
1108         int index;
1109
1110         if (!ifr) {
1111                 netif_err(adapter, drv, adapter->netdev,
1112                           "SIOCSHWTSTAMP, ifr == NULL\n");
1113                 return -EINVAL;
1114         }
1115
1116         if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
1117                 return -EFAULT;
1118
1119         if (config.flags) {
1120                 netif_warn(adapter, drv, adapter->netdev,
1121                            "ignoring hwtstamp_config.flags == 0x%08X, expected 0\n",
1122                            config.flags);
1123         }
1124
1125         switch (config.tx_type) {
1126         case HWTSTAMP_TX_OFF:
1127                 for (index = 0; index < LAN743X_MAX_TX_CHANNELS;
1128                         index++)
1129                         lan743x_tx_set_timestamping_mode(&adapter->tx[index],
1130                                                          false, false);
1131                 lan743x_ptp_set_sync_ts_insert(adapter, false);
1132                 break;
1133         case HWTSTAMP_TX_ON:
1134                 for (index = 0; index < LAN743X_MAX_TX_CHANNELS;
1135                         index++)
1136                         lan743x_tx_set_timestamping_mode(&adapter->tx[index],
1137                                                          true, false);
1138                 lan743x_ptp_set_sync_ts_insert(adapter, false);
1139                 break;
1140         case HWTSTAMP_TX_ONESTEP_SYNC:
1141                 for (index = 0; index < LAN743X_MAX_TX_CHANNELS;
1142                         index++)
1143                         lan743x_tx_set_timestamping_mode(&adapter->tx[index],
1144                                                          true, true);
1145
1146                 lan743x_ptp_set_sync_ts_insert(adapter, true);
1147                 break;
1148         default:
1149                 netif_warn(adapter, drv, adapter->netdev,
1150                            "  tx_type = %d, UNKNOWN\n", config.tx_type);
1151                 ret = -EINVAL;
1152                 break;
1153         }
1154
1155         if (!ret)
1156                 return copy_to_user(ifr->ifr_data, &config,
1157                         sizeof(config)) ? -EFAULT : 0;
1158         return ret;
1159 }