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