diff --git a/meson.build b/meson.build index 9c505e82..acb9cd58 100644 --- a/meson.build +++ b/meson.build @@ -31,6 +31,7 @@ openrtx_src = ['openrtx/src/core/state.c', 'openrtx/src/core/dsp.cpp', 'openrtx/src/core/cps.c', 'openrtx/src/core/crc.c', + 'openrtx/src/core/audio_codec.c', 'openrtx/src/core/data_conversion.c', 'openrtx/src/core/memory_profiling.cpp', 'openrtx/src/ui/ui.c', diff --git a/openrtx/include/core/audio_codec.h b/openrtx/include/core/audio_codec.h new file mode 100644 index 00000000..acb67445 --- /dev/null +++ b/openrtx/include/core/audio_codec.h @@ -0,0 +1,97 @@ +/*************************************************************************** + * Copyright (C) 2022 by Federico Amedeo Izzo IU2NUO, * + * Niccolò Izzo IU2KIN * + * Frederik Saraci IU2NRO * + * Silvano Seva IU2KWO * + * * + * 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 3 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, see * + ***************************************************************************/ + +#ifndef AUDIO_CODEC_H +#define AUDIO_CODEC_H + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Initialise audio codec manager, allocating data buffers. + */ +void codec_init(); + +/** + * Shutdown audio codec manager and deallocate data buffers. + */ +void codec_terminate(); + +/** + * Start encoding of audio data from a given audio source. + * Only an encoding or decoding operation at a time is possible: in case there + * is already an operation in progress, this function returns false. + * + * @param source: audio source for encoding. + * @return true on success, false on failure. + */ +bool codec_startEncode(const enum AudioSource source); + +/** + * Start dencoding of audio data sending the uncompressed samples to a given + * audio destination. + * Only an encoding or decoding operation at a time is possible: in case there + * is already an operation in progress, this function returns false. + * + * @param destination: destination for decoded audio. + * @return true on success, false on failure. + */ +bool codec_startDecode(const enum AudioSink destination); + +/** + * Stop an ongoing encoding or decoding operation. + */ +void codec_stop(); + +/** + * Get a compressed audio frame from the internal queue. Each frame is composed + * of 8 bytes. + * + * @param frame: pointer to a destination buffer where to put the encoded frame. + * @param blocking: if true the execution flow will be blocked whenever the + * internal buffer is empty and resumed as soon as an encoded frame is available. + * @return true on success, false if there is no encoding operation ongoing or + * the queue is empty and the function is nonblocking. + */ +bool codec_popFrame(uint8_t *frame, const bool blocking); + +/** + * Push a a compressed audio frame to the internal queue for decoding. + * Each frame is composed of 8 bytes. + * + * @param frame: frame to be pushed to the queue. + * @param blocking: if true the execution flow will be blocked whenever the + * internal buffer is full and resumed as soon as space for an encoded frame is + * available. + * @return true on success, false if there is no decoding operation ongoing or + * the queue is full and the function is nonblocking. + */ +bool codec_pushFrame(const uint8_t *frame, const bool blocking); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/openrtx/include/rtx/OpMode_M17.h b/openrtx/include/rtx/OpMode_M17.h index b9dd4ca6..a1aa6156 100644 --- a/openrtx/include/rtx/OpMode_M17.h +++ b/openrtx/include/rtx/OpMode_M17.h @@ -1,8 +1,8 @@ /*************************************************************************** - * Copyright (C) 2021 by Federico Amedeo Izzo IU2NUO, * - * Niccolò Izzo IU2KIN * - * Frederik Saraci IU2NRO * - * Silvano Seva IU2KWO * + * Copyright (C) 2021 - 2022 by Federico Amedeo Izzo IU2NUO, * + * Niccolò Izzo IU2KIN * + * Frederik Saraci IU2NRO * + * Silvano Seva IU2KWO * * * * 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 * @@ -24,7 +24,6 @@ #include #include #include -#include #include "OpMode.h" /** @@ -104,17 +103,7 @@ private: */ void sendData(const bool lastFrame = false); - /** - * Start CODEC2 thread. - */ - void startCodec(); - - /** - * Stop CODEC2 thread. - */ - void stopCodec(); - - bool enterRx; ///< Flag for RX management. + bool enterRx; ///< Flag for RX management. M17::M17Modulator modulator; ///< M17 modulator. M17::M17Demodulator demodulator; ///< M17 demodulator. M17::M17Transmitter m17Tx; ///< M17 transmission manager. diff --git a/openrtx/src/core/audio_codec.c b/openrtx/src/core/audio_codec.c new file mode 100644 index 00000000..0566087d --- /dev/null +++ b/openrtx/src/core/audio_codec.c @@ -0,0 +1,241 @@ +/*************************************************************************** + * Copyright (C) 2022 by Federico Amedeo Izzo IU2NUO, * + * Niccolò Izzo IU2KIN * + * Frederik Saraci IU2NRO * + * Silvano Seva IU2KWO * + * * + * 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 3 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, see * + ***************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#define BUF_SIZE 4 + +static struct CODEC2 *codec2; +static stream_sample_t *audioBuf; +static streamId audioStream; + +static bool running; +static pthread_t codecThread; +static pthread_mutex_t mutex; +static pthread_cond_t not_empty; + +static uint8_t readPos; +static uint8_t writePos; +static uint8_t numElements; +static uint64_t dataBuffer[BUF_SIZE]; + +static void *encodeFunc(void *arg); +static void *decodeFunc(void *arg); +static void startThread(void *(*func) (void *)); + + +void codec_init() +{ + running = false; + readPos = 0; + writePos = 0; + numElements = 0; + memset(dataBuffer, 0x00, BUF_SIZE * sizeof(uint64_t)); + + audioBuf = ((stream_sample_t *) malloc(320 * sizeof(stream_sample_t))); + + pthread_mutex_init(&mutex, NULL); + pthread_cond_init(¬_empty, NULL); +} + +void codec_terminate() +{ + if(running) codec_stop(); + + pthread_mutex_destroy(&mutex); + pthread_cond_destroy(¬_empty); + + if(audioBuf != NULL) free(audioBuf); +} + +bool codec_startEncode(const enum AudioSource source) +{ + if(running) return false; + if(audioBuf == NULL) return false; + + audioStream = inputStream_start(source, PRIO_TX, audioBuf, 320, + BUF_CIRC_DOUBLE, 8000); + + if(audioStream == -1) return false; + + readPos = 0; + writePos = 0; + numElements = 0; + running = true; + startThread(encodeFunc); + + return true; +} + +bool codec_startDecode(const enum AudioSink destination) +{ + if(running) return false; + if(audioBuf == NULL) return false; + + audioStream = outputStream_start(destination, PRIO_RX, audioBuf, 320, + BUF_CIRC_DOUBLE, 8000); + + if(audioStream == -1) return false; + + readPos = 0; + writePos = 0; + numElements = 0; + running = true; + startThread(decodeFunc); + + return true; +} + +void codec_stop() +{ + running = false; + pthread_join(codecThread, NULL); +} + +bool codec_popFrame(uint8_t *frame, const bool blocking) +{ + if(running == false) return false; + + uint64_t element; + pthread_mutex_lock(&mutex); + + if(numElements == 0) + { + if(blocking) + { + while(numElements == 0) + { + pthread_cond_wait(¬_empty, &mutex); + } + } + else + { + pthread_mutex_unlock(&mutex); + return false; + } + } + + element = dataBuffer[readPos]; + readPos = (readPos + 1) % BUF_SIZE; + numElements -= 1; + pthread_mutex_unlock(&mutex); + + // Do memcpy after mutex unlock to reduce time inside the critical section + memcpy(frame, &element, 8); + + return true; +} + +bool codec_pushFrame(const uint8_t *frame, const bool blocking) +{ + (void) frame; + (void) blocking; + + return false; +} + + + + +static void *encodeFunc(void *arg) +{ + (void) arg; + + codec2 = codec2_create(CODEC2_MODE_3200); + + while(running) + { + dataBlock_t audio = inputStream_getData(audioStream); + + if(audio.data != NULL) + { + #if defined(PLATFORM_MD3x0) || defined(PLATFORM_MDUV3x0) + // Pre-amplification stage + for(size_t i = 0; i < audio.len; i++) audio.data[i] <<= 3; + + // DC removal + dsp_dcRemoval(audio.data, audio.len); + + // Post-amplification stage + for(size_t i = 0; i < audio.len; i++) audio.data[i] *= 4; + #endif + + // CODEC2 encodes 160ms of speech into 8 bytes: here we write the + // new encoded data into a buffer of 16 bytes writing the first + // half and then the second one, sequentially. + // Data ready flag is rised once all the 16 bytes contain new data. + uint64_t frame = 0; + codec2_encode(codec2, ((uint8_t*) &frame), audio.data); + + pthread_mutex_lock(&mutex); + + // If buffer is full erase the oldest frame + if(numElements >= BUF_SIZE) + { + readPos = (readPos + 1) % BUF_SIZE; + } + + dataBuffer[writePos] = frame; + writePos = (writePos + 1) % BUF_SIZE; + + if(numElements == 0) pthread_cond_signal(¬_empty); + if(numElements < BUF_SIZE) numElements += 1; + + pthread_mutex_unlock(&mutex); + } + } + + inputStream_stop(audioStream); + codec2_destroy(codec2); + + return NULL; +} + +static void *decodeFunc(void *arg) +{ + (void) arg; + + running = false; + return NULL; +} + +static void startThread(void *(*func) (void *)) +{ + pthread_attr_t codecAttr; + pthread_attr_init(&codecAttr); + pthread_attr_setstacksize(&codecAttr, 16384); + + #ifdef _MIOSIX + // Set priority of CODEC2 thread to the maximum one, the same of RTX thread. + struct sched_param param; + param.sched_priority = sched_get_priority_max(0); + pthread_attr_setschedparam(&codecAttr, ¶m); + #endif + + pthread_create(&codecThread, &codecAttr, func, NULL); +} diff --git a/openrtx/src/protocols/M17/M17Demodulator.cpp b/openrtx/src/protocols/M17/M17Demodulator.cpp index 6bbddb60..850238d8 100644 --- a/openrtx/src/protocols/M17/M17Demodulator.cpp +++ b/openrtx/src/protocols/M17/M17Demodulator.cpp @@ -191,13 +191,14 @@ sync_t M17Demodulator::nextFrameSync(int32_t offset) int32_t conv = convolution(i, stream_syncword, M17_SYNCWORD_SYMBOLS); updateCorrelationStats(conv); updateQuantizationStats(i); + + #ifdef PLATFORM_LINUX int16_t sample = 0; if (i < 0) // When we are at negative offsets use bridge buffer sample = basebandBridge[M17_BRIDGE_SIZE + i]; else // Otherwise use regular data buffer sample = baseband.data[i]; - #ifdef PLATFORM_LINUX fprintf(csv_log, "%" PRId16 ",%d,%f,%d\n", sample, conv - static_cast< int32_t >(conv_ema), diff --git a/openrtx/src/rtx/OpMode_M17.cpp b/openrtx/src/rtx/OpMode_M17.cpp index ad001abd..6bb46a37 100644 --- a/openrtx/src/rtx/OpMode_M17.cpp +++ b/openrtx/src/rtx/OpMode_M17.cpp @@ -1,8 +1,8 @@ /*************************************************************************** - * Copyright (C) 2021 by Federico Amedeo Izzo IU2NUO, * - * Niccolò Izzo IU2KIN * - * Frederik Saraci IU2NRO * - * Silvano Seva IU2KWO * + * Copyright (C) 2021 - 2022 by Federico Amedeo Izzo IU2NUO, * + * Niccolò Izzo IU2KIN * + * Frederik Saraci IU2NRO * + * Silvano Seva IU2KWO * * * * 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 * @@ -18,89 +18,16 @@ * along with this program; if not, see * ***************************************************************************/ -#include -#include #include #include #include #include #include -#include -#include -#include +#include #include -#include using namespace std; -pthread_t codecThread; // Thread running CODEC2 -pthread_mutex_t codecMtx; // Mutex for shared access between codec and rtx threads -pthread_cond_t codecCv; // Condition variable for data ready -bool runCodec; // Flag signalling that codec is running -bool newData; // Flag signalling that new data is available -array< uint8_t, 16 > encodedData; // Buffer for encoded data - -void *threadFunc(void *arg) -{ - (void) arg; - - struct CODEC2 *codec2 = codec2_create(CODEC2_MODE_3200); - unique_ptr< stream_sample_t > audioBuf(new stream_sample_t[320]); - streamId micId = inputStream_start(SOURCE_MIC, PRIO_TX, - audioBuf.get(), 320, - BUF_CIRC_DOUBLE, 8000); - - size_t pos = 0; - - while(runCodec) - { - dataBlock_t audio = inputStream_getData(micId); - - if(audio.data != NULL) - { - #if defined(PLATFORM_MD3x0) || defined(PLATFORM_MDUV3x0) - // Pre-amplification stage - for(size_t i = 0; i < audio.len; i++) audio.data[i] <<= 3; - - // DC removal - dsp_dcRemoval(audio.data, audio.len); - - // Post-amplification stage - for(size_t i = 0; i < audio.len; i++) audio.data[i] *= 4; - #endif - - // CODEC2 encodes 160ms of speech into 8 bytes: here we write the - // new encoded data into a buffer of 16 bytes writing the first - // half and then the second one, sequentially. - // Data ready flag is rised once all the 16 bytes contain new data. - uint8_t *curPos = encodedData.data() + 8*pos; - codec2_encode(codec2, curPos, audio.data); - pos++; - if(pos >= 2) - { - pthread_mutex_lock(&codecMtx); - newData = true; - pthread_cond_signal(&codecCv); - pthread_mutex_unlock(&codecMtx); - pos = 0; - } - } - } - - // Unlock waiting thread(s) - pthread_mutex_lock(&codecMtx); - newData = true; - pthread_cond_signal(&codecCv); - pthread_mutex_unlock(&codecMtx); - - // Tear down codec and input stream - codec2_destroy(codec2); - inputStream_stop(micId); - - return NULL; -} - - OpMode_M17::OpMode_M17() : enterRx(true), m17Tx(modulator) { @@ -113,9 +40,7 @@ OpMode_M17::~OpMode_M17() void OpMode_M17::enable() { - pthread_mutex_init(&codecMtx, NULL); - pthread_cond_init(&codecCv, NULL); - + codec_init(); modulator.init(); demodulator.init(); enterRx = true; @@ -123,11 +48,8 @@ void OpMode_M17::enable() void OpMode_M17::disable() { - stopCodec(); - pthread_mutex_destroy(&codecMtx); - pthread_cond_destroy(&codecCv); - enterRx = false; + codec_terminate(); modulator.terminate(); demodulator.terminate(); } @@ -164,7 +86,7 @@ void OpMode_M17::update(rtxStatus_t *const status, const bool newCfg) audio_enableMic(); radio_enableTx(); - startCodec(); + codec_startEncode(SOURCE_MIC); std::string source_address(status->source_address); std::string destination_address(status->destination_address); @@ -187,7 +109,7 @@ void OpMode_M17::update(rtxStatus_t *const status, const bool newCfg) audio_disableMic(); radio_disableRtx(); - stopCodec(); + codec_stop(); status->opStatus = OFF; enterRx = true; @@ -217,38 +139,8 @@ void OpMode_M17::sendData(bool lastFrame) payload_t dataFrame; // Wait until there are 16 bytes of compressed speech, then send them - pthread_mutex_lock(&codecMtx); - while(newData == false) - { - pthread_cond_wait(&codecCv, &codecMtx); - } - newData = false; - pthread_mutex_unlock(&codecMtx); + codec_popFrame(dataFrame.data(), true); + codec_popFrame(dataFrame.data() + 8, true); - std::copy(encodedData.begin(), encodedData.end(), dataFrame.begin()); m17Tx.send(dataFrame, lastFrame); } - -void OpMode_M17::startCodec() -{ - runCodec = true; - newData = false; - - pthread_attr_t codecAttr; - pthread_attr_init(&codecAttr); - pthread_attr_setstacksize(&codecAttr, 16384); - #ifdef _MIOSIX - // Set priority of CODEC2 thread to the maximum one, the same of RTX thread. - struct sched_param param; - param.sched_priority = sched_get_priority_max(0); - pthread_attr_setschedparam(&codecAttr, ¶m); - #endif - pthread_create(&codecThread, &codecAttr, threadFunc, NULL); -} - -void OpMode_M17::stopCodec() -{ - // Shut down CODEC2 thread and wait until it effectively stops - runCodec = false; - pthread_join(codecThread, NULL); -}