Modify autoconf tests for intrinsics to stop clang from optimizing them away.
[opus.git] / src / opus_multistream_encoder.c
index c5fb366..9e85773 100644 (file)
 #include <stdarg.h>
 #include "float_cast.h"
 #include "os_support.h"
+#include "mathops.h"
+#include "mdct.h"
+#include "modes.h"
+#include "bands.h"
+#include "quant_bands.h"
+#include "pitch.h"
+
+typedef struct {
+   int nb_streams;
+   int nb_coupled_streams;
+   unsigned char mapping[8];
+} VorbisLayout;
+
+/* Index is nb_channel-1*/
+static const VorbisLayout vorbis_mappings[8] = {
+      {1, 0, {0}},                      /* 1: mono */
+      {1, 1, {0, 1}},                   /* 2: stereo */
+      {2, 1, {0, 2, 1}},                /* 3: 1-d surround */
+      {2, 2, {0, 1, 2, 3}},             /* 4: quadraphonic surround */
+      {3, 2, {0, 4, 1, 2, 3}},          /* 5: 5-channel surround */
+      {4, 2, {0, 4, 1, 2, 3, 5}},       /* 6: 5.1 surround */
+      {4, 3, {0, 4, 1, 2, 3, 5, 6}},    /* 7: 6.1 surround */
+      {5, 3, {0, 6, 1, 2, 3, 4, 5, 7}}, /* 8: 7.1 surround */
+};
+
+typedef void (*opus_copy_channel_in_func)(
+  opus_val16 *dst,
+  int dst_stride,
+  const void *src,
+  int src_stride,
+  int src_channel,
+  int frame_size
+);
 
 struct OpusMSEncoder {
    ChannelLayout layout;
+   int arch;
+   int lfe_stream;
+   int application;
    int variable_duration;
+   int surround;
    opus_int32 bitrate_bps;
-   opus_val32 subframe_mem[3];
+   float subframe_mem[3];
    /* Encoder states go here */
+   /* then opus_val32 window_mem[channels*120]; */
+   /* then opus_val32 preemph_mem[channels]; */
 };
 
+static opus_val32 *ms_get_preemph_mem(OpusMSEncoder *st)
+{
+   int s;
+   char *ptr;
+   int coupled_size, mono_size;
+
+   coupled_size = opus_encoder_get_size(2);
+   mono_size = opus_encoder_get_size(1);
+   ptr = (char*)st + align(sizeof(OpusMSEncoder));
+   for (s=0;s<st->layout.nb_streams;s++)
+   {
+      if (s < st->layout.nb_coupled_streams)
+         ptr += align(coupled_size);
+      else
+         ptr += align(mono_size);
+   }
+   /* void* cast avoids clang -Wcast-align warning */
+   return (opus_val32*)(void*)(ptr+st->layout.nb_channels*120*sizeof(opus_val32));
+}
+
+static opus_val32 *ms_get_window_mem(OpusMSEncoder *st)
+{
+   int s;
+   char *ptr;
+   int coupled_size, mono_size;
+
+   coupled_size = opus_encoder_get_size(2);
+   mono_size = opus_encoder_get_size(1);
+   ptr = (char*)st + align(sizeof(OpusMSEncoder));
+   for (s=0;s<st->layout.nb_streams;s++)
+   {
+      if (s < st->layout.nb_coupled_streams)
+         ptr += align(coupled_size);
+      else
+         ptr += align(mono_size);
+   }
+   /* void* cast avoids clang -Wcast-align warning */
+   return (opus_val32*)(void*)ptr;
+}
 
 static int validate_encoder_layout(const ChannelLayout *layout)
 {
@@ -65,6 +143,221 @@ static int validate_encoder_layout(const ChannelLayout *layout)
    return 1;
 }
 
