[deliverable/linux.git] / net / core / timestamping.c

/*
 * PTP 1588 clock support - support for timestamping in PHY devices
 *
 * Copyright (C) 2010 OMICRON electronics GmbH
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
#include <linux/errqueue.h>
#include <linux/phy.h>
#include <linux/ptp_classify.h>
#include <linux/skbuff.h>
#include <linux/export.h>

static unsigned int classify(const struct sk_buff *skb)
{
	if (likely(skb->dev && skb->dev->phydev &&
		   skb->dev->phydev->drv))
		return ptp_classify_raw(skb);
	else
		return PTP_CLASS_NONE;
}

void skb_clone_tx_timestamp(struct sk_buff *skb)
{
	struct phy_device *phydev;
	struct sk_buff *clone;
	struct sock *sk = skb->sk;
	unsigned int type;

	if (!sk)
		return;

	type = classify(skb);
	if (type == PTP_CLASS_NONE)
		return;

	phydev = skb->dev->phydev;
	if (likely(phydev->drv->txtstamp)) {
		if (!atomic_inc_not_zero(&sk->sk_refcnt))
			return;

		clone = skb_clone(skb, GFP_ATOMIC);
		if (!clone) {
			sock_put(sk);
			return;
		}

		clone->sk = sk;
		phydev->drv->txtstamp(phydev, clone, type);
	}
}
EXPORT_SYMBOL_GPL(skb_clone_tx_timestamp);

void skb_complete_tx_timestamp(struct sk_buff *skb,
			       struct skb_shared_hwtstamps *hwtstamps)
{
	struct sock *sk = skb->sk;
	struct sock_exterr_skb *serr;
	int err;

	if (!hwtstamps) {
		sock_put(sk);
		kfree_skb(skb);
		return;
	}

	*skb_hwtstamps(skb) = *hwtstamps;

	serr = SKB_EXT_ERR(skb);
	memset(serr, 0, sizeof(*serr));
	serr->ee.ee_errno = ENOMSG;
	serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING;
	skb->sk = NULL;

	err = sock_queue_err_skb(sk, skb);

	sock_put(sk);
	if (err)
		kfree_skb(skb);
}
EXPORT_SYMBOL_GPL(skb_complete_tx_timestamp);

bool skb_defer_rx_timestamp(struct sk_buff *skb)
{
	struct phy_device *phydev;
	unsigned int type;

	if (skb_headroom(skb) < ETH_HLEN)
		return false;
	__skb_push(skb, ETH_HLEN);

	type = classify(skb);

	__skb_pull(skb, ETH_HLEN);

	if (type == PTP_CLASS_NONE)
		return false;

	phydev = skb->dev->phydev;
	if (likely(phydev->drv->rxtstamp))
		return phydev->drv->rxtstamp(phydev, skb, type);

	return false;
}
EXPORT_SYMBOL_GPL(skb_defer_rx_timestamp);
Commit	Line	Data
c1f19b51 RC	1	/*
	2	* PTP 1588 clock support - support for timestamping in PHY devices
	3	*
	4	* Copyright (C) 2010 OMICRON electronics GmbH
	5	*
	6	* This program is free software; you can redistribute it and/or modify
	7	* it under the terms of the GNU General Public License as published by
	8	* the Free Software Foundation; either version 2 of the License, or
	9	* (at your option) any later version.
	10	*
	11	* This program is distributed in the hope that it will be useful,
	12	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	13	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	14	* GNU General Public License for more details.
	15	*
	16	* You should have received a copy of the GNU General Public License
	17	* along with this program; if not, write to the Free Software
	18	* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
	19	*/
	20	#include <linux/errqueue.h>
	21	#include <linux/phy.h>
	22	#include <linux/ptp_classify.h>
	23	#include <linux/skbuff.h>
bc3b2d7f	24	#include <linux/export.h>
c1f19b51	25
62ab0812	26	static unsigned int classify(const struct sk_buff *skb)
c1f19b51	27	{
e62d2df0	28	if (likely(skb->dev && skb->dev->phydev &&
c1f19b51	29	skb->dev->phydev->drv))
164d8c66	30	return ptp_classify_raw(skb);
c1f19b51 RC	31	else
	32	return PTP_CLASS_NONE;
	33	}
	34
	35	void skb_clone_tx_timestamp(struct sk_buff *skb)
	36	{
	37	struct phy_device *phydev;
	38	struct sk_buff *clone;
	39	struct sock *sk = skb->sk;
	40	unsigned int type;
	41
	42	if (!sk)
	43	return;
	44
	45	type = classify(skb);
b9c701ed SS	46	if (type == PTP_CLASS_NONE)
	47	return;
	48
	49	phydev = skb->dev->phydev;
	50	if (likely(phydev->drv->txtstamp)) {
	51	if (!atomic_inc_not_zero(&sk->sk_refcnt))
	52	return;
c1f19b51	53
b9c701ed SS	54	clone = skb_clone(skb, GFP_ATOMIC);
	55	if (!clone) {
	56	sock_put(sk);
	57	return;
c1f19b51	58	}
b9c701ed SS	59
	60	clone->sk = sk;
	61	phydev->drv->txtstamp(phydev, clone, type);
c1f19b51 RC	62	}
c1f19b51 RC	63	}
1c17216e	64	EXPORT_SYMBOL_GPL(skb_clone_tx_timestamp);
c1f19b51 RC	65
	66	void skb_complete_tx_timestamp(struct sk_buff *skb,
	67	struct skb_shared_hwtstamps *hwtstamps)
	68	{
	69	struct sock *sk = skb->sk;
	70	struct sock_exterr_skb *serr;
	71	int err;
	72
da92b194 RC	73	if (!hwtstamps) {
	74	sock_put(sk);
	75	kfree_skb(skb);
c1f19b51	76	return;
da92b194	77	}
c1f19b51 RC	78
c1f19b51 RC	79	skb_hwtstamps(skb) = hwtstamps;
e62d2df0	80
c1f19b51 RC	81	serr = SKB_EXT_ERR(skb);
	82	memset(serr, 0, sizeof(*serr));
	83	serr->ee.ee_errno = ENOMSG;
	84	serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING;
	85	skb->sk = NULL;
e62d2df0	86
c1f19b51	87	err = sock_queue_err_skb(sk, skb);
e62d2df0	88
da92b194	89	sock_put(sk);
c1f19b51 RC	90	if (err)
	91	kfree_skb(skb);
	92	}
	93	EXPORT_SYMBOL_GPL(skb_complete_tx_timestamp);
	94
	95	bool skb_defer_rx_timestamp(struct sk_buff *skb)
	96	{
	97	struct phy_device *phydev;
	98	unsigned int type;
	99
a19faf02 ED	100	if (skb_headroom(skb) < ETH_HLEN)
	101	return false;
	102	__skb_push(skb, ETH_HLEN);
c1f19b51 RC	103
	104	type = classify(skb);
	105
a19faf02	106	__skb_pull(skb, ETH_HLEN);
c1f19b51	107
b9c701ed SS	108	if (type == PTP_CLASS_NONE)
	109	return false;
	110
	111	phydev = skb->dev->phydev;
	112	if (likely(phydev->drv->rxtstamp))
	113	return phydev->drv->rxtstamp(phydev, skb, type);
c1f19b51 RC	114
	115	return false;
	116	}
238442f6	117	EXPORT_SYMBOL_GPL(skb_defer_rx_timestamp);