Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0
0002 //
0003 // Copyright (C) 2018 MOSER-BAER AG
0004 //
0005 
0006 #define pr_fmt(fmt) "InES_PTP: " fmt
0007 
0008 #include <linux/ethtool.h>
0009 #include <linux/export.h>
0010 #include <linux/if_vlan.h>
0011 #include <linux/mii_timestamper.h>
0012 #include <linux/module.h>
0013 #include <linux/net_tstamp.h>
0014 #include <linux/of.h>
0015 #include <linux/of_address.h>
0016 #include <linux/of_irq.h>
0017 #include <linux/phy.h>
0018 #include <linux/platform_device.h>
0019 #include <linux/ptp_classify.h>
0020 #include <linux/ptp_clock_kernel.h>
0021 #include <linux/stddef.h>
0022 
0023 MODULE_DESCRIPTION("Driver for the ZHAW InES PTP time stamping IP core");
0024 MODULE_AUTHOR("Richard Cochran <richardcochran@gmail.com>");
0025 MODULE_VERSION("1.0");
0026 MODULE_LICENSE("GPL");
0027 
0028 /* GLOBAL register */
0029 #define MCAST_MAC_SELECT_SHIFT  2
0030 #define MCAST_MAC_SELECT_MASK   0x3
0031 #define IO_RESET        BIT(1)
0032 #define PTP_RESET       BIT(0)
0033 
0034 /* VERSION register */
0035 #define IF_MAJOR_VER_SHIFT  12
0036 #define IF_MAJOR_VER_MASK   0xf
0037 #define IF_MINOR_VER_SHIFT  8
0038 #define IF_MINOR_VER_MASK   0xf
0039 #define FPGA_MAJOR_VER_SHIFT    4
0040 #define FPGA_MAJOR_VER_MASK 0xf
0041 #define FPGA_MINOR_VER_SHIFT    0
0042 #define FPGA_MINOR_VER_MASK 0xf
0043 
0044 /* INT_STAT register */
0045 #define RX_INTR_STATUS_3    BIT(5)
0046 #define RX_INTR_STATUS_2    BIT(4)
0047 #define RX_INTR_STATUS_1    BIT(3)
0048 #define TX_INTR_STATUS_3    BIT(2)
0049 #define TX_INTR_STATUS_2    BIT(1)
0050 #define TX_INTR_STATUS_1    BIT(0)
0051 
0052 /* INT_MSK register */
0053 #define RX_INTR_MASK_3      BIT(5)
0054 #define RX_INTR_MASK_2      BIT(4)
0055 #define RX_INTR_MASK_1      BIT(3)
0056 #define TX_INTR_MASK_3      BIT(2)
0057 #define TX_INTR_MASK_2      BIT(1)
0058 #define TX_INTR_MASK_1      BIT(0)
0059 
0060 /* BUF_STAT register */
0061 #define RX_FIFO_NE_3        BIT(5)
0062 #define RX_FIFO_NE_2        BIT(4)
0063 #define RX_FIFO_NE_1        BIT(3)
0064 #define TX_FIFO_NE_3        BIT(2)
0065 #define TX_FIFO_NE_2        BIT(1)
0066 #define TX_FIFO_NE_1        BIT(0)
0067 
0068 /* PORT_CONF register */
0069 #define CM_ONE_STEP     BIT(6)
0070 #define PHY_SPEED_SHIFT     4
0071 #define PHY_SPEED_MASK      0x3
0072 #define P2P_DELAY_WR_POS_SHIFT  2
0073 #define P2P_DELAY_WR_POS_MASK   0x3
0074 #define PTP_MODE_SHIFT      0
0075 #define PTP_MODE_MASK       0x3
0076 
0077 /* TS_STAT_TX register */
0078 #define TS_ENABLE       BIT(15)
0079 #define DATA_READ_POS_SHIFT 8
0080 #define DATA_READ_POS_MASK  0x1f
0081 #define DISCARDED_EVENTS_SHIFT  4
0082 #define DISCARDED_EVENTS_MASK   0xf
0083 
0084 #define INES_N_PORTS        3
0085 #define INES_REGISTER_SIZE  0x80
0086 #define INES_PORT_OFFSET    0x20
0087 #define INES_PORT_SIZE      0x20
0088 #define INES_FIFO_DEPTH     90
0089 #define INES_MAX_EVENTS     100
0090 
0091 #define BC_PTP_V1       0
0092 #define BC_PTP_V2       1
0093 #define TC_E2E_PTP_V2       2
0094 #define TC_P2P_PTP_V2       3
0095 
0096 #define PHY_SPEED_10        0
0097 #define PHY_SPEED_100       1
0098 #define PHY_SPEED_1000      2
0099 
0100 #define PORT_CONF \
0101     ((PHY_SPEED_1000 << PHY_SPEED_SHIFT) | (BC_PTP_V2 << PTP_MODE_SHIFT))
0102 
0103 #define ines_read32(s, r)   __raw_readl((void __iomem *)&s->regs->r)
0104 #define ines_write32(s, v, r)   __raw_writel(v, (void __iomem *)&s->regs->r)
0105 
0106 #define MESSAGE_TYPE_SYNC       1
0107 #define MESSAGE_TYPE_P_DELAY_REQ    2
0108 #define MESSAGE_TYPE_P_DELAY_RESP   3
0109 #define MESSAGE_TYPE_DELAY_REQ      4
0110 
0111 static LIST_HEAD(ines_clocks);
0112 static DEFINE_MUTEX(ines_clocks_lock);
0113 
0114 struct ines_global_regs {
0115     u32 id;
0116     u32 test;
0117     u32 global;
0118     u32 version;
0119     u32 test2;
0120     u32 int_stat;
0121     u32 int_msk;
0122     u32 buf_stat;
0123 };
0124 
0125 struct ines_port_registers {
0126     u32 port_conf;
0127     u32 p_delay;
0128     u32 ts_stat_tx;
0129     u32 ts_stat_rx;
0130     u32 ts_tx;
0131     u32 ts_rx;
0132 };
0133 
0134 struct ines_timestamp {
0135     struct list_head list;
0136     unsigned long   tmo;
0137     u16     tag;
0138     u64     sec;
0139     u64     nsec;
0140     u64     clkid;
0141     u16     portnum;
0142     u16     seqid;
0143 };
0144 
0145 struct ines_port {
0146     struct ines_port_registers  *regs;
0147     struct mii_timestamper      mii_ts;
0148     struct ines_clock       *clock;
0149     bool                rxts_enabled;
0150     bool                txts_enabled;
0151     unsigned int            index;
0152     struct delayed_work     ts_work;
0153     /* lock protects event list and tx_skb */
0154     spinlock_t          lock;
0155     struct sk_buff          *tx_skb;
0156     struct list_head        events;
0157     struct list_head        pool;
0158     struct ines_timestamp       pool_data[INES_MAX_EVENTS];
0159 };
0160 
0161 struct ines_clock {
0162     struct ines_port        port[INES_N_PORTS];
0163     struct ines_global_regs __iomem *regs;
0164     void __iomem            *base;
0165     struct device_node      *node;
0166     struct device           *dev;
0167     struct list_head        list;
0168 };
0169 
0170 static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
0171                struct ines_timestamp *ts, struct device *dev);
0172 static int ines_rxfifo_read(struct ines_port *port);
0173 static u64 ines_rxts64(struct ines_port *port, unsigned int words);
0174 static bool ines_timestamp_expired(struct ines_timestamp *ts);
0175 static u64 ines_txts64(struct ines_port *port, unsigned int words);
0176 static void ines_txtstamp_work(struct work_struct *work);
0177 static bool is_sync_pdelay_resp(struct sk_buff *skb, int type);
0178 static u8 tag_to_msgtype(u8 tag);
0179 
0180 static void ines_clock_cleanup(struct ines_clock *clock)
0181 {
0182     struct ines_port *port;
0183     int i;
0184 
0185     for (i = 0; i < INES_N_PORTS; i++) {
0186         port = &clock->port[i];
0187         cancel_delayed_work_sync(&port->ts_work);
0188     }
0189 }
0190 
0191 static int ines_clock_init(struct ines_clock *clock, struct device *device,
0192                void __iomem *addr)
0193 {
0194     struct device_node *node = device->of_node;
0195     unsigned long port_addr;
0196     struct ines_port *port;
0197     int i, j;
0198 
0199     INIT_LIST_HEAD(&clock->list);
0200     clock->node = node;
0201     clock->dev  = device;
0202     clock->base = addr;
0203     clock->regs = clock->base;
0204 
0205     for (i = 0; i < INES_N_PORTS; i++) {
0206         port = &clock->port[i];
0207         port_addr = (unsigned long) clock->base +
0208             INES_PORT_OFFSET + i * INES_PORT_SIZE;
0209         port->regs = (struct ines_port_registers *) port_addr;
0210         port->clock = clock;
0211         port->index = i;
0212         INIT_DELAYED_WORK(&port->ts_work, ines_txtstamp_work);
0213         spin_lock_init(&port->lock);
0214         INIT_LIST_HEAD(&port->events);
0215         INIT_LIST_HEAD(&port->pool);
0216         for (j = 0; j < INES_MAX_EVENTS; j++)
0217             list_add(&port->pool_data[j].list, &port->pool);
0218     }
0219 
0220     ines_write32(clock, 0xBEEF, test);
0221     ines_write32(clock, 0xBEEF, test2);
0222 
0223     dev_dbg(device, "ID      0x%x\n", ines_read32(clock, id));
0224     dev_dbg(device, "TEST    0x%x\n", ines_read32(clock, test));
0225     dev_dbg(device, "VERSION 0x%x\n", ines_read32(clock, version));
0226     dev_dbg(device, "TEST2   0x%x\n", ines_read32(clock, test2));
0227 
0228     for (i = 0; i < INES_N_PORTS; i++) {
0229         port = &clock->port[i];
0230         ines_write32(port, PORT_CONF, port_conf);
0231     }
0232 
0233     return 0;
0234 }
0235 
0236 static struct ines_port *ines_find_port(struct device_node *node, u32 index)
0237 {
0238     struct ines_port *port = NULL;
0239     struct ines_clock *clock;
0240     struct list_head *this;
0241 
0242     mutex_lock(&ines_clocks_lock);
0243     list_for_each(this, &ines_clocks) {
0244         clock = list_entry(this, struct ines_clock, list);
0245         if (clock->node == node) {
0246             port = &clock->port[index];
0247             break;
0248         }
0249     }
0250     mutex_unlock(&ines_clocks_lock);
0251     return port;
0252 }
0253 
0254 static u64 ines_find_rxts(struct ines_port *port, struct sk_buff *skb, int type)
0255 {
0256     struct list_head *this, *next;
0257     struct ines_timestamp *ts;
0258     unsigned long flags;
0259     u64 ns = 0;
0260 
0261     if (type == PTP_CLASS_NONE)
0262         return 0;
0263 
0264     spin_lock_irqsave(&port->lock, flags);
0265     ines_rxfifo_read(port);
0266     list_for_each_safe(this, next, &port->events) {
0267         ts = list_entry(this, struct ines_timestamp, list);
0268         if (ines_timestamp_expired(ts)) {
0269             list_del_init(&ts->list);
0270             list_add(&ts->list, &port->pool);
0271             continue;
0272         }
0273         if (ines_match(skb, type, ts, port->clock->dev)) {
0274             ns = ts->sec * 1000000000ULL + ts->nsec;
0275             list_del_init(&ts->list);
0276             list_add(&ts->list, &port->pool);
0277             break;
0278         }
0279     }
0280     spin_unlock_irqrestore(&port->lock, flags);
0281 
0282     return ns;
0283 }
0284 
0285 static u64 ines_find_txts(struct ines_port *port, struct sk_buff *skb)
0286 {
0287     unsigned int class = ptp_classify_raw(skb), i;
0288     u32 data_rd_pos, buf_stat, mask, ts_stat_tx;
0289     struct ines_timestamp ts;
0290     unsigned long flags;
0291     u64 ns = 0;
0292 
0293     mask = TX_FIFO_NE_1 << port->index;
0294 
0295     spin_lock_irqsave(&port->lock, flags);
0296 
0297     for (i = 0; i < INES_FIFO_DEPTH; i++) {
0298 
0299         buf_stat = ines_read32(port->clock, buf_stat);
0300         if (!(buf_stat & mask)) {
0301             dev_dbg(port->clock->dev,
0302                   "Tx timestamp FIFO unexpectedly empty\n");
0303             break;
0304         }
0305         ts_stat_tx = ines_read32(port, ts_stat_tx);
0306         data_rd_pos = (ts_stat_tx >> DATA_READ_POS_SHIFT) &
0307             DATA_READ_POS_MASK;
0308         if (data_rd_pos) {
0309             dev_err(port->clock->dev,
0310                 "unexpected Tx read pos %u\n", data_rd_pos);
0311             break;
0312         }
0313 
0314         ts.tag     = ines_read32(port, ts_tx);
0315         ts.sec     = ines_txts64(port, 3);
0316         ts.nsec    = ines_txts64(port, 2);
0317         ts.clkid   = ines_txts64(port, 4);
0318         ts.portnum = ines_read32(port, ts_tx);
0319         ts.seqid   = ines_read32(port, ts_tx);
0320 
0321         if (ines_match(skb, class, &ts, port->clock->dev)) {
0322             ns = ts.sec * 1000000000ULL + ts.nsec;
0323             break;
0324         }
0325     }
0326 
0327     spin_unlock_irqrestore(&port->lock, flags);
0328     return ns;
0329 }
0330 
0331 static int ines_hwtstamp(struct mii_timestamper *mii_ts, struct ifreq *ifr)
0332 {
0333     struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
0334     u32 cm_one_step = 0, port_conf, ts_stat_rx, ts_stat_tx;
0335     struct hwtstamp_config cfg;
0336     unsigned long flags;
0337 
0338     if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
0339         return -EFAULT;
0340 
0341     switch (cfg.tx_type) {
0342     case HWTSTAMP_TX_OFF:
0343         ts_stat_tx = 0;
0344         break;
0345     case HWTSTAMP_TX_ON:
0346         ts_stat_tx = TS_ENABLE;
0347         break;
0348     case HWTSTAMP_TX_ONESTEP_P2P:
0349         ts_stat_tx = TS_ENABLE;
0350         cm_one_step = CM_ONE_STEP;
0351         break;
0352     default:
0353         return -ERANGE;
0354     }
0355 
0356     switch (cfg.rx_filter) {
0357     case HWTSTAMP_FILTER_NONE:
0358         ts_stat_rx = 0;
0359         break;
0360     case HWTSTAMP_FILTER_ALL:
0361     case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
0362     case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
0363     case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
0364         return -ERANGE;
0365     case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
0366     case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
0367     case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
0368     case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
0369     case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
0370     case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
0371     case HWTSTAMP_FILTER_PTP_V2_EVENT:
0372     case HWTSTAMP_FILTER_PTP_V2_SYNC:
0373     case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
0374         ts_stat_rx = TS_ENABLE;
0375         cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
0376         break;
0377     default:
0378         return -ERANGE;
0379     }
0380 
0381     spin_lock_irqsave(&port->lock, flags);
0382 
0383     port_conf = ines_read32(port, port_conf);
0384     port_conf &= ~CM_ONE_STEP;
0385     port_conf |= cm_one_step;
0386 
0387     ines_write32(port, port_conf, port_conf);
0388     ines_write32(port, ts_stat_rx, ts_stat_rx);
0389     ines_write32(port, ts_stat_tx, ts_stat_tx);
0390 
0391     port->rxts_enabled = ts_stat_rx == TS_ENABLE;
0392     port->txts_enabled = ts_stat_tx == TS_ENABLE;
0393 
0394     spin_unlock_irqrestore(&port->lock, flags);
0395 
0396     return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
0397 }
0398 
0399 static void ines_link_state(struct mii_timestamper *mii_ts,
0400                 struct phy_device *phydev)
0401 {
0402     struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
0403     u32 port_conf, speed_conf;
0404     unsigned long flags;
0405 
0406     switch (phydev->speed) {
0407     case SPEED_10:
0408         speed_conf = PHY_SPEED_10 << PHY_SPEED_SHIFT;
0409         break;
0410     case SPEED_100:
0411         speed_conf = PHY_SPEED_100 << PHY_SPEED_SHIFT;
0412         break;
0413     case SPEED_1000:
0414         speed_conf = PHY_SPEED_1000 << PHY_SPEED_SHIFT;
0415         break;
0416     default:
0417         dev_err(port->clock->dev, "bad speed: %d\n", phydev->speed);
0418         return;
0419     }
0420     spin_lock_irqsave(&port->lock, flags);
0421 
0422     port_conf = ines_read32(port, port_conf);
0423     port_conf &= ~(0x3 << PHY_SPEED_SHIFT);
0424     port_conf |= speed_conf;
0425 
0426     ines_write32(port, port_conf, port_conf);
0427 
0428     spin_unlock_irqrestore(&port->lock, flags);
0429 }
0430 
0431 static bool ines_match(struct sk_buff *skb, unsigned int ptp_class,
0432                struct ines_timestamp *ts, struct device *dev)
0433 {
0434     struct ptp_header *hdr;
0435     u16 portn, seqid;
0436     u8 msgtype;
0437     u64 clkid;
0438 
0439     if (unlikely(ptp_class & PTP_CLASS_V1))
0440         return false;
0441 
0442     hdr = ptp_parse_header(skb, ptp_class);
0443     if (!hdr)
0444         return false;
0445 
0446     msgtype = ptp_get_msgtype(hdr, ptp_class);
0447     clkid = be64_to_cpup((__be64 *)&hdr->source_port_identity.clock_identity.id[0]);
0448     portn = be16_to_cpu(hdr->source_port_identity.port_number);
0449     seqid = be16_to_cpu(hdr->sequence_id);
0450 
0451     if (tag_to_msgtype(ts->tag & 0x7) != msgtype) {
0452         dev_dbg(dev, "msgtype mismatch ts %hhu != skb %hhu\n",
0453             tag_to_msgtype(ts->tag & 0x7), msgtype);
0454         return false;
0455     }
0456     if (ts->clkid != clkid) {
0457         dev_dbg(dev, "clkid mismatch ts %llx != skb %llx\n",
0458             ts->clkid, clkid);
0459         return false;
0460     }
0461     if (ts->portnum != portn) {
0462         dev_dbg(dev, "portn mismatch ts %hu != skb %hu\n",
0463             ts->portnum, portn);
0464         return false;
0465     }
0466     if (ts->seqid != seqid) {
0467         dev_dbg(dev, "seqid mismatch ts %hu != skb %hu\n",
0468             ts->seqid, seqid);
0469         return false;
0470     }
0471 
0472     return true;
0473 }
0474 
0475 static bool ines_rxtstamp(struct mii_timestamper *mii_ts,
0476               struct sk_buff *skb, int type)
0477 {
0478     struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
0479     struct skb_shared_hwtstamps *ssh;
0480     u64 ns;
0481 
0482     if (!port->rxts_enabled)
0483         return false;
0484 
0485     ns = ines_find_rxts(port, skb, type);
0486     if (!ns)
0487         return false;
0488 
0489     ssh = skb_hwtstamps(skb);
0490     ssh->hwtstamp = ns_to_ktime(ns);
0491     netif_rx(skb);
0492 
0493     return true;
0494 }
0495 
0496 static int ines_rxfifo_read(struct ines_port *port)
0497 {
0498     u32 data_rd_pos, buf_stat, mask, ts_stat_rx;
0499     struct ines_timestamp *ts;
0500     unsigned int i;
0501 
0502     mask = RX_FIFO_NE_1 << port->index;
0503 
0504     for (i = 0; i < INES_FIFO_DEPTH; i++) {
0505         if (list_empty(&port->pool)) {
0506             dev_err(port->clock->dev, "event pool is empty\n");
0507             return -1;
0508         }
0509         buf_stat = ines_read32(port->clock, buf_stat);
0510         if (!(buf_stat & mask))
0511             break;
0512 
0513         ts_stat_rx = ines_read32(port, ts_stat_rx);
0514         data_rd_pos = (ts_stat_rx >> DATA_READ_POS_SHIFT) &
0515             DATA_READ_POS_MASK;
0516         if (data_rd_pos) {
0517             dev_err(port->clock->dev, "unexpected Rx read pos %u\n",
0518                 data_rd_pos);
0519             break;
0520         }
0521 
0522         ts = list_first_entry(&port->pool, struct ines_timestamp, list);
0523         ts->tmo     = jiffies + HZ;
0524         ts->tag     = ines_read32(port, ts_rx);
0525         ts->sec     = ines_rxts64(port, 3);
0526         ts->nsec    = ines_rxts64(port, 2);
0527         ts->clkid   = ines_rxts64(port, 4);
0528         ts->portnum = ines_read32(port, ts_rx);
0529         ts->seqid   = ines_read32(port, ts_rx);
0530 
0531         list_del_init(&ts->list);
0532         list_add_tail(&ts->list, &port->events);
0533     }
0534 
0535     return 0;
0536 }
0537 
0538 static u64 ines_rxts64(struct ines_port *port, unsigned int words)
0539 {
0540     unsigned int i;
0541     u64 result;
0542     u16 word;
0543 
0544     word = ines_read32(port, ts_rx);
0545     result = word;
0546     words--;
0547     for (i = 0; i < words; i++) {
0548         word = ines_read32(port, ts_rx);
0549         result <<= 16;
0550         result |= word;
0551     }
0552     return result;
0553 }
0554 
0555 static bool ines_timestamp_expired(struct ines_timestamp *ts)
0556 {
0557     return time_after(jiffies, ts->tmo);
0558 }
0559 
0560 static int ines_ts_info(struct mii_timestamper *mii_ts,
0561             struct ethtool_ts_info *info)
0562 {
0563     info->so_timestamping =
0564         SOF_TIMESTAMPING_TX_HARDWARE |
0565         SOF_TIMESTAMPING_TX_SOFTWARE |
0566         SOF_TIMESTAMPING_RX_HARDWARE |
0567         SOF_TIMESTAMPING_RX_SOFTWARE |
0568         SOF_TIMESTAMPING_SOFTWARE |
0569         SOF_TIMESTAMPING_RAW_HARDWARE;
0570 
0571     info->phc_index = -1;
0572 
0573     info->tx_types =
0574         (1 << HWTSTAMP_TX_OFF) |
0575         (1 << HWTSTAMP_TX_ON) |
0576         (1 << HWTSTAMP_TX_ONESTEP_P2P);
0577 
0578     info->rx_filters =
0579         (1 << HWTSTAMP_FILTER_NONE) |
0580         (1 << HWTSTAMP_FILTER_PTP_V2_EVENT);
0581 
0582     return 0;
0583 }
0584 
0585 static u64 ines_txts64(struct ines_port *port, unsigned int words)
0586 {
0587     unsigned int i;
0588     u64 result;
0589     u16 word;
0590 
0591     word = ines_read32(port, ts_tx);
0592     result = word;
0593     words--;
0594     for (i = 0; i < words; i++) {
0595         word = ines_read32(port, ts_tx);
0596         result <<= 16;
0597         result |= word;
0598     }
0599     return result;
0600 }
0601 
0602 static bool ines_txts_onestep(struct ines_port *port, struct sk_buff *skb, int type)
0603 {
0604     unsigned long flags;
0605     u32 port_conf;
0606 
0607     spin_lock_irqsave(&port->lock, flags);
0608     port_conf = ines_read32(port, port_conf);
0609     spin_unlock_irqrestore(&port->lock, flags);
0610 
0611     if (port_conf & CM_ONE_STEP)
0612         return is_sync_pdelay_resp(skb, type);
0613 
0614     return false;
0615 }
0616 
0617 static void ines_txtstamp(struct mii_timestamper *mii_ts,
0618               struct sk_buff *skb, int type)
0619 {
0620     struct ines_port *port = container_of(mii_ts, struct ines_port, mii_ts);
0621     struct sk_buff *old_skb = NULL;
0622     unsigned long flags;
0623 
0624     if (!port->txts_enabled || ines_txts_onestep(port, skb, type)) {
0625         kfree_skb(skb);
0626         return;
0627     }
0628 
0629     spin_lock_irqsave(&port->lock, flags);
0630 
0631     if (port->tx_skb)
0632         old_skb = port->tx_skb;
0633 
0634     port->tx_skb = skb;
0635 
0636     spin_unlock_irqrestore(&port->lock, flags);
0637 
0638     kfree_skb(old_skb);
0639 
0640     schedule_delayed_work(&port->ts_work, 1);
0641 }
0642 
0643 static void ines_txtstamp_work(struct work_struct *work)
0644 {
0645     struct ines_port *port =
0646         container_of(work, struct ines_port, ts_work.work);
0647     struct skb_shared_hwtstamps ssh;
0648     struct sk_buff *skb;
0649     unsigned long flags;
0650     u64 ns;
0651 
0652     spin_lock_irqsave(&port->lock, flags);
0653     skb = port->tx_skb;
0654     port->tx_skb = NULL;
0655     spin_unlock_irqrestore(&port->lock, flags);
0656 
0657     ns = ines_find_txts(port, skb);
0658     if (!ns) {
0659         kfree_skb(skb);
0660         return;
0661     }
0662     ssh.hwtstamp = ns_to_ktime(ns);
0663     skb_complete_tx_timestamp(skb, &ssh);
0664 }
0665 
0666 static bool is_sync_pdelay_resp(struct sk_buff *skb, int type)
0667 {
0668     struct ptp_header *hdr;
0669     u8 msgtype;
0670 
0671     hdr = ptp_parse_header(skb, type);
0672     if (!hdr)
0673         return false;
0674 
0675     msgtype = ptp_get_msgtype(hdr, type);
0676 
0677     switch (msgtype) {
0678     case PTP_MSGTYPE_SYNC:
0679     case PTP_MSGTYPE_PDELAY_RESP:
0680         return true;
0681     default:
0682         return false;
0683     }
0684 }
0685 
0686 static u8 tag_to_msgtype(u8 tag)
0687 {
0688     switch (tag) {
0689     case MESSAGE_TYPE_SYNC:
0690         return PTP_MSGTYPE_SYNC;
0691     case MESSAGE_TYPE_P_DELAY_REQ:
0692         return PTP_MSGTYPE_PDELAY_REQ;
0693     case MESSAGE_TYPE_P_DELAY_RESP:
0694         return PTP_MSGTYPE_PDELAY_RESP;
0695     case MESSAGE_TYPE_DELAY_REQ:
0696         return PTP_MSGTYPE_DELAY_REQ;
0697     }
0698     return 0xf;
0699 }
0700 
0701 static struct mii_timestamper *ines_ptp_probe_channel(struct device *device,
0702                               unsigned int index)
0703 {
0704     struct device_node *node = device->of_node;
0705     struct ines_port *port;
0706 
0707     if (index > INES_N_PORTS - 1) {
0708         dev_err(device, "bad port index %u\n", index);
0709         return ERR_PTR(-EINVAL);
0710     }
0711     port = ines_find_port(node, index);
0712     if (!port) {
0713         dev_err(device, "missing port index %u\n", index);
0714         return ERR_PTR(-ENODEV);
0715     }
0716     port->mii_ts.rxtstamp = ines_rxtstamp;
0717     port->mii_ts.txtstamp = ines_txtstamp;
0718     port->mii_ts.hwtstamp = ines_hwtstamp;
0719     port->mii_ts.link_state = ines_link_state;
0720     port->mii_ts.ts_info = ines_ts_info;
0721 
0722     return &port->mii_ts;
0723 }
0724 
0725 static void ines_ptp_release_channel(struct device *device,
0726                      struct mii_timestamper *mii_ts)
0727 {
0728 }
0729 
0730 static struct mii_timestamping_ctrl ines_ctrl = {
0731     .probe_channel = ines_ptp_probe_channel,
0732     .release_channel = ines_ptp_release_channel,
0733 };
0734 
0735 static int ines_ptp_ctrl_probe(struct platform_device *pld)
0736 {
0737     struct ines_clock *clock;
0738     void __iomem *addr;
0739     int err = 0;
0740 
0741     addr = devm_platform_ioremap_resource(pld, 0);
0742     if (IS_ERR(addr)) {
0743         err = PTR_ERR(addr);
0744         goto out;
0745     }
0746     clock = kzalloc(sizeof(*clock), GFP_KERNEL);
0747     if (!clock) {
0748         err = -ENOMEM;
0749         goto out;
0750     }
0751     if (ines_clock_init(clock, &pld->dev, addr)) {
0752         kfree(clock);
0753         err = -ENOMEM;
0754         goto out;
0755     }
0756     err = register_mii_tstamp_controller(&pld->dev, &ines_ctrl);
0757     if (err) {
0758         kfree(clock);
0759         goto out;
0760     }
0761     mutex_lock(&ines_clocks_lock);
0762     list_add_tail(&ines_clocks, &clock->list);
0763     mutex_unlock(&ines_clocks_lock);
0764 
0765     dev_set_drvdata(&pld->dev, clock);
0766 out:
0767     return err;
0768 }
0769 
0770 static int ines_ptp_ctrl_remove(struct platform_device *pld)
0771 {
0772     struct ines_clock *clock = dev_get_drvdata(&pld->dev);
0773 
0774     unregister_mii_tstamp_controller(&pld->dev);
0775     mutex_lock(&ines_clocks_lock);
0776     list_del(&clock->list);
0777     mutex_unlock(&ines_clocks_lock);
0778     ines_clock_cleanup(clock);
0779     kfree(clock);
0780     return 0;
0781 }
0782 
0783 static const struct of_device_id ines_ptp_ctrl_of_match[] = {
0784     { .compatible = "ines,ptp-ctrl" },
0785     { }
0786 };
0787 
0788 MODULE_DEVICE_TABLE(of, ines_ptp_ctrl_of_match);
0789 
0790 static struct platform_driver ines_ptp_ctrl_driver = {
0791     .probe  = ines_ptp_ctrl_probe,
0792     .remove = ines_ptp_ctrl_remove,
0793     .driver = {
0794         .name = "ines_ptp_ctrl",
0795         .of_match_table = of_match_ptr(ines_ptp_ctrl_of_match),
0796     },
0797 };
0798 module_platform_driver(ines_ptp_ctrl_driver);
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!