+static void channel_pos(int channels, int pos[8])
+{
+   /* Position in the mix: 0 don't mix, 1: left, 2: center, 3:right */
+   if (channels==4)
+   {
+      pos[0]=1;
+      pos[1]=3;
+      pos[2]=1;
+      pos[3]=3;
+   } else if (channels==3||channels==5||channels==6)
+   {
+      pos[0]=1;
+      pos[1]=2;
+      pos[2]=3;
+      pos[3]=1;
+      pos[4]=3;
+      pos[5]=0;
+   } else if (channels==7)
+   {
+      pos[0]=1;
+      pos[1]=2;
+      pos[2]=3;
+      pos[3]=1;
+      pos[4]=3;
+      pos[5]=2;
+      pos[6]=0;
+   } else if (channels==8)
+   {
+      pos[0]=1;
+      pos[1]=2;
+      pos[2]=3;
+      pos[3]=1;
+      pos[4]=3;
+      pos[5]=1;
+      pos[6]=3;
+      pos[7]=0;
+   }
+}
+
+#if 1
+/* Computes a rough approximation of log2(2^a + 2^b) */
+static opus_val16 logSum(opus_val16 a, opus_val16 b)
+{
+   opus_val16 max;
+   opus_val32 diff;
+   opus_val16 frac;
+   static const opus_val16 diff_table[17] = {
+         QCONST16(0.5000000f, DB_SHIFT), QCONST16(0.2924813f, DB_SHIFT), QCONST16(0.1609640f, DB_SHIFT), QCONST16(0.0849625f, DB_SHIFT),
+         QCONST16(0.0437314f, DB_SHIFT), QCONST16(0.0221971f, DB_SHIFT), QCONST16(0.0111839f, DB_SHIFT), QCONST16(0.0056136f, DB_SHIFT),
+         QCONST16(0.0028123f, DB_SHIFT)
+   };
+   int low;
+   if (a>b)
+   {
+      max = a;
+      diff = SUB32(EXTEND32(a),EXTEND32(b));
+   } else {
+      max = b;
+      diff = SUB32(EXTEND32(b),EXTEND32(a));
+   }
+   if (!(diff < QCONST16(8.f, DB_SHIFT)))  /* inverted to catch NaNs */
+      return max;
+#ifdef FIXED_POINT
+   low = SHR32(diff, DB_SHIFT-1);
+   frac = SHL16(diff - SHL16(low, DB_SHIFT-1), 16-DB_SHIFT);
+#else
+   low = (int)floor(2*diff);
+   frac = 2*diff - low;
+#endif
+   return max + diff_table[low] + MULT16_16_Q15(frac, SUB16(diff_table[low+1], diff_table[low]));
+}
+#else
+opus_val16 logSum(opus_val16 a, opus_val16 b)
+{
+   return log2(pow(4, a)+ pow(4, b))/2;
+}
+#endif
+
+void surround_analysis(const CELTMode *celt_mode, const void *pcm, opus_val16 *bandLogE, opus_val32 *mem, opus_val32 *preemph_mem,
+      int len, int overlap, int channels, int rate, opus_copy_channel_in_func copy_channel_in, int arch
+)
+{
+   int c;
+   int i;
+   int LM;
+   int pos[8] = {0};
+   int upsample;
+   int frame_size;
+   opus_val16 channel_offset;
+   opus_val32 bandE[21];
+   opus_val16 maskLogE[3][21];
+   VARDECL(opus_val32, in);
+   VARDECL(opus_val16, x);
+   VARDECL(opus_val32, freq);
+   SAVE_STACK;
+
+   upsample = resampling_factor(rate);
+   frame_size = len*upsample;
+
+   for (LM=0;LM<celt_mode->maxLM;LM++)
+      if (celt_mode->shortMdctSize<<LM==frame_size)
+         break;
+
+   ALLOC(in, frame_size+overlap, opus_val32);
+   ALLOC(x, len, opus_val16);
+   ALLOC(freq, frame_size, opus_val32);
+
+   channel_pos(channels, pos);
+
+   for (c=0;c<3;c++)
+      for (i=0;i<21;i++)
+         maskLogE[c][i] = -QCONST16(28.f, DB_SHIFT);
+
+   for (c=0;c<channels;c++)
+   {
+      OPUS_COPY(in, mem+c*overlap, overlap);
+      (*copy_channel_in)(x, 1, pcm, channels, c, len);
+      celt_preemphasis(x, in+overlap, frame_size, 1, upsample, celt_mode->preemph, preemph_mem+c, 0);
+#ifndef FIXED_POINT
+      {
+         opus_val32 sum;
+         sum = celt_inner_prod(in, in, frame_size+overlap, 0);
+         /* This should filter out both NaNs and ridiculous signals that could
+            cause NaNs further down. */
+         if (!(sum < 1e9f) || celt_isnan(sum))
+         {
+            OPUS_CLEAR(in, frame_size+overlap);
+            preemph_mem[c] = 0;
+         }
+      }
+#endif
+      clt_mdct_forward(&celt_mode->mdct, in, freq, celt_mode->window,
+            overlap, celt_mode->maxLM-LM, 1, arch);
+      if (upsample != 1)
+      {
+         int bound = len;
+         for (i=0;i<bound;i++)
+            freq[i] *= upsample;
+         for (;i<frame_size;i++)
+            freq[i] = 0;
+      }
+
+      compute_band_energies(celt_mode, freq, bandE, 21, 1, LM);
+      amp2Log2(celt_mode, 21, 21, bandE, bandLogE+21*c, 1);
+      /* Apply spreading function with -6 dB/band going up and -12 dB/band going down. */
+      for (i=1;i<21;i++)
+         bandLogE[21*c+i] = MAX16(bandLogE[21*c+i], bandLogE[21*c+i-1]-QCONST16(1.f, DB_SHIFT));
+      for (i=19;i>=0;i--)
+         bandLogE[21*c+i] = MAX16(bandLogE[21*c+i], bandLogE[21*c+i+1]-QCONST16(2.f, DB_SHIFT));
+      if (pos[c]==1)
+      {
+         for (i=0;i<21;i++)
+            maskLogE[0][i] = logSum(maskLogE[0][i], bandLogE[21*c+i]);
+      } else if (pos[c]==3)
+      {
+         for (i=0;i<21;i++)
+            maskLogE[2][i] = logSum(maskLogE[2][i], bandLogE[21*c+i]);
+      } else if (pos[c]==2)
+      {
+         for (i=0;i<21;i++)
+         {
+            maskLogE[0][i] = logSum(maskLogE[0][i], bandLogE[21*c+i]-QCONST16(.5f, DB_SHIFT));
+            maskLogE[2][i] = logSum(maskLogE[2][i], bandLogE[21*c+i]-QCONST16(.5f, DB_SHIFT));
+         }
+      }
+#if 0
+      for (i=0;i<21;i++)
+         printf("%f ", bandLogE[21*c+i]);
+      float sum=0;
+      for (i=0;i<21;i++)
+         sum += bandLogE[21*c+i];
+      printf("%f ", sum/21);
+#endif
+      OPUS_COPY(mem+c*overlap, in+frame_size, overlap);
+   }
+   for (i=0;i<21;i++)
+      maskLogE[1][i] = MIN32(maskLogE[0][i],maskLogE[2][i]);
+   channel_offset = HALF16(celt_log2(QCONST32(2.f,14)/(channels-1)));
+   for (c=0;c<3;c++)
+      for (i=0;i<21;i++)
+         maskLogE[c][i] += channel_offset;
+#if 0
+   for (c=0;c<3;c++)
+   {
+      for (i=0;i<21;i++)
+         printf("%f ", maskLogE[c][i]);
+   }
+#endif
+   for (c=0;c<channels;c++)
+   {
+      opus_val16 *mask;
+      if (pos[c]!=0)
+      {
+         mask = &maskLogE[pos[c]-1][0];
+         for (i=0;i<21;i++)
+            bandLogE[21*c+i] = bandLogE[21*c+i] - mask[i];
+      } else {
+         for (i=0;i<21;i++)
+            bandLogE[21*c+i] = 0;
+      }
+#if 0
+      for (i=0;i<21;i++)
+         printf("%f ", bandLogE[21*c+i]);
+      printf("\n");
+#endif
+#if 0
+      float sum=0;
+      for (i=0;i<21;i++)
+         sum += bandLogE[21*c+i];
+      printf("%f ", sum/(float)QCONST32(21.f, DB_SHIFT));
+      printf("\n");
+#endif
+   }
+   RESTORE_STACK;
+}
 
 opus_int32 opus_multistream_encoder_get_size(int nb_streams, int nb_coupled_streams)
 {
@@ -79,16 +372,52 @@ opus_int32 opus_multistream_encoder_get_size(int nb_streams, int nb_coupled_stre
         + (nb_streams-nb_coupled_streams) * align(mono_size);
 }
 
+opus_int32 opus_multistream_surround_encoder_get_size(int channels, int mapping_family)
+{
+   int nb_streams;
+   int nb_coupled_streams;
+   opus_int32 size;
 
+   if (mapping_family==0)
+   {
+      if (channels==1)
+      {
+         nb_streams=1;
+         nb_coupled_streams=0;
+      } else if (channels==2)
+      {
+         nb_streams=1;
+         nb_coupled_streams=1;
+      } else
+         return 0;
+   } else if (mapping_family==1 && channels<=8 && channels>=1)
+   {
+      nb_streams=vorbis_mappings[channels-1].nb_streams;
+      nb_coupled_streams=vorbis_mappings[channels-1].nb_coupled_streams;
+   } else if (mapping_family==255)
+   {
+      nb_streams=channels;
+      nb_coupled_streams=0;
+   } else
+      return 0;
+   size = opus_multistream_encoder_get_size(nb_streams, nb_coupled_streams);
+   if (channels>2)
+   {
+      size += channels*(120*sizeof(opus_val32) + sizeof(opus_val32));
+   }
+   return size;
+}
 
-int opus_multistream_encoder_init(
+
+static int opus_multistream_encoder_init_impl(
       OpusMSEncoder *st,
       opus_int32 Fs,
       int channels,
       int streams,
       int coupled_streams,
       const unsigned char *mapping,
-      int application
+      int application,
+      int surround
 )
 {
    int coupled_size;
@@ -97,14 +426,19 @@ int opus_multistream_encoder_init(
    char *ptr;
 
    if ((channels>255) || (channels<1) || (coupled_streams>streams) ||
-       (coupled_streams+streams>255) || (streams<1) || (coupled_streams<0))
+       (streams<1) || (coupled_streams<0) || (streams>255-coupled_streams))
       return OPUS_BAD_ARG;
 
+   st->arch = opus_select_arch();
    st->layout.nb_channels = channels;
    st->layout.nb_streams = streams;
    st->layout.nb_coupled_streams = coupled_streams;
-
+   st->subframe_mem[0]=st->subframe_mem[1]=st->subframe_mem[2]=0;
+   if (!surround)
+      st->lfe_stream = -1;
    st->bitrate_bps = OPUS_AUTO;
+   st->application = application;
+   st->variable_duration = OPUS_FRAMESIZE_ARG;
    for (i=0;i<st->layout.nb_channels;i++)
       st->layout.mapping[i] = mapping[i];
    if (!validate_layout(&st->layout) || !validate_encoder_layout(&st->layout))
@@ -117,17 +451,91 @@ int opus_multistream_encoder_init(
    {
       ret = opus_encoder_init((OpusEncoder*)ptr, Fs, 2, application);
       if(ret!=OPUS_OK)return ret;
+      if (i==st->lfe_stream)
+         opus_encoder_ctl((OpusEncoder*)ptr, OPUS_SET_LFE(1));
       ptr += align(coupled_size);
    }
    for (;i<st->layout.nb_streams;i++)
    {
       ret = opus_encoder_init((OpusEncoder*)ptr, Fs, 1, application);
+      if (i==st->lfe_stream)
+         opus_encoder_ctl((OpusEncoder*)ptr, OPUS_SET_LFE(1));
       if(ret!=OPUS_OK)return ret;
       ptr += align(mono_size);
    }
+   if (surround)
+   {
+      OPUS_CLEAR(ms_get_preemph_mem(st), channels);
+      OPUS_CLEAR(ms_get_window_mem(st), channels*120);
+   }
+   st->surround = surround;
    return OPUS_OK;
 }
 
+int opus_multistream_encoder_init(
+      OpusMSEncoder *st,
+      opus_int32 Fs,
+      int channels,
+      int streams,
+      int coupled_streams,
+      const unsigned char *mapping,
+      int application
+)
+{
+   return opus_multistream_encoder_init_impl(st, Fs, channels, streams, coupled_streams, mapping, application, 0);
+}
+
+int opus_multistream_surround_encoder_init(
+      OpusMSEncoder *st,
+      opus_int32 Fs,
+      int channels,
+      int mapping_family,
+      int *streams,
+      int *coupled_streams,
+      unsigned char *mapping,
+      int application
+)
+{
+   if ((channels>255) || (channels<1))
+      return OPUS_BAD_ARG;
+   st->lfe_stream = -1;
+   if (mapping_family==0)
+   {
+      if (channels==1)
+      {
+         *streams=1;
+         *coupled_streams=0;
+         mapping[0]=0;
+      } else if (channels==2)
+      {
+         *streams=1;
+         *coupled_streams=1;
+         mapping[0]=0;
+         mapping[1]=1;
+      } else
+         return OPUS_UNIMPLEMENTED;
+   } else if (mapping_family==1 && channels<=8 && channels>=1)
+   {
+      int i;
+      *streams=vorbis_mappings[channels-1].nb_streams;
+      *coupled_streams=vorbis_mappings[channels-1].nb_coupled_streams;
+      for (i=0;i<channels;i++)
+         mapping[i] = vorbis_mappings[channels-1].mapping[i];
+      if (channels>=6)
+         st->lfe_stream = *streams-1;
+   } else if (mapping_family==255)
+   {
+      int i;
+      *streams=channels;
+      *coupled_streams=0;
+      for(i=0;i<channels;i++)
+         mapping[i] = i;
+   } else
+      return OPUS_UNIMPLEMENTED;
+   return opus_multistream_encoder_init_impl(st, Fs, channels, *streams, *coupled_streams,
+         mapping, application, channels>2&&mapping_family==1);
+}
+
 OpusMSEncoder *opus_multistream_encoder_create(
       opus_int32 Fs,
       int channels,
@@ -141,7 +549,7 @@ OpusMSEncoder *opus_multistream_encoder_create(
    int ret;
    OpusMSEncoder *st;
    if ((channels>255) || (channels<1) || (coupled_streams>streams) ||
-       (coupled_streams+streams>255) || (streams<1) || (coupled_streams<0))
+       (streams<1) || (coupled_streams<0) || (streams>255-coupled_streams))
    {
       if (error)
          *error = OPUS_BAD_ARG;
@@ -165,14 +573,126 @@ OpusMSEncoder *opus_multistream_encoder_create(
    return st;
 }
 
-typedef void (*opus_copy_channel_in_func)(
-  opus_val16 *dst,
-  int dst_stride,
-  const void *src,
-  int src_stride,
-  int src_channel,
-  int frame_size
-);
+OpusMSEncoder *opus_multistream_surround_encoder_create(
+      opus_int32 Fs,
+      int channels,
+      int mapping_family,
+      int *streams,
+      int *coupled_streams,
+      unsigned char *mapping,
+      int application,
+      int *error
+)
+{
+   int ret;
+   opus_int32 size;
+   OpusMSEncoder *st;
+   if ((channels>255) || (channels<1))
+   {
+      if (error)
+         *error = OPUS_BAD_ARG;
+      return NULL;
+   }
+   size = opus_multistream_surround_encoder_get_size(channels, mapping_family);
+   if (!size)
+   {
+      if (error)
+         *error = OPUS_UNIMPLEMENTED;
+      return NULL;
+   }
+   st = (OpusMSEncoder *)opus_alloc(size);
+   if (st==NULL)
+   {
+      if (error)
+         *error = OPUS_ALLOC_FAIL;
+      return NULL;
+   }
+   ret = opus_multistream_surround_encoder_init(st, Fs, channels, mapping_family, streams, coupled_streams, mapping, application);
+   if (ret != OPUS_OK)
+   {
+      opus_free(st);
+      st = NULL;
+   }
+   if (error)
+      *error = ret;
+   return st;
+}
+
+static opus_int32 surround_rate_allocation(
+      OpusMSEncoder *st,
+      opus_int32 *rate,
+      int frame_size
+      )
+{
+   int i;
+   opus_int32 channel_rate;
+   opus_int32 Fs;
+   char *ptr;
+   int stream_offset;
+   int lfe_offset;
+   int coupled_ratio; /* Q8 */
+   int lfe_ratio;     /* Q8 */
+   opus_int32 rate_sum=0;
+
+   ptr = (char*)st + align(sizeof(OpusMSEncoder));
+   opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_SAMPLE_RATE(&Fs));
+
+   if (st->bitrate_bps > st->layout.nb_channels*40000)
+      stream_offset = 20000;
+   else
+      stream_offset = st->bitrate_bps/st->layout.nb_channels/2;
+   stream_offset += 60*(Fs/frame_size-50);
+   /* We start by giving each stream (coupled or uncoupled) the same bitrate.
+      This models the main saving of coupled channels over uncoupled. */
+   /* The LFE stream is an exception to the above and gets fewer bits. */
+   lfe_offset = 3500 + 60*(Fs/frame_size-50);
+   /* Coupled streams get twice the mono rate after the first 20 kb/s. */
+   coupled_ratio = 512;
+   /* Should depend on the bitrate, for now we assume LFE gets 1/8 the bits of mono */
+   lfe_ratio = 32;
+
+   /* Compute bitrate allocation between streams */
+   if (st->bitrate_bps==OPUS_AUTO)
+   {
+      channel_rate = Fs+60*Fs/frame_size;
+   } else if (st->bitrate_bps==OPUS_BITRATE_MAX)
+   {
+      channel_rate = 300000;
+   } else {
+      int nb_lfe;
+      int nb_uncoupled;
+      int nb_coupled;
+      int total;
+      nb_lfe = (st->lfe_stream!=-1);
+      nb_coupled = st->layout.nb_coupled_streams;
+      nb_uncoupled = st->layout.nb_streams-nb_coupled-nb_lfe;
+      total = (nb_uncoupled<<8)         /* mono */
+            + coupled_ratio*nb_coupled /* stereo */
+            + nb_lfe*lfe_ratio;
+      channel_rate = 256*(st->bitrate_bps-lfe_offset*nb_lfe-stream_offset*(nb_coupled+nb_uncoupled))/total;
+   }
+#ifndef FIXED_POINT
+   if (st->variable_duration==OPUS_FRAMESIZE_VARIABLE && frame_size != Fs/50)
+   {
+      opus_int32 bonus;
+      bonus = 60*(Fs/frame_size-50);
+      channel_rate += bonus;
+   }
+#endif
+
+   for (i=0;i<st->layout.nb_streams;i++)
+   {
+      if (i<st->layout.nb_coupled_streams)
+         rate[i] = stream_offset+(channel_rate*coupled_ratio>>8);
+      else if (i!=st->lfe_stream)
+         rate[i] = stream_offset+channel_rate;
+      else
+         rate[i] = lfe_offset+(channel_rate*lfe_ratio>>8);
+      rate[i] = IMAX(rate[i], 500);
+      rate_sum += rate[i];
+   }
+   return rate_sum;
+}
 
 /* Max size in case the encoder decides to return three frames */
 #define MS_FRAME_TMP (3*1275+7)
@@ -181,13 +701,12 @@ static int opus_multistream_encode_native
     OpusMSEncoder *st,
     opus_copy_channel_in_func copy_channel_in,
     const void *pcm,
-    int frame_size,
+    int analysis_frame_size,
     unsigned char *data,
     opus_int32 max_data_bytes,
-    int lsb_depth
-#ifndef FIXED_POINT
-    , downmix_func downmix
-#endif
+    int lsb_depth,
+    downmix_func downmix,
+    int float_api
 )
 {
    opus_int32 Fs;
@@ -197,40 +716,52 @@ static int opus_multistream_encode_native
    char *ptr;
    int tot_size;
    VARDECL(opus_val16, buf);
+   VARDECL(opus_val16, bandSMR);
    unsigned char tmp_data[MS_FRAME_TMP];
    OpusRepacketizer rp;
-   int orig_frame_size;
-   int coded_channels;
-   opus_int32 channel_rate;
+   opus_int32 vbr;
+   const CELTMode *celt_mode;
+   opus_int32 bitrates[256];
+   opus_val16 bandLogE[42];
+   opus_val32 *mem = NULL;
+   opus_val32 *preemph_mem=NULL;
+   int frame_size;
+   opus_int32 rate_sum;
+   opus_int32 smallest_packet;
    ALLOC_STACK;
 
+   if (st->surround)
+   {
+      preemph_mem = ms_get_preemph_mem(st);
+      mem = ms_get_window_mem(st);
+   }
+
    ptr = (char*)st + align(sizeof(OpusMSEncoder));
    opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_SAMPLE_RATE(&Fs));
+   opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_VBR(&vbr));
+   opus_encoder_ctl((OpusEncoder*)ptr, CELT_GET_MODE(&celt_mode));
 
-   if (400*frame_size < Fs)
-   {
-      RESTORE_STACK;
-      return OPUS_BAD_ARG;
-   }
-   orig_frame_size = IMIN(frame_size,Fs/50);
-   if (st->variable_duration)
    {
-      int LM = 3;
-      int channels;
       opus_int32 delay_compensation;
+      int channels;
 
       channels = st->layout.nb_streams + st->layout.nb_coupled_streams;
       opus_encoder_ctl((OpusEncoder*)ptr, OPUS_GET_LOOKAHEAD(&delay_compensation));
       delay_compensation -= Fs/400;
-#ifndef FIXED_POINT
-      LM = optimize_framesize(pcm, frame_size, channels, Fs, st->bitrate_bps,
-            0.f, st->subframe_mem, delay_compensation, downmix);
+      frame_size = compute_frame_size(pcm, analysis_frame_size,
+            st->variable_duration, channels, Fs, st->bitrate_bps,
+            delay_compensation, downmix
+#ifndef DISABLE_FLOAT_API
+            , st->subframe_mem
 #endif
-      while ((Fs/400<<LM)>frame_size)
-         LM--;
-      frame_size = (Fs/400<<LM);
+            );
    }
 
+   if (400*frame_size < Fs)
+   {
+      RESTORE_STACK;
+      return OPUS_BAD_ARG;
+   }
    /* Validate frame_size before using it to allocate stack space.
       This mirrors the checks in opus_encode[_float](). */
    if (400*frame_size != Fs && 200*frame_size != Fs &&
@@ -240,35 +771,38 @@ static int opus_multistream_encode_native
       RESTORE_STACK;
       return OPUS_BAD_ARG;
    }
-   ALLOC(buf, 2*frame_size, opus_val16);
-   coupled_size = opus_encoder_get_size(2);
-   mono_size = opus_encoder_get_size(1);
 
-   if (max_data_bytes < 4*st->layout.nb_streams-1)
+   /* Smallest packet the encoder can produce. */
+   smallest_packet = st->layout.nb_streams*2-1;
+   if (max_data_bytes < smallest_packet)
    {
       RESTORE_STACK;
       return OPUS_BUFFER_TOO_SMALL;
    }
+   ALLOC(buf, 2*frame_size, opus_val16);
+   coupled_size = opus_encoder_get_size(2);
+   mono_size = opus_encoder_get_size(1);
 
-   /* Compute bitrate allocation between streams (this could be a lot better) */
-   coded_channels = st->layout.nb_streams + st->layout.nb_coupled_streams;
-   if (st->bitrate_bps==OPUS_AUTO)
-   {
-      channel_rate = Fs+60*Fs/orig_frame_size;
-   } else if (st->bitrate_bps==OPUS_BITRATE_MAX)
+   ALLOC(bandSMR, 21*st->layout.nb_channels, opus_val16);
+   if (st->surround)
    {
-      channel_rate = 300000;
-   } else {
-      channel_rate = st->bitrate_bps/coded_channels;
+      surround_analysis(celt_mode, pcm, bandSMR, mem, preemph_mem, frame_size, 120, st->layout.nb_channels, Fs, copy_channel_in, st->arch);
    }
-#ifndef FIXED_POINT
-   if (st->variable_duration && orig_frame_size != frame_size)
+
+   /* Compute bitrate allocation between streams (this could be a lot better) */
+   rate_sum = surround_rate_allocation(st, bitrates, frame_size);
+
+   if (!vbr)
    {
-      opus_int32 bonus;
-      bonus = 60*(48000/frame_size-48000/orig_frame_size);
-      channel_rate += bonus;
+      if (st->bitrate_bps == OPUS_AUTO)
+      {
+         max_data_bytes = IMIN(max_data_bytes, 3*rate_sum/(3*8*Fs/frame_size));
+      } else if (st->bitrate_bps != OPUS_BITRATE_MAX)
+      {
+         max_data_bytes = IMIN(max_data_bytes, IMAX(smallest_packet,
+                          3*st->bitrate_bps/(3*8*Fs/frame_size)));
+      }
    }
-#endif
    ptr = (char*)st + align(sizeof(OpusMSEncoder));
    for (s=0;s<st->layout.nb_streams;s++)
    {
@@ -278,7 +812,28 @@ static int opus_multistream_encode_native
          ptr += align(coupled_size);
       else
          ptr += align(mono_size);
-      opus_encoder_ctl(enc, OPUS_SET_BITRATE(channel_rate * (s < st->layout.nb_coupled_streams ? 2 : 1)));
+      opus_encoder_ctl(enc, OPUS_SET_BITRATE(bitrates[s]));
+      if (st->surround)
+      {
+         opus_int32 equiv_rate;
+         equiv_rate = st->bitrate_bps;
+         if (frame_size*50 < Fs)
+            equiv_rate -= 60*(Fs/frame_size - 50)*st->layout.nb_channels;
+         if (equiv_rate > 10000*st->layout.nb_channels)
+            opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_FULLBAND));
+         else if (equiv_rate > 7000*st->layout.nb_channels)
+            opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_SUPERWIDEBAND));
+         else if (equiv_rate > 5000*st->layout.nb_channels)
+            opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_WIDEBAND));
+         else
+            opus_encoder_ctl(enc, OPUS_SET_BANDWIDTH(OPUS_BANDWIDTH_NARROWBAND));
+         if (s < st->layout.nb_coupled_streams)
+         {
+            /* To preserve the spatial image, force stereo CELT on coupled streams */
+            opus_encoder_ctl(enc, OPUS_SET_FORCE_MODE(MODE_CELT_ONLY));
+            opus_encoder_ctl(enc, OPUS_SET_FORCE_CHANNELS(2));
+         }
+      }
    }
 
    ptr = (char*)st + align(sizeof(OpusMSEncoder));
