commit dccaa9e091d2fd658634de31a1ab272072759cda
author Richard Cochran <richardcochran@gmail.com> Tue Sep 20 01:43:16 2011 +0000
committer David S. Miller <davem@davemloft.net> Mon Sep 26 16:02:47 2011 -0400
tree 38548ebcbccb061c0ca22dff5275b1f0426b7a8f
parent 3ce23fa9780b70525932c5e4b5ac401c67390fae

dp83640: add time stamp insertion for sync messages

This commit adds one step support to the phyter. When enabled, the
hardware does not provide time stamps for transmitted sync messages but
instead inserts the stamp into the outgoing packet.

Signed-off-by: Richard Cochran <richard.cochran@omicron.at>
Signed-off-by: David S. Miller <davem@davemloft.net>

drivers/net/phy/dp83640.c

diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c
index f999379..be381c2 100644
--- a/drivers/net/phy/dp83640.c
+++ b/drivers/net/phy/dp83640.c
@@ -761,6 +761,41 @@
 	}
 }
 
+static int is_sync(struct sk_buff *skb, int type)
+{
+	u8 *data = skb->data, *msgtype;
+	unsigned int offset = 0;
+
+	switch (type) {
+	case PTP_CLASS_V1_IPV4:
+	case PTP_CLASS_V2_IPV4:
+		offset = ETH_HLEN + IPV4_HLEN(data) + UDP_HLEN;
+		break;
+	case PTP_CLASS_V1_IPV6:
+	case PTP_CLASS_V2_IPV6:
+		offset = OFF_PTP6;
+		break;
+	case PTP_CLASS_V2_L2:
+		offset = ETH_HLEN;
+		break;
+	case PTP_CLASS_V2_VLAN:
+		offset = ETH_HLEN + VLAN_HLEN;
+		break;
+	default:
+		return 0;
+	}
+
+	if (type & PTP_CLASS_V1)
+		offset += OFF_PTP_CONTROL;
+
+	if (skb->len < offset + 1)
+		return 0;
+
+	msgtype = data + offset;
+
+	return (*msgtype & 0xf) == 0;
+}
+
 static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts)
 {
 	u16 *seqid;
@@ -1010,16 +1045,10 @@
 	if (cfg.flags) /* reserved for future extensions */
 		return -EINVAL;
 
-	switch (cfg.tx_type) {
-	case HWTSTAMP_TX_OFF:
-		dp83640->hwts_tx_en = 0;
-		break;
-	case HWTSTAMP_TX_ON:
-		dp83640->hwts_tx_en = 1;
-		break;
-	default:
+	if (cfg.tx_type < 0 || cfg.tx_type > HWTSTAMP_TX_ONESTEP_SYNC)
 		return -ERANGE;
-	}
+
+	dp83640->hwts_tx_en = cfg.tx_type;
 
 	switch (cfg.rx_filter) {
 	case HWTSTAMP_FILTER_NONE:
@@ -1074,6 +1103,9 @@
 	if (dp83640->hwts_tx_en)
 		txcfg0 |= TX_TS_EN;
 
+	if (dp83640->hwts_tx_en == HWTSTAMP_TX_ONESTEP_SYNC)
+		txcfg0 |= SYNC_1STEP | CHK_1STEP;
+
 	if (dp83640->hwts_rx_en)
 		rxcfg0 |= RX_TS_EN;
 
@@ -1156,12 +1188,24 @@
 {
 	struct dp83640_private *dp83640 = phydev->priv;
 
-	if (!dp83640->hwts_tx_en) {
+	switch (dp83640->hwts_tx_en) {
+
+	case HWTSTAMP_TX_ONESTEP_SYNC:
+		if (is_sync(skb, type)) {
+			kfree_skb(skb);
+			return;
+		}
+		/* fall through */
+	case HWTSTAMP_TX_ON:
+		skb_queue_tail(&dp83640->tx_queue, skb);
+		schedule_work(&dp83640->ts_work);
+		break;
+
+	case HWTSTAMP_TX_OFF:
+	default:
 		kfree_skb(skb);
-		return;
+		break;
 	}
-	skb_queue_tail(&dp83640->tx_queue, skb);
-	schedule_work(&dp83640->ts_work);
 }
 
 static struct phy_driver dp83640_driver = {