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 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 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 /****************************************************************************************/ 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