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 void ocelot_hwstamp_get(struct ocelot *ocelot, int port, 518 struct kernel_hwtstamp_config *cfg) 519 { 520 struct ocelot_port *ocelot_port = ocelot->ports[port]; 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 EXPORT_SYMBOL(ocelot_hwstamp_get); 537 538 int ocelot_hwstamp_set(struct ocelot *ocelot, int port, 539 struct kernel_hwtstamp_config *cfg, 540 struct netlink_ext_ack *extack) 541 { 542 struct ocelot_port *ocelot_port = ocelot->ports[port]; 543 bool l2 = false, l4 = false; 544 int ptp_cmd; 545 int err; 546 547 /* Tx type sanity check */ 548 err = ocelot_ptp_tx_type_to_cmd(cfg->tx_type, &ptp_cmd); 549 if (err) 550 return err; 551 552 switch (cfg->rx_filter) { 553 case HWTSTAMP_FILTER_NONE: 554 break; 555 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 556 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 557 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 558 l4 = true; 559 break; 560 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 561 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 562 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 563 l2 = true; 564 break; 565 case HWTSTAMP_FILTER_PTP_V2_EVENT: 566 case HWTSTAMP_FILTER_PTP_V2_SYNC: 567 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 568 l2 = true; 569 l4 = true; 570 break; 571 default: 572 return -ERANGE; 573 } 574 575 err = ocelot_setup_ptp_traps(ocelot, port, l2, l4); 576 if (err) 577 return err; 578 579 ocelot_port->ptp_cmd = ptp_cmd; 580 581 cfg->rx_filter = ocelot_traps_to_ptp_rx_filter(ocelot_port->trap_proto); 582 583 return 0; 584 } 585 EXPORT_SYMBOL(ocelot_hwstamp_set); 586 587 int ocelot_get_ts_info(struct ocelot *ocelot, int port, 588 struct kernel_ethtool_ts_info *info) 589 { 590 if (ocelot->ptp_clock) { 591 info->phc_index = ptp_clock_index(ocelot->ptp_clock); 592 } else { 593 info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE; 594 return 0; 595 } 596 info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE | 597 SOF_TIMESTAMPING_TX_HARDWARE | 598 SOF_TIMESTAMPING_RX_HARDWARE | 599 SOF_TIMESTAMPING_RAW_HARDWARE; 600 info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) | 601 BIT(HWTSTAMP_TX_ONESTEP_SYNC); 602 info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) | 603 BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) | 604 BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 605 BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT); 606 607 return 0; 608 } 609 EXPORT_SYMBOL(ocelot_get_ts_info); 610 611 static struct sk_buff *ocelot_port_dequeue_ptp_tx_skb(struct ocelot *ocelot, 612 int port, u8 ts_id, 613 u32 seqid) 614 { 615 struct ocelot_port *ocelot_port = ocelot->ports[port]; 616 struct sk_buff *skb, *skb_tmp, *skb_match = NULL; 617 struct ptp_header *hdr; 618 619 spin_lock(&ocelot->ts_id_lock); 620 621 skb_queue_walk_safe(&ocelot_port->tx_skbs, skb, skb_tmp) { 622 if (OCELOT_SKB_CB(skb)->ts_id != ts_id) 623 continue; 624 625 /* Check that the timestamp ID is for the expected PTP 626 * sequenceId. We don't have to test ptp_parse_header() against 627 * NULL, because we've pre-validated the packet's ptp_class. 628 */ 629 hdr = ptp_parse_header(skb, OCELOT_SKB_CB(skb)->ptp_class); 630 if (seqid != ntohs(hdr->sequence_id)) 631 continue; 632 633 __skb_unlink(skb, &ocelot_port->tx_skbs); 634 ocelot->ptp_skbs_in_flight--; 635 skb_match = skb; 636 break; 637 } 638 639 spin_unlock(&ocelot->ts_id_lock); 640 641 return skb_match; 642 } 643 644 static int ocelot_port_queue_ptp_tx_skb(struct ocelot *ocelot, int port, 645 struct sk_buff *clone) 646 { 647 struct ocelot_port *ocelot_port = ocelot->ports[port]; 648 DECLARE_BITMAP(ts_id_in_flight, OCELOT_MAX_PTP_ID); 649 struct sk_buff *skb, *skb_tmp; 650 unsigned long n; 651 652 spin_lock(&ocelot->ts_id_lock); 653 654 /* To get a better chance of acquiring a timestamp ID, first flush the 655 * stale packets still waiting in the TX timestamping queue. They are 656 * probably lost. 657 */ 658 skb_queue_walk_safe(&ocelot_port->tx_skbs, skb, skb_tmp) { 659 if (time_before(OCELOT_SKB_CB(skb)->ptp_tx_time + 660 OCELOT_PTP_TX_TSTAMP_TIMEOUT, jiffies)) { 661 u64_stats_update_begin(&ocelot_port->ts_stats->syncp); 662 ocelot_port->ts_stats->lost++; 663 u64_stats_update_end(&ocelot_port->ts_stats->syncp); 664 665 dev_dbg_ratelimited(ocelot->dev, 666 "port %d invalidating stale timestamp ID %u which seems lost\n", 667 port, OCELOT_SKB_CB(skb)->ts_id); 668 669 __skb_unlink(skb, &ocelot_port->tx_skbs); 670 kfree_skb(skb); 671 ocelot->ptp_skbs_in_flight--; 672 } else { 673 __set_bit(OCELOT_SKB_CB(skb)->ts_id, ts_id_in_flight); 674 } 675 } 676 677 if (ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) { 678 spin_unlock(&ocelot->ts_id_lock); 679 return -EBUSY; 680 } 681 682 n = find_first_zero_bit(ts_id_in_flight, OCELOT_MAX_PTP_ID); 683 if (n == OCELOT_MAX_PTP_ID) { 684 spin_unlock(&ocelot->ts_id_lock); 685 return -EBUSY; 686 } 687 688 /* Found an available timestamp ID, use it */ 689 OCELOT_SKB_CB(clone)->ts_id = n; 690 OCELOT_SKB_CB(clone)->ptp_tx_time = jiffies; 691 ocelot->ptp_skbs_in_flight++; 692 __skb_queue_tail(&ocelot_port->tx_skbs, clone); 693 694 spin_unlock(&ocelot->ts_id_lock); 695 696 dev_dbg_ratelimited(ocelot->dev, "port %d timestamp id %lu\n", port, n); 697 698 return 0; 699 } 700 701 static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb, 702 unsigned int ptp_class) 703 { 704 struct ptp_header *hdr; 705 u8 msgtype, twostep; 706 707 hdr = ptp_parse_header(skb, ptp_class); 708 if (!hdr) 709 return false; 710 711 msgtype = ptp_get_msgtype(hdr, ptp_class); 712 twostep = hdr->flag_field[0] & 0x2; 713 714 if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0) 715 return true; 716 717 return false; 718 } 719 720 int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port, 721 struct sk_buff *skb, 722 struct sk_buff **clone) 723 { 724 struct ocelot_port *ocelot_port = ocelot->ports[port]; 725 u8 ptp_cmd = ocelot_port->ptp_cmd; 726 unsigned int ptp_class; 727 int err; 728 729 /* Don't do anything if PTP timestamping not enabled */ 730 if (!ptp_cmd) 731 return 0; 732 733 ptp_class = ptp_classify_raw(skb); 734 if (ptp_class == PTP_CLASS_NONE) { 735 err = -EINVAL; 736 goto error; 737 } 738 739 /* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */ 740 if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) { 741 if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) { 742 OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; 743 744 u64_stats_update_begin(&ocelot_port->ts_stats->syncp); 745 ocelot_port->ts_stats->onestep_pkts_unconfirmed++; 746 u64_stats_update_end(&ocelot_port->ts_stats->syncp); 747 748 return 0; 749 } 750 751 /* Fall back to two-step timestamping */ 752 ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; 753 } 754 755 if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) { 756 *clone = skb_clone_sk(skb); 757 if (!(*clone)) { 758 err = -ENOMEM; 759 goto error; 760 } 761 762 /* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */ 763 err = ocelot_port_queue_ptp_tx_skb(ocelot, port, *clone); 764 if (err) { 765 kfree_skb(*clone); 766 goto error; 767 } 768 769 skb_shinfo(*clone)->tx_flags |= SKBTX_IN_PROGRESS; 770 OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd; 771 OCELOT_SKB_CB(*clone)->ptp_class = ptp_class; 772 } 773 774 return 0; 775 776 error: 777 u64_stats_update_begin(&ocelot_port->ts_stats->syncp); 778 ocelot_port->ts_stats->err++; 779 u64_stats_update_end(&ocelot_port->ts_stats->syncp); 780 return err; 781 } 782 EXPORT_SYMBOL(ocelot_port_txtstamp_request); 783 784 static void ocelot_get_hwtimestamp(struct ocelot *ocelot, 785 struct timespec64 *ts) 786 { 787 unsigned long flags; 788 u32 val; 789 790 spin_lock_irqsave(&ocelot->ptp_clock_lock, flags); 791 792 /* Read current PTP time to get seconds */ 793 val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN); 794 795 val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM); 796 val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE); 797 ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN); 798 ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN); 799 800 /* Read packet HW timestamp from FIFO */ 801 val = ocelot_read(ocelot, SYS_PTP_TXSTAMP); 802 ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val); 803 804 /* Sec has incremented since the ts was registered */ 805 if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC)) 806 ts->tv_sec--; 807 808 spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags); 809 } 810 811 void ocelot_get_txtstamp(struct ocelot *ocelot) 812 { 813 int budget = OCELOT_PTP_QUEUE_SZ; 814 815 while (budget--) { 816 struct skb_shared_hwtstamps shhwtstamps; 817 struct ocelot_port *ocelot_port; 818 u32 val, id, seqid, txport; 819 struct sk_buff *skb_match; 820 struct timespec64 ts; 821 822 val = ocelot_read(ocelot, SYS_PTP_STATUS); 823 824 /* Check if a timestamp can be retrieved */ 825 if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD)) 826 break; 827 828 WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL); 829 830 /* Retrieve the ts ID and Tx port */ 831 id = SYS_PTP_STATUS_PTP_MESS_ID_X(val); 832 txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val); 833 seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val); 834 ocelot_port = ocelot->ports[txport]; 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 u64_stats_update_begin(&ocelot_port->ts_stats->syncp); 841 ocelot_port->ts_stats->err++; 842 u64_stats_update_end(&ocelot_port->ts_stats->syncp); 843 844 dev_dbg_ratelimited(ocelot->dev, 845 "port %d received TX timestamp (seqid %d, ts id %u) for packet previously declared stale\n", 846 txport, seqid, id); 847 848 goto next_ts; 849 } 850 851 u64_stats_update_begin(&ocelot_port->ts_stats->syncp); 852 ocelot_port->ts_stats->pkts++; 853 u64_stats_update_end(&ocelot_port->ts_stats->syncp); 854 855 /* Get the h/w timestamp */ 856 ocelot_get_hwtimestamp(ocelot, &ts); 857 858 /* Set the timestamp into the skb */ 859 memset(&shhwtstamps, 0, sizeof(shhwtstamps)); 860 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec); 861 skb_complete_tx_timestamp(skb_match, &shhwtstamps); 862 863 next_ts: 864 ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT); 865 } 866 } 867 EXPORT_SYMBOL(ocelot_get_txtstamp); 868 869 int ocelot_init_timestamp(struct ocelot *ocelot, 870 const struct ptp_clock_info *info) 871 { 872 struct ptp_clock *ptp_clock; 873 int i; 874 875 ocelot->ptp_info = *info; 876 877 for (i = 0; i < OCELOT_PTP_PINS_NUM; i++) { 878 struct ptp_pin_desc *p = &ocelot->ptp_pins[i]; 879 880 snprintf(p->name, sizeof(p->name), "switch_1588_dat%d", i); 881 p->index = i; 882 p->func = PTP_PF_NONE; 883 } 884 885 ocelot->ptp_info.pin_config = &ocelot->ptp_pins[0]; 886 887 ptp_clock = ptp_clock_register(&ocelot->ptp_info, ocelot->dev); 888 if (IS_ERR(ptp_clock)) 889 return PTR_ERR(ptp_clock); 890 /* Check if PHC support is missing at the configuration level */ 891 if (!ptp_clock) 892 return 0; 893 894 ocelot->ptp_clock = ptp_clock; 895 896 ocelot_write(ocelot, SYS_PTP_CFG_PTP_STAMP_WID(30), SYS_PTP_CFG); 897 ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_LOW); 898 ocelot_write(ocelot, 0xffffffff, ANA_TABLES_PTP_ID_HIGH); 899 900 ocelot_write(ocelot, PTP_CFG_MISC_PTP_EN, PTP_CFG_MISC); 901 902 return 0; 903 } 904 EXPORT_SYMBOL(ocelot_init_timestamp); 905 906 int ocelot_deinit_timestamp(struct ocelot *ocelot) 907 { 908 if (ocelot->ptp_clock) 909 ptp_clock_unregister(ocelot->ptp_clock); 910 return 0; 911 } 912 EXPORT_SYMBOL(ocelot_deinit_timestamp); 913