omxsp_fftfwd_rtoccs_s16s32_sfs.c

The OpenMAX DL (Development Layer) APIs contain a comprehensive set of audio, video, signal processi

/**
 * 
 * File Name:  omxSP_FFTFwd_RToCCS_S16S32_Sfs.c
 * OpenMAX DL: v1.0.2
 * Revision:   10586
 * Date:       Wednesday, March 5, 2008
 * 
 * (c) Copyright 2007-2008 ARM Limited. All Rights Reserved.
 * 
 * 
 * Description:
 * Compute a forward FFT for a real-valued signal
 */

#include "omxtypes.h"
#include "armOMX.h"
#include "omxSP.h"
#include "armCOMM.h"
#include "armSP.h"
#include <math.h>

/** Real FFT, 16-bit */
/**
 * Function:  omxSP_FFTFwd_RToCCS_S16S32_Sfs   (2.2.4.4.2)
 *
 * Description:
 * These functions compute an FFT for a real-valued signal of length of 2^order,
 * where 0 <= order <= 12. Transform length is determined by the 
 * specification structure, which must be initialized prior to calling the FFT 
 * function using the appropriate helper, i.e., <FFTInit_R_S16S32>. 
 * The relationship between the input and output sequences 
 * can be expressed in terms of the DFT, i.e.:
 *
 *     x[n] = (2^(-scalefactor)/N)  . SUM[k=0,...,N-1] X[k].e^(jnk.2.pi/N)
 *     n=0,1,2,...N-1
 *     N=2^order.
 *
 * The conjugate-symmetric output sequence is represented using a CCS vector, 
 * which is of length N+2, and is organized as follows: 
 *
 *   Index:      0  1  2  3  4  5   . . .   N-2       N-1       N       N+1 
 *   Component:  R0 0  R1 I1 R2 I2  . . .   R[N/2-1]  I[N/2-1]  R[N/2]  0 
 *
 * where R[n] and I[n], respectively, denote the real and imaginary components 
 * for FFT bin 'n'. Bins  are numbered from 0 to N/2, where N is the FFT length. 
 * Bin index 0 corresponds to the DC component, and bin index N/2 corresponds to the 
 * foldover frequency. 
 *
 * Input Arguments:
 *   pSrc - pointer to the real-valued input sequence, of length 2^order; 
 *          must be aligned on a 32-byte boundary. 
 *   pFFTSpec - pointer to the preallocated and initialized specification 
 *            structure 
 *   scaleFactor - output scale factor; valid range is [0, 32] 
 *
 * Output Arguments:
 *   pDst - pointer to output sequence, represented using CCS format, of 
 *            length (2^order)+2; must be aligned on a 32-byte boundary. 
 *
 * Return Value:
 *    
 *    OMX_Sts_NoErr - no error 
 *    OMX_Sts_BadArgErr - bad arguments, if one or more of the following is true: 
 *    -    one of the pointers pSrc, pDst, or pFFTSpec is NULL 
 *    -    pSrc or pDst is not aligned on a 32-byte boundary 
 *    -    scaleFactor<0 or scaleFactor >32 
 *
 */

