dump-funcs.c 13.2 KB
Newer Older
1 2 3 4 5 6
/*
 * Copyright (C) 2012 CERN (www.cern.ch)
 * Author: Alessandro Rubini
 *
 * Released according to the GNU GPL, version 2 or any later version.
 */
7 8
#include <stdio.h>
#include <string.h>
9
#include <ppsi/ieee1588_types.h> /* from ../include */
10 11 12
#include "decent_types.h"
#include "ptpdump.h"

13 14 15 16
#define WR_MODE_ON_MASK 0x8
#define CALIBRATED_MASK 0x4
#define WR_CONFIG_MASK 0x3

17 18
static int dump_vlan(char *prefix, int vlan);

19
static int dumpstruct(char *p1, char *p2, char *name, void *ptr, int size)
20 21 22 23
{
	int ret, i;
	unsigned char *p = ptr;

24
	ret = printf("%s%s%s (size %i)\n", p1, p2, name, size);
25 26
	for (i = 0; i < size; ) {
		if ((i & 0xf) == 0)
27
			ret += printf("%s%s", p1, p2);
28
		ret += printf("%02x", p[i]);
29
		i++;
30
		ret += printf(i & 3 ? " " : i & 0xf ? "  " : "\n");
31 32
	}
	if (i & 0xf)
33
		ret += printf("\n");
34 35 36
	return ret;
}

37
#if __STDC_HOSTED__
38
static void dump_time(char *prefix, const struct pp_time *t)
39 40 41 42
{
	struct timeval tv;
	struct tm tm;

43 44
	tv.tv_sec = t->secs;
	tv.tv_usec = (t->scaled_nsecs >> 16) / 1000;
45
	localtime_r(&tv.tv_sec, &tm);
46
	printf("%sTIME: (%li - 0x%lx) %02i:%02i:%02i.%06li%s\n", prefix,
47
	       tv.tv_sec, tv.tv_sec,
48
	       tm.tm_hour, tm.tm_min, tm.tm_sec, (long)tv.tv_usec,
49
	       is_incorrect(t) ? " invalid" : "");
50
}
51
#else
52
static void dump_time(char *prefix, const struct pp_time *t)
53
{
54 55 56 57
	printf("%sTIME: (%li - 0x%lx) %li.%06li%s\n", prefix,
	       (long)t->secs, (long)t->secs, (long)t->secs,
	       (long)(t->scaled_nsecs >> 16) / 1000,
	       is_incorrect(t) ? " invalid" : "");
58
}
59 60
#endif

61 62
/* Returns the header size, used by the caller to adjust the next pointer */
static int dump_eth(char *prefix, struct ethhdr *eth)
63 64 65
{
	unsigned char *d = eth->h_dest;
	unsigned char *s = eth->h_source;
66
	int proto = ntohs(eth->h_proto);
67 68 69 70 71 72 73 74 75 76 77 78
	int ret;

	/* Between eth header and payload may be a VLAN tag;
	 * NOTE: We cannot distinguish between both cases looking at
	 * the content of a vlan variable, because vlan number may come from
	 * frame itself or from the socket (CMSG) */
	if (proto == 0x8100) {
		ret = sizeof(struct pp_vlanhdr); /* ETH header + VLAN tag */
		/* Get the proto knowing that there is a VLAN tag */
		proto = ntohs(((struct pp_vlanhdr *)eth)->h_proto);
	} else
		ret = sizeof(struct ethhdr);
79

80
	printf("%sETH: %04x (%02x:%02x:%02x:%02x:%02x:%02x -> "
81
	       "%02x:%02x:%02x:%02x:%02x:%02x)\n", prefix, proto,
82 83
	       s[0], s[1], s[2], s[3], s[4], s[5],
	       d[0], d[1], d[2], d[3], d[4], d[5]);
84
	return ret;
85 86
}

