xref: /external/sonivox/arm-fm-22k/lib_src/eas_chorus.c

/*----------------------------------------------------------------------------
 *
 * File:
 * eas_chorus.c
 *
 * Contents and purpose:
 * Contains the implementation of the Chorus effect.
 *
 *
 * Copyright Sonic Network Inc. 2006

 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *----------------------------------------------------------------------------
 * Revision Control:
 *   $Revision: 499 $
 *   $Date: 2006-12-11 16:07:20 -0800 (Mon, 11 Dec 2006) $
 *----------------------------------------------------------------------------
*/

#include "eas_data.h"
#include "eas_effects.h"
#include "eas_math.h"
#include "eas_chorusdata.h"
#include "eas_chorus.h"
#include "eas_config.h"
#include "eas_host.h"
#include "eas_report.h"

/* prototypes for effects interface */
static EAS_RESULT ChorusInit (EAS_DATA_HANDLE pEASData, EAS_VOID_PTR *pInstData);
static void ChorusProcess (EAS_VOID_PTR pInstData, EAS_PCM *pSrc, EAS_PCM *pDst, EAS_I32 numSamples);
static EAS_RESULT ChorusShutdown (EAS_DATA_HANDLE pEASData, EAS_VOID_PTR pInstData);
static EAS_RESULT ChorusGetParam (EAS_VOID_PTR pInstData, EAS_I32 param, EAS_I32 *pValue);
static EAS_RESULT ChorusSetParam (EAS_VOID_PTR pInstData, EAS_I32 param, EAS_I32 value);

/* common effects interface for configuration module */
const S_EFFECTS_INTERFACE EAS_Chorus =
{
    ChorusInit,
    ChorusProcess,
    ChorusShutdown,
    ChorusGetParam,
    ChorusSetParam
};



//LFO shape table used by the chorus, larger table would sound better
//this is a sine wave, where 32767 = 1.0
static const EAS_I16 EAS_chorusShape[CHORUS_SHAPE_SIZE] = {
    0, 1608, 3212, 4808, 6393, 7962, 9512, 11309, 12539, 14010, 15446, 16846, 18204, 19519, 20787, 22005, 23170,
    24279, 25329, 26319, 27245, 28105, 28898, 29621, 30273, 30852, 31356, 31785, 32137, 32412, 32609, 32728,
    32767, 32728, 32609, 32412, 32137, 31785, 31356, 30852, 30273, 29621, 28898, 28105, 27245, 26319, 25329,
    24279, 23170, 22005, 20787, 19519, 18204, 16846, 15446, 14010, 12539, 11039, 9512, 7962, 6393, 4808, 3212,
    1608, 0, -1608, -3212, -4808, -6393, -7962, -9512, -11309, -12539, -14010, -15446, -16846, -18204, -19519,
    -20787, -22005, -23170, -24279, -25329, -26319, -27245, -28105, -28898, -29621, -30273, -30852, -31356, -31785,
    -32137, -32412, -32609, -32728, -32767, -32728, -32609, -32412, -32137, -31785, -31356, -30852, -30273, -29621,
    -28898, -28105, -27245, -26319, -25329, -24279, -23170, -22005, -20787, -19519, -18204, -16846, -15446, -14010,
    -12539, -11039, -9512, -7962, -6393, -4808, -3212, -1608
};

