common/phase_shifter.h

   1 #ifndef PHASE_SHIFTER_H
   2 #define PHASE_SHIFTER_H
   3
   4 #ifdef HAVE_SSE_INTRINSICS
   5 #include <xmmintrin.h>
   6 #elif defined(HAVE_NEON)
   7 #include <arm_neon.h>
   8 #endif
   9
  10 #include <array>
  11 #include <cmath>
  12 #include <cstddef>
  13
  14 #include "alnumbers.h"
  15 #include "alspan.h"
  16
  17
  18 /* Implements a wide-band +90 degree phase-shift. Note that this should be
  19  * given one sample less of a delay (FilterSize/2 - 1) compared to the direct
  20  * signal delay (FilterSize/2) to properly align.
  21  */
  22 template<std::size_t FilterSize>
  23 struct PhaseShifterT {
  24     static_assert(FilterSize >= 16, "FilterSize needs to be at least 16");
  25     static_assert((FilterSize&(FilterSize-1)) == 0, "FilterSize needs to be power-of-two");
  26
  27     alignas(16) std::array<float,FilterSize/2> mCoeffs{};
  28
  29     PhaseShifterT()
  30     {
  31         /* Every other coefficient is 0, so we only need to calculate and store
  32          * the non-0 terms and double-step over the input to apply it. The
  33          * calculated coefficients are in reverse to make applying in the time-
  34          * domain more efficient.
  35          */
  36         for(std::size_t i{0};i < FilterSize/2;++i)
  37         {
  38             const int k{static_cast<int>(i*2 + 1) - int{FilterSize/2}};
  39
  40             /* Calculate the Blackman window value for this coefficient. */
  41             const double w{2.0*al::numbers::pi * static_cast<double>(i*2 + 1)
  42                 / double{FilterSize}};
  43             const double window{0.3635819 - 0.4891775*std::cos(w) + 0.1365995*std::cos(2.0*w)
  44                 - 0.0106411*std::cos(3.0*w)};
  45
  46             const double pk{al::numbers::pi * static_cast<double>(k)};
  47             mCoeffs[i] = static_cast<float>(window * (1.0-std::cos(pk)) / pk);
  48         }
  49     }
  50
  51     void process(const al::span<float> dst, const al::span<const float> src) const;
  52
  53 private:
  54 #if defined(HAVE_NEON)
  55     static auto unpacklo(float32x4_t a, float32x4_t b)
  56     {
  57         float32x2x2_t result{vzip_f32(vget_low_f32(a), vget_low_f32(b))};
  58         return vcombine_f32(result.val[0], result.val[1]);
  59     }
  60     static auto unpackhi(float32x4_t a, float32x4_t b)
  61     {
  62         float32x2x2_t result{vzip_f32(vget_high_f32(a), vget_high_f32(b))};
  63         return vcombine_f32(result.val[0], result.val[1]);
  64     }
  65     static auto load4(float32_t a, float32_t b, float32_t c, float32_t d)
  66     {
  67         float32x4_t ret{vmovq_n_f32(a)};
  68         ret = vsetq_lane_f32(b, ret, 1);
  69         ret = vsetq_lane_f32(c, ret, 2);
  70         ret = vsetq_lane_f32(d, ret, 3);
  71         return ret;
  72     }
  73     static void vtranspose4(float32x4_t &x0, float32x4_t &x1, float32x4_t &x2, float32x4_t &x3)
  74     {
  75         float32x4x2_t t0_{vzipq_f32(x0, x2)};
  76         float32x4x2_t t1_{vzipq_f32(x1, x3)};
  77         float32x4x2_t u0_{vzipq_f32(t0_.val[0], t1_.val[0])};
  78         float32x4x2_t u1_{vzipq_f32(t0_.val[1], t1_.val[1])};
  79         x0 = u0_.val[0];
  80         x1 = u0_.val[1];
  81         x2 = u1_.val[0];
  82         x3 = u1_.val[1];
  83     }
  84 #endif
  85 };
  86
  87 template<std::size_t S>
  88 inline
  89 void PhaseShifterT<S>::process(const al::span<float> dst, const al::span<const float> src) const
  90 {
  91     auto in = src.begin();
  92 #ifdef HAVE_SSE_INTRINSICS
  93     if(const std::size_t todo{dst.size()>>2})
  94     {
  95         auto out = al::span{reinterpret_cast<__m128*>(dst.data()), todo};
  96         std::generate(out.begin(), out.end(), [&in,this]
  97         {
  98             __m128 r0{_mm_setzero_ps()};
  99             __m128 r1{_mm_setzero_ps()};
 100             __m128 r2{_mm_setzero_ps()};
 101             __m128 r3{_mm_setzero_ps()};
 102             for(std::size_t j{0};j < mCoeffs.size();j+=4)
 103             {
 104                 const __m128 coeffs{_mm_load_ps(&mCoeffs[j])};
 105                 const __m128 s0{_mm_loadu_ps(&in[j*2])};
 106                 const __m128 s1{_mm_loadu_ps(&in[j*2 + 4])};
 107                 const __m128 s2{_mm_movehl_ps(_mm_movelh_ps(s1, s1), s0)};
 108                 const __m128 s3{_mm_loadh_pi(_mm_movehl_ps(s1, s1),
 109                     reinterpret_cast<const __m64*>(&in[j*2 + 8]))};
 110
 111                 __m128 s{_mm_shuffle_ps(s0, s1, _MM_SHUFFLE(2, 0, 2, 0))};
 112                 r0 = _mm_add_ps(r0, _mm_mul_ps(s, coeffs));
 113
 114                 s = _mm_shuffle_ps(s0, s1, _MM_SHUFFLE(3, 1, 3, 1));
 115                 r1 = _mm_add_ps(r1, _mm_mul_ps(s, coeffs));
 116
 117                 s = _mm_shuffle_ps(s2, s3, _MM_SHUFFLE(2, 0, 2, 0));
 118                 r2 = _mm_add_ps(r2, _mm_mul_ps(s, coeffs));
 119
 120                 s = _mm_shuffle_ps(s2, s3, _MM_SHUFFLE(3, 1, 3, 1));
 121                 r3 = _mm_add_ps(r3, _mm_mul_ps(s, coeffs));
 122             }
 123             in += 4;
 124
 125             _MM_TRANSPOSE4_PS(r0, r1, r2, r3);
 126             return _mm_add_ps(_mm_add_ps(r0, r1), _mm_add_ps(r2, r3));
 127         });
 128     }
 129     if(const std::size_t todo{dst.size()&3})
 130     {
 131         auto out = dst.last(todo);
 132         std::generate(out.begin(), out.end(), [&in,this]
 133         {
 134             __m128 r4{_mm_setzero_ps()};
 135             for(std::size_t j{0};j < mCoeffs.size();j+=4)
 136             {
 137                 const __m128 coeffs{_mm_load_ps(&mCoeffs[j])};
 138                 const __m128 s{_mm_setr_ps(in[j*2], in[j*2 + 2], in[j*2 + 4], in[j*2 + 6])};
 139                 r4 = _mm_add_ps(r4, _mm_mul_ps(s, coeffs));
 140             }
 141             ++in;
 142             r4 = _mm_add_ps(r4, _mm_shuffle_ps(r4, r4, _MM_SHUFFLE(0, 1, 2, 3)));
 143             r4 = _mm_add_ps(r4, _mm_movehl_ps(r4, r4));
 144             return _mm_cvtss_f32(r4);
 145         });
 146     }
 147
 148 #elif defined(HAVE_NEON)
 149
 150     if(const std::size_t todo{dst.size()>>2})
 151     {
 152         auto out = al::span{reinterpret_cast<float32x4_t*>(dst.data()), todo};
 153         std::generate(out.begin(), out.end(), [&in,this]
 154         {
 155             float32x4_t r0{vdupq_n_f32(0.0f)};
 156             float32x4_t r1{vdupq_n_f32(0.0f)};
 157             float32x4_t r2{vdupq_n_f32(0.0f)};
 158             float32x4_t r3{vdupq_n_f32(0.0f)};
 159             for(std::size_t j{0};j < mCoeffs.size();j+=4)
 160             {
 161                 const float32x4_t coeffs{vld1q_f32(&mCoeffs[j])};
 162                 const float32x4_t s0{vld1q_f32(&in[j*2])};
 163                 const float32x4_t s1{vld1q_f32(&in[j*2 + 4])};
 164                 const float32x4_t s2{vcombine_f32(vget_high_f32(s0), vget_low_f32(s1))};
 165                 const float32x4_t s3{vcombine_f32(vget_high_f32(s1), vld1_f32(&in[j*2 + 8]))};
 166                 const float32x4x2_t values0{vuzpq_f32(s0, s1)};
 167                 const float32x4x2_t values1{vuzpq_f32(s2, s3)};
 168
 169                 r0 = vmlaq_f32(r0, values0.val[0], coeffs);
 170                 r1 = vmlaq_f32(r1, values0.val[1], coeffs);
 171                 r2 = vmlaq_f32(r2, values1.val[0], coeffs);
 172                 r3 = vmlaq_f32(r3, values1.val[1], coeffs);
 173             }
 174             in += 4;
 175
 176             vtranspose4(r0, r1, r2, r3);
 177             return vaddq_f32(vaddq_f32(r0, r1), vaddq_f32(r2, r3));
 178         });
 179     }
 180     if(const std::size_t todo{dst.size()&3})
 181     {
 182         auto out = dst.last(todo);
 183         std::generate(out.begin(), out.end(), [&in,this]
 184         {
 185             float32x4_t r4{vdupq_n_f32(0.0f)};
 186             for(std::size_t j{0};j < mCoeffs.size();j+=4)
 187             {
 188                 const float32x4_t coeffs{vld1q_f32(&mCoeffs[j])};
 189                 const float32x4_t s{load4(in[j*2], in[j*2 + 2], in[j*2 + 4], in[j*2 + 6])};
 190                 r4 = vmlaq_f32(r4, s, coeffs);
 191             }
 192             ++in;
 193             r4 = vaddq_f32(r4, vrev64q_f32(r4));
 194             return vget_lane_f32(vadd_f32(vget_low_f32(r4), vget_high_f32(r4)), 0);
 195         });
 196     }
 197
 198 #else
 199
 200     std::generate(dst.begin(), dst.end(), [&in,this]
 201     {
 202         float ret{0.0f};
 203         for(std::size_t j{0};j < mCoeffs.size();++j)
 204             ret += in[j*2] * mCoeffs[j];
 205         ++in;
 206         return ret;
 207     });
 208 #endif
 209 }
 210
 211 #endif /* PHASE_SHIFTER_H */