@@ -289,11 +844,13 @@ static int opus_multistream_encode_native
       OpusEncoder *enc;
       int len;
       int curr_max;
+      int c1, c2;
 
       opus_repacketizer_init(&rp);
       enc = (OpusEncoder*)ptr;
       if (s < st->layout.nb_coupled_streams)
       {
+         int i;
          int left, right;
          left = get_left_channel(&st->layout, s, -1);
          right = get_right_channel(&st->layout, s, -1);
@@ -302,18 +859,43 @@ static int opus_multistream_encode_native
          (*copy_channel_in)(buf+1, 2,
             pcm, st->layout.nb_channels, right, frame_size);
          ptr += align(coupled_size);
+         if (st->surround)
+         {
+            for (i=0;i<21;i++)
+            {
+               bandLogE[i] = bandSMR[21*left+i];
+               bandLogE[21+i] = bandSMR[21*right+i];
+            }
+         }
+         c1 = left;
+         c2 = right;
       } else {
+         int i;
          int chan = get_mono_channel(&st->layout, s, -1);
          (*copy_channel_in)(buf, 1,
             pcm, st->layout.nb_channels, chan, frame_size);
          ptr += align(mono_size);
+         if (st->surround)
+         {
+            for (i=0;i<21;i++)
+               bandLogE[i] = bandSMR[21*chan+i];
+         }
+         c1 = chan;
+         c2 = -1;
       }
