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 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 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
 583 static 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
 600 static 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
 615 static 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
 641 static 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
 664 static 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
 684 static 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
 699 static 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
 723 static void ines_ptp_release_channel(struct device *device,
 724                                      struct mii_timestamper *mii_ts)
 725 {
 726 }
 727
 728 static struct mii_timestamping_ctrl ines_ctrl = {
 729         .probe_channel = ines_ptp_probe_channel,
 730         .release_channel = ines_ptp_release_channel,
 731 };
 732
 733 static 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);
 764 out:
 765         return err;
 766 }
 767
 768 static 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
 781 static const struct of_device_id ines_ptp_ctrl_of_match[] = {
 782         { .compatible = "ines,ptp-ctrl" },
 783         { }
 784 };
 785
 786 MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match);
 787
 788 static 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 };
 796 module_platform_driver(ines_ptp_ctrl_driver);