Version 0.1.0 from FTP
[asterisk/asterisk.git] / codecs / gsm / src / lpc.c
1 /*
2  * Copyright 1992 by Jutta Degener and Carsten Bormann, Technische
3  * Universitaet Berlin.  See the accompanying file "COPYRIGHT" for
4  * details.  THERE IS ABSOLUTELY NO WARRANTY FOR THIS SOFTWARE.
5  */
6
7 /* $Header$ */
8
9 #include <stdio.h>
10 #include <assert.h>
11
12 #include "private.h"
13
14 #include "gsm.h"
15 #include "proto.h"
16
17 #undef  P
18
19 /*
20  *  4.2.4 .. 4.2.7 LPC ANALYSIS SECTION
21  */
22
23 /* 4.2.4 */
24
25
26 static void Autocorrelation P2((s, L_ACF),
27         word     * s,           /* [0..159]     IN/OUT  */
28         longword * L_ACF)       /* [0..8]       OUT     */
29 /*
30  *  The goal is to compute the array L_ACF[k].  The signal s[i] must
31  *  be scaled in order to avoid an overflow situation.
32  */
33 {
34         register int    k, i;
35
36         word            temp, smax, scalauto;
37
38 #ifdef  USE_FLOAT_MUL
39         float           float_s[160];
40 #endif
41
42         /*  Dynamic scaling of the array  s[0..159]
43          */
44
45         /*  Search for the maximum.
46          */
47         smax = 0;
48         for (k = 0; k <= 159; k++) {
49                 temp = GSM_ABS( s[k] );
50                 if (temp > smax) smax = temp;
51         }
52
53         /*  Computation of the scaling factor.
54          */
55         if (smax == 0) scalauto = 0;
56         else {
57                 assert(smax > 0);
58                 scalauto = 4 - gsm_norm( (longword)smax << 16 );/* sub(4,..) */
59         }
60
61         /*  Scaling of the array s[0...159]
62          */
63
64         if (scalauto > 0) {
65
66 # ifdef USE_FLOAT_MUL
67 #   define SCALE(n)     \
68         case n: for (k = 0; k <= 159; k++) \
69                         float_s[k] = (float)    \
70                                 (s[k] = GSM_MULT_R(s[k], 16384 >> (n-1)));\
71                 break;
72 # else 
73 #   define SCALE(n)     \
74         case n: for (k = 0; k <= 159; k++) \
75                         s[k] = GSM_MULT_R( s[k], 16384 >> (n-1) );\
76                 break;
77 # endif /* USE_FLOAT_MUL */
78
79                 switch (scalauto) {
80                 SCALE(1)
81                 SCALE(2)
82                 SCALE(3)
83                 SCALE(4)
84                 }
85 # undef SCALE
86         }
87 # ifdef USE_FLOAT_MUL
88         else for (k = 0; k <= 159; k++) float_s[k] = (float) s[k];
89 # endif
90
91         /*  Compute the L_ACF[..].
92          */
93         {
94 # ifdef USE_FLOAT_MUL
95                 register float * sp = float_s;
96                 register float   sl = *sp;
97
98 #               define STEP(k)   L_ACF[k] += (longword)(sl * sp[ -(k) ]);
99 # else
100                 word  * sp = s;
101                 word    sl = *sp;
102
103 #               define STEP(k)   L_ACF[k] += ((longword)sl * sp[ -(k) ]);
104 # endif
105
106 #       define NEXTI     sl = *++sp
107
108
109         for (k = 9; k--; L_ACF[k] = 0) ;
110
111         STEP (0);
112         NEXTI;
113         STEP(0); STEP(1);
114         NEXTI;
115         STEP(0); STEP(1); STEP(2);
116         NEXTI;
117         STEP(0); STEP(1); STEP(2); STEP(3);
118         NEXTI;
119         STEP(0); STEP(1); STEP(2); STEP(3); STEP(4);
120         NEXTI;
121         STEP(0); STEP(1); STEP(2); STEP(3); STEP(4); STEP(5);
122         NEXTI;
123         STEP(0); STEP(1); STEP(2); STEP(3); STEP(4); STEP(5); STEP(6);
124         NEXTI;
125         STEP(0); STEP(1); STEP(2); STEP(3); STEP(4); STEP(5); STEP(6); STEP(7);
126
127         for (i = 8; i <= 159; i++) {
128
129                 NEXTI;
130
131                 STEP(0);
132                 STEP(1); STEP(2); STEP(3); STEP(4);
133                 STEP(5); STEP(6); STEP(7); STEP(8);
134         }
135
136         for (k = 9; k--; L_ACF[k] <<= 1) ; 
137
138         }
139         /*   Rescaling of the array s[0..159]
140          */
141         if (scalauto > 0) {
142                 assert(scalauto <= 4); 
143                 for (k = 160; k--; *s++ <<= scalauto) ;
144         }
145 }
146
147 #if defined(USE_FLOAT_MUL) && defined(FAST)
148
149 static void Fast_Autocorrelation P2((s, L_ACF),
150         word * s,               /* [0..159]     IN/OUT  */
151         longword * L_ACF)       /* [0..8]       OUT     */
152 {
153         register int    k, i;
154         float f_L_ACF[9];
155         float scale;
156
157         float          s_f[160];
158         register float *sf = s_f;
159
160         for (i = 0; i < 160; ++i) sf[i] = s[i];
161         for (k = 0; k <= 8; k++) {
162                 register float L_temp2 = 0;
163                 register float *sfl = sf - k;
164                 for (i = k; i < 160; ++i) L_temp2 += sf[i] * sfl[i];
165                 f_L_ACF[k] = L_temp2;
166         }
167         scale = MAX_LONGWORD / f_L_ACF[0];
168
169         for (k = 0; k <= 8; k++) {
170                 L_ACF[k] = f_L_ACF[k] * scale;
171         }
172 }
173 #endif  /* defined (USE_FLOAT_MUL) && defined (FAST) */
174
175 /* 4.2.5 */
176
177 static void Reflection_coefficients P2( (L_ACF, r),
178         longword        * L_ACF,                /* 0...8        IN      */
179         register word   * r                     /* 0...7        OUT     */
180 )
181 {
182         register int    i, m, n;
183         register word   temp;
184         register longword ltmp;
185         word            ACF[9]; /* 0..8 */
186         word            P[  9]; /* 0..8 */
187         word            K[  9]; /* 2..8 */
188
189         /*  Schur recursion with 16 bits arithmetic.
190          */
191
192         if (L_ACF[0] == 0) {
193                 for (i = 8; i--; *r++ = 0) ;
194                 return;
195         }
196
197         assert( L_ACF[0] != 0 );
198         temp = gsm_norm( L_ACF[0] );
199
200         assert(temp >= 0 && temp < 32);
201
202         /* ? overflow ? */
203         for (i = 0; i <= 8; i++) ACF[i] = SASR( L_ACF[i] << temp, 16 );
204
205         /*   Initialize array P[..] and K[..] for the recursion.
206          */
207
208         for (i = 1; i <= 7; i++) K[ i ] = ACF[ i ];
209         for (i = 0; i <= 8; i++) P[ i ] = ACF[ i ];
210
211         /*   Compute reflection coefficients
212          */
213         for (n = 1; n <= 8; n++, r++) {
214
215                 temp = P[1];
216                 temp = GSM_ABS(temp);
217                 if (P[0] < temp) {
218                         for (i = n; i <= 8; i++) *r++ = 0;
219                         return;
220                 }
221
222                 *r = gsm_div( temp, P[0] );
223
224                 assert(*r >= 0);
225                 if (P[1] > 0) *r = -*r;         /* r[n] = sub(0, r[n]) */
226                 assert (*r != MIN_WORD);
227                 if (n == 8) return; 
228
229                 /*  Schur recursion
230                  */
231                 temp = GSM_MULT_R( P[1], *r );
232                 P[0] = GSM_ADD( P[0], temp );
233
234                 for (m = 1; m <= 8 - n; m++) {
235                         temp     = GSM_MULT_R( K[ m   ],    *r );
236                         P[m]     = GSM_ADD(    P[ m+1 ],  temp );
237
238                         temp     = GSM_MULT_R( P[ m+1 ],    *r );
239                         K[m]     = GSM_ADD(    K[ m   ],  temp );
240                 }
241         }
242 }
243
244 /* 4.2.6 */
245
246 static void Transformation_to_Log_Area_Ratios P1((r),
247         register word   * r                     /* 0..7    IN/OUT */
248 )
249 /*
250  *  The following scaling for r[..] and LAR[..] has been used:
251  *
252  *  r[..]   = integer( real_r[..]*32768. ); -1 <= real_r < 1.
253  *  LAR[..] = integer( real_LAR[..] * 16384 );
254  *  with -1.625 <= real_LAR <= 1.625
255  */
256 {
257         register word   temp;
258         register int    i;
259
260
261         /* Computation of the LAR[0..7] from the r[0..7]
262          */
263         for (i = 1; i <= 8; i++, r++) {
264
265                 temp = *r;
266                 temp = GSM_ABS(temp);
267                 assert(temp >= 0);
268
269                 if (temp < 22118) {
270                         temp >>= 1;
271                 } else if (temp < 31130) {
272                         assert( temp >= 11059 );
273                         temp -= 11059;
274                 } else {
275                         assert( temp >= 26112 );
276                         temp -= 26112;
277                         temp <<= 2;
278                 }
279
280                 *r = *r < 0 ? -temp : temp;
281                 assert( *r != MIN_WORD );
282         }
283 }
284
285 /* 4.2.7 */
286
287 static void Quantization_and_coding P1((LAR),
288         register word * LAR     /* [0..7]       IN/OUT  */
289 )
290 {
291         register word   temp;
292         longword        ltmp;
293
294
295         /*  This procedure needs four tables; the following equations
296          *  give the optimum scaling for the constants:
297          *  
298          *  A[0..7] = integer( real_A[0..7] * 1024 )
299          *  B[0..7] = integer( real_B[0..7] *  512 )
300          *  MAC[0..7] = maximum of the LARc[0..7]
301          *  MIC[0..7] = minimum of the LARc[0..7]
302          */
303
304 #       undef STEP
305 #       define  STEP( A, B, MAC, MIC )          \
306                 temp = GSM_MULT( A,   *LAR );   \
307                 temp = GSM_ADD(  temp,   B );   \
308                 temp = GSM_ADD(  temp, 256 );   \
309                 temp = SASR(     temp,   9 );   \
310                 *LAR  =  temp>MAC ? MAC - MIC : (temp<MIC ? 0 : temp - MIC); \
311                 LAR++;
312
313         STEP(  20480,     0,  31, -32 );
314         STEP(  20480,     0,  31, -32 );
315         STEP(  20480,  2048,  15, -16 );
316         STEP(  20480, -2560,  15, -16 );
317
318         STEP(  13964,    94,   7,  -8 );
319         STEP(  15360, -1792,   7,  -8 );
320         STEP(   8534,  -341,   3,  -4 );
321         STEP(   9036, -1144,   3,  -4 );
322
323 #       undef   STEP
324 }
325
326 void Gsm_LPC_Analysis P3((S, s,LARc),
327         struct gsm_state *S,
328         word             * s,           /* 0..159 signals       IN/OUT  */
329         word             * LARc)        /* 0..7   LARc's        OUT     */
330 {
331         longword        L_ACF[9];
332
333 #if defined(USE_FLOAT_MUL) && defined(FAST)
334         if (S->fast) Fast_Autocorrelation (s,     L_ACF );
335         else
336 #endif
337         Autocorrelation                   (s,     L_ACF );
338         Reflection_coefficients           (L_ACF, LARc  );
339         Transformation_to_Log_Area_Ratios (LARc);
340         Quantization_and_coding           (LARc);
341 }