2022-04-01 07:21:27 +00:00
|
|
|
/***************************************************************************
|
2023-02-08 15:33:02 +00:00
|
|
|
* Copyright (C) 2022 - 2023 by Federico Amedeo Izzo IU2NUO, *
|
|
|
|
* Niccolò Izzo IU2KIN *
|
|
|
|
* Frederik Saraci IU2NRO *
|
|
|
|
* Silvano Seva IU2KWO *
|
2022-04-01 07:21:27 +00:00
|
|
|
* *
|
|
|
|
* 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/> *
|
|
|
|
***************************************************************************/
|
|
|
|
|
2023-03-15 18:36:35 +00:00
|
|
|
#include <audio_stream.h>
|
2022-04-01 07:21:27 +00:00
|
|
|
#include <audio_codec.h>
|
|
|
|
#include <pthread.h>
|
|
|
|
#include <codec2.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <string.h>
|
2023-05-20 12:14:18 +00:00
|
|
|
#include <stdio.h>
|
|
|
|
#include <errno.h>
|
2022-04-01 07:21:27 +00:00
|
|
|
#include <dsp.h>
|
|
|
|
|
|
|
|
#define BUF_SIZE 4
|
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
static pathId audioPath;
|
2022-04-01 07:21:27 +00:00
|
|
|
|
2022-08-17 16:03:04 +00:00
|
|
|
static uint8_t initCnt = 0;
|
2022-04-01 07:21:27 +00:00
|
|
|
static bool running;
|
2022-08-17 19:42:08 +00:00
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
static bool reqStop;
|
2022-04-01 07:21:27 +00:00
|
|
|
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 *));
|
2023-05-20 12:14:18 +00:00
|
|
|
static void stopThread();
|
2022-04-01 07:21:27 +00:00
|
|
|
|
|
|
|
|
|
|
|
void codec_init()
|
|
|
|
{
|
2022-08-17 16:03:04 +00:00
|
|
|
if(initCnt > 0)
|
|
|
|
{
|
|
|
|
pthread_mutex_lock(&mutex);
|
|
|
|
initCnt += 1;
|
|
|
|
pthread_mutex_unlock(&mutex);
|
|
|
|
return;
|
|
|
|
}
|
2022-08-17 19:42:08 +00:00
|
|
|
else
|
|
|
|
{
|
|
|
|
initCnt = 1;
|
|
|
|
}
|
2022-08-17 16:03:04 +00:00
|
|
|
|
2022-04-01 07:21:27 +00:00
|
|
|
running = false;
|
|
|
|
readPos = 0;
|
|
|
|
writePos = 0;
|
|
|
|
numElements = 0;
|
|
|
|
memset(dataBuffer, 0x00, BUF_SIZE * sizeof(uint64_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()
|
|
|
|
{
|
2022-08-17 16:03:04 +00:00
|
|
|
pthread_mutex_lock(&mutex);
|
|
|
|
initCnt -= 1;
|
|
|
|
pthread_mutex_unlock(&mutex);
|
|
|
|
|
|
|
|
if(initCnt > 0) return;
|
2023-05-20 12:14:18 +00:00
|
|
|
if(running) stopThread();
|
2022-04-01 07:21:27 +00:00
|
|
|
|
|
|
|
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
|
|
|
}
|
|
|
|
|
2023-05-20 10:36:12 +00:00
|
|
|
bool codec_startEncode(const pathId path)
|
2022-04-01 07:21:27 +00:00
|
|
|
{
|
2023-05-20 10:36:12 +00:00
|
|
|
// Bad incoming path
|
|
|
|
if(audioPath_getStatus(path) != PATH_OPEN)
|
|
|
|
return false;
|
2022-04-01 07:21:27 +00:00
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
if(running)
|
2022-08-17 19:42:08 +00:00
|
|
|
{
|
2023-05-20 12:14:18 +00:00
|
|
|
if(audioPath_getStatus(audioPath) == PATH_OPEN)
|
|
|
|
return false;
|
|
|
|
else
|
|
|
|
stopThread();
|
2022-08-17 19:42:08 +00:00
|
|
|
}
|
2022-04-01 07:21:27 +00:00
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
running = true;
|
|
|
|
audioPath = path;
|
2022-04-01 07:21:27 +00:00
|
|
|
startThread(encodeFunc);
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2023-05-20 10:36:12 +00:00
|
|
|
bool codec_startDecode(const pathId path)
|
2022-04-01 07:21:27 +00:00
|
|
|
{
|
2023-05-20 10:36:12 +00:00
|
|
|
// Bad incoming path
|
|
|
|
if(audioPath_getStatus(path) != PATH_OPEN)
|
|
|
|
return false;
|
2022-04-01 07:21:27 +00:00
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
if(running)
|
2022-08-17 19:42:08 +00:00
|
|
|
{
|
2023-05-20 12:14:18 +00:00
|
|
|
if(audioPath_getStatus(audioPath) == PATH_OPEN)
|
|
|
|
return false;
|
|
|
|
else
|
|
|
|
stopThread();
|
2022-08-17 19:42:08 +00:00
|
|
|
}
|
2022-04-01 07:21:27 +00:00
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
running = true;
|
|
|
|
audioPath = path;
|
2022-04-01 07:21:27 +00:00
|
|
|
startThread(decodeFunc);
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
void codec_stop(const pathId path)
|
2022-04-01 07:21:27 +00:00
|
|
|
{
|
2023-05-20 12:14:18 +00:00
|
|
|
if(running == false)
|
|
|
|
return;
|
2022-04-30 06:53:31 +00:00
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
if(audioPath != path)
|
|
|
|
return;
|
2022-08-17 19:42:08 +00:00
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
stopThread();
|
2022-04-01 07:21:27 +00:00
|
|
|
}
|
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
int codec_popFrame(uint8_t *frame, const bool blocking)
|
2022-04-01 07:21:27 +00:00
|
|
|
{
|
2023-05-20 12:14:18 +00:00
|
|
|
if(running == false)
|
|
|
|
return -EPERM;
|
2022-04-01 07:21:27 +00:00
|
|
|
|
|
|
|
uint64_t element;
|
|
|
|
|
2022-10-01 09:27:30 +00:00
|
|
|
// No data available and non-blocking call: just return false.
|
|
|
|
if((numElements == 0) && (blocking == false))
|
2023-05-20 12:14:18 +00:00
|
|
|
return -EAGAIN;
|
2022-10-01 09:27:30 +00:00
|
|
|
|
|
|
|
// Blocking call: wait until some data is pushed
|
|
|
|
pthread_mutex_lock(&mutex);
|
|
|
|
while(numElements == 0)
|
2022-04-01 07:21:27 +00:00
|
|
|
{
|
2022-10-01 09:27:30 +00:00
|
|
|
pthread_cond_wait(¬_empty, &mutex);
|
2022-04-01 07:21:27 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
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
|
2022-08-17 19:42:08 +00:00
|
|
|
// critical section
|
2022-04-01 07:21:27 +00:00
|
|
|
memcpy(frame, &element, 8);
|
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
return 0;
|
2022-04-01 07:21:27 +00:00
|
|
|
}
|
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
int codec_pushFrame(const uint8_t *frame, const bool blocking)
|
2022-04-01 07:21:27 +00:00
|
|
|
{
|
2023-05-20 12:14:18 +00:00
|
|
|
if(running == false)
|
|
|
|
return -EPERM;
|
2022-04-04 16:34:46 +00:00
|
|
|
|
|
|
|
// 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);
|
|
|
|
|
|
|
|
|
2022-10-01 09:27:30 +00:00
|
|
|
// No space available and non-blocking call: return
|
2022-04-04 16:34:46 +00:00
|
|
|
if((numElements >= BUF_SIZE) && (blocking == false))
|
2023-05-20 12:14:18 +00:00
|
|
|
return -EAGAIN;
|
2022-04-01 07:21:27 +00:00
|
|
|
|
2022-10-01 09:27:30 +00:00
|
|
|
// Blocking call: wait until there is some free space
|
|
|
|
pthread_mutex_lock(&mutex);
|
2022-04-04 16:34:46 +00:00
|
|
|
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);
|
2023-05-20 12:14:18 +00:00
|
|
|
return 0;
|
2022-04-01 07:21:27 +00:00
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static void *encodeFunc(void *arg)
|
|
|
|
{
|
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
streamId iStream;
|
|
|
|
pathId iPath = (pathId) arg;
|
|
|
|
stream_sample_t audioBuf[320];
|
|
|
|
struct CODEC2 *codec2;
|
|
|
|
filter_state_t dcrState;
|
|
|
|
|
|
|
|
iStream = audioStream_start(iPath, audioBuf, 320, 8000,
|
|
|
|
STREAM_INPUT | BUF_CIRC_DOUBLE);
|
|
|
|
if(iStream < 0)
|
|
|
|
{
|
|
|
|
running = false;
|
|
|
|
return NULL;
|
|
|
|
}
|
2022-04-02 12:57:15 +00:00
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
dsp_resetFilterState(&dcrState);
|
2022-04-01 07:21:27 +00:00
|
|
|
codec2 = codec2_create(CODEC2_MODE_3200);
|
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
while(reqStop == false)
|
2022-04-01 07:21:27 +00:00
|
|
|
{
|
2023-05-20 12:14:18 +00:00
|
|
|
// Invalid path, quit
|
|
|
|
if(audioPath_getStatus(iPath) != PATH_OPEN)
|
|
|
|
break;
|
|
|
|
|
|
|
|
dataBlock_t audio = inputStream_getData(iStream);
|
2022-04-01 07:21:27 +00:00
|
|
|
|
|
|
|
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);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
audioStream_terminate(iStream);
|
2022-04-01 07:21:27 +00:00
|
|
|
codec2_destroy(codec2);
|
|
|
|
|
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
static void *decodeFunc(void *arg)
|
|
|
|
{
|
2023-05-20 12:14:18 +00:00
|
|
|
streamId oStream;
|
|
|
|
pathId oPath = (pathId) arg;
|
|
|
|
stream_sample_t audioBuf[320];
|
|
|
|
struct CODEC2 *codec2;
|
|
|
|
|
|
|
|
// Open output stream
|
|
|
|
memset(audioBuf, 0x00, 320 * sizeof(stream_sample_t));
|
|
|
|
oStream = audioStream_start(oPath, audioBuf, 320, 8000,
|
|
|
|
STREAM_OUTPUT | BUF_CIRC_DOUBLE);
|
|
|
|
if(oStream < 0)
|
|
|
|
{
|
|
|
|
running = false;
|
|
|
|
return NULL;
|
|
|
|
}
|
2022-04-01 07:21:27 +00:00
|
|
|
|
2022-04-04 16:34:46 +00:00
|
|
|
codec2 = codec2_create(CODEC2_MODE_3200);
|
|
|
|
|
2022-10-16 07:52:25 +00:00
|
|
|
// Ensure that thread start is correctly synchronized with the output
|
|
|
|
// stream to avoid having the decode function writing in a memory area
|
|
|
|
// being read at the same time by the output stream system causing cracking
|
|
|
|
// noises at speaker output. Behaviour observed on both Module17 and MD-UV380
|
2023-05-20 12:14:18 +00:00
|
|
|
outputStream_sync(oStream, false);
|
2022-10-16 07:52:25 +00:00
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
while(reqStop == false)
|
2022-04-04 16:34:46 +00:00
|
|
|
{
|
2023-05-20 12:14:18 +00:00
|
|
|
// Invalid path, quit
|
|
|
|
if(audioPath_getStatus(oPath) != PATH_OPEN)
|
|
|
|
break;
|
|
|
|
|
2022-04-04 16:34:46 +00:00
|
|
|
// 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);
|
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
stream_sample_t *audioBuf = outputStream_getIdleBuffer(oStream);
|
2022-04-04 16:34:46 +00:00
|
|
|
|
|
|
|
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
|
2022-09-30 05:59:59 +00:00
|
|
|
for(size_t i = 0; i < 160; i++) audioBuf[i] *= 2;
|
2022-05-31 19:28:09 +00:00
|
|
|
#endif
|
|
|
|
|
2022-04-04 16:34:46 +00:00
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
memset(audioBuf, 0x00, 160 * sizeof(stream_sample_t));
|
|
|
|
}
|
|
|
|
|
2023-05-20 12:14:18 +00:00
|
|
|
outputStream_sync(oStream, true);
|
2022-04-04 16:34:46 +00:00
|
|
|
}
|
|
|
|
|
2022-08-17 19:42:08 +00:00
|
|
|
// Stop stream and wait until its effective termination
|
2023-05-20 12:14:18 +00:00
|
|
|
audioStream_stop(oStream);
|
2022-04-04 16:34:46 +00:00
|
|
|
codec2_destroy(codec2);
|
|
|
|
|
2022-04-01 07:21:27 +00:00
|
|
|
return NULL;
|
|
|
|
}
|
|
|
|
|
|
|
|
static void startThread(void *(*func) (void *))
|
|
|
|
{
|
2023-05-20 12:14:18 +00:00
|
|
|
readPos = 0;
|
|
|
|
writePos = 0;
|
|
|
|
numElements = 0;
|
|
|
|
reqStop = false;
|
|
|
|
|
2022-07-26 20:49:38 +00:00
|
|
|
#ifdef _MIOSIX
|
|
|
|
// Set stack size of CODEC2 thread to 16kB.
|
2022-04-01 07:21:27 +00:00
|
|
|
pthread_attr_t codecAttr;
|
|
|
|
pthread_attr_init(&codecAttr);
|
|
|
|
pthread_attr_setstacksize(&codecAttr, 16384);
|
|
|
|
|
|
|
|
// 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);
|
|
|
|
|
2022-07-26 20:49:38 +00:00
|
|
|
// Start thread
|
2023-05-20 12:14:18 +00:00
|
|
|
pthread_create(&codecThread, &codecAttr, func, ((void *) audioPath));
|
2022-07-26 20:49:38 +00:00
|
|
|
#else
|
2023-05-20 12:14:18 +00:00
|
|
|
pthread_create(&codecThread, NULL, func, ((void *) audioPath));
|
2022-07-26 20:49:38 +00:00
|
|
|
#endif
|
|
|
|
|
2022-04-01 07:21:27 +00:00
|
|
|
}
|
2023-05-20 12:14:18 +00:00
|
|
|
|
|
|
|
static void stopThread()
|
|
|
|
{
|
|
|
|
reqStop = true;
|
|
|
|
pthread_join(codecThread, NULL);
|
|
|
|
running = false;
|
|
|
|
}
|