blob: b202515827868afd8be75f47b03f4a967c2821a4
1 | /* |
2 | * IIR filter |
3 | * Copyright (c) 2008 Konstantin Shishkov |
4 | * |
5 | * This file is part of FFmpeg. |
6 | * |
7 | * FFmpeg is free software; you can redistribute it and/or |
8 | * modify it under the terms of the GNU Lesser General Public |
9 | * License as published by the Free Software Foundation; either |
10 | * version 2.1 of the License, or (at your option) any later version. |
11 | * |
12 | * FFmpeg is distributed in the hope that it will be useful, |
13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
15 | * Lesser General Public License for more details. |
16 | * |
17 | * You should have received a copy of the GNU Lesser General Public |
18 | * License along with FFmpeg; if not, write to the Free Software |
19 | * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA |
20 | */ |
21 | |
22 | /** |
23 | * @file |
24 | * different IIR filters implementation |
25 | */ |
26 | |
27 | #include <math.h> |
28 | |
29 | #include "libavutil/attributes.h" |
30 | #include "libavutil/common.h" |
31 | |
32 | #include "iirfilter.h" |
33 | |
34 | /** |
35 | * IIR filter global parameters |
36 | */ |
37 | typedef struct FFIIRFilterCoeffs { |
38 | int order; |
39 | float gain; |
40 | int *cx; |
41 | float *cy; |
42 | } FFIIRFilterCoeffs; |
43 | |
44 | /** |
45 | * IIR filter state |
46 | */ |
47 | typedef struct FFIIRFilterState { |
48 | float x[1]; |
49 | } FFIIRFilterState; |
50 | |
51 | /// maximum supported filter order |
52 | #define MAXORDER 30 |
53 | |
54 | static av_cold int butterworth_init_coeffs(void *avc, |
55 | struct FFIIRFilterCoeffs *c, |
56 | enum IIRFilterMode filt_mode, |
57 | int order, float cutoff_ratio, |
58 | float stopband) |
59 | { |
60 | int i, j; |
61 | double wa; |
62 | double p[MAXORDER + 1][2]; |
63 | |
64 | if (filt_mode != FF_FILTER_MODE_LOWPASS) { |
65 | av_log(avc, AV_LOG_ERROR, "Butterworth filter currently only supports " |
66 | "low-pass filter mode\n"); |
67 | return -1; |
68 | } |
69 | if (order & 1) { |
70 | av_log(avc, AV_LOG_ERROR, "Butterworth filter currently only supports " |
71 | "even filter orders\n"); |
72 | return -1; |
73 | } |
74 | |
75 | wa = 2 * tan(M_PI * 0.5 * cutoff_ratio); |
76 | |
77 | c->cx[0] = 1; |
78 | for (i = 1; i < (order >> 1) + 1; i++) |
79 | c->cx[i] = c->cx[i - 1] * (order - i + 1LL) / i; |
80 | |
81 | p[0][0] = 1.0; |
82 | p[0][1] = 0.0; |
83 | for (i = 1; i <= order; i++) |
84 | p[i][0] = p[i][1] = 0.0; |
85 | for (i = 0; i < order; i++) { |
86 | double zp[2]; |
87 | double th = (i + (order >> 1) + 0.5) * M_PI / order; |
88 | double a_re, a_im, c_re, c_im; |
89 | zp[0] = cos(th) * wa; |
90 | zp[1] = sin(th) * wa; |
91 | a_re = zp[0] + 2.0; |
92 | c_re = zp[0] - 2.0; |
93 | a_im = |
94 | c_im = zp[1]; |
95 | zp[0] = (a_re * c_re + a_im * c_im) / (c_re * c_re + c_im * c_im); |
96 | zp[1] = (a_im * c_re - a_re * c_im) / (c_re * c_re + c_im * c_im); |
97 | |
98 | for (j = order; j >= 1; j--) { |
99 | a_re = p[j][0]; |
100 | a_im = p[j][1]; |
101 | p[j][0] = a_re * zp[0] - a_im * zp[1] + p[j - 1][0]; |
102 | p[j][1] = a_re * zp[1] + a_im * zp[0] + p[j - 1][1]; |
103 | } |
104 | a_re = p[0][0] * zp[0] - p[0][1] * zp[1]; |
105 | p[0][1] = p[0][0] * zp[1] + p[0][1] * zp[0]; |
106 | p[0][0] = a_re; |
107 | } |
108 | c->gain = p[order][0]; |
109 | for (i = 0; i < order; i++) { |
110 | c->gain += p[i][0]; |
111 | c->cy[i] = (-p[i][0] * p[order][0] + -p[i][1] * p[order][1]) / |
112 | (p[order][0] * p[order][0] + p[order][1] * p[order][1]); |
113 | } |
114 | c->gain /= 1 << order; |
115 | |
116 | return 0; |
117 | } |
118 | |
119 | static av_cold int biquad_init_coeffs(void *avc, struct FFIIRFilterCoeffs *c, |
120 | enum IIRFilterMode filt_mode, int order, |
121 | float cutoff_ratio, float stopband) |
122 | { |
123 | double cos_w0, sin_w0; |
124 | double a0, x0, x1; |
125 | |
126 | if (filt_mode != FF_FILTER_MODE_HIGHPASS && |
127 | filt_mode != FF_FILTER_MODE_LOWPASS) { |
128 | av_log(avc, AV_LOG_ERROR, "Biquad filter currently only supports " |
129 | "high-pass and low-pass filter modes\n"); |
130 | return -1; |
131 | } |
132 | if (order != 2) { |
133 | av_log(avc, AV_LOG_ERROR, "Biquad filter must have order of 2\n"); |
134 | return -1; |
135 | } |
136 | |
137 | cos_w0 = cos(M_PI * cutoff_ratio); |
138 | sin_w0 = sin(M_PI * cutoff_ratio); |
139 | |
140 | a0 = 1.0 + (sin_w0 / 2.0); |
141 | |
142 | if (filt_mode == FF_FILTER_MODE_HIGHPASS) { |
143 | c->gain = ((1.0 + cos_w0) / 2.0) / a0; |
144 | x0 = ((1.0 + cos_w0) / 2.0) / a0; |
145 | x1 = (-(1.0 + cos_w0)) / a0; |
146 | } else { // FF_FILTER_MODE_LOWPASS |
147 | c->gain = ((1.0 - cos_w0) / 2.0) / a0; |
148 | x0 = ((1.0 - cos_w0) / 2.0) / a0; |
149 | x1 = (1.0 - cos_w0) / a0; |
150 | } |
151 | c->cy[0] = (-1.0 + (sin_w0 / 2.0)) / a0; |
152 | c->cy[1] = (2.0 * cos_w0) / a0; |
153 | |
154 | // divide by gain to make the x coeffs integers. |
155 | // during filtering, the delay state will include the gain multiplication |
156 | c->cx[0] = lrintf(x0 / c->gain); |
157 | c->cx[1] = lrintf(x1 / c->gain); |
158 | |
159 | return 0; |
160 | } |
161 | |
162 | av_cold struct FFIIRFilterCoeffs *ff_iir_filter_init_coeffs(void *avc, |
163 | enum IIRFilterType filt_type, |
164 | enum IIRFilterMode filt_mode, |
165 | int order, float cutoff_ratio, |
166 | float stopband, float ripple) |
167 | { |
168 | FFIIRFilterCoeffs *c; |
169 | int ret = 0; |
170 | |
171 | if (order <= 0 || order > MAXORDER || cutoff_ratio >= 1.0) |
172 | return NULL; |
173 | |
174 | FF_ALLOCZ_OR_GOTO(avc, c, sizeof(FFIIRFilterCoeffs), |
175 | init_fail); |
176 | FF_ALLOC_OR_GOTO(avc, c->cx, sizeof(c->cx[0]) * ((order >> 1) + 1), |
177 | init_fail); |
178 | FF_ALLOC_OR_GOTO(avc, c->cy, sizeof(c->cy[0]) * order, |
179 | init_fail); |
180 | c->order = order; |
181 | |
182 | switch (filt_type) { |
183 | case FF_FILTER_TYPE_BUTTERWORTH: |
184 | ret = butterworth_init_coeffs(avc, c, filt_mode, order, cutoff_ratio, |
185 | stopband); |
186 | break; |
187 | case FF_FILTER_TYPE_BIQUAD: |
188 | ret = biquad_init_coeffs(avc, c, filt_mode, order, cutoff_ratio, |
189 | stopband); |
190 | break; |
191 | default: |
192 | av_log(avc, AV_LOG_ERROR, "filter type is not currently implemented\n"); |
193 | goto init_fail; |
194 | } |
195 | |
196 | if (!ret) |
197 | return c; |
198 | |
199 | init_fail: |
200 | ff_iir_filter_free_coeffsp(&c); |
201 | return NULL; |
202 | } |
203 | |
204 | av_cold struct FFIIRFilterState *ff_iir_filter_init_state(int order) |
205 | { |
206 | FFIIRFilterState *s = av_mallocz(sizeof(FFIIRFilterState) + sizeof(s->x[0]) * (order - 1)); |
207 | return s; |
208 | } |
209 | |
210 | #define CONV_S16(dest, source) dest = av_clip_int16(lrintf(source)); |
211 | |
212 | #define CONV_FLT(dest, source) dest = source; |
213 | |
214 | #define FILTER_BW_O4_1(i0, i1, i2, i3, fmt) \ |
215 | in = *src0 * c->gain + \ |
216 | c->cy[0] * s->x[i0] + \ |
217 | c->cy[1] * s->x[i1] + \ |
218 | c->cy[2] * s->x[i2] + \ |
219 | c->cy[3] * s->x[i3]; \ |
220 | res = (s->x[i0] + in) * 1 + \ |
221 | (s->x[i1] + s->x[i3]) * 4 + \ |
222 | s->x[i2] * 6; \ |
223 | CONV_ ## fmt(*dst0, res) \ |
224 | s->x[i0] = in; \ |
225 | src0 += sstep; \ |
226 | dst0 += dstep; |
227 | |
228 | #define FILTER_BW_O4(type, fmt) { \ |
229 | int i; \ |
230 | const type *src0 = src; \ |
231 | type *dst0 = dst; \ |
232 | for (i = 0; i < size; i += 4) { \ |
233 | float in, res; \ |
234 | FILTER_BW_O4_1(0, 1, 2, 3, fmt); \ |
235 | FILTER_BW_O4_1(1, 2, 3, 0, fmt); \ |
236 | FILTER_BW_O4_1(2, 3, 0, 1, fmt); \ |
237 | FILTER_BW_O4_1(3, 0, 1, 2, fmt); \ |
238 | } \ |
239 | } |
240 | |
241 | #define FILTER_DIRECT_FORM_II(type, fmt) { \ |
242 | int i; \ |
243 | const type *src0 = src; \ |
244 | type *dst0 = dst; \ |
245 | for (i = 0; i < size; i++) { \ |
246 | int j; \ |
247 | float in, res; \ |
248 | in = *src0 * c->gain; \ |
249 | for (j = 0; j < c->order; j++) \ |
250 | in += c->cy[j] * s->x[j]; \ |
251 | res = s->x[0] + in + s->x[c->order >> 1] * c->cx[c->order >> 1]; \ |
252 | for (j = 1; j < c->order >> 1; j++) \ |
253 | res += (s->x[j] + s->x[c->order - j]) * c->cx[j]; \ |
254 | for (j = 0; j < c->order - 1; j++) \ |
255 | s->x[j] = s->x[j + 1]; \ |
256 | CONV_ ## fmt(*dst0, res) \ |
257 | s->x[c->order - 1] = in; \ |
258 | src0 += sstep; \ |
259 | dst0 += dstep; \ |
260 | } \ |
261 | } |
262 | |
263 | #define FILTER_O2(type, fmt) { \ |
264 | int i; \ |
265 | const type *src0 = src; \ |
266 | type *dst0 = dst; \ |
267 | for (i = 0; i < size; i++) { \ |
268 | float in = *src0 * c->gain + \ |
269 | s->x[0] * c->cy[0] + \ |
270 | s->x[1] * c->cy[1]; \ |
271 | CONV_ ## fmt(*dst0, s->x[0] + in + s->x[1] * c->cx[1]) \ |
272 | s->x[0] = s->x[1]; \ |
273 | s->x[1] = in; \ |
274 | src0 += sstep; \ |
275 | dst0 += dstep; \ |
276 | } \ |
277 | } |
278 | |
279 | void ff_iir_filter(const struct FFIIRFilterCoeffs *c, |
280 | struct FFIIRFilterState *s, int size, |
281 | const int16_t *src, ptrdiff_t sstep, |
282 | int16_t *dst, ptrdiff_t dstep) |
283 | { |
284 | if (c->order == 2) { |
285 | FILTER_O2(int16_t, S16) |
286 | } else if (c->order == 4) { |
287 | FILTER_BW_O4(int16_t, S16) |
288 | } else { |
289 | FILTER_DIRECT_FORM_II(int16_t, S16) |
290 | } |
291 | } |
292 | |
293 | void ff_iir_filter_flt(const struct FFIIRFilterCoeffs *c, |
294 | struct FFIIRFilterState *s, int size, |
295 | const float *src, ptrdiff_t sstep, |
296 | float *dst, ptrdiff_t dstep) |
297 | { |
298 | if (c->order == 2) { |
299 | FILTER_O2(float, FLT) |
300 | } else if (c->order == 4) { |
301 | FILTER_BW_O4(float, FLT) |
302 | } else { |
303 | FILTER_DIRECT_FORM_II(float, FLT) |
304 | } |
305 | } |
306 | |
307 | av_cold void ff_iir_filter_free_statep(struct FFIIRFilterState **state) |
308 | { |
309 | av_freep(state); |
310 | } |
311 | |
312 | av_cold void ff_iir_filter_free_coeffsp(struct FFIIRFilterCoeffs **coeffsp) |
313 | { |
314 | struct FFIIRFilterCoeffs *coeffs = *coeffsp; |
315 | if (coeffs) { |
316 | av_freep(&coeffs->cx); |
317 | av_freep(&coeffs->cy); |
318 | } |
319 | av_freep(coeffsp); |
320 | } |
321 | |
322 | void ff_iir_filter_init(FFIIRFilterContext *f) { |
323 | f->filter_flt = ff_iir_filter_flt; |
324 | |
325 | if (HAVE_MIPSFPU) |
326 | ff_iir_filter_init_mips(f); |
327 | } |
328 |