Fix compiler warnings
[opus.git] / src / opus.c
index 0d68dad..cdbd13a 100644 (file)
@@ -15,8 +15,8 @@
    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
-   A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
-   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+   A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
+   OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 #include "opus.h"
 #include "opus_private.h"
 
+#ifndef DISABLE_FLOAT_API
+OPUS_EXPORT void opus_pcm_soft_clip(float *_x, int N, int C, float *declip_mem)
+{
+   int c;
+   int i;
+   float *x;
+
+   if (C<1 || N<1 || !_x || !declip_mem) return;
+
+   /* First thing: saturate everything to +/- 2 which is the highest level our
+      non-linearity can handle. At the point where the signal reaches +/-2,
+      the derivative will be zero anyway, so this doesn't introduce any
+      discontinuity in the derivative. */
+   for (i=0;i<N*C;i++)
+      _x[i] = MAX16(-2.f, MIN16(2.f, _x[i]));
+   for (c=0;c<C;c++)
+   {
+      float a;
+      float x0;
+      int curr;
+
+      x = _x+c;
+      a = declip_mem[c];
+      /* Continue applying the non-linearity from the previous frame to avoid
+         any discontinuity. */
+      for (i=0;i<N;i++)
+      {
+         if (x[i*C]*a>=0)
+            break;
+         x[i*C] = x[i*C]+a*x[i*C]*x[i*C];
+      }
+
+      curr=0;
+      x0 = x[0];
+      while(1)
+      {
+         int start, end;
+         float maxval;
+         int special=0;
+         int peak_pos;
+         for (i=curr;i<N;i++)
+         {
+            if (x[i*C]>1 || x[i*C]<-1)
+               break;
+         }
+         if (i==N)
+         {
+            a=0;
+            break;
+         }
+         peak_pos = i;
+         start=end=i;
+         maxval=ABS16(x[i*C]);
+         /* Look for first zero crossing before clipping */
+         while (start>0 && x[i*C]*x[(start-1)*C]>=0)
+            start--;
+         /* Look for first zero crossing after clipping */
+         while (end<N && x[i*C]*x[end*C]>=0)
+         {
+            /* Look for other peaks until the next zero-crossing. */
+            if (ABS16(x[end*C])>maxval)
+            {
+               maxval = ABS16(x[end*C]);
+               peak_pos = end;
+            }
+            end++;
+         }
+         /* Detect the special case where we clip before the first zero crossing */
+         special = (start==0 && x[i*C]*x[0]>=0);
+
+         /* Compute a such that maxval + a*maxval^2 = 1 */
+         a=(maxval-1)/(maxval*maxval);
+         /* Slightly boost "a" by 2^-22. This is just enough to ensure -ffast-math
+            does not cause output values larger than +/-1, but small enough not
+            to matter even for 24-bit output.  */
+         a += a*2.4e-7f;
+         if (x[i*C]>0)
+            a = -a;
+         /* Apply soft clipping */
+         for (i=start;i<end;i++)
+            x[i*C] = x[i*C]+a*x[i*C]*x[i*C];
+
+         if (special && peak_pos>=2)
+         {
+            /* Add a linear ramp from the first sample to the signal peak.
+               This avoids a discontinuity at the beginning of the frame. */
+            float delta;
+            float offset = x0-x[0];
+            delta = offset / peak_pos;
+            for (i=curr;i<peak_pos;i++)
+            {
+               offset -= delta;
+               x[i*C] += offset;
+               x[i*C] = MAX16(-1.f, MIN16(1.f, x[i*C]));
+            }
+         }
+         curr = end;
+         if (curr==N)
+            break;
+      }
+      declip_mem[c] = a;
+   }
+}
+#endif
+
 int encode_size(int size, unsigned char *data)
 {
    if (size < 252)
@@ -45,3 +150,207 @@ int encode_size(int size, unsigned char *data)
    }
 }
 
