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