drivers/net/ethernet/mscc/ocelot_ptp.c

   1 // SPDX-License-Identifier: (GPL-2.0 OR MIT)
   2 /* Microsemi Ocelot PTP clock driver
   3  *
   4  * Copyright (c) 2017 Microsemi Corporation
   5  * Copyright 2020 NXP
   6  */
   7 #include <linux/time64.h>
   8
   9 #include <linux/dsa/ocelot.h>
  10 #include <linux/ptp_classify.h>
  11 #include <soc/mscc/ocelot_ptp.h>
  12 #include <soc/mscc/ocelot_sys.h>
  13 #include <soc/mscc/ocelot_vcap.h>
  14 #include <soc/mscc/ocelot.h>
  15 #include "ocelot.h"
  16
  17 int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
  18 {
  19         struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
  20         unsigned long flags;
  21         time64_t s;
  22         u32 val;
  23         s64 ns;
  24
  25         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
  26
  27         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
  28         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
  29         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
  30         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
  31
  32         s = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN) & 0xffff;
  33         s <<= 32;
  34         s += ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
  35         ns = ocelot_read_rix(ocelot, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
  36
  37         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
  38
  39         /* Deal with negative values */
  40         if (ns >= 0x3ffffff0 && ns <= 0x3fffffff) {
  41                 s--;
  42                 ns &= 0xf;
  43                 ns += 999999984;
  44         }
  45
  46         set_normalized_timespec64(ts, s, ns);
  47         return 0;
  48 }
  49 EXPORT_SYMBOL(ocelot_ptp_gettime64);
  50
  51 int ocelot_ptp_settime64(struct ptp_clock_info *ptp,
  52                          const struct timespec64 *ts)
  53 {
  54         struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
  55         unsigned long flags;
  56         u32 val;
  57
  58         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
  59
  60         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
  61         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
  62         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
  63
  64         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
  65
  66         ocelot_write_rix(ocelot, lower_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_LSB,
  67                          TOD_ACC_PIN);
  68         ocelot_write_rix(ocelot, upper_32_bits(ts->tv_sec), PTP_PIN_TOD_SEC_MSB,
  69                          TOD_ACC_PIN);
  70         ocelot_write_rix(ocelot, ts->tv_nsec, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
  71
  72         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
  73         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
  74         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_LOAD);
  75
  76         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
  77
  78         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
  79
  80         if (ocelot->ops->tas_clock_adjust)
  81                 ocelot->ops->tas_clock_adjust(ocelot);
  82
  83         return 0;
  84 }
  85 EXPORT_SYMBOL(ocelot_ptp_settime64);
  86
  87 int ocelot_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
  88 {
  89         if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
  90                 struct ocelot *ocelot = container_of(ptp, struct ocelot,
  91                                                      ptp_info);
  92                 unsigned long flags;
  93                 u32 val;
  94
  95                 spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
  96
  97                 val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
  98                 val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK |
  99                          PTP_PIN_CFG_DOM);
 100                 val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
 101
 102                 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
 103
 104                 ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
 105                 ocelot_write_rix(ocelot, 0, PTP_PIN_TOD_SEC_MSB, TOD_ACC_PIN);
 106                 ocelot_write_rix(ocelot, delta, PTP_PIN_TOD_NSEC, TOD_ACC_PIN);
 107
 108                 val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
 109                 val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK |
 110                          PTP_PIN_CFG_DOM);
 111                 val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_DELTA);
 112
 113                 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
 114
 115                 spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 116
 117                 if (ocelot->ops->tas_clock_adjust)
 118                         ocelot->ops->tas_clock_adjust(ocelot);
 119         } else {
 120                 /* Fall back using ocelot_ptp_settime64 which is not exact. */
 121                 struct timespec64 ts;
 122                 u64 now;
 123
 124                 ocelot_ptp_gettime64(ptp, &ts);
 125
 126                 now = ktime_to_ns(timespec64_to_ktime(ts));
 127                 ts = ns_to_timespec64(now + delta);
 128
 129                 ocelot_ptp_settime64(ptp, &ts);
 130         }
 131
 132         return 0;
 133 }
 134 EXPORT_SYMBOL(ocelot_ptp_adjtime);
 135
 136 int ocelot_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
 137 {
 138         struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
 139         u32 unit = 0, direction = 0;
 140         unsigned long flags;
 141         u64 adj = 0;
 142
 143         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
 144
 145         if (!scaled_ppm)
 146                 goto disable_adj;
 147
 148         if (scaled_ppm < 0) {
 149                 direction = PTP_CFG_CLK_ADJ_CFG_DIR;
 150                 scaled_ppm = -scaled_ppm;
 151         }
 152
 153         adj = PSEC_PER_SEC << 16;
 154         do_div(adj, scaled_ppm);
 155         do_div(adj, 1000);
 156
 157         /* If the adjustment value is too large, use ns instead */
 158         if (adj >= (1L << 30)) {
 159                 unit = PTP_CFG_CLK_ADJ_FREQ_NS;
 160                 do_div(adj, 1000);
 161         }
 162
 163         /* Still too big */
 164         if (adj >= (1L << 30))
 165                 goto disable_adj;
 166
 167         ocelot_write(ocelot, unit | adj, PTP_CLK_CFG_ADJ_FREQ);
 168         ocelot_write(ocelot, PTP_CFG_CLK_ADJ_CFG_ENA | direction,
 169                      PTP_CLK_CFG_ADJ_CFG);
 170
 171         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 172         return 0;
 173
 174 disable_adj:
 175         ocelot_write(ocelot, 0, PTP_CLK_CFG_ADJ_CFG);
 176
 177         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 178         return 0;
 179 }
 180 EXPORT_SYMBOL(ocelot_ptp_adjfine);
 181
 182 int ocelot_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin,
 183                       enum ptp_pin_function func, unsigned int chan)
 184 {
 185         switch (func) {
 186         case PTP_PF_NONE:
 187         case PTP_PF_PEROUT:
 188                 break;
 189         case PTP_PF_EXTTS:
 190         case PTP_PF_PHYSYNC:
 191                 return -1;
 192         }
 193         return 0;
 194 }
 195 EXPORT_SYMBOL(ocelot_ptp_verify);
 196
 197 int ocelot_ptp_enable(struct ptp_clock_info *ptp,
 198                       struct ptp_clock_request *rq, int on)
 199 {
 200         struct ocelot *ocelot = container_of(ptp, struct ocelot, ptp_info);
 201         struct timespec64 ts_phase, ts_period;
 202         enum ocelot_ptp_pins ptp_pin;
 203         unsigned long flags;
 204         bool pps = false;
 205         int pin = -1;
 206         s64 wf_high;
 207         s64 wf_low;
 208         u32 val;
 209
 210         switch (rq->type) {
 211         case PTP_CLK_REQ_PEROUT:
 212                 /* Reject requests with unsupported flags */
 213                 if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE |
 214                                          PTP_PEROUT_PHASE))
 215                         return -EOPNOTSUPP;
 216
 217                 pin = ptp_find_pin(ocelot->ptp_clock, PTP_PF_PEROUT,
 218                                    rq->perout.index);
 219                 if (pin == 0)
 220                         ptp_pin = PTP_PIN_0;
 221                 else if (pin == 1)
 222                         ptp_pin = PTP_PIN_1;
 223                 else if (pin == 2)
 224                         ptp_pin = PTP_PIN_2;
 225                 else if (pin == 3)
 226                         ptp_pin = PTP_PIN_3;
 227                 else
 228                         return -EBUSY;
 229
 230                 ts_period.tv_sec = rq->perout.period.sec;
 231                 ts_period.tv_nsec = rq->perout.period.nsec;
 232
 233                 if (ts_period.tv_sec == 1 && ts_period.tv_nsec == 0)
 234                         pps = true;
 235
 236                 /* Handle turning off */
 237                 if (!on) {
 238                         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
 239                         val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE);
 240                         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin);
 241                         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 242                         break;
 243                 }
 244
 245                 if (rq->perout.flags & PTP_PEROUT_PHASE) {
 246                         ts_phase.tv_sec = rq->perout.phase.sec;
 247                         ts_phase.tv_nsec = rq->perout.phase.nsec;
 248                 } else {
 249                         /* Compatibility */
 250                         ts_phase.tv_sec = rq->perout.start.sec;
 251                         ts_phase.tv_nsec = rq->perout.start.nsec;
 252                 }
 253                 if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) {
 254                         dev_warn(ocelot->dev,
 255                                  "Absolute start time not supported!\n");
 256                         dev_warn(ocelot->dev,
 257                                  "Accept nsec for PPS phase adjustment, otherwise start time should be 0 0.\n");
 258                         return -EINVAL;
 259                 }
 260
 261                 /* Calculate waveform high and low times */
 262                 if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) {
 263                         struct timespec64 ts_on;
 264
 265                         ts_on.tv_sec = rq->perout.on.sec;
 266                         ts_on.tv_nsec = rq->perout.on.nsec;
 267
 268                         wf_high = timespec64_to_ns(&ts_on);
 269                 } else {
 270                         if (pps) {
 271                                 wf_high = 1000;
 272                         } else {
 273                                 wf_high = timespec64_to_ns(&ts_period);
 274                                 wf_high = div_s64(wf_high, 2);
 275                         }
 276                 }
 277
 278                 wf_low = timespec64_to_ns(&ts_period);
 279                 wf_low -= wf_high;
 280
 281                 /* Handle PPS request */
 282                 if (pps) {
 283                         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
 284                         ocelot_write_rix(ocelot, ts_phase.tv_nsec,
 285                                          PTP_PIN_WF_LOW_PERIOD, ptp_pin);
 286                         ocelot_write_rix(ocelot, wf_high,
 287                                          PTP_PIN_WF_HIGH_PERIOD, ptp_pin);
 288                         val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_CLOCK);
 289                         val |= PTP_PIN_CFG_SYNC;
 290                         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin);
 291                         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 292                         break;
 293                 }
 294
 295                 /* Handle periodic clock */
 296                 if (wf_high > 0x3fffffff || wf_high <= 0x6)
 297                         return -EINVAL;
 298                 if (wf_low > 0x3fffffff || wf_low <= 0x6)
 299                         return -EINVAL;
 300
 301                 spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
 302                 ocelot_write_rix(ocelot, wf_low, PTP_PIN_WF_LOW_PERIOD,
 303                                  ptp_pin);
 304                 ocelot_write_rix(ocelot, wf_high, PTP_PIN_WF_HIGH_PERIOD,
 305                                  ptp_pin);
 306                 val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_CLOCK);
 307                 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin);
 308                 spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 309                 break;
 310         default:
 311                 return -EOPNOTSUPP;
 312         }
 313         return 0;
 314 }
 315 EXPORT_SYMBOL(ocelot_ptp_enable);
 316
 317 static void ocelot_populate_l2_ptp_trap_key(struct ocelot_vcap_filter *trap)
 318 {
 319         trap->key_type = OCELOT_VCAP_KEY_ETYPE;
 320         *(__be16 *)trap->key.etype.etype.value = htons(ETH_P_1588);
 321         *(__be16 *)trap->key.etype.etype.mask = htons(0xffff);
 322 }
 323
 324 static void
 325 ocelot_populate_ipv4_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
 326 {
 327         trap->key_type = OCELOT_VCAP_KEY_IPV4;
 328         trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
 329         trap->key.ipv4.proto.mask[0] = 0xff;
 330         trap->key.ipv4.dport.value = PTP_EV_PORT;
 331         trap->key.ipv4.dport.mask = 0xffff;
 332 }
 333
 334 static void
 335 ocelot_populate_ipv6_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
 336 {
 337         trap->key_type = OCELOT_VCAP_KEY_IPV6;
 338         trap->key.ipv6.proto.value[0] = IPPROTO_UDP;
 339         trap->key.ipv6.proto.mask[0] = 0xff;
 340         trap->key.ipv6.dport.value = PTP_EV_PORT;
 341         trap->key.ipv6.dport.mask = 0xffff;
 342 }
 343
 344 static void
 345 ocelot_populate_ipv4_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
 346 {
 347         trap->key_type = OCELOT_VCAP_KEY_IPV4;
 348         trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
 349         trap->key.ipv4.proto.mask[0] = 0xff;
 350         trap->key.ipv4.dport.value = PTP_GEN_PORT;
 351         trap->key.ipv4.dport.mask = 0xffff;
 352 }
 353
 354 static void
 355 ocelot_populate_ipv6_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
 356 {
 357         trap->key_type = OCELOT_VCAP_KEY_IPV6;
 358         trap->key.ipv6.proto.value[0] = IPPROTO_UDP;
 359         trap->key.ipv6.proto.mask[0] = 0xff;
 360         trap->key.ipv6.dport.value = PTP_GEN_PORT;
 361         trap->key.ipv6.dport.mask = 0xffff;
 362 }
 363
 364 static int ocelot_l2_ptp_trap_add(struct ocelot *ocelot, int port)
 365 {
 366         unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
 367
 368         return ocelot_trap_add(ocelot, port, l2_cookie, true,
 369                                ocelot_populate_l2_ptp_trap_key);
 370 }
 371
 372 static int ocelot_l2_ptp_trap_del(struct ocelot *ocelot, int port)
 373 {
 374         unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
 375
 376         return ocelot_trap_del(ocelot, port, l2_cookie);
 377 }
 378
 379 static int ocelot_ipv4_ptp_trap_add(struct ocelot *ocelot, int port)
 380 {
 381         unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
 382         unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
 383         int err;
 384
 385         err = ocelot_trap_add(ocelot, port, ipv4_ev_cookie, true,
 386                               ocelot_populate_ipv4_ptp_event_trap_key);
 387         if (err)
 388                 return err;
 389
 390         err = ocelot_trap_add(ocelot, port, ipv4_gen_cookie, false,
 391                               ocelot_populate_ipv4_ptp_general_trap_key);
 392         if (err)
 393                 ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
 394
 395         return err;
 396 }
 397
 398 static int ocelot_ipv4_ptp_trap_del(struct ocelot *ocelot, int port)
 399 {
 400         unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
 401         unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
 402         int err;
 403
 404         err = ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
 405         err |= ocelot_trap_del(ocelot, port, ipv4_gen_cookie);
 406         return err;
 407 }
 408
 409 static int ocelot_ipv6_ptp_trap_add(struct ocelot *ocelot, int port)
 410 {
 411         unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
 412         unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
 413         int err;
 414
 415         err = ocelot_trap_add(ocelot, port, ipv6_ev_cookie, true,
 416                               ocelot_populate_ipv6_ptp_event_trap_key);
 417         if (err)
 418                 return err;
 419
 420         err = ocelot_trap_add(ocelot, port, ipv6_gen_cookie, false,
 421                               ocelot_populate_ipv6_ptp_general_trap_key);
 422         if (err)
 423                 ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
 424
 425         return err;
 426 }
 427
 428 static int ocelot_ipv6_ptp_trap_del(struct ocelot *ocelot, int port)
 429 {
 430         unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
 431         unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
 432         int err;
 433
 434         err = ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
 435         err |= ocelot_trap_del(ocelot, port, ipv6_gen_cookie);
 436         return err;
 437 }
 438
 439 static int ocelot_setup_ptp_traps(struct ocelot *ocelot, int port,
 440                                   bool l2, bool l4)
 441 {
 442         int err;
 443
 444         if (l2)
 445                 err = ocelot_l2_ptp_trap_add(ocelot, port);
 446         else
 447                 err = ocelot_l2_ptp_trap_del(ocelot, port);
 448         if (err)
 449                 return err;
 450
 451         if (l4) {
 452                 err = ocelot_ipv4_ptp_trap_add(ocelot, port);
 453                 if (err)
 454                         goto err_ipv4;
 455
 456                 err = ocelot_ipv6_ptp_trap_add(ocelot, port);
 457                 if (err)
 458                         goto err_ipv6;
 459         } else {
 460                 err = ocelot_ipv4_ptp_trap_del(ocelot, port);
 461
 462                 err |= ocelot_ipv6_ptp_trap_del(ocelot, port);
 463         }
 464         if (err)
 465                 return err;
 466
 467         return 0;
 468
 469 err_ipv6:
 470         ocelot_ipv4_ptp_trap_del(ocelot, port);
 471 err_ipv4:
 472         if (l2)
 473                 ocelot_l2_ptp_trap_del(ocelot, port);
 474         return err;
 475 }
 476
 477 int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
 478 {
 479         return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
 480                             sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
 481 }
 482 EXPORT_SYMBOL(ocelot_hwstamp_get);
 483
 484 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
 485 {
 486         struct ocelot_port *ocelot_port = ocelot->ports[port];
 487         bool l2 = false, l4 = false;
 488         struct hwtstamp_config cfg;
 489         int err;
 490
 491         if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
 492                 return -EFAULT;
 493
 494         /* Tx type sanity check */
 495         switch (cfg.tx_type) {
 496         case HWTSTAMP_TX_ON:
 497                 ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
 498                 break;
 499         case HWTSTAMP_TX_ONESTEP_SYNC:
 500                 /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
 501                  * need to update the origin time.
 502                  */
 503                 ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
 504                 break;
 505         case HWTSTAMP_TX_OFF:
 506                 ocelot_port->ptp_cmd = 0;
 507                 break;
 508         default:
 509                 return -ERANGE;
 510         }
 511
 512         mutex_lock(&ocelot->ptp_lock);
 513
 514         switch (cfg.rx_filter) {
 515         case HWTSTAMP_FILTER_NONE:
 516                 break;
 517         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 518         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 519         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
 520                 l4 = true;
 521                 break;
 522         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
 523         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
 524         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
 525                 l2 = true;
 526                 break;
 527         case HWTSTAMP_FILTER_PTP_V2_EVENT:
 528         case HWTSTAMP_FILTER_PTP_V2_SYNC:
 529         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 530                 l2 = true;
 531                 l4 = true;
 532                 break;
 533         default:
 534                 mutex_unlock(&ocelot->ptp_lock);
 535                 return -ERANGE;
 536         }
 537
 538         err = ocelot_setup_ptp_traps(ocelot, port, l2, l4);
 539         if (err) {
 540                 mutex_unlock(&ocelot->ptp_lock);
 541                 return err;
 542         }
 543
 544         if (l2 && l4)
 545                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 546         else if (l2)
 547                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
 548         else if (l4)
 549                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
 550         else
 551                 cfg.rx_filter = HWTSTAMP_FILTER_NONE;
 552
 553         /* Commit back the result & save it */
 554         memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
 555         mutex_unlock(&ocelot->ptp_lock);
 556
 557         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
 558 }
 559 EXPORT_SYMBOL(ocelot_hwstamp_set);
 560
 561 int ocelot_get_ts_info(struct ocelot *ocelot, int port,
 562                        struct ethtool_ts_info *info)
 563 {
 564         info->phc_index = ocelot->ptp_clock ?
 565                           ptp_clock_index(ocelot->ptp_clock) : -1;
 566         if (info->phc_index == -1) {
 567                 info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
 568                                          SOF_TIMESTAMPING_RX_SOFTWARE |
 569                                          SOF_TIMESTAMPING_SOFTWARE;
 570                 return 0;
 571         }
 572         info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
 573                                  SOF_TIMESTAMPING_RX_SOFTWARE |
 574                                  SOF_TIMESTAMPING_SOFTWARE |
 575                                  SOF_TIMESTAMPING_TX_HARDWARE |
 576                                  SOF_TIMESTAMPING_RX_HARDWARE |
 577                                  SOF_TIMESTAMPING_RAW_HARDWARE;
 578         info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
 579                          BIT(HWTSTAMP_TX_ONESTEP_SYNC);
 580         info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
 581                            BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) |
 582                            BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
 583                            BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
 584
 585         return 0;
 586 }
 587 EXPORT_SYMBOL(ocelot_get_ts_info);
 588
 589 static int ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
 590                                         struct sk_buff *clone)
 591 {
 592         struct ocelot_port *ocelot_port = ocelot->ports[port];
 593         unsigned long flags;
 594
 595         spin_lock_irqsave(&ocelot->ts_id_lock, flags);
 596
 597         if (ocelot_port->ptp_skbs_in_flight == OCELOT_MAX_PTP_ID ||
 598             ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) {
 599                 spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
 600                 return -EBUSY;
 601         }
 602
 603         skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
 604         /* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */
 605         OCELOT_SKB_CB(clone)->ts_id = ocelot_port->ts_id;
 606
 607         ocelot_port->ts_id++;
 608         if (ocelot_port->ts_id == OCELOT_MAX_PTP_ID)
 609                 ocelot_port->ts_id = 0;
 610
 611         ocelot_port->ptp_skbs_in_flight++;
 612         ocelot->ptp_skbs_in_flight++;
 613
 614         skb_queue_tail(&ocelot_port->tx_skbs, clone);
 615
 616         spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
 617
 618         return 0;
 619 }
 620
 621 static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb,
 622                                        unsigned int ptp_class)
 623 {
 624         struct ptp_header *hdr;
 625         u8 msgtype, twostep;
 626
 627         hdr = ptp_parse_header(skb, ptp_class);
 628         if (!hdr)
 629                 return false;
 630
 631         msgtype = ptp_get_msgtype(hdr, ptp_class);
 632         twostep = hdr->flag_field[0] & 0x2;
 633
 634         if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0)
 635                 return true;
 636
 637         return false;
 638 }
 639
 640 int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port,
 641                                  struct sk_buff *skb,
 642                                  struct sk_buff **clone)
 643 {
 644         struct ocelot_port *ocelot_port = ocelot->ports[port];
 645         u8 ptp_cmd = ocelot_port->ptp_cmd;
 646         unsigned int ptp_class;
 647         int err;
 648
 649         /* Don't do anything if PTP timestamping not enabled */
 650         if (!ptp_cmd)
 651                 return 0;
 652
 653         ptp_class = ptp_classify_raw(skb);
 654         if (ptp_class == PTP_CLASS_NONE)
 655                 return -EINVAL;
 656
 657         /* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */
 658         if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) {
 659                 if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) {
 660                         OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
 661                         return 0;
 662                 }
 663
 664                 /* Fall back to two-step timestamping */
 665                 ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
 666         }
 667
 668         if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
 669                 *clone = skb_clone_sk(skb);
 670                 if (!(*clone))
 671                         return -ENOMEM;
 672
 673                 err = ocelot_port_add_txtstamp_skb(ocelot, port, *clone);
 674                 if (err)
 675                         return err;
 676
 677                 OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
 678                 OCELOT_SKB_CB(*clone)->ptp_class = ptp_class;
 679         }
 680
 681         return 0;
 682 }
 683 EXPORT_SYMBOL(ocelot_port_txtstamp_request);
 684
 685 static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
 686                                    struct timespec64 *ts)
 687 {
 688         unsigned long flags;
 689         u32 val;
 690
 691         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
 692
 693         /* Read current PTP time to get seconds */
 694         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
 695
 696         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
 697         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
 698         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
 699         ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
 700
 701         /* Read packet HW timestamp from FIFO */
 702         val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
 703         ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
 704
 705         /* Sec has incremented since the ts was registered */
 706         if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
 707                 ts->tv_sec--;
 708
 709         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 710 }
 711
 712 static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid)
 713 {
 714         struct ptp_header *hdr;
 715
 716         hdr = ptp_parse_header(clone, OCELOT_SKB_CB(clone)->ptp_class);
 717         if (WARN_ON(!hdr))
 718                 return false;
 719
 720         return seqid == ntohs(hdr->sequence_id);
 721 }
 722
 723 void ocelot_get_txtstamp(struct ocelot *ocelot)
 724 {
 725         int budget = OCELOT_PTP_QUEUE_SZ;
 726
 727         while (budget--) {
 728                 struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
 729                 struct skb_shared_hwtstamps shhwtstamps;
 730                 u32 val, id, seqid, txport;
 731                 struct ocelot_port *port;
 732                 struct timespec64 ts;
 733                 unsigned long flags;
 734
 735                 val = ocelot_read(ocelot, SYS_PTP_STATUS);
 736
 737                 /* Check if a timestamp can be retrieved */
 738                 if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
 739                         break;
 740
 741                 WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
 742
 743                 /* Retrieve the ts ID and Tx port */
 744                 id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
 745                 txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
 746                 seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val);
 747
 748                 port = ocelot->ports[txport];
 749
 750                 spin_lock(&ocelot->ts_id_lock);
 751                 port->ptp_skbs_in_flight--;
 752                 ocelot->ptp_skbs_in_flight--;
 753                 spin_unlock(&ocelot->ts_id_lock);
 754
 755                 /* Retrieve its associated skb */
 756 try_again:
 757                 spin_lock_irqsave(&port->tx_skbs.lock, flags);
 758
 759                 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
 760                         if (OCELOT_SKB_CB(skb)->ts_id != id)
 761                                 continue;
 762                         __skb_unlink(skb, &port->tx_skbs);
 763                         skb_match = skb;
 764                         break;
 765                 }
 766
 767                 spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
 768
 769                 if (WARN_ON(!skb_match))
 770                         continue;
 771
 772                 if (!ocelot_validate_ptp_skb(skb_match, seqid)) {
 773                         dev_err_ratelimited(ocelot->dev,
 774                                             "port %d received stale TX timestamp for seqid %d, discarding\n",
 775                                             txport, seqid);
 776                         dev_kfree_skb_any(skb);
 777                         goto try_again;
 778                 }
 779
 780                 /* Get the h/w timestamp */
 781                 ocelot_get_hwtimestamp(ocelot, &ts);
 782
 783                 /* Set the timestamp into the skb */
 784                 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
 785                 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
 786                 skb_complete_tx_timestamp(skb_match, &shhwtstamps);
 787
 788                 /* Next ts */
 789                 ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
 790         }
 791 }
 792 EXPORT_SYMBOL(ocelot_get_txtstamp);
 793
 794 int ocelot_init_timestamp(struct ocelot *ocelot,
 795                           const struct ptp_clock_info *info)
 796 {
 797         struct ptp_clock *ptp_clock;
 798         int i;
 799
 800         ocelot->ptp_info = *info;
 801
 802         for (i = 0; i < OCELOT_PTP_PINS_NUM; i++) {
 803                 struct ptp_pin_desc *p = &ocelot->ptp_pins[i];
 804
 805                 snprintf(p->name, sizeof(p->name), "switch_1588_dat%d", i);
 806                 p->index = i;
 807                 p->func = PTP_PF_NONE;
 808         }
 809
 810         ocelot->ptp_info.pin_config = &ocelot->ptp_pins[0];
 811
 812         ptp_clock = ptp_clock_register(&ocelot->ptp_info, ocelot->dev);
 813         if (IS_ERR(ptp_clock))
 814                 return PTR_ERR(ptp_clock);
 815         /* Check if PHC support is missing at the configuration level */
 816         if (!ptp_clock)
 817                 return 0;
 818
 819         ocelot->ptp_clock = ptp_clock;
 820
 821         ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG);
 822         ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW);
 823         ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH);
 824
 825         ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC);
 826
 827         /* There is no device reconfiguration, PTP Rx stamping is always
 828          * enabled.
 829          */
 830         ocelot->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 831
 832         return 0;
 833 }
 834 EXPORT_SYMBOL(ocelot_init_timestamp);
 835
 836 int ocelot_deinit_timestamp(struct ocelot *ocelot)
 837 {
 838         if (ocelot->ptp_clock)
 839                 ptp_clock_unregister(ocelot->ptp_clock);
 840         return 0;
 841 }
 842 EXPORT_SYMBOL(ocelot_deinit_timestamp);