cng.c 19.2 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
/*
 cng.c
 Comfort Noise Generation

 Copyright (C) 2015 Belledonne Communications, Grenoble, France
 Author : Johan Pascal
 
 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 2
 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, write to the Free Software
20
 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36
 */
#include <string.h>
#include <stdlib.h>

#include "typedef.h"
#include "codecParameters.h"
#include "basicOperationsMacros.h"
#include "utils.h"

#include "cng.h"
#include "decodeAdaptativeCodeVector.h"
#include "decodeLSP.h"
#include "interpolateqLSP.h"
#include "qLSP2LP.h"
#include "g729FixedPointMath.h"
#include "codebooks.h"
37
#include "LP2LSPConversion.h"
38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234

/* buffers allocation */
static const word16_t SIDqLSPInitialValues[NB_LSP_COEFF] = {31441,  27566,  21458,  13612,   4663, -4663, -13612, -21458, -27566, -31441}; /* in Q0.15 the initials values for the previous qLSP buffer */

bcg729CNGChannelContextStruct *initBcg729CNGChannel() {
	/* create the context structure */
	bcg729CNGChannelContextStruct *CNGChannelContext = malloc(sizeof(bcg729CNGChannelContextStruct));
	memset(CNGChannelContext, 0, sizeof(*CNGChannelContext));

	memcpy(CNGChannelContext->qLSP, SIDqLSPInitialValues, NB_LSP_COEFF*sizeof(word16_t)); /* initialise the previousqLSP buffer */

	return CNGChannelContext;
}