87
static void dump_ip(char *prefix, struct iphdr *ip)
88
{
89 90
	unsigned int s = ntohl(ip->saddr);
	unsigned int d = ntohl(ip->daddr);
91
	printf("%sIP: %i (%i.%i.%i.%i -> %i.%i.%i.%i) len %i\n", prefix,
92 93 94 95 96 97
	       ip->protocol,
	       (s >> 24) & 0xff, (s >> 16) & 0xff, (s >> 8) & 0xff, s & 0xff,
	       (d >> 24) & 0xff, (d >> 16) & 0xff, (d >> 8) & 0xff, d & 0xff,
	       ntohs(ip->tot_len));
}

98
static void dump_udp(char *prefix, struct udphdr *udp)
99
{
100
	printf("%sUDP: (%i -> %i) len %i\n", prefix,
101 102 103 104
	       ntohs(udp->source), ntohs(udp->dest), ntohs(udp->len));
}

/* Helpers for fucking data structures */
105
static void dump_1stamp(char *prefix, char *s, struct stamp *t)
106 107 108 109
{
	uint64_t  sec = (uint64_t)(ntohs(t->sec.msb)) << 32;

	sec |= (uint64_t)(ntohl(t->sec.lsb));
110
	printf("%s%s%lu.%09i\n", prefix,
111
	       s, (unsigned long)sec, (int)ntohl(t->nsec));
112 113
}

114
static void dump_1quality(char *prefix, char *s, ClockQuality *q)
115
{
116 117
	printf("%s%s%02x-%02x-%04x\n", prefix, s, (unsigned int) q->clockClass,
			(unsigned int) q->clockAccuracy, (unsigned int) q->offsetScaledLogVariance);
118 119
}

120
static void dump_1clockid(char *prefix, char *s, ClockIdentity i)
121
{
122
	printf("%s%s%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x\n", prefix, s,
123 124
	       i.id[0], i.id[1], i.id[2], i.id[3],
	       i.id[4], i.id[5], i.id[6], i.id[7]);
125 126
}

127
static void dump_1port(char *prefix, char *s, unsigned char *p)
128
{
129 130
	printf("%s%s%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x-%02x\n",
	       prefix, s,
131 132 133 134 135
	       p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8], p[9]);
}


/* Helpers for each message types */
136
static void dump_msg_announce(char *prefix, struct ptp_announce *p)
137
{
138 139 140 141 142
	ClockQuality	grandmasterClockQuality;

	memcpy( &grandmasterClockQuality,&p->grandmasterClockQuality, sizeof(ClockQuality));
	grandmasterClockQuality.offsetScaledLogVariance=ntohs(grandmasterClockQuality.offsetScaledLogVariance);

143 144 145
	dump_1stamp(prefix, "MSG-ANNOUNCE: stamp ",
		    &p->originTimestamp);
	dump_1quality(prefix, "MSG-ANNOUNCE: grandmaster-quality ",
146
		      &grandmasterClockQuality);
147
	printf("%sMSG-ANNOUNCE: grandmaster-prio %i %i\n", prefix,
148
	       p->grandmasterPriority1, p->grandmasterPriority2);
149
	dump_1clockid(prefix, "MSG-ANNOUNCE: grandmaster-id ",
150 151 152
		      p->grandmasterIdentity);
}

153
static void dump_msg_sync_etc(char *prefix, char *s, struct ptp_sync_etc *p)
154
{
155
	dump_1stamp(prefix, s, &p->stamp);
156 157
}

158
static void dump_msg_resp_etc(char *prefix, char *s, struct ptp_sync_etc *p)
159
{
160 161
	dump_1stamp(prefix, s, &p->stamp);
	dump_1port(prefix, s, p->port);
162 163
}

