summaryrefslogtreecommitdiffstats
path: root/examples/ptpclient/ptpclient.c
blob: 83821eb8d7a0175d7b77a4e85efbb4306ac008a2 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
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
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
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
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
/* SPDX-License-Identifier: BSD-3-Clause
 * Copyright(c) 2015 Intel Corporation
 */

/*
 * This application is a simple Layer 2 PTP v2 client. It shows delta values
 * which are used to synchronize the PHC clock. if the "-T 1" parameter is
 * passed to the application the Linux kernel clock is also synchronized.
 */

#include <stdint.h>
#include <inttypes.h>
#include <rte_eal.h>
#include <rte_ethdev.h>
#include <rte_cycles.h>
#include <rte_lcore.h>
#include <rte_mbuf.h>
#include <rte_ip.h>
#include <limits.h>
#include <sys/time.h>
#include <getopt.h>

#define RX_RING_SIZE 1024
#define TX_RING_SIZE 1024

#define NUM_MBUFS            8191
#define MBUF_CACHE_SIZE       250

/* Values for the PTP messageType field. */
#define SYNC                  0x0
#define DELAY_REQ             0x1
#define PDELAY_REQ            0x2
#define PDELAY_RESP           0x3
#define FOLLOW_UP             0x8
#define DELAY_RESP            0x9
#define PDELAY_RESP_FOLLOW_UP 0xA
#define ANNOUNCE              0xB
#define SIGNALING             0xC
#define MANAGEMENT            0xD

#define NSEC_PER_SEC        1000000000L
#define KERNEL_TIME_ADJUST_LIMIT  20000
#define PTP_PROTOCOL             0x88F7

struct rte_mempool *mbuf_pool;
uint32_t ptp_enabled_port_mask;
uint8_t ptp_enabled_port_nb;
static uint8_t ptp_enabled_ports[RTE_MAX_ETHPORTS];

static const struct rte_eth_conf port_conf_default = {
	.rxmode = {
		.max_rx_pkt_len = ETHER_MAX_LEN,
		.ignore_offload_bitfield = 1,
	},
};

static const struct ether_addr ether_multicast = {
	.addr_bytes = {0x01, 0x1b, 0x19, 0x0, 0x0, 0x0}
};

/* Structs used for PTP handling. */
struct tstamp {
	uint16_t   sec_msb;
	uint32_t   sec_lsb;
	uint32_t   ns;
}  __attribute__((packed));

struct clock_id {
	uint8_t id[8];
};

struct port_id {
	struct clock_id        clock_id;
	uint16_t               port_number;
}  __attribute__((packed));

struct ptp_header {
	uint8_t              msg_type;
	uint8_t              ver;
	uint16_t             message_length;
	uint8_t              domain_number;
	uint8_t              reserved1;
	uint8_t              flag_field[2];
	int64_t              correction;
	uint32_t             reserved2;
	struct port_id       source_port_id;
	uint16_t             seq_id;
	uint8_t              control;
	int8_t               log_message_interval;
} __attribute__((packed));

struct sync_msg {
	struct ptp_header   hdr;
	struct tstamp       origin_tstamp;
} __attribute__((packed));

struct follow_up_msg {
	struct ptp_header   hdr;
	struct tstamp       precise_origin_tstamp;
	uint8_t             suffix[0];
} __attribute__((packed));

struct delay_req_msg {
	struct ptp_header   hdr;
	struct tstamp       origin_tstamp;
} __attribute__((packed));

struct delay_resp_msg {
	struct ptp_header    hdr;
	struct tstamp        rx_tstamp;
	struct port_id       req_port_id;
	uint8_t              suffix[0];
} __attribute__((packed));

struct ptp_message {
	union {
		struct ptp_header          header;
		struct sync_msg            sync;
		struct delay_req_msg       delay_req;
		struct follow_up_msg       follow_up;
		struct delay_resp_msg      delay_resp;
	} __attribute__((packed));
};

struct ptpv2_data_slave_ordinary {
	struct rte_mbuf *m;
	struct timespec tstamp1;
	struct timespec tstamp2;
	struct timespec tstamp3;
	struct timespec tstamp4;
	struct clock_id client_clock_id;
	struct clock_id master_clock_id;
	struct timeval new_adj;
	int64_t delta;
	uint16_t portid;
	uint16_t seqID_SYNC;
	uint16_t seqID_FOLLOWUP;
	uint8_t ptpset;
	uint8_t kernel_time_set;
	uint16_t current_ptp_port;
};