/*----------------------------------------------------------------------------
 * InitializeChorus()
 *----------------------------------------------------------------------------
 * Purpose: Initializes chorus parameters
 *
 *
 * Inputs:
 *
 * Outputs:
 *
 *----------------------------------------------------------------------------
*/
static EAS_RESULT ChorusInit (EAS_DATA_HANDLE pEASData, EAS_VOID_PTR *pInstData)
{
    S_CHORUS_OBJECT *pChorusData;
    S_CHORUS_PRESET *pPreset;
    EAS_I32 index;

    /* check Configuration Module for data allocation */
    if (pEASData->staticMemoryModel)
        pChorusData = EAS_CMEnumFXData(EAS_MODULE_CHORUS);

    /* allocate dynamic memory */
    else
        pChorusData = EAS_HWMalloc(pEASData->hwInstData, sizeof(S_CHORUS_OBJECT));

    if (pChorusData == NULL)
    {
        { /* dpp: EAS_ReportEx(_EAS_SEVERITY_FATAL, "Failed to allocate Chorus memory\n"); */ }
        return EAS_ERROR_MALLOC_FAILED;
    }

    /* clear the structure */
    EAS_HWMemSet(pChorusData, 0, sizeof(S_CHORUS_OBJECT));

    ChorusReadInPresets(pChorusData);

    /* set some default values */
    pChorusData->bypass =       EAS_CHORUS_BYPASS_DEFAULT;
    pChorusData->preset =       EAS_CHORUS_PRESET_DEFAULT;
    pChorusData->m_nLevel =     EAS_CHORUS_LEVEL_DEFAULT;
    pChorusData->m_nRate =      EAS_CHORUS_RATE_DEFAULT;
    pChorusData->m_nDepth =     EAS_CHORUS_DEPTH_DEFAULT;

    //chorus rate and depth need some massaging from preset value (which is sample rate independent)

    //convert rate from steps of .05 Hz to value which can be used as phase increment,
    //with current CHORUS_SHAPE_SIZE and rate limits, this fits into 16 bits
    //want to compute ((shapeSize * 65536) * (storedRate/20))/sampleRate;
    //computing it as below allows rate steps to be evenly spaced
    //uses 32 bit divide, but only once when new value is selected
    pChorusData->m_nRate = (EAS_I16)
        ((((EAS_I32)CHORUS_SHAPE_SIZE<<16)/(20*(EAS_I32)_OUTPUT_SAMPLE_RATE)) * pChorusData->m_nRate);

    //convert depth from steps of .05 ms, to samples, with 16 bit whole part, discard fraction
    //want to compute ((depth * sampleRate)/20000)
    //use the following approximation since 105/32 is roughly 65536/20000
    /*lint -e{704} use shift for performance */
    pChorusData->m_nDepth = (EAS_I16)
        (((((EAS_I32)pChorusData->m_nDepth * _OUTPUT_SAMPLE_RATE)>>5) * 105) >> 16);

    pChorusData->m_nLevel = pChorusData->m_nLevel;

    //zero delay memory for chorus
    for (index = CHORUS_L_SIZE - 1; index >= 0; index--)
    {
        pChorusData->chorusDelayL[index] = 0;
    }
    for (index = CHORUS_R_SIZE - 1; index >= 0; index--)
    {
        pChorusData->chorusDelayR[index] = 0;
    }

    //init delay line index, these are used to implement circular delay buffer
    pChorusData->chorusIndexL = 0;
    pChorusData->chorusIndexR = 0;

    //init LFO phase
    //16 bit whole part, 16 bit fraction
    pChorusData->lfoLPhase = 0;
    pChorusData->lfoRPhase = (CHORUS_SHAPE_SIZE << 16) >> 2; // 1/4 of total, i.e. 90 degrees out of phase;

    //init chorus delay position
    //right now chorus delay is a compile-time value, as is sample rate
    pChorusData->chorusTapPosition = (EAS_I16)((CHORUS_DELAY_MS * _OUTPUT_SAMPLE_RATE)/1000);

    //now copy from the new preset into Chorus
    pPreset = &pChorusData->m_sPreset.m_sPreset[pChorusData->m_nNextChorus];

    pChorusData->m_nLevel = pPreset->m_nLevel;
    pChorusData->m_nRate =  pPreset->m_nRate;
    pChorusData->m_nDepth = pPreset->m_nDepth;

    pChorusData->m_nRate = (EAS_I16)
        ((((EAS_I32)CHORUS_SHAPE_SIZE<<16)/(20*(EAS_I32)_OUTPUT_SAMPLE_RATE)) * pChorusData->m_nRate);

    /*lint -e{704} use shift for performance */
    pChorusData->m_nDepth = (EAS_I16)
        (((((EAS_I32)pChorusData->m_nDepth * _OUTPUT_SAMPLE_RATE)>>5) * 105) >> 16);

    *pInstData = pChorusData;

    return EAS_SUCCESS;
} /* end ChorusInit */

