Re: [PATCH RFC net-next v2 2/2] af_packet: Add port specific handling for HSR

[Willem de Bruijn <willemdebruijn.kernel@gmail.com>]

Sebastian Andrzej Siewior wrote:
> 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 a flag for sendmsg() to indicate that the
> packet already contains a HSR header.

The same point about adding per protocol state to sk_buff applies
to a slightly lesser extent to PF_PACKET.

Adding this much HSR + PTP specific code there is a non-starter.

I should have said this in v1. This likely makes my skb_extensions
suggestion a non-starter sorry.

We need to find a different way to

Rx: get the port info from the slave device to userspace.
Tx: send out the intended slave device.

Let's separate the two challenges (and patches).

On Rx, could your process just attach the PF_PACKET socket to the
slave devices and filter on HSR PTP packets? Then separately drop
these packets in hsr_handle_frame (as already done?) or TC ingress, so
that they only arrive in userspace?

On Tx, can you share a bit more why there are two cases, one where the
master has to add the header, but also one where it does not (so
userspace has presumably inserted it).

The second case is simpler: can just write directly the whole packet
to the intended slave device.

For the first case, could skb->mark be used as port selector when
writing from a packet socket to the master device? That already works
with sock_cmsg_send.

> 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 since only those get
> the tagging by the HSR stack.
> 
> For the control message used by sendmsg(), type PACKET_HSR_INFO is added
> with PACKET_HSR_INFO_HAS_HDR as the only option. If this is used, the
> skb will be known as containing the HSR header. This option can only be
> used if the socket is bound to a specific HSR port.
> 
> To lower the impact for non-PTP-HSR/PRP user, a static branch is used to
> to avoid a compare & branch if the socket was not bound to a hsr-slave
> device. If CONFIG_HSR is not set, the compiler will be smart enough to
> remove the code entirely (keeping only PACKET_HSR_BIND_PORT for
> getsockopt()).
> 
> Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
> ---
>  include/uapi/linux/if_packet.h |   9 +++
>  net/packet/af_packet.c         | 167 +++++++++++++++++++++++++++++++++++++++++
>  net/packet/internal.h          |   1 +
>  3 files changed, 177 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 72d0935139f0f..98322ad57a234 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>
> @@ -284,6 +285,94 @@ static int packet_xmit(const struct packet_sock *po, struct sk_buff *skb)
>  	return dev_direct_xmit(skb, packet_pick_tx_queue(skb));
>  }
>  
> +#if IS_ENABLED(CONFIG_HSR)
> +
> +static DEFINE_STATIC_KEY_FALSE(hsr_ptp_support_enabled);
> +
> +static inline bool packet_add_ptp_settings(struct sk_buff *skb,
> +					   bool header, unsigned int port)
> +{
> +	if (!static_branch_unlikely(&hsr_ptp_support_enabled))
> +		return true;
> +
> +	if (!port)
> +		return true;
> +
> +	return hsr_skb_add_header_port(skb, header, port);
> +}
> +
> +static inline bool packet_filter_hsr_port(struct packet_sock *po, struct sk_buff *skb)
> +{
> +	unsigned int req_port;
> +	bool req_hdr;
> +
> +	if (!static_branch_unlikely(&hsr_ptp_support_enabled))
> +		return false;
> +
> +	if (!po->hsr_bound_port)
> +		return false;
> +
> +	if (!hsr_skb_get_header_port(skb, &req_hdr, &req_port))
> +		return true;
> +
> +	if (req_port == po->hsr_bound_port)
> +		return false;
> +	return true;
> +}
> +
> +static int packet_cmsg_send(struct msghdr *msg, struct packet_sock *po,
> +			    bool *hsr_has_hdr)
> +{
> +	struct cmsghdr *cmsg;
> +	int ret = -EINVAL;
> +	u32 val;
> +
> +	if (!static_branch_unlikely(&hsr_ptp_support_enabled))
> +		return 0;
> +
> +	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_has_hdr = true;
> +	}
> +	ret = 0;
> +out:
> +	return ret;
> +}
> +
> +#else
> +
> +static bool packet_add_ptp_settings(struct sk_buff *skb,
> +				    bool header, unsigned int port)
> +{
> +	return true;
> +}
> +
> +static bool packet_filter_hsr_port(struct packet_sock *po, struct sk_buff *skb)
> +{
> +	return false;
> +}
> +
> +static int packet_cmsg_send(struct msghdr *msg, struct packet_sock *po,
> +			    bool *hsr_has_hdr)
> +{
> +	return 0;
> +}
> +#endif
> +
>  static struct net_device *packet_cached_dev_get(struct packet_sock *po)
>  {
>  	struct net_device *dev;
> @@ -2131,6 +2220,9 @@ static int packet_rcv(struct sk_buff *skb, struct net_device *dev,
>  	if (!net_eq(dev_net(dev), sock_net(sk)))
>  		goto drop;
>  
> +	if (packet_filter_hsr_port(po, skb))
> +		goto drop;
> +
>  	skb->dev = dev;
>  
>  	if (dev_has_header(dev)) {
> @@ -2260,6 +2352,9 @@ static int tpacket_rcv(struct sk_buff *skb, struct net_device *dev,
>  	if (!net_eq(dev_net(dev), sock_net(sk)))
>  		goto drop;
>  
> +	if (packet_filter_hsr_port(po, skb))
> +		goto drop;
> +
>  	if (dev_has_header(dev)) {
>  		if (sk->sk_type != SOCK_DGRAM)
>  			skb_push(skb, skb->data - skb_mac_header(skb));
> @@ -2731,6 +2826,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;
> +	bool hsr_has_hdr = false;
>  	long timeo;
>  
>  	mutex_lock(&po->pg_vec_lock);
> @@ -2775,6 +2871,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_has_hdr);
> +		if (unlikely(err))
> +			goto out_put;
>  	}
>  
>  	if (po->sk.sk_socket->type == SOCK_RAW)
> @@ -2863,6 +2963,10 @@ static int tpacket_snd(struct packet_sock *po, struct msghdr *msg)
>  				goto out_status;
>  			}
>  		}
> +		if (!packet_add_ptp_settings(skb, hsr_has_hdr, po->hsr_bound_port)) {
> +			err = -ENOMEM;
> +			goto out_status;
> +		}
>  
>  		if (vnet_hdr_sz) {
>  			if (virtio_net_hdr_to_skb(skb, vnet_hdr, vio_le())) {
> @@ -2950,6 +3054,7 @@ static int packet_snd(struct socket *sock, struct msghdr *msg, size_t len)
>  	int offset = 0;
>  	struct packet_sock *po = pkt_sk(sk);
>  	int vnet_hdr_sz = READ_ONCE(po->vnet_hdr_sz);
> +	bool hsr_has_hdr = false;
>  	int hlen, tlen, linear;
>  	int extra_len = 0;
>  
> @@ -2988,6 +3093,10 @@ static int packet_snd(struct socket *sock, struct msghdr *msg, size_t len)
>  		err = sock_cmsg_send(sk, msg, &sockc);
>  		if (unlikely(err))
>  			goto out_unlock;
> +
> +		err = packet_cmsg_send(msg, po, &hsr_has_hdr);
> +		if (unlikely(err))
> +			goto out_unlock;
>  	}
>  
>  	if (sock->type == SOCK_RAW)
> @@ -3047,6 +3156,10 @@ static int packet_snd(struct socket *sock, struct msghdr *msg, size_t len)
>  	}
>  
>  	skb_setup_tx_timestamp(skb, &sockc);
> +	if (!packet_add_ptp_settings(skb, hsr_has_hdr, po->hsr_bound_port)) {
> +		err = -ENOMEM;
> +		goto out_free;
> +	}
>  
>  	if (!vnet_hdr.gso_type && (len > dev->mtu + reserve + extra_len) &&
>  	    !packet_extra_vlan_len_allowed(dev, skb)) {
> @@ -3166,6 +3279,11 @@ static int packet_release(struct socket *sock)
>  		fanout_release_data(f);
>  		kvfree(f);
>  	}
> +
> +#if IS_ENABLED(CONFIG_HSR)
> +	if (po->hsr_bound_port)
> +		static_branch_dec(&hsr_ptp_support_enabled);
> +#endif
>  	/*
>  	 *	Now the socket is dead. No more input will appear.
>  	 */
> @@ -4044,6 +4162,40 @@ packet_setsockopt(struct socket *sock, int level, int optname, sockptr_t optval,
>  		packet_sock_flag_set(po, PACKET_SOCK_QDISC_BYPASS, val);
>  		return 0;
>  	}
> +	case PACKET_HSR_BIND_PORT:
> +	{
> +#if IS_ENABLED(CONFIG_HSR)
> +		int old_val;
> +		int val;
> +
> +		if (optlen != sizeof(val))
> +			return -EINVAL;
> +		if (copy_from_sockptr(&val, optval, sizeof(val)))
> +			return -EFAULT;
> +
> +		old_val = !!po->hsr_bound_port;
> +		switch (val) {
> +		case PACKET_HSR_BIND_PORT_AB:
> +			po->hsr_bound_port = 0;
> +			break;
> +		case PACKET_HSR_BIND_PORT_A:
> +			po->hsr_bound_port = HSR_PT_SLAVE_A;
> +			break;
> +		case PACKET_HSR_BIND_PORT_B:
> +			po->hsr_bound_port = HSR_PT_SLAVE_B;
> +			break;
> +		default:
> +			return -EINVAL;
> +		}
> +		if (old_val != !!po->hsr_bound_port) {
> +			if (po->hsr_bound_port)
> +				static_branch_inc(&hsr_ptp_support_enabled);
> +			else
> +				static_branch_dec(&hsr_ptp_support_enabled);
> +		}
> +		return 0;
> +#endif
> +	}
>  	default:
>  		return -ENOPROTOOPT;
>  	}
> @@ -4164,6 +4316,21 @@ static int packet_getsockopt(struct socket *sock, int level, int optname,
>  	case PACKET_QDISC_BYPASS:
>  		val = packet_sock_flag(po, PACKET_SOCK_QDISC_BYPASS);
>  		break;
> +	case PACKET_HSR_BIND_PORT:
> +		switch (po->hsr_bound_port) {
> +		case 0:
> +			val = PACKET_HSR_BIND_PORT_AB;
> +			break;
> +		case HSR_PT_SLAVE_A:
> +			val = PACKET_HSR_BIND_PORT_A;
> +			break;
> +		case HSR_PT_SLAVE_B:
> +			val = PACKET_HSR_BIND_PORT_B;
> +			break;
> +		default:
> +			return -EINVAL;
> +		}
> +		break;
>  	default:
>  		return -ENOPROTOOPT;
>  	}
> diff --git a/net/packet/internal.h b/net/packet/internal.h
> index b76e645cd78d1..24d63275d432f 100644
> --- a/net/packet/internal.h
> +++ b/net/packet/internal.h
> @@ -114,6 +114,7 @@ struct packet_sock {
>  	unsigned long		flags;
>  	int			ifindex;	/* bound device		*/
>  	u8			vnet_hdr_sz;
> +	u8			hsr_bound_port;
>  	__be16			num;
>  	struct packet_rollover	*rollover;
>  	struct packet_mclist	*mclist;
> 
> -- 
> 2.53.0
>