latency.c 7.24 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13
/*
 * This work is part of the White Rabbit project
 *
 * Copyright (C) 2016 GSI (www.gsi.de)
 * Author: Alessandro Rubini <a.rubini@gsi.de>
 *
 * Released according to the GNU GPL, version 2 or any later version.
 */
#include <wrc.h>
#include <wrpc.h>
#include <ptpd_netif.h>
#include <shell.h>
#include "ipv4.h"
14 15 16 17
#include <endpoint.h> /* get_mac_addr() */
#include <ppsi/jiffies.h> /* time_before() */

#define jiffies timer_get_tics()
18

19 20 21 22 23 24 25 26 27 28
#ifdef CONFIG_LATENCY_SYSLOG
#define HAS_SYSLOG 1
static int lat_verbose = 0;
#else
#define HAS_SYSLOG 0
static int lat_verbose = 1;
#endif

static unsigned long prios[] = {7, 6, 0}; /* the prio for the 3 frames */

29 30
static int ltest_fake_delay_ns;

31 32 33 34 35 36 37 38
/* latency probe: we need to enqueue 3 short frames: 64*3+overhead = 256 */
static uint8_t __latency_queue[256];
static struct wrpc_socket *latency_socket, __static_latency_socket = {
	.queue.buff = __latency_queue,
	.queue.size = sizeof(__latency_queue),
};

static struct wr_sockaddr latency_addr = {
39
	.mac = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}, /* for sending */
40 41 42 43 44 45 46 47 48 49 50 51
	.ethertype = 0, /* htons(CONFIG_LATENCY_ETHTYPE) -- not constant! */
};

static void latency_init(void)
{
	latency_addr.ethertype = htons(CONFIG_LATENCY_ETHTYPE);
	latency_socket = ptpd_netif_create_socket(&__static_latency_socket,
						  &latency_addr,
						  PTPD_SOCK_RAW_ETHERNET, 0);
}

static struct latency_frame {
52 53
	uint32_t type;   /* 1, 2, 3 */
	uint32_t sequence;
54
	struct wr_timestamp ts[2];
55 56
} frame;

57
static uint32_t prev_sequence, prev_type;
58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75

static void ts_sub(struct wr_timestamp *t2, struct wr_timestamp *t1,
		      struct wr_timestamp *res)
{
	int64_t resll; /* every field is signed: good */

	resll = t2->sec - t1->sec;
	resll *= 1000 * 1000 * 1000;
	resll += t2->nsec - t1->nsec;
	resll *= 1000;
	resll += t2->phase - t1->phase;
	/* seconds must be 0 and result must be positive */
	res->sec = 0;
	res->phase = __div64_32((uint64_t *)&resll, 1000);
	res->nsec = resll;
}

static void latency_warning(void) {
76 77
	if (!lat_verbose)
		return;
78
	pp_printf("lat: unexpected %i.%i after %i.%i\n",
79 80 81
		  frame.sequence, frame.type, prev_sequence, prev_type);
}

82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121
/* report once a minute */
static void latency_report(struct wr_timestamp *lat)
{
	static unsigned long nextj;
	static unsigned long min[2], max[2], tot[2], n;
	char buf[128];
	int i;

	if (!nextj) {
		unsigned char mac[6];

		/* first time; pick a time in the future */
		get_mac_addr(mac);
		nextj = jiffies + TICS_PER_SECOND * (10 + (mac[5] % 60));
		pp_printf("%s: first sending at %li\n", __func__, nextj);
	}

	n++;
	for (i = 0; i < 2; i++) {
		if (!min[i] || lat[i].nsec < min[i])
			min[i] = lat[i].nsec;
		if ( lat[i].nsec > max[i])
			max[i] = lat[i].nsec;
		tot[i] += lat[i].nsec;
	}

	if (time_before(jiffies, nextj))
		return;
	nextj += 60 * TICS_PER_SECOND;

	if (!n)
		return;

	pp_sprintf(buf, "ltest: %li samples, "
		   "min %li %li, avg %li %li, max %li %li\n",
		   n, min[0], min[1], tot[0]/n, tot[1]/n, max[0], max[1]);
	n = min[0] = min[1] = max[0] = max[1] = tot[0] = tot[1] = 0;
	syslog_report(buf);
}

