[PATCH net-next v7 0/8] hsr: Add additional info to send/ receive skbs

[PATCH net-next v7 0/8] hsr: Add additional info to send/ receive skbs

[Sebastian Andrzej Siewior]

I am trying to extend linuxptp to support PTP over a HSR network.

This is the kernel side of the changes. In short PTP over HSR sends its
packets to a multicast address and every node needs to forward the PTP
packet (SYNC and FOLLOW-UP for instance) within the HSR ring.
In order to achieve this, the HSR stack must not duplicate and forward
the PTP packets as it would do with other packets. The delay caused by
the duplication and forwarding adds overhead which in turn makes the
timing information within the PTP packet inaccurate.

My current approach is to open the slave devices (eth0/ eth1) from
userland in order to receive the PTP packets. Sending happens from the
hsr0 device. The actual packet has an inline header prepended of type
struct hsr_inline_header. The size of the header is equivalent to
ethhdr. The header has a type (h_proto) at the same position as ethhdr
and expects it to be ETH_P_1588 as this extra meta information is only
relevant for PTP packets. It makes no sense to send PTP packets via the
HSR interface because it gets duplicated and the timestamp information
is lost so this should not break anything. As an additional safe guard
there is a magic value at h_source position. The value has '0xaf' at the
most significant byte which makes the address a locally administered
multicast address.

The header passes two information from userland: On which slave port
the packet has to be sent and does the HSR stack need to prepend a
header or not.
The header is skipped so that the remaining stack sees the actual data
and can send it as requested.

The PRP packets are sent directly via the SLAVE interface. The standard
mandates not add a PRP trailer (PRP, redundancy control trailer) to PTP
packets. There is not really a reason to use hsr interface.

HSR hardware offloading is optional. The driver needs to know if the
operating mode is HSR or PRP. In PRP mode it needs to check the ether
type and for ETH_P_1588 it must not perform any offloading.
In HSR mode, for ether-type ETH_P_1588 there must be no offloading. If
the ether-type is ETH_P_HSR there must be no offloading if the
encapsulated protocol is ETH_P_1588.

This has been tested in a pure software environment and in an HW-assisted
environment where the HW is able to duplicate and duplicate packets
but does not do it for PTP packets.
It has not been tested within an environment where the HW is able to
forward the PTP packet and correctly update the timing information.

---
v6…v7: https://patch.msgid.link/20260923-hsr_ptp-v6-0-6ea07b3fb8a8@linutronix.de
- Change the logic in hsr_create_tagged_frame():
  - Don't update ->csum_start (the cloning takes care of this)
  - pskb_may_pull() does not need to include HSR_HLEN (it only copies
    'movelen')
  - Set network header to 'movelen' which takes VLAN into account
- Drop also PTP packets which were received on HSR_PT_INTERLINK (not
  just HSR A/B).
- Move include HSR header to include/uapi/linux/hsr_ptp.h, so it is also
  available in userland.
- selftests:
  - Add hsr_ptp.sh alphabetically ordered to the Makefile
  - Use "$0" in hsr_ptp.sh
  - Drop unnused arguments in open_socket()

v5…v6: https://lore.kernel.org/r/20260527-hsr_ptp-v5-0-158a7633eac0@linutronix.de
- hsr_ptp_test: Let it wait up to 100ms for a packet. Otherwise it will
  complain if the recevied packet is not already in socket.
- Use TEST_GEN_PROGS instead TEST_GEN_FILES, the test can not run
  without additional arguments.
- Make the inline header mandatory for sending ETH_P_1588 packets via
  the HSR device.
- Use kfree_skb() instead kfree() for the skb.
- Rephrase the commit message saying that skb_clone() shares
  skb_shared_info and does not copy it.

v4…v5: https://lore.kernel.org/r/20260508-hsr_ptp-v4-1-aa19aa7c6a71@linutronix.de
- Split the patch into smaller pieces
- Added a test for the added inline header (which signals the port while
  sending packets).
- Replaced __pskb_copy() with skb_clone() + skb_cow_head() in
  hsr_create_tagged_frame() to preserve timestamp request.

v3…v4: https://lore.kernel.org/r/20260429-hsr_ptp-v3-1-afbf8f200f48@linutronix.de
- Removed skb extention. The information within HSR is passed via
  struct hsr_frame_info. Driver with HSR-offloading capabilities need to
  know the HSR mode (HSR or PRP) and parse the skb to decide what needs
  to be done (whether to send on both ports and if adding a header is
  needed).

v2…v3: https://patch.msgid.link/20260309-hsr_ptp-v2-0-798262aad3a4@linutronix.de
- Remove af_packet changes entirely.
- Add an internal header to pass additional information for HSR-PTP
  packets.
- Remove PRP, userland will use slave devices directly.
- Drop all received PTP packets. Userland needs to use the slave device
  for RX.

v1…v2: https://patch.msgid.link/20260204-hsr_ptp-v1-0-b421c69a77da@linutronix.de
- Added PRP support
- skb extention is used instead of extending struct skb_shared_info
- in af_packet
  - packet_sendmsg_spkt() is no longer extended
  - jump labels are used to avoid the overhead if there no socket that
    is using this HSR extension.

---

---
Sebastian Andrzej Siewior (8):
      hsr: Add header_ops::parse_protocol
      hsr: Use skb_clone() while adding the HSR header
      hsr: Add a magic header for sending PTP packets
      hsr: Drop received PTP packets
      hsr: Use the port and header information in hsr_forward_skb()
      hsr: Assign a socket for cloned skbs
      hsr: Move struct hsr_ethhdr to a global header
      selftests: hsr: Add test for the inline PTP header on HSR

 MAINTAINERS                                    |   1 +
 include/linux/if_hsr.h                         |   6 +
 include/uapi/linux/hsr_ptp.h                   |  18 +
 net/hsr/hsr_device.c                           |  67 +++-
 net/hsr/hsr_forward.c                          |  78 +++-
 net/hsr/hsr_forward.h                          |   3 +-
 net/hsr/hsr_framereg.h                         |   2 +
 net/hsr/hsr_main.h                             |   5 -
 net/hsr/hsr_slave.c                            |  34 +-
 tools/include/uapi/linux/hsr_ptp.h             |  18 +
 tools/testing/selftests/net/hsr/.gitignore     |   1 +
 tools/testing/selftests/net/hsr/Makefile       |   5 +
 tools/testing/selftests/net/hsr/hsr_ptp.sh     | 109 ++++++
 tools/testing/selftests/net/hsr/hsr_ptp_test.c | 482 +++++++++++++++++++++++++
 14 files changed, 784 insertions(+), 45 deletions(-)
---
base-commit: 5a956dde5526a634dca7ccad27c051ebcc306089
change-id: 20260204-hsr_ptp-1f6380f1d35f

Best regards,
--  
Sebastian Andrzej Siewior <bigeasy@linutronix.de>

[PATCH net-next v7 1/8] hsr: Add header_ops::parse_protocol

[Sebastian Andrzej Siewior]

af_packet uses dev_parse_header_protocol() to assign skb::protocol.
The hsr stack does not assign a callback so the field is assigned to 0.

The header used by the hsr stack corresponds to the ethernet header for
the "first" part so the protocol member is at the same position.

Use eth_header_parse_protocol() for the hsr_header_ops's parse_protocol
callback.

Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
---
 net/hsr/hsr_device.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/net/hsr/hsr_device.c b/net/hsr/hsr_device.c
index d14de44e14b7a..6d96bc4a12446 100644
--- a/net/hsr/hsr_device.c
+++ b/net/hsr/hsr_device.c
@@ -247,6 +247,7 @@ static netdev_tx_t hsr_dev_xmit(struct sk_buff *skb, struct net_device *dev)
 static const struct header_ops hsr_header_ops = {
 	.create	 = eth_header,
 	.parse	 = eth_header_parse,
+	.parse_protocol = eth_header_parse_protocol,
 };
 
 static struct sk_buff *hsr_init_skb(struct hsr_port *master, int extra)

