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
  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
  30 struct 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
  38 struct mlxsw_sp1_ptp_key {
  39         u8 local_port;
  40         u8 message_type;
  41         u16 sequence_id;
  42         u8 domain_number;
  43         bool ingress;
  44 };
  45
  46 struct 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
  55 static 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
  61 struct 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
  73 static 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
  95 static 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
 103 static int
 104 mlxsw_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
 114 static 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
 124 static int
 125 mlxsw_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
 151 static 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
 180 static 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
 194 static 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
 212 static 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
 227 static 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
 237 static 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
 250 struct mlxsw_sp_ptp_clock *
 251 mlxsw_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
 299 err_ptp_clock_register:
 300         cancel_delayed_work_sync(&clock->overflow_work);
 301         kfree(clock);
 302         return ERR_PTR(err);
 303 }
 304
 305 void 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
 312 static 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 offset = 0;
 318         unsigned int ptp_class;
 319         u8 *data;
 320
 321         data = skb_mac_header(skb);
 322         ptp_class = ptp_classify_raw(skb);
 323
 324         switch (ptp_class & PTP_CLASS_VMASK) {
 325         case PTP_CLASS_V1:
 326         case PTP_CLASS_V2:
 327                 break;
 328         default:
 329                 return -ERANGE;
 330         }
 331
 332         if (ptp_class & PTP_CLASS_VLAN)
 333                 offset += VLAN_HLEN;
 334
 335         switch (ptp_class & PTP_CLASS_PMASK) {
 336         case PTP_CLASS_IPV4:
 337                 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
 338                 break;
 339         case PTP_CLASS_IPV6:
 340                 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
 341                 break;
 342         case PTP_CLASS_L2:
 343                 offset += ETH_HLEN;
 344                 break;
 345         default:
 346                 return -ERANGE;
 347         }
 348
 349         /* PTP header is 34 bytes. */
 350         if (skb->len < offset + 34)
 351                 return -EINVAL;
 352
 353         *p_message_type = data[offset] & 0x0f;
 354         *p_domain_number = data[offset + 4];
 355         *p_sequence_id = (u16)(data[offset + 30]) << 8 | data[offset + 31];
 356         return 0;
 357 }
 358
 359 /* Returns NULL on successful insertion, a pointer on conflict, or an ERR_PTR on
 360  * error.
 361  */
 362 static int
 363 mlxsw_sp1_ptp_unmatched_save(struct mlxsw_sp *mlxsw_sp,
 364                              struct mlxsw_sp1_ptp_key key,
 365                              struct sk_buff *skb,
 366                              u64 timestamp)
 367 {
 368         int cycles = MLXSW_SP1_PTP_HT_GC_TIMEOUT / MLXSW_SP1_PTP_HT_GC_INTERVAL;
 369         struct mlxsw_sp_ptp_state *ptp_state = mlxsw_sp->ptp_state;
 370         struct mlxsw_sp1_ptp_unmatched *unmatched;
 371         int err;
 372
 373         unmatched = kzalloc(sizeof(*unmatched), GFP_ATOMIC);
 374         if (!unmatched)
 375                 return -ENOMEM;
 376
 377         unmatched->key = key;
 378         unmatched->skb = skb;
 379         unmatched->timestamp = timestamp;
 380         unmatched->gc_cycle = mlxsw_sp->ptp_state->gc_cycle + cycles;
 381
 382         err = rhltable_insert(&ptp_state->unmatched_ht, &unmatched->ht_node,
 383                               mlxsw_sp1_ptp_unmatched_ht_params);
 384         if (err)
 385                 kfree(unmatched);
 386
 387         return err;
 388 }
 389
 390 static struct mlxsw_sp1_ptp_unmatched *
 391 mlxsw_sp1_ptp_unmatched_lookup(struct mlxsw_sp *mlxsw_sp,
 392                                struct mlxsw_sp1_ptp_key key, int *p_length)
 393 {
 394         struct mlxsw_sp1_ptp_unmatched *unmatched, *last = NULL;
 395         struct rhlist_head *tmp, *list;
 396         int length = 0;
 397
 398         list = rhltable_lookup(&mlxsw_sp->ptp_state->unmatched_ht, &key,
 399                                mlxsw_sp1_ptp_unmatched_ht_params);
 400         rhl_for_each_entry_rcu(unmatched, tmp, list, ht_node) {
 401                 last = unmatched;
 402                 length++;
 403         }
 404
 405         *p_length = length;
 406         return last;
 407 }
 408
 409 static int
 410 mlxsw_sp1_ptp_unmatched_remove(struct mlxsw_sp *mlxsw_sp,
 411                                struct mlxsw_sp1_ptp_unmatched *unmatched)
 412 {
 413         return rhltable_remove(&mlxsw_sp->ptp_state->unmatched_ht,
 414                                &unmatched->ht_node,
 415                                mlxsw_sp1_ptp_unmatched_ht_params);
 416 }
 417
 418 /* This function is called in the following scenarios:
 419  *
 420  * 1) When a packet is matched with its timestamp.
 421  * 2) In several situation when it is necessary to immediately pass on
 422  *    an SKB without a timestamp.
 423  * 3) From GC indirectly through mlxsw_sp1_ptp_unmatched_finish().
 424  *    This case is similar to 2) above.
 425  */
 426 static void mlxsw_sp1_ptp_packet_finish(struct mlxsw_sp *mlxsw_sp,
 427                                         struct sk_buff *skb, u8 local_port,
 428                                         bool ingress,
 429                                         struct skb_shared_hwtstamps *hwtstamps)
 430 {
 431         struct mlxsw_sp_port *mlxsw_sp_port;
 432
 433         /* Between capturing the packet and finishing it, there is a window of
 434          * opportunity for the originating port to go away (e.g. due to a
 435          * split). Also make sure the SKB device reference is still valid.
 436          */
 437         mlxsw_sp_port = mlxsw_sp->ports[local_port];
 438         if (!(mlxsw_sp_port && (!skb->dev || skb->dev == mlxsw_sp_port->dev))) {
 439                 dev_kfree_skb_any(skb);
 440                 return;
 441         }
 442
 443         if (ingress) {
 444                 if (hwtstamps)
 445                         *skb_hwtstamps(skb) = *hwtstamps;
 446                 mlxsw_sp_rx_listener_no_mark_func(skb, local_port, mlxsw_sp);
 447         } else {
 448                 /* skb_tstamp_tx() allows hwtstamps to be NULL. */
 449                 skb_tstamp_tx(skb, hwtstamps);
 450                 dev_kfree_skb_any(skb);
 451         }
 452 }
 453
 454 static void mlxsw_sp1_packet_timestamp(struct mlxsw_sp *mlxsw_sp,
 455                                        struct mlxsw_sp1_ptp_key key,
 456                                        struct sk_buff *skb,
 457                                        u64 timestamp)
 458 {
 459         struct skb_shared_hwtstamps hwtstamps;
 460         u64 nsec;
 461
 462         spin_lock_bh(&mlxsw_sp->clock->lock);
 463         nsec = timecounter_cyc2time(&mlxsw_sp->clock->tc, timestamp);
 464         spin_unlock_bh(&mlxsw_sp->clock->lock);
 465
 466         hwtstamps.hwtstamp = ns_to_ktime(nsec);
 467         mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb,
 468                                     key.local_port, key.ingress, &hwtstamps);
 469 }
 470
 471 static void
 472 mlxsw_sp1_ptp_unmatched_finish(struct mlxsw_sp *mlxsw_sp,
 473                                struct mlxsw_sp1_ptp_unmatched *unmatched)
 474 {
 475         if (unmatched->skb && unmatched->timestamp)
 476                 mlxsw_sp1_packet_timestamp(mlxsw_sp, unmatched->key,
 477                                            unmatched->skb,
 478                                            unmatched->timestamp);
 479         else if (unmatched->skb)
 480                 mlxsw_sp1_ptp_packet_finish(mlxsw_sp, unmatched->skb,
 481                                             unmatched->key.local_port,
 482                                             unmatched->key.ingress, NULL);
 483         kfree_rcu(unmatched, rcu);
 484 }
 485
 486 static void mlxsw_sp1_ptp_unmatched_free_fn(void *ptr, void *arg)
 487 {
 488         struct mlxsw_sp1_ptp_unmatched *unmatched = ptr;
 489
 490         /* This is invoked at a point where the ports are gone already. Nothing
 491          * to do with whatever is left in the HT but to free it.
 492          */
 493         if (unmatched->skb)
 494                 dev_kfree_skb_any(unmatched->skb);
 495         kfree_rcu(unmatched, rcu);
 496 }
 497
 498 static void mlxsw_sp1_ptp_got_piece(struct mlxsw_sp *mlxsw_sp,
 499                                     struct mlxsw_sp1_ptp_key key,
 500                                     struct sk_buff *skb, u64 timestamp)
 501 {
 502         struct mlxsw_sp1_ptp_unmatched *unmatched;
 503         int length;
 504         int err;
 505
 506         rcu_read_lock();
 507
 508         spin_lock(&mlxsw_sp->ptp_state->unmatched_lock);
 509
 510         unmatched = mlxsw_sp1_ptp_unmatched_lookup(mlxsw_sp, key, &length);
 511         if (skb && unmatched && unmatched->timestamp) {
 512                 unmatched->skb = skb;
 513         } else if (timestamp && unmatched && unmatched->skb) {
 514                 unmatched->timestamp = timestamp;
 515         } else {
 516                 /* Either there is no entry to match, or one that is there is
 517                  * incompatible.
 518                  */
 519                 if (length < 100)
 520                         err = mlxsw_sp1_ptp_unmatched_save(mlxsw_sp, key,
 521                                                            skb, timestamp);
 522                 else
 523                         err = -E2BIG;
 524                 if (err && skb)
 525                         mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb,
 526                                                     key.local_port,
 527                                                     key.ingress, NULL);
 528                 unmatched = NULL;
 529         }
 530
 531         if (unmatched) {
 532                 err = mlxsw_sp1_ptp_unmatched_remove(mlxsw_sp, unmatched);
 533                 WARN_ON_ONCE(err);
 534         }
 535
 536         spin_unlock(&mlxsw_sp->ptp_state->unmatched_lock);
 537
 538         if (unmatched)
 539                 mlxsw_sp1_ptp_unmatched_finish(mlxsw_sp, unmatched);
 540
 541         rcu_read_unlock();
 542 }
 543
 544 static void mlxsw_sp1_ptp_got_packet(struct mlxsw_sp *mlxsw_sp,
 545                                      struct sk_buff *skb, u8 local_port,
 546                                      bool ingress)
 547 {
 548         struct mlxsw_sp_port *mlxsw_sp_port;
 549         struct mlxsw_sp1_ptp_key key;
 550         u8 types;
 551         int err;
 552
 553         mlxsw_sp_port = mlxsw_sp->ports[local_port];
 554         if (!mlxsw_sp_port)
 555                 goto immediate;
 556
 557         types = ingress ? mlxsw_sp_port->ptp.ing_types :
 558                           mlxsw_sp_port->ptp.egr_types;
 559         if (!types)
 560                 goto immediate;
 561
 562         memset(&key, 0, sizeof(key));
 563         key.local_port = local_port;
 564         key.ingress = ingress;
 565
 566         err = mlxsw_sp_ptp_parse(skb, &key.domain_number, &key.message_type,
 567                                  &key.sequence_id);
 568         if (err)
 569                 goto immediate;
 570
 571         /* For packets whose timestamping was not enabled on this port, don't
 572          * bother trying to match the timestamp.
 573          */
 574         if (!((1 << key.message_type) & types))
 575                 goto immediate;
 576
 577         mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, skb, 0);
 578         return;
 579
 580 immediate:
 581         mlxsw_sp1_ptp_packet_finish(mlxsw_sp, skb, local_port, ingress, NULL);
 582 }
 583
 584 void mlxsw_sp1_ptp_got_timestamp(struct mlxsw_sp *mlxsw_sp, bool ingress,
 585                                  u8 local_port, u8 message_type,
 586                                  u8 domain_number, u16 sequence_id,
 587                                  u64 timestamp)
 588 {
 589         struct mlxsw_sp_port *mlxsw_sp_port;
 590         struct mlxsw_sp1_ptp_key key;
 591         u8 types;
 592
 593         mlxsw_sp_port = mlxsw_sp->ports[local_port];
 594         if (!mlxsw_sp_port)
 595                 return;
 596
 597         types = ingress ? mlxsw_sp_port->ptp.ing_types :
 598                           mlxsw_sp_port->ptp.egr_types;
 599
 600         /* For message types whose timestamping was not enabled on this port,
 601          * don't bother with the timestamp.
 602          */
 603         if (!((1 << message_type) & types))
 604                 return;
 605
 606         memset(&key, 0, sizeof(key));
 607         key.local_port = local_port;
 608         key.domain_number = domain_number;
 609         key.message_type = message_type;
 610         key.sequence_id = sequence_id;
 611         key.ingress = ingress;
 612
 613         mlxsw_sp1_ptp_got_piece(mlxsw_sp, key, NULL, timestamp);
 614 }
 615
 616 void mlxsw_sp1_ptp_receive(struct mlxsw_sp *mlxsw_sp, struct sk_buff *skb,
 617                            u8 local_port)
 618 {
 619         skb_reset_mac_header(skb);
 620         mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, true);
 621 }
 622
 623 void mlxsw_sp1_ptp_transmitted(struct mlxsw_sp *mlxsw_sp,
 624                                struct sk_buff *skb, u8 local_port)
 625 {
 626         mlxsw_sp1_ptp_got_packet(mlxsw_sp, skb, local_port, false);
 627 }
 628
 629 static void
 630 mlxsw_sp1_ptp_ht_gc_collect(struct mlxsw_sp_ptp_state *ptp_state,
 631                             struct mlxsw_sp1_ptp_unmatched *unmatched)
 632 {
 633         struct mlxsw_sp_ptp_port_dir_stats *stats;
 634         struct mlxsw_sp_port *mlxsw_sp_port;
 635         int err;
 636
 637         /* If an unmatched entry has an SKB, it has to be handed over to the
 638          * networking stack. This is usually done from a trap handler, which is
 639          * invoked in a softirq context. Here we are going to do it in process
 640          * context. If that were to be interrupted by a softirq, it could cause
 641          * a deadlock when an attempt is made to take an already-taken lock
 642          * somewhere along the sending path. Disable softirqs to prevent this.
 643          */
 644         local_bh_disable();
 645
 646         spin_lock(&ptp_state->unmatched_lock);
 647         err = rhltable_remove(&ptp_state->unmatched_ht, &unmatched->ht_node,
 648                               mlxsw_sp1_ptp_unmatched_ht_params);
 649         spin_unlock(&ptp_state->unmatched_lock);
 650
 651         if (err)
 652                 /* The packet was matched with timestamp during the walk. */
 653                 goto out;
 654
 655         mlxsw_sp_port = ptp_state->mlxsw_sp->ports[unmatched->key.local_port];
 656         if (mlxsw_sp_port) {
 657                 stats = unmatched->key.ingress ?
 658                         &mlxsw_sp_port->ptp.stats.rx_gcd :
 659                         &mlxsw_sp_port->ptp.stats.tx_gcd;
 660                 if (unmatched->skb)
 661                         stats->packets++;
 662                 else
 663                         stats->timestamps++;
 664         }
 665
 666         /* mlxsw_sp1_ptp_unmatched_finish() invokes netif_receive_skb(). While
 667          * the comment at that function states that it can only be called in
 668          * soft IRQ context, this pattern of local_bh_disable() +
 669          * netif_receive_skb(), in process context, is seen elsewhere in the
 670          * kernel, notably in pktgen.
 671          */
 672         mlxsw_sp1_ptp_unmatched_finish(ptp_state->mlxsw_sp, unmatched);
 673
 674 out:
 675         local_bh_enable();
 676 }
 677
 678 static void mlxsw_sp1_ptp_ht_gc(struct work_struct *work)
 679 {
 680         struct delayed_work *dwork = to_delayed_work(work);
 681         struct mlxsw_sp1_ptp_unmatched *unmatched;
 682         struct mlxsw_sp_ptp_state *ptp_state;
 683         struct rhashtable_iter iter;
 684         u32 gc_cycle;
 685         void *obj;
 686
 687         ptp_state = container_of(dwork, struct mlxsw_sp_ptp_state, ht_gc_dw);
 688         gc_cycle = ptp_state->gc_cycle++;
 689
 690         rhltable_walk_enter(&ptp_state->unmatched_ht, &iter);
 691         rhashtable_walk_start(&iter);
 692         while ((obj = rhashtable_walk_next(&iter))) {
 693                 if (IS_ERR(obj))
 694                         continue;
 695
 696                 unmatched = obj;
 697                 if (unmatched->gc_cycle <= gc_cycle)
 698                         mlxsw_sp1_ptp_ht_gc_collect(ptp_state, unmatched);
 699         }
 700         rhashtable_walk_stop(&iter);
 701         rhashtable_walk_exit(&iter);
 702
 703         mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw,
 704                                MLXSW_SP1_PTP_HT_GC_INTERVAL);
 705 }
 706
 707 static int mlxsw_sp_ptp_mtptpt_set(struct mlxsw_sp *mlxsw_sp,
 708                                    enum mlxsw_reg_mtptpt_trap_id trap_id,
 709                                    u16 message_type)
 710 {
 711         char mtptpt_pl[MLXSW_REG_MTPTPT_LEN];
 712
 713         mlxsw_reg_mtptptp_pack(mtptpt_pl, trap_id, message_type);
 714         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtptpt), mtptpt_pl);
 715 }
 716
 717 static int mlxsw_sp1_ptp_set_fifo_clr_on_trap(struct mlxsw_sp *mlxsw_sp,
 718                                               bool clr)
 719 {
 720         char mogcr_pl[MLXSW_REG_MOGCR_LEN] = {0};
 721         int err;
 722
 723         err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl);
 724         if (err)
 725                 return err;
 726
 727         mlxsw_reg_mogcr_ptp_iftc_set(mogcr_pl, clr);
 728         mlxsw_reg_mogcr_ptp_eftc_set(mogcr_pl, clr);
 729         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mogcr), mogcr_pl);
 730 }
 731
 732 static int mlxsw_sp1_ptp_mtpppc_set(struct mlxsw_sp *mlxsw_sp,
 733                                     u16 ing_types, u16 egr_types)
 734 {
 735         char mtpppc_pl[MLXSW_REG_MTPPPC_LEN];
 736
 737         mlxsw_reg_mtpppc_pack(mtpppc_pl, ing_types, egr_types);
 738         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(mtpppc), mtpppc_pl);
 739 }
 740
 741 struct mlxsw_sp1_ptp_shaper_params {
 742         u32 ethtool_speed;
 743         enum mlxsw_reg_qpsc_port_speed port_speed;
 744         u8 shaper_time_exp;
 745         u8 shaper_time_mantissa;
 746         u8 shaper_inc;
 747         u8 shaper_bs;
 748         u8 port_to_shaper_credits;
 749         int ing_timestamp_inc;
 750         int egr_timestamp_inc;
 751 };
 752
 753 static const struct mlxsw_sp1_ptp_shaper_params
 754 mlxsw_sp1_ptp_shaper_params[] = {
 755         {
 756                 .ethtool_speed          = SPEED_100,
 757                 .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_100M,
 758                 .shaper_time_exp        = 4,
 759                 .shaper_time_mantissa   = 12,
 760                 .shaper_inc             = 9,
 761                 .shaper_bs              = 1,
 762                 .port_to_shaper_credits = 1,
 763                 .ing_timestamp_inc      = -313,
 764                 .egr_timestamp_inc      = 313,
 765         },
 766         {
 767                 .ethtool_speed          = SPEED_1000,
 768                 .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_1G,
 769                 .shaper_time_exp        = 0,
 770                 .shaper_time_mantissa   = 12,
 771                 .shaper_inc             = 6,
 772                 .shaper_bs              = 0,
 773                 .port_to_shaper_credits = 1,
 774                 .ing_timestamp_inc      = -35,
 775                 .egr_timestamp_inc      = 35,
 776         },
 777         {
 778                 .ethtool_speed          = SPEED_10000,
 779                 .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_10G,
 780                 .shaper_time_exp        = 0,
 781                 .shaper_time_mantissa   = 2,
 782                 .shaper_inc             = 14,
 783                 .shaper_bs              = 1,
 784                 .port_to_shaper_credits = 1,
 785                 .ing_timestamp_inc      = -11,
 786                 .egr_timestamp_inc      = 11,
 787         },
 788         {
 789                 .ethtool_speed          = SPEED_25000,
 790                 .port_speed             = MLXSW_REG_QPSC_PORT_SPEED_25G,
 791                 .shaper_time_exp        = 0,
 792                 .shaper_time_mantissa   = 0,
 793                 .shaper_inc             = 11,
 794                 .shaper_bs              = 1,
 795                 .port_to_shaper_credits = 1,
 796                 .ing_timestamp_inc      = -14,
 797                 .egr_timestamp_inc      = 14,
 798         },
 799 };
 800
 801 #define MLXSW_SP1_PTP_SHAPER_PARAMS_LEN ARRAY_SIZE(mlxsw_sp1_ptp_shaper_params)
 802
 803 static int mlxsw_sp1_ptp_shaper_params_set(struct mlxsw_sp *mlxsw_sp)
 804 {
 805         const struct mlxsw_sp1_ptp_shaper_params *params;
 806         char qpsc_pl[MLXSW_REG_QPSC_LEN];
 807         int i, err;
 808
 809         for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) {
 810                 params = &mlxsw_sp1_ptp_shaper_params[i];
 811                 mlxsw_reg_qpsc_pack(qpsc_pl, params->port_speed,
 812                                     params->shaper_time_exp,
 813                                     params->shaper_time_mantissa,
 814                                     params->shaper_inc, params->shaper_bs,
 815                                     params->port_to_shaper_credits,
 816                                     params->ing_timestamp_inc,
 817                                     params->egr_timestamp_inc);
 818                 err = mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qpsc), qpsc_pl);
 819                 if (err)
 820                         return err;
 821         }
 822
 823         return 0;
 824 }
 825
 826 struct mlxsw_sp_ptp_state *mlxsw_sp1_ptp_init(struct mlxsw_sp *mlxsw_sp)
 827 {
 828         struct mlxsw_sp_ptp_state *ptp_state;
 829         u16 message_type;
 830         int err;
 831
 832         err = mlxsw_sp1_ptp_shaper_params_set(mlxsw_sp);
 833         if (err)
 834                 return ERR_PTR(err);
 835
 836         ptp_state = kzalloc(sizeof(*ptp_state), GFP_KERNEL);
 837         if (!ptp_state)
 838                 return ERR_PTR(-ENOMEM);
 839         ptp_state->mlxsw_sp = mlxsw_sp;
 840
 841         spin_lock_init(&ptp_state->unmatched_lock);
 842
 843         err = rhltable_init(&ptp_state->unmatched_ht,
 844                             &mlxsw_sp1_ptp_unmatched_ht_params);
 845         if (err)
 846                 goto err_hashtable_init;
 847
 848         /* Delive these message types as PTP0. */
 849         message_type = BIT(MLXSW_SP_PTP_MESSAGE_TYPE_SYNC) |
 850                        BIT(MLXSW_SP_PTP_MESSAGE_TYPE_DELAY_REQ) |
 851                        BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_REQ) |
 852                        BIT(MLXSW_SP_PTP_MESSAGE_TYPE_PDELAY_RESP);
 853         err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0,
 854                                       message_type);
 855         if (err)
 856                 goto err_mtptpt_set;
 857
 858         /* Everything else is PTP1. */
 859         message_type = ~message_type;
 860         err = mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1,
 861                                       message_type);
 862         if (err)
 863                 goto err_mtptpt1_set;
 864
 865         err = mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, true);
 866         if (err)
 867                 goto err_fifo_clr;
 868
 869         INIT_DELAYED_WORK(&ptp_state->ht_gc_dw, mlxsw_sp1_ptp_ht_gc);
 870         mlxsw_core_schedule_dw(&ptp_state->ht_gc_dw,
 871                                MLXSW_SP1_PTP_HT_GC_INTERVAL);
 872         return ptp_state;
 873
 874 err_fifo_clr:
 875         mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0);
 876 err_mtptpt1_set:
 877         mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0);
 878 err_mtptpt_set:
 879         rhltable_destroy(&ptp_state->unmatched_ht);
 880 err_hashtable_init:
 881         kfree(ptp_state);
 882         return ERR_PTR(err);
 883 }
 884
 885 void mlxsw_sp1_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state)
 886 {
 887         struct mlxsw_sp *mlxsw_sp = ptp_state->mlxsw_sp;
 888
 889         cancel_delayed_work_sync(&ptp_state->ht_gc_dw);
 890         mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp, 0, 0);
 891         mlxsw_sp1_ptp_set_fifo_clr_on_trap(mlxsw_sp, false);
 892         mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP1, 0);
 893         mlxsw_sp_ptp_mtptpt_set(mlxsw_sp, MLXSW_REG_MTPTPT_TRAP_ID_PTP0, 0);
 894         rhltable_free_and_destroy(&ptp_state->unmatched_ht,
 895                                   &mlxsw_sp1_ptp_unmatched_free_fn, NULL);
 896         kfree(ptp_state);
 897 }
 898
 899 int mlxsw_sp1_ptp_hwtstamp_get(struct mlxsw_sp_port *mlxsw_sp_port,
 900                                struct hwtstamp_config *config)
 901 {
 902         *config = mlxsw_sp_port->ptp.hwtstamp_config;
 903         return 0;
 904 }
 905
 906 static int mlxsw_sp_ptp_get_message_types(const struct hwtstamp_config *config,
 907                                           u16 *p_ing_types, u16 *p_egr_types,
 908                                           enum hwtstamp_rx_filters *p_rx_filter)
 909 {
 910         enum hwtstamp_rx_filters rx_filter = config->rx_filter;
 911         enum hwtstamp_tx_types tx_type = config->tx_type;
 912         u16 ing_types = 0x00;
 913         u16 egr_types = 0x00;
 914
 915         switch (tx_type) {
 916         case HWTSTAMP_TX_OFF:
 917                 egr_types = 0x00;
 918                 break;
 919         case HWTSTAMP_TX_ON:
 920                 egr_types = 0xff;
 921                 break;
 922         case HWTSTAMP_TX_ONESTEP_SYNC:
 923                 return -ERANGE;
 924         }
 925
 926         switch (rx_filter) {
 927         case HWTSTAMP_FILTER_NONE:
 928                 ing_types = 0x00;
 929                 break;
 930         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 931         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 932         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
 933         case HWTSTAMP_FILTER_PTP_V2_SYNC:
 934                 ing_types = 0x01;
 935                 break;
 936         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
 937         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
 938         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
 939         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 940                 ing_types = 0x02;
 941                 break;
 942         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 943         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 944         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
 945         case HWTSTAMP_FILTER_PTP_V2_EVENT:
 946                 ing_types = 0x0f;
 947                 break;
 948         case HWTSTAMP_FILTER_ALL:
 949                 ing_types = 0xff;
 950                 break;
 951         case HWTSTAMP_FILTER_SOME:
 952         case HWTSTAMP_FILTER_NTP_ALL:
 953                 return -ERANGE;
 954         }
 955
 956         *p_ing_types = ing_types;
 957         *p_egr_types = egr_types;
 958         *p_rx_filter = rx_filter;
 959         return 0;
 960 }
 961
 962 static int mlxsw_sp1_ptp_mtpppc_update(struct mlxsw_sp_port *mlxsw_sp_port,
 963                                        u16 ing_types, u16 egr_types)
 964 {
 965         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
 966         struct mlxsw_sp_port *tmp;
 967         u16 orig_ing_types = 0;
 968         u16 orig_egr_types = 0;
 969         int err;
 970         int i;
 971
 972         /* MTPPPC configures timestamping globally, not per port. Find the
 973          * configuration that contains all configured timestamping requests.
 974          */
 975         for (i = 1; i < mlxsw_core_max_ports(mlxsw_sp->core); i++) {
 976                 tmp = mlxsw_sp->ports[i];
 977                 if (tmp) {
 978                         orig_ing_types |= tmp->ptp.ing_types;
 979                         orig_egr_types |= tmp->ptp.egr_types;
 980                 }
 981                 if (tmp && tmp != mlxsw_sp_port) {
 982                         ing_types |= tmp->ptp.ing_types;
 983                         egr_types |= tmp->ptp.egr_types;
 984                 }
 985         }
 986
 987         if ((ing_types || egr_types) && !(orig_ing_types || orig_egr_types)) {
 988                 err = mlxsw_sp_nve_inc_parsing_depth_get(mlxsw_sp);
 989                 if (err) {
 990                         netdev_err(mlxsw_sp_port->dev, "Failed to increase parsing depth");
 991                         return err;
 992                 }
 993         }
 994         if (!(ing_types || egr_types) && (orig_ing_types || orig_egr_types))
 995                 mlxsw_sp_nve_inc_parsing_depth_put(mlxsw_sp);
 996
 997         return mlxsw_sp1_ptp_mtpppc_set(mlxsw_sp_port->mlxsw_sp,
 998                                        ing_types, egr_types);
 999 }
