volk_32fc_s32f_atan2_32f.h

Go to the documentation of this file.

/* -*- c++ -*- */
/*
 * Copyright 2014 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 *
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#ifndef INCLUDED_volk_32fc_s32f_atan2_32f_a_H
#define INCLUDED_volk_32fc_s32f_atan2_32f_a_H

#include <inttypes.h>
#include <stdio.h>
#include <math.h>

#ifdef LV_HAVE_SSE4_1
#include <smmintrin.h>

#ifdef LV_HAVE_LIB_SIMDMATH
#include <simdmath.h>
#endif /* LV_HAVE_LIB_SIMDMATH */

/*!
  \brief performs the atan2 on the input vector and stores the results in the output vector.
  \param outputVector The byte-aligned vector where the results will be stored.
  \param inputVector The byte-aligned input vector containing interleaved IQ data (I = cos, Q = sin).
  \param normalizeFactor The atan2 results will be divided by this normalization factor.
  \param num_points The number of complex values in the input vector.
*/
static inline void volk_32fc_s32f_atan2_32f_a_sse4_1(float* outputVector,  const lv_32fc_t* complexVector, const float normalizeFactor, unsigned int num_points){
  const float* complexVectorPtr = (float*)complexVector;
  float* outPtr = outputVector;

  unsigned int number = 0;
  const float invNormalizeFactor = 1.0 / normalizeFactor;

#ifdef LV_HAVE_LIB_SIMDMATH
  const unsigned int quarterPoints = num_points / 4;
  __m128 testVector = _mm_set_ps1(2*M_PI);
  __m128 correctVector = _mm_set_ps1(M_PI);
  __m128 vNormalizeFactor = _mm_set_ps1(invNormalizeFactor);
  __m128 phase;
  __m128 complex1, complex2, iValue, qValue;
  __m128 keepMask;

  for (; number < quarterPoints; number++) {
    // Load IQ data:
    complex1 = _mm_load_ps(complexVectorPtr);
    complexVectorPtr += 4;
    complex2 = _mm_load_ps(complexVectorPtr);
    complexVectorPtr += 4;
    // Deinterleave IQ data:
    iValue = _mm_shuffle_ps(complex1, complex2, _MM_SHUFFLE(2,0,2,0));
    qValue = _mm_shuffle_ps(complex1, complex2, _MM_SHUFFLE(3,1,3,1));
    // Arctan to get phase:
    phase = atan2f4(qValue, iValue);
    // When Q = 0 and I < 0, atan2f4 sucks and returns 2pi vice pi.
    // Compare to 2pi:
    keepMask = _mm_cmpneq_ps(phase,testVector);
    phase = _mm_blendv_ps(correctVector, phase, keepMask);
    // done with above correction.
    phase = _mm_mul_ps(phase, vNormalizeFactor);
    _mm_store_ps((float*)outPtr, phase);
    outPtr += 4;
  }
  number = quarterPoints * 4;
#endif /* LV_HAVE_SIMDMATH_H */

  for (; number < num_points; number++) {
    const float real = *complexVectorPtr++;
    const float imag = *complexVectorPtr++;
    *outPtr++ = atan2f(imag, real) * invNormalizeFactor;
  }
}
#endif /* LV_HAVE_SSE4_1 */


#ifdef LV_HAVE_SSE
#include <xmmintrin.h>

#ifdef LV_HAVE_LIB_SIMDMATH
#include <simdmath.h>
#endif /* LV_HAVE_LIB_SIMDMATH */

/*!
  \brief performs the atan2 on the input vector and stores the results in the output vector.
  \param outputVector The byte-aligned vector where the results will be stored.
  \param inputVector The byte-aligned input vector containing interleaved IQ data (I = cos, Q = sin).
  \param normalizeFactor The atan2 results will be divided by this normalization factor.
  \param num_points The number of complex values in the input vector.
*/
static inline void volk_32fc_s32f_atan2_32f_a_sse(float* outputVector,  const lv_32fc_t* complexVector, const float normalizeFactor, unsigned int num_points){
  const float* complexVectorPtr = (float*)complexVector;
  float* outPtr = outputVector;

  unsigned int number = 0;
  const float invNormalizeFactor = 1.0 / normalizeFactor;

#ifdef LV_HAVE_LIB_SIMDMATH
  const unsigned int quarterPoints = num_points / 4;
  __m128 testVector = _mm_set_ps1(2*M_PI);
  __m128 correctVector = _mm_set_ps1(M_PI);
  __m128 vNormalizeFactor = _mm_set_ps1(invNormalizeFactor);
  __m128 phase;
  __m128 complex1, complex2, iValue, qValue;
  __m128 mask;
  __m128 keepMask;

  for (; number < quarterPoints; number++) {
    // Load IQ data:
    complex1 = _mm_load_ps(complexVectorPtr);
    complexVectorPtr += 4;
    complex2 = _mm_load_ps(complexVectorPtr);
    complexVectorPtr += 4;
    // Deinterleave IQ data:
    iValue = _mm_shuffle_ps(complex1, complex2, _MM_SHUFFLE(2,0,2,0));
    qValue = _mm_shuffle_ps(complex1, complex2, _MM_SHUFFLE(3,1,3,1));
    // Arctan to get phase:
    phase = atan2f4(qValue, iValue);
    // When Q = 0 and I < 0, atan2f4 sucks and returns 2pi vice pi.
    // Compare to 2pi:
    keepMask = _mm_cmpneq_ps(phase,testVector);
    phase = _mm_and_ps(phase, keepMask);
    mask = _mm_andnot_ps(keepMask, correctVector);
    phase = _mm_or_ps(phase, mask);
    // done with above correction.
    phase = _mm_mul_ps(phase, vNormalizeFactor);
    _mm_store_ps((float*)outPtr, phase);
    outPtr += 4;
  }
  number = quarterPoints * 4;
#endif /* LV_HAVE_SIMDMATH_H */

  for (; number < num_points; number++) {
    const float real = *complexVectorPtr++;
    const float imag = *complexVectorPtr++;
    *outPtr++ = atan2f(imag, real) * invNormalizeFactor;
  }
}
#endif /* LV_HAVE_SSE */

#ifdef LV_HAVE_GENERIC
/*!
  \brief performs the atan2 on the input vector and stores the results in the output vector.
  \param outputVector The vector where the results will be stored.
  \param inputVector Input vector containing interleaved IQ data (I = cos, Q = sin).
  \param normalizeFactor The atan2 results will be divided by this normalization factor.
  \param num_points The number of complex values in the input vector.
*/
static inline void volk_32fc_s32f_atan2_32f_generic(float* outputVector, const lv_32fc_t* inputVector, const float normalizeFactor, unsigned int num_points){
  float* outPtr = outputVector;
  const float* inPtr = (float*)inputVector;
  const float invNormalizeFactor = 1.0 / normalizeFactor;
  unsigned int number;
  for ( number = 0; number < num_points; number++) {
    const float real = *inPtr++;
    const float imag = *inPtr++;
    *outPtr++ = atan2f(imag, real) * invNormalizeFactor;
  }
}
#endif /* LV_HAVE_GENERIC */




#endif /* INCLUDED_volk_32fc_s32f_atan2_32f_a_H */