tg2sip/webrtc_dsp/common_audio/signal_processing/complex_fft.c

/*
 *  Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */


/*
 * This file contains the function WebRtcSpl_ComplexFFT().
 * The description header can be found in signal_processing_library.h
 *
 */

#include "common_audio/signal_processing/complex_fft_tables.h"
#include "common_audio/signal_processing/include/signal_processing_library.h"
#include "rtc_base/system/arch.h"

#define CFFTSFT 14
#define CFFTRND 1
#define CFFTRND2 16384

#define CIFFTSFT 14
#define CIFFTRND 1


int WebRtcSpl_ComplexFFT(int16_t frfi[], int stages, int mode)
{
    int i, j, l, k, istep, n, m;
    int16_t wr, wi;
    int32_t tr32, ti32, qr32, qi32;

    /* The 1024-value is a constant given from the size of kSinTable1024[],
     * and should not be changed depending on the input parameter 'stages'
     */
    n = 1 << stages;
    if (n > 1024)
        return -1;

    l = 1;
    k = 10 - 1; /* Constant for given kSinTable1024[]. Do not change
         depending on the input parameter 'stages' */

    if (mode == 0)
    {
        // mode==0: Low-complexity and Low-accuracy mode
        while (l < n)
        {
            istep = l << 1;

            for (m = 0; m < l; ++m)
            {
                j = m << k;

                /* The 256-value is a constant given as 1/4 of the size of
                 * kSinTable1024[], and should not be changed depending on the input
                 * parameter 'stages'. It will result in 0 <= j < N_SINE_WAVE/2
                 */
                wr = kSinTable1024[j + 256];
                wi = -kSinTable1024[j];

                for (i = m; i < n; i += istep)
                {
                    j = i + l;

                    tr32 = (wr * frfi[2 * j] - wi * frfi[2 * j + 1]) >> 15;

                    ti32 = (wr * frfi[2 * j + 1] + wi * frfi[2 * j]) >> 15;

                    qr32 = (int32_t)frfi[2 * i];
                    qi32 = (int32_t)frfi[2 * i + 1];
                    frfi[2 * j] = (int16_t)((qr32 - tr32) >> 1);
                    frfi[2 * j + 1] = (int16_t)((qi32 - ti32) >> 1);
                    frfi[2 * i] = (int16_t)((qr32 + tr32) >> 1);
                    frfi[2 * i + 1] = (int16_t)((qi32 + ti32) >> 1);
                }
            }

            --k;
            l = istep;

        }

    } else
    {
        // mode==1: High-complexity and High-accuracy mode
        while (l < n)
        {
            istep = l << 1;

            for (m = 0; m < l; ++m)
            {
                j = m << k;

                /* The 256-value is a constant given as 1/4 of the size of
                 * kSinTable1024[], and should not be changed depending on the input
                 * parameter 'stages'. It will result in 0 <= j < N_SINE_WAVE/2
                 */
                wr = kSinTable1024[j + 256];
                wi = -kSinTable1024[j];

#ifdef WEBRTC_ARCH_ARM_V7
                int32_t wri = 0;
                __asm __volatile("pkhbt %0, %1, %2, lsl #16" : "=r"(wri) :
                    "r"((int32_t)wr), "r"((int32_t)wi));
#endif

                for (i = m; i < n; i += istep)
                {
                    j = i + l;

#ifdef WEBRTC_ARCH_ARM_V7
                    register int32_t frfi_r;
                    __asm __volatile(
                        "pkhbt %[frfi_r], %[frfi_even], %[frfi_odd],"
                        " lsl #16\n\t"
                        "smlsd %[tr32], %[wri], %[frfi_r], %[cfftrnd]\n\t"
                        "smladx %[ti32], %[wri], %[frfi_r], %[cfftrnd]\n\t"
                        :[frfi_r]"=&r"(frfi_r),
                         [tr32]"=&r"(tr32),
                         [ti32]"=r"(ti32)
                        :[frfi_even]"r"((int32_t)frfi[2*j]),
                         [frfi_odd]"r"((int32_t)frfi[2*j +1]),
                         [wri]"r"(wri),
                         [cfftrnd]"r"(CFFTRND));
#else
                    tr32 = wr * frfi[2 * j] - wi * frfi[2 * j + 1] + CFFTRND;

                    ti32 = wr * frfi[2 * j + 1] + wi * frfi[2 * j] + CFFTRND;
#endif

                    tr32 >>= 15 - CFFTSFT;
                    ti32 >>= 15 - CFFTSFT;

                    qr32 = ((int32_t)frfi[2 * i]) * (1 << CFFTSFT);
                    qi32 = ((int32_t)frfi[2 * i + 1]) * (1 << CFFTSFT);

                    frfi[2 * j] = (int16_t)(
                        (qr32 - tr32 + CFFTRND2) >> (1 + CFFTSFT));
                    frfi[2 * j + 1] = (int16_t)(
                        (qi32 - ti32 + CFFTRND2) >> (1 + CFFTSFT));
                    frfi[2 * i] = (int16_t)(
                        (qr32 + tr32 + CFFTRND2) >> (1 + CFFTSFT));
                    frfi[2 * i + 1] = (int16_t)(
                        (qi32 + ti32 + CFFTRND2) >> (1 + CFFTSFT));
                }
            }

            --k;
            l = istep;
        }
    }
    return 0;
}

