drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c

   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 #include <linux/refcount.h>
  15
  16 #include "spectrum.h"
  17 #include "spectrum_ptp.h"
  18 #include "core.h"
  19
  20 #define MLXSW_SP1_PTP_CLOCK_CYCLES_SHIFT        29
  21 #define MLXSW_SP1_PTP_CLOCK_FREQ_KHZ            156257 /* 6.4nSec */
  22 #define MLXSW_SP1_PTP_CLOCK_MASK                64
  23
  24 #define MLXSW_SP1_PTP_HT_GC_INTERVAL            500 /* ms */
  25
  26 /* How long, approximately, should the unmatched entries stay in the hash table
  27  * before they are collected. Should be evenly divisible by the GC interval.
  28  */
  29 #define MLXSW_SP1_PTP_HT_GC_TIMEOUT             1000 /* ms */
  30
  31 struct mlxsw_sp_ptp_state {
  32         struct mlxsw_sp *mlxsw_sp;
  33 };
  34
  35 struct mlxsw_sp1_ptp_state {
  36         struct mlxsw_sp_ptp_state common;
  37         struct rhltable unmatched_ht;
  38         spinlock_t unmatched_lock; /* protects the HT */
  39         struct delayed_work ht_gc_dw;
  40         u32 gc_cycle;
  41 };
  42
  43 struct mlxsw_sp2_ptp_state {
  44         struct mlxsw_sp_ptp_state common;
  45         refcount_t ptp_port_enabled_ref; /* Number of ports with time stamping
  46                                           * enabled.
  47                                           */
  48         struct hwtstamp_config config;
  49 };
  50
  51 struct mlxsw_sp1_ptp_key {
  52         u16 local_port;
  53         u8 message_type;
  54         u16 sequence_id;
  55         u8 domain_number;
  56         bool ingress;
  57 };
  58
  59 struct mlxsw_sp1_ptp_unmatched {
  60         struct mlxsw_sp1_ptp_key key;
  61         struct rhlist_head ht_node;
  62         struct rcu_head rcu;
  63         struct sk_buff *skb;
  64         u64 timestamp;
  65         u32 gc_cycle;
  66 };
  67
  68 static const struct rhashtable_params mlxsw_sp1_ptp_unmatched_ht_params = {
  69         .key_len = sizeof_field(struct mlxsw_sp1_ptp_unmatched, key),
  70         .key_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, key),
  71         .head_offset = offsetof(struct mlxsw_sp1_ptp_unmatched, ht_node),
  72 };
  73
  74 struct mlxsw_sp_ptp_clock {
  75         struct mlxsw_core *core;
  76         struct ptp_clock *ptp;
  77         struct ptp_clock_info ptp_info;
  78 };
  79
  80 struct mlxsw_sp1_ptp_clock {
  81         struct mlxsw_sp_ptp_clock common;
  82         spinlock_t lock; /* protect this structure */
  83         struct cyclecounter cycles;
  84         struct timecounter tc;
  85         u32 nominal_c_mult;
  86         unsigned long overflow_period;
  87         struct delayed_work overflow_work;
  88 };
  89
  90 static struct mlxsw_sp1_ptp_state *
  91 mlxsw_sp1_ptp_state(struct mlxsw_sp *mlxsw_sp)
  92 {
  93         return container_of(mlxsw_sp->ptp_state, struct mlxsw_sp1_ptp_state,
  94                             common);
  95 }
  96
  97 static struct mlxsw_sp2_ptp_state *
  98 mlxsw_sp2_ptp_state(struct mlxsw_sp *mlxsw_sp)
  99 {
 100         return container_of(mlxsw_sp->ptp_state, struct mlxsw_sp2_ptp_state,
 101                             common);
 102 }
 103
 104 static struct mlxsw_sp1_ptp_clock *
 105 mlxsw_sp1_ptp_clock(struct ptp_clock_info *ptp)
 106 {
 107         return container_of(ptp, struct mlxsw_sp1_ptp_clock, common.ptp_info);
 108 }
 109
 110 static u64 __mlxsw_sp1_ptp_read_frc(struct mlxsw_sp1_ptp_clock *clock,
 111                                     struct ptp_system_timestamp *sts)
 112 {
 113         struct mlxsw_core *mlxsw_core = clock->common.core;
 114         u32 frc_h1, frc_h2, frc_l;
 115
 116         frc_h1 = mlxsw_core_read_frc_h(mlxsw_core);
 117         ptp_read_system_prets(sts);
 118         frc_l = mlxsw_core_read_frc_l(mlxsw_core);
 119         ptp_read_system_postts(sts);
 120         frc_h2 = mlxsw_core_read_frc_h(mlxsw_core);
 121
 122         if (frc_h1 != frc_h2) {
 123                 /* wrap around */
 124                 ptp_read_system_prets(sts);
 125                 frc_l = mlxsw_core_read_frc_l(mlxsw_core);
 126                 ptp_read_system_postts(sts);
 127         }
 128
 129         return (u64) frc_l | (u64) frc_h2 << 32;
 130 }
 131
 132 static u64 mlxsw_sp1_ptp_read_frc(const struct cyclecounter *cc)
 133 {
 134         struct mlxsw_sp1_ptp_clock *clock =
 135                 container_of(cc, struct mlxsw_sp1_ptp_clock, cycles);
 136
 137         return __mlxsw_sp1_ptp_read_frc(clock, NULL) & cc->mask;
 138 }
 139
 140 static int
 141 mlxsw_sp_ptp_phc_adjfreq(struct mlxsw_sp_ptp_clock *clock, int freq_adj)
 142 {
 143         struct mlxsw_core *mlxsw_core = clock->core;
 144         char mtutc_pl[MLXSW_REG_MTUTC_LEN];
 145
 146         mlxsw_reg_mtutc_pack(mtutc_pl, MLXSW_REG_MTUTC_OPERATION_ADJUST_FREQ,
 147                              freq_adj, 0, 0, 0);
 148         return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
 149 }
 150
 151 static u64 mlxsw_sp1_ptp_ns2cycles(const struct timecounter *tc, u64 nsec)
 152 {
 153         u64 cycles = (u64) nsec;
 154
 155         cycles <<= tc->cc->shift;
 156         cycles = div_u64(cycles, tc->cc->mult);
 157
 158         return cycles;
 159 }
 160
 161 static int
 162 mlxsw_sp1_ptp_phc_settime(struct mlxsw_sp1_ptp_clock *clock, u64 nsec)
 163 {
 164         struct mlxsw_core *mlxsw_core = clock->common.core;
 165         u64 next_sec, next_sec_in_nsec, cycles;
 166         char mtutc_pl[MLXSW_REG_MTUTC_LEN];
 167         char mtpps_pl[MLXSW_REG_MTPPS_LEN];
 168         int err;
 169
 170         next_sec = div_u64(nsec, NSEC_PER_SEC) + 1;
 171         next_sec_in_nsec = next_sec * NSEC_PER_SEC;
 172
 173         spin_lock_bh(&clock->lock);
 174         cycles = mlxsw_sp1_ptp_ns2cycles(&clock->tc, next_sec_in_nsec);
 175         spin_unlock_bh(&clock->lock);
 176
 177         mlxsw_reg_mtpps_vpin_pack(mtpps_pl, cycles);
 178         err = mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtpps), mtpps_pl);
 179         if (err)
 180                 return err;
 181
 182         mlxsw_reg_mtutc_pack(mtutc_pl,
 183                              MLXSW_REG_MTUTC_OPERATION_SET_TIME_AT_NEXT_SEC,
 184                              0, next_sec, 0, 0);
 185         return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
 186 }
 187
 188 static int mlxsw_sp1_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 189 {
 190         struct mlxsw_sp1_ptp_clock *clock = mlxsw_sp1_ptp_clock(ptp);
 191         int neg_adj = 0;
 192         u32 diff;
 193         u64 adj;
 194         s32 ppb;
 195
 196         ppb = scaled_ppm_to_ppb(scaled_ppm);
 197
 198         if (ppb < 0) {
 199                 neg_adj = 1;
 200                 ppb = -ppb;
 201         }
 202
 203         adj = clock->nominal_c_mult;
 204         adj *= ppb;
 205         diff = div_u64(adj, NSEC_PER_SEC);
 206
 207         spin_lock_bh(&clock->lock);
 208         timecounter_read(&clock->tc);
 209         clock->cycles.mult = neg_adj ? clock->nominal_c_mult - diff :
 210                                        clock->nominal_c_mult + diff;
 211         spin_unlock_bh(&clock->lock);
 212
 213         return mlxsw_sp_ptp_phc_adjfreq(&clock->common, neg_adj ? -ppb : ppb);
 214 }
 215
 216 static int mlxsw_sp1_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 217 {
 218         struct mlxsw_sp1_ptp_clock *clock = mlxsw_sp1_ptp_clock(ptp);
 219         u64 nsec;
 220
 221         spin_lock_bh(&clock->lock);
 222         timecounter_adjtime(&clock->tc, delta);
 223         nsec = timecounter_read(&clock->tc);
 224         spin_unlock_bh(&clock->lock);
 225
 226         return mlxsw_sp1_ptp_phc_settime(clock, nsec);
 227 }
 228
 229 static int mlxsw_sp1_ptp_gettimex(struct ptp_clock_info *ptp,
 230                                   struct timespec64 *ts,
 231                                   struct ptp_system_timestamp *sts)
 232 {
 233         struct mlxsw_sp1_ptp_clock *clock = mlxsw_sp1_ptp_clock(ptp);
 234         u64 cycles, nsec;
 235
 236         spin_lock_bh(&clock->lock);
 237         cycles = __mlxsw_sp1_ptp_read_frc(clock, sts);
 238         nsec = timecounter_cyc2time(&clock->tc, cycles);
 239         spin_unlock_bh(&clock->lock);
 240
 241         *ts = ns_to_timespec64(nsec);
 242
 243         return 0;
 244 }
 245
 246 static int mlxsw_sp1_ptp_settime(struct ptp_clock_info *ptp,
 247                                  const struct timespec64 *ts)
 248 {
 249         struct mlxsw_sp1_ptp_clock *clock = mlxsw_sp1_ptp_clock(ptp);
 250         u64 nsec = timespec64_to_ns(ts);
 251
 252         spin_lock_bh(&clock->lock);
 253         timecounter_init(&clock->tc, &clock->cycles, nsec);
 254         nsec = timecounter_read(&clock->tc);
 255         spin_unlock_bh(&clock->lock);
 256
 257         return mlxsw_sp1_ptp_phc_settime(clock, nsec);
 258 }
 259
 260 static const struct ptp_clock_info mlxsw_sp1_ptp_clock_info = {
 261         .owner          = THIS_MODULE,
 262         .name           = "mlxsw_sp_clock",
 263         .max_adj        = 100000000,
 264         .adjfine        = mlxsw_sp1_ptp_adjfine,
 265         .adjtime        = mlxsw_sp1_ptp_adjtime,
 266         .gettimex64     = mlxsw_sp1_ptp_gettimex,
 267         .settime64      = mlxsw_sp1_ptp_settime,
 268 };
 269
 270 static void mlxsw_sp1_ptp_clock_overflow(struct work_struct *work)
 271 {
 272         struct delayed_work *dwork = to_delayed_work(work);
 273         struct mlxsw_sp1_ptp_clock *clock;
 274
 275         clock = container_of(dwork, struct mlxsw_sp1_ptp_clock, overflow_work);
 276
 277         spin_lock_bh(&clock->lock);
 278         timecounter_read(&clock->tc);
 279         spin_unlock_bh(&clock->lock);
 280         mlxsw_core_schedule_dw(&clock->overflow_work, clock->overflow_period);
 281 }
 282
 283 struct mlxsw_sp_ptp_clock *
 284 mlxsw_sp1_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev)
 285 {
 286         u64 overflow_cycles, nsec, frac = 0;
 287         struct mlxsw_sp1_ptp_clock *clock;
 288         int err;
 289
 290         clock = kzalloc(sizeof(*clock), GFP_KERNEL);
 291         if (!clock)
 292                 return ERR_PTR(-ENOMEM);
 293
 294         spin_lock_init(&clock->lock);
 295         clock->cycles.read = mlxsw_sp1_ptp_read_frc;
 296         clock->cycles.shift = MLXSW_SP1_PTP_CLOCK_CYCLES_SHIFT;
 297         clock->cycles.mult = clocksource_khz2mult(MLXSW_SP1_PTP_CLOCK_FREQ_KHZ,
 298                                                   clock->cycles.shift);
 299         clock->nominal_c_mult = clock->cycles.mult;
 300         clock->cycles.mask = CLOCKSOURCE_MASK(MLXSW_SP1_PTP_CLOCK_MASK);
 301         clock->common.core = mlxsw_sp->core;
 302
 303         timecounter_init(&clock->tc, &clock->cycles, 0);
 304
 305         /* Calculate period in seconds to call the overflow watchdog - to make
 306          * sure counter is checked at least twice every wrap around.
 307          * The period is calculated as the minimum between max HW cycles count
 308          * (The clock source mask) and max amount of cycles that can be
 309          * multiplied by clock multiplier where the result doesn't exceed
 310          * 64bits.
 311          */
 312         overflow_cycles = div64_u64(~0ULL >> 1, clock->cycles.mult);
 313         overflow_cycles = min(overflow_cycles, div_u64(clock->cycles.mask, 3));
 314
 315         nsec = cyclecounter_cyc2ns(&clock->cycles, overflow_cycles, 0, &frac);
 316         clock->overflow_period = nsecs_to_jiffies(nsec);
 317
 318         INIT_DELAYED_WORK(&clock->overflow_work, mlxsw_sp1_ptp_clock_overflow);
 319         mlxsw_core_schedule_dw(&clock->overflow_work, 0);
 320
 321         clock->common.ptp_info = mlxsw_sp1_ptp_clock_info;
 322         clock->common.ptp = ptp_clock_register(&clock->common.ptp_info, dev);
 323         if (IS_ERR(clock->common.ptp)) {
 324                 err = PTR_ERR(clock->common.ptp);
 325                 dev_err(dev, "ptp_clock_register failed %d\n", err);
 326                 goto err_ptp_clock_register;
 327         }
 328
 329         return &clock->common;
 330
 331 err_ptp_clock_register:
 332         cancel_delayed_work_sync(&clock->overflow_work);
 333         kfree(clock);
 334         return ERR_PTR(err);
 335 }
 336
 337 void mlxsw_sp1_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock_common)
 338 {
 339         struct mlxsw_sp1_ptp_clock *clock =
 340                 container_of(clock_common, struct mlxsw_sp1_ptp_clock, common);
 341
 342         ptp_clock_unregister(clock_common->ptp);
 343         cancel_delayed_work_sync(&clock->overflow_work);
 344         kfree(clock);
 345 }
 346
 347 static u64 mlxsw_sp2_ptp_read_utc(struct mlxsw_sp_ptp_clock *clock,
 348                                   struct ptp_system_timestamp *sts)
 349 {
 350         struct mlxsw_core *mlxsw_core = clock->core;
 351         u32 utc_sec1, utc_sec2, utc_nsec;
 352
 353         utc_sec1 = mlxsw_core_read_utc_sec(mlxsw_core);
 354         ptp_read_system_prets(sts);
 355         utc_nsec = mlxsw_core_read_utc_nsec(mlxsw_core);
 356         ptp_read_system_postts(sts);
 357         utc_sec2 = mlxsw_core_read_utc_sec(mlxsw_core);
 358
 359         if (utc_sec1 != utc_sec2) {
 360                 /* Wrap around. */
 361                 ptp_read_system_prets(sts);
 362                 utc_nsec = mlxsw_core_read_utc_nsec(mlxsw_core);
 363                 ptp_read_system_postts(sts);
 364         }
 365
 366         return (u64)utc_sec2 * NSEC_PER_SEC + utc_nsec;
 367 }
 368
 369 static int
 370 mlxsw_sp2_ptp_phc_settime(struct mlxsw_sp_ptp_clock *clock, u64 nsec)
 371 {
 372         struct mlxsw_core *mlxsw_core = clock->core;
 373         char mtutc_pl[MLXSW_REG_MTUTC_LEN];
 374         u32 sec, nsec_rem;
 375
 376         sec = div_u64_rem(nsec, NSEC_PER_SEC, &nsec_rem);
 377         mlxsw_reg_mtutc_pack(mtutc_pl,
 378                              MLXSW_REG_MTUTC_OPERATION_SET_TIME_IMMEDIATE,
 379                              0, sec, nsec_rem, 0);
 380         return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
 381 }
 382
 383 static int mlxsw_sp2_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 384 {
 385         struct mlxsw_sp_ptp_clock *clock =
 386                 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
 387         s32 ppb = scaled_ppm_to_ppb(scaled_ppm);
 388
 389         /* In Spectrum-2 and newer ASICs, the frequency adjustment in MTUTC is
 390          * reversed, positive values mean to decrease the frequency. Adjust the
 391          * sign of PPB to this behavior.
 392          */
 393         return mlxsw_sp_ptp_phc_adjfreq(clock, -ppb);
 394 }
 395
 396 static int mlxsw_sp2_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 397 {
 398         struct mlxsw_sp_ptp_clock *clock =
 399                 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
 400         struct mlxsw_core *mlxsw_core = clock->core;
 401         char mtutc_pl[MLXSW_REG_MTUTC_LEN];
 402
 403         /* HW time adjustment range is s16. If out of range, set time instead. */
 404         if (delta < S16_MIN || delta > S16_MAX) {
 405                 u64 nsec;
 406
 407                 nsec = mlxsw_sp2_ptp_read_utc(clock, NULL);
 408                 nsec += delta;
 409
 410                 return mlxsw_sp2_ptp_phc_settime(clock, nsec);
 411         }
 412
 413         mlxsw_reg_mtutc_pack(mtutc_pl,
 414                              MLXSW_REG_MTUTC_OPERATION_ADJUST_TIME,
 415                              0, 0, 0, delta);
 416         return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
 417 }
 418
 419 static int mlxsw_sp2_ptp_gettimex(struct ptp_clock_info *ptp,
 420                                   struct timespec64 *ts,
 421                                   struct ptp_system_timestamp *sts)
 422 {
 423         struct mlxsw_sp_ptp_clock *clock =
 424                 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
 425         u64 nsec;
 426
 427         nsec = mlxsw_sp2_ptp_read_utc(clock, sts);
 428         *ts = ns_to_timespec64(nsec);
 429
 430         return 0;
 431 }
 432
 433 static int mlxsw_sp2_ptp_settime(struct ptp_clock_info *ptp,
 434                                  const struct timespec64 *ts)
 435 {
 436         struct mlxsw_sp_ptp_clock *clock =
 437                 container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
 438         u64 nsec = timespec64_to_ns(ts);
 439
 440         return mlxsw_sp2_ptp_phc_settime(clock, nsec);
 441 }
 442
 443 static const struct ptp_clock_info mlxsw_sp2_ptp_clock_info = {
 444         .owner          = THIS_MODULE,
 445         .name           = "mlxsw_sp_clock",
 446         .max_adj        = MLXSW_REG_MTUTC_MAX_FREQ_ADJ,
 447         .adjfine        = mlxsw_sp2_ptp_adjfine,
 448         .adjtime        = mlxsw_sp2_ptp_adjtime,
 449         .gettimex64     = mlxsw_sp2_ptp_gettimex,
 450         .settime64      = mlxsw_sp2_ptp_settime,
 451 };
 452
 453 struct mlxsw_sp_ptp_clock *
 454 mlxsw_sp2_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev)
 455 {
 456         struct mlxsw_sp_ptp_clock *clock;
 457         int err;
 458
 459         clock = kzalloc(sizeof(*clock), GFP_KERNEL);
 460         if (!clock)
 461                 return ERR_PTR(-ENOMEM);
 462
 463         clock->core = mlxsw_sp->core;
 464
 465         clock->ptp_info = mlxsw_sp2_ptp_clock_info;
 466
 467         err = mlxsw_sp2_ptp_phc_settime(clock, 0);
 468         if (err) {
 469                 dev_err(dev, "setting UTC time failed %d\n", err);
 470                 goto err_ptp_phc_settime;
 471         }
 472
 473         clock->ptp = ptp_clock_register(&clock->ptp_info, dev);
 474         if (IS_ERR(clock->ptp)) {
 475                 err = PTR_ERR(clock->ptp);
 476                 dev_err(dev, "ptp_clock_register failed %d\n", err);
 477                 goto err_ptp_clock_register;
 478         }
 479
 480         return clock;
 481
 482 err_ptp_clock_register:
 483 err_ptp_phc_settime:
 484         kfree(clock);
 485         return ERR_PTR(err);
 486 }
 487
 488 void mlxsw_sp2_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock)
 489 {
 490         ptp_clock_unregister(clock->ptp);
 491         kfree(clock);
 492 }
 493
 494 static int mlxsw_sp_ptp_parse(struct sk_buff *skb,
 495                               u8 *p_domain_number,
 496                               u8 *p_message_type,
 497                               u16 *p_sequence_id)
 498 {
 499         unsigned int ptp_class;
 500         struct ptp_header *hdr;
 501
 502         ptp_class = ptp_classify_raw(skb);
 503
 504         switch (ptp_class & PTP_CLASS_VMASK) {
 505         case PTP_CLASS_V1:
 506         case PTP_CLASS_V2:
 507                 break;
 508         default:
 509                 return -ERANGE;
 510         }
 511
 512         hdr = ptp_parse_header(skb, ptp_class);
 513         if (!hdr)
 514                 return -EINVAL;
 515
 516         *p_message_type  = ptp_get_msgtype(hdr, ptp_class);
 517         *p_domain_number = hdr->domain_number;
 518         *p_sequence_id   = be16_to_cpu(hdr->sequence_id);
 519
 520         return 0;
 521 }
 522
 523 /* Returns NULL on successful insertion, a pointer on conflict, or an ERR_PTR on
 524  * error.
 525  */
 526 static int
 527 mlxsw_sp1_ptp_unmatched_save(struct mlxsw_sp *mlxsw_sp,
 528                              struct mlxsw_sp1_ptp_key key,
 529                              struct sk_buff *skb,
 530                              u64 timestamp)
 531 {
 532         int cycles = MLXSW_SP1_PTP_HT_GC_TIMEOUT / MLXSW_SP1_PTP_HT_GC_INTERVAL;
 533         struct mlxsw_sp1_ptp_state *ptp_state = mlxsw_sp1_ptp_state(mlxsw_sp);
 534         struct mlxsw_sp1_ptp_unmatched *unmatched;
 535         int err;
 536
 537         unmatched = kzalloc(sizeof(*unmatched), GFP_ATOMIC);
 538         if (!unmatched)
 539                 return -ENOMEM;
 540
 541         unmatched->key = key;
 542         unmatched->skb = skb;
 543         unmatched->timestamp = timestamp;
 544         unmatched->gc_cycle = ptp_state->gc_cycle + cycles;
 545
 546         err = rhltable_insert(&ptp_state->unmatched_ht, &unmatched->ht_node,
 547                               mlxsw_sp1_ptp_unmatched_ht_params);
 548         if (err)
 549                 kfree(unmatched);
 550
 551         return err;
 552 }
 553
 554 static struct mlxsw_sp1_ptp_unmatched *
 555 mlxsw_sp1_ptp_unmatched_lookup(struct mlxsw_sp *mlxsw_sp,
 556                                struct mlxsw_sp1_ptp_key key, int *p_length)
 557 {
 558         struct mlxsw_sp1_ptp_state *ptp_state = mlxsw_sp1_ptp_state(mlxsw_sp);
 559         struct mlxsw_sp1_ptp_unmatched *unmatched, *last = NULL;
 560         struct rhlist_head *tmp, *list;
 561         int length = 0;
 562
 563         list = rhltable_lookup(&ptp_state->unmatched_ht, &key,
 564                                mlxsw_sp1_ptp_unmatched_ht_params);
 565         rhl_for_each_entry_rcu(unmatched, tmp, list, ht_node) {
 566                 last = unmatched;
 567                 length++;
 568         }
 569
 570         *p_length = length;
 571         return last;
 572 }
 573
 574 static int
 575 mlxsw_sp1_ptp_unmatched_remove(struct mlxsw_sp *mlxsw_sp,
 576                                struct mlxsw_sp1_ptp_unmatched *unmatched)
 577 {
 578         struct mlxsw_sp1_ptp_state *ptp_state = mlxsw_sp1_ptp_state(mlxsw_sp);
 579
 580         return rhltable_remove(&ptp_state->unmatched_ht,
 581                                &unmatched->ht_node,
 582                                mlxsw_sp1_ptp_unmatched_ht_params);
 583 }
 584
 585 /* This function is called in the following scenarios:
 586  *
 587  * 1) When a packet is matched with its timestamp.
 588  * 2) In several situation when it is necessary to immediately pass on
 589  *    an SKB without a timestamp.
 590  * 3) From GC indirectly through mlxsw_sp1_ptp_unmatched_finish().
 591  *    This case is similar to 2) above.
 592  */
 593 static void mlxsw_sp1_ptp_packet_finish(struct mlxsw_sp *mlxsw_sp,
 594                                         struct sk_buff *skb, u16 local_port,
 595                                         bool ingress,
 596                                         struct skb_shared_hwtstamps *hwtstamps)
 597 {
 598         struct mlxsw_sp_port *mlxsw_sp_port;
 599
 600         /* Between capturing the packet and finishing it, there is a window of
 601          * opportunity for the originating port to go away (e.g. due to a
 602          * split). Also make sure the SKB device reference is still valid.
 603          */
 604         mlxsw_sp_port = mlxsw_sp->ports[local_port];
 605         if (!(mlxsw_sp_port && (!skb->dev || skb->dev == mlxsw_sp_port->dev))) {
 606                 dev_kfree_skb_any(skb);
 607                 return;
 608         }
 609
 610         if (ingress) {
 611                 if (hwtstamps)
 612                         *skb_hwtstamps(skb) = *hwtstamps;
 613                 mlxsw_sp_rx_listener_no_mark_func(skb, local_port, mlxsw_sp);
 614         } else {
 615                 /* skb_tstamp_tx() allows hwtstamps to be NULL. */
 616                 skb_tstamp_tx(skb, hwtstamps);
 617                 dev_kfree_skb_any(skb);
 618         }
 619 }
 620
 621 static void mlxsw_sp1_packet_timestamp(struct mlxsw_sp *mlxsw_sp,
 622                                        struct mlxsw_sp1_ptp_key key,
 623                                        struct sk_buff *skb,
 624                                        u64 timestamp)
 625 {
 626         struct mlxsw_sp_ptp_clock *clock_common = mlxsw_sp->clock;
 627         struct mlxsw_sp1_ptp_clock *clock =
 628                 container_of(clock_common, struct mlxsw_sp1_ptp_clock, common);
 629
 630         struct skb_shared_hwtstamps hwtstamps;
 631         u64 nsec;
 632
 633         spin_lock_bh(&clock->lock);
 634         nsec = timecounter_cyc2time(&clock->tc, timestamp);
 635         spin_unlock_bh(&clock->lock);
 636
 637         hwtstamps.hwtstamp = ns_to_ktime(nsec);
 638         mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb,
 639                                     key.local_port, key.ingress, &hwtstamps);
 640 }
 641
 642 static void
 643 mlxsw_sp1_ptp_unmatched_finish(struct mlxsw_sp *mlxsw_sp,
 644                                struct mlxsw_sp1_ptp_unmatched *unmatched)
 645 {
 646         if (unmatched->skb && unmatched->timestamp)
 647                 mlxsw_sp1_packet_timestamp(mlxsw_sp, unmatched->key,
 648                                            unmatched->skb,
 649                                            unmatched->timestamp);
 650         else if (unmatched->skb)
 651                 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, unmatched->skb,
 652                                             unmatched->key.local_port,
 653                                             unmatched->key.ingress, NULL);
 654         kfree_rcu(unmatched, rcu);
 655 }
 656
 657 static void mlxsw_sp1_ptp_unmatched_free_fn(void *ptr, void *arg)
 658 {
 659         struct mlxsw_sp1_ptp_unmatched *unmatched = ptr;
 660
 661         /* This is invoked at a point where the ports are gone already. Nothing
 662          * to do with whatever is left in the HT but to free it.
 663          */
 664         if (unmatched->skb)
 665                 dev_kfree_skb_any(unmatched->skb);
 666         kfree_rcu(unmatched, rcu);
 667 }
 668
 669 static void mlxsw_sp1_ptp_got_piece(struct mlxsw_sp *mlxsw_sp,
 670                                     struct mlxsw_sp1_ptp_key key,
 671                                     struct sk_buff *skb, u64 timestamp)
 672 {
 673         struct mlxsw_sp1_ptp_state *ptp_state = mlxsw_sp1_ptp_state(mlxsw_sp);
 674         struct mlxsw_sp1_ptp_unmatched *unmatched;
 675         int length;
 676         int err;
 677
 678         rcu_read_lock();
 679
 680         spin_lock(&ptp_state->unmatched_lock);
 681
 682         unmatched = mlxsw_sp1_ptp_unmatched_lookup(mlxsw_sp, key, &length);
 683         if (skb && unmatched && unmatched->timestamp) {
 684                 unmatched->skb = skb;
 685         } else if (timestamp && unmatched && unmatched->skb) {
 686                 unmatched->timestamp = timestamp;
 687         } else {
 688                 /* Either there is no entry to match, or one that is there is
 689                  * incompatible.
 690                  */
 691                 if (length < 100)
 692                         err = mlxsw_sp1_ptp_unmatched_save(mlxsw_sp, key,
 693                                                            skb, timestamp);
 694                 else
 695                         err = -E2BIG;
 696                 if (err && skb)
 697                         mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb,
 698                                                     key.local_port,
 699                                                     key.ingress, NULL);
 700                 unmatched = NULL;
 701         }
 702
 703         if (unmatched) {
 704                 err = mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp, unmatched);
 705                 WARN_ON_ONCE(err);
 706         }
 707
 708         spin_unlock(&ptp_state->unmatched_lock);
 709
 710         if (unmatched)
 711                 mlxsw_sp1_ptp_unmatched_finish(mlxsw_sp, unmatched);
 712
 713         rcu_read_unlock();
 714 }
 715
 716 static void mlxsw_sp1_ptp_got_packet(struct mlxsw_sp *mlxsw_sp,
 717                                      struct sk_buff *skb, u16 local_port,
 718                                      bool ingress)
 719 {
 720         struct mlxsw_sp_port *mlxsw_sp_port;
 721         struct mlxsw_sp1_ptp_key key;
 722         u8 types;
 723         int err;
 724
 725         mlxsw_sp_port = mlxsw_sp->ports[local_port];
 726         if (!mlxsw_sp_port)
 727                 goto immediate;
 728
 729         types = ingress ? mlxsw_sp_port->ptp.ing_types :
 730                           mlxsw_sp_port->ptp.egr_types;
 731         if (!types)
 732                 goto immediate;
 733
 734         memset(&key, 0, sizeof(key));
 735         key.local_port = local_port;
 736         key.ingress = ingress;
 737
 738         err = mlxsw_sp_ptp_parse(skb, &key.domain_number, &key.message_type,
 739                                  &key.sequence_id);
 740         if (err)
 741                 goto immediate;
 742
 743         /* For packets whose timestamping was not enabled on this port, don't
 744          * bother trying to match the timestamp.
 745          */
 746         if (!((1 << key.message_type) & types))
 747                 goto immediate;
 748
 749         mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, skb, 0);
 750         return;
 751
 752 immediate:
 753         mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, local_port, ingress, NULL);
 754 }
 755
 756 void mlxsw_sp1_ptp_got_timestamp(struct mlxsw_sp *mlxsw_sp, bool ingress,
 757                                  u16 local_port, u8 message_type,
 758                                  u8 domain_number, u16 sequence_id,
 759                                  u64 timestamp)
 760 {
 761         struct mlxsw_sp_port *mlxsw_sp_port;
 762         struct mlxsw_sp1_ptp_key key;
 763         u8 types;
 764
 765         if (WARN_ON_ONCE(!mlxsw_sp_local_port_is_valid(mlxsw_sp, local_port)))
 766                 return;
 767         mlxsw_sp_port = mlxsw_sp->ports[local_port];
 768         if (!mlxsw_sp_port)
 769                 return;
 770
 771         types = ingress ? mlxsw_sp_port->ptp.ing_types :
 772                           mlxsw_sp_port->ptp.egr_types;
 773
 774         /* For message types whose timestamping was not enabled on this port,
 775          * don't bother with the timestamp.
 776          */
 777         if (!((1 << message_type) & types))
 778                 return;
 779
 780         memset(&key, 0, sizeof(key));
 781         key.local_port = local_port;
 782         key.domain_number = domain_number;
 783         key.message_type = message_type;
 784         key.sequence_id = sequence_id;
 785         key.ingress = ingress;
 786
 787         mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, NULL, timestamp);
 788 }
 789
 790 void mlxsw_sp1_ptp_receive(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb,
 791                            u16 local_port)
 792 {
 793         skb_reset_mac_header(skb);
 794         mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, true);
 795 }
 796
 797 void mlxsw_sp1_ptp_transmitted(struct mlxsw_sp *mlxsw_sp,
 798                                struct sk_buff *skb, u16 local_port)
 799 {
 800         mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, false);
 801 }
 802
 803 static void
 804 mlxsw_sp1_ptp_ht_gc_collect(struct mlxsw_sp1_ptp_state *ptp_state,
 805                             struct mlxsw_sp1_ptp_unmatched *unmatched)
 806 {
 807         struct mlxsw_sp *mlxsw_sp = ptp_state->common.mlxsw_sp;
 808         struct mlxsw_sp_ptp_port_dir_stats *stats;
 809         struct mlxsw_sp_port *mlxsw_sp_port;
 810         int err;
 811
 812         /* If an unmatched entry has an SKB, it has to be handed over to the
 813          * networking stack. This is usually done from a trap handler, which is
 814          * invoked in a softirq context. Here we are going to do it in process
 815          * context. If that were to be interrupted by a softirq, it could cause
 816          * a deadlock when an attempt is made to take an already-taken lock
 817          * somewhere along the sending path. Disable softirqs to prevent this.
 818          */
 819         local_bh_disable();
 820
 821         spin_lock(&ptp_state->unmatched_lock);
 822         err = rhltable_remove(&ptp_state->unmatched_ht, &unmatched->ht_node,
 823                               mlxsw_sp1_ptp_unmatched_ht_params);
 824         spin_unlock(&ptp_state->unmatched_lock);
 825
 826         if (err)
 827                 /* The packet was matched with timestamp during the walk. */
 828                 goto out;
 829
 830         mlxsw_sp_port = mlxsw_sp->ports[unmatched->key.local_port];
 831         if (mlxsw_sp_port) {
 832                 stats = unmatched->key.ingress ?
 833                         &mlxsw_sp_port->ptp.stats.rx_gcd :
 834                         &mlxsw_sp_port->ptp.stats.tx_gcd;
 835                 if (unmatched->skb)
 836                         stats->packets++;
 837                 else
 838                         stats->timestamps++;
 839         }
 840
 841         /* mlxsw_sp1_ptp_unmatched_finish() invokes netif_receive_skb(). While
 842          * the comment at that function states that it can only be called in
 843          * soft IRQ context, this pattern of local_bh_disable() +
 844          * netif_receive_skb(), in process context, is seen elsewhere in the
 845          * kernel, notably in pktgen.
 846          */
 847         mlxsw_sp1_ptp_unmatched_finish(mlxsw_sp, unmatched);
 848
 849 out:
 850         local_bh_enable();
 851 }
 852
 853 static void mlxsw_sp1_ptp_ht_gc(struct work_struct *work)
 854 {
 855         struct delayed_work *dwork = to_delayed_work(work);
 856         struct mlxsw_sp1_ptp_unmatched *unmatched;
 857         struct mlxsw_sp1_ptp_state *ptp_state;
 858         struct rhashtable_iter iter;
 859         u32 gc_cycle;
 860         void *obj;
 861
 862         ptp_state = container_of(dwork, struct mlxsw_sp1_ptp_state, ht_gc_dw);
 863         gc_cycle = ptp_state->gc_cycle++;
 864
 865         rhltable_walk_enter(&ptp_state->unmatched_ht, &iter);
 866         rhashtable_walk_start(&iter);
 867         while ((obj = rhashtable_walk_next(&iter))) {
 868                 if (IS_ERR(obj))
 869                         continue;
 870
 871                 unmatched = obj;
 872                 if (unmatched->gc_cycle <= gc_cycle)
 873                         mlxsw_sp1_ptp_ht_gc_collect(ptp_state, unmatched);
 874         }
 875         rhashtable_walk_stop(&iter);
 876         rhashtable_walk_exit(&iter);
 877
 878         mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw,
 879                                MLXSW_SP1_PTP_HT_GC_INTERVAL);
 880 }
 881
 882 static int mlxsw_sp_ptp_mtptpt_set(struct mlxsw_sp *mlxsw_sp,
 883                                    enum mlxsw_reg_mtptpt_trap_id trap_id,
 884                                    u16 message_type)
 885 {
 886         char mtptpt_pl[MLXSW_REG_MTPTPT_LEN];
 887
 888         mlxsw_reg_mtptpt_pack(mtptpt_pl, trap_id, message_type);
 889         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtptpt), mtptpt_pl);
 890 }
 891
 892 static int mlxsw_sp1_ptp_set_fifo_clr_on_trap(struct mlxsw_sp *mlxsw_sp,
 893                                               bool clr)
 894 {
 895         char mogcr_pl[MLXSW_REG_MOGCR_LEN] = {0};
 896         int err;
 897
 898         err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl);
 899         if (err)
 900                 return err;
 901
 902         mlxsw_reg_mogcr_ptp_iftc_set(mogcr_pl, clr);
 903         mlxsw_reg_mogcr_ptp_eftc_set(mogcr_pl, clr);
 904         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl);
 905 }
 906
 907 static int mlxsw_sp1_ptp_mtpppc_set(struct mlxsw_sp *mlxsw_sp,
 908                                     u16 ing_types, u16 egr_types)
 909 {
 910         char mtpppc_pl[MLXSW_REG_MTPPPC_LEN];
 911
 912         mlxsw_reg_mtpppc_pack(mtpppc_pl, ing_types, egr_types);
 913         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtpppc), mtpppc_pl);
 914 }
 915
 916 struct mlxsw_sp1_ptp_shaper_params {
 917         u32 ethtool_speed;
 918         enum mlxsw_reg_qpsc_port_speed port_speed;
 919         u8 shaper_time_exp;
 920         u8 shaper_time_mantissa;
 921         u8 shaper_inc;
 922         u8 shaper_bs;
 923         u8 port_to_shaper_credits;
 924         int ing_timestamp_inc;
 925         int egr_timestamp_inc;
 926 };
 927
 928 static const struct mlxsw_sp1_ptp_shaper_params
 929 mlxsw_sp1_ptp_shaper_params[] = {
 930         {
 931                 .ethtool_speed          = SPEED_100,
 932                 .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_100M,
 933                 .shaper_time_exp        = 4,
 934                 .shaper_time_mantissa   = 12,
 935                 .shaper_inc             = 9,
 936                 .shaper_bs              = 1,
 937                 .port_to_shaper_credits = 1,
 938                 .ing_timestamp_inc      = -313,
 939                 .egr_timestamp_inc      = 313,
 940         },
 941         {
 942                 .ethtool_speed          = SPEED_1000,
 943                 .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_1G,
 944                 .shaper_time_exp        = 0,
 945                 .shaper_time_mantissa   = 12,
 946                 .shaper_inc             = 6,
 947                 .shaper_bs              = 0,
 948                 .port_to_shaper_credits = 1,
 949                 .ing_timestamp_inc      = -35,
 950                 .egr_timestamp_inc      = 35,
 951         },
 952         {
 953                 .ethtool_speed          = SPEED_10000,
 954                 .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_10G,
 955                 .shaper_time_exp        = 0,
 956                 .shaper_time_mantissa   = 2,
 957                 .shaper_inc             = 14,
 958                 .shaper_bs              = 1,
 959                 .port_to_shaper_credits = 1,
 960                 .ing_timestamp_inc      = -11,
 961                 .egr_timestamp_inc      = 11,
 962         },
 963         {
 964                 .ethtool_speed          = SPEED_25000,
 965                 .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_25G,
 966                 .shaper_time_exp        = 0,
 967                 .shaper_time_mantissa   = 0,
 968                 .shaper_inc             = 11,
 969                 .shaper_bs              = 1,
 970                 .port_to_shaper_credits = 1,
 971                 .ing_timestamp_inc      = -14,
 972                 .egr_timestamp_inc      = 14,
 973         },
 974 };
 975
 976 #define MLXSW_SP1_PTP_SHAPER_PARAMS_LEN ARRAY_SIZE(mlxsw_sp1_ptp_shaper_params)
 977
 978 static int mlxsw_sp1_ptp_shaper_params_set(struct mlxsw_sp *mlxsw_sp)
 979 {
 980         const struct mlxsw_sp1_ptp_shaper_params *params;
 981         char qpsc_pl[MLXSW_REG_QPSC_LEN];
 982         int i, err;
 983
 984         for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) {
 985                 params = &mlxsw_sp1_ptp_shaper_params[i];
 986                 mlxsw_reg_qpsc_pack(qpsc_pl, params->port_speed,
 987                                     params->shaper_time_exp,
 988                                     params->shaper_time_mantissa,
 989                                     params->shaper_inc, params->shaper_bs,
 990                                     params->port_to_shaper_credits,
 991                                     params->ing_timestamp_inc,
 992                                     params->egr_timestamp_inc);
 993                 err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpsc), qpsc_pl);
 994                 if (err)
 995                         return err;
 996         }
 997
 998         return 0;
 999 }
