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