124be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org/* 224be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org * Copyright (c) 2014 The WebRTC project authors. All Rights Reserved. 324be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org * 424be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org * Use of this source code is governed by a BSD-style license 524be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org * that can be found in the LICENSE file in the root of the source 624be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org * tree. An additional intellectual property rights grant can be found 724be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org * in the file PATENTS. All contributing project authors may 824be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org * be found in the AUTHORS file in the root of the source tree. 924be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org */ 1024be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 1124be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org#include "webrtc/common_audio/fir_filter_sse.h" 1224be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 1324be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org#include <assert.h> 1424be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org#include <string.h> 1524be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org#include <xmmintrin.h> 1624be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 1724be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org#include "webrtc/system_wrappers/interface/aligned_malloc.h" 1824be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 1924be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.orgnamespace webrtc { 2024be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 2124be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.orgFIRFilterSSE2::FIRFilterSSE2(const float* coefficients, 2224be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org size_t coefficients_length, 2324be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org size_t max_input_length) 2424be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org : // Closest higher multiple of four. 2524be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org coefficients_length_((coefficients_length + 3) & ~0x03), 2624be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org state_length_(coefficients_length_ - 1), 2724be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org coefficients_(static_cast<float*>( 2824be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org AlignedMalloc(sizeof(float) * coefficients_length_, 16))), 2924be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org state_(static_cast<float*>( 3024be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org AlignedMalloc(sizeof(float) * (max_input_length + state_length_), 3124be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 16))) { 3224be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org // Add zeros at the end of the coefficients. 3324be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org size_t padding = coefficients_length_ - coefficients_length; 341f53a8f5c9591dd9e9896b2bb1e5456dbff6cb96bjornv@webrtc.org memset(coefficients_.get(), 0, padding * sizeof(coefficients_[0])); 3524be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org // The coefficients are reversed to compensate for the order in which the 3624be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org // input samples are acquired (most recent last). 3724be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org for (size_t i = 0; i < coefficients_length; ++i) { 3824be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org coefficients_[i + padding] = coefficients[coefficients_length - i - 1]; 3924be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org } 4024be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org memset(state_.get(), 411f53a8f5c9591dd9e9896b2bb1e5456dbff6cb96bjornv@webrtc.org 0, 4224be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org (max_input_length + state_length_) * sizeof(state_[0])); 4324be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org} 4424be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 4524be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.orgvoid FIRFilterSSE2::Filter(const float* in, size_t length, float* out) { 4624be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org assert(length > 0); 4724be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 4824be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org memcpy(&state_[state_length_], in, length * sizeof(*in)); 4924be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 5024be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org // Convolves the input signal |in| with the filter kernel |coefficients_| 5124be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org // taking into account the previous state. 5224be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org for (size_t i = 0; i < length; ++i) { 5324be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org float* in_ptr = &state_[i]; 5424be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org float* coef_ptr = coefficients_.get(); 5524be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 5624be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org __m128 m_sum = _mm_setzero_ps(); 5724be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org __m128 m_in; 5824be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 5924be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org // Depending on if the pointer is aligned with 16 bytes or not it is loaded 6024be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org // differently. 6124be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org if (reinterpret_cast<uintptr_t>(in_ptr) & 0x0F) { 6224be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org for (size_t j = 0; j < coefficients_length_; j += 4) { 6324be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org m_in = _mm_loadu_ps(in_ptr + j); 6424be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org m_sum = _mm_add_ps(m_sum, _mm_mul_ps(m_in, _mm_load_ps(coef_ptr + j))); 6524be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org } 6624be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org } else { 6724be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org for (size_t j = 0; j < coefficients_length_; j += 4) { 6824be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org m_in = _mm_load_ps(in_ptr + j); 6924be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org m_sum = _mm_add_ps(m_sum, _mm_mul_ps(m_in, _mm_load_ps(coef_ptr + j))); 7024be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org } 7124be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org } 7224be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org m_sum = _mm_add_ps(_mm_movehl_ps(m_sum, m_sum), m_sum); 7324be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org _mm_store_ss(out + i, _mm_add_ss(m_sum, _mm_shuffle_ps(m_sum, m_sum, 1))); 7424be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org } 7524be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 7624be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org // Update current state. 7724be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org memmove(state_.get(), &state_[length], state_length_ * sizeof(state_[0])); 7824be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org} 7924be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org 8024be06c54a94d1bf555dccdfd4e2a33ad69f70c4aluebs@webrtc.org} // namespace webrtc 81