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 switch (cfg.tx_type) { 342 case HWTSTAMP_TX_OFF: 343 ts_stat_tx = 0; 344 break; 345 case HWTSTAMP_TX_ON: 346 ts_stat_tx = TS_ENABLE; 347 break; 348 case HWTSTAMP_TX_ONESTEP_P2P: 349 ts_stat_tx = TS_ENABLE; 350 cm_one_step = CM_ONE_STEP; 351 break; 352 default: 353 return -ERANGE; 354 } 355 356 switch (cfg.rx_filter) { 357 case HWTSTAMP_FILTER_NONE: 358 ts_stat_rx = 0; 359 break; 360 case HWTSTAMP_FILTER_ALL: 361 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT: 362 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC: 363 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ: 364 return -ERANGE; 365 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: 366 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: 367 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: 368 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: 369 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: 370 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: 371 case HWTSTAMP_FILTER_PTP_V2_EVENT: 372 case HWTSTAMP_FILTER_PTP_V2_SYNC: 373 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: 374 ts_stat_rx = TS_ENABLE; 375 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; 376 break; 377 default: 378 return -ERANGE; 379 } 380 381 spin_lock_irqsave(&port->lock, flags); 382 383 port_conf = ines_read32(port, port_conf); 384 port_conf &= ~CM_ONE_STEP; 385 port_conf |= cm_one_step; 386 387 ines_write32(port, port_conf, port_conf); 388 ines_write32(port, ts_stat_rx, ts_stat_rx); 389 ines_write32(port, ts_stat_tx, ts_stat_tx); 390 391 port->rxts_enabled = ts_stat_rx == TS_ENABLE; 392 port->txts_enabled = ts_stat_tx == TS_ENABLE; 393 394 spin_unlock_irqrestore(&port->lock, flags); 395 396 return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0; 397 } 398 399 static void ines_link_state(struct mii_timestamper *mii_ts, 400 struct phy_device *phydev) 401 { 402 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts); 403 u32 port_conf, speed_conf; 404 unsigned long flags; 405 406 switch (phydev->speed) { 407 case SPEED_10: 408 speed_conf = PHY_SPEED_10 << PHY_SPEED_SHIFT; 409 break; 410 case SPEED_100: 411 speed_conf = PHY_SPEED_100 << PHY_SPEED_SHIFT; 412 break; 413 case SPEED_1000: 414 speed_conf = PHY_SPEED_1000 << PHY_SPEED_SHIFT; 415 break; 416 default: 417 dev_err(port->clock->dev, "bad speed: %d\n", phydev->speed); 418 return; 419 } 420 spin_lock_irqsave(&port->lock, flags); 421 422 port_conf = ines_read32(port, port_conf); 423 port_conf &= ~(0x3 << PHY_SPEED_SHIFT); 424 port_conf |= speed_conf; 425 426 ines_write32(port, port_conf, port_conf); 427 428 spin_unlock_irqrestore(&port->lock, flags); 429 } 430 431 static bool ines_match(struct sk_buff *skb, unsigned int ptp_class, 432 struct ines_timestamp *ts, struct device *dev) 433 { 434 struct ptp_header *hdr; 435 u16 portn, seqid; 436 u8 msgtype; 437 u64 clkid; 438 439 if (unlikely(ptp_class & PTP_CLASS_V1)) 440 return false; 441 442 hdr = ptp_parse_header(skb, ptp_class); 443 if (!hdr) 444 return false; 445 446 msgtype = ptp_get_msgtype(hdr, ptp_class); 447 clkid = be64_to_cpup((__be64 *)&hdr->source_port_identity.clock_identity.id[0]); 448 portn = be16_to_cpu(hdr->source_port_identity.port_number); 449 seqid = be16_to_cpu(hdr->sequence_id); 450 451 if (tag_to_msgtype(ts->tag & 0x7) != msgtype) { 452 dev_dbg(dev, "msgtype mismatch ts %hhu != skb %hhu\n", 453 tag_to_msgtype(ts->tag & 0x7), msgtype); 454 return false; 455 } 456 if (ts->clkid != clkid) { 457 dev_dbg(dev, "clkid mismatch ts %llx != skb %llx\n", 458 ts->clkid, clkid); 459 return false; 460 } 461 if (ts->portnum != portn) { 462 dev_dbg(dev, "portn mismatch ts %hu != skb %hu\n", 463 ts->portnum, portn); 464 return false; 465 } 466 if (ts->seqid != seqid) { 467 dev_dbg(dev, "seqid mismatch ts %hu != skb %hu\n", 468 ts->seqid, seqid); 469 return false; 470 } 471 472 return true; 473 } 474 475 static bool ines_rxtstamp(struct mii_timestamper *mii_ts, 476 struct sk_buff *skb, int type) 477 { 478 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts); 479 struct skb_shared_hwtstamps *ssh; 480 u64 ns; 481 482 if (!port->rxts_enabled) 483 return false; 484 485 ns = ines_find_rxts(port, skb, type); 486 if (!ns) 487 return false; 488 489 ssh = skb_hwtstamps(skb); 490 ssh->hwtstamp = ns_to_ktime(ns); 491 netif_rx(skb); 492 493 return true; 494 } 495 496 static int ines_rxfifo_read(struct ines_port *port) 497 { 498 u32 data_rd_pos, buf_stat, mask, ts_stat_rx; 499 struct ines_timestamp *ts; 500 unsigned int i; 501 502 mask = RX_FIFO_NE_1 << port->index; 503 504 for (i = 0; i < INES_FIFO_DEPTH; i++) { 505 if (list_empty(&port->pool)) { 506 dev_err(port->clock->dev, "event pool is empty\n"); 507 return -1; 508 } 509 buf_stat = ines_read32(port->clock, buf_stat); 510 if (!(buf_stat & mask)) 511 break; 512 513 ts_stat_rx = ines_read32(port, ts_stat_rx); 514 data_rd_pos = (ts_stat_rx >> DATA_READ_POS_SHIFT) & 515 DATA_READ_POS_MASK; 516 if (data_rd_pos) { 517 dev_err(port->clock->dev, "unexpected Rx read pos %u\n", 518 data_rd_pos); 519 break; 520 } 521 522 ts = list_first_entry(&port->pool, struct ines_timestamp, list); 523 ts->tmo = jiffies + HZ; 524 ts->tag = ines_read32(port, ts_rx); 525 ts->sec = ines_rxts64(port, 3); 526 ts->nsec = ines_rxts64(port, 2); 527 ts->clkid = ines_rxts64(port, 4); 528 ts->portnum = ines_read32(port, ts_rx); 529 ts->seqid = ines_read32(port, ts_rx); 530 531 list_del_init(&ts->list); 532 list_add_tail(&ts->list, &port->events); 533 } 534 535 return 0; 536 } 537 538 static u64 ines_rxts64(struct ines_port *port, unsigned int words) 539 { 540 unsigned int i; 541 u64 result; 542 u16 word; 543 544 word = ines_read32(port, ts_rx); 545 result = word; 546 words--; 547 for (i = 0; i < words; i++) { 548 word = ines_read32(port, ts_rx); 549 result <<= 16; 550 result |= word; 551 } 552 return result; 553 } 554 555 static bool ines_timestamp_expired(struct ines_timestamp *ts) 556 { 557 return time_after(jiffies, ts->tmo); 558 } 559 560 static int ines_ts_info(struct mii_timestamper *mii_ts, 561 struct ethtool_ts_info *info) 562 { 563 info->so_timestamping = 564 SOF_TIMESTAMPING_TX_HARDWARE | 565 SOF_TIMESTAMPING_TX_SOFTWARE | 566 SOF_TIMESTAMPING_RX_HARDWARE | 567 SOF_TIMESTAMPING_RX_SOFTWARE | 568 SOF_TIMESTAMPING_SOFTWARE | 569 SOF_TIMESTAMPING_RAW_HARDWARE; 570 571 info->phc_index = -1; 572 573 info->tx_types = 574 (1 << HWTSTAMP_TX_OFF) | 575 (1 << HWTSTAMP_TX_ON) | 576 (1 << HWTSTAMP_TX_ONESTEP_P2P); 577 578 info->rx_filters = 579 (1 << HWTSTAMP_FILTER_NONE) | 580 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT); 581 582 return 0; 583 } 584 585 static u64 ines_txts64(struct ines_port *port, unsigned int words) 586 { 587 unsigned int i; 588 u64 result; 589 u16 word; 590 591 word = ines_read32(port, ts_tx); 592 result = word; 593 words--; 594 for (i = 0; i < words; i++) { 595 word = ines_read32(port, ts_tx); 596 result <<= 16; 597 result |= word; 598 } 599 return result; 600 } 601 602 static bool ines_txts_onestep(struct ines_port *port, struct sk_buff *skb, int type) 603 { 604 unsigned long flags; 605 u32 port_conf; 606 607 spin_lock_irqsave(&port->lock, flags); 608 port_conf = ines_read32(port, port_conf); 609 spin_unlock_irqrestore(&port->lock, flags); 610 611 if (port_conf & CM_ONE_STEP) 612 return is_sync_pdelay_resp(skb, type); 613 614 return false; 615 } 616 617 static void ines_txtstamp(struct mii_timestamper *mii_ts, 618 struct sk_buff *skb, int type) 619 { 620 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts); 621 struct sk_buff *old_skb = NULL; 622 unsigned long flags; 623 624 if (!port->txts_enabled || ines_txts_onestep(port, skb, type)) { 625 kfree_skb(skb); 626 return; 627 } 628 629 spin_lock_irqsave(&port->lock, flags); 630 631 if (port->tx_skb) 632 old_skb = port->tx_skb; 633 634 port->tx_skb = skb; 635 636 spin_unlock_irqrestore(&port->lock, flags); 637 638 kfree_skb(old_skb); 639 640 schedule_delayed_work(&port->ts_work, 1); 641 } 642 643 static void ines_txtstamp_work(struct work_struct *work) 644 { 645 struct ines_port *port = 646 container_of(work, struct ines_port, ts_work.work); 647 struct skb_shared_hwtstamps ssh; 648 struct sk_buff *skb; 649 unsigned long flags; 650 u64 ns; 651 652 spin_lock_irqsave(&port->lock, flags); 653 skb = port->tx_skb; 654 port->tx_skb = NULL; 655 spin_unlock_irqrestore(&port->lock, flags); 656 657 ns = ines_find_txts(port, skb); 658 if (!ns) { 659 kfree_skb(skb); 660 return; 661 } 662 ssh.hwtstamp = ns_to_ktime(ns); 663 skb_complete_tx_timestamp(skb, &ssh); 664 } 665 666 static bool is_sync_pdelay_resp(struct sk_buff *skb, int type) 667 { 668 struct ptp_header *hdr; 669 u8 msgtype; 670 671 hdr = ptp_parse_header(skb, type); 672 if (!hdr) 673 return false; 674 675 msgtype = ptp_get_msgtype(hdr, type); 676 677 switch (msgtype) { 678 case PTP_MSGTYPE_SYNC: 679 case PTP_MSGTYPE_PDELAY_RESP: 680 return true; 681 default: 682 return false; 683 } 684 } 685 686 static u8 tag_to_msgtype(u8 tag) 687 { 688 switch (tag) { 689 case MESSAGE_TYPE_SYNC: 690 return PTP_MSGTYPE_SYNC; 691 case MESSAGE_TYPE_P_DELAY_REQ: 692 return PTP_MSGTYPE_PDELAY_REQ; 693 case MESSAGE_TYPE_P_DELAY_RESP: 694 return PTP_MSGTYPE_PDELAY_RESP; 695 case MESSAGE_TYPE_DELAY_REQ: 696 return PTP_MSGTYPE_DELAY_REQ; 697 } 698 return 0xf; 699 } 700 701 static struct mii_timestamper *ines_ptp_probe_channel(struct device *device, 702 unsigned int index) 703 { 704 struct device_node *node = device->of_node; 705 struct ines_port *port; 706 707 if (index > INES_N_PORTS - 1) { 708 dev_err(device, "bad port index %u\n", index); 709 return ERR_PTR(-EINVAL); 710 } 711 port = ines_find_port(node, index); 712 if (!port) { 713 dev_err(device, "missing port index %u\n", index); 714 return ERR_PTR(-ENODEV); 715 } 716 port->mii_ts.rxtstamp = ines_rxtstamp; 717 port->mii_ts.txtstamp = ines_txtstamp; 718 port->mii_ts.hwtstamp = ines_hwtstamp; 719 port->mii_ts.link_state = ines_link_state; 720 port->mii_ts.ts_info = ines_ts_info; 721 722 return &port->mii_ts; 723 } 724 725 static void ines_ptp_release_channel(struct device *device, 726 struct mii_timestamper *mii_ts) 727 { 728 } 729 730 static struct mii_timestamping_ctrl ines_ctrl = { 731 .probe_channel = ines_ptp_probe_channel, 732 .release_channel = ines_ptp_release_channel, 733 }; 734 735 static int ines_ptp_ctrl_probe(struct platform_device *pld) 736 { 737 struct ines_clock *clock; 738 void __iomem *addr; 739 int err = 0; 740 741 addr = devm_platform_ioremap_resource(pld, 0); 742 if (IS_ERR(addr)) { 743 err = PTR_ERR(addr); 744 goto out; 745 } 746 clock = kzalloc(sizeof(*clock), GFP_KERNEL); 747 if (!clock) { 748 err = -ENOMEM; 749 goto out; 750 } 751 if (ines_clock_init(clock, &pld->dev, addr)) { 752 kfree(clock); 753 err = -ENOMEM; 754 goto out; 755 } 756 err = register_mii_tstamp_controller(&pld->dev, &ines_ctrl); 757 if (err) { 758 kfree(clock); 759 goto out; 760 } 761 mutex_lock(&ines_clocks_lock); 762 list_add_tail(&ines_clocks, &clock->list); 763 mutex_unlock(&ines_clocks_lock); 764 765 dev_set_drvdata(&pld->dev, clock); 766 out: 767 return err; 768 } 769 770 static int ines_ptp_ctrl_remove(struct platform_device *pld) 771 { 772 struct ines_clock *clock = dev_get_drvdata(&pld->dev); 773 774 unregister_mii_tstamp_controller(&pld->dev); 775 mutex_lock(&ines_clocks_lock); 776 list_del(&clock->list); 777 mutex_unlock(&ines_clocks_lock); 778 ines_clock_cleanup(clock); 779 kfree(clock); 780 return 0; 781 } 782 783 static const struct of_device_id ines_ptp_ctrl_of_match[] = { 784 { .compatible = "ines,ptp-ctrl" }, 785 { } 786 }; 787 788 MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match); 789 790 static struct platform_driver ines_ptp_ctrl_driver = { 791 .probe = ines_ptp_ctrl_probe, 792 .remove = ines_ptp_ctrl_remove, 793 .driver = { 794 .name = "ines_ptp_ctrl", 795 .of_match_table = of_match_ptr(ines_ptp_ctrl_of_match), 796 }, 797 }; 798 module_platform_driver(ines_ptp_ctrl_driver); 799