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 #include <linux/export.h> 25 26 static unsigned int classify(const struct sk_buff *skb) 27 { 28 if (likely(skb->dev && skb->dev->phydev && 29 skb->dev->phydev->drv)) 30 return ptp_classify_raw(skb); 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); 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; 53 54 clone = skb_clone(skb, GFP_ATOMIC); 55 if (!clone) { 56 sock_put(sk); 57 return; 58 } 59 60 clone->sk = sk; 61 phydev->drv->txtstamp(phydev, clone, type); 62 } 63 } 64 EXPORT_SYMBOL_GPL(skb_clone_tx_timestamp); 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 73 if (!hwtstamps) { 74 sock_put(sk); 75 kfree_skb(skb); 76 return; 77 } 78 79 *skb_hwtstamps(skb) = *hwtstamps; 80 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; 86 87 err = sock_queue_err_skb(sk, skb); 88 89 sock_put(sk); 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 100 if (skb_headroom(skb) < ETH_HLEN) 101 return false; 102 __skb_push(skb, ETH_HLEN); 103 104 type = classify(skb); 105 106 __skb_pull(skb, ETH_HLEN); 107 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); 114 115 return false; 116 } 117 EXPORT_SYMBOL_GPL(skb_defer_rx_timestamp); 118