diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..aabeb848 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "linux/avbmcl/OpenAvnu"] + path = linux/avbmcl/OpenAvnu + url = https://github.com/chris-kuhr/OpenAvnu.git diff --git a/linux/avbmcl/JackAVBDriver.cpp b/linux/avbmcl/JackAVBDriver.cpp new file mode 100644 index 00000000..06320c4e --- /dev/null +++ b/linux/avbmcl/JackAVBDriver.cpp @@ -0,0 +1,387 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "JackAVBDriver.h" +#include "JackEngineControl.h" +#include "JackLockedEngine.h" +#include "JackGraphManager.h" +#include "JackWaitThreadedDriver.h" +#include "JackTools.h" +#include "driver_interface.h" + +#define MIN(x,y) ((x)<(y) ? (x) : (y)) + +using namespace std; + +namespace Jack +{ +JackAVBDriver::JackAVBDriver(const char* name, const char* alias, JackLockedEngine* engine, JackSynchro* table, + char* stream_id, char* destination_mac, char* eth_dev, + int sample_rate, int period_size, int adjust, + int num_periods, int capture_ports, int playback_ports) + : JackWaiterDriver(name, alias, engine, table) +{ + jack_log("JackAVBDriver::JackAVBPDriver Ethernet Device %s", eth_dev); + jack_log("Stream ID: %02x %02x %02x %02x %02x %02x %02x %02x", + (uint8_t) stream_id[0], + (uint8_t) stream_id[1], + (uint8_t) stream_id[2], + (uint8_t) stream_id[3], + (uint8_t) stream_id[4], + (uint8_t) stream_id[5], + (uint8_t) stream_id[6], + (uint8_t) stream_id[7]); + jack_log("Destination MAC Address: %02x:%02x:%02x:%02x:%02x:%02x", + (uint8_t) destination_mac[0], + (uint8_t) destination_mac[1], + (uint8_t) destination_mac[2], + (uint8_t) destination_mac[3], + (uint8_t) destination_mac[4], + (uint8_t) destination_mac[5]); + printf("JackAVBDriver::JackAVBPDriver Ethernet Device %s\n", eth_dev); + printf("Stream ID: %02x %02x %02x %02x %02x %02x %02x %02x\n", + (uint8_t) stream_id[0], + (uint8_t) stream_id[1], + (uint8_t) stream_id[2], + (uint8_t) stream_id[3], + (uint8_t) stream_id[4], + (uint8_t) stream_id[5], + (uint8_t) stream_id[6], + (uint8_t) stream_id[7]); + printf("Destination MAC Address: %02x:%02x:%02x:%02x:%02x:%02x\n", + (uint8_t) destination_mac[0], + (uint8_t) destination_mac[1], + (uint8_t) destination_mac[2], + (uint8_t) destination_mac[3], + (uint8_t) destination_mac[4], + (uint8_t) destination_mac[5]); + num_packets_even_odd = 0; // even = 0, odd = 1 + lastPeriodDuration = 0; + timeCompensation = 0; + monotonicTime = 0; + preRunCnt = 3; + + init_avb_driver( &(this->avb_ctx), + eth_dev, + stream_id, + destination_mac, + sample_rate, + period_size, + num_periods, + adjust, + capture_ports, + playback_ports + ); +} + +JackAVBDriver::~JackAVBDriver() +{ + // No destructor yet. +} + +int JackAVBDriver::Close() +{ + // Generic audio driver close + int res = JackWaiterDriver::Close(); + FreePorts(); + shutdown_avb_driver(&avb_ctx); + return res; +} + +int JackAVBDriver::AllocPorts() +{ + jack_port_id_t port_index; + char buf[64]; + int chn = 0; + + for (chn = 0; chn < (int)avb_ctx.capture_channels; chn++) { + memset(buf, 0, sizeof(buf)); + snprintf (buf, sizeof(buf) - 1, "system:capture_%u", chn + 1); + if (fEngine->PortRegister(fClientControl.fRefNum, buf, JACK_DEFAULT_AUDIO_TYPE, + CaptureDriverFlags, fEngineControl->fBufferSize, &port_index) < 0) { + jack_error("driver: cannot register port for %s", buf); + return -1; + } + + avb_ctx.capture_ports = jack_slist_append (avb_ctx.capture_ports, (void *)(intptr_t)port_index); + } + + for (chn = 0; chn < (int)avb_ctx.playback_channels; chn++) { + memset(buf, 0, sizeof(buf)); + snprintf (buf, sizeof(buf) - 1, "system:playback_%u", chn + 1); + + if (fEngine->PortRegister(fClientControl.fRefNum, buf, JACK_DEFAULT_AUDIO_TYPE, + PlaybackDriverFlags, fEngineControl->fBufferSize, &port_index) < 0) { + jack_error("driver: cannot register port for %s", buf); + return -1; + } + + avb_ctx.playback_ports = jack_slist_append (avb_ctx.playback_ports, (void *)(intptr_t)port_index); + } + //port = fGraphManager->GetPort(port_index); + return 0; +} + +bool JackAVBDriver::Initialize() +{ + jack_log("JackAVBDriver::Init"); + FreePorts(); + + //display some additional infos + printf("AVB driver started\n"); + + if (startup_avb_driver(&avb_ctx)) { + return false; + } + + //register jack ports + if (AllocPorts() != 0) { + jack_error("Can't allocate ports."); + return false; + } + + //driver parametering + JackTimedDriver::SetBufferSize(avb_ctx.period_size); + JackTimedDriver::SetSampleRate(avb_ctx.sample_rate); + + JackDriver::NotifyBufferSize(avb_ctx.period_size); + JackDriver::NotifySampleRate(avb_ctx.sample_rate); + + return true; +} + +int JackAVBDriver::Read() +{ + int ret = 0; + JSList *node = avb_ctx.capture_ports; + uint64_t cumulative_rx_int_ns = 0; + uint64_t lateness = 0; + int n = 0; + + for(n=0; npreRunCnt >= 0 ){ + cumulative_rx_int_ns -= this->timeCompensation; + }*/ + //jack_errors("duration: %lld", cumulative_rx_int_ns); + } + this->monotonicTime += cumulative_rx_int_ns; +/* + if( this->lastPeriodDuration != 0 ){ + this->timeCompensation = cumulative_rx_int_ns - this->lastPeriodDuration; + } + this->lastPeriodDuration = cumulative_rx_int_ns; +*/ + + float cumulative_rx_int_us = (cumulative_rx_int_ns / 1000) - 0.5; + if ( cumulative_rx_int_us > avb_ctx.period_usecs ) { + ret = 1; + NotifyXRun(fBeginDateUst, cumulative_rx_int_us); + jack_error("avtp_xruns... duration: %.2f ms", lateness / 1000000); + } + + JackDriver::CycleTakeBeginTime(); + + if ( ret ) return -1; + + while (node != NULL) { + jack_port_id_t port_index = (jack_port_id_t)(intptr_t) node->data; + JackPort *port = fGraphManager->GetPort(port_index); + jack_default_audio_sample_t* buf = (jack_default_audio_sample_t*)fGraphManager->GetBuffer(port_index, fEngineControl->fBufferSize); + //memcpy(buf, 0, avb_ctx.period_size * sizeof(jack_default_audio_sample_t)); + node = jack_slist_next (node); + } + return 0; +} + +int JackAVBDriver::Write() +{ + JSList *node = avb_ctx.playback_ports; + while (node != NULL) { + jack_port_id_t port_index = (jack_port_id_t)(intptr_t) node->data; + JackPort *port = fGraphManager->GetPort(port_index); + jack_default_audio_sample_t* buf = (jack_default_audio_sample_t*)fGraphManager->GetBuffer(port_index, fEngineControl->fBufferSize); + //memcpy(buf, 0, avb_ctx.period_size * sizeof(jack_default_audio_sample_t)); + node = jack_slist_next (node); + } + return 0; +} + +void JackAVBDriver::FreePorts () +{ + JSList *node = avb_ctx.capture_ports; + + while (node != NULL) { + JSList *this_node = node; + jack_port_id_t port_index = (jack_port_id_t)(intptr_t) node->data; + node = jack_slist_remove_link(node, this_node); + jack_slist_free_1(this_node); + fEngine->PortUnRegister(fClientControl.fRefNum, port_index); + } + avb_ctx.capture_ports = NULL; + node = avb_ctx.playback_ports; + + while (node != NULL) { + JSList *this_node = node; + jack_port_id_t port_index = (jack_port_id_t)(intptr_t) node->data; + node = jack_slist_remove_link(node, this_node); + jack_slist_free_1(this_node); + fEngine->PortUnRegister(fClientControl.fRefNum, port_index); + } + avb_ctx.playback_ports = NULL; +} + +#ifdef __cplusplus +extern "C" +{ +#endif + + inline int argumentsSplitDelimiters(char* inputString, char* outputArray, int array_len) + { + int tokenCnt=0; + char *token; + char *der_string = strdup(inputString); + int m = 0; + for( m=0;mdata; + switch (param->character) { + case 'i': + capture_ports = param->value.ui; + break; + case 'o': + playback_ports = param->value.ui; + break; + case 'r': + sample_rate = param->value.ui; + break; + case 'p': + period_size = param->value.ui; + break; + case 'n': + num_periods = param->value.ui; + break; + case 'a': + adjust = param->value.ui; + break; + case 'e': + sprintf(eth_dev, "%s", param->value.str); + printf("Eth Dev: %s %s\n", param->value.str, eth_dev);fflush(stdout); + break; + case 's': + // split stream ID + argumentsSplitDelimiters((char *)param->value.str, sid, 8); + printf("Stream ID: %s %02x %02x %02x %02x %02x %02x %02x %02x \n", param->value.str, + sid[0], sid[1], sid[2], sid[3], sid[4], sid[5], sid[6], sid[7]);fflush(stdout); + break; + case 'm': + // split destination mac address + argumentsSplitDelimiters((char *)param->value.str, dmac, 6); + printf("Destination MAC Address: %s %02x %02x %02x %02x %02x %02x \n", param->value.str, + dmac[0], dmac[1], dmac[2], dmac[3], dmac[4], dmac[5]);fflush(stdout); + break; + } + } + + try { + Jack::JackDriverClientInterface* driver = new Jack::JackWaitThreadedDriver ( + new Jack::JackAVBDriver("system", "avb_mc", engine, table, sid, dmac, eth_dev, + sample_rate, period_size, num_periods, + adjust, capture_ports, playback_ports)); + + if (driver->Open(period_size, sample_rate, 1, 1, capture_ports, playback_ports, + 0, "from_master", "to_master", 0, 0) == 0) { + return driver; + } else { + delete driver; + return NULL; + } + + } catch (...) { + return NULL; + } + } + +#ifdef __cplusplus +} +#endif +} diff --git a/linux/avbmcl/JackAVBDriver.h b/linux/avbmcl/JackAVBDriver.h new file mode 100644 index 00000000..75189d6d --- /dev/null +++ b/linux/avbmcl/JackAVBDriver.h @@ -0,0 +1,65 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#ifndef _JACK_AVB_DRIVER_H_ +#define _JACK_AVB_DRIVER_H_ + +#include "JackTimedDriver.h" +#include "avb.h" + +namespace Jack +{ + +// Brief This class describes the AVB Backend +class JackAVBDriver : public JackWaiterDriver +{ + private: + avb_driver_state_t avb_ctx; + int num_packets_even_odd; + uint64_t lastPeriodDuration; + uint64_t timeCompensation; + uint64_t monotonicTime; + int preRunCnt; + + public: + JackAVBDriver(const char* name, const char* alias, JackLockedEngine* engine, JackSynchro* table, + char* stream_id, char* destination_mac, char* eth_dev, + int sample_rate, int period_size, int num_periods, + int adjust, int capture_ports, int playback_ports); + virtual ~JackAVBDriver(); + + int Close(); + int Attach(){return 0;} + int Detach(){return 0;} + + int Read(); + int Write(); + + bool Initialize(); + int AllocPorts(); + void FreePorts(); + + // BufferSize can't be changed + bool IsFixedBufferSize(){return true;} + int SetBufferSize(jack_nframes_t buffer_size){return -1;} + int SetSampleRate(jack_nframes_t sample_rate){return -1;} +}; + +} + +#endif //_JACK_AVB_DRIVER_H_ diff --git a/linux/avbmcl/README.md b/linux/avbmcl/README.md new file mode 100644 index 00000000..95f9aae8 --- /dev/null +++ b/linux/avbmcl/README.md @@ -0,0 +1,59 @@ +Experimental Audio Video Bridging Media Clock Listener JACK Backend + +For a detailed description refer to http://lac.linuxaudio.org/2019/doc/kuhr.pdf + +##### +# Requirements: +##### +- Intel i210 Ethernet Adapter +- Running OpenAvnu installation (igb_avb kernel module, libigb, gPTP daemon, MAAP daemon, MRP daemon, Shaper daemon) +- AVB Media Clock Talker on the AVB network segment must be active and accept any connection (only SRP, no AVDECC ACMP) +- linux/if_packet.h + + +##### +# Linux build steps: +##### +# Init submodule linux/avbmcl/OpenAvnu +cd linux/avbmcl +git submodule update --init +cd ../../ +# Build JACK: +./waf configure +./waf build +sudo ./waf install + + + +##### +# Example parameters: +##### + +# Choose AVB backend: + -d avbmcl + +# Choose ethernet device name: + --eth-dev enp5s0 + +# Stream ID of the media clock stream: + --stream-id 00:22:97:00:41:2c:00:00 + +# Destination MAC address of the media clock talker + --dst-mac 91:e0:f0:11:11:11 + +# Use JACK periods with accumulated reception intervals (1) +# or with constant 125000ns (0): + -a 1 + +# JACK periods (32, 64 and 128 have been tested): + -p + +# JACK sample rate (only 48k has been tested): + -r + + +##### +# Example call: +##### + +jackd -R -P8 -davbmcl --eth-dev enp5s0 --stream-id 00:22:97:00:41:2c:00:00 --dst-mac 91:e0:f0:11:11:11 -a 1 -r 48000 -p 64 diff --git a/linux/avbmcl/avb.c b/linux/avbmcl/avb.c new file mode 100644 index 00000000..614cbb26 --- /dev/null +++ b/linux/avbmcl/avb.c @@ -0,0 +1,506 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "avb.h" +#include "mrp_client_control_socket.h" +#include "mrp_client_interface.h" +#include "mrp_client_send_msg.h" + +#define SHM_SIZE (sizeof(gPtpTimeData) + sizeof(pthread_mutex_t)) // Shared memory size +#define SHM_NAME "/ptp" // Shared memory name +#define SHM_NAME_LENGTH 100 + +extern int errno; +FILE* filepointer; +int shm_fd; +mrp_ctx_t *mrp_ctx; +volatile int mrp_running = 1; +struct sockaddr_in *si_other_avb = NULL; +struct pollfd *avtp_transport_socket_fds = NULL; + +// POSIX Shared Memory for Listener Context +int mrp_shm_open(int _init) +{ + char shm_name[SHM_NAME_LENGTH]; + memset(shm_name, 0, SHM_NAME_LENGTH); + sprintf(shm_name, "/mediaclock_jack_mrp_ctx"); + fprintf(filepointer, "Open listener shm %s\n", shm_name);fflush(filepointer); + + if( _init == 1){ + if ((shm_fd = shm_open(shm_name, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR)) == -1) { + fprintf(filepointer, "Open listener shm failed %s\n", strerror( errno ) );fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + fprintf(filepointer, "Setting size of listener shm\n");fflush(filepointer); + if( ftruncate( shm_fd, sizeof(mrp_ctx_t) ) == -1 ){ + fprintf(filepointer, "Setting size of listener shm failed %s\n", strerror( errno ) );fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + + if( MAP_FAILED == (mrp_ctx = (mrp_ctx_t*) mmap( 0, sizeof(mrp_ctx_t), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0 ) ) ){ + fprintf(filepointer, "Get listener shm address failed %s\n", strerror( errno ) );fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + + memset(mrp_ctx, 0, sizeof(mrp_ctx_t) ); + + } else { + if ((shm_fd = shm_open(shm_name, O_RDWR, S_IRUSR | S_IWUSR)) == -1) { + fprintf(filepointer, "Open listener shm failed %s\n", strerror( errno ) );fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + + if( MAP_FAILED == (mrp_ctx = (mrp_ctx_t*) mmap( 0, sizeof(mrp_ctx_t), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0 ) ) ){ + fprintf(filepointer, "Get listener shm address failed %s\n", strerror( errno ) );fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + } + return RETURN_VALUE_SUCCESS; +} + +int mrp_shm_close(int _remove) +{ + char shm_name[SHM_NAME_LENGTH]; + memset(shm_name, 0, SHM_NAME_LENGTH); + + if( close( shm_fd ) == -1){ + fprintf(filepointer, "Close listener shm failed %s\n", strerror( errno ) );fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + + if( _remove ){ + sprintf(shm_name, "/mediaclock_jack"); + if( shm_unlink( shm_name ) == -1 ){ + fprintf(filepointer, "Unlink listener shm failed %s\n", strerror( errno ) );fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + } + return RETURN_VALUE_SUCCESS; +} + +// MRP Client Functions +int check_stream_id(FILE* filepointer2, avb_driver_state_t **avb_ctx, int *buf_offset, char *buf) +{ + unsigned int streamid[8]; + int hit = 0; + int i = 0; + + fprintf(filepointer2, "Event on Stream Id: ");fflush(filepointer2); + for(i = 0; i < 8 ; (*buf_offset)+=2, i++) { + sscanf(&buf[*buf_offset],"%02x",&streamid[i]); + fprintf(filepointer2, "%02x ", streamid[i]);fflush(filepointer2); + } + fprintf(filepointer2, "\n");fflush(filepointer2); + + hit = 0; + for(i = 0; i < 8 ; i++) { + if( streamid[i] == (*avb_ctx)->streamid8[i]){ + hit++; + } else { + break; + } + } + if( hit == 8 ){ + fprintf(filepointer2, " Message for media clock listener stream Id %02x:%02x%02x:%02x%02x:%02x%02x:%02x\n", + (*avb_ctx)->streamid8[0], (*avb_ctx)->streamid8[1], + (*avb_ctx)->streamid8[2], (*avb_ctx)->streamid8[3], + (*avb_ctx)->streamid8[4], (*avb_ctx)->streamid8[5], + (*avb_ctx)->streamid8[6], (*avb_ctx)->streamid8[7]); + fflush(filepointer2); + return RETURN_VALUE_SUCCESS; + } + return RETURN_VALUE_FAILURE; +} + +int check_listener_dst_mac(FILE* filepointer2, avb_driver_state_t **avb_ctx, int *buf_offset, char *buf) +{ + unsigned int mac_addr[6]; + int hit = 0; + int i = 0; + + fprintf(filepointer2, "Check destination MAC... ");fflush(filepointer2); + for(i = 0; i < 6 ; (*buf_offset)+=2, i++) { + sscanf(&buf[*buf_offset],"%02x",&mac_addr[i]); +// fprintf(filepointer2, "%02x == %02x ?\n", mac_addr[i], (*avb_ctx)->destination_mac_address[i]);fflush(filepointer2); + if( mac_addr[i] == (*avb_ctx)->destination_mac_address[i]){ + if( ++hit == 6 ){ + return RETURN_VALUE_SUCCESS; + } + } else { + return RETURN_VALUE_FAILURE; + } + } + return RETURN_VALUE_FAILURE; +} + +int find_next_line(FILE* filepointer2, char* buf, int* buf_offset, int buflen, int* buf_pos) +{ + fprintf(filepointer2, " try to find a newline buflen %d bufpos %d bufoffset %d\n", + buflen, *buf_pos, *buf_offset);fflush(filepointer2); + while (((*buf_offset) < buflen) && (buf[*buf_offset] != '\n') && (buf[*buf_offset] != '\0')){ +// fprintf(filepointer2, " bufoffset %d buf %s\n", *buf_offset, &buf[*buf_offset]);fflush(filepointer2); + (*buf_offset)++; + } + if ((*buf_offset) == buflen || buf[*buf_offset] == '\0'){ + fprintf(filepointer2, " end of message buflen %d bufoffset %d\n", buflen, *buf_offset);fflush(filepointer2); + return RETURN_VALUE_FAILURE; + } + *buf_pos = ++(*buf_offset); + fprintf(filepointer2, " found new line bufpos %d bufoffset %d\n", *buf_pos, *buf_offset);fflush(filepointer2); + + return RETURN_VALUE_SUCCESS; +} + +int process_mrp_msg(FILE* filepointer2, avb_driver_state_t **avb_ctx, char *buf, int buflen) +{ + unsigned int id=0; + unsigned int priority=0; + unsigned int vid=0; + int buf_offset=0; + int buf_pos = 0; + +next_line_listener: + + fprintf(filepointer2, "%s", buf);fflush(filepointer2); + fprintf(filepointer2, "mrp status = %d\n", mrp_ctx->mrp_status);fflush(filepointer2); + + // 1st character indicates application + // [MVS] - MAC, VLAN or STREAM + if (strncmp(buf, "SNE T:", 6) == 0 || strncmp(buf, "SJO T:", 6) == 0) { + fprintf(filepointer2, " SNE T or SJO T: %s\n", buf);fflush(filepointer2); + buf_offset = 6; // skip "Sxx T:" + while ((buf_offset < buflen) && ('S' != buf[buf_offset++])); + if (buf_offset == buflen) + return RETURN_VALUE_FAILURE; + buf_offset++; + + if( RETURN_VALUE_SUCCESS == check_stream_id(filepointer2, avb_ctx, &buf_offset, buf)){ + buf_offset+=3; + if( check_listener_dst_mac(filepointer2, avb_ctx, &buf_offset, buf) + && mrp_ctx->mrp_status == LISTENER_WAITING ){ + mrp_ctx->mrp_status = LISTENER_READY; +// return RETURN_VALUE_SUCCESS; + } + } + } else + if (strncmp(buf, "SJO D:", 6) == 0) { + fprintf(filepointer2, " SJO D: %s\n", buf);fflush(filepointer2); + +// buf_offset=8; + sscanf(&(buf[8]), "%d", &id); +// buf_offset+=4; + sscanf(&(buf[12]), "%d", &priority); +// buf_offset+=4; + sscanf(&(buf[16]), "%x", &vid); + + if( vid == 0 || priority == 0 || id == 0){ + fprintf(filepointer2, " found 0-mvrp message ... skipping line\n");fflush(filepointer2); + + char* msgbuf2= malloc(1500); + + if (NULL == msgbuf2) return RETURN_VALUE_FAILURE; + + memset(msgbuf2, 0, 1500); + sprintf(msgbuf2, "V--:I=%04x",vid); + fprintf(filepointer2, "Leave VLAN %s\n",msgbuf2);fflush(filepointer2); + + mrp_client_send_mrp_msg( filepointer2, mrp_client_get_Control_socket(), msgbuf2, 1500); + free(msgbuf2); + + if( find_next_line( filepointer2, buf, &buf_offset, buflen, &buf_pos ) == RETURN_VALUE_FAILURE ){ + return RETURN_VALUE_SUCCESS; + } else { + goto next_line_listener; + } + } + + if (id == 6 && mrp_ctx->domain_a_valid == 0 ){ // Class A + mrp_ctx->domain_class_a_id = id; + mrp_ctx->domain_class_a_priority = priority; + mrp_ctx->domain_class_a_vid = vid; + mrp_ctx->domain_a_valid = 1; + fprintf(filepointer2, " Domain A for Mediaclock Listener valid %x %x %x %x\n", + mrp_ctx->domain_class_a_id, + mrp_ctx->domain_class_a_priority, + mrp_ctx->domain_class_a_vid, + mrp_ctx->domain_a_valid);fflush(filepointer2); + } else if (id == 5 && mrp_ctx->domain_b_valid == 0 ){ // Class B + mrp_ctx->domain_class_b_id = id; + mrp_ctx->domain_class_b_priority = priority; + mrp_ctx->domain_class_b_vid = vid; + mrp_ctx->domain_b_valid = 1; + fprintf(filepointer2, " Domain B for Mediaclock Listener valid %x %x %x %x\n", + mrp_ctx->domain_class_b_id, + mrp_ctx->domain_class_b_priority, + mrp_ctx->domain_class_b_vid, + mrp_ctx->domain_b_valid);fflush(filepointer2); + } +// buf_offset+=4; + return RETURN_VALUE_SUCCESS; + } + return RETURN_VALUE_SUCCESS; +} + +int mrp_thread(avb_driver_state_t **avb_ctx) +{ + char *msgbuf; + struct sockaddr_in client_addr; + struct msghdr msg; + struct iovec iov; + int bytes = 0; + struct pollfd fds; + int rc; + FILE* filepointer2; + struct timespec tim, tim2; + int cnt_sec_to_listener_ready = 0; + + if( ! (filepointer2 = fopen("mrp.log", "w"))){ + printf("Error Opening file %d\n", errno); + return RETURN_VALUE_FAILURE; + } + + msgbuf = (char *)malloc(MAX_MRPD_CMDSZ); + + if (msgbuf == NULL ){ + fprintf(filepointer2, "mrp_talker_monitor_thread - NULL == msgbuf\n");fflush(filepointer2); + return RETURN_VALUE_FAILURE; + } + + if(RETURN_VALUE_FAILURE == mrp_shm_open( 0 ) ){ + return RETURN_VALUE_FAILURE; + } + + tim.tv_sec = 0; + tim.tv_nsec = 10 * MILISLEEP_TIME; + tim2.tv_sec = 0; + tim2.tv_nsec = 0; + + fds.fd = mrp_client_get_Control_socket(); + fds.events = POLLIN; + fds.revents = 0; + + // Register this Client with MRP Daemon + bool connect_to_daemon = true; + + while ( mrp_running ) { + if( connect_to_daemon ){ + fprintf(filepointer2, "connect to MRP daemon...");fflush(filepointer2); + memset(&msg, 0, sizeof(msg)); + memset(&client_addr, 0, sizeof(client_addr)); + memset(msgbuf, 0, MAX_MRPD_CMDSZ); + iov.iov_len = MAX_MRPD_CMDSZ; + iov.iov_base = msgbuf; + msg.msg_name = &client_addr; + msg.msg_namelen = sizeof(client_addr); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + + sprintf(msgbuf, "S??"); + rc = mrp_client_send_mrp_msg(filepointer2, mrp_client_get_Control_socket(), msgbuf, 1500); + connect_to_daemon = false; + fprintf(filepointer2, "successfully.\n");fflush(filepointer2); + } + + if( (rc = poll(&fds, 1, 100) ) == 0) continue; + if( (rc < 0) || ( (fds.revents & POLLIN) == 0 ) ) { + fprintf(filepointer2, "mrp client process: rc = poll(&fds, 1, 100) failed\n");fflush(filepointer2); + break; + } + + memset(&msg, 0, sizeof(msg)); + memset(&client_addr, 0, sizeof(client_addr)); + memset(msgbuf, 0, MAX_MRPD_CMDSZ); + iov.iov_len = MAX_MRPD_CMDSZ; + iov.iov_base = msgbuf; + msg.msg_name = &client_addr; + msg.msg_namelen = sizeof(client_addr); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + + if( (bytes = recvmsg(mrp_client_get_Control_socket(), &msg, 0)) <=0) continue; + + fprintf( filepointer2, "\nprocess_mrp_msg %d:\n", bytes);fflush(filepointer2); + + process_mrp_msg(filepointer2, avb_ctx, msgbuf, bytes); + fprintf( filepointer2, "\n\n");fflush(filepointer2); + + + + + if( 900 == cnt_sec_to_listener_ready++) { + if ( mrp_client_listener_send_ready( filepointer2, avb_ctx, mrp_ctx ) > RETURN_VALUE_FAILURE) { + fprintf(filepointer2, "send_ready success\n");fflush(filepointer2); + } + } + + + + + + + + + + + + nanosleep(&tim , &tim2); + } + + fprintf(filepointer2, "quit_mrp: quitting\n");fflush(filepointer2); + + if (NULL == msgbuf){ + fprintf(filepointer2, "LISTENER_FAILED\n");fflush(filepointer2); + return RETURN_VALUE_FAILURE; + } + + memset(msgbuf, 0, 1500); + sprintf(msgbuf, "BYE"); + + rc = mrp_client_send_mrp_msg(filepointer2, mrp_client_get_Control_socket(), msgbuf, 1500); + free(msgbuf); + fclose(filepointer2); + mrp_shm_close(0); + + if (rc != 1500) + return RETURN_VALUE_FAILURE; + else + return RETURN_VALUE_SUCCESS; +} + +void *worker_thread_mrp(void* v_avb_ctx) +{ + avb_driver_state_t *t_avb_ctx = (avb_driver_state_t *) v_avb_ctx; + mrp_thread( &t_avb_ctx ); + pthread_exit(0); +} + +// AVB Backend +int init_avb_driver( avb_driver_state_t *avb_ctx, const char* name, + char* stream_id, char* destination_mac, + int sample_rate, int period_size, int num_periods, int adjust, int capture_ports, int playback_ports) +{ + char filename[100]; + sprintf(filename, "jackAVBdriver.log"); + if( ! (filepointer = fopen(filename, "w"))){ + printf("Error Opening file %d\n", errno); + fclose(filepointer); + return -1; // EXIT_FAILURE + } + + if(RETURN_VALUE_FAILURE == mrp_shm_open( 1 ) ){ + return -1; // EXIT_FAILURE + } + + if( mrp_client_init_Control_socket( filepointer ) == RETURN_VALUE_FAILURE ) { + fprintf(filepointer, "Error initializing MRP socket\n");fflush(filepointer); + fclose(filepointer); + return -1; // EXIT_FAILURE + } + + if( pthread_create( &avb_ctx->thread, NULL, (&worker_thread_mrp), (void*) avb_ctx ) != 0 ) { + fprintf(filepointer, "Error creating thread\n");fflush(filepointer); + fclose(filepointer); + return -1; // EXIT_FAILURE + } else { + fprintf(filepointer, "Success creating thread\n");fflush(filepointer); + } + + fprintf(filepointer, "JackAVBDriver::JackAVBPDriver Ethernet Device %s\n", name);fflush(filepointer); + fprintf(filepointer, "Stream ID: %02x %02x %02x %02x %02x %02x %02x %02x\n", + (uint8_t) stream_id[0], + (uint8_t) stream_id[1], + (uint8_t) stream_id[2], + (uint8_t) stream_id[3], + (uint8_t) stream_id[4], + (uint8_t) stream_id[5], + (uint8_t) stream_id[6], + (uint8_t) stream_id[7]);fflush(filepointer); + fprintf(filepointer, "Destination MAC Address: %02x:%02x:%02x:%02x:%02x:%02x\n", + (uint8_t) destination_mac[0], + (uint8_t) destination_mac[1], + (uint8_t) destination_mac[2], + (uint8_t) destination_mac[3], + (uint8_t) destination_mac[4], + (uint8_t) destination_mac[5]);fflush(filepointer); + avb_ctx->playback_channels = playback_ports; + avb_ctx->capture_channels = capture_ports; + avb_ctx->adjust = adjust; + avb_ctx->sample_rate = sample_rate; + avb_ctx->period_size = period_size; + avb_ctx->period_usecs = (uint64_t) ((float)period_size / (float)sample_rate * 1000000); + avb_ctx->num_packets = (int)( avb_ctx->period_size / 6 ) + 1; + + fprintf(filepointer,"sample_rate: %d, period size: %d, period usec: %lud, num_packets: %d\n", + avb_ctx->sample_rate, avb_ctx->period_size, + avb_ctx->period_usecs, avb_ctx->num_packets);fflush(filepointer); + + if( RETURN_VALUE_FAILURE == avtp_mcl_create(filepointer, &avb_ctx, name, + stream_id, destination_mac, + &si_other_avb, &avtp_transport_socket_fds)){ + fprintf(filepointer, "Creation failed!\n");fflush(filepointer); + fclose(filepointer); + return -1; // EXIT_FAILURE + } + + if( RETURN_VALUE_FAILURE == mrp_client_listener_send_leave(filepointer, &avb_ctx, mrp_ctx )){ + fprintf(filepointer, "send leave failed\n");fflush(filepointer); + } else { + fprintf(filepointer, "send leave success\n");fflush(filepointer); + } + + if( RETURN_VALUE_FAILURE == mrp_client_getDomain_joinVLAN( filepointer, &avb_ctx, mrp_ctx) ){ + fprintf(filepointer, "mrp_client_getDomain_joinVLAN failed\n");fflush(filepointer); + fclose(filepointer); + return -1; // EXIT_FAILURE + } + return 0; // EXIT_SUCCESS +} + + +int startup_avb_driver( avb_driver_state_t *avb_ctx ) +{ + if( RETURN_VALUE_FAILURE == mrp_client_listener_await_talker( filepointer, &avb_ctx, mrp_ctx)){ + fprintf(filepointer, "mrp_client_listener_await_talker failed\n");fflush(filepointer); + fclose(filepointer); + return -1; // EXIT_FAILURE + } else { + return 0; // EXIT_SUCCESS + } +} + +uint64_t await_avtp_rx_ts( avb_driver_state_t *avb_ctx, int packet_num, uint64_t *lateness ) +{ +// return avtp_mcl_wait_for_rx_ts( filepointer, &avb_ctx, &si_other_avb, &avtp_transport_socket_fds, packet_num ); + return avtp_mcl_wait_for_rx_ts_const( filepointer, &avb_ctx, &si_other_avb, &avtp_transport_socket_fds, packet_num, lateness ); +} + +int shutdown_avb_driver( avb_driver_state_t *avb_ctx ) +{ + if( RETURN_VALUE_FAILURE == mrp_client_listener_send_leave(filepointer, &avb_ctx, mrp_ctx )){ + fprintf(filepointer, "send leave failed\n");fflush(filepointer); + } else { + fprintf(filepointer, "send leave success\n");fflush(filepointer); + } + + mrp_running = 0; + pthread_join(avb_ctx->thread, NULL); + mrp_shm_close( 1 ); + avtp_mcl_delete( filepointer, &avb_ctx ); + fclose(filepointer); + return 0; // EXIT_SUCCESS +} diff --git a/linux/avbmcl/avb.h b/linux/avbmcl/avb.h new file mode 100644 index 00000000..adb15210 --- /dev/null +++ b/linux/avbmcl/avb.h @@ -0,0 +1,41 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#ifndef _JACK_AVTP_DRIVER_H_ +#define _JACK_AVTP_DRIVER_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "avb_definitions.h" +#include "media_clock_listener.h" + +int init_avb_driver( avb_driver_state_t *avb_ctx, const char* name, + char* stream_id, char* destination_mac, + int sample_rate, int period_size, int num_periods, int adjust, int capture_ports, int playback_ports); +int startup_avb_driver( avb_driver_state_t *avb_ctx); +uint64_t await_avtp_rx_ts( avb_driver_state_t *avb_ctx, int packet_num, uint64_t *lateness ); +int shutdown_avb_driver( avb_driver_state_t *avb_ctx); + +#ifdef __cplusplus +} +#endif + +#endif //_JACK_AVTP_DRIVER_H_ diff --git a/linux/avbmcl/avb_definitions.h b/linux/avbmcl/avb_definitions.h new file mode 100644 index 00000000..a3f8cad9 --- /dev/null +++ b/linux/avbmcl/avb_definitions.h @@ -0,0 +1,150 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#ifndef _AVB_DEFINITIONS_H_ +#define _AVB_DEFINITIONS_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include "jack/jslist.h" + +#include "OpenAvnu/daemons/mrpd/mrpd.h" +#include "OpenAvnu/daemons/mrpd/mrp.h" + +#define RETURN_VALUE_FAILURE 0 +#define RETURN_VALUE_SUCCESS 1 + +#define MILISLEEP_TIME 1000000 +#define USLEEP_TIME 1000 + +#define MAX_DEV_STR_LEN 32 +#define BUFLEN 1500 +#define ETHERNET_Q_HDR_LENGTH 18 +#define ETHERNET_HDR_LENGTH 14 +#define IP_HDR_LENGTH 20 +#define UDP_HDR_LENGTH 8 +#define AVB_ETHER_TYPE 0x22f0 +#define ARRAYSIZE(arr) (sizeof(arr) / sizeof(arr[0])) + +typedef struct etherheader_q +{ + unsigned char ether_dhost[6]; // destination eth addr + unsigned char ether_shost[6]; // source ether addr + unsigned int vlan_id; // VLAN ID field + unsigned short int ether_type; // packet type ID field +} etherheader_q_t; + +typedef struct etherheader +{ + unsigned char ether_dhost[6]; // destination eth addr + unsigned char ether_shost[6]; // source ether addr + unsigned short int ether_type; // packet type ID field +} etherheader_t; + + +typedef struct mrp_ctx{ + volatile int mrp_status; + volatile int domain_a_valid; + volatile int domain_b_valid; + int domain_class_a_id; + int domain_class_a_priority; + u_int16_t domain_class_a_vid; + int domain_class_b_id; + int domain_class_b_priority; + u_int16_t domain_class_b_vid; +}mrp_ctx_t; + +typedef enum mrpStatus{ + TL_UNDEFINED, + TALKER_IDLE, + TALKER_ADVERTISE, + TALKER_ASKFAILED, + TALKER_READYFAILED, + TALKER_CONNECTING, + TALKER_CONNECTED, + TALKER_ERROR, + LISTENER_IDLE, + LISTENER_WAITING, + LISTENER_READY, + LISTENER_CONNECTED, + LISTENER_ERROR, + LISTENER_FAILED +} mrpStatus_t; + + +typedef struct _avb_driver_state avb_driver_state_t; + +struct _avb_driver_state { + + uint8_t streamid8[8]; + uint8_t destination_mac_address[6]; + unsigned char serverMACAddress[6]; + char avbdev[MAX_DEV_STR_LEN]; + struct sockaddr_in si_other_avb; + int raw_transport_socket; + struct ifreq if_idx; + struct ifreq if_mac; + + pthread_t thread; + pthread_mutex_t threadLock; + pthread_cond_t dataReady; + + jack_nframes_t sample_rate; + + jack_nframes_t period_size; + jack_time_t period_usecs; + int num_packets; + int adjust; + + unsigned int capture_channels; + unsigned int playback_channels; + + JSList *capture_ports; + JSList *playback_ports; + JSList *playback_srcs; + JSList *capture_srcs; + + jack_client_t *client; +}; + +#ifdef __cplusplus +} +#endif + +#endif // _AVB_DEFINITIONS_H_ diff --git a/linux/avbmcl/avb_sockets.c b/linux/avbmcl/avb_sockets.c new file mode 100644 index 00000000..17a40687 --- /dev/null +++ b/linux/avbmcl/avb_sockets.c @@ -0,0 +1,145 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "avb_sockets.h" + +#define DUMMY_STREAMID (0xABCDEF) + +// IEEE 1722 AVTP Receive Socket +int enable_1722avtp_filter( FILE* filepointer, int raw_transport_socket, unsigned char *destinationMacAddress) +{ + // tcpdump -i enp9s0 "ether dst 91:e0:f0:00:c3:51" -dd + // { 0x20, 0, 0, 0x00000002 }, + // { 0x15, 0, 3, 0xf000c351 }, + // { 0x28, 0, 0, 0x00000000 }, + // { 0x15, 0, 1, 0x000091e0 }, + // { 0x6, 0, 0, 0x00040000 }, + // { 0x6, 0, 0, 0x00000000 }, + + unsigned int low_mac=0, hi_mac=0; + low_mac = (((destinationMacAddress[0] & 0xFF) & 0xFFFFFFFF) << 8 ) | + (destinationMacAddress[1] & 0xFF ); + hi_mac = (((destinationMacAddress[2] & 0xFF) & 0xFFFFFFFF) << 24 ) | + (((destinationMacAddress[3] & 0xFF ) & 0xFFFFFFFF) << 16 ) | + (((destinationMacAddress[4] & 0xFF ) & 0xFFFFFFFF) << 8 ) | + (destinationMacAddress[5] & 0xFF ); + + struct sock_filter code[] = { + { 0x20, 0, 0, 0x00000002 }, + { 0x15, 0, 3, hi_mac }, + { 0x28, 0, 0, 0x00000000 }, + { 0x15, 0, 1, low_mac }, + { 0x6, 0, 0, 0x00040000 }, + { 0x6, 0, 0, 0x00000000 }, + }; + + struct sock_fprog bpf = { + .len = ARRAYSIZE(code), + .filter = code, + }; + + if(setsockopt(raw_transport_socket, SOL_SOCKET, SO_ATTACH_FILTER, &bpf, sizeof(bpf)) < 0) { + fprintf(filepointer, "setsockopt error: %s \n", strerror(errno));fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + return RETURN_VALUE_FAILURE; +} + +int create_RAW_AVB_Transport_Socket( FILE* filepointer, int* raw_transport_socket, const char* eth_dev) +{ + struct ifreq ifr; + memset((char*)&ifr, 0, sizeof(struct ifreq)); + int sockopt=0; + + struct ifreq ifopts; + memset((char*)&ifopts, 0, sizeof(struct ifreq)); + int s; + + strncpy (ifr.ifr_name, eth_dev, IFNAMSIZ - 1); + ifr.ifr_name[sizeof(ifr.ifr_name)-1] = '\0'; + + if (( s = socket(PF_PACKET, SOCK_RAW, htons(AVB_ETHER_TYPE/*ETH_P_ALL*/))) < 0){ + fprintf(filepointer, "[RAW_TRANSPORT] Error creating RAW Socket \n");fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + *raw_transport_socket = s; + + strncpy(ifopts.ifr_name, ifr.ifr_name, IFNAMSIZ-1); + + if( ioctl(*raw_transport_socket, SIOCGIFFLAGS, &ifopts) == -1) { + fprintf(filepointer, "[RAW_TRANSPORT] No such interface");fflush(filepointer); + fprintf(filepointer, "Zero \n");fflush(filepointer); + close(*raw_transport_socket); + return RETURN_VALUE_FAILURE; + } + + ifopts.ifr_flags |= IFF_PROMISC; + + if( ioctl(*raw_transport_socket, SIOCSIFFLAGS, &ifopts) == -1){ + fprintf(filepointer, "[RAW_TRANSPORT] Interface is down. \n");fflush(filepointer); + close(*raw_transport_socket); + return RETURN_VALUE_FAILURE; + } + + if (setsockopt(*raw_transport_socket, SOL_SOCKET, SO_REUSEADDR, &sockopt, sizeof( sockopt)) == -1) { + fprintf(filepointer, "[RAW_TRANSPORT] setsockopt failed \n");fflush(filepointer); + close(*raw_transport_socket); + return RETURN_VALUE_FAILURE; + } + + // Set Timestamping Option => requires recvmsg to be used => timestamp in ancillary data + int timestamp_flags = 0; + + // NECESARRY FOR CMSGs TO WORK => see LAC2019 paper + // timestamp_flags |= SOF_TIMESTAMPING_TX_HARDWARE; + timestamp_flags |= SOF_TIMESTAMPING_RX_HARDWARE; + timestamp_flags |= SOF_TIMESTAMPING_SYS_HARDWARE; + timestamp_flags |= SOF_TIMESTAMPING_RAW_HARDWARE; + + struct hwtstamp_config hwconfig; + memset( &hwconfig, 0, sizeof( hwconfig )); + hwconfig.rx_filter = HWTSTAMP_FILTER_ALL; + // hwconfig.tx_type = HWTSTAMP_TX_OFF; + hwconfig.tx_type = HWTSTAMP_TX_ON; // NECESARRY FOR CMSGs TO WORK => see LAC2019 paper + + struct ifreq hwtstamp; + memset((char*)&hwtstamp, 0, sizeof(struct ifreq)); + strncpy(hwtstamp.ifr_name, ifr.ifr_name, IFNAMSIZ-1); + hwtstamp.ifr_data = (void *) &hwconfig; + + if( ioctl( *raw_transport_socket, SIOCSHWTSTAMP, &hwtstamp ) == -1 ) { + fprintf(filepointer, "[RAW TRANSPORT] ioctl timestamping failed %d %s \n", errno, strerror(errno));fflush(filepointer); + close(*raw_transport_socket); + return RETURN_VALUE_FAILURE; + } + + if (setsockopt(*raw_transport_socket, SOL_SOCKET, SO_TIMESTAMPING, ×tamp_flags, sizeof(timestamp_flags) ) == -1) { + fprintf(filepointer, "[RAW TRANSPORT] setsockopt timestamping failed %d %s \n", errno, strerror(errno));fflush(filepointer); + close(*raw_transport_socket); + return RETURN_VALUE_FAILURE; + } else { + fprintf(filepointer, "[RAW TRANSPORT] Timestamp Socket \n");fflush(filepointer); + } + + if (setsockopt(*raw_transport_socket, SOL_SOCKET, SO_BINDTODEVICE, eth_dev, IFNAMSIZ-1) == -1) { + fprintf(filepointer, "[RAW_TRANSPORT] SO_BINDTODEVICE failed \n");fflush(filepointer); + close(*raw_transport_socket); + return RETURN_VALUE_FAILURE; + } + return RETURN_VALUE_SUCCESS; +} diff --git a/linux/avbmcl/avb_sockets.h b/linux/avbmcl/avb_sockets.h new file mode 100644 index 00000000..586a9e6f --- /dev/null +++ b/linux/avbmcl/avb_sockets.h @@ -0,0 +1,36 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#ifndef _AVB_SOCKET_H_ +#define _AVB_SOCKET_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "avb_definitions.h" + +int enable_1722avtp_filter( FILE* filepointer, int raw_transport_socket, unsigned char *destinationMacAddress); +int create_RAW_AVB_Transport_Socket( FILE* filepointer, int* raw_transport_socket, const char* eth_dev); + +#ifdef __cplusplus +} +#endif + +#endif //_AVB_SOCKET_H_ diff --git a/linux/avbmcl/media_clock_listener.c b/linux/avbmcl/media_clock_listener.c new file mode 100644 index 00000000..fa3f8cc8 --- /dev/null +++ b/linux/avbmcl/media_clock_listener.c @@ -0,0 +1,276 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "media_clock_listener.h" + +#define NUM_TS 10000000 + +extern int errno; +static uint64_t last_packet_time_ns = 0; +uint64_t timestamps[NUM_TS]; +int ts_cnt =0; + +int avtp_mcl_create( FILE* filepointer, avb_driver_state_t **avb_ctx, const char* avb_dev_name, + char* stream_id, char* destination_mac, + struct sockaddr_in **si_other_avb, struct pollfd **avtp_transport_socket_fds) +{ + fprintf(filepointer, "Create Mediaclock Listener\n");fflush(filepointer); + memset( timestamps, 0, sizeof(uint64_t)*NUM_TS); + //00:22:97:00:41:2c:00:00 91:e0:f0:11:11:11 + memcpy((*avb_ctx)->streamid8, stream_id, 8); + memcpy((*avb_ctx)->destination_mac_address, destination_mac, 6); + + fprintf(filepointer, "create RAW AVTP Socket %s \n", avb_dev_name);fflush(filepointer); + (*avtp_transport_socket_fds) = (struct pollfd*)malloc(sizeof(struct pollfd)); + memset((*avtp_transport_socket_fds), 0, sizeof(struct sockaddr_in)); + (*si_other_avb) = (struct sockaddr_in*)malloc(sizeof(struct sockaddr_in)); + memset((*si_other_avb), 0, sizeof(struct sockaddr_in)); + + if( create_RAW_AVB_Transport_Socket(filepointer, &((*avtp_transport_socket_fds)->fd), avb_dev_name) > RETURN_VALUE_FAILURE ){ + fprintf(filepointer, "enable IEEE1722 AVTP MAC filter %x:%x:%x:%x:%x:%x \n", + (*avb_ctx)->destination_mac_address[0], + (*avb_ctx)->destination_mac_address[1], + (*avb_ctx)->destination_mac_address[2], + (*avb_ctx)->destination_mac_address[3], + (*avb_ctx)->destination_mac_address[4], + (*avb_ctx)->destination_mac_address[5]);fflush(filepointer); + + enable_1722avtp_filter(filepointer, (*avtp_transport_socket_fds)->fd, (*avb_ctx)->destination_mac_address); + (*avtp_transport_socket_fds)->events = POLLIN; + } else { + fprintf(filepointer, "Listener Creation failed\n");fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + + fprintf(filepointer, "Get Domain VLAN\n");fflush(filepointer); + return RETURN_VALUE_SUCCESS; +} + +void avtp_mcl_delete( FILE* filepointer, avb_driver_state_t **avb_ctx ) +{ + FILE* filepointer2; + int i = 0; + + if( ! (filepointer2 = fopen("mcs_ts.log", "w")) ){ + printf("Error Opening file %d\n", errno); + return; + } + + for(i = 0; i < NUM_TS; i++){ + if(timestamps[i] != 0 ){ + fprintf(filepointer2, "%lud\n",timestamps[i]);fflush(filepointer2); + } + } + fclose(filepointer2); +} + +uint64_t avtp_mcl_wait_for_rx_ts( FILE* filepointer, avb_driver_state_t **avb_ctx, + struct sockaddr_in **si_other_avb, + struct pollfd **avtp_transport_socket_fds, + int packet_num ) +{ + char stream_packet[BUFLEN]; + +// struct cmsghdr { +// socklen_t cmsg_len; // data byte count, including hdr +// int cmsg_level; // originating protocol +// int cmsg_type; // protocol-specific type +// // followed by unsigned char cmsg_data[]; +// }; +// +// struct msghdr { +// void *msg_name; // optional address +// socklen_t msg_namelen; // size of address +// struct iovec *msg_iov; // scatter/gather array +// size_t msg_iovlen; // # elements in msg_iov +// void *msg_control; // ancillary data, see below +// size_t msg_controllen; // ancillary data buffer len +// int msg_flags; // flags on received message +// }; + + struct msghdr msg; + struct cmsghdr *cmsg; + struct sockaddr_ll remote; + struct iovec sgentry; + struct { + struct cmsghdr cm; + char control[256]; + } control; + + memset( &msg, 0, sizeof( msg )); + msg.msg_iov = &sgentry; + msg.msg_iovlen = 1; + sgentry.iov_base = stream_packet; + sgentry.iov_len = BUFLEN; + + memset( &remote, 0, sizeof(remote)); + msg.msg_name = (caddr_t) &remote; + msg.msg_namelen = sizeof( remote ); + msg.msg_control = &control; + msg.msg_controllen = sizeof(control); + + int status = recvmsg((*avtp_transport_socket_fds)->fd, &msg, 0);//NULL); + + if (status == 0) { + fprintf(filepointer, "EOF\n");fflush(filepointer); + return -1; + } else if (status < 0) { + fprintf(filepointer, "Error recvmsg: %d %d %s\n", status, errno, strerror(errno));fflush(filepointer); + return -1; + } + if( // Compare Stream IDs + ((*avb_ctx)->streamid8[0] == (uint8_t) stream_packet[18]) && + ((*avb_ctx)->streamid8[1] == (uint8_t) stream_packet[19]) && + ((*avb_ctx)->streamid8[2] == (uint8_t) stream_packet[20]) && + ((*avb_ctx)->streamid8[3] == (uint8_t) stream_packet[21]) && + ((*avb_ctx)->streamid8[4] == (uint8_t) stream_packet[22]) && + ((*avb_ctx)->streamid8[5] == (uint8_t) stream_packet[23]) && + ((*avb_ctx)->streamid8[6] == (uint8_t) stream_packet[24]) && + ((*avb_ctx)->streamid8[7] == (uint8_t) stream_packet[25]) + ){ + uint64_t adjust_packet_time_ns = 0; + uint64_t packet_arrival_time_ns = 0; + uint64_t rx_int_to_last_packet_ns = 0; + + // Packet Arrival Time from Device + cmsg = CMSG_FIRSTHDR(&msg); + while( cmsg != NULL ) { + if( cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMPING ) { + struct timespec *ts_device, *ts_system; + ts_system = ((struct timespec *) CMSG_DATA(cmsg)) + 1; + ts_device = ts_system + 1; + packet_arrival_time_ns = (ts_device->tv_sec*1000000000LL + ts_device->tv_nsec); + if( ts_cnt < NUM_TS ) + timestamps[ts_cnt++] = packet_arrival_time_ns; + break; + } + cmsg = CMSG_NXTHDR(&msg,cmsg); + } + + rx_int_to_last_packet_ns = packet_arrival_time_ns - last_packet_time_ns; + last_packet_time_ns = packet_arrival_time_ns; + + if( packet_num == (*avb_ctx)->num_packets -1){ + + adjust_packet_time_ns = (uint64_t) ( ( (float)((*avb_ctx)->period_size % 6 ) / (float)(*avb_ctx)->sample_rate ) * 1000000000LL); + } else { + adjust_packet_time_ns = (*avb_ctx)->adjust ? rx_int_to_last_packet_ns : 125000; + } + return adjust_packet_time_ns -1000; + } + return -1; +} + + + + +uint64_t avtp_mcl_wait_for_rx_ts_const( FILE* filepointer, avb_driver_state_t **avb_ctx, + struct sockaddr_in **si_other_avb, + struct pollfd **avtp_transport_socket_fds, + int packet_num, uint64_t *lateness ) +{ + char stream_packet[BUFLEN]; + +// struct cmsghdr { +// socklen_t cmsg_len; // data byte count, including hdr +// int cmsg_level; // originating protocol +// int cmsg_type; // protocol-specific type +// // followed by unsigned char cmsg_data[]; +// }; +// +// struct msghdr { +// void *msg_name; // optional address +// socklen_t msg_namelen; // size of address +// struct iovec *msg_iov; // scatter/gather array +// size_t msg_iovlen; // # elements in msg_iov +// void *msg_control; // ancillary data, see below +// size_t msg_controllen; // ancillary data buffer len +// int msg_flags; // flags on received message +// }; + + struct msghdr msg; + struct cmsghdr *cmsg; + struct sockaddr_ll remote; + struct iovec sgentry; + struct { + struct cmsghdr cm; + char control[256]; + } control; + + memset( &msg, 0, sizeof( msg )); + msg.msg_iov = &sgentry; + msg.msg_iovlen = 1; + sgentry.iov_base = stream_packet; + sgentry.iov_len = BUFLEN; + + memset( &remote, 0, sizeof(remote)); + msg.msg_name = (caddr_t) &remote; + msg.msg_namelen = sizeof( remote ); + msg.msg_control = &control; + msg.msg_controllen = sizeof(control); + + int status = recvmsg((*avtp_transport_socket_fds)->fd, &msg, 0);//NULL); + + if (status == 0) { + fprintf(filepointer, "EOF\n");fflush(filepointer); + return -1; + } else if (status < 0) { + fprintf(filepointer, "Error recvmsg: %d %d %s\n", status, errno, strerror(errno));fflush(filepointer); + return -1; + } + if( // Compare Stream IDs + ((*avb_ctx)->streamid8[0] == (uint8_t) stream_packet[18]) && + ((*avb_ctx)->streamid8[1] == (uint8_t) stream_packet[19]) && + ((*avb_ctx)->streamid8[2] == (uint8_t) stream_packet[20]) && + ((*avb_ctx)->streamid8[3] == (uint8_t) stream_packet[21]) && + ((*avb_ctx)->streamid8[4] == (uint8_t) stream_packet[22]) && + ((*avb_ctx)->streamid8[5] == (uint8_t) stream_packet[23]) && + ((*avb_ctx)->streamid8[6] == (uint8_t) stream_packet[24]) && + ((*avb_ctx)->streamid8[7] == (uint8_t) stream_packet[25]) + ){ + uint64_t adjust_packet_time_ns = 0; + uint64_t packet_arrival_time_ns = 0; + + // Packet Arrival Time from Device + cmsg = CMSG_FIRSTHDR(&msg); + while( cmsg != NULL ) { + if( cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMPING ) { + struct timespec *ts_device, *ts_system; + ts_system = ((struct timespec *) CMSG_DATA(cmsg)) + 1; + ts_device = ts_system + 1; + packet_arrival_time_ns = (ts_device->tv_sec*1000000000LL + ts_device->tv_nsec); + if( ts_cnt < NUM_TS ) + timestamps[ts_cnt++] = packet_arrival_time_ns; + break; + } + cmsg = CMSG_NXTHDR(&msg,cmsg); + } + + (*lateness) += ( packet_arrival_time_ns - last_packet_time_ns ) - 125000; + last_packet_time_ns = packet_arrival_time_ns; + + if( packet_num == (*avb_ctx)->num_packets -1){ + adjust_packet_time_ns = (uint64_t) ( ( (float)((*avb_ctx)->period_size % 6 ) / (float)(*avb_ctx)->sample_rate ) * 1000000000LL); + } else { + adjust_packet_time_ns = 125000; + } + return adjust_packet_time_ns -1000; + } + return -1; +} + diff --git a/linux/avbmcl/media_clock_listener.h b/linux/avbmcl/media_clock_listener.h new file mode 100644 index 00000000..099687b9 --- /dev/null +++ b/linux/avbmcl/media_clock_listener.h @@ -0,0 +1,49 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#ifndef _MEDIA_CLOCK_LISTENER_H_ +#define _MEDIA_CLOCK_LISTENER_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include "avb_definitions.h" +#include "avb_sockets.h" +#include "mrp_client_interface.h" + +int avtp_mcl_create( FILE* filepointer, avb_driver_state_t **avb_ctx, const char* avb_dev_name, + char* stream_id, char* destination_mac, + struct sockaddr_in **si_other_avb, + struct pollfd **avtp_transport_socket_fds); +void avtp_mcl_delete( FILE* filepointer, avb_driver_state_t **avb_ctx); +uint64_t avtp_mcl_wait_for_rx_ts( FILE* filepointer, avb_driver_state_t **avb_ctx, + struct sockaddr_in **si_other_avb, + struct pollfd **avtp_transport_socket_fds, + int packet_num ); +uint64_t avtp_mcl_wait_for_rx_ts_const( FILE* filepointer, avb_driver_state_t **avb_ctx, + struct sockaddr_in **si_other_avb, + struct pollfd **avtp_transport_socket_fds, + int packet_num, uint64_t *lateness ); +#ifdef __cplusplus +} +#endif + +#endif //_MEDIA_CLOCK_LISTENER_H_ diff --git a/linux/avbmcl/mrp_client_control_socket.c b/linux/avbmcl/mrp_client_control_socket.c new file mode 100644 index 00000000..9354a7fe --- /dev/null +++ b/linux/avbmcl/mrp_client_control_socket.c @@ -0,0 +1,71 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "mrp_client_control_socket.h" + +static int control_socket = -1; + +int mrp_client_get_Control_socket() +{ + return control_socket; +} + +int mrp_client_init_Control_socket( FILE* filepointer ) +{ + + // in POSIX fd 0,1,2 are reserved + // if (2 > (*avb_ctx)->mrp_ctx.control_socket) { + // if (-1 > (*avb_ctx)->mrp_ctx.control_socket) + // close((*avb_ctx)->mrp_ctx.control_socket); + // return RETURN_VALUE_FAILURE; + // } + struct sockaddr_in addr; + int sockopt=0; + + fprintf(filepointer, "Create MRP control socket.\n");fflush(filepointer); + memset((char*)&addr, 0, sizeof(struct sockaddr_in)); + addr.sin_family = AF_INET; + + // Listener... why 0? + addr.sin_port = htons(0); + // addr.sin_port = htons(MRPD_PORT_DEFAULT); + inet_aton("127.0.0.1", &addr.sin_addr); + + if( (control_socket = socket(addr.sin_family, SOCK_DGRAM, IPPROTO_UDP)) < 0 ){ + fprintf(filepointer, "Failed to create socket. %d %s\n", errno, strerror(errno));fflush(filepointer); + fclose(filepointer); + return RETURN_VALUE_FAILURE; + } + + // Allow the socket to be reused - incase connection is closed prematurely + if (setsockopt(control_socket, SOL_SOCKET, SO_REUSEADDR, &sockopt, sizeof( sockopt)) == -1) { + fprintf(filepointer, "setsockopt failed %d %s\n", errno, strerror(errno));fflush(filepointer); + close(control_socket); + fclose(filepointer); + return RETURN_VALUE_FAILURE; + } + + if( bind(control_socket, (struct sockaddr*)&addr, sizeof(addr)) < 0) { + fprintf(filepointer, "Could not bind socket. %d %s\n", errno, strerror(errno));fflush(filepointer); + close(control_socket); + fclose(filepointer); + return RETURN_VALUE_FAILURE; + } else { + return RETURN_VALUE_SUCCESS; + } +} diff --git a/linux/avbmcl/mrp_client_control_socket.h b/linux/avbmcl/mrp_client_control_socket.h new file mode 100644 index 00000000..4efc6610 --- /dev/null +++ b/linux/avbmcl/mrp_client_control_socket.h @@ -0,0 +1,36 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#ifndef _MRP_CONTROL_SOCKET_H_ +#define _MRP_CONTROL_SOCKET_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "avb_definitions.h" + +int mrp_client_get_Control_socket( ); +int mrp_client_init_Control_socket( FILE* filepointer ); + +#ifdef __cplusplus +} +#endif + +#endif //_MRP_CONTROL_SOCKET_H_ diff --git a/linux/avbmcl/mrp_client_interface.c b/linux/avbmcl/mrp_client_interface.c new file mode 100644 index 00000000..fbe64a70 --- /dev/null +++ b/linux/avbmcl/mrp_client_interface.c @@ -0,0 +1,231 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "mrp_client_interface.h" +#include "mrp_client_control_socket.h" +#include "mrp_client_send_msg.h" + +#include + +extern int errno; + +int mrp_client_getDomain(FILE* filepointer, mrp_ctx_t *mrp_ctx) +{ + int ret=0; + char* msgbuf= malloc(1500); + + // we may not get a notification if we are joining late, + //so query for what is already there ... + if (NULL == msgbuf){ + fprintf(filepointer, "failed to create msgbuf. %d %s\n", errno, strerror(errno));fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + + memset(msgbuf, 0, 1500); + sprintf(msgbuf, "S??"); + fprintf(filepointer, "Get Domain %s\n",msgbuf);fflush(filepointer); + ret = mrp_client_send_mrp_msg( filepointer, mrp_client_get_Control_socket(), msgbuf, 1500); + + fprintf(filepointer, "Query SRP Domain: %s\n", msgbuf);fflush(filepointer); + free(msgbuf); + if (ret != 1500){ + fprintf(filepointer, "failed to create socket. %d %s\n", errno, strerror(errno));fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + + while ( (mrp_ctx->domain_a_valid == 0) || (mrp_ctx->domain_b_valid == 0 ) || + (mrp_ctx->domain_class_a_vid == 0) || (mrp_ctx->domain_class_b_vid == 0) ){ + usleep(20000); + } + +// if (mrp_ctx->domain_a_valid > 0) { +// class_a->id = mrp_ctx->domain_class_a_id; +// class_a->priority = mrp_ctx->domain_class_a_priority; +// class_a->vid = mrp_ctx->domain_class_a_vid; +// } +// if (mrp_ctx->domain_b_valid > 0) { +// class_b->id = mrp_ctx->domain_class_b_id; +// class_b->priority = mrp_ctx->domain_class_b_priority; +// class_b->vid = mrp_ctx->domain_class_b_vid; +// } + + return RETURN_VALUE_SUCCESS; +} + +int mrp_client_report_domain_status(FILE* filepointer, mrp_ctx_t *mrp_ctx) +{ + char* msgbuf= malloc(1500); + int rc=0; + + if (NULL == msgbuf){ + fprintf(filepointer, "mrp_listener_report_domain_status - NULL == msgbuf \t LISTENER_FAILED \n");fflush(filepointer); + mrp_ctx->mrp_status = LISTENER_FAILED; + return RETURN_VALUE_FAILURE; + } + memset(msgbuf, 0, 1500); + sprintf(msgbuf, "S+D:C=%d,P=%d,V=%04x", mrp_ctx->domain_class_a_id, mrp_ctx->domain_class_a_priority, mrp_ctx->domain_class_a_vid); + fprintf(filepointer, "Report Domain Status %s\n",msgbuf);fflush(filepointer); + rc = mrp_client_send_mrp_msg( filepointer, mrp_client_get_Control_socket(), msgbuf, 1500); + free(msgbuf); + + if (rc != 1500){ + fprintf(filepointer, "mrp_listener_report_domain_status - rc != 1500 \t LISTENER_FAILED \n");fflush(filepointer); + mrp_ctx->mrp_status = LISTENER_FAILED; + return RETURN_VALUE_FAILURE; + } + fprintf(filepointer, "mrp_listener_report_domain_status\t LISTENER_IDLE \n");fflush(filepointer); + mrp_ctx->mrp_status = LISTENER_IDLE; + return RETURN_VALUE_SUCCESS; +} + +int mrp_client_joinVLAN(FILE* filepointer, mrp_ctx_t *mrp_ctx) +{ + char* msgbuf= malloc(1500); + int rc=0; + + if (NULL == msgbuf) return RETURN_VALUE_FAILURE; + + memset(msgbuf, 0, 1500); + sprintf(msgbuf, "V++:I=%04x",mrp_ctx->domain_class_a_vid); + fprintf(filepointer, "Joing VLAN %s\n",msgbuf);fflush(filepointer); + rc = mrp_client_send_mrp_msg( filepointer, mrp_client_get_Control_socket(), msgbuf, 1500); + free(msgbuf); + + if( rc != 1500){ + fprintf(filepointer, "int mrp_listener_join_vlan - rc != 1500\t LISTENER_FAILED\n");fflush(filepointer); + mrp_ctx->mrp_status = LISTENER_FAILED; + return RETURN_VALUE_FAILURE; + } else { + fprintf(filepointer, "int mrp_listener_join_vlan - rc != 1500\t LISTENER_IDLE\n");fflush(filepointer); + mrp_ctx->mrp_status = LISTENER_IDLE; + return RETURN_VALUE_SUCCESS; + } + return RETURN_VALUE_FAILURE; +} + +int mrp_client_getDomain_joinVLAN(FILE* filepointer, avb_driver_state_t **avb_ctx, mrp_ctx_t *mrp_ctx) +{ + fprintf(filepointer, "calling mrp_get_domain()\n");fflush(filepointer); + + if ( mrp_client_getDomain( filepointer, mrp_ctx) > RETURN_VALUE_FAILURE) { + fprintf(filepointer, "success calling mrp_get_domain()\n");fflush(filepointer); + } else { + fprintf(filepointer, "failed calling mrp_get_domain()\n");fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + + fprintf(filepointer, "detected domain Class A PRIO=%d VID=%04x...\n",mrp_ctx->domain_class_a_priority, + mrp_ctx->domain_class_a_vid);fflush(filepointer); + + if ( mrp_client_report_domain_status( filepointer, mrp_ctx) > RETURN_VALUE_FAILURE ) { + fprintf(filepointer, "report_domain_status success\n");fflush(filepointer); + } else { + fprintf(filepointer, "report_domain_status failed\n");fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + + if ( mrp_client_joinVLAN( filepointer, mrp_ctx) > RETURN_VALUE_FAILURE) { + fprintf(filepointer, "join_vlan success\n");fflush(filepointer); + } else { + fprintf(filepointer, "join_vlan failed\n");fflush(filepointer); + return RETURN_VALUE_FAILURE; + } + return RETURN_VALUE_SUCCESS; +} + +int mrp_client_listener_await_talker(FILE* filepointer, avb_driver_state_t **avb_ctx, mrp_ctx_t *mrp_ctx) +{ + if( mrp_ctx->mrp_status == LISTENER_READY) { + fprintf(filepointer, "Already connected to a talker...\n");fflush(filepointer); + return RETURN_VALUE_FAILURE; + } else { + mrp_ctx->mrp_status = LISTENER_WAITING; + fprintf(filepointer, "Waiting for talker...\n");fflush(filepointer); + fprintf(filepointer, "int mrp_listener_await_talker - rc != 1500\t LISTENER_WAITING\n");fflush(filepointer); + + struct timespec tim, tim2; + tim.tv_sec = 0; + tim.tv_nsec = MILISLEEP_TIME * 5; + while(mrp_ctx->mrp_status == LISTENER_WAITING + && mrp_ctx->mrp_status != LISTENER_READY ){ + nanosleep(&tim , &tim2); + } + + if ( mrp_client_listener_send_ready( filepointer, avb_ctx, mrp_ctx ) > RETURN_VALUE_FAILURE) { + fprintf(filepointer, "send_ready success\n");fflush(filepointer); + return RETURN_VALUE_SUCCESS; + } + } + return RETURN_VALUE_FAILURE; +} + +int mrp_client_listener_send_ready(FILE* filepointer, avb_driver_state_t **avb_ctx, mrp_ctx_t *mrp_ctx) +{ + char *databuf= malloc(1500); + int rc=0; + + if (NULL == databuf) return RETURN_VALUE_FAILURE; + + memset(databuf, 0, 1500); + sprintf(databuf, "S+L:L=%02x%02x%02x%02x%02x%02x%02x%02x,D=2", + (*avb_ctx)->streamid8[0], (*avb_ctx)->streamid8[1], + (*avb_ctx)->streamid8[2], (*avb_ctx)->streamid8[3], + (*avb_ctx)->streamid8[4], (*avb_ctx)->streamid8[5], + (*avb_ctx)->streamid8[6], (*avb_ctx)->streamid8[7]); + rc = mrp_client_send_mrp_msg( filepointer, mrp_client_get_Control_socket(), databuf, 1500); + fprintf(filepointer, "Send Ready %s\n",databuf);fflush(filepointer); + free(databuf); + + if (rc != 1500){ + fprintf(filepointer, "mrp_listener_send_ready - rc != 1500\t LISTENER_FAILED\n");fflush(filepointer); + mrp_ctx->mrp_status = LISTENER_FAILED; + return RETURN_VALUE_FAILURE; + } + fprintf(filepointer, "mrp_listener_send_ready - rc != 1500\t LISTENER_IDLE\n");fflush(filepointer); + return RETURN_VALUE_SUCCESS; +} + + +int mrp_client_listener_send_leave(FILE* filepointer, avb_driver_state_t **avb_ctx, mrp_ctx_t *mrp_ctx) +{ + char *databuf= malloc(1500); + int rc=0; + + if (NULL == databuf){ + return RETURN_VALUE_FAILURE; + } + memset(databuf, 0, 1500); + sprintf(databuf, "S-L:L=%02x%02x%02x%02x%02x%02x%02x%02x,D=3", + (*avb_ctx)->streamid8[0], (*avb_ctx)->streamid8[1], + (*avb_ctx)->streamid8[2], (*avb_ctx)->streamid8[3], + (*avb_ctx)->streamid8[4], (*avb_ctx)->streamid8[5], + (*avb_ctx)->streamid8[6], (*avb_ctx)->streamid8[7]); + rc = mrp_client_send_mrp_msg(filepointer, mrp_client_get_Control_socket(), databuf, 1500); + fprintf(filepointer, "Send Leave %s\n",databuf);fflush(filepointer); + free(databuf); + + if (rc != 1500){ + fprintf(filepointer, "mrp_listener_send_leave - rc != 1500\t LISTENER_FAILED\n");fflush(filepointer); + mrp_ctx->mrp_status = LISTENER_FAILED; + return RETURN_VALUE_FAILURE; + } else { + fprintf(filepointer, "mrp_listener_send_leave - rc != 1500\t LISTENER_IDLE\n");fflush(filepointer); + mrp_ctx->mrp_status = LISTENER_IDLE; + return RETURN_VALUE_SUCCESS; + } +} diff --git a/linux/avbmcl/mrp_client_interface.h b/linux/avbmcl/mrp_client_interface.h new file mode 100644 index 00000000..90f52177 --- /dev/null +++ b/linux/avbmcl/mrp_client_interface.h @@ -0,0 +1,38 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#ifndef _MRP_CLIENT_INTERFACE_H_ +#define _MRP_CLIENT_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "avb_definitions.h" + +int mrp_client_getDomain_joinVLAN(FILE* filepointer, avb_driver_state_t **avb_ctx, mrp_ctx_t *mrp_ctx); +int mrp_client_listener_await_talker(FILE* filepointer,avb_driver_state_t **avb_ctx, mrp_ctx_t *mrp_ctx); +int mrp_client_listener_send_ready(FILE* filepointer,avb_driver_state_t **avb_ctx, mrp_ctx_t *mrp_ctx); +int mrp_client_listener_send_leave(FILE* filepointer,avb_driver_state_t **avb_ctx, mrp_ctx_t *mrp_ctx); + +#ifdef __cplusplus +} +#endif + +#endif //_MRP_CLIENT_INTERFACE_H_ diff --git a/linux/avbmcl/mrp_client_send_msg.c b/linux/avbmcl/mrp_client_send_msg.c new file mode 100644 index 00000000..a711c3d9 --- /dev/null +++ b/linux/avbmcl/mrp_client_send_msg.c @@ -0,0 +1,38 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#include "mrp_client_send_msg.h" +#include "mrp_client_control_socket.h" + +int mrp_client_send_mrp_msg(FILE* filepointer, int control_socket, char *notify_data, int notify_len) +{ + struct sockaddr_in addr; + + if ( control_socket == -1 ) + return RETURN_VALUE_FAILURE; + if (notify_data == NULL) + return RETURN_VALUE_FAILURE; + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_port = htons(MRPD_PORT_DEFAULT); + addr.sin_addr.s_addr = inet_addr("127.0.0.1"); + inet_aton("127.0.0.1", &addr.sin_addr); + + return sendto( control_socket, notify_data, notify_len, 0, (struct sockaddr*)&addr, (socklen_t)sizeof(addr)); +} diff --git a/linux/avbmcl/mrp_client_send_msg.h b/linux/avbmcl/mrp_client_send_msg.h new file mode 100644 index 00000000..dc787990 --- /dev/null +++ b/linux/avbmcl/mrp_client_send_msg.h @@ -0,0 +1,35 @@ +/* +Copyright (C) 2016-2019 Christoph Kuhr + +This program is free software; you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation; either version 2 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program; if not, write to the Free Software +Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +*/ + +#ifndef _MRP_CLIENT_SEND_MSG_H_ +#define _MRP_CLIENT_SEND_MSG_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "avb_definitions.h" + +int mrp_client_send_mrp_msg(FILE* filepointer, int control_socket, char *notify_data, int notify_len); + +#ifdef __cplusplus +} +#endif + +#endif // _MRP_CLIENT_SEND_MSG_H_ diff --git a/wscript b/wscript index 86eb3954..6bb63f10 100644 --- a/wscript +++ b/wscript @@ -163,7 +163,12 @@ def options(opt): winmme.check( header_name=['windows.h', 'mmsystem.h'], msg='Checking for header mmsystem.h') - + avbmcl = opt.add_auto_option( + 'avbmcl', + help='Enable AVB Media Clock Listener driver', + conf_dest='BUILD_DRIVER_AVBMCL') + avbmcl.check(header_name='linux/if_packet.h') + avbmcl.check(lib='igb') celt = opt.add_auto_option( 'celt', help='Build with CELT') @@ -609,6 +614,17 @@ def create_driver_obj(bld, **kw): def build_drivers(bld): # Non-hardware driver sources. Lexically sorted. + + avbmcl_src = [ + 'linux/avbmcl/JackAVBDriver.cpp', + 'linux/avbmcl/avb.c', + 'linux/avbmcl/avb_sockets.c', + 'linux/avbmcl/media_clock_listener.c', + 'linux/avbmcl/mrp_client_control_socket.c', + 'linux/avbmcl/mrp_client_interface.c', + 'linux/avbmcl/mrp_client_send_msg.c' + ] + dummy_src = [ 'common/JackDummyDriver.cpp' ] @@ -712,6 +728,13 @@ def build_drivers(bld): ] # Create non-hardware driver objects. Lexically sorted. + + if bld.env['BUILD_DRIVER_AVBMCL']: + create_driver_obj( + bld, + target = 'avbmcl', + source = avbmcl_src) + create_driver_obj( bld, target='dummy',