[net-next-2.6.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>

static struct sock_filter ptp_filter[] = {
	PTP_FILTER
};

static unsigned int classify(struct sk_buff *skb)
{
	if (likely(skb->dev &&
		   skb->dev->phydev &&
		   skb->dev->phydev->drv))
		return sk_run_filter(skb, ptp_filter);
	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);

	switch (type) {
	case PTP_CLASS_V1_IPV4:
	case PTP_CLASS_V1_IPV6:
	case PTP_CLASS_V2_IPV4:
	case PTP_CLASS_V2_IPV6:
	case PTP_CLASS_V2_L2:
	case PTP_CLASS_V2_VLAN:
		phydev = skb->dev->phydev;
		if (likely(phydev->drv->txtstamp)) {
			clone = skb_clone(skb, GFP_ATOMIC);
			if (!clone)
				return;
			clone->sk = sk;
			phydev->drv->txtstamp(phydev, clone, type);
		}
		break;
	default:
		break;
	}
}

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)
		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);
	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;

	skb_push(skb, ETH_HLEN);

	type = classify(skb);

	skb_pull(skb, ETH_HLEN);

	switch (type) {
	case PTP_CLASS_V1_IPV4:
	case PTP_CLASS_V1_IPV6:
	case PTP_CLASS_V2_IPV4:
	case PTP_CLASS_V2_IPV6:
	case PTP_CLASS_V2_L2:
	case PTP_CLASS_V2_VLAN:
		phydev = skb->dev->phydev;
		if (likely(phydev->drv->rxtstamp))
			return phydev->drv->rxtstamp(phydev, skb, type);
		break;
	default:
		break;
	}

	return false;
}

void __init skb_timestamping_init(void)
{
	BUG_ON(sk_chk_filter(ptp_filter, ARRAY_SIZE(ptp_filter)));
}
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>
	24
	25	static struct sock_filter ptp_filter[] = {
	26	PTP_FILTER
	27	};
	28
	29	static unsigned int classify(struct sk_buff *skb)
	30	{
	31	if (likely(skb->dev &&
	32	skb->dev->phydev &&
	33	skb->dev->phydev->drv))
93aaae2e	34	return sk_run_filter(skb, ptp_filter);
c1f19b51 RC	35	else
	36	return PTP_CLASS_NONE;
	37	}
	38
	39	void skb_clone_tx_timestamp(struct sk_buff *skb)
	40	{
	41	struct phy_device *phydev;
	42	struct sk_buff *clone;
	43	struct sock *sk = skb->sk;
	44	unsigned int type;
	45
	46	if (!sk)
	47	return;
	48
	49	type = classify(skb);
	50
	51	switch (type) {
	52	case PTP_CLASS_V1_IPV4:
	53	case PTP_CLASS_V1_IPV6:
	54	case PTP_CLASS_V2_IPV4:
	55	case PTP_CLASS_V2_IPV6:
	56	case PTP_CLASS_V2_L2:
	57	case PTP_CLASS_V2_VLAN:
	58	phydev = skb->dev->phydev;
	59	if (likely(phydev->drv->txtstamp)) {
	60	clone = skb_clone(skb, GFP_ATOMIC);
	61	if (!clone)
	62	return;
	63	clone->sk = sk;
	64	phydev->drv->txtstamp(phydev, clone, type);
	65	}
	66	break;
	67	default:
	68	break;
	69	}
	70	}
	71
	72	void skb_complete_tx_timestamp(struct sk_buff *skb,
	73	struct skb_shared_hwtstamps *hwtstamps)
	74	{
	75	struct sock *sk = skb->sk;
	76	struct sock_exterr_skb *serr;
	77	int err;
	78
	79	if (!hwtstamps)
	80	return;
	81
	82	skb_hwtstamps(skb) = hwtstamps;
	83	serr = SKB_EXT_ERR(skb);
	84	memset(serr, 0, sizeof(*serr));
	85	serr->ee.ee_errno = ENOMSG;
	86	serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING;
	87	skb->sk = NULL;
	88	err = sock_queue_err_skb(sk, skb);
	89	if (err)
	90	kfree_skb(skb);
	91	}
	92	EXPORT_SYMBOL_GPL(skb_complete_tx_timestamp);
	93
	94	bool skb_defer_rx_timestamp(struct sk_buff *skb)
	95	{
	96	struct phy_device *phydev;
	97	unsigned int type;
	98
99	skb_push(skb, ETH_HLEN);
100
101	type = classify(skb);
102
103	skb_pull(skb, ETH_HLEN);
104
105	switch (type) {
106	case PTP_CLASS_V1_IPV4:
107	case PTP_CLASS_V1_IPV6:
108	case PTP_CLASS_V2_IPV4:
109	case PTP_CLASS_V2_IPV6:
110	case PTP_CLASS_V2_L2:
111	case PTP_CLASS_V2_VLAN:
112	phydev = skb->dev->phydev;
113	if (likely(phydev->drv->rxtstamp))
114	return phydev->drv->rxtstamp(phydev, skb, type);
115	break;
116	default:
117	break;
118	}
119
120	return false;
121	}
122
123	void __init skb_timestamping_init(void)
124	{
125	BUG_ON(sk_chk_filter(ptp_filter, ARRAY_SIZE(ptp_filter)));
126	}