You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
491 lines
13 KiB
491 lines
13 KiB
// SPDX-License-Identifier: GPL-2.0-only
|
|
/*
|
|
* Digital Voice Modem - MBE Vocoder
|
|
* GPLv2 Open Source. Use is subject to license terms.
|
|
* DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
|
|
*
|
|
*/
|
|
|
|
/*
|
|
* Copyright (C) 2010 mbelib Author
|
|
* GPG Key ID: 0xEA5EFE2C (9E7A 5527 9CDC EBF7 BF1B D772 4F98 E863 EA5E FE2C)
|
|
*
|
|
* 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.
|
|
*/
|
|
|
|
#include <stdlib.h>
|
|
#include <stdio.h>
|
|
#define _USE_MATH_DEFINES
|
|
#include <math.h>
|
|
|
|
#include "mbe.h"
|
|
#include "imbe7200x4400_const.h"
|
|
|
|
#ifdef _MSC_VER
|
|
#pragma warning(disable: 4244)
|
|
#endif
|
|
#if defined(__GNUC__) || defined(__GNUG__)
|
|
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
|
|
#endif
|
|
|
|
// ---------------------------------------------------------------------------
|
|
// Global Functions
|
|
// ---------------------------------------------------------------------------
|
|
|
|
/* */
|
|
|
|
int mbe_eccImbe7200x4400C0(char imbe_fr[8][23])
|
|
{
|
|
int j, errs;
|
|
char in[23], out[23];
|
|
|
|
for (j = 0; j < 23; j++) {
|
|
in[j] = imbe_fr[0][j];
|
|
}
|
|
|
|
errs = mbe_golay2312(in, out);
|
|
for (j = 0; j < 23; j++) {
|
|
imbe_fr[0][j] = out[j];
|
|
}
|
|
|
|
return (errs);
|
|
}
|
|
|
|
/* */
|
|
|
|
int mbe_eccImbe7200x4400Data(char imbe_fr[8][23], char* imbe_d)
|
|
{
|
|
int i, j, errs;
|
|
char* imbe, gin[23], gout[23], hin[15], hout[15];
|
|
|
|
errs = 0;
|
|
imbe = imbe_d;
|
|
for (i = 0; i < 4; i++) {
|
|
if (i > 0) {
|
|
for (j = 0; j < 23; j++) {
|
|
gin[j] = imbe_fr[i][j];
|
|
}
|
|
|
|
errs += mbe_golay2312(gin, gout);
|
|
for (j = 22; j > 10; j--) {
|
|
*imbe = gout[j];
|
|
imbe++;
|
|
}
|
|
}
|
|
else {
|
|
for (j = 22; j > 10; j--) {
|
|
*imbe = imbe_fr[i][j];
|
|
imbe++;
|
|
}
|
|
}
|
|
}
|
|
|
|
for (i = 4; i < 7; i++) {
|
|
for (j = 0; j < 15; j++) {
|
|
hin[j] = imbe_fr[i][j];
|
|
}
|
|
|
|
errs += mbe_hamming1511(hin, hout);
|
|
for (j = 14; j >= 4; j--) {
|
|
*imbe = hout[j];
|
|
imbe++;
|
|
}
|
|
}
|
|
|
|
for (j = 6; j >= 0; j--) {
|
|
*imbe = imbe_fr[7][j];
|
|
imbe++;
|
|
}
|
|
|
|
return (errs);
|
|
}
|
|
|
|
/* */
|
|
|
|
int mbe_decodeImbe4400Parms(char* imbe_d, mbe_parms* cur_mp, mbe_parms* prev_mp)
|
|
{
|
|
int Bm, ji, b, i, j, k, l, L, K, L9, m, am, ak;
|
|
int intkl[57];
|
|
int b0, b2, bm;
|
|
float Cik[7][11], rho, flokl[57], deltal[57];
|
|
float Sum77, Tl[57], Gm[7], Ri[7], sum, c1, c2;
|
|
const float* ba1, *ba2;
|
|
char tmpstr[13];
|
|
const int* bo1, *bo2;
|
|
char bb[58][12];
|
|
|
|
// copy repeat from prev_mp
|
|
cur_mp->repeat = prev_mp->repeat;
|
|
|
|
// decode fundamental frequency w0 from b0
|
|
tmpstr[8] = 0;
|
|
tmpstr[0] = imbe_d[0] + 48;
|
|
tmpstr[1] = imbe_d[1] + 48;
|
|
tmpstr[2] = imbe_d[2] + 48;
|
|
tmpstr[3] = imbe_d[3] + 48;
|
|
tmpstr[4] = imbe_d[4] + 48;
|
|
tmpstr[5] = imbe_d[5] + 48;
|
|
tmpstr[6] = imbe_d[85] + 48;
|
|
tmpstr[7] = imbe_d[86] + 48;
|
|
b0 = strtol(tmpstr, NULL, 2);
|
|
|
|
if (b0 > 207) {
|
|
if ((b0 >= 216) && (b0 <= 219)) {
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: Silence");
|
|
#endif
|
|
}
|
|
else {
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: Invalid fundamental frequency");
|
|
#endif
|
|
}
|
|
return (1);
|
|
}
|
|
|
|
cur_mp->w0 = ((float)(4 * M_PI) / (float)((float)b0 + 39.5));
|
|
|
|
// decode L from w0
|
|
L = (int)(0.9254 * (int)((M_PI / cur_mp->w0) + 0.25));
|
|
if ((L > 56) || (L < 9)) {
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: invalid L: %i", L);
|
|
#endif
|
|
return (1);
|
|
}
|
|
|
|
cur_mp->L = L;
|
|
L9 = L - 9;
|
|
|
|
// decode K from L
|
|
if (L < 37) {
|
|
K = (int)((float)(L + 2) / (float)3);
|
|
cur_mp->K = K;
|
|
}
|
|
else {
|
|
K = 12;
|
|
cur_mp->K = 12;
|
|
}
|
|
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: b0:%i L:%i K:%i", b0, L, K);
|
|
#endif
|
|
|
|
// read bits from imbe_d into b0..bL+1
|
|
bo1 = bo[L9][0];
|
|
bo2 = bo1 + 1;
|
|
for (i = 6; i < 85; i++) {
|
|
bb[*bo1][*bo2] = imbe_d[i];
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: bo1: %i, bo2: %i", *bo1, *bo2);
|
|
#endif
|
|
bo1 += 2;
|
|
bo2 += 2;
|
|
}
|
|
|
|
// Vl
|
|
j = 1;
|
|
k = (K - 1);
|
|
for (i = 1; i <= L; i++) {
|
|
cur_mp->Vl[i] = bb[1][k];
|
|
if (j == 3) {
|
|
j = 1;
|
|
if (k > 0) {
|
|
k--;
|
|
}
|
|
else {
|
|
k = 0;
|
|
}
|
|
}
|
|
else {
|
|
j++;
|
|
}
|
|
}
|
|
|
|
// decode G1 from b2
|
|
tmpstr[6] = 0;
|
|
tmpstr[0] = bb[2][5] + 48;
|
|
tmpstr[1] = bb[2][4] + 48;
|
|
tmpstr[2] = bb[2][3] + 48;
|
|
tmpstr[3] = bb[2][2] + 48;
|
|
tmpstr[4] = bb[2][1] + 48;
|
|
tmpstr[5] = bb[2][0] + 48;
|
|
b2 = strtol(tmpstr, NULL, 2);
|
|
Gm[1] = B2[b2];
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: G1: %e, %s, %i", Gm[1], tmpstr, b2);
|
|
#endif
|
|
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: tmpstr: %s b2: %i g1: %e", tmpstr, b2, Gm[1]);
|
|
#endif
|
|
|
|
// decode G2..G6 (from b3..b7) with annex E
|
|
// equation 68
|
|
ba1 = ba[L9][0];
|
|
ba2 = ba1 + 1;
|
|
|
|
for (i = 2; i < 7; i++) {
|
|
tmpstr[(int)*ba1] = 0;
|
|
k = 0;
|
|
for (j = ((int)*ba1 - 1); j >= 0; j--) {
|
|
tmpstr[k] = bb[i + 1][j] + 48;
|
|
k++;
|
|
}
|
|
bm = strtol(tmpstr, NULL, 2);
|
|
Gm[i] = (*ba2 * ((float)bm - powf(2, (*ba1 - 1)) + (float)0.5));
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: G%i: %e, %s, %i, ba1: %e, ba2: %e", i, Gm[i], tmpstr, bm, *ba1, *ba2);
|
|
#endif
|
|
ba1 += 2;
|
|
ba2 += 2;
|
|
}
|
|
|
|
// inverse DCT Gi to give Ri (also known as Ci,1)
|
|
for (i = 1; i <= 6; i++) {
|
|
sum = 0;
|
|
for (m = 1; m <= 6; m++) {
|
|
if (m == 1) {
|
|
am = 1;
|
|
}
|
|
else {
|
|
am = 2;
|
|
}
|
|
sum = sum + ((float)am * Gm[m] * cosf((M_PI * (float)(m - 1) * ((float)i - 0.5)) / (float)6));
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: sum: %e ", sum);
|
|
#endif
|
|
}
|
|
Ri[i] = sum;
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: R%i: %e", i, Ri[i]);
|
|
#endif
|
|
}
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: R1: %e", Ri[1]);
|
|
#endif
|
|
|
|
// load b8..bL+1 into Ci,k
|
|
m = 8;
|
|
for (i = 1; i <= 6; i++) {
|
|
Cik[i][1] = Ri[i];
|
|
for (k = 2; k <= ImbeJi[L9][i - 1]; k++) {
|
|
Bm = hoba[L9][m - 8];
|
|
for (b = 0; b < Bm; b++) {
|
|
tmpstr[b] = bb[m][(Bm - b) - 1] + 48;
|
|
}
|
|
|
|
if (Bm == 0) {
|
|
Cik[i][k] = 0;
|
|
}
|
|
else {
|
|
tmpstr[Bm] = 0;
|
|
bm = strtol(tmpstr, NULL, 2);
|
|
Cik[i][k] = ((quantstep[Bm - 1] * standdev[k - 2]) * (((float)bm - powf(2, (Bm - 1))) + 0.5));
|
|
}
|
|
m++;
|
|
}
|
|
}
|
|
|
|
// inverse DCT each Ci,k to give ci,j (Tl)
|
|
l = 1;
|
|
for (i = 1; i <= 6; i++) {
|
|
ji = ImbeJi[L9][i - 1];
|
|
for (j = 1; j <= ji; j++) {
|
|
sum = 0;
|
|
for (k = 1; k <= ji; k++) {
|
|
if (k == 1) {
|
|
ak = 1;
|
|
}
|
|
else {
|
|
ak = 2;
|
|
}
|
|
sum = sum + ((float)ak * Cik[i][k] * cosf((M_PI * (float)(k - 1) * ((float)j - 0.5)) / (float)ji));
|
|
}
|
|
Tl[l] = sum;
|
|
l++;
|
|
}
|
|
}
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: T1: %e", Tl[1]);
|
|
#endif
|
|
|
|
// determine log2Ml by applying ci,j to previous log2Ml
|
|
if (cur_mp->L <= 15) {
|
|
rho = 0.4;
|
|
}
|
|
else if (cur_mp->L <= 24) {
|
|
rho = (0.03 * (float)cur_mp->L) - 0.05;
|
|
}
|
|
else {
|
|
rho = 0.7;
|
|
}
|
|
|
|
// fix for when L > L(-1)
|
|
if (cur_mp->L > prev_mp->L) {
|
|
for (l = prev_mp->L + 1; l <= cur_mp->L; l++) {
|
|
prev_mp->Ml[l] = prev_mp->Ml[prev_mp->L];
|
|
prev_mp->log2Ml[l] = prev_mp->log2Ml[prev_mp->L];
|
|
}
|
|
}
|
|
|
|
// Part 1
|
|
Sum77 = 0;
|
|
for (l = 1; l <= cur_mp->L; l++) {
|
|
// eq. 75
|
|
flokl[l] = ((float)prev_mp->L / (float)cur_mp->L) * (float)l;
|
|
intkl[l] = (int)(flokl[l]);
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: flokl: %e, intkl: %i", flokl[l], intkl[l]);
|
|
#endif
|
|
// eq. 76
|
|
deltal[l] = flokl[l] - (float)intkl[l];
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: deltal: %e", deltal[l]);
|
|
#endif
|
|
// eq 77
|
|
Sum77 = Sum77 + ((((float)1 - deltal[l]) * prev_mp->log2Ml[intkl[l]]) + (deltal[l] * prev_mp->log2Ml[intkl[l] + 1]));
|
|
}
|
|
Sum77 = ((rho / (float)cur_mp->L) * Sum77);
|
|
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: Sum77: %e", Sum77);
|
|
#endif
|
|
|
|
// Part 2
|
|
for (l = 1; l <= cur_mp->L; l++) {
|
|
c1 = (rho * ((float)1 - deltal[l]) * prev_mp->log2Ml[intkl[l]]);
|
|
c2 = (rho * deltal[l] * prev_mp->log2Ml[intkl[l] + 1]);
|
|
cur_mp->log2Ml[l] = Tl[l] + c1 + c2 - Sum77;
|
|
cur_mp->Ml[l] = powf(2, cur_mp->log2Ml[l]);
|
|
#ifdef IMBE_DEBUG
|
|
fprintf(stderr, "MBE: IMBE: rho: %e c1: %e c2: %e Sum77: %e T%i: %e log2M%i: %e M%i: %e", rho, c1, c2, Sum77, l, Tl[l], l, cur_mp->log2Ml[l], l, cur_mp->Ml[l]);
|
|
#endif
|
|
}
|
|
|
|
return (0);
|
|
}
|
|
|
|
/* */
|
|
|
|
void mbe_demodulateImbe7200x4400Data(char imbe[8][23])
|
|
{
|
|
int i, j, k;
|
|
unsigned short pr[115];
|
|
unsigned short foo;
|
|
char tmpstr[24];
|
|
|
|
// create pseudo-random modulator
|
|
j = 0;
|
|
tmpstr[12] = 0;
|
|
for (i = 22; i >= 11; i--) {
|
|
tmpstr[j] = (imbe[0][i] + 48);
|
|
j++;
|
|
}
|
|
|
|
foo = strtol(tmpstr, NULL, 2);
|
|
pr[0] = (16 * foo);
|
|
for (i = 1; i < 115; i++) {
|
|
pr[i] = (173 * pr[i - 1]) + 13849 - (65536 * (((173 * pr[i - 1]) + 13849) / 65536));
|
|
}
|
|
|
|
for (i = 1; i < 115; i++) {
|
|
pr[i] = pr[i] / 32768;
|
|
}
|
|
|
|
// demodulate imbe with pr
|
|
k = 1;
|
|
for (i = 1; i < 4; i++) {
|
|
for (j = 22; j >= 0; j--) {
|
|
imbe[i][j] = ((imbe[i][j]) ^ pr[k]);
|
|
k++;
|
|
}
|
|
}
|
|
|
|
for (i = 4; i < 7; i++) {
|
|
for (j = 14; j >= 0; j--) {
|
|
imbe[i][j] = ((imbe[i][j]) ^ pr[k]);
|
|
k++;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* */
|
|
|
|
void mbe_processImbe4400DataF(float* aout_buf, int* errs, int* errs2, char* err_str, char imbe_d[88], mbe_parms* cur_mp, mbe_parms* prev_mp, mbe_parms* prev_mp_enhanced, int uvquality)
|
|
{
|
|
int i, bad;
|
|
|
|
for (i = 0; i < *errs2; i++) {
|
|
*err_str = '=';
|
|
err_str++;
|
|
}
|
|
|
|
bad = mbe_decodeImbe4400Parms(imbe_d, cur_mp, prev_mp);
|
|
if ((bad == 1) || (*errs2 > 5)) {
|
|
mbe_useLastMbeParms(cur_mp, prev_mp);
|
|
cur_mp->repeat++;
|
|
*err_str = 'R';
|
|
err_str++;
|
|
}
|
|
else {
|
|
cur_mp->repeat = 0;
|
|
}
|
|
|
|
if (cur_mp->repeat <= 3) {
|
|
mbe_moveMbeParms(cur_mp, prev_mp);
|
|
mbe_spectralAmpEnhance(cur_mp);
|
|
mbe_synthesizeSpeechF(aout_buf, cur_mp, prev_mp_enhanced, uvquality);
|
|
mbe_moveMbeParms(cur_mp, prev_mp_enhanced);
|
|
}
|
|
else {
|
|
*err_str = 'M';
|
|
err_str++;
|
|
mbe_synthesizeSilenceF(aout_buf);
|
|
mbe_initMbeParms(cur_mp, prev_mp, prev_mp_enhanced);
|
|
}
|
|
*err_str = 0;
|
|
}
|
|
|
|
/* */
|
|
|
|
void mbe_processImbe4400Data(short* aout_buf, int* errs, int* errs2, char* err_str, char imbe_d[88], mbe_parms* cur_mp, mbe_parms* prev_mp, mbe_parms* prev_mp_enhanced, int uvquality)
|
|
{
|
|
float float_buf[160];
|
|
mbe_processImbe4400DataF(float_buf, errs, errs2, err_str, imbe_d, cur_mp, prev_mp, prev_mp_enhanced, uvquality);
|
|
mbe_floatToShort(float_buf, aout_buf);
|
|
}
|
|
|
|
/* */
|
|
|
|
void mbe_processImbe7200x4400FrameF(float* aout_buf, int* errs, int* errs2, char* err_str, char imbe_fr[8][23], char imbe_d[88], mbe_parms* cur_mp, mbe_parms* prev_mp, mbe_parms* prev_mp_enhanced, int uvquality)
|
|
{
|
|
*errs = 0;
|
|
*errs2 = 0;
|
|
*errs = mbe_eccImbe7200x4400C0(imbe_fr);
|
|
mbe_demodulateImbe7200x4400Data(imbe_fr);
|
|
*errs2 = *errs;
|
|
*errs2 += mbe_eccImbe7200x4400Data(imbe_fr, imbe_d);
|
|
|
|
mbe_processImbe4400DataF(aout_buf, errs, errs2, err_str, imbe_d, cur_mp, prev_mp, prev_mp_enhanced, uvquality);
|
|
}
|
|
|
|
/* */
|
|
|
|
void mbe_processImbe7200x4400Frame(short* aout_buf, int* errs, int* errs2, char* err_str, char imbe_fr[8][23], char imbe_d[88], mbe_parms* cur_mp, mbe_parms* prev_mp, mbe_parms* prev_mp_enhanced, int uvquality)
|
|
{
|
|
float float_buf[160];
|
|
mbe_processImbe7200x4400FrameF(float_buf, errs, errs2, err_str, imbe_fr, imbe_d, cur_mp, prev_mp, prev_mp_enhanced, uvquality);
|
|
mbe_floatToShort(float_buf, aout_buf);
|
|
}
|