/* * 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 struct sock_filter ptp_filter[] = {	PTP_FILTER}; static unsigned int classify(const 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)) {			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);		}		break;	default:		break;	}}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); 	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;}EXPORT_SYMBOL_GPL(skb_defer_rx_timestamp); void __init skb_timestamping_init(void){	BUG_ON(sk_chk_filter(ptp_filter, ARRAY_SIZE(ptp_filter)));}