164
/* TLV dumper, now white-rabbit aware */
165
static int wr_dump_tlv(char *prefix, struct ptp_tlv *tlv, int totallen)
166 167 168
{
	/* the field includes 6 bytes of the header, ecludes 4 of them. Bah! */
	int explen = ntohs(tlv->len) + 4;
baujc's avatar
baujc committed
169 170 171 172 173 174 175 176 177 178 179 180 181

	if ( CONFIG_HAS_PROFILE_WR ) {
		static char *wr_message_name[] = {
		    "SLAVE_PRESENT",
		    "LOCK",
		    "LOCKED",
		    "CALIBRATE",
		    "CALIBRATED",
		    "WR_MODE_ON",
		};
		uint16_t messageId;
		char *messageId_str = NULL;

182
	
baujc's avatar
baujc committed
183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207
		printf("%sTLV: type %04x len %i oui %02x:%02x:%02x "
			   "sub %02x:%02x:%02x\n", prefix, ntohs(tlv->type), explen,
			   tlv->oui[0], tlv->oui[1], tlv->oui[2],
			   tlv->subtype[0], tlv->subtype[1], tlv->subtype[2]);
		if (explen > totallen) {
			printf("%sTLV: too short (expected %i, total %i)\n", prefix,
				   explen, totallen);
			return totallen;
		}

		if (memcmp(tlv->oui, "\x08\x00\x30", 3) /* WR_TLV_ORGANIZATION_ID */
			/* WR_TLV_MAGIC_NUMBER, WR_TLV_WR_VERSION_NUMBER */
			|| memcmp(tlv->subtype, "\xDE\xAD\x01", 3)
			) {
			/* Now dump non-wr tlv in binary, count only payload */
			dumpstruct(prefix, "TLV: ", "tlv-content", tlv->data,
				   explen - sizeof(*tlv));
			return explen;
		}

		messageId = (tlv->data[0] << 8) + tlv->data[1];
		if (SLAVE_PRESENT <= messageId && messageId <= WR_MODE_ON)
			messageId_str = wr_message_name[messageId - SLAVE_PRESENT];
		if (messageId == ANN_SUFIX)
			messageId_str = "ANN_SUFIX";
208
	
baujc's avatar
baujc committed
209 210 211 212 213 214 215 216 217
		if (messageId_str) {
			printf("%sTLV: messageId %s(0x%x)\n", prefix, messageId_str,
				   messageId);
			switch(messageId){
			case SLAVE_PRESENT:
			case LOCK:
			case LOCKED:
			case WR_MODE_ON:
				/* no more to be printed */
218
				break;
baujc's avatar
baujc committed
219 220 221 222 223 224 225 226 227 228 229 230 231 232
			case CALIBRATE:
				if (totallen < 8 || explen < 8) { /* 2+1+1+4 */
					printf("%sTLV: too short (expected %i, total "
						   "%i)\n", prefix, explen, totallen);
					return totallen;
				}
				printf("%sTLV: calSendPattern %s, calRetry %u, "
					   "calPeriod %d\n",
					   prefix,
					   tlv->data[2] ? "True":"False",
					   tlv->data[3],
					   (tlv->data[4] << 24) + (tlv->data[5] << 16)
					   + (tlv->data[6] << 8) + tlv->data[7]
					   );
233
				break;
baujc's avatar
baujc committed
234 235 236 237 238 239 240 241 242 243 244
			case CALIBRATED:
				/* TODO: print as ints */
				if (totallen < 18 || explen < 18) { /* 2+8+8 */
					printf("%sTLV: too short (expected %i, total "
						   "%i)\n", prefix, explen, totallen);
					return totallen;
				}
				dumpstruct(prefix, "TLV: ", "deltaTx", &tlv->data[2],
					   8);
				dumpstruct(prefix, "TLV: ", "deltaRx", &tlv->data[10],
					   8);
245
				break;
baujc's avatar
baujc committed
246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278
			case ANN_SUFIX:
				{
				int flags = tlv->data[3]; /* data[2] is unused */
				char *wr_config_str;
				if (totallen < 4 || explen < 4) { /* 2+2 */
					printf("%sTLV: too short (expected %i, total "
						   "%i)\n", prefix, explen, totallen);
					return totallen;
				}
				switch (flags & WR_CONFIG_MASK) {
				case NON_WR:
					wr_config_str = "NON_WR";
					break;
				case WR_S_ONLY:
					wr_config_str = "WR_S_ONLY";
					break;
				case WR_M_ONLY:
					wr_config_str = "WR_M_ONLY";
					break;
				case WR_M_AND_S:
					wr_config_str = "WR_M_AND_S";
					break;
				default:
					wr_config_str="";
					break;
				}
				printf("%sTLV: wrFlags: wrConfig %s, calibrated %s, "
					   "wrModeOn %s\n",
					   prefix,
					   wr_config_str,
					   flags & CALIBRATED_MASK ? "True":"False",
					   flags & WR_MODE_ON_MASK ? "True":"False"
					   );
279
				break;
baujc's avatar
baujc committed
280
				}
281 282
			}
		}
baujc's avatar
baujc committed
283 284 285
		return explen;
	} else
		return explen > totallen ? totallen : explen;
286 287
}

