PageRenderTime 40ms CodeModel.GetById 2ms app.highlight 31ms RepoModel.GetById 1ms app.codeStats 1ms

/examples/live_stream/talker.c

https://github.com/masche842/Open-AVB
C | 889 lines | 719 code | 114 blank | 56 comment | 126 complexity | ba4acb07bb99de6f647c7ac919362494 MD5 | raw file
  1 /*
  2  * Copyright (c) <2013>, Intel Corporation.
  3  *
  4  * This program is free software; you can redistribute it and/or modify it
  5  * under the terms and conditions of the GNU Lesser General Public License,
  6  * version 2.1, as published by the Free Software Foundation.
  7  *
  8  * This program is distributed in the hope it will be useful, but WITHOUT
  9  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 10  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for
 11  * more details.
 12  *
 13  * You should have received a copy of the GNU Lesser General Public License along with
 14  * this program; if not, write to the Free Software Foundation, Inc.,
 15  * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
 16  *
 17  */
 18
 19#include <errno.h>
 20#include <fcntl.h>
 21#include <math.h>
 22#include <poll.h>
 23#include <pthread.h>
 24#include <signal.h>
 25#include <stdio.h>
 26#include <stdlib.h>
 27#include <string.h>
 28#include <syslog.h>
 29#include <unistd.h>
 30#include <sched.h>
 31
 32#include <arpa/inet.h>
 33
 34#include <linux/if.h>
 35
 36#include <netinet/in.h>
 37#include <net/ethernet.h>
 38#include <netpacket/packet.h>
 39
 40#include <pci/pci.h>
 41
 42#include <sys/ioctl.h>
 43#include <sys/mman.h>
 44#include <sys/resource.h>
 45#include <sys/socket.h>
 46#include <sys/stat.h>
 47#include <sys/time.h>
 48#include <sys/queue.h>
 49#include <sys/un.h>
 50#include <sys/user.h>
 51
 52#include "avb.h"
 53#include "mrpd.h"
 54#include "mrp.h"
 55#include "msrp.h"
 56
 57
 58/* global variables */
 59int control_socket = -1;
 60volatile int halt_tx = 0;
 61volatile int listeners = 0;
 62volatile int mrp_okay;
 63volatile int mrp_error = 0;;
 64volatile int domain_a_valid = 0;
 65volatile int domain_b_valid = 0;
 66int domain_class_b_id;
 67int domain_class_b_priority;
 68int domain_class_b_vid;
 69int g_start_feed_socket = 0;
 70device_t igb_dev;
 71pthread_t monitor_thread;
 72pthread_attr_t monitor_attr;
 73uint32_t payload_length;
 74
 75unsigned char monitor_stream_id[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 76unsigned char STATION_ADDR[] = { 0, 0, 0, 0, 0, 0 };
 77unsigned char STREAM_ID[] = { 0, 0, 0, 0, 0, 0, 0, 0 };
 78/* IEEE 1722 reserved address */
 79unsigned char DEST_ADDR[] = { 0x91, 0xE0, 0xF0, 0x00, 0x0E, 0x80 };
 80
 81#define STREAMID	0xABCDEF
 82
 83/* (1) packet every 125 usec */
 84#define PACKET_IPG		125000
 85
 86#define USE_MRPD 1
 87
 88#ifdef USE_MRPD
 89
 90int domain_class_a_id;
 91int domain_class_a_priority;
 92int domain_class_a_vid;
 93
 94int send_mrp_msg(char *notify_data, int notify_len)
 95{
 96	struct sockaddr_in addr;
 97	socklen_t addr_len;
 98
 99	memset(&addr, 0, sizeof(addr));
100	addr.sin_family = AF_INET;
101	addr.sin_port = htons(MRPD_PORT_DEFAULT);
102	inet_aton("127.0.0.1", &addr.sin_addr);
103	addr_len = sizeof(addr);
104	if (control_socket != -1)
105		return (sendto
106			(control_socket, notify_data, notify_len, 0,
107			 (struct sockaddr *)&addr, addr_len));
108
109	return 0;
110}
111
112int mrp_connect()
113{
114	struct sockaddr_in addr;
115	int sock_fd = -1;
116
117	sock_fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
118	if (sock_fd < 0)
119		goto out;
120
121	memset(&addr, 0, sizeof(addr));
122	addr.sin_family = AF_INET;
123	addr.sin_port = htons(MRPD_PORT_DEFAULT);
124	inet_aton("127.0.0.1", &addr.sin_addr);
125	memset(&addr, 0, sizeof(addr));
126	control_socket = sock_fd;
127	return 0;
128
129 out:	if (sock_fd != -1)
130		close(sock_fd);
131	sock_fd = -1;
132	return -1;
133}
134
135int mrp_disconnect()
136{
137	char *msgbuf;
138	int rc;
139
140	msgbuf = malloc(64);
141	if (NULL == msgbuf)
142		return -1;
143
144	memset(msgbuf, 0, 64);
145	sprintf(msgbuf, "BYE");
146	mrp_okay = 0;
147	rc = send_mrp_msg(msgbuf, 1500);
148
149	free(msgbuf);
150	return rc;
151}
152
153int recv_mrp_okay()
154{
155	while ((mrp_okay == 0) && (mrp_error == 0))
156		usleep(20000);
157
158	return 0;
159}
160
161int mrp_register_domain(int *class_id, int *priority, u_int16_t * vid)
162{
163	char *msgbuf;
164	int rc;
165
166	msgbuf = malloc(64);
167	if (NULL == msgbuf)
168		return -1;
169
170	memset(msgbuf, 0, 64);
171	sprintf(msgbuf, "S+D:C=%d,P=%d,V=%04x", *class_id, *priority, *vid);
172	mrp_okay = 0;
173	rc = send_mrp_msg(msgbuf, 1500);
174
175	free(msgbuf);
176	return rc;
177}
178
179int mrp_get_domain(int *class_a_id, int *a_priority, u_int16_t * a_vid,
180		   int *class_b_id, int *b_priority, u_int16_t * b_vid)
181{
182	char *msgbuf;
183
184	/* we may not get a notification if we are joining late,
185	 * so query for what is already there ...
186	 */
187	msgbuf = malloc(64);
188	if (NULL == msgbuf)
189		return -1;
190
191	memset(msgbuf, 0, 64);
192	sprintf(msgbuf, "S??");
193	send_mrp_msg(msgbuf, 64);
194	free(msgbuf);
195
196	while (!halt_tx && (domain_a_valid == 0) && (domain_b_valid == 0))
197		usleep(20000);
198
199	*class_a_id = 0;
200	*a_priority = 0;
201	*a_vid = 0;
202	*class_b_id = 0;
203	*b_priority = 0;
204	*b_vid = 0;
205	if (domain_a_valid) {
206		*class_a_id = domain_class_a_id;
207		*a_priority = domain_class_a_priority;
208		*a_vid = domain_class_a_vid;
209	}
210	if (domain_b_valid) {
211		*class_b_id = domain_class_b_id;
212		*b_priority = domain_class_b_priority;
213		*b_vid = domain_class_b_vid;
214	}
215
216	return 0;
217}
218
219int mrp_await_listener(unsigned char *streamid)
220{
221	char *msgbuf;
222
223	memcpy(monitor_stream_id, streamid, sizeof(monitor_stream_id));
224	msgbuf = malloc(64);
225	if (NULL == msgbuf)
226		return -1;
227
228	memset(msgbuf, 0, 64);
229	sprintf(msgbuf, "S??");
230	send_mrp_msg(msgbuf, 64);
231	free(msgbuf);
232
233	/* either already there ... or need to wait ... */
234	while (!halt_tx && (listeners == 0))
235		usleep(20000);
236
237	return 0;
238}
239
240int process_mrp_msg(char *buf, int buflen)
241{
242
243	/*
244	 * 1st character indicates application
245	 * [MVS] - MAC, VLAN or STREAM
246	 */
247	unsigned int id;
248	unsigned int priority;
249	unsigned int vid;
250	int i, j, k;
251	unsigned int substate;
252	unsigned char recovered_streamid[8];
253
254	k = 0;
255next_line:if (k >= buflen)
256		return 0;
257	switch (buf[k]) {
258	case 'E':
259		fprintf(stderr, "%s from mrpd\n", buf);
260		fflush(stdout);
261		mrp_error = 1;
262		break;
263	case 'O':
264		mrp_okay = 1;
265		break;
266	case 'M':
267	case 'V':
268		fprintf(stderr, "%s unhandled from mrpd\n", buf);
269		fflush(stdout);
270
271		/* unhandled for now */
272		break;
273	case 'L':
274
275		/* parse a listener attribute - see if it matches our monitor_stream_id */
276		i = k;
277		while (buf[i] != 'D')
278			i++;
279		i += 2;		/* skip the ':' */
280		sscanf(&(buf[i]), "%d", &substate);
281		while (buf[i] != 'S')
282			i++;
283		i += 2;		/* skip the ':' */
284		for (j = 0; j < 8; j++) {
285			sscanf(&(buf[i + 2 * j]), "%02x", &id);
286			recovered_streamid[j] = (unsigned char)id;
287		} printf
288		    ("FOUND STREAM ID=%02x%02x%02x%02x%02x%02x%02x%02x ",
289		     recovered_streamid[0], recovered_streamid[1],
290		     recovered_streamid[2], recovered_streamid[3],
291		     recovered_streamid[4], recovered_streamid[5],
292		     recovered_streamid[6], recovered_streamid[7]);
293		switch (substate) {
294		case 0:
295			fprintf(stderr, "with state ignore\n");
296			break;
297		case 1:
298			fprintf(stderr, "with state askfailed\n");
299			break;
300		case 2:
301			fprintf(stderr, "with state ready\n");
302			break;
303		case 3:
304			fprintf(stderr, "with state readyfail\n");
305			break;
306		default:
307			fprintf(stderr, "with state UNKNOWN (%d)\n", substate);
308			break;
309		}
310		if (substate > MSRP_LISTENER_ASKFAILED) {
311			if (memcmp
312			    (recovered_streamid, monitor_stream_id,
313			     sizeof(recovered_streamid)) == 0) {
314				listeners = 1;
315				fprintf(stderr, "added listener\n");
316			}
317		}
318		fflush(stdout);
319
320		/* try to find a newline ... */
321		while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
322			i++;
323		if (i == buflen)
324			return 0;
325		if (buf[i] == '\0')
326			return 0;
327		i++;
328		k = i;
329		goto next_line;
330		break;
331	case 'D':
332		i = k + 4;
333
334		/* save the domain attribute */
335		sscanf(&(buf[i]), "%d", &id);
336		while (buf[i] != 'P')
337			i++;
338		i += 2;		/* skip the ':' */
339		sscanf(&(buf[i]), "%d", &priority);
340		while (buf[i] != 'V')
341			i++;
342		i += 2;		/* skip the ':' */
343		sscanf(&(buf[i]), "%x", &vid);
344		if (id == 6) {
345			domain_class_a_id = id;
346			domain_class_a_priority = priority;
347			domain_class_a_vid = vid;
348			domain_a_valid = 1;
349		} else {
350			domain_class_b_id = id;
351			domain_class_b_priority = priority;
352			domain_class_b_vid = vid;
353			domain_b_valid = 1;
354		}
355		while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
356			i++;
357		if ((i == buflen) || (buf[i] == '\0'))
358			return 0;
359		i++;
360		k = i;
361		goto next_line;
362		break;
363	case 'T':
364
365		/* as simple_talker we don't care about other talkers */
366		i = k;
367		while ((i < buflen) && (buf[i] != '\n') && (buf[i] != '\0'))
368			i++;
369		if (i == buflen)
370			return 0;
371		if (buf[i] == '\0')
372			return 0;
373		i++;
374		k = i;
375		goto next_line;
376		break;
377	case 'S':
378
379		/* handle the leave/join events */
380		switch (buf[k + 4]) {
381		case 'L':
382			i = k + 5;
383			while (buf[i] != 'D')
384				i++;
385			i += 2;	/* skip the ':' */
386			sscanf(&(buf[i]), "%d", &substate);
387			while (buf[i] != 'S')
388				i++;
389			i += 2;	/* skip the ':' */
390			for (j = 0; j < 8; j++) {
391				sscanf(&(buf[i + 2 * j]), "%02x", &id);
392				recovered_streamid[j] = (unsigned char)id;
393			} printf
394			    ("EVENT on STREAM ID=%02x%02x%02x%02x%02x%02x%02x%02x ",
395			     recovered_streamid[0], recovered_streamid[1],
396			     recovered_streamid[2], recovered_streamid[3],
397			     recovered_streamid[4], recovered_streamid[5],
398			     recovered_streamid[6], recovered_streamid[7]);
399			switch (substate) {
400			case 0:
401				fprintf(stderr, "with state ignore\n");
402				break;
403			case 1:
404				fprintf(stderr, "with state askfailed\n");
405				break;
406			case 2:
407				fprintf(stderr, "with state ready\n");
408				break;
409			case 3:
410				fprintf(stderr, "with state readyfail\n");
411				break;
412			default:
413				fprintf(stderr, "with state UNKNOWN (%d)\n", substate);
414				break;
415			}
416			switch (buf[k + 1]) {
417			case 'L':
418				fprintf(stderr, "got a leave indication\n");
419				if (memcmp
420				    (recovered_streamid, monitor_stream_id,
421				     sizeof(recovered_streamid)) == 0) {
422					listeners = 0;
423					fprintf(stderr, "listener left\n");
424				}
425				break;
426			case 'J':
427			case 'N':
428				fprintf(stderr, "got a new/join indication\n");
429				if (substate > MSRP_LISTENER_ASKFAILED) {
430					if (memcmp
431					    (recovered_streamid,
432					     monitor_stream_id,
433					     sizeof(recovered_streamid)) == 0)
434						listeners = 1;
435				}
436				break;
437			}
438
439			/* only care about listeners ... */
440		default:
441			return 0;
442			break;
443		}
444		break;
445	case '\0':
446		break;
447	}
448	return 0;
449}
450
451void *mrp_monitor_thread(void *arg)
452{
453	char *msgbuf;
454	struct sockaddr_in client_addr;
455	struct msghdr msg;
456	struct iovec iov;
457	int bytes = 0;
458	struct pollfd fds;
459	int rc;
460
461	if (NULL == arg)
462		rc = 0;
463	else
464		rc = 1;
465
466	msgbuf = (char *)malloc(MAX_MRPD_CMDSZ);
467	if (NULL == msgbuf)
468		return NULL;
469
470	while (!halt_tx) {
471		fds.fd = control_socket;
472		fds.events = POLLIN;
473		fds.revents = 0;
474		rc = poll(&fds, 1, 100);
475		if (rc < 0) {
476			free(msgbuf);
477			pthread_exit(NULL);
478		}
479		if (rc == 0)
480			continue;
481		if ((fds.revents & POLLIN) == 0) {
482			free(msgbuf);
483			pthread_exit(NULL);
484		}
485		memset(&msg, 0, sizeof(msg));
486		memset(&client_addr, 0, sizeof(client_addr));
487		memset(msgbuf, 0, MAX_MRPD_CMDSZ);
488		iov.iov_len = MAX_MRPD_CMDSZ;
489		iov.iov_base = msgbuf;
490		msg.msg_name = &client_addr;
491		msg.msg_namelen = sizeof(client_addr);
492		msg.msg_iov = &iov;
493		msg.msg_iovlen = 1;
494		bytes = recvmsg(control_socket, &msg, 0);
495		if (bytes < 0)
496			continue;
497		process_mrp_msg(msgbuf, bytes);
498	}
499	free(msgbuf);
500	pthread_exit(NULL);
501}
502
503int mrp_monitor()
504{
505	pthread_attr_init(&monitor_attr);
506	pthread_create(&monitor_thread, NULL, mrp_monitor_thread, NULL);
507
508	return 0;
509}
510
511int mrp_join_listener(uint8_t * streamid)
512{
513	char *msgbuf;
514	int rc;
515	msgbuf = malloc(1500);
516	if (NULL == msgbuf)
517		return -1;
518	memset(msgbuf, 0, 1500);
519	sprintf(msgbuf, "S+L:S=%02X%02X%02X%02X%02X%02X%02X%02X"
520		",D=2", streamid[0], streamid[1], streamid[2], streamid[3],
521		streamid[4], streamid[5], streamid[6], streamid[7]);
522	mrp_okay = 0;
523	rc = send_mrp_msg(msgbuf, 1500);
524
525	free(msgbuf);
526	return rc;
527}
528
529int
530mrp_advertise_stream(uint8_t * streamid,
531		     uint8_t * destaddr,
532		     u_int16_t vlan,
533		     int pktsz, int interval, int priority, int latency)
534{
535	char *msgbuf;
536	int rc;
537	msgbuf = malloc(1500);
538	if (NULL == msgbuf)
539		return -1;
540	memset(msgbuf, 0, 1500);
541	sprintf(msgbuf, "S++:S=%02X%02X%02X%02X%02X%02X%02X%02X"
542		",A=%02X%02X%02X%02X%02X%02X"
543		",V=%04X"
544		",Z=%d"
545		",I=%d"
546		",P=%d"
547		",L=%d", streamid[0], streamid[1], streamid[2],
548		streamid[3], streamid[4], streamid[5], streamid[6],
549		streamid[7], destaddr[0], destaddr[1], destaddr[2],
550		destaddr[3], destaddr[4], destaddr[5], vlan, pktsz,
551		interval, priority << 5, latency);
552	mrp_okay = 0;
553	rc = send_mrp_msg(msgbuf, 1500);
554
555	/* rc = recv_mrp_okay(); */
556	free(msgbuf);
557	return rc;
558}
559
560int
561mrp_unadvertise_stream(uint8_t * streamid,
562		       uint8_t * destaddr,
563		       u_int16_t vlan,
564		       int pktsz, int interval, int priority, int latency)
565{
566	char *msgbuf;
567	int rc;
568	msgbuf = malloc(1500);
569	if (NULL == msgbuf)
570		return -1;
571	memset(msgbuf, 0, 1500);
572	sprintf(msgbuf, "S--:S=%02X%02X%02X%02X%02X%02X%02X%02X"
573		",A=%02X%02X%02X%02X%02X%02X"
574		",V=%04X"
575		",Z=%d"
576		",I=%d"
577		",P=%d"
578		",L=%d", streamid[0], streamid[1], streamid[2],
579		streamid[3], streamid[4], streamid[5], streamid[6],
580		streamid[7], destaddr[0], destaddr[1], destaddr[2],
581		destaddr[3], destaddr[4], destaddr[5], vlan, pktsz,
582		interval, priority << 5, latency);
583	mrp_okay = 0;
584	rc = send_mrp_msg(msgbuf, 1500);
585
586	/* rc = recv_mrp_okay(); */
587	free(msgbuf);
588	return rc;
589}
590
591#endif
592
593uint64_t reverse_64(uint64_t val)
594{
595	uint32_t low, high;
596
597	low = val & 0xffffffff;
598	high = (val >> 32) & 0xffffffff;
599	low = htonl(low);
600	high = htonl(high);
601
602	val = 0;
603	val = val | low;
604	val = (val << 32) | high;
605
606	return val;
607}
608
609void sigint_handler(int signum)
610{
611	fprintf(stderr, "got SIGINT\n");
612	halt_tx = signum;
613}
614
615int get_mac_addr(int8_t *iface)
616{
617	int lsock = socket(PF_PACKET, SOCK_RAW, htons(0x800));
618	struct ifreq if_request;
619	int rc;
620
621	if (lsock < 0)
622		return -1;
623
624	memset(&if_request, 0, sizeof(if_request));
625	strncpy(if_request.ifr_name, (const char *)iface, sizeof(if_request.ifr_name));
626
627	rc = ioctl(lsock, SIOCGIFHWADDR, &if_request);
628	if (rc < 0) {
629		close(lsock);
630		return -1;
631	}
632	memcpy(STATION_ADDR, if_request.ifr_hwaddr.sa_data,
633			sizeof(STATION_ADDR));
634	close(lsock);
635
636	return 0;
637}
638
639int main(int argc, char *argv[])
640{
641	struct igb_dma_alloc a_page;
642	struct igb_packet a_packet;
643	struct igb_packet *tmp_packet;
644	struct igb_packet *cleaned_packets;
645	struct igb_packet *free_packets;
646	six1883_header *h61883;
647	seventeen22_header *h1722;
648	unsigned i;
649	int err, seq_number, frame_size;
650	int8_t *iface = NULL;
651#ifdef DOMAIN_QUERY
652	int class_b_id = 0;
653	int b_priority = 0;
654	u_int16_t b_vid = 0;
655#endif
656#ifdef USE_MRPD
657	int class_a_id = 0;
658	int a_priority = 0;
659	u_int16_t a_vid = 0;
660#endif
661	unsigned samples_count = 0;
662	uint32_t packet_size;
663	int32_t read_bytes;
664	uint8_t *data_ptr;
665	void *stream_packet;
666	long long int frame_sequence = 0;
667	struct sched_param sched;
668
669	if (argc < 2) {
670		fprintf(stderr,"%s <if_name> <payload>\n", argv[0]);
671		return -1;
672	}
673
674	iface = (int8_t *)strdup(argv[1]);
675	packet_size = atoi(argv[2]);;
676	payload_length = atoi(argv[2]);;
677	packet_size += sizeof(six1883_header) + sizeof(seventeen22_header) + sizeof(eth_header);
678
679#ifdef USE_MRPD
680	err = mrp_connect();
681	if (err) {
682		fprintf(stderr, "socket creation failed\n");
683		return (errno);
684	}
685#endif
686
687	err = pci_connect(&igb_dev);
688	if (err) {
689		fprintf(stderr, "connect failed (%s) - are you running as root?\n", strerror(errno));
690		return (errno);
691	}
692
693	err = igb_init(&igb_dev);
694	if (err) {
695		fprintf(stderr, "init failed (%s) - is the driver really loaded?\n", strerror(errno));
696		return (errno);
697	}
698
699	err = igb_dma_malloc_page(&igb_dev, &a_page);
700	if (err) {
701		fprintf(stderr, "malloc failed (%s) - out of memory?\n", strerror(errno));
702		return (errno);
703	}
704
705	signal(SIGINT, sigint_handler);
706
707	err = get_mac_addr(iface);
708	if (err) {
709		fprintf(stderr, "failed to open iface(%s)\n",iface);
710		return -1;
711	}
712
713#ifdef USE_MRPD
714	mrp_monitor();
715	domain_a_valid = 1;
716	class_a_id = MSRP_SR_CLASS_A;
717	a_priority = MSRP_SR_CLASS_A_PRIO;
718	a_vid = 2;
719	fprintf(stderr, "detected domain Class A PRIO=%d VID=%04x...\n", a_priority,
720	       a_vid);
721
722        mrp_register_domain(&class_a_id, &a_priority, &a_vid);
723
724	domain_a_valid = 1;
725	a_vid = 2;
726	fprintf(stderr, "detected domain Class A PRIO=%d VID=%04x...\n", a_priority, a_vid);
727#endif
728
729	igb_set_class_bandwidth(&igb_dev, PACKET_IPG / 125000, 0, packet_size - 22, 0);
730
731	memset(STREAM_ID, 0, sizeof(STREAM_ID));
732	memcpy(STREAM_ID, STATION_ADDR, sizeof(STATION_ADDR));
733
734	a_packet.dmatime = a_packet.attime = a_packet.flags = 0;
735	a_packet.map.paddr = a_page.dma_paddr;
736	a_packet.map.mmap_size = a_page.mmap_size;
737	a_packet.offset = 0;
738	a_packet.vaddr = a_page.dma_vaddr + a_packet.offset;
739	a_packet.len = packet_size;
740	free_packets = NULL;
741	seq_number = 0;
742
743	frame_size = payload_length + sizeof(six1883_header) + sizeof(seventeen22_header) + sizeof(eth_header);
744
745	stream_packet = avb_create_packet(payload_length);
746
747	h1722 = (seventeen22_header *)((uint8_t*)stream_packet + sizeof(eth_header));
748	h61883 = (six1883_header *)((uint8_t*)stream_packet + sizeof(eth_header) +
749					sizeof(seventeen22_header));
750
751	/*initalize h1722 header */
752	avb_initialize_h1722_to_defaults(h1722);
753	/* set the length */
754	avb_set_1722_length(h1722, htons(payload_length + sizeof(six1883_header)));
755	avb_set_1722_stream_id(h1722,reverse_64(STREAMID));
756	avb_set_1722_sid_valid(h1722, 0x1);
757
758	
759	/*initalize h61883 header */
760	avb_initialize_61883_to_defaults(h61883);
761	avb_set_61883_format_tag(h61883, 0x1);
762	avb_set_61883_packet_channel(h61883, 0x1f);
763	avb_set_61883_packet_tcode(h61883, 0xa);
764	avb_set_61883_source_id(h61883 , 0x3f);
765	avb_set_61883_data_block_size(h61883, 0x1);
766	avb_set_61883_eoh(h61883, 0x2);
767	avb_set_61883_format_id(h61883, 0x10);
768	avb_set_61883_format_dependent_field(h61883, 0x2);
769	avb_set_61883_syt(h61883, 0xffff);
770
771	/* initilaze the source & destination mac address */
772	avb_eth_header_set_mac(stream_packet, DEST_ADDR, iface);
773
774	/* set 1772 eth type */
775	avb_1722_set_eth_type(stream_packet);
776
777	/* divide the dma page into buffers for packets */
778	for (i = 1; i < ((a_page.mmap_size) / packet_size); i++) {
779		tmp_packet = malloc(sizeof(struct igb_packet));
780		if (NULL == tmp_packet) {
781			fprintf(stderr, "failed to allocate igb_packet memory!\n");
782			return (errno);
783		}
784		*tmp_packet = a_packet;
785		tmp_packet->offset = (i * packet_size);
786		tmp_packet->vaddr += tmp_packet->offset;
787		tmp_packet->next = free_packets;
788		memset(tmp_packet->vaddr, 0, packet_size);	/* MAC header at least */
789		memcpy(((char *)tmp_packet->vaddr), stream_packet, frame_size);
790		tmp_packet->len = frame_size;
791		free_packets = tmp_packet;
792	}
793
794#ifdef USE_MRPD
795	/* 
796	 * subtract 16 bytes for the MAC header/Q-tag - pktsz is limited to the 
797	 * data payload of the ethernet frame .
798	 *
799	 * IPG is scaled to the Class (A) observation interval of packets per 125 usec
800	 */
801	fprintf(stderr, "advertising stream ...\n");
802	mrp_advertise_stream(STREAM_ID, DEST_ADDR, a_vid, packet_size - 16,
803		PACKET_IPG / 125000, a_priority, 3900);
804	fprintf(stderr, "awaiting a listener ...\n");
805	mrp_await_listener(STREAM_ID);
806#endif
807
808	memset(&sched, 0 , sizeof (sched));
809	sched.sched_priority = 1;
810	sched_setscheduler(0, SCHED_RR, &sched);
811
812	while (listeners && !halt_tx)
813	{
814		tmp_packet = free_packets;
815		if (NULL == tmp_packet)
816			goto cleanup;
817
818		stream_packet = ((char *)tmp_packet->vaddr);
819		free_packets = tmp_packet->next;
820
821		/* unfortuntely unless this thread is at rtprio
822		 * you get pre-empted between fetching the time
823		 * and programming the packet and get a late packet
824		 */
825		h1722 = (seventeen22_header *)((uint8_t*)stream_packet + sizeof(eth_header));
826		avb_set_1722_seq_number(h1722, seq_number++);
827		if (seq_number % 4 == 0)
828			avb_set_1722_timestamp_valid(h1722, 0);
829		else
830			avb_set_1722_timestamp_valid(h1722, 1);
831
832		data_ptr = (uint8_t *)((uint8_t*)stream_packet + sizeof(eth_header) + sizeof(seventeen22_header) 
833					+ sizeof(six1883_header));
834		
835		read_bytes = read(0, (void *)data_ptr, payload_length);
836		/* Error case while reading the input file */
837		if (read_bytes < 0) {
838			fprintf(stderr,"Failed to read from STDIN %s\n", argv[2]);
839			continue;
840		}
841		samples_count += read_bytes;
842		h61883 = (six1883_header *)((uint8_t*)stream_packet + sizeof(eth_header) + sizeof(seventeen22_header));
843		avb_set_61883_data_block_continuity(h61883 , samples_count);
844
845		err = igb_xmit(&igb_dev, 0, tmp_packet);
846		if (!err) {
847			fprintf(stderr,"frame sequence = %lld\n", frame_sequence++);
848			continue;
849		} else {
850			fprintf(stderr,"Failed frame sequence = %lld !!!!\n", frame_sequence++);
851		}
852
853		if (ENOSPC == err) {
854			/* put back for now */
855			tmp_packet->next = free_packets;
856			free_packets = tmp_packet;
857		}
858cleanup:
859		igb_clean(&igb_dev, &cleaned_packets);
860		while (cleaned_packets) {
861			tmp_packet = cleaned_packets;
862			cleaned_packets = cleaned_packets->next;
863			tmp_packet->next = free_packets;
864			free_packets = tmp_packet;
865		}
866	}
867
868	if (halt_tx == 0)
869		fprintf(stderr, "listener left ...\n");
870
871	halt_tx = 1;
872	sleep(1);
873#ifdef USE_MRPD
874	mrp_unadvertise_stream(STREAM_ID, DEST_ADDR, a_vid, packet_size - 16,
875			       PACKET_IPG / 125000, a_priority, 3900);
876#endif
877	/* disable Qav */
878	igb_set_class_bandwidth(&igb_dev, 0, 0, 0, 0);
879#ifdef USE_MRPD
880	err = mrp_disconnect();
881#endif
882	igb_dma_free_page(&igb_dev, &a_page);
883
884	err = igb_detach(&igb_dev);
885
886	pthread_exit(NULL);
887
888	return 0;
889}