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

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

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 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>
> ---
>  include/linux/if_hsr.h | 11 +++++++++
>  net/hsr/hsr_device.c   | 61 ++++++++++++++++++++++++++++++++++++++++----------
>  net/hsr/hsr_forward.c  |  3 ++-
>  net/hsr/hsr_forward.h  |  3 ++-
>  net/hsr/hsr_slave.c    |  4 ++--
>  5 files changed, 66 insertions(+), 16 deletions(-)
> 
> diff --git a/include/linux/if_hsr.h b/include/linux/if_hsr.h
> index f4cf2dd36d193..1db1bd0fa181d 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 <linux/skbuff.h>
>  
>  struct net_device;
>  
> @@ -22,6 +23,16 @@ enum hsr_port_type {
>  	HSR_PT_PORTS,	/* This must be the last item in the enum */
>  };
>  
> +#define HSR_INLINE_HDR	0xaf485352
> +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;
> +} __packed;
> +
>  /* HSR Tag.
>   * As defined in IEC-62439-3:2010, the HSR tag is really { ethertype = 0x88FB,
>   * path, LSDU_size, sequence Nr }. But we let eth_header() create { h_dest,
> diff --git a/net/hsr/hsr_device.c b/net/hsr/hsr_device.c
> index a999ddccc46c5..797e2cf391998 100644
> --- a/net/hsr/hsr_device.c
> +++ b/net/hsr/hsr_device.c
> @@ -223,24 +223,61 @@ 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;
> +		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)) {
> +			struct ethhdr *eth_hdr;
> +
> +			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;
> +
> +			eth_hdr = skb_pull(skb, sizeof(struct hsr_inline_header));
> +			if (has_header)
> +				hdr_len = ETH_HLEN + HSR_HLEN;
> +			else
> +				hdr_len = ETH_HLEN;
> +			skb_set_network_header(skb, hdr_len);
> +			/* Ensure the header can be accessed */
> +			if (!pskb_may_pull(skb, hdr_len))
> +				goto drop;
> +			skb->protocol = eth_hdr->h_proto;
> +		}

Should the packet be dropped otherwise?

>  	}
> +
> +	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;
>  }