diff options
Diffstat (limited to 'drivers/net/ethernet/ti/am65-cpts.c')
-rw-r--r-- | drivers/net/ethernet/ti/am65-cpts.c | 85 |
1 files changed, 62 insertions, 23 deletions
diff --git a/drivers/net/ethernet/ti/am65-cpts.c b/drivers/net/ethernet/ti/am65-cpts.c index c66618d91c28..29e0b9ff23bf 100644 --- a/drivers/net/ethernet/ti/am65-cpts.c +++ b/drivers/net/ethernet/ti/am65-cpts.c @@ -859,29 +859,6 @@ static long am65_cpts_ts_work(struct ptp_clock_info *ptp) return delay; } -/** - * am65_cpts_rx_enable - enable rx timestamping - * @cpts: cpts handle - * @en: enable - * - * This functions enables rx packets timestamping. The CPTS can timestamp all - * rx packets. - */ -void am65_cpts_rx_enable(struct am65_cpts *cpts, bool en) -{ - u32 val; - - mutex_lock(&cpts->ptp_clk_lock); - val = am65_cpts_read32(cpts, control); - if (en) - val |= AM65_CPTS_CONTROL_TSTAMP_EN; - else - val &= ~AM65_CPTS_CONTROL_TSTAMP_EN; - am65_cpts_write32(cpts, val, control); - mutex_unlock(&cpts->ptp_clk_lock); -} -EXPORT_SYMBOL_GPL(am65_cpts_rx_enable); - static int am65_skb_get_mtype_seqid(struct sk_buff *skb, u32 *mtype_seqid) { unsigned int ptp_class = ptp_classify_raw(skb); @@ -906,6 +883,68 @@ static int am65_skb_get_mtype_seqid(struct sk_buff *skb, u32 *mtype_seqid) return 1; } +static u64 am65_cpts_get_rx_ts(struct am65_cpts *cpts, struct sk_buff *skb, + u32 skb_mtype_seqid) +{ + struct list_head *this, *next; + struct am65_cpts_event *event; + unsigned long flags; + u32 mtype_seqid; + u64 ns = 0; + + am65_cpts_fifo_read(cpts); + spin_lock_irqsave(&cpts->lock, flags); + list_for_each_safe(this, next, &cpts->events) { + event = list_entry(this, struct am65_cpts_event, list); + if (time_after(jiffies, event->tmo)) { + list_del_init(&event->list); + list_add(&event->list, &cpts->pool); + continue; + } + + mtype_seqid = event->event1 & + (AM65_CPTS_EVENT_1_MESSAGE_TYPE_MASK | + AM65_CPTS_EVENT_1_SEQUENCE_ID_MASK | + AM65_CPTS_EVENT_1_EVENT_TYPE_MASK); + + if (mtype_seqid == skb_mtype_seqid) { + ns = event->timestamp; + list_del_init(&event->list); + list_add(&event->list, &cpts->pool); + break; + } + } + spin_unlock_irqrestore(&cpts->lock, flags); + + return ns; +} + +void am65_cpts_rx_timestamp(struct am65_cpts *cpts, struct sk_buff *skb) +{ + struct am65_cpts_skb_cb_data *skb_cb = (struct am65_cpts_skb_cb_data *)skb->cb; + struct skb_shared_hwtstamps *ssh; + int ret; + u64 ns; + + skb_reset_mac_header(skb); + ret = am65_skb_get_mtype_seqid(skb, &skb_cb->skb_mtype_seqid); + if (!ret) + return; /* if not PTP class packet */ + + skb_cb->skb_mtype_seqid |= (AM65_CPTS_EV_RX << AM65_CPTS_EVENT_1_EVENT_TYPE_SHIFT); + + dev_dbg(cpts->dev, "%s mtype seqid %08x\n", __func__, skb_cb->skb_mtype_seqid); + + ns = am65_cpts_get_rx_ts(cpts, skb, skb_cb->skb_mtype_seqid); + if (!ns) + return; + + ssh = skb_hwtstamps(skb); + memset(ssh, 0, sizeof(*ssh)); + ssh->hwtstamp = ns_to_ktime(ns); +} +EXPORT_SYMBOL_GPL(am65_cpts_rx_timestamp); + /** * am65_cpts_tx_timestamp - save tx packet for timestamping * @cpts: cpts handle |