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 47 switch (type) { 48 case PTP_CLASS_V1_IPV4: 49 case PTP_CLASS_V1_IPV6: 50 case PTP_CLASS_V2_IPV4: 51 case PTP_CLASS_V2_IPV6: 52 case PTP_CLASS_V2_L2: 53 case PTP_CLASS_V2_VLAN: 54 phydev = skb->dev->phydev; 55 if (likely(phydev->drv->txtstamp)) { 56 if (!atomic_inc_not_zero(&sk->sk_refcnt)) 57 return; 58 59 clone = skb_clone(skb, GFP_ATOMIC); 60 if (!clone) { 61 sock_put(sk); 62 return; 63 } 64 65 clone->sk = sk; 66 phydev->drv->txtstamp(phydev, clone, type); 67 } 68 break; 69 default: 70 break; 71 } 72 } 73 EXPORT_SYMBOL_GPL(skb_clone_tx_timestamp); 74 75 void skb_complete_tx_timestamp(struct sk_buff *skb, 76 struct skb_shared_hwtstamps *hwtstamps) 77 { 78 struct sock *sk = skb->sk; 79 struct sock_exterr_skb *serr; 80 int err; 81 82 if (!hwtstamps) { 83 sock_put(sk); 84 kfree_skb(skb); 85 return; 86 } 87 88 *skb_hwtstamps(skb) = *hwtstamps; 89 90 serr = SKB_EXT_ERR(skb); 91 memset(serr, 0, sizeof(*serr)); 92 serr->ee.ee_errno = ENOMSG; 93 serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING; 94 skb->sk = NULL; 95 96 err = sock_queue_err_skb(sk, skb); 97 98 sock_put(sk); 99 if (err) 100 kfree_skb(skb); 101 } 102 EXPORT_SYMBOL_GPL(skb_complete_tx_timestamp); 103 104 bool skb_defer_rx_timestamp(struct sk_buff *skb) 105 { 106 struct phy_device *phydev; 107 unsigned int type; 108 109 if (skb_headroom(skb) < ETH_HLEN) 110 return false; 111 __skb_push(skb, ETH_HLEN); 112 113 type = classify(skb); 114 115 __skb_pull(skb, ETH_HLEN); 116 117 switch (type) { 118 case PTP_CLASS_V1_IPV4: 119 case PTP_CLASS_V1_IPV6: 120 case PTP_CLASS_V2_IPV4: 121 case PTP_CLASS_V2_IPV6: 122 case PTP_CLASS_V2_L2: 123 case PTP_CLASS_V2_VLAN: 124 phydev = skb->dev->phydev; 125 if (likely(phydev->drv->rxtstamp)) 126 return phydev->drv->rxtstamp(phydev, skb, type); 127 break; 128 default: 129 break; 130 } 131 132 return false; 133 } 134 EXPORT_SYMBOL_GPL(skb_defer_rx_timestamp); 135