drivers/net/ethernet/aquantia/atlantic/aq_ptp.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /* Aquantia Corporation Network Driver
   3  * Copyright (C) 2014-2019 Aquantia Corporation. All rights reserved
   4  */
   5
   6 /* File aq_ptp.c:
   7  * Definition of functions for Linux PTP support.
   8  */
   9
  10 #include <linux/ptp_clock_kernel.h>
  11 #include <linux/ptp_classify.h>
  12 #include <linux/interrupt.h>
  13 #include <linux/clocksource.h>
  14
  15 #include "aq_nic.h"
  16 #include "aq_ptp.h"
  17 #include "aq_ring.h"
  18 #include "aq_phy.h"
  19 #include "aq_filters.h"
  20
  21 #define AQ_PTP_TX_TIMEOUT        (HZ *  10)
  22
  23 #define POLL_SYNC_TIMER_MS 15
  24
  25 enum ptp_speed_offsets {
  26         ptp_offset_idx_10 = 0,
  27         ptp_offset_idx_100,
  28         ptp_offset_idx_1000,
  29         ptp_offset_idx_2500,
  30         ptp_offset_idx_5000,
  31         ptp_offset_idx_10000,
  32 };
  33
  34 struct ptp_skb_ring {
  35         struct sk_buff **buff;
  36         spinlock_t lock;
  37         unsigned int size;
  38         unsigned int head;
  39         unsigned int tail;
  40 };
  41
  42 struct ptp_tx_timeout {
  43         spinlock_t lock;
  44         bool active;
  45         unsigned long tx_start;
  46 };
  47
  48 struct aq_ptp_s {
  49         struct aq_nic_s *aq_nic;
  50         struct hwtstamp_config hwtstamp_config;
  51         spinlock_t ptp_lock;
  52         spinlock_t ptp_ring_lock;
  53         struct ptp_clock *ptp_clock;
  54         struct ptp_clock_info ptp_info;
  55
  56         atomic_t offset_egress;
  57         atomic_t offset_ingress;
  58
  59         struct aq_ring_param_s ptp_ring_param;
  60
  61         struct ptp_tx_timeout ptp_tx_timeout;
  62
  63         unsigned int idx_vector;
  64         struct napi_struct napi;
  65
  66         struct aq_ring_s ptp_tx;
  67         struct aq_ring_s ptp_rx;
  68         struct aq_ring_s hwts_rx;
  69
  70         struct ptp_skb_ring skb_ring;
  71
  72         struct aq_rx_filter_l3l4 udp_filter;
  73         struct aq_rx_filter_l2 eth_type_filter;
  74
  75         struct delayed_work poll_sync;
  76         u32 poll_timeout_ms;
  77
  78         bool extts_pin_enabled;
  79         u64 last_sync1588_ts;
  80 };
  81
  82 struct ptp_tm_offset {
  83         unsigned int mbps;
  84         int egress;
  85         int ingress;
  86 };
  87
  88 static struct ptp_tm_offset ptp_offset[6];
  89
  90 void aq_ptp_tm_offset_set(struct aq_nic_s *aq_nic, unsigned int mbps)
  91 {
  92         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
  93         int i, egress, ingress;
  94
  95         if (!aq_ptp)
  96                 return;
  97
  98         egress = 0;
  99         ingress = 0;
 100
 101         for (i = 0; i < ARRAY_SIZE(ptp_offset); i++) {
 102                 if (mbps == ptp_offset[i].mbps) {
 103                         egress = ptp_offset[i].egress;
 104                         ingress = ptp_offset[i].ingress;
 105                         break;
 106                 }
 107         }
 108
 109         atomic_set(&aq_ptp->offset_egress, egress);
 110         atomic_set(&aq_ptp->offset_ingress, ingress);
 111 }
 112
 113 static int __aq_ptp_skb_put(struct ptp_skb_ring *ring, struct sk_buff *skb)
 114 {
 115         unsigned int next_head = (ring->head + 1) % ring->size;
 116
 117         if (next_head == ring->tail)
 118                 return -ENOMEM;
 119
 120         ring->buff[ring->head] = skb_get(skb);
 121         ring->head = next_head;
 122
 123         return 0;
 124 }
 125
 126 static int aq_ptp_skb_put(struct ptp_skb_ring *ring, struct sk_buff *skb)
 127 {
 128         unsigned long flags;
 129         int ret;
 130
 131         spin_lock_irqsave(&ring->lock, flags);
 132         ret = __aq_ptp_skb_put(ring, skb);
 133         spin_unlock_irqrestore(&ring->lock, flags);
 134
 135         return ret;
 136 }
 137
 138 static struct sk_buff *__aq_ptp_skb_get(struct ptp_skb_ring *ring)
 139 {
 140         struct sk_buff *skb;
 141
 142         if (ring->tail == ring->head)
 143                 return NULL;
 144
 145         skb = ring->buff[ring->tail];
 146         ring->tail = (ring->tail + 1) % ring->size;
 147
 148         return skb;
 149 }
 150
 151 static struct sk_buff *aq_ptp_skb_get(struct ptp_skb_ring *ring)
 152 {
 153         unsigned long flags;
 154         struct sk_buff *skb;
 155
 156         spin_lock_irqsave(&ring->lock, flags);
 157         skb = __aq_ptp_skb_get(ring);
 158         spin_unlock_irqrestore(&ring->lock, flags);
 159
 160         return skb;
 161 }
 162
 163 static unsigned int aq_ptp_skb_buf_len(struct ptp_skb_ring *ring)
 164 {
 165         unsigned long flags;
 166         unsigned int len;
 167
 168         spin_lock_irqsave(&ring->lock, flags);
 169         len = (ring->head >= ring->tail) ?
 170         ring->head - ring->tail :
 171         ring->size - ring->tail + ring->head;
 172         spin_unlock_irqrestore(&ring->lock, flags);
 173
 174         return len;
 175 }
 176
 177 static int aq_ptp_skb_ring_init(struct ptp_skb_ring *ring, unsigned int size)
 178 {
 179         struct sk_buff **buff = kmalloc(sizeof(*buff) * size, GFP_KERNEL);
 180
 181         if (!buff)
 182                 return -ENOMEM;
 183
 184         spin_lock_init(&ring->lock);
 185
 186         ring->buff = buff;
 187         ring->size = size;
 188         ring->head = 0;
 189         ring->tail = 0;
 190
 191         return 0;
 192 }
 193
 194 static void aq_ptp_skb_ring_clean(struct ptp_skb_ring *ring)
 195 {
 196         struct sk_buff *skb;
 197
 198         while ((skb = aq_ptp_skb_get(ring)) != NULL)
 199                 dev_kfree_skb_any(skb);
 200 }
 201
 202 static void aq_ptp_skb_ring_release(struct ptp_skb_ring *ring)
 203 {
 204         if (ring->buff) {
 205                 aq_ptp_skb_ring_clean(ring);
 206                 kfree(ring->buff);
 207                 ring->buff = NULL;
 208         }
 209 }
 210
 211 static void aq_ptp_tx_timeout_init(struct ptp_tx_timeout *timeout)
 212 {
 213         spin_lock_init(&timeout->lock);
 214         timeout->active = false;
 215 }
 216
 217 static void aq_ptp_tx_timeout_start(struct aq_ptp_s *aq_ptp)
 218 {
 219         struct ptp_tx_timeout *timeout = &aq_ptp->ptp_tx_timeout;
 220         unsigned long flags;
 221
 222         spin_lock_irqsave(&timeout->lock, flags);
 223         timeout->active = true;
 224         timeout->tx_start = jiffies;
 225         spin_unlock_irqrestore(&timeout->lock, flags);
 226 }
 227
 228 static void aq_ptp_tx_timeout_update(struct aq_ptp_s *aq_ptp)
 229 {
 230         if (!aq_ptp_skb_buf_len(&aq_ptp->skb_ring)) {
 231                 struct ptp_tx_timeout *timeout = &aq_ptp->ptp_tx_timeout;
 232                 unsigned long flags;
 233
 234                 spin_lock_irqsave(&timeout->lock, flags);
 235                 timeout->active = false;
 236                 spin_unlock_irqrestore(&timeout->lock, flags);
 237         }
 238 }
 239
 240 static void aq_ptp_tx_timeout_check(struct aq_ptp_s *aq_ptp)
 241 {
 242         struct ptp_tx_timeout *timeout = &aq_ptp->ptp_tx_timeout;
 243         unsigned long flags;
 244         bool timeout_flag;
 245
 246         timeout_flag = false;
 247
 248         spin_lock_irqsave(&timeout->lock, flags);
 249         if (timeout->active) {
 250                 timeout_flag = time_is_before_jiffies(timeout->tx_start +
 251                                                       AQ_PTP_TX_TIMEOUT);
 252                 /* reset active flag if timeout detected */
 253                 if (timeout_flag)
 254                         timeout->active = false;
 255         }
 256         spin_unlock_irqrestore(&timeout->lock, flags);
 257
 258         if (timeout_flag) {
 259                 aq_ptp_skb_ring_clean(&aq_ptp->skb_ring);
 260                 netdev_err(aq_ptp->aq_nic->ndev,
 261                            "PTP Timeout. Clearing Tx Timestamp SKBs\n");
 262         }
 263 }
 264
 265 /* aq_ptp_adjfine
 266  * @ptp: the ptp clock structure
 267  * @ppb: parts per billion adjustment from base
 268  *
 269  * adjust the frequency of the ptp cycle counter by the
 270  * indicated ppb from the base frequency.
 271  */
 272 static int aq_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 273 {
 274         struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
 275         struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
 276
 277         mutex_lock(&aq_nic->fwreq_mutex);
 278         aq_nic->aq_hw_ops->hw_adj_clock_freq(aq_nic->aq_hw,
 279                                              scaled_ppm_to_ppb(scaled_ppm));
 280         mutex_unlock(&aq_nic->fwreq_mutex);
 281
 282         return 0;
 283 }
 284
 285 /* aq_ptp_adjtime
 286  * @ptp: the ptp clock structure
 287  * @delta: offset to adjust the cycle counter by
 288  *
 289  * adjust the timer by resetting the timecounter structure.
 290  */
 291 static int aq_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 292 {
 293         struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
 294         struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
 295         unsigned long flags;
 296
 297         spin_lock_irqsave(&aq_ptp->ptp_lock, flags);
 298         aq_nic->aq_hw_ops->hw_adj_sys_clock(aq_nic->aq_hw, delta);
 299         spin_unlock_irqrestore(&aq_ptp->ptp_lock, flags);
 300
 301         return 0;
 302 }
 303
 304 /* aq_ptp_gettime
 305  * @ptp: the ptp clock structure
 306  * @ts: timespec structure to hold the current time value
 307  *
 308  * read the timecounter and return the correct value on ns,
 309  * after converting it into a struct timespec.
 310  */
 311 static int aq_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
 312 {
 313         struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
 314         struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
 315         unsigned long flags;
 316         u64 ns;
 317
 318         spin_lock_irqsave(&aq_ptp->ptp_lock, flags);
 319         aq_nic->aq_hw_ops->hw_get_ptp_ts(aq_nic->aq_hw, &ns);
 320         spin_unlock_irqrestore(&aq_ptp->ptp_lock, flags);
 321
 322         *ts = ns_to_timespec64(ns);
 323
 324         return 0;
 325 }
 326
 327 /* aq_ptp_settime
 328  * @ptp: the ptp clock structure
 329  * @ts: the timespec containing the new time for the cycle counter
 330  *
 331  * reset the timecounter to use a new base value instead of the kernel
 332  * wall timer value.
 333  */
 334 static int aq_ptp_settime(struct ptp_clock_info *ptp,
 335                           const struct timespec64 *ts)
 336 {
 337         struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
 338         struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
 339         unsigned long flags;
 340         u64 ns = timespec64_to_ns(ts);
 341         u64 now;
 342
 343         spin_lock_irqsave(&aq_ptp->ptp_lock, flags);
 344         aq_nic->aq_hw_ops->hw_get_ptp_ts(aq_nic->aq_hw, &now);
 345         aq_nic->aq_hw_ops->hw_adj_sys_clock(aq_nic->aq_hw, (s64)ns - (s64)now);
 346
 347         spin_unlock_irqrestore(&aq_ptp->ptp_lock, flags);
 348
 349         return 0;
 350 }
 351
 352 static void aq_ptp_convert_to_hwtstamp(struct aq_ptp_s *aq_ptp,
 353                                        struct skb_shared_hwtstamps *hwtstamp,
 354                                        u64 timestamp)
 355 {
 356         memset(hwtstamp, 0, sizeof(*hwtstamp));
 357         hwtstamp->hwtstamp = ns_to_ktime(timestamp);
 358 }
 359
 360 static int aq_ptp_hw_pin_conf(struct aq_nic_s *aq_nic, u32 pin_index, u64 start,
 361                               u64 period)
 362 {
 363         if (period)
 364                 netdev_dbg(aq_nic->ndev,
 365                            "Enable GPIO %d pulsing, start time %llu, period %u\n",
 366                            pin_index, start, (u32)period);
 367         else
 368                 netdev_dbg(aq_nic->ndev,
 369                            "Disable GPIO %d pulsing, start time %llu, period %u\n",
 370                            pin_index, start, (u32)period);
 371
 372         /* Notify hardware of request to being sending pulses.
 373          * If period is ZERO then pulsen is disabled.
 374          */
 375         mutex_lock(&aq_nic->fwreq_mutex);
 376         aq_nic->aq_hw_ops->hw_gpio_pulse(aq_nic->aq_hw, pin_index,
 377                                          start, (u32)period);
 378         mutex_unlock(&aq_nic->fwreq_mutex);
 379
 380         return 0;
 381 }
 382
 383 static int aq_ptp_perout_pin_configure(struct ptp_clock_info *ptp,
 384                                        struct ptp_clock_request *rq, int on)
 385 {
 386         struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
 387         struct ptp_clock_time *t = &rq->perout.period;
 388         struct ptp_clock_time *s = &rq->perout.start;
 389         struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
 390         u64 start, period;
 391         u32 pin_index = rq->perout.index;
 392
 393         /* verify the request channel is there */
 394         if (pin_index >= ptp->n_per_out)
 395                 return -EINVAL;
 396
 397         /* we cannot support periods greater
 398          * than 4 seconds due to reg limit
 399          */
 400         if (t->sec > 4 || t->sec < 0)
 401                 return -ERANGE;
 402
 403         /* convert to unsigned 64b ns,
 404          * verify we can put it in a 32b register
 405          */
 406         period = on ? t->sec * NSEC_PER_SEC + t->nsec : 0;
 407
 408         /* verify the value is in range supported by hardware */
 409         if (period > U32_MAX)
 410                 return -ERANGE;
 411         /* convert to unsigned 64b ns */
 412         /* TODO convert to AQ time */
 413         start = on ? s->sec * NSEC_PER_SEC + s->nsec : 0;
 414
 415         aq_ptp_hw_pin_conf(aq_nic, pin_index, start, period);
 416
 417         return 0;
 418 }
 419
 420 static int aq_ptp_pps_pin_configure(struct ptp_clock_info *ptp,
 421                                     struct ptp_clock_request *rq, int on)
 422 {
 423         struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
 424         struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
 425         u64 start, period;
 426         u32 pin_index = 0;
 427         u32 rest = 0;
 428
 429         /* verify the request channel is there */
 430         if (pin_index >= ptp->n_per_out)
 431                 return -EINVAL;
 432
 433         aq_nic->aq_hw_ops->hw_get_ptp_ts(aq_nic->aq_hw, &start);
 434         div_u64_rem(start, NSEC_PER_SEC, &rest);
 435         period = on ? NSEC_PER_SEC : 0; /* PPS - pulse per second */
 436         start = on ? start - rest + NSEC_PER_SEC *
 437                 (rest > 990000000LL ? 2 : 1) : 0;
 438
 439         aq_ptp_hw_pin_conf(aq_nic, pin_index, start, period);
 440
 441         return 0;
 442 }
 443
 444 static void aq_ptp_extts_pin_ctrl(struct aq_ptp_s *aq_ptp)
 445 {
 446         struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
 447         u32 enable = aq_ptp->extts_pin_enabled;
 448
 449         if (aq_nic->aq_hw_ops->hw_extts_gpio_enable)
 450                 aq_nic->aq_hw_ops->hw_extts_gpio_enable(aq_nic->aq_hw, 0,
 451                                                         enable);
 452 }
 453
 454 static int aq_ptp_extts_pin_configure(struct ptp_clock_info *ptp,
 455                                       struct ptp_clock_request *rq, int on)
 456 {
 457         struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
 458
 459         u32 pin_index = rq->extts.index;
 460
 461         if (pin_index >= ptp->n_ext_ts)
 462                 return -EINVAL;
 463
 464         aq_ptp->extts_pin_enabled = !!on;
 465         if (on) {
 466                 aq_ptp->poll_timeout_ms = POLL_SYNC_TIMER_MS;
 467                 cancel_delayed_work_sync(&aq_ptp->poll_sync);
 468                 schedule_delayed_work(&aq_ptp->poll_sync,
 469                                       msecs_to_jiffies(aq_ptp->poll_timeout_ms));
 470         }
 471
 472         aq_ptp_extts_pin_ctrl(aq_ptp);
 473         return 0;
 474 }
 475
 476 /* aq_ptp_gpio_feature_enable
 477  * @ptp: the ptp clock structure
 478  * @rq: the requested feature to change
 479  * @on: whether to enable or disable the feature
 480  */
 481 static int aq_ptp_gpio_feature_enable(struct ptp_clock_info *ptp,
 482                                       struct ptp_clock_request *rq, int on)
 483 {
 484         switch (rq->type) {
 485         case PTP_CLK_REQ_EXTTS:
 486                 return aq_ptp_extts_pin_configure(ptp, rq, on);
 487         case PTP_CLK_REQ_PEROUT:
 488                 return aq_ptp_perout_pin_configure(ptp, rq, on);
 489         case PTP_CLK_REQ_PPS:
 490                 return aq_ptp_pps_pin_configure(ptp, rq, on);
 491         default:
 492                 return -EOPNOTSUPP;
 493         }
 494
 495         return 0;
 496 }
 497
 498 /* aq_ptp_verify
 499  * @ptp: the ptp clock structure
 500  * @pin: index of the pin in question
 501  * @func: the desired function to use
 502  * @chan: the function channel index to use
 503  */
 504 static int aq_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin,
 505                          enum ptp_pin_function func, unsigned int chan)
 506 {
 507         /* verify the requested pin is there */
 508         if (!ptp->pin_config || pin >= ptp->n_pins)
 509                 return -EINVAL;
 510
 511         /* enforce locked channels, no changing them */
 512         if (chan != ptp->pin_config[pin].chan)
 513                 return -EINVAL;
 514
 515         /* we want to keep the functions locked as well */
 516         if (func != ptp->pin_config[pin].func)
 517                 return -EINVAL;
 518
 519         return 0;
 520 }
 521
 522 /* aq_ptp_tx_hwtstamp - utility function which checks for TX time stamp
 523  * @adapter: the private adapter struct
 524  *
 525  * if the timestamp is valid, we convert it into the timecounter ns
 526  * value, then store that result into the hwtstamps structure which
 527  * is passed up the network stack
 528  */
 529 void aq_ptp_tx_hwtstamp(struct aq_nic_s *aq_nic, u64 timestamp)
 530 {
 531         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 532         struct sk_buff *skb = aq_ptp_skb_get(&aq_ptp->skb_ring);
 533         struct skb_shared_hwtstamps hwtstamp;
 534
 535         if (!skb) {
 536                 netdev_err(aq_nic->ndev, "have timestamp but tx_queues empty\n");
 537                 return;
 538         }
 539
 540         timestamp += atomic_read(&aq_ptp->offset_egress);
 541         aq_ptp_convert_to_hwtstamp(aq_ptp, &hwtstamp, timestamp);
 542         skb_tstamp_tx(skb, &hwtstamp);
 543         dev_kfree_skb_any(skb);
 544
 545         aq_ptp_tx_timeout_update(aq_ptp);
 546 }
 547
 548 /* aq_ptp_rx_hwtstamp - utility function which checks for RX time stamp
 549  * @adapter: pointer to adapter struct
 550  * @skb: particular skb to send timestamp with
 551  *
 552  * if the timestamp is valid, we convert it into the timecounter ns
 553  * value, then store that result into the hwtstamps structure which
 554  * is passed up the network stack
 555  */
 556 static void aq_ptp_rx_hwtstamp(struct aq_ptp_s *aq_ptp, struct sk_buff *skb,
 557                                u64 timestamp)
 558 {
 559         timestamp -= atomic_read(&aq_ptp->offset_ingress);
 560         aq_ptp_convert_to_hwtstamp(aq_ptp, skb_hwtstamps(skb), timestamp);
 561 }
 562
 563 void aq_ptp_hwtstamp_config_get(struct aq_ptp_s *aq_ptp,
 564                                 struct hwtstamp_config *config)
 565 {
 566         *config = aq_ptp->hwtstamp_config;
 567 }
 568
 569 static void aq_ptp_prepare_filters(struct aq_ptp_s *aq_ptp)
 570 {
 571         aq_ptp->udp_filter.cmd = HW_ATL_RX_ENABLE_FLTR_L3L4 |
 572                                HW_ATL_RX_ENABLE_CMP_PROT_L4 |
 573                                HW_ATL_RX_UDP |
 574                                HW_ATL_RX_ENABLE_CMP_DEST_PORT_L4 |
 575                                HW_ATL_RX_HOST << HW_ATL_RX_ACTION_FL3F4_SHIFT |
 576                                HW_ATL_RX_ENABLE_QUEUE_L3L4 |
 577                                aq_ptp->ptp_rx.idx << HW_ATL_RX_QUEUE_FL3L4_SHIFT;
 578         aq_ptp->udp_filter.p_dst = PTP_EV_PORT;
 579
 580         aq_ptp->eth_type_filter.ethertype = ETH_P_1588;
 581         aq_ptp->eth_type_filter.queue = aq_ptp->ptp_rx.idx;
 582 }
 583
 584 int aq_ptp_hwtstamp_config_set(struct aq_ptp_s *aq_ptp,
 585                                struct hwtstamp_config *config)
 586 {
 587         struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
 588         const struct aq_hw_ops *hw_ops;
 589         int err = 0;
 590
 591         hw_ops = aq_nic->aq_hw_ops;
 592         if (config->tx_type == HWTSTAMP_TX_ON ||
 593             config->rx_filter == HWTSTAMP_FILTER_PTP_V2_EVENT) {
 594                 aq_ptp_prepare_filters(aq_ptp);
 595                 if (hw_ops->hw_filter_l3l4_set) {
 596                         err = hw_ops->hw_filter_l3l4_set(aq_nic->aq_hw,
 597                                                          &aq_ptp->udp_filter);
 598                 }
 599                 if (!err && hw_ops->hw_filter_l2_set) {
 600                         err = hw_ops->hw_filter_l2_set(aq_nic->aq_hw,
 601                                                        &aq_ptp->eth_type_filter);
 602                 }
 603                 aq_utils_obj_set(&aq_nic->flags, AQ_NIC_PTP_DPATH_UP);
 604         } else {
 605                 aq_ptp->udp_filter.cmd &= ~HW_ATL_RX_ENABLE_FLTR_L3L4;
 606                 if (hw_ops->hw_filter_l3l4_set) {
 607                         err = hw_ops->hw_filter_l3l4_set(aq_nic->aq_hw,
 608                                                          &aq_ptp->udp_filter);
 609                 }
 610                 if (!err && hw_ops->hw_filter_l2_clear) {
 611                         err = hw_ops->hw_filter_l2_clear(aq_nic->aq_hw,
 612                                                         &aq_ptp->eth_type_filter);
 613                 }
 614                 aq_utils_obj_clear(&aq_nic->flags, AQ_NIC_PTP_DPATH_UP);
 615         }
 616
 617         if (err)
 618                 return -EREMOTEIO;
 619
 620         aq_ptp->hwtstamp_config = *config;
 621
 622         return 0;
 623 }
 624
 625 bool aq_ptp_ring(struct aq_nic_s *aq_nic, struct aq_ring_s *ring)
 626 {
 627         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 628
 629         if (!aq_ptp)
 630                 return false;
 631
 632         return &aq_ptp->ptp_tx == ring ||
 633                &aq_ptp->ptp_rx == ring || &aq_ptp->hwts_rx == ring;
 634 }
 635
 636 u16 aq_ptp_extract_ts(struct aq_nic_s *aq_nic, struct sk_buff *skb, u8 *p,
 637                       unsigned int len)
 638 {
 639         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 640         u64 timestamp = 0;
 641         u16 ret = aq_nic->aq_hw_ops->rx_extract_ts(aq_nic->aq_hw,
 642                                                    p, len, &timestamp);
 643
 644         if (ret > 0)
 645                 aq_ptp_rx_hwtstamp(aq_ptp, skb, timestamp);
 646
 647         return ret;
 648 }
 649
 650 static int aq_ptp_poll(struct napi_struct *napi, int budget)
 651 {
 652         struct aq_ptp_s *aq_ptp = container_of(napi, struct aq_ptp_s, napi);
 653         struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
 654         bool was_cleaned = false;
 655         int work_done = 0;
 656         int err;
 657
 658         /* Processing PTP TX traffic */
 659         err = aq_nic->aq_hw_ops->hw_ring_tx_head_update(aq_nic->aq_hw,
 660                                                         &aq_ptp->ptp_tx);
 661         if (err < 0)
 662                 goto err_exit;
 663
 664         if (aq_ptp->ptp_tx.sw_head != aq_ptp->ptp_tx.hw_head) {
 665                 aq_ring_tx_clean(&aq_ptp->ptp_tx);
 666
 667                 was_cleaned = true;
 668         }
 669
 670         /* Processing HW_TIMESTAMP RX traffic */
 671         err = aq_nic->aq_hw_ops->hw_ring_hwts_rx_receive(aq_nic->aq_hw,
 672                                                          &aq_ptp->hwts_rx);
 673         if (err < 0)
 674                 goto err_exit;
 675
 676         if (aq_ptp->hwts_rx.sw_head != aq_ptp->hwts_rx.hw_head) {
 677                 aq_ring_hwts_rx_clean(&aq_ptp->hwts_rx, aq_nic);
 678
 679                 err = aq_nic->aq_hw_ops->hw_ring_hwts_rx_fill(aq_nic->aq_hw,
 680                                                               &aq_ptp->hwts_rx);
 681
 682                 was_cleaned = true;
 683         }
 684
 685         /* Processing PTP RX traffic */
 686         err = aq_nic->aq_hw_ops->hw_ring_rx_receive(aq_nic->aq_hw,
 687                                                     &aq_ptp->ptp_rx);
 688         if (err < 0)
 689                 goto err_exit;
 690
 691         if (aq_ptp->ptp_rx.sw_head != aq_ptp->ptp_rx.hw_head) {
 692                 unsigned int sw_tail_old;
 693
 694                 err = aq_ring_rx_clean(&aq_ptp->ptp_rx, napi, &work_done, budget);
 695                 if (err < 0)
 696                         goto err_exit;
 697
 698                 sw_tail_old = aq_ptp->ptp_rx.sw_tail;
 699                 err = aq_ring_rx_fill(&aq_ptp->ptp_rx);
 700                 if (err < 0)
 701                         goto err_exit;
 702
 703                 err = aq_nic->aq_hw_ops->hw_ring_rx_fill(aq_nic->aq_hw,
 704                                                          &aq_ptp->ptp_rx,
 705                                                          sw_tail_old);
 706                 if (err < 0)
 707                         goto err_exit;
 708         }
 709
 710         if (was_cleaned)
 711                 work_done = budget;
 712
 713         if (work_done < budget) {
 714                 napi_complete_done(napi, work_done);
 715                 aq_nic->aq_hw_ops->hw_irq_enable(aq_nic->aq_hw,
 716                                         1 << aq_ptp->ptp_ring_param.vec_idx);
 717         }
 718
 719 err_exit:
 720         return work_done;
 721 }
 722
 723 static irqreturn_t aq_ptp_isr(int irq, void *private)
 724 {
 725         struct aq_ptp_s *aq_ptp = private;
 726         int err = 0;
 727
 728         if (!aq_ptp) {
 729                 err = -EINVAL;
 730                 goto err_exit;
 731         }
 732         napi_schedule(&aq_ptp->napi);
 733
 734 err_exit:
 735         return err >= 0 ? IRQ_HANDLED : IRQ_NONE;
 736 }
 737
 738 int aq_ptp_xmit(struct aq_nic_s *aq_nic, struct sk_buff *skb)
 739 {
 740         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 741         struct aq_ring_s *ring = &aq_ptp->ptp_tx;
 742         unsigned long irq_flags;
 743         int err = NETDEV_TX_OK;
 744         unsigned int frags;
 745
 746         if (skb->len <= 0) {
 747                 dev_kfree_skb_any(skb);
 748                 goto err_exit;
 749         }
 750
 751         frags = skb_shinfo(skb)->nr_frags + 1;
 752         /* Frags cannot be bigger 16KB
 753          * because PTP usually works
 754          * without Jumbo even in a background
 755          */
 756         if (frags > AQ_CFG_SKB_FRAGS_MAX || frags > aq_ring_avail_dx(ring)) {
 757                 /* Drop packet because it doesn't make sence to delay it */
 758                 dev_kfree_skb_any(skb);
 759                 goto err_exit;
 760         }
 761
 762         err = aq_ptp_skb_put(&aq_ptp->skb_ring, skb);
 763         if (err) {
 764                 netdev_err(aq_nic->ndev, "SKB Ring is overflow (%u)!\n",
 765                            ring->size);
 766                 return NETDEV_TX_BUSY;
 767         }
 768         skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 769         aq_ptp_tx_timeout_start(aq_ptp);
 770         skb_tx_timestamp(skb);
 771
 772         spin_lock_irqsave(&aq_nic->aq_ptp->ptp_ring_lock, irq_flags);
 773         frags = aq_nic_map_skb(aq_nic, skb, ring);
 774
 775         if (likely(frags)) {
 776                 err = aq_nic->aq_hw_ops->hw_ring_tx_xmit(aq_nic->aq_hw,
 777                                                        ring, frags);
 778                 if (err >= 0) {
 779                         ++ring->stats.tx.packets;
 780                         ring->stats.tx.bytes += skb->len;
 781                 }
 782         } else {
 783                 err = NETDEV_TX_BUSY;
 784         }
 785         spin_unlock_irqrestore(&aq_nic->aq_ptp->ptp_ring_lock, irq_flags);
 786
 787 err_exit:
 788         return err;
 789 }
 790
 791 void aq_ptp_service_task(struct aq_nic_s *aq_nic)
 792 {
 793         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 794
 795         if (!aq_ptp)
 796                 return;
 797
 798         aq_ptp_tx_timeout_check(aq_ptp);
 799 }
 800
 801 int aq_ptp_irq_alloc(struct aq_nic_s *aq_nic)
 802 {
 803         struct pci_dev *pdev = aq_nic->pdev;
 804         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 805         int err = 0;
 806
 807         if (!aq_ptp)
 808                 return 0;
 809
 810         if (pdev->msix_enabled || pdev->msi_enabled) {
 811                 err = request_irq(pci_irq_vector(pdev, aq_ptp->idx_vector),
 812                                   aq_ptp_isr, 0, aq_nic->ndev->name, aq_ptp);
 813         } else {
 814                 err = -EINVAL;
 815                 goto err_exit;
 816         }
 817
 818 err_exit:
 819         return err;
 820 }
 821
 822 void aq_ptp_irq_free(struct aq_nic_s *aq_nic)
 823 {
 824         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 825         struct pci_dev *pdev = aq_nic->pdev;
 826
 827         if (!aq_ptp)
 828                 return;
 829
 830         free_irq(pci_irq_vector(pdev, aq_ptp->idx_vector), aq_ptp);
 831 }
 832
 833 int aq_ptp_ring_init(struct aq_nic_s *aq_nic)
 834 {
 835         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 836         int err = 0;
 837
 838         if (!aq_ptp)
 839                 return 0;
 840
 841         err = aq_ring_init(&aq_ptp->ptp_tx);
 842         if (err < 0)
 843                 goto err_exit;
 844         err = aq_nic->aq_hw_ops->hw_ring_tx_init(aq_nic->aq_hw,
 845                                                  &aq_ptp->ptp_tx,
 846                                                  &aq_ptp->ptp_ring_param);
 847         if (err < 0)
 848                 goto err_exit;
 849
 850         err = aq_ring_init(&aq_ptp->ptp_rx);
 851         if (err < 0)
 852                 goto err_exit;
 853         err = aq_nic->aq_hw_ops->hw_ring_rx_init(aq_nic->aq_hw,
 854                                                  &aq_ptp->ptp_rx,
 855                                                  &aq_ptp->ptp_ring_param);
 856         if (err < 0)
 857                 goto err_exit;
 858
 859         err = aq_ring_rx_fill(&aq_ptp->ptp_rx);
 860         if (err < 0)
 861                 goto err_rx_free;
 862         err = aq_nic->aq_hw_ops->hw_ring_rx_fill(aq_nic->aq_hw,
 863                                                  &aq_ptp->ptp_rx,
 864                                                  0U);
 865         if (err < 0)
 866                 goto err_rx_free;
 867
 868         err = aq_ring_init(&aq_ptp->hwts_rx);
 869         if (err < 0)
 870                 goto err_rx_free;
 871         err = aq_nic->aq_hw_ops->hw_ring_rx_init(aq_nic->aq_hw,
 872                                                  &aq_ptp->hwts_rx,
 873                                                  &aq_ptp->ptp_ring_param);
 874         if (err < 0)
 875                 goto err_exit;
 876         err = aq_nic->aq_hw_ops->hw_ring_hwts_rx_fill(aq_nic->aq_hw,
 877                                                       &aq_ptp->hwts_rx);
 878         if (err < 0)
 879                 goto err_exit;
 880
 881         return err;
 882
 883 err_rx_free:
 884         aq_ring_rx_deinit(&aq_ptp->ptp_rx);
 885 err_exit:
 886         return err;
 887 }
 888
 889 int aq_ptp_ring_start(struct aq_nic_s *aq_nic)
 890 {
 891         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 892         int err = 0;
 893
 894         if (!aq_ptp)
 895                 return 0;
 896
 897         err = aq_nic->aq_hw_ops->hw_ring_tx_start(aq_nic->aq_hw, &aq_ptp->ptp_tx);
 898         if (err < 0)
 899                 goto err_exit;
 900
 901         err = aq_nic->aq_hw_ops->hw_ring_rx_start(aq_nic->aq_hw, &aq_ptp->ptp_rx);
 902         if (err < 0)
 903                 goto err_exit;
 904
 905         err = aq_nic->aq_hw_ops->hw_ring_rx_start(aq_nic->aq_hw,
 906                                                   &aq_ptp->hwts_rx);
 907         if (err < 0)
 908                 goto err_exit;
 909
 910         napi_enable(&aq_ptp->napi);
 911
 912 err_exit:
 913         return err;
 914 }
 915
 916 void aq_ptp_ring_stop(struct aq_nic_s *aq_nic)
 917 {
 918         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 919
 920         if (!aq_ptp)
 921                 return;
 922
 923         aq_nic->aq_hw_ops->hw_ring_tx_stop(aq_nic->aq_hw, &aq_ptp->ptp_tx);
 924         aq_nic->aq_hw_ops->hw_ring_rx_stop(aq_nic->aq_hw, &aq_ptp->ptp_rx);
 925
 926         aq_nic->aq_hw_ops->hw_ring_rx_stop(aq_nic->aq_hw, &aq_ptp->hwts_rx);
 927
 928         napi_disable(&aq_ptp->napi);
 929 }
 930
 931 void aq_ptp_ring_deinit(struct aq_nic_s *aq_nic)
 932 {
 933         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 934
 935         if (!aq_ptp || !aq_ptp->ptp_tx.aq_nic || !aq_ptp->ptp_rx.aq_nic)
 936                 return;
 937
 938         aq_ring_tx_clean(&aq_ptp->ptp_tx);
 939         aq_ring_rx_deinit(&aq_ptp->ptp_rx);
 940 }
 941
 942 #define PTP_8TC_RING_IDX             8
 943 #define PTP_4TC_RING_IDX            16
 944 #define PTP_HWST_RING_IDX           31
 945
 946 int aq_ptp_ring_alloc(struct aq_nic_s *aq_nic)
 947 {
 948         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
 949         unsigned int tx_ring_idx, rx_ring_idx;
 950         struct aq_ring_s *hwts;
 951         u32 tx_tc_mode, rx_tc_mode;
 952         struct aq_ring_s *ring;
 953         int err;
 954
 955         if (!aq_ptp)
 956                 return 0;
 957
 958         /* Index must to be 8 (8 TCs) or 16 (4 TCs).
 959          * It depends from Traffic Class mode.
 960          */
 961         aq_nic->aq_hw_ops->hw_tx_tc_mode_get(aq_nic->aq_hw, &tx_tc_mode);
 962         if (tx_tc_mode == 0)
 963                 tx_ring_idx = PTP_8TC_RING_IDX;
 964         else
 965                 tx_ring_idx = PTP_4TC_RING_IDX;
 966
 967         ring = aq_ring_tx_alloc(&aq_ptp->ptp_tx, aq_nic,
 968                                 tx_ring_idx, &aq_nic->aq_nic_cfg);
 969         if (!ring) {
 970                 err = -ENOMEM;
 971                 goto err_exit;
 972         }
 973
 974         aq_nic->aq_hw_ops->hw_rx_tc_mode_get(aq_nic->aq_hw, &rx_tc_mode);
 975         if (rx_tc_mode == 0)
 976                 rx_ring_idx = PTP_8TC_RING_IDX;
 977         else
 978                 rx_ring_idx = PTP_4TC_RING_IDX;
 979
 980         ring = aq_ring_rx_alloc(&aq_ptp->ptp_rx, aq_nic,
 981                                 rx_ring_idx, &aq_nic->aq_nic_cfg);
 982         if (!ring) {
 983                 err = -ENOMEM;
 984                 goto err_exit_ptp_tx;
 985         }
 986
 987         hwts = aq_ring_hwts_rx_alloc(&aq_ptp->hwts_rx, aq_nic, PTP_HWST_RING_IDX,
 988                                      aq_nic->aq_nic_cfg.rxds,
 989                                      aq_nic->aq_nic_cfg.aq_hw_caps->rxd_size);
 990         if (!hwts) {
 991                 err = -ENOMEM;
 992                 goto err_exit_ptp_rx;
 993         }
 994
 995         err = aq_ptp_skb_ring_init(&aq_ptp->skb_ring, aq_nic->aq_nic_cfg.rxds);
 996         if (err != 0) {
 997                 err = -ENOMEM;
 998                 goto err_exit_hwts_rx;
 999         }
1000
1001         aq_ptp->ptp_ring_param.vec_idx = aq_ptp->idx_vector;
1002         aq_ptp->ptp_ring_param.cpu = aq_ptp->ptp_ring_param.vec_idx +
1003                         aq_nic_get_cfg(aq_nic)->aq_rss.base_cpu_number;
1004         cpumask_set_cpu(aq_ptp->ptp_ring_param.cpu,
1005                         &aq_ptp->ptp_ring_param.affinity_mask);
1006
1007         return 0;
1008
1009 err_exit_hwts_rx:
1010         aq_ring_free(&aq_ptp->hwts_rx);
1011 err_exit_ptp_rx:
1012         aq_ring_free(&aq_ptp->ptp_rx);
1013 err_exit_ptp_tx:
1014         aq_ring_free(&aq_ptp->ptp_tx);
1015 err_exit:
1016         return err;
1017 }
1018
1019 void aq_ptp_ring_free(struct aq_nic_s *aq_nic)
1020 {
1021         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
1022
1023         if (!aq_ptp)
1024                 return;
1025
1026         aq_ring_free(&aq_ptp->ptp_tx);
1027         aq_ring_free(&aq_ptp->ptp_rx);
1028         aq_ring_free(&aq_ptp->hwts_rx);
1029
1030         aq_ptp_skb_ring_release(&aq_ptp->skb_ring);
1031 }
1032
1033 #define MAX_PTP_GPIO_COUNT 4
1034
1035 static struct ptp_clock_info aq_ptp_clock = {
1036         .owner          = THIS_MODULE,
1037         .name           = "atlantic ptp",
1038         .max_adj        = 999999999,
1039         .n_ext_ts       = 0,
1040         .pps            = 0,
1041         .adjfine        = aq_ptp_adjfine,
1042         .adjtime        = aq_ptp_adjtime,
1043         .gettime64      = aq_ptp_gettime,
1044         .settime64      = aq_ptp_settime,
1045         .n_per_out      = 0,
1046         .enable         = aq_ptp_gpio_feature_enable,
1047         .n_pins         = 0,
1048         .verify         = aq_ptp_verify,
1049         .pin_config     = NULL,
1050 };
1051
1052 #define ptp_offset_init(__idx, __mbps, __egress, __ingress)   do { \
1053                 ptp_offset[__idx].mbps = (__mbps); \
1054                 ptp_offset[__idx].egress = (__egress); \
1055                 ptp_offset[__idx].ingress = (__ingress); } \
1056                 while (0)
1057
1058 static void aq_ptp_offset_init_from_fw(const struct hw_aq_ptp_offset *offsets)
1059 {
1060         int i;
1061
1062         /* Load offsets for PTP */
1063         for (i = 0; i < ARRAY_SIZE(ptp_offset); i++) {
1064                 switch (i) {
1065                 /* 100M */
1066                 case ptp_offset_idx_100:
1067                         ptp_offset_init(i, 100,
1068                                         offsets->egress_100,
1069                                         offsets->ingress_100);
1070                         break;
1071                 /* 1G */
1072                 case ptp_offset_idx_1000:
1073                         ptp_offset_init(i, 1000,
1074                                         offsets->egress_1000,
1075                                         offsets->ingress_1000);
1076                         break;
1077                 /* 2.5G */
1078                 case ptp_offset_idx_2500:
1079                         ptp_offset_init(i, 2500,
1080                                         offsets->egress_2500,
1081                                         offsets->ingress_2500);
1082                         break;
1083                 /* 5G */
1084                 case ptp_offset_idx_5000:
1085                         ptp_offset_init(i, 5000,
1086                                         offsets->egress_5000,
1087                                         offsets->ingress_5000);
1088                         break;
1089                 /* 10G */
1090                 case ptp_offset_idx_10000:
1091                         ptp_offset_init(i, 10000,
1092                                         offsets->egress_10000,
1093                                         offsets->ingress_10000);
1094                         break;
1095                 }
1096         }
1097 }
1098
1099 static void aq_ptp_offset_init(const struct hw_aq_ptp_offset *offsets)
1100 {
1101         memset(ptp_offset, 0, sizeof(ptp_offset));
1102
1103         aq_ptp_offset_init_from_fw(offsets);
1104 }
1105
1106 static void aq_ptp_gpio_init(struct ptp_clock_info *info,
1107                              struct hw_aq_info *hw_info)
1108 {
1109         struct ptp_pin_desc pin_desc[MAX_PTP_GPIO_COUNT];
1110         u32 extts_pin_cnt = 0;
1111         u32 out_pin_cnt = 0;
1112         u32 i;
1113
1114         memset(pin_desc, 0, sizeof(pin_desc));
1115
1116         for (i = 0; i < MAX_PTP_GPIO_COUNT - 1; i++) {
1117                 if (hw_info->gpio_pin[i] ==
1118                     (GPIO_PIN_FUNCTION_PTP0 + out_pin_cnt)) {
1119                         snprintf(pin_desc[out_pin_cnt].name,
1120                                  sizeof(pin_desc[out_pin_cnt].name),
1121                                  "AQ_GPIO%d", i);
1122                         pin_desc[out_pin_cnt].index = out_pin_cnt;
1123                         pin_desc[out_pin_cnt].chan = out_pin_cnt;
1124                         pin_desc[out_pin_cnt++].func = PTP_PF_PEROUT;
1125                 }
1126         }
1127
1128         info->n_per_out = out_pin_cnt;
1129
1130         if (hw_info->caps_ex & BIT(CAPS_EX_PHY_CTRL_TS_PIN)) {
1131                 extts_pin_cnt += 1;
1132
1133                 snprintf(pin_desc[out_pin_cnt].name,
1134                          sizeof(pin_desc[out_pin_cnt].name),
1135                           "AQ_GPIO%d", out_pin_cnt);
1136                 pin_desc[out_pin_cnt].index = out_pin_cnt;
1137                 pin_desc[out_pin_cnt].chan = 0;
1138                 pin_desc[out_pin_cnt].func = PTP_PF_EXTTS;
1139         }
1140
1141         info->n_pins = out_pin_cnt + extts_pin_cnt;
1142         info->n_ext_ts = extts_pin_cnt;
1143
1144         if (!info->n_pins)
1145                 return;
1146
1147         info->pin_config = kcalloc(info->n_pins, sizeof(struct ptp_pin_desc),
1148                                    GFP_KERNEL);
1149
1150         if (!info->pin_config)
1151                 return;
1152
1153         memcpy(info->pin_config, &pin_desc,
1154                sizeof(struct ptp_pin_desc) * info->n_pins);
1155 }
1156
1157 void aq_ptp_clock_init(struct aq_nic_s *aq_nic)
1158 {
1159         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
1160         struct timespec64 ts;
1161
1162         ktime_get_real_ts64(&ts);
1163         aq_ptp_settime(&aq_ptp->ptp_info, &ts);
1164 }
1165
1166 static void aq_ptp_poll_sync_work_cb(struct work_struct *w);
1167
1168 int aq_ptp_init(struct aq_nic_s *aq_nic, unsigned int idx_vec)
1169 {
1170         struct hw_atl_utils_mbox mbox;
1171         struct ptp_clock *clock;
1172         struct aq_ptp_s *aq_ptp;
1173         int err = 0;
1174
1175         if (!aq_nic->aq_hw_ops->hw_get_ptp_ts) {
1176                 aq_nic->aq_ptp = NULL;
1177                 return 0;
1178         }
1179
1180         if (!aq_nic->aq_fw_ops->enable_ptp) {
1181                 aq_nic->aq_ptp = NULL;
1182                 return 0;
1183         }
1184
1185         hw_atl_utils_mpi_read_stats(aq_nic->aq_hw, &mbox);
1186
1187         if (!(mbox.info.caps_ex & BIT(CAPS_EX_PHY_PTP_EN))) {
1188                 aq_nic->aq_ptp = NULL;
1189                 return 0;
1190         }
1191
1192         aq_ptp_offset_init(&mbox.info.ptp_offset);
1193
1194         aq_ptp = kzalloc(sizeof(*aq_ptp), GFP_KERNEL);
1195         if (!aq_ptp) {
1196                 err = -ENOMEM;
1197                 goto err_exit;
1198         }
1199
1200         aq_ptp->aq_nic = aq_nic;
1201
1202         spin_lock_init(&aq_ptp->ptp_lock);
1203         spin_lock_init(&aq_ptp->ptp_ring_lock);
1204
1205         aq_ptp->ptp_info = aq_ptp_clock;
1206         aq_ptp_gpio_init(&aq_ptp->ptp_info, &mbox.info);
1207         clock = ptp_clock_register(&aq_ptp->ptp_info, &aq_nic->ndev->dev);
1208         if (!clock || IS_ERR(clock)) {
1209                 netdev_err(aq_nic->ndev, "ptp_clock_register failed\n");
1210                 err = PTR_ERR(clock);
1211                 goto err_exit;
1212         }
1213         aq_ptp->ptp_clock = clock;
1214         aq_ptp_tx_timeout_init(&aq_ptp->ptp_tx_timeout);
1215
1216         atomic_set(&aq_ptp->offset_egress, 0);
1217         atomic_set(&aq_ptp->offset_ingress, 0);
1218
1219         netif_napi_add(aq_nic_get_ndev(aq_nic), &aq_ptp->napi,
1220                        aq_ptp_poll, AQ_CFG_NAPI_WEIGHT);
1221
1222         aq_ptp->idx_vector = idx_vec;
1223
1224         aq_nic->aq_ptp = aq_ptp;
1225
1226         /* enable ptp counter */
1227         aq_utils_obj_set(&aq_nic->aq_hw->flags, AQ_HW_PTP_AVAILABLE);
1228         mutex_lock(&aq_nic->fwreq_mutex);
1229         aq_nic->aq_fw_ops->enable_ptp(aq_nic->aq_hw, 1);
1230         aq_ptp_clock_init(aq_nic);
1231         mutex_unlock(&aq_nic->fwreq_mutex);
1232
1233         INIT_DELAYED_WORK(&aq_ptp->poll_sync, &aq_ptp_poll_sync_work_cb);
1234         aq_ptp->eth_type_filter.location =
1235                         aq_nic_reserve_filter(aq_nic, aq_rx_filter_ethertype);
1236         aq_ptp->udp_filter.location =
1237                         aq_nic_reserve_filter(aq_nic, aq_rx_filter_l3l4);
1238
1239         return 0;
1240
1241 err_exit:
1242         if (aq_ptp)
1243                 kfree(aq_ptp->ptp_info.pin_config);
1244         kfree(aq_ptp);
1245         aq_nic->aq_ptp = NULL;
1246         return err;
1247 }
1248
1249 void aq_ptp_unregister(struct aq_nic_s *aq_nic)
1250 {
1251         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
1252
1253         if (!aq_ptp)
1254                 return;
1255
1256         ptp_clock_unregister(aq_ptp->ptp_clock);
1257 }
1258
1259 void aq_ptp_free(struct aq_nic_s *aq_nic)
1260 {
1261         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
1262
1263         if (!aq_ptp)
1264                 return;
1265
1266         aq_nic_release_filter(aq_nic, aq_rx_filter_ethertype,
1267                               aq_ptp->eth_type_filter.location);
1268         aq_nic_release_filter(aq_nic, aq_rx_filter_l3l4,
1269                               aq_ptp->udp_filter.location);
1270         cancel_delayed_work_sync(&aq_ptp->poll_sync);
1271         /* disable ptp */
1272         mutex_lock(&aq_nic->fwreq_mutex);
1273         aq_nic->aq_fw_ops->enable_ptp(aq_nic->aq_hw, 0);
1274         mutex_unlock(&aq_nic->fwreq_mutex);
1275
1276         kfree(aq_ptp->ptp_info.pin_config);
1277
1278         netif_napi_del(&aq_ptp->napi);
1279         kfree(aq_ptp);
1280         aq_nic->aq_ptp = NULL;
1281 }
1282
1283 struct ptp_clock *aq_ptp_get_ptp_clock(struct aq_ptp_s *aq_ptp)
1284 {
1285         return aq_ptp->ptp_clock;
1286 }
1287
1288 /* PTP external GPIO nanoseconds count */
1289 static uint64_t aq_ptp_get_sync1588_ts(struct aq_nic_s *aq_nic)
1290 {
1291         u64 ts = 0;
1292
1293         if (aq_nic->aq_hw_ops->hw_get_sync_ts)
1294                 aq_nic->aq_hw_ops->hw_get_sync_ts(aq_nic->aq_hw, &ts);
1295
1296         return ts;
1297 }
1298
1299 static void aq_ptp_start_work(struct aq_ptp_s *aq_ptp)
1300 {
1301         if (aq_ptp->extts_pin_enabled) {
1302                 aq_ptp->poll_timeout_ms = POLL_SYNC_TIMER_MS;
1303                 aq_ptp->last_sync1588_ts =
1304                                 aq_ptp_get_sync1588_ts(aq_ptp->aq_nic);
1305                 schedule_delayed_work(&aq_ptp->poll_sync,
1306                                       msecs_to_jiffies(aq_ptp->poll_timeout_ms));
1307         }
1308 }
1309
1310 int aq_ptp_link_change(struct aq_nic_s *aq_nic)
1311 {
1312         struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
1313
1314         if (!aq_ptp)
1315                 return 0;
1316
1317         if (aq_nic->aq_hw->aq_link_status.mbps)
1318                 aq_ptp_start_work(aq_ptp);
1319         else
1320                 cancel_delayed_work_sync(&aq_ptp->poll_sync);
1321
1322         return 0;
1323 }
1324
1325 static bool aq_ptp_sync_ts_updated(struct aq_ptp_s *aq_ptp, u64 *new_ts)
1326 {
1327         struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
1328         u64 sync_ts2;
1329         u64 sync_ts;
1330
1331         sync_ts = aq_ptp_get_sync1588_ts(aq_nic);
1332
1333         if (sync_ts != aq_ptp->last_sync1588_ts) {
1334                 sync_ts2 = aq_ptp_get_sync1588_ts(aq_nic);
1335                 if (sync_ts != sync_ts2) {
1336                         sync_ts = sync_ts2;
1337                         sync_ts2 = aq_ptp_get_sync1588_ts(aq_nic);
1338                         if (sync_ts != sync_ts2) {
1339                                 netdev_err(aq_nic->ndev,
1340                                            "%s: Unable to get correct GPIO TS",
1341                                            __func__);
1342                                 sync_ts = 0;
1343                         }
1344                 }
1345
1346                 *new_ts = sync_ts;
1347                 return true;
1348         }
1349         return false;
1350 }
1351
1352 static int aq_ptp_check_sync1588(struct aq_ptp_s *aq_ptp)
1353 {
1354         struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
1355         u64 sync_ts;
1356
1357          /* Sync1588 pin was triggered */
1358         if (aq_ptp_sync_ts_updated(aq_ptp, &sync_ts)) {
1359                 if (aq_ptp->extts_pin_enabled) {
1360                         struct ptp_clock_event ptp_event;
1361                         u64 time = 0;
1362
1363                         aq_nic->aq_hw_ops->hw_ts_to_sys_clock(aq_nic->aq_hw,
1364                                                               sync_ts, &time);
1365                         ptp_event.index = aq_ptp->ptp_info.n_pins - 1;
1366                         ptp_event.timestamp = time;
1367
1368                         ptp_event.type = PTP_CLOCK_EXTTS;
1369                         ptp_clock_event(aq_ptp->ptp_clock, &ptp_event);
1370                 }
1371
1372                 aq_ptp->last_sync1588_ts = sync_ts;
1373         }
1374
1375         return 0;
1376 }
1377
1378 void aq_ptp_poll_sync_work_cb(struct work_struct *w)
1379 {
1380         struct delayed_work *dw = to_delayed_work(w);
1381         struct aq_ptp_s *aq_ptp = container_of(dw, struct aq_ptp_s, poll_sync);
1382
1383         aq_ptp_check_sync1588(aq_ptp);
1384
1385         if (aq_ptp->extts_pin_enabled) {
1386                 unsigned long timeout = msecs_to_jiffies(aq_ptp->poll_timeout_ms);
1387
1388                 schedule_delayed_work(&aq_ptp->poll_sync, timeout);
1389         }
1390 }