Commit 7255f628 authored by Alessandro Rubini's avatar Alessandro Rubini

tools/mtp: new tool-set, still to be cleaned up and documented

This is the verbatim copy of what I wrote as example code for an
article about time stamping. It's called "mini time protocol".  Some
of the code, in turn, comes from my previous experiments with White
Rabbit.

I'd better have the programs here as I use them often. As time permits
I'll add some more features and documentation in the ppsi manual.
Signed-off-by: Alessandro Rubini's avatarAlessandro Rubini <rubini@gnudd.com>
parent c7cc61d8
mtp_udp
mtp_packet
mtp_stamp
onestamp
CFLAGS = -Wall -O2 -ggdb
OBJ = mtp_udp.o mtp_packet.o onestamp.o mtp_stamp.o
PRG = $(OBJ:.o=)
LIB = libmtp.a
LOBJ = stamp-funcs.o report.o
LDFLAGS = -L. -lmtp
all: $(OBJ) $(PRG)
$(LIB): $(LOBJ)
ar r $@ $^
%: %.o $(LIB)
$(CC) $*.o $(LDFLAGS) -o $@
clean:
rm -f $(PRG) $(LIB) *.o *~
\ No newline at end of file
/* Common stuff for these misc tools */
#include <sys/types.h>
#ifndef SO_TIMESTAMPING
# define SO_TIMESTAMPING 37
# define SCM_TIMESTAMPING SO_TIMESTAMPING
#endif
#ifndef SIOCSHWTSTAMP
# define SIOCSHWTSTAMP 0x89b0
#endif
#ifndef ETH_P_1588
# define ETH_P_1588 0x88F7
#endif
extern int make_stamping_socket(FILE *errchan, char *argv0, char *ifname,
int tx_type, int rx_filter, int bits,
unsigned char *macaddr, int proto);
extern ssize_t send_and_stamp(int sock, void *buf, size_t len, int flags);
extern ssize_t recv_and_stamp(int sock, void *buf, size_t len, int flags);
extern int print_stamp(FILE *out, char *prefix, FILE *err /* may be NULL */);
extern int get_stamp(struct timespec ts[4]);
/* Functions to make the difference between timevals and timespecs */
static inline int tvdiff(struct timeval *tv2, struct timeval *tv1)
{
return (tv2->tv_sec - tv1->tv_sec) * 1000 * 1000
+ tv2->tv_usec - tv1->tv_usec;
}
static inline int tsdiff(struct timespec *tv2, struct timespec *tv1)
{
return (tv2->tv_sec - tv1->tv_sec) * 1000 * 1000 * 1000
+ tv2->tv_nsec - tv1->tv_nsec;
}
#include <stdint.h>
enum mtp_ptype {
MTP_FORWARD = 0xb8,
MTP_BACKWARD,
MTP_BACKSTAMP
};
struct mtp_packet {
uint32_t tragic;
uint32_t ptype;
struct timespec t[4][3];
};
static inline void tv_to_ts(struct timeval *tv, struct timespec *ts)
{
ts->tv_sec = tv->tv_sec;
ts->tv_nsec = tv->tv_usec * 1000;
}
extern void mtp_result(struct mtp_packet *pkt);
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <time.h>
#include <string.h>
#include <errno.h>
#include <netdb.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netpacket/packet.h>
#include <net/ethernet.h>
#include <net/if.h>
#include "mtp.h"
#define MTP_PROTO 0x6d74 /* 'm' 't' */
struct eth_packet {
struct ethhdr hdr;
struct mtp_packet mtp;
};
static void run_passive_host(int argc, char **argv, int sock,
unsigned char *ourmac)
{
int i;
socklen_t slen;
struct sockaddr_ll addr;
struct timeval tv1, tv2;
struct eth_packet pkt;
/* get forward packet and stamp it */
slen = sizeof(addr);
i = recvfrom(sock, &pkt, sizeof(pkt), MSG_TRUNC,
(struct sockaddr *)&addr, &slen);
gettimeofday(&tv1, NULL);
if (i < 0) {
fprintf(stderr, "%s: recvfrom(): %s\n", argv[0],
strerror(errno));
exit(1);
}
if (i < sizeof(pkt)) {
fprintf(stderr, "%s: short packet 1\n", argv[0]);
exit(1);
}
if (pkt.mtp.ptype != MTP_FORWARD) {
fprintf(stderr, "%s: wrong packet 1\n", argv[0]);
exit(1);
}
/* send backward packet -- swap macaddress */
memcpy(pkt.hdr.h_dest, pkt.hdr.h_source, ETH_ALEN);
memcpy(pkt.hdr.h_source, ourmac, ETH_ALEN);
pkt.mtp.ptype = MTP_BACKWARD;
gettimeofday(&tv2, NULL);
send(sock, &pkt, sizeof(pkt), 0);
/* send stamps */
tv_to_ts(&tv1, pkt.mtp.t[1]);
tv_to_ts(&tv2, pkt.mtp.t[2]);
pkt.mtp.ptype = MTP_BACKSTAMP;
send(sock, &pkt, sizeof(pkt), 0);
}
static int run_active_host(int argc, char **argv, int sock,
struct sockaddr_ll *addr, unsigned char *ourmac)
{
int i, tragic;
socklen_t slen;
struct timeval tv0, tv3;
struct eth_packet pkt;
char othermac[ETH_ALEN];
/* retrieve the remote mac*/
if (sscanf(argv[2], "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
othermac+0, othermac+1, othermac+2,
othermac+3, othermac+4, othermac+5) != ETH_ALEN) {
fprintf(stderr, "%s: %s: can't parse macaddress\n",
argv[0], argv[2]);
exit(1);
}
/* stamp and send the first packet */
memset(&pkt, 0, sizeof(pkt));
memcpy(pkt.hdr.h_dest, othermac, ETH_ALEN);
memcpy(pkt.hdr.h_source, ourmac, ETH_ALEN);
pkt.hdr.h_proto = addr->sll_protocol;
pkt.mtp.ptype = MTP_FORWARD;
srand(time(NULL));
pkt.mtp.tragic = tragic = rand();
gettimeofday(&tv0, NULL);
send(sock, &pkt, sizeof(pkt), 0);
/* get the second packet -- and discard it */
slen = sizeof(*addr);
i = recvfrom(sock, &pkt, sizeof(pkt), MSG_TRUNC,
(struct sockaddr *)addr, &slen);
gettimeofday(&tv3, NULL);
if (i < 0) {
fprintf(stderr, "%s: recvfrom(): %s\n", argv[0],
strerror(errno));
exit(1);
}
if (i < sizeof(pkt)) {
fprintf(stderr, "%s: short packet 1\n", argv[0]);
exit(1);
}
if (pkt.mtp.ptype != MTP_BACKWARD || pkt.mtp.tragic != tragic) {
fprintf(stderr, "%s: wrong packet 1\n", argv[0]);
exit(1);
}
/* get the final packet */
slen = sizeof(*addr);
i = recvfrom(sock, &pkt, sizeof(pkt), MSG_TRUNC,
(struct sockaddr *)addr, &slen);
if (i < 0) {
fprintf(stderr, "%s: recvfrom(): %s\n", argv[0],
strerror(errno));
exit(1);
}
if (i < sizeof(pkt)) {
fprintf(stderr, "%s: short packet 2\n", argv[0]);
exit(1);
}
if (pkt.mtp.ptype != MTP_BACKSTAMP || pkt.mtp.tragic != tragic) {
fprintf(stderr, "%s: wrong packet 2\n", argv[0]);
exit(1);
}
/* add our stamp, we are using only the first value */
tv_to_ts(&tv0, pkt.mtp.t[0]);
tv_to_ts(&tv3, pkt.mtp.t[3]);
mtp_result(&pkt.mtp);
return 0;
}
int main(int argc, char **argv)
{
int sock, iindex, mtp_listen = 0;
struct sockaddr_ll addr;
struct ifreq ifr;
unsigned char ourmac[ETH_ALEN];
if (argc != 3) {
fprintf(stderr, "%s: Use: \"%s <eth> -l\" or "
"\"%s <eth> <macaddress>\"\n",
argv[0], argv[0], argv[0]);
exit(1);
}
if (!strcmp(argv[2], "-l")) {
mtp_listen = 1;
}
/* open file descriptor */
sock = socket(AF_PACKET, SOCK_RAW, ETH_P_ALL); //htons(MTP_PROTO));
if (sock < 0) {
fprintf(stderr, "%s: socket(): %s\n", argv[0],
strerror(errno));
exit(1);
}
/* get interface index */
memset (&ifr, 0, sizeof(ifr));
strcpy(ifr.ifr_name, argv[1]);
if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0) {
fprintf(stderr, "%s: SIOCGIFINDEX(%s): %s\n", argv[0],
argv[1], strerror(errno));
exit(1);
}
iindex = ifr.ifr_ifindex;
/* get our mac address */
if (ioctl(sock, SIOCGIFHWADDR, &ifr) < 0) {
fprintf(stderr, "%s: SIOCGIFHWADDR(%s): %s\n", argv[0],
argv[1], strerror(errno));
exit(1);
}
memcpy(ourmac, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
/* bind to a port, so we can get data */
memset(&addr, 0, sizeof(addr));
addr.sll_family = AF_PACKET;
addr.sll_protocol = htons(MTP_PROTO);
addr.sll_ifindex = iindex;
if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
fprintf(stderr, "%s: bind(%s): %s\n", argv[0], argv[1],
strerror(errno));
exit(1);
}
if (!mtp_listen)
return run_active_host(argc, argv, sock, &addr, ourmac);
while (1)
run_passive_host(argc, argv, sock, ourmac);
}
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <time.h>
#include <string.h>
#include <errno.h>
#include <netdb.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netpacket/packet.h>
#include <net/ethernet.h>
#include <net/if.h>
#include "mtp.h"
#include "net_tstamp.h" /* copied from Linux headers */
#include "misc-common.h"
#define MTP_PROTO 0x7878 //0x6d74 /* 'm' 't' */
struct eth_packet {
struct ethhdr hdr;
struct mtp_packet mtp;
};
static void run_passive_host(int argc, char **argv, int sock,
unsigned char *ourmac)
{
int i;
socklen_t slen;
struct sockaddr_ll addr;
struct timespec ts1[4], ts2[4];
struct eth_packet pkt;
/* get forward packet and stamp it */
slen = sizeof(addr);
i = recv_and_stamp(sock, &pkt, sizeof(pkt), MSG_TRUNC);
if (i < 0) {
fprintf(stderr, "%s: recvfrom(): %s\n", argv[0],
strerror(errno));
exit(1);
}
if (i < sizeof(pkt)) {
fprintf(stderr, "%s: short packet 1\n", argv[0]);
exit(1);
}
if (pkt.mtp.ptype != MTP_FORWARD) {
fprintf(stderr, "%s: wrong packet 1\n", argv[0]);
exit(1);
}
get_stamp(ts1); /* the 4 stamps collected by recv_and_stamp() */
/* Wait a while, so our peer can get the tx-done interrupt */
usleep(100);
/* send backward packet -- swap macaddress */
memcpy(pkt.hdr.h_dest, pkt.hdr.h_source, ETH_ALEN);
memcpy(pkt.hdr.h_source, ourmac, ETH_ALEN);
pkt.mtp.ptype = MTP_BACKWARD;
send_and_stamp(sock, &pkt, sizeof(pkt), 0);
get_stamp(ts2); /* the 4 stamps collected by recv_and_stamp() */
/*
* fill the packet: the first ts is the same as the second
* (see onestamp output), so only copy the next three
*/
memcpy(pkt.mtp.t[1], ts1+1, sizeof(pkt.mtp.t[1]));
memcpy(pkt.mtp.t[2], ts2+1, sizeof(pkt.mtp.t[2]));
pkt.mtp.ptype = MTP_BACKSTAMP;
/* se don't use the stamp but the message queue must be emptied */
send_and_stamp(sock, &pkt, sizeof(pkt), 0);
}
static int run_active_host(int argc, char **argv, int sock,
unsigned char *ourmac)
{
int i, tragic;
struct timespec ts0[4], ts3[4];
struct eth_packet pkt;
char othermac[ETH_ALEN];
/* retrieve the remote mac*/
if (sscanf(argv[2], "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
othermac+0, othermac+1, othermac+2,
othermac+3, othermac+4, othermac+5) != ETH_ALEN) {
fprintf(stderr, "%s: %s: can't parse macaddress\n",
argv[0], argv[2]);
exit(1);
}
/* stamp and send the first packet */
memset(&pkt, 0, sizeof(pkt));
memcpy(pkt.hdr.h_dest, othermac, ETH_ALEN);
memcpy(pkt.hdr.h_source, ourmac, ETH_ALEN);
pkt.hdr.h_proto = ntohs(MTP_PROTO);
pkt.mtp.ptype = MTP_FORWARD;
srand(time(NULL));
pkt.mtp.tragic = tragic = rand();
send_and_stamp(sock, &pkt, sizeof(pkt), 0);
get_stamp(ts0);
/* get the second packet -- and discard it */
i = recv_and_stamp(sock, &pkt, sizeof(pkt), MSG_TRUNC);
if (i < 0) {
fprintf(stderr, "%s: recvfrom(): %s\n", argv[0],
strerror(errno));
exit(1);
}
if (i < sizeof(pkt)) {
fprintf(stderr, "%s: short packet 1\n", argv[0]);
exit(1);
}
if (pkt.mtp.ptype != MTP_BACKWARD || pkt.mtp.tragic != tragic) {
fprintf(stderr, "%s: wrong packet 1\n", argv[0]);
exit(1);
}
get_stamp(ts3);
/* get the final packet */
i = recv(sock, &pkt, sizeof(pkt), MSG_TRUNC);
if (i < 0) {
fprintf(stderr, "%s: recvfrom(): %s\n", argv[0],
strerror(errno));
exit(1);
}
if (i < sizeof(pkt)) {
fprintf(stderr, "%s: short packet 2\n", argv[0]);
exit(1);
}
if (pkt.mtp.ptype != MTP_BACKSTAMP || pkt.mtp.tragic != tragic) {
fprintf(stderr, "%s: wrong packet 2\n", argv[0]);
exit(1);
}
/* add our stamp, we are using only the first value */
memcpy(pkt.mtp.t[0], ts0+1, sizeof(pkt.mtp.t[0]));
memcpy(pkt.mtp.t[3], ts3+1, sizeof(pkt.mtp.t[3]));
mtp_result(&pkt.mtp);
return 0;
}
int main(int argc, char **argv)
{
int sock, mtp_listen = 0;
unsigned char ourmac[ETH_ALEN];
if (argc != 3) {
fprintf(stderr, "%s: Use: \"%s <eth> -l\" or "
"\"%s <eth> <macaddress>\"\n",
argv[0], argv[0], argv[0]);
exit(1);
}
if (!strcmp(argv[2], "-l")) {
mtp_listen = 1;
}
/* open file descriptor */
sock = make_stamping_socket(stderr, argv[0], argv[1],
HWTSTAMP_TX_ON, HWTSTAMP_FILTER_ALL,
127 /* all bits for stamping */,
ourmac, MTP_PROTO);
if (sock < 0) /* message already printed */
exit(1);
if (!mtp_listen)
return run_active_host(argc, argv, sock, ourmac);
while (1)
run_passive_host(argc, argv, sock, ourmac);
}
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <time.h>
#include <string.h>
#include <errno.h>
#include <netdb.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include "mtp.h"
#define MTP_PORT 0x6d74 /* 'm' 't' */
static void run_passive_host(int argc, char **argv, int sock)
{
int i;
socklen_t slen;
struct sockaddr_in addr;
struct timeval tv1, tv2;
struct mtp_packet pkt;
/* get forward packet and stamp it */
slen = sizeof(addr);
i = recvfrom(sock, &pkt, sizeof(pkt), MSG_TRUNC,
(struct sockaddr *)&addr, &slen);
gettimeofday(&tv1, NULL);
if (i < 0) {
fprintf(stderr, "%s: recvfrom(): %s\n", argv[0],
strerror(errno));
exit(1);
}
if (i < sizeof(pkt)) {
fprintf(stderr, "%s: short packet 1\n", argv[0]);
exit(1);
}
if (pkt.ptype != MTP_FORWARD) {
fprintf(stderr, "%s: wrong packet 1\n", argv[0]);
exit(1);
}
/* send backward packet */
pkt.ptype = MTP_BACKWARD;
gettimeofday(&tv2, NULL);
sendto(sock, &pkt, sizeof(pkt), 0,
(struct sockaddr *)&addr, sizeof(addr));
/* send stamps */
tv_to_ts(&tv1, pkt.t[1]);
tv_to_ts(&tv2, pkt.t[2]);
pkt.ptype = MTP_BACKSTAMP;
sendto(sock, &pkt, sizeof(pkt), 0,
(struct sockaddr *)&addr, sizeof(addr));
}
static int run_active_host(int argc, char **argv, int sock,
struct sockaddr_in *addr)
{
int i, tragic;
socklen_t slen;
struct hostent *h;
struct timeval tv0, tv3;
struct mtp_packet pkt;
/* retrieve the remote host */
h = gethostbyname(argv[1]);
if (!h) {
fprintf(stderr, "%s: %s: can't resolve hostname\n",
argv[0], argv[1]);
exit(1);
}
addr->sin_addr.s_addr = *(uint32_t *)h->h_addr;
/* stamp and send the first packet */
memset(&pkt, 0, sizeof(pkt));
pkt.ptype = MTP_FORWARD;
srand(time(NULL));
pkt.tragic = tragic = rand();
gettimeofday(&tv0, NULL);
sendto(sock, &pkt, sizeof(pkt), 0,
(struct sockaddr *)addr, sizeof(*addr));
/* get the second packet -- stamp and discard it */
slen = sizeof(*addr);
i = recvfrom(sock, &pkt, sizeof(pkt), MSG_TRUNC,
(struct sockaddr *)addr, &slen);
gettimeofday(&tv3, NULL);
if (i < 0) {
fprintf(stderr, "%s: recvfrom(): %s\n", argv[0],
strerror(errno));
exit(1);
}
if (i < sizeof(pkt)) {
fprintf(stderr, "%s: short packet 1\n", argv[0]);
exit(1);
}
if (pkt.ptype != MTP_BACKWARD || pkt.tragic != tragic) {
fprintf(stderr, "%s: wrong packet 1\n", argv[0]);
exit(1);
}
/* get the final packet */
slen = sizeof(*addr);
i = recvfrom(sock, &pkt, sizeof(pkt), MSG_TRUNC,
(struct sockaddr *)addr, &slen);
if (i < 0) {
fprintf(stderr, "%s: recvfrom(): %s\n", argv[0],
strerror(errno));
exit(1);
}
if (i < sizeof(pkt)) {
fprintf(stderr, "%s: short packet 2\n", argv[0]);
exit(1);
}
if (pkt.ptype != MTP_BACKSTAMP || pkt.tragic != tragic) {
fprintf(stderr, "%s: wrong packet 2\n", argv[0]);
exit(1);
}
/* add our stamp, we are using only the first value */
tv_to_ts(&tv0, pkt.t[0]);
tv_to_ts(&tv3, pkt.t[3]);
mtp_result(&pkt);
return 0;
}
int main(int argc, char **argv)
{
int sock, mtp_listen = 0;
struct sockaddr_in addr;
if (argc != 2) {
fprintf(stderr, "%s: Use: \"%s -l\" or \"%s <host>\"\n",
argv[0], argv[0], argv[0]);
exit(1);
}
if (!strcmp(argv[1], "-l")) {
mtp_listen = 1;
}
/* open file descriptor */
sock = socket(AF_INET, SOCK_DGRAM, 0);
if (sock < 0) {
fprintf(stderr, "%s: socket(): %s\n", argv[0],
strerror(errno));
exit(1);
}
/* bind to a port, so we can get data */
memset(&addr, 0, sizeof(addr));
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = INADDR_ANY;
addr.sin_port = htons(MTP_PORT);
if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
fprintf(stderr, "%s: bind(): %s\n", argv[0],
strerror(errno));
exit(1);
}
if (!mtp_listen)
return run_active_host(argc, argv, sock, &addr);
while (1)
run_passive_host(argc, argv, sock);
}
/*
* Userspace API for hardware time stamping of network packets
*
* Copyright (C) 2008,2009 Intel Corporation
* Author: Patrick Ohly <patrick.ohly@intel.com>
*
*/
#ifndef _NET_TIMESTAMPING_H
#define _NET_TIMESTAMPING_H
#include <linux/socket.h> /* for SO_TIMESTAMPING */
/* SO_TIMESTAMPING gets an integer bit field comprised of these values */
enum {
SOF_TIMESTAMPING_TX_HARDWARE = (1<<0),
SOF_TIMESTAMPING_TX_SOFTWARE = (1<<1),
SOF_TIMESTAMPING_RX_HARDWARE = (1<<2),
SOF_TIMESTAMPING_RX_SOFTWARE = (1<<3),
SOF_TIMESTAMPING_SOFTWARE = (1<<4),
SOF_TIMESTAMPING_SYS_HARDWARE = (1<<5),
SOF_TIMESTAMPING_RAW_HARDWARE = (1<<6),
SOF_TIMESTAMPING_MASK =
(SOF_TIMESTAMPING_RAW_HARDWARE - 1) |
SOF_TIMESTAMPING_RAW_HARDWARE
};
/**
* struct hwtstamp_config - %SIOCSHWTSTAMP parameter
*
* @flags: no flags defined right now, must be zero
* @tx_type: one of HWTSTAMP_TX_*
* @rx_type: one of one of HWTSTAMP_FILTER_*
*
* %SIOCSHWTSTAMP expects a &struct ifreq with a ifr_data pointer to
* this structure. dev_ifsioc() in the kernel takes care of the
* translation between 32 bit userspace and 64 bit kernel. The
* structure is intentionally chosen so that it has the same layout on
* 32 and 64 bit systems, don't break this!
*/
struct hwtstamp_config {
int flags;
int tx_type;
int rx_filter;
};
/* possible values for hwtstamp_config->tx_type */
enum {
/*
* No outgoing packet will need hardware time stamping;
* should a packet arrive which asks for it, no hardware
* time stamping will be done.
*/
HWTSTAMP_TX_OFF,
/*
* Enables hardware time stamping for outgoing packets;
* the sender of the packet decides which are to be
* time stamped by setting %SOF_TIMESTAMPING_TX_SOFTWARE
* before sending the packet.
*/
HWTSTAMP_TX_ON,
};
/* possible values for hwtstamp_config->rx_filter */
enum {
/* time stamp no incoming packet at all */
HWTSTAMP_FILTER_NONE,
/* time stamp any incoming packet */
HWTSTAMP_FILTER_ALL,
/* return value: time stamp all packets requested plus some others */
HWTSTAMP_FILTER_SOME,
/* PTP v1, UDP, any kind of event packet */
HWTSTAMP_FILTER_PTP_V1_L4_EVENT,
/* PTP v1, UDP, Sync packet */
HWTSTAMP_FILTER_PTP_V1_L4_SYNC,
/* PTP v1, UDP, Delay_req packet */
HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ,
/* PTP v2, UDP, any kind of event packet */
HWTSTAMP_FILTER_PTP_V2_L4_EVENT,
/* PTP v2, UDP, Sync packet */