timestamping.c

  1// SPDX-License-Identifier: GPL-2.0-or-later
  2/*
  3 * PTP 1588 clock support - support for timestamping in PHY devices
  4 *
  5 * Copyright (C) 2010 OMICRON electronics GmbH
  6 */
  7#include <linux/errqueue.h>
  8#include <linux/phy.h>
  9#include <linux/ptp_classify.h>
 10#include <linux/skbuff.h>
 11#include <linux/export.h>
 12#include <linux/ptp_clock_kernel.h>
 13
 14static unsigned int classify(const struct sk_buff *skb)
 15{
 16	if (likely(skb->dev && skb->dev->phydev &&
 17		   skb->dev->phydev->mii_ts))
 18		return ptp_classify_raw(skb);
 19	else
 20		return PTP_CLASS_NONE;
 21}
 22
 23void skb_clone_tx_timestamp(struct sk_buff *skb)
 24{
 25	struct hwtstamp_provider *hwprov;
 26	struct mii_timestamper *mii_ts;
 27	struct phy_device *phydev;
 28	struct sk_buff *clone;
 29	unsigned int type;
 30
 31	if (!skb->sk || !skb->dev)
 32		return;
 33
 34	rcu_read_lock();
 35	hwprov = rcu_dereference(skb->dev->hwprov);
 36	if (hwprov) {
 37		if (hwprov->source != HWTSTAMP_SOURCE_PHYLIB ||
 38		    !hwprov->phydev) {
 39			rcu_read_unlock();
 40			return;
 41		}
 42
 43		phydev = hwprov->phydev;
 44	} else {
 45		phydev = skb->dev->phydev;
 46		if (!phy_is_default_hwtstamp(phydev)) {
 47			rcu_read_unlock();
 48			return;
 49		}
 50	}
 51	rcu_read_unlock();
 52
 53	type = classify(skb);
 54	if (type == PTP_CLASS_NONE)
 55		return;
 56
 57	mii_ts = phydev->mii_ts;
 58	if (likely(mii_ts->txtstamp)) {
 59		clone = skb_clone_sk(skb);
 60		if (!clone)
 61			return;
 62		mii_ts->txtstamp(mii_ts, clone, type);
 63	}
 64}
 65EXPORT_SYMBOL_GPL(skb_clone_tx_timestamp);
 66
 67bool skb_defer_rx_timestamp(struct sk_buff *skb)
 68{
 69	struct hwtstamp_provider *hwprov;
 70	struct mii_timestamper *mii_ts;
 71	struct phy_device *phydev;
 72	unsigned int type;
 73
 74	if (!skb->dev)
 75		return false;
 76
 77	rcu_read_lock();
 78	hwprov = rcu_dereference(skb->dev->hwprov);
 79	if (hwprov) {
 80		if (hwprov->source != HWTSTAMP_SOURCE_PHYLIB ||
 81		    !hwprov->phydev) {
 82			rcu_read_unlock();
 83			return false;
 84		}
 85
 86		phydev = hwprov->phydev;
 87	} else {
 88		phydev = skb->dev->phydev;
 89		if (!phy_is_default_hwtstamp(phydev)) {
 90			rcu_read_unlock();
 91			return false;
 92		}
 93	}
 94	rcu_read_unlock();
 95
 96	if (skb_headroom(skb) < ETH_HLEN)
 97		return false;
 98
 99	__skb_push(skb, ETH_HLEN);
100
101	type = ptp_classify_raw(skb);
102
103	__skb_pull(skb, ETH_HLEN);
104
105	if (type == PTP_CLASS_NONE)
106		return false;
107
108	mii_ts = phydev->mii_ts;
109	if (likely(mii_ts->rxtstamp))
110		return mii_ts->rxtstamp(mii_ts, skb, type);
111
112	return false;
113}
114EXPORT_SYMBOL_GPL(skb_defer_rx_timestamp);