static struct ptpv2_data_slave_ordinary ptp_data;

static inline uint64_t timespec64_to_ns(const struct timespec *ts)
{
	return ((uint64_t) ts->tv_sec * NSEC_PER_SEC) + ts->tv_nsec;
}

static struct timeval
ns_to_timeval(int64_t nsec)
{
	struct timespec t_spec = {0, 0};
	struct timeval t_eval = {0, 0};
	int32_t rem;

	if (nsec == 0)
		return t_eval;
	rem = nsec % NSEC_PER_SEC;
	t_spec.tv_sec = nsec / NSEC_PER_SEC;

	if (rem < 0) {
		t_spec.tv_sec--;
		rem += NSEC_PER_SEC;
	}

	t_spec.tv_nsec = rem;
	t_eval.tv_sec = t_spec.tv_sec;
	t_eval.tv_usec = t_spec.tv_nsec / 1000;

	return t_eval;
}

/*
 * Initializes a given port using global settings and with the RX buffers
 * coming from the mbuf_pool passed as a parameter.
 */
static inline int
port_init(uint16_t port, struct rte_mempool *mbuf_pool)
{
	struct rte_eth_dev_info dev_info;
	struct rte_eth_conf port_conf = port_conf_default;
	const uint16_t rx_rings = 1;
	const uint16_t tx_rings = 1;
	int retval;
	uint16_t q;
	uint16_t nb_rxd = RX_RING_SIZE;
	uint16_t nb_txd = TX_RING_SIZE;

	if (port >= rte_eth_dev_count())
		return -1;

	rte_eth_dev_info_get(port, &dev_info);
	if (dev_info.tx_offload_capa & DEV_TX_OFFLOAD_MBUF_FAST_FREE)
		port_conf.txmode.offloads |=
			DEV_TX_OFFLOAD_MBUF_FAST_FREE;
	/* Force full Tx path in the driver, required for IEEE1588 */
	port_conf.txmode.offloads |= DEV_TX_OFFLOAD_MULTI_SEGS;

	/* Configure the Ethernet device. */
	retval = rte_eth_dev_configure(port, rx_rings, tx_rings, &port_conf);
	if (retval != 0)
		return retval;

	retval = rte_eth_dev_adjust_nb_rx_tx_desc(port, &nb_rxd, &nb_txd);
	if (retval != 0)
		return retval;

	/* Allocate and set up 1 RX queue per Ethernet port. */
	for (q = 0; q < rx_rings; q++) {
		retval = rte_eth_rx_queue_setup(port, q, nb_rxd,
				rte_eth_dev_socket_id(port), NULL, mbuf_pool);

		if (retval < 0)
			return retval;
	}

	/* Allocate and set up 1 TX queue per Ethernet port. */
	for (q = 0; q < tx_rings; q++) {
		/* Setup txq_flags */
		struct rte_eth_txconf *txconf;

		txconf = &dev_info.default_txconf;
		txconf->txq_flags = ETH_TXQ_FLAGS_IGNORE;
		txconf->offloads = port_conf.txmode.offloads;

		retval = rte_eth_tx_queue_setup(port, q, nb_txd,
				rte_eth_dev_socket_id(port), txconf);
		if (retval < 0)
			return retval;
	}

	/* Start the Ethernet port. */
	retval = rte_eth_dev_start(port);
	if (retval < 0)
		return retval;

	/* Enable timesync timestamping for the Ethernet device */
	rte_eth_timesync_enable(port);

	/* Enable RX in promiscuous mode for the Ethernet device. */
	rte_eth_promiscuous_enable(port);

	return 0;
}