/*******************************************************************************************/
/* computeComfortNoiseExcitationVector : as is spec B4.4 and B4.5                          */
/*    parameters:                                                                          */
/*      -(i): targetGain : the gain from ea B.19 in Q3                                     */
/*      -(i/o): randomGeneratorSeed : used to get a pseudo random number, is updated       */
/*      -(i/o): excitationVector in Q0, accessed in range [-L_PAST_EXCITATION,L_FRAME-1]   */
/*                                                        [-154,79]                        */
/*                                                                                         */
/*******************************************************************************************/
void computeComfortNoiseExcitationVector(word16_t targetGain, uint16_t *randomGeneratorSeed, word16_t *excitationVector) {
	int16_t fracPitchDelay,intPitchDelay;
	uint16_t randomNumberBuffer;
	uint8_t subframeIndex = 0;
	int i,j;
	/* shall we check targetGain is 0?? and set excitation vector[0,L_FRAME[ to 0 in this case?*/

	for (subframeIndex=0; subframeIndex<L_FRAME; subframeIndex+=L_SUBFRAME) { /* process 2 subframes */
		word16_t gaussianRandomExcitation[L_SUBFRAME];
		word32_t Eg = 0;
		word32_t Gg = 0; /* gain to be applied to the randomly generated gaussian to reach a Eg of 1/4*L_SUBFRAME*targetGain(note this doesn't appears in spec, only in ITU code)*/
		word16_t Ga = 0; /* gain to be applied to adaptative subframe excitation in Q0.15 */
		word32_t Ea = 0;
		word32_t Ei = 0; /* inter excitation : Sum[0..39] adaptativeExcitation*fixedExcitation, defined in eq B21*/
		word32_t K = 0;
		word32_t Gf = 0; /* gain to be applied to the fixed codebook excitation */
		word32_t x2 = 0; /* second root of the 2nd degre equation solved to get Gf */
		word16_t sign[4]; /* sign of the impulses of fixed codebook excitation */
		word16_t position[4]; /* positions of the impulses of fixed codebook excitation */
		uint8_t deltaScaleFactor=0;
		word64_t delta;

		/* generate pseudo random pitch delay in range [40,103] for adaptative codebook */
		randomNumberBuffer = pseudoRandom(randomGeneratorSeed); /* get 16 bits of pseudoRandom value*/
		fracPitchDelay = (int16_t)(randomNumberBuffer&0x0003)-1; /* fraction part of the pitch delay shall be -1, 0 or 1 */
		if (fracPitchDelay==2) fracPitchDelay = 0;
		randomNumberBuffer = randomNumberBuffer>>2; /* 14 random bits left */
		intPitchDelay = (randomNumberBuffer&0x003F) + 40; /* intPitchDelay in [40,103] */
		randomNumberBuffer = randomNumberBuffer>>6; /* 8 random bits left */
		/* generate pseudo random sign and position for fixed codebook */
		position[0] = (randomNumberBuffer&0x0007)*5;
		randomNumberBuffer = randomNumberBuffer>>3; /* 5 random bits left */
		sign[0] = randomNumberBuffer&0x0001;
		randomNumberBuffer = randomNumberBuffer>>1; /* 4 random bits left */
		position[1] = (randomNumberBuffer&0x0007)*5+1;
		randomNumberBuffer = randomNumberBuffer>>3; /* 1 random bits left */
		sign[1] = randomNumberBuffer&0x0001,
		randomNumberBuffer = pseudoRandom(randomGeneratorSeed); /* get 16 bits of pseudoRandom value*/
		position[2] = (randomNumberBuffer&0x0007)*5+2;
		randomNumberBuffer = randomNumberBuffer>>3; /* 13 random bits left */
		sign[2] = randomNumberBuffer&0x0001;
		randomNumberBuffer = randomNumberBuffer>>1; /* 12 random bits left */
		position[3] = (randomNumberBuffer&0x0001)+3; /*j+3*/
		randomNumberBuffer = randomNumberBuffer>>1; /* 11 random bits left */
		position[3] += (randomNumberBuffer&0x0007)*5;
		randomNumberBuffer = randomNumberBuffer>>3; /* 8 random bits left */
		sign[3] = randomNumberBuffer&0x0001;
		randomNumberBuffer = randomNumberBuffer>>1; /* 7 random bits left */
		/* randomly generate Ga : adaptative gain eqB.22: max is 0,5  */
		Ga = (pseudoRandom(randomGeneratorSeed)&0x1fff)<<1; /* get 16 bits of pseudoRandom value but make sure it is < 0.5 in Q15 */



		/* generate gaussian random excitation : get generation algo from ITU code, no reference to this in the spec... 
		 * Compute also the subframe energy(Sum [0..39] excitation^2) and then scale the sample to get an subframe average energy to targetGain/4 */
		for (i=0; i<L_SUBFRAME; i++) {
			word32_t tmpBuffer=0;
			for (j=0; j<12; j++) {
				tmpBuffer=ADD32(tmpBuffer, (word16_t)pseudoRandom(randomGeneratorSeed)); /* cast the unsigned 16 bits value to a signed one */
			}
			gaussianRandomExcitation[i]=(word16_t)(SHR(tmpBuffer,7));
			Eg = MAC16_16(Eg, gaussianRandomExcitation[i], gaussianRandomExcitation[i]);
		}

		/* compute coefficient = 1/2*sqrt(L_SUBFRAME/gaussianRandomExcitationSubframeEnergy)*targetGain (always>0) - retrieved from ITU code, no mention of this in the spec */
    		/* comments in code say 1/4*sqrt() but code actually implement 1/2... */
		Gg = MULT16_32_Q15(GAUSSIAN_EXCITATION_COEFF_FACTOR, g729InvSqrt_Q0Q31(Eg)); /* multiplicand in Q1.13 and Q0.31 -> result in Q1.29 */
		Gg = MULT16_32_Q15(targetGain, Gg); /* multiplicand in Q3 and Q1.29 -> result in Q17 */

		for (i=0; i<L_SUBFRAME; i++) {
			/* operate on positive value only to avoid problem when shifting to 0 a negative one */
			if (gaussianRandomExcitation[i]<0) {
				gaussianRandomExcitation[i] = -SATURATE(PSHR( MULT16_32_Q15(-gaussianRandomExcitation[i], Gg), 2), MAXINT16); /* gaussianRandom in Q0, targetGain in Q17 -> result in Q2, need to shift it by 2. */
			} else {
				gaussianRandomExcitation[i] = PSHR( MULT16_32_Q15(gaussianRandomExcitation[i], Gg), 2);
			}
		}
		/* generate random adaptative excitation and apply random gain Ga */
		computeAdaptativeCodebookVector(excitationVector+subframeIndex, fracPitchDelay, intPitchDelay);

		for (i=0; i<L_SUBFRAME; i++) {
			excitationVector[i+subframeIndex] = SATURATE(MULT16_16_P15(excitationVector[i+subframeIndex], Ga), MAXINT16);
		}


		/* add gaussian excitation to the adaptative one */
		for (i=0; i<L_SUBFRAME; i++) {
			excitationVector[i+subframeIndex] = SATURATE(ADD32(excitationVector[i+subframeIndex], gaussianRandomExcitation[i]),MAXINT16);
		}
	
		/* Compute Gf: fixed excitation gain salving equation B.21 in form 4*Gf^2 + 2I*Ga*Gf + Ea*Ga^2 - K = 0 (4Gf^2 + 2bGf + c = 0)*/
		/* compute Ea*Ga^2 */
		for (i=0; i<L_SUBFRAME; i++) {
			Ea = MAC16_16(Ea, excitationVector[i+subframeIndex], excitationVector[i+subframeIndex]); /* Ea actually contains Ea*Ga^2 as we already applied gain Ga on adaptative excitation, in Q0 */
		}

		/* compute Ei = I*Ga : fixed codebook hasn't been scaled yet, so it just 4 signed pulses of height 1, just get these one in our computation, in Q0 */
		Ei = 0;
		for (i=0; i<4; i++) {
			if (sign[i] == 0) { /* negative impulse */
				Ei = SUB32(Ei,excitationVector[position[i]+subframeIndex]);
			} else { /* positive impulse */
				Ei = ADD32(Ei,excitationVector[position[i]+subframeIndex]);
			}
		}

		/* compute K = L_SUBRAME*targetGain in Q3 */
		K = MULT16_32(targetGain, SHR(MULT16_16(L_SUBFRAME, targetGain), 3));
		
		/* compute delta = b^2 -ac = Ei^2 + 4*(K-Ea) in Q0 */
		delta = ADD64(MULT32_32(Ei,Ei), SHR(SUB64(K,SHL64((word64_t)Ea,3)),1));

		if (delta<0) {
			/* cancel adaptative excitation, keep gaussian one only */
			for (i=0; i<L_SUBFRAME; i++) {
				excitationVector[i+subframeIndex] = gaussianRandomExcitation[i];
			}

			/* compute again Ei and delta = Ei^2 + 3/4*K (from ITU code, no idea why) */
			for (i=0; i<4; i++) {
				if (sign[i] == 0) { /* negative impulse */
					Ei = SUB32(Ei,excitationVector[position[i]+subframeIndex]);
				} else { /* positive impulse */
					Ei = ADD32(Ei,excitationVector[position[i]+subframeIndex]);
				}
			}
			delta = ADD64(MULT32_32(Ei,Ei), MULT16_32_P15(COEFF_K,K)); /* COEFF_K is 0.75 in Q15 */
		}

		/* scale delta to have it fitting on 32 bits */
		deltaScaleFactor = 0;
		while (delta>=0x0000000080000000) {
			delta = delta>>1;
			deltaScaleFactor++;
		}
		/* deltaScaleFactor must be even */
		if (deltaScaleFactor%2==1) {
			delta = delta >> 1;
			deltaScaleFactor++;
		}

		delta =  g729Sqrt_Q0Q7((word32_t)delta); /* delta in Q(7-deltaScaleFactor/2)*/

		/* scale b (Ei) to the same scale */
		Ei = VSHR32(Ei, deltaScaleFactor/2-7);

		/* compute the two roots and pick the one with smaller absolute value */
		/* roots are (-b +-sqrt(delta))/a. We always have a=4, divide by four when rescaling from Q(7-deltaScaleFactor) to Q0 the final result */
		Gf = SUB32(delta, Ei); /* x1 = -b + sqrt(delta) in Q(7-deltaScaleFactor) */
		x2 = -ADD32(delta, Ei); /* x2 = -b - sqrt(delta) in Q(7-deltaScaleFactor) */
		if (ABS(x2)<ABS(Gf)) Gf = x2; /* pick the smallest (in absolute value of the roots) */
		Gf = VSHR32(Gf, 2+7-deltaScaleFactor/2); /* scale back Gf in Q0 and divide by 4 (+2 in the right shift) to get the actual root */

		/* add fixed codebook excitation */
		for (i=0; i<4; i++) {
			if (sign[i] == 0) { /* negative impulse */
				excitationVector[position[i]+subframeIndex] = SUB32(excitationVector[position[i]+subframeIndex], Gf);
			} else { /* positive impulse */
				excitationVector[position[i]+subframeIndex] = ADD32(excitationVector[position[i]+subframeIndex], Gf);
			}
		}
	}
}