/*----------------------------------------------------------------------------
 * WeightedTap()
 *----------------------------------------------------------------------------
 * Purpose: Does fractional array look-up using linear interpolation
 *
 * first convert indexDesired to actual desired index by taking into account indexReference
 * then do linear interpolation between two actual samples using fractional part
 *
 * Inputs:
 * array: pointer to array of signed 16 bit values, typically either PCM data or control data
 * indexReference: the circular buffer relative offset
 * indexDesired: the fractional index we are looking up (16 bits index + 16 bits fraction)
 * indexLimit: the total size of the array, used to compute buffer wrap
 *
 * Outputs:
 * Value from the input array, linearly interpolated between two actual data values
 *
 *----------------------------------------------------------------------------
*/
static EAS_I16 WeightedTap(const EAS_I16 *array, EAS_I16 indexReference, EAS_I32 indexDesired, EAS_I16 indexLimit)
{
    EAS_I16 index;
    EAS_I16 fraction;
    EAS_I16 val1;
    EAS_I16 val2;

    //separate indexDesired into whole and fractional parts
    /*lint -e{704} use shift for performance */
    index = (EAS_I16)(indexDesired >> 16);
    /*lint -e{704} use shift for performance */
    fraction = (EAS_I16)((indexDesired>>1) & 0x07FFF); //just use 15 bits of fractional part

    //adjust whole part by indexReference
    index = indexReference - index;
    //make sure we stay within array bounds, this implements circular buffer
    while (index < 0)
    {
        index += indexLimit;
    }

    //get two adjacent values from the array
    val1 = array[index];

    //handle special case when index == 0, else typical case
    if (index == 0)
    {
        val2 = array[indexLimit-1]; //get last value from array
    }
    else
    {
        val2 = array[index-1]; //get previous value from array
    }

    //compute linear interpolation as (val1 + ((val2-val1)*fraction))
    return(val1 + (EAS_I16)MULT_EG1_EG1(val2-val1,fraction));
}

