add Vendor TP WTP Timestamp to Echo Request

build/Makefile_common.am

@@ -102,7 +102,8 @@ capwap_SOURCES = $(top_srcdir)/src/common/capwap.c \
 	$(top_srcdir)/src/common/capwap_element_80211_wtpradiofailalarm.c \
 	$(top_srcdir)/src/common/capwap_element_80211_wtpradioinformation.c \
 	$(top_srcdir)/src/common/capwap_element_80211n_radioconf.c \
-	$(top_srcdir)/src/common/capwap_element_80211n_station_information.c
+	$(top_srcdir)/src/common/capwap_element_80211n_station_information.c \
+	$(top_srcdir)/src/common/capwap_element_vendor_travelping_wtp_timestamp.c
 
 if DEBUG_BUILD
 capwap_SOURCES += $(top_srcdir)/src/common/capwap_debug.c

src/common/capwap_element.c

@@ -91,8 +91,9 @@ static const struct capwap_message_elements_ops * capwap_80211_message_elements[
 /* */
 #define element_ops(Id, Ops) [(Id) - 1] = &(Ops)
 static const struct capwap_message_elements_ops * capwap_vendor_travelping_message_elements[] = {
-	element_ops(CAPWAP_ELEMENT_80211N_RADIO_CONF_TYPE,	capwap_element_80211n_radioconf_ops),
+	element_ops(CAPWAP_ELEMENT_VENDOR_TRAVELPING_WTP_TIMESTAMP_TYPE,	capwap_element_vendor_travelping_wtp_timestamp_ops),
-	element_ops(CAPWAP_ELEMENT_80211N_STATION_INFO_TYPE,	capwap_element_80211n_station_info_ops)
+	element_ops(CAPWAP_ELEMENT_80211N_RADIO_CONF_TYPE,			capwap_element_80211n_radioconf_ops),
+	element_ops(CAPWAP_ELEMENT_80211N_STATION_INFO_TYPE,			capwap_element_80211n_station_info_ops)
 };
 #undef element_ops
 

src/common/capwap_element_vendor_travelping_wtp_timestamp.c

@@ -0,0 +1,109 @@
+#include "capwap.h"
+#include "capwap_element.h"
+
+/*
+ *    0                   1                   2                   3
+ *    0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *   |                           Second                              |
+ *   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *   |                           Fraction                            |
+ *   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ * WTP Timestamp
+ *
+ * Vendor Id:  18681 (Travelping GmbH)
+ * Type:       2
+ *
+ */
+
+struct ntp_time_t {
+        uint32_t   second;
+        uint32_t   fraction;
+};
+
+static inline void convert_ntp_time_into_unix_time(struct ntp_time_t *ntp, struct timeval *tv)
+{
+    tv->tv_sec = ntp->second - 0x83AA7E80; // the seconds from Jan 1, 1900 to Jan 1, 1970
+    tv->tv_usec = (uint32_t)( (double)ntp->fraction * 1.0e6 / (double)(1LL<<32) );
+}
+
+static inline void convert_unix_time_into_ntp_time(struct timeval *tv, struct ntp_time_t *ntp)
+{
+    ntp->second = tv->tv_sec + 0x83AA7E80;
+    ntp->fraction = (uint32_t)( (double)(tv->tv_usec+1) * (double)(1LL<<32) * 1.0e-6 );
+}
+
+/* */
+static void
+capwap_vendor_travelping_wtp_timestamp_element_create(void *data,
+						      capwap_message_elements_handle handle,
+						      struct capwap_write_message_elements_ops *func)
+{
+	struct capwap_vendor_travelping_wtp_timestamp_element *element =
+		(struct capwap_vendor_travelping_wtp_timestamp_element *)data;
+	struct ntp_time_t ntp;
+
+	ASSERT(data != NULL);
+
+	convert_unix_time_into_ntp_time(&element->tv, &ntp);
+
+	func->write_u32(handle, ntp.second);
+	func->write_u32(handle, ntp.fraction);
+}
+
+/* */
+static void *
+capwap_vendor_travelping_wtp_timestamp_element_parsing(capwap_message_elements_handle handle,
+						       struct capwap_read_message_elements_ops *func)
+{
+	struct capwap_vendor_travelping_wtp_timestamp_element *data;
+	struct ntp_time_t ntp;
+
+	ASSERT(handle != NULL);
+	ASSERT(func != NULL);
+
+	if (func->read_ready(handle) != 8) {
+		log_printf(LOG_DEBUG, "Invalid Vendor Travelping WTP Timestamp element");
+		return NULL;
+	}
+
+	/* */
+	data = (struct capwap_vendor_travelping_wtp_timestamp_element *)
+		capwap_alloc(sizeof(struct capwap_vendor_travelping_wtp_timestamp_element));
+
+	/* Retrieve data */
+	func->read_u32(handle, &ntp.second);
+	func->read_u32(handle, &ntp.fraction);
+
+	convert_ntp_time_into_unix_time(&ntp, &data->tv);
+
+	return data;
+}
+
+/* */
+static void *
+capwap_vendor_travelping_wtp_timestamp_element_clone(void *data)
+{
+	ASSERT(data != NULL);
+
+	return capwap_clone(data, sizeof(struct capwap_vendor_travelping_wtp_timestamp_element));
+}
+
+/* */
+static void
+capwap_vendor_travelping_wtp_timestamp_element_free(void* data)
+{
+	ASSERT(data != NULL);
+
+	capwap_free(data);
+}
+
+/* */
+const struct capwap_message_elements_ops capwap_element_vendor_travelping_wtp_timestamp_ops = {
+	.category = CAPWAP_MESSAGE_ELEMENT_SINGLE,
+	.create = capwap_vendor_travelping_wtp_timestamp_element_create,
+	.parse = capwap_vendor_travelping_wtp_timestamp_element_parsing,
+	.clone = capwap_vendor_travelping_wtp_timestamp_element_clone,
+	.free = capwap_vendor_travelping_wtp_timestamp_element_free
+};

src/common/capwap_element_vendor_travelping_wtp_timestamp.h

@@ -0,0 +1,19 @@
+#ifndef __CAPWAP_ELEMENT_VENDOR_TRAVELPING_WTP_TIMESTAMP_HEADER__
+#define __CAPWAP_ELEMENT_VENDOR_TRAVELPING_WTP_TIMESTAMP_HEADER__
+
+#define CAPWAP_ELEMENT_VENDOR_TRAVELPING_WTP_TIMESTAMP_VENDOR	CAPWAP_VENDOR_TRAVELPING_ID
+#define CAPWAP_ELEMENT_VENDOR_TRAVELPING_WTP_TIMESTAMP_TYPE	2
+#define CAPWAP_ELEMENT_VENDOR_TRAVELPING_WTP_TIMESTAMP			\
+	(struct capwap_message_element_id){				\
+		.vendor = CAPWAP_ELEMENT_VENDOR_TRAVELPING_WTP_TIMESTAMP_VENDOR, \
+		.type = CAPWAP_ELEMENT_VENDOR_TRAVELPING_WTP_TIMESTAMP_TYPE \
+	}
+
+
+struct capwap_vendor_travelping_wtp_timestamp_element {
+	struct timeval tv;
+};
+
+extern const struct capwap_message_elements_ops capwap_element_vendor_travelping_wtp_timestamp_ops;
+
+#endif /* __CAPWAP_ELEMENT_VENDOR_TRAVELPING_WTP_TIMESTAMP_HEADER__ */

src/common/capwap_vendor_travelping.h

@@ -3,6 +3,8 @@
 
 #define CAPWAP_VENDOR_TRAVELPING_ID		18681
 
+#include "capwap_element_vendor_travelping_wtp_timestamp.h"
+
 /* draft-ietf-opsawg-capwap-extension-06 */
 #include "capwap_element_80211n_radioconf.h"
 #include "capwap_element_80211n_station_information.h"

src/wtp/wtp.h

@@ -122,6 +122,9 @@ struct wtp_t {
 
 	int sta_max_inactivity;
 
+	/* Echo statistics */
+	struct timeval echo_latency;
+
 	/* */
 	unsigned short fragmentid;
 	struct capwap_packet_rxmng* rxmngpacket;

src/wtp/wtp_dfa_run.c

@@ -16,6 +16,9 @@ static int send_echo_request(void)
 	int result = -1;
 	struct capwap_header_data capwapheader;
 	struct capwap_packet_txmng* txmngpacket;
+	struct capwap_vendor_travelping_wtp_timestamp_element timestamp;
+
+	gettimeofday(&timestamp.tv, NULL);
 
 	/* Build packet */
 	capwap_header_init(&capwapheader, CAPWAP_RADIOID_NONE, g_wtp.binding);
@@ -23,7 +26,9 @@ static int send_echo_request(void)
 							      g_wtp.localseqnumber, g_wtp.mtu);
 
 	/* Add message element */
-	/* CAPWAP_ELEMENT_VENDORPAYLOAD */				/* TODO */
+	capwap_packet_txmng_add_message_element(txmngpacket,
+						CAPWAP_ELEMENT_VENDOR_TRAVELPING_WTP_TIMESTAMP,
+						&timestamp);
 
 	/* Echo request complete, get fragment packets */
 	wtp_free_reference_last_request();
@@ -47,18 +52,34 @@ static int send_echo_request(void)
 }
 
 /* */
