From e757b7a9d41feb485614c76e4843fde5566df895 Mon Sep 17 00:00:00 2001
From: Richard Cochran <richardcochran@gmail.com>
Date: Fri, 27 Jul 2012 12:00:47 +0200
Subject: [PATCH] Add a configuration option to support 802.1AS only hardware.

Some of the time stamping hardware out there only recognizes layer 2
packets, and these do not work without changing the receive filter in
the SIOCSHWTSTAMP request.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
---
 config.c    | 4 ++++
 config.h    | 1 +
 default.cfg | 1 +
 ptp4l.c     | 3 ++-
 sk.c        | 6 ++++--
 sk.h        | 6 ++++++
 6 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/config.c b/config.c
index efa76f2..faa0cfe 100644
--- a/config.c
+++ b/config.c
@@ -92,6 +92,10 @@ static void scan_line(char *s, struct config *cfg)
 		if (val > 0)
 			*cfg->tx_timestamp_retries = val;
 
+	} else if (1 == sscanf(s, " rx_timestamp_l2only %u", &val)) {
+
+		*cfg->rx_timestamp_l2only = val ? 1 : 0;
+
 	} else if (1 == sscanf(s, " pi_proportional_const %lf", &df)) {
 
 		if (df > 0.0 && df < 1.0)
diff --git a/config.h b/config.h
index 1c278f1..8fde9c1 100644
--- a/config.h
+++ b/config.h
@@ -26,6 +26,7 @@ struct config {
 	struct defaultDS *dds;
 	struct port_defaults *pod;
 	int *tx_timestamp_retries;
+	int *rx_timestamp_l2only;
 	double *pi_proportional_const;
 	double *pi_integral_const;
 	unsigned char *ptp_dst_mac;
diff --git a/default.cfg b/default.cfg
index b92c682..0f245eb 100644
--- a/default.cfg
+++ b/default.cfg
@@ -20,6 +20,7 @@ announceReceiptTimeout	3
 # Run time options
 #
 tx_timestamp_retries	2
+rx_timestamp_l2only	0
 pi_proportional_const	0.0
 pi_integral_const	0.0
 #
diff --git a/ptp4l.c b/ptp4l.c
index cb29eb3..dbb8333 100644
--- a/ptp4l.c
+++ b/ptp4l.c
@@ -30,7 +30,7 @@
 
 #define DEFAULT_PHC "/dev/ptp0"
 
-int sk_tx_retries = 2; /*see sk.c*/
+int sk_tx_retries = 2, sk_prefer_layer2 = 0; /*see sk.c*/
 double configured_pi_kp, configured_pi_ki; /*see pi.c*/
 extern unsigned char ptp_dst_mac[]; /*see raw.c*/
 extern unsigned char p2p_dst_mac[]; /*see raw.c*/
@@ -219,6 +219,7 @@ int main(int argc, char *argv[])
 	cfg_settings.dds = &ds;
 	cfg_settings.pod = &pod;
 	cfg_settings.tx_timestamp_retries = &sk_tx_retries;
+	cfg_settings.rx_timestamp_l2only = &sk_prefer_layer2;
 	cfg_settings.pi_proportional_const = &configured_pi_kp;
 	cfg_settings.pi_integral_const = &configured_pi_ki;
 	cfg_settings.ptp_dst_mac = ptp_dst_mac;
diff --git a/sk.c b/sk.c
index 333fcd9..577ace0 100644
--- a/sk.c
+++ b/sk.c
@@ -45,7 +45,8 @@ static int hwts_init(int fd, char *device)
 
 	ifreq.ifr_data = (void *) &cfg;
 	cfg.tx_type    = HWTSTAMP_TX_ON;
-	cfg.rx_filter  = HWTSTAMP_FILTER_PTP_V2_EVENT;
+	cfg.rx_filter  = sk_prefer_layer2 ?
+		HWTSTAMP_FILTER_PTP_V2_L2_EVENT : HWTSTAMP_FILTER_PTP_V2_EVENT;
 
 	req = cfg;
 	err = ioctl(fd, SIOCSHWTSTAMP, &ifreq);
@@ -59,7 +60,8 @@ static int hwts_init(int fd, char *device)
 		pr_warning("rx_filter %d not %d", cfg.rx_filter, req.rx_filter);
 
 		if (cfg.tx_type != HWTSTAMP_TX_ON ||
-		    cfg.rx_filter != HWTSTAMP_FILTER_ALL) {
+		    (cfg.rx_filter != HWTSTAMP_FILTER_ALL &&
+		     cfg.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)) {
 			return -1;
 		}
 	}
diff --git a/sk.h b/sk.h
index 14ade94..5f1ee4b 100644
--- a/sk.h
+++ b/sk.h
@@ -73,4 +73,10 @@ int sk_timestamping_init(int fd, char *device, enum timestamp_type type);
  */
 extern int sk_tx_retries;
 
+/**
+ * Request the hardware receive filter that is limited to layer 2 time
+ * stamping.
+ */
+extern int sk_prefer_layer2;
+
 #endif