static void
print_clock_info(struct ptpv2_data_slave_ordinary *ptp_data)
{
	int64_t nsec;
	struct timespec net_time, sys_time;

	printf("Master Clock id: %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x",
		ptp_data->master_clock_id.id[0],
		ptp_data->master_clock_id.id[1],
		ptp_data->master_clock_id.id[2],
		ptp_data->master_clock_id.id[3],
		ptp_data->master_clock_id.id[4],
		ptp_data->master_clock_id.id[5],
		ptp_data->master_clock_id.id[6],
		ptp_data->master_clock_id.id[7]);

	printf("\nT2 - Slave  Clock.  %lds %ldns",
			(ptp_data->tstamp2.tv_sec),
			(ptp_data->tstamp2.tv_nsec));

	printf("\nT1 - Master Clock.  %lds %ldns ",
			ptp_data->tstamp1.tv_sec,
			(ptp_data->tstamp1.tv_nsec));

	printf("\nT3 - Slave  Clock.  %lds %ldns",
			ptp_data->tstamp3.tv_sec,
			(ptp_data->tstamp3.tv_nsec));

	printf("\nT4 - Master Clock.  %lds %ldns ",
			ptp_data->tstamp4.tv_sec,
			(ptp_data->tstamp4.tv_nsec));

	printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
			ptp_data->delta);

	clock_gettime(CLOCK_REALTIME, &sys_time);
	rte_eth_timesync_read_time(ptp_data->current_ptp_port, &net_time);

	time_t ts = net_time.tv_sec;

	printf("\n\nComparison between Linux kernel Time and PTP:");

	printf("\nCurrent PTP Time: %.24s %.9ld ns",
			ctime(&ts), net_time.tv_nsec);

	nsec = (int64_t)timespec64_to_ns(&net_time) -
			(int64_t)timespec64_to_ns(&sys_time);
	ptp_data->new_adj = ns_to_timeval(nsec);

	gettimeofday(&ptp_data->new_adj, NULL);

	time_t tp = ptp_data->new_adj.tv_sec;

	printf("\nCurrent SYS Time: %.24s %.6ld ns",
				ctime(&tp), ptp_data->new_adj.tv_usec);

	printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
				nsec);

	printf("[Ctrl+C to quit]\n");

	/* Clear screen and put cursor in column 1, row 1 */
	printf("\033[2J\033[1;1H");
}

static int64_t
delta_eval(struct ptpv2_data_slave_ordinary *ptp_data)
{
	int64_t delta;
	uint64_t t1 = 0;
	uint64_t t2 = 0;
	uint64_t t3 = 0;
	uint64_t t4 = 0;

	t1 = timespec64_to_ns(&ptp_data->tstamp1);
	t2 = timespec64_to_ns(&ptp_data->tstamp2);
	t3 = timespec64_to_ns(&ptp_data->tstamp3);
	t4 = timespec64_to_ns(&ptp_data->tstamp4);

	delta = -((int64_t)((t2 - t1) - (t4 - t3))) / 2;

	return delta;
}

/*
 * Parse the PTP SYNC message.
 */
static void
parse_sync(struct ptpv2_data_slave_ordinary *ptp_data, uint16_t rx_tstamp_idx)
{
	struct ptp_header *ptp_hdr;

	ptp_hdr = (struct ptp_header *)(rte_pktmbuf_mtod(ptp_data->m, char *)
			+ sizeof(struct ether_hdr));
	ptp_data->seqID_SYNC = rte_be_to_cpu_16(ptp_hdr->seq_id);

	if (ptp_data->ptpset == 0) {
		rte_memcpy(&ptp_data->master_clock_id,
				&ptp_hdr->source_port_id.clock_id,
				sizeof(struct clock_id));
		ptp_data->ptpset = 1;
	}

	if (memcmp(&ptp_hdr->source_port_id.clock_id,
			&ptp_hdr->source_port_id.clock_id,
			sizeof(struct clock_id)) == 0) {

		if (ptp_data->ptpset == 1)
			rte_eth_timesync_read_rx_timestamp(ptp_data->portid,
					&ptp_data->tstamp2, rx_tstamp_idx);
	}

}

/*
 * Parse the PTP FOLLOWUP message and send DELAY_REQ to the master clock.
 */
