1 /* 2 * Marvell 88E6xxx Switch hardware timestamping support 3 * 4 * Copyright (c) 2008 Marvell Semiconductor 5 * 6 * Copyright (c) 2017 National Instruments 7 * Erik Hons <erik.hons@ni.com> 8 * Brandon Streiff <brandon.streiff@ni.com> 9 * Dane Wagner <dane.wagner@ni.com> 10 * 11 * This program is free software; you can redistribute it and/or modify 12 * it under the terms of the GNU General Public License as published by 13 * the Free Software Foundation; either version 2 of the License, or 14 * (at your option) any later version. 15 */ 16 17 #include "chip.h" 18 #include "global2.h" 19 #include "hwtstamp.h" 20 #include "ptp.h" 21 #include <linux/ptp_classify.h> 22 23 #define SKB_PTP_TYPE(__skb) (*(unsigned int *)((__skb)->cb)) 24 25 static int mv88e6xxx_port_ptp_read(struct mv88e6xxx_chip *chip, int port, 26 int addr, u16 *data, int len) 27 { 28 if (!chip->info->ops->avb_ops->port_ptp_read) 29 return -EOPNOTSUPP; 30 31 return chip->info->ops->avb_ops->port_ptp_read(chip, port, addr, 32 data, len); 33 } 34 35 static int mv88e6xxx_port_ptp_write(struct mv88e6xxx_chip *chip, int port, 36 int addr, u16 data) 37 { 38 if (!chip->info->ops->avb_ops->port_ptp_write) 39 return -EOPNOTSUPP; 40 41 return chip->info->ops->avb_ops->port_ptp_write(chip, port, addr, 42 data); 43 } 44 45 static int mv88e6xxx_ptp_write(struct mv88e6xxx_chip *chip, int addr, 46 u16 data) 47 { 48 if (!chip->info->ops->avb_ops->ptp_write) 49 return -EOPNOTSUPP; 50 51 return chip->info->ops->avb_ops->ptp_write(chip, addr, data); 52 } 53 54 /* TX_TSTAMP_TIMEOUT: This limits the time spent polling for a TX 55 * timestamp. When working properly, hardware will produce a timestamp 56 * within 1ms. Software may enounter delays due to MDIO contention, so 57 * the timeout is set accordingly. 58 */ 59 #define TX_TSTAMP_TIMEOUT msecs_to_jiffies(20) 60 61 int mv88e6xxx_get_ts_info(struct dsa_switch *ds, int port, 62 struct ethtool_ts_info *info) 63 { 64 struct mv88e6xxx_chip *chip = ds->priv; 65 66 if (!chip->info->ptp_support) 67 return -EOPNOTSUPP; 68 69 info->so_timestamping = 70 SOF_TIMESTAMPING_TX_HARDWARE | 71 SOF_TIMESTAMPING_RX_HARDWARE | 72 SOF_TIMESTAMPING_RAW_HARDWARE; 73 info->phc_index = ptp_clock_index(chip->ptp_clock); 74 info->tx_types = 75 (1 << HWTSTAMP_TX_OFF) | 76 (1 << HWTSTAMP_TX_ON); 77 info->rx_filters = 78 (1 << HWTSTAMP_FILTER_NONE) | 79 (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) | 80 (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) | 81 (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) | 82 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) | 83 (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) | 84 (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) | 85 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) | 86 (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) | 87 (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ); 88 89 return 0; 90 } 91 92 static int mv88e6xxx_set_hwtstamp_config(struct mv88e6xxx_chip *chip, int port, 93 struct hwtstamp_config *config) 94 { 95 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 96 bool tstamp_enable = false; 97 u16 port_config0; 98 int err; 99 100 /* Prevent the TX/RX paths from trying to interact with the 101 * timestamp hardware while we reconfigure it. 102 */ 103 clear_bit_unlock(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state); 104 105 /* reserved for future extensions */ 106 if (config->flags) 107 return -EINVAL; 108 109 switch (config->tx_type) { 110 case HWTSTAMP_TX_OFF: 111 tstamp_enable = false; 112 break; 113 case HWTSTAMP_TX_ON: 114 tstamp_enable = true; 115 break; 116 default: 117 return -ERANGE; 118 } 119 120 /* The switch supports timestamping both L2 and L4; one cannot be 121 * disabled independently of the other. 122 */ 123 switch (config->rx_filter) { 124 case HWTSTAMP_FILTER_NONE: 125 tstamp_enable = false; 126 break; 127 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 128 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 129 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 130 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 131 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 132 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 133 case HWTSTAMP_FILTER_PTP_V2_EVENT: 134 case HWTSTAMP_FILTER_PTP_V2_SYNC: 135 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 136 config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 137 break; 138 case HWTSTAMP_FILTER_ALL: 139 default: 140 config->rx_filter = HWTSTAMP_FILTER_NONE; 141 return -ERANGE; 142 } 143 144 if (tstamp_enable) { 145 /* Disable transportSpecific value matching, so that packets 146 * with either 1588 (0) and 802.1AS (1) will be timestamped. 147 */ 148 port_config0 = MV88E6XXX_PORT_PTP_CFG0_DISABLE_TSPEC_MATCH; 149 } else { 150 /* Disable PTP. This disables both RX and TX timestamping. */ 151 port_config0 = MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP; 152 } 153 154 mutex_lock(&chip->reg_lock); 155 err = mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0, 156 port_config0); 157 mutex_unlock(&chip->reg_lock); 158 159 if (err < 0) 160 return err; 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 /* Get the start of the PTP header in this skb */ 211 static u8 *parse_ptp_header(struct sk_buff *skb, unsigned int type) 212 { 213 u8 *data = skb_mac_header(skb); 214 unsigned int offset = 0; 215 216 if (type & PTP_CLASS_VLAN) 217 offset += VLAN_HLEN; 218 219 switch (type & PTP_CLASS_PMASK) { 220 case PTP_CLASS_IPV4: 221 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; 222 break; 223 case PTP_CLASS_IPV6: 224 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; 225 break; 226 case PTP_CLASS_L2: 227 offset += ETH_HLEN; 228 break; 229 default: 230 return NULL; 231 } 232 233 /* Ensure that the entire header is present in this packet. */ 234 if (skb->len + ETH_HLEN < offset + 34) 235 return NULL; 236 237 return data + offset; 238 } 239 240 /* Returns a pointer to the PTP header if the caller should time stamp, 241 * or NULL if the caller should not. 242 */ 243 static u8 *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip, int port, 244 struct sk_buff *skb, unsigned int type) 245 { 246 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 247 u8 *hdr; 248 249 if (!chip->info->ptp_support) 250 return NULL; 251 252 hdr = parse_ptp_header(skb, type); 253 if (!hdr) 254 return NULL; 255 256 if (!test_bit(MV88E6XXX_HWTSTAMP_ENABLED, &ps->state)) 257 return NULL; 258 259 return hdr; 260 } 261 262 static int mv88e6xxx_ts_valid(u16 status) 263 { 264 if (!(status & MV88E6XXX_PTP_TS_VALID)) 265 return 0; 266 if (status & MV88E6XXX_PTP_TS_STATUS_MASK) 267 return 0; 268 return 1; 269 } 270 271 static int seq_match(struct sk_buff *skb, u16 ts_seqid) 272 { 273 unsigned int type = SKB_PTP_TYPE(skb); 274 u8 *hdr = parse_ptp_header(skb, type); 275 __be16 *seqid; 276 277 seqid = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID); 278 279 return ts_seqid == ntohs(*seqid); 280 } 281 282 static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip, 283 struct mv88e6xxx_port_hwtstamp *ps, 284 struct sk_buff *skb, u16 reg, 285 struct sk_buff_head *rxq) 286 { 287 u16 buf[4] = { 0 }, status, seq_id; 288 struct skb_shared_hwtstamps *shwt; 289 struct sk_buff_head received; 290 u64 ns, timelo, timehi; 291 unsigned long flags; 292 int err; 293 294 /* The latched timestamp belongs to one of the received frames. */ 295 __skb_queue_head_init(&received); 296 spin_lock_irqsave(&rxq->lock, flags); 297 skb_queue_splice_tail_init(rxq, &received); 298 spin_unlock_irqrestore(&rxq->lock, flags); 299 300 mutex_lock(&chip->reg_lock); 301 err = mv88e6xxx_port_ptp_read(chip, ps->port_id, 302 reg, buf, ARRAY_SIZE(buf)); 303 mutex_unlock(&chip->reg_lock); 304 if (err) 305 pr_err("failed to get the receive time stamp\n"); 306 307 status = buf[0]; 308 timelo = buf[1]; 309 timehi = buf[2]; 310 seq_id = buf[3]; 311 312 if (status & MV88E6XXX_PTP_TS_VALID) { 313 mutex_lock(&chip->reg_lock); 314 err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0); 315 mutex_unlock(&chip->reg_lock); 316 if (err) 317 pr_err("failed to clear the receive status\n"); 318 } 319 /* Since the device can only handle one time stamp at a time, 320 * we purge any extra frames from the queue. 321 */ 322 for ( ; skb; skb = __skb_dequeue(&received)) { 323 if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) { 324 ns = timehi << 16 | timelo; 325 326 mutex_lock(&chip->reg_lock); 327 ns = timecounter_cyc2time(&chip->tstamp_tc, ns); 328 mutex_unlock(&chip->reg_lock); 329 shwt = skb_hwtstamps(skb); 330 memset(shwt, 0, sizeof(*shwt)); 331 shwt->hwtstamp = ns_to_ktime(ns); 332 status &= ~MV88E6XXX_PTP_TS_VALID; 333 } 334 netif_rx_ni(skb); 335 } 336 } 337 338 static void mv88e6xxx_rxtstamp_work(struct mv88e6xxx_chip *chip, 339 struct mv88e6xxx_port_hwtstamp *ps) 340 { 341 struct sk_buff *skb; 342 343 skb = skb_dequeue(&ps->rx_queue); 344 345 if (skb) 346 mv88e6xxx_get_rxts(chip, ps, skb, MV88E6XXX_PORT_PTP_ARR0_STS, 347 &ps->rx_queue); 348 349 skb = skb_dequeue(&ps->rx_queue2); 350 if (skb) 351 mv88e6xxx_get_rxts(chip, ps, skb, MV88E6XXX_PORT_PTP_ARR1_STS, 352 &ps->rx_queue2); 353 } 354 355 static int is_pdelay_resp(u8 *msgtype) 356 { 357 return (*msgtype & 0xf) == 3; 358 } 359 360 bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port, 361 struct sk_buff *skb, unsigned int type) 362 { 363 struct mv88e6xxx_port_hwtstamp *ps; 364 struct mv88e6xxx_chip *chip; 365 u8 *hdr; 366 367 chip = ds->priv; 368 ps = &chip->port_hwtstamp[port]; 369 370 if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT) 371 return false; 372 373 hdr = mv88e6xxx_should_tstamp(chip, port, skb, type); 374 if (!hdr) 375 return false; 376 377 SKB_PTP_TYPE(skb) = type; 378 379 if (is_pdelay_resp(hdr)) 380 skb_queue_tail(&ps->rx_queue2, skb); 381 else 382 skb_queue_tail(&ps->rx_queue, skb); 383 384 ptp_schedule_worker(chip->ptp_clock, 0); 385 386 return true; 387 } 388 389 static int mv88e6xxx_txtstamp_work(struct mv88e6xxx_chip *chip, 390 struct mv88e6xxx_port_hwtstamp *ps) 391 { 392 struct skb_shared_hwtstamps shhwtstamps; 393 u16 departure_block[4], status; 394 struct sk_buff *tmp_skb; 395 u32 time_raw; 396 int err; 397 u64 ns; 398 399 if (!ps->tx_skb) 400 return 0; 401 402 mutex_lock(&chip->reg_lock); 403 err = mv88e6xxx_port_ptp_read(chip, ps->port_id, 404 MV88E6XXX_PORT_PTP_DEP_STS, 405 departure_block, 406 ARRAY_SIZE(departure_block)); 407 mutex_unlock(&chip->reg_lock); 408 409 if (err) 410 goto free_and_clear_skb; 411 412 if (!(departure_block[0] & MV88E6XXX_PTP_TS_VALID)) { 413 if (time_is_before_jiffies(ps->tx_tstamp_start + 414 TX_TSTAMP_TIMEOUT)) { 415 dev_warn(chip->dev, "p%d: clearing tx timestamp hang\n", 416 ps->port_id); 417 goto free_and_clear_skb; 418 } 419 /* The timestamp should be available quickly, while getting it 420 * is high priority and time bounded to only 10ms. A poll is 421 * warranted so restart the work. 422 */ 423 return 1; 424 } 425 426 /* We have the timestamp; go ahead and clear valid now */ 427 mutex_lock(&chip->reg_lock); 428 mv88e6xxx_port_ptp_write(chip, ps->port_id, 429 MV88E6XXX_PORT_PTP_DEP_STS, 0); 430 mutex_unlock(&chip->reg_lock); 431 432 status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK; 433 if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) { 434 dev_warn(chip->dev, "p%d: tx timestamp overrun\n", ps->port_id); 435 goto free_and_clear_skb; 436 } 437 438 if (departure_block[3] != ps->tx_seq_id) { 439 dev_warn(chip->dev, "p%d: unexpected seq. id\n", ps->port_id); 440 goto free_and_clear_skb; 441 } 442 443 memset(&shhwtstamps, 0, sizeof(shhwtstamps)); 444 time_raw = ((u32)departure_block[2] << 16) | departure_block[1]; 445 mutex_lock(&chip->reg_lock); 446 ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw); 447 mutex_unlock(&chip->reg_lock); 448 shhwtstamps.hwtstamp = ns_to_ktime(ns); 449 450 dev_dbg(chip->dev, 451 "p%d: txtstamp %llx status 0x%04x skb ID 0x%04x hw ID 0x%04x\n", 452 ps->port_id, ktime_to_ns(shhwtstamps.hwtstamp), 453 departure_block[0], ps->tx_seq_id, departure_block[3]); 454 455 /* skb_complete_tx_timestamp() will free up the client to make 456 * another timestamp-able transmit. We have to be ready for it 457 * -- by clearing the ps->tx_skb "flag" -- beforehand. 458 */ 459 460 tmp_skb = ps->tx_skb; 461 ps->tx_skb = NULL; 462 clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state); 463 skb_complete_tx_timestamp(tmp_skb, &shhwtstamps); 464 465 return 0; 466 467 free_and_clear_skb: 468 dev_kfree_skb_any(ps->tx_skb); 469 ps->tx_skb = NULL; 470 clear_bit_unlock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state); 471 472 return 0; 473 } 474 475 long mv88e6xxx_hwtstamp_work(struct ptp_clock_info *ptp) 476 { 477 struct mv88e6xxx_chip *chip = ptp_to_chip(ptp); 478 struct dsa_switch *ds = chip->ds; 479 struct mv88e6xxx_port_hwtstamp *ps; 480 int i, restart = 0; 481 482 for (i = 0; i < ds->num_ports; i++) { 483 if (!dsa_is_user_port(ds, i)) 484 continue; 485 486 ps = &chip->port_hwtstamp[i]; 487 if (test_bit(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, &ps->state)) 488 restart |= mv88e6xxx_txtstamp_work(chip, ps); 489 490 mv88e6xxx_rxtstamp_work(chip, ps); 491 } 492 493 return restart ? 1 : -1; 494 } 495 496 bool mv88e6xxx_port_txtstamp(struct dsa_switch *ds, int port, 497 struct sk_buff *clone, unsigned int type) 498 { 499 struct mv88e6xxx_chip *chip = ds->priv; 500 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 501 __be16 *seq_ptr; 502 u8 *hdr; 503 504 if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP)) 505 return false; 506 507 hdr = mv88e6xxx_should_tstamp(chip, port, clone, type); 508 if (!hdr) 509 return false; 510 511 seq_ptr = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID); 512 513 if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS, 514 &ps->state)) 515 return false; 516 517 ps->tx_skb = clone; 518 ps->tx_tstamp_start = jiffies; 519 ps->tx_seq_id = be16_to_cpup(seq_ptr); 520 521 ptp_schedule_worker(chip->ptp_clock, 0); 522 return true; 523 } 524 525 static int mv88e6xxx_hwtstamp_port_setup(struct mv88e6xxx_chip *chip, int port) 526 { 527 struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port]; 528 529 ps->port_id = port; 530 531 skb_queue_head_init(&ps->rx_queue); 532 skb_queue_head_init(&ps->rx_queue2); 533 534 return mv88e6xxx_port_ptp_write(chip, port, MV88E6XXX_PORT_PTP_CFG0, 535 MV88E6XXX_PORT_PTP_CFG0_DISABLE_PTP); 536 } 537 538 int mv88e6xxx_hwtstamp_setup(struct mv88e6xxx_chip *chip) 539 { 540 int err; 541 int i; 542 543 /* Disable timestamping on all ports. */ 544 for (i = 0; i < mv88e6xxx_num_ports(chip); ++i) { 545 err = mv88e6xxx_hwtstamp_port_setup(chip, i); 546 if (err) 547 return err; 548 } 549 550 /* MV88E6XXX_PTP_MSG_TYPE is a mask of PTP message types to 551 * timestamp. This affects all ports that have timestamping enabled, 552 * but the timestamp config is per-port; thus we configure all events 553 * here and only support the HWTSTAMP_FILTER_*_EVENT filter types. 554 */ 555 err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_MSGTYPE, 556 MV88E6XXX_PTP_MSGTYPE_ALL_EVENT); 557 if (err) 558 return err; 559 560 /* Use ARRIVAL1 for peer delay response messages. */ 561 err = mv88e6xxx_ptp_write(chip, MV88E6XXX_PTP_TS_ARRIVAL_PTR, 562 MV88E6XXX_PTP_MSGTYPE_PDLAY_RES); 563 if (err) 564 return err; 565 566 /* 88E6341 devices default to timestamping at the PHY, but this has 567 * a hardware issue that results in unreliable timestamps. Force 568 * these devices to timestamp at the MAC. 569 */ 570 if (chip->info->family == MV88E6XXX_FAMILY_6341) { 571 u16 val = MV88E6341_PTP_CFG_UPDATE | 572 MV88E6341_PTP_CFG_MODE_IDX | 573 MV88E6341_PTP_CFG_MODE_TS_AT_MAC; 574 err = mv88e6xxx_ptp_write(chip, MV88E6341_PTP_CFG, val); 575 if (err) 576 return err; 577 } 578 579 return 0; 580 } 581 582 void mv88e6xxx_hwtstamp_free(struct mv88e6xxx_chip *chip) 583 { 584 } 585