+static int parse_size(const unsigned char *data, opus_int32 len, opus_int16 *size)
+{
+   if (len<1)
+   {
+      *size = -1;
+      return -1;
+   } else if (data[0]<252)
+   {
+      *size = data[0];
+      return 1;
+   } else if (len<2)
+   {
+      *size = -1;
+      return -1;
+   } else {
+      *size = 4*data[1] + data[0];
+      return 2;
+   }
+}
+
+int opus_packet_get_samples_per_frame(const unsigned char *data,
+      opus_int32 Fs)
+{
+   int audiosize;
+   if (data[0]&0x80)
+   {
+      audiosize = ((data[0]>>3)&0x3);
+      audiosize = (Fs<<audiosize)/400;
+   } else if ((data[0]&0x60) == 0x60)
+   {
+      audiosize = (data[0]&0x08) ? Fs/50 : Fs/100;
+   } else {
+      audiosize = ((data[0]>>3)&0x3);
+      if (audiosize == 3)
+         audiosize = Fs*60/1000;
+      else
+         audiosize = (Fs<<audiosize)/100;
+   }
+   return audiosize;
+}
+
+int opus_packet_parse_impl(const unsigned char *data, opus_int32 len,
+      int self_delimited, unsigned char *out_toc,
+      const unsigned char *frames[48], opus_int16 size[48],
+      int *payload_offset, opus_int32 *packet_offset)
+{
+   int i, bytes;
+   int count;
+   int cbr;
+   unsigned char ch, toc;
+   int framesize;
+   opus_int32 last_size;
+   opus_int32 pad = 0;
+   const unsigned char *data0 = data;
+
+   if (size==NULL || len<0)
+      return OPUS_BAD_ARG;
+   if (len==0)
+      return OPUS_INVALID_PACKET;
+
+   framesize = opus_packet_get_samples_per_frame(data, 48000);
+
+   cbr = 0;
+   toc = *data++;
+   len--;
+   last_size = len;
+   switch (toc&0x3)
+   {
+   /* One frame */
+   case 0:
+      count=1;
+      break;
+   /* Two CBR frames */
+   case 1:
+      count=2;
+      cbr = 1;
+      if (!self_delimited)
+      {
+         if (len&0x1)
+            return OPUS_INVALID_PACKET;
+         last_size = len/2;
+         /* If last_size doesn't fit in size[0], we'll catch it later */
+         size[0] = (opus_int16)last_size;
+      }
+      break;
+   /* Two VBR frames */
+   case 2:
+      count = 2;
+      bytes = parse_size(data, len, size);
+      len -= bytes;
+      if (size[0]<0 || size[0] > len)
+         return OPUS_INVALID_PACKET;
+      data += bytes;
+      last_size = len-size[0];
+      break;
+   /* Multiple CBR/VBR frames (from 0 to 120 ms) */
+   default: /*case 3:*/
+      if (len<1)
+         return OPUS_INVALID_PACKET;
+      /* Number of frames encoded in bits 0 to 5 */
+      ch = *data++;
+      count = ch&0x3F;
+      if (count <= 0 || framesize*count > 5760)
+         return OPUS_INVALID_PACKET;
+      len--;
+      /* Padding flag is bit 6 */
+      if (ch&0x40)
+      {
+         int p;
+         do {
+            int tmp;
+            if (len<=0)
+               return OPUS_INVALID_PACKET;
+            p = *data++;
+            len--;
+            tmp = p==255 ? 254: p;
+            len -= tmp;
+            pad += tmp;
+         } while (p==255);
+      }
+      if (len<0)
+         return OPUS_INVALID_PACKET;
+      /* VBR flag is bit 7 */
+      cbr = !(ch&0x80);
+      if (!cbr)
+      {
+         /* VBR case */
+         last_size = len;
+         for (i=0;i<count-1;i++)
+         {
+            bytes = parse_size(data, len, size+i);
+            len -= bytes;
+            if (size[i]<0 || size[i] > len)
+               return OPUS_INVALID_PACKET;
+            data += bytes;
+            last_size -= bytes+size[i];
+         }
+         if (last_size<0)
+            return OPUS_INVALID_PACKET;
+      } else if (!self_delimited)
+      {
+         /* CBR case */
+         last_size = len/count;
+         if (last_size*count!=len)
+            return OPUS_INVALID_PACKET;
+         for (i=0;i<count-1;i++)
+            size[i] = (opus_int16)last_size;
+      }
+      break;
+   }
+   /* Self-delimited framing has an extra size for the last frame. */
+   if (self_delimited)
+   {
+      bytes = parse_size(data, len, size+count-1);
+      len -= bytes;
+      if (size[count-1]<0 || size[count-1] > len)
+         return OPUS_INVALID_PACKET;
+      data += bytes;
+      /* For CBR packets, apply the size to all the frames. */
+      if (cbr)
+      {
+         if (size[count-1]*count > len)
+            return OPUS_INVALID_PACKET;
+         for (i=0;i<count-1;i++)
+            size[i] = size[count-1];
+      } else if (bytes+size[count-1] > last_size)
+         return OPUS_INVALID_PACKET;
+   } else
+   {
+      /* Because it's not encoded explicitly, it's possible the size of the
+         last packet (or all the packets, for the CBR case) is larger than
+         1275. Reject them here.*/
+      if (last_size > 1275)
+         return OPUS_INVALID_PACKET;
+      size[count-1] = (opus_int16)last_size;
+   }
+
+   if (payload_offset)
+      *payload_offset = (int)(data-data0);
+
+   for (i=0;i<count;i++)
+   {
+      if (frames)
+         frames[i] = data;
+      data += size[i];
+   }
+
+   if (packet_offset)
+      *packet_offset = pad+(opus_int32)(data-data0);
+
+   if (out_toc)
+      *out_toc = toc;
+
+   return count;
+}
+
+int opus_packet_parse(const unsigned char *data, opus_int32 len,
+      unsigned char *out_toc, const unsigned char *frames[48],
+      opus_int16 size[48], int *payload_offset)
+{
+   return opus_packet_parse_impl(data, len, 0, out_toc,
+                                 frames, size, payload_offset, NULL);
+}
+