OMXResult omxSP_FFTFwd_RToCCS_S16S32_Sfs(
     const OMX_S16 *pSrc,
     OMX_S32 *pDst,
     const OMXFFTSpec_R_S16S32 *pFFTSpec,
     OMX_INT scaleFactor
)
{
    ARMsFFTSpec_R_FC64 *pFFTStruct;
    OMX_INT     i, j, k, l, nBy2, size, N, NBy4, point, stage, offset;
    OMX_U16     *pRevIndex;
    OMX_F64     h, c, s, c1, c2, s1, s2, *out, ar, ai, br, bi, tr, ti;
    OMX_F64     c1r, c1i, c2r, c2i, t1r, t1i, t2r, t2i, z1r, z1i, z2r, z2i;
    OMX_FC64    *pExp;

    /* Input parameter check */ 
    armRetArgErrIf(pSrc == NULL, OMX_Sts_BadArgErr)
    armRetArgErrIf(armNot32ByteAligned(pSrc), OMX_Sts_BadArgErr)
    armRetArgErrIf(pDst == NULL, OMX_Sts_BadArgErr)
    armRetArgErrIf(armNot32ByteAligned(pDst), OMX_Sts_BadArgErr)
    armRetArgErrIf(pFFTSpec == NULL, OMX_Sts_BadArgErr)
    armRetArgErrIf(scaleFactor < 0, OMX_Sts_BadArgErr)
    armRetArgErrIf(scaleFactor > 32, OMX_Sts_BadArgErr)

    /* Order range check */ 
    pFFTStruct = (ARMsFFTSpec_R_FC64 *) pFFTSpec;
    N = pFFTStruct->N;
    armRetArgErrIf(N < 1, OMX_Sts_BadArgErr)
    armRetArgErrIf(N > (1 << 12), OMX_Sts_BadArgErr)

    /* Handle order zero case separate */
    if (N == 1)
    {
        *pDst = armSatRoundLeftShift_S32(*pSrc, -scaleFactor);
        *(pDst + 1) = 0;
        *(pDst + 2) = 0;
        return OMX_Sts_NoErr;        
    }

    /* Do N/2 fft in float */
    out = pFFTStruct->pBuf;
    if (out == NULL)
    {
        return OMX_Sts_MemAllocErr;
    }

    /* bit reversal */    
    pRevIndex = pFFTStruct->pBitRev;
    out [0] = pSrc [0];
    out [1] = pSrc [1];
    for (i = 2; i < N; i += 2)
    {
        j = 2 * pRevIndex [(i >> 1) - 1];
        out [j] = pSrc [i];
        out [j + 1] = pSrc [i + 1];
    }
    
    NBy4 = N >> 2;
    pExp = pFFTStruct->pTwiddle;
    point = 2;
    for (stage = NBy4; stage > 0; stage >>= 1)
    {
        l = 0;
        for (i = 0; i < point ; i+= 2)
        {
            c = pExp[l << 1].Re;
            s = pExp[l << 1].Im;

            k = point + i;
            offset = 0;
            for (j = 0; j < stage; j++)
            {
                ar = out [offset + i];
                ai = out [offset + i + 1];
                br = out [offset + k];
                bi = out [offset + k + 1];
                /* B * W */
                tr = c*br - s*bi;
                ti = c*bi + s*br;

                out [offset + k] = ar - tr;
                out [offset + k + 1] = ai - ti;
                out [offset + i] = ar + tr;
                out [offset + i + 1] = ai + ti;

                offset += (2 * point);
            }

            l += stage;
        }
        point <<= 1;
    }

    /* Real */
    h = 0.5;
    size = N;
    nBy2 = size >> 1;

    for (i = 2, j = N - 2; i <= nBy2; i += 2, j -= 2)
    {
        c1r = out [i];     
        c1i = -out [i + 1];    
        c2r = out [j];     
        c2i = -out [j + 1];    
        z1r = out [i];     
        z1i = out [i + 1];    
        z2r = out [j];     
        z2i = out [j + 1];        

        /* CPLX_ADD (pT1, pZ2, pC1); */
        t1r = z2r + c1r;
        t1i = z2i + c1i;
        /* CPLX_SUB (pT2, pZ2, pC1); */
        t2r = z2r - c1r;
        t2i = z2i - c1i;
        /* CPLX_ADD (pC1, pZ1, pC2); */
        c1r = z1r + c2r;
        c1i = z1i + c2i;
        /* CPLX_SUB (pC2, pZ1, pC2); */
        c2r = z1r - c2r;
        c2i = z1i - c2i;

        c1 = pExp[i >> 1].Re;
        s1 = pExp[i >> 1].Im;
        c2 = pExp[j >> 1].Re;
        s2 = pExp[j >> 1].Im;

        /* CPLX_MUL (pZ1, pC2, pE1); */
        z1r = c2r*c1 - c2i*s1;
        z1i = c2r*s1 + c2i*c1;
        /* CPLX_MUL (pZ2, pT2, pE2); */
        z2r = t2r*c2 - t2i*s2;
        z2i = t2r*s2 + t2i*c2;
        
        /*pC2->Re = pZ1->Im;
        pC2->Im = -pZ1->Re;
        pT2->Re = pZ2->Im;
        pT2->Im = -pZ2->Re;*/
        c2r = z1i;
        c2i = -z1r;
        t2r = z2i;
        t2i = -z2r;

        out [i] = h * (c2r + c1r);        
        out [i + 1] = h * (c2i + c1i);
        
        out [j] = h * (t2r + t1r); 
        out [j + 1] = h * (t2i + t1i);
    }

    out [0] = (c1 = out [0]) + out [1];
    out [N] = c1 - out [1];
    out [1] = 0;
    out [N+1] = 0;

    /* revert back from float */
    for (i = 0; i < N + 2; i += 2)
    {        
        out [i] /= (OMX_F64)(1LL << scaleFactor);
        out [i + 1] /= (OMX_F64)(1LL << scaleFactor);
        pDst [i] = armSatRoundFloatToS32 (out [i]);
        pDst [i + 1] = armSatRoundFloatToS32 (out [i + 1]);
    }

    return OMX_Sts_NoErr;
}

/*****************************************************************************
 *                              END OF FILE
 *****************************************************************************/