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 25 static struct sock_filter ptp_filter[] = { 26 PTP_FILTER 27 }; 28 29 static unsigned int classify(const struct sk_buff *skb) 30 { 31 if (likely(skb->dev && 32 skb->dev->phydev && 33 skb->dev->phydev->drv)) 34 return sk_run_filter(skb, ptp_filter); 35 else 36 return PTP_CLASS_NONE; 37 } 38 39 void skb_clone_tx_timestamp(struct sk_buff *skb) 40 { 41 struct phy_device *phydev; 42 struct sk_buff *clone; 43 struct sock *sk = skb->sk; 44 unsigned int type; 45 46 if (!sk) 47 return; 48 49 type = classify(skb); 50 51 switch (type) { 52 case PTP_CLASS_V1_IPV4: 53 case PTP_CLASS_V1_IPV6: 54 case PTP_CLASS_V2_IPV4: 55 case PTP_CLASS_V2_IPV6: 56 case PTP_CLASS_V2_L2: 57 case PTP_CLASS_V2_VLAN: 58 phydev = skb->dev->phydev; 59 if (likely(phydev->drv->txtstamp)) { 60 clone = skb_clone(skb, GFP_ATOMIC); 61 if (!clone) 62 return; 63 clone->sk = sk; 64 phydev->drv->txtstamp(phydev, clone, type); 65 } 66 break; 67 default: 68 break; 69 } 70 } 71 EXPORT_SYMBOL_GPL(skb_clone_tx_timestamp); 72 73 void skb_complete_tx_timestamp(struct sk_buff *skb, 74 struct skb_shared_hwtstamps *hwtstamps) 75 { 76 struct sock *sk = skb->sk; 77 struct sock_exterr_skb *serr; 78 int err; 79 80 if (!hwtstamps) 81 return; 82 83 *skb_hwtstamps(skb) = *hwtstamps; 84 serr = SKB_EXT_ERR(skb); 85 memset(serr, 0, sizeof(*serr)); 86 serr->ee.ee_errno = ENOMSG; 87 serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING; 88 skb->sk = NULL; 89 err = sock_queue_err_skb(sk, skb); 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 switch (type) { 109 case PTP_CLASS_V1_IPV4: 110 case PTP_CLASS_V1_IPV6: 111 case PTP_CLASS_V2_IPV4: 112 case PTP_CLASS_V2_IPV6: 113 case PTP_CLASS_V2_L2: 114 case PTP_CLASS_V2_VLAN: 115 phydev = skb->dev->phydev; 116 if (likely(phydev->drv->rxtstamp)) 117 return phydev->drv->rxtstamp(phydev, skb, type); 118 break; 119 default: 120 break; 121 } 122 123 return false; 124 } 125 EXPORT_SYMBOL_GPL(skb_defer_rx_timestamp); 126 127 void __init skb_timestamping_init(void) 128 { 129 BUG_ON(sk_chk_filter(ptp_filter, ARRAY_SIZE(ptp_filter))); 130 } 131