lavr: add option for dithering during sample format conversion to s16
[libav.git] / libavresample / dither.c
1 /*
2 * Copyright (c) 2012 Justin Ruggles <justin.ruggles@gmail.com>
3 *
4 * Triangular with Noise Shaping is based on opusfile.
5 * Copyright (c) 1994-2012 by the Xiph.Org Foundation and contributors
6 *
7 * This file is part of Libav.
8 *
9 * Libav is free software; you can redistribute it and/or
10 * modify it under the terms of the GNU Lesser General Public
11 * License as published by the Free Software Foundation; either
12 * version 2.1 of the License, or (at your option) any later version.
13 *
14 * Libav 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 GNU
17 * Lesser General Public License for more details.
18 *
19 * You should have received a copy of the GNU Lesser General Public
20 * License along with Libav; if not, write to the Free Software
21 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
22 */
23
24 /**
25 * @file
26 * Dithered Audio Sample Quantization
27 *
28 * Converts from dbl, flt, or s32 to s16 using dithering.
29 */
30
31 #include <math.h>
32 #include <stdint.h>
33
34 #include "libavutil/common.h"
35 #include "libavutil/lfg.h"
36 #include "libavutil/mem.h"
37 #include "libavutil/samplefmt.h"
38 #include "audio_convert.h"
39 #include "dither.h"
40 #include "internal.h"
41
42 typedef struct DitherState {
43 int mute;
44 unsigned int seed;
45 AVLFG lfg;
46 float *noise_buf;
47 int noise_buf_size;
48 int noise_buf_ptr;
49 float dither_a[4];
50 float dither_b[4];
51 } DitherState;
52
53 struct DitherContext {
54 DitherDSPContext ddsp;
55 enum AVResampleDitherMethod method;
56
57 int mute_dither_threshold; // threshold for disabling dither
58 int mute_reset_threshold; // threshold for resetting noise shaping
59 const float *ns_coef_b; // noise shaping coeffs
60 const float *ns_coef_a; // noise shaping coeffs
61
62 int channels;
63 DitherState *state; // dither states for each channel
64
65 AudioData *flt_data; // input data in fltp
66 AudioData *s16_data; // dithered output in s16p
67 AudioConvert *ac_in; // converter for input to fltp
68 AudioConvert *ac_out; // converter for s16p to s16 (if needed)
69
70 void (*quantize)(int16_t *dst, const float *src, float *dither, int len);
71 int samples_align;
72 };
73
74 /* mute threshold, in seconds */
75 #define MUTE_THRESHOLD_SEC 0.000333
76
77 /* scale factor for 16-bit output.
78 The signal is attenuated slightly to avoid clipping */
79 #define S16_SCALE 32753.0f
80
81 /* scale to convert lfg from INT_MIN/INT_MAX to -0.5/0.5 */
82 #define LFG_SCALE (1.0f / (2.0f * INT32_MAX))
83
84 /* noise shaping coefficients */
85
86 static const float ns_48_coef_b[4] = {
87 2.2374f, -0.7339f, -0.1251f, -0.6033f
88 };
89
90 static const float ns_48_coef_a[4] = {
91 0.9030f, 0.0116f, -0.5853f, -0.2571f
92 };
93
94 static const float ns_44_coef_b[4] = {
95 2.2061f, -0.4707f, -0.2534f, -0.6213f
96 };
97
98 static const float ns_44_coef_a[4] = {
99 1.0587f, 0.0676f, -0.6054f, -0.2738f
100 };
101
102 static void dither_int_to_float_rectangular_c(float *dst, int *src, int len)
103 {
104 int i;
105 for (i = 0; i < len; i++)
106 dst[i] = src[i] * LFG_SCALE;
107 }
108
109 static void dither_int_to_float_triangular_c(float *dst, int *src0, int len)
110 {
111 int i;
112 int *src1 = src0 + len;
113
114 for (i = 0; i < len; i++) {
115 float r = src0[i] * LFG_SCALE;
116 r += src1[i] * LFG_SCALE;
117 dst[i] = r;
118 }
119 }
120
121 static void quantize_c(int16_t *dst, const float *src, float *dither, int len)
122 {
123 int i;
124 for (i = 0; i < len; i++)
125 dst[i] = av_clip_int16(lrintf(src[i] * S16_SCALE + dither[i]));
126 }
127
128 #define SQRT_1_6 0.40824829046386301723f
129
130 static void dither_highpass_filter(float *src, int len)
131 {
132 int i;
133
134 /* filter is from libswresample in FFmpeg */
135 for (i = 0; i < len - 2; i++)
136 src[i] = (-src[i] + 2 * src[i + 1] - src[i + 2]) * SQRT_1_6;
137 }
138
139 static int generate_dither_noise(DitherContext *c, DitherState *state,
140 int min_samples)
141 {
142 int i;
143 int nb_samples = FFALIGN(min_samples, 16) + 16;
144 int buf_samples = nb_samples *
145 (c->method == AV_RESAMPLE_DITHER_RECTANGULAR ? 1 : 2);
146 unsigned int *noise_buf_ui;
147
148 av_freep(&state->noise_buf);
149 state->noise_buf_size = state->noise_buf_ptr = 0;
150
151 state->noise_buf = av_malloc(buf_samples * sizeof(*state->noise_buf));
152 if (!state->noise_buf)
153 return AVERROR(ENOMEM);
154 state->noise_buf_size = FFALIGN(min_samples, 16);
155 noise_buf_ui = (unsigned int *)state->noise_buf;
156
157 av_lfg_init(&state->lfg, state->seed);
158 for (i = 0; i < buf_samples; i++)
159 noise_buf_ui[i] = av_lfg_get(&state->lfg);
160
161 c->ddsp.dither_int_to_float(state->noise_buf, noise_buf_ui, nb_samples);
162
163 if (c->method == AV_RESAMPLE_DITHER_TRIANGULAR_HP)
164 dither_highpass_filter(state->noise_buf, nb_samples);
165
166 return 0;
167 }
168
169 static void quantize_triangular_ns(DitherContext *c, DitherState *state,
170 int16_t *dst, const float *src,
171 int nb_samples)
172 {
173 int i, j;
174 float *dither = &state->noise_buf[state->noise_buf_ptr];
175
176 if (state->mute > c->mute_reset_threshold)
177 memset(state->dither_a, 0, sizeof(state->dither_a));
178
179 for (i = 0; i < nb_samples; i++) {
180 float err = 0;
181 float sample = src[i] * S16_SCALE;
182
183 for (j = 0; j < 4; j++) {
184 err += c->ns_coef_b[j] * state->dither_b[j] -
185 c->ns_coef_a[j] * state->dither_a[j];
186 }
187 for (j = 3; j > 0; j--) {
188 state->dither_a[j] = state->dither_a[j - 1];
189 state->dither_b[j] = state->dither_b[j - 1];
190 }
191 state->dither_a[0] = err;
192 sample -= err;
193
194 if (state->mute > c->mute_dither_threshold) {
195 dst[i] = av_clip_int16(lrintf(sample));
196 state->dither_b[0] = 0;
197 } else {
198 dst[i] = av_clip_int16(lrintf(sample + dither[i]));
199 state->dither_b[0] = av_clipf(dst[i] - sample, -1.5f, 1.5f);
200 }
201
202 state->mute++;
203 if (src[i])
204 state->mute = 0;
205 }
206 }
207
208 static int convert_samples(DitherContext *c, int16_t **dst, float * const *src,
209 int channels, int nb_samples)
210 {
211 int ch, ret;
212 int aligned_samples = FFALIGN(nb_samples, 16);
213
214 for (ch = 0; ch < channels; ch++) {
215 DitherState *state = &c->state[ch];
216
217 if (state->noise_buf_size < aligned_samples) {
218 ret = generate_dither_noise(c, state, nb_samples);
219 if (ret < 0)
220 return ret;
221 } else if (state->noise_buf_size - state->noise_buf_ptr < aligned_samples) {
222 state->noise_buf_ptr = 0;
223 }
224
225 if (c->method == AV_RESAMPLE_DITHER_TRIANGULAR_NS) {
226 quantize_triangular_ns(c, state, dst[ch], src[ch], nb_samples);
227 } else {
228 c->quantize(dst[ch], src[ch],
229 &state->noise_buf[state->noise_buf_ptr],
230 FFALIGN(nb_samples, c->samples_align));
231 }
232
233 state->noise_buf_ptr += aligned_samples;
234 }
235
236 return 0;
237 }
238
239 int ff_convert_dither(DitherContext *c, AudioData *dst, AudioData *src)
240 {
241 int ret;
242 AudioData *flt_data;
243
244 /* output directly to dst if it is planar */
245 if (dst->sample_fmt == AV_SAMPLE_FMT_S16P)
246 c->s16_data = dst;
247 else {
248 /* make sure s16_data is large enough for the output */
249 ret = ff_audio_data_realloc(c->s16_data, src->nb_samples);
250 if (ret < 0)
251 return ret;
252 }
253
254 if (src->sample_fmt != AV_SAMPLE_FMT_FLTP) {
255 /* make sure flt_data is large enough for the input */
256 ret = ff_audio_data_realloc(c->flt_data, src->nb_samples);
257 if (ret < 0)
258 return ret;
259 flt_data = c->flt_data;
260
261 /* convert input samples to fltp and scale to s16 range */
262 ret = ff_audio_convert(c->ac_in, flt_data, src);
263 if (ret < 0)
264 return ret;
265 } else {
266 flt_data = src;
267 }
268
269 /* check alignment and padding constraints */
270 if (c->method != AV_RESAMPLE_DITHER_TRIANGULAR_NS) {
271 int ptr_align = FFMIN(flt_data->ptr_align, c->s16_data->ptr_align);
272 int samples_align = FFMIN(flt_data->samples_align, c->s16_data->samples_align);
273 int aligned_len = FFALIGN(src->nb_samples, c->ddsp.samples_align);
274
275 if (!(ptr_align % c->ddsp.ptr_align) && samples_align >= aligned_len) {
276 c->quantize = c->ddsp.quantize;
277 c->samples_align = c->ddsp.samples_align;
278 } else {
279 c->quantize = quantize_c;
280 c->samples_align = 1;
281 }
282 }
283
284 ret = convert_samples(c, (int16_t **)c->s16_data->data,
285 (float * const *)flt_data->data, src->channels,
286 src->nb_samples);
287 if (ret < 0)
288 return ret;
289
290 c->s16_data->nb_samples = src->nb_samples;
291
292 /* interleave output to dst if needed */
293 if (dst->sample_fmt == AV_SAMPLE_FMT_S16) {
294 ret = ff_audio_convert(c->ac_out, dst, c->s16_data);
295 if (ret < 0)
296 return ret;
297 } else
298 c->s16_data = NULL;
299
300 return 0;
301 }
302
303 void ff_dither_free(DitherContext **cp)
304 {
305 DitherContext *c = *cp;
306 int ch;
307
308 if (!c)
309 return;
310 ff_audio_data_free(&c->flt_data);
311 ff_audio_data_free(&c->s16_data);
312 ff_audio_convert_free(&c->ac_in);
313 ff_audio_convert_free(&c->ac_out);
314 for (ch = 0; ch < c->channels; ch++)
315 av_free(c->state[ch].noise_buf);
316 av_free(c->state);
317 av_freep(cp);
318 }
319
320 static void dither_init(DitherDSPContext *ddsp,
321 enum AVResampleDitherMethod method)
322 {
323 ddsp->quantize = quantize_c;
324 ddsp->ptr_align = 1;
325 ddsp->samples_align = 1;
326
327 if (method == AV_RESAMPLE_DITHER_RECTANGULAR)
328 ddsp->dither_int_to_float = dither_int_to_float_rectangular_c;
329 else
330 ddsp->dither_int_to_float = dither_int_to_float_triangular_c;
331 }
332
333 DitherContext *ff_dither_alloc(AVAudioResampleContext *avr,
334 enum AVSampleFormat out_fmt,
335 enum AVSampleFormat in_fmt,
336 int channels, int sample_rate)
337 {
338 AVLFG seed_gen;
339 DitherContext *c;
340 int ch;
341
342 if (av_get_packed_sample_fmt(out_fmt) != AV_SAMPLE_FMT_S16 ||
343 av_get_bytes_per_sample(in_fmt) <= 2) {
344 av_log(avr, AV_LOG_ERROR, "dithering %s to %s is not supported\n",
345 av_get_sample_fmt_name(in_fmt), av_get_sample_fmt_name(out_fmt));
346 return NULL;
347 }
348
349 c = av_mallocz(sizeof(*c));
350 if (!c)
351 return NULL;
352
353 if (avr->dither_method == AV_RESAMPLE_DITHER_TRIANGULAR_NS &&
354 sample_rate != 48000 && sample_rate != 44100) {
355 av_log(avr, AV_LOG_WARNING, "sample rate must be 48000 or 44100 Hz "
356 "for triangular_ns dither. using triangular_hp instead.\n");
357 avr->dither_method = AV_RESAMPLE_DITHER_TRIANGULAR_HP;
358 }
359 c->method = avr->dither_method;
360 dither_init(&c->ddsp, c->method);
361
362 if (c->method == AV_RESAMPLE_DITHER_TRIANGULAR_NS) {
363 if (sample_rate == 48000) {
364 c->ns_coef_b = ns_48_coef_b;
365 c->ns_coef_a = ns_48_coef_a;
366 } else {
367 c->ns_coef_b = ns_44_coef_b;
368 c->ns_coef_a = ns_44_coef_a;
369 }
370 }
371
372 /* Either s16 or s16p output format is allowed, but s16p is used
373 internally, so we need to use a temp buffer and interleave if the output
374 format is s16 */
375 if (out_fmt != AV_SAMPLE_FMT_S16P) {
376 c->s16_data = ff_audio_data_alloc(channels, 1024, AV_SAMPLE_FMT_S16P,
377 "dither s16 buffer");
378 if (!c->s16_data)
379 goto fail;
380
381 c->ac_out = ff_audio_convert_alloc(avr, out_fmt, AV_SAMPLE_FMT_S16P,
382 channels, sample_rate);
383 if (!c->ac_out)
384 goto fail;
385 }
386
387 if (in_fmt != AV_SAMPLE_FMT_FLTP) {
388 c->flt_data = ff_audio_data_alloc(channels, 1024, AV_SAMPLE_FMT_FLTP,
389 "dither flt buffer");
390 if (!c->flt_data)
391 goto fail;
392
393 c->ac_in = ff_audio_convert_alloc(avr, AV_SAMPLE_FMT_FLTP, in_fmt,
394 channels, sample_rate);
395 if (!c->ac_in)
396 goto fail;
397 }
398
399 c->state = av_mallocz(channels * sizeof(*c->state));
400 if (!c->state)
401 goto fail;
402 c->channels = channels;
403
404 /* calculate thresholds for turning off dithering during periods of
405 silence to avoid replacing digital silence with quiet dither noise */
406 c->mute_dither_threshold = lrintf(sample_rate * MUTE_THRESHOLD_SEC);
407 c->mute_reset_threshold = c->mute_dither_threshold * 4;
408
409 /* initialize dither states */
410 av_lfg_init(&seed_gen, 0xC0FFEE);
411 for (ch = 0; ch < channels; ch++) {
412 DitherState *state = &c->state[ch];
413 state->mute = c->mute_reset_threshold + 1;
414 state->seed = av_lfg_get(&seed_gen);
415 generate_dither_noise(c, state, FFMAX(32768, sample_rate / 2));
416 }
417
418 return c;
419
420 fail:
421 ff_dither_free(&c);
422 return NULL;
423 }