2022-04-01 07:21:27 +00:00
|
|
|
/***************************************************************************
|
|
|
|
* 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 <http://www.gnu.org/licenses/> *
|
|
|
|
***************************************************************************/
|
|
|
|
|
|
|
|
#include <interfaces/audio_stream.h>
|
|
|
|
#include <audio_codec.h>
|
|
|
|
#include <pthread.h>
|
|
|
|
#include <codec2.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <string.h>
|
|
|
|
#include <dsp.h>
|
|
|
|
|
|
|
|
#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;
|
2022-04-04 16:34:46 +00:00
|
|
|
static pthread_cond_t not_full;
|
2022-04-01 07:21:27 +00:00
|
|
|
|
|
|
|
static uint8_t readPos;
|
|
|
|
static uint8_t writePos;
|
|
|
|
static uint8_t numElements;
|
|
|
|
static uint64_t dataBuffer[BUF_SIZE];
|
|
|
|
|
2022-04-02 08:16:32 +00:00
|
|
|
#ifdef PLATFORM_MOD17
|
|
|
|
static const uint8_t micGainPre = 4;
|
2022-04-02 12:40:35 +00:00
|
|
|
static const uint8_t micGainPost = 3;
|
2022-04-02 08:16:32 +00:00
|
|
|
#else
|
|
|
|
static const uint8_t micGainPre = 8;
|
|
|
|
static const uint8_t micGainPost = 4;
|
|
|
|
#endif
|
|
|
|
|
2022-04-01 07:21:27 +00:00
|
|
|
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);
|
2022-04-04 16:34:46 +00:00
|
|
|
pthread_cond_init(¬_full, NULL);
|
2022-04-01 07:21:27 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
void codec_terminate()
|
|
|
|
{
|
|
|
|
if(running) codec_stop();
|
|
|
|
|
|
|
|
pthread_mutex_destroy(&mutex);
|
|
|
|
pthread_cond_destroy(¬_empty);
|
2022-04-04 16:34:46 +00:00
|
|
|
pthread_cond_destroy(¬_full);
|
2022-04-01 07:21:27 +00:00
|
|
|
|
|
|
|
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;
|
|
|
|
|
2022-04-04 16:34:46 +00:00
|
|
|
memset(audioBuf, 0x00, 320 * sizeof(stream_sample_t));
|
2022-04-01 07:21:27 +00:00
|
|
|
audioStream = outputStream_start(destination, PRIO_RX, audioBuf, 320,
|
2022-04-04 16:34:46 +00:00
|
|
|
BUF_CIRC_DOUBLE, 8000);
|
2022-04-01 07:21:27 +00:00
|
|
|
|
|
|
|
if(audioStream == -1) return false;
|
|
|
|
|
|
|
|
readPos = 0;
|
|
|
|
writePos = 0;
|
|
|
|
numElements = 0;
|
|
|
|
running = true;
|
|
|
|
startThread(decodeFunc);
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
void codec_stop()
|
|
|
|
{
|
2022-04-30 06:53:31 +00:00
|
|
|
if(running == false) return;
|
|
|
|
|
2022-04-01 07:21:27 +00:00
|
|
|
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);
|
|
|
|
|
2022-04-04 16:34:46 +00:00
|
|
|
// Do memcpy after mutex unlock to reduce execution time spent inside the
|
|
|
|
// critical section
|
2022-04-01 07:21:27 +00:00
|
|
|
memcpy(frame, &element, 8);
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
bool codec_pushFrame(const uint8_t *frame, const bool blocking)
|
|
|
|
{
|
2022-04-04 16:34:46 +00:00
|
|
|
if(running == false) return false;
|
|
|
|
|
|
|
|
// Copy data to a temporary variable before mutex lock to reduce execution
|
|
|
|
// time spent inside the critical section
|
|
|
|
uint64_t element;
|
|
|
|
memcpy(&element, frame, 8);
|
|
|
|
|
|
|
|
pthread_mutex_lock(&mutex);
|
|
|
|
|
|
|
|
if((numElements >= BUF_SIZE) && (blocking == false))
|
|
|
|
{
|
|
|
|
// No space available and non-blocking call: unlock mutex and return
|
|
|
|
pthread_mutex_unlock(&mutex);
|
|
|
|
return false;
|
|
|
|
}
|
2022-04-01 07:21:27 +00:00
|
|
|
|
2022-04-04 16:34:46 +00:00
|
|
|
// The call is blocking: wait until there is some free space
|
|
|
|
while(numElements >= BUF_SIZE)
|
|
|
|
{
|
|
|
|
pthread_cond_wait(¬_full, &mutex);
|
|
|
|
}
|
|
|
|
|
|
|
|
// There is free space, push data into the queue
|
|
|
|
dataBuffer[writePos] = element;
|
|
|
|
writePos = (writePos + 1) % BUF_SIZE;
|
|
|
|
|
|
|
|
// Signal that the queue is not empty
|
|
|
|
if(numElements == 0) pthread_cond_signal(¬_empty);
|
|
|
|
numElements += 1;
|
|
|
|
|
|
|
|
pthread_mutex_unlock(&mutex);
|
|
|
|
return true;
|
2022-04-01 07:21:27 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static void *encodeFunc(void *arg)
|
|
|
|
{
|
|
|
|
(void) arg;
|
|
|
|
|
2022-04-02 12:57:15 +00:00
|
|
|
filter_state_t dcrState;
|
|
|
|
dsp_resetFilterState(&dcrState);
|
|
|
|
|
2022-04-01 07:21:27 +00:00
|
|
|
codec2 = codec2_create(CODEC2_MODE_3200);
|
|
|
|
|
|
|
|
while(running)
|
|
|
|
{
|
|
|
|
dataBlock_t audio = inputStream_getData(audioStream);
|
|
|
|
|
|
|
|
if(audio.data != NULL)
|
|
|
|
{
|
2022-04-02 08:16:32 +00:00
|
|
|
#ifndef PLATFORM_LINUX
|
2022-04-01 07:21:27 +00:00
|
|
|
// Pre-amplification stage
|
2022-04-02 08:16:32 +00:00
|
|
|
for(size_t i = 0; i < audio.len; i++) audio.data[i] *= micGainPre;
|
2022-04-01 07:21:27 +00:00
|
|
|
|
|
|
|
// DC removal
|
2022-04-02 12:57:15 +00:00
|
|
|
dsp_dcRemoval(&dcrState, audio.data, audio.len);
|
2022-04-01 07:21:27 +00:00
|
|
|
|
|
|
|
// Post-amplification stage
|
2022-04-02 08:16:32 +00:00
|
|
|
for(size_t i = 0; i < audio.len; i++) audio.data[i] *= micGainPost;
|
2022-04-01 07:21:27 +00:00
|
|
|
#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;
|
|
|
|
|
2022-04-04 16:34:46 +00:00
|
|
|
codec2 = codec2_create(CODEC2_MODE_3200);
|
|
|
|
|
|
|
|
while(running)
|
|
|
|
{
|
|
|
|
// Try popping data from the queue
|
|
|
|
uint64_t frame = 0;
|
|
|
|
bool newData = false;
|
|
|
|
|
|
|
|
pthread_mutex_lock(&mutex);
|
|
|
|
|
|
|
|
if(numElements != 0)
|
|
|
|
{
|
|
|
|
frame = dataBuffer[readPos];
|
|
|
|
readPos = (readPos + 1) % BUF_SIZE;
|
|
|
|
if(numElements >= BUF_SIZE) pthread_cond_signal(¬_full);
|
|
|
|
numElements -= 1;
|
|
|
|
newData = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
pthread_mutex_unlock(&mutex);
|
|
|
|
|
|
|
|
stream_sample_t *audioBuf = outputStream_getIdleBuffer(audioStream);
|
|
|
|
|
|
|
|
if(newData)
|
|
|
|
{
|
|
|
|
codec2_decode(codec2, audioBuf, ((uint8_t *) &frame));
|
2022-05-31 19:28:09 +00:00
|
|
|
|
|
|
|
#ifdef PLATFORM_MD3x0
|
|
|
|
// Bump up volume a little bit, as on MD3x0 is quite low
|
|
|
|
for(size_t i = 0; i < 160; i++) audioBuf[i] *= 4;
|
|
|
|
#endif
|
|
|
|
|
2022-04-04 16:34:46 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
memset(audioBuf, 0x00, 160 * sizeof(stream_sample_t));
|
|
|
|
}
|
|
|
|
|
|
|
|
outputStream_sync(audioStream, true);
|
|
|
|
}
|
|
|
|
|
|
|
|
outputStream_stop(audioStream);
|
|
|
|
codec2_destroy(codec2);
|
|
|
|
|
2022-04-01 07:21:27 +00:00
|
|
|
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);
|
|
|
|
}
|