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