+      if (st->surround)
+         opus_encoder_ctl(enc, OPUS_SET_ENERGY_MASK(bandLogE));
       /* number of bytes left (+Toc) */
       curr_max = max_data_bytes - tot_size;
-      /* Reserve three bytes for the last stream and four for the others */
-      curr_max -= IMAX(0,4*(st->layout.nb_streams-s-1)-1);
+      /* Reserve one byte for the last stream and two for the others */
+      curr_max -= IMAX(0,2*(st->layout.nb_streams-s-1)-1);
       curr_max = IMIN(curr_max,MS_FRAME_TMP);
-      len = opus_encode_native(enc, buf, frame_size, tmp_data, curr_max, lsb_depth);
+      /* Repacketizer will add one or two bytes for self-delimited frames */
+      if (s != st->layout.nb_streams-1) curr_max -=  curr_max>253 ? 2 : 1;
+      if (!vbr && s == st->layout.nb_streams-1)
+         opus_encoder_ctl(enc, OPUS_SET_BITRATE(curr_max*(8*Fs/frame_size)));
+      len = opus_encode_native(enc, buf, frame_size, tmp_data, curr_max, lsb_depth,
+            pcm, analysis_frame_size, c1, c2, st->layout.nb_channels, downmix, float_api);
       if (len<0)
       {
          RESTORE_STACK;
@@ -323,13 +905,14 @@ static int opus_multistream_encode_native
          while taking into account the fact that the encoder can now return
          more than one frame at a time (e.g. 60 ms CELT-only) */
       opus_repacketizer_cat(&rp, tmp_data, len);
-      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);
+      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, !vbr && s == st->layout.nb_streams-1);
       data += len;
       tot_size += len;
    }
