3 * Copyright (c) 2008 Konstantin Shishkov
5 * This file is part of FFmpeg.
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.
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.
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
24 * different IIR filters implementation
27 #include "iirfilter.h"
29 #include "libavutil/attributes.h"
30 #include "libavutil/common.h"
33 * IIR filter global parameters
35 typedef struct FFIIRFilterCoeffs
{
45 typedef struct FFIIRFilterState
{
49 /// maximum supported filter order
52 static av_cold
int butterworth_init_coeffs(void *avc
,
53 struct FFIIRFilterCoeffs
*c
,
54 enum IIRFilterMode filt_mode
,
55 int order
, float cutoff_ratio
,
60 double p
[MAXORDER
+ 1][2];
62 if (filt_mode
!= FF_FILTER_MODE_LOWPASS
) {
63 av_log(avc
, AV_LOG_ERROR
, "Butterworth filter currently only supports "
64 "low-pass filter mode\n");
68 av_log(avc
, AV_LOG_ERROR
, "Butterworth filter currently only supports "
69 "even filter orders\n");
73 wa
= 2 * tan(M_PI
* 0.5 * cutoff_ratio
);
76 for(i
= 1; i
< (order
>> 1) + 1; i
++)
77 c
->cx
[i
] = c
->cx
[i
- 1] * (order
- i
+ 1LL) / i
;
81 for(i
= 1; i
<= order
; i
++)
82 p
[i
][0] = p
[i
][1] = 0.0;
83 for(i
= 0; i
< order
; i
++){
85 double th
= (i
+ (order
>> 1) + 0.5) * M_PI
/ order
;
86 double a_re
, a_im
, c_re
, c_im
;
93 zp
[0] = (a_re
* c_re
+ a_im
* c_im
) / (c_re
* c_re
+ c_im
* c_im
);
94 zp
[1] = (a_im
* c_re
- a_re
* c_im
) / (c_re
* c_re
+ c_im
* c_im
);
96 for(j
= order
; j
>= 1; j
--)
100 p
[j
][0] = a_re
*zp
[0] - a_im
*zp
[1] + p
[j
-1][0];
101 p
[j
][1] = a_re
*zp
[1] + a_im
*zp
[0] + p
[j
-1][1];
103 a_re
= p
[0][0]*zp
[0] - p
[0][1]*zp
[1];
104 p
[0][1] = p
[0][0]*zp
[1] + p
[0][1]*zp
[0];
107 c
->gain
= p
[order
][0];
108 for(i
= 0; i
< order
; i
++){
110 c
->cy
[i
] = (-p
[i
][0] * p
[order
][0] + -p
[i
][1] * p
[order
][1]) /
111 (p
[order
][0] * p
[order
][0] + p
[order
][1] * p
[order
][1]);
113 c
->gain
/= 1 << order
;
118 static av_cold
int biquad_init_coeffs(void *avc
, struct FFIIRFilterCoeffs
*c
,
119 enum IIRFilterMode filt_mode
, int order
,
120 float cutoff_ratio
, float stopband
)
122 double cos_w0
, sin_w0
;
125 if (filt_mode
!= FF_FILTER_MODE_HIGHPASS
&&
126 filt_mode
!= FF_FILTER_MODE_LOWPASS
) {
127 av_log(avc
, AV_LOG_ERROR
, "Biquad filter currently only supports "
128 "high-pass and low-pass filter modes\n");
132 av_log(avc
, AV_LOG_ERROR
, "Biquad filter must have order of 2\n");
136 cos_w0
= cos(M_PI
* cutoff_ratio
);
137 sin_w0
= sin(M_PI
* cutoff_ratio
);
139 a0
= 1.0 + (sin_w0
/ 2.0);
141 if (filt_mode
== FF_FILTER_MODE_HIGHPASS
) {
142 c
->gain
= ((1.0 + cos_w0
) / 2.0) / a0
;
143 x0
= ((1.0 + cos_w0
) / 2.0) / a0
;
144 x1
= (-(1.0 + cos_w0
)) / a0
;
145 } else { // FF_FILTER_MODE_LOWPASS
146 c
->gain
= ((1.0 - cos_w0
) / 2.0) / a0
;
147 x0
= ((1.0 - cos_w0
) / 2.0) / a0
;
148 x1
= (1.0 - cos_w0
) / a0
;
150 c
->cy
[0] = (-1.0 + (sin_w0
/ 2.0)) / a0
;
151 c
->cy
[1] = (2.0 * cos_w0
) / a0
;
153 // divide by gain to make the x coeffs integers.
154 // during filtering, the delay state will include the gain multiplication
155 c
->cx
[0] = lrintf(x0
/ c
->gain
);
156 c
->cx
[1] = lrintf(x1
/ c
->gain
);
161 av_cold
struct FFIIRFilterCoeffs
* ff_iir_filter_init_coeffs(void *avc
,
162 enum IIRFilterType filt_type
,
163 enum IIRFilterMode filt_mode
,
164 int order
, float cutoff_ratio
,
165 float stopband
, float ripple
)
167 FFIIRFilterCoeffs
*c
;
170 if (order
<= 0 || order
> MAXORDER
|| cutoff_ratio
>= 1.0)
173 FF_ALLOCZ_OR_GOTO(avc
, c
, sizeof(FFIIRFilterCoeffs
),
175 FF_ALLOC_OR_GOTO (avc
, c
->cx
, sizeof(c
->cx
[0]) * ((order
>> 1) + 1),
177 FF_ALLOC_OR_GOTO (avc
, c
->cy
, sizeof(c
->cy
[0]) * order
,
182 case FF_FILTER_TYPE_BUTTERWORTH
:
183 ret
= butterworth_init_coeffs(avc
, c
, filt_mode
, order
, cutoff_ratio
,
186 case FF_FILTER_TYPE_BIQUAD
:
187 ret
= biquad_init_coeffs(avc
, c
, filt_mode
, order
, cutoff_ratio
,
191 av_log(avc
, AV_LOG_ERROR
, "filter type is not currently implemented\n");
199 ff_iir_filter_free_coeffsp(&c
);
203 av_cold
struct FFIIRFilterState
* ff_iir_filter_init_state(int order
)
205 FFIIRFilterState
* s
= av_mallocz(sizeof(FFIIRFilterState
) + sizeof(s
->x
[0]) * (order
- 1));
209 #define CONV_S16(dest, source) dest = av_clip_int16(lrintf(source));
211 #define CONV_FLT(dest, source) dest = source;
213 #define FILTER_BW_O4_1(i0, i1, i2, i3, fmt) \
214 in = *src0 * c->gain \
215 + c->cy[0]*s->x[i0] + c->cy[1]*s->x[i1] \
216 + c->cy[2]*s->x[i2] + c->cy[3]*s->x[i3]; \
217 res = (s->x[i0] + in )*1 \
218 + (s->x[i1] + s->x[i3])*4 \
220 CONV_##fmt(*dst0, res) \
225 #define FILTER_BW_O4(type, fmt) { \
227 const type *src0 = src; \
229 for (i = 0; i < size; i += 4) { \
231 FILTER_BW_O4_1(0, 1, 2, 3, fmt); \
232 FILTER_BW_O4_1(1, 2, 3, 0, fmt); \
233 FILTER_BW_O4_1(2, 3, 0, 1, fmt); \
234 FILTER_BW_O4_1(3, 0, 1, 2, fmt); \
238 #define FILTER_DIRECT_FORM_II(type, fmt) { \
240 const type *src0 = src; \
242 for (i = 0; i < size; i++) { \
245 in = *src0 * c->gain; \
246 for(j = 0; j < c->order; j++) \
247 in += c->cy[j] * s->x[j]; \
248 res = s->x[0] + in + s->x[c->order >> 1] * c->cx[c->order >> 1]; \
249 for(j = 1; j < c->order >> 1; j++) \
250 res += (s->x[j] + s->x[c->order - j]) * c->cx[j]; \
251 for(j = 0; j < c->order - 1; j++) \
252 s->x[j] = s->x[j + 1]; \
253 CONV_##fmt(*dst0, res) \
254 s->x[c->order - 1] = in; \
260 #define FILTER_O2(type, fmt) { \
262 const type *src0 = src; \
264 for (i = 0; i < size; i++) { \
265 float in = *src0 * c->gain + \
266 s->x[0] * c->cy[0] + \
267 s->x[1] * c->cy[1]; \
268 CONV_##fmt(*dst0, s->x[0] + in + s->x[1] * c->cx[1]) \
276 void ff_iir_filter(const struct FFIIRFilterCoeffs
*c
,
277 struct FFIIRFilterState
*s
, int size
,
278 const int16_t *src
, int sstep
, int16_t *dst
, int dstep
)
281 FILTER_O2(int16_t, S16
)
282 } else if (c
->order
== 4) {
283 FILTER_BW_O4(int16_t, S16
)
285 FILTER_DIRECT_FORM_II(int16_t, S16
)
289 void ff_iir_filter_flt(const struct FFIIRFilterCoeffs
*c
,
290 struct FFIIRFilterState
*s
, int size
,
291 const float *src
, int sstep
, float *dst
, int dstep
)
294 FILTER_O2(float, FLT
)
295 } else if (c
->order
== 4) {
296 FILTER_BW_O4(float, FLT
)
298 FILTER_DIRECT_FORM_II(float, FLT
)
302 av_cold
void ff_iir_filter_free_statep(struct FFIIRFilterState
**state
)
307 av_cold
void ff_iir_filter_free_coeffsp(struct FFIIRFilterCoeffs
**coeffsp
)
309 struct FFIIRFilterCoeffs
*coeffs
= *coeffsp
;
311 av_freep(&coeffs
->cx
);
312 av_freep(&coeffs
->cy
);
317 void ff_iir_filter_init(FFIIRFilterContext
*f
) {
318 f
->filter_flt
= ff_iir_filter_flt
;
321 ff_iir_filter_init_mips(f
);
331 struct FFIIRFilterCoeffs
*fcoeffs
= NULL
;
332 struct FFIIRFilterState
*fstate
= NULL
;
333 float cutoff_coeff
= 0.4;
334 int16_t x
[SIZE
], y
[SIZE
];
337 fcoeffs
= ff_iir_filter_init_coeffs(NULL
, FF_FILTER_TYPE_BUTTERWORTH
,
338 FF_FILTER_MODE_LOWPASS
, FILT_ORDER
,
339 cutoff_coeff
, 0.0, 0.0);
340 fstate
= ff_iir_filter_init_state(FILT_ORDER
);
342 for (i
= 0; i
< SIZE
; i
++) {
343 x
[i
] = lrint(0.75 * INT16_MAX
* sin(0.5*M_PI
*i
*i
/SIZE
));
346 ff_iir_filter(fcoeffs
, fstate
, SIZE
, x
, 1, y
, 1);
348 for (i
= 0; i
< SIZE
; i
++)
349 printf("%6d %6d\n", x
[i
], y
[i
]);
351 ff_iir_filter_free_coeffsp(&fcoeffs
);
352 ff_iir_filter_free_statep(&fstate
);