/*----------------------------------------------------------------------------
 * ChorusProcess()
 *----------------------------------------------------------------------------
 * Purpose: compute the chorus on the input buffer, and mix into output buffer
 *
 *
 * Inputs:
 * src: pointer to input buffer of PCM values to be processed
 * dst: pointer to output buffer of PCM values we are to sume the result with
 * bufSize: the number of sample frames (i.e. stereo samples) in the buffer
 *
 * Outputs:
 * None
 *
 *----------------------------------------------------------------------------
*/
//compute the chorus, and mix into output buffer
static void ChorusProcess (EAS_VOID_PTR pInstData, EAS_PCM *pSrc, EAS_PCM *pDst, EAS_I32 numSamples)
{
    EAS_I32 ix;
    EAS_I32 nChannelNumber;
    EAS_I16 lfoValueLeft;
    EAS_I16 lfoValueRight;
    EAS_I32 positionOffsetL;
    EAS_I32 positionOffsetR;
    EAS_PCM tapL;
    EAS_PCM tapR;
    EAS_I32 tempValue;
    EAS_PCM nInputSample;
    EAS_I32 nOutputSample;
    EAS_PCM *pIn;
    EAS_PCM *pOut;

    S_CHORUS_OBJECT *pChorusData;

    pChorusData = (S_CHORUS_OBJECT*) pInstData;

    //if the chorus is disabled or turned all the way down
    if (pChorusData->bypass == EAS_TRUE || pChorusData->m_nLevel == 0)
    {
        if (pSrc != pDst)
            EAS_HWMemCpy(pSrc, pDst, numSamples * NUM_OUTPUT_CHANNELS * (EAS_I32) sizeof(EAS_PCM));
        return;
    }

    if (pChorusData->m_nNextChorus != pChorusData->m_nCurrentChorus)
    {
        ChorusUpdate(pChorusData);
    }

    for (nChannelNumber = 0; nChannelNumber < NUM_OUTPUT_CHANNELS; nChannelNumber++)
    {

        pIn = pSrc + nChannelNumber;
        pOut = pDst + nChannelNumber;

        if(nChannelNumber==0)
        {
            for (ix = 0; ix < numSamples; ix++)
            {
                nInputSample = *pIn;
                pIn += NUM_OUTPUT_CHANNELS;

                //feed input into chorus delay line
                pChorusData->chorusDelayL[pChorusData->chorusIndexL] = nInputSample;

                //compute chorus lfo value using phase as fractional index into chorus shape table
                //resulting value is between -1.0 and 1.0, expressed as signed 16 bit number
                lfoValueLeft = WeightedTap(EAS_chorusShape, 0, pChorusData->lfoLPhase, CHORUS_SHAPE_SIZE);

                //scale chorus depth by lfo value to get relative fractional sample index
                //index is expressed as 32 bit number with 16 bit fractional part
                /*lint -e{703} use shift for performance */
                positionOffsetL = pChorusData->m_nDepth * (((EAS_I32)lfoValueLeft) << 1);

                //add fixed chorus delay to get actual fractional sample index
                positionOffsetL += ((EAS_I32)pChorusData->chorusTapPosition) << 16;

                //get tap value from chorus delay using fractional sample index
                tapL = WeightedTap(pChorusData->chorusDelayL, pChorusData->chorusIndexL, positionOffsetL, CHORUS_L_SIZE);

                //scale by chorus level, then sum with input buffer contents and saturate
                tempValue = MULT_EG1_EG1(tapL, pChorusData->m_nLevel);
                nOutputSample = SATURATE(tempValue + nInputSample);

                *pOut = (EAS_I16)SATURATE(nOutputSample);
                pOut += NUM_OUTPUT_CHANNELS;


                //increment chorus delay index and make it wrap as needed
                //this implements circular buffer
                if ((pChorusData->chorusIndexL+=1) >= CHORUS_L_SIZE)
                    pChorusData->chorusIndexL = 0;

                //increment fractional lfo phase, and make it wrap as needed
                pChorusData->lfoLPhase += pChorusData->m_nRate;
                while (pChorusData->lfoLPhase >= (CHORUS_SHAPE_SIZE<<16))
                {
                    pChorusData->lfoLPhase -= (CHORUS_SHAPE_SIZE<<16);
                }
            }
        }
        else
        {
            for (ix = 0; ix < numSamples; ix++)
            {
                nInputSample = *pIn;
                pIn += NUM_OUTPUT_CHANNELS;

                //feed input into chorus delay line
                pChorusData->chorusDelayR[pChorusData->chorusIndexR] = nInputSample;

                //compute chorus lfo value using phase as fractional index into chorus shape table
                //resulting value is between -1.0 and 1.0, expressed as signed 16 bit number
                lfoValueRight = WeightedTap(EAS_chorusShape, 0, pChorusData->lfoRPhase, CHORUS_SHAPE_SIZE);

                //scale chorus depth by lfo value to get relative fractional sample index
                //index is expressed as 32 bit number with 16 bit fractional part
                /*lint -e{703} use shift for performance */
                positionOffsetR = pChorusData->m_nDepth * (((EAS_I32)lfoValueRight) << 1);

                //add fixed chorus delay to get actual fractional sample index
                positionOffsetR += ((EAS_I32)pChorusData->chorusTapPosition) << 16;

                //get tap value from chorus delay using fractional sample index
                tapR = WeightedTap(pChorusData->chorusDelayR, pChorusData->chorusIndexR, positionOffsetR, CHORUS_R_SIZE);

                //scale by chorus level, then sum with output buffer contents and saturate
                tempValue = MULT_EG1_EG1(tapR, pChorusData->m_nLevel);
                nOutputSample = SATURATE(tempValue + nInputSample);

                *pOut = (EAS_I16)SATURATE(nOutputSample);
                pOut += NUM_OUTPUT_CHANNELS;

                //increment chorus delay index and make it wrap as needed
                //this implements circular buffer
                if ((pChorusData->chorusIndexR+=1) >= CHORUS_R_SIZE)
                    pChorusData->chorusIndexR = 0;

                //increment fractional lfo phase, and make it wrap as needed
                pChorusData->lfoRPhase += pChorusData->m_nRate;
                while (pChorusData->lfoRPhase >= (CHORUS_SHAPE_SIZE<<16))
                {
                    pChorusData->lfoRPhase -= (CHORUS_SHAPE_SIZE<<16);
                }
            }
        }

    }
}  /* end ChorusProcess */