+   /*printf("\n");*/
    RESTORE_STACK;
    return tot_size;
-
 }
 
 #if !defined(DISABLE_FLOAT_API)
@@ -343,7 +926,7 @@ static void opus_copy_channel_in_float(
 )
 {
    const float *float_src;
-   int i;
+   opus_int32 i;
    float_src = (const float *)src;
    for (i=0;i<frame_size;i++)
 #if defined(FIXED_POINT)
@@ -364,7 +947,7 @@ static void opus_copy_channel_in_short(
 )
 {
    const opus_int16 *short_src;
-   int i;
+   opus_int32 i;
    short_src = (const opus_int16 *)src;
    for (i=0;i<frame_size;i++)
 #if defined(FIXED_POINT)
@@ -374,6 +957,7 @@ static void opus_copy_channel_in_short(
 #endif
 }
 
+
 #ifdef FIXED_POINT
 int opus_multistream_encode(
     OpusMSEncoder *st,
@@ -384,7 +968,7 @@ int opus_multistream_encode(
 )
 {
    return opus_multistream_encode_native(st, opus_copy_channel_in_short,
-      pcm, frame_size, data, max_data_bytes, 16);
+      pcm, frame_size, data, max_data_bytes, 16, downmix_int, 0);
 }
 
 #ifndef DISABLE_FLOAT_API
@@ -397,7 +981,7 @@ int opus_multistream_encode_float(
 )
 {
    return opus_multistream_encode_native(st, opus_copy_channel_in_float,
-      pcm, frame_size, data, max_data_bytes, 16);
+      pcm, frame_size, data, max_data_bytes, 16, downmix_float, 1);
 }
 #endif
 
@@ -413,7 +997,7 @@ int opus_multistream_encode_float
 )
 {
    return opus_multistream_encode_native(st, opus_copy_channel_in_float,
-      pcm, frame_size, data, max_data_bytes, 24, downmix_float);
+      pcm, frame_size, data, max_data_bytes, 24, downmix_float, 1);
 }
 
 int opus_multistream_encode(
@@ -425,7 +1009,7 @@ int opus_multistream_encode(
 )
 {
    return opus_multistream_encode_native(st, opus_copy_channel_in_short,
-      pcm, frame_size, data, max_data_bytes, 16, downmix_int);
+      pcm, frame_size, data, max_data_bytes, 16, downmix_int, 0);
 }
 #endif
 
