diff --git a/scripts/latency-ping b/scripts/latency-ping new file mode 100755 index 00000000000..09a710b10e5 --- /dev/null +++ b/scripts/latency-ping @@ -0,0 +1,89 @@ +#!/bin/bash +SCRIPT_LOCATION=$(dirname "$(readlink -f "$0")"); +if [ -f "$SCRIPT_LOCATION/rip-environment" ] && [ -z "$EMC2_HOME" ]; then + . "$SCRIPT_LOCATION/rip-environment" +fi + +T=$(mktemp -d) +trap 'cd / && [ -d "$T" ] && rm -rf "$T"' SIGINT SIGTERM EXIT +cd "$T" || { echo "E: Cannot change directory to '$T'"; exit 1; } + +calc() { awk "BEGIN { print ($1); }" < /dev/null; } +icalc() { awk "BEGIN { printf \"%.0f\n\", ($1); }" < /dev/null; } + +parse_time () { + case $1 in + -) echo "0" ;; + *ns) icalc "${1%ns}" ;; + *us|*µs) icalc "1000*${1%us}" ;; + *ms) icalc "1000*1000*${1%ms}" ;; + *s) icalc "1000*1000*1000*${1%s}" ;; + *) if [ "$1" -lt 1000 ]; then icalc "1000*$1"; else icalc "$1"; fi ;; + esac +} + +human_time () { + if [ "$1" -eq 0 ]; then echo "-" + elif [ "$1" -ge 1000000000 ]; then echo "$(calc "$1"/1000/1000/1000)s" + elif [ "$1" -ge 1000000 ]; then echo "$(calc "$1"/1000/1000)ms" + elif [ "$1" -ge 1000 ]; then echo "$(calc "$1"/1000)µs" + else echo "$1ns" + fi +} + +usage () { + echo "Usage:" + echo " latency-ping ip_address [if_name] [timeout] [period] # for single thread" + echo "" + echo "Example:" + echo " latency-ping 192.168.122.1 enp1s0 800000 1000000" + echo "" + echo "Defaults: period=${PERIOD}nS" + echo "Equivalently: period=$(human_time "$PERIOD")" + echo "" + echo "Times may be specified with suffix \"s\", \"ms\", \"us\" \"µs\", or \"ns\"" + echo "Times without a suffix and less than 1000 are taken to be in us;" + echo "other times without a suffix are taken to be in ns" + echo "" + echo "If two periods are specified then the shortest time period will" + echo "always be the base-period" + echo "" + echo "The worst-case latency seen in any run of latency-test" + echo "is written to the file ~/.latency" + exit 1 +} + +TIMEOUT=$(parse_time 800us) +PERIOD=$(parse_time 1ms) + +case $1 in + -h|--help) usage;; +esac + +case $# in +1) IP="$1" ;; +2) IP="$1" IF="$2" ;; +3) IP="$1" IF="$2" TIMEOUT=$(parse_time "$3") ;; +4) IP="$1" IF="$2" TIMEOUT=$(parse_time "$3") PERIOD=$(parse_time "$4") ;; +*) usage;; +esac + +if [ -z "$IF" ]; then +cat > lat.hal < lat.hal < +#include +#include +#include +#include +#include +#include +#include +#include + +static char *ip_address = "127.0.0.1"; +RTAPI_MP_STRING(ip_address, "IP address to ping"); + +static char *if_name = NULL; +RTAPI_MP_STRING(if_name, "Interface name to ping"); + +static int timeout = 800000; +RTAPI_MP_INT(timeout, "Timeout in ns"); + +#define PING_PKT_SIZE 64 + +typedef struct{ + char *ip_address; + int sockfd; + struct sockaddr_in addr; + struct arpreq arp_request; + long long int time_start; + long long int time_end; + uint16_t tx_seq; + uint16_t tx_id; + bool send; + unsigned char tbuffer[PING_PKT_SIZE]; + unsigned char rbuffer[2*PING_PKT_SIZE]; +}ping_data; + +static ping_data data={}; + +#undef max +#define max(a,b) ((a)>(b)?(a):(b)) + +#if BYTE_ORDER == LITTLE_ENDIAN +# define ODDBYTE(v) (v) +#elif BYTE_ORDER == BIG_ENDIAN +# define ODDBYTE(v) ((unsigned short)(v) << 8) +#else +# define ODDBYTE(v) htons((unsigned short)(v) << 8) +#endif + +//in_cksum from: https://github.com/iputils/iputils/blob/master/ping/ping.c +//Licence: BSD-3-Clause +static unsigned short +in_cksum(const unsigned short *addr, int len, unsigned short csum) +{ + int nleft = len; + const unsigned short *w = addr; + unsigned short answer; + int sum = csum; + + /* + * Our algorithm is simple, using a 32 bit accumulator (sum), + * we add sequential 16 bit words to it, and at the end, fold + * back all the carry bits from the top 16 bits into the lower + * 16 bits. + */ + while (nleft > 1) { + sum += *w++; + nleft -= 2; + } + + /* mop up an odd byte, if necessary */ + if (nleft == 1) + sum += ODDBYTE(*(unsigned char *)w); /* le16toh() may be unavailable on old systems */ + + /* + * add back carry outs from top 16 bits to low 16 bits + */ + sum = (sum >> 16) + (sum & 0xffff); /* add hi 16 to low 16 */ + sum += (sum >> 16); /* add carry */ + answer = ~sum; /* truncate to 16 bits */ + return (answer); +} + +bool setup_ping(ping_data *data){ + int sockfd; + data->addr.sin_family = AF_INET; + data->addr.sin_port = htons(0); + data->addr.sin_addr.s_addr = inet_addr(data->ip_address); + + // Create socket + sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_ICMP); + if (sockfd < 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: socket %m"); + return false; + } + + int ttl_val = 64; + // Set socket options at IP to TTL 64 + if(setsockopt(sockfd, SOL_IP, IP_TTL, &ttl_val, sizeof(ttl_val)) != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: setsockopt IP_TTL %m\n"); + close(sockfd); + return false; + } + + struct timeval tv; + tv.tv_sec = timeout/1000000000; + tv.tv_usec = (timeout/1000)%1000000; + + // Setting timeout + if(setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv) != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: setsockopt SO_RCVTIMEO %m\n"); + close(sockfd); + return false; + } + data->tx_id=getpid(); + data->sockfd=sockfd; + data->send=true; + return true; +} + +bool setup_arp(ping_data *data, char* if_name) { + int ret = 0; + memset(&data->arp_request, 0, sizeof(data->arp_request)); + ((struct sockaddr_in *)&data->arp_request.arp_pa)->sin_family = AF_INET; + ((struct sockaddr_in *)&data->arp_request.arp_pa)->sin_addr = data->addr.sin_addr; + strncpy(data->arp_request.arp_dev, if_name, sizeof(data->arp_request.arp_dev)-1); + ret = ioctl(data->sockfd, SIOCGARP, &data->arp_request); + if(ret < 0){ + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: ioctl SIOCGARP %m\n"); + return false; + } + if(!(data->arp_request.arp_flags | ATF_COM)) { + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: arp request failed\n"); + return false; + } else { + rtapi_print_msg(RTAPI_MSG_ERR, "ping: ip=%s mac=%.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n", + data->ip_address, + (uint8_t)data->arp_request.arp_ha.sa_data[0], + (uint8_t)data->arp_request.arp_ha.sa_data[1], + (uint8_t)data->arp_request.arp_ha.sa_data[2], + (uint8_t)data->arp_request.arp_ha.sa_data[3], + (uint8_t)data->arp_request.arp_ha.sa_data[4], + (uint8_t)data->arp_request.arp_ha.sa_data[5]); + } + + data->arp_request.arp_flags |= ATF_PERM; + ret = ioctl(data->sockfd, SIOCSARP, &data->arp_request); + if(ret < 0){ + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: ioctl SIOCSARP %m\n"); + return false; + } + + return true; +} + +bool cleanup_arp(ping_data *data){ + int ret = ioctl(data->sockfd, SIOCDARP, &data->arp_request); + if(ret < 0){ + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: ioctl SIOCSARP %m\n"); + return false; + } + return true; +} + +bool send_ping(ping_data *data) { + struct icmphdr *hdr = (struct icmphdr *)(data->tbuffer); + unsigned char *msg = data->tbuffer + sizeof(struct icmphdr); + + // Fill the packet + bzero(&data->tbuffer, sizeof(data->tbuffer)); + hdr->type = ICMP_ECHO; + hdr->un.echo.id = htons(data->tx_id); + + for (size_t i = 0; i < PING_PKT_SIZE - sizeof(struct icmphdr); i++) + msg[i] = i + '0'; + + hdr->un.echo.sequence = htons(data->tx_seq); + hdr->checksum = in_cksum((unsigned short *)data->tbuffer, sizeof(data->tbuffer), 0); + + // Send packet + data->time_start = rtapi_get_time(); + ssize_t ret = sendto(data->sockfd, data->tbuffer, sizeof(data->tbuffer), 0, (struct sockaddr*)&data->addr, sizeof(data->addr)); + if (ret <= 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: sendto %m\n"); + return false; + }else{ + return true; + } +} + +enum{ + ERR_TIMEOUT = -1, + ERR_SKIP = -2, + ERR_CORRUPT = -3 +}; + +rtapi_s32 recv_ping(ping_data *data) { + struct sockaddr_in r_addr; + rtapi_s32 rtt_nsec = 0; + + // Receive packet + socklen_t addr_len = sizeof(r_addr); + ssize_t ret = recvfrom(data->sockfd, data->rbuffer, sizeof(data->rbuffer), 0, (struct sockaddr*)&r_addr, &addr_len); + if (ret <= 0) { + if( errno == EAGAIN){ + return ERR_TIMEOUT; + }else{ + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: recvfrom failed: %m\n"); + return ERR_CORRUPT; + } + } else { + data->time_end = rtapi_get_time(); + + struct icmphdr *recv_hdr = (struct icmphdr *)(data->rbuffer); + unsigned short checksum = in_cksum((unsigned short *)data->rbuffer, ret, 0); + uint16_t rx_seq=ntohs(recv_hdr->un.echo.sequence); + if (checksum != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: received checksum wrong\n"); + return ERR_CORRUPT; + } else if (recv_hdr->type != 0 || recv_hdr->code != 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: package corrupt: received size %li with ICMP type %d code %d tx_seq %d rx_seq %d\n", ret, recv_hdr->type, recv_hdr->code, data->tx_seq, rx_seq); + return ERR_CORRUPT; + } else if (data->tx_seq > rx_seq) { + //Probably did skiped a package due to timeout, don't send on next round, so we get to the correct sequence again + return ERR_SKIP; + } else if (data->tx_seq < rx_seq) { + //The sequence of the received package should never be > than the sent one + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: sequence wrong: received size %li with ICMP type %d code %d tx_seq %d rx_seq %d\n", ret, recv_hdr->type, recv_hdr->code, data->tx_seq, rx_seq); + return ERR_CORRUPT; + } else { + rtt_nsec = data->time_end - data->time_start; + //rtapi_print_msg(RTAPI_MSG_ERR, "%li bytes from %s msg_seq = %d rtt = %.3f us.\n", ret, data->ip_address, rx_seq, (double)rtt_nsec/1000); + } + data->tx_seq ++; + } + + return rtt_nsec; +} + +// Note: the extra setup is run _before_ the pins are created +EXTRA_SETUP() { + (void)prefix; + (void)extra_arg; + (void)__comp_inst; + data.ip_address=ip_address; + if(!setup_ping(&data)){ + return -1; + } + + if(if_name == NULL){ + rtapi_print_msg(RTAPI_MSG_ERR, "ping: warning: if_name not given, can not set\n" + " static arp. Might increase latency.\n"); + } else { + //Ping once to resolve arp + if(send_ping(&data)){ + recv_ping(&data); + } + + if(!setup_arp(&data, if_name)){ + return -1; + } + } + + return 0; +} + +EXTRA_CLEANUP() { + if(if_name != NULL){ + cleanup_arp(&data); + } +} + +FUNCTION(_) { + (void)period; + bool ret = true; + if(data.send){ + ret = send_ping(&data); + } + if(ret){ + rtapi_s32 rtt = recv_ping(&data); + data.send = true; + if(rtt == ERR_TIMEOUT){ + ping_timeout ++; + }else if(rtt == ERR_CORRUPT){ + ping_err ++; + }else if(rtt == ERR_SKIP){ + data.send = false; + }else if(rtt >= 0){ + ping_rtt = rtt; + ping_rtt_max = max(ping_rtt_max, rtt); + }else{ + rtapi_print_msg(RTAPI_MSG_ERR, "ping: error: bug"); + } + } + if(reset) { + ping_timeout = 0; + ping_err = 0; + ping_rtt_max = 0; + } +} +#endif