Add experimental support for ambisonic encoding
[opus.git] / src / opus_multistream_encoder.c
1 /* Copyright (c) 2011 Xiph.Org Foundation
2    Written by Jean-Marc Valin */
3 /*
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14
15    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
16    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
17    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
18    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
19    OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
20    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
21    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 #include "opus_multistream.h"
33 #include "opus.h"
34 #include "opus_private.h"
35 #include "stack_alloc.h"
36 #include <stdarg.h>
37 #include "float_cast.h"
38 #include "os_support.h"
39 #include "mathops.h"
40 #include "mdct.h"
41 #include "modes.h"
42 #include "bands.h"
43 #include "quant_bands.h"
44 #include "pitch.h"
45
46 typedef struct {
47    int nb_streams;
48    int nb_coupled_streams;
49    unsigned char mapping[8];
50 } VorbisLayout;
51
52 /* Index is nb_channel-1*/
53 static const VorbisLayout vorbis_mappings[8] = {
54       {1, 0, {0}},                      /* 1: mono */
55       {1, 1, {0, 1}},                   /* 2: stereo */
56       {2, 1, {0, 2, 1}},                /* 3: 1-d surround */
57       {2, 2, {0, 1, 2, 3}},             /* 4: quadraphonic surround */
58       {3, 2, {0, 4, 1, 2, 3}},          /* 5: 5-channel surround */
59       {4, 2, {0, 4, 1, 2, 3, 5}},       /* 6: 5.1 surround */
60       {4, 3, {0, 4, 1, 2, 3, 5, 6}},    /* 7: 6.1 surround */
61       {5, 3, {0, 6, 1, 2, 3, 4, 5, 7}}, /* 8: 7.1 surround */
62 };
63
64 typedef void (*opus_copy_channel_in_func)(
65   opus_val16 *dst,
66   int dst_stride,
67   const void *src,
68   int src_stride,
69   int src_channel,
70   int frame_size
71 );
72
73 typedef enum {
74   MAPPING_TYPE_NONE,
75   MAPPING_TYPE_SURROUND,
76 #ifdef ENABLE_EXPERIMENTAL_AMBISONICS
77   MAPPING_TYPE_AMBISONICS,
78 #endif
79 } MappingType;
80
81 struct OpusMSEncoder {
82    ChannelLayout layout;
83    int arch;
84    int lfe_stream;
85    int application;
86    int variable_duration;
87    MappingType mapping_type;
88    opus_int32 bitrate_bps;
89    float subframe_mem[3];
90    /* Encoder states go here */
91    /* then opus_val32 window_mem[channels*120]; */
92    /* then opus_val32 preemph_mem[channels]; */
93 };
94
95 static opus_val32 *ms_get_preemph_mem(OpusMSEncoder *st)
96 {
97    int s;
98    char *ptr;
99    int coupled_size, mono_size;
100
101    coupled_size = opus_encoder_get_size(2);
102    mono_size = opus_encoder_get_size(1);
103    ptr = (char*)st + align(sizeof(OpusMSEncoder));
104    for (s=0;s<st->layout.nb_streams;s++)
105    {
106       if (s < st->layout.nb_coupled_streams)
107          ptr += align(coupled_size);
108       else
109          ptr += align(mono_size);
110    }
111    /* void* cast avoids clang -Wcast-align warning */
112    return (opus_val32*)(void*)(ptr+st->layout.nb_channels*120*sizeof(opus_val32));
113 }
114
115 static opus_val32 *ms_get_window_mem(OpusMSEncoder *st)
116 {
117    int s;
118    char *ptr;
119    int coupled_size, mono_size;
120
121    coupled_size = opus_encoder_get_size(2);
122    mono_size = opus_encoder_get_size(1);
123    ptr = (char*)st + align(sizeof(OpusMSEncoder));
124    for (s=0;s<st->layout.nb_streams;s++)
125    {
126       if (s < st->layout.nb_coupled_streams)
127          ptr += align(coupled_size);
128       else
129          ptr += align(mono_size);
130    }
131    /* void* cast avoids clang -Wcast-align warning */
132    return (opus_val32*)(void*)ptr;
133 }
134
135 static int validate_encoder_layout(const ChannelLayout *layout)
136 {
137    int s;
138    for (s=0;s<layout->nb_streams;s++)
139    {
140       if (s < layout->nb_coupled_streams)
141       {
142          if (get_left_channel(layout, s, -1)==-1)
143             return 0;
144          if (get_right_channel(layout, s, -1)==-1)
145             return 0;
146       } else {
147          if (get_mono_channel(layout, s, -1)==-1)
148             return 0;
149       }
150    }
151    return 1;
152 }
153
154 static void channel_pos(int channels, int pos[8])
155 {
156    /* Position in the mix: 0 don't mix, 1: left, 2: center, 3:right */
157    if (channels==4)
158    {
159       pos[0]=1;
160       pos[1]=3;
161       pos[2]=1;
162       pos[3]=3;
163    } else if (channels==3||channels==5||channels==6)
164    {
165       pos[0]=1;
166       pos[1]=2;
167       pos[2]=3;
168       pos[3]=1;
169       pos[4]=3;
170       pos[5]=0;
171    } else if (channels==7)
172    {
173       pos[0]=1;
174       pos[1]=2;
175       pos[2]=3;
176       pos[3]=1;
177       pos[4]=3;
178       pos[5]=2;
179       pos[6]=0;
180    } else if (channels==8)
181    {
182       pos[0]=1;
183       pos[1]=2;
184       pos[2]=3;
185       pos[3]=1;
186       pos[4]=3;
187       pos[5]=1;
188       pos[6]=3;
189       pos[7]=0;
190    }
191 }
192
193 #if 1
194 /* Computes a rough approximation of log2(2^a + 2^b) */
195 static opus_val16 logSum(opus_val16 a, opus_val16 b)
196 {
197    opus_val16 max;
198    opus_val32 diff;
199    opus_val16 frac;
200    static const opus_val16 diff_table[17] = {
201          QCONST16(0.5000000f, DB_SHIFT), QCONST16(0.2924813f, DB_SHIFT), QCONST16(0.1609640f, DB_SHIFT), QCONST16(0.0849625f, DB_SHIFT),
202          QCONST16(0.0437314f, DB_SHIFT), QCONST16(0.0221971f, DB_SHIFT), QCONST16(0.0111839f, DB_SHIFT), QCONST16(0.0056136f, DB_SHIFT),
203          QCONST16(0.0028123f, DB_SHIFT)
204    };
205    int low;
206    if (a>b)
207    {
208       max = a;
209       diff = SUB32(EXTEND32(a),EXTEND32(b));
210    } else {
211       max = b;
212       diff = SUB32(EXTEND32(b),EXTEND32(a));
213    }
214    if (!(diff < QCONST16(8.f, DB_SHIFT)))  /* inverted to catch NaNs */
215       return max;
216 #ifdef FIXED_POINT
217    low = SHR32(diff, DB_SHIFT-1);
218    frac = SHL16(diff - SHL16(low, DB_SHIFT-1), 16-DB_SHIFT);
219 #else
220    low = (int)floor(2*diff);
221    frac = 2*diff - low;
222 #endif
223    return max + diff_table[low] + MULT16_16_Q15(frac, SUB16(diff_table[low+1], diff_table[low]));
224 }
225 #else
226 opus_val16 logSum(opus_val16 a, opus_val16 b)
227 {
228    return log2(pow(4, a)+ pow(4, b))/2;
229 }
230 #endif
231
232 void surround_analysis(const CELTMode *celt_mode, const void *pcm, opus_val16 *bandLogE, opus_val32 *mem, opus_val32 *preemph_mem,
233       int len, int overlap, int channels, int rate, opus_copy_channel_in_func copy_channel_in, int arch
234 )
235 {
236    int c;
237    int i;
238    int LM;
239    int pos[8] = {0};
240    int upsample;
241    int frame_size;
242    opus_val16 channel_offset;
243    opus_val32 bandE[21];
244    opus_val16 maskLogE[3][21];
245    VARDECL(opus_val32, in);
246    VARDECL(opus_val16, x);
247    VARDECL(opus_val32, freq);
248    SAVE_STACK;
249
250    upsample = resampling_factor(rate);
251    frame_size = len*upsample;
252
253    /* LM = log2(frame_size / 120) */
254    for (LM=0;LM<celt_mode->maxLM;LM++)
255       if (celt_mode->shortMdctSize<<LM==frame_size)
256          break;
257
258    ALLOC(in, frame_size+overlap, opus_val32);
259    ALLOC(x, len, opus_val16);
260    ALLOC(freq, frame_size, opus_val32);
261
262    channel_pos(channels, pos);
263
264    for (c=0;c<3;c++)
265       for (i=0;i<21;i++)
266          maskLogE[c][i] = -QCONST16(28.f, DB_SHIFT);
267
268    for (c=0;c<channels;c++)
269    {
270       OPUS_COPY(in, mem+c*overlap, overlap);
271       (*copy_channel_in)(x, 1, pcm, channels, c, len);
272       celt_preemphasis(x, in+overlap, frame_size, 1, upsample, celt_mode->preemph, preemph_mem+c, 0);
273 #ifndef FIXED_POINT
274       {
275          opus_val32 sum;
276          sum = celt_inner_prod(in, in, frame_size+overlap, 0);
277          /* This should filter out both NaNs and ridiculous signals that could
278             cause NaNs further down. */
279          if (!(sum < 1e9f) || celt_isnan(sum))
280          {
281             OPUS_CLEAR(in, frame_size+overlap);
282             preemph_mem[c] = 0;
283          }
284       }
285 #endif
286       clt_mdct_forward(&celt_mode->mdct, in, freq, celt_mode->window,
287             overlap, celt_mode->maxLM-LM, 1, arch);
288       if (upsample != 1)
289       {
290          int bound = len;
291          for (i=0;i<bound;i++)
292             freq[i] *= upsample;
293          for (;i<frame_size;i++)
294             freq[i] = 0;
295       }
296
297       compute_band_energies(celt_mode, freq, bandE, 21, 1, LM);
298       amp2Log2(celt_mode, 21, 21, bandE, bandLogE+21*c, 1);
299       /* Apply spreading function with -6 dB/band going up and -12 dB/band going down. */
300       for (i=1;i<21;i++)
301          bandLogE[21*c+i] = MAX16(bandLogE[21*c+i], bandLogE[21*c+i-1]-QCONST16(1.f, DB_SHIFT));
302       for (i=19;i>=0;i--)
303          bandLogE[21*c+i] = MAX16(bandLogE[21*c+i], bandLogE[21*c+i+1]-QCONST16(2.f, DB_SHIFT));
304       if (pos[c]==1)
305       {
306          for (i=0;i<21;i++)
307             maskLogE[0][i] = logSum(maskLogE[0][i], bandLogE[21*c+i]);
308       } else if (pos[c]==3)
309       {
310          for (i=0;i<21;i++)
311             maskLogE[2][i] = logSum(maskLogE[2][i], bandLogE[21*c+i]);
312       } else if (pos[c]==2)
313       {
314          for (i=0;i<21;i++)
315          {
316             maskLogE[0][i] = logSum(maskLogE[0][i], bandLogE[21*c+i]-QCONST16(.5f, DB_SHIFT));
317             maskLogE[2][i] = logSum(maskLogE[2][i], bandLogE[21*c+i]-QCONST16(.5f, DB_SHIFT));
318          }
319       }
320 #if 0
321       for (i=0;i<21;i++)
322          printf("%f ", bandLogE[21*c+i]);
323       float sum=0;
324       for (i=0;i<21;i++)
325          sum += bandLogE[21*c+i];
326       printf("%f ", sum/21);
327 #endif
328       OPUS_COPY(mem+c*overlap, in+frame_size, overlap);
329    }
330    for (i=0;i<21;i++)
331       maskLogE[1][i] = MIN32(maskLogE[0][i],maskLogE[2][i]);
332    channel_offset = HALF16(celt_log2(QCONST32(2.f,14)/(channels-1)));
333    for (c=0;c<3;c++)
334       for (i=0;i<21;i++)
335          maskLogE[c][i] += channel_offset;
336 #if 0
337    for (c=0;c<3;c++)
338    {
339       for (i=0;i<21;i++)
340          printf("%f ", maskLogE[c][i]);
341    }
342 #endif
343    for (c=0;c<channels;c++)
344    {
345       opus_val16 *mask;
346       if (pos[c]!=0)
347       {
348          mask = &maskLogE[pos[c]-1][0];
349          for (i=0;i<21;i++)
350             bandLogE[21*c+i] = bandLogE[21*c+i] - mask[i];
351       } else {
352          for (i=0;i<21;i++)
353             bandLogE[21*c+i] = 0;
354       }
355 #if 0
356       for (i=0;i<21;i++)
357          printf("%f ", bandLogE[21*c+i]);
358       printf("\n");
359 #endif
360 #if 0
361       float sum=0;
362       for (i=0;i<21;i++)
363          sum += bandLogE[21*c+i];
364       printf("%f ", sum/(float)QCONST32(21.f, DB_SHIFT));
365       printf("\n");
366 #endif
367    }
368    RESTORE_STACK;
369 }
370
371 opus_int32 opus_multistream_encoder_get_size(int nb_streams, int nb_coupled_streams)
372 {
373    int coupled_size;
374    int mono_size;
375
376    if(nb_streams<1||nb_coupled_streams>nb_streams||nb_coupled_streams<0)return 0;
377    coupled_size = opus_encoder_get_size(2);
378    mono_size = opus_encoder_get_size(1);
379    return align(sizeof(OpusMSEncoder))
380         + nb_coupled_streams * align(coupled_size)
381         + (nb_streams-nb_coupled_streams) * align(mono_size);
382 }
383
384 opus_int32 opus_multistream_surround_encoder_get_size(int channels, int mapping_family)
385 {
386    int nb_streams;
387    int nb_coupled_streams;
388    opus_int32 size;
389
390    if (mapping_family==0)
391    {
392       if (channels==1)
393       {
394          nb_streams=1;
395          nb_coupled_streams=0;
396       } else if (channels==2)
397       {
398          nb_streams=1;
399          nb_coupled_streams=1;
400       } else
401          return 0;
402    } else if (mapping_family==1 && channels<=8 && channels>=1)
403    {
404       nb_streams=vorbis_mappings[channels-1].nb_streams;
405       nb_coupled_streams=vorbis_mappings[channels-1].nb_coupled_streams;
406    } else if (mapping_family==255)
407    {
408       nb_streams=channels;
409       nb_coupled_streams=0;
410 #ifdef ENABLE_EXPERIMENTAL_AMBISONICS
411    } else if (mapping_family==254)
412    {
413       nb_streams=channels;
414       nb_coupled_streams=0;
415 #endif
416    } else
417       return 0;
418    size = opus_multistream_encoder_get_size(nb_streams, nb_coupled_streams);
419    if (channels>2)
420    {
421       size += channels*(120*sizeof(opus_val32) + sizeof(opus_val32));
422    }
423    return size;
424 }
425
426 static int opus_multistream_encoder_init_impl(
427       OpusMSEncoder *st,
428       opus_int32 Fs,
429       int channels,
430       int streams,
431       int coupled_streams,
432       const unsigned char *mapping,
433       int application,
434       MappingType mapping_type
435 )
436 {
437    int coupled_size;
438    int mono_size;
439    int i, ret;
440    char *ptr;
441
442    if ((channels>255) || (channels<1) || (coupled_streams>streams) ||
443        (streams<1) || (coupled_streams<0) || (streams>255-coupled_streams))
444       return OPUS_BAD_ARG;
445
446    st->arch = opus_select_arch();
447    st->layout.nb_channels = channels;
448    st->layout.nb_streams = streams;
449    st->layout.nb_coupled_streams = coupled_streams;
450    st->subframe_mem[0]=st->subframe_mem[1]=st->subframe_mem[2]=0;
451    if (mapping_type != MAPPING_TYPE_SURROUND)
452       st->lfe_stream = -1;
453    st->bitrate_bps = OPUS_AUTO;
454    st->application = application;
455    st->variable_duration = OPUS_FRAMESIZE_ARG;
456    for (i=0;i<st->layout.nb_channels;i++)
457       st->layout.mapping[i] = mapping[i];
458    if (!validate_layout(&st->layout) || !validate_encoder_layout(&st->layout))
459       return OPUS_BAD_ARG;
460    ptr = (char*)st + align(sizeof(OpusMSEncoder));
461    coupled_size = opus_encoder_get_size(2);
462    mono_size = opus_encoder_get_size(1);
463
464    for (i=0;i<st->layout.nb_coupled_streams;i++)
465    {
466       ret = opus_encoder_init((OpusEncoder*)ptr, Fs, 2, application);
467       if(ret!=OPUS_OK)return ret;
468       if (i==st->lfe_stream)
469          opus_encoder_ctl((OpusEncoder*)ptr, OPUS_SET_LFE(1));
470       ptr += align(coupled_size);
471    }
472    for (;i<st->layout.nb_streams;i++)
473    {
474       ret = opus_encoder_init((OpusEncoder*)ptr, Fs, 1, application);
475       if (i==st->lfe_stream)
476          opus_encoder_ctl((OpusEncoder*)ptr, OPUS_SET_LFE(1));
477       if(ret!=OPUS_OK)return ret;
478       ptr += align(mono_size);
479    }
480    if (mapping_type == MAPPING_TYPE_SURROUND)
481    {
482       OPUS_CLEAR(ms_get_preemph_mem(st), channels);
483       OPUS_CLEAR(ms_get_window_mem(st), channels*120);
484    }
485    st->mapping_type = mapping_type;
486    return OPUS_OK;
487 }
488
489 int opus_multistream_encoder_init(
490       OpusMSEncoder *st,
491       opus_int32 Fs,
492       int channels,
493       int streams,
494       int coupled_streams,
495       const unsigned char *mapping,
496       int application
497 )
498 {
499    return opus_multistream_encoder_init_impl(st, Fs, channels, streams,
500                                              coupled_streams, mapping,
501                                              application, MAPPING_TYPE_NONE);
502 }
503
504 int opus_multistream_surround_encoder_init(
505       OpusMSEncoder *st,
506       opus_int32 Fs,
507       int channels,
508       int mapping_family,
509       int *streams,
510       int *coupled_streams,
511       unsigned char *mapping,
512       int application
513 )
514 {
515    int mapping_type;
516    if ((channels>255) || (channels<1))
517       return OPUS_BAD_ARG;
518    st->lfe_stream = -1;
519    if (mapping_family==0)
520    {
521       if (channels==1)
522       {
523          *streams=1;
524          *coupled_streams=0;
525          mapping[0]=0;
526       } else if (channels==2)
527       {
528          *streams=1;
529          *coupled_streams=1;
530          mapping[0]=0;
531          mapping[1]=1;
532       } else
533          return OPUS_UNIMPLEMENTED;
534    } else if (mapping_family==1 && channels<=8 && channels>=1)
535    {
536       int i;
537       *streams=vorbis_mappings[channels-1].nb_streams;
538       *coupled_streams=vorbis_mappings[channels-1].nb_coupled_streams;
539       for (i=0;i<channels;i++)
540          mapping[i] = vorbis_mappings[channels-1].mapping[i];
541       if (channels>=6)
542          st->lfe_stream = *streams-1;
543    } else if (mapping_family==255)
544    {
545       int i;
546       *streams=channels;
547       *coupled_streams=0;
548       for(i=0;i<channels;i++)
549          mapping[i] = i;
550 #ifdef ENABLE_EXPERIMENTAL_AMBISONICS
551    } else if (mapping_family==254)
552    {
553       int i;
554       *streams=channels;
555       *coupled_streams=0;
556       for(i=0;i<channels;i++)
557          mapping[i] = i;
558 #endif
559    } else
560       return OPUS_UNIMPLEMENTED;
561
562    if (channels>2 && mapping_family==1) {
563       mapping_type = MAPPING_TYPE_SURROUND;
564 #ifdef ENABLE_EXPERIMENTAL_AMBISONICS
565    } else if (mapping_family==254)
566    {
567       mapping_type = MAPPING_TYPE_AMBISONICS;
568 #endif
569    } else
570    {
571       mapping_type = MAPPING_TYPE_NONE;
572    }
573    return opus_multistream_encoder_init_impl(st, Fs, channels, *streams,
574                                              *coupled_streams, mapping,
575                                              application, mapping_type);
576 }
577
578 OpusMSEncoder *opus_multistream_encoder_create(
579       opus_int32 Fs,
580       int channels,
581       int streams,
582       int coupled_streams,
583       const unsigned char *mapping,
584       int application,
585       int *error
586 )
587 {
588    int ret;
589    OpusMSEncoder *st;
590    if ((channels>255) || (channels<1) || (coupled_streams>streams) ||
591        (streams<1) || (coupled_streams<0) || (streams>255-coupled_streams))
592    {
593       if (error)
594          *error = OPUS_BAD_ARG;
595       return NULL;
596    }
597    st = (OpusMSEncoder *)opus_alloc(opus_multistream_encoder_get_size(streams, coupled_streams));
598    if (st==NULL)
599    {
600       if (error)
601          *error = OPUS_ALLOC_FAIL;
602       return NULL;
603    }
604    ret = opus_multistream_encoder_init(st, Fs, channels, streams, coupled_streams, mapping, application);
605    if (ret != OPUS_OK)
606    {
607       opus_free(st);
608       st = NULL;
609    }
610    if (error)
611       *error = ret;
612    return st;
613 }
614
615 OpusMSEncoder *opus_multistream_surround_encoder_create(
616       opus_int32 Fs,
617       int channels,
618       int mapping_family,
619       int *streams,
620       int *coupled_streams,
621       unsigned char *mapping,
622       int application,
623       int *error
624 )
625 {
626    int ret;
627    opus_int32 size;
628    OpusMSEncoder *st;
629    if ((channels>255) || (channels<1))
630    {
631       if (error)
632          *error = OPUS_BAD_ARG;
633       return NULL;
634    }
635    size = opus_multistream_surround_encoder_get_size(channels, mapping_family);
636    if (!size)
637    {
638       if (error)
639          *error = OPUS_UNIMPLEMENTED;
640       return NULL;
641    }
642    st = (OpusMSEncoder *)opus_alloc(size);
643    if (st==NULL)
644    {
645       if (error)
646          *error = OPUS_ALLOC_FAIL;
647       return NULL;
648    }
649    ret = opus_multistream_surround_encoder_init(st, Fs, channels, mapping_family, streams, coupled_streams, mapping, application);
650    if (ret != OPUS_OK)
651    {
652       opus_free(st);
653       st = NULL;
654    }
655    if (error)
656       *error = ret;
657    return st;
658 }
659
660 static opus_int32 surround_rate_allocation(
661       OpusMSEncoder *st,
662       opus_int32 *rate,
663       int frame_size
664       )
665 {
666    int i;
667    opus_int32 channel_rate;
668    opus_int32 Fs;
669    char *ptr;
670    int stream_offset;
671    int lfe_offset;
672    int coupled_ratio; /* Q8 */
673    int lfe_ratio;     /* Q8 */
674    opus_int32 rate_sum=0;
675
676    ptr = (char*)st + align(sizeof(OpusMSEncoder));
677    opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_SAMPLE_RATE(&Fs));
678
679    if (st->bitrate_bps > st->layout.nb_channels*40000)
680       stream_offset = 20000;
681    else
682       stream_offset = st->bitrate_bps/st->layout.nb_channels/2;
683    stream_offset += 60*(Fs/frame_size-50);
684    /* We start by giving each stream (coupled or uncoupled) the same bitrate.
685       This models the main saving of coupled channels over uncoupled. */
686    /* The LFE stream is an exception to the above and gets fewer bits. */
687    lfe_offset = 3500 + 60*(Fs/frame_size-50);
688    /* Coupled streams get twice the mono rate after the first 20 kb/s. */
689    coupled_ratio = 512;
690    /* Should depend on the bitrate, for now we assume LFE gets 1/8 the bits of mono */
691    lfe_ratio = 32;
692
693    /* Compute bitrate allocation between streams */
694    if (st->bitrate_bps==OPUS_AUTO)
695    {
696       channel_rate = Fs+60*Fs/frame_size;
697    } else if (st->bitrate_bps==OPUS_BITRATE_MAX)
698    {
699       channel_rate = 300000;
700    } else {
701       int nb_lfe;
702       int nb_uncoupled;
703       int nb_coupled;
704       int total;
705       nb_lfe = (st->lfe_stream!=-1);
706       nb_coupled = st->layout.nb_coupled_streams;
707       nb_uncoupled = st->layout.nb_streams-nb_coupled-nb_lfe;
708       total = (nb_uncoupled<<8)         /* mono */
709             + coupled_ratio*nb_coupled /* stereo */
710             + nb_lfe*lfe_ratio;
711       channel_rate = 256*(st->bitrate_bps-lfe_offset*nb_lfe-stream_offset*(nb_coupled+nb_uncoupled))/total;
712    }
713 #ifndef FIXED_POINT
714    if (st->variable_duration==OPUS_FRAMESIZE_VARIABLE && frame_size != Fs/50)
715    {
716       opus_int32 bonus;
717       bonus = 60*(Fs/frame_size-50);
718       channel_rate += bonus;
719    }
720 #endif
721
722    for (i=0;i<st->layout.nb_streams;i++)
723    {
724       if (i<st->layout.nb_coupled_streams)
725          rate[i] = stream_offset+(channel_rate*coupled_ratio>>8);
726       else if (i!=st->lfe_stream)
727          rate[i] = stream_offset+channel_rate;
728       else
729          rate[i] = lfe_offset+(channel_rate*lfe_ratio>>8);
730       rate[i] = IMAX(rate[i], 500);
731       rate_sum += rate[i];
732    }
733    return rate_sum;
734 }
735
736 /* Max size in case the encoder decides to return three frames */
737 #define MS_FRAME_TMP (3*1275+7)
738 static int opus_multistream_encode_native
739 (
740     OpusMSEncoder *st,
741     opus_copy_channel_in_func copy_channel_in,
742     const void *pcm,
743     int analysis_frame_size,
744     unsigned char *data,
745     opus_int32 max_data_bytes,
746     int lsb_depth,
747     downmix_func downmix,
748     int float_api
749 )
750 {
751    opus_int32 Fs;
752    int coupled_size;
753    int mono_size;
754    int s;
755    char *ptr;
756    int tot_size;
757    VARDECL(opus_val16, buf);
758    VARDECL(opus_val16, bandSMR);
759    unsigned char tmp_data[MS_FRAME_TMP];
760    OpusRepacketizer rp;
761    opus_int32 vbr;
762    const CELTMode *celt_mode;
763    opus_int32 bitrates[256];
764    opus_val16 bandLogE[42];
765    opus_val32 *mem = NULL;
766    opus_val32 *preemph_mem=NULL;
767    int frame_size;
768    opus_int32 rate_sum;
769    opus_int32 smallest_packet;
770    ALLOC_STACK;
771
772    if (st->mapping_type == MAPPING_TYPE_SURROUND)
773    {
774       preemph_mem = ms_get_preemph_mem(st);
775       mem = ms_get_window_mem(st);
776    }
777
778    ptr = (char*)st + align(sizeof(OpusMSEncoder));
779    opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_SAMPLE_RATE(&Fs));
780    opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_VBR(&vbr));
781    opus_encoder_ctl((OpusEncoder*)ptr, CELT_GET_MODE(&celt_mode));
782
783    {
784       opus_int32 delay_compensation;
785       int channels;
786
787       channels = st->layout.nb_streams + st->layout.nb_coupled_streams;
788       opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_LOOKAHEAD(&delay_compensation));
789       delay_compensation -= Fs/400;
790       frame_size = compute_frame_size(pcm, analysis_frame_size,
791             st->variable_duration, channels, Fs, st->bitrate_bps,
792             delay_compensation, downmix
793 #ifndef DISABLE_FLOAT_API
794             , st->subframe_mem
795 #endif
796             );
797    }
798
799    if (400*frame_size < Fs)
800    {
801       RESTORE_STACK;
802       return OPUS_BAD_ARG;
803    }
804    /* Validate frame_size before using it to allocate stack space.
805       This mirrors the checks in opus_encode[_float](). */
806    if (400*frame_size != Fs && 200*frame_size != Fs &&
807        100*frame_size != Fs &&  50*frame_size != Fs &&
808         25*frame_size != Fs &&  50*frame_size != 3*Fs)
809    {
810       RESTORE_STACK;
811       return OPUS_BAD_ARG;
812    }
813
814    /* Smallest packet the encoder can produce. */
815    smallest_packet = st->layout.nb_streams*2-1;
816    if (max_data_bytes < smallest_packet)
817    {
818       RESTORE_STACK;
819       return OPUS_BUFFER_TOO_SMALL;
820    }
821    ALLOC(buf, 2*frame_size, opus_val16);
822    coupled_size = opus_encoder_get_size(2);
823    mono_size = opus_encoder_get_size(1);
824
825    ALLOC(bandSMR, 21*st->layout.nb_channels, opus_val16);
826    if (st->mapping_type == MAPPING_TYPE_SURROUND)
827    {
828       surround_analysis(celt_mode, pcm, bandSMR, mem, preemph_mem, frame_size, 120, st->layout.nb_channels, Fs, copy_channel_in, st->arch);
829    }
830
831    /* Compute bitrate allocation between streams (this could be a lot better) */
832    rate_sum = surround_rate_allocation(st, bitrates, frame_size);
833
834    if (!vbr)
835    {
836       if (st->bitrate_bps == OPUS_AUTO)
837       {
838          max_data_bytes = IMIN(max_data_bytes, 3*rate_sum/(3*8*Fs/frame_size));
839       } else if (st->bitrate_bps != OPUS_BITRATE_MAX)
840       {
841          max_data_bytes = IMIN(max_data_bytes, IMAX(smallest_packet,
842                           3*st->bitrate_bps/(3*8*Fs/frame_size)));
843       }
844    }
845    ptr = (char*)st + align(sizeof(OpusMSEncoder));
846    for (s=0;s<st->layout.nb_streams;s++)
847    {
848       OpusEncoder *enc;
849       enc = (OpusEncoder*)ptr;
850       if (s < st->layout.nb_coupled_streams)
851          ptr += align(coupled_size);
852       else
853          ptr += align(mono_size);
854       opus_encoder_ctl(enc, OPUS_SET_BITRATE(bitrates[s]));
855       if (st->mapping_type == MAPPING_TYPE_SURROUND)
856       {
857          opus_int32 equiv_rate;
858          equiv_rate = st->bitrate_bps;
859          if (frame_size*50 < Fs)
860             equiv_rate -= 60*(Fs/frame_size - 50)*st->layout.nb_channels;
861          if (equiv_rate > 10000*st->layout.nb_channels)
862             opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_FULLBAND));
863          else if (equiv_rate > 7000*st->layout.nb_channels)
864             opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_SUPERWIDEBAND));
865          else if (equiv_rate > 5000*st->layout.nb_channels)
866             opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_WIDEBAND));
867          else
868             opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_NARROWBAND));
869          if (s < st->layout.nb_coupled_streams)
870          {
871             /* To preserve the spatial image, force stereo CELT on coupled streams */
872             opus_encoder_ctl(enc, OPUS_SET_FORCE_MODE(MODE_CELT_ONLY));
873             opus_encoder_ctl(enc, OPUS_SET_FORCE_CHANNELS(2));
874          }
875       }
876    }
877
878    ptr = (char*)st + align(sizeof(OpusMSEncoder));
879    /* Counting ToC */
880    tot_size = 0;
881    for (s=0;s<st->layout.nb_streams;s++)
882    {
883       OpusEncoder *enc;
884       int len;
885       int curr_max;
886       int c1, c2;
887
888       opus_repacketizer_init(&rp);
889       enc = (OpusEncoder*)ptr;
890       if (s < st->layout.nb_coupled_streams)
891       {
892          int i;
893          int left, right;
894          left = get_left_channel(&st->layout, s, -1);
895          right = get_right_channel(&st->layout, s, -1);
896          (*copy_channel_in)(buf, 2,
897             pcm, st->layout.nb_channels, left, frame_size);
898          (*copy_channel_in)(buf+1, 2,
899             pcm, st->layout.nb_channels, right, frame_size);
900          ptr += align(coupled_size);
901          if (st->mapping_type == MAPPING_TYPE_SURROUND)
902          {
903             for (i=0;i<21;i++)
904             {
905                bandLogE[i] = bandSMR[21*left+i];
906                bandLogE[21+i] = bandSMR[21*right+i];
907             }
908          }
909          c1 = left;
910          c2 = right;
911       } else {
912          int i;
913          int chan = get_mono_channel(&st->layout, s, -1);
914          (*copy_channel_in)(buf, 1,
915             pcm, st->layout.nb_channels, chan, frame_size);
916          ptr += align(mono_size);
917          if (st->mapping_type == MAPPING_TYPE_SURROUND)
918          {
919             for (i=0;i<21;i++)
920                bandLogE[i] = bandSMR[21*chan+i];
921          }
922          c1 = chan;
923          c2 = -1;
924       }
925       if (st->mapping_type == MAPPING_TYPE_SURROUND)
926          opus_encoder_ctl(enc, OPUS_SET_ENERGY_MASK(bandLogE));
927       /* number of bytes left (+Toc) */
928       curr_max = max_data_bytes - tot_size;
929       /* Reserve one byte for the last stream and two for the others */
930       curr_max -= IMAX(0,2*(st->layout.nb_streams-s-1)-1);
931       curr_max = IMIN(curr_max,MS_FRAME_TMP);
932       /* Repacketizer will add one or two bytes for self-delimited frames */
933       if (s != st->layout.nb_streams-1) curr_max -=  curr_max>253 ? 2 : 1;
934       if (!vbr && s == st->layout.nb_streams-1)
935          opus_encoder_ctl(enc, OPUS_SET_BITRATE(curr_max*(8*Fs/frame_size)));
936       len = opus_encode_native(enc, buf, frame_size, tmp_data, curr_max, lsb_depth,
937             pcm, analysis_frame_size, c1, c2, st->layout.nb_channels, downmix, float_api);
938       if (len<0)
939       {
940          RESTORE_STACK;
941          return len;
942       }
943       /* We need to use the repacketizer to add the self-delimiting lengths
944          while taking into account the fact that the encoder can now return
945          more than one frame at a time (e.g. 60 ms CELT-only) */
946       opus_repacketizer_cat(&rp, tmp_data, len);
947       len = opus_repacketizer_out_range_impl(&rp, 0, opus_repacketizer_get_nb_frames(&rp),
948             data, max_data_bytes-tot_size, s != st->layout.nb_streams-1, !vbr && s == st->layout.nb_streams-1);
949       data += len;
950       tot_size += len;
951    }
952    /*printf("\n");*/
953    RESTORE_STACK;
954    return tot_size;
955 }
956
957 #if !defined(DISABLE_FLOAT_API)
958 static void opus_copy_channel_in_float(
959   opus_val16 *dst,
960   int dst_stride,
961   const void *src,
962   int src_stride,
963   int src_channel,
964   int frame_size
965 )
966 {
967    const float *float_src;
968    opus_int32 i;
969    float_src = (const float *)src;
970    for (i=0;i<frame_size;i++)
971 #if defined(FIXED_POINT)
972       dst[i*dst_stride] = FLOAT2INT16(float_src[i*src_stride+src_channel]);
973 #else
974       dst[i*dst_stride] = float_src[i*src_stride+src_channel];
975 #endif
976 }
977 #endif
978
979 static void opus_copy_channel_in_short(
980   opus_val16 *dst,
981   int dst_stride,
982   const void *src,
983   int src_stride,
984   int src_channel,
985   int frame_size
986 )
987 {
988    const opus_int16 *short_src;
989    opus_int32 i;
990    short_src = (const opus_int16 *)src;
991    for (i=0;i<frame_size;i++)
992 #if defined(FIXED_POINT)
993       dst[i*dst_stride] = short_src[i*src_stride+src_channel];
994 #else
995       dst[i*dst_stride] = (1/32768.f)*short_src[i*src_stride+src_channel];
996 #endif
997 }
998
999
1000 #ifdef FIXED_POINT
1001 int opus_multistream_encode(
1002     OpusMSEncoder *st,
1003     const opus_val16 *pcm,
1004     int frame_size,
1005     unsigned char *data,
1006     opus_int32 max_data_bytes
1007 )
1008 {
1009    return opus_multistream_encode_native(st, opus_copy_channel_in_short,
1010       pcm, frame_size, data, max_data_bytes, 16, downmix_int, 0);
1011 }
1012
1013 #ifndef DISABLE_FLOAT_API
1014 int opus_multistream_encode_float(
1015     OpusMSEncoder *st,
1016     const float *pcm,
1017     int frame_size,
1018     unsigned char *data,
1019     opus_int32 max_data_bytes
1020 )
1021 {
1022    return opus_multistream_encode_native(st, opus_copy_channel_in_float,
1023       pcm, frame_size, data, max_data_bytes, 16, downmix_float, 1);
1024 }
1025 #endif
1026
1027 #else
1028
1029 int opus_multistream_encode_float
1030 (
1031     OpusMSEncoder *st,
1032     const opus_val16 *pcm,
1033     int frame_size,
1034     unsigned char *data,
1035     opus_int32 max_data_bytes
1036 )
1037 {
1038    return opus_multistream_encode_native(st, opus_copy_channel_in_float,
1039       pcm, frame_size, data, max_data_bytes, 24, downmix_float, 1);
1040 }
1041
1042 int opus_multistream_encode(
1043     OpusMSEncoder *st,
1044     const opus_int16 *pcm,
1045     int frame_size,
1046     unsigned char *data,
1047     opus_int32 max_data_bytes
1048 )
1049 {
1050    return opus_multistream_encode_native(st, opus_copy_channel_in_short,
1051       pcm, frame_size, data, max_data_bytes, 16, downmix_int, 0);
1052 }
1053 #endif
1054
1055 int opus_multistream_encoder_ctl(OpusMSEncoder *st, int request, ...)
1056 {
1057    va_list ap;
1058    int coupled_size, mono_size;
1059    char *ptr;
1060    int ret = OPUS_OK;
1061
1062    va_start(ap, request);
1063
1064    coupled_size = opus_encoder_get_size(2);
1065    mono_size = opus_encoder_get_size(1);
1066    ptr = (char*)st + align(sizeof(OpusMSEncoder));
1067    switch (request)
1068    {
1069    case OPUS_SET_BITRATE_REQUEST:
1070    {
1071       opus_int32 value = va_arg(ap, opus_int32);
1072       if (value<0 && value!=OPUS_AUTO && value!=OPUS_BITRATE_MAX)
1073       {
1074          goto bad_arg;
1075       }
1076       st->bitrate_bps = value;
1077    }
1078    break;
1079    case OPUS_GET_BITRATE_REQUEST:
1080    {
1081       int s;
1082       opus_int32 *value = va_arg(ap, opus_int32*);
1083       if (!value)
1084       {
1085          goto bad_arg;
1086       }
1087       *value = 0;
1088       for (s=0;s<st->layout.nb_streams;s++)
1089       {
1090          opus_int32 rate;
1091          OpusEncoder *enc;
1092          enc = (OpusEncoder*)ptr;
1093          if (s < st->layout.nb_coupled_streams)
1094             ptr += align(coupled_size);
1095          else
1096             ptr += align(mono_size);
1097          opus_encoder_ctl(enc, request, &rate);
1098          *value += rate;
1099       }
1100    }
1101    break;
1102    case OPUS_GET_LSB_DEPTH_REQUEST:
1103    case OPUS_GET_VBR_REQUEST:
1104    case OPUS_GET_APPLICATION_REQUEST:
1105    case OPUS_GET_BANDWIDTH_REQUEST:
1106    case OPUS_GET_COMPLEXITY_REQUEST:
1107    case OPUS_GET_PACKET_LOSS_PERC_REQUEST:
1108    case OPUS_GET_DTX_REQUEST:
1109    case OPUS_GET_VOICE_RATIO_REQUEST:
1110    case OPUS_GET_VBR_CONSTRAINT_REQUEST:
1111    case OPUS_GET_SIGNAL_REQUEST:
1112    case OPUS_GET_LOOKAHEAD_REQUEST:
1113    case OPUS_GET_SAMPLE_RATE_REQUEST:
1114    case OPUS_GET_INBAND_FEC_REQUEST:
1115    case OPUS_GET_FORCE_CHANNELS_REQUEST:
1116    case OPUS_GET_PREDICTION_DISABLED_REQUEST:
1117    {
1118       OpusEncoder *enc;
1119       /* For int32* GET params, just query the first stream */
1120       opus_int32 *value = va_arg(ap, opus_int32*);
1121       enc = (OpusEncoder*)ptr;
1122       ret = opus_encoder_ctl(enc, request, value);
1123    }
1124    break;
1125    case OPUS_GET_FINAL_RANGE_REQUEST:
1126    {
1127       int s;
1128       opus_uint32 *value = va_arg(ap, opus_uint32*);
1129       opus_uint32 tmp;
1130       if (!value)
1131       {
1132          goto bad_arg;
1133       }
1134       *value=0;
1135       for (s=0;s<st->layout.nb_streams;s++)
1136       {
1137          OpusEncoder *enc;
1138          enc = (OpusEncoder*)ptr;
1139          if (s < st->layout.nb_coupled_streams)
1140             ptr += align(coupled_size);
1141          else
1142             ptr += align(mono_size);
1143          ret = opus_encoder_ctl(enc, request, &tmp);
1144          if (ret != OPUS_OK) break;
1145          *value ^= tmp;
1146       }
1147    }
1148    break;
1149    case OPUS_SET_LSB_DEPTH_REQUEST:
1150    case OPUS_SET_COMPLEXITY_REQUEST:
1151    case OPUS_SET_VBR_REQUEST:
1152    case OPUS_SET_VBR_CONSTRAINT_REQUEST:
1153    case OPUS_SET_MAX_BANDWIDTH_REQUEST:
1154    case OPUS_SET_BANDWIDTH_REQUEST:
1155    case OPUS_SET_SIGNAL_REQUEST:
1156    case OPUS_SET_APPLICATION_REQUEST:
1157    case OPUS_SET_INBAND_FEC_REQUEST:
1158    case OPUS_SET_PACKET_LOSS_PERC_REQUEST:
1159    case OPUS_SET_DTX_REQUEST:
1160    case OPUS_SET_FORCE_MODE_REQUEST:
1161    case OPUS_SET_FORCE_CHANNELS_REQUEST:
1162    case OPUS_SET_PREDICTION_DISABLED_REQUEST:
1163    {
1164       int s;
1165       /* This works for int32 params */
1166       opus_int32 value = va_arg(ap, opus_int32);
1167       for (s=0;s<st->layout.nb_streams;s++)
1168       {
1169          OpusEncoder *enc;
1170
1171          enc = (OpusEncoder*)ptr;
1172          if (s < st->layout.nb_coupled_streams)
1173             ptr += align(coupled_size);
1174          else
1175             ptr += align(mono_size);
1176          ret = opus_encoder_ctl(enc, request, value);
1177          if (ret != OPUS_OK)
1178             break;
1179       }
1180    }
1181    break;
1182    case OPUS_MULTISTREAM_GET_ENCODER_STATE_REQUEST:
1183    {
1184       int s;
1185       opus_int32 stream_id;
1186       OpusEncoder **value;
1187       stream_id = va_arg(ap, opus_int32);
1188       if (stream_id<0 || stream_id >= st->layout.nb_streams)
1189          ret = OPUS_BAD_ARG;
1190       value = va_arg(ap, OpusEncoder**);
1191       if (!value)
1192       {
1193          goto bad_arg;
1194       }
1195       for (s=0;s<stream_id;s++)
1196       {
1197          if (s < st->layout.nb_coupled_streams)
1198             ptr += align(coupled_size);
1199          else
1200             ptr += align(mono_size);
1201       }
1202       *value = (OpusEncoder*)ptr;
1203    }
1204    break;
1205    case OPUS_SET_EXPERT_FRAME_DURATION_REQUEST:
1206    {
1207        opus_int32 value = va_arg(ap, opus_int32);
1208        st->variable_duration = value;
1209    }
1210    break;
1211    case OPUS_GET_EXPERT_FRAME_DURATION_REQUEST:
1212    {
1213        opus_int32 *value = va_arg(ap, opus_int32*);
1214        if (!value)
1215        {
1216           goto bad_arg;
1217        }
1218        *value = st->variable_duration;
1219    }
1220    break;
1221    case OPUS_RESET_STATE:
1222    {
1223       int s;
1224       st->subframe_mem[0] = st->subframe_mem[1] = st->subframe_mem[2] = 0;
1225       if (st->mapping_type == MAPPING_TYPE_SURROUND)
1226       {
1227          OPUS_CLEAR(ms_get_preemph_mem(st), st->layout.nb_channels);
1228          OPUS_CLEAR(ms_get_window_mem(st), st->layout.nb_channels*120);
1229       }
1230       for (s=0;s<st->layout.nb_streams;s++)
1231       {
1232          OpusEncoder *enc;
1233          enc = (OpusEncoder*)ptr;
1234          if (s < st->layout.nb_coupled_streams)
1235             ptr += align(coupled_size);
1236          else
1237             ptr += align(mono_size);
1238          ret = opus_encoder_ctl(enc, OPUS_RESET_STATE);
1239          if (ret != OPUS_OK)
1240             break;
1241       }
1242    }
1243    break;
1244    default:
1245       ret = OPUS_UNIMPLEMENTED;
1246       break;
1247    }
1248
1249    va_end(ap);
1250    return ret;
1251 bad_arg:
1252    va_end(ap);
1253    return OPUS_BAD_ARG;
1254 }
1255
1256 void opus_multistream_encoder_destroy(OpusMSEncoder *st)
1257 {
1258     opus_free(st);
1259 }