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 |
|
1 |
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 |
✗✓ |
1 |
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 |
✗✓ |
1 |
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 |
|
1 |
wa = 2 * tan(M_PI * 0.5 * cutoff_ratio); |
76 |
|
|
|
77 |
|
1 |
c->cx[0] = 1; |
78 |
✓✓ |
3 |
for (i = 1; i < (order >> 1) + 1; i++) |
79 |
|
2 |
c->cx[i] = c->cx[i - 1] * (order - i + 1LL) / i; |
80 |
|
|
|
81 |
|
1 |
p[0][0] = 1.0; |
82 |
|
1 |
p[0][1] = 0.0; |
83 |
✓✓ |
5 |
for (i = 1; i <= order; i++) |
84 |
|
4 |
p[i][0] = p[i][1] = 0.0; |
85 |
✓✓ |
5 |
for (i = 0; i < order; i++) { |
86 |
|
|
double zp[2]; |
87 |
|
4 |
double th = (i + (order >> 1) + 0.5) * M_PI / order; |
88 |
|
|
double a_re, a_im, c_re, c_im; |
89 |
|
4 |
zp[0] = cos(th) * wa; |
90 |
|
4 |
zp[1] = sin(th) * wa; |
91 |
|
4 |
a_re = zp[0] + 2.0; |
92 |
|
4 |
c_re = zp[0] - 2.0; |
93 |
|
4 |
a_im = |
94 |
|
4 |
c_im = zp[1]; |
95 |
|
4 |
zp[0] = (a_re * c_re + a_im * c_im) / (c_re * c_re + c_im * c_im); |
96 |
|
4 |
zp[1] = (a_im * c_re - a_re * c_im) / (c_re * c_re + c_im * c_im); |
97 |
|
|
|
98 |
✓✓ |
20 |
for (j = order; j >= 1; j--) { |
99 |
|
16 |
a_re = p[j][0]; |
100 |
|
16 |
a_im = p[j][1]; |
101 |
|
16 |
p[j][0] = a_re * zp[0] - a_im * zp[1] + p[j - 1][0]; |
102 |
|
16 |
p[j][1] = a_re * zp[1] + a_im * zp[0] + p[j - 1][1]; |
103 |
|
|
} |
104 |
|
4 |
a_re = p[0][0] * zp[0] - p[0][1] * zp[1]; |
105 |
|
4 |
p[0][1] = p[0][0] * zp[1] + p[0][1] * zp[0]; |
106 |
|
4 |
p[0][0] = a_re; |
107 |
|
|
} |
108 |
|
1 |
c->gain = p[order][0]; |
109 |
✓✓ |
5 |
for (i = 0; i < order; i++) { |
110 |
|
4 |
c->gain += p[i][0]; |
111 |
|
4 |
c->cy[i] = (-p[i][0] * p[order][0] + -p[i][1] * p[order][1]) / |
112 |
|
4 |
(p[order][0] * p[order][0] + p[order][1] * p[order][1]); |
113 |
|
|
} |
114 |
|
1 |
c->gain /= 1 << order; |
115 |
|
|
|
116 |
|
1 |
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 |
|
1 |
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 |
|
1 |
int ret = 0; |
170 |
|
|
|
171 |
✓✗✓✗
✗✓ |
1 |
if (order <= 0 || order > MAXORDER || cutoff_ratio >= 1.0) |
172 |
|
|
return NULL; |
173 |
|
|
|
174 |
✗✓ |
1 |
FF_ALLOCZ_OR_GOTO(avc, c, sizeof(FFIIRFilterCoeffs), |
175 |
|
|
init_fail); |
176 |
✗✓✗✗
|
1 |
FF_ALLOC_OR_GOTO(avc, c->cx, sizeof(c->cx[0]) * ((order >> 1) + 1), |
177 |
|
|
init_fail); |
178 |
✗✓✗✗
|
1 |
FF_ALLOC_OR_GOTO(avc, c->cy, sizeof(c->cy[0]) * order, |
179 |
|
|
init_fail); |
180 |
|
1 |
c->order = order; |
181 |
|
|
|
182 |
✓✗✗ |
1 |
switch (filt_type) { |
183 |
|
1 |
case FF_FILTER_TYPE_BUTTERWORTH: |
184 |
|
1 |
ret = butterworth_init_coeffs(avc, c, filt_mode, order, cutoff_ratio, |
185 |
|
|
stopband); |
186 |
|
1 |
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 |
✓✗ |
1 |
if (!ret) |
197 |
|
1 |
return c; |
198 |
|
|
|
199 |
|
|
init_fail: |
200 |
|
|
ff_iir_filter_free_coeffsp(&c); |
201 |
|
|
return NULL; |
202 |
|
|
} |
203 |
|
|
|
204 |
|
1 |
av_cold struct FFIIRFilterState *ff_iir_filter_init_state(int order) |
205 |
|
|
{ |
206 |
|
1 |
FFIIRFilterState *s = av_mallocz(sizeof(FFIIRFilterState) + sizeof(s->x[0]) * (order - 1)); |
207 |
|
1 |
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 |
|
1 |
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 |
✗✓ |
1 |
if (c->order == 2) { |
285 |
|
|
FILTER_O2(int16_t, S16) |
286 |
✓✗ |
1 |
} else if (c->order == 4) { |
287 |
✓✓ |
257 |
FILTER_BW_O4(int16_t, S16) |
288 |
|
|
} else { |
289 |
|
|
FILTER_DIRECT_FORM_II(int16_t, S16) |
290 |
|
|
} |
291 |
|
1 |
} |
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 |
|
1 |
av_cold void ff_iir_filter_free_statep(struct FFIIRFilterState **state) |
308 |
|
|
{ |
309 |
|
1 |
av_freep(state); |
310 |
|
1 |
} |
311 |
|
|
|
312 |
|
12 |
av_cold void ff_iir_filter_free_coeffsp(struct FFIIRFilterCoeffs **coeffsp) |
313 |
|
|
{ |
314 |
|
12 |
struct FFIIRFilterCoeffs *coeffs = *coeffsp; |
315 |
✓✓ |
12 |
if (coeffs) { |
316 |
|
1 |
av_freep(&coeffs->cx); |
317 |
|
1 |
av_freep(&coeffs->cy); |
318 |
|
|
} |
319 |
|
12 |
av_freep(coeffsp); |
320 |
|
12 |
} |
321 |
|
|
|
322 |
|
11 |
void ff_iir_filter_init(FFIIRFilterContext *f) { |
323 |
|
11 |
f->filter_flt = ff_iir_filter_flt; |
324 |
|
|
|
325 |
|
|
if (HAVE_MIPSFPU) |
326 |
|
|
ff_iir_filter_init_mips(f); |
327 |
|
11 |
} |