diff options
Diffstat (limited to 'apps/codecs/demac/libdemac/filter.c')
-rw-r--r-- | apps/codecs/demac/libdemac/filter.c | 215 |
1 files changed, 215 insertions, 0 deletions
diff --git a/apps/codecs/demac/libdemac/filter.c b/apps/codecs/demac/libdemac/filter.c new file mode 100644 index 0000000000..76faa67aa7 --- /dev/null +++ b/apps/codecs/demac/libdemac/filter.c | |||
@@ -0,0 +1,215 @@ | |||
1 | /* | ||
2 | |||
3 | libdemac - A Monkey's Audio decoder | ||
4 | |||
5 | $Id:$ | ||
6 | |||
7 | Copyright (C) Dave Chapman 2007 | ||
8 | |||
9 | This program is free software; you can redistribute it and/or modify | ||
10 | it under the terms of the GNU General Public License as published by | ||
11 | the Free Software Foundation; either version 2 of the License, or | ||
12 | (at your option) any later version. | ||
13 | |||
14 | This program is distributed in the hope that it will be useful, | ||
15 | but WITHOUT ANY WARRANTY; without even the implied warranty of | ||
16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | ||
17 | GNU General Public License for more details. | ||
18 | |||
19 | You should have received a copy of the GNU General Public License | ||
20 | along with this program; if not, write to the Free Software | ||
21 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110, USA | ||
22 | |||
23 | */ | ||
24 | |||
25 | #include <string.h> | ||
26 | #include <inttypes.h> | ||
27 | |||
28 | #include "demac.h" | ||
29 | #include "filter.h" | ||
30 | |||
31 | #include "vector_math16.h" | ||
32 | |||
33 | struct filter_t { | ||
34 | int16_t* coeffs; /* ORDER entries */ | ||
35 | |||
36 | /* We store all the filter delays in a single buffer */ | ||
37 | int16_t* historybuffer; /* ORDER*2 + HISTORY_SIZE entries */ | ||
38 | |||
39 | int16_t* delay; | ||
40 | int16_t* adaptcoeffs; | ||
41 | |||
42 | int avg; | ||
43 | }; | ||
44 | |||
45 | /* We name the functions according to the ORDER and FRACBITS | ||
46 | pre-processor symbols and build multiple .o files from this .c file | ||
47 | - this increases code-size but gives the compiler more scope for | ||
48 | optimising the individual functions, as well as replacing a lot of | ||
49 | variables with constants. | ||
50 | */ | ||
51 | |||
52 | #if FRACBITS == 11 | ||
53 | #if ORDER == 16 | ||
54 | #define INIT_FILTER init_filter_16_11 | ||
55 | #define APPLY_FILTER apply_filter_16_11 | ||
56 | #elif ORDER == 64 | ||
57 | #define INIT_FILTER init_filter_64_11 | ||
58 | #define APPLY_FILTER apply_filter_64_11 | ||
59 | #endif | ||
60 | #elif FRACBITS == 13 | ||
61 | #define INIT_FILTER init_filter_256_13 | ||
62 | #define APPLY_FILTER apply_filter_256_13 | ||
63 | #elif FRACBITS == 10 | ||
64 | #define INIT_FILTER init_filter_32_10 | ||
65 | #define APPLY_FILTER apply_filter_32_10 | ||
66 | #elif FRACBITS == 15 | ||
67 | #define INIT_FILTER init_filter_1280_15 | ||
68 | #define APPLY_FILTER apply_filter_1280_15 | ||
69 | #endif | ||
70 | |||
71 | /* Some macros to handle the fixed-point stuff */ | ||
72 | |||
73 | #define FP_HALF (1 << (FRACBITS - 1)) /* 0.5 in fixed-point format. */ | ||
74 | #define FP_TO_INT(x) ((x + FP_HALF) >> FRACBITS); /* round(x) */ | ||
75 | |||
76 | #define SATURATE(x) (int16_t)(((x) == (int16_t)(x)) ? (x) : ((x) >> 31) ^ 0x7FFF); | ||
77 | |||
78 | /* Apply the filter with state f to count entries in data[] */ | ||
79 | |||
80 | static inline void do_apply_filter_3980(struct filter_t* f, int32_t* data, int count) | ||
81 | { | ||
82 | int res; | ||
83 | int absres; | ||
84 | |||
85 | while(count--) | ||
86 | { | ||
87 | res = FP_TO_INT(scalarproduct(f->delay - ORDER, f->coeffs)); | ||
88 | |||
89 | if (*data < 0) | ||
90 | vector_add(f->coeffs, f->adaptcoeffs - ORDER); | ||
91 | else if (*data > 0) | ||
92 | vector_sub(f->coeffs, f->adaptcoeffs - ORDER); | ||
93 | |||
94 | /* Convert res from (32-FRACBITS).FRACBITS fixed-point format to an | ||
95 | integer (rounding to nearest) and add the input value to | ||
96 | it */ | ||
97 | res += *data; | ||
98 | |||
99 | *data++ = res; | ||
100 | |||
101 | /* Update the output history */ | ||
102 | *f->delay++ = SATURATE(res); | ||
103 | |||
104 | /* Version 3.98 and later files */ | ||
105 | |||
106 | /* Update the adaption coefficients */ | ||
107 | absres = (res < 0 ? -res : res); | ||
108 | |||
109 | if (absres > (f->avg * 3)) | ||
110 | *f->adaptcoeffs = ((res >> 25) & 64) - 32; | ||
111 | else if (absres > (f->avg * 4) / 3) | ||
112 | *f->adaptcoeffs = ((res >> 26) & 32) - 16; | ||
113 | else if (absres > 0) | ||
114 | *f->adaptcoeffs = ((res >> 27) & 16) - 8; | ||
115 | else | ||
116 | *f->adaptcoeffs = 0; | ||
117 | |||
118 | f->avg += (absres - f->avg) / 16; | ||
119 | |||
120 | f->adaptcoeffs[-1] >>= 1; | ||
121 | f->adaptcoeffs[-2] >>= 1; | ||
122 | f->adaptcoeffs[-8] >>= 1; | ||
123 | |||
124 | f->adaptcoeffs++; | ||
125 | |||
126 | /* Have we filled the history buffer? */ | ||
127 | if (f->delay == f->historybuffer + HISTORY_SIZE + (ORDER*2)) { | ||
128 | memmove(f->historybuffer, f->delay - (ORDER*2), | ||
129 | (ORDER*2) * sizeof(int16_t)); | ||
130 | f->delay = f->historybuffer + ORDER*2; | ||
131 | f->adaptcoeffs = f->historybuffer + ORDER; | ||
132 | } | ||
133 | } | ||
134 | } | ||
135 | |||
136 | static inline void do_apply_filter_3970(struct filter_t* f, int32_t* data, int count) | ||
137 | { | ||
138 | int res; | ||
139 | |||
140 | while(count--) | ||
141 | { | ||
142 | res = FP_TO_INT(scalarproduct(f->delay - ORDER, f->coeffs)); | ||
143 | |||
144 | if (*data < 0) | ||
145 | vector_add(f->coeffs, f->adaptcoeffs - ORDER); | ||
146 | else if (*data > 0) | ||
147 | vector_sub(f->coeffs, f->adaptcoeffs - ORDER); | ||
148 | |||
149 | /* Convert res from (32-FRACBITS).FRACBITS fixed-point format to an | ||
150 | integer (rounding to nearest) and add the input value to | ||
151 | it */ | ||
152 | res += *data; | ||
153 | |||
154 | *data++ = res; | ||
155 | |||
156 | /* Update the output history */ | ||
157 | *f->delay++ = SATURATE(res); | ||
158 | |||
159 | /* Version ??? to < 3.98 files (untested) */ | ||
160 | f->adaptcoeffs[0] = (res == 0) ? 0 : ((res >> 28) & 8) - 4; | ||
161 | f->adaptcoeffs[-4] >>= 1; | ||
162 | f->adaptcoeffs[-8] >>= 1; | ||
163 | |||
164 | f->adaptcoeffs++; | ||
165 | |||
166 | /* Have we filled the history buffer? */ | ||
167 | if (f->delay == f->historybuffer + HISTORY_SIZE + (ORDER*2)) { | ||
168 | memmove(f->historybuffer, f->delay - (ORDER*2), | ||
169 | (ORDER*2) * sizeof(int16_t)); | ||
170 | f->delay = f->historybuffer + ORDER*2; | ||
171 | f->adaptcoeffs = f->historybuffer + ORDER; | ||
172 | } | ||
173 | } | ||
174 | } | ||
175 | |||
176 | static struct filter_t filter0 IBSS_ATTR; | ||
177 | static struct filter_t filter1 IBSS_ATTR; | ||
178 | |||
179 | static void do_init_filter(struct filter_t* f, int16_t* buf) | ||
180 | { | ||
181 | f->coeffs = buf; | ||
182 | f->historybuffer = buf + ORDER; | ||
183 | |||
184 | /* Zero the output history buffer */ | ||
185 | memset(f->historybuffer, 0 , (ORDER*2) * sizeof(int16_t)); | ||
186 | f->delay = f->historybuffer + ORDER*2; | ||
187 | f->adaptcoeffs = f->historybuffer + ORDER; | ||
188 | |||
189 | /* Zero the co-efficients */ | ||
190 | memset(f->coeffs, 0, ORDER * sizeof(int16_t)); | ||
191 | |||
192 | /* Zero the running average */ | ||
193 | f->avg = 0; | ||
194 | } | ||
195 | |||
196 | void INIT_FILTER(int16_t* buf) | ||
197 | { | ||
198 | do_init_filter(&filter0, buf); | ||
199 | do_init_filter(&filter1, buf + ORDER * 3 + HISTORY_SIZE); | ||
200 | } | ||
201 | |||
202 | int APPLY_FILTER(int fileversion, int32_t* data0, int32_t* data1, int count) | ||
203 | { | ||
204 | if (fileversion >= 3980) { | ||
205 | do_apply_filter_3980(&filter0, data0, count); | ||
206 | if (data1 != NULL) | ||
207 | do_apply_filter_3980(&filter1, data1, count); | ||
208 | } else { | ||
209 | do_apply_filter_3970(&filter0, data0, count); | ||
210 | if (data1 != NULL) | ||
211 | do_apply_filter_3970(&filter1, data1, count); | ||
212 | } | ||
213 | |||
214 | return 0; | ||
215 | } | ||