1 | /* |
2 | Copyright 2018 Google Inc. All Rights Reserved. |
3 | |
4 | Licensed under the Apache License, Version 2.0 (the "License"); |
5 | you may not use this file except in compliance with the License. |
6 | You may obtain a copy of the License at |
7 | |
8 | http://www.apache.org/licenses/LICENSE-2.0 |
9 | |
10 | Unless required by applicable law or agreed to in writing, software |
11 | distributed under the License is distributed on an "AS-IS" BASIS, |
12 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
13 | See the License for the specific language governing permissions and |
14 | limitations under the License. |
15 | */ |
16 | |
17 | #include "dsp/multi_channel_iir.h" |
18 | |
19 | #include <algorithm> |
20 | #include <limits> |
21 | |
22 | #include "base/constants_and_types.h" |
23 | #include "base/logging.h" |
24 | #include "base/simd_macros.h" |
25 | |
26 | |
27 | namespace vraudio { |
28 | |
29 | std::unique_ptr<MultiChannelIir> MultiChannelIir::Create( |
30 | size_t num_channels, size_t frames_per_buffer, |
31 | const std::vector<std::vector<float>>& numerators, |
32 | const std::vector<std::vector<float>>& denominators) { |
33 | CHECK_EQ(denominators.size(), numerators.size()); |
34 | CHECK_EQ(denominators.size(), num_channels); |
35 | CHECK_EQ(num_channels % SIMD_LENGTH, 0U); |
36 | CHECK_GT(num_channels, 0U); |
37 | for (size_t channel = 0; channel < num_channels; ++channel) { |
38 | CHECK_GT(denominators[channel].size(), 0U); |
39 | CHECK_GT(std::abs(denominators[channel][0]), |
40 | std::numeric_limits<float>::epsilon()); |
41 | CHECK_EQ(denominators[channel].size(), numerators[channel].size()); |
42 | } |
43 | |
44 | std::unique_ptr<MultiChannelIir> multi_channel_iir(new MultiChannelIir( |
45 | num_channels, frames_per_buffer, numerators[0].size())); |
46 | CHECK(multi_channel_iir); |
47 | |
48 | for (size_t channel = 0; channel < num_channels; ++channel) { |
49 | size_t interleaved_index = channel; |
50 | for (size_t i = 0; i < numerators[channel].size(); ++i) { |
51 | // Make the denominator polynomials monic, (divide all coefficients by the |
52 | // the first denominator coefficients). Furthermore negate all |
53 | // coefficients beyond the first (see equation in Process method). |
54 | // Copy the coefficients into the |numerator_| and |denominator_| buffers. |
55 | multi_channel_iir->numerator_[0][interleaved_index] = |
56 | numerators[channel][i] / denominators[channel][0]; |
57 | multi_channel_iir->denominator_[0][interleaved_index] = |
58 | denominators[channel][i] / |
59 | ((i > 0) ? -denominators[channel][0] : denominators[channel][0]); |
60 | interleaved_index += num_channels; |
61 | } |
62 | } |
63 | |
64 | multi_channel_iir->delay_line_.Clear(); |
65 | return multi_channel_iir; |
66 | } |
67 | |
68 | void MultiChannelIir::Process(AudioBuffer::Channel* interleaved_buffer) { |
69 | |
70 | DCHECK(interleaved_buffer); |
71 | DCHECK_EQ(interleaved_buffer->size(), num_channels_ * frames_per_buffer_); |
72 | const SimdVector* simd_numerator = |
73 | reinterpret_cast<SimdVector*>(&(numerator_[0][0])); |
74 | const SimdVector* simd_denominator = |
75 | reinterpret_cast<SimdVector*>(&(denominator_[0][0])); |
76 | SimdVector* simd_delay_line = |
77 | reinterpret_cast<SimdVector*>(&(delay_line_[0][0])); |
78 | SimdVector* simd_buffer = |
79 | reinterpret_cast<SimdVector*>(&((*interleaved_buffer)[0])); |
80 | |
81 | const size_t num_channel_chunks = num_channels_ / SIMD_LENGTH; |
82 | const size_t num_buffer_chunks = interleaved_buffer->size() / SIMD_LENGTH; |
83 | const size_t delay_length_in_chunks = num_channel_chunks * num_coefficients_; |
84 | |
85 | for (size_t current_frame = 0, individual_frame = 0; |
86 | current_frame < num_buffer_chunks; |
87 | current_frame += num_channel_chunks, individual_frame += num_channels_) { |
88 | DCHECK_LT(individual_frame, interleaved_buffer->size()); |
89 | // Copy the current sample into the delay line at the very start. |
90 | // {x[n], w[n-1], w[n-2], . . .} where each x, w represents all channels. |
91 | std::copy_n(first: interleaved_buffer->begin() + individual_frame, n: num_channels_, |
92 | result: delay_line_[0].begin() + (delay_line_front_ * SIMD_LENGTH)); |
93 | // Using A Direct Form II implementation difference equation: |
94 | // Source: Digital Signal Processing Principles Algorithms & Applications |
95 | // Fourth Edition. John G. Prolakis and Dimitris G. Manolakis - Chap 9 |
96 | // w[n] = x[n] - (a1/a0)*w[n-1] - (a2/a0)*w[n-2] - . . . |
97 | // y(n) = (b0/a0)*w[n] + (b1/a0)*w[n-1] + (b2/a0)*w[n-2] + . . . |
98 | // where x[n] is input, w[n] is storage and y[n] is output. |
99 | // The division by a0 has been performed in the constructor along with the |
100 | // negation of the denominator coefficients beyond the first. Note also |
101 | // that each term here refers to a set of channels. |
102 | for (size_t channel_chunk = 0; channel_chunk < num_channel_chunks; |
103 | ++channel_chunk) { |
104 | // First zero out the relevant section of the buffer before accumulation. |
105 | // Zero constant used for loading zeros into a neon simd array, as the |
106 | // |vld1q_dup_f32| neon intrinsic requires an lvalue parameter. |
107 | const float kZerof = 0.0f; |
108 | simd_buffer[current_frame + channel_chunk] = SIMD_LOAD_ONE_FLOAT(kZerof); |
109 | for (size_t coeff_offset = num_channel_chunks; |
110 | coeff_offset < delay_length_in_chunks; |
111 | coeff_offset += num_channel_chunks) { |
112 | // Denominator part. |
113 | const size_t multiplication_index = channel_chunk + coeff_offset; |
114 | const size_t delay_multiplication_index = |
115 | (multiplication_index + delay_line_front_) % delay_length_in_chunks; |
116 | const size_t delay_write_index = channel_chunk + delay_line_front_; |
117 | simd_delay_line[delay_write_index] = |
118 | SIMD_MULTIPLY_ADD(simd_denominator[multiplication_index], |
119 | simd_delay_line[delay_multiplication_index], |
120 | simd_delay_line[delay_write_index]); |
121 | } |
122 | for (size_t coeff_offset = 0; coeff_offset < delay_length_in_chunks; |
123 | coeff_offset += num_channel_chunks) { |
124 | // Numerator part. |
125 | const size_t multiplication_index = channel_chunk + coeff_offset; |
126 | const size_t write_index = current_frame + channel_chunk; |
127 | const size_t delay_multiplication_index = |
128 | (multiplication_index + delay_line_front_) % delay_length_in_chunks; |
129 | simd_buffer[write_index] = |
130 | SIMD_MULTIPLY_ADD(simd_numerator[multiplication_index], |
131 | simd_delay_line[delay_multiplication_index], |
132 | simd_buffer[write_index]); |
133 | } |
134 | } |
135 | // Update the index to the wrapped around 'front' of the delay line. |
136 | delay_line_front_ = ((static_cast<int>(delay_line_front_) - |
137 | num_channel_chunks + delay_length_in_chunks) % |
138 | delay_length_in_chunks); |
139 | } |
140 | } |
141 | |
142 | MultiChannelIir::MultiChannelIir(size_t num_channels, size_t frames_per_buffer, |
143 | size_t num_coefficients) |
144 | : num_channels_(num_channels), |
145 | frames_per_buffer_(frames_per_buffer), |
146 | num_coefficients_(num_coefficients), |
147 | delay_line_front_(0), |
148 | numerator_(kNumMonoChannels, num_coefficients_ * num_channels_), |
149 | denominator_(kNumMonoChannels, num_coefficients_ * num_channels_), |
150 | delay_line_(kNumMonoChannels, num_coefficients_ * num_channels_) {} |
151 | |
152 | } // namespace vraudio |
153 | |