Index: tools/tools/mq-testing/udp/Makefile =================================================================== --- /dev/null +++ tools/tools/mq-testing/udp/Makefile @@ -0,0 +1,12 @@ + +PROGS:= server pktgen +CFLAGS:= -Wall -Werror -O2 -lpthread + +all: ${PROGS} + +server: + +pktgen: + +clean: + rm -f *.o ${PROGS} Index: tools/tools/mq-testing/udp/common.h =================================================================== --- /dev/null +++ tools/tools/mq-testing/udp/common.h @@ -0,0 +1,34 @@ +/*- + * Copyright (c) 2015, Tiwei Bie + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice unmodified, this list of conditions, and the following + * disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/* $FreeBSD$ */ + +#ifndef _COMMON_H_ +#define _COMMON_H_ + +#define SERVER_PORT 8000 + +#endif Index: tools/tools/mq-testing/udp/pktgen.c =================================================================== --- /dev/null +++ tools/tools/mq-testing/udp/pktgen.c @@ -0,0 +1,375 @@ +/*- + * Copyright (c) 2015, Tiwei Bie + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice unmodified, this list of conditions, and the following + * disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/* $FreeBSD$ */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "common.h" + +#define IFCONFIG_BIN "/sbin/ifconfig " +#define IFCONFIG_ARGS "%s %s %s up" + +static char vmedev[MAXPATHLEN]; +static uint8_t vmeif_ether[ETHER_ADDR_LEN]; +static char vmeif_ipstr[INET6_ADDRSTRLEN] = ""; +static struct in6_addr vmeif_ip6; +static struct in_addr vmeif_ip; + +static char vmeif_name[IFNAMSIZ] = "vme0"; +static char vmeif_ipstr_v4[] = "192.168.10.1"; +static char vmeif_ipstr_v6[] = "fec0::1"; +static char *payload = "hello, world!"; +static int pktsize = 100; +static int quiet = 0; +static int cpu = -1; +static uint64_t total_sent = 0; +static int ipversion = 4; + +static void (*build_udp_packet)(uint8_t *, int); + +/* The checksum function is taken from tools/tools/netmap/pkt-gen.c */ +static uint16_t +checksum(const void *data, uint16_t len, uint32_t sum) +{ + const uint8_t *addr = data; + uint32_t i; + + /* Checksum all the pairs of bytes first... */ + for (i = 0; i < (len & ~1U); i += 2) { + sum += (uint16_t)ntohs(*(uint16_t *)(addr + i)); + if (sum > 0xFFFF) + sum -= 0xFFFF; + } + /* + * If there's a single byte left over, checksum it, too. + * Network byte order is big-endian, so the remaining byte is + * the high byte. + */ + if (i < len) { + sum += addr[i] << 8; + if (sum > 0xFFFF) + sum -= 0xFFFF; + } + return (sum); +} + +static uint16_t +wrapsum(uint32_t sum) +{ + sum = ~sum & 0xFFFF; + return (htons(sum)); +} + +static inline int +min(int a, int b) +{ + return (a < b) ? a : b; +} + +static void +build_udp_packet_v4(uint8_t *buf, int pkt_size) +{ + struct ether_header *eh = (struct ether_header *)buf; + struct ip *ip = (struct ip *)(eh + 1); + struct udphdr *udp = (struct udphdr *)(ip + 1); + char *data = (char *)(udp + 1); + + /* Just fake an address */ + eh->ether_shost[0] = 0x01; + eh->ether_shost[1] = 0xbd; + eh->ether_shost[2] = 0xbc; + eh->ether_shost[3] = 0x4d; + eh->ether_shost[4] = 0xfb; + eh->ether_shost[5] = 0xfb; + memcpy(eh->ether_dhost, vmeif_ether, ETHER_ADDR_LEN); + eh->ether_type = htons(ETHERTYPE_IP); + + ip->ip_v = IPVERSION; + ip->ip_hl = sizeof(*ip) >> 2; + ip->ip_tos = IPTOS_LOWDELAY; + ip->ip_len = htons(pkt_size - sizeof(*eh)); + ip->ip_id = 0; + ip->ip_off = htons(IP_DF); /* XXX: Don't fragment */ + ip->ip_ttl = IPDEFTTL; + ip->ip_p = IPPROTO_UDP; + ip->ip_src.s_addr = htonl(0x0a000001 + (rand() % 0xfffffe)); + ip->ip_dst = vmeif_ip; + + ip->ip_sum = 0; + ip->ip_sum = wrapsum(checksum(ip, sizeof(*ip), 0)); + + udp->uh_sport = htons(1 + rand() % 65535); + udp->uh_dport = htons(SERVER_PORT); + udp->uh_ulen = htons(pkt_size - sizeof(*eh) - sizeof(*ip)); + udp->uh_sum = 0; + + memcpy(data, payload, min(strlen(payload) + 1, + pkt_size - (sizeof(*eh) + sizeof(*ip) + sizeof(*udp)))); +} + +static void +build_udp_packet_v6(uint8_t *buf, int pkt_size) +{ + struct ether_header *eh = (struct ether_header *)buf; + struct ip6_hdr *ip6 = (struct ip6_hdr *)(eh + 1); + struct udphdr *udp = (struct udphdr *)(ip6 + 1); + char *data = (char *)(udp + 1); + + /* Just fake an address */ + eh->ether_shost[0] = 0x01; + eh->ether_shost[1] = 0xbd; + eh->ether_shost[2] = 0xbc; + eh->ether_shost[3] = 0x4d; + eh->ether_shost[4] = 0xfb; + eh->ether_shost[5] = 0xfb; + memcpy(eh->ether_dhost, vmeif_ether, ETHER_ADDR_LEN); + eh->ether_type = htons(ETHERTYPE_IPV6); + + ip6->ip6_flow = 0; + ip6->ip6_plen = htons(pkt_size - sizeof(*eh) - sizeof(*ip6)); + ip6->ip6_nxt = IPPROTO_UDP; + ip6->ip6_hlim = 0xff; + ip6->ip6_vfc |= IPV6_VERSION; + ip6->ip6_dst = vmeif_ip6; + for (int i = 0; i < 16; i++) + ip6->ip6_src.s6_addr[i] = rand(); + + udp->uh_sport = htons(1 + rand() % 65535); + udp->uh_dport = htons(SERVER_PORT); + udp->uh_ulen = htons(pkt_size - sizeof(*eh) - sizeof(*ip6)); + udp->uh_sum = 1; + + memcpy(data, payload, min(strlen(payload) + 1, + pkt_size - (sizeof(*eh) + sizeof(*ip6) + sizeof(*udp)))); +} + +static int +get_ifaddr(const char *ifname, uint8_t *ether_addr) +{ + struct ifaddrs *ifa, *p; + struct sockaddr_dl *sdl; + char *addr; + int error = -1; + + if (getifaddrs(&ifa) != 0) + return (-1); + + for (p = ifa; p != NULL; p = p->ifa_next) { + if (strcmp(p->ifa_name, ifname) == 0 && + p->ifa_addr->sa_family == AF_LINK && + p->ifa_data != NULL) { + sdl = (struct sockaddr_dl *)p->ifa_addr; + addr = &sdl->sdl_data[sdl->sdl_nlen]; + memcpy(ether_addr, addr, ETHER_ADDR_LEN); + error = 0; + break; + } + } + + freeifaddrs(ifa); + return (error); +} + +static void +bind_cpu(int cpu) +{ + cpuset_t mask; + + CPU_ZERO(&mask); + CPU_SET(cpu, &mask); + if (pthread_setaffinity_np(pthread_self(), sizeof(mask), &mask) != 0) + perror("pthread_setaffinity_np"); +} + +static void +signal_handler(int arg) +{ + printf("\r%lu packets injected.\n", total_sent); + + if (arg == SIGINT) + exit(EXIT_SUCCESS); +} + +static void +usage(const char *prgname) +{ + fprintf(stderr, + "Usage: %s [-a ip address] [-i vme interface] [-l pktsize]\n" + " %*s [-p payload] [-b cpu] [-46hq]\n", + prgname, (int)strlen(prgname), ""); + exit(EXIT_FAILURE); +} + +int +main(int argc, char **argv) +{ + int vmefd; + char cmd[sizeof(IFCONFIG_BIN) + sizeof(vmeif_name) + + sizeof(vmeif_ipstr) + 50]; + uint8_t *buf; + int ch; + + while ((ch = getopt(argc, argv, "a:b:i:l:p:hq46")) != -1) { + switch (ch) { + case 'a': + strlcpy(vmeif_ipstr, optarg, sizeof(vmeif_ipstr)); + break; + case 'b': + sscanf(optarg, "%d", &cpu); + break; + case 'i': + strlcpy(vmeif_name, optarg, sizeof(vmeif_name)); + break; + case 'l': + sscanf(optarg, "%d", &pktsize); + break; + case 'p': + payload = strdup(optarg); + break; + case 'q': + quiet = 1; + break; + case '4': + ipversion = 4; + break; + case '6': + ipversion = 6; + break; + case 'h': + default: + usage(argv[0]); + break; + } + } + + if (vmeif_ipstr[0] == '\0') { + if (ipversion == 4) + strlcpy(vmeif_ipstr, vmeif_ipstr_v4, + sizeof(vmeif_ipstr)); + else + strlcpy(vmeif_ipstr, vmeif_ipstr_v6, + sizeof(vmeif_ipstr)); + } + + if (!quiet) { + printf("vme name: %s\n", vmeif_name); + printf("IP version: %d\n", ipversion); + printf("vme IP: %s\n", vmeif_ipstr); + printf("packet size: %d\n", pktsize); + printf("payload: %s\n", payload); + if (cpu != -1) + printf("cpu: %d\n", cpu); + } + + if (cpu != -1) + bind_cpu(cpu); + + signal(SIGINT, signal_handler); + + snprintf(vmedev, sizeof(vmedev), "/dev/%s", vmeif_name); + + vmefd = open(vmedev, O_RDWR); + if (vmefd == -1) { + fprintf(stderr, "Failed to open %s: %s\n", vmedev, + strerror(errno)); + exit(EXIT_FAILURE); + } + + switch (ipversion) { + case 4: + if (inet_pton(AF_INET, vmeif_ipstr, &vmeif_ip) != 1) { + fprintf(stderr, "Malformed address %s specified\n", + vmeif_ipstr); + exit(EXIT_FAILURE); + } + snprintf(cmd, sizeof(cmd), IFCONFIG_BIN IFCONFIG_ARGS, + vmeif_name, "inet", vmeif_ipstr); + build_udp_packet = build_udp_packet_v4; + break; + case 6: + if (inet_pton(AF_INET6, vmeif_ipstr, &vmeif_ip6) != 1) { + fprintf(stderr, "Malformed address %s specified\n", + vmeif_ipstr); + exit(EXIT_FAILURE); + } + snprintf(cmd, sizeof(cmd), IFCONFIG_BIN IFCONFIG_ARGS, + vmeif_name, "inet6", vmeif_ipstr); + build_udp_packet = build_udp_packet_v6; + break; + default: + fprintf(stderr, "Only support ipv4 and ipv6.\n"); + exit(EXIT_FAILURE); + } + + if (system(cmd) != 0) { + fprintf(stderr, "Failed to setup %s\n", vmeif_name); + exit(EXIT_FAILURE); + } + + if (get_ifaddr(vmeif_name, vmeif_ether) != 0) { + fprintf(stderr, "Failed to get %s's MAC address\n", vmeif_name); + exit(EXIT_FAILURE); + } + + if ((buf = malloc(pktsize)) == NULL) { + perror("Failed to allocate memory for packet"); + exit(EXIT_FAILURE); + } + + /* Keep injecting packets to network stack */ + while (1) { + build_udp_packet(buf, pktsize); + if (write(vmefd, buf, pktsize) == -1) { + perror("write"); + exit(EXIT_FAILURE); + } + total_sent++; + } + + return (0); +} Index: tools/tools/mq-testing/udp/server.c =================================================================== --- /dev/null +++ tools/tools/mq-testing/udp/server.c @@ -0,0 +1,151 @@ +/*- + * Copyright (c) 2015, Tiwei Bie + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice unmodified, this list of conditions, and the following + * disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR + * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES + * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. + * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT + * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +/* $FreeBSD$ */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "common.h" + +static int ipversion = 4; +static int verbose = 0; + +static void +usage(const char *prgname) +{ + fprintf(stderr, "Usage: %s [-46hv]\n", prgname); + exit(EXIT_FAILURE); +} + +static char * +addr2str(int af, struct sockaddr *addr) +{ + static char msg[1024]; + char addrstr[INET6_ADDRSTRLEN]; + + switch (af) { + case AF_INET: + snprintf(msg, sizeof(msg), "%s:%d", + inet_ntoa(((struct sockaddr_in *)addr)->sin_addr), + ntohs(((struct sockaddr_in *)addr)->sin_port)); + break; + case AF_INET6: + snprintf(msg, sizeof(msg), "%s:%d", + inet_ntop(AF_INET6, + &((struct sockaddr_in6 *)addr)->sin6_addr, + addrstr, sizeof(addrstr)), + ntohs(((struct sockaddr_in6 *)addr)->sin6_port)); + break; + } + + return (msg); +} + +int +main(int argc, char **argv) +{ + int sd, af; + struct sockaddr *lsin, *from; + struct sockaddr_in lsin4, from4; + struct sockaddr_in6 lsin6, from6; + socklen_t addrlen, length; + char msg[BUFSIZ]; + int n, ch, i; + + while ((ch = getopt(argc, argv, "46hv")) != -1) { + switch (ch) { + case '4': + ipversion = 4; + break; + case '6': + ipversion = 6; + break; + case 'v': + verbose = 1; + break; + case 'h': + default: + usage(argv[0]); + break; + } + } + + if (ipversion == 4) { + af = AF_INET; + lsin = (struct sockaddr *)&lsin4; + from = (struct sockaddr *)&from4; + addrlen = sizeof(struct sockaddr_in); + + memset(&lsin4, 0, sizeof(lsin4)); + lsin4.sin_family = AF_INET; + lsin4.sin_addr.s_addr = INADDR_ANY; + lsin4.sin_port = htons(SERVER_PORT); + } else { + af = AF_INET6; + lsin = (struct sockaddr *)&lsin6; + from = (struct sockaddr *)&from6; + addrlen = sizeof(struct sockaddr_in6); + + memset(&lsin6, 0, sizeof(lsin6)); + lsin6.sin6_family = AF_INET6; + lsin6.sin6_addr = in6addr_any; + lsin6.sin6_port = htons(SERVER_PORT); + } + + sd = socket(af, SOCK_DGRAM, 0); + if (sd < 0) + err(EXIT_FAILURE, "socket"); + + if (bind(sd, lsin, addrlen) < 0) + err(EXIT_FAILURE, "bind"); + + while (1) { + length = addrlen; + n = recvfrom(sd, msg, sizeof(msg), 0, from, &length); + if (n < 0) + err(EXIT_FAILURE, "recvfrom"); + + if (verbose) { + printf("from=%s len=%d ", addr2str(af, from), n); + for (i = 0; i < n && msg[i]; i++) { + if (isprint(msg[i])) + printf("%c", msg[i]); + } + printf("\n"); + } + } + + return (0); +}