drivers/ptp/ptp_ines.c

   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);