Loading...
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 | /*
* PTP 1588 clock support - support for timestamping in PHY devices
*
* Copyright (C) 2010 OMICRON electronics GmbH
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include <linux/errqueue.h>
#include <linux/phy.h>
#include <linux/ptp_classify.h>
#include <linux/skbuff.h>
#include <linux/export.h>
static struct sock_filter ptp_filter[] = {
PTP_FILTER
};
static unsigned int classify(const struct sk_buff *skb)
{
if (likely(skb->dev &&
skb->dev->phydev &&
skb->dev->phydev->drv))
return sk_run_filter(skb, ptp_filter);
else
return PTP_CLASS_NONE;
}
void skb_clone_tx_timestamp(struct sk_buff *skb)
{
struct phy_device *phydev;
struct sk_buff *clone;
struct sock *sk = skb->sk;
unsigned int type;
if (!sk)
return;
type = classify(skb);
switch (type) {
case PTP_CLASS_V1_IPV4:
case PTP_CLASS_V1_IPV6:
case PTP_CLASS_V2_IPV4:
case PTP_CLASS_V2_IPV6:
case PTP_CLASS_V2_L2:
case PTP_CLASS_V2_VLAN:
phydev = skb->dev->phydev;
if (likely(phydev->drv->txtstamp)) {
if (!atomic_inc_not_zero(&sk->sk_refcnt))
return;
clone = skb_clone(skb, GFP_ATOMIC);
if (!clone) {
sock_put(sk);
return;
}
clone->sk = sk;
phydev->drv->txtstamp(phydev, clone, type);
}
break;
default:
break;
}
}
EXPORT_SYMBOL_GPL(skb_clone_tx_timestamp);
void skb_complete_tx_timestamp(struct sk_buff *skb,
struct skb_shared_hwtstamps *hwtstamps)
{
struct sock *sk = skb->sk;
struct sock_exterr_skb *serr;
int err;
if (!hwtstamps) {
sock_put(sk);
kfree_skb(skb);
return;
}
*skb_hwtstamps(skb) = *hwtstamps;
serr = SKB_EXT_ERR(skb);
memset(serr, 0, sizeof(*serr));
serr->ee.ee_errno = ENOMSG;
serr->ee.ee_origin = SO_EE_ORIGIN_TIMESTAMPING;
skb->sk = NULL;
err = sock_queue_err_skb(sk, skb);
sock_put(sk);
if (err)
kfree_skb(skb);
}
EXPORT_SYMBOL_GPL(skb_complete_tx_timestamp);
bool skb_defer_rx_timestamp(struct sk_buff *skb)
{
struct phy_device *phydev;
unsigned int type;
if (skb_headroom(skb) < ETH_HLEN)
return false;
__skb_push(skb, ETH_HLEN);
type = classify(skb);
__skb_pull(skb, ETH_HLEN);
switch (type) {
case PTP_CLASS_V1_IPV4:
case PTP_CLASS_V1_IPV6:
case PTP_CLASS_V2_IPV4:
case PTP_CLASS_V2_IPV6:
case PTP_CLASS_V2_L2:
case PTP_CLASS_V2_VLAN:
phydev = skb->dev->phydev;
if (likely(phydev->drv->rxtstamp))
return phydev->drv->rxtstamp(phydev, skb, type);
break;
default:
break;
}
return false;
}
EXPORT_SYMBOL_GPL(skb_defer_rx_timestamp);
void __init skb_timestamping_init(void)
{
BUG_ON(sk_chk_filter(ptp_filter, ARRAY_SIZE(ptp_filter)));
}
|