bool halTimeGetLocaltime(U64 *localTime64)
{
AVB_TRACE_ENTRY(AVB_TRACE_TIME);
if (igb_get_wallclock(igb_dev, localTime64, NULL ) > 0) {
IF_LOG_INTERVAL(1000) AVB_LOG_ERROR("Failed to get wallclock time");
AVB_TRACE_EXIT(AVB_TRACE_TIME);
return FALSE;
}
AVB_TRACE_EXIT(AVB_TRACE_TIME);
return TRUE;
}
int main(int argc, char *argv[])
{
unsigned i;
int err;
struct igb_dma_alloc a_page;
struct igb_packet a_packet;
struct igb_packet *tmp_packet;
struct igb_packet *cleaned_packets;
struct igb_packet *free_packets;
int c;
u_int64_t last_time;
int rc = 0;
char *interface = NULL;
int class_a_id = 0;
int a_priority = 0;
u_int16_t a_vid = 0;
#ifdef DOMAIN_QUERY
int class_b_id = 0;
int b_priority = 0;
u_int16_t b_vid = 0;
#endif
int seqnum;
int time_stamp;
unsigned total_samples = 0;
gPtpTimeData td;
int32_t sample_buffer[SAMPLES_PER_FRAME * SRC_CHANNELS];
seventeen22_header *header0;
six1883_header *header1;
six1883_sample *sample;
uint64_t now_local, now_8021as;
uint64_t update_8021as;
unsigned delta_8021as, delta_local;
long double ml_ratio;
for (;;) {
c = getopt(argc, argv, "hi:");
if (c < 0)
break;
switch (c) {
case 'h':
usage();
break;
case 'i':
if (interface) {
printf
("only one interface per daemon is supported\n");
usage();
}
interface = strdup(optarg);
break;
}
}
if (optind < argc)
usage();
if (NULL == interface) {
usage();
}
rc = mrp_connect();
if (rc) {
printf("socket creation failed\n");
return (errno);
}
err = pci_connect();
if (err) {
printf("connect failed (%s) - are you running as root?\n",
strerror(errno));
return (errno);
}
err = igb_init(&igb_dev);
if (err) {
printf("init failed (%s) - is the driver really loaded?\n",
strerror(errno));
return (errno);
}
err = igb_dma_malloc_page(&igb_dev, &a_page);
if (err) {
printf("malloc failed (%s) - out of memory?\n",
strerror(errno));
return (errno);
}
signal(SIGINT, sigint_handler);
rc = get_mac_address(interface);
if (rc) {
printf("failed to open interface\n");
usage();
}
mrp_monitor();
#ifdef DOMAIN_QUERY
/*
* should use mrp_get_domain() above but this is a simplification
*/
#endif
domain_a_valid = 1;
class_a_id = MSRP_SR_CLASS_A;
a_priority = MSRP_SR_CLASS_A_PRIO;
a_vid = 2;
printf("detected domain Class A PRIO=%d VID=%04x...\n", a_priority,
a_vid);
#define PKT_SZ 100
mrp_register_domain(&class_a_id, &a_priority, &a_vid);
igb_set_class_bandwidth(&igb_dev, PACKET_IPG / 125000, 0, PKT_SZ - 22,
0);
memset(STREAM_ID, 0, sizeof(STREAM_ID));
memcpy(STREAM_ID, STATION_ADDR, sizeof(STATION_ADDR));
a_packet.dmatime = a_packet.attime = a_packet.flags = 0;
a_packet.map.paddr = a_page.dma_paddr;
a_packet.map.mmap_size = a_page.mmap_size;
a_packet.offset = 0;
a_packet.vaddr = a_page.dma_vaddr + a_packet.offset;
a_packet.len = PKT_SZ;
free_packets = NULL;
seqnum = 0;
/* divide the dma page into buffers for packets */
for (i = 1; i < ((a_page.mmap_size) / PKT_SZ); i++) {
tmp_packet = malloc(sizeof(struct igb_packet));
if (NULL == tmp_packet) {
printf("failed to allocate igb_packet memory!\n");
return (errno);
}
*tmp_packet = a_packet;
tmp_packet->offset = (i * PKT_SZ);
tmp_packet->vaddr += tmp_packet->offset;
tmp_packet->next = free_packets;
memset(tmp_packet->vaddr, 0, PKT_SZ); /* MAC header at least */
memcpy(tmp_packet->vaddr, DEST_ADDR, sizeof(DEST_ADDR));
memcpy(tmp_packet->vaddr + 6, STATION_ADDR,
sizeof(STATION_ADDR));
/* Q-tag */
((char *)tmp_packet->vaddr)[12] = 0x81;
((char *)tmp_packet->vaddr)[13] = 0x00;
((char *)tmp_packet->vaddr)[14] =
((a_priority << 13 | a_vid)) >> 8;
((char *)tmp_packet->vaddr)[15] =
((a_priority << 13 | a_vid)) & 0xFF;
((char *)tmp_packet->vaddr)[16] = 0x22; /* 1722 eth type */
((char *)tmp_packet->vaddr)[17] = 0xF0;
/* 1722 header update + payload */
header0 =
(seventeen22_header *) (((char *)tmp_packet->vaddr) + 18);
header0->cd_indicator = 0;
header0->subtype = 0;
header0->sid_valid = 1;
header0->version = 0;
header0->reset = 0;
header0->reserved0 = 0;
header0->gateway_valid = 0;
header0->reserved1 = 0;
header0->timestamp_uncertain = 0;
memset(&(header0->stream_id), 0, sizeof(header0->stream_id));
memcpy(&(header0->stream_id), STATION_ADDR,
sizeof(STATION_ADDR));
header0->length = htons(32);
header1 = (six1883_header *) (header0 + 1);
header1->format_tag = 1;
header1->packet_channel = 0x1F;
header1->packet_tcode = 0xA;
header1->app_control = 0x0;
header1->reserved0 = 0;
header1->source_id = 0x3F;
header1->data_block_size = 1;
header1->fraction_number = 0;
header1->quadlet_padding_count = 0;
header1->source_packet_header = 0;
header1->reserved1 = 0;
header1->eoh = 0x2;
header1->format_id = 0x10;
header1->format_dependent_field = 0x02;
header1->syt = 0xFFFF;
tmp_packet->len =
18 + sizeof(seventeen22_header) + sizeof(six1883_header) +
(SAMPLES_PER_FRAME * CHANNELS * sizeof(six1883_sample));
free_packets = tmp_packet;
}
/*
* subtract 16 bytes for the MAC header/Q-tag - pktsz is limited to the
* data payload of the ethernet frame .
*
* IPG is scaled to the Class (A) observation interval of packets per 125 usec
*/
fprintf(stderr, "advertising stream ...\n");
mrp_advertise_stream(STREAM_ID, DEST_ADDR, a_vid, PKT_SZ - 16,
PACKET_IPG / 125000, a_priority, 3900);
fprintf(stderr, "awaiting a listener ...\n");
mrp_await_listener(STREAM_ID);
printf("got a listener ...\n");
halt_tx = 0;
gptpinit();
gptpscaling(&td);
if( igb_get_wallclock( &igb_dev, &now_local, NULL ) != 0 ) {
fprintf( stderr, "Failed to get wallclock time\n" );
return -1;
}
update_8021as = td.local_time - td.ml_phoffset;
delta_local = (unsigned)(now_local - td.local_time);
ml_ratio = -1 * (((long double)td.ml_freqoffset) / 1000000000000) + 1;
delta_8021as = (unsigned)(ml_ratio * delta_local);
now_8021as = update_8021as + delta_8021as;
last_time = now_local + XMIT_DELAY;
time_stamp = now_8021as + RENDER_DELAY;
rc = nice(-20);
while (listeners && !halt_tx) {
tmp_packet = free_packets;
if (NULL == tmp_packet)
goto cleanup;
header0 =
(seventeen22_header *) (((char *)tmp_packet->vaddr) + 18);
header1 = (six1883_header *) (header0 + 1);
free_packets = tmp_packet->next;
/* unfortuntely unless this thread is at rtprio
* you get pre-empted between fetching the time
* and programming the packet and get a late packet
*/
tmp_packet->attime = last_time + PACKET_IPG;
last_time += PACKET_IPG;
get_samples(SAMPLES_PER_FRAME, sample_buffer);
header0->seq_number = seqnum++;
if (seqnum % 4 == 0)
header0->timestamp_valid = 0;
else
header0->timestamp_valid = 1;
time_stamp = htonl(time_stamp);
header0->timestamp = time_stamp;
time_stamp = ntohl(time_stamp);
time_stamp += PACKET_IPG;
header1->data_block_continuity = total_samples;
total_samples += SAMPLES_PER_FRAME*CHANNELS;
sample =
(six1883_sample *) (((char *)tmp_packet->vaddr) +
(18 + sizeof(seventeen22_header) +
sizeof(six1883_header)));
for (i = 0; i < SAMPLES_PER_FRAME * CHANNELS; ++i) {
uint32_t tmp = htonl(sample_buffer[i]);
sample[i].label = 0x40;
memcpy(&(sample[i].value), &(tmp),
sizeof(sample[i].value));
}
err = igb_xmit(&igb_dev, 0, tmp_packet);
if (!err) {
continue;
}
if (ENOSPC == err) {
/* put back for now */
tmp_packet->next = free_packets;
free_packets = tmp_packet;
}
cleanup: igb_clean(&igb_dev, &cleaned_packets);
i = 0;
while (cleaned_packets) {
i++;
tmp_packet = cleaned_packets;
cleaned_packets = cleaned_packets->next;
tmp_packet->next = free_packets;
free_packets = tmp_packet;
}
}
rc = nice(0);
if (halt_tx == 0)
printf("listener left ...\n");
halt_tx = 1;
mrp_unadvertise_stream(STREAM_ID, DEST_ADDR, a_vid, PKT_SZ - 16,
PACKET_IPG / 125000, a_priority, 3900);
igb_set_class_bandwidth(&igb_dev, 0, 0, 0, 0); /* disable Qav */
rc = mrp_disconnect();
igb_dma_free_page(&igb_dev, &a_page);
err = igb_detach(&igb_dev);
pthread_exit(NULL);
return (0);
}
