1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * Marvell 88E6xxx Switch PTP support 4 * 5 * Copyright (c) 2008 Marvell Semiconductor 6 * 7 * Copyright (c) 2017 National Instruments 8 * Erik Hons <erik.hons@ni.com> 9 * Brandon Streiff <brandon.streiff@ni.com> 10 * Dane Wagner <dane.wagner@ni.com> 11 */ 12 13 #include "chip.h" 14 #include "global1.h" 15 #include "global2.h" 16 #include "hwtstamp.h" 17 #include "ptp.h" 18 19 #define MV88E6XXX_MAX_ADJ_PPB 1000000 20 21 /* Family MV88E6250: 22 * Raw timestamps are in units of 10-ns clock periods. 23 * 24 * clkadj = scaled_ppm * 10*2^28 / (10^6 * 2^16) 25 * simplifies to 26 * clkadj = scaled_ppm * 2^7 / 5^5 27 */ 28 #define MV88E6250_CC_SHIFT 28 29 #define MV88E6250_CC_MULT (10 << MV88E6250_CC_SHIFT) 30 #define MV88E6250_CC_MULT_NUM (1 << 7) 31 #define MV88E6250_CC_MULT_DEM 3125ULL 32 33 /* Other families: 34 * Raw timestamps are in units of 8-ns clock periods. 35 * 36 * clkadj = scaled_ppm * 8*2^28 / (10^6 * 2^16) 37 * simplifies to 38 * clkadj = scaled_ppm * 2^9 / 5^6 39 */ 40 #define MV88E6XXX_CC_SHIFT 28 41 #define MV88E6XXX_CC_MULT (8 << MV88E6XXX_CC_SHIFT) 42 #define MV88E6XXX_CC_MULT_NUM (1 << 9) 43 #define MV88E6XXX_CC_MULT_DEM 15625ULL 44 45 #define TAI_EVENT_WORK_INTERVAL msecs_to_jiffies(100) 46 47 #define cc_to_chip(cc) container_of(cc, struct mv88e6xxx_chip, tstamp_cc) 48 #define dw_overflow_to_chip(dw) container_of(dw, struct mv88e6xxx_chip, \ 49 overflow_work) 50 #define dw_tai_event_to_chip(dw) container_of(dw, struct mv88e6xxx_chip, \ 51 tai_event_work) 52 53 static int mv88e6xxx_tai_read(struct mv88e6xxx_chip *chip, int addr, 54 u16 *data, int len) 55 { 56 if (!chip->info->ops->avb_ops->tai_read) 57 return -EOPNOTSUPP; 58 59 return chip->info->ops->avb_ops->tai_read(chip, addr, data, len); 60 } 61 62 static int mv88e6xxx_tai_write(struct mv88e6xxx_chip *chip, int addr, u16 data) 63 { 64 if (!chip->info->ops->avb_ops->tai_write) 65 return -EOPNOTSUPP; 66 67 return chip->info->ops->avb_ops->tai_write(chip, addr, data); 68 } 69 70 /* TODO: places where this are called should be using pinctrl */ 71 static int mv88e6352_set_gpio_func(struct mv88e6xxx_chip *chip, int pin, 72 int func, int input) 73 { 74 int err; 75 76 if (!chip->info->ops->gpio_ops) 77 return -EOPNOTSUPP; 78 79 err = chip->info->ops->gpio_ops->set_dir(chip, pin, input); 80 if (err) 81 return err; 82 83 return chip->info->ops->gpio_ops->set_pctl(chip, pin, func); 84 } 85 86 static u64 mv88e6352_ptp_clock_read(const struct cyclecounter *cc) 87 { 88 struct mv88e6xxx_chip *chip = cc_to_chip(cc); 89 u16 phc_time[2]; 90 int err; 91 92 err = mv88e6xxx_tai_read(chip, MV88E6XXX_TAI_TIME_LO, phc_time, 93 ARRAY_SIZE(phc_time)); 94 if (err) 95 return 0; 96 else 97 return ((u32)phc_time[1] << 16) | phc_time[0]; 98 } 99 100 static u64 mv88e6165_ptp_clock_read(const struct cyclecounter *cc) 101 { 102 struct mv88e6xxx_chip *chip = cc_to_chip(cc); 103 u16 phc_time[2]; 104 int err; 105 106 err = mv88e6xxx_tai_read(chip, MV88E6XXX_PTP_GC_TIME_LO, phc_time, 107 ARRAY_SIZE(phc_time)); 108 if (err) 109 return 0; 110 else 111 return ((u32)phc_time[1] << 16) | phc_time[0]; 112 } 113 114 /* mv88e6352_config_eventcap - configure TAI event capture 115 * @event: PTP_CLOCK_PPS (internal) or PTP_CLOCK_EXTTS (external) 116 * @rising: zero for falling-edge trigger, else rising-edge trigger 117 * 118 * This will also reset the capture sequence counter. 119 */ 120 static int mv88e6352_config_eventcap(struct mv88e6xxx_chip *chip, int event, 121 int rising) 122 { 123 u16 global_config; 124 u16 cap_config; 125 int err; 126 127 chip->evcap_config = MV88E6XXX_TAI_CFG_CAP_OVERWRITE | 128 MV88E6XXX_TAI_CFG_CAP_CTR_START; 129 if (!rising) 130 chip->evcap_config |= MV88E6XXX_TAI_CFG_EVREQ_FALLING; 131 132 global_config = (chip->evcap_config | chip->trig_config); 133 err = mv88e6xxx_tai_write(chip, MV88E6XXX_TAI_CFG, global_config); 134 if (err) 135 return err; 136 137 if (event == PTP_CLOCK_PPS) { 138 cap_config = MV88E6XXX_TAI_EVENT_STATUS_CAP_TRIG; 139 } else if (event == PTP_CLOCK_EXTTS) { 140 /* if STATUS_CAP_TRIG is unset we capture PTP_EVREQ events */ 141 cap_config = 0; 142 } else { 143 return -EINVAL; 144 } 145 146 /* Write the capture config; this also clears the capture counter */ 147 err = mv88e6xxx_tai_write(chip, MV88E6XXX_TAI_EVENT_STATUS, 148 cap_config); 149 150 return err; 151 } 152 153 static void mv88e6352_tai_event_work(struct work_struct *ugly) 154 { 155 struct delayed_work *dw = to_delayed_work(ugly); 156 struct mv88e6xxx_chip *chip = dw_tai_event_to_chip(dw); 157 struct ptp_clock_event ev; 158 u16 status[4]; 159 u32 raw_ts; 160 int err; 161 162 mv88e6xxx_reg_lock(chip); 163 err = mv88e6xxx_tai_read(chip, MV88E6XXX_TAI_EVENT_STATUS, 164 status, ARRAY_SIZE(status)); 165 mv88e6xxx_reg_unlock(chip); 166 167 if (err) { 168 dev_err(chip->dev, "failed to read TAI status register\n"); 169 return; 170 } 171 if (status[0] & MV88E6XXX_TAI_EVENT_STATUS_ERROR) { 172 dev_warn(chip->dev, "missed event capture\n"); 173 return; 174 } 175 if (!(status[0] & MV88E6XXX_TAI_EVENT_STATUS_VALID)) 176 goto out; 177 178 raw_ts = ((u32)status[2] << 16) | status[1]; 179 180 /* Clear the valid bit so the next timestamp can come in */ 181 status[0] &= ~MV88E6XXX_TAI_EVENT_STATUS_VALID; 182 mv88e6xxx_reg_lock(chip); 183 err = mv88e6xxx_tai_write(chip, MV88E6XXX_TAI_EVENT_STATUS, status[0]); 184 mv88e6xxx_reg_unlock(chip); 185 if (err) { 186 dev_err(chip->dev, "failed to write TAI status register\n"); 187 return; 188 } 189 190 /* This is an external timestamp */ 191 ev.type = PTP_CLOCK_EXTTS; 192 193 /* We only have one timestamping channel. */ 194 ev.index = 0; 195 mv88e6xxx_reg_lock(chip); 196 ev.timestamp = timecounter_cyc2time(&chip->tstamp_tc, raw_ts); 197 mv88e6xxx_reg_unlock(chip); 198 199 ptp_clock_event(chip->ptp_clock, &ev); 200 out: 201 schedule_delayed_work(&chip->tai_event_work, TAI_EVENT_WORK_INTERVAL); 202 } 203 204 static int mv88e6xxx_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 205 { 206 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 207 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 208 int neg_adj = 0; 209 u32 diff, mult; 210 u64 adj; 211 212 if (scaled_ppm < 0) { 213 neg_adj = 1; 214 scaled_ppm = -scaled_ppm; 215 } 216 217 mult = ptp_ops->cc_mult; 218 adj = ptp_ops->cc_mult_num; 219 adj *= scaled_ppm; 220 diff = div_u64(adj, ptp_ops->cc_mult_dem); 221 222 mv88e6xxx_reg_lock(chip); 223 224 timecounter_read(&chip->tstamp_tc); 225 chip->tstamp_cc.mult = neg_adj ? mult - diff : mult + diff; 226 227 mv88e6xxx_reg_unlock(chip); 228 229 return 0; 230 } 231 232 static int mv88e6xxx_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 233 { 234 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 235 236 mv88e6xxx_reg_lock(chip); 237 timecounter_adjtime(&chip->tstamp_tc, delta); 238 mv88e6xxx_reg_unlock(chip); 239 240 return 0; 241 } 242 243 static int mv88e6xxx_ptp_gettime(struct ptp_clock_info *ptp, 244 struct timespec64 *ts) 245 { 246 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 247 u64 ns; 248 249 mv88e6xxx_reg_lock(chip); 250 ns = timecounter_read(&chip->tstamp_tc); 251 mv88e6xxx_reg_unlock(chip); 252 253 *ts = ns_to_timespec64(ns); 254 255 return 0; 256 } 257 258 static int mv88e6xxx_ptp_settime(struct ptp_clock_info *ptp, 259 const struct timespec64 *ts) 260 { 261 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 262 u64 ns; 263 264 ns = timespec64_to_ns(ts); 265 266 mv88e6xxx_reg_lock(chip); 267 timecounter_init(&chip->tstamp_tc, &chip->tstamp_cc, ns); 268 mv88e6xxx_reg_unlock(chip); 269 270 return 0; 271 } 272 273 static int mv88e6352_ptp_enable_extts(struct mv88e6xxx_chip *chip, 274 struct ptp_clock_request *rq, int on) 275 { 276 int rising = (rq->extts.flags & PTP_RISING_EDGE); 277 int func; 278 int pin; 279 int err; 280 281 /* Reject requests with unsupported flags */ 282 if (rq->extts.flags & ~(PTP_ENABLE_FEATURE | 283 PTP_RISING_EDGE | 284 PTP_FALLING_EDGE | 285 PTP_STRICT_FLAGS)) 286 return -EOPNOTSUPP; 287 288 /* Reject requests to enable time stamping on both edges. */ 289 if ((rq->extts.flags & PTP_STRICT_FLAGS) && 290 (rq->extts.flags & PTP_ENABLE_FEATURE) && 291 (rq->extts.flags & PTP_EXTTS_EDGES) == PTP_EXTTS_EDGES) 292 return -EOPNOTSUPP; 293 294 pin = ptp_find_pin(chip->ptp_clock, PTP_PF_EXTTS, rq->extts.index); 295 296 if (pin < 0) 297 return -EBUSY; 298 299 mv88e6xxx_reg_lock(chip); 300 301 if (on) { 302 func = MV88E6352_G2_SCRATCH_GPIO_PCTL_EVREQ; 303 304 err = mv88e6352_set_gpio_func(chip, pin, func, true); 305 if (err) 306 goto out; 307 308 schedule_delayed_work(&chip->tai_event_work, 309 TAI_EVENT_WORK_INTERVAL); 310 311 err = mv88e6352_config_eventcap(chip, PTP_CLOCK_EXTTS, rising); 312 } else { 313 func = MV88E6352_G2_SCRATCH_GPIO_PCTL_GPIO; 314 315 err = mv88e6352_set_gpio_func(chip, pin, func, true); 316 317 cancel_delayed_work_sync(&chip->tai_event_work); 318 } 319 320 out: 321 mv88e6xxx_reg_unlock(chip); 322 323 return err; 324 } 325 326 static int mv88e6352_ptp_enable(struct ptp_clock_info *ptp, 327 struct ptp_clock_request *rq, int on) 328 { 329 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 330 331 switch (rq->type) { 332 case PTP_CLK_REQ_EXTTS: 333 return mv88e6352_ptp_enable_extts(chip, rq, on); 334 default: 335 return -EOPNOTSUPP; 336 } 337 } 338 339 static int mv88e6352_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin, 340 enum ptp_pin_function func, unsigned int chan) 341 { 342 switch (func) { 343 case PTP_PF_NONE: 344 case PTP_PF_EXTTS: 345 break; 346 case PTP_PF_PEROUT: 347 case PTP_PF_PHYSYNC: 348 return -EOPNOTSUPP; 349 } 350 return 0; 351 } 352 353 const struct mv88e6xxx_ptp_ops mv88e6165_ptp_ops = { 354 .clock_read = mv88e6165_ptp_clock_read, 355 .global_enable = mv88e6165_global_enable, 356 .global_disable = mv88e6165_global_disable, 357 .arr0_sts_reg = MV88E6165_PORT_PTP_ARR0_STS, 358 .arr1_sts_reg = MV88E6165_PORT_PTP_ARR1_STS, 359 .dep_sts_reg = MV88E6165_PORT_PTP_DEP_STS, 360 .rx_filters = (1 << HWTSTAMP_FILTER_NONE) | 361 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 362 (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) | 363 (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) | 364 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) | 365 (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) | 366 (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ), 367 .cc_shift = MV88E6XXX_CC_SHIFT, 368 .cc_mult = MV88E6XXX_CC_MULT, 369 .cc_mult_num = MV88E6XXX_CC_MULT_NUM, 370 .cc_mult_dem = MV88E6XXX_CC_MULT_DEM, 371 }; 372 373 const struct mv88e6xxx_ptp_ops mv88e6250_ptp_ops = { 374 .clock_read = mv88e6352_ptp_clock_read, 375 .ptp_enable = mv88e6352_ptp_enable, 376 .ptp_verify = mv88e6352_ptp_verify, 377 .event_work = mv88e6352_tai_event_work, 378 .port_enable = mv88e6352_hwtstamp_port_enable, 379 .port_disable = mv88e6352_hwtstamp_port_disable, 380 .n_ext_ts = 1, 381 .arr0_sts_reg = MV88E6XXX_PORT_PTP_ARR0_STS, 382 .arr1_sts_reg = MV88E6XXX_PORT_PTP_ARR1_STS, 383 .dep_sts_reg = MV88E6XXX_PORT_PTP_DEP_STS, 384 .rx_filters = (1 << HWTSTAMP_FILTER_NONE) | 385 (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) | 386 (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) | 387 (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) | 388 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 389 (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) | 390 (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) | 391 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) | 392 (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) | 393 (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ), 394 .cc_shift = MV88E6250_CC_SHIFT, 395 .cc_mult = MV88E6250_CC_MULT, 396 .cc_mult_num = MV88E6250_CC_MULT_NUM, 397 .cc_mult_dem = MV88E6250_CC_MULT_DEM, 398 }; 399 400 const struct mv88e6xxx_ptp_ops mv88e6352_ptp_ops = { 401 .clock_read = mv88e6352_ptp_clock_read, 402 .ptp_enable = mv88e6352_ptp_enable, 403 .ptp_verify = mv88e6352_ptp_verify, 404 .event_work = mv88e6352_tai_event_work, 405 .port_enable = mv88e6352_hwtstamp_port_enable, 406 .port_disable = mv88e6352_hwtstamp_port_disable, 407 .n_ext_ts = 1, 408 .arr0_sts_reg = MV88E6XXX_PORT_PTP_ARR0_STS, 409 .arr1_sts_reg = MV88E6XXX_PORT_PTP_ARR1_STS, 410 .dep_sts_reg = MV88E6XXX_PORT_PTP_DEP_STS, 411 .rx_filters = (1 << HWTSTAMP_FILTER_NONE) | 412 (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) | 413 (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) | 414 (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) | 415 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 416 (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) | 417 (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) | 418 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) | 419 (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) | 420 (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ), 421 .cc_shift = MV88E6XXX_CC_SHIFT, 422 .cc_mult = MV88E6XXX_CC_MULT, 423 .cc_mult_num = MV88E6XXX_CC_MULT_NUM, 424 .cc_mult_dem = MV88E6XXX_CC_MULT_DEM, 425 }; 426 427 const struct mv88e6xxx_ptp_ops mv88e6390_ptp_ops = { 428 .clock_read = mv88e6352_ptp_clock_read, 429 .ptp_enable = mv88e6352_ptp_enable, 430 .ptp_verify = mv88e6352_ptp_verify, 431 .event_work = mv88e6352_tai_event_work, 432 .port_enable = mv88e6352_hwtstamp_port_enable, 433 .port_disable = mv88e6352_hwtstamp_port_disable, 434 .set_ptp_cpu_port = mv88e6390_g1_set_ptp_cpu_port, 435 .n_ext_ts = 1, 436 .arr0_sts_reg = MV88E6XXX_PORT_PTP_ARR0_STS, 437 .arr1_sts_reg = MV88E6XXX_PORT_PTP_ARR1_STS, 438 .dep_sts_reg = MV88E6XXX_PORT_PTP_DEP_STS, 439 .rx_filters = (1 << HWTSTAMP_FILTER_NONE) | 440 (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) | 441 (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) | 442 (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) | 443 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 444 (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) | 445 (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) | 446 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) | 447 (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) | 448 (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ), 449 .cc_shift = MV88E6XXX_CC_SHIFT, 450 .cc_mult = MV88E6XXX_CC_MULT, 451 .cc_mult_num = MV88E6XXX_CC_MULT_NUM, 452 .cc_mult_dem = MV88E6XXX_CC_MULT_DEM, 453 }; 454 455 static u64 mv88e6xxx_ptp_clock_read(const struct cyclecounter *cc) 456 { 457 struct mv88e6xxx_chip *chip = cc_to_chip(cc); 458 459 if (chip->info->ops->ptp_ops->clock_read) 460 return chip->info->ops->ptp_ops->clock_read(cc); 461 462 return 0; 463 } 464 465 /* With a 125MHz input clock, the 32-bit timestamp counter overflows in ~34.3 466 * seconds; this task forces periodic reads so that we don't miss any. 467 */ 468 #define MV88E6XXX_TAI_OVERFLOW_PERIOD (HZ * 16) 469 static void mv88e6xxx_ptp_overflow_check(struct work_struct *work) 470 { 471 struct delayed_work *dw = to_delayed_work(work); 472 struct mv88e6xxx_chip *chip = dw_overflow_to_chip(dw); 473 struct timespec64 ts; 474 475 mv88e6xxx_ptp_gettime(&chip->ptp_clock_info, &ts); 476 477 schedule_delayed_work(&chip->overflow_work, 478 MV88E6XXX_TAI_OVERFLOW_PERIOD); 479 } 480 481 int mv88e6xxx_ptp_setup(struct mv88e6xxx_chip *chip) 482 { 483 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 484 int i; 485 486 /* Set up the cycle counter */ 487 memset(&chip->tstamp_cc, 0, sizeof(chip->tstamp_cc)); 488 chip->tstamp_cc.read = mv88e6xxx_ptp_clock_read; 489 chip->tstamp_cc.mask = CYCLECOUNTER_MASK(32); 490 chip->tstamp_cc.mult = ptp_ops->cc_mult; 491 chip->tstamp_cc.shift = ptp_ops->cc_shift; 492 493 timecounter_init(&chip->tstamp_tc, &chip->tstamp_cc, 494 ktime_to_ns(ktime_get_real())); 495 496 INIT_DELAYED_WORK(&chip->overflow_work, mv88e6xxx_ptp_overflow_check); 497 if (ptp_ops->event_work) 498 INIT_DELAYED_WORK(&chip->tai_event_work, ptp_ops->event_work); 499 500 chip->ptp_clock_info.owner = THIS_MODULE; 501 snprintf(chip->ptp_clock_info.name, sizeof(chip->ptp_clock_info.name), 502 "%s", dev_name(chip->dev)); 503 504 chip->ptp_clock_info.n_ext_ts = ptp_ops->n_ext_ts; 505 chip->ptp_clock_info.n_per_out = 0; 506 chip->ptp_clock_info.n_pins = mv88e6xxx_num_gpio(chip); 507 chip->ptp_clock_info.pps = 0; 508 509 for (i = 0; i < chip->ptp_clock_info.n_pins; ++i) { 510 struct ptp_pin_desc *ppd = &chip->pin_config[i]; 511 512 snprintf(ppd->name, sizeof(ppd->name), "mv88e6xxx_gpio%d", i); 513 ppd->index = i; 514 ppd->func = PTP_PF_NONE; 515 } 516 chip->ptp_clock_info.pin_config = chip->pin_config; 517 518 chip->ptp_clock_info.max_adj = MV88E6XXX_MAX_ADJ_PPB; 519 chip->ptp_clock_info.adjfine = mv88e6xxx_ptp_adjfine; 520 chip->ptp_clock_info.adjtime = mv88e6xxx_ptp_adjtime; 521 chip->ptp_clock_info.gettime64 = mv88e6xxx_ptp_gettime; 522 chip->ptp_clock_info.settime64 = mv88e6xxx_ptp_settime; 523 chip->ptp_clock_info.enable = ptp_ops->ptp_enable; 524 chip->ptp_clock_info.verify = ptp_ops->ptp_verify; 525 chip->ptp_clock_info.do_aux_work = mv88e6xxx_hwtstamp_work; 526 527 if (ptp_ops->set_ptp_cpu_port) { 528 struct dsa_port *dp; 529 int upstream = 0; 530 int err; 531 532 dsa_switch_for_each_user_port(dp, chip->ds) { 533 upstream = dsa_upstream_port(chip->ds, dp->index); 534 break; 535 } 536 537 err = ptp_ops->set_ptp_cpu_port(chip, upstream); 538 if (err) { 539 dev_err(chip->dev, "Failed to set PTP CPU destination port!\n"); 540 return err; 541 } 542 } 543 544 chip->ptp_clock = ptp_clock_register(&chip->ptp_clock_info, chip->dev); 545 if (IS_ERR(chip->ptp_clock)) 546 return PTR_ERR(chip->ptp_clock); 547 548 schedule_delayed_work(&chip->overflow_work, 549 MV88E6XXX_TAI_OVERFLOW_PERIOD); 550 551 return 0; 552 } 553 554 void mv88e6xxx_ptp_free(struct mv88e6xxx_chip *chip) 555 { 556 if (chip->ptp_clock) { 557 cancel_delayed_work_sync(&chip->overflow_work); 558 if (chip->info->ops->ptp_ops->event_work) 559 cancel_delayed_work_sync(&chip->tai_event_work); 560 561 ptp_clock_unregister(chip->ptp_clock); 562 chip->ptp_clock = NULL; 563 } 564 } 565