From 6b56dcfb94347af43b0213863b79bcee3530b742 Mon Sep 17 00:00:00 2001
From: rodri <rgl@antares-labs.eu>
Date: Fri, 18 Dec 2020 16:28:05 +0000
Subject: added echo+ support.

---
 Makefile   |  1 +
 dat.h      |  1 +
 echoplus.c | 47 +++++++++++++++++++++++++++++++++++++++++++++++
 fns.h      |  7 +++++++
 main.c     | 29 ++++++++++++++++++++++++++++-
 util.c     | 15 +++++++++++++++
 6 files changed, 99 insertions(+), 1 deletion(-)
 create mode 100644 echoplus.c

diff --git a/Makefile b/Makefile
index 132c7da..7499c81 100644
--- a/Makefile
+++ b/Makefile
@@ -6,6 +6,7 @@ O=o
 TARG=udp7
 OFILES=\
 	main.$O\
+	echoplus.$O\
 	util.$O\
 
 HFILES=\
diff --git a/dat.h b/dat.h
index f900861..e6f6845 100644
--- a/dat.h
+++ b/dat.h
@@ -1,5 +1,6 @@
 #define nil NULL
 
+typedef unsigned char uchar;
 typedef long long vlong;
 typedef unsigned int uint;
 typedef unsigned long ulong;
diff --git a/echoplus.c b/echoplus.c
new file mode 100644
index 0000000..fa83e3d
--- /dev/null
+++ b/echoplus.c
@@ -0,0 +1,47 @@
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdarg.h>
+#include <string.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <utf.h>
+#include <fmt.h>
+#include "dat.h"
+#include "fns.h"
+
+int
+getechoplus(UDPEchoPlus *ephdr, uchar *data)
+{
+	ephdr->gensn = get32(data);
+	data += 4;
+	ephdr->ressn = get32(data);
+	data += 4;
+	ephdr->rxtime = get32(data);
+	data += 4;
+	ephdr->txtime = get32(data);
+	data += 4;
+	ephdr->ntxfails = get32(data);
+	data += 4;
+	ephdr->iterno = get32(data);
+
+	return 0;
+}
+
+void
+putechoplus(UDPEchoPlus *ephdr, uchar *data)
+{
+	put32(data, ephdr->gensn);
+	data += 4;
+	put32(data, ephdr->ressn);
+	data += 4;
+	put32(data, ephdr->rxtime);
+	data += 4;
+	put32(data, ephdr->txtime);
+	data += 4;
+	put32(data, ephdr->ntxfails);
+	data += 4;
+	put32(data, ephdr->iterno);
+}
diff --git a/fns.h b/fns.h
index ae1477c..34f9c5a 100644
--- a/fns.h
+++ b/fns.h
@@ -1,6 +1,13 @@
+/* util */
 void sysfatal(char*, ...);
 void *emalloc(ulong);
 sockaddr_in *mkinetsa(char*, int);
 int listentcp(int);
 int bindudp(int);
 int acceptcall(int, char*, int);
+u32int get32(uchar*);
+void put32(uchar*, u32int);
+
+/* echoplus */
+int getechoplus(UDPEchoPlus*, uchar*);
+void putechoplus(UDPEchoPlus*, uchar*);
diff --git a/main.c b/main.c
index 8d059e9..01ab687 100644
--- a/main.c
+++ b/main.c
@@ -69,9 +69,14 @@ void*
 udpechosrv(void *a)
 {
 	sockaddr_in csa;
-	char buf[4096], caddr[128], *cs;
+	uchar buf[4096];
+	char caddr[128], *cs;
 	int n, port;
 	uint csalen;
+	UDPEchoPlus ephdr;
+
+	struct timespec ts;
+	uvlong us;
 
 	memset(&csa, 0, sizeof(sockaddr_in));
 	csalen = sizeof(sockaddr_in);
@@ -84,6 +89,10 @@ udpechosrv(void *a)
 		}
 		pthread_mutex_unlock(&attendlock);
 
+		clock_gettime(CLOCK_REALTIME, &ts);
+		us = ts.tv_sec*1e6;
+		us += ts.tv_nsec/1e3;
+
 		cs = inet_ntoa(csa.sin_addr);
 		port = ntohs(csa.sin_port);
 		snprint(caddr, sizeof caddr, "udp!%s!%d", cs, port);
@@ -91,6 +100,24 @@ udpechosrv(void *a)
 		if(debug)
 			fprint(2, "thr#%lu received %d byte datagram from %s\n", pthread_self(), n, caddr);
 
+		/* check for echo+ data */
+		if(n >= 24){
+			if(getechoplus(&ephdr, buf) < 0)
+				goto EchoBack;
+
+			ephdr.ressn = ephdr.gensn;
+			ephdr.rxtime = us;
+			ephdr.ntxfails = 0; /* being over-optimistic */
+
+			clock_gettime(CLOCK_REALTIME, &ts);
+			us = ts.tv_sec*1e6;
+			us += ts.tv_nsec/1e3;
+
+			ephdr.txtime = us;
+
+			putechoplus(&ephdr, buf);
+		}
+EchoBack:
 		if(sendto(lfd, buf, n, 0, (sockaddr*)&csa, csalen) < 0)
 			continue;
 
diff --git a/util.c b/util.c
index 26acbc3..9e4dfc9 100644
--- a/util.c
+++ b/util.c
@@ -111,3 +111,18 @@ acceptcall(int lfd, char *caddr, int caddrlen)
 	snprint(caddr, caddrlen, "tcp!%s!%d", cs, port);
 	return fd;
 }
+
+u32int
+get32(uchar *p)
+{
+	return (p[0]<<24) | (p[1]<<16) | (p[2]<<8) | p[3];
+}
+
+void
+put32(uchar *p, u32int v)
+{
+	p[0] = v>>24;
+	p[1] = v>>16;
+	p[2] = v>>8;
+	p[3] = v;
+}
-- 
cgit v1.2.3