examples/simple_talker/simple_talker.c

   1 /******************************************************************************
   2
   3   Copyright (c) 2012, Intel Corporation
   4   All rights reserved.
   5
   6   Redistribution and use in source and binary forms, with or without
   7   modification, are permitted provided that the following conditions are met:
   8
   9    1. Redistributions of source code must retain the above copyright notice,
  10       this list of conditions and the following disclaimer.
  11
  12    2. Redistributions in binary form must reproduce the above copyright
  13       notice, this list of conditions and the following disclaimer in the
  14       documentation and/or other materials provided with the distribution.
  15
  16    3. Neither the name of the Intel Corporation nor the names of its
  17       contributors may be used to endorse or promote products derived from
  18       this software without specific prior written permission.
  19
  20   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  21   AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  22   IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  23   ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  24   LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  25   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  26   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  27   INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  28   CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  29   ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  30   POSSIBILITY OF SUCH DAMAGE.
  31
  32 ******************************************************************************/
  33
  34 #include <unistd.h>
  35 #include <fcntl.h>
  36 #include <stdlib.h>
  37 #include <stdio.h>
  38 #include <string.h>
  39 #include <syslog.h>
  40 #include <signal.h>
  41 #include <errno.h>
  42 #include <sys/ioctl.h>
  43 #include <sys/time.h>
  44 #include <sys/resource.h>
  45 #include <sys/mman.h>
  46 #include <sys/user.h>
  47 #include <pci/pci.h>
  48 #include <sys/socket.h>
  49 #include <linux/if.h>
  50 #include <netpacket/packet.h>
  51 #include <netinet/in.h>
  52 #include <arpa/inet.h>
  53 #include <net/ethernet.h>
  54 #include <sys/un.h>
  55 #include <pthread.h>
  56 #include <poll.h>
  57
  58 #include <sys/mman.h>
  59 #include <sys/stat.h>
  60 #include <fcntl.h>
  61
  62 #include "igb.h"
  63 #include "mrpd.h"
  64 #include "mrp.h"
  65 #include "msrp.h"
  66 #include <math.h>
  67 #include <endian.h>
  68 #include <stdint.h>
  69
  70 typedef struct {
  71   int64_t ml_phoffset;
  72   int64_t ls_phoffset;
  73   int32_t ml_freqoffset;
  74   int32_t ls_freqoffset;
  75   int64_t local_time;
  76 } gPtpTimeData;
  77
  78
  79 #define SHM_SIZE 4*8 + sizeof(pthread_mutex_t) /* 3 - 64 bit and 2 - 32 bits */
  80 #define SHM_NAME  "/ptp"
  81
  82 #define MAX_SAMPLE_VALUE ((1U << ((sizeof(int32_t)*8)-1))-1)
  83
  84 #define SRC_CHANNELS (2)
  85 #define SAMPLES_PER_SECOND (48000)
  86 #define FREQUENCY (480)
  87 #define SAMPLES_PER_CYCLE (SAMPLES_PER_SECOND/FREQUENCY)
  88 #define GAIN (.5)
  89
  90 //1722 header
  91 #define ETH_TYPE 0x22F0
  92
  93 #define CD_SUBTYPE 0x02         /* for simple audio format */
  94 #define SV_VER_MR_RS_GV_TV 0x81
  95 #define RS_TU   0x00
  96 #define SAMPLE_FORMAT_NON_INTR_FLOAT 0x02
  97 #define NOMINAL_SAMPLE_RATE 0x09
  98 #define LINEAR_SAMPLE_MSB 0x20
  99 unsigned char GATEWAY_INFO[] =
 100     { SAMPLE_FORMAT_NON_INTR_FLOAT, 0, NOMINAL_SAMPLE_RATE, LINEAR_SAMPLE_MSB };
 101
 102 #define SAMPLE_SIZE 4           /* 4 bytes */
 103 #define SAMPLES_PER_FRAME 6
 104 #define CHANNELS 2
 105 #define PAYLOAD_SIZE SAMPLE_SIZE*SAMPLES_PER_FRAME*CHANNELS     /* 6*4 * 2 channels  = 48 bytes */
 106
 107 #define IGB_BIND_NAMESZ 24
 108
 109 #define XMIT_DELAY (200000000)  /* us */
 110 #define RENDER_DELAY (XMIT_DELAY+2000000)       /* us */
 111 typedef enum { false = 0, true = 1 } bool;
 112 typedef struct __attribute__ ((packed)) {
 113         uint64_t subtype:7;
 114         uint64_t cd_indicator:1;
 115         uint64_t timestamp_valid:1;
 116         uint64_t gateway_valid:1;
 117         uint64_t reserved0:1;
 118         uint64_t reset:1;
 119         uint64_t version:3;
 120         uint64_t sid_valid:1;
 121         uint64_t seq_number:8;
 122         uint64_t timestamp_uncertain:1;
 123         uint64_t reserved1:7;
 124         uint64_t stream_id;
 125         uint64_t timestamp:32;
 126         uint64_t gateway_info:32;
 127         uint64_t length:16;
 128 } seventeen22_header;
 129
 130 /* 61883 CIP with SYT Field */
 131 typedef struct {
 132         uint16_t packet_channel:6;
 133         uint16_t format_tag:2;
 134         uint16_t app_control:4;
 135         uint16_t packet_tcode:4;
 136         uint16_t source_id:6;
 137         uint16_t reserved0:2;
 138         uint16_t data_block_size:8;
 139         uint16_t reserved1:2;
 140         uint16_t source_packet_header:1;
 141         uint16_t quadlet_padding_count:3;
 142         uint16_t fraction_number:2;
 143         uint16_t data_block_continuity:8;
 144         uint16_t format_id:6;
 145         uint16_t eoh:2;
 146         uint16_t format_dependent_field:8;
 147         uint16_t syt;
 148 } six1883_header;
 149
 150 typedef struct {
 151         uint8_t label;
 152         uint8_t value[3];
 153 } six1883_sample;
 154
 155 /* global variables */
 156 int control_socket = -1;
 157 device_t igb_dev;
 158 volatile int halt_tx = 0;
 159 volatile int listeners = 0;
 160 volatile int mrp_okay;
 161 volatile int mrp_error = 0;;
 162 volatile int domain_a_valid = 0;
 163 int domain_class_a_id;
 164 int domain_class_a_priority;
 165 int domain_class_a_vid;
 166 volatile int domain_b_valid = 0;
 167 int domain_class_b_id;
 168 int domain_class_b_priority;
 169 int domain_class_b_vid;
 170
 171 #define VERSION_STR     "1.0"
 172 static const char *version_str = "simple_talker v" VERSION_STR "\n"
 173     "Copyright (c) 2012, Intel Corporation\n";
 174
 175 #define MRPD_PORT_DEFAULT 7500
 176 static inline uint64_t ST_rdtsc(void)
 177 {
 178         uint64_t ret;
 179         unsigned c, d;
 180         asm volatile ("rdtsc":"=a" (c), "=d"(d));
 181         ret = d;
 182         ret <<= 32;
 183         ret |= c;
 184         return ret;
 185 }
 186
 187 static int shm_fd = -1;
 188 static char *memory_offset_buffer = NULL;
 189 int gptpinit(void)
 190 {
 191         shm_fd = shm_open(SHM_NAME, O_RDWR, 0);
 192         if (shm_fd == -1) {
 193                 perror("shm_open()");
 194                 return false;
 195         }
 196         memory_offset_buffer =
 197             (char *)mmap(NULL, SHM_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED,
 198                          shm_fd, 0);
 199         if (memory_offset_buffer == (char *)-1) {
 200                 perror("mmap()");
 201                 memory_offset_buffer = NULL;
 202                 shm_unlink(SHM_NAME);
 203                 return false;
 204         }
 205         return true;
 206 }
 207
 208 void gptpdeinit(void)
 209 {
 210         if (memory_offset_buffer != NULL) {
 211                 munmap(memory_offset_buffer, SHM_SIZE);
 212         }
 213         if (shm_fd != -1) {
 214                 close(shm_fd);
 215         }
 216 }
 217
 218 int gptpscaling(gPtpTimeData * td)
 219 {
 220         pthread_mutex_lock((pthread_mutex_t *) memory_offset_buffer);
 221         memcpy(td, memory_offset_buffer + sizeof(pthread_mutex_t), sizeof(*td));
 222         pthread_mutex_unlock((pthread_mutex_t *) memory_offset_buffer);
 223
 224         fprintf(stderr, "ml_phoffset = %lld, ls_phoffset = %lld\n",
 225                 td->ml_phoffset, td->ls_phoffset);
 226         fprintf(stderr, "ml_freqffset = %d, ls_freqoffset = %d\n",
 227                 td->ml_freqoffset, td->ls_freqoffset);
 228
 229         return true;
 230 }
 231
 232 void gensine32(int32_t * buf, unsigned count)
 233 {
 234         long double interval = (2 * ((long double)M_PI)) / count;
 235         unsigned i;
 236         for (i = 0; i < count; ++i) {
 237                 buf[i] =
 238                     (int32_t) (MAX_SAMPLE_VALUE * sinl(i * interval) * GAIN);
 239         }
 240 }
 241
 242 int get_samples(unsigned count, int32_t * buffer)
 243 {
 244         static int init = 0;
 245         static int32_t samples_onechannel[100];
 246         static unsigned index = 0;
 247
 248         if (init == 0) {
 249                 gensine32(samples_onechannel, 100);
 250                 init = 1;
 251         }
 252
 253         while (count > 0) {
 254                 int i;
 255                 for (i = 0; i < SRC_CHANNELS; ++i) {
 256                         *(buffer++) = samples_onechannel[index];
 257                 }
 258                 index = (index + 1) % 100;
 259                 --count;
 260         }
 261
 262         return 0;
 263 }
 264
 265 int mrp_join_listener(uint8_t * streamid);
 266 int send_mrp_msg(char *notify_data, int notify_len)
 267 {
 268         struct sockaddr_in addr;
 269         socklen_t addr_len;
 270         memset(&addr, 0, sizeof(addr));
 271         addr.sin_family = AF_INET;
 272         addr.sin_port = htons(MRPD_PORT_DEFAULT);
 273         inet_aton("127.0.0.1", &addr.sin_addr);
 274         addr_len = sizeof(addr);
 275         if (control_socket != -1)
 276                 return (sendto
 277                         (control_socket, notify_data, notify_len, 0,
 278                          (struct sockaddr *)&addr, addr_len));
 279
 280         else
 281                 return (0);
 282 }
 283
 284 int mrp_connect()
 285 {
 286         struct sockaddr_in addr;
 287         int sock_fd = -1;
 288         sock_fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
 289         if (sock_fd < 0)
 290                 goto out;
 291         memset(&addr, 0, sizeof(addr));
 292         addr.sin_family = AF_INET;
 293         addr.sin_port = htons(MRPD_PORT_DEFAULT);
 294         inet_aton("127.0.0.1", &addr.sin_addr);
 295         memset(&addr, 0, sizeof(addr));
 296         control_socket = sock_fd;
 297         return (0);
 298  out:   if (sock_fd != -1)
 299                 close(sock_fd);
 300         sock_fd = -1;
 301         return (-1);
 302 }
 303
 304 int mrp_disconnect()
 305 {
 306         char *msgbuf;
 307         int rc;
 308         msgbuf = malloc(64);
 309         if (NULL == msgbuf)
 310                 return -1;
 311         memset(msgbuf, 0, 64);
 312         sprintf(msgbuf, "BYE");
 313         mrp_okay = 0;
 314         rc = send_mrp_msg(msgbuf, 1500);
 315
 316         /* rc = recv_mrp_okay(); */
 317         free(msgbuf);
 318         return rc;
 319 }
 320
 321 int recv_mrp_okay()
 322 {
 323         while ((mrp_okay == 0) && (mrp_error == 0))
 324                 usleep(20000);
 325         return 0;
 326 }
 327
 328 int mrp_register_domain(int *class_id, int *priority, u_int16_t * vid)
 329 {
 330         char *msgbuf;
 331         int rc;
 332         msgbuf = malloc(64);
 333         if (NULL == msgbuf)
 334                 return -1;
 335         memset(msgbuf, 0, 64);
 336         sprintf(msgbuf, "S+D:C=%d,P=%d,V=%04x", *class_id, *priority, *vid);
 337         mrp_okay = 0;
 338         rc = send_mrp_msg(msgbuf, 1500);
 339
 340         /* rc = recv_mrp_okay(); */
 341         free(msgbuf);
 342         return rc;
 343 }
 344
 345 int mrp_get_domain(int *class_a_id, int *a_priority, u_int16_t * a_vid,
 346                    int *class_b_id, int *b_priority, u_int16_t * b_vid)
 347 {
 348         char *msgbuf;
 349
 350         /* we may not get a notification if we are joining late,
 351          * so query for what is already there ...
 352          */
 353         msgbuf = malloc(64);
 354         if (NULL == msgbuf)
 355                 return -1;
 356         memset(msgbuf, 0, 64);
 357         sprintf(msgbuf, "S??");
 358         send_mrp_msg(msgbuf, 64);
 359         free(msgbuf);
 360         while (!halt_tx && (domain_a_valid == 0) && (domain_b_valid == 0))
 361                 usleep(20000);
 362         *class_a_id = 0;
 363         *a_priority = 0;
 364         *a_vid = 0;
 365         *class_b_id = 0;
 366         *b_priority = 0;
 367         *b_vid = 0;
 368         if (domain_a_valid) {
 369                 *class_a_id = domain_class_a_id;
 370                 *a_priority = domain_class_a_priority;
 371                 *a_vid = domain_class_a_vid;
 372         }
 373         if (domain_b_valid) {
 374                 *class_b_id = domain_class_b_id;
 375                 *b_priority = domain_class_b_priority;
 376                 *b_vid = domain_class_b_vid;
 377         }
 378         return (0);
 379 }
 380 unsigned char monitor_stream_id[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 381
 382 int mrp_await_listener(unsigned char *streamid)
 383 {
 384         char *msgbuf;
 385         memcpy(monitor_stream_id, streamid, sizeof(monitor_stream_id));
 386         msgbuf = malloc(64);
 387         if (NULL == msgbuf)
 388                 return -1;
 389         memset(msgbuf, 0, 64);
 390         sprintf(msgbuf, "S??");
 391         send_mrp_msg(msgbuf, 64);
 392         free(msgbuf);
 393
 394         /* either already there ... or need to wait ... */
 395         while (!halt_tx && (listeners == 0))
 396                 usleep(20000);
 397         return (0);
 398 }
 399
 400 int process_mrp_msg(char *buf, int buflen)
 401 {
 402
 403         /*
 404          * 1st character indicates application
 405          * [MVS] - MAC, VLAN or STREAM
 406          */
 407         unsigned int id;
 408         unsigned int priority;
 409         unsigned int vid;
 410         int i, j, k;
 411         unsigned int substate;
 412         unsigned char recovered_streamid[8];
 413         k = 0;
 414  next_line:if (k >= buflen)
 415                 return (0);
 416         switch (buf[k]) {
 417         case 'E':
 418                 printf("%s from mrpd\n", buf);
 419                 fflush(stdout);
 420                 mrp_error = 1;
 421                 break;
 422         case 'O':
 423                 mrp_okay = 1;
 424                 break;
 425         case 'M':
 426         case 'V':
 427                 printf("%s unhandled from mrpd\n", buf);
 428                 fflush(stdout);
 429
 430                 /* unhandled for now */
 431                 break;
 432         case 'L':
 433
 434                 /* parse a listener attribute - see if it matches our monitor_stream_id */
 435                 i = k;
 436                 while (buf[i] != 'D')
 437                         i++;
 438                 i += 2;         /* skip the ':' */
 439                 sscanf(&(buf[i]), "%d", &substate);
 440                 while (buf[i] != 'S')
 441                         i++;
 442                 i += 2;         /* skip the ':' */
 443                 for (j = 0; j < 8; j++) {
 444                         sscanf(&(buf[i + 2 * j]), "%02x", &id);
 445                         recovered_streamid[j] = (unsigned char)id;
 446                 } printf
 447                     ("FOUND STREAM ID=%02x%02x%02x%02x%02x%02x%02x%02x ",
 448                      recovered_streamid[0], recovered_streamid[1],
 449                      recovered_streamid[2], recovered_streamid[3],
 450                      recovered_streamid[4], recovered_streamid[5],
 451                      recovered_streamid[6], recovered_streamid[7]);
 452                 switch (substate) {
 453                 case 0:
 454                         printf("with state ignore\n");
 455                         break;
 456                 case 1:
 457                         printf("with state askfailed\n");
 458                         break;
 459                 case 2:
 460                         printf("with state ready\n");
 461                         break;
 462                 case 3:
 463                         printf("with state readyfail\n");
 464                         break;
 465                 default:
 466                         printf("with state UNKNOWN (%d)\n", substate);
 467                         break;
 468                 }
 469                 if (substate > MSRP_LISTENER_ASKFAILED) {
 470                         if (memcmp
 471                             (recovered_streamid, monitor_stream_id,
 472                              sizeof(recovered_streamid)) == 0) {
 473                                 listeners = 1;
 474                                 printf("added listener\n");
 475                         }
 476                 }
 477                 fflush(stdout);
 478
 479                 /* try to find a newline ... */
 480                 while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
 481                         i++;
 482                 if (i == buflen)
 483                         return (0);
 484                 if (buf[i] == '\0')
 485                         return (0);
 486                 i++;
 487                 k = i;
 488                 goto next_line;
 489                 break;
 490         case 'D':
 491                 i = k + 4;
 492
 493                 /* save the domain attribute */
 494                 sscanf(&(buf[i]), "%d", &id);
 495                 while (buf[i] != 'P')
 496                         i++;
 497                 i += 2;         /* skip the ':' */
 498                 sscanf(&(buf[i]), "%d", &priority);
 499                 while (buf[i] != 'V')
 500                         i++;
 501                 i += 2;         /* skip the ':' */
 502                 sscanf(&(buf[i]), "%x", &vid);
 503                 if (id == 6) {
 504                         domain_class_a_id = id;
 505                         domain_class_a_priority = priority;
 506                         domain_class_a_vid = vid;
 507                         domain_a_valid = 1;
 508                 } else {
 509                         domain_class_b_id = id;
 510                         domain_class_b_priority = priority;
 511                         domain_class_b_vid = vid;
 512                         domain_b_valid = 1;
 513                 }
 514                 while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
 515                         i++;
 516                 if ((i == buflen) || (buf[i] == '\0'))
 517                         return (0);
 518                 i++;
 519                 k = i;
 520                 goto next_line;
 521                 break;
 522         case 'T':
 523
 524                 /* as simple_talker we don't care about other talkers */
 525                 i = k;
 526                 while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
 527                         i++;
 528                 if (i == buflen)
 529                         return (0);
 530                 if (buf[i] == '\0')
 531                         return (0);
 532                 i++;
 533                 k = i;
 534                 goto next_line;
 535                 break;
 536         case 'S':
 537
 538                 /* handle the leave/join events */
 539                 switch (buf[k + 4]) {
 540                 case 'L':
 541                         i = k + 5;
 542                         while (buf[i] != 'D')
 543                                 i++;
 544                         i += 2; /* skip the ':' */
 545                         sscanf(&(buf[i]), "%d", &substate);
 546                         while (buf[i] != 'S')
 547                                 i++;
 548                         i += 2; /* skip the ':' */
 549                         for (j = 0; j < 8; j++) {
 550                                 sscanf(&(buf[i + 2 * j]), "%02x", &id);
 551                                 recovered_streamid[j] = (unsigned char)id;
 552                         } printf
 553                             ("EVENT on STREAM ID=%02x%02x%02x%02x%02x%02x%02x%02x ",
 554                              recovered_streamid[0], recovered_streamid[1],
 555                              recovered_streamid[2], recovered_streamid[3],
 556                              recovered_streamid[4], recovered_streamid[5],
 557                              recovered_streamid[6], recovered_streamid[7]);
 558                         switch (substate) {
 559                         case 0:
 560                                 printf("with state ignore\n");
 561                                 break;
 562                         case 1:
 563                                 printf("with state askfailed\n");
 564                                 break;
 565                         case 2:
 566                                 printf("with state ready\n");
 567                                 break;
 568                         case 3:
 569                                 printf("with state readyfail\n");
 570                                 break;
 571                         default:
 572                                 printf("with state UNKNOWN (%d)\n", substate);
 573                                 break;
 574                         }
 575                         switch (buf[k + 1]) {
 576                         case 'L':
 577                                 printf("got a leave indication\n");
 578                                 if (memcmp
 579                                     (recovered_streamid, monitor_stream_id,
 580                                      sizeof(recovered_streamid)) == 0) {
 581                                         listeners = 0;
 582                                         printf("listener left\n");
 583                                 }
 584                                 break;
 585                         case 'J':
 586                         case 'N':
 587                                 printf("got a new/join indication\n");
 588                                 if (substate > MSRP_LISTENER_ASKFAILED) {
 589                                         if (memcmp
 590                                             (recovered_streamid,
 591                                              monitor_stream_id,
 592                                              sizeof(recovered_streamid)) == 0)
 593                                                 listeners = 1;
 594                                 }
 595                                 break;
 596                         }
 597
 598                         /* only care about listeners ... */
 599                 default:
 600                         return (0);
 601                         break;
 602                 }
 603                 break;
 604         case '\0':
 605                 break;
 606         }
 607         return (0);
 608 }
 609
 610 void *mrp_monitor_thread(void *arg)
 611 {
 612         char *msgbuf;
 613         struct sockaddr_in client_addr;
 614         struct msghdr msg;
 615         struct iovec iov;
 616         int bytes = 0;
 617         struct pollfd fds;
 618         int rc;
 619         if (NULL == arg)
 620                 rc = 0;
 621
 622         else
 623                 rc = 1;
 624         msgbuf = (char *)malloc(MAX_MRPD_CMDSZ);
 625         if (NULL == msgbuf)
 626                 return NULL;
 627         while (!halt_tx) {
 628                 fds.fd = control_socket;
 629                 fds.events = POLLIN;
 630                 fds.revents = 0;
 631                 rc = poll(&fds, 1, 100);
 632                 if (rc < 0) {
 633                         free(msgbuf);
 634                         pthread_exit(NULL);
 635                 }
 636                 if (rc == 0)
 637                         continue;
 638                 if ((fds.revents & POLLIN) == 0) {
 639                         free(msgbuf);
 640                         pthread_exit(NULL);
 641                 }
 642                 memset(&msg, 0, sizeof(msg));
 643                 memset(&client_addr, 0, sizeof(client_addr));
 644                 memset(msgbuf, 0, MAX_MRPD_CMDSZ);
 645                 iov.iov_len = MAX_MRPD_CMDSZ;
 646                 iov.iov_base = msgbuf;
 647                 msg.msg_name = &client_addr;
 648                 msg.msg_namelen = sizeof(client_addr);
 649                 msg.msg_iov = &iov;
 650                 msg.msg_iovlen = 1;
 651                 bytes = recvmsg(control_socket, &msg, 0);
 652                 if (bytes < 0)
 653                         continue;
 654                 process_mrp_msg(msgbuf, bytes);
 655         }
 656         free(msgbuf);
 657         pthread_exit(NULL);
 658 }
 659
 660 pthread_t monitor_thread;
 661 pthread_attr_t monitor_attr;
 662 int mrp_monitor()
 663 {
 664         pthread_attr_init(&monitor_attr);
 665         pthread_create(&monitor_thread, NULL, mrp_monitor_thread, NULL);
 666         return (0);
 667 }
 668
 669 int mrp_join_listener(uint8_t * streamid)
 670 {
 671         char *msgbuf;
 672         int rc;
 673         msgbuf = malloc(1500);
 674         if (NULL == msgbuf)
 675                 return -1;
 676         memset(msgbuf, 0, 1500);
 677         sprintf(msgbuf, "S+L:S=%02X%02X%02X%02X%02X%02X%02X%02X"
 678                 ",D=2", streamid[0], streamid[1], streamid[2], streamid[3],
 679                 streamid[4], streamid[5], streamid[6], streamid[7]);
 680         mrp_okay = 0;
 681         rc = send_mrp_msg(msgbuf, 1500);
 682
 683         /* rc = recv_mrp_okay(); */
 684         free(msgbuf);
 685         return rc;
 686 }
 687
 688 int
 689 mrp_advertise_stream(uint8_t * streamid,
 690                      uint8_t * destaddr,
 691                      u_int16_t vlan,
 692                      int pktsz, int interval, int priority, int latency)
 693 {
 694         char *msgbuf;
 695         int rc;
 696         msgbuf = malloc(1500);
 697         if (NULL == msgbuf)
 698                 return -1;
 699         memset(msgbuf, 0, 1500);
 700         sprintf(msgbuf, "S++:S=%02X%02X%02X%02X%02X%02X%02X%02X"
 701                 ",A=%02X%02X%02X%02X%02X%02X"
 702                 ",V=%04X"
 703                 ",Z=%d"
 704                 ",I=%d"
 705                 ",P=%d"
 706                 ",L=%d", streamid[0], streamid[1], streamid[2],
 707                 streamid[3], streamid[4], streamid[5], streamid[6],
 708                 streamid[7], destaddr[0], destaddr[1], destaddr[2],
 709                 destaddr[3], destaddr[4], destaddr[5], vlan, pktsz,
 710                 interval, priority << 5, latency);
 711         mrp_okay = 0;
 712         rc = send_mrp_msg(msgbuf, 1500);
 713
 714         /* rc = recv_mrp_okay(); */
 715         free(msgbuf);
 716         return rc;
 717 }
 718
 719 int
 720 mrp_unadvertise_stream(uint8_t * streamid,
 721                        uint8_t * destaddr,
 722                        u_int16_t vlan,
 723                        int pktsz, int interval, int priority, int latency)
 724 {
 725         char *msgbuf;
 726         int rc;
 727         msgbuf = malloc(1500);
 728         if (NULL == msgbuf)
 729                 return -1;
 730         memset(msgbuf, 0, 1500);
 731         sprintf(msgbuf, "S--:S=%02X%02X%02X%02X%02X%02X%02X%02X"
 732                 ",A=%02X%02X%02X%02X%02X%02X"
 733                 ",V=%04X"
 734                 ",Z=%d"
 735                 ",I=%d"
 736                 ",P=%d"
 737                 ",L=%d", streamid[0], streamid[1], streamid[2],
 738                 streamid[3], streamid[4], streamid[5], streamid[6],
 739                 streamid[7], destaddr[0], destaddr[1], destaddr[2],
 740                 destaddr[3], destaddr[4], destaddr[5], vlan, pktsz,
 741                 interval, priority << 5, latency);
 742         mrp_okay = 0;
 743         rc = send_mrp_msg(msgbuf, 1500);
 744
 745         /* rc = recv_mrp_okay(); */
 746         free(msgbuf);
 747         return rc;
 748 }
 749
 750 void sigint_handler(int signum)
 751 {
 752         printf("got SIGINT\n");
 753         halt_tx = signum;
 754 }
 755
 756 int pci_connect()
 757 {
 758         struct pci_access *pacc;
 759         struct pci_dev *dev;
 760         int err;
 761         char devpath[IGB_BIND_NAMESZ];
 762         memset(&igb_dev, 0, sizeof(device_t));
 763         pacc = pci_alloc();
 764         pci_init(pacc);
 765         pci_scan_bus(pacc);
 766         for (dev = pacc->devices; dev; dev = dev->next) {
 767                 pci_fill_info(dev,
 768                               PCI_FILL_IDENT | PCI_FILL_BASES | PCI_FILL_CLASS);
 769                 igb_dev.pci_vendor_id = dev->vendor_id;
 770                 igb_dev.pci_device_id = dev->device_id;
 771                 igb_dev.domain = dev->domain;
 772                 igb_dev.bus = dev->bus;
 773                 igb_dev.dev = dev->dev;
 774                 igb_dev.func = dev->func;
 775                 snprintf(devpath, IGB_BIND_NAMESZ, "%04x:%02x:%02x.%d",
 776                          dev->domain, dev->bus, dev->dev, dev->func);
 777                 err = igb_probe(&igb_dev);
 778                 if (err) {
 779                         continue;
 780                 }
 781                 printf("attaching to %s\n", devpath);
 782                 err = igb_attach(devpath, &igb_dev);
 783                 if (err) {
 784                         printf("attach failed! (%s)\n", strerror(errno));
 785                         continue;
 786                 }
 787                 goto out;
 788         }
 789         pci_cleanup(pacc);
 790         return ENXIO;
 791  out:   pci_cleanup(pacc);
 792         return 0;
 793 }
 794
 795 unsigned char STATION_ADDR[] = { 0, 0, 0, 0, 0, 0 };
 796 unsigned char STREAM_ID[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 797
 798 /* IEEE 1722 reserved address */
 799 unsigned char DEST_ADDR[] = { 0x91, 0xE0, 0xF0, 0x00, 0x0e, 0x80 };
 800
 801 int get_mac_address(char *interface)
 802 {
 803         struct ifreq if_request;
 804         int lsock;
 805         int rc;
 806         lsock = socket(PF_PACKET, SOCK_RAW, htons(0x800));
 807         if (lsock < 0)
 808                 return -1;
 809         memset(&if_request, 0, sizeof(if_request));
 810         strncpy(if_request.ifr_name, interface, sizeof(if_request.ifr_name));
 811         rc = ioctl(lsock, SIOCGIFHWADDR, &if_request);
 812         if (rc < 0) {
 813                 close(lsock);
 814                 return -1;
 815         }
 816         memcpy(STATION_ADDR, if_request.ifr_hwaddr.sa_data,
 817                sizeof(STATION_ADDR));
 818         close(lsock);
 819         return 0;
 820 }
 821
 822 static void usage(void)
 823 {
 824         fprintf(stderr, "\n"
 825                 "usage: simple_talker [-h] -i interface-name"
 826                 "\n"
 827                 "options:\n"
 828                 "    -h  show this message\n"
 829                 "    -i  specify interface for AVB connection\n"
 830                 "\n" "%s" "\n", version_str);
 831         exit(1);
 832 }
 833
 834 #define PACKET_IPG      (125000)        /* (1) packet every 125 usec */
 835
 836 int main(int argc, char *argv[])
 837 {
 838         unsigned i;
 839         int err;
 840         struct igb_dma_alloc a_page;
 841         struct igb_packet a_packet;
 842         struct igb_packet *tmp_packet;
 843         struct igb_packet *cleaned_packets;
 844         struct igb_packet *free_packets;
 845         int c;
 846         u_int64_t last_time;
 847         int rc = 0;
 848         char *interface = NULL;
 849         int class_a_id = 0;
 850         int a_priority = 0;
 851         u_int16_t a_vid = 0;
 852 #ifdef DOMAIN_QUERY
 853         int class_b_id = 0;
 854         int b_priority = 0;
 855         u_int16_t b_vid = 0;
 856 #endif
 857         int seqnum;
 858         int time_stamp;
 859         unsigned total_samples = 0;
 860         gPtpTimeData td;
 861         int32_t sample_buffer[SAMPLES_PER_FRAME * SRC_CHANNELS];
 862         seventeen22_header *header0;
 863         six1883_header *header1;
 864         six1883_sample *sample;
 865         uint64_t now_local, now_8021as;
 866         uint64_t update_8021as;
 867         unsigned delta_8021as, delta_local;
 868         long double ml_ratio;
 869
 870         for (;;) {
 871                 c = getopt(argc, argv, "hi:");
 872                 if (c < 0)
 873                         break;
 874                 switch (c) {
 875                 case 'h':
 876                         usage();
 877                         break;
 878                 case 'i':
 879                         if (interface) {
 880                                 printf
 881                                     ("only one interface per daemon is supported\n");
 882                                 usage();
 883                         }
 884                         interface = strdup(optarg);
 885                         break;
 886                 }
 887         }
 888         if (optind < argc)
 889                 usage();
 890         if (NULL == interface) {
 891                 usage();
 892         }
 893         rc = mrp_connect();
 894         if (rc) {
 895                 printf("socket creation failed\n");
 896                 return (errno);
 897         }
 898         err = pci_connect();
 899         if (err) {
 900                 printf("connect failed (%s) - are you running as root?\n",
 901                        strerror(errno));
 902                 return (errno);
 903         }
 904         err = igb_init(&igb_dev);
 905         if (err) {
 906                 printf("init failed (%s) - is the driver really loaded?\n",
 907                        strerror(errno));
 908                 return (errno);
 909         }
 910         err = igb_dma_malloc_page(&igb_dev, &a_page);
 911         if (err) {
 912                 printf("malloc failed (%s) - out of memory?\n",
 913                        strerror(errno));
 914                 return (errno);
 915         }
 916         signal(SIGINT, sigint_handler);
 917         rc = get_mac_address(interface);
 918         if (rc) {
 919                 printf("failed to open interface\n");
 920                 usage();
 921         }
 922
 923         mrp_monitor();
 924 #ifdef DOMAIN_QUERY
 925         /*
 926          * should use mrp_get_domain() above but this is a simplification
 927          */
 928 #endif
 929         domain_a_valid = 1;
 930         class_a_id = MSRP_SR_CLASS_A;
 931         a_priority = MSRP_SR_CLASS_A_PRIO;
 932         a_vid = 2;
 933         printf("detected domain Class A PRIO=%d VID=%04x...\n", a_priority,
 934                a_vid);
 935
 936 #define PKT_SZ  100
 937
 938         mrp_register_domain(&class_a_id, &a_priority, &a_vid);
 939         igb_set_class_bandwidth(&igb_dev, PACKET_IPG / 125000, 0, PKT_SZ - 22,
 940                                 0);
 941
 942         memset(STREAM_ID, 0, sizeof(STREAM_ID));
 943         memcpy(STREAM_ID, STATION_ADDR, sizeof(STATION_ADDR));
 944
 945         a_packet.dmatime = a_packet.attime = a_packet.flags = 0;
 946         a_packet.map.paddr = a_page.dma_paddr;
 947         a_packet.map.mmap_size = a_page.mmap_size;
 948         a_packet.offset = 0;
 949         a_packet.vaddr = a_page.dma_vaddr + a_packet.offset;
 950         a_packet.len = PKT_SZ;
 951         free_packets = NULL;
 952         seqnum = 0;
 953
 954         /* divide the dma page into buffers for packets */
 955         for (i = 1; i < ((a_page.mmap_size) / PKT_SZ); i++) {
 956                 tmp_packet = malloc(sizeof(struct igb_packet));
 957                 if (NULL == tmp_packet) {
 958                         printf("failed to allocate igb_packet memory!\n");
 959                         return (errno);
 960                 }
 961                 *tmp_packet = a_packet;
 962                 tmp_packet->offset = (i * PKT_SZ);
 963                 tmp_packet->vaddr += tmp_packet->offset;
 964                 tmp_packet->next = free_packets;
 965                 memset(tmp_packet->vaddr, 0, PKT_SZ);   /* MAC header at least */
 966                 memcpy(tmp_packet->vaddr, DEST_ADDR, sizeof(DEST_ADDR));
 967                 memcpy(tmp_packet->vaddr + 6, STATION_ADDR,
 968                        sizeof(STATION_ADDR));
 969
 970                 /* Q-tag */
 971                 ((char *)tmp_packet->vaddr)[12] = 0x81;
 972                 ((char *)tmp_packet->vaddr)[13] = 0x00;
 973                 ((char *)tmp_packet->vaddr)[14] =
 974                     ((a_priority << 13 | a_vid)) >> 8;
 975                 ((char *)tmp_packet->vaddr)[15] =
 976                     ((a_priority << 13 | a_vid)) & 0xFF;
 977                 ((char *)tmp_packet->vaddr)[16] = 0x22; /* 1722 eth type */
 978                 ((char *)tmp_packet->vaddr)[17] = 0xF0;
 979
 980                 /* 1722 header update + payload */
 981                 header0 =
 982                     (seventeen22_header *) (((char *)tmp_packet->vaddr) + 18);
 983                 header0->cd_indicator = 0;
 984                 header0->subtype = 0;
 985                 header0->sid_valid = 1;
 986                 header0->version = 0;
 987                 header0->reset = 0;
 988                 header0->reserved0 = 0;
 989                 header0->gateway_valid = 0;
 990                 header0->reserved1 = 0;
 991                 header0->timestamp_uncertain = 0;
 992                 memset(&(header0->stream_id), 0, sizeof(header0->stream_id));
 993                 memcpy(&(header0->stream_id), STATION_ADDR,
 994                        sizeof(STATION_ADDR));
 995                 header0->length = htons(32);
 996                 header1 = (six1883_header *) (header0 + 1);
 997                 header1->format_tag = 1;
 998                 header1->packet_channel = 0x1F;
 999                 header1->packet_tcode = 0xA;
1000                 header1->app_control = 0x0;
1001                 header1->reserved0 = 0;
1002                 header1->source_id = 0x3F;
1003                 header1->data_block_size = 1;
1004                 header1->fraction_number = 0;
1005                 header1->quadlet_padding_count = 0;
1006                 header1->source_packet_header = 0;
1007                 header1->reserved1 = 0;
1008                 header1->eoh = 0x2;
1009                 header1->format_id = 0x10;
1010                 header1->format_dependent_field = 0x02;
1011                 header1->syt = 0xFFFF;
1012                 tmp_packet->len =
1013                     18 + sizeof(seventeen22_header) + sizeof(six1883_header) +
1014                     (SAMPLES_PER_FRAME * CHANNELS * sizeof(six1883_sample));
1015                 free_packets = tmp_packet;
1016         }
1017
1018         /*
1019          * subtract 16 bytes for the MAC header/Q-tag - pktsz is limited to the
1020          * data payload of the ethernet frame .
1021          *
1022          * IPG is scaled to the Class (A) observation interval of packets per 125 usec
1023          */
1024         fprintf(stderr, "advertising stream ...\n");
1025         mrp_advertise_stream(STREAM_ID, DEST_ADDR, a_vid, PKT_SZ - 16,
1026                              PACKET_IPG / 125000, a_priority, 3900);
1027         fprintf(stderr, "awaiting a listener ...\n");
1028         mrp_await_listener(STREAM_ID);
1029         printf("got a listener ...\n");
1030         halt_tx = 0;
1031
1032         gptpinit();
1033         gptpscaling(&td);
1034
1035         if( igb_get_wallclock( &igb_dev, &now_local, NULL ) != 0 ) {
1036           fprintf( stderr, "Failed to get wallclock time\n" );
1037           return -1;
1038         }
1039         update_8021as = td.local_time - td.ml_phoffset;
1040         delta_local = (unsigned)(now_local - td.local_time);
1041         ml_ratio = -1 * (((long double)td.ml_freqoffset) / 1000000000000) + 1;
1042         delta_8021as = (unsigned)(ml_ratio * delta_local);
1043         now_8021as = update_8021as + delta_8021as;
1044
1045         last_time = now_local + XMIT_DELAY;
1046         time_stamp = now_8021as + RENDER_DELAY;
1047
1048         rc = nice(-20);
1049
1050         while (listeners && !halt_tx) {
1051                 tmp_packet = free_packets;
1052                 if (NULL == tmp_packet)
1053                         goto cleanup;
1054                 header0 =
1055                     (seventeen22_header *) (((char *)tmp_packet->vaddr) + 18);
1056                 header1 = (six1883_header *) (header0 + 1);
1057                 free_packets = tmp_packet->next;
1058
1059                 /* unfortuntely unless this thread is at rtprio
1060                  * you get pre-empted between fetching the time
1061                  * and programming the packet and get a late packet
1062                  */
1063                 tmp_packet->attime = last_time + PACKET_IPG;
1064                 last_time += PACKET_IPG;
1065
1066                 get_samples(SAMPLES_PER_FRAME, sample_buffer);
1067                 header0->seq_number = seqnum++;
1068                 if (seqnum % 4 == 0)
1069                         header0->timestamp_valid = 0;
1070
1071                 else
1072                         header0->timestamp_valid = 1;
1073
1074                 time_stamp = htonl(time_stamp);
1075                 header0->timestamp = time_stamp;
1076                 time_stamp = ntohl(time_stamp);
1077                 time_stamp += PACKET_IPG;
1078                 header1->data_block_continuity = total_samples;
1079                 total_samples += SAMPLES_PER_FRAME*CHANNELS;
1080                 sample =
1081                     (six1883_sample *) (((char *)tmp_packet->vaddr) +
1082                                         (18 + sizeof(seventeen22_header) +
1083                                          sizeof(six1883_header)));
1084
1085                 for (i = 0; i < SAMPLES_PER_FRAME * CHANNELS; ++i) {
1086                         uint32_t tmp = htonl(sample_buffer[i]);
1087                         sample[i].label = 0x40;
1088                         memcpy(&(sample[i].value), &(tmp),
1089                                sizeof(sample[i].value));
1090                 }
1091
1092                 err = igb_xmit(&igb_dev, 0, tmp_packet);
1093
1094                 if (!err) {
1095                         continue;
1096                 }
1097
1098                 if (ENOSPC == err) {
1099
1100                         /* put back for now */
1101                         tmp_packet->next = free_packets;
1102                         free_packets = tmp_packet;
1103                 }
1104
1105  cleanup:       igb_clean(&igb_dev, &cleaned_packets);
1106                 i = 0;
1107                 while (cleaned_packets) {
1108                         i++;
1109                         tmp_packet = cleaned_packets;
1110                         cleaned_packets = cleaned_packets->next;
1111                         tmp_packet->next = free_packets;
1112                         free_packets = tmp_packet;
1113                 }
1114         }
1115         rc = nice(0);
1116
1117         if (halt_tx == 0)
1118                 printf("listener left ...\n");
1119         halt_tx = 1;
1120
1121         mrp_unadvertise_stream(STREAM_ID, DEST_ADDR, a_vid, PKT_SZ - 16,
1122                                PACKET_IPG / 125000, a_priority, 3900);
1123
1124         igb_set_class_bandwidth(&igb_dev, 0, 0, 0, 0);  /* disable Qav */
1125
1126         rc = mrp_disconnect();
1127
1128         igb_dma_free_page(&igb_dev, &a_page);
1129
1130         err = igb_detach(&igb_dev);
1131
1132         pthread_exit(NULL);
1133
1134         return (0);
1135 }