diff --git a/include/fixedPointMacros.h b/include/fixedPointMacros.h index bf4bf10..aa1493a 100644 --- a/include/fixedPointMacros.h +++ b/include/fixedPointMacros.h @@ -28,6 +28,7 @@ /*** shifts ***/ #define SHR(a,shift) ((a) >> (shift)) #define SHL(a,shift) ((word32_t)(a) << (shift)) +#define USHL(a,shift) ((uword32_t)(a) << (shift)) /* shift right with rounding: used to extract the integer value of a Qa number */ #define PSHR(a,shift) (SHR((a)+((EXTEND32(1)<<((shift))>>1)),shift)) /* shift right with checking on sign of shift value */ @@ -53,13 +54,15 @@ /* WARNING: MULT16_32_QX use MULT16_16 macro but the first multiplication must actually be a 16bits * 32bits with result on 32 bits and not a 16*16 */ /* MULT16_16 is then implemented here as a 32*32 bits giving result on 32 bits */ #define MULT16_16(a,b) ((word32_t)((word32_t)(a))*((word32_t)(b))) +#define UMULT16_16(a,b) ((uword32_t)((word32_t)(a))*((word32_t)(b))) #define MAC16_16(c,a,b) (ADD32((c),MULT16_16((a),(b)))) #define MSU16_16(c,a,b) (SUB32((c),MULT16_16((a),(b)))) #define DIV32(a,b) (((word32_t)(a))/((word32_t)(b))) +#define UDIV32(a,b) (((uword32_t)(a))/((uword32_t)(b))) -/* Q4 operations */ -#define MULT16_16_Q4(a,b) (SHR(MULT16_16((a),(b)),4)) -#define MAC16_16_Q4(c,a,b) ADD32(c,MULT16_16_Q4(a,b)) +/* Unsigned Q4 operations */ +#define UMULT16_16_Q4(a,b) (SHR(UMULT16_16((a),(b)),4)) +#define UMAC16_16_Q4(c,a,b) ADD32(c,UMULT16_16_Q4(a,b)) /* Q11 operations */ #define MULT16_16_Q11(a,b) (SHR(MULT16_16((a),(b)),11)) diff --git a/include/g729FixedPointMath.h b/include/g729FixedPointMath.h index 72ace47..a18dd0b 100644 --- a/include/g729FixedPointMath.h +++ b/include/g729FixedPointMath.h @@ -127,21 +127,20 @@ static BCG729_INLINE word32_t g729Exp2_Q11Q16(word16_t x) #define C3 4204 /*****************************************************************************/ /* g729Sqrt_Q0Q7 : Square root */ -/* x is not tested to be >=0, shall be done by caller function */ /* paremeters: */ -/* -(i) x : 32 bits integer in Q0 */ +/* -(i) x : 32 bits unsigned integer in Q0 */ /* return value: */ -/* - sqrt(x) in Q7 on 32 bits */ +/* - sqrt(x) in Q7 on 32 bits signed integer */ /* */ /*****************************************************************************/ -static BCG729_INLINE word32_t g729Sqrt_Q0Q7(word32_t x) +static BCG729_INLINE word32_t g729Sqrt_Q0Q7(uword32_t x) { int k; word32_t rt; if (x==0) return 0; /* set x in Q14 in range [0.25,1[ */ - k = (18-countLeadingZeros(x))>>1; + k = (19-unsignedCountLeadingZeros(x))>>1; x = VSHR32(x, (k<<1)); /* x = x.2^-2k */ /* sqrt(x) ~= 0.22178 + 1.29227*x - 0.77070*x^2 + 0.25659*x^3 (for .25 < x < 1) */ diff --git a/include/utils.h b/include/utils.h index c4ace84..8f07c60 100644 --- a/include/utils.h +++ b/include/utils.h @@ -105,6 +105,27 @@ static BCG729_INLINE uint16_t countLeadingZeros(word32_t x) return leadingZeros; } +/*****************************************************************************/ +/* unsignedCountLeadingZeros : return the number of zero heading the argument*/ +/* May be replaced by one asm instruction. */ +/* parameters : */ +/* -(i) x : 32 bits unsigned int values */ +/* return value : */ +/* - number of heading zeros(MSB included. Ex: 0x0080 00000 returns 8) */ +/* */ +/*****************************************************************************/ +static BCG729_INLINE uint16_t unsignedCountLeadingZeros(uword32_t x) +{ + uint16_t leadingZeros = 0; + if (x==0) return 32; + while ((x&0x80000000)!=(uword32_t)0x80000000) { + leadingZeros++; + x <<=1; + } + return leadingZeros; +} + + /*** gain related functions ***/ /*****************************************************************************/ /* MACodeGainPrediction : spec 3.9.1 */ diff --git a/src/postFilter.c b/src/postFilter.c index df66cf3..109ea88 100644 --- a/src/postFilter.c +++ b/src/postFilter.c @@ -77,7 +77,7 @@ void postFilter(bcg729DecoderChannelContextStruct *decoderChannelContext, word16 word32_t rh1; word16_t tiltCompensatedSignal[L_SUBFRAME]; /* in Q0 */ word16_t gainScalingFactor; /* in Q12 */ - word32_t shortTermFilteredResidualSignalSquareSum = 0; + uword32_t shortTermFilteredResidualSignalSquareSum = 0; /********************************************************************/ /* Long Term Post Filter */ @@ -281,9 +281,9 @@ void postFilter(bcg729DecoderChannelContextStruct *decoderChannelContext, word16 /********************************************************************/ /*** compute G(gain scaling factor) according to eqA15 : G = Sqrt((∑s(n)^2)/∑sf(n)^2 ) ***/ - /* compute ∑sf(n)^2 scale the signal shifting left by 2 to avoid overflow on 32 bits sum */ + /* compute ∑sf(n)^2, scale the signal shifting right by 4 to avoid possible overflow on 32 bits sum */ for (i=0; ishortTermFilteredResidualSignal[i], decoderChannelContext->shortTermFilteredResidualSignal[i]); + shortTermFilteredResidualSignalSquareSum = UMAC16_16_Q4(shortTermFilteredResidualSignalSquareSum, decoderChannelContext->shortTermFilteredResidualSignal[i], decoderChannelContext->shortTermFilteredResidualSignal[i]); /* inputs are both in Q0, output is in Q-4 */ } /* if the sum is null we can't compute gain -> output of postfiltering is the output of shortTermFilter and previousAdaptativeGain is set to 0 */ @@ -295,30 +295,30 @@ void postFilter(bcg729DecoderChannelContextStruct *decoderChannelContext, word16 } } else { /* we can compute adaptativeGain and output signal */ word16_t currentAdaptativeGain; - /* compute ∑s(n)^2 scale the signal shifting left by 2 to avoid overflow on 32 bits sum */ - word32_t reconstructedSpeechSquareSum = 0; + /* compute ∑s(n)^2 scale the signal shifting right by 4 to avoid possible overflow on 32 bits sum, same shift was applied at denominator */ + uword32_t reconstructedSpeechSquareSum = 0; for (i=0; i current gain is null */ gainScalingFactor = 0; } else { - word32_t fractionResult; /* stores ∑s(n)^2)/∑sf(n)^2 */ + uword32_t fractionResult; /* stores ∑s(n)^2)/∑sf(n)^2 in Q10 on a 32 bit unsigned */ word32_t scaledShortTermFilteredResidualSignalSquareSum; /* Compute ∑s(n)^2)/∑sf(n)^2 result shall be in Q10 */ /* normalise the numerator on 32 bits */ - word16_t numeratorShift = countLeadingZeros(reconstructedSpeechSquareSum); - reconstructedSpeechSquareSum = SHL(reconstructedSpeechSquareSum, numeratorShift); /* reconstructedSpeechSquareSum*2^numeratorShift */ + word16_t numeratorShift = unsignedCountLeadingZeros(reconstructedSpeechSquareSum); + reconstructedSpeechSquareSum = USHL(reconstructedSpeechSquareSum, numeratorShift); /* reconstructedSpeechSquareSum*2^numeratorShift */ /* normalise denominator to get the result directly in Q10 if possible */ scaledShortTermFilteredResidualSignalSquareSum = VSHR32(shortTermFilteredResidualSignalSquareSum, 10-numeratorShift); /* shortTermFilteredResidualSignalSquareSum*2^(numeratorShift-10)*/ if (scaledShortTermFilteredResidualSignalSquareSum==0) {/* shift might have sent to zero the denominator */ - fractionResult = DIV32(reconstructedSpeechSquareSum, shortTermFilteredResidualSignalSquareSum); /* result in QnumeratorShift */ + fractionResult = UDIV32(reconstructedSpeechSquareSum, shortTermFilteredResidualSignalSquareSum); /* result in QnumeratorShift */ fractionResult = VSHR32(fractionResult, numeratorShift-10); /* result in Q10 */ } else { /* ok denominator is still > 0 */ - fractionResult = DIV32(reconstructedSpeechSquareSum, scaledShortTermFilteredResidualSignalSquareSum); /* result in Q10 */ + fractionResult = UDIV32(reconstructedSpeechSquareSum, scaledShortTermFilteredResidualSignalSquareSum); /* result in Q10 */ } /* now compute current Gain = Sqrt((∑s(n)^2)/∑sf(n)^2 ) */ /* g729Sqrt_Q0Q7(Q0)->Q7, by giving a Q10 as input, output is in Q12 */