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