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         struct ocelot_port *ocelot_port = ocelot->ports[port];
 443         int err;
 444
 445         ocelot_port->trap_proto &= ~(OCELOT_PROTO_PTP_L2 |
 446                                      OCELOT_PROTO_PTP_L4);
 447
 448         if (l2)
 449                 err = ocelot_l2_ptp_trap_add(ocelot, port);
 450         else
 451                 err = ocelot_l2_ptp_trap_del(ocelot, port);
 452         if (err)
 453                 return err;
 454
 455         if (l4) {
 456                 err = ocelot_ipv4_ptp_trap_add(ocelot, port);
 457                 if (err)
 458                         goto err_ipv4;
 459
 460                 err = ocelot_ipv6_ptp_trap_add(ocelot, port);
 461                 if (err)
 462                         goto err_ipv6;
 463         } else {
 464                 err = ocelot_ipv4_ptp_trap_del(ocelot, port);
 465
 466                 err |= ocelot_ipv6_ptp_trap_del(ocelot, port);
 467         }
 468         if (err)
 469                 return err;
 470
 471         if (l2)
 472                 ocelot_port->trap_proto |= OCELOT_PROTO_PTP_L2;
 473         if (l4)
 474                 ocelot_port->trap_proto |= OCELOT_PROTO_PTP_L4;
 475
 476         return 0;
 477
 478 err_ipv6:
 479         ocelot_ipv4_ptp_trap_del(ocelot, port);
 480 err_ipv4:
 481         if (l2)
 482                 ocelot_l2_ptp_trap_del(ocelot, port);
 483         return err;
 484 }
 485
 486 static int ocelot_traps_to_ptp_rx_filter(unsigned int proto)
 487 {
 488         if ((proto & OCELOT_PROTO_PTP_L2) && (proto & OCELOT_PROTO_PTP_L4))
 489                 return HWTSTAMP_FILTER_PTP_V2_EVENT;
 490         else if (proto & OCELOT_PROTO_PTP_L2)
 491                 return HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
 492         else if (proto & OCELOT_PROTO_PTP_L4)
 493                 return HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
 494
 495         return HWTSTAMP_FILTER_NONE;
 496 }
 497
 498 int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
 499 {
 500         struct ocelot_port *ocelot_port = ocelot->ports[port];
 501         struct hwtstamp_config cfg = {};
 502
 503         switch (ocelot_port->ptp_cmd) {
 504         case IFH_REW_OP_TWO_STEP_PTP:
 505                 cfg.tx_type = HWTSTAMP_TX_ON;
 506                 break;
 507         case IFH_REW_OP_ORIGIN_PTP:
 508                 cfg.tx_type = HWTSTAMP_TX_ONESTEP_SYNC;
 509                 break;
 510         default:
 511                 cfg.tx_type = HWTSTAMP_TX_OFF;
 512                 break;
 513         }
 514
 515         cfg.rx_filter = ocelot_traps_to_ptp_rx_filter(ocelot_port->trap_proto);
 516
 517         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
 518 }
 519 EXPORT_SYMBOL(ocelot_hwstamp_get);
 520
 521 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
 522 {
 523         struct ocelot_port *ocelot_port = ocelot->ports[port];
 524         bool l2 = false, l4 = false;
 525         struct hwtstamp_config cfg;
 526         int err;
 527
 528         if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
 529                 return -EFAULT;
 530
 531         /* Tx type sanity check */
 532         switch (cfg.tx_type) {
 533         case HWTSTAMP_TX_ON:
 534                 ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
 535                 break;
 536         case HWTSTAMP_TX_ONESTEP_SYNC:
 537                 /* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
 538                  * need to update the origin time.
 539                  */
 540                 ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
 541                 break;
 542         case HWTSTAMP_TX_OFF:
 543                 ocelot_port->ptp_cmd = 0;
 544                 break;
 545         default:
 546                 return -ERANGE;
 547         }
 548
 549         switch (cfg.rx_filter) {
 550         case HWTSTAMP_FILTER_NONE:
 551                 break;
 552         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 553         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 554         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
 555                 l4 = true;
 556                 break;
 557         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
 558         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
 559         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
 560                 l2 = true;
 561                 break;
 562         case HWTSTAMP_FILTER_PTP_V2_EVENT:
 563         case HWTSTAMP_FILTER_PTP_V2_SYNC:
 564         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 565                 l2 = true;
 566                 l4 = true;
 567                 break;
 568         default:
 569                 return -ERANGE;
 570         }
 571
 572         err = ocelot_setup_ptp_traps(ocelot, port, l2, l4);
 573         if (err)
 574                 return err;
 575
 576         cfg.rx_filter = ocelot_traps_to_ptp_rx_filter(ocelot_port->trap_proto);
 577
 578         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
 579 }
 580 EXPORT_SYMBOL(ocelot_hwstamp_set);
 581
 582 int ocelot_get_ts_info(struct ocelot *ocelot, int port,
 583                        struct kernel_ethtool_ts_info *info)
 584 {
 585         if (ocelot->ptp_clock) {
 586                 info->phc_index = ptp_clock_index(ocelot->ptp_clock);
 587         } else {
 588                 info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE;
 589                 return 0;
 590         }
 591         info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
 592                                  SOF_TIMESTAMPING_TX_HARDWARE |
 593                                  SOF_TIMESTAMPING_RX_HARDWARE |
 594                                  SOF_TIMESTAMPING_RAW_HARDWARE;
 595         info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
 596                          BIT(HWTSTAMP_TX_ONESTEP_SYNC);
 597         info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
 598                            BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) |
 599                            BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
 600                            BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
 601
 602         return 0;
 603 }
 604 EXPORT_SYMBOL(ocelot_get_ts_info);
 605
 606 static int ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
 607                                         struct sk_buff *clone)
 608 {
 609         struct ocelot_port *ocelot_port = ocelot->ports[port];
 610         unsigned long flags;
 611
 612         spin_lock_irqsave(&ocelot->ts_id_lock, flags);
 613
 614         if (ocelot_port->ptp_skbs_in_flight == OCELOT_MAX_PTP_ID ||
 615             ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) {
 616                 spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
 617                 return -EBUSY;
 618         }
 619
 620         skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
 621         /* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */
 622         OCELOT_SKB_CB(clone)->ts_id = ocelot_port->ts_id;
 623
 624         ocelot_port->ts_id++;
 625         if (ocelot_port->ts_id == OCELOT_MAX_PTP_ID)
 626                 ocelot_port->ts_id = 0;
 627
 628         ocelot_port->ptp_skbs_in_flight++;
 629         ocelot->ptp_skbs_in_flight++;
 630
 631         skb_queue_tail(&ocelot_port->tx_skbs, clone);
 632
 633         spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
 634
 635         return 0;
 636 }
 637
 638 static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb,
 639                                        unsigned int ptp_class)
 640 {
 641         struct ptp_header *hdr;
 642         u8 msgtype, twostep;
 643
 644         hdr = ptp_parse_header(skb, ptp_class);
 645         if (!hdr)
 646                 return false;
 647
 648         msgtype = ptp_get_msgtype(hdr, ptp_class);
 649         twostep = hdr->flag_field[0] & 0x2;
 650
 651         if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0)
 652                 return true;
 653
 654         return false;
 655 }
 656
 657 int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port,
 658                                  struct sk_buff *skb,
 659                                  struct sk_buff **clone)
 660 {
 661         struct ocelot_port *ocelot_port = ocelot->ports[port];
 662         u8 ptp_cmd = ocelot_port->ptp_cmd;
 663         unsigned int ptp_class;
 664         int err;
 665
 666         /* Don't do anything if PTP timestamping not enabled */
 667         if (!ptp_cmd)
 668                 return 0;
 669
 670         ptp_class = ptp_classify_raw(skb);
 671         if (ptp_class == PTP_CLASS_NONE)
 672                 return -EINVAL;
 673
 674         /* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */
 675         if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) {
 676                 if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) {
 677                         OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
 678                         return 0;
 679                 }
 680
 681                 /* Fall back to two-step timestamping */
 682                 ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
 683         }
 684
 685         if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
 686                 *clone = skb_clone_sk(skb);
 687                 if (!(*clone))
 688                         return -ENOMEM;
 689
 690                 err = ocelot_port_add_txtstamp_skb(ocelot, port, *clone);
 691                 if (err)
 692                         return err;
 693
 694                 OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
 695                 OCELOT_SKB_CB(*clone)->ptp_class = ptp_class;
 696         }
 697
 698         return 0;
 699 }
 700 EXPORT_SYMBOL(ocelot_port_txtstamp_request);
 701
 702 static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
 703                                    struct timespec64 *ts)
 704 {
 705         unsigned long flags;
 706         u32 val;
 707
 708         spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
 709
 710         /* Read current PTP time to get seconds */
 711         val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
 712
 713         val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
 714         val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
 715         ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
 716         ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
 717
 718         /* Read packet HW timestamp from FIFO */
 719         val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
 720         ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
 721
 722         /* Sec has incremented since the ts was registered */
 723         if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
 724                 ts->tv_sec--;
 725
 726         spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
 727 }
 728
 729 static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid)
 730 {
 731         struct ptp_header *hdr;
 732
 733         hdr = ptp_parse_header(clone, OCELOT_SKB_CB(clone)->ptp_class);
 734         if (WARN_ON(!hdr))
 735                 return false;
 736
 737         return seqid == ntohs(hdr->sequence_id);
 738 }
 739
 740 void ocelot_get_txtstamp(struct ocelot *ocelot)
 741 {
 742         int budget = OCELOT_PTP_QUEUE_SZ;
 743
 744         while (budget--) {
 745                 struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
 746                 struct skb_shared_hwtstamps shhwtstamps;
 747                 u32 val, id, seqid, txport;
 748                 struct ocelot_port *port;
 749                 struct timespec64 ts;
 750                 unsigned long flags;
 751
 752                 val = ocelot_read(ocelot, SYS_PTP_STATUS);
 753
 754                 /* Check if a timestamp can be retrieved */
 755                 if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
 756                         break;
 757
 758                 WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
 759
 760                 /* Retrieve the ts ID and Tx port */
 761                 id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
 762                 txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
 763                 seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val);
 764
 765                 port = ocelot->ports[txport];
 766
 767                 spin_lock(&ocelot->ts_id_lock);
 768                 port->ptp_skbs_in_flight--;
 769                 ocelot->ptp_skbs_in_flight--;
 770                 spin_unlock(&ocelot->ts_id_lock);
 771
 772                 /* Retrieve its associated skb */
 773 try_again:
 774                 spin_lock_irqsave(&port->tx_skbs.lock, flags);
 775
 776                 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
 777                         if (OCELOT_SKB_CB(skb)->ts_id != id)
 778                                 continue;
 779                         __skb_unlink(skb, &port->tx_skbs);
 780                         skb_match = skb;
 781                         break;
 782                 }
 783
 784                 spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
 785
 786                 if (WARN_ON(!skb_match))
 787                         continue;
 788
 789                 if (!ocelot_validate_ptp_skb(skb_match, seqid)) {
 790                         dev_err_ratelimited(ocelot->dev,
 791                                             "port %d received stale TX timestamp for seqid %d, discarding\n",
 792                                             txport, seqid);
 793                         dev_kfree_skb_any(skb);
 794                         goto try_again;
 795                 }
 796
 797                 /* Get the h/w timestamp */
 798                 ocelot_get_hwtimestamp(ocelot, &ts);
 799
 800                 /* Set the timestamp into the skb */
 801                 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
 802                 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
 803                 skb_complete_tx_timestamp(skb_match, &shhwtstamps);
 804
 805                 /* Next ts */
 806                 ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
 807         }
 808 }
 809 EXPORT_SYMBOL(ocelot_get_txtstamp);
 810
 811 int ocelot_init_timestamp(struct ocelot *ocelot,
 812                           const struct ptp_clock_info *info)
 813 {
 814         struct ptp_clock *ptp_clock;
 815         int i;
 816
 817         ocelot->ptp_info = *info;
 818
 819         for (i = 0; i < OCELOT_PTP_PINS_NUM; i++) {
 820                 struct ptp_pin_desc *p = &ocelot->ptp_pins[i];
 821
 822                 snprintf(p->name, sizeof(p->name), "switch_1588_dat%d", i);
 823                 p->index = i;
 824                 p->func = PTP_PF_NONE;
 825         }
 826
 827         ocelot->ptp_info.pin_config = &ocelot->ptp_pins[0];
 828
 829         ptp_clock = ptp_clock_register(&ocelot->ptp_info, ocelot->dev);
 830         if (IS_ERR(ptp_clock))
 831                 return PTR_ERR(ptp_clock);
 832         /* Check if PHC support is missing at the configuration level */
 833         if (!ptp_clock)
 834                 return 0;
 835
 836         ocelot->ptp_clock = ptp_clock;
 837
 838         ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG);
 839         ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW);
 840         ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH);
 841
 842         ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC);
 843
 844         return 0;
 845 }
 846 EXPORT_SYMBOL(ocelot_init_timestamp);
 847
 848 int ocelot_deinit_timestamp(struct ocelot *ocelot)
 849 {
 850         if (ocelot->ptp_clock)
 851                 ptp_clock_unregister(ocelot->ptp_clock);
 852         return 0;
 853 }
 854 EXPORT_SYMBOL(ocelot_deinit_timestamp);