linuxptp/ ptp4l uses a AF_PACKET with a RAW socket to send and receive
PTP packets. Extend the interface with the ability to bind the socket to
one of the two HSR ports and add a flag for sendmsg() to indicate that
the packet already contains a HSR header.

Once PACKET_HSR_BIND_PORT is set, the socket will be bound to requested
slave port. All incoming packets without a set port will be discarded.
This limits receiving packet to PTP only packets. The packet will be
forwarded to userland with the HSR header.

For control messages used by sendmsg(), PACKET_HSR_INFO is added with
PACKET_HSR_INFO_HAS_HDR as the only option. This option sets
HSR_SKB_INCLUDES_HEADER on the outgoing skb to indicate that the packet
already contains a HSR header. This requires that the socket is bound to
a specific HSR port so that the packet is sent only on one of the two
ports.

Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
---
 include/uapi/linux/if_packet.h |   9 ++++
 net/packet/af_packet.c         | 103 +++++++++++++++++++++++++++++++++++++++++
 net/packet/internal.h          |   1 +
 3 files changed, 113 insertions(+)

diff --git a/include/uapi/linux/if_packet.h b/include/uapi/linux/if_packet.h
index 6cd1d7a41dfb7..3443eeac8470e 100644
--- a/include/uapi/linux/if_packet.h
+++ b/include/uapi/linux/if_packet.h

