Adds functions for multistream padding/unpadding and single-stream unpadding
[opus.git] / src / opus.c
1 /* Copyright (c) 2011 Xiph.Org Foundation, Skype Limited
2    Written by Jean-Marc Valin and Koen Vos */
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.h"
33 #include "opus_private.h"
34
35 #ifndef DISABLE_FLOAT_API
36 OPUS_EXPORT void opus_pcm_soft_clip(float *_x, int N, int C, float *declip_mem)
37 {
38    int c;
39    int i;
40    float *x;
41
42    /* First thing: saturate everything to +/- 2 which is the highest level our
43       non-linearity can handle. At the point where the signal reaches +/-2,
44       the derivative will be zero anyway, so this doesn't introduce any
45       discontinuity in the derivative. */
46    for (i=0;i<N*C;i++)
47       _x[i] = MAX16(-2.f, MIN16(2.f, _x[i]));
48    for (c=0;c<C;c++)
49    {
50       float a;
51       float x0;
52       int curr;
53
54       x = _x+c;
55       a = declip_mem[c];
56       /* Continue applying the non-linearity from the previous frame to avoid
57          any discontinuity. */
58       for (i=0;i<N;i++)
59       {
60          if (x[i*C]*a>=0)
61             break;
62          x[i*C] = x[i*C]+a*x[i*C]*x[i*C];
63       }
64
65       curr=0;
66       x0 = x[0];
67       while(1)
68       {
69          int start, end;
70          float maxval;
71          int special=0;
72          int peak_pos;
73          for (i=curr;i<N;i++)
74          {
75             if (x[i*C]>1 || x[i*C]<-1)
76                break;
77          }
78          if (i==N)
79          {
80             a=0;
81             break;
82          }
83          peak_pos = i;
84          start=end=i;
85          maxval=ABS16(x[i*C]);
86          /* Look for first zero crossing before clipping */
87          while (start>0 && x[i*C]*x[(start-1)*C]>=0)
88             start--;
89          /* Look for first zero crossing after clipping */
90          while (end<N && x[i*C]*x[end*C]>=0)
91          {
92             /* Look for other peaks until the next zero-crossing. */
93             if (ABS16(x[end*C])>maxval)
94             {
95                maxval = ABS16(x[end*C]);
96                peak_pos = end;
97             }
98             end++;
99          }
100          /* Detect the special case where we clip before the first zero crossing */
101          special = (start==0 && x[i*C]*x[0]>=0);
102
103          /* Compute a such that maxval + a*maxval^2 = 1 */
104          a=(maxval-1)/(maxval*maxval);
105          if (x[i*C]>0)
106             a = -a;
107          /* Apply soft clipping */
108          for (i=start;i<end;i++)
109             x[i*C] = x[i*C]+a*x[i*C]*x[i*C];
110
111          if (special && peak_pos>=2)
112          {
113             /* Add a linear ramp from the first sample to the signal peak.
114                This avoids a discontinuity at the beginning of the frame. */
115             float delta;
116             float offset = x0-x[0];
117             delta = offset / peak_pos;
118             for (i=curr;i<peak_pos;i++)
119             {
120                offset -= delta;
121                x[i*C] += offset;
122                x[i*C] = MAX16(-1.f, MIN16(1.f, x[i*C]));
123             }
124          }
125          curr = end;
126          if (curr==N)
127             break;
128       }
129       declip_mem[c] = a;
130    }
131 }
132 #endif
133
134 int encode_size(int size, unsigned char *data)
135 {
136    if (size < 252)
137    {
138       data[0] = size;
139       return 1;
140    } else {
141       data[0] = 252+(size&0x3);
142       data[1] = (size-(int)data[0])>>2;
143       return 2;
144    }
145 }
146
147 static int parse_size(const unsigned char *data, opus_int32 len, opus_int16 *size)
148 {
149    if (len<1)
150    {
151       *size = -1;
152       return -1;
153    } else if (data[0]<252)
154    {
155       *size = data[0];
156       return 1;
157    } else if (len<2)
158    {
159       *size = -1;
160       return -1;
161    } else {
162       *size = 4*data[1] + data[0];
163       return 2;
164    }
165 }
166
167 int opus_packet_parse_impl(const unsigned char *data, opus_int32 len,
168       int self_delimited, unsigned char *out_toc,
169       const unsigned char *frames[48], opus_int16 size[48],
170       int *payload_offset, opus_int32 *packet_offset)
171 {
172    int i, bytes;
173    int count;
174    int cbr;
175    unsigned char ch, toc;
176    int framesize;
177    opus_int32 last_size;
178    opus_int32 pad = 0;
179    const unsigned char *data0 = data;
180
181    if (size==NULL)
182       return OPUS_BAD_ARG;
183
184    framesize = opus_packet_get_samples_per_frame(data, 48000);
185
186    cbr = 0;
187    toc = *data++;
188    len--;
189    last_size = len;
190    switch (toc&0x3)
191    {
192    /* One frame */
193    case 0:
194       count=1;
195       break;
196    /* Two CBR frames */
197    case 1:
198       count=2;
199       cbr = 1;
200       if (!self_delimited)
201       {
202          if (len&0x1)
203             return OPUS_INVALID_PACKET;
204          last_size = len/2;
205          /* If last_size doesn't fit in size[0], we'll catch it later */
206          size[0] = (opus_int16)last_size;
207       }
208       break;
209    /* Two VBR frames */
210    case 2:
211       count = 2;
212       bytes = parse_size(data, len, size);
213       len -= bytes;
214       if (size[0]<0 || size[0] > len)
215          return OPUS_INVALID_PACKET;
216       data += bytes;
217       last_size = len-size[0];
218       break;
219    /* Multiple CBR/VBR frames (from 0 to 120 ms) */
220    default: /*case 3:*/
221       if (len<1)
222          return OPUS_INVALID_PACKET;
223       /* Number of frames encoded in bits 0 to 5 */
224       ch = *data++;
225       count = ch&0x3F;
226       if (count <= 0 || framesize*count > 5760)
227          return OPUS_INVALID_PACKET;
228       len--;
229       /* Padding flag is bit 6 */
230       if (ch&0x40)
231       {
232          int p;
233          do {
234             int tmp;
235             if (len<=0)
236                return OPUS_INVALID_PACKET;
237             p = *data++;
238             len--;
239             tmp = p==255 ? 254: p;
240             len -= tmp;
241             pad += tmp;
242          } while (p==255);
243       }
244       if (len<0)
245          return OPUS_INVALID_PACKET;
246       /* VBR flag is bit 7 */
247       cbr = !(ch&0x80);
248       if (!cbr)
249       {
250          /* VBR case */
251          last_size = len;
252          for (i=0;i<count-1;i++)
253          {
254             bytes = parse_size(data, len, size+i);
255             len -= bytes;
256             if (size[i]<0 || size[i] > len)
257                return OPUS_INVALID_PACKET;
258             data += bytes;
259             last_size -= bytes+size[i];
260          }
261          if (last_size<0)
262             return OPUS_INVALID_PACKET;
263       } else if (!self_delimited)
264       {
265          /* CBR case */
266          last_size = len/count;
267          if (last_size*count!=len)
268             return OPUS_INVALID_PACKET;
269          for (i=0;i<count-1;i++)
270             size[i] = (opus_int16)last_size;
271       }
272       break;
273    }
274    /* Self-delimited framing has an extra size for the last frame. */
275    if (self_delimited)
276    {
277       bytes = parse_size(data, len, size+count-1);
278       len -= bytes;
279       if (size[count-1]<0 || size[count-1] > len)
280          return OPUS_INVALID_PACKET;
281       data += bytes;
282       /* For CBR packets, apply the size to all the frames. */
283       if (cbr)
284       {
285          if (size[count-1]*count > len)
286             return OPUS_INVALID_PACKET;
287          for (i=0;i<count-1;i++)
288             size[i] = size[count-1];
289       } else if (bytes+size[count-1] > last_size)
290          return OPUS_INVALID_PACKET;
291    } else
292    {
293       /* Because it's not encoded explicitly, it's possible the size of the
294          last packet (or all the packets, for the CBR case) is larger than
295          1275. Reject them here.*/
296       if (last_size > 1275)
297          return OPUS_INVALID_PACKET;
298       size[count-1] = (opus_int16)last_size;
299    }
300
301    if (payload_offset)
302       *payload_offset = (int)(data-data0);
303
304    for (i=0;i<count;i++)
305    {
306       if (frames)
307          frames[i] = data;
308       data += size[i];
309    }
310
311    if (packet_offset)
312       *packet_offset = pad+(opus_int32)(data-data0);
313
314    if (out_toc)
315       *out_toc = toc;
316
317    return count;
318 }
319
320 int opus_packet_parse(const unsigned char *data, opus_int32 len,
321       unsigned char *out_toc, const unsigned char *frames[48],
322       opus_int16 size[48], int *payload_offset)
323 {
324    return opus_packet_parse_impl(data, len, 0, out_toc,
325                                  frames, size, payload_offset, NULL);
326 }
327