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 72 void skb_complete_tx_timestamp(struct sk_buff *skb, 73 struct skb_shared_hwtstamps *hwtstamps) 74 { 75 struct sock *sk = skb->sk; 76 struct sock_exterr_skb *serr; 77 int err; 78 79 if (!hwtstamps) 80 return; 81 82 *skb_hwtstamps(skb) = *hwtstamps; 83 serr = SKB_EXT_ERR(skb); 84 memset(serr, 0, sizeof(*serr)); 85 serr->ee.ee_errno = ENOMSG; 86 serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING; 87 skb->sk = NULL; 88 err = sock_queue_err_skb(sk, skb); 89 if (err) 90 kfree_skb(skb); 91 } 92 EXPORT_SYMBOL_GPL(skb_complete_tx_timestamp); 93 94 bool skb_defer_rx_timestamp(struct sk_buff *skb) 95 { 96 struct phy_device *phydev; 97 unsigned int type; 98 99 if (skb_headroom(skb) < ETH_HLEN) 100 return false; 101 __skb_push(skb, ETH_HLEN); 102 103 type = classify(skb); 104 105 __skb_pull(skb, ETH_HLEN); 106 107 switch (type) { 108 case PTP_CLASS_V1_IPV4: 109 case PTP_CLASS_V1_IPV6: 110 case PTP_CLASS_V2_IPV4: 111 case PTP_CLASS_V2_IPV6: 112 case PTP_CLASS_V2_L2: 113 case PTP_CLASS_V2_VLAN: 114 phydev = skb->dev->phydev; 115 if (likely(phydev->drv->rxtstamp)) 116 return phydev->drv->rxtstamp(phydev, skb, type); 117 break; 118 default: 119 break; 120 } 121 122 return false; 123 } 124 125 void __init skb_timestamping_init(void) 126 { 127 BUG_ON(sk_chk_filter(ptp_filter, ARRAY_SIZE(ptp_filter))); 128 } 129