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 186 /* This is an external timestamp */ 187 ev.type = PTP_CLOCK_EXTTS; 188 189 /* We only have one timestamping channel. */ 190 ev.index = 0; 191 mv88e6xxx_reg_lock(chip); 192 ev.timestamp = timecounter_cyc2time(&chip->tstamp_tc, raw_ts); 193 mv88e6xxx_reg_unlock(chip); 194 195 ptp_clock_event(chip->ptp_clock, &ev); 196 out: 197 schedule_delayed_work(&chip->tai_event_work, TAI_EVENT_WORK_INTERVAL); 198 } 199 200 static int mv88e6xxx_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 201 { 202 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 203 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 204 int neg_adj = 0; 205 u32 diff, mult; 206 u64 adj; 207 208 if (scaled_ppm < 0) { 209 neg_adj = 1; 210 scaled_ppm = -scaled_ppm; 211 } 212 213 mult = ptp_ops->cc_mult; 214 adj = ptp_ops->cc_mult_num; 215 adj *= scaled_ppm; 216 diff = div_u64(adj, ptp_ops->cc_mult_dem); 217 218 mv88e6xxx_reg_lock(chip); 219 220 timecounter_read(&chip->tstamp_tc); 221 chip->tstamp_cc.mult = neg_adj ? mult - diff : mult + diff; 222 223 mv88e6xxx_reg_unlock(chip); 224 225 return 0; 226 } 227 228 static int mv88e6xxx_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta) 229 { 230 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 231 232 mv88e6xxx_reg_lock(chip); 233 timecounter_adjtime(&chip->tstamp_tc, delta); 234 mv88e6xxx_reg_unlock(chip); 235 236 return 0; 237 } 238 239 static int mv88e6xxx_ptp_gettime(struct ptp_clock_info *ptp, 240 struct timespec64 *ts) 241 { 242 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 243 u64 ns; 244 245 mv88e6xxx_reg_lock(chip); 246 ns = timecounter_read(&chip->tstamp_tc); 247 mv88e6xxx_reg_unlock(chip); 248 249 *ts = ns_to_timespec64(ns); 250 251 return 0; 252 } 253 254 static int mv88e6xxx_ptp_settime(struct ptp_clock_info *ptp, 255 const struct timespec64 *ts) 256 { 257 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 258 u64 ns; 259 260 ns = timespec64_to_ns(ts); 261 262 mv88e6xxx_reg_lock(chip); 263 timecounter_init(&chip->tstamp_tc, &chip->tstamp_cc, ns); 264 mv88e6xxx_reg_unlock(chip); 265 266 return 0; 267 } 268 269 static int mv88e6352_ptp_enable_extts(struct mv88e6xxx_chip *chip, 270 struct ptp_clock_request *rq, int on) 271 { 272 int rising = (rq->extts.flags & PTP_RISING_EDGE); 273 int func; 274 int pin; 275 int err; 276 277 /* Reject requests with unsupported flags */ 278 if (rq->extts.flags & ~(PTP_ENABLE_FEATURE | 279 PTP_RISING_EDGE | 280 PTP_FALLING_EDGE | 281 PTP_STRICT_FLAGS)) 282 return -EOPNOTSUPP; 283 284 /* Reject requests to enable time stamping on both edges. */ 285 if ((rq->extts.flags & PTP_STRICT_FLAGS) && 286 (rq->extts.flags & PTP_ENABLE_FEATURE) && 287 (rq->extts.flags & PTP_EXTTS_EDGES) == PTP_EXTTS_EDGES) 288 return -EOPNOTSUPP; 289 290 pin = ptp_find_pin(chip->ptp_clock, PTP_PF_EXTTS, rq->extts.index); 291 292 if (pin < 0) 293 return -EBUSY; 294 295 mv88e6xxx_reg_lock(chip); 296 297 if (on) { 298 func = MV88E6352_G2_SCRATCH_GPIO_PCTL_EVREQ; 299 300 err = mv88e6352_set_gpio_func(chip, pin, func, true); 301 if (err) 302 goto out; 303 304 schedule_delayed_work(&chip->tai_event_work, 305 TAI_EVENT_WORK_INTERVAL); 306 307 err = mv88e6352_config_eventcap(chip, PTP_CLOCK_EXTTS, rising); 308 } else { 309 func = MV88E6352_G2_SCRATCH_GPIO_PCTL_GPIO; 310 311 err = mv88e6352_set_gpio_func(chip, pin, func, true); 312 313 cancel_delayed_work_sync(&chip->tai_event_work); 314 } 315 316 out: 317 mv88e6xxx_reg_unlock(chip); 318 319 return err; 320 } 321 322 static int mv88e6352_ptp_enable(struct ptp_clock_info *ptp, 323 struct ptp_clock_request *rq, int on) 324 { 325 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 326 327 switch (rq->type) { 328 case PTP_CLK_REQ_EXTTS: 329 return mv88e6352_ptp_enable_extts(chip, rq, on); 330 default: 331 return -EOPNOTSUPP; 332 } 333 } 334 335 static int mv88e6352_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin, 336 enum ptp_pin_function func, unsigned int chan) 337 { 338 switch (func) { 339 case PTP_PF_NONE: 340 case PTP_PF_EXTTS: 341 break; 342 case PTP_PF_PEROUT: 343 case PTP_PF_PHYSYNC: 344 return -EOPNOTSUPP; 345 } 346 return 0; 347 } 348 349 const struct mv88e6xxx_ptp_ops mv88e6165_ptp_ops = { 350 .clock_read = mv88e6165_ptp_clock_read, 351 .global_enable = mv88e6165_global_enable, 352 .global_disable = mv88e6165_global_disable, 353 .arr0_sts_reg = MV88E6165_PORT_PTP_ARR0_STS, 354 .arr1_sts_reg = MV88E6165_PORT_PTP_ARR1_STS, 355 .dep_sts_reg = MV88E6165_PORT_PTP_DEP_STS, 356 .rx_filters = (1 << HWTSTAMP_FILTER_NONE) | 357 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 358 (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) | 359 (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) | 360 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) | 361 (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) | 362 (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ), 363 .cc_shift = MV88E6XXX_CC_SHIFT, 364 .cc_mult = MV88E6XXX_CC_MULT, 365 .cc_mult_num = MV88E6XXX_CC_MULT_NUM, 366 .cc_mult_dem = MV88E6XXX_CC_MULT_DEM, 367 }; 368 369 const struct mv88e6xxx_ptp_ops mv88e6250_ptp_ops = { 370 .clock_read = mv88e6352_ptp_clock_read, 371 .ptp_enable = mv88e6352_ptp_enable, 372 .ptp_verify = mv88e6352_ptp_verify, 373 .event_work = mv88e6352_tai_event_work, 374 .port_enable = mv88e6352_hwtstamp_port_enable, 375 .port_disable = mv88e6352_hwtstamp_port_disable, 376 .n_ext_ts = 1, 377 .arr0_sts_reg = MV88E6XXX_PORT_PTP_ARR0_STS, 378 .arr1_sts_reg = MV88E6XXX_PORT_PTP_ARR1_STS, 379 .dep_sts_reg = MV88E6XXX_PORT_PTP_DEP_STS, 380 .rx_filters = (1 << HWTSTAMP_FILTER_NONE) | 381 (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) | 382 (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) | 383 (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) | 384 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 385 (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) | 386 (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) | 387 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) | 388 (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) | 389 (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ), 390 .cc_shift = MV88E6250_CC_SHIFT, 391 .cc_mult = MV88E6250_CC_MULT, 392 .cc_mult_num = MV88E6250_CC_MULT_NUM, 393 .cc_mult_dem = MV88E6250_CC_MULT_DEM, 394 }; 395 396 const struct mv88e6xxx_ptp_ops mv88e6352_ptp_ops = { 397 .clock_read = mv88e6352_ptp_clock_read, 398 .ptp_enable = mv88e6352_ptp_enable, 399 .ptp_verify = mv88e6352_ptp_verify, 400 .event_work = mv88e6352_tai_event_work, 401 .port_enable = mv88e6352_hwtstamp_port_enable, 402 .port_disable = mv88e6352_hwtstamp_port_disable, 403 .n_ext_ts = 1, 404 .arr0_sts_reg = MV88E6XXX_PORT_PTP_ARR0_STS, 405 .arr1_sts_reg = MV88E6XXX_PORT_PTP_ARR1_STS, 406 .dep_sts_reg = MV88E6XXX_PORT_PTP_DEP_STS, 407 .rx_filters = (1 << HWTSTAMP_FILTER_NONE) | 408 (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) | 409 (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) | 410 (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) | 411 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 412 (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) | 413 (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) | 414 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) | 415 (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) | 416 (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ), 417 .cc_shift = MV88E6XXX_CC_SHIFT, 418 .cc_mult = MV88E6XXX_CC_MULT, 419 .cc_mult_num = MV88E6XXX_CC_MULT_NUM, 420 .cc_mult_dem = MV88E6XXX_CC_MULT_DEM, 421 }; 422 423 const struct mv88e6xxx_ptp_ops mv88e6390_ptp_ops = { 424 .clock_read = mv88e6352_ptp_clock_read, 425 .ptp_enable = mv88e6352_ptp_enable, 426 .ptp_verify = mv88e6352_ptp_verify, 427 .event_work = mv88e6352_tai_event_work, 428 .port_enable = mv88e6352_hwtstamp_port_enable, 429 .port_disable = mv88e6352_hwtstamp_port_disable, 430 .set_ptp_cpu_port = mv88e6390_g1_set_ptp_cpu_port, 431 .n_ext_ts = 1, 432 .arr0_sts_reg = MV88E6XXX_PORT_PTP_ARR0_STS, 433 .arr1_sts_reg = MV88E6XXX_PORT_PTP_ARR1_STS, 434 .dep_sts_reg = MV88E6XXX_PORT_PTP_DEP_STS, 435 .rx_filters = (1 << HWTSTAMP_FILTER_NONE) | 436 (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) | 437 (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) | 438 (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) | 439 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 440 (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) | 441 (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) | 442 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) | 443 (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) | 444 (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ), 445 .cc_shift = MV88E6XXX_CC_SHIFT, 446 .cc_mult = MV88E6XXX_CC_MULT, 447 .cc_mult_num = MV88E6XXX_CC_MULT_NUM, 448 .cc_mult_dem = MV88E6XXX_CC_MULT_DEM, 449 }; 450 451 static u64 mv88e6xxx_ptp_clock_read(const struct cyclecounter *cc) 452 { 453 struct mv88e6xxx_chip *chip = cc_to_chip(cc); 454 455 if (chip->info->ops->ptp_ops->clock_read) 456 return chip->info->ops->ptp_ops->clock_read(cc); 457 458 return 0; 459 } 460 461 /* With a 125MHz input clock, the 32-bit timestamp counter overflows in ~34.3 462 * seconds; this task forces periodic reads so that we don't miss any. 463 */ 464 #define MV88E6XXX_TAI_OVERFLOW_PERIOD (HZ * 16) 465 static void mv88e6xxx_ptp_overflow_check(struct work_struct *work) 466 { 467 struct delayed_work *dw = to_delayed_work(work); 468 struct mv88e6xxx_chip *chip = dw_overflow_to_chip(dw); 469 struct timespec64 ts; 470 471 mv88e6xxx_ptp_gettime(&chip->ptp_clock_info, &ts); 472 473 schedule_delayed_work(&chip->overflow_work, 474 MV88E6XXX_TAI_OVERFLOW_PERIOD); 475 } 476 477 int mv88e6xxx_ptp_setup(struct mv88e6xxx_chip *chip) 478 { 479 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 480 int i; 481 482 /* Set up the cycle counter */ 483 memset(&chip->tstamp_cc, 0, sizeof(chip->tstamp_cc)); 484 chip->tstamp_cc.read = mv88e6xxx_ptp_clock_read; 485 chip->tstamp_cc.mask = CYCLECOUNTER_MASK(32); 486 chip->tstamp_cc.mult = ptp_ops->cc_mult; 487 chip->tstamp_cc.shift = ptp_ops->cc_shift; 488 489 timecounter_init(&chip->tstamp_tc, &chip->tstamp_cc, 490 ktime_to_ns(ktime_get_real())); 491 492 INIT_DELAYED_WORK(&chip->overflow_work, mv88e6xxx_ptp_overflow_check); 493 if (ptp_ops->event_work) 494 INIT_DELAYED_WORK(&chip->tai_event_work, ptp_ops->event_work); 495 496 chip->ptp_clock_info.owner = THIS_MODULE; 497 snprintf(chip->ptp_clock_info.name, sizeof(chip->ptp_clock_info.name), 498 "%s", dev_name(chip->dev)); 499 500 chip->ptp_clock_info.n_ext_ts = ptp_ops->n_ext_ts; 501 chip->ptp_clock_info.n_per_out = 0; 502 chip->ptp_clock_info.n_pins = mv88e6xxx_num_gpio(chip); 503 chip->ptp_clock_info.pps = 0; 504 505 for (i = 0; i < chip->ptp_clock_info.n_pins; ++i) { 506 struct ptp_pin_desc *ppd = &chip->pin_config[i]; 507 508 snprintf(ppd->name, sizeof(ppd->name), "mv88e6xxx_gpio%d", i); 509 ppd->index = i; 510 ppd->func = PTP_PF_NONE; 511 } 512 chip->ptp_clock_info.pin_config = chip->pin_config; 513 514 chip->ptp_clock_info.max_adj = MV88E6XXX_MAX_ADJ_PPB; 515 chip->ptp_clock_info.adjfine = mv88e6xxx_ptp_adjfine; 516 chip->ptp_clock_info.adjtime = mv88e6xxx_ptp_adjtime; 517 chip->ptp_clock_info.gettime64 = mv88e6xxx_ptp_gettime; 518 chip->ptp_clock_info.settime64 = mv88e6xxx_ptp_settime; 519 chip->ptp_clock_info.enable = ptp_ops->ptp_enable; 520 chip->ptp_clock_info.verify = ptp_ops->ptp_verify; 521 chip->ptp_clock_info.do_aux_work = mv88e6xxx_hwtstamp_work; 522 523 if (ptp_ops->set_ptp_cpu_port) { 524 struct dsa_port *dp; 525 int upstream = 0; 526 int err; 527 528 dsa_switch_for_each_user_port(dp, chip->ds) { 529 upstream = dsa_upstream_port(chip->ds, dp->index); 530 break; 531 } 532 533 err = ptp_ops->set_ptp_cpu_port(chip, upstream); 534 if (err) { 535 dev_err(chip->dev, "Failed to set PTP CPU destination port!\n"); 536 return err; 537 } 538 } 539 540 chip->ptp_clock = ptp_clock_register(&chip->ptp_clock_info, chip->dev); 541 if (IS_ERR(chip->ptp_clock)) 542 return PTR_ERR(chip->ptp_clock); 543 544 schedule_delayed_work(&chip->overflow_work, 545 MV88E6XXX_TAI_OVERFLOW_PERIOD); 546 547 return 0; 548 } 549 550 void mv88e6xxx_ptp_free(struct mv88e6xxx_chip *chip) 551 { 552 if (chip->ptp_clock) { 553 cancel_delayed_work_sync(&chip->overflow_work); 554 if (chip->info->ops->ptp_ops->event_work) 555 cancel_delayed_work_sync(&chip->tai_event_work); 556 557 ptp_clock_unregister(chip->ptp_clock); 558 chip->ptp_clock = NULL; 559 } 560 } 561