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); }