288 289
static int l1sync_dump_tlv(char *prefix, struct l1sync_tlv *tlv, int totallen)
{
290
	/* the field includes 6 bytes of the header, excludes 4 of them. Bah! */
291 292
	int explen = ntohs(tlv->len) + 4;

baujc's avatar
baujc committed
293 294 295 296 297 298 299 300 301 302 303
	if ( CONFIG_HAS_EXT_L1SYNC ) {
		printf("%sTLV: type %04x len %i conf %02x act %02x\n",
				prefix,
				ntohs(tlv->type), explen,
				(int) tlv->config,
				(int) tlv->active);
		if (explen > totallen) {
			printf("%sTLV: too short (expected %i, total %i)\n", prefix,
				   explen, totallen);
			return totallen;
		}
304

baujc's avatar
baujc committed
305
		/* later:  if (memcmp(tlv->oui, "\x08\x00\x30", 3)) ... */
306

baujc's avatar
baujc committed
307 308 309 310 311 312
		/* Now dump non-l1sync tlv in binary, count only payload */
		dumpstruct(prefix, "TLV: ", "tlv-content", tlv->data,
			   explen - sizeof(*tlv));
		return explen;
	} else
		return explen > totallen ? totallen : explen;
313 314
}

315
/* A big function to dump the ptp information */
316
static void dump_payload(char *prefix, void *pl, int len)
317 318 319 320
{
	struct ptp_header *h = pl;
	void *msg_specific = (void *)(h + 1);
	int donelen = 34; /* packet length before tlv */
321 322
	int version = h->versionPTP_and_reserved & 0xf;
	int messageType = h->type_and_transport_specific & 0xf;
323
	char *cfptr = (void *)&h->correctionField;
324
	int tlv_size=0;
325

326 327
	if (version != 2) {
		printf("%sVERSION: unsupported (%i)\n", prefix, version);
328
		goto out;
329
	}
330
	printf("%sVERSION: %i (type %i, len %i, domain %i)\n", prefix,
331
	       version, messageType,
332
	       ntohs(h->messageLength), h->domainNumber);
333 334
	printf("%sFLAGS: 0x%02x%02x (correction 0x%08x:%08x %08u)\n",
	       prefix, (unsigned) h->flagField[0],(unsigned) h->flagField[1],
335 336 337
	       ntohl(*(int *)cfptr),
	       ntohl(*(int *)(cfptr + 4)),
	       ntohl(*(int *)(cfptr + 4)));
338 339
	dump_1port(prefix, "PORT: ", h->sourcePortIdentity);
	printf("%sREST: seq %i, ctrl %i, log-interval %i\n", prefix,
340
	       ntohs(h->sequenceId), h->controlField, h->logMessageInterval);
341
#define CASE(t, x) case PPM_ ##x: printf("%sMESSAGE: (" #t ") " #x "\n", prefix)
342
	switch(messageType) {
343
		CASE(E, SYNC);
344
		dump_msg_sync_etc(prefix, "MSG-SYNC: ", msg_specific);
345 346 347 348
		donelen = 44;
		break;

		CASE(E, DELAY_REQ);
349
		dump_msg_sync_etc(prefix, "MSG-DELAY_REQ: ", msg_specific);
350 351 352 353
		donelen = 44;
		break;

		CASE(G, FOLLOW_UP);
354
		dump_msg_sync_etc(prefix, "MSG-FOLLOW_UP: ", msg_specific);
355 356 357 358
		donelen = 44;
		break;

		CASE(G, DELAY_RESP);
359
		dump_msg_resp_etc(prefix, "MSG-DELAY_RESP: ", msg_specific);
360 361 362 363
		donelen = 54;
		break;

		CASE(G, ANNOUNCE);
364
		dump_msg_announce(prefix, msg_specific);
365
		donelen = 64;
366
		tlv_size=sizeof(struct ptp_tlv);
367 368 369
		break;

		CASE(G, SIGNALING);
370
		dump_1port(prefix, "MSG-SIGNALING: target-port ", msg_specific);
371
		donelen = 44;
372
		tlv_size=sizeof(struct l1sync_tlv);
373 374
		break;

375
#if __STDC_HOSTED__ /* Avoid pdelay dump within ppsi, we don't use it */
376
		CASE(E, PDELAY_REQ);
377
		dump_msg_sync_etc(prefix, "MSG-PDELAY_REQ: ", msg_specific);
378 379 380 381
		donelen = 54;
		break;

		CASE(E, PDELAY_RESP);
382
		dump_msg_resp_etc(prefix, "MSG-PDELAY_RESP: ", msg_specific);
383 384 385
		donelen = 54;
		break;

386
		CASE(G, PDELAY_R_FUP);
387 388
		dump_msg_resp_etc(prefix, "MSG-PDELAY_RESP_FOLLOWUP: ",
				  msg_specific);
389 390 391
		donelen = 54;
		break;

392 393 394
		CASE(G, MANAGEMENT);
		/* FIXME */
		break;
395
#endif
396 397
	}

398 399 400 401
	/*
	 * Dump any trailing TLV, but ignore a trailing 2-long data hunk.
	 * The trailing zeroes appear with less-than-minimum Eth messages.
	 */
402

403
	while (donelen < len && len - donelen > 2) {
404
		int n = len - donelen;
405
		if (n < tlv_size) {
406 407
			printf("%sTLV: too short (%i - %i = %i)\n", prefix,
			       len, donelen, n);
408 409
			break;
		}
410 411 412 413 414 415 416
		switch ( messageType) {
		case PPM_ANNOUNCE :
			donelen += wr_dump_tlv(prefix, pl + donelen, n);
			break;
		case PPM_SIGNALING :
			donelen += l1sync_dump_tlv(prefix, pl + donelen, n);
			break;
417 418
		default :
			goto out;
419
		}
420
	}
421
out:
422
	/* Finally, binary dump of it all */
423
	dumpstruct(prefix, "DUMP: ", "payload", pl, len);
424 425
}

