memory leaks: Memory leak cleanup patch by Corey Farrell (second set)
[asterisk/asterisk.git] / codecs / ilbc / doCPLC.c
1
2    /******************************************************************
3
4        iLBC Speech Coder ANSI-C Source Code
5
6        doCPLC.c
7
8        Copyright (C) The Internet Society (2004).
9        All Rights Reserved.
10
11    ******************************************************************/
12
13    #include <math.h>
14    #include <string.h>
15    #include <stdio.h>
16
17
18
19
20
21    #include "iLBC_define.h"
22
23    /*----------------------------------------------------------------*
24     *  Compute cross correlation and pitch gain for pitch prediction
25     *  of last subframe at given lag.
26     *---------------------------------------------------------------*/
27
28    void compCorr(
29        float *cc,      /* (o) cross correlation coefficient */
30        float *gc,      /* (o) gain */
31        float *pm,
32        float *buffer,  /* (i) signal buffer */
33        int lag,    /* (i) pitch lag */
34        int bLen,       /* (i) length of buffer */
35        int sRange      /* (i) correlation search length */
36    ){
37        int i;
38        float ftmp1, ftmp2, ftmp3;
39
40        /* Guard against getting outside buffer */
41        if ((bLen-sRange-lag)<0) {
42            sRange=bLen-lag;
43        }
44
45        ftmp1 = 0.0;
46        ftmp2 = 0.0;
47        ftmp3 = 0.0;
48        for (i=0; i<sRange; i++) {
49            ftmp1 += buffer[bLen-sRange+i] *
50                buffer[bLen-sRange+i-lag];
51            ftmp2 += buffer[bLen-sRange+i-lag] *
52                    buffer[bLen-sRange+i-lag];
53            ftmp3 += buffer[bLen-sRange+i] *
54                    buffer[bLen-sRange+i];
55        }
56
57        if (ftmp2 > 0.0) {
58            *cc = ftmp1*ftmp1/ftmp2;
59            *gc = (float)fabs(ftmp1/ftmp2);
60            *pm=(float)fabs(ftmp1)/
61                ((float)sqrt(ftmp2)*(float)sqrt(ftmp3));
62        }
63        else {
64            *cc = 0.0;
65            *gc = 0.0;
66            *pm=0.0;
67        }
68    }
69
70
71
72
73
74    /*----------------------------------------------------------------*
75     *  Packet loss concealment routine. Conceals a residual signal
76     *  and LP parameters. If no packet loss, update state.
77     *---------------------------------------------------------------*/
78
79    void doThePLC(
80        float *PLCresidual, /* (o) concealed residual */
81        float *PLClpc,      /* (o) concealed LP parameters */
82        int PLI,        /* (i) packet loss indicator
83                                   0 - no PL, 1 = PL */
84        float *decresidual, /* (i) decoded residual */
85        float *lpc,         /* (i) decoded LPC (only used for no PL) */
86        int inlag,          /* (i) pitch lag */
87        iLBC_Dec_Inst_t *iLBCdec_inst
88                            /* (i/o) decoder instance */
89    ){
90        int lag=20, randlag;
91        float gain, maxcc;
92        float use_gain;
93        float gain_comp, maxcc_comp, per, max_per=0;
94        int i, pick, use_lag;
95        float ftmp, randvec[BLOCKL_MAX], pitchfact, energy;
96
97        /* Packet Loss */
98
99        if (PLI == 1) {
100
101            iLBCdec_inst->consPLICount += 1;
102
103            /* if previous frame not lost,
104               determine pitch pred. gain */
105
106            if (iLBCdec_inst->prevPLI != 1) {
107
108                /* Search around the previous lag to find the
109                   best pitch period */
110
111                lag=inlag-3;
112                compCorr(&maxcc, &gain, &max_per,
113                    iLBCdec_inst->prevResidual,
114                    lag, iLBCdec_inst->blockl, 60);
115                for (i=inlag-2;i<=inlag+3;i++) {
116                    compCorr(&maxcc_comp, &gain_comp, &per,
117                        iLBCdec_inst->prevResidual,
118                        i, iLBCdec_inst->blockl, 60);
119
120                    if (maxcc_comp>maxcc) {
121                        maxcc=maxcc_comp;
122
123
124
125
126
127                        gain=gain_comp;
128                        lag=i;
129                        max_per=per;
130                    }
131                }
132
133            }
134
135            /* previous frame lost, use recorded lag and periodicity */
136
137            else {
138                lag=iLBCdec_inst->prevLag;
139                max_per=iLBCdec_inst->per;
140            }
141
142            /* downscaling */
143
144            use_gain=1.0;
145            if (iLBCdec_inst->consPLICount*iLBCdec_inst->blockl>320)
146                use_gain=(float)0.9;
147            else if (iLBCdec_inst->consPLICount*
148                            iLBCdec_inst->blockl>2*320)
149                use_gain=(float)0.7;
150            else if (iLBCdec_inst->consPLICount*
151                            iLBCdec_inst->blockl>3*320)
152                use_gain=(float)0.5;
153            else if (iLBCdec_inst->consPLICount*
154                            iLBCdec_inst->blockl>4*320)
155                use_gain=(float)0.0;
156
157            /* mix noise and pitch repeatition */
158            ftmp=(float)sqrt(max_per);
159            if (ftmp>(float)0.7)
160                pitchfact=(float)1.0;
161            else if (ftmp>(float)0.4)
162                pitchfact=(ftmp-(float)0.4)/((float)0.7-(float)0.4);
163            else
164                pitchfact=0.0;
165
166
167            /* avoid repetition of same pitch cycle */
168            use_lag=lag;
169            if (lag<80) {
170                use_lag=2*lag;
171            }
172
173            /* compute concealed residual */
174
175
176
177
178
179
180            energy = 0.0;
181            for (i=0; i<iLBCdec_inst->blockl; i++) {
182
183                /* noise component */
184
185                iLBCdec_inst->seed=(iLBCdec_inst->seed*69069L+1) &
186                    (0x80000000L-1);
187                randlag = 50 + ((signed long) iLBCdec_inst->seed)%70;
188                pick = i - randlag;
189
190                if (pick < 0) {
191                    randvec[i] =
192                        iLBCdec_inst->prevResidual[
193                                    iLBCdec_inst->blockl+pick];
194                } else {
195                    randvec[i] =  randvec[pick];
196                }
197
198                /* pitch repeatition component */
199                pick = i - use_lag;
200
201                if (pick < 0) {
202                    PLCresidual[i] =
203                        iLBCdec_inst->prevResidual[
204                                    iLBCdec_inst->blockl+pick];
205                } else {
206                    PLCresidual[i] = PLCresidual[pick];
207                }
208
209                /* mix random and periodicity component */
210
211                if (i<80)
212                    PLCresidual[i] = use_gain*(pitchfact *
213                                PLCresidual[i] +
214                                ((float)1.0 - pitchfact) * randvec[i]);
215                else if (i<160)
216                    PLCresidual[i] = (float)0.95*use_gain*(pitchfact *
217                                PLCresidual[i] +
218                                ((float)1.0 - pitchfact) * randvec[i]);
219                else
220                    PLCresidual[i] = (float)0.9*use_gain*(pitchfact *
221                                PLCresidual[i] +
222                                ((float)1.0 - pitchfact) * randvec[i]);
223
224                energy += PLCresidual[i] * PLCresidual[i];
225            }
226
227            /* less than 30 dB, use only noise */
228
229
230
231
232
233
234            if (sqrt(energy/(float)iLBCdec_inst->blockl) < 30.0) {
235                gain=0.0;
236                for (i=0; i<iLBCdec_inst->blockl; i++) {
237                    PLCresidual[i] = randvec[i];
238                }
239            }
240
241            /* use old LPC */
242
243            memcpy(PLClpc,iLBCdec_inst->prevLpc,
244                (LPC_FILTERORDER+1)*sizeof(float));
245
246        }
247
248        /* no packet loss, copy input */
249
250        else {
251            memcpy(PLCresidual, decresidual,
252                iLBCdec_inst->blockl*sizeof(float));
253            memcpy(PLClpc, lpc, (LPC_FILTERORDER+1)*sizeof(float));
254            iLBCdec_inst->consPLICount = 0;
255        }
256
257        /* update state */
258
259        if (PLI) {
260            iLBCdec_inst->prevLag = lag;
261            iLBCdec_inst->per=max_per;
262        }
263
264        iLBCdec_inst->prevPLI = PLI;
265        memcpy(iLBCdec_inst->prevLpc, PLClpc,
266            (LPC_FILTERORDER+1)*sizeof(float));
267        memcpy(iLBCdec_inst->prevResidual, PLCresidual,
268            iLBCdec_inst->blockl*sizeof(float));
269    }
270