-static int receive_echo_response(struct capwap_parsed_packet* packet) {
+static int receive_echo_response(struct capwap_parsed_packet* packet)
+{
 	struct capwap_resultcode_element* resultcode;
+	struct capwap_vendor_travelping_wtp_timestamp_element *timestamp;
 
 	ASSERT(packet != NULL);
 
 	/* Check the success of the Request */
-	resultcode = (struct capwap_resultcode_element*)capwap_get_message_element_data(packet, CAPWAP_ELEMENT_RESULTCODE);
+	resultcode = (struct capwap_resultcode_element *)
+		capwap_get_message_element_data(packet, CAPWAP_ELEMENT_RESULTCODE);
 	if (resultcode && !CAPWAP_RESULTCODE_OK(resultcode->code)) {
 		log_printf(LOG_WARNING, "Receive Echo Response with error: %d", (int)resultcode->code);
 		return 1;
 	}
 
+	timestamp = (struct capwap_vendor_travelping_wtp_timestamp_element *)
+		capwap_get_message_element_data(packet, CAPWAP_ELEMENT_VENDOR_TRAVELPING_WTP_TIMESTAMP);
+	if (timestamp) {
+		struct timeval now;
+
+		gettimeofday(&now, NULL);
+		timersub(&now, &timestamp->tv, &g_wtp.echo_latency);
+
+		log_printf(LOG_DEBUG, "Echo Latency: %ld.%03ld ms",
+			   g_wtp.echo_latency.tv_sec * 1000 + g_wtp.echo_latency.tv_usec / 1000,
+			   g_wtp.echo_latency.tv_usec % 1000);
+	}
+
 	/* Valid packet, free request packet */
 	wtp_free_reference_last_request();
 	return 0;