/* * 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))); }