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