-- 
2.55.0

[PATCH net-next v7 2/8] hsr: Use skb_clone() while adding the HSR header

[Sebastian Andrzej Siewior]

hsr_create_tagged_frame() creates a new skb where the HSR header can be
prepended via __pskb_copy(). This does not copy the skb_shared_info part
of the original skb so any timestamp request is lost.

An alternative is to use skb_clone() where the skb_shared_info part is
shared and then skb_cow_head() to make the header part writeable. This
avoids touching manually the tx_flags and tskey members.
Using skb_clone() allows also return early (after the clone) in the
offloaded case so this can be unified a bit.

This is a prepartion for PTP handling where timestatmps are requested.

Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
---
 net/hsr/hsr_forward.c | 24 ++++++++++++++++--------
 1 file changed, 16 insertions(+), 8 deletions(-)

diff --git a/net/hsr/hsr_forward.c b/net/hsr/hsr_forward.c
index 7734a521a96c2..6c0b1f467940d 100644
--- a/net/hsr/hsr_forward.c
+++ b/net/hsr/hsr_forward.c
@@ -342,33 +342,41 @@ struct sk_buff *hsr_create_tagged_frame(struct hsr_frame_info *frame,
 		/* set the lane id properly */
 		hsr_set_path_id(frame, hsr_ethhdr, port);
 		return skb_clone(frame->skb_hsr, GFP_ATOMIC);
-	} else if (port->dev->features & NETIF_F_HW_HSR_TAG_INS) {
-		return skb_clone(frame->skb_std, GFP_ATOMIC);
 	}
 
-	/* Create the new skb with enough headroom to fit the HSR tag */
-	skb = __pskb_copy(frame->skb_std,
-			  skb_headroom(frame->skb_std) + HSR_HLEN, GFP_ATOMIC);
+	skb = skb_clone(frame->skb_std, GFP_ATOMIC);
 	if (!skb)
 		return NULL;
-	skb_reset_mac_header(skb);
 
-	if (skb->ip_summed == CHECKSUM_PARTIAL)
-		skb->csum_start += HSR_HLEN;
+	if (port->dev->features & NETIF_F_HW_HSR_TAG_INS)
+		return skb;
+
+	/* Ensure the cloned skb has enough headroom to fit the HSR tag */
+	if (skb_cow_head(skb, HSR_HLEN))
+		goto err;
 
 	movelen = ETH_HLEN;
 	if (frame->is_vlan)
 		movelen += VLAN_HLEN;
 
+	/* The entire header area needs to be linear */
+	if (!pskb_may_pull(skb, movelen))
+		goto err;
+
 	src = skb_mac_header(skb);
 	dst = skb_push(skb, HSR_HLEN);
 	memmove(dst, src, movelen);
+	skb_set_network_header(skb, HSR_HLEN + movelen);
 	skb_reset_mac_header(skb);
+	skb_reset_mac_len(skb);
 
 	/* skb_put_padto free skb on error and hsr_fill_tag returns NULL in
 	 * that case
 	 */
 	return hsr_fill_tag(skb, frame, port, port->hsr->prot_version);
