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