122 123 124
static int latency_poll_rx(void)
{
	static struct wr_timestamp ts[2];
125
	static int nframes;
126
	static unsigned lost;
127
	struct wr_timestamp ts_tmp, lat[2];
128
	struct wr_sockaddr addr;
129
	int i, j;
130

131
	i = ptpd_netif_recvfrom(latency_socket, &addr,
132
				  &frame, sizeof(frame), &ts_tmp);
133
	if (i < sizeof(frame))
134 135 136 137
		return 0;

	/* check sequence and type is ok */
	switch(frame.type) {
138 139
	case 1:
		if (frame.sequence != prev_sequence + 1 || prev_type != 3)
140 141
			latency_warning();
		ts[0] = ts_tmp; /* 0 is always valid, it's a new set */
142
		nframes = 1;
143
		ts[1].sec = 0; /* but still incomplete info */
144

145 146
		break;

147 148
	case 2:
		if (frame.sequence != prev_sequence || prev_type != 1) {
149
			latency_warning();
150
		} else {
151
			ts[1] = ts_tmp;
152 153
			nframes++;
		}
154 155
		break;

156 157
	case 3:
		if (frame.sequence != prev_sequence || prev_type != 2) {
158
			latency_warning();
159 160
		} else {
			nframes++;
161
		}
162
		break;
163
	}
164 165 166 167

	/* count lost frames */
	i = prev_sequence * 3 + prev_type - 1; /* type: 1..3 -> 0..2 */
	j = frame.sequence * 3 + frame.type - 2;
168 169 170 171 172 173 174
	if (HAS_SYSLOG && j != i) {
		char buf[64];

		lost += (j - i);
		pp_sprintf(buf, "ltest: lost %i frames, total %i\n",
			   j - i, lost);
		syslog_report(buf);
175 176
	}

177 178
	prev_sequence = frame.sequence;
	prev_type = frame.type;
179
	if (frame.type != 3 || nframes != 3)
180 181 182 183 184
		return 1;


	net_verbose("ts_rx 1: %9li.%09i.%03i\n", (long)ts[0].sec,
		    ts[0].nsec, ts[0].phase);
185 186
	net_verbose("ts_tx 1: %9li.%09i.%03i\n", (long)frame.ts[0].sec,
		    frame.ts[0].nsec, frame.ts[0].phase);
187 188 189

	net_verbose("ts_rx 2: %9li.%09i.%03i\n", (long)ts[1].sec,
		    ts[1].nsec, ts[1].phase);
190 191 192 193 194 195 196 197 198 199 200 201 202 203
	net_verbose("ts_tx 2: %9li.%09i.%03i\n", (long)frame.ts[1].sec,
		    frame.ts[1].nsec, frame.ts[1].phase);

	ts_sub(ts + 0, frame.ts + 0, lat + 0);
	ts_sub(ts + 1, frame.ts + 1, lat + 1);

	if (lat[0].sec || lat[1].sec)
		return 1; /* not synchronized for sure */

	if (lat_verbose) {
		pp_printf("lat: %9i %6i.%03i %6i.%03i\n",
			  frame.sequence,
			  lat[0].nsec, lat[0].phase,
			  lat[1].nsec, lat[1].phase);
204
		return 1;
205
	} else {
206
		latency_report(lat);
207
	}
208 209 210
	return 1;
}

211 212


213 214
static int latency_poll_tx(void)
{
215 216
	static uint32_t sequence;
	static uint32_t lasts;
217

218 219 220 221
	/*
	 * Send three frames -- lazily in native byte order. Possibly
	 * subtract a fake delay, to trigger reporting.
	 */
222 223 224
	memset(&frame, 0, sizeof(frame));
	frame.sequence = sequence++;

225
	frame.type = 1;
226
	latency_socket->prio = prios[0];
227
	ptpd_netif_sendto(latency_socket, &latency_addr, &frame, sizeof(frame),
228
			  frame.ts + 0);
229 230 231 232 233
	frame.ts[0].nsec -= ltest_fake_delay_ns;
	if (frame.ts[0].nsec < 0) {
		frame.ts[0].nsec += 1000 * 1000 * 1000;
		frame.ts[0].sec --;
	}
234

235
	frame.type = 2;
236
	latency_socket->prio = prios[1];
237
	ptpd_netif_sendto(latency_socket, &latency_addr, &frame, sizeof(frame),
238
			  frame.ts + 1);
239 240 241 242 243
	frame.ts[1].nsec -= ltest_fake_delay_ns;
	if (frame.ts[1].nsec < 0) {
		frame.ts[1].nsec += 1000 * 1000 * 1000;
		frame.ts[1].sec --;
	}
244

245
	frame.type = 3;
246
	latency_socket->prio = prios[2];
247 248
	ptpd_netif_sendto(latency_socket, &latency_addr, &frame, sizeof(frame),
			  NULL);
249
	ltest_fake_delay_ns = 0;
250 251 252

	/* Every 10s remind we are sending ltest */
	if (!lasts) {
253 254 255
		lasts = frame.ts->sec;
	} else if (frame.ts->sec - lasts >= 10) {
		lasts = frame.ts->sec;
256 257 258
		pp_printf("latency: seq %9i sent @ %9i\n",
			  sequence, lasts);
	}
259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291
	return 1;

}

static uint32_t lastt;
static uint32_t latency_period_ms;

static int latency_poll(void)
{
	if (!latency_period_ms)
		return latency_poll_rx();

	/* Periodically send the frames */
	if (task_not_yet(&lastt, latency_period_ms))
		return 0;
	return latency_poll_tx();
}

DEFINE_WRC_TASK(uptime) = {
	.name = "latency-probe",
	.init = latency_init,
	.job = latency_poll,
};


static int cmd_ltest(const char *args[])
{
	int v = 0, v1 = 0;

	if (args[1]) {
		fromdec(args[1], &v1); /* ms */
	}
	if (args[0]) {
292 293 294 295
		if (HAS_SYSLOG && !strcmp(args[0], "verbose"))
			lat_verbose = 1;
		else if (HAS_SYSLOG && !strcmp(args[0], "quiet"))
			lat_verbose = 0;
296 297
		else if (!strcmp(args[0], "fake"))
			fromdec(args[1], &ltest_fake_delay_ns);
298 299 300 301 302
		else {
			fromdec(args[0], &v);
			latency_period_ms = v * 1000 + v1;
			lastt = 0; /* reset, so it fires immediately */
		}
303
	}
304 305
	pp_printf("%i.%03i (%s)\n", latency_period_ms / 1000,
		  latency_period_ms % 1000, lat_verbose ? "verbose" : "quiet");
306 307 308 309 310 311 312 313
	return 0;
}

DEFINE_WRC_COMMAND(ltest) = {
	.name = "ltest",
	.exec = cmd_ltest,
};