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 static int lan743x_get_channel(u32 ch_map)
  29 {
  30         int idx;
  31
  32         for (idx = 0; idx < 32; idx++) {
  33                 if (ch_map & (0x1 << idx))
  34                         return idx;
  35         }
  36
  37         return -EINVAL;
  38 }
  39
  40 int lan743x_gpio_init(struct lan743x_adapter *adapter)
  41 {
  42         struct lan743x_gpio *gpio = &adapter->gpio;
  43
  44         spin_lock_init(&gpio->gpio_lock);
  45
  46         gpio->gpio_cfg0 = 0; /* set all direction to input, data = 0 */
  47         gpio->gpio_cfg1 = 0x0FFF0000;/* disable all gpio, set to open drain */
  48         gpio->gpio_cfg2 = 0;/* set all to 1588 low polarity level */
  49         gpio->gpio_cfg3 = 0;/* disable all 1588 output */
  50         lan743x_csr_write(adapter, GPIO_CFG0, gpio->gpio_cfg0);
  51         lan743x_csr_write(adapter, GPIO_CFG1, gpio->gpio_cfg1);
  52         lan743x_csr_write(adapter, GPIO_CFG2, gpio->gpio_cfg2);
  53         lan743x_csr_write(adapter, GPIO_CFG3, gpio->gpio_cfg3);
  54
  55         return 0;
  56 }
  57
  58 static void lan743x_ptp_wait_till_cmd_done(struct lan743x_adapter *adapter,
  59                                            u32 bit_mask)
  60 {
  61         int timeout = 1000;
  62         u32 data = 0;
  63
  64         while (timeout &&
  65                (data = (lan743x_csr_read(adapter, PTP_CMD_CTL) &
  66                bit_mask))) {
  67                 usleep_range(1000, 20000);
  68                 timeout--;
  69         }
  70         if (data) {
  71                 netif_err(adapter, drv, adapter->netdev,
  72                           "timeout waiting for cmd to be done, cmd = 0x%08X\n",
  73                           bit_mask);
  74         }
  75 }
  76
  77 static void lan743x_ptp_tx_ts_enqueue_ts(struct lan743x_adapter *adapter,
  78                                          u32 seconds, u32 nano_seconds,
  79                                          u32 header)
  80 {
  81         struct lan743x_ptp *ptp = &adapter->ptp;
  82
  83         spin_lock_bh(&ptp->tx_ts_lock);
  84         if (ptp->tx_ts_queue_size < LAN743X_PTP_NUMBER_OF_TX_TIMESTAMPS) {
  85                 ptp->tx_ts_seconds_queue[ptp->tx_ts_queue_size] = seconds;
  86                 ptp->tx_ts_nseconds_queue[ptp->tx_ts_queue_size] = nano_seconds;
  87                 ptp->tx_ts_header_queue[ptp->tx_ts_queue_size] = header;
  88                 ptp->tx_ts_queue_size++;
  89         } else {
  90                 netif_err(adapter, drv, adapter->netdev,
  91                           "tx ts queue overflow\n");
  92         }
  93         spin_unlock_bh(&ptp->tx_ts_lock);
  94 }
  95
  96 static void lan743x_ptp_tx_ts_complete(struct lan743x_adapter *adapter)
  97 {
  98         struct lan743x_ptp *ptp = &adapter->ptp;
  99         struct skb_shared_hwtstamps tstamps;
 100         u32 header, nseconds, seconds;
 101         bool ignore_sync = false;
 102         struct sk_buff *skb;
 103         int c, i;
 104
 105         spin_lock_bh(&ptp->tx_ts_lock);
 106         c = ptp->tx_ts_skb_queue_size;
 107
 108         if (c > ptp->tx_ts_queue_size)
 109                 c = ptp->tx_ts_queue_size;
 110         if (c <= 0)
 111                 goto done;
 112
 113         for (i = 0; i < c; i++) {
 114                 ignore_sync = ((ptp->tx_ts_ignore_sync_queue &
 115                                 BIT(i)) != 0);
 116                 skb = ptp->tx_ts_skb_queue[i];
 117                 nseconds = ptp->tx_ts_nseconds_queue[i];
 118                 seconds = ptp->tx_ts_seconds_queue[i];
 119                 header = ptp->tx_ts_header_queue[i];
 120
 121                 memset(&tstamps, 0, sizeof(tstamps));
 122                 tstamps.hwtstamp = ktime_set(seconds, nseconds);
 123                 if (!ignore_sync ||
 124                     ((header & PTP_TX_MSG_HEADER_MSG_TYPE_) !=
 125                     PTP_TX_MSG_HEADER_MSG_TYPE_SYNC_))
 126                         skb_tstamp_tx(skb, &tstamps);
 127
 128                 dev_kfree_skb(skb);
 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
 136         /* shift queue */
 137         ptp->tx_ts_ignore_sync_queue >>= c;
 138         for (i = c; i < LAN743X_PTP_NUMBER_OF_TX_TIMESTAMPS; i++) {
 139                 ptp->tx_ts_skb_queue[i - c] = ptp->tx_ts_skb_queue[i];
 140                 ptp->tx_ts_seconds_queue[i - c] = ptp->tx_ts_seconds_queue[i];
 141                 ptp->tx_ts_nseconds_queue[i - c] = ptp->tx_ts_nseconds_queue[i];
 142                 ptp->tx_ts_header_queue[i - c] = ptp->tx_ts_header_queue[i];
 143
 144                 ptp->tx_ts_skb_queue[i] = NULL;
 145                 ptp->tx_ts_seconds_queue[i] = 0;
 146                 ptp->tx_ts_nseconds_queue[i] = 0;
 147                 ptp->tx_ts_header_queue[i] = 0;
 148         }
 149         ptp->tx_ts_skb_queue_size -= c;
 150         ptp->tx_ts_queue_size -= c;
 151 done:
 152         ptp->pending_tx_timestamps -= c;
 153         spin_unlock_bh(&ptp->tx_ts_lock);
 154 }
 155
 156 static int lan743x_ptp_reserve_event_ch(struct lan743x_adapter *adapter,
 157                                         int event_channel)
 158 {
 159         struct lan743x_ptp *ptp = &adapter->ptp;
 160         int result = -ENODEV;
 161
 162         mutex_lock(&ptp->command_lock);
 163         if (!(test_bit(event_channel, &ptp->used_event_ch))) {
 164                 ptp->used_event_ch |= BIT(event_channel);
 165                 result = event_channel;
 166         } else {
 167                 netif_warn(adapter, drv, adapter->netdev,
 168                            "attempted to reserved a used event_channel = %d\n",
 169                            event_channel);
 170         }
 171         mutex_unlock(&ptp->command_lock);
 172         return result;
 173 }
 174
 175 static void lan743x_ptp_release_event_ch(struct lan743x_adapter *adapter,
 176                                          int event_channel)
 177 {
 178         struct lan743x_ptp *ptp = &adapter->ptp;
 179
 180         mutex_lock(&ptp->command_lock);
 181         if (test_bit(event_channel, &ptp->used_event_ch)) {
 182                 ptp->used_event_ch &= ~BIT(event_channel);
 183         } else {
 184                 netif_warn(adapter, drv, adapter->netdev,
 185                            "attempted release on a not used event_channel = %d\n",
 186                            event_channel);
 187         }
 188         mutex_unlock(&ptp->command_lock);
 189 }
 190
 191 static void lan743x_ptp_clock_get(struct lan743x_adapter *adapter,
 192                                   u32 *seconds, u32 *nano_seconds,
 193                                   u32 *sub_nano_seconds);
 194 static void lan743x_ptp_io_clock_get(struct lan743x_adapter *adapter,
 195                                      u32 *sec, u32 *nsec, u32 *sub_nsec);
 196 static void lan743x_ptp_clock_step(struct lan743x_adapter *adapter,
 197                                    s64 time_step_ns);
 198
 199 static void lan743x_led_mux_enable(struct lan743x_adapter *adapter,
 200                                    int pin, bool enable)
 201 {
 202         struct lan743x_ptp *ptp = &adapter->ptp;
 203
 204         if (ptp->leds_multiplexed &&
 205             ptp->led_enabled[pin]) {
 206                 u32 val = lan743x_csr_read(adapter, HW_CFG);
 207
 208                 if (enable)
 209                         val |= LAN743X_LED_ENABLE(pin);
 210                 else
 211                         val &= ~LAN743X_LED_ENABLE(pin);
 212
 213                 lan743x_csr_write(adapter, HW_CFG, val);
 214         }
 215 }
 216
 217 static void lan743x_led_mux_save(struct lan743x_adapter *adapter)
 218 {
 219         struct lan743x_ptp *ptp = &adapter->ptp;
 220         u32 id_rev = adapter->csr.id_rev & ID_REV_ID_MASK_;
 221
 222         if (id_rev == ID_REV_ID_LAN7430_) {
 223                 int i;
 224                 u32 val = lan743x_csr_read(adapter, HW_CFG);
 225
 226                 for (i = 0; i < LAN7430_N_LED; i++) {
 227                         bool led_enabled = (val & LAN743X_LED_ENABLE(i)) != 0;
 228
 229                         ptp->led_enabled[i] = led_enabled;
 230                 }
 231                 ptp->leds_multiplexed = true;
 232         } else {
 233                 ptp->leds_multiplexed = false;
 234         }
 235 }
 236
 237 static void lan743x_led_mux_restore(struct lan743x_adapter *adapter)
 238 {
 239         u32 id_rev = adapter->csr.id_rev & ID_REV_ID_MASK_;
 240
 241         if (id_rev == ID_REV_ID_LAN7430_) {
 242                 int i;
 243
 244                 for (i = 0; i < LAN7430_N_LED; i++)
 245                         lan743x_led_mux_enable(adapter, i, true);
 246         }
 247 }
 248
 249 static int lan743x_gpio_rsrv_ptp_out(struct lan743x_adapter *adapter,
 250                                      int pin, int event_channel)
 251 {
 252         struct lan743x_gpio *gpio = &adapter->gpio;
 253         unsigned long irq_flags = 0;
 254         int bit_mask = BIT(pin);
 255         int ret = -EBUSY;
 256
 257         spin_lock_irqsave(&gpio->gpio_lock, irq_flags);
 258
 259         if (!(gpio->used_bits & bit_mask)) {
 260                 gpio->used_bits |= bit_mask;
 261                 gpio->output_bits |= bit_mask;
 262                 gpio->ptp_bits |= bit_mask;
 263
 264                 /* assign pin to GPIO function */
 265                 lan743x_led_mux_enable(adapter, pin, false);
 266
 267                 /* set as output, and zero initial value */
 268                 gpio->gpio_cfg0 |= GPIO_CFG0_GPIO_DIR_BIT_(pin);
 269                 gpio->gpio_cfg0 &= ~GPIO_CFG0_GPIO_DATA_BIT_(pin);
 270                 lan743x_csr_write(adapter, GPIO_CFG0, gpio->gpio_cfg0);
 271
 272                 /* enable gpio, and set buffer type to push pull */
 273                 gpio->gpio_cfg1 &= ~GPIO_CFG1_GPIOEN_BIT_(pin);
 274                 gpio->gpio_cfg1 |= GPIO_CFG1_GPIOBUF_BIT_(pin);
 275                 lan743x_csr_write(adapter, GPIO_CFG1, gpio->gpio_cfg1);
 276
 277                 /* set 1588 polarity to high */
 278                 gpio->gpio_cfg2 |= GPIO_CFG2_1588_POL_BIT_(pin);
 279                 lan743x_csr_write(adapter, GPIO_CFG2, gpio->gpio_cfg2);
 280
 281                 if (event_channel == 0) {
 282                         /* use channel A */
 283                         gpio->gpio_cfg3 &= ~GPIO_CFG3_1588_CH_SEL_BIT_(pin);
 284                 } else {
 285                         /* use channel B */
 286                         gpio->gpio_cfg3 |= GPIO_CFG3_1588_CH_SEL_BIT_(pin);
 287                 }
 288                 gpio->gpio_cfg3 |= GPIO_CFG3_1588_OE_BIT_(pin);
 289                 lan743x_csr_write(adapter, GPIO_CFG3, gpio->gpio_cfg3);
 290
 291                 ret = pin;
 292         }
 293         spin_unlock_irqrestore(&gpio->gpio_lock, irq_flags);
 294         return ret;
 295 }
 296
 297 static void lan743x_gpio_release(struct lan743x_adapter *adapter, int pin)
 298 {
 299         struct lan743x_gpio *gpio = &adapter->gpio;
 300         unsigned long irq_flags = 0;
 301         int bit_mask = BIT(pin);
 302
 303         spin_lock_irqsave(&gpio->gpio_lock, irq_flags);
 304         if (gpio->used_bits & bit_mask) {
 305                 gpio->used_bits &= ~bit_mask;
 306                 if (gpio->output_bits & bit_mask) {
 307                         gpio->output_bits &= ~bit_mask;
 308
 309                         if (gpio->ptp_bits & bit_mask) {
 310                                 gpio->ptp_bits &= ~bit_mask;
 311                                 /* disable ptp output */
 312                                 gpio->gpio_cfg3 &= ~GPIO_CFG3_1588_OE_BIT_(pin);
 313                                 lan743x_csr_write(adapter, GPIO_CFG3,
 314                                                   gpio->gpio_cfg3);
 315                         }
 316                         /* release gpio output */
 317
 318                         /* disable gpio */
 319                         gpio->gpio_cfg1 |= GPIO_CFG1_GPIOEN_BIT_(pin);
 320                         gpio->gpio_cfg1 &= ~GPIO_CFG1_GPIOBUF_BIT_(pin);
 321                         lan743x_csr_write(adapter, GPIO_CFG1, gpio->gpio_cfg1);
 322
 323                         /* reset back to input */
 324                         gpio->gpio_cfg0 &= ~GPIO_CFG0_GPIO_DIR_BIT_(pin);
 325                         gpio->gpio_cfg0 &= ~GPIO_CFG0_GPIO_DATA_BIT_(pin);
 326                         lan743x_csr_write(adapter, GPIO_CFG0, gpio->gpio_cfg0);
 327
 328                         /* assign pin to original function */
 329                         lan743x_led_mux_enable(adapter, pin, true);
 330                 }
 331         }
 332         spin_unlock_irqrestore(&gpio->gpio_lock, irq_flags);
 333 }
 334
 335 static int lan743x_ptpci_adjfine(struct ptp_clock_info *ptpci, long scaled_ppm)
 336 {
 337         struct lan743x_ptp *ptp =
 338                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 339         struct lan743x_adapter *adapter =
 340                 container_of(ptp, struct lan743x_adapter, ptp);
 341         u32 lan743x_rate_adj = 0;
 342         u64 u64_delta;
 343
 344         if ((scaled_ppm < (-LAN743X_PTP_MAX_FINE_ADJ_IN_SCALED_PPM)) ||
 345             scaled_ppm > LAN743X_PTP_MAX_FINE_ADJ_IN_SCALED_PPM) {
 346                 return -EINVAL;
 347         }
 348
 349         /* diff_by_scaled_ppm returns true if the difference is negative */
 350         if (diff_by_scaled_ppm(1ULL << 35, scaled_ppm, &u64_delta))
 351                 lan743x_rate_adj = (u32)u64_delta;
 352         else
 353                 lan743x_rate_adj = (u32)u64_delta | PTP_CLOCK_RATE_ADJ_DIR_;
 354
 355         lan743x_csr_write(adapter, PTP_CLOCK_RATE_ADJ,
 356                           lan743x_rate_adj);
 357
 358         return 0;
 359 }
 360
 361 static int lan743x_ptpci_adjtime(struct ptp_clock_info *ptpci, s64 delta)
 362 {
 363         struct lan743x_ptp *ptp =
 364                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 365         struct lan743x_adapter *adapter =
 366                 container_of(ptp, struct lan743x_adapter, ptp);
 367
 368         lan743x_ptp_clock_step(adapter, delta);
 369
 370         return 0;
 371 }
 372
 373 static int lan743x_ptpci_gettime64(struct ptp_clock_info *ptpci,
 374                                    struct timespec64 *ts)
 375 {
 376         struct lan743x_ptp *ptp =
 377                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 378         struct lan743x_adapter *adapter =
 379                 container_of(ptp, struct lan743x_adapter, ptp);
 380         u32 nano_seconds = 0;
 381         u32 seconds = 0;
 382
 383         if (adapter->is_pci11x1x)
 384                 lan743x_ptp_io_clock_get(adapter, &seconds, &nano_seconds,
 385                                          NULL);
 386         else
 387                 lan743x_ptp_clock_get(adapter, &seconds, &nano_seconds, NULL);
 388         ts->tv_sec = seconds;
 389         ts->tv_nsec = nano_seconds;
 390
 391         return 0;
 392 }
 393
 394 static int lan743x_ptpci_settime64(struct ptp_clock_info *ptpci,
 395                                    const struct timespec64 *ts)
 396 {
 397         struct lan743x_ptp *ptp =
 398                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 399         struct lan743x_adapter *adapter =
 400                 container_of(ptp, struct lan743x_adapter, ptp);
 401         u32 nano_seconds = 0;
 402         u32 seconds = 0;
 403
 404         if (ts) {
 405                 if (ts->tv_sec > 0xFFFFFFFFLL ||
 406                     ts->tv_sec < 0) {
 407                         netif_warn(adapter, drv, adapter->netdev,
 408                                    "ts->tv_sec out of range, %lld\n",
 409                                    ts->tv_sec);
 410                         return -ERANGE;
 411                 }
 412                 if (ts->tv_nsec >= 1000000000L ||
 413                     ts->tv_nsec < 0) {
 414                         netif_warn(adapter, drv, adapter->netdev,
 415                                    "ts->tv_nsec out of range, %ld\n",
 416                                    ts->tv_nsec);
 417                         return -ERANGE;
 418                 }
 419                 seconds = ts->tv_sec;
 420                 nano_seconds = ts->tv_nsec;
 421                 lan743x_ptp_clock_set(adapter, seconds, nano_seconds, 0);
 422         } else {
 423                 netif_warn(adapter, drv, adapter->netdev, "ts == NULL\n");
 424                 return -EINVAL;
 425         }
 426
 427         return 0;
 428 }
 429
 430 static void lan743x_ptp_perout_off(struct lan743x_adapter *adapter,
 431                                    unsigned int index)
 432 {
 433         struct lan743x_ptp *ptp = &adapter->ptp;
 434         u32 general_config = 0;
 435         struct lan743x_ptp_perout *perout = &ptp->perout[index];
 436
 437         if (perout->gpio_pin >= 0) {
 438                 lan743x_gpio_release(adapter, perout->gpio_pin);
 439                 perout->gpio_pin = -1;
 440         }
 441
 442         if (perout->event_ch >= 0) {
 443                 /* set target to far in the future, effectively disabling it */
 444                 lan743x_csr_write(adapter,
 445                                   PTP_CLOCK_TARGET_SEC_X(perout->event_ch),
 446                                   0xFFFF0000);
 447                 lan743x_csr_write(adapter,
 448                                   PTP_CLOCK_TARGET_NS_X(perout->event_ch),
 449                                   0);
 450
 451                 general_config = lan743x_csr_read(adapter, PTP_GENERAL_CONFIG);
 452                 general_config |= PTP_GENERAL_CONFIG_RELOAD_ADD_X_
 453                                   (perout->event_ch);
 454                 lan743x_csr_write(adapter, PTP_GENERAL_CONFIG, general_config);
 455                 lan743x_ptp_release_event_ch(adapter, perout->event_ch);
 456                 perout->event_ch = -1;
 457         }
 458 }
 459
 460 static int lan743x_ptp_perout(struct lan743x_adapter *adapter, int on,
 461                               struct ptp_perout_request *perout_request)
 462 {
 463         struct lan743x_ptp *ptp = &adapter->ptp;
 464         u32 period_sec = 0, period_nsec = 0;
 465         u32 start_sec = 0, start_nsec = 0;
 466         u32 general_config = 0;
 467         int pulse_width = 0;
 468         int perout_pin = 0;
 469         unsigned int index = perout_request->index;
 470         struct lan743x_ptp_perout *perout = &ptp->perout[index];
 471         int ret = 0;
 472
 473         /* Reject requests with unsupported flags */
 474         if (perout_request->flags & ~PTP_PEROUT_DUTY_CYCLE)
 475                 return -EOPNOTSUPP;
 476
 477         if (on) {
 478                 perout_pin = ptp_find_pin(ptp->ptp_clock, PTP_PF_PEROUT,
 479                                           perout_request->index);
 480                 if (perout_pin < 0)
 481                         return -EBUSY;
 482         } else {
 483                 lan743x_ptp_perout_off(adapter, index);
 484                 return 0;
 485         }
 486
 487         if (perout->event_ch >= 0 ||
 488             perout->gpio_pin >= 0) {
 489                 /* already on, turn off first */
 490                 lan743x_ptp_perout_off(adapter, index);
 491         }
 492
 493         perout->event_ch = lan743x_ptp_reserve_event_ch(adapter, index);
 494
 495         if (perout->event_ch < 0) {
 496                 netif_warn(adapter, drv, adapter->netdev,
 497                            "Failed to reserve event channel %d for PEROUT\n",
 498                            index);
 499                 ret = -EBUSY;
 500                 goto failed;
 501         }
 502
 503         perout->gpio_pin = lan743x_gpio_rsrv_ptp_out(adapter,
 504                                                      perout_pin,
 505                                                      perout->event_ch);
 506
 507         if (perout->gpio_pin < 0) {
 508                 netif_warn(adapter, drv, adapter->netdev,
 509                            "Failed to reserve gpio %d for PEROUT\n",
 510                            perout_pin);
 511                 ret = -EBUSY;
 512                 goto failed;
 513         }
 514
 515         start_sec = perout_request->start.sec;
 516         start_sec += perout_request->start.nsec / 1000000000;
 517         start_nsec = perout_request->start.nsec % 1000000000;
 518
 519         period_sec = perout_request->period.sec;
 520         period_sec += perout_request->period.nsec / 1000000000;
 521         period_nsec = perout_request->period.nsec % 1000000000;
 522
 523         if (perout_request->flags & PTP_PEROUT_DUTY_CYCLE) {
 524                 struct timespec64 ts_on, ts_period;
 525                 s64 wf_high, period64, half;
 526                 s32 reminder;
 527
 528                 ts_on.tv_sec = perout_request->on.sec;
 529                 ts_on.tv_nsec = perout_request->on.nsec;
 530                 wf_high = timespec64_to_ns(&ts_on);
 531                 ts_period.tv_sec = perout_request->period.sec;
 532                 ts_period.tv_nsec = perout_request->period.nsec;
 533                 period64 = timespec64_to_ns(&ts_period);
 534
 535                 if (period64 < 200) {
 536                         netif_warn(adapter, drv, adapter->netdev,
 537                                    "perout period too small, minimum is 200nS\n");
 538                         ret = -EOPNOTSUPP;
 539                         goto failed;
 540                 }
 541                 if (wf_high >= period64) {
 542                         netif_warn(adapter, drv, adapter->netdev,
 543                                    "pulse width must be smaller than period\n");
 544                         ret = -EINVAL;
 545                         goto failed;
 546                 }
 547
 548                 /* Check if we can do 50% toggle on an even value of period.
 549                  * If the period number is odd, then check if the requested
 550                  * pulse width is the same as one of pre-defined width values.
 551                  * Otherwise, return failure.
 552                  */
 553                 half = div_s64_rem(period64, 2, &reminder);
 554                 if (!reminder) {
 555                         if (half == wf_high) {
 556                                 /* It's 50% match. Use the toggle option */
 557                                 pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_TOGGLE_;
 558                                 /* In this case, devide period value by 2 */
 559                                 ts_period = ns_to_timespec64(div_s64(period64, 2));
 560                                 period_sec = ts_period.tv_sec;
 561                                 period_nsec = ts_period.tv_nsec;
 562
 563                                 goto program;
 564                         }
 565                 }
 566                 /* if we can't do toggle, then the width option needs to be the exact match */
 567                 if (wf_high == 200000000) {
 568                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_200MS_;
 569                 } else if (wf_high == 10000000) {
 570                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_10MS_;
 571                 } else if (wf_high == 1000000) {
 572                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_1MS_;
 573                 } else if (wf_high == 100000) {
 574                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_100US_;
 575                 } else if (wf_high == 10000) {
 576                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_10US_;
 577                 } else if (wf_high == 100) {
 578                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_100NS_;
 579                 } else {
 580                         netif_warn(adapter, drv, adapter->netdev,
 581                                    "duty cycle specified is not supported\n");
 582                         ret = -EOPNOTSUPP;
 583                         goto failed;
 584                 }
 585         } else {
 586                 if (period_sec == 0) {
 587                         if (period_nsec >= 400000000) {
 588                                 pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_200MS_;
 589                         } else if (period_nsec >= 20000000) {
 590                                 pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_10MS_;
 591                         } else if (period_nsec >= 2000000) {
 592                                 pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_1MS_;
 593                         } else if (period_nsec >= 200000) {
 594                                 pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_100US_;
 595                         } else if (period_nsec >= 20000) {
 596                                 pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_10US_;
 597                         } else if (period_nsec >= 200) {
 598                                 pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_100NS_;
 599                         } else {
 600                                 netif_warn(adapter, drv, adapter->netdev,
 601                                            "perout period too small, minimum is 200nS\n");
 602                                 ret = -EOPNOTSUPP;
 603                                 goto failed;
 604                         }
 605                 } else {
 606                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_200MS_;
 607                 }
 608         }
 609 program:
 610
 611         /* turn off by setting target far in future */
 612         lan743x_csr_write(adapter,
 613                           PTP_CLOCK_TARGET_SEC_X(perout->event_ch),
 614                           0xFFFF0000);
 615         lan743x_csr_write(adapter,
 616                           PTP_CLOCK_TARGET_NS_X(perout->event_ch), 0);
 617
 618         /* Configure to pulse every period */
 619         general_config = lan743x_csr_read(adapter, PTP_GENERAL_CONFIG);
 620         general_config &= ~(PTP_GENERAL_CONFIG_CLOCK_EVENT_X_MASK_
 621                           (perout->event_ch));
 622         general_config |= PTP_GENERAL_CONFIG_CLOCK_EVENT_X_SET_
 623                           (perout->event_ch, pulse_width);
 624         general_config &= ~PTP_GENERAL_CONFIG_RELOAD_ADD_X_
 625                           (perout->event_ch);
 626         lan743x_csr_write(adapter, PTP_GENERAL_CONFIG, general_config);
 627
 628         /* set the reload to one toggle cycle */
 629         lan743x_csr_write(adapter,
 630                           PTP_CLOCK_TARGET_RELOAD_SEC_X(perout->event_ch),
 631                           period_sec);
 632         lan743x_csr_write(adapter,
 633                           PTP_CLOCK_TARGET_RELOAD_NS_X(perout->event_ch),
 634                           period_nsec);
 635
 636         /* set the start time */
 637         lan743x_csr_write(adapter,
 638                           PTP_CLOCK_TARGET_SEC_X(perout->event_ch),
 639                           start_sec);
 640         lan743x_csr_write(adapter,
 641                           PTP_CLOCK_TARGET_NS_X(perout->event_ch),
 642                           start_nsec);
 643
 644         return 0;
 645
 646 failed:
 647         lan743x_ptp_perout_off(adapter, index);
 648         return ret;
 649 }
 650
 651 static void lan743x_ptp_io_perout_off(struct lan743x_adapter *adapter,
 652                                       u32 index)
 653 {
 654         struct lan743x_ptp *ptp = &adapter->ptp;
 655         int perout_pin;
 656         int event_ch;
 657         u32 gen_cfg;
 658         int val;
 659
 660         event_ch = ptp->ptp_io_perout[index];
 661         if (event_ch >= 0) {
 662                 /* set target to far in the future, effectively disabling it */
 663                 lan743x_csr_write(adapter,
 664                                   PTP_CLOCK_TARGET_SEC_X(event_ch),
 665                                   0xFFFF0000);
 666                 lan743x_csr_write(adapter,
 667                                   PTP_CLOCK_TARGET_NS_X(event_ch),
 668                                   0);
 669
 670                 gen_cfg = lan743x_csr_read(adapter, HS_PTP_GENERAL_CONFIG);
 671                 gen_cfg &= ~(HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_X_MASK_
 672                                     (event_ch));
 673                 gen_cfg &= ~(HS_PTP_GENERAL_CONFIG_EVENT_POL_X_(event_ch));
 674                 gen_cfg |= HS_PTP_GENERAL_CONFIG_RELOAD_ADD_X_(event_ch);
 675                 lan743x_csr_write(adapter, HS_PTP_GENERAL_CONFIG, gen_cfg);
 676                 if (event_ch)
 677                         lan743x_csr_write(adapter, PTP_INT_STS,
 678                                           PTP_INT_TIMER_INT_B_);
 679                 else
 680                         lan743x_csr_write(adapter, PTP_INT_STS,
 681                                           PTP_INT_TIMER_INT_A_);
 682                 lan743x_ptp_release_event_ch(adapter, event_ch);
 683                 ptp->ptp_io_perout[index] = -1;
 684         }
 685
 686         perout_pin = ptp_find_pin(ptp->ptp_clock, PTP_PF_PEROUT, index);
 687
 688         /* Deselect Event output */
 689         val = lan743x_csr_read(adapter, PTP_IO_EVENT_OUTPUT_CFG);
 690
 691         /* Disables the output of Local Time Target compare events */
 692         val &= ~PTP_IO_EVENT_OUTPUT_CFG_EN_(perout_pin);
 693         lan743x_csr_write(adapter, PTP_IO_EVENT_OUTPUT_CFG, val);
 694
 695         /* Configured as an opendrain driver*/
 696         val = lan743x_csr_read(adapter, PTP_IO_PIN_CFG);
 697         val &= ~PTP_IO_PIN_CFG_OBUF_TYPE_(perout_pin);
 698         lan743x_csr_write(adapter, PTP_IO_PIN_CFG, val);
 699         /* Dummy read to make sure write operation success */
 700         val = lan743x_csr_read(adapter, PTP_IO_PIN_CFG);
 701 }
 702
 703 static int lan743x_ptp_io_perout(struct lan743x_adapter *adapter, int on,
 704                                  struct ptp_perout_request *perout_request)
 705 {
 706         struct lan743x_ptp *ptp = &adapter->ptp;
 707         u32 period_sec, period_nsec;
 708         u32 start_sec, start_nsec;
 709         u32 pulse_sec, pulse_nsec;
 710         int pulse_width;
 711         int perout_pin;
 712         int event_ch;
 713         u32 gen_cfg;
 714         u32 index;
 715         int val;
 716
 717         index = perout_request->index;
 718         event_ch = ptp->ptp_io_perout[index];
 719
 720         if (on) {
 721                 perout_pin = ptp_find_pin(ptp->ptp_clock, PTP_PF_PEROUT, index);
 722                 if (perout_pin < 0)
 723                         return -EBUSY;
 724         } else {
 725                 lan743x_ptp_io_perout_off(adapter, index);
 726                 return 0;
 727         }
 728
 729         if (event_ch >= LAN743X_PTP_N_EVENT_CHAN) {
 730                 /* already on, turn off first */
 731                 lan743x_ptp_io_perout_off(adapter, index);
 732         }
 733
 734         event_ch = lan743x_ptp_reserve_event_ch(adapter, index);
 735         if (event_ch < 0) {
 736                 netif_warn(adapter, drv, adapter->netdev,
 737                            "Failed to reserve event channel %d for PEROUT\n",
 738                            index);
 739                 goto failed;
 740         }
 741         ptp->ptp_io_perout[index] = event_ch;
 742
 743         if (perout_request->flags & PTP_PEROUT_DUTY_CYCLE) {
 744                 pulse_sec = perout_request->on.sec;
 745                 pulse_sec += perout_request->on.nsec / 1000000000;
 746                 pulse_nsec = perout_request->on.nsec % 1000000000;
 747         } else {
 748                 pulse_sec = perout_request->period.sec;
 749                 pulse_sec += perout_request->period.nsec / 1000000000;
 750                 pulse_nsec = perout_request->period.nsec % 1000000000;
 751         }
 752
 753         if (pulse_sec == 0) {
 754                 if (pulse_nsec >= 400000000) {
 755                         pulse_width = PTP_GENERAL_CONFIG_CLOCK_EVENT_200MS_;
 756                 } else if (pulse_nsec >= 200000000) {
 757                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_100MS_;
 758                 } else if (pulse_nsec >= 100000000) {
 759                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_50MS_;
 760                 } else if (pulse_nsec >= 20000000) {
 761                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_10MS_;
 762                 } else if (pulse_nsec >= 10000000) {
 763                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_5MS_;
 764                 } else if (pulse_nsec >= 2000000) {
 765                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_1MS_;
 766                 } else if (pulse_nsec >= 1000000) {
 767                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_500US_;
 768                 } else if (pulse_nsec >= 200000) {
 769                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_100US_;
 770                 } else if (pulse_nsec >= 100000) {
 771                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_50US_;
 772                 } else if (pulse_nsec >= 20000) {
 773                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_10US_;
 774                 } else if (pulse_nsec >= 10000) {
 775                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_5US_;
 776                 } else if (pulse_nsec >= 2000) {
 777                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_1US_;
 778                 } else if (pulse_nsec >= 1000) {
 779                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_500NS_;
 780                 } else if (pulse_nsec >= 200) {
 781                         pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_100NS_;
 782                 } else {
 783                         netif_warn(adapter, drv, adapter->netdev,
 784                                    "perout period too small, min is 200nS\n");
 785                         goto failed;
 786                 }
 787         } else {
 788                 pulse_width = HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_200MS_;
 789         }
 790
 791         /* turn off by setting target far in future */
 792         lan743x_csr_write(adapter,
 793                           PTP_CLOCK_TARGET_SEC_X(event_ch),
 794                           0xFFFF0000);
 795         lan743x_csr_write(adapter,
 796                           PTP_CLOCK_TARGET_NS_X(event_ch), 0);
 797
 798         /* Configure to pulse every period */
 799         gen_cfg = lan743x_csr_read(adapter, HS_PTP_GENERAL_CONFIG);
 800         gen_cfg &= ~(HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_X_MASK_(event_ch));
 801         gen_cfg |= HS_PTP_GENERAL_CONFIG_CLOCK_EVENT_X_SET_
 802                           (event_ch, pulse_width);
 803         gen_cfg |= HS_PTP_GENERAL_CONFIG_EVENT_POL_X_(event_ch);
 804         gen_cfg &= ~(HS_PTP_GENERAL_CONFIG_RELOAD_ADD_X_(event_ch));
 805         lan743x_csr_write(adapter, HS_PTP_GENERAL_CONFIG, gen_cfg);
 806
 807         /* set the reload to one toggle cycle */
 808         period_sec = perout_request->period.sec;
 809         period_sec += perout_request->period.nsec / 1000000000;
 810         period_nsec = perout_request->period.nsec % 1000000000;
 811         lan743x_csr_write(adapter,
 812                           PTP_CLOCK_TARGET_RELOAD_SEC_X(event_ch),
 813                           period_sec);
 814         lan743x_csr_write(adapter,
 815                           PTP_CLOCK_TARGET_RELOAD_NS_X(event_ch),
 816                           period_nsec);
 817
 818         start_sec = perout_request->start.sec;
 819         start_sec += perout_request->start.nsec / 1000000000;
 820         start_nsec = perout_request->start.nsec % 1000000000;
 821
 822         /* set the start time */
 823         lan743x_csr_write(adapter,
 824                           PTP_CLOCK_TARGET_SEC_X(event_ch),
 825                           start_sec);
 826         lan743x_csr_write(adapter,
 827                           PTP_CLOCK_TARGET_NS_X(event_ch),
 828                           start_nsec);
 829
 830         /* Enable LTC Target Read */
 831         val = lan743x_csr_read(adapter, PTP_CMD_CTL);
 832         val |= PTP_CMD_CTL_PTP_LTC_TARGET_READ_;
 833         lan743x_csr_write(adapter, PTP_CMD_CTL, val);
 834
 835         /* Configure as an push/pull driver */
 836         val = lan743x_csr_read(adapter, PTP_IO_PIN_CFG);
 837         val |= PTP_IO_PIN_CFG_OBUF_TYPE_(perout_pin);
 838         lan743x_csr_write(adapter, PTP_IO_PIN_CFG, val);
 839
 840         /* Select Event output */
 841         val = lan743x_csr_read(adapter, PTP_IO_EVENT_OUTPUT_CFG);
 842         if (event_ch)
 843                 /* Channel B as the output */
 844                 val |= PTP_IO_EVENT_OUTPUT_CFG_SEL_(perout_pin);
 845         else
 846                 /* Channel A as the output */
 847                 val &= ~PTP_IO_EVENT_OUTPUT_CFG_SEL_(perout_pin);
 848
 849         /* Enables the output of Local Time Target compare events */
 850         val |= PTP_IO_EVENT_OUTPUT_CFG_EN_(perout_pin);
 851         lan743x_csr_write(adapter, PTP_IO_EVENT_OUTPUT_CFG, val);
 852
 853         return 0;
 854
 855 failed:
 856         lan743x_ptp_io_perout_off(adapter, index);
 857         return -ENODEV;
 858 }
 859
 860 static void lan743x_ptp_io_extts_off(struct lan743x_adapter *adapter,
 861                                      u32 index)
 862 {
 863         struct lan743x_ptp *ptp = &adapter->ptp;
 864         struct lan743x_extts *extts;
 865         int val;
 866
 867         extts = &ptp->extts[index];
 868         /* PTP Interrupt Enable Clear Register */
 869         if (extts->flags & PTP_FALLING_EDGE)
 870                 val = PTP_INT_EN_FE_EN_CLR_(index);
 871         else
 872                 val = PTP_INT_EN_RE_EN_CLR_(index);
 873         lan743x_csr_write(adapter, PTP_INT_EN_CLR, val);
 874
 875         /* Disables PTP-IO edge lock */
 876         val = lan743x_csr_read(adapter, PTP_IO_CAP_CONFIG);
 877         if (extts->flags & PTP_FALLING_EDGE) {
 878                 val &= ~PTP_IO_CAP_CONFIG_LOCK_FE_(index);
 879                 val &= ~PTP_IO_CAP_CONFIG_FE_CAP_EN_(index);
 880         } else {
 881                 val &= ~PTP_IO_CAP_CONFIG_LOCK_RE_(index);
 882                 val &= ~PTP_IO_CAP_CONFIG_RE_CAP_EN_(index);
 883         }
 884         lan743x_csr_write(adapter, PTP_IO_CAP_CONFIG, val);
 885
 886         /* PTP-IO De-select register */
 887         val = lan743x_csr_read(adapter, PTP_IO_SEL);
 888         val &= ~PTP_IO_SEL_MASK_;
 889         lan743x_csr_write(adapter, PTP_IO_SEL, val);
 890
 891         /* Clear timestamp */
 892         memset(&extts->ts, 0, sizeof(struct timespec64));
 893         extts->flags = 0;
 894 }
 895
 896 static int lan743x_ptp_io_event_cap_en(struct lan743x_adapter *adapter,
 897                                        u32 flags, u32 channel)
 898 {
 899         struct lan743x_ptp *ptp = &adapter->ptp;
 900         int val;
 901
 902         if ((flags & PTP_EXTTS_EDGES) ==  PTP_EXTTS_EDGES)
 903                 return -EOPNOTSUPP;
 904
 905         mutex_lock(&ptp->command_lock);
 906         /* PTP-IO Event Capture Enable */
 907         val = lan743x_csr_read(adapter, PTP_IO_CAP_CONFIG);
 908         if (flags & PTP_FALLING_EDGE) {
 909                 val &= ~PTP_IO_CAP_CONFIG_LOCK_RE_(channel);
 910                 val &= ~PTP_IO_CAP_CONFIG_RE_CAP_EN_(channel);
 911                 val |= PTP_IO_CAP_CONFIG_LOCK_FE_(channel);
 912                 val |= PTP_IO_CAP_CONFIG_FE_CAP_EN_(channel);
 913         } else {
 914                 /* Rising eventing as Default */
 915                 val &= ~PTP_IO_CAP_CONFIG_LOCK_FE_(channel);
 916                 val &= ~PTP_IO_CAP_CONFIG_FE_CAP_EN_(channel);
 917                 val |= PTP_IO_CAP_CONFIG_LOCK_RE_(channel);
 918                 val |= PTP_IO_CAP_CONFIG_RE_CAP_EN_(channel);
 919         }
 920         lan743x_csr_write(adapter, PTP_IO_CAP_CONFIG, val);
 921
 922         /* PTP-IO Select */
 923         val = lan743x_csr_read(adapter, PTP_IO_SEL);
 924         val &= ~PTP_IO_SEL_MASK_;
 925         val |= channel << PTP_IO_SEL_SHIFT_;
 926         lan743x_csr_write(adapter, PTP_IO_SEL, val);
 927
 928         /* PTP Interrupt Enable Register */
 929         if (flags & PTP_FALLING_EDGE)
 930                 val = PTP_INT_EN_FE_EN_SET_(channel);
 931         else
 932                 val = PTP_INT_EN_RE_EN_SET_(channel);
 933         lan743x_csr_write(adapter, PTP_INT_EN_SET, val);
 934
 935         mutex_unlock(&ptp->command_lock);
 936
 937         return 0;
 938 }
 939
 940 static int lan743x_ptp_io_extts(struct lan743x_adapter *adapter, int on,
 941                                 struct ptp_extts_request *extts_request)
 942 {
 943         struct lan743x_ptp *ptp = &adapter->ptp;
 944         u32 flags = extts_request->flags;
 945         u32 index = extts_request->index;
 946         struct lan743x_extts *extts;
 947         int extts_pin;
 948         int ret = 0;
 949
 950         extts = &ptp->extts[index];
 951
 952         if (on) {
 953                 extts_pin = ptp_find_pin(ptp->ptp_clock, PTP_PF_EXTTS, index);
 954                 if (extts_pin < 0)
 955                         return -EBUSY;
 956
 957                 ret = lan743x_ptp_io_event_cap_en(adapter, flags, index);
 958                 if (!ret)
 959                         extts->flags = flags;
 960         } else {
 961                 lan743x_ptp_io_extts_off(adapter, index);
 962         }
 963
 964         return ret;
 965 }
 966
 967 static int lan743x_ptpci_enable(struct ptp_clock_info *ptpci,
 968                                 struct ptp_clock_request *request, int on)
 969 {
 970         struct lan743x_ptp *ptp =
 971                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
 972         struct lan743x_adapter *adapter =
 973                 container_of(ptp, struct lan743x_adapter, ptp);
 974
 975         if (request) {
 976                 switch (request->type) {
 977                 case PTP_CLK_REQ_EXTTS:
 978                         if (request->extts.index < ptpci->n_ext_ts)
 979                                 return lan743x_ptp_io_extts(adapter, on,
 980                                                          &request->extts);
 981                         return -EINVAL;
 982                 case PTP_CLK_REQ_PEROUT:
 983                         if (request->perout.index < ptpci->n_per_out) {
 984                                 if (adapter->is_pci11x1x)
 985                                         return lan743x_ptp_io_perout(adapter, on,
 986                                                              &request->perout);
 987                                 else
 988                                         return lan743x_ptp_perout(adapter, on,
 989                                                           &request->perout);
 990                         }
 991                         return -EINVAL;
 992                 case PTP_CLK_REQ_PPS:
 993                         return -EINVAL;
 994                 default:
 995                         netif_err(adapter, drv, adapter->netdev,
 996                                   "request->type == %d, Unknown\n",
 997                                   request->type);
 998                         break;
 999                 }
1000         } else {
1001                 netif_err(adapter, drv, adapter->netdev, "request == NULL\n");
1002         }
1003         return 0;
1004 }
1005
1006 static int lan743x_ptpci_verify_pin_config(struct ptp_clock_info *ptp,
1007                                            unsigned int pin,
1008                                            enum ptp_pin_function func,
1009                                            unsigned int chan)
1010 {
1011         struct lan743x_ptp *lan_ptp =
1012                 container_of(ptp, struct lan743x_ptp, ptp_clock_info);
1013         struct lan743x_adapter *adapter =
1014                 container_of(lan_ptp, struct lan743x_adapter, ptp);
1015         int result = 0;
1016
1017         /* Confirm the requested function is supported. Parameter
1018          * validation is done by the caller.
1019          */
1020         switch (func) {
1021         case PTP_PF_NONE:
1022         case PTP_PF_PEROUT:
1023                 break;
1024         case PTP_PF_EXTTS:
1025                 if (!adapter->is_pci11x1x)
1026                         result = -1;
1027                 break;
1028         case PTP_PF_PHYSYNC:
1029         default:
1030                 result = -1;
1031                 break;
1032         }
1033         return result;
1034 }
1035
1036 static void lan743x_ptp_io_event_clock_get(struct lan743x_adapter *adapter,
1037                                            bool fe, u8 channel,
1038                                            struct timespec64 *ts)
1039 {
1040         struct lan743x_ptp *ptp = &adapter->ptp;
1041         struct lan743x_extts *extts;
1042         u32 sec, nsec;
1043
1044         mutex_lock(&ptp->command_lock);
1045         if (fe) {
1046                 sec = lan743x_csr_read(adapter, PTP_IO_FE_LTC_SEC_CAP_X);
1047                 nsec = lan743x_csr_read(adapter, PTP_IO_FE_LTC_NS_CAP_X);
1048         } else {
1049                 sec = lan743x_csr_read(adapter, PTP_IO_RE_LTC_SEC_CAP_X);
1050                 nsec = lan743x_csr_read(adapter, PTP_IO_RE_LTC_NS_CAP_X);
1051         }
1052
1053         mutex_unlock(&ptp->command_lock);
1054
1055         /* Update Local timestamp */
1056         extts = &ptp->extts[channel];
1057         extts->ts.tv_sec = sec;
1058         extts->ts.tv_nsec = nsec;
1059         ts->tv_sec = sec;
1060         ts->tv_nsec = nsec;
1061 }
1062
1063 static long lan743x_ptpci_do_aux_work(struct ptp_clock_info *ptpci)
1064 {
1065         struct lan743x_ptp *ptp =
1066                 container_of(ptpci, struct lan743x_ptp, ptp_clock_info);
1067         struct lan743x_adapter *adapter =
1068                 container_of(ptp, struct lan743x_adapter, ptp);
1069         u32 cap_info, cause, header, nsec, seconds;
1070         bool new_timestamp_available = false;
1071         struct ptp_clock_event ptp_event;
1072         struct timespec64 ts;
1073         int ptp_int_sts;
1074         int count = 0;
1075         int channel;
1076         s64 ns;
1077
1078         ptp_int_sts = lan743x_csr_read(adapter, PTP_INT_STS);
1079         while ((count < 100) && ptp_int_sts) {
1080                 count++;
1081
1082                 if (ptp_int_sts & PTP_INT_BIT_TX_TS_) {
1083                         cap_info = lan743x_csr_read(adapter, PTP_CAP_INFO);
1084
1085                         if (PTP_CAP_INFO_TX_TS_CNT_GET_(cap_info) > 0) {
1086                                 seconds = lan743x_csr_read(adapter,
1087                                                            PTP_TX_EGRESS_SEC);
1088                                 nsec = lan743x_csr_read(adapter,
1089                                                         PTP_TX_EGRESS_NS);
1090                                 cause = (nsec &
1091                                          PTP_TX_EGRESS_NS_CAPTURE_CAUSE_MASK_);
1092                                 header = lan743x_csr_read(adapter,
1093                                                           PTP_TX_MSG_HEADER);
1094
1095                                 if (cause ==
1096                                     PTP_TX_EGRESS_NS_CAPTURE_CAUSE_SW_) {
1097                                         nsec &= PTP_TX_EGRESS_NS_TS_NS_MASK_;
1098                                         lan743x_ptp_tx_ts_enqueue_ts(adapter,
1099                                                                      seconds,
1100                                                                      nsec,
1101                                                                      header);
1102                                         new_timestamp_available = true;
1103                                 } else if (cause ==
1104                                            PTP_TX_EGRESS_NS_CAPTURE_CAUSE_AUTO_) {
1105                                         netif_err(adapter, drv, adapter->netdev,
1106                                                   "Auto capture cause not supported\n");
1107                                 } else {
1108                                         netif_warn(adapter, drv, adapter->netdev,
1109                                                    "unknown tx timestamp capture cause\n");
1110                                 }
1111                         } else {
1112                                 netif_warn(adapter, drv, adapter->netdev,
1113                                            "TX TS INT but no TX TS CNT\n");
1114                         }
1115                         lan743x_csr_write(adapter, PTP_INT_STS,
1116                                           PTP_INT_BIT_TX_TS_);
1117                 }
1118
1119                 if (ptp_int_sts & PTP_INT_IO_FE_MASK_) {
1120                         do {
1121                                 channel = lan743x_get_channel((ptp_int_sts &
1122                                                         PTP_INT_IO_FE_MASK_) >>
1123                                                         PTP_INT_IO_FE_SHIFT_);
1124                                 if (channel >= 0 &&
1125                                     channel < PCI11X1X_PTP_IO_MAX_CHANNELS) {
1126                                         lan743x_ptp_io_event_clock_get(adapter,
1127                                                                        true,
1128                                                                        channel,
1129                                                                        &ts);
1130                                         /* PTP Falling Event post */
1131                                         ns = timespec64_to_ns(&ts);
1132                                         ptp_event.timestamp = ns;
1133                                         ptp_event.index = channel;
1134                                         ptp_event.type = PTP_CLOCK_EXTTS;
1135                                         ptp_clock_event(ptp->ptp_clock,
1136                                                         &ptp_event);
1137                                         lan743x_csr_write(adapter, PTP_INT_STS,
1138                                                           PTP_INT_IO_FE_SET_
1139                                                           (channel));
1140                                         ptp_int_sts &= ~(1 <<
1141                                                          (PTP_INT_IO_FE_SHIFT_ +
1142                                                           channel));
1143                                 } else {
1144                                         /* Clear falling event interrupts */
1145                                         lan743x_csr_write(adapter, PTP_INT_STS,
1146                                                           PTP_INT_IO_FE_MASK_);
1147                                         ptp_int_sts &= ~PTP_INT_IO_FE_MASK_;
1148                                 }
1149                         } while (ptp_int_sts & PTP_INT_IO_FE_MASK_);
1150                 }
1151
1152                 if (ptp_int_sts & PTP_INT_IO_RE_MASK_) {
1153                         do {
1154                                 channel = lan743x_get_channel((ptp_int_sts &
1155                                                        PTP_INT_IO_RE_MASK_) >>
1156                                                        PTP_INT_IO_RE_SHIFT_);
1157                                 if (channel >= 0 &&
1158                                     channel < PCI11X1X_PTP_IO_MAX_CHANNELS) {
1159                                         lan743x_ptp_io_event_clock_get(adapter,
1160                                                                        false,
1161                                                                        channel,
1162                                                                        &ts);
1163                                         /* PTP Rising Event post */
1164                                         ns = timespec64_to_ns(&ts);
1165                                         ptp_event.timestamp = ns;
1166                                         ptp_event.index = channel;
1167                                         ptp_event.type = PTP_CLOCK_EXTTS;
1168                                         ptp_clock_event(ptp->ptp_clock,
1169                                                         &ptp_event);
1170                                         lan743x_csr_write(adapter, PTP_INT_STS,
1171                                                           PTP_INT_IO_RE_SET_
1172                                                           (channel));
1173                                         ptp_int_sts &= ~(1 <<
1174                                                          (PTP_INT_IO_RE_SHIFT_ +
1175                                                           channel));
1176                                 } else {
1177                                         /* Clear Rising event interrupt */
1178                                         lan743x_csr_write(adapter, PTP_INT_STS,
1179                                                           PTP_INT_IO_RE_MASK_);
1180                                         ptp_int_sts &= ~PTP_INT_IO_RE_MASK_;
1181                                 }
1182                         } while (ptp_int_sts & PTP_INT_IO_RE_MASK_);
1183                 }
1184
1185                 ptp_int_sts = lan743x_csr_read(adapter, PTP_INT_STS);
1186         }
1187
1188         if (new_timestamp_available)
1189                 lan743x_ptp_tx_ts_complete(adapter);
1190
1191         lan743x_csr_write(adapter, INT_EN_SET, INT_BIT_1588_);
1192
1193         return -1;
1194 }
1195
1196 static void lan743x_ptp_clock_get(struct lan743x_adapter *adapter,
1197                                   u32 *seconds, u32 *nano_seconds,
1198                                   u32 *sub_nano_seconds)
1199 {
1200         struct lan743x_ptp *ptp = &adapter->ptp;
1201
1202         mutex_lock(&ptp->command_lock);
1203
1204         lan743x_csr_write(adapter, PTP_CMD_CTL, PTP_CMD_CTL_PTP_CLOCK_READ_);
1205         lan743x_ptp_wait_till_cmd_done(adapter, PTP_CMD_CTL_PTP_CLOCK_READ_);
1206
1207         if (seconds)
1208                 (*seconds) = lan743x_csr_read(adapter, PTP_CLOCK_SEC);
1209
1210         if (nano_seconds)
1211                 (*nano_seconds) = lan743x_csr_read(adapter, PTP_CLOCK_NS);
1212
1213         if (sub_nano_seconds)
1214                 (*sub_nano_seconds) =
1215                 lan743x_csr_read(adapter, PTP_CLOCK_SUBNS);
1216
1217         mutex_unlock(&ptp->command_lock);
1218 }
1219
1220 static void lan743x_ptp_io_clock_get(struct lan743x_adapter *adapter,
1221                                      u32 *sec, u32 *nsec, u32 *sub_nsec)
1222 {
1223         struct lan743x_ptp *ptp = &adapter->ptp;
1224
1225         mutex_lock(&ptp->command_lock);
1226         lan743x_csr_write(adapter, PTP_CMD_CTL, PTP_CMD_CTL_PTP_CLOCK_READ_);
1227         lan743x_ptp_wait_till_cmd_done(adapter, PTP_CMD_CTL_PTP_CLOCK_READ_);
1228
1229         if (sec)
1230                 (*sec) = lan743x_csr_read(adapter, PTP_LTC_RD_SEC_LO);
1231
1232         if (nsec)
1233                 (*nsec) = lan743x_csr_read(adapter, PTP_LTC_RD_NS);
1234
1235         if (sub_nsec)
1236                 (*sub_nsec) =
1237                 lan743x_csr_read(adapter, PTP_LTC_RD_SUBNS);
1238
1239         mutex_unlock(&ptp->command_lock);
1240 }
1241
1242 static void lan743x_ptp_clock_step(struct lan743x_adapter *adapter,
1243                                    s64 time_step_ns)
1244 {
1245         struct lan743x_ptp *ptp = &adapter->ptp;
1246         u32 nano_seconds_step = 0;
1247         u64 abs_time_step_ns = 0;
1248         u32 unsigned_seconds = 0;
1249         u32 nano_seconds = 0;
1250         u32 remainder = 0;
1251         s32 seconds = 0;
1252
1253         if (time_step_ns >  15000000000LL) {
1254                 /* convert to clock set */
1255                 if (adapter->is_pci11x1x)
1256                         lan743x_ptp_io_clock_get(adapter, &unsigned_seconds,
1257                                                  &nano_seconds, NULL);
1258                 else
1259                         lan743x_ptp_clock_get(adapter, &unsigned_seconds,
1260                                               &nano_seconds, NULL);
1261                 unsigned_seconds += div_u64_rem(time_step_ns, 1000000000LL,
1262                                                 &remainder);
1263                 nano_seconds += remainder;
1264                 if (nano_seconds >= 1000000000) {
1265                         unsigned_seconds++;
1266                         nano_seconds -= 1000000000;
1267                 }
1268                 lan743x_ptp_clock_set(adapter, unsigned_seconds,
1269                                       nano_seconds, 0);
1270                 return;
1271         } else if (time_step_ns < -15000000000LL) {
1272                 /* convert to clock set */
1273                 time_step_ns = -time_step_ns;
1274
1275                 if (adapter->is_pci11x1x) {
1276                         lan743x_ptp_io_clock_get(adapter, &unsigned_seconds,
1277                                                  &nano_seconds, NULL);
1278                 } else {
1279                         lan743x_ptp_clock_get(adapter, &unsigned_seconds,
1280                                               &nano_seconds, NULL);
1281                 }
1282                 unsigned_seconds -= div_u64_rem(time_step_ns, 1000000000LL,
1283                                                 &remainder);
1284                 nano_seconds_step = remainder;
1285                 if (nano_seconds < nano_seconds_step) {
1286                         unsigned_seconds--;
1287                         nano_seconds += 1000000000;
1288                 }
1289                 nano_seconds -= nano_seconds_step;
1290                 lan743x_ptp_clock_set(adapter, unsigned_seconds,
1291                                       nano_seconds, 0);
1292                 return;
1293         }
1294
1295         /* do clock step */
1296         if (time_step_ns >= 0) {
1297                 abs_time_step_ns = (u64)(time_step_ns);
1298                 seconds = (s32)div_u64_rem(abs_time_step_ns, 1000000000,
1299                                            &remainder);
1300                 nano_seconds = (u32)remainder;
1301         } else {
1302                 abs_time_step_ns = (u64)(-time_step_ns);
1303                 seconds = -((s32)div_u64_rem(abs_time_step_ns, 1000000000,
1304                                              &remainder));
1305                 nano_seconds = (u32)remainder;
1306                 if (nano_seconds > 0) {
1307                         /* subtracting nano seconds is not allowed
1308                          * convert to subtracting from seconds,
1309                          * and adding to nanoseconds
1310                          */
1311                         seconds--;
1312                         nano_seconds = (1000000000 - nano_seconds);
1313                 }
1314         }
1315
1316         if (nano_seconds > 0) {
1317                 /* add 8 ns to cover the likely normal increment */
1318                 nano_seconds += 8;
1319         }
1320
1321         if (nano_seconds >= 1000000000) {
1322                 /* carry into seconds */
1323                 seconds++;
1324                 nano_seconds -= 1000000000;
1325         }
1326
1327         while (seconds) {
1328                 mutex_lock(&ptp->command_lock);
1329                 if (seconds > 0) {
1330                         u32 adjustment_value = (u32)seconds;
1331
1332                         if (adjustment_value > 0xF)
1333                                 adjustment_value = 0xF;
1334                         lan743x_csr_write(adapter, PTP_CLOCK_STEP_ADJ,
1335                                           PTP_CLOCK_STEP_ADJ_DIR_ |
1336                                           adjustment_value);
1337                         seconds -= ((s32)adjustment_value);
1338                 } else {
1339                         u32 adjustment_value = (u32)(-seconds);
1340
1341                         if (adjustment_value > 0xF)
1342                                 adjustment_value = 0xF;
1343                         lan743x_csr_write(adapter, PTP_CLOCK_STEP_ADJ,
1344                                           adjustment_value);
1345                         seconds += ((s32)adjustment_value);
1346                 }
1347                 lan743x_csr_write(adapter, PTP_CMD_CTL,
1348                                   PTP_CMD_CTL_PTP_CLOCK_STEP_SEC_);
1349                 lan743x_ptp_wait_till_cmd_done(adapter,
1350                                                PTP_CMD_CTL_PTP_CLOCK_STEP_SEC_);
1351                 mutex_unlock(&ptp->command_lock);
1352         }
1353         if (nano_seconds) {
1354                 mutex_lock(&ptp->command_lock);
1355                 lan743x_csr_write(adapter, PTP_CLOCK_STEP_ADJ,
1356                                   PTP_CLOCK_STEP_ADJ_DIR_ |
1357                                   (nano_seconds &
1358                                   PTP_CLOCK_STEP_ADJ_VALUE_MASK_));
1359                 lan743x_csr_write(adapter, PTP_CMD_CTL,
1360                                   PTP_CMD_CTL_PTP_CLK_STP_NSEC_);
1361                 lan743x_ptp_wait_till_cmd_done(adapter,
1362                                                PTP_CMD_CTL_PTP_CLK_STP_NSEC_);
1363                 mutex_unlock(&ptp->command_lock);
1364         }
1365 }
1366
1367 void lan743x_ptp_isr(void *context)
1368 {
1369         struct lan743x_adapter *adapter = (struct lan743x_adapter *)context;
1370         struct lan743x_ptp *ptp = NULL;
1371         int enable_flag = 1;
1372         u32 ptp_int_sts = 0;
1373
1374         ptp = &adapter->ptp;
1375
1376         lan743x_csr_write(adapter, INT_EN_CLR, INT_BIT_1588_);
1377
1378         ptp_int_sts = lan743x_csr_read(adapter, PTP_INT_STS);
1379         ptp_int_sts &= lan743x_csr_read(adapter, PTP_INT_EN_SET);
1380
1381         if (ptp_int_sts & PTP_INT_BIT_TX_TS_) {
1382                 ptp_schedule_worker(ptp->ptp_clock, 0);
1383                 enable_flag = 0;/* tasklet will re-enable later */
1384         }
1385         if (ptp_int_sts & PTP_INT_BIT_TX_SWTS_ERR_) {
1386                 netif_err(adapter, drv, adapter->netdev,
1387                           "PTP TX Software Timestamp Error\n");
1388                 /* clear int status bit */
1389                 lan743x_csr_write(adapter, PTP_INT_STS,
1390                                   PTP_INT_BIT_TX_SWTS_ERR_);
1391         }
1392         if (ptp_int_sts & PTP_INT_BIT_TIMER_B_) {
1393                 /* clear int status bit */
1394                 lan743x_csr_write(adapter, PTP_INT_STS,
1395                                   PTP_INT_BIT_TIMER_B_);
1396         }
1397         if (ptp_int_sts & PTP_INT_BIT_TIMER_A_) {
1398                 /* clear int status bit */
1399                 lan743x_csr_write(adapter, PTP_INT_STS,
1400                                   PTP_INT_BIT_TIMER_A_);
1401         }
1402
1403         if (enable_flag) {
1404                 /* re-enable isr */
1405                 lan743x_csr_write(adapter, INT_EN_SET, INT_BIT_1588_);
1406         }
1407 }
1408
1409 static void lan743x_ptp_tx_ts_enqueue_skb(struct lan743x_adapter *adapter,
1410                                           struct sk_buff *skb, bool ignore_sync)
1411 {
1412         struct lan743x_ptp *ptp = &adapter->ptp;
1413
1414         spin_lock_bh(&ptp->tx_ts_lock);
1415         if (ptp->tx_ts_skb_queue_size < LAN743X_PTP_NUMBER_OF_TX_TIMESTAMPS) {
1416                 ptp->tx_ts_skb_queue[ptp->tx_ts_skb_queue_size] = skb;
1417                 if (ignore_sync)
1418                         ptp->tx_ts_ignore_sync_queue |=
1419                                 BIT(ptp->tx_ts_skb_queue_size);
1420                 ptp->tx_ts_skb_queue_size++;
1421         } else {
1422                 /* this should never happen, so long as the tx channel
1423                  * calls and honors the result from
1424                  * lan743x_ptp_request_tx_timestamp
1425                  */
1426                 netif_err(adapter, drv, adapter->netdev,
1427                           "tx ts skb queue overflow\n");
1428                 dev_kfree_skb(skb);
1429         }
1430         spin_unlock_bh(&ptp->tx_ts_lock);
1431 }
1432
1433 static void lan743x_ptp_sync_to_system_clock(struct lan743x_adapter *adapter)
1434 {
1435         struct timespec64 ts;
1436
1437         ktime_get_clocktai_ts64(&ts);
1438
1439         lan743x_ptp_clock_set(adapter, ts.tv_sec, ts.tv_nsec, 0);
1440 }
1441
1442 void lan743x_ptp_update_latency(struct lan743x_adapter *adapter,
1443                                 u32 link_speed)
1444 {
1445         switch (link_speed) {
1446         case 10:
1447                 lan743x_csr_write(adapter, PTP_LATENCY,
1448                                   PTP_LATENCY_TX_SET_(0) |
1449                                   PTP_LATENCY_RX_SET_(0));
1450                 break;
1451         case 100:
1452                 lan743x_csr_write(adapter, PTP_LATENCY,
1453                                   PTP_LATENCY_TX_SET_(181) |
1454                                   PTP_LATENCY_RX_SET_(594));
1455                 break;
1456         case 1000:
1457                 lan743x_csr_write(adapter, PTP_LATENCY,
1458                                   PTP_LATENCY_TX_SET_(30) |
1459                                   PTP_LATENCY_RX_SET_(525));
1460                 break;
1461         }
1462 }
1463
1464 int lan743x_ptp_init(struct lan743x_adapter *adapter)
1465 {
1466         struct lan743x_ptp *ptp = &adapter->ptp;
1467         int i;
1468
1469         mutex_init(&ptp->command_lock);
1470         spin_lock_init(&ptp->tx_ts_lock);
1471         ptp->used_event_ch = 0;
1472
1473         for (i = 0; i < LAN743X_PTP_N_EVENT_CHAN; i++) {
1474                 ptp->perout[i].event_ch = -1;
1475                 ptp->perout[i].gpio_pin = -1;
1476         }
1477
1478         lan743x_led_mux_save(adapter);
1479
1480         return 0;
1481 }
1482
1483 int lan743x_ptp_open(struct lan743x_adapter *adapter)
1484 {
1485         struct lan743x_ptp *ptp = &adapter->ptp;
1486         int ret = -ENODEV;
1487         u32 temp;
1488         int i;
1489         int n_pins;
1490
1491         lan743x_ptp_reset(adapter);
1492         lan743x_ptp_sync_to_system_clock(adapter);
1493         temp = lan743x_csr_read(adapter, PTP_TX_MOD2);
1494         temp |= PTP_TX_MOD2_TX_PTP_CLR_UDPV4_CHKSUM_;
1495         lan743x_csr_write(adapter, PTP_TX_MOD2, temp);
1496         lan743x_ptp_enable(adapter);
1497         lan743x_csr_write(adapter, INT_EN_SET, INT_BIT_1588_);
1498         lan743x_csr_write(adapter, PTP_INT_EN_SET,
1499                           PTP_INT_BIT_TX_SWTS_ERR_ | PTP_INT_BIT_TX_TS_);
1500         ptp->flags |= PTP_FLAG_ISR_ENABLED;
1501
1502         if (!IS_ENABLED(CONFIG_PTP_1588_CLOCK))
1503                 return 0;
1504
1505         switch (adapter->csr.id_rev & ID_REV_ID_MASK_) {
1506         case ID_REV_ID_LAN7430_:
1507                 n_pins = LAN7430_N_GPIO;
1508                 break;
1509         case ID_REV_ID_LAN7431_:
1510         case ID_REV_ID_A011_:
1511         case ID_REV_ID_A041_:
1512                 n_pins = LAN7431_N_GPIO;
1513                 break;
1514         default:
1515                 netif_warn(adapter, drv, adapter->netdev,
1516                            "Unknown LAN743x (%08x). Assuming no GPIO\n",
1517                            adapter->csr.id_rev);
1518                 n_pins = 0;
1519                 break;
1520         }
1521
1522         if (n_pins > LAN743X_PTP_N_GPIO)
1523                 n_pins = LAN743X_PTP_N_GPIO;
1524
1525         for (i = 0; i < n_pins; i++) {
1526                 struct ptp_pin_desc *ptp_pin = &ptp->pin_config[i];
1527
1528                 snprintf(ptp_pin->name,
1529                          sizeof(ptp_pin->name), "lan743x_ptp_pin_%02d", i);
1530                 ptp_pin->index = i;
1531                 ptp_pin->func = PTP_PF_NONE;
1532         }
1533
1534         ptp->ptp_clock_info.owner = THIS_MODULE;
1535         snprintf(ptp->ptp_clock_info.name, 16, "%pm",
1536                  adapter->netdev->dev_addr);
1537         ptp->ptp_clock_info.max_adj = LAN743X_PTP_MAX_FREQ_ADJ_IN_PPB;
1538         ptp->ptp_clock_info.n_alarm = 0;
1539         ptp->ptp_clock_info.n_ext_ts = LAN743X_PTP_N_EXTTS;
1540         ptp->ptp_clock_info.n_per_out = LAN743X_PTP_N_EVENT_CHAN;
1541         ptp->ptp_clock_info.n_pins = n_pins;
1542         ptp->ptp_clock_info.pps = LAN743X_PTP_N_PPS;
1543         ptp->ptp_clock_info.pin_config = ptp->pin_config;
1544         ptp->ptp_clock_info.adjfine = lan743x_ptpci_adjfine;
1545         ptp->ptp_clock_info.adjtime = lan743x_ptpci_adjtime;
1546         ptp->ptp_clock_info.gettime64 = lan743x_ptpci_gettime64;
1547         ptp->ptp_clock_info.getcrosststamp = NULL;
1548         ptp->ptp_clock_info.settime64 = lan743x_ptpci_settime64;
1549         ptp->ptp_clock_info.enable = lan743x_ptpci_enable;
1550         ptp->ptp_clock_info.do_aux_work = lan743x_ptpci_do_aux_work;
1551         ptp->ptp_clock_info.verify = lan743x_ptpci_verify_pin_config;
1552
1553         ptp->ptp_clock = ptp_clock_register(&ptp->ptp_clock_info,
1554                                             &adapter->pdev->dev);
1555
1556         if (IS_ERR(ptp->ptp_clock)) {
1557                 netif_err(adapter, ifup, adapter->netdev,
1558                           "ptp_clock_register failed\n");
1559                 goto done;
1560         }
1561         ptp->flags |= PTP_FLAG_PTP_CLOCK_REGISTERED;
1562         netif_info(adapter, ifup, adapter->netdev,
1563                    "successfully registered ptp clock\n");
1564
1565         return 0;
1566 done:
1567         lan743x_ptp_close(adapter);
1568         return ret;
1569 }
1570
1571 void lan743x_ptp_close(struct lan743x_adapter *adapter)
1572 {
1573         struct lan743x_ptp *ptp = &adapter->ptp;
1574         int index;
1575
1576         if (IS_ENABLED(CONFIG_PTP_1588_CLOCK) &&
1577             (ptp->flags & PTP_FLAG_PTP_CLOCK_REGISTERED)) {
1578                 ptp_clock_unregister(ptp->ptp_clock);
1579                 ptp->ptp_clock = NULL;
1580                 ptp->flags &= ~PTP_FLAG_PTP_CLOCK_REGISTERED;
1581                 netif_info(adapter, drv, adapter->netdev,
1582                            "ptp clock unregister\n");
1583         }
1584
1585         if (ptp->flags & PTP_FLAG_ISR_ENABLED) {
1586                 lan743x_csr_write(adapter, PTP_INT_EN_CLR,
1587                                   PTP_INT_BIT_TX_SWTS_ERR_ |
1588                                   PTP_INT_BIT_TX_TS_);
1589                 lan743x_csr_write(adapter, INT_EN_CLR, INT_BIT_1588_);
1590                 ptp->flags &= ~PTP_FLAG_ISR_ENABLED;
1591         }
1592
1593         /* clean up pending timestamp requests */
1594         lan743x_ptp_tx_ts_complete(adapter);
1595         spin_lock_bh(&ptp->tx_ts_lock);
1596         for (index = 0;
1597                 index < LAN743X_PTP_NUMBER_OF_TX_TIMESTAMPS;
1598                 index++) {
1599                 struct sk_buff *skb = ptp->tx_ts_skb_queue[index];
1600
1601                 dev_kfree_skb(skb);
1602                 ptp->tx_ts_skb_queue[index] = NULL;
1603                 ptp->tx_ts_seconds_queue[index] = 0;
1604                 ptp->tx_ts_nseconds_queue[index] = 0;
1605         }
1606         ptp->tx_ts_skb_queue_size = 0;
1607         ptp->tx_ts_queue_size = 0;
1608         ptp->pending_tx_timestamps = 0;
1609         spin_unlock_bh(&ptp->tx_ts_lock);
1610
1611         lan743x_led_mux_restore(adapter);
1612
1613         lan743x_ptp_disable(adapter);
1614 }
1615
1616 static void lan743x_ptp_set_sync_ts_insert(struct lan743x_adapter *adapter,
1617                                            bool ts_insert_enable)
1618 {
1619         u32 ptp_tx_mod = lan743x_csr_read(adapter, PTP_TX_MOD);
1620
1621         if (ts_insert_enable)
1622                 ptp_tx_mod |= PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_;
1623         else
1624                 ptp_tx_mod &= ~PTP_TX_MOD_TX_PTP_SYNC_TS_INSERT_;
1625
1626         lan743x_csr_write(adapter, PTP_TX_MOD, ptp_tx_mod);
1627 }
1628
1629 static bool lan743x_ptp_is_enabled(struct lan743x_adapter *adapter)
1630 {
1631         if (lan743x_csr_read(adapter, PTP_CMD_CTL) & PTP_CMD_CTL_PTP_ENABLE_)
1632                 return true;
1633         return false;
1634 }
1635
1636 static void lan743x_ptp_enable(struct lan743x_adapter *adapter)
1637 {
1638         struct lan743x_ptp *ptp = &adapter->ptp;
1639
1640         mutex_lock(&ptp->command_lock);
1641
1642         if (lan743x_ptp_is_enabled(adapter)) {
1643                 netif_warn(adapter, drv, adapter->netdev,
1644                            "PTP already enabled\n");
1645                 goto done;
1646         }
1647         lan743x_csr_write(adapter, PTP_CMD_CTL, PTP_CMD_CTL_PTP_ENABLE_);
1648 done:
1649         mutex_unlock(&ptp->command_lock);
1650 }
1651
1652 static void lan743x_ptp_disable(struct lan743x_adapter *adapter)
1653 {
1654         struct lan743x_ptp *ptp = &adapter->ptp;
1655
1656         mutex_lock(&ptp->command_lock);
1657         if (!lan743x_ptp_is_enabled(adapter)) {
1658                 netif_warn(adapter, drv, adapter->netdev,
1659                            "PTP already disabled\n");
1660                 goto done;
1661         }
1662         lan743x_csr_write(adapter, PTP_CMD_CTL, PTP_CMD_CTL_PTP_DISABLE_);
1663         lan743x_ptp_wait_till_cmd_done(adapter, PTP_CMD_CTL_PTP_ENABLE_);
1664 done:
1665         mutex_unlock(&ptp->command_lock);
1666 }
1667
1668 static void lan743x_ptp_reset(struct lan743x_adapter *adapter)
1669 {
1670         struct lan743x_ptp *ptp = &adapter->ptp;
1671
1672         mutex_lock(&ptp->command_lock);
1673
1674         if (lan743x_ptp_is_enabled(adapter)) {
1675                 netif_err(adapter, drv, adapter->netdev,
1676                           "Attempting reset while enabled\n");
1677                 goto done;
1678         }
1679
1680         lan743x_csr_write(adapter, PTP_CMD_CTL, PTP_CMD_CTL_PTP_RESET_);
1681         lan743x_ptp_wait_till_cmd_done(adapter, PTP_CMD_CTL_PTP_RESET_);
1682 done:
1683         mutex_unlock(&ptp->command_lock);
1684 }
1685
1686 static void lan743x_ptp_clock_set(struct lan743x_adapter *adapter,
1687                                   u32 seconds, u32 nano_seconds,
1688                                   u32 sub_nano_seconds)
1689 {
1690         struct lan743x_ptp *ptp = &adapter->ptp;
1691
1692         mutex_lock(&ptp->command_lock);
1693
1694         lan743x_csr_write(adapter, PTP_CLOCK_SEC, seconds);
1695         lan743x_csr_write(adapter, PTP_CLOCK_NS, nano_seconds);
1696         lan743x_csr_write(adapter, PTP_CLOCK_SUBNS, sub_nano_seconds);
1697
1698         lan743x_csr_write(adapter, PTP_CMD_CTL, PTP_CMD_CTL_PTP_CLOCK_LOAD_);
1699         lan743x_ptp_wait_till_cmd_done(adapter, PTP_CMD_CTL_PTP_CLOCK_LOAD_);
1700         mutex_unlock(&ptp->command_lock);
1701 }
1702
1703 bool lan743x_ptp_request_tx_timestamp(struct lan743x_adapter *adapter)
1704 {
1705         struct lan743x_ptp *ptp = &adapter->ptp;
1706         bool result = false;
1707
1708         spin_lock_bh(&ptp->tx_ts_lock);
1709         if (ptp->pending_tx_timestamps < LAN743X_PTP_NUMBER_OF_TX_TIMESTAMPS) {
1710                 /* request granted */
1711                 ptp->pending_tx_timestamps++;
1712                 result = true;
1713         }
1714         spin_unlock_bh(&ptp->tx_ts_lock);
1715         return result;
1716 }
1717
1718 void lan743x_ptp_unrequest_tx_timestamp(struct lan743x_adapter *adapter)
1719 {
1720         struct lan743x_ptp *ptp = &adapter->ptp;
1721
1722         spin_lock_bh(&ptp->tx_ts_lock);
1723         if (ptp->pending_tx_timestamps > 0)
1724                 ptp->pending_tx_timestamps--;
1725         else
1726                 netif_err(adapter, drv, adapter->netdev,
1727                           "unrequest failed, pending_tx_timestamps==0\n");
1728         spin_unlock_bh(&ptp->tx_ts_lock);
1729 }
1730
1731 void lan743x_ptp_tx_timestamp_skb(struct lan743x_adapter *adapter,
1732                                   struct sk_buff *skb, bool ignore_sync)
1733 {
1734         lan743x_ptp_tx_ts_enqueue_skb(adapter, skb, ignore_sync);
1735
1736         lan743x_ptp_tx_ts_complete(adapter);
1737 }
1738
1739 int lan743x_ptp_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
1740 {
1741         struct lan743x_adapter *adapter = netdev_priv(netdev);
1742         struct hwtstamp_config config;
1743         int ret = 0;
1744         int index;
1745
1746         if (!ifr) {
1747                 netif_err(adapter, drv, adapter->netdev,
1748                           "SIOCSHWTSTAMP, ifr == NULL\n");
1749                 return -EINVAL;
1750         }
1751
1752         if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
1753                 return -EFAULT;
1754
1755         switch (config.tx_type) {
1756         case HWTSTAMP_TX_OFF:
1757                 for (index = 0; index < adapter->used_tx_channels;
1758                      index++)
1759                         lan743x_tx_set_timestamping_mode(&adapter->tx[index],
1760                                                          false, false);
1761                 lan743x_ptp_set_sync_ts_insert(adapter, false);
1762                 break;
1763         case HWTSTAMP_TX_ON:
1764                 for (index = 0; index < adapter->used_tx_channels;
1765                         index++)
1766                         lan743x_tx_set_timestamping_mode(&adapter->tx[index],
1767                                                          true, false);
1768                 lan743x_ptp_set_sync_ts_insert(adapter, false);
1769                 break;
1770         case HWTSTAMP_TX_ONESTEP_SYNC:
1771                 for (index = 0; index < adapter->used_tx_channels;
1772                         index++)
1773                         lan743x_tx_set_timestamping_mode(&adapter->tx[index],
1774                                                          true, true);
1775
1776                 lan743x_ptp_set_sync_ts_insert(adapter, true);
1777                 break;
1778         case HWTSTAMP_TX_ONESTEP_P2P:
1779                 ret = -ERANGE;
1780                 break;
1781         default:
1782                 netif_warn(adapter, drv, adapter->netdev,
1783                            "  tx_type = %d, UNKNOWN\n", config.tx_type);
1784                 ret = -EINVAL;
1785                 break;
1786         }
1787
1788         if (!ret)
1789                 return copy_to_user(ifr->ifr_data, &config,
1790                         sizeof(config)) ? -EFAULT : 0;
1791         return ret;
1792 }