linux/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c
<<
>>
Prefs
   1// SPDX-License-Identifier: BSD-3-Clause OR GPL-2.0
   2/* Copyright (c) 2019 Mellanox Technologies. All rights reserved */
   3
   4#include <linux/ptp_clock_kernel.h>
   5#include <linux/clocksource.h>
   6#include <linux/timecounter.h>
   7#include <linux/spinlock.h>
   8#include <linux/device.h>
   9#include <linux/rhashtable.h>
  10#include <linux/ptp_classify.h>
  11#include <linux/if_ether.h>
  12#include <linux/if_vlan.h>
  13#include <linux/net_tstamp.h>
  14
  15#include "spectrum.h"
  16#include "spectrum_ptp.h"
  17#include "core.h"
  18
  19#define MLXSW_SP1_PTP_CLOCK_CYCLES_SHIFT        29
  20#define MLXSW_SP1_PTP_CLOCK_FREQ_KHZ            156257 /* 6.4nSec */
  21#define MLXSW_SP1_PTP_CLOCK_MASK                64
  22
  23#define MLXSW_SP1_PTP_HT_GC_INTERVAL            500 /* ms */
  24
  25/* How long, approximately, should the unmatched entries stay in the hash table
  26 * before they are collected. Should be evenly divisible by the GC interval.
  27 */
  28#define MLXSW_SP1_PTP_HT_GC_TIMEOUT             1000 /* ms */
  29
  30struct mlxsw_sp_ptp_state {
  31        struct mlxsw_sp *mlxsw_sp;
  32        struct rhltable unmatched_ht;
  33        spinlock_t unmatched_lock; /* protects the HT */
  34        struct delayed_work ht_gc_dw;
  35        u32 gc_cycle;
  36};
  37
  38struct mlxsw_sp1_ptp_key {
  39        u16 local_port;
  40        u8 message_type;
  41        u16 sequence_id;
  42        u8 domain_number;
  43        bool ingress;
  44};
  45
  46struct mlxsw_sp1_ptp_unmatched {
  47        struct mlxsw_sp1_ptp_key key;
  48        struct rhlist_head ht_node;
  49        struct rcu_head rcu;
  50        struct sk_buff *skb;
  51        u64 timestamp;
  52        u32 gc_cycle;
  53};
  54
  55static const struct rhashtable_params mlxsw_sp1_ptp_unmatched_ht_params = {
  56        .key_len = sizeof_field(struct mlxsw_sp1_ptp_unmatched, key),
  57        .key_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, key),
  58        .head_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, ht_node),
  59};
  60
  61struct mlxsw_sp_ptp_clock {
  62        struct mlxsw_core *core;
  63        spinlock_t lock; /* protect this structure */
  64        struct cyclecounter cycles;
  65        struct timecounter tc;
  66        u32 nominal_c_mult;
  67        struct ptp_clock *ptp;
  68        struct ptp_clock_info ptp_info;
  69        unsigned long overflow_period;
  70        struct delayed_work overflow_work;
  71};
  72
  73static u64 __mlxsw_sp1_ptp_read_frc(struct mlxsw_sp_ptp_clock *clock,
  74                                    struct ptp_system_timestamp *sts)
  75{
  76        struct mlxsw_core *mlxsw_core = clock->core;
  77        u32 frc_h1, frc_h2, frc_l;
  78
  79        frc_h1 = mlxsw_core_read_frc_h(mlxsw_core);
  80        ptp_read_system_prets(sts);
  81        frc_l = mlxsw_core_read_frc_l(mlxsw_core);
  82        ptp_read_system_postts(sts);
  83        frc_h2 = mlxsw_core_read_frc_h(mlxsw_core);
  84
  85        if (frc_h1 != frc_h2) {
  86                /* wrap around */
  87                ptp_read_system_prets(sts);
  88                frc_l = mlxsw_core_read_frc_l(mlxsw_core);
  89                ptp_read_system_postts(sts);
  90        }
  91
  92        return (u64) frc_l | (u64) frc_h2 << 32;
  93}
  94
  95static u64 mlxsw_sp1_ptp_read_frc(const struct cyclecounter *cc)
  96{
  97        struct mlxsw_sp_ptp_clock *clock =
  98                container_of(cc, struct mlxsw_sp_ptp_clock, cycles);
  99
 100        return __mlxsw_sp1_ptp_read_frc(clock, NULL) & cc->mask;
 101}
 102
 103static int
 104mlxsw_sp1_ptp_phc_adjfreq(struct mlxsw_sp_ptp_clock *clock, int freq_adj)
 105{
 106        struct mlxsw_core *mlxsw_core = clock->core;
 107        char mtutc_pl[MLXSW_REG_MTUTC_LEN];
 108
 109        mlxsw_reg_mtutc_pack(mtutc_pl, MLXSW_REG_MTUTC_OPERATION_ADJUST_FREQ,
 110                             freq_adj, 0);
 111        return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
 112}
 113
 114static u64 mlxsw_sp1_ptp_ns2cycles(const struct timecounter *tc, u64 nsec)
 115{
 116        u64 cycles = (u64) nsec;
 117
 118        cycles <<= tc->cc->shift;
 119        cycles = div_u64(cycles, tc->cc->mult);
 120
 121        return cycles;
 122}
 123
 124static int
 125mlxsw_sp1_ptp_phc_settime(struct mlxsw_sp_ptp_clock *clock, u64 nsec)
 126{
 127        struct mlxsw_core *mlxsw_core = clock->core;
 128        u64 next_sec, next_sec_in_nsec, cycles;
 129        char mtutc_pl[MLXSW_REG_MTUTC_LEN];
 130        char mtpps_pl[MLXSW_REG_MTPPS_LEN];
 131        int err;
 132
 133        next_sec = div_u64(nsec, NSEC_PER_SEC) + 1;
 134        next_sec_in_nsec = next_sec * NSEC_PER_SEC;
 135
 136        spin_lock_bh(&clock->lock);
 137        cycles = mlxsw_sp1_ptp_ns2cycles(&clock->tc, next_sec_in_nsec);
 138        spin_unlock_bh(&clock->lock);
 139
 140        mlxsw_reg_mtpps_vpin_pack(mtpps_pl, cycles);
 141        err = mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtpps), mtpps_pl);
 142        if (err)
 143                return err;
 144
 145        mlxsw_reg_mtutc_pack(mtutc_pl,
 146                             MLXSW_REG_MTUTC_OPERATION_SET_TIME_AT_NEXT_SEC,
 147                             0, next_sec);
 148        return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
 149}
 150
 151static int mlxsw_sp1_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 152{
 153        struct mlxsw_sp_ptp_clock *clock =
 154                container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
 155        int neg_adj = 0;
 156        u32 diff;
 157        u64 adj;
 158        s32 ppb;
 159
 160        ppb = scaled_ppm_to_ppb(scaled_ppm);
 161
 162        if (ppb < 0) {
 163                neg_adj = 1;
 164                ppb = -ppb;
 165        }
 166
 167        adj = clock->nominal_c_mult;
 168        adj *= ppb;
 169        diff = div_u64(adj, NSEC_PER_SEC);
 170
 171        spin_lock_bh(&clock->lock);
 172        timecounter_read(&clock->tc);
 173        clock->cycles.mult = neg_adj ? clock->nominal_c_mult - diff :
 174                                       clock->nominal_c_mult + diff;
 175        spin_unlock_bh(&clock->lock);
 176
 177        return mlxsw_sp1_ptp_phc_adjfreq(clock, neg_adj ? -ppb : ppb);
 178}
 179
 180static int mlxsw_sp1_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 181{
 182        struct mlxsw_sp_ptp_clock *clock =
 183                container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
 184        u64 nsec;
 185
 186        spin_lock_bh(&clock->lock);
 187        timecounter_adjtime(&clock->tc, delta);
 188        nsec = timecounter_read(&clock->tc);
 189        spin_unlock_bh(&clock->lock);
 190
 191        return mlxsw_sp1_ptp_phc_settime(clock, nsec);
 192}
 193
 194static int mlxsw_sp1_ptp_gettimex(struct ptp_clock_info *ptp,
 195                                  struct timespec64 *ts,
 196                                  struct ptp_system_timestamp *sts)
 197{
 198        struct mlxsw_sp_ptp_clock *clock =
 199                container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
 200        u64 cycles, nsec;
 201
 202        spin_lock_bh(&clock->lock);
 203        cycles = __mlxsw_sp1_ptp_read_frc(clock, sts);
 204        nsec = timecounter_cyc2time(&clock->tc, cycles);
 205        spin_unlock_bh(&clock->lock);
 206
 207        *ts = ns_to_timespec64(nsec);
 208
 209        return 0;
 210}
 211
 212static int mlxsw_sp1_ptp_settime(struct ptp_clock_info *ptp,
 213                                 const struct timespec64 *ts)
 214{
 215        struct mlxsw_sp_ptp_clock *clock =
 216                container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
 217        u64 nsec = timespec64_to_ns(ts);
 218
 219        spin_lock_bh(&clock->lock);
 220        timecounter_init(&clock->tc, &clock->cycles, nsec);
 221        nsec = timecounter_read(&clock->tc);
 222        spin_unlock_bh(&clock->lock);
 223
 224        return mlxsw_sp1_ptp_phc_settime(clock, nsec);
 225}
 226
 227static const struct ptp_clock_info mlxsw_sp1_ptp_clock_info = {
 228        .owner          = THIS_MODULE,
 229        .name           = "mlxsw_sp_clock",
 230        .max_adj        = 100000000,
 231        .adjfine        = mlxsw_sp1_ptp_adjfine,
 232        .adjtime        = mlxsw_sp1_ptp_adjtime,
 233        .gettimex64     = mlxsw_sp1_ptp_gettimex,
 234        .settime64      = mlxsw_sp1_ptp_settime,
 235};
 236
 237static void mlxsw_sp1_ptp_clock_overflow(struct work_struct *work)
 238{
 239        struct delayed_work *dwork = to_delayed_work(work);
 240        struct mlxsw_sp_ptp_clock *clock;
 241
 242        clock = container_of(dwork, struct mlxsw_sp_ptp_clock, overflow_work);
 243
 244        spin_lock_bh(&clock->lock);
 245        timecounter_read(&clock->tc);
 246        spin_unlock_bh(&clock->lock);
 247        mlxsw_core_schedule_dw(&clock->overflow_work, clock->overflow_period);
 248}
 249
 250struct mlxsw_sp_ptp_clock *
 251mlxsw_sp1_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev)
 252{
 253        u64 overflow_cycles, nsec, frac = 0;
 254        struct mlxsw_sp_ptp_clock *clock;
 255        int err;
 256
 257        clock = kzalloc(sizeof(*clock), GFP_KERNEL);
 258        if (!clock)
 259                return ERR_PTR(-ENOMEM);
 260
 261        spin_lock_init(&clock->lock);
 262        clock->cycles.read = mlxsw_sp1_ptp_read_frc;
 263        clock->cycles.shift = MLXSW_SP1_PTP_CLOCK_CYCLES_SHIFT;
 264        clock->cycles.mult = clocksource_khz2mult(MLXSW_SP1_PTP_CLOCK_FREQ_KHZ,
 265                                                  clock->cycles.shift);
 266        clock->nominal_c_mult = clock->cycles.mult;
 267        clock->cycles.mask = CLOCKSOURCE_MASK(MLXSW_SP1_PTP_CLOCK_MASK);
 268        clock->core = mlxsw_sp->core;
 269
 270        timecounter_init(&clock->tc, &clock->cycles,
 271                         ktime_to_ns(ktime_get_real()));
 272
 273        /* Calculate period in seconds to call the overflow watchdog - to make
 274         * sure counter is checked at least twice every wrap around.
 275         * The period is calculated as the minimum between max HW cycles count
 276         * (The clock source mask) and max amount of cycles that can be
 277         * multiplied by clock multiplier where the result doesn't exceed
 278         * 64bits.
 279         */
 280        overflow_cycles = div64_u64(~0ULL >> 1, clock->cycles.mult);
 281        overflow_cycles = min(overflow_cycles, div_u64(clock->cycles.mask, 3));
 282
 283        nsec = cyclecounter_cyc2ns(&clock->cycles, overflow_cycles, 0, &frac);
 284        clock->overflow_period = nsecs_to_jiffies(nsec);
 285
 286        INIT_DELAYED_WORK(&clock->overflow_work, mlxsw_sp1_ptp_clock_overflow);
 287        mlxsw_core_schedule_dw(&clock->overflow_work, 0);
 288
 289        clock->ptp_info = mlxsw_sp1_ptp_clock_info;
 290        clock->ptp = ptp_clock_register(&clock->ptp_info, dev);
 291        if (IS_ERR(clock->ptp)) {
 292                err = PTR_ERR(clock->ptp);
 293                dev_err(dev, "ptp_clock_register failed %d\n", err);
 294                goto err_ptp_clock_register;
 295        }
 296
 297        return clock;
 298
 299err_ptp_clock_register:
 300        cancel_delayed_work_sync(&clock->overflow_work);
 301        kfree(clock);
 302        return ERR_PTR(err);
 303}
 304
 305void mlxsw_sp1_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock)
 306{
 307        ptp_clock_unregister(clock->ptp);
 308        cancel_delayed_work_sync(&clock->overflow_work);
 309        kfree(clock);
 310}
 311
 312static int mlxsw_sp_ptp_parse(struct sk_buff *skb,
 313                              u8 *p_domain_number,
 314                              u8 *p_message_type,
 315                              u16 *p_sequence_id)
 316{
 317        unsigned int ptp_class;
 318        struct ptp_header *hdr;
 319
 320        ptp_class = ptp_classify_raw(skb);
 321
 322        switch (ptp_class & PTP_CLASS_VMASK) {
 323        case PTP_CLASS_V1:
 324        case PTP_CLASS_V2:
 325                break;
 326        default:
 327                return -ERANGE;
 328        }
 329
 330        hdr = ptp_parse_header(skb, ptp_class);
 331        if (!hdr)
 332                return -EINVAL;
 333
 334        *p_message_type  = ptp_get_msgtype(hdr, ptp_class);
 335        *p_domain_number = hdr->domain_number;
 336        *p_sequence_id   = be16_to_cpu(hdr->sequence_id);
 337
 338        return 0;
 339}
 340
 341/* Returns NULL on successful insertion, a pointer on conflict, or an ERR_PTR on
 342 * error.
 343 */
 344static int
 345mlxsw_sp1_ptp_unmatched_save(struct mlxsw_sp *mlxsw_sp,
 346                             struct mlxsw_sp1_ptp_key key,
 347                             struct sk_buff *skb,
 348                             u64 timestamp)
 349{
 350        int cycles = MLXSW_SP1_PTP_HT_GC_TIMEOUT / MLXSW_SP1_PTP_HT_GC_INTERVAL;
 351        struct mlxsw_sp_ptp_state *ptp_state = mlxsw_sp->ptp_state;
 352        struct mlxsw_sp1_ptp_unmatched *unmatched;
 353        int err;
 354
 355        unmatched = kzalloc(sizeof(*unmatched), GFP_ATOMIC);
 356        if (!unmatched)
 357                return -ENOMEM;
 358
 359        unmatched->key = key;
 360        unmatched->skb = skb;
 361        unmatched->timestamp = timestamp;
 362        unmatched->gc_cycle = mlxsw_sp->ptp_state->gc_cycle + cycles;
 363
 364        err = rhltable_insert(&ptp_state->unmatched_ht, &unmatched->ht_node,
 365                              mlxsw_sp1_ptp_unmatched_ht_params);
 366        if (err)
 367                kfree(unmatched);
 368
 369        return err;
 370}
 371
 372static struct mlxsw_sp1_ptp_unmatched *
 373mlxsw_sp1_ptp_unmatched_lookup(struct mlxsw_sp *mlxsw_sp,
 374                               struct mlxsw_sp1_ptp_key key, int *p_length)
 375{
 376        struct mlxsw_sp1_ptp_unmatched *unmatched, *last = NULL;
 377        struct rhlist_head *tmp, *list;
 378        int length = 0;
 379
 380        list = rhltable_lookup(&mlxsw_sp->ptp_state->unmatched_ht, &key,
 381                               mlxsw_sp1_ptp_unmatched_ht_params);
 382        rhl_for_each_entry_rcu(unmatched, tmp, list, ht_node) {
 383                last = unmatched;
 384                length++;
 385        }
 386
 387        *p_length = length;
 388        return last;
 389}
 390
 391static int
 392mlxsw_sp1_ptp_unmatched_remove(struct mlxsw_sp *mlxsw_sp,
 393                               struct mlxsw_sp1_ptp_unmatched *unmatched)
 394{
 395        return rhltable_remove(&mlxsw_sp->ptp_state->unmatched_ht,
 396                               &unmatched->ht_node,
 397                               mlxsw_sp1_ptp_unmatched_ht_params);
 398}
 399
 400/* This function is called in the following scenarios:
 401 *
 402 * 1) When a packet is matched with its timestamp.
 403 * 2) In several situation when it is necessary to immediately pass on
 404 *    an SKB without a timestamp.
 405 * 3) From GC indirectly through mlxsw_sp1_ptp_unmatched_finish().
 406 *    This case is similar to 2) above.
 407 */
 408static void mlxsw_sp1_ptp_packet_finish(struct mlxsw_sp *mlxsw_sp,
 409                                        struct sk_buff *skb, u16 local_port,
 410                                        bool ingress,
 411                                        struct skb_shared_hwtstamps *hwtstamps)
 412{
 413        struct mlxsw_sp_port *mlxsw_sp_port;
 414
 415        /* Between capturing the packet and finishing it, there is a window of
 416         * opportunity for the originating port to go away (e.g. due to a
 417         * split). Also make sure the SKB device reference is still valid.
 418         */
 419        mlxsw_sp_port = mlxsw_sp->ports[local_port];
 420        if (!(mlxsw_sp_port && (!skb->dev || skb->dev == mlxsw_sp_port->dev))) {
 421                dev_kfree_skb_any(skb);
 422                return;
 423        }
 424
 425        if (ingress) {
 426                if (hwtstamps)
 427                        *skb_hwtstamps(skb) = *hwtstamps;
 428                mlxsw_sp_rx_listener_no_mark_func(skb, local_port, mlxsw_sp);
 429        } else {
 430                /* skb_tstamp_tx() allows hwtstamps to be NULL. */
 431                skb_tstamp_tx(skb, hwtstamps);
 432                dev_kfree_skb_any(skb);
 433        }
 434}
 435
 436static void mlxsw_sp1_packet_timestamp(struct mlxsw_sp *mlxsw_sp,
 437                                       struct mlxsw_sp1_ptp_key key,
 438                                       struct sk_buff *skb,
 439                                       u64 timestamp)
 440{
 441        struct skb_shared_hwtstamps hwtstamps;
 442        u64 nsec;
 443
 444        spin_lock_bh(&mlxsw_sp->clock->lock);
 445        nsec = timecounter_cyc2time(&mlxsw_sp->clock->tc, timestamp);
 446        spin_unlock_bh(&mlxsw_sp->clock->lock);
 447
 448        hwtstamps.hwtstamp = ns_to_ktime(nsec);
 449        mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb,
 450                                    key.local_port, key.ingress, &hwtstamps);
 451}
 452
 453static void
 454mlxsw_sp1_ptp_unmatched_finish(struct mlxsw_sp *mlxsw_sp,
 455                               struct mlxsw_sp1_ptp_unmatched *unmatched)
 456{
 457        if (unmatched->skb && unmatched->timestamp)
 458                mlxsw_sp1_packet_timestamp(mlxsw_sp, unmatched->key,
 459                                           unmatched->skb,
 460                                           unmatched->timestamp);
 461        else if (unmatched->skb)
 462                mlxsw_sp1_ptp_packet_finish(mlxsw_sp, unmatched->skb,
 463                                            unmatched->key.local_port,
 464                                            unmatched->key.ingress, NULL);
 465        kfree_rcu(unmatched, rcu);
 466}
 467
 468static void mlxsw_sp1_ptp_unmatched_free_fn(void *ptr, void *arg)
 469{
 470        struct mlxsw_sp1_ptp_unmatched *unmatched = ptr;
 471
 472        /* This is invoked at a point where the ports are gone already. Nothing
 473         * to do with whatever is left in the HT but to free it.
 474         */
 475        if (unmatched->skb)
 476                dev_kfree_skb_any(unmatched->skb);
 477        kfree_rcu(unmatched, rcu);
 478}
 479
 480static void mlxsw_sp1_ptp_got_piece(struct mlxsw_sp *mlxsw_sp,
 481                                    struct mlxsw_sp1_ptp_key key,
 482                                    struct sk_buff *skb, u64 timestamp)
 483{
 484        struct mlxsw_sp1_ptp_unmatched *unmatched;
 485        int length;
 486        int err;
 487
 488        rcu_read_lock();
 489
 490        spin_lock(&mlxsw_sp->ptp_state->unmatched_lock);
 491
 492        unmatched = mlxsw_sp1_ptp_unmatched_lookup(mlxsw_sp, key, &length);
 493        if (skb && unmatched && unmatched->timestamp) {
 494                unmatched->skb = skb;
 495        } else if (timestamp && unmatched && unmatched->skb) {
 496                unmatched->timestamp = timestamp;
 497        } else {
 498                /* Either there is no entry to match, or one that is there is
 499                 * incompatible.
 500                 */
 501                if (length < 100)
 502                        err = mlxsw_sp1_ptp_unmatched_save(mlxsw_sp, key,
 503                                                           skb, timestamp);
 504                else
 505                        err = -E2BIG;
 506                if (err && skb)
 507                        mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb,
 508                                                    key.local_port,
 509                                                    key.ingress, NULL);
 510                unmatched = NULL;
 511        }
 512
 513        if (unmatched) {
 514                err = mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp, unmatched);
 515                WARN_ON_ONCE(err);
 516        }
 517
 518        spin_unlock(&mlxsw_sp->ptp_state->unmatched_lock);
 519
 520        if (unmatched)
 521                mlxsw_sp1_ptp_unmatched_finish(mlxsw_sp, unmatched);
 522
 523        rcu_read_unlock();
 524}
 525
 526static void mlxsw_sp1_ptp_got_packet(struct mlxsw_sp *mlxsw_sp,
 527                                     struct sk_buff *skb, u16 local_port,
 528                                     bool ingress)
 529{
 530        struct mlxsw_sp_port *mlxsw_sp_port;
 531        struct mlxsw_sp1_ptp_key key;
 532        u8 types;
 533        int err;
 534
 535        mlxsw_sp_port = mlxsw_sp->ports[local_port];
 536        if (!mlxsw_sp_port)
 537                goto immediate;
 538
 539        types = ingress ? mlxsw_sp_port->ptp.ing_types :
 540                          mlxsw_sp_port->ptp.egr_types;
 541        if (!types)
 542                goto immediate;
 543
 544        memset(&key, 0, sizeof(key));
 545        key.local_port = local_port;
 546        key.ingress = ingress;
 547
 548        err = mlxsw_sp_ptp_parse(skb, &key.domain_number, &key.message_type,
 549                                 &key.sequence_id);
 550        if (err)
 551                goto immediate;
 552
 553        /* For packets whose timestamping was not enabled on this port, don't
 554         * bother trying to match the timestamp.
 555         */
 556        if (!((1 << key.message_type) & types))
 557                goto immediate;
 558
 559        mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, skb, 0);
 560        return;
 561
 562immediate:
 563        mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, local_port, ingress, NULL);
 564}
 565
 566void mlxsw_sp1_ptp_got_timestamp(struct mlxsw_sp *mlxsw_sp, bool ingress,
 567                                 u16 local_port, u8 message_type,
 568                                 u8 domain_number, u16 sequence_id,
 569                                 u64 timestamp)
 570{
 571        unsigned int max_ports = mlxsw_core_max_ports(mlxsw_sp->core);
 572        struct mlxsw_sp_port *mlxsw_sp_port;
 573        struct mlxsw_sp1_ptp_key key;
 574        u8 types;
 575
 576        if (WARN_ON_ONCE(local_port >= max_ports))
 577                return;
 578        mlxsw_sp_port = mlxsw_sp->ports[local_port];
 579        if (!mlxsw_sp_port)
 580                return;
 581
 582        types = ingress ? mlxsw_sp_port->ptp.ing_types :
 583                          mlxsw_sp_port->ptp.egr_types;
 584
 585        /* For message types whose timestamping was not enabled on this port,
 586         * don't bother with the timestamp.
 587         */
 588        if (!((1 << message_type) & types))
 589                return;
 590
 591        memset(&key, 0, sizeof(key));
 592        key.local_port = local_port;
 593        key.domain_number = domain_number;
 594        key.message_type = message_type;
 595        key.sequence_id = sequence_id;
 596        key.ingress = ingress;
 597
 598        mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, NULL, timestamp);
 599}
 600
 601void mlxsw_sp1_ptp_receive(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb,
 602                           u16 local_port)
 603{
 604        skb_reset_mac_header(skb);
 605        mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, true);
 606}
 607
 608void mlxsw_sp1_ptp_transmitted(struct mlxsw_sp *mlxsw_sp,
 609                               struct sk_buff *skb, u16 local_port)
 610{
 611        mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, false);
 612}
 613
 614static void
 615mlxsw_sp1_ptp_ht_gc_collect(struct mlxsw_sp_ptp_state *ptp_state,
 616                            struct mlxsw_sp1_ptp_unmatched *unmatched)
 617{
 618        struct mlxsw_sp_ptp_port_dir_stats *stats;
 619        struct mlxsw_sp_port *mlxsw_sp_port;
 620        int err;
 621
 622        /* If an unmatched entry has an SKB, it has to be handed over to the
 623         * networking stack. This is usually done from a trap handler, which is
 624         * invoked in a softirq context. Here we are going to do it in process
 625         * context. If that were to be interrupted by a softirq, it could cause
 626         * a deadlock when an attempt is made to take an already-taken lock
 627         * somewhere along the sending path. Disable softirqs to prevent this.
 628         */
 629        local_bh_disable();
 630
 631        spin_lock(&ptp_state->unmatched_lock);
 632        err = rhltable_remove(&ptp_state->unmatched_ht, &unmatched->ht_node,
 633                              mlxsw_sp1_ptp_unmatched_ht_params);
 634        spin_unlock(&ptp_state->unmatched_lock);
 635
 636        if (err)
 637                /* The packet was matched with timestamp during the walk. */
 638                goto out;
 639
 640        mlxsw_sp_port = ptp_state->mlxsw_sp->ports[unmatched->key.local_port];
 641        if (mlxsw_sp_port) {
 642                stats = unmatched->key.ingress ?
 643                        &mlxsw_sp_port->ptp.stats.rx_gcd :
 644                        &mlxsw_sp_port->ptp.stats.tx_gcd;
 645                if (unmatched->skb)
 646                        stats->packets++;
 647                else
 648                        stats->timestamps++;
 649        }
 650
 651        /* mlxsw_sp1_ptp_unmatched_finish() invokes netif_receive_skb(). While
 652         * the comment at that function states that it can only be called in
 653         * soft IRQ context, this pattern of local_bh_disable() +
 654         * netif_receive_skb(), in process context, is seen elsewhere in the
 655         * kernel, notably in pktgen.
 656         */
 657        mlxsw_sp1_ptp_unmatched_finish(ptp_state->mlxsw_sp, unmatched);
 658
 659out:
 660        local_bh_enable();
 661}
 662
 663static void mlxsw_sp1_ptp_ht_gc(struct work_struct *work)
 664{
 665        struct delayed_work *dwork = to_delayed_work(work);
 666        struct mlxsw_sp1_ptp_unmatched *unmatched;
 667        struct mlxsw_sp_ptp_state *ptp_state;
 668        struct rhashtable_iter iter;
 669        u32 gc_cycle;
 670        void *obj;
 671
 672        ptp_state = container_of(dwork, struct mlxsw_sp_ptp_state, ht_gc_dw);
 673        gc_cycle = ptp_state->gc_cycle++;
 674
 675        rhltable_walk_enter(&ptp_state->unmatched_ht, &iter);
 676        rhashtable_walk_start(&iter);
 677        while ((obj = rhashtable_walk_next(&iter))) {
 678                if (IS_ERR(obj))
 679                        continue;
 680
 681                unmatched = obj;
 682                if (unmatched->gc_cycle <= gc_cycle)
 683                        mlxsw_sp1_ptp_ht_gc_collect(ptp_state, unmatched);
 684        }
 685        rhashtable_walk_stop(&iter);
 686        rhashtable_walk_exit(&iter);
 687
 688        mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw,
 689                               MLXSW_SP1_PTP_HT_GC_INTERVAL);
 690}
 691
 692static int mlxsw_sp_ptp_mtptpt_set(struct mlxsw_sp *mlxsw_sp,
 693                                   enum mlxsw_reg_mtptpt_trap_id trap_id,
 694                                   u16 message_type)
 695{
 696        char mtptpt_pl[MLXSW_REG_MTPTPT_LEN];
 697
 698        mlxsw_reg_mtptptp_pack(mtptpt_pl, trap_id, message_type);
 699        return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtptpt), mtptpt_pl);
 700}
 701
 702static int mlxsw_sp1_ptp_set_fifo_clr_on_trap(struct mlxsw_sp *mlxsw_sp,
 703                                              bool clr)
 704{
 705        char mogcr_pl[MLXSW_REG_MOGCR_LEN] = {0};
 706        int err;
 707
 708        err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl);
 709        if (err)
 710                return err;
 711
 712        mlxsw_reg_mogcr_ptp_iftc_set(mogcr_pl, clr);
 713        mlxsw_reg_mogcr_ptp_eftc_set(mogcr_pl, clr);
 714        return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl);
 715}
 716
 717static int mlxsw_sp1_ptp_mtpppc_set(struct mlxsw_sp *mlxsw_sp,
 718                                    u16 ing_types, u16 egr_types)
 719{
 720        char mtpppc_pl[MLXSW_REG_MTPPPC_LEN];
 721
 722        mlxsw_reg_mtpppc_pack(mtpppc_pl, ing_types, egr_types);
 723        return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtpppc), mtpppc_pl);
 724}
 725
 726struct mlxsw_sp1_ptp_shaper_params {
 727        u32 ethtool_speed;
 728        enum mlxsw_reg_qpsc_port_speed port_speed;
 729        u8 shaper_time_exp;
 730        u8 shaper_time_mantissa;
 731        u8 shaper_inc;
 732        u8 shaper_bs;
 733        u8 port_to_shaper_credits;
 734        int ing_timestamp_inc;
 735        int egr_timestamp_inc;
 736};
 737
 738static const struct mlxsw_sp1_ptp_shaper_params
 739mlxsw_sp1_ptp_shaper_params[] = {
 740        {
 741                .ethtool_speed          = SPEED_100,
 742                .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_100M,
 743                .shaper_time_exp        = 4,
 744                .shaper_time_mantissa   = 12,
 745                .shaper_inc             = 9,
 746                .shaper_bs              = 1,
 747                .port_to_shaper_credits = 1,
 748                .ing_timestamp_inc      = -313,
 749                .egr_timestamp_inc      = 313,
 750        },
 751        {
 752                .ethtool_speed          = SPEED_1000,
 753                .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_1G,
 754                .shaper_time_exp        = 0,
 755                .shaper_time_mantissa   = 12,
 756                .shaper_inc             = 6,
 757                .shaper_bs              = 0,
 758                .port_to_shaper_credits = 1,
 759                .ing_timestamp_inc      = -35,
 760                .egr_timestamp_inc      = 35,
 761        },
 762        {
 763                .ethtool_speed          = SPEED_10000,
 764                .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_10G,
 765                .shaper_time_exp        = 0,
 766                .shaper_time_mantissa   = 2,
 767                .shaper_inc             = 14,
 768                .shaper_bs              = 1,
 769                .port_to_shaper_credits = 1,
 770                .ing_timestamp_inc      = -11,
 771                .egr_timestamp_inc      = 11,
 772        },
 773        {
 774                .ethtool_speed          = SPEED_25000,
 775                .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_25G,
 776                .shaper_time_exp        = 0,
 777                .shaper_time_mantissa   = 0,
 778                .shaper_inc             = 11,
 779                .shaper_bs              = 1,
 780                .port_to_shaper_credits = 1,
 781                .ing_timestamp_inc      = -14,
 782                .egr_timestamp_inc      = 14,
 783        },
 784};
 785
 786#define MLXSW_SP1_PTP_SHAPER_PARAMS_LEN ARRAY_SIZE(mlxsw_sp1_ptp_shaper_params)
 787
 788static int mlxsw_sp1_ptp_shaper_params_set(struct mlxsw_sp *mlxsw_sp)
 789{
 790        const struct mlxsw_sp1_ptp_shaper_params *params;
 791        char qpsc_pl[MLXSW_REG_QPSC_LEN];
 792        int i, err;
 793
 794        for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) {
 795                params = &mlxsw_sp1_ptp_shaper_params[i];
 796                mlxsw_reg_qpsc_pack(qpsc_pl, params->port_speed,
 797                                    params->shaper_time_exp,
 798                                    params->shaper_time_mantissa,
 799                                    params->shaper_inc, params->shaper_bs,
 800                                    params->port_to_shaper_credits,
 801                                    params->ing_timestamp_inc,
 802                                    params->egr_timestamp_inc);
 803                err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpsc), qpsc_pl);
 804                if (err)
 805                        return err;
 806        }
 807
 808        return 0;
 809}
 810
 811struct mlxsw_sp_ptp_state *mlxsw_sp1_ptp_init(struct mlxsw_sp *mlxsw_sp)
 812{
 813        struct mlxsw_sp_ptp_state *ptp_state;
 814        u16 message_type;
 815        int err;
 816
 817        err = mlxsw_sp1_ptp_shaper_params_set(mlxsw_sp);
 818        if (err)
 819                return ERR_PTR(err);
 820
 821        ptp_state = kzalloc(sizeof(*ptp_state), GFP_KERNEL);
 822        if (!ptp_state)
 823                return ERR_PTR(-ENOMEM);
 824        ptp_state->mlxsw_sp = mlxsw_sp;
 825
 826        spin_lock_init(&ptp_state->unmatched_lock);
 827
 828        err = rhltable_init(&ptp_state->unmatched_ht,
 829                            &mlxsw_sp1_ptp_unmatched_ht_params);
 830        if (err)
 831                goto err_hashtable_init;
 832
 833        /* Delive these message types as PTP0. */
 834        message_type = BIT(PTP_MSGTYPE_SYNC) |
 835                       BIT(PTP_MSGTYPE_DELAY_REQ) |
 836                       BIT(PTP_MSGTYPE_PDELAY_REQ) |
 837                       BIT(PTP_MSGTYPE_PDELAY_RESP);
 838        err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0,
 839                                      message_type);
 840        if (err)
 841                goto err_mtptpt_set;
 842
 843        /* Everything else is PTP1. */
 844        message_type = ~message_type;
 845        err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1,
 846                                      message_type);
 847        if (err)
 848                goto err_mtptpt1_set;
 849
 850        err = mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, true);
 851        if (err)
 852                goto err_fifo_clr;
 853
 854        INIT_DELAYED_WORK(&ptp_state->ht_gc_dw, mlxsw_sp1_ptp_ht_gc);
 855        mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw,
 856                               MLXSW_SP1_PTP_HT_GC_INTERVAL);
 857        return ptp_state;
 858
 859err_fifo_clr:
 860        mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0);
 861err_mtptpt1_set:
 862        mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0);
 863err_mtptpt_set:
 864        rhltable_destroy(&ptp_state->unmatched_ht);
 865err_hashtable_init:
 866        kfree(ptp_state);
 867        return ERR_PTR(err);
 868}
 869
 870void mlxsw_sp1_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state)
 871{
 872        struct mlxsw_sp *mlxsw_sp = ptp_state->mlxsw_sp;
 873
 874        cancel_delayed_work_sync(&ptp_state->ht_gc_dw);
 875        mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp, 0, 0);
 876        mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, false);
 877        mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0);
 878        mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0);
 879        rhltable_free_and_destroy(&ptp_state->unmatched_ht,
 880                                  &mlxsw_sp1_ptp_unmatched_free_fn, NULL);
 881        kfree(ptp_state);
 882}
 883
 884int mlxsw_sp1_ptp_hwtstamp_get(struct mlxsw_sp_port *mlxsw_sp_port,
 885                               struct hwtstamp_config *config)
 886{
 887        *config = mlxsw_sp_port->ptp.hwtstamp_config;
 888        return 0;
 889}
 890
 891static int mlxsw_sp_ptp_get_message_types(const struct hwtstamp_config *config,
 892                                          u16 *p_ing_types, u16 *p_egr_types,
 893                                          enum hwtstamp_rx_filters *p_rx_filter)
 894{
 895        enum hwtstamp_rx_filters rx_filter = config->rx_filter;
 896        enum hwtstamp_tx_types tx_type = config->tx_type;
 897        u16 ing_types = 0x00;
 898        u16 egr_types = 0x00;
 899
 900        switch (tx_type) {
 901        case HWTSTAMP_TX_OFF:
 902                egr_types = 0x00;
 903                break;
 904        case HWTSTAMP_TX_ON:
 905                egr_types = 0xff;
 906                break;
 907        case HWTSTAMP_TX_ONESTEP_SYNC:
 908        case HWTSTAMP_TX_ONESTEP_P2P:
 909                return -ERANGE;
 910        default:
 911                return -EINVAL;
 912        }
 913
 914        switch (rx_filter) {
 915        case HWTSTAMP_FILTER_NONE:
 916                ing_types = 0x00;
 917                break;
 918        case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 919        case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 920        case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
 921        case HWTSTAMP_FILTER_PTP_V2_SYNC:
 922                ing_types = 0x01;
 923                break;
 924        case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
 925        case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
 926        case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
 927        case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 928                ing_types = 0x02;
 929                break;
 930        case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 931        case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 932        case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
 933        case HWTSTAMP_FILTER_PTP_V2_EVENT:
 934                ing_types = 0x0f;
 935                break;
 936        case HWTSTAMP_FILTER_ALL:
 937                ing_types = 0xff;
 938                break;
 939        case HWTSTAMP_FILTER_SOME:
 940        case HWTSTAMP_FILTER_NTP_ALL:
 941                return -ERANGE;
 942        default:
 943                return -EINVAL;
 944        }
 945
 946        *p_ing_types = ing_types;
 947        *p_egr_types = egr_types;
 948        *p_rx_filter = rx_filter;
 949        return 0;
 950}
 951
 952static int mlxsw_sp1_ptp_mtpppc_update(struct mlxsw_sp_port *mlxsw_sp_port,
 953                                       u16 ing_types, u16 egr_types)
 954{
 955        struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 956        struct mlxsw_sp_port *tmp;
 957        u16 orig_ing_types = 0;
 958        u16 orig_egr_types = 0;
 959        int err;
 960        int i;
 961
 962        /* MTPPPC configures timestamping globally, not per port. Find the
 963         * configuration that contains all configured timestamping requests.
 964         */
 965        for (i = 1; i < mlxsw_core_max_ports(mlxsw_sp->core); i++) {
 966                tmp = mlxsw_sp->ports[i];
 967                if (tmp) {
 968                        orig_ing_types |= tmp->ptp.ing_types;
 969                        orig_egr_types |= tmp->ptp.egr_types;
 970                }
 971                if (tmp && tmp != mlxsw_sp_port) {
 972                        ing_types |= tmp->ptp.ing_types;
 973                        egr_types |= tmp->ptp.egr_types;
 974                }
 975        }
 976
 977        if ((ing_types || egr_types) && !(orig_ing_types || orig_egr_types)) {
 978                err = mlxsw_sp_parsing_depth_inc(mlxsw_sp);
 979                if (err) {
 980                        netdev_err(mlxsw_sp_port->dev, "Failed to increase parsing depth");
 981                        return err;
 982                }
 983        }
 984        if (!(ing_types || egr_types) && (orig_ing_types || orig_egr_types))
 985                mlxsw_sp_parsing_depth_dec(mlxsw_sp);
 986
 987        return mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp_port->mlxsw_sp,
 988                                       ing_types, egr_types);
 989}
 990
 991static bool mlxsw_sp1_ptp_hwtstamp_enabled(struct mlxsw_sp_port *mlxsw_sp_port)
 992{
 993        return mlxsw_sp_port->ptp.ing_types || mlxsw_sp_port->ptp.egr_types;
 994}
 995
 996static int
 997mlxsw_sp1_ptp_port_shaper_set(struct mlxsw_sp_port *mlxsw_sp_port, bool enable)
 998{
 999        struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1000        char qeec_pl[MLXSW_REG_QEEC_LEN];
1001
1002        mlxsw_reg_qeec_ptps_pack(qeec_pl, mlxsw_sp_port->local_port, enable);
1003        return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qeec), qeec_pl);
1004}
1005
1006static int mlxsw_sp1_ptp_port_shaper_check(struct mlxsw_sp_port *mlxsw_sp_port)
1007{
1008        bool ptps = false;
1009        int err, i;
1010        u32 speed;
1011
1012        if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1013                return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, false);
1014
1015        err = mlxsw_sp_port_speed_get(mlxsw_sp_port, &speed);
1016        if (err)
1017                return err;
1018
1019        for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) {
1020                if (mlxsw_sp1_ptp_shaper_params[i].ethtool_speed == speed) {
1021                        ptps = true;
1022                        break;
1023                }
1024        }
1025
1026        return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, ptps);
1027}
1028
1029void mlxsw_sp1_ptp_shaper_work(struct work_struct *work)
1030{
1031        struct delayed_work *dwork = to_delayed_work(work);
1032        struct mlxsw_sp_port *mlxsw_sp_port;
1033        int err;
1034
1035        mlxsw_sp_port = container_of(dwork, struct mlxsw_sp_port,
1036                                     ptp.shaper_dw);
1037
1038        if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1039                return;
1040
1041        err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1042        if (err)
1043                netdev_err(mlxsw_sp_port->dev, "Failed to set up PTP shaper\n");
1044}
1045
1046int mlxsw_sp1_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port,
1047                               struct hwtstamp_config *config)
1048{
1049        enum hwtstamp_rx_filters rx_filter;
1050        u16 ing_types;
1051        u16 egr_types;
1052        int err;
1053
1054        err = mlxsw_sp_ptp_get_message_types(config, &ing_types, &egr_types,
1055                                             &rx_filter);
1056        if (err)
1057                return err;
1058
1059        err = mlxsw_sp1_ptp_mtpppc_update(mlxsw_sp_port, ing_types, egr_types);
1060        if (err)
1061                return err;
1062
1063        mlxsw_sp_port->ptp.hwtstamp_config = *config;
1064        mlxsw_sp_port->ptp.ing_types = ing_types;
1065        mlxsw_sp_port->ptp.egr_types = egr_types;
1066
1067        err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1068        if (err)
1069                return err;
1070
1071        /* Notify the ioctl caller what we are actually timestamping. */
1072        config->rx_filter = rx_filter;
1073
1074        return 0;
1075}
1076
1077int mlxsw_sp1_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp,
1078                              struct ethtool_ts_info *info)
1079{
1080        info->phc_index = ptp_clock_index(mlxsw_sp->clock->ptp);
1081
1082        info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
1083                                SOF_TIMESTAMPING_RX_HARDWARE |
1084                                SOF_TIMESTAMPING_RAW_HARDWARE;
1085
1086        info->tx_types = BIT(HWTSTAMP_TX_OFF) |
1087                         BIT(HWTSTAMP_TX_ON);
1088
1089        info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
1090                           BIT(HWTSTAMP_FILTER_ALL);
1091
1092        return 0;
1093}
1094
1095struct mlxsw_sp_ptp_port_stat {
1096        char str[ETH_GSTRING_LEN];
1097        ptrdiff_t offset;
1098};
1099
1100#define MLXSW_SP_PTP_PORT_STAT(NAME, FIELD)                             \
1101        {                                                               \
1102                .str = NAME,                                            \
1103                .offset = offsetof(struct mlxsw_sp_ptp_port_stats,      \
1104                                    FIELD),                             \
1105        }
1106
1107static const struct mlxsw_sp_ptp_port_stat mlxsw_sp_ptp_port_stats[] = {
1108        MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_packets",    rx_gcd.packets),
1109        MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_timestamps", rx_gcd.timestamps),
1110        MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_packets",    tx_gcd.packets),
1111        MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_timestamps", tx_gcd.timestamps),
1112};
1113
1114#undef MLXSW_SP_PTP_PORT_STAT
1115
1116#define MLXSW_SP_PTP_PORT_STATS_LEN \
1117        ARRAY_SIZE(mlxsw_sp_ptp_port_stats)
1118
1119int mlxsw_sp1_get_stats_count(void)
1120{
1121        return MLXSW_SP_PTP_PORT_STATS_LEN;
1122}
1123
1124void mlxsw_sp1_get_stats_strings(u8 **p)
1125{
1126        int i;
1127
1128        for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) {
1129                memcpy(*p, mlxsw_sp_ptp_port_stats[i].str,
1130                       ETH_GSTRING_LEN);
1131                *p += ETH_GSTRING_LEN;
1132        }
1133}
1134
1135void mlxsw_sp1_get_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1136                         u64 *data, int data_index)
1137{
1138        void *stats = &mlxsw_sp_port->ptp.stats;
1139        ptrdiff_t offset;
1140        int i;
1141
1142        data += data_index;
1143        for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) {
1144                offset = mlxsw_sp_ptp_port_stats[i].offset;
1145                *data++ = *(u64 *)(stats + offset);
1146        }
1147}
1148