aluebs@webrtc.org | 37ca765 | 2014-03-24 10:16:11 +0000 | [diff] [blame] | 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 | |
Mirko Bonadei | 92ea95e | 2017-09-15 06:47:31 +0200 | [diff] [blame] | 11 | #include "common_audio/fir_filter_sse.h" |
aluebs@webrtc.org | 37ca765 | 2014-03-24 10:16:11 +0000 | [diff] [blame] | 12 | |
kwiberg | c2b785d | 2016-02-24 05:22:32 -0800 | [diff] [blame] | 13 | #include <stdint.h> |
aluebs@webrtc.org | 37ca765 | 2014-03-24 10:16:11 +0000 | [diff] [blame] | 14 | #include <string.h> |
| 15 | #include <xmmintrin.h> |
| 16 | |
Mirko Bonadei | 92ea95e | 2017-09-15 06:47:31 +0200 | [diff] [blame] | 17 | #include "rtc_base/checks.h" |
Karl Wiberg | 29e7bee | 2018-03-22 14:11:52 +0100 | [diff] [blame] | 18 | #include "rtc_base/memory/aligned_malloc.h" |
aluebs@webrtc.org | 37ca765 | 2014-03-24 10:16:11 +0000 | [diff] [blame] | 19 | |
| 20 | namespace webrtc { |
| 21 | |
Patrik Höglund | f715c53 | 2017-11-17 11:04:15 +0100 | [diff] [blame] | 22 | FIRFilterSSE2::~FIRFilterSSE2() { |
| 23 | } |
| 24 | |
aluebs@webrtc.org | 37ca765 | 2014-03-24 10:16:11 +0000 | [diff] [blame] | 25 | FIRFilterSSE2::FIRFilterSSE2(const float* coefficients, |
| 26 | size_t coefficients_length, |
| 27 | size_t max_input_length) |
| 28 | : // Closest higher multiple of four. |
| 29 | coefficients_length_((coefficients_length + 3) & ~0x03), |
| 30 | state_length_(coefficients_length_ - 1), |
| 31 | coefficients_(static_cast<float*>( |
| 32 | AlignedMalloc(sizeof(float) * coefficients_length_, 16))), |
| 33 | state_(static_cast<float*>( |
| 34 | AlignedMalloc(sizeof(float) * (max_input_length + state_length_), |
| 35 | 16))) { |
| 36 | // Add zeros at the end of the coefficients. |
| 37 | size_t padding = coefficients_length_ - coefficients_length; |
bjornv@webrtc.org | 3cbd6c2 | 2014-09-04 13:21:44 +0000 | [diff] [blame] | 38 | memset(coefficients_.get(), 0, padding * sizeof(coefficients_[0])); |
aluebs@webrtc.org | 37ca765 | 2014-03-24 10:16:11 +0000 | [diff] [blame] | 39 | // The coefficients are reversed to compensate for the order in which the |
| 40 | // input samples are acquired (most recent last). |
| 41 | for (size_t i = 0; i < coefficients_length; ++i) { |
| 42 | coefficients_[i + padding] = coefficients[coefficients_length - i - 1]; |
| 43 | } |
| 44 | memset(state_.get(), |
bjornv@webrtc.org | 3cbd6c2 | 2014-09-04 13:21:44 +0000 | [diff] [blame] | 45 | 0, |
aluebs@webrtc.org | 37ca765 | 2014-03-24 10:16:11 +0000 | [diff] [blame] | 46 | (max_input_length + state_length_) * sizeof(state_[0])); |
| 47 | } |
| 48 | |
| 49 | void FIRFilterSSE2::Filter(const float* in, size_t length, float* out) { |
kwiberg | b890c95c | 2016-11-29 05:30:40 -0800 | [diff] [blame] | 50 | RTC_DCHECK_GT(length, 0); |
aluebs@webrtc.org | 37ca765 | 2014-03-24 10:16:11 +0000 | [diff] [blame] | 51 | |
| 52 | memcpy(&state_[state_length_], in, length * sizeof(*in)); |
| 53 | |
| 54 | // Convolves the input signal |in| with the filter kernel |coefficients_| |
| 55 | // taking into account the previous state. |
| 56 | for (size_t i = 0; i < length; ++i) { |
| 57 | float* in_ptr = &state_[i]; |
| 58 | float* coef_ptr = coefficients_.get(); |
| 59 | |
| 60 | __m128 m_sum = _mm_setzero_ps(); |
| 61 | __m128 m_in; |
| 62 | |
| 63 | // Depending on if the pointer is aligned with 16 bytes or not it is loaded |
| 64 | // differently. |
| 65 | if (reinterpret_cast<uintptr_t>(in_ptr) & 0x0F) { |
| 66 | for (size_t j = 0; j < coefficients_length_; j += 4) { |
| 67 | m_in = _mm_loadu_ps(in_ptr + j); |
| 68 | m_sum = _mm_add_ps(m_sum, _mm_mul_ps(m_in, _mm_load_ps(coef_ptr + j))); |
| 69 | } |
| 70 | } else { |
| 71 | for (size_t j = 0; j < coefficients_length_; j += 4) { |
| 72 | m_in = _mm_load_ps(in_ptr + j); |
| 73 | m_sum = _mm_add_ps(m_sum, _mm_mul_ps(m_in, _mm_load_ps(coef_ptr + j))); |
| 74 | } |
| 75 | } |
| 76 | m_sum = _mm_add_ps(_mm_movehl_ps(m_sum, m_sum), m_sum); |
| 77 | _mm_store_ss(out + i, _mm_add_ss(m_sum, _mm_shuffle_ps(m_sum, m_sum, 1))); |
| 78 | } |
| 79 | |
| 80 | // Update current state. |
| 81 | memmove(state_.get(), &state_[length], state_length_ * sizeof(state_[0])); |
| 82 | } |
| 83 | |
| 84 | } // namespace webrtc |