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 23MODULE_DESCRIPTION("Driver for the ZHAW InES PTP time stamping IP core"); 24MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>"); 25MODULE_VERSION("1.0"); 26MODULE_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 111static LIST_HEAD(ines_clocks); 112static DEFINE_MUTEX(ines_clocks_lock); 113 114struct 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 125struct 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 134struct 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 145struct 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 161struct 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 170static bool ines_match(struct sk_buff *skb, unsigned int ptp_class, 171 struct ines_timestamp *ts, struct device *dev); 172static int ines_rxfifo_read(struct ines_port *port); 173static u64 ines_rxts64(struct ines_port *port, unsigned int words); 174static bool ines_timestamp_expired(struct ines_timestamp *ts); 175static u64 ines_txts64(struct ines_port *port, unsigned int words); 176static void ines_txtstamp_work(struct work_struct *work); 177static bool is_sync_pdelay_resp(struct sk_buff *skb, int type); 178static u8 tag_to_msgtype(u8 tag); 179 180static 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 191static 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 236static 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 254static 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 285static 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 331static 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 397static 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 429static 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 473static 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 494static 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 536static 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 553static bool ines_timestamp_expired(struct ines_timestamp *ts) 554{ 555 return time_after(jiffies, ts->tmo); 556} 557 558static int ines_ts_info(struct mii_timestamper *mii_ts, 559 struct 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_RX_SOFTWARE | 566 SOF_TIMESTAMPING_SOFTWARE | 567 SOF_TIMESTAMPING_RAW_HARDWARE; 568 569 info->phc_index = -1; 570 571 info->tx_types = 572 (1 << HWTSTAMP_TX_OFF) | 573 (1 << HWTSTAMP_TX_ON) | 574 (1 << HWTSTAMP_TX_ONESTEP_P2P); 575 576 info->rx_filters = 577 (1 << HWTSTAMP_FILTER_NONE) | 578 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT); 579 580 return 0; 581} 582 583static u64 ines_txts64(struct ines_port *port, unsigned int words) 584{ 585 unsigned int i; 586 u64 result; 587 u16 word; 588 589 word = ines_read32(port, ts_tx); 590 result = word; 591 words--; 592 for (i = 0; i < words; i++) { 593 word = ines_read32(port, ts_tx); 594 result <<= 16; 595 result |= word; 596 } 597 return result; 598} 599 600static bool ines_txts_onestep(struct ines_port *port, struct sk_buff *skb, int type) 601{ 602 unsigned long flags; 603 u32 port_conf; 604 605 spin_lock_irqsave(&port->lock, flags); 606 port_conf = ines_read32(port, port_conf); 607 spin_unlock_irqrestore(&port->lock, flags); 608 609 if (port_conf & CM_ONE_STEP) 610 return is_sync_pdelay_resp(skb, type); 611 612 return false; 613} 614 615static void ines_txtstamp(struct mii_timestamper *mii_ts, 616 struct sk_buff *skb, int type) 617{ 618 struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts); 619 struct sk_buff *old_skb = NULL; 620 unsigned long flags; 621 622 if (!port->txts_enabled || ines_txts_onestep(port, skb, type)) { 623 kfree_skb(skb); 624 return; 625 } 626 627 spin_lock_irqsave(&port->lock, flags); 628 629 if (port->tx_skb) 630 old_skb = port->tx_skb; 631 632 port->tx_skb = skb; 633 634 spin_unlock_irqrestore(&port->lock, flags); 635 636 kfree_skb(old_skb); 637 638 schedule_delayed_work(&port->ts_work, 1); 639} 640 641static void ines_txtstamp_work(struct work_struct *work) 642{ 643 struct ines_port *port = 644 container_of(work, struct ines_port, ts_work.work); 645 struct skb_shared_hwtstamps ssh; 646 struct sk_buff *skb; 647 unsigned long flags; 648 u64 ns; 649 650 spin_lock_irqsave(&port->lock, flags); 651 skb = port->tx_skb; 652 port->tx_skb = NULL; 653 spin_unlock_irqrestore(&port->lock, flags); 654 655 ns = ines_find_txts(port, skb); 656 if (!ns) { 657 kfree_skb(skb); 658 return; 659 } 660 ssh.hwtstamp = ns_to_ktime(ns); 661 skb_complete_tx_timestamp(skb, &ssh); 662} 663 664static bool is_sync_pdelay_resp(struct sk_buff *skb, int type) 665{ 666 struct ptp_header *hdr; 667 u8 msgtype; 668 669 hdr = ptp_parse_header(skb, type); 670 if (!hdr) 671 return false; 672 673 msgtype = ptp_get_msgtype(hdr, type); 674 675 switch (msgtype) { 676 case PTP_MSGTYPE_SYNC: 677 case PTP_MSGTYPE_PDELAY_RESP: 678 return true; 679 default: 680 return false; 681 } 682} 683 684static u8 tag_to_msgtype(u8 tag) 685{ 686 switch (tag) { 687 case MESSAGE_TYPE_SYNC: 688 return PTP_MSGTYPE_SYNC; 689 case MESSAGE_TYPE_P_DELAY_REQ: 690 return PTP_MSGTYPE_PDELAY_REQ; 691 case MESSAGE_TYPE_P_DELAY_RESP: 692 return PTP_MSGTYPE_PDELAY_RESP; 693 case MESSAGE_TYPE_DELAY_REQ: 694 return PTP_MSGTYPE_DELAY_REQ; 695 } 696 return 0xf; 697} 698 699static struct mii_timestamper *ines_ptp_probe_channel(struct device *device, 700 unsigned int index) 701{ 702 struct device_node *node = device->of_node; 703 struct ines_port *port; 704 705 if (index > INES_N_PORTS - 1) { 706 dev_err(device, "bad port index %u\n", index); 707 return ERR_PTR(-EINVAL); 708 } 709 port = ines_find_port(node, index); 710 if (!port) { 711 dev_err(device, "missing port index %u\n", index); 712 return ERR_PTR(-ENODEV); 713 } 714 port->mii_ts.rxtstamp = ines_rxtstamp; 715 port->mii_ts.txtstamp = ines_txtstamp; 716 port->mii_ts.hwtstamp = ines_hwtstamp; 717 port->mii_ts.link_state = ines_link_state; 718 port->mii_ts.ts_info = ines_ts_info; 719 720 return &port->mii_ts; 721} 722 723static void ines_ptp_release_channel(struct device *device, 724 struct mii_timestamper *mii_ts) 725{ 726} 727 728static struct mii_timestamping_ctrl ines_ctrl = { 729 .probe_channel = ines_ptp_probe_channel, 730 .release_channel = ines_ptp_release_channel, 731}; 732 733static int ines_ptp_ctrl_probe(struct platform_device *pld) 734{ 735 struct ines_clock *clock; 736 void __iomem *addr; 737 int err = 0; 738 739 addr = devm_platform_ioremap_resource(pld, 0); 740 if (IS_ERR(addr)) { 741 err = PTR_ERR(addr); 742 goto out; 743 } 744 clock = kzalloc(sizeof(*clock), GFP_KERNEL); 745 if (!clock) { 746 err = -ENOMEM; 747 goto out; 748 } 749 if (ines_clock_init(clock, &pld->dev, addr)) { 750 kfree(clock); 751 err = -ENOMEM; 752 goto out; 753 } 754 err = register_mii_tstamp_controller(&pld->dev, &ines_ctrl); 755 if (err) { 756 kfree(clock); 757 goto out; 758 } 759 mutex_lock(&ines_clocks_lock); 760 list_add_tail(&ines_clocks, &clock->list); 761 mutex_unlock(&ines_clocks_lock); 762 763 dev_set_drvdata(&pld->dev, clock); 764out: 765 return err; 766} 767 768static int ines_ptp_ctrl_remove(struct platform_device *pld) 769{ 770 struct ines_clock *clock = dev_get_drvdata(&pld->dev); 771 772 unregister_mii_tstamp_controller(&pld->dev); 773 mutex_lock(&ines_clocks_lock); 774 list_del(&clock->list); 775 mutex_unlock(&ines_clocks_lock); 776 ines_clock_cleanup(clock); 777 kfree(clock); 778 return 0; 779} 780 781static const struct of_device_id ines_ptp_ctrl_of_match[] = { 782 { .compatible = "ines,ptp-ctrl" }, 783 { } 784}; 785 786MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match); 787 788static struct platform_driver ines_ptp_ctrl_driver = { 789 .probe = ines_ptp_ctrl_probe, 790 .remove = ines_ptp_ctrl_remove, 791 .driver = { 792 .name = "ines_ptp_ctrl", 793 .of_match_table = ines_ptp_ctrl_of_match, 794 }, 795}; 796module_platform_driver(ines_ptp_ctrl_driver); 797