HSR forwards all packets it received on slave port A to slave port B and one of the possible two copies to the user (master) interface. In terms of PTP this is not good because the latency introduced by forwarding makes the timestamp in the PTP packet inaccurate. Introduce a hsr_ptp field to struct skb_shared_info which can be used to store HSR specific information for sending and receiving skbs. Receive (slave ports): - HSR_PT_SLAVE_A/ HSR_PT_SLAVE_B to denote the port which received the packet. This information is only added to PTP packets. Send (master port): - HSR_PT_SLAVE_A/ HSR_PT_SLAVE_B to denote the port on which the packet has to be sent. - HSR_SKB_INCLUDES_HEADER to denote that the packet already contains a HSR header and the stack must not add the system's header to it. HSR_SKB_INCLUDES_HEADER is used to allow forwarding a PTP packet and preserving the HSR header by the sender. Cloning skbs requires to preserve the socket information so that a PTP timestamp can be associated with the socket. Signed-off-by: Sebastian Andrzej Siewior --- include/linux/if_hsr.h | 2 + include/linux/skbuff.h | 1 + net/hsr/hsr_device.c | 7 +++ net/hsr/hsr_forward.c | 114 ++++++++++++++++++++++++++++++++++++++++++++++--- net/hsr/hsr_slave.c | 16 +++++++ 5 files changed, 133 insertions(+), 7 deletions(-) diff --git a/include/linux/if_hsr.h b/include/linux/if_hsr.h index f4cf2dd36d193..1463ddbc8cddf 100644 --- a/include/linux/if_hsr.h +++ b/include/linux/if_hsr.h @@ -22,6 +22,8 @@ enum hsr_port_type { HSR_PT_PORTS, /* This must be the last item in the enum */ }; +#define HSR_SKB_INCLUDES_HEADER (1 << 4) + /* 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/include/linux/skbuff.h b/include/linux/skbuff.h index 86737076101d4..52c847e490ee8 100644 --- a/include/linux/skbuff.h +++ b/include/linux/skbuff.h @@ -605,6 +605,7 @@ struct skb_shared_info { }; unsigned int gso_type; u32 tskey; + u32 hsr_ptp; /* * Warning : all fields before dataref are cleared in __alloc_skb() diff --git a/net/hsr/hsr_device.c b/net/hsr/hsr_device.c index d1bfc49b5f017..a06ef7c88792c 100644 --- a/net/hsr/hsr_device.c +++ b/net/hsr/hsr_device.c @@ -230,6 +230,13 @@ static netdev_tx_t hsr_dev_xmit(struct sk_buff *skb, struct net_device *dev) master = hsr_port_get_hsr(hsr, HSR_PT_MASTER); if (master) { skb->dev = master->dev; + + /* skb includes HSR header but the network header is only set to + * the ethernet header. + */ + if (skb_shinfo(skb)->hsr_ptp & HSR_SKB_INCLUDES_HEADER) + skb_set_network_header(skb, ETH_HLEN + HSR_HLEN); + skb_reset_mac_header(skb); skb_reset_mac_len(skb); spin_lock_bh(&hsr->seqnr_lock); diff --git a/net/hsr/hsr_forward.c b/net/hsr/hsr_forward.c index aefc9b6936ba0..c4040e61bd914 100644 --- a/net/hsr/hsr_forward.c +++ b/net/hsr/hsr_forward.c @@ -17,6 +17,16 @@ struct hsr_node; +static unsigned int hsr_get_ptp_flags(struct sk_buff *skb) +{ + return skb_shinfo(skb)->hsr_ptp; +} + +static unsigned int hsr_keep_header(struct sk_buff *skb) +{ + return (hsr_get_ptp_flags(skb) & 0xf0) == HSR_SKB_INCLUDES_HEADER; +} + /* The uses I can see for these HSR supervision frames are: * 1) Use the frames that are sent after node initialization ("HSR_TLV.Type = * 22") to reset any sequence_nr counters belonging to that node. Useful if @@ -343,7 +353,10 @@ struct sk_buff *hsr_create_tagged_frame(struct hsr_frame_info *frame, 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); + skb = skb_clone(frame->skb_std, GFP_ATOMIC); + if (skb_shinfo(frame->skb_std)->hsr_ptp && skb) + skb->sk = frame->skb_std->sk; + return skb; } /* Create the new skb with enough headroom to fit the HSR tag */ @@ -365,6 +378,16 @@ struct sk_buff *hsr_create_tagged_frame(struct hsr_frame_info *frame, memmove(dst, src, movelen); skb_reset_mac_header(skb); + if (skb_shinfo(frame->skb_std)->hsr_ptp) { + /* Packets are bound to a port and the sender may expect time + * information. + */ + skb_shinfo(skb)->hsr_ptp = skb_shinfo(frame->skb_std)->hsr_ptp; + skb_shinfo(skb)->tx_flags = skb_shinfo(frame->skb_std)->tx_flags; + skb_shinfo(skb)->tskey = skb_shinfo(frame->skb_std)->tskey; + skb->sk = frame->skb_std->sk; + } + /* skb_put_padto free skb on error and hsr_fill_tag returns NULL in * that case */ @@ -420,7 +443,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 && !hsr_keep_header(skb)) { hsr_addr_subst_dest(frame->node_src, skb, port); /* Address substitution (IEC62439-3 pp 26, 50): replace mac @@ -504,6 +527,32 @@ bool hsr_drop_frame(struct hsr_frame_info *frame, struct hsr_port *port) return false; } +static void hsr_parse_req_master(struct hsr_frame_info *frame, + unsigned int *req_port, + bool *req_keep_header) +{ + struct skb_shared_info *si; + unsigned int val; + + *req_port = HSR_PT_NONE; + *req_keep_header = false; + + if (!frame->skb_std) + return; + + si = skb_shinfo(frame->skb_std); + if (!si->hsr_ptp) + return; + + val = si->hsr_ptp & 0xf; + if (val == HSR_PT_SLAVE_A || val == HSR_PT_SLAVE_B) + *req_port = val; + + val = si->hsr_ptp & 0xf0; + if (val == HSR_SKB_INCLUDES_HEADER) + *req_keep_header = true; +} + /* Forward the frame through all devices except: * - Back through the receiving device * - If it's a HSR frame: through a device where it has passed before @@ -521,6 +570,10 @@ static void hsr_forward_do(struct hsr_frame_info *frame) struct hsr_port *port; struct sk_buff *skb; bool sent = false; + unsigned int req_port; + bool req_keep_header; + + hsr_parse_req_master(frame, &req_port, &req_keep_header); hsr_for_each_port(frame->port_rcv->hsr, port) { struct hsr_priv *hsr = port->hsr; @@ -542,6 +595,42 @@ static void hsr_forward_do(struct hsr_frame_info *frame) if ((port->dev->features & NETIF_F_HW_HSR_DUP) && sent) continue; + { + struct skb_shared_info *si = NULL; + + if (frame->skb_hsr) + si = skb_shinfo(frame->skb_hsr); + + /* Received over a slave interface */ + if (si && si->hsr_ptp) { + /* No PTP forwarding */ + if (port->type == HSR_PT_SLAVE_A || + port->type == HSR_PT_SLAVE_B) + continue; + + if (port->type != HSR_PT_MASTER) + continue; + + skb = skb_clone(frame->skb_hsr, GFP_ATOMIC); + /* Inject the PTP frame into the master + * interface including HSR headers. + */ + goto inject_into_stack; + } + + /* Outgoing port has been specified */ + if ((req_port != HSR_PT_NONE) && (req_port != port->type)) + continue; + if (req_keep_header) { + skb = skb_clone(frame->skb_std, GFP_ATOMIC); + /* Send the frame on the specific port without + * addign HSR headers. + */ + if (skb) + skb->sk = frame->skb_std->sk; + goto inject_into_stack; + } + } /* Don't send frame over port where it has been sent before. * Also for SAN, this shouldn't be done. */ @@ -569,6 +658,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; @@ -633,6 +723,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 (hsr_keep_header(skb)) { + 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)) { @@ -697,10 +794,12 @@ 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 */ + if (!hsr_keep_header(skb)) { + 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; @@ -739,7 +838,8 @@ void hsr_forward_skb(struct sk_buff *skb, struct hsr_port *port) if (fill_frame_info(&frame, skb, port) < 0) goto out_drop; - hsr_register_frame_in(frame.node_src, port, frame.sequence_nr); + if (!hsr_keep_header(skb)) + 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_slave.c b/net/hsr/hsr_slave.c index afe06ba00ea44..8329d01eee287 100644 --- a/net/hsr/hsr_slave.c +++ b/net/hsr/hsr_slave.c @@ -81,6 +81,22 @@ static rx_handler_result_t hsr_handle_frame(struct sk_buff **pskb) hsr_forward_skb(skb, port); spin_unlock_bh(&hsr->seqnr_lock); } else { + struct hsr_ethhdr *hsr_ethhdr; + struct skb_shared_info *si; + + hsr_ethhdr = (struct hsr_ethhdr *)skb_mac_header(skb); + if (hsr_ethhdr->hsr_tag.encap_proto == htons(ETH_P_1588)) { + /* PTP packages are not supposed to be forwarded via HSR + * as-is. The latency introduced by forwarding renders + * the time information useless. + * Instead attach the port information on which it was + * received, forward both copies to userland only and + * let it deal with it. + */ + si = skb_shinfo(skb); + si->hsr_ptp = port->type; + } + hsr_forward_skb(skb, port); } -- 2.51.0 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 --- 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 #include #include +#include #include #include #include @@ -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) @@ -2863,6 +2919,7 @@ static int tpacket_snd(struct packet_sock *po, struct msghdr *msg) goto out_status; } } + skb_shinfo(skb)->hsr_ptp = hsr_setting | po->hsr_bound_port; if (vnet_hdr_sz) { if (virtio_net_hdr_to_skb(skb, vnet_hdr, vio_le())) { @@ -2952,6 +3009,7 @@ static int packet_snd(struct socket *sock, struct msghdr *msg, size_t len) int vnet_hdr_sz = READ_ONCE(po->vnet_hdr_sz); int hlen, tlen, linear; int extra_len = 0; + u32 hsr_setting = 0; /* * Get and verify the address. @@ -2988,6 +3046,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_setting); + if (unlikely(err)) + goto out_unlock; } if (sock->type == SOCK_RAW) @@ -3047,6 +3109,7 @@ static int packet_snd(struct socket *sock, struct msghdr *msg, size_t len) } skb_setup_tx_timestamp(skb, &sockc); + skb_shinfo(skb)->hsr_ptp = hsr_setting | po->hsr_bound_port; if (!vnet_hdr.gso_type && (len > dev->mtu + reserve + extra_len) && !packet_extra_vlan_len_allowed(dev, skb)) { @@ -4044,6 +4107,31 @@ 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: + { + int val; + + if (optlen != sizeof(val)) + return -EINVAL; + if (copy_from_sockptr(&val, optval, sizeof(val))) + return -EFAULT; + + switch (val) { + case 0: + 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; + } + + return 0; + } default: return -ENOPROTOOPT; } @@ -4164,6 +4252,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 = 0; + 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.51.0