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