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 OFF_PTP_CLOCK_ID        20
  97 #define OFF_PTP_PORT_NUM        28
  98
  99 #define PHY_SPEED_10            0
 100 #define PHY_SPEED_100           1
 101 #define PHY_SPEED_1000          2
 102
 103 #define PORT_CONF \
 104         ((PHY_SPEED_1000 << PHY_SPEED_SHIFT) | (BC_PTP_V2 << PTP_MODE_SHIFT))
 105
 106 #define ines_read32(s, r)       __raw_readl((void __iomem *)&s->regs->r)
 107 #define ines_write32(s, v, r)   __raw_writel(v, (void __iomem *)&s->regs->r)
 108
 109 #define MESSAGE_TYPE_SYNC               1
 110 #define MESSAGE_TYPE_P_DELAY_REQ        2
 111 #define MESSAGE_TYPE_P_DELAY_RESP       3
 112 #define MESSAGE_TYPE_DELAY_REQ          4
 113
 114 #define SYNC                            0x0
 115 #define DELAY_REQ                       0x1
 116 #define PDELAY_REQ                      0x2
 117 #define PDELAY_RESP                     0x3
 118
 119 static LIST_HEAD(ines_clocks);
 120 static DEFINE_MUTEX(ines_clocks_lock);
 121
 122 struct ines_global_regs {
 123         u32 id;
 124         u32 test;
 125         u32 global;
 126         u32 version;
 127         u32 test2;
 128         u32 int_stat;
 129         u32 int_msk;
 130         u32 buf_stat;
 131 };
 132
 133 struct ines_port_registers {
 134         u32 port_conf;
 135         u32 p_delay;
 136         u32 ts_stat_tx;
 137         u32 ts_stat_rx;
 138         u32 ts_tx;
 139         u32 ts_rx;
 140 };
 141
 142 struct ines_timestamp {
 143         struct list_head list;
 144         unsigned long   tmo;
 145         u16             tag;
 146         u64             sec;
 147         u64             nsec;
 148         u64             clkid;
 149         u16             portnum;
 150         u16             seqid;
 151 };
 152
 153 struct ines_port {
 154         struct ines_port_registers      *regs;
 155         struct mii_timestamper          mii_ts;
 156         struct ines_clock               *clock;
 157         bool                            rxts_enabled;
 158         bool                            txts_enabled;
 159         unsigned int                    index;
 160         struct delayed_work             ts_work;
 161         /* lock protects event list and tx_skb */
 162         spinlock_t                      lock;
 163         struct sk_buff                  *tx_skb;
 164         struct list_head                events;
 165         struct list_head                pool;
 166         struct ines_timestamp           pool_data[INES_MAX_EVENTS];
 167 };
 168
 169 struct ines_clock {
 170         struct ines_port                port[INES_N_PORTS];
 171         struct ines_global_regs __iomem *regs;
 172         void __iomem                    *base;
 173         struct device_node              *node;
 174         struct device                   *dev;
 175         struct list_head                list;
 176 };
 177
 178 static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
 179                        struct ines_timestamp *ts, struct device *dev);
 180 static int ines_rxfifo_read(struct ines_port *port);
 181 static u64 ines_rxts64(struct ines_port *port, unsigned int words);
 182 static bool ines_timestamp_expired(struct ines_timestamp *ts);
 183 static u64 ines_txts64(struct ines_port *port, unsigned int words);
 184 static void ines_txtstamp_work(struct work_struct *work);
 185 static bool is_sync_pdelay_resp(struct sk_buff *skb, int type);
 186 static u8 tag_to_msgtype(u8 tag);
 187
 188 static void ines_clock_cleanup(struct ines_clock *clock)
 189 {
 190         struct ines_port *port;
 191         int i;
 192
 193         for (i = 0; i < INES_N_PORTS; i++) {
 194                 port = &clock->port[i];
 195                 cancel_delayed_work_sync(&port->ts_work);
 196         }
 197 }
 198
 199 static int ines_clock_init(struct ines_clock *clock, struct device *device,
 200                            void __iomem *addr)
 201 {
 202         struct device_node *node = device->of_node;
 203         unsigned long port_addr;
 204         struct ines_port *port;
 205         int i, j;
 206
 207         INIT_LIST_HEAD(&clock->list);
 208         clock->node = node;
 209         clock->dev  = device;
 210         clock->base = addr;
 211         clock->regs = clock->base;
 212
 213         for (i = 0; i < INES_N_PORTS; i++) {
 214                 port = &clock->port[i];
 215                 port_addr = (unsigned long) clock->base +
 216                         INES_PORT_OFFSET + i * INES_PORT_SIZE;
 217                 port->regs = (struct ines_port_registers *) port_addr;
 218                 port->clock = clock;
 219                 port->index = i;
 220                 INIT_DELAYED_WORK(&port->ts_work, ines_txtstamp_work);
 221                 spin_lock_init(&port->lock);
 222                 INIT_LIST_HEAD(&port->events);
 223                 INIT_LIST_HEAD(&port->pool);
 224                 for (j = 0; j < INES_MAX_EVENTS; j++)
 225                         list_add(&port->pool_data[j].list, &port->pool);
 226         }
 227
 228         ines_write32(clock, 0xBEEF, test);
 229         ines_write32(clock, 0xBEEF, test2);
 230
 231         dev_dbg(device, "ID      0x%x\n", ines_read32(clock, id));
 232         dev_dbg(device, "TEST    0x%x\n", ines_read32(clock, test));
 233         dev_dbg(device, "VERSION 0x%x\n", ines_read32(clock, version));
 234         dev_dbg(device, "TEST2   0x%x\n", ines_read32(clock, test2));
 235
 236         for (i = 0; i < INES_N_PORTS; i++) {
 237                 port = &clock->port[i];
 238                 ines_write32(port, PORT_CONF, port_conf);
 239         }
 240
 241         return 0;
 242 }
 243
 244 static struct ines_port *ines_find_port(struct device_node *node, u32 index)
 245 {
 246         struct ines_port *port = NULL;
 247         struct ines_clock *clock;
 248         struct list_head *this;
 249
 250         mutex_lock(&ines_clocks_lock);
 251         list_for_each(this, &ines_clocks) {
 252                 clock = list_entry(this, struct ines_clock, list);
 253                 if (clock->node == node) {
 254                         port = &clock->port[index];
 255                         break;
 256                 }
 257         }
 258         mutex_unlock(&ines_clocks_lock);
 259         return port;
 260 }
 261
 262 static u64 ines_find_rxts(struct ines_port *port, struct sk_buff *skb, int type)
 263 {
 264         struct list_head *this, *next;
 265         struct ines_timestamp *ts;
 266         unsigned long flags;
 267         u64 ns = 0;
 268
 269         if (type == PTP_CLASS_NONE)
 270                 return 0;
 271
 272         spin_lock_irqsave(&port->lock, flags);
 273         ines_rxfifo_read(port);
 274         list_for_each_safe(this, next, &port->events) {
 275                 ts = list_entry(this, struct ines_timestamp, list);
 276                 if (ines_timestamp_expired(ts)) {
 277                         list_del_init(&ts->list);
 278                         list_add(&ts->list, &port->pool);
 279                         continue;
 280                 }
 281                 if (ines_match(skb, type, ts, port->clock->dev)) {
 282                         ns = ts->sec * 1000000000ULL + ts->nsec;
 283                         list_del_init(&ts->list);
 284                         list_add(&ts->list, &port->pool);
 285                         break;
 286                 }
 287         }
 288         spin_unlock_irqrestore(&port->lock, flags);
 289
 290         return ns;
 291 }
 292
 293 static u64 ines_find_txts(struct ines_port *port, struct sk_buff *skb)
 294 {
 295         unsigned int class = ptp_classify_raw(skb), i;
 296         u32 data_rd_pos, buf_stat, mask, ts_stat_tx;
 297         struct ines_timestamp ts;
 298         unsigned long flags;
 299         u64 ns = 0;
 300
 301         mask = TX_FIFO_NE_1 << port->index;
 302
 303         spin_lock_irqsave(&port->lock, flags);
 304
 305         for (i = 0; i < INES_FIFO_DEPTH; i++) {
 306
 307                 buf_stat = ines_read32(port->clock, buf_stat);
 308                 if (!(buf_stat & mask)) {
 309                         dev_dbg(port->clock->dev,
 310                                   "Tx timestamp FIFO unexpectedly empty\n");
 311                         break;
 312                 }
 313                 ts_stat_tx = ines_read32(port, ts_stat_tx);
 314                 data_rd_pos = (ts_stat_tx >> DATA_READ_POS_SHIFT) &
 315                         DATA_READ_POS_MASK;
 316                 if (data_rd_pos) {
 317                         dev_err(port->clock->dev,
 318                                 "unexpected Tx read pos %u\n", data_rd_pos);
 319                         break;
 320                 }
 321
 322                 ts.tag     = ines_read32(port, ts_tx);
 323                 ts.sec     = ines_txts64(port, 3);
 324                 ts.nsec    = ines_txts64(port, 2);
 325                 ts.clkid   = ines_txts64(port, 4);
 326                 ts.portnum = ines_read32(port, ts_tx);
 327                 ts.seqid   = ines_read32(port, ts_tx);
 328
 329                 if (ines_match(skb, class, &ts, port->clock->dev)) {
 330                         ns = ts.sec * 1000000000ULL + ts.nsec;
 331                         break;
 332                 }
 333         }
 334
 335         spin_unlock_irqrestore(&port->lock, flags);
 336         return ns;
 337 }
 338
 339 static int ines_hwtstamp(struct mii_timestamper *mii_ts, struct ifreq *ifr)
 340 {
 341         struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
 342         u32 cm_one_step = 0, port_conf, ts_stat_rx, ts_stat_tx;
 343         struct hwtstamp_config cfg;
 344         unsigned long flags;
 345
 346         if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
 347                 return -EFAULT;
 348
 349         /* reserved for future extensions */
 350         if (cfg.flags)
 351                 return -EINVAL;
 352
 353         switch (cfg.tx_type) {
 354         case HWTSTAMP_TX_OFF:
 355                 ts_stat_tx = 0;
 356                 break;
 357         case HWTSTAMP_TX_ON:
 358                 ts_stat_tx = TS_ENABLE;
 359                 break;
 360         case HWTSTAMP_TX_ONESTEP_P2P:
 361                 ts_stat_tx = TS_ENABLE;
 362                 cm_one_step = CM_ONE_STEP;
 363                 break;
 364         default:
 365                 return -ERANGE;
 366         }
 367
 368         switch (cfg.rx_filter) {
 369         case HWTSTAMP_FILTER_NONE:
 370                 ts_stat_rx = 0;
 371                 break;
 372         case HWTSTAMP_FILTER_ALL:
 373         case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
 374         case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
 375         case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
 376                 return -ERANGE;
 377         case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
 378         case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
 379         case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
 380         case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
 381         case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
 382         case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
 383         case HWTSTAMP_FILTER_PTP_V2_EVENT:
 384         case HWTSTAMP_FILTER_PTP_V2_SYNC:
 385         case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
 386                 ts_stat_rx = TS_ENABLE;
 387                 cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
 388                 break;
 389         default:
 390                 return -ERANGE;
 391         }
 392
 393         spin_lock_irqsave(&port->lock, flags);
 394
 395         port_conf = ines_read32(port, port_conf);
 396         port_conf &= ~CM_ONE_STEP;
 397         port_conf |= cm_one_step;
 398
 399         ines_write32(port, port_conf, port_conf);
 400         ines_write32(port, ts_stat_rx, ts_stat_rx);
 401         ines_write32(port, ts_stat_tx, ts_stat_tx);
 402
 403         port->rxts_enabled = ts_stat_rx == TS_ENABLE ? true : false;
 404         port->txts_enabled = ts_stat_tx == TS_ENABLE ? true : false;
 405
 406         spin_unlock_irqrestore(&port->lock, flags);
 407
 408         return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
 409 }
 410
 411 static void ines_link_state(struct mii_timestamper *mii_ts,
 412                             struct phy_device *phydev)
 413 {
 414         struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
 415         u32 port_conf, speed_conf;
 416         unsigned long flags;
 417
 418         switch (phydev->speed) {
 419         case SPEED_10:
 420                 speed_conf = PHY_SPEED_10 << PHY_SPEED_SHIFT;
 421                 break;
 422         case SPEED_100:
 423                 speed_conf = PHY_SPEED_100 << PHY_SPEED_SHIFT;
 424                 break;
 425         case SPEED_1000:
 426                 speed_conf = PHY_SPEED_1000 << PHY_SPEED_SHIFT;
 427                 break;
 428         default:
 429                 dev_err(port->clock->dev, "bad speed: %d\n", phydev->speed);
 430                 return;
 431         }
 432         spin_lock_irqsave(&port->lock, flags);
 433
 434         port_conf = ines_read32(port, port_conf);
 435         port_conf &= ~(0x3 << PHY_SPEED_SHIFT);
 436         port_conf |= speed_conf;
 437
 438         ines_write32(port, port_conf, port_conf);
 439
 440         spin_unlock_irqrestore(&port->lock, flags);
 441 }
 442
 443 static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
 444                        struct ines_timestamp *ts, struct device *dev)
 445 {
 446         u8 *msgtype, *data = skb_mac_header(skb);
 447         unsigned int offset = 0;
 448         __be16 *portn, *seqid;
 449         __be64 *clkid;
 450
 451         if (unlikely(ptp_class & PTP_CLASS_V1))
 452                 return false;
 453
 454         if (ptp_class & PTP_CLASS_VLAN)
 455                 offset += VLAN_HLEN;
 456
 457         switch (ptp_class & PTP_CLASS_PMASK) {
 458         case PTP_CLASS_IPV4:
 459                 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
 460                 break;
 461         case PTP_CLASS_IPV6:
 462                 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
 463                 break;
 464         case PTP_CLASS_L2:
 465                 offset += ETH_HLEN;
 466                 break;
 467         default:
 468                 return false;
 469         }
 470
 471         if (skb->len + ETH_HLEN < offset + OFF_PTP_SEQUENCE_ID + sizeof(*seqid))
 472                 return false;
 473
 474         msgtype = data + offset;
 475         clkid = (__be64 *)(data + offset + OFF_PTP_CLOCK_ID);
 476         portn = (__be16 *)(data + offset + OFF_PTP_PORT_NUM);
 477         seqid = (__be16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
 478
 479         if (tag_to_msgtype(ts->tag & 0x7) != (*msgtype & 0xf)) {
 480                 dev_dbg(dev, "msgtype mismatch ts %hhu != skb %hhu\n",
 481                           tag_to_msgtype(ts->tag & 0x7), *msgtype & 0xf);
 482                 return false;
 483         }
 484         if (cpu_to_be64(ts->clkid) != *clkid) {
 485                 dev_dbg(dev, "clkid mismatch ts %llx != skb %llx\n",
 486                           cpu_to_be64(ts->clkid), *clkid);
 487                 return false;
 488         }
 489         if (ts->portnum != ntohs(*portn)) {
 490                 dev_dbg(dev, "portn mismatch ts %hu != skb %hu\n",
 491                           ts->portnum, ntohs(*portn));
 492                 return false;
 493         }
 494         if (ts->seqid != ntohs(*seqid)) {
 495                 dev_dbg(dev, "seqid mismatch ts %hu != skb %hu\n",
 496                           ts->seqid, ntohs(*seqid));
 497                 return false;
 498         }
 499
 500         return true;
 501 }
 502
 503 static bool ines_rxtstamp(struct mii_timestamper *mii_ts,
 504                           struct sk_buff *skb, int type)
 505 {
 506         struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
 507         struct skb_shared_hwtstamps *ssh;
 508         u64 ns;
 509
 510         if (!port->rxts_enabled)
 511                 return false;
 512
 513         ns = ines_find_rxts(port, skb, type);
 514         if (!ns)
 515                 return false;
 516
 517         ssh = skb_hwtstamps(skb);
 518         ssh->hwtstamp = ns_to_ktime(ns);
 519         netif_rx(skb);
 520
 521         return true;
 522 }
 523
 524 static int ines_rxfifo_read(struct ines_port *port)
 525 {
 526         u32 data_rd_pos, buf_stat, mask, ts_stat_rx;
 527         struct ines_timestamp *ts;
 528         unsigned int i;
 529
 530         mask = RX_FIFO_NE_1 << port->index;
 531
 532         for (i = 0; i < INES_FIFO_DEPTH; i++) {
 533                 if (list_empty(&port->pool)) {
 534                         dev_err(port->clock->dev, "event pool is empty\n");
 535                         return -1;
 536                 }
 537                 buf_stat = ines_read32(port->clock, buf_stat);
 538                 if (!(buf_stat & mask))
 539                         break;
 540
 541                 ts_stat_rx = ines_read32(port, ts_stat_rx);
 542                 data_rd_pos = (ts_stat_rx >> DATA_READ_POS_SHIFT) &
 543                         DATA_READ_POS_MASK;
 544                 if (data_rd_pos) {
 545                         dev_err(port->clock->dev, "unexpected Rx read pos %u\n",
 546                                 data_rd_pos);
 547                         break;
 548                 }
 549
 550                 ts = list_first_entry(&port->pool, struct ines_timestamp, list);
 551                 ts->tmo     = jiffies + HZ;
 552                 ts->tag     = ines_read32(port, ts_rx);
 553                 ts->sec     = ines_rxts64(port, 3);
 554                 ts->nsec    = ines_rxts64(port, 2);
 555                 ts->clkid   = ines_rxts64(port, 4);
 556                 ts->portnum = ines_read32(port, ts_rx);
 557                 ts->seqid   = ines_read32(port, ts_rx);
 558
 559                 list_del_init(&ts->list);
 560                 list_add_tail(&ts->list, &port->events);
 561         }
 562
 563         return 0;
 564 }
 565
 566 static u64 ines_rxts64(struct ines_port *port, unsigned int words)
 567 {
 568         unsigned int i;
 569         u64 result;
 570         u16 word;
 571
 572         word = ines_read32(port, ts_rx);
 573         result = word;
 574         words--;
 575         for (i = 0; i < words; i++) {
 576                 word = ines_read32(port, ts_rx);
 577                 result <<= 16;
 578                 result |= word;
 579         }
 580         return result;
 581 }
 582
 583 static bool ines_timestamp_expired(struct ines_timestamp *ts)
 584 {
 585         return time_after(jiffies, ts->tmo);
 586 }
 587
 588 static int ines_ts_info(struct mii_timestamper *mii_ts,
 589                         struct ethtool_ts_info *info)
 590 {
 591         info->so_timestamping =
 592                 SOF_TIMESTAMPING_TX_HARDWARE |
 593                 SOF_TIMESTAMPING_TX_SOFTWARE |
 594                 SOF_TIMESTAMPING_RX_HARDWARE |
 595                 SOF_TIMESTAMPING_RX_SOFTWARE |
 596                 SOF_TIMESTAMPING_SOFTWARE |
 597                 SOF_TIMESTAMPING_RAW_HARDWARE;
 598
 599         info->phc_index = -1;
 600
 601         info->tx_types =
 602                 (1 << HWTSTAMP_TX_OFF) |
 603                 (1 << HWTSTAMP_TX_ON) |
 604                 (1 << HWTSTAMP_TX_ONESTEP_P2P);
 605
 606         info->rx_filters =
 607                 (1 << HWTSTAMP_FILTER_NONE) |
 608                 (1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
 609
 610         return 0;
 611 }
 612
 613 static u64 ines_txts64(struct ines_port *port, unsigned int words)
 614 {
 615         unsigned int i;
 616         u64 result;
 617         u16 word;
 618
 619         word = ines_read32(port, ts_tx);
 620         result = word;
 621         words--;
 622         for (i = 0; i < words; i++) {
 623                 word = ines_read32(port, ts_tx);
 624                 result <<= 16;
 625                 result |= word;
 626         }
 627         return result;
 628 }
 629
 630 static bool ines_txts_onestep(struct ines_port *port, struct sk_buff *skb, int type)
 631 {
 632         unsigned long flags;
 633         u32 port_conf;
 634
 635         spin_lock_irqsave(&port->lock, flags);
 636         port_conf = ines_read32(port, port_conf);
 637         spin_unlock_irqrestore(&port->lock, flags);
 638
 639         if (port_conf & CM_ONE_STEP)
 640                 return is_sync_pdelay_resp(skb, type);
 641
 642         return false;
 643 }
 644
 645 static void ines_txtstamp(struct mii_timestamper *mii_ts,
 646                           struct sk_buff *skb, int type)
 647 {
 648         struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
 649         struct sk_buff *old_skb = NULL;
 650         unsigned long flags;
 651
 652         if (!port->txts_enabled || ines_txts_onestep(port, skb, type)) {
 653                 kfree_skb(skb);
 654                 return;
 655         }
 656
 657         spin_lock_irqsave(&port->lock, flags);
 658
 659         if (port->tx_skb)
 660                 old_skb = port->tx_skb;
 661
 662         port->tx_skb = skb;
 663
 664         spin_unlock_irqrestore(&port->lock, flags);
 665
 666         if (old_skb)
 667                 kfree_skb(old_skb);
 668
 669         schedule_delayed_work(&port->ts_work, 1);
 670 }
 671
 672 static void ines_txtstamp_work(struct work_struct *work)
 673 {
 674         struct ines_port *port =
 675                 container_of(work, struct ines_port, ts_work.work);
 676         struct skb_shared_hwtstamps ssh;
 677         struct sk_buff *skb;
 678         unsigned long flags;
 679         u64 ns;
 680
 681         spin_lock_irqsave(&port->lock, flags);
 682         skb = port->tx_skb;
 683         port->tx_skb = NULL;
 684         spin_unlock_irqrestore(&port->lock, flags);
 685
 686         ns = ines_find_txts(port, skb);
 687         if (!ns) {
 688                 kfree_skb(skb);
 689                 return;
 690         }
 691         ssh.hwtstamp = ns_to_ktime(ns);
 692         skb_complete_tx_timestamp(skb, &ssh);
 693 }
 694
 695 static bool is_sync_pdelay_resp(struct sk_buff *skb, int type)
 696 {
 697         u8 *data = skb->data, *msgtype;
 698         unsigned int offset = 0;
 699
 700         if (type & PTP_CLASS_VLAN)
 701                 offset += VLAN_HLEN;
 702
 703         switch (type & PTP_CLASS_PMASK) {
 704         case PTP_CLASS_IPV4:
 705                 offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
 706                 break;
 707         case PTP_CLASS_IPV6:
 708                 offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
 709                 break;
 710         case PTP_CLASS_L2:
 711                 offset += ETH_HLEN;
 712                 break;
 713         default:
 714                 return 0;
 715         }
 716
 717         if (type & PTP_CLASS_V1)
 718                 offset += OFF_PTP_CONTROL;
 719
 720         if (skb->len < offset + 1)
 721                 return 0;
 722
 723         msgtype = data + offset;
 724
 725         switch ((*msgtype & 0xf)) {
 726         case SYNC:
 727         case PDELAY_RESP:
 728                 return true;
 729         default:
 730                 return false;
 731         }
 732 }
 733
 734 static u8 tag_to_msgtype(u8 tag)
 735 {
 736         switch (tag) {
 737         case MESSAGE_TYPE_SYNC:
 738                 return SYNC;
 739         case MESSAGE_TYPE_P_DELAY_REQ:
 740                 return PDELAY_REQ;
 741         case MESSAGE_TYPE_P_DELAY_RESP:
 742                 return PDELAY_RESP;
 743         case MESSAGE_TYPE_DELAY_REQ:
 744                 return DELAY_REQ;
 745         }
 746         return 0xf;
 747 }
 748
 749 static struct mii_timestamper *ines_ptp_probe_channel(struct device *device,
 750                                                       unsigned int index)
 751 {
 752         struct device_node *node = device->of_node;
 753         struct ines_port *port;
 754
 755         if (index > INES_N_PORTS - 1) {
 756                 dev_err(device, "bad port index %u\n", index);
 757                 return ERR_PTR(-EINVAL);
 758         }
 759         port = ines_find_port(node, index);
 760         if (!port) {
 761                 dev_err(device, "missing port index %u\n", index);
 762                 return ERR_PTR(-ENODEV);
 763         }
 764         port->mii_ts.rxtstamp = ines_rxtstamp;
 765         port->mii_ts.txtstamp = ines_txtstamp;
 766         port->mii_ts.hwtstamp = ines_hwtstamp;
 767         port->mii_ts.link_state = ines_link_state;
 768         port->mii_ts.ts_info = ines_ts_info;
 769
 770         return &port->mii_ts;
 771 }
 772
 773 static void ines_ptp_release_channel(struct device *device,
 774                                      struct mii_timestamper *mii_ts)
 775 {
 776 }
 777
 778 static struct mii_timestamping_ctrl ines_ctrl = {
 779         .probe_channel = ines_ptp_probe_channel,
 780         .release_channel = ines_ptp_release_channel,
 781 };
 782
 783 static int ines_ptp_ctrl_probe(struct platform_device *pld)
 784 {
 785         struct ines_clock *clock;
 786         struct resource *res;
 787         void __iomem *addr;
 788         int err = 0;
 789
 790         res = platform_get_resource(pld, IORESOURCE_MEM, 0);
 791         if (!res) {
 792                 dev_err(&pld->dev, "missing memory resource\n");
 793                 return -EINVAL;
 794         }
 795         addr = devm_ioremap_resource(&pld->dev, res);
 796         if (IS_ERR(addr)) {
 797                 err = PTR_ERR(addr);
 798                 goto out;
 799         }
 800         clock = kzalloc(sizeof(*clock), GFP_KERNEL);
 801         if (!clock) {
 802                 err = -ENOMEM;
 803                 goto out;
 804         }
 805         if (ines_clock_init(clock, &pld->dev, addr)) {
 806                 kfree(clock);
 807                 err = -ENOMEM;
 808                 goto out;
 809         }
 810         err = register_mii_tstamp_controller(&pld->dev, &ines_ctrl);
 811         if (err) {
 812                 kfree(clock);
 813                 goto out;
 814         }
 815         mutex_lock(&ines_clocks_lock);
 816         list_add_tail(&ines_clocks, &clock->list);
 817         mutex_unlock(&ines_clocks_lock);
 818
 819         dev_set_drvdata(&pld->dev, clock);
 820 out:
 821         return err;
 822 }
 823
 824 static int ines_ptp_ctrl_remove(struct platform_device *pld)
 825 {
 826         struct ines_clock *clock = dev_get_drvdata(&pld->dev);
 827
 828         unregister_mii_tstamp_controller(&pld->dev);
 829         mutex_lock(&ines_clocks_lock);
 830         list_del(&clock->list);
 831         mutex_unlock(&ines_clocks_lock);
 832         ines_clock_cleanup(clock);
 833         kfree(clock);
 834         return 0;
 835 }
 836
 837 static const struct of_device_id ines_ptp_ctrl_of_match[] = {
 838         { .compatible = "ines,ptp-ctrl" },
 839         { }
 840 };
 841
 842 MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match);
 843
 844 static struct platform_driver ines_ptp_ctrl_driver = {
 845         .probe  = ines_ptp_ctrl_probe,
 846         .remove = ines_ptp_ctrl_remove,
 847         .driver = {
 848                 .name = "ines_ptp_ctrl",
 849                 .of_match_table = of_match_ptr(ines_ptp_ctrl_of_match),
 850         },
 851 };
 852 module_platform_driver(ines_ptp_ctrl_driver);