PageRenderTime 190ms CodeModel.GetById 20ms app.highlight 123ms RepoModel.GetById 23ms app.codeStats 1ms

/examples/simple_talker/simple_talker.c

https://github.com/masche842/Open-AVB
C | 1473 lines | 1252 code | 145 blank | 76 comment | 199 complexity | d8eaa69c51d621d693d20935b7aa137c 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 <netinet/ip.h>
  53#include <arpa/inet.h>
  54#include <net/ethernet.h>
  55#include <sys/un.h>
  56#include <pthread.h>
  57#include <poll.h>
  58
  59#include <sys/mman.h>
  60#include <sys/stat.h>
  61#include <fcntl.h>
  62
  63#include "igb.h"
  64#include "mrpd.h"
  65#include "mrp.h"
  66#include "msrp.h"
  67#include <math.h>
  68#include <endian.h>
  69#include <stdint.h>
  70
  71typedef struct { 
  72  int64_t ml_phoffset;
  73  int64_t ls_phoffset;
  74  long double ml_freqoffset;
  75  long double ls_freqoffset;
  76  uint64_t local_time;
  77} gPtpTimeData;
  78
  79
  80#define SHM_SIZE 4*8 + sizeof(pthread_mutex_t) /* 3 - 64 bit and 2 - 32 bits */
  81#define SHM_NAME  "/ptp"
  82
  83#define MAX_SAMPLE_VALUE ((1U << ((sizeof(int32_t)*8)-1))-1)
  84
  85#define SRC_CHANNELS (2)
  86#define SAMPLES_PER_SECOND (48000)
  87#define FREQUENCY (480)
  88#define SAMPLES_PER_CYCLE (SAMPLES_PER_SECOND/FREQUENCY)
  89#define GAIN (.5)
  90
  91#define L16_PAYLOAD_TYPE 96 /* Should be negotiated via RTSP */
  92#define ID_B_HDR_EXT_ID 0 /* Should be negotiated via RTSP */
  93
  94#define CD_SUBTYPE 0x02		/* for simple audio format */
  95#define SV_VER_MR_RS_GV_TV 0x81
  96#define RS_TU	0x00
  97#define SAMPLE_FORMAT_NON_INTR_FLOAT 0x02
  98#define NOMINAL_SAMPLE_RATE 0x09
  99#define LINEAR_SAMPLE_MSB 0x20
 100unsigned char GATEWAY_INFO[] =
 101    { SAMPLE_FORMAT_NON_INTR_FLOAT, 0, NOMINAL_SAMPLE_RATE, LINEAR_SAMPLE_MSB };
 102
 103#define SAMPLE_SIZE 4		/* 4 bytes */
 104#define L2_SAMPLES_PER_FRAME 6
 105#define L4_SAMPLES_PER_FRAME 60
 106#define L4_SAMPLE_SIZE 2
 107#define CHANNELS 2
 108#define PAYLOAD_SIZE SAMPLE_SIZE*SAMPLES_PER_FRAME*CHANNELS	/* 6*4 * 2 channels  = 48 bytes */
 109
 110#define RTP_SUBNS_SCALE_NUM 20000000
 111#define RTP_SUBNS_SCALE_DEN  4656613
 112
 113#define IGB_BIND_NAMESZ 24
 114
 115#define XMIT_DELAY (200000000)	/* us */
 116#define RENDER_DELAY (XMIT_DELAY+2000000)	/* us */
 117
 118typedef enum { false = 0, true = 1 } bool;
 119
 120typedef struct __attribute__ ((packed)) {
 121	uint64_t subtype:7;
 122	uint64_t cd_indicator:1;
 123	uint64_t timestamp_valid:1;
 124	uint64_t gateway_valid:1;
 125	uint64_t reserved0:1;
 126	uint64_t reset:1;
 127	uint64_t version:3;
 128	uint64_t sid_valid:1;
 129	uint64_t seq_number:8;
 130	uint64_t timestamp_uncertain:1;
 131	uint64_t reserved1:7;
 132	uint64_t stream_id;
 133	uint64_t timestamp:32;
 134	uint64_t gateway_info:32;
 135	uint64_t length:16;
 136} seventeen22_header;
 137
 138/* 61883 CIP with SYT Field */
 139typedef struct __attribute__ ((packed)) {
 140	uint16_t packet_channel:6;
 141	uint16_t format_tag:2;
 142	uint16_t app_control:4;
 143	uint16_t packet_tcode:4;
 144	uint16_t source_id:6;
 145	uint16_t reserved0:2;
 146	uint16_t data_block_size:8;
 147	uint16_t reserved1:2;
 148	uint16_t source_packet_header:1;
 149	uint16_t quadlet_padding_count:3;
 150	uint16_t fraction_number:2;
 151	uint16_t data_block_continuity:8;
 152	uint16_t format_id:6;
 153	uint16_t eoh:2;
 154	uint16_t format_dependent_field:8;
 155	uint16_t syt;
 156} six1883_header;
 157
 158typedef struct __attribute__ ((packed)) {
 159	uint8_t label;
 160	uint8_t value[3];
 161} six1883_sample;
 162
 163typedef struct __attribute__ ((packed)) {
 164	uint8_t version_length;
 165	uint8_t DSCP_ECN;
 166	uint16_t ip_length;
 167	uint16_t id;
 168	uint16_t fragmentation;
 169	uint8_t ttl;
 170	uint8_t protocol;
 171	uint16_t hdr_cksum;
 172	uint32_t src;
 173	uint32_t dest;
 174
 175	uint16_t source_port;
 176	uint16_t dest_port;
 177	uint16_t udp_length;
 178	uint16_t cksum;
 179
 180	uint8_t version_cc;
 181	uint8_t mark_payload;
 182	uint16_t sequence;
 183	uint32_t timestamp;
 184	uint32_t ssrc;
 185	
 186	uint8_t tag[2];
 187	uint16_t total_length;
 188	uint8_t tag_length;
 189	uint8_t seconds[3];
 190	uint32_t nanoseconds;
 191} IP_RTP_Header;
 192
 193typedef struct __attribute__ ((packed)) {
 194	uint32_t source;
 195	uint32_t dest;
 196	uint8_t zero;
 197	uint8_t protocol;
 198	uint16_t length;
 199} IP_PseudoHeader;
 200
 201/* global variables */
 202int control_socket = -1;
 203device_t igb_dev;
 204volatile int halt_tx = 0;
 205volatile int listeners = 0;
 206volatile int mrp_okay;
 207volatile int mrp_error = 0;;
 208volatile int domain_a_valid = 0;
 209int domain_class_a_id;
 210int domain_class_a_priority;
 211int domain_class_a_vid;
 212volatile int domain_b_valid = 0;
 213int domain_class_b_id;
 214int domain_class_b_priority;
 215int domain_class_b_vid;
 216
 217#define VERSION_STR	"1.0"
 218static const char *version_str = "simple_talker v" VERSION_STR "\n"
 219    "Copyright (c) 2012, Intel Corporation\n";
 220
 221#define MRPD_PORT_DEFAULT 7500
 222
 223uint16_t inet_checksum(uint8_t *ip, int len){
 224    uint32_t sum = 0;  /* assume 32 bit long, 16 bit short */
 225	
 226	while(len > 1){
 227		sum += *(( uint16_t *) ip); ip += 2;
 228		if(sum & 0x80000000)   /* if high order bit set, fold */
 229			sum = (sum & 0xFFFF) + (sum >> 16);
 230		len -= 2;
 231	}
 232	
 233	if(len)       /* take care of left over byte */
 234		sum += (uint16_t) *(uint8_t *)ip;
 235	
 236	while(sum>>16)
 237		sum = (sum & 0xFFFF) + (sum >> 16);
 238	
 239	return ~sum;
 240}
 241
 242#if 0
 243 else {
 244			if(sum & 0x80000000)   /* if high order bit set, fold */
 245				sum = (sum & 0xFFFF) + (sum >> 16);
 246			sum += *(( uint16_t *) buf_iov->iov_base); buf_iov->iov_base += 2;
 247			buf_iov->iov_len -= 2;
 248		}
 249#endif
 250
 251uint16_t inet_checksum_sg( struct iovec *buf_iov, size_t buf_iovlen ){
 252	size_t i;
 253    uint32_t sum = 0;  /* assume 32 bit long, 16 bit short */
 254	uint8_t residual;
 255	int has_residual = 0;
 256
 257	for( i = 0; i < buf_iovlen; ++i,++buf_iov ) {
 258		if( has_residual ) {
 259			if( buf_iov->iov_len > 0 ) {
 260				if(sum & 0x80000000)   /* if high order bit set, fold */
 261					sum = (sum & 0xFFFF) + (sum >> 16);
 262				sum += residual | (*(( uint8_t *) buf_iov->iov_base) << 8);
 263				buf_iov->iov_base += 1;
 264				buf_iov->iov_len -= 1;
 265			} else {
 266				if(sum & 0x80000000)   /* if high order bit set, fold */
 267					sum = (sum & 0xFFFF) + (sum >> 16);
 268				sum += (uint16_t) residual;
 269			}
 270			has_residual = 0;
 271
 272		}
 273		while(buf_iov->iov_len > 1){
 274			if(sum & 0x80000000)   /* if high order bit set, fold */
 275				sum = (sum & 0xFFFF) + (sum >> 16);
 276			sum += *(( uint16_t *) buf_iov->iov_base); buf_iov->iov_base += 2;
 277			buf_iov->iov_len -= 2;
 278		}
 279		if( buf_iov->iov_len ) {
 280			residual = *(( uint8_t *) buf_iov->iov_base);
 281			has_residual = 1;
 282		}
 283	}
 284	if( has_residual ) {
 285		sum += (uint16_t) residual;
 286	}
 287
 288	while(sum>>16)
 289		sum = (sum & 0xFFFF) + (sum >> 16);
 290	
 291	return ~sum;
 292}
 293
 294static inline uint64_t ST_rdtsc(void)
 295{
 296	uint64_t ret;
 297	unsigned c, d;
 298	asm volatile ("rdtsc":"=a" (c), "=d"(d));
 299	ret = d;
 300	ret <<= 32;
 301	ret |= c;
 302	return ret;
 303}
 304
 305static int shm_fd = -1;
 306static char *memory_offset_buffer = NULL;
 307int gptpinit(void)
 308{
 309	shm_fd = shm_open(SHM_NAME, O_RDWR, 0);
 310	if (shm_fd == -1) {
 311		perror("shm_open()");
 312		return false;
 313	}
 314	memory_offset_buffer =
 315	    (char *)mmap(NULL, SHM_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED,
 316			 shm_fd, 0);
 317	if (memory_offset_buffer == (char *)-1) {
 318		perror("mmap()");
 319		memory_offset_buffer = NULL;
 320		shm_unlink(SHM_NAME);
 321		return false;
 322	}
 323	return true;
 324}
 325
 326void gptpdeinit(void)
 327{
 328	if (memory_offset_buffer != NULL) {
 329		munmap(memory_offset_buffer, SHM_SIZE);
 330	}
 331	if (shm_fd != -1) {
 332		close(shm_fd);
 333	}
 334}
 335
 336int gptpscaling(gPtpTimeData * td)
 337{
 338	pthread_mutex_lock((pthread_mutex_t *) memory_offset_buffer);
 339	memcpy(td, memory_offset_buffer + sizeof(pthread_mutex_t), sizeof(*td));
 340	pthread_mutex_unlock((pthread_mutex_t *) memory_offset_buffer);
 341
 342	fprintf(stderr, "ml_phoffset = %lld, ls_phoffset = %lld\n",
 343		td->ml_phoffset, td->ls_phoffset);
 344	fprintf(stderr, "ml_freqffset = %Lf, ls_freqoffset = %Lf\n",
 345		td->ml_freqoffset, td->ls_freqoffset);
 346
 347	return true;
 348}
 349
 350void gensine32(int32_t * buf, unsigned count)
 351{
 352	long double interval = (2 * ((long double)M_PI)) / count;
 353	unsigned i;
 354	for (i = 0; i < count; ++i) {
 355		buf[i] =
 356		    (int32_t) (MAX_SAMPLE_VALUE * sinl(i * interval) * GAIN);
 357	}
 358}
 359
 360int get_samples(unsigned count, int32_t * buffer)
 361{
 362	static int init = 0;
 363	static int32_t samples_onechannel[100];
 364	static unsigned index = 0;
 365
 366	if (init == 0) {
 367		gensine32(samples_onechannel, 100);
 368		init = 1;
 369	}
 370
 371	while (count > 0) {
 372		int i;
 373		for (i = 0; i < SRC_CHANNELS; ++i) {
 374			*(buffer++) = samples_onechannel[index];
 375		}
 376		index = (index + 1) % 100;
 377		--count;
 378	}
 379
 380	return 0;
 381}
 382
 383int mrp_join_listener(uint8_t * streamid);
 384int send_mrp_msg(char *notify_data, int notify_len)
 385{
 386	struct sockaddr_in addr;
 387	socklen_t addr_len;
 388	memset(&addr, 0, sizeof(addr));
 389	addr.sin_family = AF_INET;
 390	addr.sin_port = htons(MRPD_PORT_DEFAULT);
 391	inet_aton("127.0.0.1", &addr.sin_addr);
 392	addr_len = sizeof(addr);
 393	if (control_socket != -1)
 394		return (sendto
 395			(control_socket, notify_data, notify_len, 0,
 396			 (struct sockaddr *)&addr, addr_len));
 397
 398	else
 399		return (0);
 400}
 401
 402int mrp_connect()
 403{
 404	struct sockaddr_in addr;
 405	int sock_fd = -1;
 406	sock_fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
 407	if (sock_fd < 0)
 408		goto out;
 409	memset(&addr, 0, sizeof(addr));
 410	addr.sin_family = AF_INET;
 411	addr.sin_port = htons(MRPD_PORT_DEFAULT);
 412	inet_aton("127.0.0.1", &addr.sin_addr);
 413	memset(&addr, 0, sizeof(addr));
 414	control_socket = sock_fd;
 415	return (0);
 416 out:	if (sock_fd != -1)
 417		close(sock_fd);
 418	sock_fd = -1;
 419	return (-1);
 420}
 421
 422int mrp_disconnect()
 423{
 424	char *msgbuf;
 425	int rc;
 426	msgbuf = malloc(64);
 427	if (NULL == msgbuf)
 428		return -1;
 429	memset(msgbuf, 0, 64);
 430	sprintf(msgbuf, "BYE");
 431	mrp_okay = 0;
 432	rc = send_mrp_msg(msgbuf, 1500);
 433
 434	/* rc = recv_mrp_okay(); */
 435	free(msgbuf);
 436	return rc;
 437}
 438
 439int recv_mrp_okay()
 440{
 441	while ((mrp_okay == 0) && (mrp_error == 0))
 442		usleep(20000);
 443	return 0;
 444}
 445
 446int mrp_register_domain(int *class_id, int *priority, u_int16_t * vid)
 447{
 448	char *msgbuf;
 449	int rc;
 450	msgbuf = malloc(64);
 451	if (NULL == msgbuf)
 452		return -1;
 453	memset(msgbuf, 0, 64);
 454	sprintf(msgbuf, "S+D:C=%d,P=%d,V=%04x", *class_id, *priority, *vid);
 455	mrp_okay = 0;
 456	rc = send_mrp_msg(msgbuf, 1500);
 457
 458	/* rc = recv_mrp_okay(); */
 459	free(msgbuf);
 460	return rc;
 461}
 462
 463int mrp_get_domain(int *class_a_id, int *a_priority, u_int16_t * a_vid,
 464		   int *class_b_id, int *b_priority, u_int16_t * b_vid)
 465{
 466	char *msgbuf;
 467
 468	/* we may not get a notification if we are joining late,
 469	 * so query for what is already there ...
 470	 */
 471	msgbuf = malloc(64);
 472	if (NULL == msgbuf)
 473		return -1;
 474	memset(msgbuf, 0, 64);
 475	sprintf(msgbuf, "S??");
 476	send_mrp_msg(msgbuf, 64);
 477	free(msgbuf);
 478	while (!halt_tx && (domain_a_valid == 0) && (domain_b_valid == 0))
 479		usleep(20000);
 480	*class_a_id = 0;
 481	*a_priority = 0;
 482	*a_vid = 0;
 483	*class_b_id = 0;
 484	*b_priority = 0;
 485	*b_vid = 0;
 486	if (domain_a_valid) {
 487		*class_a_id = domain_class_a_id;
 488		*a_priority = domain_class_a_priority;
 489		*a_vid = domain_class_a_vid;
 490	}
 491	if (domain_b_valid) {
 492		*class_b_id = domain_class_b_id;
 493		*b_priority = domain_class_b_priority;
 494		*b_vid = domain_class_b_vid;
 495	}
 496	return (0);
 497}
 498unsigned char monitor_stream_id[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 499
 500int mrp_await_listener(unsigned char *streamid)
 501{
 502	char *msgbuf;
 503	memcpy(monitor_stream_id, streamid, sizeof(monitor_stream_id));
 504	msgbuf = malloc(64);
 505	if (NULL == msgbuf)
 506		return -1;
 507	memset(msgbuf, 0, 64);
 508	sprintf(msgbuf, "S??");
 509	send_mrp_msg(msgbuf, 64);
 510	free(msgbuf);
 511
 512	/* either already there ... or need to wait ... */
 513	while (!halt_tx && (listeners == 0))
 514		usleep(20000);
 515	return (0);
 516}
 517
 518int process_mrp_msg(char *buf, int buflen)
 519{
 520
 521	/*
 522	 * 1st character indicates application
 523	 * [MVS] - MAC, VLAN or STREAM
 524	 */
 525	unsigned int id;
 526	unsigned int priority;
 527	unsigned int vid;
 528	int i, j, k;
 529	unsigned int substate;
 530	unsigned char recovered_streamid[8];
 531	k = 0;
 532 next_line:if (k >= buflen)
 533		return (0);
 534	switch (buf[k]) {
 535	case 'E':
 536		printf("%s from mrpd\n", buf);
 537		fflush(stdout);
 538		mrp_error = 1;
 539		break;
 540	case 'O':
 541		mrp_okay = 1;
 542		break;
 543	case 'M':
 544	case 'V':
 545		printf("%s unhandled from mrpd\n", buf);
 546		fflush(stdout);
 547
 548		/* unhandled for now */
 549		break;
 550	case 'L':
 551
 552		/* parse a listener attribute - see if it matches our monitor_stream_id */
 553		i = k;
 554		while (buf[i] != 'D')
 555			i++;
 556		i += 2;		/* skip the ':' */
 557		sscanf(&(buf[i]), "%d", &substate);
 558		while (buf[i] != 'S')
 559			i++;
 560		i += 2;		/* skip the ':' */
 561		for (j = 0; j < 8; j++) {
 562			sscanf(&(buf[i + 2 * j]), "%02x", &id);
 563			recovered_streamid[j] = (unsigned char)id;
 564		} printf
 565		    ("FOUND STREAM ID=%02x%02x%02x%02x%02x%02x%02x%02x ",
 566		     recovered_streamid[0], recovered_streamid[1],
 567		     recovered_streamid[2], recovered_streamid[3],
 568		     recovered_streamid[4], recovered_streamid[5],
 569		     recovered_streamid[6], recovered_streamid[7]);
 570		switch (substate) {
 571		case 0:
 572			printf("with state ignore\n");
 573			break;
 574		case 1:
 575			printf("with state askfailed\n");
 576			break;
 577		case 2:
 578			printf("with state ready\n");
 579			break;
 580		case 3:
 581			printf("with state readyfail\n");
 582			break;
 583		default:
 584			printf("with state UNKNOWN (%d)\n", substate);
 585			break;
 586		}
 587		if (substate > MSRP_LISTENER_ASKFAILED) {
 588			if (memcmp
 589			    (recovered_streamid, monitor_stream_id,
 590			     sizeof(recovered_streamid)) == 0) {
 591				listeners = 1;
 592				printf("added listener\n");
 593			}
 594		}
 595		fflush(stdout);
 596
 597		/* try to find a newline ... */
 598		while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
 599			i++;
 600		if (i == buflen)
 601			return (0);
 602		if (buf[i] == '\0')
 603			return (0);
 604		i++;
 605		k = i;
 606		goto next_line;
 607		break;
 608	case 'D':
 609		i = k + 4;
 610
 611		/* save the domain attribute */
 612		sscanf(&(buf[i]), "%d", &id);
 613		while (buf[i] != 'P')
 614			i++;
 615		i += 2;		/* skip the ':' */
 616		sscanf(&(buf[i]), "%d", &priority);
 617		while (buf[i] != 'V')
 618			i++;
 619		i += 2;		/* skip the ':' */
 620		sscanf(&(buf[i]), "%x", &vid);
 621		if (id == 6) {
 622			domain_class_a_id = id;
 623			domain_class_a_priority = priority;
 624			domain_class_a_vid = vid;
 625			domain_a_valid = 1;
 626		} else {
 627			domain_class_b_id = id;
 628			domain_class_b_priority = priority;
 629			domain_class_b_vid = vid;
 630			domain_b_valid = 1;
 631		}
 632		while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
 633			i++;
 634		if ((i == buflen) || (buf[i] == '\0'))
 635			return (0);
 636		i++;
 637		k = i;
 638		goto next_line;
 639		break;
 640	case 'T':
 641
 642		/* as simple_talker we don't care about other talkers */
 643		i = k;
 644		while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
 645			i++;
 646		if (i == buflen)
 647			return (0);
 648		if (buf[i] == '\0')
 649			return (0);
 650		i++;
 651		k = i;
 652		goto next_line;
 653		break;
 654	case 'S':
 655
 656		/* handle the leave/join events */
 657		switch (buf[k + 4]) {
 658		case 'L':
 659			i = k + 5;
 660			while (buf[i] != 'D')
 661				i++;
 662			i += 2;	/* skip the ':' */
 663			sscanf(&(buf[i]), "%d", &substate);
 664			while (buf[i] != 'S')
 665				i++;
 666			i += 2;	/* skip the ':' */
 667			for (j = 0; j < 8; j++) {
 668				sscanf(&(buf[i + 2 * j]), "%02x", &id);
 669				recovered_streamid[j] = (unsigned char)id;
 670			} printf
 671			    ("EVENT on STREAM ID=%02x%02x%02x%02x%02x%02x%02x%02x ",
 672			     recovered_streamid[0], recovered_streamid[1],
 673			     recovered_streamid[2], recovered_streamid[3],
 674			     recovered_streamid[4], recovered_streamid[5],
 675			     recovered_streamid[6], recovered_streamid[7]);
 676			switch (substate) {
 677			case 0:
 678				printf("with state ignore\n");
 679				break;
 680			case 1:
 681				printf("with state askfailed\n");
 682				break;
 683			case 2:
 684				printf("with state ready\n");
 685				break;
 686			case 3:
 687				printf("with state readyfail\n");
 688				break;
 689			default:
 690				printf("with state UNKNOWN (%d)\n", substate);
 691				break;
 692			}
 693			switch (buf[k + 1]) {
 694			case 'L':
 695				printf("got a leave indication\n");
 696				if (memcmp
 697				    (recovered_streamid, monitor_stream_id,
 698				     sizeof(recovered_streamid)) == 0) {
 699					listeners = 0;
 700					printf("listener left\n");
 701				}
 702				break;
 703			case 'J':
 704			case 'N':
 705				printf("got a new/join indication\n");
 706				if (substate > MSRP_LISTENER_ASKFAILED) {
 707					if (memcmp
 708					    (recovered_streamid,
 709					     monitor_stream_id,
 710					     sizeof(recovered_streamid)) == 0)
 711						listeners = 1;
 712				}
 713				break;
 714			}
 715
 716			/* only care about listeners ... */
 717		default:
 718			return (0);
 719			break;
 720		}
 721		break;
 722	case '\0':
 723		break;
 724	}
 725	return (0);
 726}
 727
 728void *mrp_monitor_thread(void *arg)
 729{
 730	char *msgbuf;
 731	struct sockaddr_in client_addr;
 732	struct msghdr msg;
 733	struct iovec iov;
 734	int bytes = 0;
 735	struct pollfd fds;
 736	int rc;
 737	if (NULL == arg)
 738		rc = 0;
 739
 740	else
 741		rc = 1;
 742	msgbuf = (char *)malloc(MAX_MRPD_CMDSZ);
 743	if (NULL == msgbuf)
 744		return NULL;
 745	while (!halt_tx) {
 746		fds.fd = control_socket;
 747		fds.events = POLLIN;
 748		fds.revents = 0;
 749		rc = poll(&fds, 1, 100);
 750		if (rc < 0) {
 751			free(msgbuf);
 752			pthread_exit(NULL);
 753		}
 754		if (rc == 0)
 755			continue;
 756		if ((fds.revents & POLLIN) == 0) {
 757			free(msgbuf);
 758			pthread_exit(NULL);
 759		}
 760		memset(&msg, 0, sizeof(msg));
 761		memset(&client_addr, 0, sizeof(client_addr));
 762		memset(msgbuf, 0, MAX_MRPD_CMDSZ);
 763		iov.iov_len = MAX_MRPD_CMDSZ;
 764		iov.iov_base = msgbuf;
 765		msg.msg_name = &client_addr;
 766		msg.msg_namelen = sizeof(client_addr);
 767		msg.msg_iov = &iov;
 768		msg.msg_iovlen = 1;
 769		bytes = recvmsg(control_socket, &msg, 0);
 770		if (bytes < 0)
 771			continue;
 772		process_mrp_msg(msgbuf, bytes);
 773	}
 774	free(msgbuf);
 775	pthread_exit(NULL);
 776}
 777
 778pthread_t monitor_thread;
 779pthread_attr_t monitor_attr;
 780int mrp_monitor()
 781{
 782	pthread_attr_init(&monitor_attr);
 783	pthread_create(&monitor_thread, NULL, mrp_monitor_thread, NULL);
 784	return (0);
 785}
 786
 787int mrp_join_vlan()
 788{
 789	char *msgbuf;
 790	int rc;
 791	msgbuf = malloc(1500);
 792	if (NULL == msgbuf)
 793		return -1;
 794	memset(msgbuf, 0, 1500);
 795	sprintf(msgbuf, "V++:I=0002");
 796	rc = send_mrp_msg(msgbuf, 1500);
 797
 798	free(msgbuf);
 799	return rc;
 800}
 801
 802int mrp_join_listener(uint8_t * streamid)
 803{
 804	char *msgbuf;
 805	int rc;
 806	msgbuf = malloc(1500);
 807	if (NULL == msgbuf)
 808		return -1;
 809	memset(msgbuf, 0, 1500);
 810	sprintf(msgbuf, "S+L:S=%02X%02X%02X%02X%02X%02X%02X%02X"
 811		",D=2", streamid[0], streamid[1], streamid[2], streamid[3],
 812		streamid[4], streamid[5], streamid[6], streamid[7]);
 813	mrp_okay = 0;
 814	rc = send_mrp_msg(msgbuf, 1500);
 815
 816	/* rc = recv_mrp_okay(); */
 817	free(msgbuf);
 818	return rc;
 819}
 820
 821int
 822mrp_advertise_stream(uint8_t * streamid,
 823		     uint8_t * destaddr,
 824		     u_int16_t vlan,
 825		     int pktsz, int interval, int priority, int latency)
 826{
 827	char *msgbuf;
 828	int rc;
 829	msgbuf = malloc(1500);
 830	if (NULL == msgbuf)
 831		return -1;
 832	memset(msgbuf, 0, 1500);
 833	sprintf(msgbuf, "S++:S=%02X%02X%02X%02X%02X%02X%02X%02X"
 834		",A=%02X%02X%02X%02X%02X%02X"
 835		",V=%04X"
 836		",Z=%d"
 837		",I=%d"
 838		",P=%d"
 839		",L=%d", streamid[0], streamid[1], streamid[2],
 840		streamid[3], streamid[4], streamid[5], streamid[6],
 841		streamid[7], destaddr[0], destaddr[1], destaddr[2],
 842		destaddr[3], destaddr[4], destaddr[5], vlan, pktsz,
 843		interval, priority << 5, latency);
 844	mrp_okay = 0;
 845	rc = send_mrp_msg(msgbuf, 1500);
 846
 847	/* rc = recv_mrp_okay(); */
 848	free(msgbuf);
 849	return rc;
 850}
 851
 852int
 853mrp_unadvertise_stream(uint8_t * streamid,
 854		       uint8_t * destaddr,
 855		       u_int16_t vlan,
 856		       int pktsz, int interval, int priority, int latency)
 857{
 858	char *msgbuf;
 859	int rc;
 860	msgbuf = malloc(1500);
 861	if (NULL == msgbuf)
 862		return -1;
 863	memset(msgbuf, 0, 1500);
 864	sprintf(msgbuf, "S--:S=%02X%02X%02X%02X%02X%02X%02X%02X"
 865		",A=%02X%02X%02X%02X%02X%02X"
 866		",V=%04X"
 867		",Z=%d"
 868		",I=%d"
 869		",P=%d"
 870		",L=%d", streamid[0], streamid[1], streamid[2],
 871		streamid[3], streamid[4], streamid[5], streamid[6],
 872		streamid[7], destaddr[0], destaddr[1], destaddr[2],
 873		destaddr[3], destaddr[4], destaddr[5], vlan, pktsz,
 874		interval, priority << 5, latency);
 875	mrp_okay = 0;
 876	rc = send_mrp_msg(msgbuf, 1500);
 877
 878	/* rc = recv_mrp_okay(); */
 879	free(msgbuf);
 880	return rc;
 881}
 882
 883void sigint_handler(int signum)
 884{
 885	printf("got SIGINT\n");
 886	halt_tx = signum;
 887}
 888
 889int pci_connect()
 890{
 891	struct pci_access *pacc;
 892	struct pci_dev *dev;
 893	int err;
 894	char devpath[IGB_BIND_NAMESZ];
 895	memset(&igb_dev, 0, sizeof(device_t));
 896	pacc = pci_alloc();
 897	pci_init(pacc);
 898	pci_scan_bus(pacc);
 899	for (dev = pacc->devices; dev; dev = dev->next) {
 900		pci_fill_info(dev,
 901			      PCI_FILL_IDENT | PCI_FILL_BASES | PCI_FILL_CLASS);
 902		igb_dev.pci_vendor_id = dev->vendor_id;
 903		igb_dev.pci_device_id = dev->device_id;
 904		igb_dev.domain = dev->domain;
 905		igb_dev.bus = dev->bus;
 906		igb_dev.dev = dev->dev;
 907		igb_dev.func = dev->func;
 908		snprintf(devpath, IGB_BIND_NAMESZ, "%04x:%02x:%02x.%d",
 909			 dev->domain, dev->bus, dev->dev, dev->func);
 910		err = igb_probe(&igb_dev);
 911		if (err) {
 912			continue;
 913		}
 914		printf("attaching to %s\n", devpath);
 915		err = igb_attach(devpath, &igb_dev);
 916		if (err) {
 917			printf("attach failed! (%s)\n", strerror(errno));
 918			continue;
 919		}
 920		goto out;
 921	}
 922	pci_cleanup(pacc);
 923	return ENXIO;
 924 out:	pci_cleanup(pacc);
 925	return 0;
 926}
 927
 928 unsigned char STATION_ADDR[] = { 0, 0, 0, 0, 0, 0 };
 929 unsigned char STREAM_ID[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 930
 931/* IEEE 1722 reserved address */
 932 unsigned char L2_DEST_ADDR[] = { 0x91, 0xE0, 0xF0, 0x00, 0x0e, 0x80 };
 933 unsigned char L3_DEST_ADDR[] = { 224, 0, 0, 115 };
 934uint16_t L3_PORT = 5004;
 935
 936 void l3_to_l2_multicast( unsigned char *l2, unsigned char *l3 ) {
 937	 l2[0]  = 0x1;
 938	 l2[1]  = 0x0;
 939	 l2[2]  = 0x5e;
 940	 l2[3]  = l3[1] & 0x7F;
 941	 l2[4]  = l3[2];
 942	 l2[5]  = l3[3];
 943 }
 944
 945int get_mac_address(char *interface)
 946{
 947	struct ifreq if_request;
 948	int lsock;
 949	int rc;
 950	lsock = socket(PF_PACKET, SOCK_RAW, htons(0x800));
 951	if (lsock < 0)
 952		return -1;
 953	memset(&if_request, 0, sizeof(if_request));
 954	strncpy(if_request.ifr_name, interface, sizeof(if_request.ifr_name));
 955	rc = ioctl(lsock, SIOCGIFHWADDR, &if_request);
 956	if (rc < 0) {
 957		close(lsock);
 958		return -1;
 959	}
 960	memcpy(STATION_ADDR, if_request.ifr_hwaddr.sa_data,
 961	       sizeof(STATION_ADDR));
 962	close(lsock);
 963	return 0;
 964}
 965
 966static void usage(void)
 967{
 968	fprintf(stderr, "\n"
 969		"usage: simple_talker [-h] -i interface-name"
 970		"\n"
 971		"options:\n"
 972		"    -h  show this message\n"
 973		"    -i  specify interface for AVB connection\n"
 974		"    -t  transport equal to 2 for 1722 or 3 for RTP\n"
 975		"\n" "%s" "\n", version_str);
 976	exit(1);
 977}
 978
 979#define L2_PACKET_IPG	(125000)	/* (1) packet every 125 usec */
 980#define L4_PACKET_IPG	(1250000)	/* (1) packet every 1.25 millisec */
 981
 982int main(int argc, char *argv[])
 983{
 984	unsigned i;
 985	int err;
 986	struct igb_dma_alloc a_page;
 987	struct igb_packet a_packet;
 988	struct igb_packet *tmp_packet;
 989	struct igb_packet *cleaned_packets;
 990	struct igb_packet *free_packets;
 991	int c;
 992	u_int64_t last_time;
 993	int rc = 0;
 994	char *interface = NULL;
 995	int transport = -1;
 996	int class_a_id = 0;
 997	int a_priority = 0;
 998	u_int16_t a_vid = 0;
 999#ifdef DOMAIN_QUERY
1000	int class_b_id = 0;
1001	int b_priority = 0;
1002	u_int16_t b_vid = 0;
1003#endif
1004	uint16_t seqnum;
1005	uint32_t rtp_timestamp;
1006	uint64_t time_stamp;
1007	unsigned total_samples = 0;
1008	gPtpTimeData td;
1009	int32_t sample_buffer[L4_SAMPLES_PER_FRAME * SRC_CHANNELS];
1010
1011	seventeen22_header *l2_header0;
1012	six1883_header *l2_header1;
1013	six1883_sample *sample;
1014
1015	IP_RTP_Header *l4_headers;
1016	IP_PseudoHeader pseudo_hdr;
1017	unsigned l4_local_address = 0;
1018	int sd;
1019	struct sockaddr_in local;
1020	struct ifreq if_request;
1021
1022	uint64_t now_local, now_8021as;
1023	uint64_t update_8021as;
1024	unsigned delta_8021as, delta_local;
1025	uint8_t dest_addr[6];
1026	size_t packet_size;
1027
1028	for (;;) {
1029		c = getopt(argc, argv, "hi:t:");
1030		if (c < 0)
1031			break;
1032		switch (c) {
1033		case 'h':
1034			usage();
1035			break;
1036		case 'i':
1037			if (interface) {
1038				printf
1039				    ("only one interface per daemon is supported\n");
1040				usage();
1041			}
1042			interface = strdup(optarg);
1043			break;
1044		case 't':
1045			transport = strtoul( optarg, NULL, 10 );
1046		}
1047	}
1048	if (optind < argc)
1049		usage();
1050	if (NULL == interface) {
1051		usage();
1052	}
1053	if( transport != 2 && transport != 4 ) {
1054		fprintf( stderr, "Must specify valid transport\n" );
1055		usage();
1056	}
1057	rc = mrp_connect();
1058	if (rc) {
1059		printf("socket creation failed\n");
1060		return (errno);
1061	}
1062	err = pci_connect();
1063	if (err) {
1064		printf("connect failed (%s) - are you running as root?\n",
1065		       strerror(errno));
1066		return (errno);
1067	}
1068	err = igb_init(&igb_dev);
1069	if (err) {
1070		printf("init failed (%s) - is the driver really loaded?\n",
1071		       strerror(errno));
1072		return (errno);
1073	}
1074	err = igb_dma_malloc_page(&igb_dev, &a_page);
1075	if (err) {
1076		printf("malloc failed (%s) - out of memory?\n",
1077		       strerror(errno));
1078		return (errno);
1079	}
1080	signal(SIGINT, sigint_handler);
1081	rc = get_mac_address(interface);
1082	if (rc) {
1083		printf("failed to open interface\n");
1084		usage();
1085	}
1086	if( transport == 2 ) {
1087		memcpy( dest_addr, L2_DEST_ADDR, sizeof(dest_addr));
1088	} else {
1089		memset( &local, 0, sizeof( local ));
1090		local.sin_family = PF_INET;
1091		local.sin_addr.s_addr = htonl( INADDR_ANY );
1092		local.sin_port = htons( L3_PORT );
1093		l3_to_l2_multicast( dest_addr, L3_DEST_ADDR );
1094		memset( &if_request, 0, sizeof( if_request ));
1095		strncpy(if_request.ifr_name, interface, sizeof(if_request.ifr_name)-1);
1096		sd = socket( AF_INET, SOCK_DGRAM, 0 );
1097		if( sd == -1 ) {
1098			printf( "Failed to open socket: %s\n", strerror( errno ));
1099			return errno;
1100		}
1101		if( bind( sd, (struct sockaddr *) &local, sizeof( local )) != 0 ) {
1102			printf( "Failed to bind on socket: %s\n", strerror( errno ));
1103			return errno;
1104		}
1105		if( ioctl( sd, SIOCGIFADDR, &if_request ) != 0 ) {
1106			printf
1107				( "Failed to get interface address (ioctl) on socket: %s\n",
1108				  strerror( errno ));
1109			return errno;
1110		}
1111		memcpy
1112			( &l4_local_address,
1113			  &(( struct sockaddr_in *)&if_request.ifr_addr)->sin_addr,
1114			  sizeof( l4_local_address ));
1115		
1116	}
1117
1118	mrp_monitor();
1119#ifdef DOMAIN_QUERY
1120	/* 
1121	 * should use mrp_get_domain() above but this is a simplification 
1122	 */
1123#endif
1124	domain_a_valid = 1;
1125	class_a_id = MSRP_SR_CLASS_A;
1126	a_priority = MSRP_SR_CLASS_A_PRIO;
1127	a_vid = 2;
1128	printf("detected domain Class A PRIO=%d VID=%04x...\n", a_priority,
1129	       a_vid);
1130
1131#define PKT_SZ	100
1132
1133	mrp_register_domain(&class_a_id, &a_priority, &a_vid);
1134	mrp_join_vlan();
1135
1136	if( transport == 2 ) {
1137		igb_set_class_bandwidth
1138			(&igb_dev, 125000/L2_PACKET_IPG, 0, PKT_SZ - 22, 0);
1139	} else {
1140		igb_set_class_bandwidth
1141			(&igb_dev, 1, 0,
1142			 sizeof(*l4_headers)+L4_SAMPLES_PER_FRAME*CHANNELS*2, 0);
1143	}
1144
1145	memset(STREAM_ID, 0, sizeof(STREAM_ID));
1146	memcpy(STREAM_ID, STATION_ADDR, sizeof(STATION_ADDR));
1147
1148	if( transport == 2 ) {
1149		packet_size = PKT_SZ;
1150	} else {
1151		packet_size = 18 + sizeof(*l4_headers) +
1152				(L4_SAMPLES_PER_FRAME * CHANNELS * L4_SAMPLE_SIZE );
1153 	}
1154
1155	a_packet.dmatime = a_packet.attime = a_packet.flags = 0;
1156	a_packet.map.paddr = a_page.dma_paddr;
1157	a_packet.map.mmap_size = a_page.mmap_size;
1158	a_packet.offset = 0;
1159	a_packet.vaddr = a_page.dma_vaddr + a_packet.offset;
1160	a_packet.len = packet_size;
1161	free_packets = NULL;
1162	seqnum = 0;
1163	rtp_timestamp = 0; /* Should be random start */
1164
1165	/* divide the dma page into buffers for packets */
1166	for (i = 1; i < ((a_page.mmap_size) / packet_size); i++) {
1167		tmp_packet = malloc(sizeof(struct igb_packet));
1168		if (NULL == tmp_packet) {
1169			printf("failed to allocate igb_packet memory!\n");
1170			return (errno);
1171		}
1172		*tmp_packet = a_packet;
1173		tmp_packet->offset = (i * packet_size);
1174		tmp_packet->vaddr += tmp_packet->offset;
1175		tmp_packet->next = free_packets;
1176		memset(tmp_packet->vaddr, 0, packet_size);	/* MAC header at least */
1177		memcpy(tmp_packet->vaddr, dest_addr, sizeof(dest_addr));
1178		memcpy(tmp_packet->vaddr + 6, STATION_ADDR,
1179		       sizeof(STATION_ADDR));
1180
1181		/* Q-tag */
1182		((char *)tmp_packet->vaddr)[12] = 0x81;
1183		((char *)tmp_packet->vaddr)[13] = 0x00;
1184		((char *)tmp_packet->vaddr)[14] =
1185		    ((a_priority << 13 | a_vid)) >> 8;
1186		((char *)tmp_packet->vaddr)[15] =
1187		    ((a_priority << 13 | a_vid)) & 0xFF;
1188		if( transport == 2 ) {
1189			((char *)tmp_packet->vaddr)[16] = 0x22;	/* 1722 eth type */
1190			((char *)tmp_packet->vaddr)[17] = 0xF0;
1191		} else {
1192			((char *)tmp_packet->vaddr)[16] = 0x08;	/* IP eth type */
1193			((char *)tmp_packet->vaddr)[17] = 0x00;
1194		}
1195
1196		if( transport == 2 ) {
1197			/* 1722 header update + payload */
1198			l2_header0 =
1199				(seventeen22_header *) (((char *)tmp_packet->vaddr) + 18);
1200			l2_header0->cd_indicator = 0;
1201			l2_header0->subtype = 0;
1202			l2_header0->sid_valid = 1;
1203			l2_header0->version = 0;
1204			l2_header0->reset = 0;
1205			l2_header0->reserved0 = 0;
1206			l2_header0->gateway_valid = 0;
1207			l2_header0->reserved1 = 0;
1208			l2_header0->timestamp_uncertain = 0;
1209			memset(&(l2_header0->stream_id), 0, sizeof(l2_header0->stream_id));
1210			memcpy(&(l2_header0->stream_id), STATION_ADDR,
1211				   sizeof(STATION_ADDR));
1212			l2_header0->length = htons(32);
1213			l2_header1 = (six1883_header *) (l2_header0 + 1);
1214			l2_header1->format_tag = 1;
1215			l2_header1->packet_channel = 0x1F;
1216			l2_header1->packet_tcode = 0xA;
1217			l2_header1->app_control = 0x0;
1218			l2_header1->reserved0 = 0;
1219			l2_header1->source_id = 0x3F;
1220			l2_header1->data_block_size = 1;
1221			l2_header1->fraction_number = 0;
1222			l2_header1->quadlet_padding_count = 0;
1223			l2_header1->source_packet_header = 0;
1224			l2_header1->reserved1 = 0;
1225			l2_header1->eoh = 0x2;
1226			l2_header1->format_id = 0x10;
1227			l2_header1->format_dependent_field = 0x02;
1228			l2_header1->syt = 0xFFFF;
1229			tmp_packet->len =
1230				18 + sizeof(seventeen22_header) + sizeof(six1883_header) +
1231				(L2_SAMPLES_PER_FRAME * CHANNELS * sizeof(six1883_sample));
1232		} else {
1233			pseudo_hdr.source = l4_local_address;
1234			memcpy
1235				( &pseudo_hdr.dest, L3_DEST_ADDR, sizeof( pseudo_hdr.dest ));
1236			pseudo_hdr.zero = 0;
1237			pseudo_hdr.protocol = 0x11;
1238			pseudo_hdr.length = htons(packet_size-18-20);
1239	
1240			l4_headers =
1241				(IP_RTP_Header *) (((char *)tmp_packet->vaddr) + 18);
1242			l4_headers->version_length = 0x45;
1243			l4_headers->DSCP_ECN = 0x20;
1244			l4_headers->ip_length = htons(packet_size-18);
1245			l4_headers->id = 0;
1246			l4_headers->fragmentation = 0;
1247			l4_headers->ttl = 64;
1248			l4_headers->protocol = 0x11;
1249			l4_headers->hdr_cksum = 0;
1250			l4_headers->src = l4_local_address;
1251			memcpy
1252				( &l4_headers->dest, L3_DEST_ADDR, sizeof( l4_headers->dest ));
1253			{
1254				struct iovec iv0;
1255				iv0.iov_base = l4_headers;
1256				iv0.iov_len = 20;
1257				l4_headers->hdr_cksum =
1258					inet_checksum_sg( &iv0, 1 );
1259			}
1260
1261			l4_headers->source_port = htons( L3_PORT );
1262			l4_headers->dest_port = htons( L3_PORT );;
1263			l4_headers->udp_length = htons(packet_size-18-20);
1264
1265			l4_headers->version_cc = 2;
1266			l4_headers->mark_payload = L16_PAYLOAD_TYPE;
1267			l4_headers->sequence = 0;
1268			l4_headers->timestamp = 0;
1269			l4_headers->ssrc = 0;
1270			
1271			l4_headers->tag[0] = 0xBE;
1272			l4_headers->tag[1] = 0xDE;
1273			l4_headers->total_length = htons(2);
1274			l4_headers->tag_length = (6 << 4) | ID_B_HDR_EXT_ID;
1275
1276			tmp_packet->len =
1277				18 + sizeof(*l4_headers) +
1278				(L4_SAMPLES_PER_FRAME * CHANNELS * L4_SAMPLE_SIZE );
1279
1280		}
1281		free_packets = tmp_packet;
1282	}
1283
1284	/* 
1285	 * subtract 16 bytes for the MAC header/Q-tag - pktsz is limited to the 
1286	 * data payload of the ethernet frame .
1287	 *
1288	 * IPG is scaled to the Class (A) observation interval of packets per 125 usec
1289	 */
1290	fprintf(stderr, "advertising stream ...\n");
1291	if( transport == 2 ) {
1292		mrp_advertise_stream
1293			(STREAM_ID, dest_addr, a_vid, PKT_SZ - 16, L2_PACKET_IPG / 125000, 
1294			 a_priority, 3900);
1295	} else {
1296		/* 1 is the wrong number for frame rate, but fractional values not
1297		   allowed, not sure the significance of the value 6, but using it
1298		   consistently */
1299		mrp_advertise_stream
1300			(STREAM_ID, dest_addr, a_vid,
1301			 sizeof(*l4_headers)+L4_SAMPLES_PER_FRAME*CHANNELS*2 + 6, 1, 
1302			 a_priority, 3900);
1303	}
1304	fprintf(stderr, "awaiting a listener ...\n");
1305	//mrp_await_listener(STREAM_ID);
1306	listeners = 1;
1307	printf("got a listener ...\n");
1308	halt_tx = 0;
1309
1310	gptpinit();
1311	gptpscaling(&td);
1312
1313	if( igb_get_wallclock( &igb_dev, &now_local, NULL ) != 0 ) {
1314	  fprintf( stderr, "Failed to get wallclock time\n" );
1315	  return -1;
1316	}
1317	update_8021as = td.local_time - td.ml_phoffset;
1318	delta_local = (unsigned)(now_local - td.local_time);
1319	delta_8021as = (unsigned)(td.ml_freqoffset * delta_local);
1320	now_8021as = update_8021as + delta_8021as;
1321
1322	last_time = now_local + XMIT_DELAY;
1323	time_stamp = now_8021as + RENDER_DELAY;
1324
1325	rc = nice(-20);
1326
1327	while (listeners && !halt_tx) {
1328		tmp_packet = free_packets;
1329		if (NULL == tmp_packet)
1330			goto cleanup;
1331		
1332		free_packets = tmp_packet->next;
1333		
1334		if( transport == 2 ) {
1335			uint32_t timestamp_l;
1336			get_samples( L2_SAMPLES_PER_FRAME, sample_buffer );
1337			l2_header0 =
1338				(seventeen22_header *) (((char *)tmp_packet->vaddr) + 18);
1339			l2_header1 = (six1883_header *) (l2_header0 + 1);
1340
1341			/* unfortuntely unless this thread is at rtprio
1342			 * you get pre-empted between fetching the time
1343			 * and programming the packet and get a late packet
1344			 */
1345			tmp_packet->attime = last_time + L2_PACKET_IPG;
1346			last_time += L2_PACKET_IPG;
1347
1348			l2_header0->seq_number = seqnum++;
1349			if (seqnum % 4 == 0)
1350				l2_header0->timestamp_valid = 0;
1351
1352			else
1353				l2_header0->timestamp_valid = 1;
1354
1355			timestamp_l = time_stamp;
1356			l2_header0->timestamp = htonl(timestamp_l);
1357			time_stamp += L2_PACKET_IPG;
1358			l2_header1->data_block_continuity = total_samples;
1359			total_samples += L2_SAMPLES_PER_FRAME*CHANNELS;
1360			sample =
1361				(six1883_sample *) (((char *)tmp_packet->vaddr) +
1362									(18 + sizeof(seventeen22_header) +
1363									 sizeof(six1883_header)));
1364
1365			for (i = 0; i < L2_SAMPLES_PER_FRAME * CHANNELS; ++i) {
1366				uint32_t tmp = htonl(sample_buffer[i]);
1367				sample[i].label = 0x40;
1368				memcpy(&(sample[i].value), &(tmp),
1369					   sizeof(sample[i].value));
1370			}
1371		} else {
1372			uint16_t *l16_sample;
1373			uint8_t *tmp;
1374			get_samples( L4_SAMPLES_PER_FRAME, sample_buffer );
1375
1376			l4_headers =
1377				(IP_RTP_Header *) (((char *)tmp_packet->vaddr) + 18);
1378
1379			l4_headers->sequence =  seqnum++;
1380			l4_headers->timestamp = rtp_timestamp;
1381
1382			tmp_packet->attime = last_time + L4_PACKET_IPG;
1383			last_time += L4_PACKET_IPG;
1384
1385			l4_headers->nanoseconds = time_stamp/1000000000;
1386			tmp = (uint8_t *) &l4_headers->nanoseconds;
1387			l4_headers->seconds[0] = tmp[2];
1388			l4_headers->seconds[1] = tmp[1];
1389			l4_headers->seconds[2] = tmp[0];
1390			{
1391				uint64_t tmp;
1392				tmp  = time_stamp % 1000000000;
1393				tmp *= RTP_SUBNS_SCALE_NUM;
1394				tmp /= RTP_SUBNS_SCALE_DEN;
1395				l4_headers->nanoseconds = (uint32_t) tmp;
1396			}
1397			l4_headers->nanoseconds = htons(l4_headers->nanoseconds);
1398
1399
1400			time_stamp += L4_PACKET_IPG;
1401
1402			l16_sample = (uint16_t *) (l4_headers+1);
1403
1404			for (i = 0; i < L4_SAMPLES_PER_FRAME * CHANNELS; ++i) {
1405				uint16_t tmp = sample_buffer[i]/65536;
1406				l16_sample[i] = htons(tmp);
1407			}
1408			l4_headers->cksum = 0;
1409			{
1410				struct iovec iv[2];
1411				iv[0].iov_base = &pseudo_hdr;
1412				iv[0].iov_len = sizeof(pseudo_hdr);
1413				iv[1].iov_base = ((uint8_t *)l4_headers) + 20;
1414				iv[1].iov_len = packet_size-18-20;
1415				l4_headers->cksum =
1416					inet_checksum_sg( iv, 2 );
1417			}
1418		}
1419
1420		err = igb_xmit(&igb_dev, 0, tmp_packet);
1421
1422		if (!err) {
1423			continue;
1424		}
1425		
1426		if (ENOSPC == err) {
1427			
1428			/* put back for now */
1429			tmp_packet->next = free_packets;
1430			free_packets = tmp_packet;
1431		}
1432		
1433	cleanup:	
1434		igb_clean(&igb_dev, &cleaned_packets);
1435		i = 0;
1436		while (cleaned_packets) {
1437			i++;
1438			tmp_packet = cleaned_packets;
1439			cleaned_packets = cleaned_packets->next;
1440			tmp_packet->next = free_packets;
1441			free_packets = tmp_packet;
1442		}
1443	}
1444	rc = nice(0);
1445	
1446	if (halt_tx == 0)
1447		printf("listener left ...\n");
1448	halt_tx = 1;
1449	
1450	if( transport == 2 ) {
1451		mrp_unadvertise_stream
1452			(STREAM_ID, dest_addr, a_vid, PKT_SZ - 16, L2_PACKET_IPG / 125000,
1453			 a_priority, 3900);
1454	} else {
1455		mrp_unadvertise_stream
1456			(STREAM_ID, dest_addr, a_vid,
1457			 sizeof(*l4_headers)+L4_SAMPLES_PER_FRAME*CHANNELS*2 + 6, 1,
1458			 a_priority, 3900);
1459	}
1460	
1461	igb_set_class_bandwidth(&igb_dev, 0, 0, 0, 0);	/* disable Qav */
1462	
1463	rc = mrp_disconnect();
1464	
1465	igb_dma_free_page(&igb_dev, &a_page);
1466	
1467	err = igb_detach(&igb_dev);
1468	
1469	pthread_exit(NULL);
1470	
1471	return (0);
1472}
1473