1 // SPDX-License-Identifier: GPL-2.0 2 // 3 // Copyright (C) 2018 MOSER-BAER AG 4 // 5 6 #define pr_fmt(fmt) "InES_PTP: " fmt 7 8 #include <linux/ethtool.h> 9 #include <linux/export.h> 10 #include <linux/if_vlan.h> 11 #include <linux/mii_timestamper.h> 12 #include <linux/module.h> 13 #include <linux/net_tstamp.h> 14 #include <linux/of.h> 15 #include <linux/of_address.h> 16 #include <linux/of_irq.h> 17 #include <linux/phy.h> 18 #include <linux/platform_device.h> 19 #include <linux/ptp_classify.h> 20 #include <linux/ptp_clock_kernel.h> 21 #include <linux/stddef.h> 22 23 MODULE_DESCRIPTION("Driver for the ZHAW InES PTP time stamping IP core"); 24 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>"); 25 MODULE_VERSION("1.0"); 26 MODULE_LICENSE("GPL"); 27 28 /* GLOBAL register */ 29 #define MCAST_MAC_SELECT_SHIFT 2 30 #define MCAST_MAC_SELECT_MASK 0x3 31 #define IO_RESET BIT(1) 32 #define PTP_RESET BIT(0) 33 34 /* VERSION register */ 35 #define IF_MAJOR_VER_SHIFT 12 36 #define IF_MAJOR_VER_MASK 0xf 37 #define IF_MINOR_VER_SHIFT 8 38 #define IF_MINOR_VER_MASK 0xf 39 #define FPGA_MAJOR_VER_SHIFT 4 40 #define FPGA_MAJOR_VER_MASK 0xf 41 #define FPGA_MINOR_VER_SHIFT 0 42 #define FPGA_MINOR_VER_MASK 0xf 43 44 /* INT_STAT register */ 45 #define RX_INTR_STATUS_3 BIT(5) 46 #define RX_INTR_STATUS_2 BIT(4) 47 #define RX_INTR_STATUS_1 BIT(3) 48 #define TX_INTR_STATUS_3 BIT(2) 49 #define TX_INTR_STATUS_2 BIT(1) 50 #define TX_INTR_STATUS_1 BIT(0) 51 52 /* INT_MSK register */ 53 #define RX_INTR_MASK_3 BIT(5) 54 #define RX_INTR_MASK_2 BIT(4) 55 #define RX_INTR_MASK_1 BIT(3) 56 #define TX_INTR_MASK_3 BIT(2) 57 #define TX_INTR_MASK_2 BIT(1) 58 #define TX_INTR_MASK_1 BIT(0) 59 60 /* BUF_STAT register */ 61 #define RX_FIFO_NE_3 BIT(5) 62 #define RX_FIFO_NE_2 BIT(4) 63 #define RX_FIFO_NE_1 BIT(3) 64 #define TX_FIFO_NE_3 BIT(2) 65 #define TX_FIFO_NE_2 BIT(1) 66 #define TX_FIFO_NE_1 BIT(0) 67 68 /* PORT_CONF register */ 69 #define CM_ONE_STEP BIT(6) 70 #define PHY_SPEED_SHIFT 4 71 #define PHY_SPEED_MASK 0x3 72 #define P2P_DELAY_WR_POS_SHIFT 2 73 #define P2P_DELAY_WR_POS_MASK 0x3 74 #define PTP_MODE_SHIFT 0 75 #define PTP_MODE_MASK 0x3 76 77 /* TS_STAT_TX register */ 78 #define TS_ENABLE BIT(15) 79 #define DATA_READ_POS_SHIFT 8 80 #define DATA_READ_POS_MASK 0x1f 81 #define DISCARDED_EVENTS_SHIFT 4 82 #define DISCARDED_EVENTS_MASK 0xf 83 84 #define INES_N_PORTS 3 85 #define INES_REGISTER_SIZE 0x80 86 #define INES_PORT_OFFSET 0x20 87 #define INES_PORT_SIZE 0x20 88 #define INES_FIFO_DEPTH 90 89 #define INES_MAX_EVENTS 100 90 91 #define BC_PTP_V1 0 92 #define BC_PTP_V2 1 93 #define TC_E2E_PTP_V2 2 94 #define TC_P2P_PTP_V2 3 95 96 #define OFF_PTP_CLOCK_ID 20 97 #define OFF_PTP_PORT_NUM 28 98 99 #define PHY_SPEED_10 0 100 #define PHY_SPEED_100 1 101 #define PHY_SPEED_1000 2 102 103 #define PORT_CONF \ 104 ((PHY_SPEED_1000 << PHY_SPEED_SHIFT) | (BC_PTP_V2 << PTP_MODE_SHIFT)) 105 106 #define ines_read32(s, r) __raw_readl((void __iomem *)&s->regs->r) 107 #define ines_write32(s, v, r) __raw_writel(v, (void __iomem *)&s->regs->r) 108 109 #define MESSAGE_TYPE_SYNC 1 110 #define MESSAGE_TYPE_P_DELAY_REQ 2 111 #define MESSAGE_TYPE_P_DELAY_RESP 3 112 #define MESSAGE_TYPE_DELAY_REQ 4 113 114 #define SYNC 0x0 115 #define DELAY_REQ 0x1 116 #define PDELAY_REQ 0x2 117 #define PDELAY_RESP 0x3 118 119 static LIST_HEAD(ines_clocks); 120 static DEFINE_MUTEX(ines_clocks_lock); 121 122 struct ines_global_regs { 123 u32 id; 124 u32 test; 125 u32 global; 126 u32 version; 127 u32 test2; 128 u32 int_stat; 129 u32 int_msk; 130 u32 buf_stat; 131 }; 132 133 struct ines_port_registers { 134 u32 port_conf; 135 u32 p_delay; 136 u32 ts_stat_tx; 137 u32 ts_stat_rx; 138 u32 ts_tx; 139 u32 ts_rx; 140 }; 141 142 struct ines_timestamp { 143 struct list_head list; 144 unsigned long tmo; 145 u16 tag; 146 u64 sec; 147 u64 nsec; 148 u64 clkid; 149 u16 portnum; 150 u16 seqid; 151 }; 152 153 struct ines_port { 154 struct ines_port_registers *regs; 155 struct mii_timestamper mii_ts; 156 struct ines_clock *clock; 157 bool rxts_enabled; 158 bool txts_enabled; 159 unsigned int index; 160 struct delayed_work ts_work; 161 /* lock protects event list and tx_skb */ 162 spinlock_t lock; 163 struct sk_buff *tx_skb; 164 struct list_head events; 165 struct list_head pool; 166 struct ines_timestamp pool_data[INES_MAX_EVENTS]; 167 }; 168 169 struct ines_clock { 170 struct ines_port port[INES_N_PORTS]; 171 struct ines_global_regs __iomem *regs; 172 void __iomem *base; 173 struct device_node *node; 174 struct device *dev; 175 struct list_head list; 176 }; 177 178 static bool ines_match(struct sk_buff *skb, unsigned int ptp_class, 179 struct ines_timestamp *ts, struct device *dev); 180 static int ines_rxfifo_read(struct ines_port *port); 181 static u64 ines_rxts64(struct ines_port *port, unsigned int words); 182 static bool ines_timestamp_expired(struct ines_timestamp *ts); 183 static u64 ines_txts64(struct ines_port *port, unsigned int words); 184 static void ines_txtstamp_work(struct work_struct *work); 185 static bool is_sync_pdelay_resp(struct sk_buff *skb, int type); 186 static u8 tag_to_msgtype(u8 tag); 187 188 static void ines_clock_cleanup(struct ines_clock *clock) 189 { 190 struct ines_port *port; 191 int i; 192 193 for (i = 0; i < INES_N_PORTS; i++) { 194 port = &clock->port[i]; 195 cancel_delayed_work_sync(&port->ts_work); 196 } 197 } 198 199 static int ines_clock_init(struct ines_clock *clock, struct device *device, 200 void __iomem *addr) 201 { 202 struct device_node *node = device->of_node; 203 unsigned long port_addr; 204 struct ines_port *port; 205 int i, j; 206 207 INIT_LIST_HEAD(&clock->list); 208 clock->node = node; 209 clock->dev = device; 210 clock->base = addr; 211 clock->regs = clock->base; 212 213 for (i = 0; i < INES_N_PORTS; i++) { 214 port = &clock->port[i]; 215 port_addr = (unsigned long) clock->base + 216 INES_PORT_OFFSET + i * INES_PORT_SIZE; 217 port->regs = (struct ines_port_registers *) port_addr; 218 port->clock = clock; 219 port->index = i; 220 INIT_DELAYED_WORK(&port->ts_work, ines_txtstamp_work); 221 spin_lock_init(&port->lock); 222 INIT_LIST_HEAD(&port->events); 223 INIT_LIST_HEAD(&port->pool); 224 for (j = 0; j < INES_MAX_EVENTS; j++) 225 list_add(&port->pool_data[j].list, &port->pool); 226 } 227 228 ines_write32(clock, 0xBEEF, test); 229 ines_write32(clock, 0xBEEF, test2); 230 231 dev_dbg(device, "ID 0x%x\n", ines_read32(clock, id)); 232 dev_dbg(device, "TEST 0x%x\n", ines_read32(clock, test)); 233 dev_dbg(device, "VERSION 0x%x\n", ines_read32(clock, version)); 234 dev_dbg(device, "TEST2 0x%x\n", ines_read32(clock, test2)); 235 236 for (i = 0; i < INES_N_PORTS; i++) { 237 port = &clock->port[i]; 238 ines_write32(port, PORT_CONF, port_conf); 239 } 240 241 return 0; 242 } 243 244 static struct ines_port *ines_find_port(struct device_node *node, u32 index) 245 { 246 struct ines_port *port = NULL; 247 struct ines_clock *clock; 248 struct list_head *this; 249 250 mutex_lock(&ines_clocks_lock); 251 list_for_each(this, &ines_clocks) { 252 clock = list_entry(this, struct ines_clock, list); 253 if (clock->node == node) { 254 port = &clock->port[index]; 255 break; 256 } 257 } 258 mutex_unlock(&ines_clocks_lock); 259 return port; 260 } 261 262 static u64 ines_find_rxts(struct ines_port *port, struct sk_buff *skb, int type) 263 { 264 struct list_head *this, *next; 265 struct ines_timestamp *ts; 266 unsigned long flags; 267 u64 ns = 0; 268 269 if (type == PTP_CLASS_NONE) 270 return 0; 271 272 spin_lock_irqsave(&port->lock, flags); 273 ines_rxfifo_read(port); 274 list_for_each_safe(this, next, &port->events) { 275 ts = list_entry(this, struct ines_timestamp, list); 276 if (ines_timestamp_expired(ts)) { 277 list_del_init(&ts->list); 278 list_add(&ts->list, &port->pool); 279 continue; 280 } 281 if (ines_match(skb, type, ts, port->clock->dev)) { 282 ns = ts->sec * 1000000000ULL + ts->nsec; 283 list_del_init(&ts->list); 284 list_add(&ts->list, &port->pool); 285 break; 286 } 287 } 288 spin_unlock_irqrestore(&port->lock, flags); 289 290 return ns; 291 } 292 293 static u64 ines_find_txts(struct ines_port *port, struct sk_buff *skb) 294 { 295 unsigned int class = ptp_classify_raw(skb), i; 296 u32 data_rd_pos, buf_stat, mask, ts_stat_tx; 297 struct ines_timestamp ts; 298 unsigned long flags; 299 u64 ns = 0; 300 301 mask = TX_FIFO_NE_1 << port->index; 302 303 spin_lock_irqsave(&port->lock, flags); 304 305 for (i = 0; i < INES_FIFO_DEPTH; i++) { 306 307 buf_stat = ines_read32(port->clock, buf_stat); 308 if (!(buf_stat & mask)) { 309 dev_dbg(port->clock->dev, 310 "Tx timestamp FIFO unexpectedly empty\n"); 311 break; 312 } 313 ts_stat_tx = ines_read32(port, ts_stat_tx); 314 data_rd_pos = (ts_stat_tx >> DATA_READ_POS_SHIFT) & 315 DATA_READ_POS_MASK; 316 if (data_rd_pos) { 317 dev_err(port->clock->dev, 318 "unexpected Tx read pos %u\n", data_rd_pos); 319 break; 320 } 321 322 ts.tag = ines_read32(port, ts_tx); 323 ts.sec = ines_txts64(port, 3); 324 ts.nsec = ines_txts64(port, 2); 325 ts.clkid = ines_txts64(port, 4); 326 ts.portnum = ines_read32(port, ts_tx); 327 ts.seqid = ines_read32(port, ts_tx); 328 329 if (ines_match(skb, class, &ts, port->clock->dev)) { 330 ns = ts.sec * 1000000000ULL + ts.nsec; 331 break; 332 } 333 } 334 335 spin_unlock_irqrestore(&port->lock, flags); 336 return ns; 337 } 338 339 static int ines_hwtstamp(struct mii_timestamper *mii_ts, struct ifreq *ifr) 340 { 341 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts); 342 u32 cm_one_step = 0, port_conf, ts_stat_rx, ts_stat_tx; 343 struct hwtstamp_config cfg; 344 unsigned long flags; 345 346 if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg))) 347 return -EFAULT; 348 349 /* reserved for future extensions */ 350 if (cfg.flags) 351 return -EINVAL; 352 353 switch (cfg.tx_type) { 354 case HWTSTAMP_TX_OFF: 355 ts_stat_tx = 0; 356 break; 357 case HWTSTAMP_TX_ON: 358 ts_stat_tx = TS_ENABLE; 359 break; 360 case HWTSTAMP_TX_ONESTEP_P2P: 361 ts_stat_tx = TS_ENABLE; 362 cm_one_step = CM_ONE_STEP; 363 break; 364 default: 365 return -ERANGE; 366 } 367 368 switch (cfg.rx_filter) { 369 case HWTSTAMP_FILTER_NONE: 370 ts_stat_rx = 0; 371 break; 372 case HWTSTAMP_FILTER_ALL: 373 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: 374 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: 375 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: 376 return -ERANGE; 377 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 378 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 379 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 380 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 381 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 382 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 383 case HWTSTAMP_FILTER_PTP_V2_EVENT: 384 case HWTSTAMP_FILTER_PTP_V2_SYNC: 385 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 386 ts_stat_rx = TS_ENABLE; 387 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 388 break; 389 default: 390 return -ERANGE; 391 } 392 393 spin_lock_irqsave(&port->lock, flags); 394 395 port_conf = ines_read32(port, port_conf); 396 port_conf &= ~CM_ONE_STEP; 397 port_conf |= cm_one_step; 398 399 ines_write32(port, port_conf, port_conf); 400 ines_write32(port, ts_stat_rx, ts_stat_rx); 401 ines_write32(port, ts_stat_tx, ts_stat_tx); 402 403 port->rxts_enabled = ts_stat_rx == TS_ENABLE; 404 port->txts_enabled = ts_stat_tx == TS_ENABLE; 405 406 spin_unlock_irqrestore(&port->lock, flags); 407 408 return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0; 409 } 410 411 static void ines_link_state(struct mii_timestamper *mii_ts, 412 struct phy_device *phydev) 413 { 414 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts); 415 u32 port_conf, speed_conf; 416 unsigned long flags; 417 418 switch (phydev->speed) { 419 case SPEED_10: 420 speed_conf = PHY_SPEED_10 << PHY_SPEED_SHIFT; 421 break; 422 case SPEED_100: 423 speed_conf = PHY_SPEED_100 << PHY_SPEED_SHIFT; 424 break; 425 case SPEED_1000: 426 speed_conf = PHY_SPEED_1000 << PHY_SPEED_SHIFT; 427 break; 428 default: 429 dev_err(port->clock->dev, "bad speed: %d\n", phydev->speed); 430 return; 431 } 432 spin_lock_irqsave(&port->lock, flags); 433 434 port_conf = ines_read32(port, port_conf); 435 port_conf &= ~(0x3 << PHY_SPEED_SHIFT); 436 port_conf |= speed_conf; 437 438 ines_write32(port, port_conf, port_conf); 439 440 spin_unlock_irqrestore(&port->lock, flags); 441 } 442 443 static bool ines_match(struct sk_buff *skb, unsigned int ptp_class, 444 struct ines_timestamp *ts, struct device *dev) 445 { 446 u8 *msgtype, *data = skb_mac_header(skb); 447 unsigned int offset = 0; 448 __be16 *portn, *seqid; 449 __be64 *clkid; 450 451 if (unlikely(ptp_class & PTP_CLASS_V1)) 452 return false; 453 454 if (ptp_class & PTP_CLASS_VLAN) 455 offset += VLAN_HLEN; 456 457 switch (ptp_class & PTP_CLASS_PMASK) { 458 case PTP_CLASS_IPV4: 459 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; 460 break; 461 case PTP_CLASS_IPV6: 462 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; 463 break; 464 case PTP_CLASS_L2: 465 offset += ETH_HLEN; 466 break; 467 default: 468 return false; 469 } 470 471 if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid)) 472 return false; 473 474 msgtype = data + offset; 475 clkid = (__be64 *)(data + offset + OFF_PTP_CLOCK_ID); 476 portn = (__be16 *)(data + offset + OFF_PTP_PORT_NUM); 477 seqid = (__be16 *)(data + offset + OFF_PTP_SEQUENCE_ID); 478 479 if (tag_to_msgtype(ts->tag & 0x7) != (*msgtype & 0xf)) { 480 dev_dbg(dev, "msgtype mismatch ts %hhu != skb %hhu\n", 481 tag_to_msgtype(ts->tag & 0x7), *msgtype & 0xf); 482 return false; 483 } 484 if (cpu_to_be64(ts->clkid) != *clkid) { 485 dev_dbg(dev, "clkid mismatch ts %llx != skb %llx\n", 486 cpu_to_be64(ts->clkid), *clkid); 487 return false; 488 } 489 if (ts->portnum != ntohs(*portn)) { 490 dev_dbg(dev, "portn mismatch ts %hu != skb %hu\n", 491 ts->portnum, ntohs(*portn)); 492 return false; 493 } 494 if (ts->seqid != ntohs(*seqid)) { 495 dev_dbg(dev, "seqid mismatch ts %hu != skb %hu\n", 496 ts->seqid, ntohs(*seqid)); 497 return false; 498 } 499 500 return true; 501 } 502 503 static bool ines_rxtstamp(struct mii_timestamper *mii_ts, 504 struct sk_buff *skb, int type) 505 { 506 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts); 507 struct skb_shared_hwtstamps *ssh; 508 u64 ns; 509 510 if (!port->rxts_enabled) 511 return false; 512 513 ns = ines_find_rxts(port, skb, type); 514 if (!ns) 515 return false; 516 517 ssh = skb_hwtstamps(skb); 518 ssh->hwtstamp = ns_to_ktime(ns); 519 netif_rx(skb); 520 521 return true; 522 } 523 524 static int ines_rxfifo_read(struct ines_port *port) 525 { 526 u32 data_rd_pos, buf_stat, mask, ts_stat_rx; 527 struct ines_timestamp *ts; 528 unsigned int i; 529 530 mask = RX_FIFO_NE_1 << port->index; 531 532 for (i = 0; i < INES_FIFO_DEPTH; i++) { 533 if (list_empty(&port->pool)) { 534 dev_err(port->clock->dev, "event pool is empty\n"); 535 return -1; 536 } 537 buf_stat = ines_read32(port->clock, buf_stat); 538 if (!(buf_stat & mask)) 539 break; 540 541 ts_stat_rx = ines_read32(port, ts_stat_rx); 542 data_rd_pos = (ts_stat_rx >> DATA_READ_POS_SHIFT) & 543 DATA_READ_POS_MASK; 544 if (data_rd_pos) { 545 dev_err(port->clock->dev, "unexpected Rx read pos %u\n", 546 data_rd_pos); 547 break; 548 } 549 550 ts = list_first_entry(&port->pool, struct ines_timestamp, list); 551 ts->tmo = jiffies + HZ; 552 ts->tag = ines_read32(port, ts_rx); 553 ts->sec = ines_rxts64(port, 3); 554 ts->nsec = ines_rxts64(port, 2); 555 ts->clkid = ines_rxts64(port, 4); 556 ts->portnum = ines_read32(port, ts_rx); 557 ts->seqid = ines_read32(port, ts_rx); 558 559 list_del_init(&ts->list); 560 list_add_tail(&ts->list, &port->events); 561 } 562 563 return 0; 564 } 565 566 static u64 ines_rxts64(struct ines_port *port, unsigned int words) 567 { 568 unsigned int i; 569 u64 result; 570 u16 word; 571 572 word = ines_read32(port, ts_rx); 573 result = word; 574 words--; 575 for (i = 0; i < words; i++) { 576 word = ines_read32(port, ts_rx); 577 result <<= 16; 578 result |= word; 579 } 580 return result; 581 } 582 583 static bool ines_timestamp_expired(struct ines_timestamp *ts) 584 { 585 return time_after(jiffies, ts->tmo); 586 } 587 588 static int ines_ts_info(struct mii_timestamper *mii_ts, 589 struct ethtool_ts_info *info) 590 { 591 info->so_timestamping = 592 SOF_TIMESTAMPING_TX_HARDWARE | 593 SOF_TIMESTAMPING_TX_SOFTWARE | 594 SOF_TIMESTAMPING_RX_HARDWARE | 595 SOF_TIMESTAMPING_RX_SOFTWARE | 596 SOF_TIMESTAMPING_SOFTWARE | 597 SOF_TIMESTAMPING_RAW_HARDWARE; 598 599 info->phc_index = -1; 600 601 info->tx_types = 602 (1 << HWTSTAMP_TX_OFF) | 603 (1 << HWTSTAMP_TX_ON) | 604 (1 << HWTSTAMP_TX_ONESTEP_P2P); 605 606 info->rx_filters = 607 (1 << HWTSTAMP_FILTER_NONE) | 608 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT); 609 610 return 0; 611 } 612 613 static u64 ines_txts64(struct ines_port *port, unsigned int words) 614 { 615 unsigned int i; 616 u64 result; 617 u16 word; 618 619 word = ines_read32(port, ts_tx); 620 result = word; 621 words--; 622 for (i = 0; i < words; i++) { 623 word = ines_read32(port, ts_tx); 624 result <<= 16; 625 result |= word; 626 } 627 return result; 628 } 629 630 static bool ines_txts_onestep(struct ines_port *port, struct sk_buff *skb, int type) 631 { 632 unsigned long flags; 633 u32 port_conf; 634 635 spin_lock_irqsave(&port->lock, flags); 636 port_conf = ines_read32(port, port_conf); 637 spin_unlock_irqrestore(&port->lock, flags); 638 639 if (port_conf & CM_ONE_STEP) 640 return is_sync_pdelay_resp(skb, type); 641 642 return false; 643 } 644 645 static void ines_txtstamp(struct mii_timestamper *mii_ts, 646 struct sk_buff *skb, int type) 647 { 648 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts); 649 struct sk_buff *old_skb = NULL; 650 unsigned long flags; 651 652 if (!port->txts_enabled || ines_txts_onestep(port, skb, type)) { 653 kfree_skb(skb); 654 return; 655 } 656 657 spin_lock_irqsave(&port->lock, flags); 658 659 if (port->tx_skb) 660 old_skb = port->tx_skb; 661 662 port->tx_skb = skb; 663 664 spin_unlock_irqrestore(&port->lock, flags); 665 666 if (old_skb) 667 kfree_skb(old_skb); 668 669 schedule_delayed_work(&port->ts_work, 1); 670 } 671 672 static void ines_txtstamp_work(struct work_struct *work) 673 { 674 struct ines_port *port = 675 container_of(work, struct ines_port, ts_work.work); 676 struct skb_shared_hwtstamps ssh; 677 struct sk_buff *skb; 678 unsigned long flags; 679 u64 ns; 680 681 spin_lock_irqsave(&port->lock, flags); 682 skb = port->tx_skb; 683 port->tx_skb = NULL; 684 spin_unlock_irqrestore(&port->lock, flags); 685 686 ns = ines_find_txts(port, skb); 687 if (!ns) { 688 kfree_skb(skb); 689 return; 690 } 691 ssh.hwtstamp = ns_to_ktime(ns); 692 skb_complete_tx_timestamp(skb, &ssh); 693 } 694 695 static bool is_sync_pdelay_resp(struct sk_buff *skb, int type) 696 { 697 u8 *data = skb->data, *msgtype; 698 unsigned int offset = 0; 699 700 if (type & PTP_CLASS_VLAN) 701 offset += VLAN_HLEN; 702 703 switch (type & PTP_CLASS_PMASK) { 704 case PTP_CLASS_IPV4: 705 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN; 706 break; 707 case PTP_CLASS_IPV6: 708 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN; 709 break; 710 case PTP_CLASS_L2: 711 offset += ETH_HLEN; 712 break; 713 default: 714 return 0; 715 } 716 717 if (type & PTP_CLASS_V1) 718 offset += OFF_PTP_CONTROL; 719 720 if (skb->len < offset + 1) 721 return 0; 722 723 msgtype = data + offset; 724 725 switch ((*msgtype & 0xf)) { 726 case SYNC: 727 case PDELAY_RESP: 728 return true; 729 default: 730 return false; 731 } 732 } 733 734 static u8 tag_to_msgtype(u8 tag) 735 { 736 switch (tag) { 737 case MESSAGE_TYPE_SYNC: 738 return SYNC; 739 case MESSAGE_TYPE_P_DELAY_REQ: 740 return PDELAY_REQ; 741 case MESSAGE_TYPE_P_DELAY_RESP: 742 return PDELAY_RESP; 743 case MESSAGE_TYPE_DELAY_REQ: 744 return DELAY_REQ; 745 } 746 return 0xf; 747 } 748 749 static struct mii_timestamper *ines_ptp_probe_channel(struct device *device, 750 unsigned int index) 751 { 752 struct device_node *node = device->of_node; 753 struct ines_port *port; 754 755 if (index > INES_N_PORTS - 1) { 756 dev_err(device, "bad port index %u\n", index); 757 return ERR_PTR(-EINVAL); 758 } 759 port = ines_find_port(node, index); 760 if (!port) { 761 dev_err(device, "missing port index %u\n", index); 762 return ERR_PTR(-ENODEV); 763 } 764 port->mii_ts.rxtstamp = ines_rxtstamp; 765 port->mii_ts.txtstamp = ines_txtstamp; 766 port->mii_ts.hwtstamp = ines_hwtstamp; 767 port->mii_ts.link_state = ines_link_state; 768 port->mii_ts.ts_info = ines_ts_info; 769 770 return &port->mii_ts; 771 } 772 773 static void ines_ptp_release_channel(struct device *device, 774 struct mii_timestamper *mii_ts) 775 { 776 } 777 778 static struct mii_timestamping_ctrl ines_ctrl = { 779 .probe_channel = ines_ptp_probe_channel, 780 .release_channel = ines_ptp_release_channel, 781 }; 782 783 static int ines_ptp_ctrl_probe(struct platform_device *pld) 784 { 785 struct ines_clock *clock; 786 void __iomem *addr; 787 int err = 0; 788 789 addr = devm_platform_ioremap_resource(pld, 0); 790 if (IS_ERR(addr)) { 791 err = PTR_ERR(addr); 792 goto out; 793 } 794 clock = kzalloc(sizeof(*clock), GFP_KERNEL); 795 if (!clock) { 796 err = -ENOMEM; 797 goto out; 798 } 799 if (ines_clock_init(clock, &pld->dev, addr)) { 800 kfree(clock); 801 err = -ENOMEM; 802 goto out; 803 } 804 err = register_mii_tstamp_controller(&pld->dev, &ines_ctrl); 805 if (err) { 806 kfree(clock); 807 goto out; 808 } 809 mutex_lock(&ines_clocks_lock); 810 list_add_tail(&ines_clocks, &clock->list); 811 mutex_unlock(&ines_clocks_lock); 812 813 dev_set_drvdata(&pld->dev, clock); 814 out: 815 return err; 816 } 817 818 static int ines_ptp_ctrl_remove(struct platform_device *pld) 819 { 820 struct ines_clock *clock = dev_get_drvdata(&pld->dev); 821 822 unregister_mii_tstamp_controller(&pld->dev); 823 mutex_lock(&ines_clocks_lock); 824 list_del(&clock->list); 825 mutex_unlock(&ines_clocks_lock); 826 ines_clock_cleanup(clock); 827 kfree(clock); 828 return 0; 829 } 830 831 static const struct of_device_id ines_ptp_ctrl_of_match[] = { 832 { .compatible = "ines,ptp-ctrl" }, 833 { } 834 }; 835 836 MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match); 837 838 static struct platform_driver ines_ptp_ctrl_driver = { 839 .probe = ines_ptp_ctrl_probe, 840 .remove = ines_ptp_ctrl_remove, 841 .driver = { 842 .name = "ines_ptp_ctrl", 843 .of_match_table = of_match_ptr(ines_ptp_ctrl_of_match), 844 }, 845 }; 846 module_platform_driver(ines_ptp_ctrl_driver); 847