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 /* Reject requests with unsupported flags */ 215 if (rq->perout.flags & ~(PTP_PEROUT_DUTY_CYCLE | 216 PTP_PEROUT_PHASE)) 217 return -EOPNOTSUPP; 218 219 pin = ptp_find_pin(ocelot->ptp_clock, PTP_PF_PEROUT, 220 rq->perout.index); 221 if (pin == 0) 222 ptp_pin = PTP_PIN_0; 223 else if (pin == 1) 224 ptp_pin = PTP_PIN_1; 225 else if (pin == 2) 226 ptp_pin = PTP_PIN_2; 227 else if (pin == 3) 228 ptp_pin = PTP_PIN_3; 229 else 230 return -EBUSY; 231 232 ts_period.tv_sec = rq->perout.period.sec; 233 ts_period.tv_nsec = rq->perout.period.nsec; 234 235 if (ts_period.tv_sec == 1 && ts_period.tv_nsec == 0) 236 pps = true; 237 238 /* Handle turning off */ 239 if (!on) { 240 spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); 241 val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_IDLE); 242 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin); 243 spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); 244 break; 245 } 246 247 if (rq->perout.flags & PTP_PEROUT_PHASE) { 248 ts_phase.tv_sec = rq->perout.phase.sec; 249 ts_phase.tv_nsec = rq->perout.phase.nsec; 250 } else { 251 /* Compatibility */ 252 ts_phase.tv_sec = rq->perout.start.sec; 253 ts_phase.tv_nsec = rq->perout.start.nsec; 254 } 255 if (ts_phase.tv_sec || (ts_phase.tv_nsec && !pps)) { 256 dev_warn(ocelot->dev, 257 "Absolute start time not supported!\n"); 258 dev_warn(ocelot->dev, 259 "Accept nsec for PPS phase adjustment, otherwise start time should be 0 0.\n"); 260 return -EINVAL; 261 } 262 263 /* Calculate waveform high and low times */ 264 if (rq->perout.flags & PTP_PEROUT_DUTY_CYCLE) { 265 struct timespec64 ts_on; 266 267 ts_on.tv_sec = rq->perout.on.sec; 268 ts_on.tv_nsec = rq->perout.on.nsec; 269 270 wf_high = timespec64_to_ns(&ts_on); 271 } else { 272 if (pps) { 273 wf_high = 1000; 274 } else { 275 wf_high = timespec64_to_ns(&ts_period); 276 wf_high = div_s64(wf_high, 2); 277 } 278 } 279 280 wf_low = timespec64_to_ns(&ts_period); 281 wf_low -= wf_high; 282 283 /* Handle PPS request */ 284 if (pps) { 285 spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); 286 ocelot_write_rix(ocelot, ts_phase.tv_nsec, 287 PTP_PIN_WF_LOW_PERIOD, ptp_pin); 288 ocelot_write_rix(ocelot, wf_high, 289 PTP_PIN_WF_HIGH_PERIOD, ptp_pin); 290 val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_CLOCK); 291 val |= PTP_PIN_CFG_SYNC; 292 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin); 293 spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); 294 break; 295 } 296 297 /* Handle periodic clock */ 298 if (wf_high > 0x3fffffff || wf_high <= 0x6) 299 return -EINVAL; 300 if (wf_low > 0x3fffffff || wf_low <= 0x6) 301 return -EINVAL; 302 303 spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); 304 ocelot_write_rix(ocelot, wf_low, PTP_PIN_WF_LOW_PERIOD, 305 ptp_pin); 306 ocelot_write_rix(ocelot, wf_high, PTP_PIN_WF_HIGH_PERIOD, 307 ptp_pin); 308 val = PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_CLOCK); 309 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, ptp_pin); 310 spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); 311 break; 312 default: 313 return -EOPNOTSUPP; 314 } 315 return 0; 316 } 317 EXPORT_SYMBOL(ocelot_ptp_enable); 318 319 static void ocelot_populate_l2_ptp_trap_key(struct ocelot_vcap_filter *trap) 320 { 321 trap->key_type = OCELOT_VCAP_KEY_ETYPE; 322 *(__be16 *)trap->key.etype.etype.value = htons(ETH_P_1588); 323 *(__be16 *)trap->key.etype.etype.mask = htons(0xffff); 324 } 325 326 static void 327 ocelot_populate_ipv4_ptp_event_trap_key(struct ocelot_vcap_filter *trap) 328 { 329 trap->key_type = OCELOT_VCAP_KEY_IPV4; 330 trap->key.ipv4.proto.value[0] = IPPROTO_UDP; 331 trap->key.ipv4.proto.mask[0] = 0xff; 332 trap->key.ipv4.dport.value = PTP_EV_PORT; 333 trap->key.ipv4.dport.mask = 0xffff; 334 } 335 336 static void 337 ocelot_populate_ipv6_ptp_event_trap_key(struct ocelot_vcap_filter *trap) 338 { 339 trap->key_type = OCELOT_VCAP_KEY_IPV6; 340 trap->key.ipv6.proto.value[0] = IPPROTO_UDP; 341 trap->key.ipv6.proto.mask[0] = 0xff; 342 trap->key.ipv6.dport.value = PTP_EV_PORT; 343 trap->key.ipv6.dport.mask = 0xffff; 344 } 345 346 static void 347 ocelot_populate_ipv4_ptp_general_trap_key(struct ocelot_vcap_filter *trap) 348 { 349 trap->key_type = OCELOT_VCAP_KEY_IPV4; 350 trap->key.ipv4.proto.value[0] = IPPROTO_UDP; 351 trap->key.ipv4.proto.mask[0] = 0xff; 352 trap->key.ipv4.dport.value = PTP_GEN_PORT; 353 trap->key.ipv4.dport.mask = 0xffff; 354 } 355 356 static void 357 ocelot_populate_ipv6_ptp_general_trap_key(struct ocelot_vcap_filter *trap) 358 { 359 trap->key_type = OCELOT_VCAP_KEY_IPV6; 360 trap->key.ipv6.proto.value[0] = IPPROTO_UDP; 361 trap->key.ipv6.proto.mask[0] = 0xff; 362 trap->key.ipv6.dport.value = PTP_GEN_PORT; 363 trap->key.ipv6.dport.mask = 0xffff; 364 } 365 366 static int ocelot_l2_ptp_trap_add(struct ocelot *ocelot, int port) 367 { 368 unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot); 369 370 return ocelot_trap_add(ocelot, port, l2_cookie, true, 371 ocelot_populate_l2_ptp_trap_key); 372 } 373 374 static int ocelot_l2_ptp_trap_del(struct ocelot *ocelot, int port) 375 { 376 unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot); 377 378 return ocelot_trap_del(ocelot, port, l2_cookie); 379 } 380 381 static int ocelot_ipv4_ptp_trap_add(struct ocelot *ocelot, int port) 382 { 383 unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot); 384 unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot); 385 int err; 386 387 err = ocelot_trap_add(ocelot, port, ipv4_ev_cookie, true, 388 ocelot_populate_ipv4_ptp_event_trap_key); 389 if (err) 390 return err; 391 392 err = ocelot_trap_add(ocelot, port, ipv4_gen_cookie, false, 393 ocelot_populate_ipv4_ptp_general_trap_key); 394 if (err) 395 ocelot_trap_del(ocelot, port, ipv4_ev_cookie); 396 397 return err; 398 } 399 400 static int ocelot_ipv4_ptp_trap_del(struct ocelot *ocelot, int port) 401 { 402 unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot); 403 unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot); 404 int err; 405 406 err = ocelot_trap_del(ocelot, port, ipv4_ev_cookie); 407 err |= ocelot_trap_del(ocelot, port, ipv4_gen_cookie); 408 return err; 409 } 410 411 static int ocelot_ipv6_ptp_trap_add(struct ocelot *ocelot, int port) 412 { 413 unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot); 414 unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot); 415 int err; 416 417 err = ocelot_trap_add(ocelot, port, ipv6_ev_cookie, true, 418 ocelot_populate_ipv6_ptp_event_trap_key); 419 if (err) 420 return err; 421 422 err = ocelot_trap_add(ocelot, port, ipv6_gen_cookie, false, 423 ocelot_populate_ipv6_ptp_general_trap_key); 424 if (err) 425 ocelot_trap_del(ocelot, port, ipv6_ev_cookie); 426 427 return err; 428 } 429 430 static int ocelot_ipv6_ptp_trap_del(struct ocelot *ocelot, int port) 431 { 432 unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot); 433 unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot); 434 int err; 435 436 err = ocelot_trap_del(ocelot, port, ipv6_ev_cookie); 437 err |= ocelot_trap_del(ocelot, port, ipv6_gen_cookie); 438 return err; 439 } 440 441 static int ocelot_setup_ptp_traps(struct ocelot *ocelot, int port, 442 bool l2, bool l4) 443 { 444 struct ocelot_port *ocelot_port = ocelot->ports[port]; 445 int err; 446 447 ocelot_port->trap_proto &= ~(OCELOT_PROTO_PTP_L2 | 448 OCELOT_PROTO_PTP_L4); 449 450 if (l2) 451 err = ocelot_l2_ptp_trap_add(ocelot, port); 452 else 453 err = ocelot_l2_ptp_trap_del(ocelot, port); 454 if (err) 455 return err; 456 457 if (l4) { 458 err = ocelot_ipv4_ptp_trap_add(ocelot, port); 459 if (err) 460 goto err_ipv4; 461 462 err = ocelot_ipv6_ptp_trap_add(ocelot, port); 463 if (err) 464 goto err_ipv6; 465 } else { 466 err = ocelot_ipv4_ptp_trap_del(ocelot, port); 467 468 err |= ocelot_ipv6_ptp_trap_del(ocelot, port); 469 } 470 if (err) 471 return err; 472 473 if (l2) 474 ocelot_port->trap_proto |= OCELOT_PROTO_PTP_L2; 475 if (l4) 476 ocelot_port->trap_proto |= OCELOT_PROTO_PTP_L4; 477 478 return 0; 479 480 err_ipv6: 481 ocelot_ipv4_ptp_trap_del(ocelot, port); 482 err_ipv4: 483 if (l2) 484 ocelot_l2_ptp_trap_del(ocelot, port); 485 return err; 486 } 487 488 static int ocelot_traps_to_ptp_rx_filter(unsigned int proto) 489 { 490 if ((proto & OCELOT_PROTO_PTP_L2) && (proto & OCELOT_PROTO_PTP_L4)) 491 return HWTSTAMP_FILTER_PTP_V2_EVENT; 492 else if (proto & OCELOT_PROTO_PTP_L2) 493 return HWTSTAMP_FILTER_PTP_V2_L2_EVENT; 494 else if (proto & OCELOT_PROTO_PTP_L4) 495 return HWTSTAMP_FILTER_PTP_V2_L4_EVENT; 496 497 return HWTSTAMP_FILTER_NONE; 498 } 499 500 static int ocelot_ptp_tx_type_to_cmd(int tx_type, int *ptp_cmd) 501 { 502 switch (tx_type) { 503 case HWTSTAMP_TX_ON: 504 *ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; 505 break; 506 case HWTSTAMP_TX_ONESTEP_SYNC: 507 /* IFH_REW_OP_ONE_STEP_PTP updates the correctionField, 508 * what we need to update is the originTimestamp. 509 */ 510 *ptp_cmd = IFH_REW_OP_ORIGIN_PTP; 511 break; 512 case HWTSTAMP_TX_OFF: 513 *ptp_cmd = 0; 514 break; 515 default: 516 return -ERANGE; 517 } 518 519 return 0; 520 } 521 522 int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr) 523 { 524 struct ocelot_port *ocelot_port = ocelot->ports[port]; 525 struct hwtstamp_config cfg = {}; 526 527 switch (ocelot_port->ptp_cmd) { 528 case IFH_REW_OP_TWO_STEP_PTP: 529 cfg.tx_type = HWTSTAMP_TX_ON; 530 break; 531 case IFH_REW_OP_ORIGIN_PTP: 532 cfg.tx_type = HWTSTAMP_TX_ONESTEP_SYNC; 533 break; 534 default: 535 cfg.tx_type = HWTSTAMP_TX_OFF; 536 break; 537 } 538 539 cfg.rx_filter = ocelot_traps_to_ptp_rx_filter(ocelot_port->trap_proto); 540 541 return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0; 542 } 543 EXPORT_SYMBOL(ocelot_hwstamp_get); 544 545 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr) 546 { 547 struct ocelot_port *ocelot_port = ocelot->ports[port]; 548 int ptp_cmd, old_ptp_cmd = ocelot_port->ptp_cmd; 549 bool l2 = false, l4 = false; 550 struct hwtstamp_config cfg; 551 bool old_l2, old_l4; 552 int err; 553 554 if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg))) 555 return -EFAULT; 556 557 /* Tx type sanity check */ 558 err = ocelot_ptp_tx_type_to_cmd(cfg.tx_type, &ptp_cmd); 559 if (err) 560 return err; 561 562 switch (cfg.rx_filter) { 563 case HWTSTAMP_FILTER_NONE: 564 break; 565 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 566 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 567 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 568 l4 = true; 569 break; 570 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 571 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 572 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 573 l2 = true; 574 break; 575 case HWTSTAMP_FILTER_PTP_V2_EVENT: 576 case HWTSTAMP_FILTER_PTP_V2_SYNC: 577 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 578 l2 = true; 579 l4 = true; 580 break; 581 default: 582 return -ERANGE; 583 } 584 585 old_l2 = ocelot_port->trap_proto & OCELOT_PROTO_PTP_L2; 586 old_l4 = ocelot_port->trap_proto & OCELOT_PROTO_PTP_L4; 587 588 err = ocelot_setup_ptp_traps(ocelot, port, l2, l4); 589 if (err) 590 return err; 591 592 ocelot_port->ptp_cmd = ptp_cmd; 593 594 cfg.rx_filter = ocelot_traps_to_ptp_rx_filter(ocelot_port->trap_proto); 595 596 if (copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg))) { 597 err = -EFAULT; 598 goto out_restore_ptp_traps; 599 } 600 601 return 0; 602 out_restore_ptp_traps: 603 ocelot_setup_ptp_traps(ocelot, port, old_l2, old_l4); 604 ocelot_port->ptp_cmd = old_ptp_cmd; 605 return err; 606 } 607 EXPORT_SYMBOL(ocelot_hwstamp_set); 608 609 int ocelot_get_ts_info(struct ocelot *ocelot, int port, 610 struct kernel_ethtool_ts_info *info) 611 { 612 if (ocelot->ptp_clock) { 613 info->phc_index = ptp_clock_index(ocelot->ptp_clock); 614 } else { 615 info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE; 616 return 0; 617 } 618 info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE | 619 SOF_TIMESTAMPING_TX_HARDWARE | 620 SOF_TIMESTAMPING_RX_HARDWARE | 621 SOF_TIMESTAMPING_RAW_HARDWARE; 622 info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) | 623 BIT(HWTSTAMP_TX_ONESTEP_SYNC); 624 info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | 625 BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) | 626 BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 627 BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT); 628 629 return 0; 630 } 631 EXPORT_SYMBOL(ocelot_get_ts_info); 632 633 static struct sk_buff *ocelot_port_dequeue_ptp_tx_skb(struct ocelot *ocelot, 634 int port, u8 ts_id, 635 u32 seqid) 636 { 637 struct ocelot_port *ocelot_port = ocelot->ports[port]; 638 struct sk_buff *skb, *skb_tmp, *skb_match = NULL; 639 struct ptp_header *hdr; 640 641 spin_lock(&ocelot->ts_id_lock); 642 643 skb_queue_walk_safe(&ocelot_port->tx_skbs, skb, skb_tmp) { 644 if (OCELOT_SKB_CB(skb)->ts_id != ts_id) 645 continue; 646 647 /* Check that the timestamp ID is for the expected PTP 648 * sequenceId. We don't have to test ptp_parse_header() against 649 * NULL, because we've pre-validated the packet's ptp_class. 650 */ 651 hdr = ptp_parse_header(skb, OCELOT_SKB_CB(skb)->ptp_class); 652 if (seqid != ntohs(hdr->sequence_id)) 653 continue; 654 655 __skb_unlink(skb, &ocelot_port->tx_skbs); 656 ocelot->ptp_skbs_in_flight--; 657 skb_match = skb; 658 break; 659 } 660 661 spin_unlock(&ocelot->ts_id_lock); 662 663 return skb_match; 664 } 665 666 static int ocelot_port_queue_ptp_tx_skb(struct ocelot *ocelot, int port, 667 struct sk_buff *clone) 668 { 669 struct ocelot_port *ocelot_port = ocelot->ports[port]; 670 DECLARE_BITMAP(ts_id_in_flight, OCELOT_MAX_PTP_ID); 671 struct sk_buff *skb, *skb_tmp; 672 unsigned long n; 673 674 spin_lock(&ocelot->ts_id_lock); 675 676 /* To get a better chance of acquiring a timestamp ID, first flush the 677 * stale packets still waiting in the TX timestamping queue. They are 678 * probably lost. 679 */ 680 skb_queue_walk_safe(&ocelot_port->tx_skbs, skb, skb_tmp) { 681 if (time_before(OCELOT_SKB_CB(skb)->ptp_tx_time + 682 OCELOT_PTP_TX_TSTAMP_TIMEOUT, jiffies)) { 683 dev_warn_ratelimited(ocelot->dev, 684 "port %d invalidating stale timestamp ID %u which seems lost\n", 685 port, OCELOT_SKB_CB(skb)->ts_id); 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 return -EINVAL; 753 754 /* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */ 755 if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) { 756 if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) { 757 OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; 758 return 0; 759 } 760 761 /* Fall back to two-step timestamping */ 762 ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; 763 } 764 765 if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) { 766 *clone = skb_clone_sk(skb); 767 if (!(*clone)) 768 return -ENOMEM; 769 770 /* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */ 771 err = ocelot_port_queue_ptp_tx_skb(ocelot, port, *clone); 772 if (err) { 773 kfree_skb(*clone); 774 return err; 775 } 776 777 skb_shinfo(*clone)->tx_flags |= SKBTX_IN_PROGRESS; 778 OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; 779 OCELOT_SKB_CB(*clone)->ptp_class = ptp_class; 780 } 781 782 return 0; 783 } 784 EXPORT_SYMBOL(ocelot_port_txtstamp_request); 785 786 static void ocelot_get_hwtimestamp(struct ocelot *ocelot, 787 struct timespec64 *ts) 788 { 789 unsigned long flags; 790 u32 val; 791 792 spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); 793 794 /* Read current PTP time to get seconds */ 795 val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN); 796 797 val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM); 798 val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE); 799 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN); 800 ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN); 801 802 /* Read packet HW timestamp from FIFO */ 803 val = ocelot_read(ocelot, SYS_PTP_TXSTAMP); 804 ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val); 805 806 /* Sec has incremented since the ts was registered */ 807 if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC)) 808 ts->tv_sec--; 809 810 spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); 811 } 812 813 void ocelot_get_txtstamp(struct ocelot *ocelot) 814 { 815 int budget = OCELOT_PTP_QUEUE_SZ; 816 817 while (budget--) { 818 struct skb_shared_hwtstamps shhwtstamps; 819 u32 val, id, seqid, txport; 820 struct sk_buff *skb_match; 821 struct timespec64 ts; 822 823 val = ocelot_read(ocelot, SYS_PTP_STATUS); 824 825 /* Check if a timestamp can be retrieved */ 826 if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD)) 827 break; 828 829 WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL); 830 831 /* Retrieve the ts ID and Tx port */ 832 id = SYS_PTP_STATUS_PTP_MESS_ID_X(val); 833 txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val); 834 seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val); 835 836 /* Retrieve its associated skb */ 837 skb_match = ocelot_port_dequeue_ptp_tx_skb(ocelot, txport, id, 838 seqid); 839 if (!skb_match) { 840 dev_warn_ratelimited(ocelot->dev, 841 "port %d received TX timestamp (seqid %d, ts id %u) for packet previously declared stale\n", 842 txport, seqid, id); 843 goto next_ts; 844 } 845 846 /* Get the h/w timestamp */ 847 ocelot_get_hwtimestamp(ocelot, &ts); 848 849 /* Set the timestamp into the skb */ 850 memset(&shhwtstamps, 0, sizeof(shhwtstamps)); 851 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec); 852 skb_complete_tx_timestamp(skb_match, &shhwtstamps); 853 854 next_ts: 855 ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT); 856 } 857 } 858 EXPORT_SYMBOL(ocelot_get_txtstamp); 859 860 int ocelot_init_timestamp(struct ocelot *ocelot, 861 const struct ptp_clock_info *info) 862 { 863 struct ptp_clock *ptp_clock; 864 int i; 865 866 ocelot->ptp_info = *info; 867 868 for (i = 0; i < OCELOT_PTP_PINS_NUM; i++) { 869 struct ptp_pin_desc *p = &ocelot->ptp_pins[i]; 870 871 snprintf(p->name, sizeof(p->name), "switch_1588_dat%d", i); 872 p->index = i; 873 p->func = PTP_PF_NONE; 874 } 875 876 ocelot->ptp_info.pin_config = &ocelot->ptp_pins[0]; 877 878 ptp_clock = ptp_clock_register(&ocelot->ptp_info, ocelot->dev); 879 if (IS_ERR(ptp_clock)) 880 return PTR_ERR(ptp_clock); 881 /* Check if PHC support is missing at the configuration level */ 882 if (!ptp_clock) 883 return 0; 884 885 ocelot->ptp_clock = ptp_clock; 886 887 ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG); 888 ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW); 889 ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH); 890 891 ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC); 892 893 return 0; 894 } 895 EXPORT_SYMBOL(ocelot_init_timestamp); 896 897 int ocelot_deinit_timestamp(struct ocelot *ocelot) 898 { 899 if (ocelot->ptp_clock) 900 ptp_clock_unregister(ocelot->ptp_clock); 901 return 0; 902 } 903 EXPORT_SYMBOL(ocelot_deinit_timestamp); 904