1 /*
2  * Copyright (C) 2004-2010 NXP Software
3  * Copyright (C) 2010 The Android Open Source Project
4  *
5  * Licensed under the Apache License, Version 2.0 (the "License");
6  * you may not use this file except in compliance with the License.
7  * You may obtain a copy of the License at
8  *
9  *      http://www.apache.org/licenses/LICENSE-2.0
10  *
11  * Unless required by applicable law or agreed to in writing, software
12  * distributed under the License is distributed on an "AS IS" BASIS,
13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  * See the License for the specific language governing permissions and
15  * limitations under the License.
16  */
17 
18 /****************************************************************************************/
19 /*                                                                                      */
20 /*    Includes                                                                          */
21 /*                                                                                      */
22 /****************************************************************************************/
23 
24 #include "AGC.h"
25 #include "ScalarArithmetic.h"
26 
27 
28 /****************************************************************************************/
29 /*                                                                                      */
30 /*    Defines                                                                           */
31 /*                                                                                      */
32 /****************************************************************************************/
33 
34 #define VOL_TC_SHIFT                                        21          /* As a power of 2 */
35 #define DECAY_SHIFT                                        10           /* As a power of 2 */
36 #ifdef BUILD_FLOAT
37 #define VOL_TC_FLOAT                                      2.0f          /* As a power of 2 */
38 #define DECAY_FAC_FLOAT                                  64.0f          /* As a power of 2 */
39 #endif
40 
41 /****************************************************************************************/
42 /*                                                                                      */
43 /* FUNCTION:                  AGC_MIX_VOL_2St1Mon_D32_WRA                               */
44 /*                                                                                      */
45 /* DESCRIPTION:                                                                         */
46 /*    Apply AGC and mix signals                                                         */
47 /*                                                                                      */
48 /*                                                                                      */
49 /*  StSrc   ------------------|                                                         */
50 /*                            |                                                         */
51 /*              ______       _|_        ________                                        */
52 /*             |      |     |   |      |        |                                       */
53 /*  MonoSrc -->| AGC  |---->| + |----->| Volume |------------------------------+--->    */
54 /*             | Gain |     |___|      | Gain   |                              |        */
55 /*             |______|                |________|                              |        */
56 /*                /|\                               __________     ________    |        */
57 /*                 |                               |          |   |        |   |        */
58 /*                 |-------------------------------| AGC Gain |<--| Peak   |<--|        */
59 /*                                                 | Update   |   | Detect |            */
60 /*                                                 |__________|   |________|            */
61 /*                                                                                      */
62 /*                                                                                      */
63 /* PARAMETERS:                                                                          */
64 /*  pInstance               Instance pointer                                            */
65 /*  pStereoIn               Stereo source                                               */
66 /*  pMonoIn                 Mono band pass source                                       */
67 /*  pStereoOut              Stereo destination                                          */
68 /*                                                                                      */
69 /* RETURNS:                                                                             */
70 /*  Void                                                                                */
71 /*                                                                                      */
72 /* NOTES:                                                                               */
73 /*                                                                                      */
74 /****************************************************************************************/
75 #ifndef BUILD_FLOAT
AGC_MIX_VOL_2St1Mon_D32_WRA(AGC_MIX_VOL_2St1Mon_D32_t * pInstance,const LVM_INT32 * pStSrc,const LVM_INT32 * pMonoSrc,LVM_INT32 * pDst,LVM_UINT16 NumSamples)76 void AGC_MIX_VOL_2St1Mon_D32_WRA(AGC_MIX_VOL_2St1Mon_D32_t  *pInstance,     /* Instance pointer */
77                                  const LVM_INT32            *pStSrc,        /* Stereo source */
78                                  const LVM_INT32            *pMonoSrc,      /* Mono source */
79                                  LVM_INT32                  *pDst,          /* Stereo destination */
80                                  LVM_UINT16                 NumSamples)     /* Number of samples */
81 {
82 
83     /*
84      * General variables
85      */
86     LVM_UINT16      i;                                          /* Sample index */
87     LVM_INT32       Left;                                       /* Left sample */
88     LVM_INT32       Right;                                      /* Right sample */
89     LVM_INT32       Mono;                                       /* Mono sample */
90     LVM_INT32       AbsPeak;                                    /* Absolute peak signal */
91     LVM_INT32       HighWord;                                   /* High word in intermediate calculations */
92     LVM_INT32       LowWord;                                    /* Low word in intermediate calculations */
93     LVM_INT16       AGC_Mult;                                   /* Short AGC gain */
94     LVM_INT16       Vol_Mult;                                   /* Short volume */
95 
96 
97     /*
98      * Instance control variables
99      */
100     LVM_INT32      AGC_Gain      = pInstance->AGC_Gain;         /* Get the current AGC gain */
101     LVM_INT32      AGC_MaxGain   = pInstance->AGC_MaxGain;      /* Get maximum AGC gain */
102     LVM_INT16      AGC_GainShift = pInstance->AGC_GainShift;    /* Get the AGC shift */
103     LVM_INT16      AGC_Attack    = pInstance->AGC_Attack;       /* Attack scaler */
104     LVM_INT16      AGC_Decay     = pInstance->AGC_Decay;        /* Decay scaler */
105     LVM_INT32      AGC_Target    = pInstance->AGC_Target;       /* Get the target level */
106     LVM_INT32      Vol_Current   = pInstance->Volume;           /* Actual volume setting */
107     LVM_INT32      Vol_Target    = pInstance->Target;           /* Target volume setting */
108     LVM_INT16      Vol_Shift     = pInstance->VolumeShift;      /* Volume shift scaling */
109     LVM_INT16      Vol_TC        = pInstance->VolumeTC;         /* Time constant */
110 
111 
112     /*
113      * Process on a sample by sample basis
114      */
115     for (i=0;i<NumSamples;i++)                                  /* For each sample */
116     {
117 
118         /*
119          * Get the short scalers
120          */
121         AGC_Mult    = (LVM_INT16)(AGC_Gain >> 16);              /* Get the short AGC gain */
122         Vol_Mult    = (LVM_INT16)(Vol_Current >> 16);           /* Get the short volume gain */
123 
124 
125         /*
126          * Get the input samples
127          */
128         Left  = *pStSrc++;                                      /* Get the left sample */
129         Right = *pStSrc++;                                      /* Get the right sample */
130         Mono  = *pMonoSrc++;                                    /* Get the mono sample */
131 
132 
133         /*
134          * Apply the AGC gain to the mono input and mix with the stereo signal
135          */
136         HighWord = (AGC_Mult * (Mono >> 16));                   /* signed long (Mono) by unsigned short (AGC_Mult) multiply */
137         LowWord = (AGC_Mult * (Mono & 0xffff));
138         Mono = (HighWord + (LowWord >> 16)) << (AGC_GainShift);
139         Left  += Mono;                                          /* Mix in the mono signal */
140         Right += Mono;
141 
142 
143         /*
144          * Apply the volume and write to the output stream
145          */
146         HighWord = (Vol_Mult * (Left >> 16));                   /* signed long (Left) by unsigned short (Vol_Mult) multiply */
147         LowWord = (Vol_Mult * (Left & 0xffff));
148         Left = (HighWord + (LowWord >> 16)) << (Vol_Shift);
149         HighWord = (Vol_Mult * (Right >> 16));                  /* signed long (Right) by unsigned short (Vol_Mult) multiply */
150         LowWord = (Vol_Mult * (Right & 0xffff));
151         Right = (HighWord + (LowWord >> 16)) << (Vol_Shift);
152         *pDst++ = Left;                                         /* Save the results */
153         *pDst++ = Right;
154 
155 
156         /*
157          * Update the AGC gain
158          */
159         AbsPeak = (Abs_32(Left)>Abs_32(Right)) ? Abs_32(Left) : Abs_32(Right);  /* Get the absolute peak */
160         if (AbsPeak > AGC_Target)
161         {
162             /*
163              * The signal is too large so decrease the gain
164              */
165             HighWord = (AGC_Attack * (AGC_Gain >> 16));         /* signed long (AGC_Gain) by unsigned short (AGC_Attack) multiply */
166             LowWord = (AGC_Attack * (AGC_Gain & 0xffff));
167             AGC_Gain = (HighWord + (LowWord >> 16)) << 1;
168         }
169         else
170         {
171             /*
172              * The signal is too small so increase the gain
173              */
174             if (AGC_Gain > AGC_MaxGain)
175             {
176                 AGC_Gain -= (AGC_Decay << DECAY_SHIFT);
177             }
178             else
179             {
180                 AGC_Gain += (AGC_Decay << DECAY_SHIFT);
181             }
182         }
183 
184         /*
185          * Update the gain
186          */
187         Vol_Current += Vol_TC * ((Vol_Target - Vol_Current) >> VOL_TC_SHIFT);
188     }
189 
190 
191     /*
192      * Update the parameters
193      */
194     pInstance->Volume = Vol_Current;                            /* Actual volume setting */
195     pInstance->AGC_Gain = AGC_Gain;
196 
197     return;
198 }
199 #else
AGC_MIX_VOL_2St1Mon_D32_WRA(AGC_MIX_VOL_2St1Mon_FLOAT_t * pInstance,const LVM_FLOAT * pStSrc,const LVM_FLOAT * pMonoSrc,LVM_FLOAT * pDst,LVM_UINT16 NumSamples)200 void AGC_MIX_VOL_2St1Mon_D32_WRA(AGC_MIX_VOL_2St1Mon_FLOAT_t  *pInstance,     /* Instance pointer */
201                                  const LVM_FLOAT            *pStSrc,        /* Stereo source */
202                                  const LVM_FLOAT            *pMonoSrc,      /* Mono source */
203                                  LVM_FLOAT                  *pDst,          /* Stereo destination */
204                                  LVM_UINT16                 NumSamples)     /* Number of samples */
205 {
206 
207     /*
208      * General variables
209      */
210     LVM_UINT16      i;                                          /* Sample index */
211     LVM_FLOAT       Left;                                       /* Left sample */
212     LVM_FLOAT       Right;                                      /* Right sample */
213     LVM_FLOAT       Mono;                                       /* Mono sample */
214     LVM_FLOAT       AbsPeak;                                    /* Absolute peak signal */
215     LVM_FLOAT       AGC_Mult;                                   /* Short AGC gain */
216     LVM_FLOAT       Vol_Mult;                                   /* Short volume */
217 
218 
219     /*
220      * Instance control variables
221      */
222     LVM_FLOAT      AGC_Gain      = pInstance->AGC_Gain;         /* Get the current AGC gain */
223     LVM_FLOAT      AGC_MaxGain   = pInstance->AGC_MaxGain;      /* Get maximum AGC gain */
224     LVM_FLOAT      AGC_Attack    = pInstance->AGC_Attack;       /* Attack scaler */
225     LVM_FLOAT      AGC_Decay     = (pInstance->AGC_Decay * (1 << (DECAY_SHIFT)));/* Decay scaler */
226     LVM_FLOAT      AGC_Target    = pInstance->AGC_Target;       /* Get the target level */
227     LVM_FLOAT      Vol_Current   = pInstance->Volume;           /* Actual volume setting */
228     LVM_FLOAT      Vol_Target    = pInstance->Target;           /* Target volume setting */
229     LVM_FLOAT      Vol_TC        = pInstance->VolumeTC;         /* Time constant */
230 
231 
232     /*
233      * Process on a sample by sample basis
234      */
235     for (i = 0; i < NumSamples; i++)                                  /* For each sample */
236     {
237 
238         /*
239          * Get the short scalers
240          */
241         AGC_Mult    = (LVM_FLOAT)(AGC_Gain);              /* Get the short AGC gain */
242         Vol_Mult    = (LVM_FLOAT)(Vol_Current);           /* Get the short volume gain */
243 
244 
245         /*
246          * Get the input samples
247          */
248         Left  = *pStSrc++;                                      /* Get the left sample */
249         Right = *pStSrc++;                                      /* Get the right sample */
250         Mono  = *pMonoSrc++;                                    /* Get the mono sample */
251 
252 
253         /*
254          * Apply the AGC gain to the mono input and mix with the stereo signal
255          */
256         Left  += (Mono * AGC_Mult);                               /* Mix in the mono signal */
257         Right += (Mono * AGC_Mult);
258 
259         /*
260          * Apply the volume and write to the output stream
261          */
262         Left  = Left  * Vol_Mult;
263         Right = Right * Vol_Mult;
264         *pDst++ = Left;                                         /* Save the results */
265         *pDst++ = Right;
266 
267         /*
268          * Update the AGC gain
269          */
270         AbsPeak = Abs_Float(Left) > Abs_Float(Right) ? Abs_Float(Left) : Abs_Float(Right);
271         if (AbsPeak > AGC_Target)
272         {
273             /*
274              * The signal is too large so decrease the gain
275              */
276             AGC_Gain = AGC_Gain * AGC_Attack;
277         }
278         else
279         {
280             /*
281              * The signal is too small so increase the gain
282              */
283             if (AGC_Gain > AGC_MaxGain)
284             {
285                 AGC_Gain -= (AGC_Decay);
286             }
287             else
288             {
289                 AGC_Gain += (AGC_Decay);
290             }
291         }
292 
293         /*
294          * Update the gain
295          */
296         Vol_Current +=  (Vol_Target - Vol_Current) * ((LVM_FLOAT)Vol_TC / VOL_TC_FLOAT);
297     }
298 
299 
300     /*
301      * Update the parameters
302      */
303     pInstance->Volume = Vol_Current;                            /* Actual volume setting */
304     pInstance->AGC_Gain = AGC_Gain;
305 
306     return;
307 }
308 #ifdef SUPPORT_MC
309 /****************************************************************************************/
310 /*                                                                                      */
311 /* FUNCTION:                  AGC_MIX_VOL_Mc1Mon_D32_WRA                                */
312 /*                                                                                      */
313 /* DESCRIPTION:                                                                         */
314 /*    Apply AGC and mix signals                                                         */
315 /*                                                                                      */
316 /*                                                                                      */
317 /*  McSrc   ------------------|                                                         */
318 /*                            |                                                         */
319 /*              ______       _|_        ________                                        */
320 /*             |      |     |   |      |        |                                       */
321 /*  MonoSrc -->| AGC  |---->| + |----->| Volume |------------------------------+--->    */
322 /*             | Gain |     |___|      | Gain   |                              |        */
323 /*             |______|                |________|                              |        */
324 /*                /|\                               __________     ________    |        */
325 /*                 |                               |          |   |        |   |        */
326 /*                 |-------------------------------| AGC Gain |<--| Peak   |<--|        */
327 /*                                                 | Update   |   | Detect |            */
328 /*                                                 |__________|   |________|            */
329 /*                                                                                      */
330 /*                                                                                      */
331 /* PARAMETERS:                                                                          */
332 /*  pInstance               Instance pointer                                            */
333 /*  pMcSrc                  Multichannel source                                         */
334 /*  pMonoSrc                Mono band pass source                                       */
335 /*  pDst                    Multichannel destination                                    */
336 /*  NrFrames                Number of frames                                            */
337 /*  NrChannels              Number of channels                                          */
338 /*                                                                                      */
339 /* RETURNS:                                                                             */
340 /*  Void                                                                                */
341 /*                                                                                      */
342 /* NOTES:                                                                               */
343 /*                                                                                      */
344 /****************************************************************************************/
AGC_MIX_VOL_Mc1Mon_D32_WRA(AGC_MIX_VOL_2St1Mon_FLOAT_t * pInstance,const LVM_FLOAT * pMcSrc,const LVM_FLOAT * pMonoSrc,LVM_FLOAT * pDst,LVM_UINT16 NrFrames,LVM_UINT16 NrChannels)345 void AGC_MIX_VOL_Mc1Mon_D32_WRA(AGC_MIX_VOL_2St1Mon_FLOAT_t  *pInstance,
346                                  const LVM_FLOAT            *pMcSrc,
347                                  const LVM_FLOAT            *pMonoSrc,
348                                  LVM_FLOAT                  *pDst,
349                                  LVM_UINT16                 NrFrames,
350                                  LVM_UINT16                 NrChannels)
351 {
352 
353     /*
354      * General variables
355      */
356     LVM_UINT16      i, jj;                                      /* Sample index */
357     LVM_FLOAT       SampleVal;                                  /* Sample value */
358     LVM_FLOAT       Mono;                                       /* Mono sample */
359     LVM_FLOAT       AbsPeak;                                    /* Absolute peak signal */
360     LVM_FLOAT       AGC_Mult;                                   /* Short AGC gain */
361     LVM_FLOAT       Vol_Mult;                                   /* Short volume */
362 
363 
364     /*
365      * Instance control variables
366      */
367     LVM_FLOAT      AGC_Gain      = pInstance->AGC_Gain;         /* Get the current AGC gain */
368     LVM_FLOAT      AGC_MaxGain   = pInstance->AGC_MaxGain;      /* Get maximum AGC gain */
369     LVM_FLOAT      AGC_Attack    = pInstance->AGC_Attack;       /* Attack scaler */
370     /* Decay scaler */
371     LVM_FLOAT      AGC_Decay     = (pInstance->AGC_Decay * (1 << (DECAY_SHIFT)));
372     LVM_FLOAT      AGC_Target    = pInstance->AGC_Target;       /* Get the target level */
373     LVM_FLOAT      Vol_Current   = pInstance->Volume;           /* Actual volume setting */
374     LVM_FLOAT      Vol_Target    = pInstance->Target;           /* Target volume setting */
375     LVM_FLOAT      Vol_TC        = pInstance->VolumeTC;         /* Time constant */
376 
377 
378     /*
379      * Process on a sample by sample basis
380      */
381     for (i = 0; i < NrFrames; i++)                                  /* For each frame */
382     {
383 
384         /*
385          * Get the scalers
386          */
387         AGC_Mult    = (LVM_FLOAT)(AGC_Gain);              /* Get the AGC gain */
388         Vol_Mult    = (LVM_FLOAT)(Vol_Current);           /* Get the volume gain */
389 
390         AbsPeak = 0.0f;
391         /*
392          * Get the input samples
393          */
394         for (jj = 0; jj < NrChannels; jj++)
395         {
396             SampleVal  = *pMcSrc++;                       /* Get the sample value of jj Channel*/
397             Mono       = *pMonoSrc;                       /* Get the mono sample */
398 
399             /*
400              * Apply the AGC gain to the mono input and mix with the input signal
401              */
402             SampleVal  += (Mono * AGC_Mult);                        /* Mix in the mono signal */
403 
404             /*
405              * Apply the volume and write to the output stream
406              */
407             SampleVal  = SampleVal  * Vol_Mult;
408 
409             *pDst++ = SampleVal;                                         /* Save the results */
410 
411             /*
412              * Update the AGC gain
413              */
414             AbsPeak = Abs_Float(SampleVal) > AbsPeak ? Abs_Float(SampleVal) : AbsPeak;
415         }
416         if (AbsPeak > AGC_Target)
417         {
418             /*
419              * The signal is too large so decrease the gain
420              */
421             AGC_Gain = AGC_Gain * AGC_Attack;
422         }
423         else
424         {
425             /*
426              * The signal is too small so increase the gain
427              */
428             if (AGC_Gain > AGC_MaxGain)
429             {
430                 AGC_Gain -= (AGC_Decay);
431             }
432             else
433             {
434                 AGC_Gain += (AGC_Decay);
435             }
436         }
437         pMonoSrc++;
438         /*
439          * Update the gain
440          */
441         Vol_Current +=  (Vol_Target - Vol_Current) * ((LVM_FLOAT)Vol_TC / VOL_TC_FLOAT);
442     }
443 
444 
445     /*
446      * Update the parameters
447      */
448     pInstance->Volume = Vol_Current;                            /* Actual volume setting */
449     pInstance->AGC_Gain = AGC_Gain;
450 
451     return;
452 }
453 #endif /*SUPPORT_MC*/
454 #endif /*BUILD_FLOAT*/
455