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#include "LVREV_Private.h"
24#include "VectorArithmetic.h"
25
26
27/****************************************************************************************/
28/*                                                                                      */
29/* FUNCTION:                LVREV_Process                                               */
30/*                                                                                      */
31/* DESCRIPTION:                                                                         */
32/*  Process function for the LVREV module.                                              */
33/*                                                                                      */
34/* PARAMETERS:                                                                          */
35/*  hInstance               Instance handle                                             */
36/*  pInData                 Pointer to the input data                                   */
37/*  pOutData                Pointer to the output data                                  */
38/*  NumSamples              Number of samples in the input buffer                       */
39/*                                                                                      */
40/* RETURNS:                                                                             */
41/*  LVREV_Success           Succeeded                                                   */
42/*  LVREV_INVALIDNUMSAMPLES NumSamples was larger than the maximum block size           */
43/*  LVREV_NULLADDRESS       When one of hInstance, pInData or pOutData is NULL          */
44/*                                                                                      */
45/* NOTES:                                                                               */
46/*  1. The input and output buffers must be 32-bit aligned                              */
47/*                                                                                      */
48/****************************************************************************************/
49LVREV_ReturnStatus_en LVREV_Process(LVREV_Handle_t      hInstance,
50                                    const LVM_INT32     *pInData,
51                                    LVM_INT32           *pOutData,
52                                    const LVM_UINT16    NumSamples)
53{
54   LVREV_Instance_st     *pLVREV_Private = (LVREV_Instance_st *)hInstance;
55   LVM_INT32             *pInput  = (LVM_INT32 *)pInData;
56   LVM_INT32             *pOutput = pOutData;
57   LVM_INT32             SamplesToProcess, RemainingSamples;
58   LVM_INT32             format = 1;
59
60    /*
61     * Check for error conditions
62     */
63
64    /* Check for NULL pointers */
65    if((hInstance == LVM_NULL) || (pInData == LVM_NULL) || (pOutData == LVM_NULL))
66    {
67        return LVREV_NULLADDRESS;
68    }
69
70    /*
71     * Apply the new controls settings if required
72     */
73    if(pLVREV_Private->bControlPending == LVM_TRUE)
74    {
75        LVREV_ReturnStatus_en   errorCode;
76
77        /*
78         * Clear the pending flag and update the control settings
79         */
80        pLVREV_Private->bControlPending = LVM_FALSE;
81
82        errorCode = LVREV_ApplyNewSettings (pLVREV_Private);
83
84        if(errorCode != LVREV_SUCCESS)
85        {
86            return errorCode;
87        }
88    }
89
90    /*
91     * Trap the case where the number of samples is zero.
92     */
93    if (NumSamples == 0)
94    {
95        return LVREV_SUCCESS;
96    }
97
98    /*
99     * If OFF copy and reformat the data as necessary
100     */
101    if (pLVREV_Private->CurrentParams.OperatingMode == LVM_MODE_OFF)
102    {
103        if(pInput != pOutput)
104        {
105            /*
106             * Copy the data to the output buffer, convert to stereo is required
107             */
108
109            if(pLVREV_Private->CurrentParams.SourceFormat == LVM_MONO){
110                MonoTo2I_32(pInput, pOutput, NumSamples);
111            } else {
112                Copy_16((LVM_INT16 *)pInput,
113                        (LVM_INT16 *)pOutput,
114                        (LVM_INT16)(NumSamples << 2)); // 32 bit data, stereo
115            }
116        }
117
118        return LVREV_SUCCESS;
119    }
120
121    RemainingSamples = (LVM_INT32)NumSamples;
122
123    if (pLVREV_Private->CurrentParams.SourceFormat != LVM_MONO)
124    {
125        format = 2;
126    }
127
128    while (RemainingSamples!=0)
129    {
130        /*
131         * Process the data
132         */
133
134        if(RemainingSamples >  pLVREV_Private->MaxBlkLen)
135        {
136            SamplesToProcess =  pLVREV_Private->MaxBlkLen;
137            RemainingSamples = (LVM_INT16)(RemainingSamples - SamplesToProcess);
138        }
139        else
140        {
141            SamplesToProcess = RemainingSamples;
142            RemainingSamples = 0;
143        }
144
145        ReverbBlock(pInput, pOutput, pLVREV_Private, (LVM_UINT16)SamplesToProcess);
146
147        pInput  = (LVM_INT32 *)(pInput +(SamplesToProcess*format));
148        pOutput = (LVM_INT32 *)(pOutput+(SamplesToProcess*2));      // Always stereo output
149    }
150
151    return LVREV_SUCCESS;
152}
153
154
155
156/****************************************************************************************/
157/*                                                                                      */
158/* FUNCTION:                ReverbBlock                                                 */
159/*                                                                                      */
160/* DESCRIPTION:                                                                         */
161/*  Process function for the LVREV module.                                              */
162/*                                                                                      */
163/* PARAMETERS:                                                                          */
164/*  hInstance               Instance handle                                             */
165/*  pInData                 Pointer to the input data                                   */
166/*  pOutData                Pointer to the output data                                  */
167/*  NumSamples              Number of samples in the input buffer                       */
168/*                                                                                      */
169/* RETURNS:                                                                             */
170/*  LVREV_Success           Succeeded                                                   */
171/*  LVREV_INVALIDNUMSAMPLES NumSamples was larger than the maximum block size           */
172/*  LVREV_NULLADDRESS       When one of hInstance, pInData or pOutData is NULL          */
173/*                                                                                      */
174/* NOTES:                                                                               */
175/*  1. The input and output buffers must be 32-bit aligned                              */
176/*                                                                                      */
177/****************************************************************************************/
178
179void ReverbBlock(LVM_INT32 *pInput, LVM_INT32 *pOutput, LVREV_Instance_st *pPrivate, LVM_UINT16 NumSamples)
180{
181    LVM_INT16   j, size;
182    LVM_INT32   *pDelayLine;
183    LVM_INT32   *pDelayLineInput = pPrivate->pScratch;
184    LVM_INT32   *pScratch = pPrivate->pScratch;
185    LVM_INT32   *pIn;
186    LVM_INT32   *pTemp = pPrivate->pInputSave;
187    LVM_INT32   NumberOfDelayLines;
188
189    /******************************************************************************
190     * All calculations will go into the buffer pointed to by pTemp, this will    *
191     * then be mixed with the original input to create the final output.          *
192     *                                                                            *
193     * When INPLACE processing is selected this must be a temporary buffer and    *
194     * hence this is the worst case, so for simplicity this will ALWAYS be so     *
195     *                                                                            *
196     * The input buffer will remain untouched until the output of the mixer if    *
197     * INPLACE processing is selected.                                            *
198     *                                                                            *
199     * The temp buffer will always be NumSamples in size regardless of MONO or    *
200     * STEREO input. In the case of stereo input all processing is done in MONO   *
201     * and the final output is converted to STEREO after the mixer                *
202     ******************************************************************************/
203
204    if(pPrivate->InstanceParams.NumDelays == LVREV_DELAYLINES_4 )
205    {
206        NumberOfDelayLines = 4;
207    }
208    else if(pPrivate->InstanceParams.NumDelays == LVREV_DELAYLINES_2 )
209    {
210        NumberOfDelayLines = 2;
211    }
212    else
213    {
214        NumberOfDelayLines = 1;
215    }
216
217    if(pPrivate->CurrentParams.SourceFormat == LVM_MONO)
218    {
219        pIn = pInput;
220    }
221    else
222    {
223        /*
224         *  Stereo to mono conversion
225         */
226
227        From2iToMono_32( pInput,
228                         pTemp,
229                         (LVM_INT16)NumSamples);
230
231        pIn = pTemp;
232    }
233
234    Mult3s_32x16(pIn,
235                 (LVM_INT16)LVREV_HEADROOM,
236                 pTemp,
237                 (LVM_INT16)NumSamples);
238
239    /*
240     *  High pass filter
241     */
242    FO_1I_D32F32C31_TRC_WRA_01( &pPrivate->pFastCoef->HPCoefs,
243                                pTemp,
244                                pTemp,
245                                (LVM_INT16)NumSamples);
246    /*
247     *  Low pass filter
248     */
249    FO_1I_D32F32C31_TRC_WRA_01( &pPrivate->pFastCoef->LPCoefs,
250                                pTemp,
251                                pTemp,
252                                (LVM_INT16)NumSamples);
253
254    /*
255     *  Process all delay lines
256     */
257
258    for(j = 0; j < NumberOfDelayLines; j++)
259    {
260        pDelayLine = pPrivate->pScratchDelayLine[j];
261
262        /*
263         * All-pass filter with pop and click suppression
264         */
265        /* Get the smoothed, delayed output. Put it in the output buffer */
266        MixSoft_2St_D32C31_SAT(&pPrivate->Mixer_APTaps[j],
267                               pPrivate->pOffsetA[j],
268                               pPrivate->pOffsetB[j],
269                               pDelayLine,
270                               (LVM_INT16)NumSamples);
271        /* Re-align the all pass filter delay buffer and copying the fixed delay data to the AP delay in the process */
272        Copy_16((LVM_INT16 *)&pPrivate->pDelay_T[j][NumSamples],
273                (LVM_INT16 *)pPrivate->pDelay_T[j],
274                (LVM_INT16)((pPrivate->T[j]-NumSamples) << 1));         /* 32-bit data */
275        /* Apply the smoothed feedback and save to fixed delay input (currently empty) */
276        MixSoft_1St_D32C31_WRA(&pPrivate->Mixer_SGFeedback[j],
277                               pDelayLine,
278                               &pPrivate->pDelay_T[j][pPrivate->T[j]-NumSamples],
279                               (LVM_INT16)NumSamples);
280        /* Sum into the AP delay line */
281        Mac3s_Sat_32x16(&pPrivate->pDelay_T[j][pPrivate->T[j]-NumSamples],
282                        -0x7fff,                                        /* Invert since the feedback coefficient is negative */
283                        &pPrivate->pDelay_T[j][pPrivate->Delay_AP[j]-NumSamples],
284                        (LVM_INT16)NumSamples);
285        /* Apply smoothed feedforward sand save to fixed delay input (currently empty) */
286        MixSoft_1St_D32C31_WRA(&pPrivate->Mixer_SGFeedforward[j],
287                               &pPrivate->pDelay_T[j][pPrivate->Delay_AP[j]-NumSamples],
288                               &pPrivate->pDelay_T[j][pPrivate->T[j]-NumSamples],
289                               (LVM_INT16)NumSamples);
290        /* Sum into the AP output */
291        Mac3s_Sat_32x16(&pPrivate->pDelay_T[j][pPrivate->T[j]-NumSamples],
292                        0x7fff,
293                        pDelayLine,
294                        (LVM_INT16)NumSamples);
295
296        /*
297         *  Feedback gain
298         */
299        MixSoft_1St_D32C31_WRA(&pPrivate->FeedbackMixer[j], pDelayLine, pDelayLine, NumSamples);
300
301        /*
302         *  Low pass filter
303         */
304        FO_1I_D32F32C31_TRC_WRA_01( &pPrivate->pFastCoef->RevLPCoefs[j],
305                                    pDelayLine,
306                                    pDelayLine,
307                                    (LVM_INT16)NumSamples);
308    }
309
310    /*
311     *  Apply rotation matrix and delay samples
312     */
313    for(j = 0; j < NumberOfDelayLines; j++)
314    {
315
316        Copy_16( (LVM_INT16*)(pTemp),
317                 (LVM_INT16*)(pDelayLineInput),
318                 (LVM_INT16)(NumSamples << 1));
319
320        /*
321         *  Rotation matrix mix
322         */
323        switch(j)
324        {
325            case 3:
326                /*
327                 *  Add delay line 1 and 2 contribution
328                 */
329                 Mac3s_Sat_32x16(pPrivate->pScratchDelayLine[1], -0x8000, pDelayLineInput, (LVM_INT16)NumSamples);
330                 Mac3s_Sat_32x16(pPrivate->pScratchDelayLine[2], -0x8000, pDelayLineInput, (LVM_INT16)NumSamples);
331
332                break;
333            case 2:
334
335                /*
336                 *  Add delay line 0 and 3 contribution
337                 */
338                 Mac3s_Sat_32x16(pPrivate->pScratchDelayLine[0], -0x8000, pDelayLineInput, (LVM_INT16)NumSamples);
339                 Mac3s_Sat_32x16(pPrivate->pScratchDelayLine[3], -0x8000, pDelayLineInput, (LVM_INT16)NumSamples);
340
341                break;
342            case 1:
343                if(pPrivate->InstanceParams.NumDelays == LVREV_DELAYLINES_4)
344                {
345                    /*
346                     *  Add delay line 0 and 3 contribution
347                     */
348                    Mac3s_Sat_32x16(pPrivate->pScratchDelayLine[0], -0x8000, pDelayLineInput, (LVM_INT16)NumSamples);
349                    Add2_Sat_32x32(pPrivate->pScratchDelayLine[3], pDelayLineInput, (LVM_INT16)NumSamples);
350
351                }
352                else
353                {
354                    /*
355                     *  Add delay line 0 and 1 contribution
356                     */
357                     Mac3s_Sat_32x16(pPrivate->pScratchDelayLine[0], -0x8000, pDelayLineInput, (LVM_INT16)NumSamples);
358                     Mac3s_Sat_32x16(pPrivate->pScratchDelayLine[1], -0x8000, pDelayLineInput, (LVM_INT16)NumSamples);
359
360                }
361                break;
362            case 0:
363                if(pPrivate->InstanceParams.NumDelays == LVREV_DELAYLINES_4)
364                {
365                    /*
366                     *  Add delay line 1 and 2 contribution
367                     */
368                    Mac3s_Sat_32x16(pPrivate->pScratchDelayLine[1], -0x8000, pDelayLineInput, (LVM_INT16)NumSamples);
369                    Add2_Sat_32x32(pPrivate->pScratchDelayLine[2], pDelayLineInput, (LVM_INT16)NumSamples);
370
371                }
372                else if(pPrivate->InstanceParams.NumDelays == LVREV_DELAYLINES_2)
373                {
374                    /*
375                     *  Add delay line 0 and 1 contribution
376                     */
377                    Add2_Sat_32x32(pPrivate->pScratchDelayLine[0], pDelayLineInput, (LVM_INT16)NumSamples);
378                    Mac3s_Sat_32x16(pPrivate->pScratchDelayLine[1], -0x8000, pDelayLineInput, (LVM_INT16)NumSamples);
379
380                }
381                else
382                {
383                    /*
384                     *  Add delay line 0 contribution
385                     */
386
387                    /*             SOURCE                          DESTINATION*/
388                    Add2_Sat_32x32(pPrivate->pScratchDelayLine[0], pDelayLineInput, (LVM_INT16)NumSamples);
389                }
390                break;
391            default:
392                break;
393        }
394
395        /*
396         *  Delay samples
397         */
398        Copy_16((LVM_INT16 *)pDelayLineInput,
399                (LVM_INT16 *)&pPrivate->pDelay_T[j][pPrivate->T[j]-NumSamples],
400                (LVM_INT16)(NumSamples << 1));              /* 32-bit data */
401
402    }
403
404
405    /*
406     *  Create stereo output
407     */
408    switch(pPrivate->InstanceParams.NumDelays)
409    {
410        case LVREV_DELAYLINES_4:
411             Add2_Sat_32x32(pPrivate->pScratchDelayLine[3],
412                            pPrivate->pScratchDelayLine[0],
413                            (LVM_INT16)NumSamples);
414             Add2_Sat_32x32(pPrivate->pScratchDelayLine[2],
415                            pPrivate->pScratchDelayLine[1],
416                            (LVM_INT16)NumSamples);
417
418
419            JoinTo2i_32x32(pPrivate->pScratchDelayLine[0],
420                           pPrivate->pScratchDelayLine[1],
421                           pTemp,
422                           (LVM_INT16)NumSamples);
423
424
425            break;
426        case LVREV_DELAYLINES_2:
427
428             Copy_16( (LVM_INT16*)pPrivate->pScratchDelayLine[1],
429                      (LVM_INT16*)pScratch,
430                      (LVM_INT16)(NumSamples << 1));
431
432            Mac3s_Sat_32x16(pPrivate->pScratchDelayLine[0],
433                            -0x8000,
434                            pScratch,
435                            (LVM_INT16)NumSamples);
436
437             Add2_Sat_32x32(pPrivate->pScratchDelayLine[1],
438                            pPrivate->pScratchDelayLine[0],
439                            (LVM_INT16)NumSamples);
440
441
442             JoinTo2i_32x32(pPrivate->pScratchDelayLine[0],
443                            pScratch,
444                            pTemp,
445                            (LVM_INT16)NumSamples);
446            break;
447        case LVREV_DELAYLINES_1:
448            MonoTo2I_32(pPrivate->pScratchDelayLine[0],
449                        pTemp,
450                        (LVM_INT16)NumSamples);
451            break;
452        default:
453            break;
454    }
455
456
457    /*
458     *  Dry/wet mixer
459     */
460
461    size = (LVM_INT16)(NumSamples << 1);
462    MixSoft_2St_D32C31_SAT(&pPrivate->BypassMixer,
463                           pTemp,
464                           pTemp,
465                           pOutput,
466                           size);
467
468    /* Apply Gain*/
469
470    Shift_Sat_v32xv32 (LVREV_OUTPUTGAIN_SHIFT,
471                       pOutput,
472                       pOutput,
473                       size);
474
475    MixSoft_1St_D32C31_WRA(&pPrivate->GainMixer,
476                           pOutput,
477                           pOutput,
478                           size);
479
480    return;
481}
482
483
484/* End of file */
485
486