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. Given that this information is important for how PTP is used, PTP packets missing this information will be dropped. The two retrieved parameters are passed to hsr_forward_skb() and will be used later. Signed-off-by: Sebastian Andrzej Siewior --- MAINTAINERS | 1 + include/linux/if_hsr.h | 1 + include/uapi/linux/hsr_ptp.h | 20 +++++++++++++ net/hsr/hsr_device.c | 71 ++++++++++++++++++++++++++++++++++++-------- net/hsr/hsr_forward.c | 3 +- net/hsr/hsr_forward.h | 3 +- net/hsr/hsr_slave.c | 4 +-- 7 files changed, 87 insertions(+), 16 deletions(-) diff --git a/MAINTAINERS b/MAINTAINERS index 93abbb315065d..bd8f678710153 100644 --- a/MAINTAINERS +++ b/MAINTAINERS @@ -12306,6 +12306,7 @@ F: drivers/net/usb/hso.c HSR NETWORK PROTOCOL L: netdev@vger.kernel.org S: Orphan +F: include/uapi/linux/hsr_ptp.h F: net/hsr/ F: tools/testing/selftests/net/hsr/ 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 +#include 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..95e905441ebdc --- /dev/null +++ b/include/uapi/linux/hsr_ptp.h @@ -0,0 +1,20 @@ +/* SPDX-License-Identifier: GPL-2.0+ WITH Linux-syscall-note */ +#ifndef __UAPI_HSR_PTP_H +#define __UAPI_HSR_PTP_H + +#include + +#define HSR_INLINE_HDR 0xaf485352 +#define HSR_INLINE_HDR_PORT_A 1 +#define HSR_INLINE_HDR_PORT_B 2 + +struct hsr_inline_header { + __u8 req_tx_port; + __u8 has_hsr_header; + __be32 __pad0; + __be32 magic; + __be16 __pad1; + __be16 eth_type; +} __attribute__ ((packed)); + +#endif diff --git a/net/hsr/hsr_device.c b/net/hsr/hsr_device.c index 59df647d2fd45..1cd871b1e8b37 100644 --- a/net/hsr/hsr_device.c +++ b/net/hsr/hsr_device.c @@ -223,24 +223,71 @@ 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 req_tx_port = HSR_PT_NONE; struct hsr_priv *hsr = netdev_priv(dev); struct hsr_port *master; + bool has_hsr_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; + + if (!skb_mac_header_was_set(skb)) + skb_reset_mac_header(skb); + + hsr_opt = (struct hsr_inline_header *)skb_mac_header(skb); + if (hsr_opt->magic != htonl(HSR_INLINE_HDR)) + goto drop; + + has_hsr_header = hsr_opt->has_hsr_header; + req_tx_port = hsr_opt->req_tx_port; + if (req_tx_port != HSR_PT_SLAVE_A && req_tx_port != HSR_PT_SLAVE_B) + goto drop; + + if (hsr_opt->__pad0 != 0 || hsr_opt->__pad1 != 0) + goto drop; + + hdr_len = ETH_HLEN; + if (has_hsr_header) + hdr_len += HSR_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, req_tx_port, has_hsr_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 +409,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 +477,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 b183358207506..f49d962337f8b 100644 --- a/net/hsr/hsr_forward.c +++ b/net/hsr/hsr_forward.c @@ -771,7 +771,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 req_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..662dd369c7df2 100644 --- a/net/hsr/hsr_forward.h +++ b/net/hsr/hsr_forward.h @@ -13,7 +13,8 @@ #include #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 req_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