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