Fixes comments
[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        (coupled_streams+streams>255) || (streams<1) || (coupled_streams<0))
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        (coupled_streams+streams>255) || (streams<1) || (coupled_streams<0))
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    OpusMSEncoder *st;
570    if ((channels>255) || (channels<1))
571    {
572       if (error)
573          *error = OPUS_BAD_ARG;
574       return NULL;
575    }
576    st = (OpusMSEncoder *)opus_alloc(opus_multistream_surround_encoder_get_size(channels, mapping_family));
577    if (st==NULL)
578    {
579       if (error)
580          *error = OPUS_ALLOC_FAIL;
581       return NULL;
582    }
583    ret = opus_multistream_surround_encoder_init(st, Fs, channels, mapping_family, streams, coupled_streams, mapping, application);
584    if (ret != OPUS_OK)
585    {
586       opus_free(st);
587       st = NULL;
588    }
589    if (error)
590       *error = ret;
591    return st;
592 }
593
594 static opus_int32 surround_rate_allocation(
595       OpusMSEncoder *st,
596       opus_int32 *rate,
597       int frame_size
598       )
599 {
600    int i;
601    opus_int32 channel_rate;
602    opus_int32 Fs;
603    char *ptr;
604    int stream_offset;
605    int lfe_offset;
606    int coupled_ratio; /* Q8 */
607    int lfe_ratio;     /* Q8 */
608    opus_int32 rate_sum=0;
609
610    ptr = (char*)st + align(sizeof(OpusMSEncoder));
611    opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_SAMPLE_RATE(&Fs));
612
613    if (st->bitrate_bps > st->layout.nb_channels*40000)
614       stream_offset = 20000;
615    else
616       stream_offset = st->bitrate_bps/st->layout.nb_channels/2;
617    stream_offset += 60*(Fs/frame_size-50);
618    /* We start by giving each stream (coupled or uncoupled) the same bitrate.
619       This models the main saving of coupled channels over uncoupled. */
620    /* The LFE stream is an exception to the above and gets fewer bits. */
621    lfe_offset = 3500 + 60*(Fs/frame_size-50);
622    /* Coupled streams get twice the mono rate after the first 20 kb/s. */
623    coupled_ratio = 512;
624    /* Should depend on the bitrate, for now we assume LFE gets 1/8 the bits of mono */
625    lfe_ratio = 32;
626
627    /* Compute bitrate allocation between streams */
628    if (st->bitrate_bps==OPUS_AUTO)
629    {
630       channel_rate = Fs+60*Fs/frame_size;
631    } else if (st->bitrate_bps==OPUS_BITRATE_MAX)
632    {
633       channel_rate = 300000;
634    } else {
635       int nb_lfe;
636       int nb_uncoupled;
637       int nb_coupled;
638       int total;
639       nb_lfe = (st->lfe_stream!=-1);
640       nb_coupled = st->layout.nb_coupled_streams;
641       nb_uncoupled = st->layout.nb_streams-nb_coupled-nb_lfe;
642       total = (nb_uncoupled<<8)         /* mono */
643             + coupled_ratio*nb_coupled /* stereo */
644             + nb_lfe*lfe_ratio;
645       channel_rate = 256*(st->bitrate_bps-lfe_offset*nb_lfe-stream_offset*(nb_coupled+nb_uncoupled))/total;
646    }
647 #ifndef FIXED_POINT
648    if (st->variable_duration==OPUS_FRAMESIZE_VARIABLE && frame_size != Fs/50)
649    {
650       opus_int32 bonus;
651       bonus = 60*(Fs/frame_size-50);
652       channel_rate += bonus;
653    }
654 #endif
655
656    for (i=0;i<st->layout.nb_streams;i++)
657    {
658       if (i<st->layout.nb_coupled_streams)
659          rate[i] = stream_offset+(channel_rate*coupled_ratio>>8);
660       else if (i!=st->lfe_stream)
661          rate[i] = stream_offset+channel_rate;
662       else
663          rate[i] = lfe_offset+(channel_rate*lfe_ratio>>8);
664       rate[i] = IMAX(rate[i], 500);
665       rate_sum += rate[i];
666    }
667    return rate_sum;
668 }
669
670 /* Max size in case the encoder decides to return three frames */
671 #define MS_FRAME_TMP (3*1275+7)
672 static int opus_multistream_encode_native
673 (
674     OpusMSEncoder *st,
675     opus_copy_channel_in_func copy_channel_in,
676     const void *pcm,
677     int analysis_frame_size,
678     unsigned char *data,
679     opus_int32 max_data_bytes,
680     int lsb_depth,
681     downmix_func downmix,
682     int float_api
683 )
684 {
685    opus_int32 Fs;
686    int coupled_size;
687    int mono_size;
688    int s;
689    char *ptr;
690    int tot_size;
691    VARDECL(opus_val16, buf);
692    VARDECL(opus_val16, bandSMR);
693    unsigned char tmp_data[MS_FRAME_TMP];
694    OpusRepacketizer rp;
695    opus_int32 vbr;
696    const CELTMode *celt_mode;
697    opus_int32 bitrates[256];
698    opus_val16 bandLogE[42];
699    opus_val32 *mem = NULL;
700    opus_val32 *preemph_mem=NULL;
701    int frame_size;
702    opus_int32 rate_sum;
703    opus_int32 smallest_packet;
704    ALLOC_STACK;
705
706    if (st->surround)
707    {
708       preemph_mem = ms_get_preemph_mem(st);
709       mem = ms_get_window_mem(st);
710    }
711
712    ptr = (char*)st + align(sizeof(OpusMSEncoder));
713    opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_SAMPLE_RATE(&Fs));
714    opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_VBR(&vbr));
715    opus_encoder_ctl((OpusEncoder*)ptr, CELT_GET_MODE(&celt_mode));
716
717    {
718       opus_int32 delay_compensation;
719       int channels;
720
721       channels = st->layout.nb_streams + st->layout.nb_coupled_streams;
722       opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_LOOKAHEAD(&delay_compensation));
723       delay_compensation -= Fs/400;
724       frame_size = compute_frame_size(pcm, analysis_frame_size,
725             st->variable_duration, channels, Fs, st->bitrate_bps,
726             delay_compensation, downmix
727 #ifndef DISABLE_FLOAT_API
728             , st->subframe_mem
729 #endif
730             );
731    }
732
733    if (400*frame_size < Fs)
734    {
735       RESTORE_STACK;
736       return OPUS_BAD_ARG;
737    }
738    /* Validate frame_size before using it to allocate stack space.
739       This mirrors the checks in opus_encode[_float](). */
740    if (400*frame_size != Fs && 200*frame_size != Fs &&
741        100*frame_size != Fs &&  50*frame_size != Fs &&
742         25*frame_size != Fs &&  50*frame_size != 3*Fs)
743    {
744       RESTORE_STACK;
745       return OPUS_BAD_ARG;
746    }
747
748    /* Smallest packet the encoder can produce. */
749    smallest_packet = st->layout.nb_streams*2-1;
750    if (max_data_bytes < smallest_packet)
751    {
752       RESTORE_STACK;
753       return OPUS_BUFFER_TOO_SMALL;
754    }
755    ALLOC(buf, 2*frame_size, opus_val16);
756    coupled_size = opus_encoder_get_size(2);
757    mono_size = opus_encoder_get_size(1);
758
759    ALLOC(bandSMR, 21*st->layout.nb_channels, opus_val16);
760    if (st->surround)
761    {
762       surround_analysis(celt_mode, pcm, bandSMR, mem, preemph_mem, frame_size, 120, st->layout.nb_channels, Fs, copy_channel_in);
763    }
764
765    /* Compute bitrate allocation between streams (this could be a lot better) */
766    rate_sum = surround_rate_allocation(st, bitrates, frame_size);
767
768    if (!vbr)
769    {
770       if (st->bitrate_bps == OPUS_AUTO)
771       {
772          max_data_bytes = IMIN(max_data_bytes, 3*rate_sum/(3*8*Fs/frame_size));
773       } else if (st->bitrate_bps != OPUS_BITRATE_MAX)
774       {
775          max_data_bytes = IMIN(max_data_bytes, IMAX(smallest_packet,
776                           3*st->bitrate_bps/(3*8*Fs/frame_size)));
777       }
778    }
779    ptr = (char*)st + align(sizeof(OpusMSEncoder));
780    for (s=0;s<st->layout.nb_streams;s++)
781    {
782       OpusEncoder *enc;
783       enc = (OpusEncoder*)ptr;
784       if (s < st->layout.nb_coupled_streams)
785          ptr += align(coupled_size);
786       else
787          ptr += align(mono_size);
788       opus_encoder_ctl(enc, OPUS_SET_BITRATE(bitrates[s]));
789       if (st->surround)
790       {
791          opus_int32 equiv_rate;
792          equiv_rate = st->bitrate_bps;
793          if (frame_size*50 < Fs)
794             equiv_rate -= 60*(Fs/frame_size - 50)*st->layout.nb_channels;
795          if (equiv_rate > 10000*st->layout.nb_channels)
796             opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_FULLBAND));
797          else if (equiv_rate > 7000*st->layout.nb_channels)
798             opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_SUPERWIDEBAND));
799          else if (equiv_rate > 5000*st->layout.nb_channels)
800             opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_WIDEBAND));
801          else
802             opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_NARROWBAND));
803          if (s < st->layout.nb_coupled_streams)
804          {
805             /* To preserve the spatial image, force stereo CELT on coupled streams */
806             opus_encoder_ctl(enc, OPUS_SET_FORCE_MODE(MODE_CELT_ONLY));
807             opus_encoder_ctl(enc, OPUS_SET_FORCE_CHANNELS(2));
808          }
809       }
810    }
811
812    ptr = (char*)st + align(sizeof(OpusMSEncoder));
813    /* Counting ToC */
814    tot_size = 0;
815    for (s=0;s<st->layout.nb_streams;s++)
816    {
817       OpusEncoder *enc;
818       int len;
819       int curr_max;
820       int c1, c2;
821
822       opus_repacketizer_init(&rp);
823       enc = (OpusEncoder*)ptr;
824       if (s < st->layout.nb_coupled_streams)
825       {
826          int i;
827          int left, right;
828          left = get_left_channel(&st->layout, s, -1);
829          right = get_right_channel(&st->layout, s, -1);
830          (*copy_channel_in)(buf, 2,
831             pcm, st->layout.nb_channels, left, frame_size);
832          (*copy_channel_in)(buf+1, 2,
833             pcm, st->layout.nb_channels, right, frame_size);
834          ptr += align(coupled_size);
835          if (st->surround)
836          {
837             for (i=0;i<21;i++)
838             {
839                bandLogE[i] = bandSMR[21*left+i];
840                bandLogE[21+i] = bandSMR[21*right+i];
841             }
842          }
843          c1 = left;
844          c2 = right;
845       } else {
846          int i;
847          int chan = get_mono_channel(&st->layout, s, -1);
848          (*copy_channel_in)(buf, 1,
849             pcm, st->layout.nb_channels, chan, frame_size);
850          ptr += align(mono_size);
851          if (st->surround)
852          {
853             for (i=0;i<21;i++)
854                bandLogE[i] = bandSMR[21*chan+i];
855          }
856          c1 = chan;
857          c2 = -1;
858       }
859       if (st->surround)
860          opus_encoder_ctl(enc, OPUS_SET_ENERGY_MASK(bandLogE));
861       /* number of bytes left (+Toc) */
862       curr_max = max_data_bytes - tot_size;
863       /* Reserve one byte for the last stream and two for the others */
864       curr_max -= IMAX(0,2*(st->layout.nb_streams-s-1)-1);
865       curr_max = IMIN(curr_max,MS_FRAME_TMP);
866       /* Repacketizer will add one or two bytes for self-delimited frames */
867       if (s != st->layout.nb_streams-1) curr_max -=  curr_max>253 ? 2 : 1;
868       if (!vbr && s == st->layout.nb_streams-1)
869          opus_encoder_ctl(enc, OPUS_SET_BITRATE(curr_max*(8*Fs/frame_size)));
870       len = opus_encode_native(enc, buf, frame_size, tmp_data, curr_max, lsb_depth,
871             pcm, analysis_frame_size, c1, c2, st->layout.nb_channels, downmix, float_api);
872       if (len<0)
873       {
874          RESTORE_STACK;
875          return len;
876       }
877       /* We need to use the repacketizer to add the self-delimiting lengths
878          while taking into account the fact that the encoder can now return
879          more than one frame at a time (e.g. 60 ms CELT-only) */
880       opus_repacketizer_cat(&rp, tmp_data, len);
881       len = opus_repacketizer_out_range_impl(&rp, 0, opus_repacketizer_get_nb_frames(&rp),
882             data, max_data_bytes-tot_size, s != st->layout.nb_streams-1, !vbr && s == st->layout.nb_streams-1);
883       data += len;
884       tot_size += len;
885    }
886    /*printf("\n");*/
887    RESTORE_STACK;
888    return tot_size;
889 }
890
891 #if !defined(DISABLE_FLOAT_API)
892 static void opus_copy_channel_in_float(
893   opus_val16 *dst,
894   int dst_stride,
895   const void *src,
896   int src_stride,
897   int src_channel,
898   int frame_size
899 )
900 {
901    const float *float_src;
902    opus_int32 i;
903    float_src = (const float *)src;
904    for (i=0;i<frame_size;i++)
905 #if defined(FIXED_POINT)
906       dst[i*dst_stride] = FLOAT2INT16(float_src[i*src_stride+src_channel]);
907 #else
908       dst[i*dst_stride] = float_src[i*src_stride+src_channel];
909 #endif
910 }
911 #endif
912
913 static void opus_copy_channel_in_short(
914   opus_val16 *dst,
915   int dst_stride,
916   const void *src,
917   int src_stride,
918   int src_channel,
919   int frame_size
920 )
921 {
922    const opus_int16 *short_src;
923    opus_int32 i;
924    short_src = (const opus_int16 *)src;
925    for (i=0;i<frame_size;i++)
926 #if defined(FIXED_POINT)
927       dst[i*dst_stride] = short_src[i*src_stride+src_channel];
928 #else
929       dst[i*dst_stride] = (1/32768.f)*short_src[i*src_stride+src_channel];
930 #endif
931 }
932
933
934 #ifdef FIXED_POINT
935 int opus_multistream_encode(
936     OpusMSEncoder *st,
937     const opus_val16 *pcm,
938     int frame_size,
939     unsigned char *data,
940     opus_int32 max_data_bytes
941 )
942 {
943    return opus_multistream_encode_native(st, opus_copy_channel_in_short,
944       pcm, frame_size, data, max_data_bytes, 16, downmix_int, 0);
945 }
946
947 #ifndef DISABLE_FLOAT_API
948 int opus_multistream_encode_float(
949     OpusMSEncoder *st,
950     const float *pcm,
951     int frame_size,
952     unsigned char *data,
953     opus_int32 max_data_bytes
954 )
955 {
956    return opus_multistream_encode_native(st, opus_copy_channel_in_float,
957       pcm, frame_size, data, max_data_bytes, 16, downmix_float, 1);
958 }
959 #endif
960
961 #else
962
963 int opus_multistream_encode_float
964 (
965     OpusMSEncoder *st,
966     const opus_val16 *pcm,
967     int frame_size,
968     unsigned char *data,
969     opus_int32 max_data_bytes
970 )
971 {
972    return opus_multistream_encode_native(st, opus_copy_channel_in_float,
973       pcm, frame_size, data, max_data_bytes, 24, downmix_float, 1);
974 }
975
976 int opus_multistream_encode(
977     OpusMSEncoder *st,
978     const opus_int16 *pcm,
979     int frame_size,
980     unsigned char *data,
981     opus_int32 max_data_bytes
982 )
983 {
984    return opus_multistream_encode_native(st, opus_copy_channel_in_short,
985       pcm, frame_size, data, max_data_bytes, 16, downmix_int, 0);
986 }
987 #endif
988
989 int opus_multistream_encoder_ctl(OpusMSEncoder *st, int request, ...)
990 {
991    va_list ap;
992    int coupled_size, mono_size;
993    char *ptr;
994    int ret = OPUS_OK;
995
996    va_start(ap, request);
997
998    coupled_size = opus_encoder_get_size(2);
999    mono_size = opus_encoder_get_size(1);
1000    ptr = (char*)st + align(sizeof(OpusMSEncoder));
1001    switch (request)
1002    {
1003    case OPUS_SET_BITRATE_REQUEST:
1004    {
1005       opus_int32 value = va_arg(ap, opus_int32);
1006       if (value<0 && value!=OPUS_AUTO && value!=OPUS_BITRATE_MAX)
1007       {
1008          goto bad_arg;
1009       }
1010       st->bitrate_bps = value;
1011    }
1012    break;
1013    case OPUS_GET_BITRATE_REQUEST:
1014    {
1015       int s;
1016       opus_int32 *value = va_arg(ap, opus_int32*);
1017       if (!value)
1018       {
1019          goto bad_arg;
1020       }
1021       *value = 0;
1022       for (s=0;s<st->layout.nb_streams;s++)
1023       {
1024          opus_int32 rate;
1025          OpusEncoder *enc;
1026          enc = (OpusEncoder*)ptr;
1027          if (s < st->layout.nb_coupled_streams)
1028             ptr += align(coupled_size);
1029          else
1030             ptr += align(mono_size);
1031          opus_encoder_ctl(enc, request, &rate);
1032          *value += rate;
1033       }
1034    }
1035    break;
1036    case OPUS_GET_LSB_DEPTH_REQUEST:
1037    case OPUS_GET_VBR_REQUEST:
1038    case OPUS_GET_APPLICATION_REQUEST:
1039    case OPUS_GET_BANDWIDTH_REQUEST:
1040    case OPUS_GET_COMPLEXITY_REQUEST:
1041    case OPUS_GET_PACKET_LOSS_PERC_REQUEST:
1042    case OPUS_GET_DTX_REQUEST:
1043    case OPUS_GET_VOICE_RATIO_REQUEST:
1044    case OPUS_GET_VBR_CONSTRAINT_REQUEST:
1045    case OPUS_GET_SIGNAL_REQUEST:
1046    case OPUS_GET_LOOKAHEAD_REQUEST:
1047    case OPUS_GET_SAMPLE_RATE_REQUEST:
1048    case OPUS_GET_INBAND_FEC_REQUEST:
1049    case OPUS_GET_FORCE_CHANNELS_REQUEST:
1050    case OPUS_GET_PREDICTION_DISABLED_REQUEST:
1051    {
1052       OpusEncoder *enc;
1053       /* For int32* GET params, just query the first stream */
1054       opus_int32 *value = va_arg(ap, opus_int32*);
1055       enc = (OpusEncoder*)ptr;
1056       ret = opus_encoder_ctl(enc, request, value);
1057    }
1058    break;
1059    case OPUS_GET_FINAL_RANGE_REQUEST:
1060    {
1061       int s;
1062       opus_uint32 *value = va_arg(ap, opus_uint32*);
1063       opus_uint32 tmp;
1064       if (!value)
1065       {
1066          goto bad_arg;
1067       }
1068       *value=0;
1069       for (s=0;s<st->layout.nb_streams;s++)
1070       {
1071          OpusEncoder *enc;
1072          enc = (OpusEncoder*)ptr;
1073          if (s < st->layout.nb_coupled_streams)
1074             ptr += align(coupled_size);
1075          else
1076             ptr += align(mono_size);
1077          ret = opus_encoder_ctl(enc, request, &tmp);
1078          if (ret != OPUS_OK) break;
1079          *value ^= tmp;
1080       }
1081    }
1082    break;
1083    case OPUS_SET_LSB_DEPTH_REQUEST:
1084    case OPUS_SET_COMPLEXITY_REQUEST:
1085    case OPUS_SET_VBR_REQUEST:
1086    case OPUS_SET_VBR_CONSTRAINT_REQUEST:
1087    case OPUS_SET_MAX_BANDWIDTH_REQUEST:
1088    case OPUS_SET_BANDWIDTH_REQUEST:
1089    case OPUS_SET_SIGNAL_REQUEST:
1090    case OPUS_SET_APPLICATION_REQUEST:
1091    case OPUS_SET_INBAND_FEC_REQUEST:
1092    case OPUS_SET_PACKET_LOSS_PERC_REQUEST:
1093    case OPUS_SET_DTX_REQUEST:
1094    case OPUS_SET_FORCE_MODE_REQUEST:
1095    case OPUS_SET_FORCE_CHANNELS_REQUEST:
1096    case OPUS_SET_PREDICTION_DISABLED_REQUEST:
1097    {
1098       int s;
1099       /* This works for int32 params */
1100       opus_int32 value = va_arg(ap, opus_int32);
1101       for (s=0;s<st->layout.nb_streams;s++)
1102       {
1103          OpusEncoder *enc;
1104
1105          enc = (OpusEncoder*)ptr;
1106          if (s < st->layout.nb_coupled_streams)
1107             ptr += align(coupled_size);
1108          else
1109             ptr += align(mono_size);
1110          ret = opus_encoder_ctl(enc, request, value);
1111          if (ret != OPUS_OK)
1112             break;
1113       }
1114    }
1115    break;
1116    case OPUS_MULTISTREAM_GET_ENCODER_STATE_REQUEST:
1117    {
1118       int s;
1119       opus_int32 stream_id;
1120       OpusEncoder **value;
1121       stream_id = va_arg(ap, opus_int32);
1122       if (stream_id<0 || stream_id >= st->layout.nb_streams)
1123          ret = OPUS_BAD_ARG;
1124       value = va_arg(ap, OpusEncoder**);
1125       if (!value)
1126       {
1127          goto bad_arg;
1128       }
1129       for (s=0;s<stream_id;s++)
1130       {
1131          if (s < st->layout.nb_coupled_streams)
1132             ptr += align(coupled_size);
1133          else
1134             ptr += align(mono_size);
1135       }
1136       *value = (OpusEncoder*)ptr;
1137    }
1138    break;
1139    case OPUS_SET_EXPERT_FRAME_DURATION_REQUEST:
1140    {
1141        opus_int32 value = va_arg(ap, opus_int32);
1142        st->variable_duration = value;
1143    }
1144    break;
1145    case OPUS_GET_EXPERT_FRAME_DURATION_REQUEST:
1146    {
1147        opus_int32 *value = va_arg(ap, opus_int32*);
1148        if (!value)
1149        {
1150           goto bad_arg;
1151        }
1152        *value = st->variable_duration;
1153    }
1154    break;
1155    case OPUS_RESET_STATE:
1156    {
1157       int s;
1158       st->subframe_mem[0] = st->subframe_mem[1] = st->subframe_mem[2] = 0;
1159       if (st->surround)
1160       {
1161          OPUS_CLEAR(ms_get_preemph_mem(st), st->layout.nb_channels);
1162          OPUS_CLEAR(ms_get_window_mem(st), st->layout.nb_channels*120);
1163       }
1164       for (s=0;s<st->layout.nb_streams;s++)
1165       {
1166          OpusEncoder *enc;
1167          enc = (OpusEncoder*)ptr;
1168          if (s < st->layout.nb_coupled_streams)
1169             ptr += align(coupled_size);
1170          else
1171             ptr += align(mono_size);
1172          ret = opus_encoder_ctl(enc, OPUS_RESET_STATE);
1173          if (ret != OPUS_OK)
1174             break;
1175       }
1176    }
1177    break;
1178    default:
1179       ret = OPUS_UNIMPLEMENTED;
1180       break;
1181    }
1182
1183    va_end(ap);
1184    return ret;
1185 bad_arg:
1186    va_end(ap);
1187    return OPUS_BAD_ARG;
1188 }
1189
1190 void opus_multistream_encoder_destroy(OpusMSEncoder *st)
1191 {
1192     opus_free(st);
1193 }