lavr: only save/restore the mixing matrix if mixing is being done
[ffmpeg.git] / libavresample / audio_mix.c
1 /*
2  * Copyright (c) 2012 Justin Ruggles <justin.ruggles@gmail.com>
3  *
4  * This file is part of Libav.
5  *
6  * Libav is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Lesser General Public
8  * License as published by the Free Software Foundation; either
9  * version 2.1 of the License, or (at your option) any later version.
10  *
11  * Libav is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with Libav; if not, write to the Free Software
18  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19  */
20
21 #include <stdint.h>
22
23 #include "libavutil/common.h"
24 #include "libavutil/libm.h"
25 #include "libavutil/samplefmt.h"
26 #include "avresample.h"
27 #include "internal.h"
28 #include "audio_data.h"
29 #include "audio_mix.h"
30
31 static const char *coeff_type_names[] = { "q8", "q15", "flt" };
32
33 struct AudioMix {
34     AVAudioResampleContext *avr;
35     enum AVSampleFormat fmt;
36     enum AVMixCoeffType coeff_type;
37     uint64_t in_layout;
38     uint64_t out_layout;
39     int in_channels;
40     int out_channels;
41
42     int ptr_align;
43     int samples_align;
44     int has_optimized_func;
45     const char *func_descr;
46     const char *func_descr_generic;
47     mix_func *mix;
48     mix_func *mix_generic;
49
50     int16_t *matrix_q8[AVRESAMPLE_MAX_CHANNELS];
51     int32_t *matrix_q15[AVRESAMPLE_MAX_CHANNELS];
52     float   *matrix_flt[AVRESAMPLE_MAX_CHANNELS];
53     void   **matrix;
54 };
55
56 void ff_audio_mix_set_func(AudioMix *am, enum AVSampleFormat fmt,
57                            enum AVMixCoeffType coeff_type, int in_channels,
58                            int out_channels, int ptr_align, int samples_align,
59                            const char *descr, void *mix_func)
60 {
61     if (fmt == am->fmt && coeff_type == am->coeff_type &&
62         ( in_channels ==  am->in_channels ||  in_channels == 0) &&
63         (out_channels == am->out_channels || out_channels == 0)) {
64         char chan_str[16];
65         am->mix           = mix_func;
66         am->func_descr    = descr;
67         am->ptr_align     = ptr_align;
68         am->samples_align = samples_align;
69         if (ptr_align == 1 && samples_align == 1) {
70             am->mix_generic        = mix_func;
71             am->func_descr_generic = descr;
72         } else {
73             am->has_optimized_func = 1;
74         }
75         if (in_channels) {
76             if (out_channels)
77                 snprintf(chan_str, sizeof(chan_str), "[%d to %d] ",
78                          in_channels, out_channels);
79             else
80                 snprintf(chan_str, sizeof(chan_str), "[%d to any] ",
81                          in_channels);
82         } else if (out_channels) {
83                 snprintf(chan_str, sizeof(chan_str), "[any to %d] ",
84                          out_channels);
85         }
86         av_log(am->avr, AV_LOG_DEBUG, "audio_mix: found function: [fmt=%s] "
87                "[c=%s] %s(%s)\n", av_get_sample_fmt_name(fmt),
88                coeff_type_names[coeff_type],
89                (in_channels || out_channels) ? chan_str : "", descr);
90     }
91 }
92
93 #define MIX_FUNC_NAME(fmt, cfmt) mix_any_ ## fmt ##_## cfmt ##_c
94
95 #define MIX_FUNC_GENERIC(fmt, cfmt, stype, ctype, sumtype, expr)            \
96 static void MIX_FUNC_NAME(fmt, cfmt)(stype **samples, ctype **matrix,       \
97                                      int len, int out_ch, int in_ch)        \
98 {                                                                           \
99     int i, in, out;                                                         \
100     stype temp[AVRESAMPLE_MAX_CHANNELS];                                    \
101     for (i = 0; i < len; i++) {                                             \
102         for (out = 0; out < out_ch; out++) {                                \
103             sumtype sum = 0;                                                \
104             for (in = 0; in < in_ch; in++)                                  \
105                 sum += samples[in][i] * matrix[out][in];                    \
106             temp[out] = expr;                                               \
107         }                                                                   \
108         for (out = 0; out < out_ch; out++)                                  \
109             samples[out][i] = temp[out];                                    \
110     }                                                                       \
111 }
112
113 MIX_FUNC_GENERIC(FLTP, FLT, float,   float,   float,   sum)
114 MIX_FUNC_GENERIC(S16P, FLT, int16_t, float,   float,   av_clip_int16(lrintf(sum)))
115 MIX_FUNC_GENERIC(S16P, Q15, int16_t, int32_t, int64_t, av_clip_int16(sum >> 15))
116 MIX_FUNC_GENERIC(S16P, Q8,  int16_t, int16_t, int32_t, av_clip_int16(sum >>  8))
117
118 /* TODO: templatize the channel-specific C functions */
119
120 static void mix_2_to_1_fltp_flt_c(float **samples, float **matrix, int len,
121                                   int out_ch, int in_ch)
122 {
123     float *src0 = samples[0];
124     float *src1 = samples[1];
125     float *dst  = src0;
126     float m0    = matrix[0][0];
127     float m1    = matrix[0][1];
128
129     while (len > 4) {
130         *dst++ = *src0++ * m0 + *src1++ * m1;
131         *dst++ = *src0++ * m0 + *src1++ * m1;
132         *dst++ = *src0++ * m0 + *src1++ * m1;
133         *dst++ = *src0++ * m0 + *src1++ * m1;
134         len -= 4;
135     }
136     while (len > 0) {
137         *dst++ = *src0++ * m0 + *src1++ * m1;
138         len--;
139     }
140 }
141
142 static void mix_2_to_1_s16p_flt_c(int16_t **samples, float **matrix, int len,
143                                   int out_ch, int in_ch)
144 {
145     int16_t *src0 = samples[0];
146     int16_t *src1 = samples[1];
147     int16_t *dst  = src0;
148     float m0      = matrix[0][0];
149     float m1      = matrix[0][1];
150
151     while (len > 4) {
152         *dst++ = av_clip_int16(lrintf(*src0++ * m0 + *src1++ * m1));
153         *dst++ = av_clip_int16(lrintf(*src0++ * m0 + *src1++ * m1));
154         *dst++ = av_clip_int16(lrintf(*src0++ * m0 + *src1++ * m1));
155         *dst++ = av_clip_int16(lrintf(*src0++ * m0 + *src1++ * m1));
156         len -= 4;
157     }
158     while (len > 0) {
159         *dst++ = av_clip_int16(lrintf(*src0++ * m0 + *src1++ * m1));
160         len--;
161     }
162 }
163
164 static void mix_2_to_1_s16p_q8_c(int16_t **samples, int16_t **matrix, int len,
165                                  int out_ch, int in_ch)
166 {
167     int16_t *src0 = samples[0];
168     int16_t *src1 = samples[1];
169     int16_t *dst  = src0;
170     int16_t m0    = matrix[0][0];
171     int16_t m1    = matrix[0][1];
172
173     while (len > 4) {
174         *dst++ = (*src0++ * m0 + *src1++ * m1) >> 8;
175         *dst++ = (*src0++ * m0 + *src1++ * m1) >> 8;
176         *dst++ = (*src0++ * m0 + *src1++ * m1) >> 8;
177         *dst++ = (*src0++ * m0 + *src1++ * m1) >> 8;
178         len -= 4;
179     }
180     while (len > 0) {
181         *dst++ = (*src0++ * m0 + *src1++ * m1) >> 8;
182         len--;
183     }
184 }
185
186 static void mix_1_to_2_fltp_flt_c(float **samples, float **matrix, int len,
187                                   int out_ch, int in_ch)
188 {
189     float v;
190     float *dst0 = samples[0];
191     float *dst1 = samples[1];
192     float *src  = dst0;
193     float m0    = matrix[0][0];
194     float m1    = matrix[1][0];
195
196     while (len > 4) {
197         v = *src++;
198         *dst0++ = v * m1;
199         *dst1++ = v * m0;
200         v = *src++;
201         *dst0++ = v * m1;
202         *dst1++ = v * m0;
203         v = *src++;
204         *dst0++ = v * m1;
205         *dst1++ = v * m0;
206         v = *src++;
207         *dst0++ = v * m1;
208         *dst1++ = v * m0;
209         len -= 4;
210     }
211     while (len > 0) {
212         v = *src++;
213         *dst0++ = v * m1;
214         *dst1++ = v * m0;
215         len--;
216     }
217 }
218
219 static void mix_6_to_2_fltp_flt_c(float **samples, float **matrix, int len,
220                                   int out_ch, int in_ch)
221 {
222     float v0, v1;
223     float *src0 = samples[0];
224     float *src1 = samples[1];
225     float *src2 = samples[2];
226     float *src3 = samples[3];
227     float *src4 = samples[4];
228     float *src5 = samples[5];
229     float *dst0 = src0;
230     float *dst1 = src1;
231     float *m0   = matrix[0];
232     float *m1   = matrix[1];
233
234     while (len > 0) {
235         v0 = *src0++;
236         v1 = *src1++;
237         *dst0++ = v0      * m0[0] +
238                   v1      * m0[1] +
239                   *src2   * m0[2] +
240                   *src3   * m0[3] +
241                   *src4   * m0[4] +
242                   *src5   * m0[5];
243         *dst1++ = v0      * m1[0] +
244                   v1      * m1[1] +
245                   *src2++ * m1[2] +
246                   *src3++ * m1[3] +
247                   *src4++ * m1[4] +
248                   *src5++ * m1[5];
249         len--;
250     }
251 }
252
253 static void mix_2_to_6_fltp_flt_c(float **samples, float **matrix, int len,
254                                   int out_ch, int in_ch)
255 {
256     float v0, v1;
257     float *dst0 = samples[0];
258     float *dst1 = samples[1];
259     float *dst2 = samples[2];
260     float *dst3 = samples[3];
261     float *dst4 = samples[4];
262     float *dst5 = samples[5];
263     float *src0 = dst0;
264     float *src1 = dst1;
265
266     while (len > 0) {
267         v0 = *src0++;
268         v1 = *src1++;
269         *dst0++ = v0 * matrix[0][0] + v1 * matrix[0][1];
270         *dst1++ = v0 * matrix[1][0] + v1 * matrix[1][1];
271         *dst2++ = v0 * matrix[2][0] + v1 * matrix[2][1];
272         *dst3++ = v0 * matrix[3][0] + v1 * matrix[3][1];
273         *dst4++ = v0 * matrix[4][0] + v1 * matrix[4][1];
274         *dst5++ = v0 * matrix[5][0] + v1 * matrix[5][1];
275         len--;
276     }
277 }
278
279 static int mix_function_init(AudioMix *am)
280 {
281     /* any-to-any C versions */
282
283     ff_audio_mix_set_func(am, AV_SAMPLE_FMT_FLTP, AV_MIX_COEFF_TYPE_FLT,
284                           0, 0, 1, 1, "C", MIX_FUNC_NAME(FLTP, FLT));
285
286     ff_audio_mix_set_func(am, AV_SAMPLE_FMT_S16P, AV_MIX_COEFF_TYPE_FLT,
287                           0, 0, 1, 1, "C", MIX_FUNC_NAME(S16P, FLT));
288
289     ff_audio_mix_set_func(am, AV_SAMPLE_FMT_S16P, AV_MIX_COEFF_TYPE_Q15,
290                           0, 0, 1, 1, "C", MIX_FUNC_NAME(S16P, Q15));
291
292     ff_audio_mix_set_func(am, AV_SAMPLE_FMT_S16P, AV_MIX_COEFF_TYPE_Q8,
293                           0, 0, 1, 1, "C", MIX_FUNC_NAME(S16P, Q8));
294
295     /* channel-specific C versions */
296
297     ff_audio_mix_set_func(am, AV_SAMPLE_FMT_FLTP, AV_MIX_COEFF_TYPE_FLT,
298                           2, 1, 1, 1, "C", mix_2_to_1_fltp_flt_c);
299
300     ff_audio_mix_set_func(am, AV_SAMPLE_FMT_S16P, AV_MIX_COEFF_TYPE_FLT,
301                           2, 1, 1, 1, "C", mix_2_to_1_s16p_flt_c);
302
303     ff_audio_mix_set_func(am, AV_SAMPLE_FMT_S16P, AV_MIX_COEFF_TYPE_Q8,
304                           2, 1, 1, 1, "C", mix_2_to_1_s16p_q8_c);
305
306     ff_audio_mix_set_func(am, AV_SAMPLE_FMT_FLTP, AV_MIX_COEFF_TYPE_FLT,
307                           1, 2, 1, 1, "C", mix_1_to_2_fltp_flt_c);
308
309     ff_audio_mix_set_func(am, AV_SAMPLE_FMT_FLTP, AV_MIX_COEFF_TYPE_FLT,
310                           6, 2, 1, 1, "C", mix_6_to_2_fltp_flt_c);
311
312     ff_audio_mix_set_func(am, AV_SAMPLE_FMT_FLTP, AV_MIX_COEFF_TYPE_FLT,
313                           2, 6, 1, 1, "C", mix_2_to_6_fltp_flt_c);
314
315     if (ARCH_X86)
316         ff_audio_mix_init_x86(am);
317
318     if (!am->mix) {
319         av_log(am->avr, AV_LOG_ERROR, "audio_mix: NO FUNCTION FOUND: [fmt=%s] "
320                "[c=%s] [%d to %d]\n", av_get_sample_fmt_name(am->fmt),
321                coeff_type_names[am->coeff_type], am->in_channels,
322                am->out_channels);
323         return AVERROR_PATCHWELCOME;
324     }
325     return 0;
326 }
327
328 AudioMix *ff_audio_mix_alloc(AVAudioResampleContext *avr)
329 {
330     AudioMix *am;
331     int ret;
332
333     am = av_mallocz(sizeof(*am));
334     if (!am)
335         return NULL;
336     am->avr = avr;
337
338     if (avr->internal_sample_fmt != AV_SAMPLE_FMT_S16P &&
339         avr->internal_sample_fmt != AV_SAMPLE_FMT_FLTP) {
340         av_log(avr, AV_LOG_ERROR, "Unsupported internal format for "
341                "mixing: %s\n",
342                av_get_sample_fmt_name(avr->internal_sample_fmt));
343         goto error;
344     }
345
346     am->fmt          = avr->internal_sample_fmt;
347     am->coeff_type   = avr->mix_coeff_type;
348     am->in_layout    = avr->in_channel_layout;
349     am->out_layout   = avr->out_channel_layout;
350     am->in_channels  = avr->in_channels;
351     am->out_channels = avr->out_channels;
352
353     /* build matrix if the user did not already set one */
354     if (avr->mix_matrix) {
355         ret = ff_audio_mix_set_matrix(am, avr->mix_matrix, avr->in_channels);
356         if (ret < 0)
357             goto error;
358         av_freep(&avr->mix_matrix);
359     } else {
360         int i, j;
361         char in_layout_name[128];
362         char out_layout_name[128];
363         double *matrix_dbl = av_mallocz(avr->out_channels * avr->in_channels *
364                                         sizeof(*matrix_dbl));
365         if (!matrix_dbl)
366             goto error;
367
368         ret = avresample_build_matrix(avr->in_channel_layout,
369                                       avr->out_channel_layout,
370                                       avr->center_mix_level,
371                                       avr->surround_mix_level,
372                                       avr->lfe_mix_level,
373                                       avr->normalize_mix_level,
374                                       matrix_dbl,
375                                       avr->in_channels,
376                                       avr->matrix_encoding);
377         if (ret < 0) {
378             av_free(matrix_dbl);
379             goto error;
380         }
381
382         av_get_channel_layout_string(in_layout_name, sizeof(in_layout_name),
383                                      avr->in_channels, avr->in_channel_layout);
384         av_get_channel_layout_string(out_layout_name, sizeof(out_layout_name),
385                                      avr->out_channels, avr->out_channel_layout);
386         av_log(avr, AV_LOG_DEBUG, "audio_mix: %s to %s\n",
387                in_layout_name, out_layout_name);
388         for (i = 0; i < avr->out_channels; i++) {
389             for (j = 0; j < avr->in_channels; j++) {
390                 av_log(avr, AV_LOG_DEBUG, "  %0.3f ",
391                        matrix_dbl[i * avr->in_channels + j]);
392             }
393             av_log(avr, AV_LOG_DEBUG, "\n");
394         }
395
396         ret = ff_audio_mix_set_matrix(am, matrix_dbl, avr->in_channels);
397         if (ret < 0) {
398             av_free(matrix_dbl);
399             goto error;
400         }
401         av_free(matrix_dbl);
402     }
403
404     ret = mix_function_init(am);
405     if (ret < 0)
406         goto error;
407
408     return am;
409
410 error:
411     av_free(am);
412     return NULL;
413 }
414
415 void ff_audio_mix_free(AudioMix **am_p)
416 {
417     AudioMix *am;
418
419     if (!*am_p)
420         return;
421     am = *am_p;
422
423     if (am->matrix) {
424         av_free(am->matrix[0]);
425         am->matrix = NULL;
426     }
427     memset(am->matrix_q8,  0, sizeof(am->matrix_q8 ));
428     memset(am->matrix_q15, 0, sizeof(am->matrix_q15));
429     memset(am->matrix_flt, 0, sizeof(am->matrix_flt));
430
431     av_freep(am_p);
432 }
433
434 int ff_audio_mix(AudioMix *am, AudioData *src)
435 {
436     int use_generic = 1;
437     int len = src->nb_samples;
438
439     /* determine whether to use the optimized function based on pointer and
440        samples alignment in both the input and output */
441     if (am->has_optimized_func) {
442         int aligned_len = FFALIGN(len, am->samples_align);
443         if (!(src->ptr_align % am->ptr_align) &&
444             src->samples_align >= aligned_len) {
445             len = aligned_len;
446             use_generic = 0;
447         }
448     }
449     av_dlog(am->avr, "audio_mix: %d samples - %d to %d channels (%s)\n",
450             src->nb_samples, am->in_channels, am->out_channels,
451             use_generic ? am->func_descr_generic : am->func_descr);
452
453     if (use_generic)
454         am->mix_generic(src->data, am->matrix, len, am->out_channels,
455                         am->in_channels);
456     else
457         am->mix(src->data, am->matrix, len, am->out_channels, am->in_channels);
458
459     ff_audio_data_set_channels(src, am->out_channels);
460
461     return 0;
462 }
463
464 int ff_audio_mix_get_matrix(AudioMix *am, double *matrix, int stride)
465 {
466     int i, o;
467
468     if ( am->in_channels <= 0 ||  am->in_channels > AVRESAMPLE_MAX_CHANNELS ||
469         am->out_channels <= 0 || am->out_channels > AVRESAMPLE_MAX_CHANNELS) {
470         av_log(am, AV_LOG_ERROR, "Invalid channel counts\n");
471         return AVERROR(EINVAL);
472     }
473
474 #define GET_MATRIX_CONVERT(suffix, scale)                                   \
475     if (!am->matrix_ ## suffix[0]) {                                        \
476         av_log(am, AV_LOG_ERROR, "matrix is not set\n");                    \
477         return AVERROR(EINVAL);                                             \
478     }                                                                       \
479     for (o = 0; o < am->out_channels; o++)                                  \
480         for (i = 0; i < am->in_channels; i++)                               \
481             matrix[o * stride + i] = am->matrix_ ## suffix[o][i] * (scale);
482
483     switch (am->coeff_type) {
484     case AV_MIX_COEFF_TYPE_Q8:
485         GET_MATRIX_CONVERT(q8, 1.0 / 256.0);
486         break;
487     case AV_MIX_COEFF_TYPE_Q15:
488         GET_MATRIX_CONVERT(q15, 1.0 / 32768.0);
489         break;
490     case AV_MIX_COEFF_TYPE_FLT:
491         GET_MATRIX_CONVERT(flt, 1.0);
492         break;
493     default:
494         av_log(am, AV_LOG_ERROR, "Invalid mix coeff type\n");
495         return AVERROR(EINVAL);
496     }
497
498     return 0;
499 }
500
501 int ff_audio_mix_set_matrix(AudioMix *am, const double *matrix, int stride)
502 {
503     int i, o;
504
505     if ( am->in_channels <= 0 ||  am->in_channels > AVRESAMPLE_MAX_CHANNELS ||
506         am->out_channels <= 0 || am->out_channels > AVRESAMPLE_MAX_CHANNELS) {
507         av_log(am, AV_LOG_ERROR, "Invalid channel counts\n");
508         return AVERROR(EINVAL);
509     }
510
511     if (am->matrix) {
512         av_free(am->matrix[0]);
513         am->matrix = NULL;
514     }
515
516 #define CONVERT_MATRIX(type, expr)                                          \
517     am->matrix_## type[0] = av_mallocz(am->out_channels * am->in_channels * \
518                                        sizeof(*am->matrix_## type[0]));     \
519     if (!am->matrix_## type[0])                                             \
520         return AVERROR(ENOMEM);                                             \
521     for (o = 0; o < am->out_channels; o++) {                                \
522         if (o > 0)                                                          \
523             am->matrix_## type[o] = am->matrix_## type[o - 1] +             \
524                                     am->in_channels;                        \
525         for (i = 0; i < am->in_channels; i++) {                             \
526             double v = matrix[o * stride + i];                              \
527             am->matrix_## type[o][i] = expr;                                \
528         }                                                                   \
529     }                                                                       \
530     am->matrix = (void **)am->matrix_## type;
531
532     switch (am->coeff_type) {
533     case AV_MIX_COEFF_TYPE_Q8:
534         CONVERT_MATRIX(q8, av_clip_int16(lrint(256.0 * v)))
535         break;
536     case AV_MIX_COEFF_TYPE_Q15:
537         CONVERT_MATRIX(q15, av_clipl_int32(llrint(32768.0 * v)))
538         break;
539     case AV_MIX_COEFF_TYPE_FLT:
540         CONVERT_MATRIX(flt, v)
541         break;
542     default:
543         av_log(am, AV_LOG_ERROR, "Invalid mix coeff type\n");
544         return AVERROR(EINVAL);
545     }
546
547     /* TODO: detect situations where we can just swap around pointers
548              instead of doing matrix multiplications with 0.0 and 1.0 */
549
550     return 0;
551 }