int main(int argc, char *argv[ ]) { device_t igb_dev; struct ifreq device; int error; struct sockaddr_ll ifsock_addr; struct packet_mreq mreq; int ifindex; int socket_descriptor; char *iface; seventeen22_header *h1722; long long int frame_sequence = 0; unsigned char frame[MAX_FRAME_SIZE]; int size, length; struct sched_param sched; struct mrp_listener_ctx *ctx = malloc(sizeof(struct mrp_listener_ctx)); struct mrp_domain_attr *class_a = malloc(sizeof(struct mrp_domain_attr)); struct mrp_domain_attr *class_b = malloc(sizeof(struct mrp_domain_attr)); ctx_sig = ctx; int rc; if (argc < 2) { fprintf(stderr, "Usage : %s <interface_name> <payload>\n",argv[0]); return EINVAL; } signal(SIGINT, sigint_handler); #if USE_MRPD rc = mrp_listener_client_init(ctx); if (rc) { printf("failed to initialize global variables\n"); return EXIT_FAILURE; } if (create_socket(ctx)) { fprintf(stderr, "Socket creation failed.\n"); return errno; } rc = mrp_monitor(ctx); if (rc) { printf("failed creating MRP monitor thread\n"); return EXIT_FAILURE; } rc=mrp_get_domain(ctx, class_a, class_b); if (rc) { printf("failed calling mrp_get_domain()\n"); return EXIT_FAILURE; } printf("detected domain Class A PRIO=%d VID=%04x...\n",class_a->priority,class_a->vid); rc = report_domain_status(class_a,ctx); if (rc) { printf("report_domain_status failed\n"); return EXIT_FAILURE; } rc = join_vlan(class_a, ctx); if (rc) { printf("join_vlan failed\n"); return EXIT_FAILURE; } fprintf(stdout,"Waiting for talker...\n"); await_talker(ctx); rc = send_ready(ctx); if (rc) { printf("send_ready failed\n"); return EXIT_FAILURE; } #endif /* USE_MRPD */ iface = strdup(argv[1]); error = pci_connect(&igb_dev); if (error) { fprintf(stderr, "connect failed (%s) - are you running as root?\n", strerror(errno)); return errno; } error = igb_init(&igb_dev); if (error) { fprintf(stderr, "init failed (%s) - is the driver really loaded?\n", strerror(errno)); return errno; } socket_descriptor = socket(AF_PACKET, SOCK_RAW, htons(ETHER_TYPE_AVTP)); if (socket_descriptor < 0) { fprintf(stderr, "failed to open socket: %s \n", strerror(errno)); return EINVAL; } memset(&device, 0, sizeof(device)); memcpy(device.ifr_name, iface, IFNAMSIZ); error = ioctl(socket_descriptor, SIOCGIFINDEX, &device); if (error < 0) { fprintf(stderr, "Failed to get index of iface %s: %s\n", iface, strerror(errno)); return EINVAL; } ifindex = device.ifr_ifindex; memset(&ifsock_addr, 0, sizeof(ifsock_addr)); ifsock_addr.sll_family = AF_PACKET; ifsock_addr.sll_ifindex = ifindex; ifsock_addr.sll_protocol = htons(ETHER_TYPE_AVTP); error = bind(socket_descriptor, (struct sockaddr *) & ifsock_addr, sizeof(ifsock_addr)); if (error < 0) { fprintf(stderr, "Failed to bind: %s\n", strerror(errno)); return EINVAL; } memset(&mreq, 0, sizeof(mreq)); mreq.mr_ifindex = ifindex; mreq.mr_type = PACKET_MR_MULTICAST; mreq.mr_alen = 6; memcpy(mreq.mr_address, glob_dest_addr, mreq.mr_alen); error = setsockopt(socket_descriptor, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq)); if (error < 0) { fprintf(stderr, "Failed to add multi-cast addresses to port: %u\n", ifindex); return EINVAL; } size = sizeof(ifsock_addr); frame_sequence = 0; memset(frame, 0, sizeof(frame)); memset(&sched, 0, sizeof sched); sched.sched_priority = 1; error = sched_setscheduler(0, SCHED_RR, &sched); if (error < 0) fprintf(stderr, "Failed to select RR scheduler: %s (%d)\n", strerror(errno), errno); while (1) { error = recvfrom(socket_descriptor, frame, MAX_FRAME_SIZE, 0, (struct sockaddr *) &ifsock_addr, (socklen_t *)&size); if (error > 0) { fprintf(stderr,"frame sequence = %lld\n", frame_sequence++); h1722 = (seventeen22_header *)((uint8_t*)frame + sizeof(eth_header)); length = ntohs(h1722->length) - sizeof(six1883_header); write(1, (uint8_t *)((uint8_t*)frame + sizeof(eth_header) + sizeof(seventeen22_header) + sizeof(six1883_header)), length); } else { fprintf(stderr,"recvfrom() error for frame sequence = %lld\n", frame_sequence++); } } usleep(100); close(socket_descriptor); free(ctx); free(class_a); free(class_b); return EXIT_SUCCESS; }
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); }