1 // SPDX-License-Identifier: GPL-2.0-or-later 2 /* 3 * Marvell 88E6xxx Switch hardware timestamping 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 "global2.h" 15 #include "hwtstamp.h" 16 #include "ptp.h" 17 #include <linux/ptp_classify.h> 18 19 #define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb)) 20 21 static int mv88e6xxx_port_ptp_read(struct mv88e6xxx_chip *chip, int port, 22 int addr, u16 *data, int len) 23 { 24 if (!chip->info->ops->avb_ops->port_ptp_read) 25 return -EOPNOTSUPP; 26 27 return chip->info->ops->avb_ops->port_ptp_read(chip, port, addr, 28 data, len); 29 } 30 31 static int mv88e6xxx_port_ptp_write(struct mv88e6xxx_chip *chip, int port, 32 int addr, u16 data) 33 { 34 if (!chip->info->ops->avb_ops->port_ptp_write) 35 return -EOPNOTSUPP; 36 37 return chip->info->ops->avb_ops->port_ptp_write(chip, port, addr, 38 data); 39 } 40 41 static int mv88e6xxx_ptp_write(struct mv88e6xxx_chip *chip, int addr, 42 u16 data) 43 { 44 if (!chip->info->ops->avb_ops->ptp_write) 45 return -EOPNOTSUPP; 46 47 return chip->info->ops->avb_ops->ptp_write(chip, addr, data); 48 } 49 50 static int mv88e6xxx_ptp_read(struct mv88e6xxx_chip *chip, int addr, 51 u16 *data) 52 { 53 if (!chip->info->ops->avb_ops->ptp_read) 54 return -EOPNOTSUPP; 55 56 return chip->info->ops->avb_ops->ptp_read(chip, addr, data, 1); 57 } 58 59 /* TX_TSTAMP_TIMEOUT: This limits the time spent polling for a TX 60 * timestamp. When working properly, hardware will produce a timestamp 61 * within 1ms. Software may enounter delays due to MDIO contention, so 62 * the timeout is set accordingly. 63 */ 64 #define TX_TSTAMP_TIMEOUT msecs_to_jiffies(40) 65 66 int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port, 67 struct ethtool_ts_info *info) 68 { 69 const struct mv88e6xxx_ptp_ops *ptp_ops; 70 struct mv88e6xxx_chip *chip; 71 72 chip = ds->priv; 73 ptp_ops = chip->info->ops->ptp_ops; 74 75 if (!chip->info->ptp_support) 76 return -EOPNOTSUPP; 77 78 info->so_timestamping = 79 SOF_TIMESTAMPING_TX_HARDWARE | 80 SOF_TIMESTAMPING_RX_HARDWARE | 81 SOF_TIMESTAMPING_RAW_HARDWARE; 82 info->phc_index = ptp_clock_index(chip->ptp_clock); 83 info->tx_types = 84 (1 << HWTSTAMP_TX_OFF) | 85 (1 << HWTSTAMP_TX_ON); 86 info->rx_filters = ptp_ops->rx_filters; 87 88 return 0; 89 } 90 91 static int mv88e6xxx_set_hwtstamp_config(struct mv88e6xxx_chip *chip, int port, 92 struct hwtstamp_config *config) 93 { 94 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 95 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 96 bool tstamp_enable = false; 97 98 /* Prevent the TX/RX paths from trying to interact with the 99 * timestamp hardware while we reconfigure it. 100 */ 101 clear_bit_unlock(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state); 102 103 switch (config->tx_type) { 104 case HWTSTAMP_TX_OFF: 105 tstamp_enable = false; 106 break; 107 case HWTSTAMP_TX_ON: 108 tstamp_enable = true; 109 break; 110 default: 111 return -ERANGE; 112 } 113 114 /* The switch supports timestamping both L2 and L4; one cannot be 115 * disabled independently of the other. 116 */ 117 118 if (!(BIT(config->rx_filter) & ptp_ops->rx_filters)) { 119 config->rx_filter = HWTSTAMP_FILTER_NONE; 120 dev_dbg(chip->dev, "Unsupported rx_filter %d\n", 121 config->rx_filter); 122 return -ERANGE; 123 } 124 125 switch (config->rx_filter) { 126 case HWTSTAMP_FILTER_NONE: 127 tstamp_enable = false; 128 break; 129 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 130 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 131 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 132 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 133 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 134 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 135 case HWTSTAMP_FILTER_PTP_V2_EVENT: 136 case HWTSTAMP_FILTER_PTP_V2_SYNC: 137 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 138 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 139 break; 140 case HWTSTAMP_FILTER_ALL: 141 default: 142 config->rx_filter = HWTSTAMP_FILTER_NONE; 143 return -ERANGE; 144 } 145 146 mv88e6xxx_reg_lock(chip); 147 if (tstamp_enable) { 148 chip->enable_count += 1; 149 if (chip->enable_count == 1 && ptp_ops->global_enable) 150 ptp_ops->global_enable(chip); 151 if (ptp_ops->port_enable) 152 ptp_ops->port_enable(chip, port); 153 } else { 154 if (ptp_ops->port_disable) 155 ptp_ops->port_disable(chip, port); 156 chip->enable_count -= 1; 157 if (chip->enable_count == 0 && ptp_ops->global_disable) 158 ptp_ops->global_disable(chip); 159 } 160 mv88e6xxx_reg_unlock(chip); 161 162 /* Once hardware has been configured, enable timestamp checks 163 * in the RX/TX paths. 164 */ 165 if (tstamp_enable) 166 set_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state); 167 168 return 0; 169 } 170 171 int mv88e6xxx_port_hwtstamp_set(struct dsa_switch *ds, int port, 172 struct ifreq *ifr) 173 { 174 struct mv88e6xxx_chip *chip = ds->priv; 175 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 176 struct hwtstamp_config config; 177 int err; 178 179 if (!chip->info->ptp_support) 180 return -EOPNOTSUPP; 181 182 if (copy_from_user(&config, ifr->ifr_data, sizeof(config))) 183 return -EFAULT; 184 185 err = mv88e6xxx_set_hwtstamp_config(chip, port, &config); 186 if (err) 187 return err; 188 189 /* Save the chosen configuration to be returned later. */ 190 memcpy(&ps->tstamp_config, &config, sizeof(config)); 191 192 return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ? 193 -EFAULT : 0; 194 } 195 196 int mv88e6xxx_port_hwtstamp_get(struct dsa_switch *ds, int port, 197 struct ifreq *ifr) 198 { 199 struct mv88e6xxx_chip *chip = ds->priv; 200 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 201 struct hwtstamp_config *config = &ps->tstamp_config; 202 203 if (!chip->info->ptp_support) 204 return -EOPNOTSUPP; 205 206 return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ? 207 -EFAULT : 0; 208 } 209 210 /* Returns a pointer to the PTP header if the caller should time stamp, 211 * or NULL if the caller should not. 212 */ 213 static struct ptp_header *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip, 214 int port, struct sk_buff *skb, 215 unsigned int type) 216 { 217 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 218 struct ptp_header *hdr; 219 220 if (!chip->info->ptp_support) 221 return NULL; 222 223 hdr = ptp_parse_header(skb, type); 224 if (!hdr) 225 return NULL; 226 227 if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state)) 228 return NULL; 229 230 return hdr; 231 } 232 233 static int mv88e6xxx_ts_valid(u16 status) 234 { 235 if (!(status & MV88E6XXX_PTP_TS_VALID)) 236 return 0; 237 if (status & MV88E6XXX_PTP_TS_STATUS_MASK) 238 return 0; 239 return 1; 240 } 241 242 static int seq_match(struct sk_buff *skb, u16 ts_seqid) 243 { 244 unsigned int type = SKB_PTP_TYPE(skb); 245 struct ptp_header *hdr; 246 247 hdr = ptp_parse_header(skb, type); 248 249 return ts_seqid == ntohs(hdr->sequence_id); 250 } 251 252 static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip, 253 struct mv88e6xxx_port_hwtstamp *ps, 254 struct sk_buff *skb, u16 reg, 255 struct sk_buff_head *rxq) 256 { 257 u16 buf[4] = { 0 }, status, seq_id; 258 struct skb_shared_hwtstamps *shwt; 259 struct sk_buff_head received; 260 u64 ns, timelo, timehi; 261 unsigned long flags; 262 int err; 263 264 /* The latched timestamp belongs to one of the received frames. */ 265 __skb_queue_head_init(&received); 266 spin_lock_irqsave(&rxq->lock, flags); 267 skb_queue_splice_tail_init(rxq, &received); 268 spin_unlock_irqrestore(&rxq->lock, flags); 269 270 mv88e6xxx_reg_lock(chip); 271 err = mv88e6xxx_port_ptp_read(chip, ps->port_id, 272 reg, buf, ARRAY_SIZE(buf)); 273 mv88e6xxx_reg_unlock(chip); 274 if (err) 275 pr_err("failed to get the receive time stamp\n"); 276 277 status = buf[0]; 278 timelo = buf[1]; 279 timehi = buf[2]; 280 seq_id = buf[3]; 281 282 if (status & MV88E6XXX_PTP_TS_VALID) { 283 mv88e6xxx_reg_lock(chip); 284 err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0); 285 mv88e6xxx_reg_unlock(chip); 286 if (err) 287 pr_err("failed to clear the receive status\n"); 288 } 289 /* Since the device can only handle one time stamp at a time, 290 * we purge any extra frames from the queue. 291 */ 292 for ( ; skb; skb = __skb_dequeue(&received)) { 293 if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) { 294 ns = timehi << 16 | timelo; 295 296 mv88e6xxx_reg_lock(chip); 297 ns = timecounter_cyc2time(&chip->tstamp_tc, ns); 298 mv88e6xxx_reg_unlock(chip); 299 shwt = skb_hwtstamps(skb); 300 memset(shwt, 0, sizeof(*shwt)); 301 shwt->hwtstamp = ns_to_ktime(ns); 302 status &= ~MV88E6XXX_PTP_TS_VALID; 303 } 304 netif_rx(skb); 305 } 306 } 307 308 static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip, 309 struct mv88e6xxx_port_hwtstamp *ps) 310 { 311 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 312 struct sk_buff *skb; 313 314 skb = skb_dequeue(&ps->rx_queue); 315 316 if (skb) 317 mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr0_sts_reg, 318 &ps->rx_queue); 319 320 skb = skb_dequeue(&ps->rx_queue2); 321 if (skb) 322 mv88e6xxx_get_rxts(chip, ps, skb, ptp_ops->arr1_sts_reg, 323 &ps->rx_queue2); 324 } 325 326 static int is_pdelay_resp(const struct ptp_header *hdr) 327 { 328 return (hdr->tsmt & 0xf) == 3; 329 } 330 331 bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port, 332 struct sk_buff *skb, unsigned int type) 333 { 334 struct mv88e6xxx_port_hwtstamp *ps; 335 struct mv88e6xxx_chip *chip; 336 struct ptp_header *hdr; 337 338 chip = ds->priv; 339 ps = &chip->port_hwtstamp[port]; 340 341 if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT) 342 return false; 343 344 hdr = mv88e6xxx_should_tstamp(chip, port, skb, type); 345 if (!hdr) 346 return false; 347 348 SKB_PTP_TYPE(skb) = type; 349 350 if (is_pdelay_resp(hdr)) 351 skb_queue_tail(&ps->rx_queue2, skb); 352 else 353 skb_queue_tail(&ps->rx_queue, skb); 354 355 ptp_schedule_worker(chip->ptp_clock, 0); 356 357 return true; 358 } 359 360 static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip, 361 struct mv88e6xxx_port_hwtstamp *ps) 362 { 363 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 364 struct skb_shared_hwtstamps shhwtstamps; 365 u16 departure_block[4], status; 366 struct sk_buff *tmp_skb; 367 u32 time_raw; 368 int err; 369 u64 ns; 370 371 if (!ps->tx_skb) 372 return 0; 373 374 mv88e6xxx_reg_lock(chip); 375 err = mv88e6xxx_port_ptp_read(chip, ps->port_id, 376 ptp_ops->dep_sts_reg, 377 departure_block, 378 ARRAY_SIZE(departure_block)); 379 mv88e6xxx_reg_unlock(chip); 380 381 if (err) 382 goto free_and_clear_skb; 383 384 if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) { 385 if (time_is_before_jiffies(ps->tx_tstamp_start + 386 TX_TSTAMP_TIMEOUT)) { 387 dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n", 388 ps->port_id); 389 goto free_and_clear_skb; 390 } 391 /* The timestamp should be available quickly, while getting it 392 * is high priority and time bounded to only 10ms. A poll is 393 * warranted so restart the work. 394 */ 395 return 1; 396 } 397 398 /* We have the timestamp; go ahead and clear valid now */ 399 mv88e6xxx_reg_lock(chip); 400 mv88e6xxx_port_ptp_write(chip, ps->port_id, ptp_ops->dep_sts_reg, 0); 401 mv88e6xxx_reg_unlock(chip); 402 403 status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK; 404 if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) { 405 dev_warn(chip->dev, "p%d: tx timestamp overrun\n", ps->port_id); 406 goto free_and_clear_skb; 407 } 408 409 if (departure_block[3] != ps->tx_seq_id) { 410 dev_warn(chip->dev, "p%d: unexpected seq. id\n", ps->port_id); 411 goto free_and_clear_skb; 412 } 413 414 memset(&shhwtstamps, 0, sizeof(shhwtstamps)); 415 time_raw = ((u32)departure_block[2] << 16) | departure_block[1]; 416 mv88e6xxx_reg_lock(chip); 417 ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw); 418 mv88e6xxx_reg_unlock(chip); 419 shhwtstamps.hwtstamp = ns_to_ktime(ns); 420 421 dev_dbg(chip->dev, 422 "p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n", 423 ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp), 424 departure_block[0], ps->tx_seq_id, departure_block[3]); 425 426 /* skb_complete_tx_timestamp() will free up the client to make 427 * another timestamp-able transmit. We have to be ready for it 428 * -- by clearing the ps->tx_skb "flag" -- beforehand. 429 */ 430 431 tmp_skb = ps->tx_skb; 432 ps->tx_skb = NULL; 433 clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state); 434 skb_complete_tx_timestamp(tmp_skb, &shhwtstamps); 435 436 return 0; 437 438 free_and_clear_skb: 439 dev_kfree_skb_any(ps->tx_skb); 440 ps->tx_skb = NULL; 441 clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state); 442 443 return 0; 444 } 445 446 long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp) 447 { 448 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 449 struct dsa_switch *ds = chip->ds; 450 struct mv88e6xxx_port_hwtstamp *ps; 451 int i, restart = 0; 452 453 for (i = 0; i < ds->num_ports; i++) { 454 if (!dsa_is_user_port(ds, i)) 455 continue; 456 457 ps = &chip->port_hwtstamp[i]; 458 if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state)) 459 restart |= mv88e6xxx_txtstamp_work(chip, ps); 460 461 mv88e6xxx_rxtstamp_work(chip, ps); 462 } 463 464 return restart ? 1 : -1; 465 } 466 467 void mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port, 468 struct sk_buff *skb) 469 { 470 struct mv88e6xxx_chip *chip = ds->priv; 471 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 472 struct ptp_header *hdr; 473 struct sk_buff *clone; 474 unsigned int type; 475 476 type = ptp_classify_raw(skb); 477 if (type == PTP_CLASS_NONE) 478 return; 479 480 hdr = mv88e6xxx_should_tstamp(chip, port, skb, type); 481 if (!hdr) 482 return; 483 484 clone = skb_clone_sk(skb); 485 if (!clone) 486 return; 487 488 if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, 489 &ps->state)) { 490 kfree_skb(clone); 491 return; 492 } 493 494 ps->tx_skb = clone; 495 ps->tx_tstamp_start = jiffies; 496 ps->tx_seq_id = be16_to_cpu(hdr->sequence_id); 497 498 ptp_schedule_worker(chip->ptp_clock, 0); 499 } 500 501 int mv88e6165_global_disable(struct mv88e6xxx_chip *chip) 502 { 503 u16 val; 504 int err; 505 506 err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val); 507 if (err) 508 return err; 509 val |= MV88E6165_PTP_CFG_DISABLE_PTP; 510 511 return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val); 512 } 513 514 int mv88e6165_global_enable(struct mv88e6xxx_chip *chip) 515 { 516 u16 val; 517 int err; 518 519 err = mv88e6xxx_ptp_read(chip, MV88E6165_PTP_CFG, &val); 520 if (err) 521 return err; 522 523 val &= ~(MV88E6165_PTP_CFG_DISABLE_PTP | MV88E6165_PTP_CFG_TSPEC_MASK); 524 525 return mv88e6xxx_ptp_write(chip, MV88E6165_PTP_CFG, val); 526 } 527 528 int mv88e6352_hwtstamp_port_disable(struct mv88e6xxx_chip *chip, int port) 529 { 530 return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0, 531 MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP); 532 } 533 534 int mv88e6352_hwtstamp_port_enable(struct mv88e6xxx_chip *chip, int port) 535 { 536 return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0, 537 MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH); 538 } 539 540 static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port) 541 { 542 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 543 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 544 545 ps->port_id = port; 546 547 skb_queue_head_init(&ps->rx_queue); 548 skb_queue_head_init(&ps->rx_queue2); 549 550 if (ptp_ops->port_disable) 551 return ptp_ops->port_disable(chip, port); 552 553 return 0; 554 } 555 556 int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip) 557 { 558 const struct mv88e6xxx_ptp_ops *ptp_ops = chip->info->ops->ptp_ops; 559 int err; 560 int i; 561 562 /* Disable timestamping on all ports. */ 563 for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) { 564 err = mv88e6xxx_hwtstamp_port_setup(chip, i); 565 if (err) 566 return err; 567 } 568 569 /* Disable PTP globally */ 570 if (ptp_ops->global_disable) { 571 err = ptp_ops->global_disable(chip); 572 if (err) 573 return err; 574 } 575 576 /* Set the ethertype of L2 PTP messages */ 577 err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_GC_ETYPE, ETH_P_1588); 578 if (err) 579 return err; 580 581 /* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to 582 * timestamp. This affects all ports that have timestamping enabled, 583 * but the timestamp config is per-port; thus we configure all events 584 * here and only support the HWTSTAMP_FILTER_*_EVENT filter types. 585 */ 586 err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE, 587 MV88E6XXX_PTP_MSGTYPE_ALL_EVENT); 588 if (err) 589 return err; 590 591 /* Use ARRIVAL1 for peer delay response messages. */ 592 err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR, 593 MV88E6XXX_PTP_MSGTYPE_PDLAY_RES); 594 if (err) 595 return err; 596 597 /* 88E6341 devices default to timestamping at the PHY, but this has 598 * a hardware issue that results in unreliable timestamps. Force 599 * these devices to timestamp at the MAC. 600 */ 601 if (chip->info->family == MV88E6XXX_FAMILY_6341) { 602 u16 val = MV88E6341_PTP_CFG_UPDATE | 603 MV88E6341_PTP_CFG_MODE_IDX | 604 MV88E6341_PTP_CFG_MODE_TS_AT_MAC; 605 err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, val); 606 if (err) 607 return err; 608 } 609 610 return 0; 611 } 612 613 void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip) 614 { 615 } 616