/*******************************************************************************************/
/* decodeSIDframe : as is spec B4.4 and B4.5                                               */
/*    first check if we have a SID frame or a missing/untransmitted frame                  */
/*    for SID frame get paremeters(gain and LSP)                                           */
/*    Then generate excitation vector and update qLSP                                      */
/*    parameters:                                                                          */
/*      -(i/o):CNGChannelContext : context containing all informations needed for CNG      */
/*      -(i): previousFrameIsActiveFlag: true if last decoded frame was an active one      */
/*      -(i): bitStream: for SID frame contains received params as in spec B4.3,           */
/*                       NULL for missing/untransmitted frame                              */
235
/*      -(i): bitStreamLength : in bytes, length of previous buffer                        */
236 237 238 239 240 241 242 243 244
/*      -(i/o): excitationVector in Q0, accessed in range [-L_PAST_EXCITATION,L_FRAME-1]   */
/*                                                        [-154,79]                        */
/*      -(o): LP: 20 LP coefficients in Q12                                                */
/*      -(i/o): previousqLSP : previous quantised LSP in Q0.15 (NB_LSP_COEFF values)       */
/*      -(i/o): pseudoRandomSeed : seed used in the pseudo random number generator         */
/*      -(i/o): previousLCodeWord: in Q2.13, buffer to store the last 4 frames codewords,  */
/*              used to compute the current qLSF                                           */
/*                                                                                         */
/*******************************************************************************************/
245
void decodeSIDframe(bcg729CNGChannelContextStruct *CNGChannelContext, uint8_t previousFrameIsActiveFlag, uint8_t *bitStream, uint8_t bitStreamLength, word16_t *excitationVector, word16_t *previousqLSP, word16_t *LP, uint16_t *pseudoRandomSeed, word16_t previousLCodeWord[MA_MAX_K][NB_LSP_COEFF], uint8_t rfc3389PayloadFlag) {
246 247 248 249
	int i;
	word16_t interpolatedqLSP[NB_LSP_COEFF]; /* interpolated qLSP in Q0.15 */
	/* if this is a SID frame, decode received parameters */
	if (bitStream!=NULL) {
250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298
		if (rfc3389PayloadFlag) {
			int j;
			word32_t LPCoefficients[NB_LSP_COEFF+1]; /* in Q4.27 */
			word16_t LPCoefficientsQ12[NB_LSP_COEFF]; /* in Q12 */
			word32_t previousIterationLPCoefficients[NB_LSP_COEFF+1]; /* in Q4.27 */
			uint8_t CNFilterOrder = (bitStreamLength-1); /* first byte is noise energy level */
			word16_t k[NB_LSP_COEFF];
			word32_t receivedSIDGainLog;

			if (CNFilterOrder>NB_LSP_COEFF) { /* if rfc3389 payload have a filter order > than supported, just ignore the last coefficients */
				CNFilterOrder = NB_LSP_COEFF;
			}
			/* retrieve gain from codebook according to Gain parameter */
			//CNGChannelContext->receivedSIDGain = SIDGainCodebook[bitStream[0]];
			/* received parameter is -(10*log10(meanE) -90) so retrieve mean energy : 10^((param+90)/10) */
			/* but expected parameter is a gain applied to single sample so sqrt of previous result */
			receivedSIDGainLog = ADD32(-bitStream[0], 90);
			if (receivedSIDGainLog > 66) {
				receivedSIDGainLog = 66; /* noise level shall not be too high in any case */
			} /* receivedSIDGainLog is param+90 in Q0 */
			receivedSIDGainLog = MULT16_16(receivedSIDGainLog, 680); /* 680 is 1/(10*log10(2)) in Q11 -> receivedSIDGainLog is ((param+90)/10)/log10(2) = log2(meanE) in Q11 */
			receivedSIDGainLog = g729Exp2_Q11Q16(receivedSIDGainLog); /* receivedSIDGainLog in meanE in Q16 */
			if (receivedSIDGainLog > 0 ) { /* avoid arithmetic problem if energy is too low */
				CNGChannelContext->receivedSIDGain = (word16_t)(SHR(g729Sqrt_Q0Q7(receivedSIDGainLog), 12)); /* output of sqrt in Q15, result needed in Q3  */
				if (CNGChannelContext->receivedSIDGain < SIDGainCodebook[0]) {
					CNGChannelContext->receivedSIDGain = SIDGainCodebook[0];
				}
			} else { /* minimum target gain extracted from codebook */
				CNGChannelContext->receivedSIDGain = SIDGainCodebook[0];
			}

			/* retrieve LP from rfc3389 payload*/
			/* rebuild the LP coefficients using algo given in G711 Appendix II section 5.2.1.3 */
			/* first get back the quantized reflection coefficients from Index as in RFC3389 3.2 */
			for (i=0; i<CNFilterOrder; i++) {
				k[i] = MULT16_16(ADD16(bitStream[i+1],127), 258); /* k in Q15 */
			}
			for (i=CNFilterOrder; i<NB_LSP_COEFF; i++) { /* rfc3389 payload may provide a filter of order < NB_LSP_COEFF */
				k[i] = 0; /* in that case, just set the last coefficients to 0 */
			}

			/* rebuild LP coefficients */
			LPCoefficients[0] = ONE_IN_Q27;
			LPCoefficients[1] = -SHL(k[0],12);
			for (i=2; i<NB_LSP_COEFF+1; i++) {
				/* update the previousIterationLPCoefficients needed for this one */
				for (j=1; j<i; j++) {
					previousIterationLPCoefficients[j] = LPCoefficients[j];
				}
299

300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337
				LPCoefficients[i] = -SHL(k[i-1],16); /* current coeff in Q31 while older one in Q27*/
				for (j=1; j<i; j++) {
					LPCoefficients[j] = MAC32_32_Q31(LPCoefficients[j], LPCoefficients[i], previousIterationLPCoefficients[i-j]); /*LPCoefficients in Q27 except for LPCoefficients[i] in Q31 */
				}
				LPCoefficients[i] = SHR(LPCoefficients[i], 4);
			}

			/* convert with rounding the LP Coefficients form Q27 to Q12, ignore first coefficient which is always 1 */
			for (i=0; i<NB_LSP_COEFF; i++) {
				LPCoefficientsQ12[i] = (word16_t)SATURATE(PSHR(LPCoefficients[i+1], 15), MAXINT16);
			}

			/* compute LSP from LP as we need to store LSP in context as the LP to use are interpolated with previous ones. Note: use LSP as qLSP */
			if (!LP2LSPConversion(LPCoefficientsQ12, CNGChannelContext->qLSP)) {
				/* unable to find the 10 roots repeat previous LSP */
				memcpy(CNGChannelContext->qLSP, previousqLSP, NB_LSP_COEFF*sizeof(word16_t));
			}
		} else { /* regular G729 SID payload on 2 bytes */
			word16_t currentqLSF[NB_LSP_COEFF]; /* buffer to the current qLSF in Q2.13 */
			uint8_t L0 = (bitStream[0]>>7)&0x01;
			uint8_t L1Index = (bitStream[0]>>2)&0x1F;
			uint8_t L2Index = ((bitStream[0]&0x03)<<2) | ((bitStream[1]>>6)&0x03);

			/* retrieve gain from codebook according to Gain parameter */
			CNGChannelContext->receivedSIDGain = SIDGainCodebook[((bitStream[1])>>1)&0x1F];
			/* Use L1 and L2(parameter for first and second stage vector of LSF quantizer) to retrieve LSP using subset index to address the complete L1 and L2L3 codebook */
			for (i=0; i<NB_LSP_COEFF/2; i++) {
				currentqLSF[i]=ADD16(L1[L1SubsetIndex[L1Index]][i], L2L3[L2SubsetIndex[L2Index]][i]);
			}
			for (i=NB_LSP_COEFF/2; i<NB_LSP_COEFF; i++) {
				currentqLSF[i]=ADD16(L1[L1SubsetIndex[L1Index]][i], L2L3[L3SubsetIndex[L2Index]][i]);
			}
			computeqLSF(currentqLSF, previousLCodeWord, L0, noiseMAPredictor, noiseMAPredictorSum);

			/* convert qLSF to qLSP: qLSP = cos(qLSF) */
			for (i=0; i<NB_LSP_COEFF; i++) {
				CNGChannelContext->qLSP[i] = g729Cos_Q13Q15(currentqLSF[i]); /* ouput in Q0.15 */
			}
338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365
		}
	} /* Note: Itu implementation have information to sort missing and untransmitted packets and perform reconstruction of missed SID packet when it detects it, we cannot differentiate lost vs untransmitted packet so we don't do it */

	/* compute the LP coefficients */
	interpolateqLSP(previousqLSP, CNGChannelContext->qLSP, interpolatedqLSP);
	/* copy the current qLSP(SID ones) to previousqLSP buffer */
	for (i=0; i<NB_LSP_COEFF; i++) {
		previousqLSP[i] = CNGChannelContext->qLSP[i];
	}

	/* call the qLSP2LP function for first subframe */
	qLSP2LP(interpolatedqLSP, LP);
	/* call the qLSP2LP function for second subframe */
	qLSP2LP(CNGChannelContext->qLSP, &(LP[NB_LSP_COEFF]));


	/* apply target gain smoothing eq B.19 */
	if (previousFrameIsActiveFlag) {
		/* this is the first SID frame use transmitted(or recomputed from last valid frame energy) SID gain */
		CNGChannelContext->smoothedSIDGain = CNGChannelContext->receivedSIDGain;
	} else { /* otherwise 7/8 of last smoothed gain and 1/8 of last received gain */
		CNGChannelContext->smoothedSIDGain = SUB16(CNGChannelContext->smoothedSIDGain, (CNGChannelContext->smoothedSIDGain>>3));
		CNGChannelContext->smoothedSIDGain = ADD16(CNGChannelContext->smoothedSIDGain, (CNGChannelContext->receivedSIDGain>>3));
	}

	/* get the excitation vector */
	computeComfortNoiseExcitationVector(CNGChannelContext->smoothedSIDGain, pseudoRandomSeed, excitationVector);
}