Correct analysis downmix scaling factor
[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          /* Slightly boost "a" by 2^-22. This is just enough to ensure -ffast-math
108             does not cause output values larger than +/-1, but small enough not
109             to matter even for 24-bit output.  */
110          a += a*2.4e-7;
111          if (x[i*C]>0)
112             a = -a;
113          /* Apply soft clipping */
114          for (i=start;i<end;i++)
115             x[i*C] = x[i*C]+a*x[i*C]*x[i*C];
116
117          if (special && peak_pos>=2)
118          {
119             /* Add a linear ramp from the first sample to the signal peak.
120                This avoids a discontinuity at the beginning of the frame. */
121             float delta;
122             float offset = x0-x[0];
123             delta = offset / peak_pos;
124             for (i=curr;i<peak_pos;i++)
125             {
126                offset -= delta;
127                x[i*C] += offset;
128                x[i*C] = MAX16(-1.f, MIN16(1.f, x[i*C]));
129             }
130          }
131          curr = end;
132          if (curr==N)
133             break;
134       }
135       declip_mem[c] = a;
136    }
137 }
138 #endif
139
140 int encode_size(int size, unsigned char *data)
141 {
142    if (size < 252)
143    {
144       data[0] = size;
145       return 1;
146    } else {
147       data[0] = 252+(size&0x3);
148       data[1] = (size-(int)data[0])>>2;
149       return 2;
150    }
151 }
152
153 static int parse_size(const unsigned char *data, opus_int32 len, opus_int16 *size)
154 {
155    if (len<1)
156    {
157       *size = -1;
158       return -1;
159    } else if (data[0]<252)
160    {
161       *size = data[0];
162       return 1;
163    } else if (len<2)
164    {
165       *size = -1;
166       return -1;
167    } else {
168       *size = 4*data[1] + data[0];
169       return 2;
170    }
171 }
172
173 int opus_packet_get_samples_per_frame(const unsigned char *data,
174       opus_int32 Fs)
175 {
176    int audiosize;
177    if (data[0]&0x80)
178    {
179       audiosize = ((data[0]>>3)&0x3);
180       audiosize = (Fs<<audiosize)/400;
181    } else if ((data[0]&0x60) == 0x60)
182    {
183       audiosize = (data[0]&0x08) ? Fs/50 : Fs/100;
184    } else {
185       audiosize = ((data[0]>>3)&0x3);
186       if (audiosize == 3)
187          audiosize = Fs*60/1000;
188       else
189          audiosize = (Fs<<audiosize)/100;
190    }
191    return audiosize;
192 }
193
194 int opus_packet_parse_impl(const unsigned char *data, opus_int32 len,
195       int self_delimited, unsigned char *out_toc,
196       const unsigned char *frames[48], opus_int16 size[48],
197       int *payload_offset, opus_int32 *packet_offset)
198 {
199    int i, bytes;
200    int count;
201    int cbr;
202    unsigned char ch, toc;
203    int framesize;
204    opus_int32 last_size;
205    opus_int32 pad = 0;
206    const unsigned char *data0 = data;
207
208    if (size==NULL || len<0)
209       return OPUS_BAD_ARG;
210    if (len==0)
211       return OPUS_INVALID_PACKET;
212
213    framesize = opus_packet_get_samples_per_frame(data, 48000);
214
215    cbr = 0;
216    toc = *data++;
217    len--;
218    last_size = len;
219    switch (toc&0x3)
220    {
221    /* One frame */
222    case 0:
223       count=1;
224       break;
225    /* Two CBR frames */
226    case 1:
227       count=2;
228       cbr = 1;
229       if (!self_delimited)
230       {
231          if (len&0x1)
232             return OPUS_INVALID_PACKET;
233          last_size = len/2;
234          /* If last_size doesn't fit in size[0], we'll catch it later */
235          size[0] = (opus_int16)last_size;
236       }
237       break;
238    /* Two VBR frames */
239    case 2:
240       count = 2;
241       bytes = parse_size(data, len, size);
242       len -= bytes;
243       if (size[0]<0 || size[0] > len)
244          return OPUS_INVALID_PACKET;
245       data += bytes;
246       last_size = len-size[0];
247       break;
248    /* Multiple CBR/VBR frames (from 0 to 120 ms) */
249    default: /*case 3:*/
250       if (len<1)
251          return OPUS_INVALID_PACKET;
252       /* Number of frames encoded in bits 0 to 5 */
253       ch = *data++;
254       count = ch&0x3F;
255       if (count <= 0 || framesize*count > 5760)
256          return OPUS_INVALID_PACKET;
257       len--;
258       /* Padding flag is bit 6 */
259       if (ch&0x40)
260       {
261          int p;
262          do {
263             int tmp;
264             if (len<=0)
265                return OPUS_INVALID_PACKET;
266             p = *data++;
267             len--;
268             tmp = p==255 ? 254: p;
269             len -= tmp;
270             pad += tmp;
271          } while (p==255);
272       }
273       if (len<0)
274          return OPUS_INVALID_PACKET;
275       /* VBR flag is bit 7 */
276       cbr = !(ch&0x80);
277       if (!cbr)
278       {
279          /* VBR case */
280          last_size = len;
281          for (i=0;i<count-1;i++)
282          {
283             bytes = parse_size(data, len, size+i);
284             len -= bytes;
285             if (size[i]<0 || size[i] > len)
286                return OPUS_INVALID_PACKET;
287             data += bytes;
288             last_size -= bytes+size[i];
289          }
290          if (last_size<0)
291             return OPUS_INVALID_PACKET;
292       } else if (!self_delimited)
293       {
294          /* CBR case */
295          last_size = len/count;
296          if (last_size*count!=len)
297             return OPUS_INVALID_PACKET;
298          for (i=0;i<count-1;i++)
299             size[i] = (opus_int16)last_size;
300       }
301       break;
302    }
303    /* Self-delimited framing has an extra size for the last frame. */
304    if (self_delimited)
305    {
306       bytes = parse_size(data, len, size+count-1);
307       len -= bytes;
308       if (size[count-1]<0 || size[count-1] > len)
309          return OPUS_INVALID_PACKET;
310       data += bytes;
311       /* For CBR packets, apply the size to all the frames. */
312       if (cbr)
313       {
314          if (size[count-1]*count > len)
315             return OPUS_INVALID_PACKET;
316          for (i=0;i<count-1;i++)
317             size[i] = size[count-1];
318       } else if (bytes+size[count-1] > last_size)
319          return OPUS_INVALID_PACKET;
320    } else
321    {
322       /* Because it's not encoded explicitly, it's possible the size of the
323          last packet (or all the packets, for the CBR case) is larger than
324          1275. Reject them here.*/
325       if (last_size > 1275)
326          return OPUS_INVALID_PACKET;
327       size[count-1] = (opus_int16)last_size;
328    }
329
330    if (payload_offset)
331       *payload_offset = (int)(data-data0);
332
333    for (i=0;i<count;i++)
334    {
335       if (frames)
336          frames[i] = data;
337       data += size[i];
338    }
339
340    if (packet_offset)
341       *packet_offset = pad+(opus_int32)(data-data0);
342
343    if (out_toc)
344       *out_toc = toc;
345
346    return count;
347 }
348
349 int opus_packet_parse(const unsigned char *data, opus_int32 len,
350       unsigned char *out_toc, const unsigned char *frames[48],
351       opus_int16 size[48], int *payload_offset)
352 {
353    return opus_packet_parse_impl(data, len, 0, out_toc,
354                                  frames, size, payload_offset, NULL);
355 }
356