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 struct sock_filter ptp_filter[] = { 27 PTP_FILTER 28 }; 29 30 static unsigned int classify(const struct sk_buff *skb) 31 { 32 if (likely(skb->dev && 33 skb->dev->phydev && 34 skb->dev->phydev->drv)) 35 return sk_run_filter(skb, ptp_filter); 36 else 37 return PTP_CLASS_NONE; 38 } 39 40 void skb_clone_tx_timestamp(struct sk_buff *skb) 41 { 42 struct phy_device *phydev; 43 struct sk_buff *clone; 44 struct sock *sk = skb->sk; 45 unsigned int type; 46 47 if (!sk) 48 return; 49 50 type = classify(skb); 51 52 switch (type) { 53 case PTP_CLASS_V1_IPV4: 54 case PTP_CLASS_V1_IPV6: 55 case PTP_CLASS_V2_IPV4: 56 case PTP_CLASS_V2_IPV6: 57 case PTP_CLASS_V2_L2: 58 case PTP_CLASS_V2_VLAN: 59 phydev = skb->dev->phydev; 60 if (likely(phydev->drv->txtstamp)) { 61 if (!atomic_inc_not_zero(&sk->sk_refcnt)) 62 return; 63 clone = skb_clone(skb, GFP_ATOMIC); 64 if (!clone) { 65 sock_put(sk); 66 return; 67 } 68 clone->sk = sk; 69 phydev->drv->txtstamp(phydev, clone, type); 70 } 71 break; 72 default: 73 break; 74 } 75 } 76 EXPORT_SYMBOL_GPL(skb_clone_tx_timestamp); 77 78 void skb_complete_tx_timestamp(struct sk_buff *skb, 79 struct skb_shared_hwtstamps *hwtstamps) 80 { 81 struct sock *sk = skb->sk; 82 struct sock_exterr_skb *serr; 83 int err; 84 85 if (!hwtstamps) { 86 sock_put(sk); 87 kfree_skb(skb); 88 return; 89 } 90 91 *skb_hwtstamps(skb) = *hwtstamps; 92 serr = SKB_EXT_ERR(skb); 93 memset(serr, 0, sizeof(*serr)); 94 serr->ee.ee_errno = ENOMSG; 95 serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING; 96 skb->sk = NULL; 97 err = sock_queue_err_skb(sk, skb); 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 136 void __init skb_timestamping_init(void) 137 { 138 BUG_ON(sk_chk_filter(ptp_filter, ARRAY_SIZE(ptp_filter))); 139 } 140