+err:
+	kfree_skb(skb);
+	return NULL;
 }
 
 struct sk_buff *prp_create_tagged_frame(struct hsr_frame_info *frame,

-- 
2.55.0

[PATCH net-next v7 3/8] hsr: Add a magic header for sending PTP packets

[Sebastian Andrzej Siewior]

Sending PTP packets (ETH_P_1588) via the HSR stack is pointless in its
current shape because the requested PTP timestamp is not routed to the
sender. It also needs to be distinguished on which port the message
should be sent and whether or not a HSR header should be attached.

To pass this information, a custom header (struct hsr_inline_header) is
introduced. This header is expected and mandatory if the ether type is
ETH_P_1588. To avoid any wrong usage, there is a magic field to ensure
it really is the header.

The inline header is just prepended containing the port and header
parameter. After the header, a regular packet follows. It is ensured that
the inline header and the following ethernet or HSR header is linear
and can be accessed.

The two retrieved parameters are passed to hsr_forward_skb() and will be
used later.

Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
---
 MAINTAINERS                  |  1 +
 include/linux/if_hsr.h       |  1 +
 include/uapi/linux/hsr_ptp.h | 18 ++++++++++++
 net/hsr/hsr_device.c         | 66 ++++++++++++++++++++++++++++++++++++--------
 net/hsr/hsr_forward.c        |  3 +-
 net/hsr/hsr_forward.h        |  3 +-
 net/hsr/hsr_slave.c          |  4 +--
 7 files changed, 80 insertions(+), 16 deletions(-)

diff --git a/MAINTAINERS b/MAINTAINERS
index 72294ddfa5b75..0d8bf0fac5c4b 100644
--- a/MAINTAINERS
+++ b/MAINTAINERS
@@ -12023,6 +12023,7 @@ HSR NETWORK PROTOCOL
 L:	netdev@vger.kernel.org
 S:	Orphan
 F:	net/hsr/
+F:	include/uapi/linux/hsr_ptp.h
 F:	tools/testing/selftests/net/hsr/
 
 HT16K33 LED CONTROLLER DRIVER
diff --git a/include/linux/if_hsr.h b/include/linux/if_hsr.h
index f4cf2dd36d193..7b6b60ef592db 100644
--- a/include/linux/if_hsr.h
+++ b/include/linux/if_hsr.h
@@ -3,6 +3,7 @@
 #define _LINUX_IF_HSR_H_
 
 #include <linux/types.h>
+#include <uapi/linux/hsr_ptp.h>
 
 struct net_device;
 
diff --git a/include/uapi/linux/hsr_ptp.h b/include/uapi/linux/hsr_ptp.h
new file mode 100644
index 0000000000000..21ccd3defa6cd
--- /dev/null
+++ b/include/uapi/linux/hsr_ptp.h
@@ -0,0 +1,18 @@
+/* SPDX-License-Identifier: GPL-2.0+ WITH Linux-syscall-note */
+#ifndef __UAPI_HSR_PTP_H
+#define __UAPI_HSR_PTP_H
+
+#define HSR_INLINE_HDR  0xaf485352
+#define HSR_INLINE_HDR_PORT_A	1
+#define HSR_INLINE_HDR_PORT_B	2
+
+struct hsr_inline_header {
+	uint8_t tx_port;
+	uint8_t hsr_hdr;
+	uint8_t __pad0[4];
+	__be32 magic;
+	uint8_t __pad1[2];
+	__be16 eth_type;
+} __attribute__ ((packed));
+
+#endif
diff --git a/net/hsr/hsr_device.c b/net/hsr/hsr_device.c
index 6d96bc4a12446..c5ea7ef0209db 100644
--- a/net/hsr/hsr_device.c
+++ b/net/hsr/hsr_device.c
@@ -223,24 +223,66 @@ static netdev_features_t hsr_fix_features(struct net_device *dev,
 
 static netdev_tx_t hsr_dev_xmit(struct sk_buff *skb, struct net_device *dev)
 {
+	enum hsr_port_type tx_port = HSR_PT_NONE;
 	struct hsr_priv *hsr = netdev_priv(dev);
 	struct hsr_port *master;
+	bool has_header = false;
 
 	rcu_read_lock();
 	master = hsr_port_get_hsr(hsr, HSR_PT_MASTER);
-	if (master) {
-		skb->dev = master->dev;
-		skb_reset_mac_header(skb);
-		skb_reset_mac_len(skb);
-		spin_lock_bh(&hsr->seqnr_lock);
-		hsr_forward_skb(skb, master);
-		spin_unlock_bh(&hsr->seqnr_lock);
-	} else {
-		dev_core_stats_tx_dropped_inc(dev);
-		dev_kfree_skb_any(skb);
+	if (!master)
+		goto drop;
+
+	skb->dev = master->dev;
+	if (skb->protocol == htons(ETH_P_1588)) {
+		struct hsr_inline_header *hsr_opt;
+		struct ethhdr *eth_hdr;
+		unsigned int hdr_len;
+
+		BUILD_BUG_ON(sizeof(struct hsr_inline_header) != sizeof(struct ethhdr));
+
+		/* need to access the magic header */
+		if (!pskb_may_pull(skb, sizeof(struct hsr_inline_header)))
+			goto drop;
+
+		hsr_opt = (struct hsr_inline_header *)skb_mac_header(skb);
+		if (hsr_opt->magic != htonl(HSR_INLINE_HDR))
+			goto drop;
+
+		has_header = hsr_opt->hsr_hdr;
+		tx_port = hsr_opt->tx_port;
+		if (tx_port != HSR_PT_SLAVE_A && tx_port != HSR_PT_SLAVE_B)
+			goto drop;
+
+		if (has_header)
+			hdr_len = ETH_HLEN + HSR_HLEN;
+		else
+			hdr_len = ETH_HLEN;
+
+		/* Ensure the real header after the stripped one can be
+		 * accessed
+		 */
+		if (!pskb_may_pull(skb, sizeof(struct hsr_inline_header) +
+				   hdr_len))
+			goto drop;
+
+		eth_hdr = skb_pull(skb, sizeof(struct hsr_inline_header));
+		skb_set_network_header(skb, hdr_len);
+		skb->protocol = eth_hdr->h_proto;
 	}
+
+	skb_reset_mac_header(skb);
+	skb_reset_mac_len(skb);
+	spin_lock_bh(&hsr->seqnr_lock);
+	hsr_forward_skb(skb, master, tx_port, has_header);
+	spin_unlock_bh(&hsr->seqnr_lock);
 	rcu_read_unlock();
 
+	return NETDEV_TX_OK;
+drop:
+	rcu_read_unlock();
+	dev_core_stats_tx_dropped_inc(dev);
+	dev_kfree_skb_any(skb);
 	return NETDEV_TX_OK;
 }
 
@@ -362,7 +404,7 @@ static void send_hsr_supervision_frame(struct hsr_port *port,
 		return;
 	}
 
-	hsr_forward_skb(skb, port);
+	hsr_forward_skb(skb, port, HSR_PT_NONE, false);
 	spin_unlock_bh(&hsr->seqnr_lock);
 	return;
 }
@@ -430,7 +472,7 @@ static void send_prp_supervision_frame(struct hsr_port *master,
 		return;
 	}
 
-	hsr_forward_skb(skb, master);
+	hsr_forward_skb(skb, master, HSR_PT_NONE, false);
 	spin_unlock_bh(&hsr->seqnr_lock);
 }
 
diff --git a/net/hsr/hsr_forward.c b/net/hsr/hsr_forward.c
index 6c0b1f467940d..bf5b8c7fad306 100644
--- a/net/hsr/hsr_forward.c
+++ b/net/hsr/hsr_forward.c
@@ -773,7 +773,8 @@ static int fill_frame_info(struct hsr_frame_info *frame,
 }
 
 /* Must be called holding rcu read lock (because of the port parameter) */
-void hsr_forward_skb(struct sk_buff *skb, struct hsr_port *port)
+void hsr_forward_skb(struct sk_buff *skb, struct hsr_port *port,
+		     enum hsr_port_type tx_port, bool has_hsr_header)
 {
 	struct hsr_frame_info frame;
 
diff --git a/net/hsr/hsr_forward.h b/net/hsr/hsr_forward.h
index 206636750b300..e64b0358907a9 100644
--- a/net/hsr/hsr_forward.h
+++ b/net/hsr/hsr_forward.h
@@ -13,7 +13,8 @@
 #include <linux/netdevice.h>
 #include "hsr_main.h"
 
-void hsr_forward_skb(struct sk_buff *skb, struct hsr_port *port);
+void hsr_forward_skb(struct sk_buff *skb, struct hsr_port *port,
+		     enum hsr_port_type tx_port, bool has_hsr_header);
 struct sk_buff *prp_create_tagged_frame(struct hsr_frame_info *frame,
 					struct hsr_port *port);
 struct sk_buff *hsr_create_tagged_frame(struct hsr_frame_info *frame,
diff --git a/net/hsr/hsr_slave.c b/net/hsr/hsr_slave.c
index a546f70f9cc8b..5274ba6dd36e6 100644
--- a/net/hsr/hsr_slave.c
+++ b/net/hsr/hsr_slave.c
@@ -78,10 +78,10 @@ static rx_handler_result_t hsr_handle_frame(struct sk_buff **pskb)
 	 */
 	if (port->type == HSR_PT_INTERLINK) {
 		spin_lock_bh(&hsr->seqnr_lock);
-		hsr_forward_skb(skb, port);
+		hsr_forward_skb(skb, port, HSR_PT_NONE, false);
 		spin_unlock_bh(&hsr->seqnr_lock);
 	} else {
-		hsr_forward_skb(skb, port);
+		hsr_forward_skb(skb, port, HSR_PT_NONE, false);
 	}
 
 finish_consume:

-- 
2.55.0

Re: [PATCH net-next v7 3/8] hsr: Add a magic header for sending PTP packets

[Jakub Kicinski]

On Mon, 28 Sep 2026 14:39:37 +0200 Sebastian Andrzej Siewior wrote:
> Sending PTP packets (ETH_P_1588) via the HSR stack is pointless in its
> current shape because the requested PTP timestamp is not routed to the
> sender. It also needs to be distinguished on which port the message
> should be sent and whether or not a HSR header should be attached.
> 
> To pass this information, a custom header (struct hsr_inline_header) is
> introduced. This header is expected and mandatory if the ether type is
> ETH_P_1588. To avoid any wrong usage, there is a magic field to ensure
> it really is the header.
> 
> The inline header is just prepended containing the port and header
> parameter. After the header, a regular packet follows. It is ensured that
> the inline header and the following ethernet or HSR header is linear
> and can be accessed.
> 
> The two retrieved parameters are passed to hsr_forward_skb() and will be
> used later.
> 
> Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>

breaks build (admittedly in a weird way, some header check thing?):

In file included from <built-in>:1:
./usr/include/linux/hsr_ptp.h:10:2: error: unknown type name 'uint8_t'
   10 |         uint8_t tx_port;
      |         ^
./usr/include/linux/hsr_ptp.h:11:2: error: unknown type name 'uint8_t'
   11 |         uint8_t hsr_hdr;
      |         ^
./usr/include/linux/hsr_ptp.h:12:2: error: unknown type name 'uint8_t'
   12 |         uint8_t __pad0[4];
      |         ^
./usr/include/linux/hsr_ptp.h:13:2: error: unknown type name '__be32'
   13 |         __be32 magic;
      |         ^
./usr/include/linux/hsr_ptp.h:14:2: error: unknown type name 'uint8_t'
   14 |         uint8_t __pad1[2];
      |         ^
./usr/include/linux/hsr_ptp.h:15:2: error: unknown type name '__be16'
   15 |         __be16 eth_type;
      |         ^
-- 
pw-bot: cr

[PATCH net-next v7 4/8] hsr: Drop received PTP packets

[Sebastian Andrzej Siewior]

Receiving PTP packets via the HSR interface does not make sense. The HSR
stack will forward one copy to the user and ignore the duplicate from
the other port. The PTP stack is however interested in both copies since
they will have different content (due to different processing times
within the HSR ring) and different timestamp information which is not
forwarded at all.

Forwarding a PTP packet by the HSR stack is undesired because the
forwarding takes time, it is not accounted in the packet and this makes
the outgoing PTP packet inaccurate and therefore useless.

Drop all received PTP packets. For the PRP configuration it is the
ether type, for HSR configuration it is the encapsulated protocol, that
is checked. A PTP stack that is doing PTP over a HSR network needs to
retrieve the PTP packet on the original slave interface where it was
received.
VLAN tagged packets are not considered.

Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
---
 net/hsr/hsr_slave.c | 30 +++++++++++++++++++++++-------
 1 file changed, 23 insertions(+), 7 deletions(-)

diff --git a/net/hsr/hsr_slave.c b/net/hsr/hsr_slave.c
index 5274ba6dd36e6..a5b47f016abd7 100644
--- a/net/hsr/hsr_slave.c
+++ b/net/hsr/hsr_slave.c
@@ -23,6 +23,7 @@ bool hsr_invalid_dan_ingress_frame(__be16 protocol)
 
 static rx_handler_result_t hsr_handle_frame(struct sk_buff **pskb)
 {
+	struct hsr_ethhdr *hsr_ethhdr;
 	struct sk_buff *skb = *pskb;
 	struct hsr_port *port;
 	struct hsr_priv *hsr;
@@ -44,8 +45,7 @@ static rx_handler_result_t hsr_handle_frame(struct sk_buff **pskb)
 
 	if (hsr_addr_is_self(port->hsr, eth_hdr(skb)->h_source)) {
 		/* Directly kill frames sent by ourselves */
-		kfree_skb(skb);
-		goto finish_consume;
+		goto finish_free_consume;
 	}
 
 	/* For HSR, only tagged frames are expected (unless the device offloads
@@ -64,15 +64,28 @@ static rx_handler_result_t hsr_handle_frame(struct sk_buff **pskb)
 	skb_reset_mac_header(skb);
 	if ((!hsr->prot_version && protocol == htons(ETH_P_PRP)) ||
 	    protocol == htons(ETH_P_HSR)) {
-		if (!pskb_may_pull(skb, ETH_HLEN + HSR_HLEN)) {
-			kfree_skb(skb);
-			goto finish_consume;
-		}
+		if (!pskb_may_pull(skb, ETH_HLEN + HSR_HLEN))
+			goto finish_free_consume;
 
 		skb_set_network_header(skb, ETH_HLEN + HSR_HLEN);
 	}
 	skb_reset_mac_len(skb);
 
+	/* PTP packets are not supposed to be forwarded via HSR as-is. The
+	 * latency introduced by forwarding renders the time information
+	 * useless. Userland needs to capture the packet on the original
+	 * interface instead of hsr.
+	 */
+	if ((!hsr->prot_version && protocol == htons(ETH_P_PRP)) ||
+	    protocol == htons(ETH_P_HSR)) {
+		hsr_ethhdr = (struct hsr_ethhdr *)skb_mac_header(skb);
+		if (hsr_ethhdr->hsr_tag.encap_proto == htons(ETH_P_1588))
+			goto finish_free_consume;
+	} else {
+		if (protocol == htons(ETH_P_1588))
+			goto finish_free_consume;
+	}
+
 	/* Only the frames received over the interlink port will assign a
 	 * sequence number and require synchronisation vs other sender.
 	 */
@@ -84,7 +97,10 @@ static rx_handler_result_t hsr_handle_frame(struct sk_buff **pskb)
 		hsr_forward_skb(skb, port, HSR_PT_NONE, false);
 	}
 
-finish_consume:
+	return RX_HANDLER_CONSUMED;
+
+finish_free_consume:
+	kfree_skb(skb);
 	return RX_HANDLER_CONSUMED;
 
 finish_pass:

-- 
2.55.0

[PATCH net-next v7 5/8] hsr: Use the port and header information in hsr_forward_skb()

[Sebastian Andrzej Siewior]

The upper layer can submit a port on which the skb should be sent and
whether or not a HSR header should be added. Use this information while
forwarding the packet:
- If the port set, deliver it only on the requested port.
- If a header is already present then skip parts of the stack:
  - The packet originates from the master port and we don't want to
    assign a sequence number to it, it needs to preserve the original
    one.
  - The existing h_source must be used (and not replaced).
  - The node should already exist in the node_db if "other" packets are
    exchaned. If the node information is not yet existing then
    creating it makes no sense because it is used for deduplication of
    incoming packets and this is not used in the PTP case. If no other
    packets are exchanged then the node_db will age and then removed.

Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
---
 net/hsr/hsr_forward.c  | 45 ++++++++++++++++++++++++++++++++++++---------
 net/hsr/hsr_framereg.h |  2 ++
 2 files changed, 38 insertions(+), 9 deletions(-)

diff --git a/net/hsr/hsr_forward.c b/net/hsr/hsr_forward.c
index bf5b8c7fad306..2ce31b1da7b35 100644
--- a/net/hsr/hsr_forward.c
+++ b/net/hsr/hsr_forward.c
@@ -428,7 +428,7 @@ static void hsr_deliver_master(struct sk_buff *skb, struct net_device *dev,
 static int hsr_xmit(struct sk_buff *skb, struct hsr_port *port,
 		    struct hsr_frame_info *frame)
 {
-	if (frame->port_rcv->type == HSR_PT_MASTER) {
+	if (frame->port_rcv->type == HSR_PT_MASTER && !frame->has_foreign_header) {
 		hsr_addr_subst_dest(frame->node_src, skb, port);
 
 		/* Address substitution (IEC62439-3 pp 26, 50): replace mac
@@ -549,11 +549,12 @@ bool hsr_drop_frame(struct hsr_frame_info *frame, struct hsr_port *port)
 static void hsr_forward_do(struct hsr_frame_info *frame)
 {
 	struct hsr_port *port;
-	struct sk_buff *skb;
 	bool sent = false;
 
 	hsr_for_each_port(frame->port_rcv->hsr, port) {
 		struct hsr_priv *hsr = port->hsr;
+		struct sk_buff *skb = NULL;
+
 		/* Don't send frame back the way it came */
 		if (port == frame->port_rcv)
 			continue;
@@ -572,6 +573,17 @@ static void hsr_forward_do(struct hsr_frame_info *frame)
 		if ((port->dev->features & NETIF_F_HW_HSR_DUP) && sent)
 			continue;
 
+		/* PTP TX packets have an outgoing port specified */
+		if (frame->req_tx_port != HSR_PT_NONE && frame->req_tx_port != port->type)
+			continue;
+		/* PTP TX packets may already have a HSR header which needs to
+		 * be preserved
+		 */
+		if (frame->has_foreign_header && frame->skb_std) {
+			skb = skb_clone(frame->skb_std, GFP_ATOMIC);
+			goto inject_into_stack;
+		}
+
 		/* Don't send frame over port where it has been sent before.
 		 * Also for SAN, this shouldn't be done.
 		 */
@@ -599,6 +611,7 @@ static void hsr_forward_do(struct hsr_frame_info *frame)
 		else
 			skb = hsr->proto_ops->get_untagged_frame(frame, port);
 
+inject_into_stack:
 		if (!skb) {
 			frame->port_rcv->dev->stats.rx_dropped++;
 			continue;
@@ -663,6 +676,13 @@ int hsr_fill_frame_info(__be16 proto, struct sk_buff *skb,
 	struct hsr_port *port = frame->port_rcv;
 	struct hsr_priv *hsr = port->hsr;
 
+	if (frame->has_foreign_header) {
+		frame->skb_std = skb;
+
+		WARN_ON_ONCE(port->type != HSR_PT_MASTER);
+		WARN_ON_ONCE(skb->mac_len < sizeof(struct hsr_ethhdr));
+		return 0;
+	}
 	/* HSRv0 supervisory frames double as a tag so treat them as tagged. */
 	if ((!hsr->prot_version && proto == htons(ETH_P_PRP)) ||
 	    proto == htons(ETH_P_HSR)) {
@@ -704,7 +724,8 @@ int prp_fill_frame_info(__be16 proto, struct sk_buff *skb,
 }
 
 static int fill_frame_info(struct hsr_frame_info *frame,
-			   struct sk_buff *skb, struct hsr_port *port)
+			   struct sk_buff *skb, struct hsr_port *port,
+			   enum hsr_port_type tx_port, bool has_hsr_header)
 {
 	struct hsr_priv *hsr = port->hsr;
 	struct hsr_vlan_ethhdr *vlan_hdr;
@@ -727,10 +748,15 @@ static int fill_frame_info(struct hsr_frame_info *frame,
 	if (port->type == HSR_PT_INTERLINK)
 		n_db = &hsr->proxy_node_db;
 
-	frame->node_src = hsr_get_node(port, n_db, skb,
-				       frame->is_supervision, port->type);
-	if (!frame->node_src)
-		return -1; /* Unknown node and !is_supervision, or no mem */
+	frame->req_tx_port = tx_port;
+	frame->has_foreign_header = has_hsr_header;
+
+	if (!frame->has_foreign_header) {
+		frame->node_src = hsr_get_node(port, n_db, skb,
+					       frame->is_supervision, port->type);
+		if (!frame->node_src)
+			return -1; /* Unknown node and !is_supervision, or no mem */
+	}
 
 	ethhdr = (struct ethhdr *)skb_mac_header(skb);
 	frame->is_vlan = false;
@@ -779,10 +805,11 @@ void hsr_forward_skb(struct sk_buff *skb, struct hsr_port *port,
 	struct hsr_frame_info frame;
 
 	rcu_read_lock();
-	if (fill_frame_info(&frame, skb, port) < 0)
+	if (fill_frame_info(&frame, skb, port, tx_port, has_hsr_header) < 0)
 		goto out_drop;
 
-	hsr_register_frame_in(frame.node_src, port, frame.sequence_nr);
+	if (!frame.has_foreign_header)
+		hsr_register_frame_in(frame.node_src, port, frame.sequence_nr);
 	hsr_forward_do(&frame);
 	rcu_read_unlock();
 	/* Gets called for ingress frames as well as egress from master port.
diff --git a/net/hsr/hsr_framereg.h b/net/hsr/hsr_framereg.h
index 127a3fb64d5f0..e5e6363c0466f 100644
--- a/net/hsr/hsr_framereg.h
+++ b/net/hsr/hsr_framereg.h
@@ -29,6 +29,8 @@ struct hsr_frame_info {
 	bool is_from_san;
 	bool dst_in_node_db;
 	bool dst_in_proxy_node_db;
+	bool has_foreign_header;
+	enum hsr_port_type req_tx_port;
 };
 
 void hsr_del_self_node(struct hsr_priv *hsr);

-- 
2.55.0

[PATCH net-next v7 6/8] hsr: Assign a socket for cloned skbs

[Sebastian Andrzej Siewior]

The cloned skb does not have a socket information recorded of
the original skb. The original skb is never submitted. This means the
requested timestamp information gets lost.

Assign the socket of the original skb to the clone so the timestamp is
forwarded to the user.

Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
---
 net/hsr/hsr_forward.c | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/net/hsr/hsr_forward.c b/net/hsr/hsr_forward.c
index 2ce31b1da7b35..eb696e33bd739 100644
--- a/net/hsr/hsr_forward.c
+++ b/net/hsr/hsr_forward.c
@@ -12,6 +12,7 @@
 #include <linux/skbuff.h>
 #include <linux/etherdevice.h>
 #include <linux/if_vlan.h>
+#include <net/sock.h>
 #include "hsr_main.h"
 #include "hsr_framereg.h"
 
@@ -348,6 +349,9 @@ struct sk_buff *hsr_create_tagged_frame(struct hsr_frame_info *frame,
 	if (!skb)
 		return NULL;
 
+	if (frame->req_tx_port != HSR_PT_NONE && frame->skb_std->sk)
+		skb_set_owner_w(skb, frame->skb_std->sk);
+
 	if (port->dev->features & NETIF_F_HW_HSR_TAG_INS)
 		return skb;
 
@@ -581,6 +585,8 @@ static void hsr_forward_do(struct hsr_frame_info *frame)
 		 */
 		if (frame->has_foreign_header && frame->skb_std) {
 			skb = skb_clone(frame->skb_std, GFP_ATOMIC);
+			if (skb && frame->skb_std->sk)
+				skb_set_owner_w(skb, frame->skb_std->sk);
 			goto inject_into_stack;
 		}
 

-- 
2.55.0

[PATCH net-next v7 7/8] hsr: Move struct hsr_ethhdr to a global header

[Sebastian Andrzej Siewior]

In the offloading case the network driver needs to inspect the HSR
header to find out if the encapsulated type is a PTP packet where it
needs suppress offloading of the packet.

Move the header so it can be included the driver.

Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
---
 include/linux/if_hsr.h | 5 +++++
 net/hsr/hsr_main.h     | 5 -----
 2 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/include/linux/if_hsr.h b/include/linux/if_hsr.h
index 7b6b60ef592db..0d58559724fc9 100644
--- a/include/linux/if_hsr.h
+++ b/include/linux/if_hsr.h
@@ -39,6 +39,11 @@ struct hsr_tag {
 
 #define HSR_HLEN	6
 
+struct hsr_ethhdr {
+	struct ethhdr	ethhdr;
+	struct hsr_tag	hsr_tag;
+} __packed;
+
 #if IS_ENABLED(CONFIG_HSR)
 extern bool is_hsr_master(struct net_device *dev);
 extern int hsr_get_version(struct net_device *dev, enum hsr_version *ver);
diff --git a/net/hsr/hsr_main.h b/net/hsr/hsr_main.h
index 53e95bae0ee27..36eb82774000c 100644
--- a/net/hsr/hsr_main.h
+++ b/net/hsr/hsr_main.h
@@ -72,11 +72,6 @@ static inline void set_hsr_tag_LSDU_size(struct hsr_tag *ht, u16 LSDU_size)
 				       0xF000) | (LSDU_size & 0x0FFF));
 }
 
-struct hsr_ethhdr {
-	struct ethhdr	ethhdr;
-	struct hsr_tag	hsr_tag;
-} __packed;
-
 struct hsr_vlan_ethhdr {
 	struct vlan_ethhdr vlanhdr;
 	struct hsr_tag	hsr_tag;

-- 
2.55.0

[PATCH net-next v7 8/8] selftests: hsr: Add test for the inline PTP header on HSR

[Sebastian Andrzej Siewior]

This test verifies the inline header which is used by HSR stack with the
ether type ETH_P_1588.
The HSR stack needs to recognize this header, strip it and send the
requested packet only on the requested port.

This test needs a HSR device and the two slave devices passed. It will
will send a sample PTP packet with this inline header, requesting one of
the ports combining with and without the HSR header. A total of four
packets is sent.
The test checks both ports devices for packets and complains if the
packet was not observed on the expected port and/ or observed on the
other port. The received content of the data is compared against the
send data sample.

Test suggested by Felix Maurer.

Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
---
 tools/include/uapi/linux/hsr_ptp.h             |  18 +
 tools/testing/selftests/net/hsr/.gitignore     |   1 +
 tools/testing/selftests/net/hsr/Makefile       |   5 +
 tools/testing/selftests/net/hsr/hsr_ptp.sh     | 109 ++++++
 tools/testing/selftests/net/hsr/hsr_ptp_test.c | 482 +++++++++++++++++++++++++
 5 files changed, 615 insertions(+)

diff --git a/tools/include/uapi/linux/hsr_ptp.h b/tools/include/uapi/linux/hsr_ptp.h
new file mode 100644
index 0000000000000..21ccd3defa6cd
--- /dev/null
+++ b/tools/include/uapi/linux/hsr_ptp.h
@@ -0,0 +1,18 @@
+/* SPDX-License-Identifier: GPL-2.0+ WITH Linux-syscall-note */
+#ifndef __UAPI_HSR_PTP_H
+#define __UAPI_HSR_PTP_H
+
+#define HSR_INLINE_HDR  0xaf485352
+#define HSR_INLINE_HDR_PORT_A	1
+#define HSR_INLINE_HDR_PORT_B	2
+
+struct hsr_inline_header {
+	uint8_t tx_port;
+	uint8_t hsr_hdr;
+	uint8_t __pad0[4];
+	__be32 magic;
+	uint8_t __pad1[2];
+	__be16 eth_type;
+} __attribute__ ((packed));
+
+#endif
diff --git a/tools/testing/selftests/net/hsr/.gitignore b/tools/testing/selftests/net/hsr/.gitignore
new file mode 100644
index 0000000000000..849eecb84c974
--- /dev/null
+++ b/tools/testing/selftests/net/hsr/.gitignore
@@ -0,0 +1 @@
+hsr_ptp_test
diff --git a/tools/testing/selftests/net/hsr/Makefile b/tools/testing/selftests/net/hsr/Makefile
index 2150e487ac7d7..512ccf61b6652 100644
--- a/tools/testing/selftests/net/hsr/Makefile
+++ b/tools/testing/selftests/net/hsr/Makefile
@@ -2,9 +2,12 @@
 
 top_srcdir = ../../../../..
 
+CFLAGS += -Wall -Wl,--no-as-needed -O2 -g $(TOOLS_INCLUDES)
+
 TEST_PROGS := \
 	hsr_ping.sh \
 	hsr_prp_redbox.sh \
+	hsr_ptp.sh \
 	hsr_redbox.sh \
 	link_faults.sh \
 	prp_ping.sh \
@@ -12,4 +15,6 @@ TEST_PROGS := \
 
 TEST_FILES += hsr_common.sh
 
+TEST_GEN_FILES := hsr_ptp_test
+
 include ../../lib.mk
diff --git a/tools/testing/selftests/net/hsr/hsr_ptp.sh b/tools/testing/selftests/net/hsr/hsr_ptp.sh
new file mode 100755
index 0000000000000..23379b1ce7a56
--- /dev/null
+++ b/tools/testing/selftests/net/hsr/hsr_ptp.sh
@@ -0,0 +1,109 @@
+#!/bin/bash
+# SPDX-License-Identifier: GPL-2.0
+
+ipv6=false
+
+source ./hsr_common.sh
+
+optstring="h"
+usage() {
+	echo "Usage: $0"
+}
+
+while getopts "$optstring" option;do
+	case "$option" in
+	"h")
+		usage "$0"
+		exit 0
+		;;
+	"?")
+		usage "$0"
+		exit 1
+		;;
+esac
+done
+
+setup_hsr_interfaces()
+{
+	local HSRv="$1"
+
+	echo "INFO: Preparing interfaces for HSRv${HSRv}."
+# Three HSR nodes. Each node has one link to each of its neighbour, two links in total.
+
+#    ns1eth1 ----- ns2eth1
+#      hsr1         hsr2
+#    ns1eth2       ns2eth2
+#       |            |
+#    ns3eth1      ns3eth2
+#           \    /
+#            hsr3
+#
+	# Interfaces
+	ip link add ns1eth1 netns "$ns1" type veth peer name ns2eth1 netns "$ns2"
+	ip link add ns1eth2 netns "$ns1" type veth peer name ns3eth1 netns "$ns3"
+	ip link add ns3eth2 netns "$ns3" type veth peer name ns2eth2 netns "$ns2"
+
+	# HSRv0/1
+	ip -net "$ns1" link add name hsr1 type hsr slave1 ns1eth1 \
+		slave2 ns1eth2 supervision 45 version "$HSRv" proto 0
+	ip -net "$ns2" link add name hsr2 type hsr slave1 ns2eth1 \
+		slave2 ns2eth2 supervision 45 version "$HSRv" proto 0
+	ip -net "$ns3" link add name hsr3 type hsr slave1 ns3eth1 \
+		slave2 ns3eth2 supervision 45 version "$HSRv" proto 0
+
+	# IP for HSR
+	ip -net "$ns1" addr add 100.64.0.1/24 dev hsr1
+	ip -net "$ns1" addr add dead:beef:0::1/64 dev hsr1 nodad
+	ip -net "$ns2" addr add 100.64.0.2/24 dev hsr2
+	ip -net "$ns2" addr add dead:beef:0::2/64 dev hsr2 nodad
+	ip -net "$ns3" addr add 100.64.0.3/24 dev hsr3
+	ip -net "$ns3" addr add dead:beef:0::3/64 dev hsr3 nodad
+
+	ip -net "$ns1" link set address 00:11:22:00:01:01 dev ns1eth1
+	ip -net "$ns1" link set address 00:11:22:00:01:02 dev ns1eth2
+
+	ip -net "$ns2" link set address 00:11:22:00:02:01 dev ns2eth1
+	ip -net "$ns2" link set address 00:11:22:00:02:02 dev ns2eth2
+
+	ip -net "$ns3" link set address 00:11:22:00:03:01 dev ns3eth1
+	ip -net "$ns3" link set address 00:11:22:00:03:02 dev ns3eth2
+
+	# All Links up
+	ip -net "$ns1" link set ns1eth1 up
+	ip -net "$ns1" link set ns1eth2 up
+	ip -net "$ns1" link set hsr1 up
+
+	ip -net "$ns2" link set ns2eth1 up
+	ip -net "$ns2" link set ns2eth2 up
+	ip -net "$ns2" link set hsr2 up
+
+	ip -net "$ns3" link set ns3eth1 up
+	ip -net "$ns3" link set ns3eth2 up
+	ip -net "$ns3" link set hsr3 up
+}
+
+run_ptp_hdr_tests()
+{
+	echo "INFO: Running PTP-header tests."
+
+	ip netns exec "$ns1" ./hsr_ptp_test -H hsr1 -A ns1eth1 -B ns1eth2
+	ret=$?
+	stop_if_error "PTP header test failed (ns1)."
+
+	ip netns exec "$ns2" ./hsr_ptp_test -H hsr2 -A ns2eth1 -B ns2eth2
+	ret=$?
+	stop_if_error "PTP header test failed (ns2)."
+
+	ip netns exec "$ns3" ./hsr_ptp_test -H hsr3 -A ns3eth1 -B ns3eth2
+	ret=$?
+	stop_if_error "PTP header test failed (ns3)."
+}
+
+check_prerequisites
+trap cleanup_all_ns EXIT
+
+setup_ns ns1 ns2 ns3
+setup_hsr_interfaces 1
+run_ptp_hdr_tests
+
+exit $ret
diff --git a/tools/testing/selftests/net/hsr/hsr_ptp_test.c b/tools/testing/selftests/net/hsr/hsr_ptp_test.c
new file mode 100644
index 0000000000000..aefea98d56569
--- /dev/null
+++ b/tools/testing/selftests/net/hsr/hsr_ptp_test.c
@@ -0,0 +1,482 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * Simple test to verify the usage of the inline header used for HSR with
+ * ether type ETH_P_1588.
+ * The inline header has to be stripped, the sent packet must only appear on the
+ * specified port and the interface needs to accept a foreign HSR header and
+ * prepend its own header.
+ *
+ * Socket handling inspired by raw.c from linuxptp.
+ *
+ */
+#include <poll.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+#include <unistd.h>
+
+#include <linux/types.h>
+#include <linux/hsr_ptp.h>
+#include <linux/if_ether.h>
+#include <net/if.h>
+#include <netinet/in.h>
+#include <netpacket/packet.h>
+#include <sys/ioctl.h>
+
+#define MSEC_TO_NSEC(_x)	(_x * 1000000)
+#define USEC_PER_SEC		1000000
+#define NSEC_PER_SEC		1000000000
+
+#ifndef __packed
+#define __packed __attribute__((packed))
+#endif
+
+#define MAC_LEN  6
+typedef uint8_t eth_addr[MAC_LEN];
+
+struct eth_hdr {
+	eth_addr dst;
+	eth_addr src;
+	uint16_t type;
+} __packed;
+
+struct hsr_hdr {
+	eth_addr dst;
+	eth_addr src;
+	uint16_t type;
+	uint16_t pathid_and_LSDU_size;
+	uint16_t sequence_nr;
+	uint16_t encap_type;
+} __packed;
+
+struct hsr_meta_header {
+	struct hsr_inline_header hsr_opt;
+	union {
+		struct hsr_hdr hsr_hdr;
+		struct eth_hdr eth_hdr;
+	};
+} __packed;
+
+static uint8_t ptp_packet[] = {
+	0x00, 0x12, 0x00, 0x2c, 0x00, 0x00, 0x02, 0x00,  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x88, 0x88, 0x88, 0x88,  0x88, 0x88, 0x88, 0x88, 0x00, 0x01, 0x00, 0x01,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+};
+
+static uint8_t p2p_dst_mac[MAC_LEN] = {
+	0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e
+};
+
+static uint8_t slave_mac_addr[MAC_LEN];
+static int32_t fd_slaveA = -1;
+static int32_t fd_slaveB = -1;
+static int32_t fd_hsr = -1;
+static uint16_t hsr_seq = 37;
+
+static inline void tsnorm(struct timespec *ts)
+{
+	while (ts->tv_nsec >= NSEC_PER_SEC) {
+		ts->tv_nsec -= NSEC_PER_SEC;
+		ts->tv_sec++;
+	}
+}
+
+static inline int64_t calcdiff(struct timespec t1, struct timespec t2)
+{
+	int64_t diff = USEC_PER_SEC * (long long)((int) t1.tv_sec - (int) t2.tv_sec);
+
+	diff += ((int) t1.tv_nsec - (int) t2.tv_nsec) / 1000;
+	return diff;
+}
+
+static inline int tsgreater(struct timespec *a, struct timespec *b)
+{
+	return ((a->tv_sec > b->tv_sec) ||
+		(a->tv_sec == b->tv_sec && a->tv_nsec > b->tv_nsec));
+}
+
+static int sk_interface_index(int fd, const char *name)
+{
+	struct ifreq ifreq;
+	int32_t err;
+
+	memset(&ifreq, 0, sizeof(ifreq));
+	strncpy(ifreq.ifr_name, name, sizeof(ifreq.ifr_name) - 1);
+	err = ioctl(fd, SIOCGIFINDEX, &ifreq);
+	if (err < 0) {
+		printf("ioctl SIOCGIFINDEX failed: %m\n");
+		return err;
+	}
+	return ifreq.ifr_ifindex;
+}
+
+static int open_socket(const char *name)
+{
+	struct sockaddr_ll addr;
+	int32_t fd, index;
+
+	fd = socket(AF_PACKET, SOCK_RAW, 0);
+	if (fd < 0) {
+		printf("socket failed: %m\n");
+		goto err;
+	}
+	index = sk_interface_index(fd, name);
+	if (index < 0)
+		goto err;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.sll_ifindex = index;
+	addr.sll_family = AF_PACKET;
+	addr.sll_protocol = htons(ETH_P_ALL);
+	if (bind(fd, (struct sockaddr *) &addr, sizeof(addr))) {
+		printf("bind failed: %m\n");
+		goto err;
+	}
+	if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, name, strlen(name))) {
+		printf("setsockopt SO_BINDTODEVICE failed: %m\n");
+		goto err;
+	}
+
+	return fd;
+err:
+	if (fd >= 0)
+		close(fd);
+	return -1;
+}
+
+static int sk_interface_macaddr(const char *name)
+{
+	struct ifreq ifreq;
+	int32_t err, fd;
+
+	memset(&ifreq, 0, sizeof(ifreq));
+	strncpy(ifreq.ifr_name, name, sizeof(ifreq.ifr_name) - 1);
+
+	fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
+	if (fd < 0) {
+		printf("socket failed: %m\n");
+		return -1;
+	}
+
+	err = ioctl(fd, SIOCGIFHWADDR, &ifreq);
+	if (err < 0) {
+		printf("ioctl SIOCGIFHWADDR failed: %m\n");
+		close(fd);
+		return -1;
+	}
+
+	close(fd);
+
+	memcpy(slave_mac_addr, &ifreq.ifr_hwaddr.sa_data, MAC_LEN);
+	return 0;
+}
+
+static int raw_open(const char *hsr_dev, const char *slaveA_dev, const char *slaveB_dev)
+{
+
+	if (sk_interface_macaddr(slaveA_dev))
+		goto err;
+
+	fd_slaveA = open_socket(slaveA_dev);
+	if (fd_slaveA < 0)
+		goto err;
+
+	fd_slaveB = open_socket(slaveB_dev);
+	if (fd_slaveB < 0)
+		goto err;
+
+	fd_hsr = open_socket(hsr_dev);
+	if (fd_hsr < 0)
+		goto err;
+
+	return 0;
+
+err:
+	if (fd_slaveA >= 0)
+		close(fd_slaveA);
+	if (fd_slaveB >= 0)
+		close(fd_slaveB);
+	if (fd_hsr >= 0)
+		close(fd_hsr);
+
+	return -1;
+}
+
+static int sk_receive(int fd, void *buf, int buflen)
+{
+	struct iovec iov = { buf, buflen };
+	uint8_t control[256];
+	int32_t cnt = 0;
+	struct msghdr msg;
+
+	memset(control, 0, sizeof(control));
+	memset(&msg, 0, sizeof(msg));
+
+	msg.msg_iov = &iov;
+	msg.msg_iovlen = 1;
+	msg.msg_control = control;
+	msg.msg_controllen = sizeof(control);
+
+	cnt = recvmsg(fd, &msg, MSG_DONTWAIT);
+	if (cnt < 0)
+		return 0;
+	return cnt;
+}
+
+static int raw_recv(int fd, void *sample, int sample_len)
+{
+	struct timespec now, end;
+	struct hsr_hdr *hsr_hdr;
+	uint8_t buf[1500];
+	int32_t cnt;
+
+	/* The packet is expected to arrive within 100ms. HSR will send hello
+	 * packets so there an end time and poll() will wait until this point.
+	 */
+	clock_gettime(CLOCK_MONOTONIC, &now);
+	end = now;
+	end.tv_nsec += MSEC_TO_NSEC(100);
+	tsnorm(&end);
+again:
+
+	cnt = sk_receive(fd, buf, sizeof(buf));
+	if (cnt == 0) {
+		int64_t sleep_time;
+		struct pollfd pfd;
+		int ret;
+
+		clock_gettime(CLOCK_MONOTONIC, &now);
+		if (tsgreater(&now, &end))
+			return 0;
+
+		sleep_time = calcdiff(end, now) / 1000;
+		if (sleep_time <= 0)
+			return 0;
+
+		pfd.fd = fd;
+		pfd.events = POLLIN;
+		ret = poll(&pfd, 1, sleep_time);
+		if (ret <= 0)
+			return ret;
+		goto again;
+	}
+
+	if (cnt < sizeof(struct hsr_hdr))
+		goto again;
+
+	hsr_hdr = (void *)buf;
+	/* Embedded magic packet passed? */
+	if (hsr_hdr->type == htons(ETH_P_1588)) {
+		struct hsr_inline_header *hsr_opt = (void *)buf;
+
+		if (hsr_opt->magic == htonl(HSR_INLINE_HDR)) {
+			printf("Error: Found HSR_INLINE_HDR\n");
+			return -1;
+		}
+	}
+	/* Assuming it is *our* network and nobody but the test here sends
+	 * packets to p2p_dst_mac. Therefore any received packet needs to be
+	 * ours and every mismatch is considered as error.
+	 */
+	if (memcmp(hsr_hdr->dst, p2p_dst_mac, MAC_LEN))
+		goto again;
+
+	if (hsr_hdr->type != htons(ETH_P_HSR)) {
+		printf("Error: Unexpected ether type: 0x%x\n", htons(hsr_hdr->type));
+		return -1;
+	}
+
+	if (hsr_hdr->encap_type != htons(ETH_P_1588)) {
+		printf("Error: Unexpected encapsulated type: 0x%x\n",
+		       htons(hsr_hdr->encap_type));
+		return -1;
+	}
+
+	if (cnt < sizeof(struct hsr_hdr) + sample_len) {
+		printf("Error: Packet %d is too small for data check\n", cnt);
+		return -1;
+	}
+
+	if (!memcmp(&buf[sizeof(struct hsr_hdr)], sample, sample_len))
+		return 1;
+
+	printf("Error: Packet did not match the sample\n");
+	return -1;
+}
+
+static int recv_verify(int port, void *sample, int sample_len)
+{
+	int32_t error = 0;
+	int32_t ret;
+
+	ret = raw_recv(fd_slaveA, sample, sample_len);
+	if (ret < 0)
+		return ret;
+	if (port == HSR_INLINE_HDR_PORT_A && ret == 0) {
+		printf("Error: Missing packet on portA\n");
+		error = 1;
+	}
+	if (port == HSR_INLINE_HDR_PORT_B && ret == 1) {
+		printf("Error: Not expecting packet on portA\n");
+		error = 1;
+	}
+
+	ret = raw_recv(fd_slaveB, sample, sample_len);
+	if (ret < 0)
+		return ret;
+	if (port == HSR_INLINE_HDR_PORT_B && ret == 0) {
+		printf("Error: Missing packet on portB\n");
+		error = 1;
+	}
+	if (port == HSR_INLINE_HDR_PORT_A && ret == 1) {
+		printf("Error: Not expecting packet on portB\n");
+		error = 1;
+	}
+	return error;
+}
+
+static int32_t pkt_send(int port, bool hsr_hdr, void *data, int data_len)
+{
+	struct hsr_meta_header *hdr;
+	uint8_t packet[200];
+	ssize_t cnt;
+	size_t len;
+
+	memset(packet, 0, sizeof(packet));
+
+	len = sizeof(struct hsr_inline_header);
+	hdr = (struct hsr_meta_header *)packet;
+	memset(&hdr->hsr_opt, 0, sizeof(hdr->hsr_opt));
+	hdr->hsr_opt.magic = ntohl(HSR_INLINE_HDR);
+	hdr->hsr_opt.eth_type = htons(ETH_P_1588);
+	if (port != HSR_INLINE_HDR_PORT_A && port != HSR_INLINE_HDR_PORT_B) {
+		printf("Wrong port requested\n");
+		return -1;
+	}
+	hdr->hsr_opt.tx_port = port;
+
+	if (hsr_hdr) {
+		uint16_t pathid_size;
+
+		len += sizeof(struct hsr_hdr);
+		memcpy(&packet[len], data, data_len);
+
+		hdr->hsr_opt.hsr_hdr = 1;
+
+		memcpy(&hdr->hsr_hdr.dst, p2p_dst_mac, MAC_LEN);
+		memcpy(&hdr->hsr_hdr.src, slave_mac_addr, MAC_LEN);
+		/* Flip a bit in SRC MAC addr so it does not look like hosts */
+		hdr->hsr_hdr.src[3] ^= 0x21;
+
+		hdr->hsr_hdr.type = htons(ETH_P_HSR);
+		hdr->hsr_hdr.sequence_nr = htons(hsr_seq++);
+		hdr->hsr_hdr.encap_type = htons(ETH_P_1588);
+
+		/* The resulting packet must be alteast 66 bytes */
+		if (data_len + sizeof(struct hsr_hdr) < 66)
+			data_len = 66 - sizeof(struct hsr_hdr);
+
+		pathid_size = data_len + sizeof(struct hsr_hdr) - sizeof(struct eth_hdr);
+		pathid_size |= (port - 1) << 12;
+
+		hdr->hsr_hdr.pathid_and_LSDU_size = htons(pathid_size);
+	} else {
+		len += sizeof(struct eth_hdr);
+		hdr->hsr_opt.hsr_hdr = 0;
+
+		memcpy(&hdr->eth_hdr.dst, p2p_dst_mac, MAC_LEN);
+		memcpy(&hdr->eth_hdr.src, slave_mac_addr, MAC_LEN);
+		hdr->eth_hdr.type = htons(ETH_P_1588);
+
+		memcpy(&packet[len], data, data_len);
+	}
+
+	cnt = send(fd_hsr, packet, len + data_len, 0);
+	if (cnt < 1)
+		return -1;
+
+	return cnt;
+}
+
+int main(int argc, char *argv[])
+{
+	char *slaveA = NULL, *slaveB = NULL, *hsr_dev = NULL;
+	char *msg_mode;
+	int opt;
+
+	while ((opt = getopt(argc, argv, "A:B:H:")) != -1) {
+		switch (opt) {
+		case 'A':
+			slaveA = strdup(optarg);
+			break;
+
+		case 'B':
+			slaveB = strdup(optarg);
+			break;
+
+		case 'H':
+			hsr_dev = strdup(optarg);
+			break;
+		default: /* '?' */
+			fprintf(stderr, "Usage: %s -A slaveA -B slaveB -H hsr_device\n",
+				argv[0]);
+			exit(EXIT_FAILURE);
+		}
+	}
+
+	if (!slaveA || !slaveB || !hsr_dev) {
+		fprintf(stderr, "Missing network devices\n");
+		exit(EXIT_FAILURE);
+	}
+
+	if (raw_open(hsr_dev, slaveA, slaveB) < 0)
+		return EXIT_FAILURE;
+
+	msg_mode = "PortA, no-hsr-header";
+	if (pkt_send(HSR_INLINE_HDR_PORT_A, false, ptp_packet, sizeof(ptp_packet)) < 0) {
+		printf("Sending failed: %s\n", msg_mode);
+		return EXIT_FAILURE;
+	}
+	if (recv_verify(HSR_INLINE_HDR_PORT_A, ptp_packet, sizeof(ptp_packet))) {
+		printf("Verify failed: %s\n", msg_mode);
+		return EXIT_FAILURE;
+	}
+
+	ptp_packet[16 + 4]++;
+	msg_mode = "PortB, no-hsr-header";
+	if (pkt_send(HSR_INLINE_HDR_PORT_B, false, ptp_packet, sizeof(ptp_packet)) < 0) {
+		printf("Sending failed: %s\n", msg_mode);
+		return EXIT_FAILURE;
+	}
+	if (recv_verify(HSR_INLINE_HDR_PORT_B, ptp_packet, sizeof(ptp_packet))) {
+		printf("Verify failed: %s\n", msg_mode);
+		return EXIT_FAILURE;
+	}
+
+	ptp_packet[16 + 4]++;
+	msg_mode = "PortA, hsr-header";
+	if (pkt_send(HSR_INLINE_HDR_PORT_A, true, ptp_packet, sizeof(ptp_packet)) < 0) {
+		printf("Sending failed: %s\n", msg_mode);
+		return EXIT_FAILURE;
+	}
+	if (recv_verify(HSR_INLINE_HDR_PORT_A, ptp_packet, sizeof(ptp_packet))) {
+		printf("Verify failed: %s\n", msg_mode);
+		return EXIT_FAILURE;
+	}
+
+	ptp_packet[16 + 4]++;
+	msg_mode = "PortB, hsr-header";
+	if (pkt_send(HSR_INLINE_HDR_PORT_B, true, ptp_packet, sizeof(ptp_packet)) < 0) {
+		printf("Sending failed: %s\n", msg_mode);
+		return EXIT_FAILURE;
+	}
+	if (recv_verify(HSR_INLINE_HDR_PORT_B, ptp_packet, sizeof(ptp_packet))) {
+		printf("Verify failed: %s\n", msg_mode);
+		return EXIT_FAILURE;
+	}
+
+	return EXIT_SUCCESS;
+}

-- 
2.55.0