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