]>
Commit | Line | Data |
---|---|---|
1 | const HISTORY_SIZE: usize = 512; | |
2 | ||
3 | fn val2sign(val: i32) -> i32 { | |
4 | if val > 0 { | |
5 | -1 | |
6 | } else if val < 0 { | |
7 | 1 | |
8 | } else { | |
9 | 0 | |
10 | } | |
11 | } | |
12 | ||
13 | pub struct OldFilt { | |
14 | version: u16, | |
15 | compression: u16, | |
16 | } | |
17 | ||
18 | pub struct NewFilt { | |
19 | version: u16, | |
20 | filters: [NFilterContext; 3], | |
21 | lfilt: LastFilterContext, | |
22 | rfilt: LastFilterContext, | |
23 | } | |
24 | ||
25 | #[allow(clippy::large_enum_variant)] | |
26 | pub enum FilterMode { | |
27 | Old(OldFilt), | |
28 | New(NewFilt), | |
29 | None, | |
30 | } | |
31 | ||
32 | impl FilterMode { | |
33 | pub fn new(version: u16, compression: u16) -> Self { | |
34 | if version < 3930 { | |
35 | FilterMode::Old(OldFilt::new(version, compression)) | |
36 | } else { | |
37 | FilterMode::New(NewFilt::new(version, compression)) | |
38 | } | |
39 | } | |
40 | pub fn filter_mono(&mut self, l: &mut [i32]) { | |
41 | match *self { | |
42 | FilterMode::Old(ref mut ofilt) => ofilt.filter(l), | |
43 | FilterMode::New(ref mut nfilt) => nfilt.filter_mono(l), | |
44 | FilterMode::None => unreachable!(), | |
45 | }; | |
46 | } | |
47 | pub fn filter_stereo(&mut self, l: &mut [i32], r: &mut [i32]) { | |
48 | match *self { | |
49 | FilterMode::Old(ref mut ofilt) => { | |
50 | ofilt.filter(l); | |
51 | ofilt.filter(r); | |
52 | for (l, r) in l.iter_mut().zip(r.iter_mut()) { | |
53 | let new_l = *l - *r / 2; | |
54 | let new_r = *r + new_l; | |
55 | *l = new_l; | |
56 | *r = new_r; | |
57 | } | |
58 | }, | |
59 | FilterMode::New(ref mut nfilt) => { | |
60 | nfilt.filter_stereo(l, r); | |
61 | for (l, r) in l.iter_mut().zip(r.iter_mut()) { | |
62 | let new_l = *r - *l / 2; | |
63 | let new_r = *l + new_l; | |
64 | *l = new_l; | |
65 | *r = new_r; | |
66 | } | |
67 | }, | |
68 | FilterMode::None => unreachable!(), | |
69 | }; | |
70 | } | |
71 | } | |
72 | ||
73 | const NEW_FILTER_PARAMS: [[(u8, u8); 3]; 5] = [ | |
74 | [ (0, 0), ( 0, 0), ( 0, 0) ], | |
75 | [ (1, 11), ( 0, 0), ( 0, 0) ], | |
76 | [ (4, 11), ( 0, 0), ( 0, 0) ], | |
77 | [ (2, 10), (16, 13), ( 0, 0) ], | |
78 | [ (1, 11), (16, 13), (80, 15) ], | |
79 | ]; | |
80 | ||
81 | #[derive(Clone,Default)] | |
82 | struct NFilterContext { | |
83 | buf: Vec<i32>, | |
84 | coeffs: Vec<i32>, | |
85 | order: usize, | |
86 | bits: u8, | |
87 | avg: i32, | |
88 | new: bool, | |
89 | } | |
90 | ||
91 | impl NFilterContext { | |
92 | fn new(ord16: u8, bits: u8, new: bool) -> Self { | |
93 | let order = ord16 as usize * 16; | |
94 | Self { | |
95 | buf: if order > 0 { vec![0; order * 2 + HISTORY_SIZE] } else { Vec::new() }, | |
96 | coeffs: vec![0; order], | |
97 | order, | |
98 | bits, | |
99 | avg: 0, | |
100 | new, | |
101 | } | |
102 | } | |
103 | fn reset(&mut self) { | |
104 | for el in self.buf[..self.order * 2].iter_mut() { *el = 0; } | |
105 | for el in self.coeffs.iter_mut() { *el = 0; } | |
106 | self.avg = 0; | |
107 | } | |
108 | fn apply(&mut self, dst: &mut [i32]) { | |
109 | if self.order == 0 { return; } | |
110 | let mut adapt_pos = self.order; | |
111 | let mut delay_pos = self.order * 2; | |
112 | for el in dst.iter_mut() { | |
113 | let mut sum = 0i32; | |
114 | for (i, coef) in self.coeffs.iter_mut().enumerate() { | |
115 | sum += *coef * self.buf[delay_pos - self.order + i]; | |
116 | if *el < 0 { | |
117 | *coef += self.buf[adapt_pos - self.order + i]; | |
118 | } else if *el > 0 { | |
119 | *coef -= self.buf[adapt_pos - self.order + i]; | |
120 | } | |
121 | } | |
122 | let pred = (sum + (1 << (self.bits - 1))) >> self.bits; | |
123 | let val = *el + pred; | |
124 | *el = val; | |
125 | self.buf[delay_pos] = val.min(32767).max(-32768); | |
126 | if self.new { | |
127 | let aval = val.abs(); | |
128 | let sign = val2sign(val); | |
129 | self.buf[adapt_pos] = if aval == 0 { | |
130 | 0 | |
131 | } else if aval <= self.avg * 4 / 3 { | |
132 | sign * 8 | |
133 | } else if aval <= self.avg * 3 { | |
134 | sign * 16 | |
135 | } else { | |
136 | sign * 32 | |
137 | }; | |
138 | self.avg += (aval - self.avg) / 16; | |
139 | self.buf[adapt_pos - 1] >>= 1; | |
140 | self.buf[adapt_pos - 2] >>= 1; | |
141 | self.buf[adapt_pos - 8] >>= 1; | |
142 | } else { | |
143 | self.buf[adapt_pos] = 4 * val2sign(val); | |
144 | self.buf[adapt_pos - 4] >>= 1; | |
145 | self.buf[adapt_pos - 8] >>= 1; | |
146 | } | |
147 | delay_pos += 1; | |
148 | adapt_pos += 1; | |
149 | if delay_pos == HISTORY_SIZE + self.order * 2 { | |
150 | delay_pos = self.order * 2; | |
151 | adapt_pos = self.order; | |
152 | for i in 0..self.order * 2 { | |
153 | self.buf[i] = self.buf[HISTORY_SIZE + i]; | |
154 | } | |
155 | } | |
156 | } | |
157 | } | |
158 | } | |
159 | ||
160 | #[derive(Clone,Copy,Default)] | |
161 | struct LastFilterContext { | |
162 | last_a: i32, | |
163 | filter_a: i32, | |
164 | filter_b: i32, | |
165 | coeffs_a: [i32; 4], | |
166 | coeffs_b: [i32; 5], | |
167 | delay_a: [i32; 4], | |
168 | adapt_a: [i32; 4], | |
169 | delay_b: [i32; 5], | |
170 | adapt_b: [i32; 5], | |
171 | } | |
172 | ||
173 | impl LastFilterContext { | |
174 | fn init(&mut self) { | |
175 | const COEFFS_A_NEW: [i32; 4] = [360, 317, -109, 98]; | |
176 | ||
177 | self.filter_a = 0; | |
178 | self.filter_b = 0; | |
179 | self.coeffs_a = COEFFS_A_NEW; | |
180 | self.coeffs_b = [0; 5]; | |
181 | self.last_a = 0; | |
182 | self.delay_a = [0; 4]; | |
183 | self.adapt_a = [0; 4]; | |
184 | self.delay_b = [0; 5]; | |
185 | self.adapt_b = [0; 5]; | |
186 | } | |
187 | fn predict_a(&mut self) -> i32 { | |
188 | for i in (0..3).rev() { | |
189 | self.delay_a[i + 1] = self.delay_a[i]; | |
190 | self.adapt_a[i + 1] = self.adapt_a[i]; | |
191 | } | |
192 | self.delay_a[0] = self.last_a; | |
193 | self.delay_a[1] = self.last_a - self.delay_a[1]; | |
194 | self.adapt_a[0] = val2sign(self.delay_a[0]); | |
195 | self.adapt_a[1] = val2sign(self.delay_a[1]); | |
196 | ||
197 | self.delay_a[0] * self.coeffs_a[0] + | |
198 | self.delay_a[1] * self.coeffs_a[1] + | |
199 | self.delay_a[2] * self.coeffs_a[2] + | |
200 | self.delay_a[3] * self.coeffs_a[3] | |
201 | } | |
202 | fn predict_b(&mut self, other_a: i32) -> i32 { | |
203 | for i in (0..4).rev() { | |
204 | self.delay_b[i + 1] = self.delay_b[i]; | |
205 | self.adapt_b[i + 1] = self.adapt_b[i]; | |
206 | } | |
207 | self.delay_b[0] = other_a - ((self.filter_b * 31) >> 5); | |
208 | self.delay_b[1] = self.delay_b[0] - self.delay_b[1]; | |
209 | self.adapt_b[0] = val2sign(self.delay_b[0]); | |
210 | self.adapt_b[1] = val2sign(self.delay_b[1]); | |
211 | ||
212 | self.filter_b = other_a; | |
213 | ||
214 | (self.delay_b[0] * self.coeffs_b[0] + | |
215 | self.delay_b[1] * self.coeffs_b[1] + | |
216 | self.delay_b[2] * self.coeffs_b[2] + | |
217 | self.delay_b[3] * self.coeffs_b[3] + | |
218 | self.delay_b[4] * self.coeffs_b[4]) >> 1 | |
219 | } | |
220 | fn update_a(&mut self, pred: i32, diff: i32) -> i32 { | |
221 | self.last_a = diff + (pred >> 10); | |
222 | let sign = val2sign(diff); | |
223 | for i in 0..4 { | |
224 | self.coeffs_a[i] += self.adapt_a[i] * sign; | |
225 | } | |
226 | self.filter_a = self.last_a + ((self.filter_a * 31) >> 5); | |
227 | ||
228 | self.filter_a | |
229 | } | |
230 | fn update_b(&mut self, diff: i32) { | |
231 | let sign = val2sign(diff); | |
232 | for i in 0..5 { | |
233 | self.coeffs_b[i] += self.adapt_b[i] * sign; | |
234 | } | |
235 | } | |
236 | fn predict_3930(&mut self, diff: i32) -> i32 { | |
237 | for i in (0..3).rev() { | |
238 | self.delay_a[i + 1] = self.delay_a[i]; | |
239 | } | |
240 | self.delay_a[0] = self.last_a; | |
241 | let d0 = self.delay_a[0]; | |
242 | let d1 = self.delay_a[0] - self.delay_a[1]; | |
243 | let d2 = self.delay_a[1] - self.delay_a[2]; | |
244 | let d3 = self.delay_a[2] - self.delay_a[3]; | |
245 | ||
246 | let pred = (self.coeffs_a[0] * d0 + | |
247 | self.coeffs_a[1] * d1 + | |
248 | self.coeffs_a[2] * d2 + | |
249 | self.coeffs_a[3] * d3) >> 9; | |
250 | self.last_a = diff + pred; | |
251 | self.filter_a = self.last_a + ((self.filter_a * 31) >> 5); | |
252 | ||
253 | let sign = val2sign(diff); | |
254 | self.coeffs_a[0] += if d0 < 0 { sign } else { -sign }; | |
255 | self.coeffs_a[1] += if d1 < 0 { sign } else { -sign }; | |
256 | self.coeffs_a[2] += if d2 < 0 { sign } else { -sign }; | |
257 | self.coeffs_a[3] += if d3 < 0 { sign } else { -sign }; | |
258 | ||
259 | self.filter_a | |
260 | } | |
261 | } | |
262 | ||
263 | impl NewFilt { | |
264 | fn new(version: u16, compression: u16) -> Self { | |
265 | let cidx = (compression / 1000) as usize - 1; | |
266 | let mut obj = Self { | |
267 | version, | |
268 | filters: [NFilterContext::default(), NFilterContext::default(), NFilterContext::default()], | |
269 | lfilt: LastFilterContext::default(), | |
270 | rfilt: LastFilterContext::default(), | |
271 | }; | |
272 | obj.version = version; | |
273 | let new = version >= 3980; | |
274 | for i in 0..3 { | |
275 | let (ord16, bits) = NEW_FILTER_PARAMS[cidx][i]; | |
276 | obj.filters[i] = NFilterContext::new(ord16, bits, new); | |
277 | } | |
278 | obj | |
279 | } | |
280 | fn filter_mono(&mut self, dst: &mut [i32]) { | |
281 | for filt in self.filters.iter_mut() { | |
282 | filt.reset(); | |
283 | filt.apply(dst); | |
284 | } | |
285 | self.lfilt.init(); | |
286 | if self.version >= 3950 { | |
287 | for el in dst.iter_mut() { | |
288 | let pred = self.lfilt.predict_a(); | |
289 | *el = self.lfilt.update_a(pred, *el); | |
290 | } | |
291 | } else { | |
292 | for el in dst.iter_mut() { | |
293 | *el = self.lfilt.predict_3930(*el); | |
294 | } | |
295 | } | |
296 | } | |
297 | fn filter_stereo(&mut self, l: &mut [i32], r: &mut [i32]) { | |
298 | for filt in self.filters.iter_mut() { | |
299 | filt.reset(); | |
300 | filt.apply(l); | |
301 | filt.reset(); | |
302 | filt.apply(r); | |
303 | } | |
304 | self.lfilt.init(); | |
305 | self.rfilt.init(); | |
306 | if self.version >= 3950 { | |
307 | for (l, r) in l.iter_mut().zip(r.iter_mut()) { | |
308 | let mut pred = self.lfilt.predict_a(); | |
309 | pred += self.lfilt.predict_b(self.rfilt.filter_a); | |
310 | let new_l = self.lfilt.update_a(pred, *l); | |
311 | self.lfilt.update_b(*l); | |
312 | *l = new_l; | |
313 | ||
314 | let mut pred = self.rfilt.predict_a(); | |
315 | pred += self.rfilt.predict_b(self.lfilt.filter_a); | |
316 | let new_r = self.rfilt.update_a(pred, *r); | |
317 | self.rfilt.update_b(*r); | |
318 | *r = new_r; | |
319 | } | |
320 | } else { | |
321 | for (l, r) in l.iter_mut().zip(r.iter_mut()) { | |
322 | let new_l = self.lfilt.predict_3930(*r); | |
323 | let new_r = self.rfilt.predict_3930(*l); | |
324 | *l = new_l; | |
325 | *r = new_r; | |
326 | } | |
327 | } | |
328 | } | |
329 | } | |
330 | ||
331 | impl OldFilt { | |
332 | fn new(version: u16, compression: u16) -> Self { | |
333 | Self { | |
334 | version, compression | |
335 | } | |
336 | } | |
337 | fn filter(&mut self, dst: &mut [i32]) { | |
338 | match self.compression { | |
339 | 1000 => { | |
340 | Self::filter_fast(dst); | |
341 | }, | |
342 | 2000 => { | |
343 | Self::filter_normal(dst, 4, 10); | |
344 | }, | |
345 | 3000 => { | |
346 | Self::filter_high(dst, 16, 9); | |
347 | Self::filter_normal(dst, 16, 10); | |
348 | }, | |
349 | 4000 => { | |
350 | if self.version < 3830 { | |
351 | Self::filter_high(dst, 128, 11); | |
352 | Self::filter_normal(dst, 128, 10); | |
353 | } else { | |
354 | Self::filter_extra_high(dst); | |
355 | Self::filter_high(dst, 256, 12); | |
356 | Self::filter_normal(dst, 256, 11); | |
357 | } | |
358 | }, | |
359 | _ => unreachable!(), | |
360 | }; | |
361 | } | |
362 | fn filter_fast(dst: &mut [i32]) { | |
363 | const COEFF_A_FAST: i32 = 375; | |
364 | ||
365 | if dst.len() <= 3 { | |
366 | return; | |
367 | } | |
368 | let mut delay = [dst[1], dst[0]]; | |
369 | let mut last = dst[2]; | |
370 | let mut filter = dst[2]; | |
371 | let mut weight = COEFF_A_FAST; | |
372 | for el in dst[3..].iter_mut() { | |
373 | delay[1] = delay[0]; | |
374 | delay[0] = last; | |
375 | let pred = delay[0] * 2 - delay[1]; | |
376 | last = *el + ((pred * weight) >> 9); | |
377 | if (*el ^ pred) > 0 { | |
378 | weight += 1; | |
379 | } else { | |
380 | weight -= 1; | |
381 | } | |
382 | filter += last; | |
383 | *el = filter; | |
384 | } | |
385 | } | |
386 | fn filter_normal(dst: &mut [i32], start: usize, shift: u8) { | |
387 | const COEFFS_A_NORMAL: [i32; 3] = [64, 115, 64]; | |
388 | const COEFFS_B_NORMAL: [i32; 2] = [740, 0]; | |
389 | ||
390 | let mut last = 0; | |
391 | let mut coeffs_a = COEFFS_A_NORMAL; | |
392 | let mut coeffs_b = COEFFS_B_NORMAL; | |
393 | let mut filter_a = 0; | |
394 | let mut filter_b = 0; | |
395 | let mut delay_a = [0; 3]; | |
396 | let mut delay_b = [0; 2]; | |
397 | ||
398 | for (i, el) in dst.iter_mut().enumerate() { | |
399 | delay_a[2] = delay_a[1]; delay_a[1] = delay_a[0]; delay_a[0] = last; | |
400 | delay_b[1] = delay_b[0]; delay_b[0] = filter_b; | |
401 | if i < start { | |
402 | let val = *el + filter_a; | |
403 | last = *el; | |
404 | filter_b = *el; | |
405 | filter_a = val; | |
406 | *el = val; | |
407 | continue; | |
408 | } | |
409 | let a0 = delay_a[0] + (delay_a[2] - delay_a[1]) * 8; | |
410 | let a1 = (delay_a[0] - delay_a[1]) * 2; | |
411 | let a2 = delay_a[0]; | |
412 | let b0 = delay_b[0] * 2 - delay_b[1]; | |
413 | let b1 = delay_b[0]; | |
414 | ||
415 | let pred_a = a0 * coeffs_a[0] + a1 * coeffs_a[1] + a2 * coeffs_a[2]; | |
416 | let pred_b = b0 * coeffs_b[0] - b1 * coeffs_b[1]; | |
417 | ||
418 | let sign = val2sign(*el); | |
419 | coeffs_a[0] += (((a0 >> 30) & 2) - 1) * sign; | |
420 | coeffs_a[1] += (((a1 >> 28) & 8) - 4) * sign; | |
421 | coeffs_a[2] += (((a2 >> 28) & 8) - 4) * sign; | |
422 | last = *el + (pred_a >> 11); | |
423 | ||
424 | let sign = val2sign(last); | |
425 | coeffs_b[0] += (((b0 >> 29) & 4) - 2) * sign; | |
426 | coeffs_b[1] -= (((b1 >> 30) & 2) - 1) * sign; | |
427 | ||
428 | filter_b = last + (pred_b >> shift); | |
429 | filter_a = filter_b + ((filter_a * 31) >> 5); | |
430 | ||
431 | *el = filter_a; | |
432 | } | |
433 | } | |
434 | fn filter_high(dst: &mut [i32], order: usize, shift: u8) { | |
435 | let mut coeffs = [0i32; 256]; | |
436 | let mut delay = [0i32; 256]; | |
437 | if dst.len() <= order { | |
438 | return; | |
439 | } | |
440 | delay[..order].copy_from_slice(&dst[..order]); | |
441 | for el in dst[order..].iter_mut() { | |
442 | let sign = val2sign(*el); | |
443 | let mut sum = 0; | |
444 | for i in 0..order { | |
445 | sum += delay[i] * coeffs[i]; | |
446 | coeffs[i] -= (((delay[i] >> 30) & 2) - 1) * sign; | |
447 | } | |
448 | *el -= sum >> shift; | |
449 | for i in 0..order-1 { | |
450 | delay[i] = delay[i + 1]; | |
451 | } | |
452 | delay[order - 1] = *el; | |
453 | } | |
454 | } | |
455 | fn filter_extra_high(dst: &mut [i32]) { | |
456 | let mut coeffs = [0i32; 8]; | |
457 | let mut delay = [0i32; 8]; | |
458 | for el in dst[256..].iter_mut() { | |
459 | let sign = val2sign(*el); | |
460 | let mut sum = 0; | |
461 | for i in 0..8 { | |
462 | sum += delay[i] * coeffs[i]; | |
463 | coeffs[i] -= (((delay[i] >> 30) & 2) - 1) * sign; | |
464 | } | |
465 | for i in (0..7).rev() { | |
466 | delay[i + 1] = delay[i]; | |
467 | } | |
468 | delay[0] = *el; | |
469 | *el -= sum >> 9; | |
470 | } | |
471 | } | |
472 | } |