1 /* ------------------------------------------------------------------
2 * Copyright (C) 1998-2009 PacketVideo
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
13 * express or implied.
14 * See the License for the specific language governing permissions
15 * and limitations under the License.
16 * -------------------------------------------------------------------
17 */
18 /****************************************************************************************
19 Portions of this file are derived from the following 3GPP standard:
20
21 3GPP TS 26.073
22 ANSI-C code for the Adaptive Multi-Rate (AMR) speech codec
23 Available from http://www.3gpp.org
24
25 (C) 2004, 3GPP Organizational Partners (ARIB, ATIS, CCSA, ETSI, TTA, TTC)
26 Permission to distribute, modify and use this file under the standard license
27 terms listed above has been obtained from the copyright holder.
28 ****************************************************************************************/
29 /*
30 ------------------------------------------------------------------------------
31
32
33
34 Pathname: ./audio/gsm-amr/c/src/levinson.c
35 Funtions: Levinson_init
36 Levinson_reset
37 Levinson_exit
38 Levinson
39
40 ------------------------------------------------------------------------------
41 MODULE DESCRIPTION
42
43 This file contains the function the implements the Levinson-Durbin algorithm
44 using double-precision arithmetic. This file also includes functions to
45 initialize, allocate, and deallocate memory used by the Levinson function.
46
47 ------------------------------------------------------------------------------
48 */
49
50 /*----------------------------------------------------------------------------
51 ; INCLUDES
52 ----------------------------------------------------------------------------*/
53 #include <stdlib.h>
54 #include <string.h>
55
56 #include "levinson.h"
57 #include "basicop_malloc.h"
58 #include "basic_op.h"
59 #include "div_32.h"
60 #include "cnst.h"
61
62 /*----------------------------------------------------------------------------
63 ; MACROS
64 ; Define module specific macros here
65 ----------------------------------------------------------------------------*/
66
67 /*----------------------------------------------------------------------------
68 ; DEFINES
69 ; Include all pre-processor statements here. Include conditional
70 ; compile variables also.
71 ----------------------------------------------------------------------------*/
72
73 /*----------------------------------------------------------------------------
74 ; LOCAL FUNCTION DEFINITIONS
75 ; Function Prototype declaration
76 ----------------------------------------------------------------------------*/
77
78 /*----------------------------------------------------------------------------
79 ; LOCAL VARIABLE DEFINITIONS
80 ; Variable declaration - defined here and used outside this module
81 ----------------------------------------------------------------------------*/
82
83
84 /*
85 ------------------------------------------------------------------------------
86 FUNCTION NAME: Levinson_init
87 ------------------------------------------------------------------------------
88 INPUT AND OUTPUT DEFINITIONS
89
90 Inputs:
91 state = pointer to an array of pointers to structures of type
92 LevinsonState
93
94 Outputs:
95 pointer pointed to by state points to the newly allocated memory to
96 be used by Levinson function
97
98 Returns:
99 return_value = 0, if initialization was successful; -1, otherwise (int)
100
101 Global Variables Used:
102 None
103
104 Local Variables Needed:
105 None
106
107 ------------------------------------------------------------------------------
108 FUNCTION DESCRIPTION
109
110 This function allocates and initializes the state memory used by the
111 Levinson function.
112
113 ------------------------------------------------------------------------------
114 REQUIREMENTS
115
116 None
117
118 ------------------------------------------------------------------------------
119 REFERENCES
120
121 levinson.c, UMTS GSM AMR speech codec, R99 - Version 3.2.0, March 2, 2001
122
123 ------------------------------------------------------------------------------
124 PSEUDO-CODE
125
126 int Levinson_init (LevinsonState **state)
127 {
128 LevinsonState* s;
129
130 if (state == (LevinsonState **) NULL){
131 //fprint(stderr, "Levinson_init: invalid parameter\n");
132 return -1;
133 }
134 *state = NULL;
135
136 // allocate memory
137 if ((s= (LevinsonState *) malloc(sizeof(LevinsonState))) == NULL){
138 //fprint(stderr, "Levinson_init: can not malloc state structure\n");
139 return -1;
140 }
141
142 Levinson_reset(s);
143 *state = s;
144
145 return 0;
146 }
147
148 ------------------------------------------------------------------------------
149 RESOURCES USED [optional]
150
151 When the code is written for a specific target processor the
152 the resources used should be documented below.
153
154 HEAP MEMORY USED: x bytes
155
156 STACK MEMORY USED: x bytes
157
158 CLOCK CYCLES: (cycle count equation for this function) + (variable
159 used to represent cycle count for each subroutine
160 called)
161 where: (cycle count variable) = cycle count for [subroutine
162 name]
163
164 ------------------------------------------------------------------------------
165 CAUTION [optional]
166 [State any special notes, constraints or cautions for users of this function]
167
168 ------------------------------------------------------------------------------
169 */
170
Levinson_init(LevinsonState ** state)171 Word16 Levinson_init(LevinsonState **state)
172 {
173 LevinsonState* s;
174
175 if (state == (LevinsonState **) NULL)
176 {
177 /* fprint(stderr, "Levinson_init: invalid parameter\n"); */
178 return(-1);
179 }
180 *state = NULL;
181
182 /* allocate memory */
183 if ((s = (LevinsonState *) malloc(sizeof(LevinsonState))) == NULL)
184 {
185 /* fprint(stderr, "Levinson_init:
186 can not malloc state structure\n"); */
187 return(-1);
188 }
189
190 Levinson_reset(s);
191 *state = s;
192
193 return(0);
194 }
195
196 /****************************************************************************/
197
198
199 /*
200 ------------------------------------------------------------------------------
201 FUNCTION NAME: Levinson_reset
202 ------------------------------------------------------------------------------
203 INPUT AND OUTPUT DEFINITIONS
204
205 Inputs:
206 state = pointer to structures of type LevinsonState
207
208 Outputs:
209 old_A field of structure pointed to by state is initialized to 4096
210 (first location) and the rest to zeros
211
212 Returns:
213 return_value = 0, if reset was successful; -1, otherwise (int)
214
215 Global Variables Used:
216 None
217
218 Local Variables Needed:
219 None
220
221 ------------------------------------------------------------------------------
222 FUNCTION DESCRIPTION
223
224 This function initializes the state memory used by the Levinson function to
225 zero.
226
227 ------------------------------------------------------------------------------
228 REQUIREMENTS
229
230 None
231
232 ------------------------------------------------------------------------------
233 REFERENCES
234
235 levinson.c, UMTS GSM AMR speech codec, R99 - Version 3.2.0, March 2, 2001
236
237 ------------------------------------------------------------------------------
238 PSEUDO-CODE
239
240 int Levinson_reset (LevinsonState *state)
241 {
242 Word16 i;
243
244 if (state == (LevinsonState *) NULL){
245 fprint(stderr, "Levinson_reset: invalid parameter\n");
246 return -1;
247 }
248
249 state->old_A[0] = 4096;
250 for(i = 1; i < M + 1; i++)
251 state->old_A[i] = 0;
252
253 return 0;
254 }
255
256 ------------------------------------------------------------------------------
257 RESOURCES USED [optional]
258
259 When the code is written for a specific target processor the
260 the resources used should be documented below.
261
262 HEAP MEMORY USED: x bytes
263
264 STACK MEMORY USED: x bytes
265
266 CLOCK CYCLES: (cycle count equation for this function) + (variable
267 used to represent cycle count for each subroutine
268 called)
269 where: (cycle count variable) = cycle count for [subroutine
270 name]
271
272 ------------------------------------------------------------------------------
273 CAUTION [optional]
274 [State any special notes, constraints or cautions for users of this function]
275
276 ------------------------------------------------------------------------------
277 */
278
Levinson_reset(LevinsonState * state)279 Word16 Levinson_reset(LevinsonState *state)
280 {
281 Word16 i;
282
283 if (state == (LevinsonState *) NULL)
284 {
285 /* fprint(stderr, "Levinson_reset: invalid parameter\n"); */
286 return(-1);
287 }
288
289 state->old_A[0] = 4096;
290 for (i = 1; i < M + 1; i++)
291 {
292 state->old_A[i] = 0;
293 }
294
295 return(0);
296 }
297
298 /****************************************************************************/
299
300 /*
301 ------------------------------------------------------------------------------
302 FUNCTION NAME: Levinson_exit
303 ------------------------------------------------------------------------------
304 INPUT AND OUTPUT DEFINITIONS
305
306 Inputs:
307 state = pointer to an array of pointers to structures of type
308 LevinsonState
309
310 Outputs:
311 pointer pointed to by state is set to the NULL address
312
313 Returns:
314 None
315
316 Global Variables Used:
317 None
318
319 Local Variables Needed:
320 None
321
322 ------------------------------------------------------------------------------
323 FUNCTION DESCRIPTION
324
325 This function deallocates the state memory used by the Levinson function.
326
327 ------------------------------------------------------------------------------
328 REQUIREMENTS
329
330 None
331
332 ------------------------------------------------------------------------------
333 REFERENCES
334
335 levinson.c, UMTS GSM AMR speech codec, R99 - Version 3.2.0, March 2, 2001
336
337 ------------------------------------------------------------------------------
338 PSEUDO-CODE
339
340 void Levinson_exit (LevinsonState **state)
341 {
342 if (state == NULL || *state == NULL)
343 return;
344
345 // deallocate memory
346 free(*state);
347 *state = NULL;
348
349 return;
350 }
351
352 ------------------------------------------------------------------------------
353 RESOURCES USED [optional]
354
355 When the code is written for a specific target processor the
356 the resources used should be documented below.
357
358 HEAP MEMORY USED: x bytes
359
360 STACK MEMORY USED: x bytes
361
362 CLOCK CYCLES: (cycle count equation for this function) + (variable
363 used to represent cycle count for each subroutine
364 called)
365 where: (cycle count variable) = cycle count for [subroutine
366 name]
367
368 ------------------------------------------------------------------------------
369 CAUTION [optional]
370 [State any special notes, constraints or cautions for users of this function]
371
372 ------------------------------------------------------------------------------
373 */
374
Levinson_exit(LevinsonState ** state)375 void Levinson_exit(LevinsonState **state)
376 {
377 if (state == NULL || *state == NULL)
378 {
379 return;
380 }
381
382 /* deallocate memory */
383 free(*state);
384 *state = NULL;
385
386 return;
387 }
388
389 /****************************************************************************/
390
391
392 /*
393 ------------------------------------------------------------------------------
394 FUNCTION NAME: Levinson
395 ------------------------------------------------------------------------------
396 INPUT AND OUTPUT DEFINITIONS
397
398 Inputs:
399 st = pointer to structures of type LevinsonState
400 Rh = vector containing most significant byte of
401 autocorrelation values (Word16)
402 Rl = vector containing least significant byte of
403 autocorrelation values (Word16)
404 A = vector of LPC coefficients (10th order) (Word16)
405 rc = vector containing first four reflection coefficients (Word16)
406 pOverflow = pointer to overflow indicator (Flag)
407
408 Outputs:
409 A contains the newly calculated LPC coefficients
410 rc contains the newly calculated reflection coefficients
411
412 Returns:
413 return_value = 0 (int)
414
415 Global Variables Used:
416 None
417
418 Local Variables Needed:
419 None
420
421 ------------------------------------------------------------------------------
422 FUNCTION DESCRIPTION
423
424 This function implements the Levinson-Durbin algorithm using double-
425 precision arithmetic. This is used to compute the Linear Predictive (LP)
426 filter parameters from the speech autocorrelation values.
427
428 The algorithm implemented is as follows:
429 A[0] = 1
430 K = -R[1]/R[0]
431 A[1] = K
432 Alpha = R[0] * (1-K**2]
433
434 FOR i = 2 to M
435
436 S = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i]
437 K = -S / Alpha
438
439 FOR j = 1 to i-1
440 An[j] = A[j] + K*A[i-j] where An[i] = new A[i]
441 ENDFOR
442
443 An[i]=K
444 Alpha=Alpha * (1-K**2)
445
446 END
447
448 where:
449 R[i] = autocorrelations
450 A[i] = filter coefficients
451 K = reflection coefficient
452 Alpha = prediction gain
453
454 ------------------------------------------------------------------------------
455 REQUIREMENTS
456
457 None
458
459 ------------------------------------------------------------------------------
460 REFERENCES
461
462 levinson.c, UMTS GSM AMR speech codec, R99 - Version 3.2.0, March 2, 2001
463
464 ------------------------------------------------------------------------------
465 PSEUDO-CODE
466
467 int Levinson (
468 LevinsonState *st,
469 Word16 Rh[], // i : Rh[m+1] Vector of autocorrelations (msb)
470 Word16 Rl[], // i : Rl[m+1] Vector of autocorrelations (lsb)
471 Word16 A[], // o : A[m] LPC coefficients (m = 10)
472 Word16 rc[] // o : rc[4] First 4 reflection coefficients
473 )
474 {
475 Word16 i, j;
476 Word16 hi, lo;
477 Word16 Kh, Kl; // reflexion coefficient; hi and lo
478 Word16 alp_h, alp_l, alp_exp; // Prediction gain; hi lo and exponent
479 Word16 Ah[M + 1], Al[M + 1]; // LPC coef. in double prec.
480 Word16 Anh[M + 1], Anl[M + 1];// LPC coef.for next iteration in double
481 prec.
482 Word32 t0, t1, t2; // temporary variable
483
484 // K = A[1] = -R[1] / R[0]
485
486 t1 = L_Comp (Rh[1], Rl[1]);
487 t2 = L_abs (t1); // abs R[1]
488 t0 = Div_32 (t2, Rh[0], Rl[0]); // R[1]/R[0]
489 if (t1 > 0)
490 t0 = L_negate (t0); // -R[1]/R[0]
491 L_Extract (t0, &Kh, &Kl); // K in DPF
492
493 rc[0] = pv_round (t0);
494
495 t0 = L_shr (t0, 4); // A[1] in
496 L_Extract (t0, &Ah[1], &Al[1]); // A[1] in DPF
497
498 // Alpha = R[0] * (1-K**2)
499
500 t0 = Mpy_32 (Kh, Kl, Kh, Kl); // K*K
501 t0 = L_abs (t0); // Some case <0 !!
502 t0 = L_sub ((Word32) 0x7fffffffL, t0); // 1 - K*K
503 L_Extract (t0, &hi, &lo); // DPF format
504 t0 = Mpy_32 (Rh[0], Rl[0], hi, lo); // Alpha in
505
506 // Normalize Alpha
507
508 alp_exp = norm_l (t0);
509 t0 = L_shl (t0, alp_exp);
510 L_Extract (t0, &alp_h, &alp_l); // DPF format
511
512 *--------------------------------------*
513 * ITERATIONS I=2 to M *
514 *--------------------------------------*
515
516 for (i = 2; i <= M; i++)
517 {
518 // t0 = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i]
519
520 t0 = 0;
521 for (j = 1; j < i; j++)
522 {
523 t0 = L_add (t0, Mpy_32 (Rh[j], Rl[j], Ah[i - j], Al[i - j]));
524 }
525 t0 = L_shl (t0, 4);
526
527 t1 = L_Comp (Rh[i], Rl[i]);
528 t0 = L_add (t0, t1); // add R[i]
529
530 // K = -t0 / Alpha
531
532 t1 = L_abs (t0);
533 t2 = Div_32 (t1, alp_h, alp_l); // abs(t0)/Alpha
534 if (t0 > 0)
535 t2 = L_negate (t2); // K =-t0/Alpha
536 t2 = L_shl (t2, alp_exp); // denormalize; compare to Alpha
537 L_Extract (t2, &Kh, &Kl); // K in DPF
538
539 if (sub (i, 5) < 0)
540 {
541 rc[i - 1] = pv_round (t2);
542 }
543 // Test for unstable filter. If unstable keep old A(z)
544
545 if (sub (abs_s (Kh), 32750) > 0)
546 {
547 for (j = 0; j <= M; j++)
548 {
549 A[j] = st->old_A[j];
550 }
551
552 for (j = 0; j < 4; j++)
553 {
554 rc[j] = 0;
555 }
556
557 return 0;
558 }
559 *------------------------------------------*
560 * Compute new LPC coeff. -> An[i] *
561 * An[j]= A[j] + K*A[i-j] , j=1 to i-1 *
562 * An[i]= K *
563 *------------------------------------------*
564
565 for (j = 1; j < i; j++)
566 {
567 t0 = Mpy_32 (Kh, Kl, Ah[i - j], Al[i - j]);
568 t0 = L_add(t0, L_Comp(Ah[j], Al[j]));
569 L_Extract (t0, &Anh[j], &Anl[j]);
570 }
571 t2 = L_shr (t2, 4);
572 L_Extract (t2, &Anh[i], &Anl[i]);
573
574 // Alpha = Alpha * (1-K**2)
575
576 t0 = Mpy_32 (Kh, Kl, Kh, Kl); // K*K
577 t0 = L_abs (t0); // Some case <0 !!
578 t0 = L_sub ((Word32) 0x7fffffffL, t0); // 1 - K*K
579 L_Extract (t0, &hi, &lo); // DPF format
580 t0 = Mpy_32 (alp_h, alp_l, hi, lo);
581
582 // Normalize Alpha
583
584 j = norm_l (t0);
585 t0 = L_shl (t0, j);
586 L_Extract (t0, &alp_h, &alp_l); // DPF format
587 alp_exp = add (alp_exp, j); // Add normalization to
588 alp_exp
589
590 // A[j] = An[j]
591
592 for (j = 1; j <= i; j++)
593 {
594 Ah[j] = Anh[j];
595 Al[j] = Anl[j];
596 }
597 }
598
599 A[0] = 4096;
600 for (i = 1; i <= M; i++)
601 {
602 t0 = L_Comp (Ah[i], Al[i]);
603 st->old_A[i] = A[i] = pv_round (L_shl (t0, 1));
604 }
605
606 return 0;
607 }
608
609 ------------------------------------------------------------------------------
610 RESOURCES USED [optional]
611
612 When the code is written for a specific target processor the
613 the resources used should be documented below.
614
615 HEAP MEMORY USED: x bytes
616
617 STACK MEMORY USED: x bytes
618
619 CLOCK CYCLES: (cycle count equation for this function) + (variable
620 used to represent cycle count for each subroutine
621 called)
622 where: (cycle count variable) = cycle count for [subroutine
623 name]
624
625 ------------------------------------------------------------------------------
626 CAUTION [optional]
627 [State any special notes, constraints or cautions for users of this function]
628
629 ------------------------------------------------------------------------------
630 */
631
Levinson(LevinsonState * st,Word16 Rh[],Word16 Rl[],Word16 A[],Word16 rc[],Flag * pOverflow)632 Word16 Levinson(
633 LevinsonState *st,
634 Word16 Rh[], /* i : Rh[m+1] Vector of autocorrelations (msb) */
635 Word16 Rl[], /* i : Rl[m+1] Vector of autocorrelations (lsb) */
636 Word16 A[], /* o : A[m] LPC coefficients (m = 10) */
637 Word16 rc[], /* o : rc[4] First 4 reflection coefficients */
638 Flag *pOverflow
639 )
640 {
641 Word16 i;
642 Word16 j;
643 Word16 hi;
644 Word16 lo;
645 Word16 Kh; /* reflexion coefficient; hi and lo */
646 Word16 Kl;
647 Word16 alp_h; /* Prediction gain; hi lo and exponent*/
648 Word16 alp_l;
649 Word16 alp_exp;
650 Word16 Ah[M + 1]; /* LPC coef. in double prec. */
651 Word16 Al[M + 1];
652 Word16 Anh[M + 1]; /* LPC coef.for next iteration in */
653 Word16 Anl[M + 1]; /* double prec. */
654 Word32 t0; /* temporary variable */
655 Word32 t1; /* temporary variable */
656 Word32 t2; /* temporary variable */
657
658 Word16 *p_Rh;
659 Word16 *p_Rl;
660 Word16 *p_Ah;
661 Word16 *p_Al;
662 Word16 *p_Anh;
663 Word16 *p_Anl;
664 Word16 *p_A;
665
666 /* K = A[1] = -R[1] / R[0] */
667 t1 = ((Word32) * (Rh + 1)) << 16;
668 t1 += *(Rl + 1) << 1;
669
670 t2 = L_abs(t1); /* abs R[1] - required by Div_32 */
671 t0 = Div_32(t2, *Rh, *Rl, pOverflow); /* R[1]/R[0] */
672
673 if (t1 > 0)
674 {
675 t0 = L_negate(t0); /* -R[1]/R[0] */
676 }
677
678 /* K in DPF */
679 Kh = (Word16)(t0 >> 16);
680 Kl = (Word16)((t0 >> 1) - ((Word32)(Kh) << 15));
681
682 *rc = pv_round(t0, pOverflow);
683
684 t0 = t0 >> 4;
685
686 /* A[1] in DPF */
687 *(Ah + 1) = (Word16)(t0 >> 16);
688
689 *(Al + 1) = (Word16)((t0 >> 1) - ((Word32)(*(Ah + 1)) << 15));
690
691 /* Alpha = R[0] * (1-K**2) */
692 t0 = Mpy_32(Kh, Kl, Kh, Kl, pOverflow); /* K*K */
693 t0 = L_abs(t0); /* Some case <0 !! */
694 t0 = 0x7fffffffL - t0; /* 1 - K*K */
695
696 /* DPF format */
697 hi = (Word16)(t0 >> 16);
698 lo = (Word16)((t0 >> 1) - ((Word32)(hi) << 15));
699
700 t0 = Mpy_32(*Rh, *Rl, hi, lo, pOverflow); /* Alpha in */
701
702 /* Normalize Alpha */
703
704 alp_exp = norm_l(t0);
705 t0 = t0 << alp_exp;
706
707 /* DPF format */
708 alp_h = (Word16)(t0 >> 16);
709 alp_l = (Word16)((t0 >> 1) - ((Word32)(alp_h) << 15));
710
711 /*--------------------------------------*
712 * ITERATIONS I=2 to M *
713 *--------------------------------------*/
714
715 for (i = 2; i <= M; i++)
716 {
717 /* t0 = SUM ( R[j]*A[i-j] ,j=1,i-1 ) + R[i] */
718
719 t0 = 0;
720 p_Rh = &Rh[1];
721 p_Rl = &Rl[1];
722 p_Ah = &Ah[i-1];
723 p_Al = &Al[i-1];
724 for (j = 1; j < i; j++)
725 {
726 t0 += (((Word32) * (p_Rh)* *(p_Al--)) >> 15);
727 t0 += (((Word32) * (p_Rl++)* *(p_Ah)) >> 15);
728 t0 += ((Word32) * (p_Rh++)* *(p_Ah--));
729 }
730
731 t0 = t0 << 5;
732
733 t1 = ((Word32) * (Rh + i) << 16) + ((Word32)(*(Rl + i)) << 1);
734 t0 += t1;
735
736 /* K = -t0 / Alpha */
737
738 t1 = L_abs(t0);
739 t2 = Div_32(t1, alp_h, alp_l, pOverflow); /* abs(t0)/Alpha */
740
741 if (t0 > 0)
742 {
743 t2 = L_negate(t2); /* K =-t0/Alpha */
744 }
745
746 t2 = L_shl(t2, alp_exp, pOverflow); /* denormalize; compare to Alpha */
747 Kh = (Word16)(t2 >> 16);
748 Kl = (Word16)((t2 >> 1) - ((Word32)(Kh) << 15));
749
750 if (i < 5)
751 {
752 *(rc + i - 1) = (Word16)((t2 + 0x00008000L) >> 16);
753 }
754 /* Test for unstable filter. If unstable keep old A(z) */
755 if ((abs_s(Kh)) > 32750)
756 {
757 memcpy(A, &(st->old_A[0]), sizeof(Word16)*(M + 1));
758 memset(rc, 0, sizeof(Word16)*4);
759 return(0);
760 }
761 /*------------------------------------------*
762 * Compute new LPC coeff. -> An[i] *
763 * An[j]= A[j] + K*A[i-j] , j=1 to i-1 *
764 * An[i]= K *
765 *------------------------------------------*/
766 p_Ah = &Ah[i-1];
767 p_Al = &Al[i-1];
768 p_Anh = &Anh[1];
769 p_Anl = &Anl[1];
770 for (j = 1; j < i; j++)
771 {
772 t0 = (((Word32)Kh* *(p_Al--)) >> 15);
773 t0 += (((Word32)Kl* *(p_Ah)) >> 15);
774 t0 += ((Word32)Kh* *(p_Ah--));
775
776 t0 += (Ah[j] << 15) + Al[j];
777
778 *(p_Anh) = (Word16)(t0 >> 15);
779 *(p_Anl++) = (Word16)(t0 - ((Word32)(*(p_Anh++)) << 15));
780 }
781
782 *(p_Anh) = (Word16)(t2 >> 20);
783 *(p_Anl) = (Word16)((t2 >> 5) - ((Word32)(*(Anh + i)) << 15));
784
785 /* Alpha = Alpha * (1-K**2) */
786
787 t0 = Mpy_32(Kh, Kl, Kh, Kl, pOverflow); /* K*K */
788 t0 = L_abs(t0); /* Some case <0 !! */
789 t0 = 0x7fffffffL - t0; /* 1 - K*K */
790
791 hi = (Word16)(t0 >> 16);
792 lo = (Word16)((t0 >> 1) - ((Word32)(hi) << 15));
793
794 t0 = (((Word32)alp_h * lo) >> 15);
795 t0 += (((Word32)alp_l * hi) >> 15);
796 t0 += ((Word32)alp_h * hi);
797
798 t0 <<= 1;
799 /* Normalize Alpha */
800
801 j = norm_l(t0);
802 t0 <<= j;
803 alp_h = (Word16)(t0 >> 16);
804 alp_l = (Word16)((t0 >> 1) - ((Word32)(alp_h) << 15));
805 alp_exp += j; /* Add normalization to alp_exp */
806
807 /* A[j] = An[j] */
808 memcpy(&Ah[1], &Anh[1], sizeof(Word16)*i);
809 memcpy(&Al[1], &Anl[1], sizeof(Word16)*i);
810 }
811
812 p_A = &A[0];
813 *(p_A++) = 4096;
814 p_Ah = &Ah[1];
815 p_Al = &Al[1];
816
817 for (i = 1; i <= M; i++)
818 {
819 t0 = ((Word32) * (p_Ah++) << 15) + *(p_Al++);
820 st->old_A[i] = *(p_A++) = (Word16)((t0 + 0x00002000) >> 14);
821 }
822
823 return(0);
824 }
825