FreeCalypso > hg > efr-experiments

comparison src/agc.c @ 0:56410792419a

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
src: original EFR source from ETSI
author Mychaela Falconia <falcon@freecalypso.org>
date Wed, 03 Apr 2024 05:31:37 +0000
parents
children
comparison
equal deleted inserted replaced
-1:000000000000 0:56410792419a
1 /*************************************************************************
2 *
3 * FUNCTION: agc
4 *
5 * PURPOSE: Scales the postfilter output on a subframe basis by automatic
6 * control of the subframe gain.
7 *
8 * DESCRIPTION:
9 * sig_out[n] = sig_out[n] * gain[n];
10 * where gain[n] is the gain at the nth sample given by
11 * gain[n] = agc_fac * gain[n-1] + (1 - agc_fac) g_in/g_out
12 * g_in/g_out is the square root of the ratio of energy at the input
13 * and output of the postfilter.
14 *
15 *************************************************************************/
16
17 #include "typedef.h"
18 #include "basic_op.h"
19 #include "count.h"
20 #include "sig_proc.h"
21 #include "cnst.h"
22
23 Word16 past_gain; /* initial value of past_gain = 1.0 */
24
25 void agc (
26 Word16 *sig_in, /* (i) : postfilter input signal */
27 Word16 *sig_out, /* (i/o) : postfilter output signal */
28 Word16 agc_fac, /* (i) : AGC factor */
29 Word16 l_trm /* (i) : subframe size */
30 )
31 {
32 Word16 i, exp;
33 Word16 gain_in, gain_out, g0, gain;
34 Word32 s;
35
36 Word16 temp;
37
38 /* calculate gain_out with exponent */
39
40 temp = shr (sig_out[0], 2);
41 s = L_mult (temp, temp);
42
43 for (i = 1; i < l_trm; i++)
44 {
45 temp = shr (sig_out[i], 2);
46 s = L_mac (s, temp, temp);
47 }
48
49 test ();
50 if (s == 0)
51 {
52 past_gain = 0; move16 ();
53 return;
54 }
55 exp = sub (norm_l (s), 1);
56 gain_out = round (L_shl (s, exp));
57
58 /* calculate gain_in with exponent */
59
60 temp = shr (sig_in[0], 2);
61 s = L_mult (temp, temp);
62
63 for (i = 1; i < l_trm; i++)
64 {
65 temp = shr (sig_in[i], 2);
66 s = L_mac (s, temp, temp);
67 }
68
69 test ();
70 if (s == 0)
71 {
72 g0 = 0; move16 ();
73 }
74 else
75 {
76 i = norm_l (s);
77 gain_in = round (L_shl (s, i));
78 exp = sub (exp, i);
79
80 /*---------------------------------------------------*
81 * g0 = (1-agc_fac) * sqrt(gain_in/gain_out); *
82 *---------------------------------------------------*/
83
84 s = L_deposit_l (div_s (gain_out, gain_in));
85 s = L_shl (s, 7); /* s = gain_out / gain_in */
86 s = L_shr (s, exp); /* add exponent */
87
88 s = Inv_sqrt (s);
89 i = round (L_shl (s, 9));
90
91 /* g0 = i * (1-agc_fac) */
92 g0 = mult (i, sub (32767, agc_fac));
93 }
94
95 /* compute gain[n] = agc_fac * gain[n-1]
96 + (1-agc_fac) * sqrt(gain_in/gain_out) */
97 /* sig_out[n] = gain[n] * sig_out[n] */
98
99 gain = past_gain; move16 ();
100
101 for (i = 0; i < l_trm; i++)
102 {
103 gain = mult (gain, agc_fac);
104 gain = add (gain, g0);
105 sig_out[i] = extract_h (L_shl (L_mult (sig_out[i], gain), 3));
106 move16 ();
107 }
108
109 past_gain = gain; move16 ();
110
111 return;
112 }
113
114 void agc2 (
115 Word16 *sig_in, /* (i) : postfilter input signal */
116 Word16 *sig_out, /* (i/o) : postfilter output signal */
117 Word16 l_trm /* (i) : subframe size */
118 )
119 {
120 Word16 i, exp;
121 Word16 gain_in, gain_out, g0;
122 Word32 s;
123
124 Word16 temp;
125
126 /* calculate gain_out with exponent */
127
128 temp = shr (sig_out[0], 2);
129 s = L_mult (temp, temp);
130 for (i = 1; i < l_trm; i++)
131 {
132 temp = shr (sig_out[i], 2);
133 s = L_mac (s, temp, temp);
134 }
135
136 test ();
137 if (s == 0)
138 {
139 return;
140 }
141 exp = sub (norm_l (s), 1);
142 gain_out = round (L_shl (s, exp));
143
144 /* calculate gain_in with exponent */
145
146 temp = shr (sig_in[0], 2);
147 s = L_mult (temp, temp);
148 for (i = 1; i < l_trm; i++)
149 {
150 temp = shr (sig_in[i], 2);
151 s = L_mac (s, temp, temp);
152 }
153
154 test ();
155 if (s == 0)
156 {
157 g0 = 0; move16 ();
158 }
159 else
160 {
161 i = norm_l (s);
162 gain_in = round (L_shl (s, i));
163 exp = sub (exp, i);
164
165 /*---------------------------------------------------*
166 * g0 = sqrt(gain_in/gain_out); *
167 *---------------------------------------------------*/
168
169 s = L_deposit_l (div_s (gain_out, gain_in));
170 s = L_shl (s, 7); /* s = gain_out / gain_in */
171 s = L_shr (s, exp); /* add exponent */
172
173 s = Inv_sqrt (s);
174 g0 = round (L_shl (s, 9));
175 }
176
177 /* sig_out(n) = gain(n) sig_out(n) */
178
179 for (i = 0; i < l_trm; i++)
180 {
181 sig_out[i] = extract_h (L_shl (L_mult (sig_out[i], g0), 3));
182 move16 ();
183 }
184
185 return;
186 }