bool LinuxTimestamperGeneric::HWTimestamper_PPS_start( ) {
unsigned tssdp;
unsigned freqout;
unsigned ctrl;
unsigned tsauxc;
unsigned trgttimh;
if( igb_private == NULL ) {
igb_private = new LinuxTimestamperIGBPrivate;
}
if( pci_connect( &igb_private->igb_dev ) != 0 ) {
return false;
}
if( igb_init( &igb_private->igb_dev ) != 0 ) {
return false;
}
igb_private->igb_initd = true;
igb_lock( &igb_private->igb_dev );
// Edges must be second aligned
igb_readreg( &igb_private->igb_dev, SYSTIMH, &trgttimh );
trgttimh += 2; // First edge in 1-2 seconds
igb_writereg(&igb_private->igb_dev, TRGTTIMH0, trgttimh );
igb_writereg(&igb_private->igb_dev, TRGTTIML0, 0 );
freqout = 500000000;
igb_writereg(&igb_private->igb_dev, FREQOUT0, freqout );
igb_readreg(&igb_private->igb_dev, IGB_CTRL, &ctrl );
ctrl |= 0x400000; // set bit 22 SDP0 enabling output
igb_writereg(&igb_private->igb_dev, IGB_CTRL, ctrl );
igb_readreg(&igb_private->igb_dev, TSSDP, &tssdp);
tssdp &= ~0x40; // Set SDP0 output to freq clock 0
tssdp |= 0x80;
igb_writereg(&igb_private->igb_dev, TSSDP, tssdp);
igb_readreg(&igb_private->igb_dev, TSSDP, &tssdp);
tssdp |= 0x100; // set bit 8 -> SDP0 Time Sync Output
igb_writereg(&igb_private->igb_dev, TSSDP, tssdp);
igb_readreg( &igb_private->igb_dev, TSAUXC, &tsauxc );
tsauxc |= 0x14;
igb_writereg( &igb_private->igb_dev, TSAUXC, tsauxc );
igb_unlock( &igb_private->igb_dev );
return true;
}
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;
}
static GstFlowReturn
gst_avbsrc_create (GstPushSrc * psrc, GstBuffer ** buf)
{
if(!avb_init)
{
struct packet_mreq mreq;
struct ifreq device;
uint32_t buff_size;
pthread_t tid;
int ifindex;
/* add a signal handler for interruption Ctl+c */
signal(SIGINT, sigint_handler);
buff_size = PAYLOAD_SIZE;
start_feed_socket_init();
err = pci_connect(&igb_dev);
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;
}
socket_d = socket(AF_PACKET, SOCK_RAW, htons(ETHER_TYPE_AVTP));
if (socket_d == -1) {
printf("failed to open event socket: %s \n", strerror(errno));
return -1;
}
memset(&device, 0, sizeof(device));
memcpy(device.ifr_name, interface1, IFNAMSIZ);
err = ioctl(socket_d, SIOCGIFINDEX, &device);
if (err == -1) {
printf("Failed to get interface index: %s\n", strerror(errno));
return -1;
}
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);
err = bind(socket_d, (struct sockaddr *) & ifsock_addr, sizeof(ifsock_addr));
if (err == -1) {
printf("Call to bind() failed: %s\n", strerror(errno));
return -1;
}
memset(&mreq, 0, sizeof(mreq));
mreq.mr_ifindex = ifindex;
mreq.mr_type = PACKET_MR_MULTICAST;
mreq.mr_alen = 6;
memcpy(mreq.mr_address, DEST_ADDR, mreq.mr_alen);
err = setsockopt(socket_d, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
if (err == -1) {
printf ("Unable to add PTP multicast addresses to port id: %u\n", ifindex);
return -1;
}
TAILQ_INIT(&buffer_queue);
TAILQ_INIT(&free_queue);
pthread_mutex_init(&(buffer_queue_lock), NULL);
pthread_mutex_init(&(free_queue_lock), NULL);
if (initiliaze_queue(buff_size) < 0)
return -EINVAL;
err = pthread_create(&tid, NULL, gstreamer_main_loop, NULL);
if (err != 0)
printf("can't create thread :[%s]\n", strerror(err));
memset(frame, 0, sizeof(frame));
size = sizeof(ifsock_addr);
frame_sequence = 0;
start_of_input_data = 1;
memset(&sched, 0 , sizeof (sched));
sched.sched_priority = 25;
sched_setscheduler(0, SCHED_RR, &sched);
}
while (1) {
if (g_exit_app)
break;
err = recvfrom(socket_d, frame, FRAME_SIZE, 0, (struct sockaddr *) &ifsock_addr, (socklen_t *)&size);
if (err > 0) {
while (!free_queue.tqh_first) {
usleep(1);
}
fprintf(stderr,"frame sequence = %lld\n", frame_sequence++);
pthread_mutex_lock(&(free_queue_lock));
qptr = free_queue.tqh_first;
TAILQ_REMOVE(&free_queue, qptr, entries);
h1722 = (seventeen22_header *)((uint8_t*)frame + sizeof(eth_header));
qptr->payload_length = ntohs(h1722->length) - sizeof(six1883_header);
memcpy(qptr->payload_data, (uint8_t *)((uint8_t*)frame + sizeof(eth_header) + sizeof(seventeen22_header)
+ sizeof(six1883_header)), qptr->payload_length);
pthread_mutex_lock(&(buffer_queue_lock));
TAILQ_INSERT_TAIL(&buffer_queue, qptr, entries);
pthread_mutex_unlock(&(buffer_queue_lock));
pthread_mutex_unlock(&(free_queue_lock));
start_of_input_data = 1;
} else {
printf("Failed to receive frame !!!\n");
}
}
usleep(100);
pthread_exit(NULL);
close(socket_d);
freequeue();
return GST_FLOW_OK;
}
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);
}
