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