This test verifies the inline header which is used by HSR stack with the ether type ETH_P_1588. The HSR stack needs to recognize this header, strip it and send the requested packet only on the requested port. This test needs a HSR device and the two slave devices passed. It will will send a sample PTP packet with this inline header, requesting one of the ports combining with and without the HSR header. A total of four packets is sent. The test checks both ports devices for packets and complains if the packet was not observed on the expected port and/ or observed on the other port. The received content of the data is compared against the send data sample. Requested-by: Felix Maurer Signed-off-by: Sebastian Andrzej Siewior --- tools/testing/selftests/net/hsr/.gitignore | 1 + tools/testing/selftests/net/hsr/Makefile | 3 + tools/testing/selftests/net/hsr/hsr_ptp.sh | 109 ++++++ tools/testing/selftests/net/hsr/hsr_ptp_test.c | 438 +++++++++++++++++++++++++ 4 files changed, 551 insertions(+) diff --git a/tools/testing/selftests/net/hsr/.gitignore b/tools/testing/selftests/net/hsr/.gitignore new file mode 100644 index 0000000000000..849eecb84c974 --- /dev/null +++ b/tools/testing/selftests/net/hsr/.gitignore @@ -0,0 +1 @@ +hsr_ptp_test diff --git a/tools/testing/selftests/net/hsr/Makefile b/tools/testing/selftests/net/hsr/Makefile index 31fb9326cf533..ec11fee56d8f8 100644 --- a/tools/testing/selftests/net/hsr/Makefile +++ b/tools/testing/selftests/net/hsr/Makefile @@ -7,8 +7,11 @@ TEST_PROGS := \ hsr_redbox.sh \ link_faults.sh \ prp_ping.sh \ + hsr_ptp.sh \ # end of TEST_PROGS TEST_FILES += hsr_common.sh +TEST_GEN_PROGS := hsr_ptp_test + include ../../lib.mk diff --git a/tools/testing/selftests/net/hsr/hsr_ptp.sh b/tools/testing/selftests/net/hsr/hsr_ptp.sh new file mode 100755 index 0000000000000..034c635916f81 --- /dev/null +++ b/tools/testing/selftests/net/hsr/hsr_ptp.sh @@ -0,0 +1,109 @@ +#!/bin/bash +# SPDX-License-Identifier: GPL-2.0 + +ipv6=false + +source ./hsr_common.sh + +optstring="h4" +usage() { + echo "Usage: $0" +} + +while getopts "$optstring" option;do + case "$option" in + "h") + usage $0 + exit 0 + ;; + "?") + usage $0 + exit 1 + ;; +esac +done + +setup_hsr_interfaces() +{ + local HSRv="$1" + + echo "INFO: Preparing interfaces for HSRv${HSRv}." +# Three HSR nodes. Each node has one link to each of its neighbour, two links in total. + +# ns1eth1 ----- ns2eth1 +# hsr1 hsr2 +# ns1eth2 ns2eth2 +# | | +# ns3eth1 ns3eth2 +# \ / +# hsr3 +# + # Interfaces + ip link add ns1eth1 netns "$ns1" type veth peer name ns2eth1 netns "$ns2" + ip link add ns1eth2 netns "$ns1" type veth peer name ns3eth1 netns "$ns3" + ip link add ns3eth2 netns "$ns3" type veth peer name ns2eth2 netns "$ns2" + + # HSRv0/1 + ip -net "$ns1" link add name hsr1 type hsr slave1 ns1eth1 \ + slave2 ns1eth2 supervision 45 version "$HSRv" proto 0 + ip -net "$ns2" link add name hsr2 type hsr slave1 ns2eth1 \ + slave2 ns2eth2 supervision 45 version "$HSRv" proto 0 + ip -net "$ns3" link add name hsr3 type hsr slave1 ns3eth1 \ + slave2 ns3eth2 supervision 45 version "$HSRv" proto 0 + + # IP for HSR + ip -net "$ns1" addr add 100.64.0.1/24 dev hsr1 + ip -net "$ns1" addr add dead:beef:0::1/64 dev hsr1 nodad + ip -net "$ns2" addr add 100.64.0.2/24 dev hsr2 + ip -net "$ns2" addr add dead:beef:0::2/64 dev hsr2 nodad + ip -net "$ns3" addr add 100.64.0.3/24 dev hsr3 + ip -net "$ns3" addr add dead:beef:0::3/64 dev hsr3 nodad + + ip -net "$ns1" link set address 00:11:22:00:01:01 dev ns1eth1 + ip -net "$ns1" link set address 00:11:22:00:01:02 dev ns1eth2 + + ip -net "$ns2" link set address 00:11:22:00:02:01 dev ns2eth1 + ip -net "$ns2" link set address 00:11:22:00:02:02 dev ns2eth2 + + ip -net "$ns3" link set address 00:11:22:00:03:01 dev ns3eth1 + ip -net "$ns3" link set address 00:11:22:00:03:02 dev ns3eth2 + + # All Links up + ip -net "$ns1" link set ns1eth1 up + ip -net "$ns1" link set ns1eth2 up + ip -net "$ns1" link set hsr1 up + + ip -net "$ns2" link set ns2eth1 up + ip -net "$ns2" link set ns2eth2 up + ip -net "$ns2" link set hsr2 up + + ip -net "$ns3" link set ns3eth1 up + ip -net "$ns3" link set ns3eth2 up + ip -net "$ns3" link set hsr3 up +} + +run_ptp_hdr_tests() +{ + echo "INFO: Running PTP-header tests." + + ip netns exec "$ns1" ./hsr_ptp_test -H hsr1 -A ns1eth1 -B ns1eth2 + ret=$? + stop_if_error "PTP header test failed (ns1)." + + ip netns exec "$ns2" ./hsr_ptp_test -H hsr2 -A ns2eth1 -B ns2eth2 + ret=$? + stop_if_error "PTP header test failed (ns2)." + + ip netns exec "$ns3" ./hsr_ptp_test -H hsr3 -A ns3eth1 -B ns3eth2 + ret=$? + stop_if_error "PTP header test failed (ns3)." +} + +check_prerequisites +trap cleanup_all_ns EXIT + +setup_ns ns1 ns2 ns3 +setup_hsr_interfaces 1 +run_ptp_hdr_tests + +exit $ret diff --git a/tools/testing/selftests/net/hsr/hsr_ptp_test.c b/tools/testing/selftests/net/hsr/hsr_ptp_test.c new file mode 100644 index 0000000000000..881a723afa847 --- /dev/null +++ b/tools/testing/selftests/net/hsr/hsr_ptp_test.c @@ -0,0 +1,438 @@ +// SPDX-License-Identifier: GPL-2.0-or-later +/* + * Simple test to verify the usage of the inline header used for HSR with + * ether type ETH_P_1588. + * The inline header has to be stripped, the sent packet must only appear on the + * specified port and the interface needs to accept a foreign HSR header and + * prepand its own header. + * + * Socket handling inspired by raw.c from linuxptp. + * + */ +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#define PORT_1 1 +#define PORT_2 2 + +#ifndef __packed +#define __packed __attribute__((packed)) +#endif + +/* HSR Magic */ +#define HSR_INLINE_HDR 0xaf485352 +struct hsr_inline_header { + uint8_t tx_port; + uint8_t hsr_hdr; + uint8_t __pad0[4]; + uint32_t magic; + uint8_t __pad1[2]; + uint16_t eth_type; +} __packed; + +#define MAC_LEN 6 +typedef uint8_t eth_addr[MAC_LEN]; + +struct eth_hdr { + eth_addr dst; + eth_addr src; + uint16_t type; +} __packed; + +struct hsr_hdr { + eth_addr dst; + eth_addr src; + uint16_t type; + uint16_t pathid_and_LSDU_size; + uint16_t sequence_nr; + uint16_t encap_type; +} __packed; + +struct hsr_meta_header { + struct hsr_inline_header hsr_opt; + union { + struct hsr_hdr hsr_hdr; + struct eth_hdr eth_hdr; + }; +} __packed; + +static uint8_t ptp_packet[] = { + 0x00, 0x12, 0x00, 0x2c, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x88, 0x00, 0x01, 0x00, 0x01, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +}; + +static uint8_t p2p_dst_mac[MAC_LEN] = { + 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e +}; + +static uint8_t slave_mac_addr[MAC_LEN]; +static int32_t fd_slaveA = -1; +static int32_t fd_slaveB = -1; +static int32_t fd_hsr = -1; +static uint16_t hsr_seq = 37; + +static int sk_interface_index(int fd, const char *name) +{ + struct ifreq ifreq; + int32_t err; + + memset(&ifreq, 0, sizeof(ifreq)); + strncpy(ifreq.ifr_name, name, sizeof(ifreq.ifr_name) - 1); + err = ioctl(fd, SIOCGIFINDEX, &ifreq); + if (err < 0) { + printf("ioctl SIOCGIFINDEX failed: %m\n"); + return err; + } + return ifreq.ifr_ifindex; +} + +static int open_socket(const char *name, uint8_t *local_addr, + uint8_t *p2p_dst_mac) +{ + struct sockaddr_ll addr; + int32_t fd, index; + + fd = socket(AF_PACKET, SOCK_RAW, 0); + if (fd < 0) { + printf("socket failed: %m\n"); + goto err; + } + index = sk_interface_index(fd, name); + if (index < 0) + goto err; + + memset(&addr, 0, sizeof(addr)); + addr.sll_ifindex = index; + addr.sll_family = AF_PACKET; + addr.sll_protocol = htons(ETH_P_ALL); + if (bind(fd, (struct sockaddr *) &addr, sizeof(addr))) { + printf("bind failed: %m\n"); + goto err; + } + if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, name, strlen(name))) { + printf("setsockopt SO_BINDTODEVICE failed: %m\n"); + goto err; + } + + return fd; +err: + if (fd >= 0) + close(fd); + return -1; +} + +static int sk_interface_macaddr(const char *name) +{ + struct ifreq ifreq; + int32_t err, fd; + + memset(&ifreq, 0, sizeof(ifreq)); + strncpy(ifreq.ifr_name, name, sizeof(ifreq.ifr_name) - 1); + + fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + if (fd < 0) { + printf("socket failed: %m\n"); + return -1; + } + + err = ioctl(fd, SIOCGIFHWADDR, &ifreq); + if (err < 0) { + printf("ioctl SIOCGIFHWADDR failed: %m\n"); + close(fd); + return -1; + } + + close(fd); + + memcpy(slave_mac_addr, &ifreq.ifr_hwaddr.sa_data, MAC_LEN); + return 0; +} + +static int raw_open(const char *hsr_dev, const char *slaveA_dev, const char *slaveB_dev) +{ + + if (sk_interface_macaddr(slaveA_dev)) + goto err; + + fd_slaveA = open_socket(slaveA_dev, slave_mac_addr, p2p_dst_mac); + if (fd_slaveA < 0) + goto err; + + fd_slaveB = open_socket(slaveB_dev, slave_mac_addr, p2p_dst_mac); + if (fd_slaveB < 0) + goto err; + + fd_hsr = open_socket(hsr_dev, slave_mac_addr, p2p_dst_mac); + if (fd_hsr < 0) + goto err; + + return 0; + +err: + if (fd_slaveA >= 0) + close(fd_slaveA); + if (fd_slaveB >= 0) + close(fd_slaveB); + if (fd_hsr >= 0) + close(fd_hsr); + + return -1; +} + +static int sk_receive(int fd, void *buf, int buflen) +{ + uint8_t control[256]; + struct iovec iov = { buf, buflen }; + struct msghdr msg; + int32_t cnt = 0; + + memset(control, 0, sizeof(control)); + memset(&msg, 0, sizeof(msg)); + + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + + cnt = recvmsg(fd, &msg, MSG_DONTWAIT); + if (cnt < 0) + return 0; + return cnt; +} + +static int raw_recv(int fd, void *sample, int sample_len) +{ + struct hsr_hdr *hsr_hdr; + uint8_t buf[1500]; + int32_t cnt; + +again: + cnt = sk_receive(fd, buf, sizeof(buf)); + if (cnt == 0) + return 0; + + if (cnt < sizeof(struct hsr_hdr)) + goto again; + + hsr_hdr = (void *)buf; + /* Embedded magic packet passed? */ + if (hsr_hdr->type == htons(ETH_P_1588)) { + struct hsr_inline_header *hsr_opt = (void *)buf; + + if (hsr_opt->magic == ntohl(HSR_INLINE_HDR)) { + printf("Error: Found HSR_INLINE_HDR\n"); + return -1; + } + } + /* Assuming it is *our* network and nobody but the test here sends + * packets to p2p_dst_mac. Therefore any received packet needs to be + * ours and every mismatch is considered as error. + */ + if (memcmp(hsr_hdr->dst, p2p_dst_mac, MAC_LEN)) + goto again; + + if (hsr_hdr->type != htons(ETH_P_HSR)) { + printf("Error: Unexpected ether type: 0x%x\n", htons(hsr_hdr->type)); + return -1; + } + + if (hsr_hdr->encap_type != htons(ETH_P_1588)) { + printf("Error: Unexpected encapsulated type: 0x%x\n", + htons(hsr_hdr->encap_type)); + return -1; + } + + if (cnt < sizeof(struct hsr_hdr) + sample_len) { + printf("Error: Packet %d is too small for data check\n", cnt); + return -1; + } + + if (!memcmp(&buf[sizeof(struct hsr_hdr)], sample, sample_len)) + return 1; + + printf("Error: Packet did not match the sample\n"); + return -1; +} + +static int recv_verify(int port, void *sample, int sample_len) +{ + int32_t error = 0; + int32_t ret; + + ret = raw_recv(fd_slaveA, sample, sample_len); + if (ret < 0) + return ret; + if (port == PORT_1 && ret == 0) { + printf("Error: Missing packet on portA\n"); + error = 1; + } + if (port == PORT_2 && ret == 1) { + printf("Error: Not expecting packet on portA\n"); + error = 1; + } + + ret = raw_recv(fd_slaveB, sample, sample_len); + if (ret < 0) + return ret; + if (port == PORT_2 && ret == 0) { + printf("Error: Missing packet on portB\n"); + error = 1; + } + if (port == PORT_1 && ret == 1) { + printf("Error: Not expecting packet on portB\n"); + error = 1; + } + return error; +} + +static int32_t pkt_send(int port, bool hsr_hdr, void *data, int data_len) +{ + struct hsr_meta_header *hdr; + uint8_t packet[200]; + ssize_t cnt; + size_t len; + + memset(packet, 0, sizeof(packet)); + + len = sizeof(struct hsr_inline_header); + hdr = (struct hsr_meta_header *)packet; + memset(&hdr->hsr_opt, 0, sizeof(hdr->hsr_opt)); + hdr->hsr_opt.magic = ntohl(HSR_INLINE_HDR); + hdr->hsr_opt.eth_type = htons(ETH_P_1588); + if (port != PORT_1 && port != PORT_2) { + printf("Wrong port requested\n"); + return -1; + } + hdr->hsr_opt.tx_port = port; + + if (hsr_hdr) { + uint16_t pathid_size; + + len += sizeof(struct hsr_hdr); + memcpy(&packet[len], data, data_len); + + hdr->hsr_opt.hsr_hdr = 1; + + memcpy(&hdr->hsr_hdr.dst, p2p_dst_mac, MAC_LEN); + memcpy(&hdr->hsr_hdr.src, slave_mac_addr, MAC_LEN); + /* Flip a bit in SRC MAC addr so it does not look like hosts */ + hdr->hsr_hdr.src[3] ^= 0x21; + + hdr->hsr_hdr.type = htons(ETH_P_HSR); + hdr->hsr_hdr.sequence_nr = htons(hsr_seq++); + hdr->hsr_hdr.encap_type = htons(ETH_P_1588); + + /* The resulting packet must be alteast 66 bytes */ + if (data_len + sizeof(struct hsr_hdr) < 66) + data_len = 66 - sizeof(struct hsr_hdr); + + pathid_size = data_len + sizeof(struct hsr_hdr) - sizeof(struct eth_hdr); + pathid_size |= (port - 1) << 12; + + hdr->hsr_hdr.pathid_and_LSDU_size = ntohs(pathid_size); + } else { + len += sizeof(struct eth_hdr); + hdr->hsr_opt.hsr_hdr = 0; + + memcpy(&hdr->eth_hdr.dst, p2p_dst_mac, MAC_LEN); + memcpy(&hdr->hsr_hdr.src, slave_mac_addr, MAC_LEN); + hdr->eth_hdr.type = htons(ETH_P_1588); + + memcpy(&packet[len], data, data_len); + } + + cnt = send(fd_hsr, packet, len + data_len, 0); + if (cnt < 1) + return -1; + + return cnt; +} + +int main(int argc, char *argv[]) +{ + char *slaveA = NULL, *slaveB = NULL, *hsr_dev = NULL; + char *msg_mode; + int opt; + + while ((opt = getopt(argc, argv, "A:B:H:")) != -1) { + switch (opt) { + case 'A': + slaveA = strdup(optarg); + break; + + case 'B': + slaveB = strdup(optarg); + break; + + case 'H': + hsr_dev = strdup(optarg); + break; + default: /* '?' */ + fprintf(stderr, "Usage: %s -A slaveA -B slaveB -H hsr_device\n", + argv[0]); + exit(EXIT_FAILURE); + } + } + + if (!slaveA || !slaveB || !hsr_dev) { + fprintf(stderr, "Missing network devices\n"); + exit(EXIT_FAILURE); + } + + if (raw_open(hsr_dev, slaveA, slaveB) < 0) + return EXIT_FAILURE; + + msg_mode = "PortA, no-hsr-header"; + if (pkt_send(PORT_1, false, ptp_packet, sizeof(ptp_packet)) < 0) { + printf("Sending failed: %s\n", msg_mode); + return EXIT_FAILURE; + } + if (recv_verify(PORT_1, ptp_packet, sizeof(ptp_packet))) { + printf("Verify failed: %s\n", msg_mode); + return EXIT_FAILURE; + } + + ptp_packet[16 + 4]++; + msg_mode = "PortB, no-hsr-header"; + if (pkt_send(PORT_2, false, ptp_packet, sizeof(ptp_packet)) < 0) { + printf("Sending failed: %s\n", msg_mode); + return EXIT_FAILURE; + } + if (recv_verify(PORT_2, ptp_packet, sizeof(ptp_packet))) { + printf("Sending failed: %s\n", msg_mode); + return EXIT_FAILURE; + } + + ptp_packet[16 + 4]++; + msg_mode = "PortA, hsr-header"; + if (pkt_send(PORT_1, true, ptp_packet, sizeof(ptp_packet)) < 0) { + printf("Sending failed: %s\n", msg_mode); + return EXIT_FAILURE; + } + if (recv_verify(PORT_1, ptp_packet, sizeof(ptp_packet))) { + printf("Sending failed: %s\n", msg_mode); + return EXIT_FAILURE; + } + + ptp_packet[16 + 4]++; + msg_mode = "PortB, hsr-header"; + if (pkt_send(PORT_2, true, ptp_packet, sizeof(ptp_packet)) < 0) { + printf("Sending failed: %s\n", msg_mode); + return EXIT_FAILURE; + } + if (recv_verify(PORT_2, ptp_packet, sizeof(ptp_packet))) { + printf("Sending failed: %s\n", msg_mode); + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} -- 2.53.0