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 <stddef.h>
  12 #include <type_traits>
  13 #include <vector>
  14
  15 #include "alcomplex.h"
  16 #include "alspan.h"
  17
  18
  19 struct NoInit { };
  20
  21 /* Implements a wide-band +90 degree phase-shift. Note that this should be
  22  * given one sample less of a delay (FilterSize/2 - 1) compared to the direct
  23  * signal delay (FilterSize/2) to properly align.
  24  */
  25 template<size_t FilterSize>
  26 struct PhaseShifterT {
  27     static_assert(FilterSize >= 16, "FilterSize needs to be at least 16");
  28     static_assert((FilterSize&(FilterSize-1)) == 0, "FilterSize needs to be power-of-two");
  29
  30     alignas(16) std::array<float,FilterSize/2> mCoeffs{};
  31
  32     /* Some notes on this filter construction.
  33      *
  34      * A wide-band phase-shift filter needs a delay to maintain linearity. A
  35      * dirac impulse in the center of a time-domain buffer represents a filter
  36      * passing all frequencies through as-is with a pure delay. Converting that
  37      * to the frequency domain, adjusting the phase of each frequency bin by
  38      * +90 degrees, then converting back to the time domain, results in a FIR
  39      * filter that applies a +90 degree wide-band phase-shift.
  40      *
  41      * A particularly notable aspect of the time-domain filter response is that
  42      * every other coefficient is 0. This allows doubling the effective size of
  43      * the filter, by storing only the non-0 coefficients and double-stepping
  44      * over the input to apply it.
  45      *
  46      * Additionally, the resulting filter is independent of the sample rate.
  47      * The same filter can be applied regardless of the device's sample rate
  48      * and achieve the same effect.
  49      */
  50     PhaseShifterT()
  51     {
  52         using complex_d = std::complex<double>;
  53         constexpr size_t fft_size{FilterSize};
  54         constexpr size_t half_size{fft_size / 2};
  55
  56         auto fftBuffer = std::vector<complex_d>(fft_size, complex_d{});
  57         fftBuffer[half_size] = 1.0;
  58
  59         forward_fft(al::span{fftBuffer});
  60         fftBuffer[0] *= std::numeric_limits<double>::epsilon();
  61         for(size_t i{1};i < half_size;++i)
  62             fftBuffer[i] = complex_d{-fftBuffer[i].imag(), fftBuffer[i].real()};
  63         fftBuffer[half_size] *= std::numeric_limits<double>::epsilon();
  64         for(size_t i{half_size+1};i < fft_size;++i)
  65             fftBuffer[i] = std::conj(fftBuffer[fft_size - i]);
  66         inverse_fft(al::span{fftBuffer});
  67
  68         auto fftiter = fftBuffer.data() + fft_size - 1;
  69         for(float &coeff : mCoeffs)
  70         {
  71             coeff = static_cast<float>(fftiter->real() / double{fft_size});
  72             fftiter -= 2;
  73         }
  74     }
  75
  76     PhaseShifterT(NoInit) { }
  77
  78     void process(al::span<float> dst, const float *RESTRICT src) const;
  79
  80 private:
  81 #if defined(HAVE_NEON)
  82     static auto unpacklo(float32x4_t a, float32x4_t b)
  83     {
  84         float32x2x2_t result{vzip_f32(vget_low_f32(a), vget_low_f32(b))};
  85         return vcombine_f32(result.val[0], result.val[1]);
  86     }
  87     static auto unpackhi(float32x4_t a, float32x4_t b)
  88     {
  89         float32x2x2_t result{vzip_f32(vget_high_f32(a), vget_high_f32(b))};
  90         return vcombine_f32(result.val[0], result.val[1]);
  91     }
  92     static auto load4(float32_t a, float32_t b, float32_t c, float32_t d)
  93     {
  94         float32x4_t ret{vmovq_n_f32(a)};
  95         ret = vsetq_lane_f32(b, ret, 1);
  96         ret = vsetq_lane_f32(c, ret, 2);
  97         ret = vsetq_lane_f32(d, ret, 3);
  98         return ret;
  99     }
 100 #endif
 101 };
 102
 103 template<size_t S>
 104 inline void PhaseShifterT<S>::process(al::span<float> dst, const float *RESTRICT src) const
 105 {
 106 #ifdef HAVE_SSE_INTRINSICS
 107     if(size_t todo{dst.size()>>1})
 108     {
 109         auto *out = reinterpret_cast<__m64*>(dst.data());
 110         do {
 111             __m128 r04{_mm_setzero_ps()};
 112             __m128 r14{_mm_setzero_ps()};
 113             for(size_t j{0};j < mCoeffs.size();j+=4)
 114             {
 115                 const __m128 coeffs{_mm_load_ps(&mCoeffs[j])};
 116                 const __m128 s0{_mm_loadu_ps(&src[j*2])};
 117                 const __m128 s1{_mm_loadu_ps(&src[j*2 + 4])};
 118
 119                 __m128 s{_mm_shuffle_ps(s0, s1, _MM_SHUFFLE(2, 0, 2, 0))};
 120                 r04 = _mm_add_ps(r04, _mm_mul_ps(s, coeffs));
 121
 122                 s = _mm_shuffle_ps(s0, s1, _MM_SHUFFLE(3, 1, 3, 1));
 123                 r14 = _mm_add_ps(r14, _mm_mul_ps(s, coeffs));
 124             }
 125             src += 2;
 126
 127             __m128 r4{_mm_add_ps(_mm_unpackhi_ps(r04, r14), _mm_unpacklo_ps(r04, r14))};
 128             r4 = _mm_add_ps(r4, _mm_movehl_ps(r4, r4));
 129
 130             _mm_storel_pi(out, r4);
 131             ++out;
 132         } while(--todo);
 133     }
 134     if((dst.size()&1))
 135     {
 136         __m128 r4{_mm_setzero_ps()};
 137         for(size_t j{0};j < mCoeffs.size();j+=4)
 138         {
 139             const __m128 coeffs{_mm_load_ps(&mCoeffs[j])};
 140             const __m128 s{_mm_setr_ps(src[j*2], src[j*2 + 2], src[j*2 + 4], src[j*2 + 6])};
 141             r4 = _mm_add_ps(r4, _mm_mul_ps(s, coeffs));
 142         }
 143         r4 = _mm_add_ps(r4, _mm_shuffle_ps(r4, r4, _MM_SHUFFLE(0, 1, 2, 3)));
 144         r4 = _mm_add_ps(r4, _mm_movehl_ps(r4, r4));
 145
 146         dst.back() = _mm_cvtss_f32(r4);
 147     }
 148
 149 #elif defined(HAVE_NEON)
 150
 151     size_t pos{0};
 152     if(size_t todo{dst.size()>>1})
 153     {
 154         do {
 155             float32x4_t r04{vdupq_n_f32(0.0f)};
 156             float32x4_t r14{vdupq_n_f32(0.0f)};
 157             for(size_t j{0};j < mCoeffs.size();j+=4)
 158             {
 159                 const float32x4_t coeffs{vld1q_f32(&mCoeffs[j])};
 160                 const float32x4_t s0{vld1q_f32(&src[j*2])};
 161                 const float32x4_t s1{vld1q_f32(&src[j*2 + 4])};
 162                 const float32x4x2_t values{vuzpq_f32(s0, s1)};
 163
 164                 r04 = vmlaq_f32(r04, values.val[0], coeffs);
 165                 r14 = vmlaq_f32(r14, values.val[1], coeffs);
 166             }
 167             src += 2;
 168
 169             float32x4_t r4{vaddq_f32(unpackhi(r04, r14), unpacklo(r04, r14))};
 170             float32x2_t r2{vadd_f32(vget_low_f32(r4), vget_high_f32(r4))};
 171
 172             vst1_f32(&dst[pos], r2);
 173             pos += 2;
 174         } while(--todo);
 175     }
 176     if((dst.size()&1))
 177     {
 178         float32x4_t r4{vdupq_n_f32(0.0f)};
 179         for(size_t j{0};j < mCoeffs.size();j+=4)
 180         {
 181             const float32x4_t coeffs{vld1q_f32(&mCoeffs[j])};
 182             const float32x4_t s{load4(src[j*2], src[j*2 + 2], src[j*2 + 4], src[j*2 + 6])};
 183             r4 = vmlaq_f32(r4, s, coeffs);
 184         }
 185         r4 = vaddq_f32(r4, vrev64q_f32(r4));
 186         dst[pos] = vget_lane_f32(vadd_f32(vget_low_f32(r4), vget_high_f32(r4)), 0);
 187     }
 188
 189 #else
 190
 191     for(float &output : dst)
 192     {
 193         float ret{0.0f};
 194         for(size_t j{0};j < mCoeffs.size();++j)
 195             ret += src[j*2] * mCoeffs[j];
 196
 197         output = ret;
 198         ++src;
 199     }
 200 #endif
 201 }
 202
 203 #endif /* PHASE_SHIFTER_H */