1 // SPDX-License-Identifier: GPL-2.0+ 2 /* Microchip Sparx5 Switch driver 3 * 4 * Copyright (c) 2021 Microchip Technology Inc. and its subsidiaries. 5 * 6 * The Sparx5 Chip Register Model can be browsed at this location: 7 * https://github.com/microchip-ung/sparx-5_reginfo 8 */ 9 #include <linux/ptp_classify.h> 10 11 #include "sparx5_main_regs.h" 12 #include "sparx5_main.h" 13 14 #define TOD_ACC_PIN 0x4 15 16 enum { 17 PTP_PIN_ACTION_IDLE = 0, 18 PTP_PIN_ACTION_LOAD, 19 PTP_PIN_ACTION_SAVE, 20 PTP_PIN_ACTION_CLOCK, 21 PTP_PIN_ACTION_DELTA, 22 PTP_PIN_ACTION_TOD 23 }; 24 25 static u64 sparx5_ptp_get_1ppm(struct sparx5 *sparx5) 26 { 27 /* Represents 1ppm adjustment in 2^59 format with 1.59687500000(625) 28 * 1.99609375000(500), 3.99218750000(250) as reference 29 * The value is calculated as following: 30 * (1/1000000)/((2^-59)/X) 31 */ 32 33 u64 res = 0; 34 35 switch (sparx5->coreclock) { 36 case SPX5_CORE_CLOCK_250MHZ: 37 res = 2301339409586; 38 break; 39 case SPX5_CORE_CLOCK_328MHZ: 40 res = 1756832768924; 41 break; 42 case SPX5_CORE_CLOCK_500MHZ: 43 res = 1150669704793; 44 break; 45 case SPX5_CORE_CLOCK_625MHZ: 46 res = 920535763834; 47 break; 48 default: 49 WARN(1, "Invalid core clock"); 50 break; 51 } 52 53 return res; 54 } 55 56 static u64 sparx5_ptp_get_nominal_value(struct sparx5 *sparx5) 57 { 58 u64 res = 0; 59 60 switch (sparx5->coreclock) { 61 case SPX5_CORE_CLOCK_250MHZ: 62 res = 0x1FF0000000000000; 63 break; 64 case SPX5_CORE_CLOCK_328MHZ: 65 res = 0x18604697DD0F9B5B; 66 break; 67 case SPX5_CORE_CLOCK_500MHZ: 68 res = 0x0FF8000000000000; 69 break; 70 case SPX5_CORE_CLOCK_625MHZ: 71 res = 0x0CC6666666666666; 72 break; 73 default: 74 WARN(1, "Invalid core clock"); 75 break; 76 } 77 78 return res; 79 } 80 81 int sparx5_ptp_hwtstamp_set(struct sparx5_port *port, 82 struct kernel_hwtstamp_config *cfg, 83 struct netlink_ext_ack *extack) 84 { 85 struct sparx5 *sparx5 = port->sparx5; 86 struct sparx5_phc *phc; 87 88 /* For now don't allow to run ptp on ports that are part of a bridge, 89 * because in case of transparent clock the HW will still forward the 90 * frames, so there would be duplicate frames 91 */ 92 93 if (test_bit(port->portno, sparx5->bridge_mask)) 94 return -EINVAL; 95 96 switch (cfg->tx_type) { 97 case HWTSTAMP_TX_ON: 98 port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP; 99 break; 100 case HWTSTAMP_TX_ONESTEP_SYNC: 101 port->ptp_cmd = IFH_REW_OP_ONE_STEP_PTP; 102 break; 103 case HWTSTAMP_TX_OFF: 104 port->ptp_cmd = IFH_REW_OP_NOOP; 105 break; 106 default: 107 return -ERANGE; 108 } 109 110 switch (cfg->rx_filter) { 111 case HWTSTAMP_FILTER_NONE: 112 break; 113 case HWTSTAMP_FILTER_ALL: 114 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: 115 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: 116 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: 117 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 118 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 119 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 120 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 121 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 122 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 123 case HWTSTAMP_FILTER_PTP_V2_EVENT: 124 case HWTSTAMP_FILTER_PTP_V2_SYNC: 125 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 126 case HWTSTAMP_FILTER_NTP_ALL: 127 cfg->rx_filter = HWTSTAMP_FILTER_ALL; 128 break; 129 default: 130 return -ERANGE; 131 } 132 133 /* Commit back the result & save it */ 134 mutex_lock(&sparx5->ptp_lock); 135 phc = &sparx5->phc[SPARX5_PHC_PORT]; 136 phc->hwtstamp_config = *cfg; 137 mutex_unlock(&sparx5->ptp_lock); 138 139 return 0; 140 } 141 142 void sparx5_ptp_hwtstamp_get(struct sparx5_port *port, 143 struct kernel_hwtstamp_config *cfg) 144 { 145 struct sparx5 *sparx5 = port->sparx5; 146 struct sparx5_phc *phc; 147 148 phc = &sparx5->phc[SPARX5_PHC_PORT]; 149 *cfg = phc->hwtstamp_config; 150 } 151 152 static void sparx5_ptp_classify(struct sparx5_port *port, struct sk_buff *skb, 153 u8 *rew_op, u8 *pdu_type, u8 *pdu_w16_offset) 154 { 155 struct ptp_header *header; 156 u8 msgtype; 157 int type; 158 159 if (port->ptp_cmd == IFH_REW_OP_NOOP) { 160 *rew_op = IFH_REW_OP_NOOP; 161 *pdu_type = IFH_PDU_TYPE_NONE; 162 *pdu_w16_offset = 0; 163 return; 164 } 165 166 type = ptp_classify_raw(skb); 167 if (type == PTP_CLASS_NONE) { 168 *rew_op = IFH_REW_OP_NOOP; 169 *pdu_type = IFH_PDU_TYPE_NONE; 170 *pdu_w16_offset = 0; 171 return; 172 } 173 174 header = ptp_parse_header(skb, type); 175 if (!header) { 176 *rew_op = IFH_REW_OP_NOOP; 177 *pdu_type = IFH_PDU_TYPE_NONE; 178 *pdu_w16_offset = 0; 179 return; 180 } 181 182 *pdu_w16_offset = 7; 183 if (type & PTP_CLASS_L2) 184 *pdu_type = IFH_PDU_TYPE_PTP; 185 if (type & PTP_CLASS_IPV4) 186 *pdu_type = IFH_PDU_TYPE_IPV4_UDP_PTP; 187 if (type & PTP_CLASS_IPV6) 188 *pdu_type = IFH_PDU_TYPE_IPV6_UDP_PTP; 189 190 if (port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) { 191 *rew_op = IFH_REW_OP_TWO_STEP_PTP; 192 return; 193 } 194 195 /* If it is sync and run 1 step then set the correct operation, 196 * otherwise run as 2 step 197 */ 198 msgtype = ptp_get_msgtype(header, type); 199 if ((msgtype & 0xf) == 0) { 200 *rew_op = IFH_REW_OP_ONE_STEP_PTP; 201 return; 202 } 203 204 *rew_op = IFH_REW_OP_TWO_STEP_PTP; 205 } 206 207 static void sparx5_ptp_txtstamp_old_release(struct sparx5_port *port) 208 { 209 struct sk_buff *skb, *skb_tmp; 210 unsigned long flags; 211 212 spin_lock_irqsave(&port->tx_skbs.lock, flags); 213 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { 214 if time_after(SPARX5_SKB_CB(skb)->jiffies + SPARX5_PTP_TIMEOUT, 215 jiffies) 216 break; 217 218 __skb_unlink(skb, &port->tx_skbs); 219 dev_kfree_skb_any(skb); 220 } 221 spin_unlock_irqrestore(&port->tx_skbs.lock, flags); 222 } 223 224 int sparx5_ptp_txtstamp_request(struct sparx5_port *port, 225 struct sk_buff *skb) 226 { 227 struct sparx5 *sparx5 = port->sparx5; 228 u8 rew_op, pdu_type, pdu_w16_offset; 229 unsigned long flags; 230 231 sparx5_ptp_classify(port, skb, &rew_op, &pdu_type, &pdu_w16_offset); 232 SPARX5_SKB_CB(skb)->rew_op = rew_op; 233 SPARX5_SKB_CB(skb)->pdu_type = pdu_type; 234 SPARX5_SKB_CB(skb)->pdu_w16_offset = pdu_w16_offset; 235 236 if (rew_op != IFH_REW_OP_TWO_STEP_PTP) 237 return 0; 238 239 sparx5_ptp_txtstamp_old_release(port); 240 241 spin_lock_irqsave(&sparx5->ptp_ts_id_lock, flags); 242 if (sparx5->ptp_skbs == SPARX5_MAX_PTP_ID) { 243 spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags); 244 return -EBUSY; 245 } 246 247 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; 248 249 skb_queue_tail(&port->tx_skbs, skb); 250 SPARX5_SKB_CB(skb)->ts_id = port->ts_id; 251 SPARX5_SKB_CB(skb)->jiffies = jiffies; 252 253 sparx5->ptp_skbs++; 254 port->ts_id++; 255 if (port->ts_id == SPARX5_MAX_PTP_ID) 256 port->ts_id = 0; 257 258 spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags); 259 260 return 0; 261 } 262 263 void sparx5_ptp_txtstamp_release(struct sparx5_port *port, 264 struct sk_buff *skb) 265 { 266 struct sparx5 *sparx5 = port->sparx5; 267 unsigned long flags; 268 269 spin_lock_irqsave(&sparx5->ptp_ts_id_lock, flags); 270 port->ts_id--; 271 sparx5->ptp_skbs--; 272 skb_unlink(skb, &port->tx_skbs); 273 spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags); 274 } 275 276 void sparx5_get_hwtimestamp(struct sparx5 *sparx5, 277 struct timespec64 *ts, 278 u32 nsec) 279 { 280 /* Read current PTP time to get seconds */ 281 const struct sparx5_consts *consts = sparx5->data->consts; 282 unsigned long flags; 283 u32 curr_nsec; 284 285 spin_lock_irqsave(&sparx5->ptp_clock_lock, flags); 286 287 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 288 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(SPARX5_PHC_PORT) | 289 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0), 290 PTP_PTP_PIN_CFG_PTP_PIN_ACTION | 291 PTP_PTP_PIN_CFG_PTP_PIN_DOM | 292 PTP_PTP_PIN_CFG_PTP_PIN_SYNC, 293 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin)); 294 295 ts->tv_sec = spx5_rd(sparx5, PTP_PTP_TOD_SEC_LSB(consts->tod_pin)); 296 curr_nsec = spx5_rd(sparx5, PTP_PTP_TOD_NSEC(consts->tod_pin)); 297 298 ts->tv_nsec = nsec; 299 300 /* Sec has incremented since the ts was registered */ 301 if (curr_nsec < nsec) 302 ts->tv_sec--; 303 304 spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags); 305 } 306 307 irqreturn_t sparx5_ptp_irq_handler(int irq, void *args) 308 { 309 int budget = SPARX5_MAX_PTP_ID; 310 struct sparx5 *sparx5 = args; 311 312 while (budget--) { 313 struct sk_buff *skb, *skb_tmp, *skb_match = NULL; 314 struct skb_shared_hwtstamps shhwtstamps; 315 struct sparx5_port *port; 316 struct timespec64 ts; 317 unsigned long flags; 318 u32 val, id, txport; 319 u32 delay; 320 321 val = spx5_rd(sparx5, REW_PTP_TWOSTEP_CTRL); 322 323 /* Check if a timestamp can be retrieved */ 324 if (!(val & REW_PTP_TWOSTEP_CTRL_PTP_VLD)) 325 break; 326 327 WARN_ON(val & REW_PTP_TWOSTEP_CTRL_PTP_OVFL); 328 329 if (!(val & REW_PTP_TWOSTEP_CTRL_STAMP_TX)) 330 continue; 331 332 /* Retrieve the ts Tx port */ 333 txport = REW_PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val); 334 335 /* Retrieve its associated skb */ 336 port = sparx5->ports[txport]; 337 338 /* Retrieve the delay */ 339 delay = spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP); 340 delay = REW_PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay); 341 342 /* Get next timestamp from fifo, which needs to be the 343 * rx timestamp which represents the id of the frame 344 */ 345 spx5_rmw(REW_PTP_TWOSTEP_CTRL_PTP_NXT_SET(1), 346 REW_PTP_TWOSTEP_CTRL_PTP_NXT, 347 sparx5, REW_PTP_TWOSTEP_CTRL); 348 349 val = spx5_rd(sparx5, REW_PTP_TWOSTEP_CTRL); 350 351 /* Check if a timestamp can be retried */ 352 if (!(val & REW_PTP_TWOSTEP_CTRL_PTP_VLD)) 353 break; 354 355 /* Read RX timestamping to get the ID */ 356 id = spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP); 357 id <<= 8; 358 id |= spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP_SUBNS); 359 360 spin_lock_irqsave(&port->tx_skbs.lock, flags); 361 skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) { 362 if (SPARX5_SKB_CB(skb)->ts_id != id) 363 continue; 364 365 __skb_unlink(skb, &port->tx_skbs); 366 skb_match = skb; 367 break; 368 } 369 spin_unlock_irqrestore(&port->tx_skbs.lock, flags); 370 371 /* Next ts */ 372 spx5_rmw(REW_PTP_TWOSTEP_CTRL_PTP_NXT_SET(1), 373 REW_PTP_TWOSTEP_CTRL_PTP_NXT, 374 sparx5, REW_PTP_TWOSTEP_CTRL); 375 376 if (WARN_ON(!skb_match)) 377 continue; 378 379 spin_lock(&sparx5->ptp_ts_id_lock); 380 sparx5->ptp_skbs--; 381 spin_unlock(&sparx5->ptp_ts_id_lock); 382 383 /* Get the h/w timestamp */ 384 sparx5_get_hwtimestamp(sparx5, &ts, delay); 385 386 /* Set the timestamp into the skb */ 387 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec); 388 skb_tstamp_tx(skb_match, &shhwtstamps); 389 390 dev_kfree_skb_any(skb_match); 391 } 392 393 return IRQ_HANDLED; 394 } 395 396 static int sparx5_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 397 { 398 struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info); 399 struct sparx5 *sparx5 = phc->sparx5; 400 unsigned long flags; 401 bool neg_adj = 0; 402 u64 tod_inc; 403 u64 ref; 404 405 if (!scaled_ppm) 406 return 0; 407 408 if (scaled_ppm < 0) { 409 neg_adj = 1; 410 scaled_ppm = -scaled_ppm; 411 } 412 413 tod_inc = sparx5_ptp_get_nominal_value(sparx5); 414 415 /* The multiplication is split in 2 separate additions because of 416 * overflow issues. If scaled_ppm with 16bit fractional part was bigger 417 * than 20ppm then we got overflow. 418 */ 419 ref = sparx5_ptp_get_1ppm(sparx5) * (scaled_ppm >> 16); 420 ref += (sparx5_ptp_get_1ppm(sparx5) * (0xffff & scaled_ppm)) >> 16; 421 tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref; 422 423 spin_lock_irqsave(&sparx5->ptp_clock_lock, flags); 424 425 spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(1 << BIT(phc->index)), 426 PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS, 427 sparx5, PTP_PTP_DOM_CFG); 428 429 spx5_wr((u32)tod_inc & 0xFFFFFFFF, sparx5, 430 PTP_CLK_PER_CFG(phc->index, 0)); 431 spx5_wr((u32)(tod_inc >> 32), sparx5, 432 PTP_CLK_PER_CFG(phc->index, 1)); 433 434 spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0), 435 PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS, sparx5, 436 PTP_PTP_DOM_CFG); 437 438 spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags); 439 440 return 0; 441 } 442 443 static int sparx5_ptp_settime64(struct ptp_clock_info *ptp, 444 const struct timespec64 *ts) 445 { 446 struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info); 447 struct sparx5 *sparx5 = phc->sparx5; 448 const struct sparx5_consts *consts; 449 unsigned long flags; 450 451 consts = sparx5->data->consts; 452 453 spin_lock_irqsave(&sparx5->ptp_clock_lock, flags); 454 455 /* Must be in IDLE mode before the time can be loaded */ 456 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 457 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) | 458 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0), 459 PTP_PTP_PIN_CFG_PTP_PIN_ACTION | 460 PTP_PTP_PIN_CFG_PTP_PIN_DOM | 461 PTP_PTP_PIN_CFG_PTP_PIN_SYNC, 462 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin)); 463 464 /* Set new value */ 465 spx5_wr(PTP_PTP_TOD_SEC_MSB_PTP_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)), 466 sparx5, PTP_PTP_TOD_SEC_MSB(consts->tod_pin)); 467 spx5_wr(lower_32_bits(ts->tv_sec), 468 sparx5, PTP_PTP_TOD_SEC_LSB(consts->tod_pin)); 469 spx5_wr(ts->tv_nsec, sparx5, PTP_PTP_TOD_NSEC(consts->tod_pin)); 470 471 /* Apply new values */ 472 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) | 473 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) | 474 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0), 475 PTP_PTP_PIN_CFG_PTP_PIN_ACTION | 476 PTP_PTP_PIN_CFG_PTP_PIN_DOM | 477 PTP_PTP_PIN_CFG_PTP_PIN_SYNC, 478 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin)); 479 480 spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags); 481 482 return 0; 483 } 484 485 int sparx5_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts) 486 { 487 struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info); 488 struct sparx5 *sparx5 = phc->sparx5; 489 const struct sparx5_consts *consts; 490 unsigned long flags; 491 time64_t s; 492 s64 ns; 493 494 consts = sparx5->data->consts; 495 496 spin_lock_irqsave(&sparx5->ptp_clock_lock, flags); 497 498 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) | 499 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) | 500 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0), 501 PTP_PTP_PIN_CFG_PTP_PIN_ACTION | 502 PTP_PTP_PIN_CFG_PTP_PIN_DOM | 503 PTP_PTP_PIN_CFG_PTP_PIN_SYNC, 504 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin)); 505 506 s = spx5_rd(sparx5, PTP_PTP_TOD_SEC_MSB(consts->tod_pin)); 507 s <<= 32; 508 s |= spx5_rd(sparx5, PTP_PTP_TOD_SEC_LSB(consts->tod_pin)); 509 ns = spx5_rd(sparx5, PTP_PTP_TOD_NSEC(consts->tod_pin)); 510 ns &= PTP_PTP_TOD_NSEC_PTP_TOD_NSEC; 511 512 spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags); 513 514 /* Deal with negative values */ 515 if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) { 516 s--; 517 ns &= 0xf; 518 ns += 999999984; 519 } 520 521 set_normalized_timespec64(ts, s, ns); 522 return 0; 523 } 524 525 static int sparx5_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 526 { 527 struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info); 528 struct sparx5 *sparx5 = phc->sparx5; 529 const struct sparx5_consts *consts; 530 531 consts = sparx5->data->consts; 532 533 if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) { 534 unsigned long flags; 535 536 spin_lock_irqsave(&sparx5->ptp_clock_lock, flags); 537 538 /* Must be in IDLE mode before the time can be loaded */ 539 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) | 540 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) | 541 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0), 542 PTP_PTP_PIN_CFG_PTP_PIN_ACTION | 543 PTP_PTP_PIN_CFG_PTP_PIN_DOM | 544 PTP_PTP_PIN_CFG_PTP_PIN_SYNC, 545 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin)); 546 547 spx5_wr(PTP_PTP_TOD_NSEC_PTP_TOD_NSEC_SET(delta), 548 sparx5, PTP_PTP_TOD_NSEC(consts->tod_pin)); 549 550 /* Adjust time with the value of PTP_TOD_NSEC */ 551 spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) | 552 PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) | 553 PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0), 554 PTP_PTP_PIN_CFG_PTP_PIN_ACTION | 555 PTP_PTP_PIN_CFG_PTP_PIN_DOM | 556 PTP_PTP_PIN_CFG_PTP_PIN_SYNC, 557 sparx5, PTP_PTP_PIN_CFG(consts->tod_pin)); 558 559 spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags); 560 } else { 561 /* Fall back using sparx5_ptp_settime64 which is not exact */ 562 struct timespec64 ts; 563 u64 now; 564 565 sparx5_ptp_gettime64(ptp, &ts); 566 567 now = ktime_to_ns(timespec64_to_ktime(ts)); 568 ts = ns_to_timespec64(now + delta); 569 570 sparx5_ptp_settime64(ptp, &ts); 571 } 572 573 return 0; 574 } 575 576 static struct ptp_clock_info sparx5_ptp_clock_info = { 577 .owner = THIS_MODULE, 578 .name = "sparx5 ptp", 579 .max_adj = 200000, 580 .gettime64 = sparx5_ptp_gettime64, 581 .settime64 = sparx5_ptp_settime64, 582 .adjtime = sparx5_ptp_adjtime, 583 .adjfine = sparx5_ptp_adjfine, 584 }; 585 586 static int sparx5_ptp_phc_init(struct sparx5 *sparx5, 587 int index, 588 struct ptp_clock_info *clock_info) 589 { 590 struct sparx5_phc *phc = &sparx5->phc[index]; 591 592 phc->info = *clock_info; 593 phc->clock = ptp_clock_register(&phc->info, sparx5->dev); 594 if (IS_ERR(phc->clock)) 595 return PTR_ERR(phc->clock); 596 597 phc->index = index; 598 phc->sparx5 = sparx5; 599 600 /* PTP Rx stamping is always enabled. */ 601 phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 602 603 return 0; 604 } 605 606 int sparx5_ptp_init(struct sparx5 *sparx5) 607 { 608 u64 tod_adj = sparx5_ptp_get_nominal_value(sparx5); 609 struct sparx5_port *port; 610 int err, i; 611 612 if (!sparx5->ptp) 613 return 0; 614 615 for (i = 0; i < SPARX5_PHC_COUNT; ++i) { 616 err = sparx5_ptp_phc_init(sparx5, i, &sparx5_ptp_clock_info); 617 if (err) 618 return err; 619 } 620 621 spin_lock_init(&sparx5->ptp_clock_lock); 622 spin_lock_init(&sparx5->ptp_ts_id_lock); 623 mutex_init(&sparx5->ptp_lock); 624 625 /* Disable master counters */ 626 spx5_wr(PTP_PTP_DOM_CFG_PTP_ENA_SET(0), sparx5, PTP_PTP_DOM_CFG); 627 628 /* Configure the nominal TOD increment per clock cycle */ 629 spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0x7), 630 PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS, 631 sparx5, PTP_PTP_DOM_CFG); 632 633 for (i = 0; i < SPARX5_PHC_COUNT; ++i) { 634 spx5_wr((u32)tod_adj & 0xFFFFFFFF, sparx5, 635 PTP_CLK_PER_CFG(i, 0)); 636 spx5_wr((u32)(tod_adj >> 32), sparx5, 637 PTP_CLK_PER_CFG(i, 1)); 638 } 639 640 spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0), 641 PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS, 642 sparx5, PTP_PTP_DOM_CFG); 643 644 /* Enable master counters */ 645 spx5_wr(PTP_PTP_DOM_CFG_PTP_ENA_SET(0x7), sparx5, PTP_PTP_DOM_CFG); 646 647 for (i = 0; i < sparx5->data->consts->n_ports; i++) { 648 port = sparx5->ports[i]; 649 if (!port) 650 continue; 651 652 skb_queue_head_init(&port->tx_skbs); 653 } 654 655 return 0; 656 } 657 658 void sparx5_ptp_deinit(struct sparx5 *sparx5) 659 { 660 struct sparx5_port *port; 661 int i; 662 663 for (i = 0; i < sparx5->data->consts->n_ports; i++) { 664 port = sparx5->ports[i]; 665 if (!port) 666 continue; 667 668 skb_queue_purge(&port->tx_skbs); 669 } 670 671 for (i = 0; i < SPARX5_PHC_COUNT; ++i) 672 ptp_clock_unregister(sparx5->phc[i].clock); 673 } 674 675 void sparx5_ptp_rxtstamp(struct sparx5 *sparx5, struct sk_buff *skb, 676 u64 timestamp) 677 { 678 struct skb_shared_hwtstamps *shhwtstamps; 679 struct sparx5_phc *phc; 680 struct timespec64 ts; 681 u64 full_ts_in_ns; 682 683 if (!sparx5->ptp) 684 return; 685 686 phc = &sparx5->phc[SPARX5_PHC_PORT]; 687 sparx5_ptp_gettime64(&phc->info, &ts); 688 689 if (ts.tv_nsec < timestamp) 690 ts.tv_sec--; 691 ts.tv_nsec = timestamp; 692 full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec); 693 694 shhwtstamps = skb_hwtstamps(skb); 695 shhwtstamps->hwtstamp = full_ts_in_ns; 696 } 697