@@ -60,6 +60,7 @@ struct sockaddr_ll {
 #define PACKET_FANOUT_DATA		22
 #define PACKET_IGNORE_OUTGOING		23
 #define PACKET_VNET_HDR_SZ		24
+#define PACKET_HSR_BIND_PORT		25
 
 #define PACKET_FANOUT_HASH		0
 #define PACKET_FANOUT_LB		1

@@ -74,6 +75,14 @@ struct sockaddr_ll {
 #define PACKET_FANOUT_FLAG_IGNORE_OUTGOING     0x4000
 #define PACKET_FANOUT_FLAG_DEFRAG	0x8000
 
+/* For HSR, bind port */
+#define PACKET_HSR_BIND_PORT_AB		0
+#define PACKET_HSR_BIND_PORT_A		1
+#define PACKET_HSR_BIND_PORT_B		2
+/* HSR, CMSG */
+#define PACKET_HSR_INFO			1
+#define PACKET_HSR_INFO_HAS_HDR		1
+
 struct tpacket_stats {
 	unsigned int	tp_packets;
 	unsigned int	tp_drops;

diff --git a/net/packet/af_packet.c b/net/packet/af_packet.c
index 494d628d10a51..cd7c4ad034bc5 100644
--- a/net/packet/af_packet.c
+++ b/net/packet/af_packet.c

@@ -82,6 +82,7 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/mutex.h>
+#include <linux/if_hsr.h>
 #include <linux/if_vlan.h>
 #include <linux/virtio_net.h>
 #include <linux/errqueue.h>

@@ -1938,6 +1939,36 @@ static void packet_parse_headers(struct sk_buff *skb, struct socket *sock)
 	skb_probe_transport_header(skb);
 }
 
+static int packet_cmsg_send(struct msghdr *msg, struct packet_sock *po,
+			    unsigned int *hsr_setting)
+{
+	struct cmsghdr *cmsg;
+	int ret = -EINVAL;
+	u32 val;
+
+	for_each_cmsghdr(cmsg, msg) {
+		if (!CMSG_OK(msg, cmsg))
+			goto out;
+		if (cmsg->cmsg_level != SOL_PACKET)
+			continue;
+		if (cmsg->cmsg_type != PACKET_HSR_INFO)
+			continue;
+		if (cmsg->cmsg_len != CMSG_LEN(sizeof(u32)))
+			goto out;
+
+		val = *(u32 *)CMSG_DATA(cmsg);
+		if (val != PACKET_HSR_INFO_HAS_HDR)
+			goto out;
+		if (!po->hsr_bound_port)
+			goto out;
+
+		*hsr_setting = HSR_SKB_INCLUDES_HEADER;
+	}
+	ret = 0;
+out:
+	return ret;
+}
+
 /*
  *	Output a raw packet to a device layer. This bypasses all the other
  *	protocol layers and you must therefore supply it with a complete frame

@@ -1947,6 +1978,7 @@ static int packet_sendmsg_spkt(struct socket *sock, struct msghdr *msg,
 			       size_t len)
 {
 	struct sock *sk = sock->sk;
+	struct packet_sock *po = pkt_sk(sk);
 	DECLARE_SOCKADDR(struct sockaddr_pkt *, saddr, msg->msg_name);
 	struct sk_buff *skb = NULL;
 	struct net_device *dev;

@@ -1954,6 +1986,7 @@ static int packet_sendmsg_spkt(struct socket *sock, struct msghdr *msg,
 	__be16 proto = 0;
 	int err;
 	int extra_len = 0;
+	u32 hsr_setting = 0;
 
 	/*
 	 *	Get and verify the address.

@@ -2044,6 +2077,9 @@ static int packet_sendmsg_spkt(struct socket *sock, struct msghdr *msg,
 		err = sock_cmsg_send(sk, msg, &sockc);
 		if (unlikely(err))
 			goto out_unlock;
+		err = packet_cmsg_send(msg, po, &hsr_setting);
+		if (unlikely(err))
+			goto out_unlock;

 	}
 
 	skb->protocol = proto;

@@ -2052,6 +2088,7 @@ static int packet_sendmsg_spkt(struct socket *sock, struct msghdr *msg,
 	skb->mark = sockc.mark;
 	skb_set_delivery_type_by_clockid(skb, sockc.transmit_time, sk->sk_clockid);
 	skb_setup_tx_timestamp(skb, &sockc);
+	skb_shinfo(skb)->hsr_ptp = hsr_setting | po->hsr_bound_port;
 
 	if (unlikely(extra_len == 4))
 		skb->no_fcs = 1;

@@ -2131,6 +2168,13 @@ static int packet_rcv(struct sk_buff *skb, struct net_device *dev,
 	if (!net_eq(dev_net(dev), sock_net(sk)))
 		goto drop;
 
+	if (po->hsr_bound_port) {
+		struct skb_shared_info *si = skb_shinfo(skb);
+
+		if (po->hsr_bound_port != si->hsr_ptp)
+			goto drop;
+	}
+

 	skb->dev = dev;
 
 	if (dev_has_header(dev)) {

@@ -2260,6 +2304,13 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
 	if (!net_eq(dev_net(dev), sock_net(sk)))
 		goto drop;
 
+	if (po->hsr_bound_port) {
+		struct skb_shared_info *si = skb_shinfo(skb);
+
+		if (po->hsr_bound_port != si->hsr_ptp)
+			goto drop;
+	}
+
 	if (dev_has_header(dev)) {
 		if (sk->sk_type != SOCK_DGRAM)
 			skb_push(skb, skb->data - skb_mac_header(skb));

@@ -2731,6 +2782,7 @@ static int tpacket_snd(struct packet_sock *po, struct msghdr *msg)
 	int len_sum = 0;
 	int status = TP_STATUS_AVAILABLE;
 	int hlen, tlen, copylen = 0;
+	u32 hsr_setting = 0;
 	long timeo;
 
 	mutex_lock(&po->pg_vec_lock);

@@ -2775,6 +2827,10 @@ static int tpacket_snd(struct packet_sock *po, struct msghdr *msg)
 		err = sock_cmsg_send(&po->sk, msg, &sockc);
 		if (unlikely(err))
 			goto out_put;
+
+		err = packet_cmsg_send(msg, po, &hsr_setting);
+		if (unlikely(err))
+			goto out_put;
 	}
 
 	if (po->sk.sk_socket->type == SOCK_RAW)