static void
parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
{
	struct ether_hdr *eth_hdr;
	struct ptp_header *ptp_hdr;
	struct clock_id *client_clkid;
	struct ptp_message *ptp_msg;
	struct rte_mbuf *created_pkt;
	struct tstamp *origin_tstamp;
	struct ether_addr eth_multicast = ether_multicast;
	size_t pkt_size;
	int wait_us;
	struct rte_mbuf *m = ptp_data->m;

	eth_hdr = rte_pktmbuf_mtod(m, struct ether_hdr *);
	ptp_hdr = (struct ptp_header *)(rte_pktmbuf_mtod(m, char *)
			+ sizeof(struct ether_hdr));
	if (memcmp(&ptp_data->master_clock_id,
			&ptp_hdr->source_port_id.clock_id,
			sizeof(struct clock_id)) != 0)
		return;

	ptp_data->seqID_FOLLOWUP = rte_be_to_cpu_16(ptp_hdr->seq_id);
	ptp_msg = (struct ptp_message *) (rte_pktmbuf_mtod(m, char *) +
					  sizeof(struct ether_hdr));

	origin_tstamp = &ptp_msg->follow_up.precise_origin_tstamp;
	ptp_data->tstamp1.tv_nsec = ntohl(origin_tstamp->ns);
	ptp_data->tstamp1.tv_sec =
		((uint64_t)ntohl(origin_tstamp->sec_lsb)) |
		(((uint64_t)ntohs(origin_tstamp->sec_msb)) << 32);

	if (ptp_data->seqID_FOLLOWUP == ptp_data->seqID_SYNC) {

		created_pkt = rte_pktmbuf_alloc(mbuf_pool);
		pkt_size = sizeof(struct ether_hdr) +
			sizeof(struct ptp_message);
		created_pkt->data_len = pkt_size;
		created_pkt->pkt_len = pkt_size;
		eth_hdr = rte_pktmbuf_mtod(created_pkt, struct ether_hdr *);
		rte_eth_macaddr_get(ptp_data->portid, &eth_hdr->s_addr);

		/* Set multicast address 01-1B-19-00-00-00. */
		ether_addr_copy(&eth_multicast, &eth_hdr->d_addr);

		eth_hdr->ether_type = htons(PTP_PROTOCOL);
		ptp_msg = (struct ptp_message *)
			(rte_pktmbuf_mtod(created_pkt, char *) +
			sizeof(struct ether_hdr));

		ptp_msg->delay_req.hdr.seq_id = htons(ptp_data->seqID_SYNC);
		ptp_msg->delay_req.hdr.msg_type = DELAY_REQ;
		ptp_msg->delay_req.hdr.ver = 2;
		ptp_msg->delay_req.hdr.control = 1;
		ptp_msg->delay_req.hdr.log_message_interval = 127;

		/* Set up clock id. */
		client_clkid =
			&ptp_msg->delay_req.hdr.source_port_id.clock_id;

		client_clkid->id[0] = eth_hdr->s_addr.addr_bytes[0];
		client_clkid->id[1] = eth_hdr->s_addr.addr_bytes[1];
		client_clkid->id[2] = eth_hdr->s_addr.addr_bytes[2];
		client_clkid->id[3] = 0xFF;
		client_clkid->id[4] = 0xFE;
		client_clkid->id[5] = eth_hdr->s_addr.addr_bytes[3];
		client_clkid->id[6] = eth_hdr->s_addr.addr_bytes[4];
		client_clkid->id[7] = eth_hdr->s_addr.addr_bytes[5];

		rte_memcpy(&ptp_data->client_clock_id,
			   client_clkid,
			   sizeof(struct clock_id));

		/* Enable flag for hardware timestamping. */
		created_pkt->ol_flags |= PKT_TX_IEEE1588_TMST;

		/*Read value from NIC to prevent latching with old value. */
		rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
				&ptp_data->tstamp3);

		/* Transmit the packet. */
		rte_eth_tx_burst(ptp_data->portid, 0, &created_pkt, 1);

		wait_us = 0;
		ptp_data->tstamp3.tv_nsec = 0;
		ptp_data->tstamp3.tv_sec = 0;

		/* Wait at least 1 us to read TX timestamp. */
		while ((rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
				&ptp_data->tstamp3) < 0) && (wait_us < 1000)) {
			rte_delay_us(1);
			wait_us++;
		}
	}
}

/*
 * Update the kernel time with the difference between it and the current NIC
 * time.
 */
static inline void
update_kernel_time(void)
{
	int64_t nsec;
	struct timespec net_time, sys_time;

	clock_gettime(CLOCK_REALTIME, &sys_time);
	rte_eth_timesync_read_time(ptp_data.current_ptp_port, &net_time);

	nsec = (int64_t)timespec64_to_ns(&net_time) -
	       (int64_t)timespec64_to_ns(&sys_time);

	ptp_data.new_adj = ns_to_timeval(nsec);

	/*
	 * If difference between kernel time and system time in NIC is too big
	 * (more than +/- 20 microseconds), use clock_settime to set directly
	 * the kernel time, as adjtime is better for small adjustments (takes
	 * longer to adjust the time).
	 */

	if (nsec > KERNEL_TIME_ADJUST_LIMIT || nsec < -KERNEL_TIME_ADJUST_LIMIT)
		clock_settime(CLOCK_REALTIME, &net_time);
	else
		adjtime(&ptp_data.new_adj, 0);


}

/*
 * Parse the DELAY_RESP message.
 */
static void
parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
{
	struct rte_mbuf *m = ptp_data->m;
	struct ptp_message *ptp_msg;
	struct tstamp *rx_tstamp;
	uint16_t seq_id;

	ptp_msg = (struct ptp_message *) (rte_pktmbuf_mtod(m, char *) +
					sizeof(struct ether_hdr));
	seq_id = rte_be_to_cpu_16(ptp_msg->delay_resp.hdr.seq_id);
	if (memcmp(&ptp_data->client_clock_id,
		   &ptp_msg->delay_resp.req_port_id.clock_id,
		   sizeof(struct clock_id)) == 0) {
		if (seq_id == ptp_data->seqID_FOLLOWUP) {
			rx_tstamp = &ptp_msg->delay_resp.rx_tstamp;
			ptp_data->tstamp4.tv_nsec = ntohl(rx_tstamp->ns);
			ptp_data->tstamp4.tv_sec =
				((uint64_t)ntohl(rx_tstamp->sec_lsb)) |
				(((uint64_t)ntohs(rx_tstamp->sec_msb)) << 32);

			/* Evaluate the delta for adjustment. */
			ptp_data->delta = delta_eval(ptp_data);

			rte_eth_timesync_adjust_time(ptp_data->portid,
						     ptp_data->delta);

			ptp_data->current_ptp_port = ptp_data->portid;

			/* Update kernel time if enabled in app parameters. */
			if (ptp_data->kernel_time_set == 1)
				update_kernel_time();



		}
	}
}

/* This function processes PTP packets, implementing slave PTP IEEE1588 L2
 * functionality.
 */
static void
parse_ptp_frames(uint16_t portid, struct rte_mbuf *m) {
	struct ptp_header *ptp_hdr;
	struct ether_hdr *eth_hdr;
	uint16_t eth_type;

	eth_hdr = rte_pktmbuf_mtod(m, struct ether_hdr *);
	eth_type = rte_be_to_cpu_16(eth_hdr->ether_type);

	if (eth_type == PTP_PROTOCOL) {
		ptp_data.m = m;
		ptp_data.portid = portid;
		ptp_hdr = (struct ptp_header *)(rte_pktmbuf_mtod(m, char *)
					+ sizeof(struct ether_hdr));

		switch (ptp_hdr->msg_type) {
		case SYNC:
			parse_sync(&ptp_data, m->timesync);
			break;
		case FOLLOW_UP:
			parse_fup(&ptp_data);
			break;
		case DELAY_RESP:
			parse_drsp(&ptp_data);
			print_clock_info(&ptp_data);
			break;
		default:
			break;
		}
	}
}

/*
 * The lcore main. This is the main thread that does the work, reading from an
 * input port and writing to an output port.
 */
static __attribute__((noreturn)) void
lcore_main(void)
{
	uint16_t portid;
	unsigned nb_rx;
	struct rte_mbuf *m;

	/*
	 * Check that the port is on the same NUMA node as the polling thread
	 * for best performance.
	 */
	printf("\nCore %u Waiting for SYNC packets. [Ctrl+C to quit]\n",
			rte_lcore_id());

	/* Run until the application is quit or killed. */

	while (1) {
		/* Read packet from RX queues. */
		for (portid = 0; portid < ptp_enabled_port_nb; portid++) {

			portid = ptp_enabled_ports[portid];
			nb_rx = rte_eth_rx_burst(portid, 0, &m, 1);

			if (likely(nb_rx == 0))
				continue;

			if (m->ol_flags & PKT_RX_IEEE1588_PTP)
				parse_ptp_frames(portid, m);

			rte_pktmbuf_free(m);
		}
	}
}

static void
print_usage(const char *prgname)
{
	printf("%s [EAL options] -- -p PORTMASK -T VALUE\n"
		" -T VALUE: 0 - Disable, 1 - Enable Linux Clock"
		" Synchronization (0 default)\n"
		" -p PORTMASK: hexadecimal bitmask of ports to configure\n",
		prgname);
}

static int
ptp_parse_portmask(const char *portmask)
{
	char *end = NULL;
	unsigned long pm;

	/* Parse the hexadecimal string. */
	pm = strtoul(portmask, &end, 16);

	if ((portmask[0] == '\0') || (end == NULL) || (*end != '\0'))
		return -1;

	if (pm == 0)
		return -1;

	return pm;
}

static int
parse_ptp_kernel(const char *param)
{
	char *end = NULL;
	unsigned long pm;

	/* Parse the hexadecimal string. */
	pm = strtoul(param, &end, 16);

	if ((param[0] == '\0') || (end == NULL) || (*end != '\0'))
		return -1;
	if (pm == 0)
		return 0;

	return 1;
}

/* Parse the commandline arguments. */
static int
ptp_parse_args(int argc, char **argv)
{
	int opt, ret;
	char **argvopt;
	int option_index;
	char *prgname = argv[0];
	static struct option lgopts[] = { {NULL, 0, 0, 0} };

	argvopt = argv;

	while ((opt = getopt_long(argc, argvopt, "p:T:",
				  lgopts, &option_index)) != EOF) {

		switch (opt) {

		/* Portmask. */
		case 'p':
			ptp_enabled_port_mask = ptp_parse_portmask(optarg);
			if (ptp_enabled_port_mask == 0) {
				printf("invalid portmask\n");
				print_usage(prgname);
				return -1;
			}
			break;
		/* Time synchronization. */
		case 'T':
			ret = parse_ptp_kernel(optarg);
			if (ret < 0) {
				print_usage(prgname);
				return -1;
			}

			ptp_data.kernel_time_set = ret;
			break;

		default:
			print_usage(prgname);
			return -1;
		}
	}

	argv[optind-1] = prgname;

	optind = 1; /* Reset getopt lib. */

	return 0;
}

/*
 * The main function, which does initialization and calls the per-lcore
 * functions.
 */
int
main(int argc, char *argv[])
{
	unsigned nb_ports;

	uint16_t portid;

	/* Initialize the Environment Abstraction Layer (EAL). */
	int ret = rte_eal_init(argc, argv);

	if (ret < 0)
		rte_exit(EXIT_FAILURE, "Error with EAL initialization\n");

	memset(&ptp_data, '\0', sizeof(struct ptpv2_data_slave_ordinary));

	argc -= ret;
	argv += ret;

	ret = ptp_parse_args(argc, argv);
	if (ret < 0)
		rte_exit(EXIT_FAILURE, "Error with PTP initialization\n");

	/* Check that there is an even number of ports to send/receive on. */
	nb_ports = rte_eth_dev_count();

	/* Creates a new mempool in memory to hold the mbufs. */
	mbuf_pool = rte_pktmbuf_pool_create("MBUF_POOL", NUM_MBUFS * nb_ports,
		MBUF_CACHE_SIZE, 0, RTE_MBUF_DEFAULT_BUF_SIZE, rte_socket_id());

	if (mbuf_pool == NULL)
		rte_exit(EXIT_FAILURE, "Cannot create mbuf pool\n");

	/* Initialize all ports. */
	for (portid = 0; portid < nb_ports; portid++) {
		if ((ptp_enabled_port_mask & (1 << portid)) != 0) {
			if (port_init(portid, mbuf_pool) == 0) {
				ptp_enabled_ports[ptp_enabled_port_nb] = portid;
				ptp_enabled_port_nb++;
			} else {
				rte_exit(EXIT_FAILURE,
					 "Cannot init port %"PRIu8 "\n",
					 portid);
			}
		} else
			printf("Skipping disabled port %u\n", portid);
	}

	if (ptp_enabled_port_nb == 0) {
		rte_exit(EXIT_FAILURE,
			"All available ports are disabled."
			" Please set portmask.\n");
	}

	if (rte_lcore_count() > 1)
		printf("\nWARNING: Too many lcores enabled. Only 1 used.\n");

	/* Call lcore_main on the master core only. */
	lcore_main();

	return 0;
}