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