426
/* This dumps a complete udp frame, starting from the eth header */
427
int dump_udppkt(char *prefix, void *buf, int len, const struct pp_time *t,
428
		int vlan)
429 430
{
	struct ethhdr *eth = buf;
431 432 433
	struct iphdr *ip;
	struct udphdr *udp;
	void *payload;
434

435 436
	if (t)
		dump_time(prefix, t);
437

438 439
	dump_vlan(prefix, vlan);

440
	ip = buf + dump_eth(prefix, eth);
441
	dump_ip(prefix, ip);
442 443

	udp = (void *)(ip + 1);
444
	dump_udp(prefix, udp);
445 446

	payload = (void *)(udp + 1);
447
	dump_payload(prefix, payload, len - (payload - buf));
448

449 450 451
	return 0;
}

452
/* This dumps the payload only, used for udp frames without headers */
453
int dump_payloadpkt(char *prefix, void *buf, int len, const struct pp_time *t)
454
{
455 456
	if (t)
		dump_time(prefix, t);
457
	dump_payload(prefix, buf, len);
458 459 460 461
	return 0;
}

/* This dumps everything, used for raw frames with headers and ptp payload */
462
int dump_1588pkt(char *prefix, void *buf, int len, const struct pp_time *t,
463
		 int vlan)
464 465
{
	struct ethhdr *eth = buf;
466
	void *payload;
467

468 469
	if (t)
		dump_time(prefix, t);
470
	dump_vlan(prefix, vlan);
471
	payload = buf + dump_eth(prefix, eth);
472
	dump_payload(prefix, payload, len - (payload - buf));
473

474 475
	return 0;
}
476

477
static int dump_vlan(char *prefix, int vlan)
478
{
479
	if (vlan >= 0)
480 481 482 483
		printf("%sVLAN %i\n", prefix, vlan);

	return 0;
}