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