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