/*----------------------------------------------------------------------------
 * ChorusShutdown()
 *----------------------------------------------------------------------------
 * Purpose:
 * Initializes the Chorus effect.
 *
 * Inputs:
 * pInstData        - handle to instance data
 *
 * Outputs:
 *
 *
 * Side Effects:
 *
 *----------------------------------------------------------------------------
*/
static EAS_RESULT ChorusShutdown (EAS_DATA_HANDLE pEASData, EAS_VOID_PTR pInstData)
{
    /* check Configuration Module for static memory allocation */
    if (!pEASData->staticMemoryModel)
        EAS_HWFree(pEASData->hwInstData, pInstData);
    return EAS_SUCCESS;
} /* end ChorusShutdown */

/*----------------------------------------------------------------------------
 * ChorusGetParam()
 *----------------------------------------------------------------------------
 * Purpose:
 * Get a Chorus parameter
 *
 * Inputs:
 * pInstData        - handle to instance data
 * param            - parameter index
 * *pValue          - pointer to variable to hold retrieved value
 *
 * Outputs:
 *
 *
 * Side Effects:
 *
 *----------------------------------------------------------------------------
*/
static EAS_RESULT ChorusGetParam (EAS_VOID_PTR pInstData, EAS_I32 param, EAS_I32 *pValue)
{
    S_CHORUS_OBJECT *p;

    p = (S_CHORUS_OBJECT*) pInstData;

    switch (param)
    {
        case EAS_PARAM_CHORUS_BYPASS:
            *pValue = (EAS_I32) p->bypass;
            break;
        case EAS_PARAM_CHORUS_PRESET:
            *pValue = (EAS_I8) p->m_nCurrentChorus;
            break;
        case EAS_PARAM_CHORUS_RATE:
            *pValue = (EAS_I32) p->m_nRate;
            break;
        case EAS_PARAM_CHORUS_DEPTH:
            *pValue = (EAS_I32) p->m_nDepth;
            break;
        case EAS_PARAM_CHORUS_LEVEL:
            *pValue = (EAS_I32) p->m_nLevel;
            break;
        default:
            return EAS_ERROR_INVALID_PARAMETER;
    }
    return EAS_SUCCESS;
} /* end ChorusGetParam */


/*----------------------------------------------------------------------------
 * ChorusSetParam()
 *----------------------------------------------------------------------------
 * Purpose:
 * Set a Chorus parameter
 *
 * Inputs:
 * pInstData        - handle to instance data
 * param            - parameter index
 * *pValue          - new paramter value
 *
 * Outputs:
 *
 *
 * Side Effects:
 *
 *----------------------------------------------------------------------------
*/
static EAS_RESULT ChorusSetParam (EAS_VOID_PTR pInstData, EAS_I32 param, EAS_I32 value)
{
    S_CHORUS_OBJECT *p;

    p = (S_CHORUS_OBJECT*) pInstData;

    switch (param)
    {
        case EAS_PARAM_CHORUS_BYPASS:
            p->bypass = (EAS_BOOL) value;
            break;
        case EAS_PARAM_CHORUS_PRESET:
            if(value!=EAS_PARAM_CHORUS_PRESET1 && value!=EAS_PARAM_CHORUS_PRESET2 &&
                value!=EAS_PARAM_CHORUS_PRESET3 && value!=EAS_PARAM_CHORUS_PRESET4)
                return EAS_ERROR_INVALID_PARAMETER;
            p->m_nNextChorus = (EAS_I8)value;
            break;
        case EAS_PARAM_CHORUS_RATE:
            if(value<EAS_CHORUS_RATE_MIN || value>EAS_CHORUS_RATE_MAX)
                return EAS_ERROR_INVALID_PARAMETER;
            p->m_nRate = (EAS_I16) value;
            break;
        case EAS_PARAM_CHORUS_DEPTH:
            if(value<EAS_CHORUS_DEPTH_MIN || value>EAS_CHORUS_DEPTH_MAX)
                return EAS_ERROR_INVALID_PARAMETER;
            p->m_nDepth = (EAS_I16) value;
            break;
        case EAS_PARAM_CHORUS_LEVEL:
            if(value<EAS_CHORUS_LEVEL_MIN || value>EAS_CHORUS_LEVEL_MAX)
                return EAS_ERROR_INVALID_PARAMETER;
            p->m_nLevel = (EAS_I16) value;
            break;

        default:
            return EAS_ERROR_INVALID_PARAMETER;
    }
    return EAS_SUCCESS;
} /* end ChorusSetParam */