1000
1001 static bool mlxsw_sp1_ptp_hwtstamp_enabled(struct mlxsw_sp_port *mlxsw_sp_port)
1002 {
1003         return mlxsw_sp_port->ptp.ing_types || mlxsw_sp_port->ptp.egr_types;
1004 }
1005
1006 static int
1007 mlxsw_sp1_ptp_port_shaper_set(struct mlxsw_sp_port *mlxsw_sp_port, bool enable)
1008 {
1009         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1010         char qeec_pl[MLXSW_REG_QEEC_LEN];
1011
1012         mlxsw_reg_qeec_ptps_pack(qeec_pl, mlxsw_sp_port->local_port, enable);
1013         return mlxsw_reg_write(mlxsw_sp->core, MLXSW_REG(qeec), qeec_pl);
1014 }
1015
1016 static int mlxsw_sp1_ptp_port_shaper_check(struct mlxsw_sp_port *mlxsw_sp_port)
1017 {
1018         const struct mlxsw_sp_port_type_speed_ops *port_type_speed_ops;
1019         struct mlxsw_sp *mlxsw_sp = mlxsw_sp_port->mlxsw_sp;
1020         char ptys_pl[MLXSW_REG_PTYS_LEN];
1021         u32 eth_proto_oper, speed;
1022         bool ptps = false;
1023         int err, i;
1024
1025         if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1026                 return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, false);
1027
1028         port_type_speed_ops = mlxsw_sp->port_type_speed_ops;
1029         port_type_speed_ops->reg_ptys_eth_pack(mlxsw_sp, ptys_pl,
1030                                                mlxsw_sp_port->local_port, 0,
1031                                                false);
1032         err = mlxsw_reg_query(mlxsw_sp->core, MLXSW_REG(ptys), ptys_pl);
1033         if (err)
1034                 return err;
1035         port_type_speed_ops->reg_ptys_eth_unpack(mlxsw_sp, ptys_pl, NULL, NULL,
1036                                                  &eth_proto_oper);
1037
1038         speed = port_type_speed_ops->from_ptys_speed(mlxsw_sp, eth_proto_oper);
1039         for (i = 0; i < MLXSW_SP1_PTP_SHAPER_PARAMS_LEN; i++) {
1040                 if (mlxsw_sp1_ptp_shaper_params[i].ethtool_speed == speed) {
1041                         ptps = true;
1042                         break;
1043                 }
1044         }
1045
1046         return mlxsw_sp1_ptp_port_shaper_set(mlxsw_sp_port, ptps);
1047 }
1048
1049 void mlxsw_sp1_ptp_shaper_work(struct work_struct *work)
1050 {
1051         struct delayed_work *dwork = to_delayed_work(work);
1052         struct mlxsw_sp_port *mlxsw_sp_port;
1053         int err;
1054
1055         mlxsw_sp_port = container_of(dwork, struct mlxsw_sp_port,
1056                                      ptp.shaper_dw);
1057
1058         if (!mlxsw_sp1_ptp_hwtstamp_enabled(mlxsw_sp_port))
1059                 return;
1060
1061         err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1062         if (err)
1063                 netdev_err(mlxsw_sp_port->dev, "Failed to set up PTP shaper\n");
1064 }
1065
1066 int mlxsw_sp1_ptp_hwtstamp_set(struct mlxsw_sp_port *mlxsw_sp_port,
1067                                struct hwtstamp_config *config)
1068 {
1069         enum hwtstamp_rx_filters rx_filter;
1070         u16 ing_types;
1071         u16 egr_types;
1072         int err;
1073
1074         err = mlxsw_sp_ptp_get_message_types(config, &ing_types, &egr_types,
1075                                              &rx_filter);
1076         if (err)
1077                 return err;
1078
1079         err = mlxsw_sp1_ptp_mtpppc_update(mlxsw_sp_port, ing_types, egr_types);
1080         if (err)
1081                 return err;
1082
1083         mlxsw_sp_port->ptp.hwtstamp_config = *config;
1084         mlxsw_sp_port->ptp.ing_types = ing_types;
1085         mlxsw_sp_port->ptp.egr_types = egr_types;
1086
1087         err = mlxsw_sp1_ptp_port_shaper_check(mlxsw_sp_port);
1088         if (err)
1089                 return err;
1090
1091         /* Notify the ioctl caller what we are actually timestamping. */
1092         config->rx_filter = rx_filter;
1093
1094         return 0;
1095 }
1096
1097 int mlxsw_sp1_ptp_get_ts_info(struct mlxsw_sp *mlxsw_sp,
1098                               struct ethtool_ts_info *info)
1099 {
1100         info->phc_index = ptp_clock_index(mlxsw_sp->clock->ptp);
1101
1102         info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
1103                                 SOF_TIMESTAMPING_RX_HARDWARE |
1104                                 SOF_TIMESTAMPING_RAW_HARDWARE;
1105
1106         info->tx_types = BIT(HWTSTAMP_TX_OFF) |
1107                          BIT(HWTSTAMP_TX_ON);
1108
1109         info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
1110                            BIT(HWTSTAMP_FILTER_ALL);
1111
1112         return 0;
1113 }
1114
1115 struct mlxsw_sp_ptp_port_stat {
1116         char str[ETH_GSTRING_LEN];
1117         ptrdiff_t offset;
1118 };
1119
1120 #define MLXSW_SP_PTP_PORT_STAT(NAME, FIELD)                             \
1121         {                                                               \
1122                 .str = NAME,                                            \
1123                 .offset = offsetof(struct mlxsw_sp_ptp_port_stats,      \
1124                                     FIELD),                             \
1125         }
1126
1127 static const struct mlxsw_sp_ptp_port_stat mlxsw_sp_ptp_port_stats[] = {
1128         MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_packets",    rx_gcd.packets),
1129         MLXSW_SP_PTP_PORT_STAT("ptp_rx_gcd_timestamps", rx_gcd.timestamps),
1130         MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_packets",    tx_gcd.packets),
1131         MLXSW_SP_PTP_PORT_STAT("ptp_tx_gcd_timestamps", tx_gcd.timestamps),
1132 };
1133
1134 #undef MLXSW_SP_PTP_PORT_STAT
1135
1136 #define MLXSW_SP_PTP_PORT_STATS_LEN \
1137         ARRAY_SIZE(mlxsw_sp_ptp_port_stats)
1138
1139 int mlxsw_sp1_get_stats_count(void)
1140 {
1141         return MLXSW_SP_PTP_PORT_STATS_LEN;
1142 }
1143
1144 void mlxsw_sp1_get_stats_strings(u8 **p)
1145 {
1146         int i;
1147
1148         for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) {
1149                 memcpy(*p, mlxsw_sp_ptp_port_stats[i].str,
1150                        ETH_GSTRING_LEN);
1151                 *p += ETH_GSTRING_LEN;
1152         }
1153 }
1154
1155 void mlxsw_sp1_get_stats(struct mlxsw_sp_port *mlxsw_sp_port,
1156                          u64 *data, int data_index)
1157 {
1158         void *stats = &mlxsw_sp_port->ptp.stats;
1159         ptrdiff_t offset;
1160         int i;
1161
1162         data += data_index;
1163         for (i = 0; i < MLXSW_SP_PTP_PORT_STATS_LEN; i++) {
1164                 offset = mlxsw_sp_ptp_port_stats[i].offset;
1165                 *data++ = *(u64 *)(stats + offset);
1166         }
1167 }