Add a shift parameter to celp_lp_synthesis_filter for reuse by the G.723.1 decoder
[ffmpeg.git] / libavcodec / celp_filters.c
1 /*
2  * various filters for ACELP-based codecs
3  *
4  * Copyright (c) 2008 Vladimir Voroshilov
5  *
6  * This file is part of FFmpeg.
7  *
8  * FFmpeg is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU Lesser General Public
10  * License as published by the Free Software Foundation; either
11  * version 2.1 of the License, or (at your option) any later version.
12  *
13  * FFmpeg is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
16  * Lesser General Public License for more details.
17  *
18  * You should have received a copy of the GNU Lesser General Public
19  * License along with FFmpeg; if not, write to the Free Software
20  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
21  */
22
23 #include <inttypes.h>
24
25 #include "avcodec.h"
26 #include "celp_filters.h"
27
28 void ff_celp_convolve_circ(int16_t* fc_out, const int16_t* fc_in,
29                            const int16_t* filter, int len)
30 {
31     int i, k;
32
33     memset(fc_out, 0, len * sizeof(int16_t));
34
35     /* Since there are few pulses over an entire subframe (i.e. almost
36        all fc_in[i] are zero) it is faster to loop over fc_in first. */
37     for (i = 0; i < len; i++) {
38         if (fc_in[i]) {
39             for (k = 0; k < i; k++)
40                 fc_out[k] += (fc_in[i] * filter[len + k - i]) >> 15;
41
42             for (k = i; k < len; k++)
43                 fc_out[k] += (fc_in[i] * filter[      k - i]) >> 15;
44         }
45     }
46 }
47
48 void ff_celp_circ_addf(float *out, const float *in,
49                        const float *lagged, int lag, float fac, int n)
50 {
51     int k;
52     for (k = 0; k < lag; k++)
53         out[k] = in[k] + fac * lagged[n + k - lag];
54     for (; k < n; k++)
55         out[k] = in[k] + fac * lagged[    k - lag];
56 }
57
58 int ff_celp_lp_synthesis_filter(int16_t *out, const int16_t *filter_coeffs,
59                                 const int16_t *in, int buffer_length,
60                                 int filter_length, int stop_on_overflow,
61                                 int shift, int rounder)
62 {
63     int i,n;
64
65     for (n = 0; n < buffer_length; n++) {
66         int sum = rounder;
67         for (i = 1; i <= filter_length; i++)
68             sum -= filter_coeffs[i-1] * out[n-i];
69
70         sum = ((sum >> 12) + in[n]) >> shift;
71
72         if (sum + 0x8000 > 0xFFFFU) {
73             if (stop_on_overflow)
74                 return 1;
75             sum = (sum >> 31) ^ 32767;
76         }
77         out[n] = sum;
78     }
79
80     return 0;
81 }
82
83 void ff_celp_lp_synthesis_filterf(float *out, const float *filter_coeffs,
84                                   const float* in, int buffer_length,
85                                   int filter_length)
86 {
87     int i,n;
88
89 #if 0 // Unoptimized code path for improved readability
90     for (n = 0; n < buffer_length; n++) {
91         out[n] = in[n];
92         for (i = 1; i <= filter_length; i++)
93             out[n] -= filter_coeffs[i-1] * out[n-i];
94     }
95 #else
96     float out0, out1, out2, out3;
97     float old_out0, old_out1, old_out2, old_out3;
98     float a,b,c;
99
100     a = filter_coeffs[0];
101     b = filter_coeffs[1];
102     c = filter_coeffs[2];
103     b -= filter_coeffs[0] * filter_coeffs[0];
104     c -= filter_coeffs[1] * filter_coeffs[0];
105     c -= filter_coeffs[0] * b;
106
107     old_out0 = out[-4];
108     old_out1 = out[-3];
109     old_out2 = out[-2];
110     old_out3 = out[-1];
111     for (n = 0; n <= buffer_length - 4; n+=4) {
112         float tmp0,tmp1,tmp2;
113         float val;
114
115         out0 = in[0];
116         out1 = in[1];
117         out2 = in[2];
118         out3 = in[3];
119
120         out0 -= filter_coeffs[2] * old_out1;
121         out1 -= filter_coeffs[2] * old_out2;
122         out2 -= filter_coeffs[2] * old_out3;
123
124         out0 -= filter_coeffs[1] * old_out2;
125         out1 -= filter_coeffs[1] * old_out3;
126
127         out0 -= filter_coeffs[0] * old_out3;
128
129         val = filter_coeffs[3];
130
131         out0 -= val * old_out0;
132         out1 -= val * old_out1;
133         out2 -= val * old_out2;
134         out3 -= val * old_out3;
135
136         old_out3 = out[-5];
137
138         for (i = 5; i <= filter_length; i += 2) {
139             val = filter_coeffs[i-1];
140
141             out0 -= val * old_out3;
142             out1 -= val * old_out0;
143             out2 -= val * old_out1;
144             out3 -= val * old_out2;
145
146             old_out2 = out[-i-1];
147
148             val = filter_coeffs[i];
149
150             out0 -= val * old_out2;
151             out1 -= val * old_out3;
152             out2 -= val * old_out0;
153             out3 -= val * old_out1;
154
155             FFSWAP(float, old_out0, old_out2);
156             old_out1 = old_out3;
157             old_out3 = out[-i-2];
158         }
159
160         tmp0 = out0;
161         tmp1 = out1;
162         tmp2 = out2;
163
164         out3 -= a * tmp2;
165         out2 -= a * tmp1;
166         out1 -= a * tmp0;
167
168         out3 -= b * tmp1;
169         out2 -= b * tmp0;
170
171         out3 -= c * tmp0;
172
173
174         out[0] = out0;
175         out[1] = out1;
176         out[2] = out2;
177         out[3] = out3;
178
179         old_out0 = out0;
180         old_out1 = out1;
181         old_out2 = out2;
182         old_out3 = out3;
183
184         out += 4;
185         in  += 4;
186     }
187
188     out -= n;
189     in -= n;
190     for (; n < buffer_length; n++) {
191         out[n] = in[n];
192         for (i = 1; i <= filter_length; i++)
193             out[n] -= filter_coeffs[i-1] * out[n-i];
194     }
195 #endif
196 }
197
198 void ff_celp_lp_zero_synthesis_filterf(float *out, const float *filter_coeffs,
199                                        const float *in, int buffer_length,
200                                        int filter_length)
201 {
202     int i,n;
203
204     for (n = 0; n < buffer_length; n++) {
205         out[n] = in[n];
206         for (i = 1; i <= filter_length; i++)
207             out[n] += filter_coeffs[i-1] * in[n-i];
208     }
209 }