michael@0: /* Copyright (c) 2011 Xiph.Org Foundation, Skype Limited
michael@0:    Written by Jean-Marc Valin and Koen Vos */
michael@0: /*
michael@0:    Redistribution and use in source and binary forms, with or without
michael@0:    modification, are permitted provided that the following conditions
michael@0:    are met:
michael@0: 
michael@0:    - Redistributions of source code must retain the above copyright
michael@0:    notice, this list of conditions and the following disclaimer.
michael@0: 
michael@0:    - Redistributions in binary form must reproduce the above copyright
michael@0:    notice, this list of conditions and the following disclaimer in the
michael@0:    documentation and/or other materials provided with the distribution.
michael@0: 
michael@0:    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
michael@0:    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
michael@0:    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
michael@0:    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
michael@0:    OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
michael@0:    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
michael@0:    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
michael@0:    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
michael@0:    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
michael@0:    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
michael@0:    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
michael@0: */
michael@0: 
michael@0: #ifdef HAVE_CONFIG_H
michael@0: #include "config.h"
michael@0: #endif
michael@0: 
michael@0: #include "opus.h"
michael@0: #include "opus_private.h"
michael@0: 
michael@0: #ifndef DISABLE_FLOAT_API
michael@0: OPUS_EXPORT void opus_pcm_soft_clip(float *_x, int N, int C, float *declip_mem)
michael@0: {
michael@0:    int c;
michael@0:    int i;
michael@0:    float *x;
michael@0: 
michael@0:    if (C<1 || N<1 || !_x || !declip_mem) return;
michael@0: 
michael@0:    /* First thing: saturate everything to +/- 2 which is the highest level our
michael@0:       non-linearity can handle. At the point where the signal reaches +/-2,
michael@0:       the derivative will be zero anyway, so this doesn't introduce any
michael@0:       discontinuity in the derivative. */
michael@0:    for (i=0;i<N*C;i++)
michael@0:       _x[i] = MAX16(-2.f, MIN16(2.f, _x[i]));
michael@0:    for (c=0;c<C;c++)
michael@0:    {
michael@0:       float a;
michael@0:       float x0;
michael@0:       int curr;
michael@0: 
michael@0:       x = _x+c;
michael@0:       a = declip_mem[c];
michael@0:       /* Continue applying the non-linearity from the previous frame to avoid
michael@0:          any discontinuity. */
michael@0:       for (i=0;i<N;i++)
michael@0:       {
michael@0:          if (x[i*C]*a>=0)
michael@0:             break;
michael@0:          x[i*C] = x[i*C]+a*x[i*C]*x[i*C];
michael@0:       }
michael@0: 
michael@0:       curr=0;
michael@0:       x0 = x[0];
michael@0:       while(1)
michael@0:       {
michael@0:          int start, end;
michael@0:          float maxval;
michael@0:          int special=0;
michael@0:          int peak_pos;
michael@0:          for (i=curr;i<N;i++)
michael@0:          {
michael@0:             if (x[i*C]>1 || x[i*C]<-1)
michael@0:                break;
michael@0:          }
michael@0:          if (i==N)
michael@0:          {
michael@0:             a=0;
michael@0:             break;
michael@0:          }
michael@0:          peak_pos = i;
michael@0:          start=end=i;
michael@0:          maxval=ABS16(x[i*C]);
michael@0:          /* Look for first zero crossing before clipping */
michael@0:          while (start>0 && x[i*C]*x[(start-1)*C]>=0)
michael@0:             start--;
michael@0:          /* Look for first zero crossing after clipping */
michael@0:          while (end<N && x[i*C]*x[end*C]>=0)
michael@0:          {
michael@0:             /* Look for other peaks until the next zero-crossing. */
michael@0:             if (ABS16(x[end*C])>maxval)
michael@0:             {
michael@0:                maxval = ABS16(x[end*C]);
michael@0:                peak_pos = end;
michael@0:             }
michael@0:             end++;
michael@0:          }
michael@0:          /* Detect the special case where we clip before the first zero crossing */
michael@0:          special = (start==0 && x[i*C]*x[0]>=0);
michael@0: 
michael@0:          /* Compute a such that maxval + a*maxval^2 = 1 */
michael@0:          a=(maxval-1)/(maxval*maxval);
michael@0:          if (x[i*C]>0)
michael@0:             a = -a;
michael@0:          /* Apply soft clipping */
michael@0:          for (i=start;i<end;i++)
michael@0:             x[i*C] = x[i*C]+a*x[i*C]*x[i*C];
michael@0: 
michael@0:          if (special && peak_pos>=2)
michael@0:          {
michael@0:             /* Add a linear ramp from the first sample to the signal peak.
michael@0:                This avoids a discontinuity at the beginning of the frame. */
michael@0:             float delta;
michael@0:             float offset = x0-x[0];
michael@0:             delta = offset / peak_pos;
michael@0:             for (i=curr;i<peak_pos;i++)
michael@0:             {
michael@0:                offset -= delta;
michael@0:                x[i*C] += offset;
michael@0:                x[i*C] = MAX16(-1.f, MIN16(1.f, x[i*C]));
michael@0:             }
michael@0:          }
michael@0:          curr = end;
michael@0:          if (curr==N)
michael@0:             break;
michael@0:       }
michael@0:       declip_mem[c] = a;
michael@0:    }
michael@0: }
michael@0: #endif
michael@0: 
michael@0: int encode_size(int size, unsigned char *data)
michael@0: {
michael@0:    if (size < 252)
michael@0:    {
michael@0:       data[0] = size;
michael@0:       return 1;
michael@0:    } else {
michael@0:       data[0] = 252+(size&0x3);
michael@0:       data[1] = (size-(int)data[0])>>2;
michael@0:       return 2;
michael@0:    }
michael@0: }
michael@0: 
michael@0: static int parse_size(const unsigned char *data, opus_int32 len, opus_int16 *size)
michael@0: {
michael@0:    if (len<1)
michael@0:    {
michael@0:       *size = -1;
michael@0:       return -1;
michael@0:    } else if (data[0]<252)
michael@0:    {
michael@0:       *size = data[0];
michael@0:       return 1;
michael@0:    } else if (len<2)
michael@0:    {
michael@0:       *size = -1;
michael@0:       return -1;
michael@0:    } else {
michael@0:       *size = 4*data[1] + data[0];
michael@0:       return 2;
michael@0:    }
michael@0: }
michael@0: 
michael@0: int opus_packet_parse_impl(const unsigned char *data, opus_int32 len,
michael@0:       int self_delimited, unsigned char *out_toc,
michael@0:       const unsigned char *frames[48], opus_int16 size[48],
michael@0:       int *payload_offset, opus_int32 *packet_offset)
michael@0: {
michael@0:    int i, bytes;
michael@0:    int count;
michael@0:    int cbr;
michael@0:    unsigned char ch, toc;
michael@0:    int framesize;
michael@0:    opus_int32 last_size;
michael@0:    opus_int32 pad = 0;
michael@0:    const unsigned char *data0 = data;
michael@0: 
michael@0:    if (size==NULL)
michael@0:       return OPUS_BAD_ARG;
michael@0: 
michael@0:    framesize = opus_packet_get_samples_per_frame(data, 48000);
michael@0: 
michael@0:    cbr = 0;
michael@0:    toc = *data++;
michael@0:    len--;
michael@0:    last_size = len;
michael@0:    switch (toc&0x3)
michael@0:    {
michael@0:    /* One frame */
michael@0:    case 0:
michael@0:       count=1;
michael@0:       break;
michael@0:    /* Two CBR frames */
michael@0:    case 1:
michael@0:       count=2;
michael@0:       cbr = 1;
michael@0:       if (!self_delimited)
michael@0:       {
michael@0:          if (len&0x1)
michael@0:             return OPUS_INVALID_PACKET;
michael@0:          last_size = len/2;
michael@0:          /* If last_size doesn't fit in size[0], we'll catch it later */
michael@0:          size[0] = (opus_int16)last_size;
michael@0:       }
michael@0:       break;
michael@0:    /* Two VBR frames */
michael@0:    case 2:
michael@0:       count = 2;
michael@0:       bytes = parse_size(data, len, size);
michael@0:       len -= bytes;
michael@0:       if (size[0]<0 || size[0] > len)
michael@0:          return OPUS_INVALID_PACKET;
michael@0:       data += bytes;
michael@0:       last_size = len-size[0];
michael@0:       break;
michael@0:    /* Multiple CBR/VBR frames (from 0 to 120 ms) */
michael@0:    default: /*case 3:*/
michael@0:       if (len<1)
michael@0:          return OPUS_INVALID_PACKET;
michael@0:       /* Number of frames encoded in bits 0 to 5 */
michael@0:       ch = *data++;
michael@0:       count = ch&0x3F;
michael@0:       if (count <= 0 || framesize*count > 5760)
michael@0:          return OPUS_INVALID_PACKET;
michael@0:       len--;
michael@0:       /* Padding flag is bit 6 */
michael@0:       if (ch&0x40)
michael@0:       {
michael@0:          int p;
michael@0:          do {
michael@0:             int tmp;
michael@0:             if (len<=0)
michael@0:                return OPUS_INVALID_PACKET;
michael@0:             p = *data++;
michael@0:             len--;
michael@0:             tmp = p==255 ? 254: p;
michael@0:             len -= tmp;
michael@0:             pad += tmp;
michael@0:          } while (p==255);
michael@0:       }
michael@0:       if (len<0)
michael@0:          return OPUS_INVALID_PACKET;
michael@0:       /* VBR flag is bit 7 */
michael@0:       cbr = !(ch&0x80);
michael@0:       if (!cbr)
michael@0:       {
michael@0:          /* VBR case */
michael@0:          last_size = len;
michael@0:          for (i=0;i<count-1;i++)
michael@0:          {
michael@0:             bytes = parse_size(data, len, size+i);
michael@0:             len -= bytes;
michael@0:             if (size[i]<0 || size[i] > len)
michael@0:                return OPUS_INVALID_PACKET;
michael@0:             data += bytes;
michael@0:             last_size -= bytes+size[i];
michael@0:          }
michael@0:          if (last_size<0)
michael@0:             return OPUS_INVALID_PACKET;
michael@0:       } else if (!self_delimited)
michael@0:       {
michael@0:          /* CBR case */
michael@0:          last_size = len/count;
michael@0:          if (last_size*count!=len)
michael@0:             return OPUS_INVALID_PACKET;
michael@0:          for (i=0;i<count-1;i++)
michael@0:             size[i] = (opus_int16)last_size;
michael@0:       }
michael@0:       break;
michael@0:    }
michael@0:    /* Self-delimited framing has an extra size for the last frame. */
michael@0:    if (self_delimited)
michael@0:    {
michael@0:       bytes = parse_size(data, len, size+count-1);
michael@0:       len -= bytes;
michael@0:       if (size[count-1]<0 || size[count-1] > len)
michael@0:          return OPUS_INVALID_PACKET;
michael@0:       data += bytes;
michael@0:       /* For CBR packets, apply the size to all the frames. */
michael@0:       if (cbr)
michael@0:       {
michael@0:          if (size[count-1]*count > len)
michael@0:             return OPUS_INVALID_PACKET;
michael@0:          for (i=0;i<count-1;i++)
michael@0:             size[i] = size[count-1];
michael@0:       } else if (bytes+size[count-1] > last_size)
michael@0:          return OPUS_INVALID_PACKET;
michael@0:    } else
michael@0:    {
michael@0:       /* Because it's not encoded explicitly, it's possible the size of the
michael@0:          last packet (or all the packets, for the CBR case) is larger than
michael@0:          1275. Reject them here.*/
michael@0:       if (last_size > 1275)
michael@0:          return OPUS_INVALID_PACKET;
michael@0:       size[count-1] = (opus_int16)last_size;
michael@0:    }
michael@0: 
michael@0:    if (payload_offset)
michael@0:       *payload_offset = (int)(data-data0);
michael@0: 
michael@0:    for (i=0;i<count;i++)
michael@0:    {
michael@0:       if (frames)
michael@0:          frames[i] = data;
michael@0:       data += size[i];
michael@0:    }
michael@0: 
michael@0:    if (packet_offset)
michael@0:       *packet_offset = pad+(opus_int32)(data-data0);
michael@0: 
michael@0:    if (out_toc)
michael@0:       *out_toc = toc;
michael@0: 
michael@0:    return count;
michael@0: }
michael@0: 
michael@0: int opus_packet_parse(const unsigned char *data, opus_int32 len,
michael@0:       unsigned char *out_toc, const unsigned char *frames[48],
michael@0:       opus_int16 size[48], int *payload_offset)
michael@0: {
michael@0:    return opus_packet_parse_impl(data, len, 0, out_toc,
michael@0:                                  frames, size, payload_offset, NULL);
michael@0: }
michael@0: