examples/ptpclient/ptpclient.c

   1 /* SPDX-License-Identifier: BSD-3-Clause
   2  * Copyright(c) 2015 Intel Corporation
   3  */
   4
   5 /*
   6  * This application is a simple Layer 2 PTP v2 client. It shows delta values
   7  * which are used to synchronize the PHC clock. if the "-T 1" parameter is
   8  * passed to the application the Linux kernel clock is also synchronized.
   9  */
  10
  11 #include <stdint.h>
  12 #include <inttypes.h>
  13 #include <rte_eal.h>
  14 #include <rte_ethdev.h>
  15 #include <rte_cycles.h>
  16 #include <rte_lcore.h>
  17 #include <rte_mbuf.h>
  18 #include <rte_ip.h>
  19 #include <limits.h>
  20 #include <sys/time.h>
  21 #include <getopt.h>
  22
  23 #define RX_RING_SIZE 128
  24 #define TX_RING_SIZE 512
  25
  26 #define NUM_MBUFS            8191
  27 #define MBUF_CACHE_SIZE       250
  28
  29 /* Values for the PTP messageType field. */
  30 #define SYNC                  0x0
  31 #define DELAY_REQ             0x1
  32 #define PDELAY_REQ            0x2
  33 #define PDELAY_RESP           0x3
  34 #define FOLLOW_UP             0x8
  35 #define DELAY_RESP            0x9
  36 #define PDELAY_RESP_FOLLOW_UP 0xA
  37 #define ANNOUNCE              0xB
  38 #define SIGNALING             0xC
  39 #define MANAGEMENT            0xD
  40
  41 #define NSEC_PER_SEC        1000000000L
  42 #define KERNEL_TIME_ADJUST_LIMIT  20000
  43 #define PTP_PROTOCOL             0x88F7
  44
  45 struct rte_mempool *mbuf_pool;
  46 uint32_t ptp_enabled_port_mask;
  47 uint8_t ptp_enabled_port_nb;
  48 static uint8_t ptp_enabled_ports[RTE_MAX_ETHPORTS];
  49
  50 static const struct rte_eth_conf port_conf_default = {
  51         .rxmode = { .max_rx_pkt_len = ETHER_MAX_LEN }
  52 };
  53
  54 static const struct ether_addr ether_multicast = {
  55         .addr_bytes = {0x01, 0x1b, 0x19, 0x0, 0x0, 0x0}
  56 };
  57
  58 /* Structs used for PTP handling. */
  59 struct tstamp {
  60         uint16_t   sec_msb;
  61         uint32_t   sec_lsb;
  62         uint32_t   ns;
  63 }  __attribute__((packed));
  64
  65 struct clock_id {
  66         uint8_t id[8];
  67 };
  68
  69 struct port_id {
  70         struct clock_id        clock_id;
  71         uint16_t               port_number;
  72 }  __attribute__((packed));
  73
  74 struct ptp_header {
  75         uint8_t              msg_type;
  76         uint8_t              ver;
  77         uint16_t             message_length;
  78         uint8_t              domain_number;
  79         uint8_t              reserved1;
  80         uint8_t              flag_field[2];
  81         int64_t              correction;
  82         uint32_t             reserved2;
  83         struct port_id       source_port_id;
  84         uint16_t             seq_id;
  85         uint8_t              control;
  86         int8_t               log_message_interval;
  87 } __attribute__((packed));
  88
  89 struct sync_msg {
  90         struct ptp_header   hdr;
  91         struct tstamp       origin_tstamp;
  92 } __attribute__((packed));
  93
  94 struct follow_up_msg {
  95         struct ptp_header   hdr;
  96         struct tstamp       precise_origin_tstamp;
  97         uint8_t             suffix[0];
  98 } __attribute__((packed));
  99
 100 struct delay_req_msg {
 101         struct ptp_header   hdr;
 102         struct tstamp       origin_tstamp;
 103 } __attribute__((packed));
 104
 105 struct delay_resp_msg {
 106         struct ptp_header    hdr;
 107         struct tstamp        rx_tstamp;
 108         struct port_id       req_port_id;
 109         uint8_t              suffix[0];
 110 } __attribute__((packed));
 111
 112 struct ptp_message {
 113         union {
 114                 struct ptp_header          header;
 115                 struct sync_msg            sync;
 116                 struct delay_req_msg       delay_req;
 117                 struct follow_up_msg       follow_up;
 118                 struct delay_resp_msg      delay_resp;
 119         } __attribute__((packed));
 120 };
 121
 122 struct ptpv2_data_slave_ordinary {
 123         struct rte_mbuf *m;
 124         struct timespec tstamp1;
 125         struct timespec tstamp2;
 126         struct timespec tstamp3;
 127         struct timespec tstamp4;
 128         struct clock_id client_clock_id;
 129         struct clock_id master_clock_id;
 130         struct timeval new_adj;
 131         int64_t delta;
 132         uint16_t portid;
 133         uint16_t seqID_SYNC;
 134         uint16_t seqID_FOLLOWUP;
 135         uint8_t ptpset;
 136         uint8_t kernel_time_set;
 137         uint16_t current_ptp_port;
 138 };
 139
 140 static struct ptpv2_data_slave_ordinary ptp_data;
 141
 142 static inline uint64_t timespec64_to_ns(const struct timespec *ts)
 143 {
 144         return ((uint64_t) ts->tv_sec * NSEC_PER_SEC) + ts->tv_nsec;
 145 }
 146
 147 static struct timeval
 148 ns_to_timeval(int64_t nsec)
 149 {
 150         struct timespec t_spec = {0, 0};
 151         struct timeval t_eval = {0, 0};
 152         int32_t rem;
 153
 154         if (nsec == 0)
 155                 return t_eval;
 156         rem = nsec % NSEC_PER_SEC;
 157         t_spec.tv_sec = nsec / NSEC_PER_SEC;
 158
 159         if (rem < 0) {
 160                 t_spec.tv_sec--;
 161                 rem += NSEC_PER_SEC;
 162         }
 163
 164         t_spec.tv_nsec = rem;
 165         t_eval.tv_sec = t_spec.tv_sec;
 166         t_eval.tv_usec = t_spec.tv_nsec / 1000;
 167
 168         return t_eval;
 169 }
 170
 171 /*
 172  * Initializes a given port using global settings and with the RX buffers
 173  * coming from the mbuf_pool passed as a parameter.
 174  */
 175 static inline int
 176 port_init(uint16_t port, struct rte_mempool *mbuf_pool)
 177 {
 178         struct rte_eth_dev_info dev_info;
 179         struct rte_eth_conf port_conf = port_conf_default;
 180         const uint16_t rx_rings = 1;
 181         const uint16_t tx_rings = 1;
 182         int retval;
 183         uint16_t q;
 184         uint16_t nb_rxd = RX_RING_SIZE;
 185         uint16_t nb_txd = TX_RING_SIZE;
 186
 187         if (port >= rte_eth_dev_count())
 188                 return -1;
 189
 190         /* Configure the Ethernet device. */
 191         retval = rte_eth_dev_configure(port, rx_rings, tx_rings, &port_conf);
 192         if (retval != 0)
 193                 return retval;
 194
 195         retval = rte_eth_dev_adjust_nb_rx_tx_desc(port, &nb_rxd, &nb_txd);
 196         if (retval != 0)
 197                 return retval;
 198
 199         /* Allocate and set up 1 RX queue per Ethernet port. */
 200         for (q = 0; q < rx_rings; q++) {
 201                 retval = rte_eth_rx_queue_setup(port, q, nb_rxd,
 202                                 rte_eth_dev_socket_id(port), NULL, mbuf_pool);
 203
 204                 if (retval < 0)
 205                         return retval;
 206         }
 207
 208         /* Allocate and set up 1 TX queue per Ethernet port. */
 209         for (q = 0; q < tx_rings; q++) {
 210                 /* Setup txq_flags */
 211                 struct rte_eth_txconf *txconf;
 212
 213                 rte_eth_dev_info_get(q, &dev_info);
 214                 txconf = &dev_info.default_txconf;
 215                 txconf->txq_flags = 0;
 216
 217                 retval = rte_eth_tx_queue_setup(port, q, nb_txd,
 218                                 rte_eth_dev_socket_id(port), txconf);
 219                 if (retval < 0)
 220                         return retval;
 221         }
 222
 223         /* Start the Ethernet port. */
 224         retval = rte_eth_dev_start(port);
 225         if (retval < 0)
 226                 return retval;
 227
 228         /* Enable timesync timestamping for the Ethernet device */
 229         rte_eth_timesync_enable(port);
 230
 231         /* Enable RX in promiscuous mode for the Ethernet device. */
 232         rte_eth_promiscuous_enable(port);
 233
 234         return 0;
 235 }
 236
 237 static void
 238 print_clock_info(struct ptpv2_data_slave_ordinary *ptp_data)
 239 {
 240         int64_t nsec;
 241         struct timespec net_time, sys_time;
 242
 243         printf("Master Clock id: %02x:%02x:%02x:%02x:%02x:%02x:%02x:%02x",
 244                 ptp_data->master_clock_id.id[0],
 245                 ptp_data->master_clock_id.id[1],
 246                 ptp_data->master_clock_id.id[2],
 247                 ptp_data->master_clock_id.id[3],
 248                 ptp_data->master_clock_id.id[4],
 249                 ptp_data->master_clock_id.id[5],
 250                 ptp_data->master_clock_id.id[6],
 251                 ptp_data->master_clock_id.id[7]);
 252
 253         printf("\nT2 - Slave  Clock.  %lds %ldns",
 254                         (ptp_data->tstamp2.tv_sec),
 255                         (ptp_data->tstamp2.tv_nsec));
 256
 257         printf("\nT1 - Master Clock.  %lds %ldns ",
 258                         ptp_data->tstamp1.tv_sec,
 259                         (ptp_data->tstamp1.tv_nsec));
 260
 261         printf("\nT3 - Slave  Clock.  %lds %ldns",
 262                         ptp_data->tstamp3.tv_sec,
 263                         (ptp_data->tstamp3.tv_nsec));
 264
 265         printf("\nT4 - Master Clock.  %lds %ldns ",
 266                         ptp_data->tstamp4.tv_sec,
 267                         (ptp_data->tstamp4.tv_nsec));
 268
 269         printf("\nDelta between master and slave clocks:%"PRId64"ns\n",
 270                         ptp_data->delta);
 271
 272         clock_gettime(CLOCK_REALTIME, &sys_time);
 273         rte_eth_timesync_read_time(ptp_data->current_ptp_port, &net_time);
 274
 275         time_t ts = net_time.tv_sec;
 276
 277         printf("\n\nComparison between Linux kernel Time and PTP:");
 278
 279         printf("\nCurrent PTP Time: %.24s %.9ld ns",
 280                         ctime(&ts), net_time.tv_nsec);
 281
 282         nsec = (int64_t)timespec64_to_ns(&net_time) -
 283                         (int64_t)timespec64_to_ns(&sys_time);
 284         ptp_data->new_adj = ns_to_timeval(nsec);
 285
 286         gettimeofday(&ptp_data->new_adj, NULL);
 287
 288         time_t tp = ptp_data->new_adj.tv_sec;
 289
 290         printf("\nCurrent SYS Time: %.24s %.6ld ns",
 291                                 ctime(&tp), ptp_data->new_adj.tv_usec);
 292
 293         printf("\nDelta between PTP and Linux Kernel time:%"PRId64"ns\n",
 294                                 nsec);
 295
 296         printf("[Ctrl+C to quit]\n");
 297
 298         /* Clear screen and put cursor in column 1, row 1 */
 299         printf("\033[2J\033[1;1H");
 300 }
 301
 302 static int64_t
 303 delta_eval(struct ptpv2_data_slave_ordinary *ptp_data)
 304 {
 305         int64_t delta;
 306         uint64_t t1 = 0;
 307         uint64_t t2 = 0;
 308         uint64_t t3 = 0;
 309         uint64_t t4 = 0;
 310
 311         t1 = timespec64_to_ns(&ptp_data->tstamp1);
 312         t2 = timespec64_to_ns(&ptp_data->tstamp2);
 313         t3 = timespec64_to_ns(&ptp_data->tstamp3);
 314         t4 = timespec64_to_ns(&ptp_data->tstamp4);
 315
 316         delta = -((int64_t)((t2 - t1) - (t4 - t3))) / 2;
 317
 318         return delta;
 319 }
 320
 321 /*
 322  * Parse the PTP SYNC message.
 323  */
 324 static void
 325 parse_sync(struct ptpv2_data_slave_ordinary *ptp_data, uint16_t rx_tstamp_idx)
 326 {
 327         struct ptp_header *ptp_hdr;
 328
 329         ptp_hdr = (struct ptp_header *)(rte_pktmbuf_mtod(ptp_data->m, char *)
 330                         + sizeof(struct ether_hdr));
 331         ptp_data->seqID_SYNC = rte_be_to_cpu_16(ptp_hdr->seq_id);
 332
 333         if (ptp_data->ptpset == 0) {
 334                 rte_memcpy(&ptp_data->master_clock_id,
 335                                 &ptp_hdr->source_port_id.clock_id,
 336                                 sizeof(struct clock_id));
 337                 ptp_data->ptpset = 1;
 338         }
 339
 340         if (memcmp(&ptp_hdr->source_port_id.clock_id,
 341                         &ptp_hdr->source_port_id.clock_id,
 342                         sizeof(struct clock_id)) == 0) {
 343
 344                 if (ptp_data->ptpset == 1)
 345                         rte_eth_timesync_read_rx_timestamp(ptp_data->portid,
 346                                         &ptp_data->tstamp2, rx_tstamp_idx);
 347         }
 348
 349 }
 350
 351 /*
 352  * Parse the PTP FOLLOWUP message and send DELAY_REQ to the master clock.
 353  */
 354 static void
 355 parse_fup(struct ptpv2_data_slave_ordinary *ptp_data)
 356 {
 357         struct ether_hdr *eth_hdr;
 358         struct ptp_header *ptp_hdr;
 359         struct clock_id *client_clkid;
 360         struct ptp_message *ptp_msg;
 361         struct rte_mbuf *created_pkt;
 362         struct tstamp *origin_tstamp;
 363         struct ether_addr eth_multicast = ether_multicast;
 364         size_t pkt_size;
 365         int wait_us;
 366         struct rte_mbuf *m = ptp_data->m;
 367
 368         eth_hdr = rte_pktmbuf_mtod(m, struct ether_hdr *);
 369         ptp_hdr = (struct ptp_header *)(rte_pktmbuf_mtod(m, char *)
 370                         + sizeof(struct ether_hdr));
 371         if (memcmp(&ptp_data->master_clock_id,
 372                         &ptp_hdr->source_port_id.clock_id,
 373                         sizeof(struct clock_id)) != 0)
 374                 return;
 375
 376         ptp_data->seqID_FOLLOWUP = rte_be_to_cpu_16(ptp_hdr->seq_id);
 377         ptp_msg = (struct ptp_message *) (rte_pktmbuf_mtod(m, char *) +
 378                                           sizeof(struct ether_hdr));
 379
 380         origin_tstamp = &ptp_msg->follow_up.precise_origin_tstamp;
 381         ptp_data->tstamp1.tv_nsec = ntohl(origin_tstamp->ns);
 382         ptp_data->tstamp1.tv_sec =
 383                 ((uint64_t)ntohl(origin_tstamp->sec_lsb)) |
 384                 (((uint64_t)ntohs(origin_tstamp->sec_msb)) << 32);
 385
 386         if (ptp_data->seqID_FOLLOWUP == ptp_data->seqID_SYNC) {
 387
 388                 created_pkt = rte_pktmbuf_alloc(mbuf_pool);
 389                 pkt_size = sizeof(struct ether_hdr) +
 390                         sizeof(struct ptp_message);
 391                 created_pkt->data_len = pkt_size;
 392                 created_pkt->pkt_len = pkt_size;
 393                 eth_hdr = rte_pktmbuf_mtod(created_pkt, struct ether_hdr *);
 394                 rte_eth_macaddr_get(ptp_data->portid, &eth_hdr->s_addr);
 395
 396                 /* Set multicast address 01-1B-19-00-00-00. */
 397                 ether_addr_copy(&eth_multicast, &eth_hdr->d_addr);
 398
 399                 eth_hdr->ether_type = htons(PTP_PROTOCOL);
 400                 ptp_msg = (struct ptp_message *)
 401                         (rte_pktmbuf_mtod(created_pkt, char *) +
 402                         sizeof(struct ether_hdr));
 403
 404                 ptp_msg->delay_req.hdr.seq_id = htons(ptp_data->seqID_SYNC);
 405                 ptp_msg->delay_req.hdr.msg_type = DELAY_REQ;
 406                 ptp_msg->delay_req.hdr.ver = 2;
 407                 ptp_msg->delay_req.hdr.control = 1;
 408                 ptp_msg->delay_req.hdr.log_message_interval = 127;
 409
 410                 /* Set up clock id. */
 411                 client_clkid =
 412                         &ptp_msg->delay_req.hdr.source_port_id.clock_id;
 413
 414                 client_clkid->id[0] = eth_hdr->s_addr.addr_bytes[0];
 415                 client_clkid->id[1] = eth_hdr->s_addr.addr_bytes[1];
 416                 client_clkid->id[2] = eth_hdr->s_addr.addr_bytes[2];
 417                 client_clkid->id[3] = 0xFF;
 418                 client_clkid->id[4] = 0xFE;
 419                 client_clkid->id[5] = eth_hdr->s_addr.addr_bytes[3];
 420                 client_clkid->id[6] = eth_hdr->s_addr.addr_bytes[4];
 421                 client_clkid->id[7] = eth_hdr->s_addr.addr_bytes[5];
 422
 423                 rte_memcpy(&ptp_data->client_clock_id,
 424                            client_clkid,
 425                            sizeof(struct clock_id));
 426
 427                 /* Enable flag for hardware timestamping. */
 428                 created_pkt->ol_flags |= PKT_TX_IEEE1588_TMST;
 429
 430                 /*Read value from NIC to prevent latching with old value. */
 431                 rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
 432                                 &ptp_data->tstamp3);
 433
 434                 /* Transmit the packet. */
 435                 rte_eth_tx_burst(ptp_data->portid, 0, &created_pkt, 1);
 436
 437                 wait_us = 0;
 438                 ptp_data->tstamp3.tv_nsec = 0;
 439                 ptp_data->tstamp3.tv_sec = 0;
 440
 441                 /* Wait at least 1 us to read TX timestamp. */
 442                 while ((rte_eth_timesync_read_tx_timestamp(ptp_data->portid,
 443                                 &ptp_data->tstamp3) < 0) && (wait_us < 1000)) {
 444                         rte_delay_us(1);
 445                         wait_us++;
 446                 }
 447         }
 448 }
 449
 450 /*
 451  * Update the kernel time with the difference between it and the current NIC
 452  * time.
 453  */
 454 static inline void
 455 update_kernel_time(void)
 456 {
 457         int64_t nsec;
 458         struct timespec net_time, sys_time;
 459
 460         clock_gettime(CLOCK_REALTIME, &sys_time);
 461         rte_eth_timesync_read_time(ptp_data.current_ptp_port, &net_time);
 462
 463         nsec = (int64_t)timespec64_to_ns(&net_time) -
 464                (int64_t)timespec64_to_ns(&sys_time);
 465
 466         ptp_data.new_adj = ns_to_timeval(nsec);
 467
 468         /*
 469          * If difference between kernel time and system time in NIC is too big
 470          * (more than +/- 20 microseconds), use clock_settime to set directly
 471          * the kernel time, as adjtime is better for small adjustments (takes
 472          * longer to adjust the time).
 473          */
 474
 475         if (nsec > KERNEL_TIME_ADJUST_LIMIT || nsec < -KERNEL_TIME_ADJUST_LIMIT)
 476                 clock_settime(CLOCK_REALTIME, &net_time);
 477         else
 478                 adjtime(&ptp_data.new_adj, 0);
 479
 480
 481 }
 482
 483 /*
 484  * Parse the DELAY_RESP message.
 485  */
 486 static void
 487 parse_drsp(struct ptpv2_data_slave_ordinary *ptp_data)
 488 {
 489         struct rte_mbuf *m = ptp_data->m;
 490         struct ptp_message *ptp_msg;
 491         struct tstamp *rx_tstamp;
 492         uint16_t seq_id;
 493
 494         ptp_msg = (struct ptp_message *) (rte_pktmbuf_mtod(m, char *) +
 495                                         sizeof(struct ether_hdr));
 496         seq_id = rte_be_to_cpu_16(ptp_msg->delay_resp.hdr.seq_id);
 497         if (memcmp(&ptp_data->client_clock_id,
 498                    &ptp_msg->delay_resp.req_port_id.clock_id,
 499                    sizeof(struct clock_id)) == 0) {
 500                 if (seq_id == ptp_data->seqID_FOLLOWUP) {
 501                         rx_tstamp = &ptp_msg->delay_resp.rx_tstamp;
 502                         ptp_data->tstamp4.tv_nsec = ntohl(rx_tstamp->ns);
 503                         ptp_data->tstamp4.tv_sec =
 504                                 ((uint64_t)ntohl(rx_tstamp->sec_lsb)) |
 505                                 (((uint64_t)ntohs(rx_tstamp->sec_msb)) << 32);
 506
 507                         /* Evaluate the delta for adjustment. */
 508                         ptp_data->delta = delta_eval(ptp_data);
 509
 510                         rte_eth_timesync_adjust_time(ptp_data->portid,
 511                                                      ptp_data->delta);
 512
 513                         ptp_data->current_ptp_port = ptp_data->portid;
 514
 515                         /* Update kernel time if enabled in app parameters. */
 516                         if (ptp_data->kernel_time_set == 1)
 517                                 update_kernel_time();
 518
 519
 520
 521                 }
 522         }
 523 }
 524
 525 /* This function processes PTP packets, implementing slave PTP IEEE1588 L2
 526  * functionality.
 527  */
 528 static void
 529 parse_ptp_frames(uint16_t portid, struct rte_mbuf *m) {
 530         struct ptp_header *ptp_hdr;
 531         struct ether_hdr *eth_hdr;
 532         uint16_t eth_type;
 533
 534         eth_hdr = rte_pktmbuf_mtod(m, struct ether_hdr *);
 535         eth_type = rte_be_to_cpu_16(eth_hdr->ether_type);
 536
 537         if (eth_type == PTP_PROTOCOL) {
 538                 ptp_data.m = m;
 539                 ptp_data.portid = portid;
 540                 ptp_hdr = (struct ptp_header *)(rte_pktmbuf_mtod(m, char *)
 541                                         + sizeof(struct ether_hdr));
 542
 543                 switch (ptp_hdr->msg_type) {
 544                 case SYNC:
 545                         parse_sync(&ptp_data, m->timesync);
 546                         break;
 547                 case FOLLOW_UP:
 548                         parse_fup(&ptp_data);
 549                         break;
 550                 case DELAY_RESP:
 551                         parse_drsp(&ptp_data);
 552                         print_clock_info(&ptp_data);
 553                         break;
 554                 default:
 555                         break;
 556                 }
 557         }
 558 }
 559
 560 /*
 561  * The lcore main. This is the main thread that does the work, reading from an
 562  * input port and writing to an output port.
 563  */
 564 static __attribute__((noreturn)) void
 565 lcore_main(void)
 566 {
 567         uint16_t portid;
 568         unsigned nb_rx;
 569         struct rte_mbuf *m;
 570
 571         /*
 572          * Check that the port is on the same NUMA node as the polling thread
 573          * for best performance.
 574          */
 575         printf("\nCore %u Waiting for SYNC packets. [Ctrl+C to quit]\n",
 576                         rte_lcore_id());
 577
 578         /* Run until the application is quit or killed. */
 579
 580         while (1) {
 581                 /* Read packet from RX queues. */
 582                 for (portid = 0; portid < ptp_enabled_port_nb; portid++) {
 583
 584                         portid = ptp_enabled_ports[portid];
 585                         nb_rx = rte_eth_rx_burst(portid, 0, &m, 1);
 586
 587                         if (likely(nb_rx == 0))
 588                                 continue;
 589
 590                         if (m->ol_flags & PKT_RX_IEEE1588_PTP)
 591                                 parse_ptp_frames(portid, m);
 592
 593                         rte_pktmbuf_free(m);
 594                 }
 595         }
 596 }
 597
 598 static void
 599 print_usage(const char *prgname)
 600 {
 601         printf("%s [EAL options] -- -p PORTMASK -T VALUE\n"
 602                 " -T VALUE: 0 - Disable, 1 - Enable Linux Clock"
 603                 " Synchronization (0 default)\n"
 604                 " -p PORTMASK: hexadecimal bitmask of ports to configure\n",
 605                 prgname);
 606 }
 607
 608 static int
 609 ptp_parse_portmask(const char *portmask)
 610 {
 611         char *end = NULL;
 612         unsigned long pm;
 613
 614         /* Parse the hexadecimal string. */
 615         pm = strtoul(portmask, &end, 16);
 616
 617         if ((portmask[0] == '\0') || (end == NULL) || (*end != '\0'))
 618                 return -1;
 619
 620         if (pm == 0)
 621                 return -1;
 622
 623         return pm;
 624 }
 625
 626 static int
 627 parse_ptp_kernel(const char *param)
 628 {
 629         char *end = NULL;
 630         unsigned long pm;
 631
 632         /* Parse the hexadecimal string. */
 633         pm = strtoul(param, &end, 16);
 634
 635         if ((param[0] == '\0') || (end == NULL) || (*end != '\0'))
 636                 return -1;
 637         if (pm == 0)
 638                 return 0;
 639
 640         return 1;
 641 }
 642
 643 /* Parse the commandline arguments. */
 644 static int
 645 ptp_parse_args(int argc, char **argv)
 646 {
 647         int opt, ret;
 648         char **argvopt;
 649         int option_index;
 650         char *prgname = argv[0];
 651         static struct option lgopts[] = { {NULL, 0, 0, 0} };
 652
 653         argvopt = argv;
 654
 655         while ((opt = getopt_long(argc, argvopt, "p:T:",
 656                                   lgopts, &option_index)) != EOF) {
 657
 658                 switch (opt) {
 659
 660                 /* Portmask. */
 661                 case 'p':
 662                         ptp_enabled_port_mask = ptp_parse_portmask(optarg);
 663                         if (ptp_enabled_port_mask == 0) {
 664                                 printf("invalid portmask\n");
 665                                 print_usage(prgname);
 666                                 return -1;
 667                         }
 668                         break;
 669                 /* Time synchronization. */
 670                 case 'T':
 671                         ret = parse_ptp_kernel(optarg);
 672                         if (ret < 0) {
 673                                 print_usage(prgname);
 674                                 return -1;
 675                         }
 676
 677                         ptp_data.kernel_time_set = ret;
 678                         break;
 679
 680                 default:
 681                         print_usage(prgname);
 682                         return -1;
 683                 }
 684         }
 685
 686         argv[optind-1] = prgname;
 687
 688         optind = 1; /* Reset getopt lib. */
 689
 690         return 0;
 691 }
 692
 693 /*
 694  * The main function, which does initialization and calls the per-lcore
 695  * functions.
 696  */
 697 int
 698 main(int argc, char *argv[])
 699 {
 700         unsigned nb_ports;
 701
 702         uint16_t portid;
 703
 704         /* Initialize the Environment Abstraction Layer (EAL). */
 705         int ret = rte_eal_init(argc, argv);
 706
 707         if (ret < 0)
 708                 rte_exit(EXIT_FAILURE, "Error with EAL initialization\n");
 709
 710         memset(&ptp_data, '\0', sizeof(struct ptpv2_data_slave_ordinary));
 711
 712         argc -= ret;
 713         argv += ret;
 714
 715         ret = ptp_parse_args(argc, argv);
 716         if (ret < 0)
 717                 rte_exit(EXIT_FAILURE, "Error with PTP initialization\n");
 718
 719         /* Check that there is an even number of ports to send/receive on. */
 720         nb_ports = rte_eth_dev_count();
 721
 722         /* Creates a new mempool in memory to hold the mbufs. */
 723         mbuf_pool = rte_pktmbuf_pool_create("MBUF_POOL", NUM_MBUFS * nb_ports,
 724                 MBUF_CACHE_SIZE, 0, RTE_MBUF_DEFAULT_BUF_SIZE, rte_socket_id());
 725
 726         if (mbuf_pool == NULL)
 727                 rte_exit(EXIT_FAILURE, "Cannot create mbuf pool\n");
 728
 729         /* Initialize all ports. */
 730         for (portid = 0; portid < nb_ports; portid++) {
 731                 if ((ptp_enabled_port_mask & (1 << portid)) != 0) {
 732                         if (port_init(portid, mbuf_pool) == 0) {
 733                                 ptp_enabled_ports[ptp_enabled_port_nb] = portid;
 734                                 ptp_enabled_port_nb++;
 735                         } else {
 736                                 rte_exit(EXIT_FAILURE,
 737                                          "Cannot init port %"PRIu8 "\n",
 738                                          portid);
 739                         }
 740                 } else
 741                         printf("Skipping disabled port %u\n", portid);
 742         }
 743
 744         if (ptp_enabled_port_nb == 0) {
 745                 rte_exit(EXIT_FAILURE,
 746                         "All available ports are disabled."
 747                         " Please set portmask.\n");
 748         }
 749
 750         if (rte_lcore_count() > 1)
 751                 printf("\nWARNING: Too many lcores enabled. Only 1 used.\n");
 752
 753         /* Call lcore_main on the master core only. */
 754         lcore_main();
 755
 756         return 0;
 757 }