/*----------------------------------------------------------------------------
 * ChorusReadInPresets()
 *----------------------------------------------------------------------------
 * Purpose: sets global Chorus preset bank to defaults
 *
 * Inputs:
 *
 * Outputs:
 *
 *----------------------------------------------------------------------------
*/
static EAS_RESULT ChorusReadInPresets(S_CHORUS_OBJECT *pChorusData)
{

    int preset = 0;
    int defaultPreset = 0;

    //now init any remaining presets to defaults
    for (defaultPreset = preset; defaultPreset < CHORUS_MAX_TYPE; defaultPreset++)
    {
        S_CHORUS_PRESET *pPreset = &pChorusData->m_sPreset.m_sPreset[defaultPreset];
        if (defaultPreset == 0 || defaultPreset > CHORUS_MAX_TYPE-1)
        {
            pPreset->m_nDepth = 39;
            pPreset->m_nRate = 30;
            pPreset->m_nLevel = 32767;
        }
        else if (defaultPreset == 1)
        {
            pPreset->m_nDepth = 21;
            pPreset->m_nRate = 45;
            pPreset->m_nLevel = 25000;
        }
        else if (defaultPreset == 2)
        {
            pPreset->m_nDepth = 53;
            pPreset->m_nRate = 25;
            pPreset->m_nLevel = 32000;
        }
        else if (defaultPreset == 3)
        {
            pPreset->m_nDepth = 32;
            pPreset->m_nRate = 37;
            pPreset->m_nLevel = 29000;
        }
    }

    return EAS_SUCCESS;
}


/*----------------------------------------------------------------------------
 * ChorusUpdate
 *----------------------------------------------------------------------------
 * Purpose:
 * Update the Chorus preset parameters as required
 *
 * Inputs:
 *
 * Outputs:
 *
 *
 * Side Effects:
 * - chorus paramters will be changed
 * - m_nCurrentRoom := m_nNextRoom
 *----------------------------------------------------------------------------
*/
static EAS_RESULT ChorusUpdate(S_CHORUS_OBJECT *pChorusData)
{
    S_CHORUS_PRESET *pPreset = &pChorusData->m_sPreset.m_sPreset[pChorusData->m_nNextChorus];

    pChorusData->m_nLevel = pPreset->m_nLevel;
    pChorusData->m_nRate =  pPreset->m_nRate;
    pChorusData->m_nDepth = pPreset->m_nDepth;

    pChorusData->m_nRate = (EAS_I16)
        ((((EAS_I32)CHORUS_SHAPE_SIZE<<16)/(20*(EAS_I32)_OUTPUT_SAMPLE_RATE)) * pChorusData->m_nRate);

    /*lint -e{704} use shift for performance */
    pChorusData->m_nDepth = (EAS_I16)
        (((((EAS_I32)pChorusData->m_nDepth * _OUTPUT_SAMPLE_RATE)>>5) * 105) >> 16);

    pChorusData->m_nCurrentChorus = pChorusData->m_nNextChorus;

    return EAS_SUCCESS;

}   /* end ChorusUpdate */