PageRenderTime 897ms CodeModel.GetById 161ms app.highlight 475ms RepoModel.GetById 249ms app.codeStats 1ms

/examples/jackd-talker/jackd_talker.c

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