mirror of
https://github.com/nostar/tcd.git
synced 2026-11-08 04:21:47 +00:00
395 lines
9.9 KiB
C++
395 lines
9.9 KiB
C++
/*
|
|
* Permission to use, copy, modify, and/or distribute this software for any
|
|
* purpose with or without fee is hereby granted, provided that the above
|
|
* copyright notice and this permission notice appear in all copies.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS" AND ISC DISCLAIMS ALL WARRANTIES WITH
|
|
* REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
|
|
* AND FITNESS. IN NO EVENT SHALL ISC BE LIABLE FOR ANY SPECIAL, DIRECT,
|
|
* INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
|
|
* LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE
|
|
* OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
|
|
* PERFORMANCE OF THIS SOFTWARE.
|
|
*/
|
|
#define _USE_MATH_DEFINES
|
|
#include <cstring>
|
|
#include <cmath>
|
|
|
|
#include "mbevocoder.h"
|
|
#include "vocoder_tables.h"
|
|
|
|
static inline uint32_t golay_24_encode(uint32_t code_word_in)
|
|
{
|
|
static const uint32_t encoding[12] = {
|
|
040006165,
|
|
020003073,
|
|
010007550,
|
|
04003664,
|
|
02001732,
|
|
01006631,
|
|
0403315,
|
|
0201547,
|
|
0106706,
|
|
045227,
|
|
024476,
|
|
014353
|
|
};
|
|
|
|
uint32_t code_word_out = 0;
|
|
for(uint16_t i = 0; i < 12; i++) {
|
|
uint32_t temp_word = code_word_in & (1 << (11 - i));
|
|
if(temp_word >= 1) {
|
|
code_word_out = code_word_out ^ encoding[i];
|
|
}
|
|
}
|
|
return(code_word_out);
|
|
}
|
|
|
|
static inline uint32_t golay_23_encode(uint32_t code_word_in)
|
|
{
|
|
return golay_24_encode(code_word_in) >> 1;
|
|
}
|
|
|
|
static inline void store_reg(int reg, uint8_t val[], int len)
|
|
{
|
|
for (int i=0; i<len; i++){
|
|
val[i] = (reg >> (len-1-i)) & 1;
|
|
}
|
|
}
|
|
|
|
static inline int load_reg(const uint8_t val[], int len)
|
|
{
|
|
int acc = 0;
|
|
for (int i=0; i<len; i++){
|
|
acc = (acc << 1) + (val[i] & 1);
|
|
}
|
|
return acc;
|
|
}
|
|
|
|
MBEVocoder::MBEVocoder()
|
|
{
|
|
m_mbelibParms = new mbelibParms();
|
|
m_audio_out_temp_buf_p = m_audio_out_temp_buf;
|
|
|
|
memset(m_audio_out_buf, 0, sizeof(short) * 2 * 48000);
|
|
m_audio_out_buf_p = m_audio_out_buf;
|
|
m_audio_out_nb_samples = 0;
|
|
m_audio_out_buf_size = 48000; // given in number of unique samples
|
|
|
|
initMbeParms();
|
|
memset(ambe_d, 0, 49);
|
|
m_dstar_gain_s = -99.0f;
|
|
}
|
|
|
|
MBEVocoder::~MBEVocoder()
|
|
{
|
|
delete m_mbelibParms;
|
|
}
|
|
|
|
void MBEVocoder::decode_2400x1200(int16_t *pcm, uint8_t *ambe)
|
|
{
|
|
int samples = 0;
|
|
process_2400x1200(ambe);
|
|
int16_t *p = getAudio(samples);
|
|
memcpy(pcm, p, samples * sizeof(int16_t));
|
|
resetAudio();
|
|
}
|
|
|
|
void MBEVocoder::decode_2450x1150(int16_t *pcm, uint8_t *ambe)
|
|
{
|
|
int samples = 0;
|
|
process_2450x1150(ambe);
|
|
int16_t *p = getAudio(samples);
|
|
memcpy(pcm, p, samples * sizeof(int16_t));
|
|
resetAudio();
|
|
}
|
|
|
|
void MBEVocoder::decode_2450(int16_t *pcm, uint8_t *ambe)
|
|
{
|
|
int samples = 0;
|
|
process_2450(ambe);
|
|
int16_t *p = getAudio(samples);
|
|
memcpy(pcm, p, samples * sizeof(int16_t));
|
|
resetAudio();
|
|
}
|
|
|
|
void MBEVocoder::encode_2400x1200(int16_t *pcm, uint8_t *ambe)
|
|
{
|
|
int b[9];
|
|
int16_t frame_vector[8]; // result ignored
|
|
uint8_t ambe_frame[72];
|
|
|
|
uint8_t pbuf[48];
|
|
uint8_t tbuf[48];
|
|
int tbufp = 0;
|
|
memset(ambe, 0, 9);
|
|
|
|
vocoder.imbe_encode(frame_vector, pcm);
|
|
encode_ambe(vocoder.param(), b, m_mbelibParms->m_cur_mp, m_mbelibParms->m_prev_mp, true, DSTAR_GAIN_ADJUST, &m_dstar_gain_s);
|
|
b[8] >>= 1;
|
|
|
|
for (int i=0; i < 9; i++) {
|
|
store_reg(b[i], &tbuf[tbufp], b_lengths[i]);
|
|
tbufp += b_lengths[i];
|
|
}
|
|
|
|
for (int i=0; i < 48; i++)
|
|
pbuf[i] = tbuf[m_list[i]];
|
|
|
|
int u0 = load_reg(pbuf+0, 12);
|
|
int u1 = load_reg(pbuf+12, 12);
|
|
|
|
int m1 = PRNG_TABLE[u0];
|
|
int c0 = golay_24_encode(u0);
|
|
int c1 = golay_24_encode(u1) ^ m1;
|
|
|
|
uint8_t pre_buf[72];
|
|
store_reg(c0, pre_buf, 24);
|
|
store_reg(c1, pre_buf+24, 24);
|
|
memcpy(pre_buf+48, pbuf+24, 24);
|
|
|
|
for (int i=0; i < 72; i++)
|
|
ambe_frame[d_list[i]] = pre_buf[i];
|
|
|
|
for(int i = 0; i < 9; ++i){
|
|
for(int j = 0; j < 8; ++j){
|
|
ambe[i] |= (ambe_frame[(i*8)+j] << j);
|
|
}
|
|
}
|
|
}
|
|
|
|
void MBEVocoder::encode_2450x1150(int16_t *pcm, uint8_t *ambe)
|
|
{
|
|
unsigned int aOrig = 0U;
|
|
unsigned int bOrig = 0U;
|
|
unsigned int cOrig = 0U;
|
|
unsigned int MASK = 0x000800U;
|
|
uint8_t tmp[9];
|
|
|
|
memset(tmp, 0, 9);
|
|
memset(ambe, 0, 9);
|
|
encode_2450(pcm, tmp);
|
|
|
|
for (unsigned int i = 0U; i < 12U; i++, MASK >>= 1) {
|
|
unsigned int n1 = i;
|
|
unsigned int n2 = i + 12U;
|
|
if (READ_BIT(tmp, n1))
|
|
aOrig |= MASK;
|
|
if (READ_BIT(tmp, n2))
|
|
bOrig |= MASK;
|
|
}
|
|
|
|
MASK = 0x1000000U;
|
|
for (unsigned int i = 0U; i < 25U; i++, MASK >>= 1) {
|
|
unsigned int n = i + 24U;
|
|
if (READ_BIT(tmp, n))
|
|
cOrig |= MASK;
|
|
}
|
|
|
|
unsigned int a = golay_24_encode(aOrig);
|
|
|
|
// The PRNG
|
|
unsigned int p = PRNG_TABLE[aOrig] >> 1;
|
|
|
|
unsigned int b = golay_23_encode(bOrig);
|
|
b ^= p;
|
|
|
|
MASK = 0x800000U;
|
|
for (unsigned int i = 0U; i < 24U; i++, MASK >>= 1) {
|
|
unsigned int aPos = A_TABLE[i];
|
|
WRITE_BIT(ambe, aPos, a & MASK);
|
|
}
|
|
|
|
MASK = 0x400000U;
|
|
for (unsigned int i = 0U; i < 23U; i++, MASK >>= 1) {
|
|
unsigned int bPos = B_TABLE[i];
|
|
WRITE_BIT(ambe, bPos, b & MASK);
|
|
}
|
|
|
|
MASK = 0x1000000U;
|
|
for (unsigned int i = 0U; i < 25U; i++, MASK >>= 1) {
|
|
unsigned int cPos = C_TABLE[i];
|
|
WRITE_BIT(ambe, cPos, cOrig & MASK);
|
|
}
|
|
}
|
|
|
|
void MBEVocoder::encode_2450(int16_t *pcm, uint8_t *ambe)
|
|
{
|
|
int b[9];
|
|
int16_t frame_vector[8]; // result ignored
|
|
uint8_t ambe_frame[56];
|
|
memset(ambe, 0, 9);
|
|
|
|
vocoder.imbe_encode(frame_vector, pcm);
|
|
encode_ambe(vocoder.param(), b, m_mbelibParms->m_cur_mp, m_mbelibParms->m_prev_mp, false, 1.0);
|
|
memset(ambe_frame, 0, 56);
|
|
|
|
ambe_frame[0] = (b[0] >> 6) & 1;
|
|
ambe_frame[1] = (b[0] >> 5) & 1;
|
|
ambe_frame[2] = (b[0] >> 4) & 1;
|
|
ambe_frame[3] = (b[0] >> 3) & 1;
|
|
ambe_frame[4] = (b[1] >> 4) & 1;
|
|
ambe_frame[5] = (b[1] >> 3) & 1;
|
|
ambe_frame[6] = (b[1] >> 2) & 1;
|
|
ambe_frame[7] = (b[1] >> 1) & 1;
|
|
ambe_frame[8] = (b[2] >> 4) & 1;
|
|
ambe_frame[9] = (b[2] >> 3) & 1;
|
|
ambe_frame[10] = (b[2] >> 2) & 1;
|
|
ambe_frame[11] = (b[2] >> 1) & 1;
|
|
ambe_frame[12] = (b[3] >> 8) & 1;
|
|
ambe_frame[13] = (b[3] >> 7) & 1;
|
|
ambe_frame[14] = (b[3] >> 6) & 1;
|
|
ambe_frame[15] = (b[3] >> 5) & 1;
|
|
ambe_frame[16] = (b[3] >> 4) & 1;
|
|
ambe_frame[17] = (b[3] >> 3) & 1;
|
|
ambe_frame[18] = (b[3] >> 2) & 1;
|
|
ambe_frame[19] = (b[3] >> 1) & 1;
|
|
ambe_frame[20] = (b[4] >> 6) & 1;
|
|
ambe_frame[21] = (b[4] >> 5) & 1;
|
|
ambe_frame[22] = (b[4] >> 4) & 1;
|
|
ambe_frame[23] = (b[4] >> 3) & 1;
|
|
ambe_frame[24] = (b[5] >> 4) & 1;
|
|
ambe_frame[25] = (b[5] >> 3) & 1;
|
|
ambe_frame[26] = (b[5] >> 2) & 1;
|
|
ambe_frame[27] = (b[5] >> 1) & 1;
|
|
ambe_frame[28] = (b[6] >> 3) & 1;
|
|
ambe_frame[29] = (b[6] >> 2) & 1;
|
|
ambe_frame[30] = (b[6] >> 1) & 1;
|
|
ambe_frame[31] = (b[7] >> 3) & 1;
|
|
ambe_frame[32] = (b[7] >> 2) & 1;
|
|
ambe_frame[33] = (b[7] >> 1) & 1;
|
|
ambe_frame[34] = (b[8] >> 2) & 1;
|
|
ambe_frame[35] = b[1] & 1;
|
|
ambe_frame[36] = b[2] & 1;
|
|
ambe_frame[37] = (b[0] >> 2) & 1;
|
|
ambe_frame[38] = (b[0] >> 1) & 1;
|
|
ambe_frame[39] = b[0] & 1;
|
|
ambe_frame[40] = b[3] & 1;
|
|
ambe_frame[41] = (b[4] >> 2) & 1;
|
|
ambe_frame[42] = (b[4] >> 1) & 1;
|
|
ambe_frame[43] = b[4] & 1;
|
|
ambe_frame[44] = b[5] & 1;
|
|
ambe_frame[45] = b[6] & 1;
|
|
ambe_frame[46] = b[7] & 1;
|
|
ambe_frame[47] = (b[8] >> 1) & 1;
|
|
ambe_frame[48] = b[8] & 1;
|
|
|
|
for(int i = 0; i < 7; ++i){
|
|
for(int j = 0; j < 8; ++j){
|
|
ambe[i] |= (ambe_frame[(i*8)+j] << (7-j));
|
|
}
|
|
}
|
|
}
|
|
|
|
void MBEVocoder::initMbeParms()
|
|
{
|
|
mbe_initMbeParms(m_mbelibParms->m_cur_mp, m_mbelibParms->m_prev_mp, m_mbelibParms->m_prev_mp_enhanced);
|
|
//m_errs = 0;
|
|
m_errs2 = 0;
|
|
m_err_str[0] = 0;
|
|
}
|
|
|
|
void MBEVocoder::process_2400x1200(unsigned char *d)
|
|
{
|
|
char ambe_fr[4][24];
|
|
|
|
memset(ambe_fr, 0, 96);
|
|
w = dW;
|
|
x = dX;
|
|
|
|
for(int i = 0; i < 9; ++i){
|
|
for(int j = 0; j < 8; ++j){
|
|
ambe_fr[*w][*x] = (1 & (d[i] >> j));
|
|
w++;
|
|
x++;
|
|
}
|
|
}
|
|
|
|
mbe_processAmbe3600x2400Framef(m_audio_out_temp_buf, &m_errs2, m_err_str, ambe_fr, ambe_d,m_mbelibParms-> m_cur_mp, m_mbelibParms->m_prev_mp, m_mbelibParms->m_prev_mp_enhanced, 3);
|
|
static const float kOut = powf(10.0f, DSTAR_OUT_GAIN_DB / 20.0f);
|
|
for (int i = 0; i < 160; i++) m_audio_out_temp_buf[i] *= kOut;
|
|
processAudio();
|
|
}
|
|
|
|
void MBEVocoder::process_2450x1150(unsigned char *d)
|
|
{
|
|
char ambe_fr[4][24];
|
|
|
|
memset(ambe_fr, 0, 96);
|
|
w = rW;
|
|
x = rX;
|
|
y = rY;
|
|
z = rZ;
|
|
|
|
for(int i = 0; i < 9; ++i){
|
|
for(int j = 0; j < 8; j+=2){
|
|
ambe_fr[*y][*z] = (1 & (d[i] >> (7 - (j+1))));
|
|
ambe_fr[*w][*x] = (1 & (d[i] >> (7 - j)));
|
|
w++;
|
|
x++;
|
|
y++;
|
|
z++;
|
|
}
|
|
}
|
|
|
|
mbe_processAmbe3600x2450Framef(m_audio_out_temp_buf, &m_errs2, m_err_str, ambe_fr, ambe_d,m_mbelibParms-> m_cur_mp, m_mbelibParms->m_prev_mp, m_mbelibParms->m_prev_mp_enhanced, 3);
|
|
processAudio();
|
|
}
|
|
|
|
void MBEVocoder::process_2450(unsigned char *d)
|
|
{
|
|
char ambe_data[49];
|
|
char dvsi_data[7];
|
|
memset(dvsi_data, 0, 7);
|
|
|
|
for(int i = 0; i < 6; ++i){
|
|
for(int j = 0; j < 8; j++){
|
|
ambe_data[j+(8*i)] = (1 & (d[i] >> (7 - j)));
|
|
}
|
|
}
|
|
ambe_data[48] = (1 & (d[6] >> 7));
|
|
processData(ambe_data);
|
|
}
|
|
|
|
void MBEVocoder::processData(char ambe_data[49])
|
|
{
|
|
mbe_processAmbe2450Dataf(m_audio_out_temp_buf, &m_errs2, m_err_str, ambe_data, m_mbelibParms->m_cur_mp,m_mbelibParms->m_prev_mp, m_mbelibParms->m_prev_mp_enhanced, 3);
|
|
processAudio();
|
|
}
|
|
|
|
short * MBEVocoder::getAudio(int& nbSamples)
|
|
{
|
|
nbSamples = m_audio_out_nb_samples;
|
|
return m_audio_out_buf;
|
|
}
|
|
|
|
void MBEVocoder::resetAudio()
|
|
{
|
|
m_audio_out_nb_samples = 0;
|
|
m_audio_out_buf_p = m_audio_out_buf;
|
|
}
|
|
|
|
void MBEVocoder::processAudio()
|
|
{
|
|
m_audio_out_temp_buf_p = m_audio_out_temp_buf;
|
|
|
|
if (m_audio_out_nb_samples + 160 >= m_audio_out_buf_size){
|
|
resetAudio();
|
|
}
|
|
|
|
for (int i = 0; i < 160; i++){
|
|
if (*m_audio_out_temp_buf_p > static_cast<float>(32760)){
|
|
*m_audio_out_temp_buf_p = static_cast<float>(32760);
|
|
}
|
|
else if (*m_audio_out_temp_buf_p < static_cast<float>(-32760)){
|
|
*m_audio_out_temp_buf_p = static_cast<float>(-32760);
|
|
}
|
|
|
|
*m_audio_out_buf_p = static_cast<short>(*m_audio_out_temp_buf_p);
|
|
m_audio_out_buf_p++;
|
|
|
|
m_audio_out_nb_samples++;
|
|
m_audio_out_temp_buf_p++;
|
|
}
|
|
}
|