@@ -447,7 +1031,9 @@ int opus_multistream_encoder_ctl(OpusMSEncoder *st, int request, ...)
    {
       opus_int32 value = va_arg(ap, opus_int32);
       if (value<0 && value!=OPUS_AUTO && value!=OPUS_BITRATE_MAX)
+      {
          goto bad_arg;
+      }
       st->bitrate_bps = value;
    }
    break;
@@ -455,6 +1041,10 @@ int opus_multistream_encoder_ctl(OpusMSEncoder *st, int request, ...)
    {
       int s;
       opus_int32 *value = va_arg(ap, opus_int32*);
+      if (!value)
+      {
+         goto bad_arg;
+      }
       *value = 0;
       for (s=0;s<st->layout.nb_streams;s++)
       {
@@ -484,6 +1074,7 @@ int opus_multistream_encoder_ctl(OpusMSEncoder *st, int request, ...)
    case OPUS_GET_SAMPLE_RATE_REQUEST:
    case OPUS_GET_INBAND_FEC_REQUEST:
    case OPUS_GET_FORCE_CHANNELS_REQUEST:
+   case OPUS_GET_PREDICTION_DISABLED_REQUEST:
    {
       OpusEncoder *enc;
       /* For int32* GET params, just query the first stream */
@@ -497,6 +1088,10 @@ int opus_multistream_encoder_ctl(OpusMSEncoder *st, int request, ...)
       int s;
       opus_uint32 *value = va_arg(ap, opus_uint32*);
       opus_uint32 tmp;
+      if (!value)
+      {
+         goto bad_arg;
+      }
       *value=0;
       for (s=0;s<st->layout.nb_streams;s++)
       {
@@ -516,6 +1111,7 @@ int opus_multistream_encoder_ctl(OpusMSEncoder *st, int request, ...)
    case OPUS_SET_COMPLEXITY_REQUEST:
    case OPUS_SET_VBR_REQUEST:
    case OPUS_SET_VBR_CONSTRAINT_REQUEST:
+   case OPUS_SET_MAX_BANDWIDTH_REQUEST:
    case OPUS_SET_BANDWIDTH_REQUEST:
    case OPUS_SET_SIGNAL_REQUEST:
    case OPUS_SET_APPLICATION_REQUEST:
@@ -524,6 +1120,7 @@ int opus_multistream_encoder_ctl(OpusMSEncoder *st, int request, ...)
    case OPUS_SET_DTX_REQUEST:
    case OPUS_SET_FORCE_MODE_REQUEST:
    case OPUS_SET_FORCE_CHANNELS_REQUEST:
+   case OPUS_SET_PREDICTION_DISABLED_REQUEST:
    {
       int s;
       /* This works for int32 params */
@@ -552,6 +1149,10 @@ int opus_multistream_encoder_ctl(OpusMSEncoder *st, int request, ...)
       if (stream_id<0 || stream_id >= st->layout.nb_streams)
          ret = OPUS_BAD_ARG;
       value = va_arg(ap, OpusEncoder**);
+      if (!value)
+      {
+         goto bad_arg;
+      }
       for (s=0;s<stream_id;s++)
       {
          if (s < st->layout.nb_coupled_streams)
@@ -562,20 +1163,45 @@ int opus_multistream_encoder_ctl(OpusMSEncoder *st, int request, ...)
       *value = (OpusEncoder*)ptr;
    }
    break;
-   case OPUS_SET_EXPERT_VARIABLE_DURATION_REQUEST:
+   case OPUS_SET_EXPERT_FRAME_DURATION_REQUEST:
    {
        opus_int32 value = va_arg(ap, opus_int32);
-       if (value<0 || value>1)
-          goto bad_arg;
        st->variable_duration = value;
    }
    break;
-   case OPUS_GET_EXPERT_VARIABLE_DURATION_REQUEST:
+   case OPUS_GET_EXPERT_FRAME_DURATION_REQUEST:
    {
        opus_int32 *value = va_arg(ap, opus_int32*);
+       if (!value)
+       {
+          goto bad_arg;
+       }
        *value = st->variable_duration;
    }
    break;
+   case OPUS_RESET_STATE:
+   {
+      int s;
+      st->subframe_mem[0] = st->subframe_mem[1] = st->subframe_mem[2] = 0;
+      if (st->surround)
+      {
+         OPUS_CLEAR(ms_get_preemph_mem(st), st->layout.nb_channels);
+         OPUS_CLEAR(ms_get_window_mem(st), st->layout.nb_channels*120);
+      }
+      for (s=0;s<st->layout.nb_streams;s++)
+      {
+         OpusEncoder *enc;
+         enc = (OpusEncoder*)ptr;
+         if (s < st->layout.nb_coupled_streams)
+            ptr += align(coupled_size);
+         else
+            ptr += align(mono_size);
+         ret = opus_encoder_ctl(enc, OPUS_RESET_STATE);
+         if (ret != OPUS_OK)
+            break;
+      }
+   }
+   break;
    default:
       ret = OPUS_UNIMPLEMENTED;
       break;
@@ -592,5 +1218,3 @@ void opus_multistream_encoder_destroy(OpusMSEncoder *st)
 {
     opus_free(st);
 }
-
-