1000
1001 static int mlxsw_sp_ptp_traps_set(struct mlxsw_sp *mlxsw_sp)
1002 {
1003         u16 event_message_type;
1004         int err;
1005
1006         /* Deliver these message types as PTP0. */
1007         event_message_type = BIT(PTP_MSGTYPE_SYNC) |
1008                              BIT(PTP_MSGTYPE_DELAY_REQ) |
1009                              BIT(PTP_MSGTYPE_PDELAY_REQ) |
1010                              BIT(PTP_MSGTYPE_PDELAY_RESP);
1011
1012         err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0,
1013                                       event_message_type);
1014         if (err)
1015                 return err;
1016
1017         /* Everything else is PTP1. */
1018         err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1,
1019                                       ~event_message_type);
1020         if (err)
1021                 goto err_mtptpt1_set;
1022
1023         return 0;
1024
1025 err_mtptpt1_set:
1026         mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0);
1027         return err;
1028 }
1029
1030 static void mlxsw_sp_ptp_traps_unset(struct mlxsw_sp *mlxsw_sp)
1031 {
1032         mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0);
1033         mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0);
1034 }
1035
1036 struct mlxsw_sp_ptp_state *mlxsw_sp1_ptp_init(struct mlxsw_sp *mlxsw_sp)
1037 {
1038         struct mlxsw_sp1_ptp_state *ptp_state;
1039         int err;
1040
1041         err = mlxsw_sp1_ptp_shaper_params_set(mlxsw_sp);
1042         if (err)
1043                 return ERR_PTR(err);
1044
1045         ptp_state = kzalloc(sizeof(*ptp_state), GFP_KERNEL);
1046         if (!ptp_state)
1047                 return ERR_PTR(-ENOMEM);
1048         ptp_state->common.mlxsw_sp = mlxsw_sp;
1049
1050         spin_lock_init(&ptp_state->unmatched_lock);
1051
1052         err = rhltable_init(&ptp_state->unmatched_ht,
1053                             &mlxsw_sp1_ptp_unmatched_ht_params);
1054         if (err)
1055                 goto err_hashtable_init;
1056
1057         err = mlxsw_sp_ptp_traps_set(mlxsw_sp);
1058         if (err)
1059                 goto err_ptp_traps_set;
1060
1061         err = mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, true);
1062         if (err)
1063                 goto err_fifo_clr;
1064
1065         INIT_DELAYED_WORK(&ptp_state->ht_gc_dw, mlxsw_sp1_ptp_ht_gc);
1066         mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw,
1067                                MLXSW_SP1_PTP_HT_GC_INTERVAL);
1068         return &ptp_state->common;
1069
1070 err_fifo_clr:
1071         mlxsw_sp_ptp_traps_unset(mlxsw_sp);
1072 err_ptp_traps_set:
1073         rhltable_destroy(&ptp_state->unmatched_ht);
1074 err_hashtable_init:
1075         kfree(ptp_state);
1076         return ERR_PTR(err);
1077 }
1078
1079 void mlxsw_sp1_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state_common)
1080 {
1081         struct mlxsw_sp *mlxsw_sp = ptp_state_common->mlxsw_sp;
1082         struct mlxsw_sp1_ptp_state *ptp_state;
1083
1084         ptp_state = mlxsw_sp1_ptp_state(mlxsw_sp);
1085
1086         cancel_delayed_work_sync(&ptp_state->ht_gc_dw);
1087         mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp, 0, 0);
1088         mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, false);
1089         mlxsw_sp_ptp_traps_unset(mlxsw_sp);
1090         rhltable_free_and_destroy(&ptp_state->unmatched_ht,
1091                                   &mlxsw_sp1_ptp_unmatched_free_fn, NULL);
1092         kfree(ptp_state);
1093 }
1094
1095 int mlxsw_sp1_ptp_hwtstamp_get(struct mlxsw_sp_port *mlxsw_sp_port,
1096                                struct hwtstamp_config *config)
1097 {
1098         *config = mlxsw_sp_port->ptp.hwtstamp_config;
1099         return 0;
1100 }
1101
1102 static int
1103 mlxsw_sp1_ptp_get_message_types(const struct hwtstamp_config *config,
1104                                 u16 *p_ing_types, u16 *p_egr_types,
1105                                 enum hwtstamp_rx_filters *p_rx_filter)
1106 {
1107         enum hwtstamp_rx_filters rx_filter = config->rx_filter;
1108         enum hwtstamp_tx_types tx_type = config->tx_type;
1109         u16 ing_types = 0x00;
1110         u16 egr_types = 0x00;
1111
1112         switch (tx_type) {
1113         case HWTSTAMP_TX_OFF:
1114                 egr_types = 0x00;
1115                 break;
1116         case HWTSTAMP_TX_ON:
1117                 egr_types = 0xff;
1118                 break;
1119         case HWTSTAMP_TX_ONESTEP_SYNC:
1120         case HWTSTAMP_TX_ONESTEP_P2P:
1121                 return -ERANGE;
1122         default:
1123                 return -EINVAL;
1124         }
1125
1126         switch (rx_filter) {
1127         case HWTSTAMP_FILTER_NONE:
1128                 ing_types = 0x00;
1129                 break;
1130         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
1131         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
1132         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
1133         case HWTSTAMP_FILTER_PTP_V2_SYNC:
1134                 ing_types = 0x01;
1135                 break;
1136         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
1137         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
1138         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
1139         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
1140                 ing_types = 0x02;
1141                 break;
1142         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
1143         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
1144         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
1145         case HWTSTAMP_FILTER_PTP_V2_EVENT:
1146                 ing_types = 0x0f;
1147                 break;
1148         case HWTSTAMP_FILTER_ALL:
1149                 ing_types = 0xff;
1150                 break;
1151         case HWTSTAMP_FILTER_SOME:
1152         case HWTSTAMP_FILTER_NTP_ALL:
1153                 return -ERANGE;
1154         default:
1155                 return -EINVAL;
1156         }
1157
1158         *p_ing_types = ing_types;
1159         *p_egr_types = egr_types;
1160         *p_rx_filter = rx_filter;
1161         return 0;
1162 }
1163
1164 static int mlxsw_sp1_ptp_mtpppc_update(struct mlxsw_sp_port *mlxsw_sp_port,
1165                                        u16 ing_types, u16 egr_types)
1166 {
1167         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1168         struct mlxsw_sp_port *tmp;
1169         u16 orig_ing_types = 0;
1170         u16 orig_egr_types = 0;
1171         int err;
1172         int i;
1173
1174         /* MTPPPC configures timestamping globally, not per port. Find the
1175          * configuration that contains all configured timestamping requests.
1176          */
1177         for (i = 1; i < mlxsw_core_max_ports(mlxsw_sp->core); i++) {
1178                 tmp = mlxsw_sp->ports[i];
1179                 if (tmp) {
1180                         orig_ing_types |= tmp->ptp.ing_types;
1181                         orig_egr_types |= tmp->ptp.egr_types;
1182                 }
1183                 if (tmp && tmp != mlxsw_sp_port) {
1184                         ing_types |= tmp->ptp.ing_types;
1185                         egr_types |= tmp->ptp.egr_types;
1186                 }
1187         }
1188
1189         if ((ing_types || egr_types) && !(orig_ing_types || orig_egr_types)) {
1190                 err = mlxsw_sp_parsing_depth_inc(mlxsw_sp);
1191                 if (err) {
1192                         netdev_err(mlxsw_sp_port->dev, "Failed to increase parsing depth");
1193                         return err;
1194                 }
1195         }
1196         if (!(ing_types || egr_types) && (orig_ing_types || orig_egr_types))
1197                 mlxsw_sp_parsing_depth_dec(mlxsw_sp);
1198
1199         return mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp_port->mlxsw_sp,
1200                                        ing_types, egr_types);
1201 }
1202
1203 static bool mlxsw_sp1_ptp_hwtstamp_enabled(struct mlxsw_sp_port *mlxsw_sp_port)
1204 {
1205         return mlxsw_sp_port->ptp.ing_types || mlxsw_sp_port->ptp.egr_types;
1206 }
1207
1208 static int
1209 mlxsw_sp1_ptp_port_shaper_set(struct mlxsw_sp_port *mlxsw_sp_port, bool enable)
1210 {
1211         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1212         char qeec_pl[MLXSW_REG_QEEC_LEN];
1213
1214         mlxsw_reg_qeec_ptps_pack(qeec_pl, mlxsw_sp_port->local_port, enable);
1215         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qeec), qeec_pl);
1216 }
1217
1218 static int mlxsw_sp1_ptp_port_shaper_check(struct mlxsw_sp_port *mlxsw_sp_port)
1219 {
1220         bool ptps = false;
1221         int err, i;
1222         u32 speed;
1223
1224         if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1225                 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, false);
1226
1227         err = mlxsw_sp_port_speed_get(mlxsw_sp_port, &speed);
1228         if (err)
1229                 return err;
1230
1231         for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) {
1232                 if (mlxsw_sp1_ptp_shaper_params[i].ethtool_speed == speed) {
1233                         ptps = true;
1234                         break;
1235                 }
1236         }
1237
1238         return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, ptps);
1239 }
1240
1241 void mlxsw_sp1_ptp_shaper_work(struct work_struct *work)
1242 {
1243         struct delayed_work *dwork = to_delayed_work(work);
1244         struct mlxsw_sp_port *mlxsw_sp_port;
1245         int err;
1246
1247         mlxsw_sp_port = container_of(dwork, struct mlxsw_sp_port,
1248                                      ptp.shaper_dw);
1249
1250         if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1251                 return;
1252
1253         err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1254         if (err)
1255                 netdev_err(mlxsw_sp_port->dev, "Failed to set up PTP shaper\n");
1256 }
1257
1258 int mlxsw_sp1_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port,
1259                                struct hwtstamp_config *config)
1260 {
1261         enum hwtstamp_rx_filters rx_filter;
1262         u16 ing_types;
1263         u16 egr_types;
1264         int err;
1265
1266         err = mlxsw_sp1_ptp_get_message_types(config, &ing_types, &egr_types,
1267                                               &rx_filter);
1268         if (err)
1269                 return err;
1270
1271         err = mlxsw_sp1_ptp_mtpppc_update(mlxsw_sp_port, ing_types, egr_types);
1272         if (err)
1273                 return err;
1274
1275         mlxsw_sp_port->ptp.hwtstamp_config = *config;
1276         mlxsw_sp_port->ptp.ing_types = ing_types;
1277         mlxsw_sp_port->ptp.egr_types = egr_types;
1278
1279         err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1280         if (err)
1281                 return err;
1282
1283         /* Notify the ioctl caller what we are actually timestamping. */
1284         config->rx_filter = rx_filter;
1285
1286         return 0;
1287 }
1288
1289 int mlxsw_sp1_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp,
1290                               struct ethtool_ts_info *info)
1291 {
1292         info->phc_index = ptp_clock_index(mlxsw_sp->clock->ptp);
1293
1294         info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
1295                                 SOF_TIMESTAMPING_RX_HARDWARE |
1296                                 SOF_TIMESTAMPING_RAW_HARDWARE;
1297
1298         info->tx_types = BIT(HWTSTAMP_TX_OFF) |
1299                          BIT(HWTSTAMP_TX_ON);
1300
1301         info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
1302                            BIT(HWTSTAMP_FILTER_ALL);
1303
1304         return 0;
1305 }
1306
1307 struct mlxsw_sp_ptp_port_stat {
1308         char str[ETH_GSTRING_LEN];
1309         ptrdiff_t offset;
1310 };
1311
1312 #define MLXSW_SP_PTP_PORT_STAT(NAME, FIELD)                             \
1313         {                                                               \
1314                 .str = NAME,                                            \
1315                 .offset = offsetof(struct mlxsw_sp_ptp_port_stats,      \
1316                                     FIELD),                             \
1317         }
1318
1319 static const struct mlxsw_sp_ptp_port_stat mlxsw_sp_ptp_port_stats[] = {
1320         MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_packets",    rx_gcd.packets),
1321         MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_timestamps", rx_gcd.timestamps),
1322         MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_packets",    tx_gcd.packets),
1323         MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_timestamps", tx_gcd.timestamps),
1324 };
1325
1326 #undef MLXSW_SP_PTP_PORT_STAT
1327
1328 #define MLXSW_SP_PTP_PORT_STATS_LEN \
1329         ARRAY_SIZE(mlxsw_sp_ptp_port_stats)
1330
1331 int mlxsw_sp1_get_stats_count(void)
1332 {
1333         return MLXSW_SP_PTP_PORT_STATS_LEN;
1334 }
1335
1336 void mlxsw_sp1_get_stats_strings(u8 **p)
1337 {
1338         int i;
1339
1340         for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) {
1341                 memcpy(*p, mlxsw_sp_ptp_port_stats[i].str,
1342                        ETH_GSTRING_LEN);
1343                 *p += ETH_GSTRING_LEN;
1344         }
1345 }
1346
1347 void mlxsw_sp1_get_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1348                          u64 *data, int data_index)
1349 {
1350         void *stats = &mlxsw_sp_port->ptp.stats;
1351         ptrdiff_t offset;
1352         int i;
1353
1354         data += data_index;
1355         for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) {
1356                 offset = mlxsw_sp_ptp_port_stats[i].offset;
1357                 *data++ = *(u64 *)(stats + offset);
1358         }
1359 }
1360
1361 struct mlxsw_sp_ptp_state *mlxsw_sp2_ptp_init(struct mlxsw_sp *mlxsw_sp)
1362 {
1363         struct mlxsw_sp2_ptp_state *ptp_state;
1364         int err;
1365
1366         ptp_state = kzalloc(sizeof(*ptp_state), GFP_KERNEL);
1367         if (!ptp_state)
1368                 return ERR_PTR(-ENOMEM);
1369
1370         ptp_state->common.mlxsw_sp = mlxsw_sp;
1371
1372         err = mlxsw_sp_ptp_traps_set(mlxsw_sp);
1373         if (err)
1374                 goto err_ptp_traps_set;
1375
1376         refcount_set(&ptp_state->ptp_port_enabled_ref, 0);
1377         return &ptp_state->common;
1378
1379 err_ptp_traps_set:
1380         kfree(ptp_state);
1381         return ERR_PTR(err);
1382 }
1383
1384 void mlxsw_sp2_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state_common)
1385 {
1386         struct mlxsw_sp *mlxsw_sp = ptp_state_common->mlxsw_sp;
1387         struct mlxsw_sp2_ptp_state *ptp_state;
1388
1389         ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp);
1390
1391         mlxsw_sp_ptp_traps_unset(mlxsw_sp);
1392         kfree(ptp_state);
1393 }
1394
1395 static u32 mlxsw_ptp_utc_time_stamp_sec_get(struct mlxsw_core *mlxsw_core,
1396                                             u8 cqe_ts_sec)
1397 {
1398         u32 utc_sec = mlxsw_core_read_utc_sec(mlxsw_core);
1399
1400         if (cqe_ts_sec > (utc_sec & 0xff))
1401                 /* Time stamp above the last bits of UTC (UTC & 0xff) means the
1402                  * latter has wrapped after the time stamp was collected.
1403                  */
1404                 utc_sec -= 256;
1405
1406         utc_sec &= ~0xff;
1407         utc_sec |= cqe_ts_sec;
1408
1409         return utc_sec;
1410 }
1411
1412 static void mlxsw_sp2_ptp_hwtstamp_fill(struct mlxsw_core *mlxsw_core,
1413                                         const struct mlxsw_skb_cb *cb,
1414                                         struct skb_shared_hwtstamps *hwtstamps)
1415 {
1416         u64 ts_sec, ts_nsec, nsec;
1417
1418         WARN_ON_ONCE(!cb->cqe_ts.sec && !cb->cqe_ts.nsec);
1419
1420         /* The time stamp in the CQE is represented by 38 bits, which is a short
1421          * representation of UTC time. Software should create the full time
1422          * stamp using the global UTC clock. The seconds have only 8 bits in the
1423          * CQE, to create the full time stamp, use the current UTC time and fix
1424          * the seconds according to the relation between UTC seconds and CQE
1425          * seconds.
1426          */
1427         ts_sec = mlxsw_ptp_utc_time_stamp_sec_get(mlxsw_core, cb->cqe_ts.sec);
1428         ts_nsec = cb->cqe_ts.nsec;
1429
1430         nsec = ts_sec * NSEC_PER_SEC + ts_nsec;
1431
1432         hwtstamps->hwtstamp = ns_to_ktime(nsec);
1433 }
1434
1435 void mlxsw_sp2_ptp_receive(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb,
1436                            u16 local_port)
1437 {
1438         struct skb_shared_hwtstamps hwtstamps;
1439
1440         mlxsw_sp2_ptp_hwtstamp_fill(mlxsw_sp->core, mlxsw_skb_cb(skb),
1441                                     &hwtstamps);
1442         *skb_hwtstamps(skb) = hwtstamps;
1443         mlxsw_sp_rx_listener_no_mark_func(skb, local_port, mlxsw_sp);
1444 }
1445
1446 void mlxsw_sp2_ptp_transmitted(struct mlxsw_sp *mlxsw_sp,
1447                                struct sk_buff *skb, u16 local_port)
1448 {
1449         struct skb_shared_hwtstamps hwtstamps;
1450
1451         mlxsw_sp2_ptp_hwtstamp_fill(mlxsw_sp->core, mlxsw_skb_cb(skb),
1452                                     &hwtstamps);
1453         skb_tstamp_tx(skb, &hwtstamps);
1454         dev_kfree_skb_any(skb);
1455 }
1456
1457 int mlxsw_sp2_ptp_hwtstamp_get(struct mlxsw_sp_port *mlxsw_sp_port,
1458                                struct hwtstamp_config *config)
1459 {
1460         struct mlxsw_sp2_ptp_state *ptp_state;
1461
1462         ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp_port->mlxsw_sp);
1463
1464         *config = ptp_state->config;
1465         return 0;
1466 }
1467
1468 static int
1469 mlxsw_sp2_ptp_get_message_types(const struct hwtstamp_config *config,
1470                                 u16 *p_ing_types, u16 *p_egr_types,
1471                                 enum hwtstamp_rx_filters *p_rx_filter)
1472 {
1473         enum hwtstamp_rx_filters rx_filter = config->rx_filter;
1474         enum hwtstamp_tx_types tx_type = config->tx_type;
1475         u16 ing_types = 0x00;
1476         u16 egr_types = 0x00;
1477
1478         *p_rx_filter = rx_filter;
1479
1480         switch (rx_filter) {
1481         case HWTSTAMP_FILTER_NONE:
1482                 ing_types = 0x00;
1483                 break;
1484         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
1485         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
1486         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
1487         case HWTSTAMP_FILTER_PTP_V2_SYNC:
1488         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
1489         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
1490         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
1491         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
1492         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
1493         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
1494         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
1495         case HWTSTAMP_FILTER_PTP_V2_EVENT:
1496                 /* In Spectrum-2 and above, all packets get time stamp by
1497                  * default and the driver fill the time stamp only for event
1498                  * packets. Return all event types even if only specific types
1499                  * were required.
1500                  */
1501                 ing_types = 0x0f;
1502                 *p_rx_filter = HWTSTAMP_FILTER_SOME;
1503                 break;
1504         case HWTSTAMP_FILTER_ALL:
1505         case HWTSTAMP_FILTER_SOME:
1506         case HWTSTAMP_FILTER_NTP_ALL:
1507                 return -ERANGE;
1508         default:
1509                 return -EINVAL;
1510         }
1511
1512         switch (tx_type) {
1513         case HWTSTAMP_TX_OFF:
1514                 egr_types = 0x00;
1515                 break;
1516         case HWTSTAMP_TX_ON:
1517                 egr_types = 0x0f;
1518                 break;
1519         case HWTSTAMP_TX_ONESTEP_SYNC:
1520         case HWTSTAMP_TX_ONESTEP_P2P:
1521                 return -ERANGE;
1522         default:
1523                 return -EINVAL;
1524         }
1525
1526         *p_ing_types = ing_types;
1527         *p_egr_types = egr_types;
1528         return 0;
1529 }
1530
1531 static int mlxsw_sp2_ptp_mtpcpc_set(struct mlxsw_sp *mlxsw_sp, bool ptp_trap_en,
1532                                     u16 ing_types, u16 egr_types)
1533 {
1534         char mtpcpc_pl[MLXSW_REG_MTPCPC_LEN];
1535
1536         mlxsw_reg_mtpcpc_pack(mtpcpc_pl, false, 0, ptp_trap_en, ing_types,
1537                               egr_types);
1538         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtpcpc), mtpcpc_pl);
1539 }
1540
1541 static int mlxsw_sp2_ptp_enable(struct mlxsw_sp *mlxsw_sp, u16 ing_types,
1542                                 u16 egr_types,
1543                                 struct hwtstamp_config new_config)
1544 {
1545         struct mlxsw_sp2_ptp_state *ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp);
1546         int err;
1547
1548         err = mlxsw_sp2_ptp_mtpcpc_set(mlxsw_sp, true, ing_types, egr_types);
1549         if (err)
1550                 return err;
1551
1552         ptp_state->config = new_config;
1553         return 0;
1554 }
1555
1556 static int mlxsw_sp2_ptp_disable(struct mlxsw_sp *mlxsw_sp,
1557                                  struct hwtstamp_config new_config)
1558 {
1559         struct mlxsw_sp2_ptp_state *ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp);
1560         int err;
1561
1562         err = mlxsw_sp2_ptp_mtpcpc_set(mlxsw_sp, false, 0, 0);
1563         if (err)
1564                 return err;
1565
1566         ptp_state->config = new_config;
1567         return 0;
1568 }
1569
1570 static int mlxsw_sp2_ptp_configure_port(struct mlxsw_sp_port *mlxsw_sp_port,
1571                                         u16 ing_types, u16 egr_types,
1572                                         struct hwtstamp_config new_config)
1573 {
1574         struct mlxsw_sp2_ptp_state *ptp_state;
1575         int err;
1576
1577         ASSERT_RTNL();
1578
1579         ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp_port->mlxsw_sp);
1580
1581         if (refcount_inc_not_zero(&ptp_state->ptp_port_enabled_ref))
1582                 return 0;
1583
1584         err = mlxsw_sp2_ptp_enable(mlxsw_sp_port->mlxsw_sp, ing_types,
1585                                    egr_types, new_config);
1586         if (err)
1587                 return err;
1588
1589         refcount_set(&ptp_state->ptp_port_enabled_ref, 1);
1590
1591         return 0;
1592 }
1593
1594 static int mlxsw_sp2_ptp_deconfigure_port(struct mlxsw_sp_port *mlxsw_sp_port,
1595                                           struct hwtstamp_config new_config)
1596 {
1597         struct mlxsw_sp2_ptp_state *ptp_state;
1598         int err;
1599
1600         ASSERT_RTNL();
1601
1602         ptp_state = mlxsw_sp2_ptp_state(mlxsw_sp_port->mlxsw_sp);
1603
1604         if (!refcount_dec_and_test(&ptp_state->ptp_port_enabled_ref))
1605                 return 0;
1606
1607         err = mlxsw_sp2_ptp_disable(mlxsw_sp_port->mlxsw_sp, new_config);
1608         if (err)
1609                 goto err_ptp_disable;
1610
1611         return 0;
1612
1613 err_ptp_disable:
1614         refcount_set(&ptp_state->ptp_port_enabled_ref, 1);
1615         return err;
1616 }
1617
1618 int mlxsw_sp2_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port,
1619                                struct hwtstamp_config *config)
1620 {
1621         enum hwtstamp_rx_filters rx_filter;
1622         struct hwtstamp_config new_config;
1623         u16 new_ing_types, new_egr_types;
1624         bool ptp_enabled;
1625         int err;
1626
1627         err = mlxsw_sp2_ptp_get_message_types(config, &new_ing_types,
1628                                               &new_egr_types, &rx_filter);
1629         if (err)
1630                 return err;
1631
1632         new_config.flags = config->flags;
1633         new_config.tx_type = config->tx_type;
1634         new_config.rx_filter = rx_filter;
1635
1636         ptp_enabled = mlxsw_sp_port->ptp.ing_types ||
1637                       mlxsw_sp_port->ptp.egr_types;
1638
1639         if ((new_ing_types || new_egr_types) && !ptp_enabled) {
1640                 err = mlxsw_sp2_ptp_configure_port(mlxsw_sp_port, new_ing_types,
1641                                                    new_egr_types, new_config);
1642                 if (err)
1643                         return err;
1644         } else if (!new_ing_types && !new_egr_types && ptp_enabled) {
1645                 err = mlxsw_sp2_ptp_deconfigure_port(mlxsw_sp_port, new_config);
1646                 if (err)
1647                         return err;
1648         }
1649
1650         mlxsw_sp_port->ptp.ing_types = new_ing_types;
1651         mlxsw_sp_port->ptp.egr_types = new_egr_types;
1652
1653         /* Notify the ioctl caller what we are actually timestamping. */
1654         config->rx_filter = rx_filter;
1655
1656         return 0;
1657 }
1658
1659 int mlxsw_sp2_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp,
1660                               struct ethtool_ts_info *info)
1661 {
1662         info->phc_index = ptp_clock_index(mlxsw_sp->clock->ptp);
1663
1664         info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
1665                                 SOF_TIMESTAMPING_RX_HARDWARE |
1666                                 SOF_TIMESTAMPING_RAW_HARDWARE;
1667
1668         info->tx_types = BIT(HWTSTAMP_TX_OFF) |
1669                          BIT(HWTSTAMP_TX_ON);
1670
1671         info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
1672                            BIT(HWTSTAMP_FILTER_PTP_V1_L4_EVENT) |
1673                            BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);
1674
1675         return 0;
1676 }
1677
1678 int mlxsw_sp_ptp_txhdr_construct(struct mlxsw_core *mlxsw_core,
1679                                  struct mlxsw_sp_port *mlxsw_sp_port,
1680                                  struct sk_buff *skb,
1681                                  const struct mlxsw_tx_info *tx_info)
1682 {
1683         mlxsw_sp_txhdr_construct(skb, tx_info);
1684         return 0;
1685 }
1686
1687 int mlxsw_sp2_ptp_txhdr_construct(struct mlxsw_core *mlxsw_core,
1688                                   struct mlxsw_sp_port *mlxsw_sp_port,
1689                                   struct sk_buff *skb,
1690                                   const struct mlxsw_tx_info *tx_info)
1691 {
1692         /* In Spectrum-2 and Spectrum-3, in order for PTP event packets to have
1693          * their correction field correctly set on the egress port they must be
1694          * transmitted as data packets. Such packets ingress the ASIC via the
1695          * CPU port and must have a VLAN tag, as the CPU port is not configured
1696          * with a PVID. Push the default VLAN (4095), which is configured as
1697          * egress untagged on all the ports.
1698          */
1699         if (!skb_vlan_tagged(skb)) {
1700                 skb = vlan_insert_tag_set_proto(skb, htons(ETH_P_8021Q),
1701                                                 MLXSW_SP_DEFAULT_VID);
1702                 if (!skb) {
1703                         this_cpu_inc(mlxsw_sp_port->pcpu_stats->tx_dropped);
1704                         return -ENOMEM;
1705                 }
1706         }
1707
1708         return mlxsw_sp_txhdr_ptp_data_construct(mlxsw_core, mlxsw_sp_port, skb,
1709                                                  tx_info);
1710 }