src/third_party/webrtc/common_audio/fir_filter_sse.cc

   1 /*
   2  *  Copyright (c) 2014 The WebRTC project authors. All Rights Reserved.
   3  *
   4  *  Use of this source code is governed by a BSD-style license
   5  *  that can be found in the LICENSE file in the root of the source
   6  *  tree. An additional intellectual property rights grant can be found
   7  *  in the file PATENTS.  All contributing project authors may
   8  *  be found in the AUTHORS file in the root of the source tree.
   9  */
  10
  11 #include "webrtc/common_audio/fir_filter_sse.h"
  12
  13 #include <assert.h>
  14 #include <string.h>
  15 #include <xmmintrin.h>
  16
  17 #include "webrtc/system_wrappers/interface/aligned_malloc.h"
  18
  19 namespace webrtc {
  20
  21 FIRFilterSSE2::FIRFilterSSE2(const float* coefficients,
  22                              size_t coefficients_length,
  23                              size_t max_input_length)
  24     :  // Closest higher multiple of four.
  25       coefficients_length_((coefficients_length + 3) & ~0x03),
  26       state_length_(coefficients_length_ - 1),
  27       coefficients_(static_cast<float*>(
  28           AlignedMalloc(sizeof(float) * coefficients_length_, 16))),
  29       state_(static_cast<float*>(
  30           AlignedMalloc(sizeof(float) * (max_input_length + state_length_),
  31                         16))) {
  32   // Add zeros at the end of the coefficients.
  33   size_t padding = coefficients_length_ - coefficients_length;
  34   memset(coefficients_.get(), 0, padding * sizeof(coefficients_[0]));
  35   // The coefficients are reversed to compensate for the order in which the
  36   // input samples are acquired (most recent last).
  37   for (size_t i = 0; i < coefficients_length; ++i) {
  38     coefficients_[i + padding] = coefficients[coefficients_length - i - 1];
  39   }
  40   memset(state_.get(),
  41          0,
  42          (max_input_length + state_length_) * sizeof(state_[0]));
  43 }
  44
  45 void FIRFilterSSE2::Filter(const float* in, size_t length, float* out) {
  46   assert(length > 0);
  47
  48   memcpy(&state_[state_length_], in, length * sizeof(*in));
  49
  50   // Convolves the input signal |in| with the filter kernel |coefficients_|
  51   // taking into account the previous state.
  52   for (size_t i = 0; i < length; ++i) {
  53     float* in_ptr = &state_[i];
  54     float* coef_ptr = coefficients_.get();
  55
  56     __m128 m_sum = _mm_setzero_ps();
  57     __m128 m_in;
  58
  59     // Depending on if the pointer is aligned with 16 bytes or not it is loaded
  60     // differently.
  61     if (reinterpret_cast<uintptr_t>(in_ptr) & 0x0F) {
  62       for (size_t j = 0; j < coefficients_length_; j += 4) {
  63         m_in = _mm_loadu_ps(in_ptr + j);
  64         m_sum = _mm_add_ps(m_sum, _mm_mul_ps(m_in, _mm_load_ps(coef_ptr + j)));
  65       }
  66     } else {
  67       for (size_t j = 0; j < coefficients_length_; j += 4) {
  68         m_in = _mm_load_ps(in_ptr + j);
  69         m_sum = _mm_add_ps(m_sum, _mm_mul_ps(m_in, _mm_load_ps(coef_ptr + j)));
  70       }
  71     }
  72     m_sum = _mm_add_ps(_mm_movehl_ps(m_sum, m_sum), m_sum);
  73     _mm_store_ss(out + i, _mm_add_ss(m_sum, _mm_shuffle_ps(m_sum, m_sum, 1)));
  74   }
  75
  76   // Update current state.
  77   memmove(state_.get(), &state_[length], state_length_ * sizeof(state_[0]));
  78 }
  79
  80 }  // namespace webrtc