int WebRtcSpl_ComplexIFFT(int16_t frfi[], int stages, int mode)
{
    size_t i, j, l, istep, n, m;
    int k, scale, shift;
    int16_t wr, wi;
    int32_t tr32, ti32, qr32, qi32;
    int32_t tmp32, round2;

    /* The 1024-value is a constant given from the size of kSinTable1024[],
     * and should not be changed depending on the input parameter 'stages'
     */
    n = ((size_t)1) << stages;
    if (n > 1024)
        return -1;

    scale = 0;

    l = 1;
    k = 10 - 1; /* Constant for given kSinTable1024[]. Do not change
         depending on the input parameter 'stages' */

    while (l < n)
    {
        // variable scaling, depending upon data
        shift = 0;
        round2 = 8192;

        tmp32 = WebRtcSpl_MaxAbsValueW16(frfi, 2 * n);
        if (tmp32 > 13573)
        {
            shift++;
            scale++;
            round2 <<= 1;
        }
        if (tmp32 > 27146)
        {
            shift++;
            scale++;
            round2 <<= 1;
        }

        istep = l << 1;

        if (mode == 0)
        {
            // mode==0: Low-complexity and Low-accuracy mode
            for (m = 0; m < l; ++m)
            {
                j = m << k;

                /* The 256-value is a constant given as 1/4 of the size of
                 * kSinTable1024[], and should not be changed depending on the input
                 * parameter 'stages'. It will result in 0 <= j < N_SINE_WAVE/2
                 */
                wr = kSinTable1024[j + 256];
                wi = kSinTable1024[j];

                for (i = m; i < n; i += istep)
                {
                    j = i + l;

                    tr32 = (wr * frfi[2 * j] - wi * frfi[2 * j + 1]) >> 15;

                    ti32 = (wr * frfi[2 * j + 1] + wi * frfi[2 * j]) >> 15;

                    qr32 = (int32_t)frfi[2 * i];
                    qi32 = (int32_t)frfi[2 * i + 1];
                    frfi[2 * j] = (int16_t)((qr32 - tr32) >> shift);
                    frfi[2 * j + 1] = (int16_t)((qi32 - ti32) >> shift);
                    frfi[2 * i] = (int16_t)((qr32 + tr32) >> shift);
                    frfi[2 * i + 1] = (int16_t)((qi32 + ti32) >> shift);
                }
            }
        } else
        {
            // mode==1: High-complexity and High-accuracy mode

            for (m = 0; m < l; ++m)
            {
                j = m << k;

                /* The 256-value is a constant given as 1/4 of the size of
                 * kSinTable1024[], and should not be changed depending on the input
                 * parameter 'stages'. It will result in 0 <= j < N_SINE_WAVE/2
                 */
                wr = kSinTable1024[j + 256];
                wi = kSinTable1024[j];

#ifdef WEBRTC_ARCH_ARM_V7
                int32_t wri = 0;
                __asm __volatile("pkhbt %0, %1, %2, lsl #16" : "=r"(wri) :
                    "r"((int32_t)wr), "r"((int32_t)wi));
#endif

                for (i = m; i < n; i += istep)
                {
                    j = i + l;

#ifdef WEBRTC_ARCH_ARM_V7
                    register int32_t frfi_r;
                    __asm __volatile(
                      "pkhbt %[frfi_r], %[frfi_even], %[frfi_odd], lsl #16\n\t"
                      "smlsd %[tr32], %[wri], %[frfi_r], %[cifftrnd]\n\t"
                      "smladx %[ti32], %[wri], %[frfi_r], %[cifftrnd]\n\t"
                      :[frfi_r]"=&r"(frfi_r),
                       [tr32]"=&r"(tr32),
                       [ti32]"=r"(ti32)
                      :[frfi_even]"r"((int32_t)frfi[2*j]),
                       [frfi_odd]"r"((int32_t)frfi[2*j +1]),
                       [wri]"r"(wri),
                       [cifftrnd]"r"(CIFFTRND)
                    );
#else

                    tr32 = wr * frfi[2 * j] - wi * frfi[2 * j + 1] + CIFFTRND;

                    ti32 = wr * frfi[2 * j + 1] + wi * frfi[2 * j] + CIFFTRND;
#endif
                    tr32 >>= 15 - CIFFTSFT;
                    ti32 >>= 15 - CIFFTSFT;

                    qr32 = ((int32_t)frfi[2 * i]) * (1 << CIFFTSFT);
                    qi32 = ((int32_t)frfi[2 * i + 1]) * (1 << CIFFTSFT);

                    frfi[2 * j] = (int16_t)(
                        (qr32 - tr32 + round2) >> (shift + CIFFTSFT));
                    frfi[2 * j + 1] = (int16_t)(
                        (qi32 - ti32 + round2) >> (shift + CIFFTSFT));
                    frfi[2 * i] = (int16_t)(
                        (qr32 + tr32 + round2) >> (shift + CIFFTSFT));
                    frfi[2 * i + 1] = (int16_t)(
                        (qi32 + ti32 + round2) >> (shift + CIFFTSFT));
                }
            }

        }
        --k;
        l = istep;
    }
    return scale;
}