From c8b0db93236f7b7730c4ae62d92fac3fc3aaa99f Mon Sep 17 00:00:00 2001 From: Christoph Kuhr Date: Thu, 4 Apr 2019 17:56:27 +0200 Subject: [PATCH] manual merge with develop branch --- .gitmodules | 3 + linux/avb/JackAVBDriver.cpp | 431 +++++++++++++++++++++ linux/avb/JackAVBDriver.h | 66 ++++ linux/avb/OpenAvnu | 1 + linux/avb/avb.c | 514 ++++++++++++++++++++++++++ linux/avb/avb.h | 71 ++++ linux/avb/avb_definitions.h | 156 ++++++++ linux/avb/avb_sockets.c | 149 ++++++++ linux/avb/avb_sockets.h | 72 ++++ linux/avb/media_clock_listener.c | 185 +++++++++ linux/avb/media_clock_listener.h | 68 ++++ linux/avb/mrp_client_control_socket.c | 77 ++++ linux/avb/mrp_client_control_socket.h | 51 +++ linux/avb/mrp_client_interface.c | 265 +++++++++++++ linux/avb/mrp_client_interface.h | 54 +++ linux/avb/mrp_client_send_msg.c | 37 ++ linux/avb/mrp_client_send_msg.h | 49 +++ linux/avb/start_jackd_avtp_backend.sh | 26 ++ wscript | 37 ++ 19 files changed, 2312 insertions(+) create mode 100644 .gitmodules create mode 100644 linux/avb/JackAVBDriver.cpp create mode 100644 linux/avb/JackAVBDriver.h create mode 160000 linux/avb/OpenAvnu create mode 100644 linux/avb/avb.c create mode 100644 linux/avb/avb.h create mode 100644 linux/avb/avb_definitions.h create mode 100644 linux/avb/avb_sockets.c create mode 100644 linux/avb/avb_sockets.h create mode 100644 linux/avb/media_clock_listener.c create mode 100644 linux/avb/media_clock_listener.h create mode 100644 linux/avb/mrp_client_control_socket.c create mode 100644 linux/avb/mrp_client_control_socket.h create mode 100644 linux/avb/mrp_client_interface.c create mode 100644 linux/avb/mrp_client_interface.h create mode 100644 linux/avb/mrp_client_send_msg.c create mode 100644 linux/avb/mrp_client_send_msg.h create mode 100644 linux/avb/start_jackd_avtp_backend.sh diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..1b211cfb --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "linux/avb/OpenAvnu"] + path = linux/avb/OpenAvnu + url = https://github.com/chris-kuhr/OpenAvnu.git diff --git a/linux/avb/JackAVBDriver.cpp b/linux/avb/JackAVBDriver.cpp new file mode 100644 index 00000000..df9bbb53 --- /dev/null +++ b/linux/avb/JackAVBDriver.cpp @@ -0,0 +1,431 @@ +/* +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; + + + +/* + * "enp4s0" + */ +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 + + 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. +} + +//open, close, attach and detach------------------------------------------------------ + +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; +} + +//init and restart-------------------------------------------------------------------- +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; + } + + //monitor + //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; +} + + +//jack ports and buffers-------------------------------------------------------------- + +//driver processes-------------------------------------------------------------------- + +int JackAVBDriver::Read() +{ + int ret = 0; + JSList *node = avb_ctx.capture_ports; + + + uint64_t cumulative_rx_int_ns = 0; + int n = 0; + for(n=0; n avb_ctx.period_usecs) { + ret = 1; + NotifyXRun(fBeginDateUst, cumulative_rx_int_us); + jack_error("netxruns... duration: %fms", cumulative_rx_int_us / 1000); + } + + 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; +} + +//driver loader----------------------------------------------------------------------- + +#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); + + for(int 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/avb/JackAVBDriver.h b/linux/avb/JackAVBDriver.h new file mode 100644 index 00000000..4d60233d --- /dev/null +++ b/linux/avb/JackAVBDriver.h @@ -0,0 +1,66 @@ +/* +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 __JackNetDriver__ +#define __JackNetDriver__ + +#include "JackTimedDriver.h" +#include "avb.h" + + +namespace Jack +{ +/** +\Brief This class describes the Net Backend +*/ + +class JackAVBDriver : public JackWaiterDriver +{ + private: + + avb_driver_state_t avb_ctx; + int num_packets_even_odd; + + 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 diff --git a/linux/avb/OpenAvnu b/linux/avb/OpenAvnu new file mode 160000 index 00000000..985f3e38 --- /dev/null +++ b/linux/avb/OpenAvnu @@ -0,0 +1 @@ +Subproject commit 985f3e382e0eecd74b269a7f1d52ce0719914489 diff --git a/linux/avb/avb.c b/linux/avb/avb.c new file mode 100644 index 00000000..5fc4db71 --- /dev/null +++ b/linux/avb/avb.c @@ -0,0 +1,514 @@ +/* +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" + + +#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; + + fprintf(filepointer2, "Event on Stream Id: ");fflush(filepointer2); + for(int 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(int 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; + + fprintf(filepointer2, "Check destination MAC... ");fflush(filepointer2); + for(int 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) +{ + /* + * 1st character indicates application + * [MVS] - MAC, VLAN or STREAM + */ + 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); + 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); + // fclose(fp); + + 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; + + 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); + + 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); + + if (rc != 1500) + return RETURN_VALUE_FAILURE; + else + return RETURN_VALUE_SUCCESS; + + free(msgbuf); + fclose(filepointer2); + mrp_shm_close(0); + + 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 /*EXIT_FAILURE*/ -1; + } + + if(RETURN_VALUE_FAILURE == mrp_shm_open( 1 ) ){ + return RETURN_VALUE_FAILURE; + } + + if( mrp_client_init_Control_socket( filepointer ) == RETURN_VALUE_FAILURE ) { + fprintf(filepointer, "Error initializing MRP socket\n");fflush(filepointer); + fclose(filepointer); + return /*EXIT_FAILURE*/ -1; + } + + 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 /*EXIT_FAILURE*/ -1; + } 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 /*EXIT_FAILURE*/ -1; + } + + 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 /*EXIT_FAILURE*/ -1; + } + + return 0; +} + +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 /*EXIT_FAILURE*/ -1; + } else { + return 0; + } +} + +uint64_t await_avtp_rx_ts( avb_driver_state_t *avb_ctx, int packet_num ) +{ + return avtp_mcl_wait_for_rx_ts( filepointer, &avb_ctx, &si_other_avb, &avtp_transport_socket_fds, packet_num ); +} + + +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; +} diff --git a/linux/avb/avb.h b/linux/avb/avb.h new file mode 100644 index 00000000..0242bc3c --- /dev/null +++ b/linux/avb/avb.h @@ -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. +*/ +#ifndef JACK1722DRIVER_H +#define JACK1722DRIVER_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // Needed for mlockall() +#include // needed for getrusage +#include // needed for getrusage +#include // needed for sysconf(int name); +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "avb_definitions.h" +#include "media_clock_listener.h" +#include "mrp_client_control_socket.h" +#include "mrp_client_send_msg.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 ); +int shutdown_avb_driver( avb_driver_state_t *avb_ctx); + + +#ifdef __cplusplus +} +#endif +#endif //JACK1722DRIVER_H diff --git a/linux/avb/avb_definitions.h b/linux/avb/avb_definitions.h new file mode 100644 index 00000000..56730399 --- /dev/null +++ b/linux/avb/avb_definitions.h @@ -0,0 +1,156 @@ +/* +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 SRC_GLOBAL_DEFINITIONS_H_ +#define SRC_GLOBAL_DEFINITIONS_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + + +#define _GNU_SOURCE + +#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" +#include "OpenAvnu/daemons/mrpd/msrp.h" // spurious dep daemons/mrpd/msrp.h:50:#define MSRP_LISTENER_ASKFAILED + + +#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 /* SRC_GLOBAL_DEFINITIONS_H_ */ diff --git a/linux/avb/avb_sockets.c b/linux/avb/avb_sockets.c new file mode 100644 index 00000000..94d63658 --- /dev/null +++ b/linux/avb/avb_sockets.c @@ -0,0 +1,149 @@ +/* +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; + +// 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*/ + + 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/avb/avb_sockets.h b/linux/avb/avb_sockets.h new file mode 100644 index 00000000..1deb9ca0 --- /dev/null +++ b/linux/avb/avb_sockets.h @@ -0,0 +1,72 @@ +/* +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 AVBSOCKET_H_ +#define AVBSOCKET_H_ + + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include +#include +#include +#include + + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include //Provides declarations for udp header +#include //Provides declarations for udp header +#include +#include +#include +#include +#include +#include +#include +//#include +#include +#include + +#include +#include + + +#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 diff --git a/linux/avb/media_clock_listener.c b/linux/avb/media_clock_listener.c new file mode 100644 index 00000000..eca74371 --- /dev/null +++ b/linux/avb/media_clock_listener.c @@ -0,0 +1,185 @@ +/* +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" +extern int errno; + +static uint64_t last_packet_time_ns = 0; + +#define NUM_TS 10000000 +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; + + if( ! (filepointer2 = fopen("mcs_ts.log", "w")) ){ + printf("Error Opening file %d\n", errno); + return; + } + + for(int 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; + +} diff --git a/linux/avb/media_clock_listener.h b/linux/avb/media_clock_listener.h new file mode 100644 index 00000000..9d365922 --- /dev/null +++ b/linux/avb/media_clock_listener.h @@ -0,0 +1,68 @@ +/* +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 MEDIACLOCKLISTENER_H +#define MEDIACLOCKLISTENER_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "avb_definitions.h" +#include "avb_sockets.h" +#include "mrp_client_interface.h" +#include "mrp_client_control_socket.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 ); + +#ifdef __cplusplus +} +#endif +#endif //MEDIACLOCKLISTENER_H diff --git a/linux/avb/mrp_client_control_socket.c b/linux/avb/mrp_client_control_socket.c new file mode 100644 index 00000000..a9637336 --- /dev/null +++ b/linux/avb/mrp_client_control_socket.c @@ -0,0 +1,77 @@ +/* +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/avb/mrp_client_control_socket.h b/linux/avb/mrp_client_control_socket.h new file mode 100644 index 00000000..1319e0b9 --- /dev/null +++ b/linux/avb/mrp_client_control_socket.h @@ -0,0 +1,51 @@ +/* +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 +#include +#include +#include +#include // needed for sysconf(int name); +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "avb_definitions.h" +#include "mrp_client_send_msg.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/avb/mrp_client_interface.c b/linux/avb/mrp_client_interface.c new file mode 100644 index 00000000..eafd9767 --- /dev/null +++ b/linux/avb/mrp_client_interface.c @@ -0,0 +1,265 @@ +/* +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" + + +extern int errno; + + +int mrp_client_getDomain(FILE* filepointer, mrp_ctx_t *mrp_ctx) +{ + int ret=0; + + /* + * we may not get a notification if we are joining late, + * so query for what is already there ... + */ + char* msgbuf= malloc(1500); + 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/avb/mrp_client_interface.h b/linux/avb/mrp_client_interface.h new file mode 100644 index 00000000..2b78835d --- /dev/null +++ b/linux/avb/mrp_client_interface.h @@ -0,0 +1,54 @@ +/* +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_CL_IF_H_ +#define _MRP_CL_IF_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include +#include +#include // needed for sysconf(int name); +#include +#include +#include +#include +#include +#include +#include + +#include "avb_definitions.h" +#include "mrp_client_control_socket.h" +#include "mrp_client_send_msg.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_CL_IF_H_ */ diff --git a/linux/avb/mrp_client_send_msg.c b/linux/avb/mrp_client_send_msg.c new file mode 100644 index 00000000..7af7d9ff --- /dev/null +++ b/linux/avb/mrp_client_send_msg.c @@ -0,0 +1,37 @@ +/* +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" + +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/avb/mrp_client_send_msg.h b/linux/avb/mrp_client_send_msg.h new file mode 100644 index 00000000..7defdb98 --- /dev/null +++ b/linux/avb/mrp_client_send_msg.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 _MRP_SEND_MSG_H_ +#define _MRP_SEND_MSG_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include +#include +#include // needed for sysconf(int name); +#include +#include +#include +#include +#include +#include +#include + + +#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_SEND_MSG_H_ */ diff --git a/linux/avb/start_jackd_avtp_backend.sh b/linux/avb/start_jackd_avtp_backend.sh new file mode 100644 index 00000000..79ca857c --- /dev/null +++ b/linux/avb/start_jackd_avtp_backend.sh @@ -0,0 +1,26 @@ +#!/bin/sh + +# +# Choose AVB backend: +# -davb +# +# 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 + +jackd -R -P8 -davb --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/wscript b/wscript index 442dd200..c00960ff 100644 --- a/wscript +++ b/wscript @@ -538,6 +538,30 @@ def build_drivers(bld): 'common/netjack_packet.c' ] + + + + + + + + avb_src = [ + 'linux/avb/JackAVBDriver.cpp', + 'linux/avb/avb.c', + 'linux/avb/avb_sockets.c', + 'linux/avb/media_clock_listener.c', + 'linux/avb/mrp_client_control_socket.c', + 'linux/avb/mrp_client_interface.c', + 'linux/avb/mrp_client_send_msg.c' + ] + + + + + + + + proxy_src = [ 'common/JackProxyDriver.cpp' ] @@ -639,6 +663,19 @@ def build_drivers(bld): source = netone_src, use = ['SAMPLERATE', 'CELT']) + + + + + create_driver_obj( + bld, + target = 'avb', + source = avb_src) + + + + + create_driver_obj( bld, target = 'proxy',