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.
383 lines
19 KiB
383 lines
19 KiB
/*
|
|
* Copyright (c) 2011-2019 Belledonne Communications SARL.
|
|
*
|
|
* This file is part of bcg729.
|
|
*
|
|
* 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 "typedef.h"
|
|
#include "basicOperationsMacros.h"
|
|
#include "codecParameters.h"
|
|
#include "g729FixedPointMath.h"
|
|
#include "codebooks.h"
|
|
|
|
/*****************************************************************************/
|
|
/* insertionSort : sort an array in growing order using insertion algorithm */
|
|
/* parameters : */
|
|
/* -(i/o) x: the array to be sorted */
|
|
/* -(i) length: the array length */
|
|
/* */
|
|
/*****************************************************************************/
|
|
void insertionSort(word16_t x[], int length)
|
|
{
|
|
int i, j;
|
|
word16_t currentValue;
|
|
|
|
for (i=1; i<length; i++) {
|
|
currentValue = x[i];
|
|
|
|
j = i-1;
|
|
while ((j>=0) && (x[j]>currentValue)) {
|
|
x[j+1] = x[j];
|
|
j--;
|
|
}
|
|
|
|
x[j+1] = currentValue;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/* getMinInArray : get the minimum value from an array */
|
|
/* parameters : */
|
|
/* -(i) x: the array to be searched */
|
|
/* -(i) length: the array length */
|
|
/* returns : the minimum value found in the array */
|
|
/* */
|
|
/*****************************************************************************/
|
|
word16_t getMinInArray(word16_t x[], int length) {
|
|
word16_t min = MAXINT16;
|
|
int i;
|
|
for (i=0; i<length; i++) {
|
|
if (x[i]<min) min = x[i];
|
|
}
|
|
return min;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/* computeParity : compute parity for pitch delay adaptative codebook index */
|
|
/* XOR of the 6 MSB (pitchDelay on 8 bits) */
|
|
/* parameters : */
|
|
/* -(i) adaptativeCodebookIndex: the pitch delay on 8 bits */
|
|
/* return value : */
|
|
/* the parity bit */
|
|
/* */
|
|
/*****************************************************************************/
|
|
uint16_t computeParity(uint16_t adaptativeCodebookIndex)
|
|
{
|
|
int i;
|
|
uint16_t parity = 1;
|
|
adaptativeCodebookIndex = SHR(adaptativeCodebookIndex,2); /* ignore the two LSB */
|
|
|
|
for (i=0; i<6; i++) {
|
|
parity ^= adaptativeCodebookIndex&(uint16_t)1; /* XOR with the LSB */
|
|
adaptativeCodebookIndex = SHR(adaptativeCodebookIndex,1);
|
|
}
|
|
|
|
return parity;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/* rearrangeCoefficients: rearrange coefficients according to spec 3.2.4 */
|
|
/* Have a minimum distance of J beetwen two consecutive coefficients */
|
|
/* parameters: */
|
|
/* -(i/o) qLSP: 10 ordered coefficients in Q13 replaced by new values */
|
|
/* if needed */
|
|
/* -(i) J: minimum distance between coefficients in Q0.13 (10 or 5) */
|
|
/* */
|
|
/*****************************************************************************/
|
|
void rearrangeCoefficients(word16_t qLSP[], word16_t J)
|
|
{ /* qLSP in Q2.13 and J in Q0.13(fitting on 4 bits: possible values 10 and 5) */
|
|
int i=1;
|
|
word16_t delta; /* in Q0.13 */
|
|
|
|
for (i=1; i<NB_LSP_COEFF; i++) {
|
|
delta = (ADD16(SUB16(qLSP[i-1],qLSP[i]),J))/2; /* delta = (l[i-1] - l[i] +J)/2 */
|
|
if (delta > 0) {
|
|
qLSP[i-1] = SUB16(qLSP[i-1], delta); /* qLSP still in Q2.13 */
|
|
qLSP[i] = ADD16(qLSP[i], delta);
|
|
}
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/* synthesisFilter : compute 1/[A(z)] using the following algorithm */
|
|
/* filteredSignal[n] = inputSignal[n] */
|
|
/* - Sum(i=1..10)filterCoefficients[i]*filteredSignal[n-i] */
|
|
/* for n in [0, L_SUBFRAME[ */
|
|
/* parameters: */
|
|
/* -(i) inputSignal: 40 values in Q0 */
|
|
/* -(i) filterCoefficients: 10 coefficients in Q12 */
|
|
/* -(i/o) filteredSignal: 50 values in Q0 accessed in ranges [-10,-1] */
|
|
/* as input and [0, 39] as output. */
|
|
/* */
|
|
/*****************************************************************************/
|
|
void synthesisFilter(word16_t inputSignal[], word16_t filterCoefficients[], word16_t filteredSignal[])
|
|
{
|
|
int i;
|
|
for (i=0; i<L_SUBFRAME; i++) {
|
|
word32_t acc = SHL(inputSignal[i],12); /* acc get the first term of the sum, in Q12 (inputSignal is in Q0)*/
|
|
int j;
|
|
for (j=0; j<NB_LSP_COEFF; j++) {
|
|
acc = MSU16_16(acc, filterCoefficients[j], filteredSignal[i-j-1]); /* filterCoefficients in Q12 and signal in Q0 -> acc in Q12 */
|
|
}
|
|
filteredSignal[i] = (word16_t)SATURATE(PSHR(acc, 12), MAXINT16); /* shift right acc to get it back in Q0 and check overflow on 16 bits */
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/* correlateVectors : compute the correlations between two vectors of */
|
|
/* L_SUBFRAME length: c[i] = Sum(x[j]*y[j-i]) j in [i, L_SUBFRAME[ */
|
|
/* parameters: */
|
|
/* -(i) x : L_SUBFRAME length input vector on 16 bits */
|
|
/* -(i) y : L_SUBFRAME length input vector on 16 bits */
|
|
/* -(o) c : L_SUBFRAME length output vector on 32 bits */
|
|
/* */
|
|
/*****************************************************************************/
|
|
void correlateVectors (word16_t x[], word16_t y[], word32_t c[])
|
|
{
|
|
int i,j;
|
|
for (i=0; i<L_SUBFRAME; i++) {
|
|
c[i] = 0;
|
|
for (j=i; j<L_SUBFRAME; j++) {
|
|
c[i] = MAC16_16(c[i], x[j], y[j-i]);
|
|
}
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
/*** gain related functions ***/
|
|
/*****************************************************************************/
|
|
/* MACodeGainPrediction : spec 3.9.1 */
|
|
/* parameters: */
|
|
/* -(i) previousGainPredictionError: qU(m) in eq69 4 values in Q10 */
|
|
/* -(i) fixedCodebookVector: the current subframe fixed codebook vector */
|
|
/* 40 values in Q1.13 */
|
|
/* return value : */
|
|
/* - predicted Fixed Codebook gain on 32 bits in Q16 range [3, 1830] */
|
|
/* */
|
|
/*****************************************************************************/
|
|
word32_t MACodeGainPrediction(word16_t *previousGainPredictionError, word16_t *fixedCodebookVector)
|
|
{
|
|
|
|
/***** algorithm described in spec 3.9.1 *****/
|
|
/*** first compute eq66: E = 10log10((Sum0-39(fixedCodebookVector^2))/40) ***/
|
|
/*** directly used in eq71 in E| - E = 30 - 10log10((Sum0-39(fixedCodebookVector^2))/40) */
|
|
/* = 30 - 10log10((Sum*2^26)/(40*2^26)) */
|
|
/* = 30 + 10log10(40*2^26) - (10/log2(10))*log2(Sum*2^26) [The Sum is computed in Q26] */
|
|
/* = 30 + 94.2884 - 3.0103*log2(Sum*2^26) */
|
|
/* = 124.2884 - 3.0103*log2(Sum*2^26) = E| - E as used in eq71 */
|
|
|
|
/* compute the sum of squares of fixedCodebookVector in Q26 */
|
|
int i;
|
|
word32_t fixedCodebookVectorSquaresSum = 0;
|
|
word32_t acc;
|
|
|
|
for (i=0; i<L_SUBFRAME; i++) {
|
|
if (fixedCodebookVector[i] != 0) { /* as most of the codebook vector is egal to 0, it worth checking it to avoid useless multiplications */
|
|
/* fixedCodebookVector in Q1.13 and final sum in range [4, 8.48] (4 values filled with abs values: 1,1,1.8 and 1.8 in the vector give max value of sum) */
|
|
/* fixedCodebookVectorSquaresSum is then in Q4.26 */
|
|
fixedCodebookVectorSquaresSum = MAC16_16(fixedCodebookVectorSquaresSum, fixedCodebookVector[i], fixedCodebookVector[i]);
|
|
}
|
|
}
|
|
|
|
/* compute E| - E as in eq71, result in Q16 */
|
|
/* acc = 124.2884 - 3.0103*log2(Sum) */
|
|
/* acc = 8145364[32 bits Q16] - 24660[16 bits Q2.13]*log2(Sum)[32 bits Q16] */
|
|
acc = MAC16_32_Q13(8145364, -24660, g729Log2_Q0Q16(fixedCodebookVectorSquaresSum)); /* acc in Q16 */
|
|
|
|
/* accumulate the MA prediction described in eq69 to the previous Sum, result will be in E~(m) + E| -E as used in eq71 */
|
|
/* acc in Q16->Q24, previousGainPredictionError in Q10 and MAPredictionCoefficients in Q0.14*/
|
|
acc = SHL(acc,8); /* acc in Q24 to match the fixed point of next accumulations */
|
|
for (i=0; i<4; i++) {
|
|
acc = MAC16_16(acc, previousGainPredictionError[i], MAPredictionCoefficients[i]);
|
|
}
|
|
/* acc range [10, 65] -> Q6.24 */
|
|
|
|
|
|
/* compute eq71, we already have the exposant in acc so */
|
|
/* g'c = 10^(acc/20) */
|
|
/* = 2^((acc*ln(10))/(20*ln(2))) */
|
|
/* = 2^(0,1661*acc) */
|
|
acc = SHR(acc,2); /* Q24->Q22 */
|
|
acc = MULT16_32_Q15(5442, acc); /* 5442 is 0.1661 in Q15 -> acc now in Q4.22 range [1.6, 10.8] */
|
|
acc = PSHR(acc,11); /* get acc in Q4.11 */
|
|
|
|
return g729Exp2_Q11Q16((word16_t)acc); /* acc fits on 16 bits, cast it to word16_t to send it to the exp2 function, output in Q16*/
|
|
}
|
|
|
|
|
|
/*****************************************************************************/
|
|
/* computeGainPredictionError : apply eq72 to compute current fixed Codebook */
|
|
/* gain prediction error and store the result in the adhoc array */
|
|
/* parameters : */
|
|
/* -(i) fixedCodebookGainCorrectionFactor: gamma in eq72 in Q3.12 */
|
|
/* -(i/o) previousGainPredictionError: array to be updated in Q10 */
|
|
/* */
|
|
/*****************************************************************************/
|
|
void computeGainPredictionError(word16_t fixedCodebookGainCorrectionFactor, word16_t *previousGainPredictionError)
|
|
{
|
|
/* need to compute eq72: 20log10(fixedCodebookGainCorrectionFactor) */
|
|
/* = (20/log2(10))*log2(fixedCodebookGainCorrectionFactor) */
|
|
/* = 6.0206*log2(fixedCodebookGainCorrectionFactor) */
|
|
/* log2 input in Q0, output in Q16,fixedCodebookGainCorrectionFactor being in Q12, we shall substract 12 in Q16(786432) to the result of log2 function -> final result in Q2.16 */
|
|
word32_t currentGainPredictionError = SUB32(g729Log2_Q0Q16(fixedCodebookGainCorrectionFactor), 786432);
|
|
currentGainPredictionError = PSHR(MULT16_32_Q12(24660, currentGainPredictionError),6); /* 24660 = 6.0206 in Q3.12 -> mult result in Q16, precise shift right to get it in Q4.10 */
|
|
|
|
/* shift the array and insert the current Prediction Error */
|
|
previousGainPredictionError[3] = previousGainPredictionError[2];
|
|
previousGainPredictionError[2] = previousGainPredictionError[1];
|
|
previousGainPredictionError[1] = previousGainPredictionError[0];
|
|
previousGainPredictionError[0] = (word16_t)currentGainPredictionError;
|
|
}
|
|
|
|
|
|
/*** bitStream to parameters Array conversions functions ***/
|
|
/* Note: these functions are in utils.c because used by test source code too */
|
|
|
|
/*****************************************************************************/
|
|
/* parametersArray2BitStream : convert array of parameters to bitStream */
|
|
/* according to spec 4 - Table 8 and following mapping of values */
|
|
/* 0 -> L0 (1 bit) */
|
|
/* 1 -> L1 (7 bits) */
|
|
/* 2 -> L2 (5 bits) */
|
|
/* 3 -> L3 (5 bits) */
|
|
/* 4 -> P1 (8 bit) */
|
|
/* 5 -> P0 (1 bits) */
|
|
/* 6 -> C1 (13 bits) */
|
|
/* 7 -> S1 (4 bits) */
|
|
/* 8 -> GA1(3 bits) */
|
|
/* 9 -> GB1(4 bits) */
|
|
/* 10 -> P2 (5 bits) */
|
|
/* 11 -> C2 (13 bits) */
|
|
/* 12 -> S2 (4 bits) */
|
|
/* 13 -> GA2(3 bits) */
|
|
/* 14 -> GB2(4 bits) */
|
|
/* parameters: */
|
|
/* -(i) parameters : 16 values parameters array */
|
|
/* -(o) bitStream : the 16 values streamed on 80 bits in a */
|
|
/* 10*8bits values array */
|
|
/* */
|
|
/*****************************************************************************/
|
|
void parametersArray2BitStream(uint16_t parameters[], uint8_t bitStream[])
|
|
{
|
|
bitStream[0] = ((parameters[0]&((uint16_t) 0x1))<<7) |
|
|
(parameters[1]&((uint16_t) 0x7f));
|
|
|
|
bitStream[1] = ((parameters[2]&((uint16_t) 0x1f))<<3) |
|
|
((parameters[3]>>2)&((uint16_t) 0x7));
|
|
|
|
bitStream[2] = ((parameters[3]&((uint16_t) 0x3))<<6) |
|
|
((parameters[4]>>2)&((uint16_t) 0x3f));
|
|
|
|
bitStream[3] = ((parameters[4]&((uint16_t) 0x3))<<6) |
|
|
((parameters[5]&((uint16_t) 0x1))<<5) |
|
|
((parameters[6]>>8)&((uint16_t) 0x1f));
|
|
|
|
bitStream[4] = ((parameters[6])&((uint16_t) 0xff));
|
|
|
|
bitStream[5] = ((parameters[7]&((uint16_t) 0xf))<<4) |
|
|
((parameters[8]&((uint16_t) 0x7))<<1) |
|
|
((parameters[9]>>3)&((uint16_t) 0x1));
|
|
|
|
bitStream[6] = ((parameters[9]&((uint16_t) 0x7))<<5) |
|
|
(parameters[10]&((uint16_t) 0x1f));
|
|
|
|
bitStream[7] = ((parameters[11]>>5)&((uint16_t) 0xff));
|
|
|
|
bitStream[8] = ((parameters[11]&((uint16_t) 0x1f))<<3) |
|
|
((parameters[12]>>1)&((uint16_t) 0x7));
|
|
|
|
bitStream[9] = ((parameters[12]&((uint16_t) 0x1))<<7) |
|
|
((parameters[13]&((uint16_t) 0x7))<<4) |
|
|
(parameters[14]&((uint16_t) 0xf));
|
|
|
|
return;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/* CNGparametersArray2BitStream : convert array of parameters to bitStream */
|
|
/* according to spec B4.3 - Table B2 and following mapping of values */
|
|
/* 0 -> L0 (1 bit) */
|
|
/* 1 -> L1 (5 bits) */
|
|
/* 2 -> L2 (4 bits) */
|
|
/* 3 -> Gain (5 bits) */
|
|
/* parameters: */
|
|
/* -(i) parameters : 4 values parameters array */
|
|
/* -(o) bitStream : the 4 values streamed on 15 bits in a */
|
|
/* 2*8bits values array */
|
|
/* */
|
|
/*****************************************************************************/
|
|
|
|
void CNGparametersArray2BitStream(uint16_t parameters[], uint8_t bitStream[]) {
|
|
bitStream[0] = ((parameters[0]&((uint16_t) 0x1))<<7) |
|
|
((parameters[1]&((uint16_t) 0x1f))<<2) |
|
|
((parameters[2]>>2)&((uint16_t) 0x3));
|
|
|
|
bitStream[1] = ((parameters[2]&((uint16_t) 0x03))<<6) |
|
|
((parameters[3]&((uint16_t) 0x1f))<<1);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/* parametersArray2BitStream : convert bitStream to an array of parameters */
|
|
/* reverse operation of previous funtion */
|
|
/* parameters: */
|
|
/* -(i) bitStream : the 16 values streamed on 80 bits in a */
|
|
/* 10*8bits values array */
|
|
/* -(o) parameters : 16 values parameters array */
|
|
/* */
|
|
/*****************************************************************************/
|
|
void parametersBitStream2Array(uint8_t bitStream[], uint16_t parameters[])
|
|
{
|
|
parameters[0] = (bitStream[0]>>7)&(uint16_t)0x1;
|
|
parameters[1] = bitStream[0]&(uint16_t)0x7f;
|
|
parameters[2] = (bitStream[1]>>3)&(uint16_t)0x1f;
|
|
parameters[3] = (((uint16_t)bitStream[1]&(uint16_t)0x7)<<2) | ((bitStream[2]>>6)&(uint16_t)0x3);
|
|
parameters[4] = (((uint16_t)bitStream[2])&(uint16_t)0x3f)<<2 | ((bitStream[3]>>6)&(uint16_t)0x3);;
|
|
parameters[5] = (bitStream[3]>>5)&(uint16_t)0x1;
|
|
parameters[6] = (((uint16_t)(bitStream[3]&(uint16_t)0x1f))<<8)| bitStream[4];
|
|
parameters[7] = (bitStream[5]>>4)&(uint16_t)0xf;
|
|
parameters[8] = (bitStream[5]>>1)&(uint16_t)0x7;
|
|
parameters[9] = (((uint16_t)bitStream[5]&(uint16_t)0x1)<<3)|((bitStream[6]>>5)&(uint16_t)0x7);
|
|
parameters[10]= (uint16_t)bitStream[6]&(uint16_t)0x1f;
|
|
parameters[11]= (((uint16_t)bitStream[7])<<5)|((bitStream[8]>>3)&(uint16_t)0x1f);
|
|
parameters[12]= ((bitStream[8]&(uint16_t)0x7)<<1) | ((bitStream[9]>>7)&(uint16_t)0x1);
|
|
parameters[13]= (bitStream[9]>>4)&(uint16_t)0x7;
|
|
parameters[14]= bitStream[9]&(uint16_t)0xf;
|
|
|
|
return;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/* pseudoRandom : generate pseudo random number as in spec 4.4.4 eq96 */
|
|
/* parameters: */
|
|
/* -(i/o) randomGeneratorSeed(updated by this function) */
|
|
/* return value : */
|
|
/* - a unsigned 16 bits pseudo random number */
|
|
/* */
|
|
/*****************************************************************************/
|
|
uint16_t pseudoRandom(uint16_t *randomGeneratorSeed)
|
|
{
|
|
/* pseudoRandomSeed is stored in an uint16_t var, we shall not worry about overflow here */
|
|
/* pseudoRandomSeed*31821 + 13849; */
|
|
*randomGeneratorSeed = MAC16_16(13849, (*randomGeneratorSeed), 31821);
|
|
return *randomGeneratorSeed;
|
|
}
|