Fix surround encoding
[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_parse_impl(const unsigned char *data, opus_int32 len,
170       int self_delimited, unsigned char *out_toc,
171       const unsigned char *frames[48], opus_int16 size[48],
172       int *payload_offset, opus_int32 *packet_offset)
173 {
174    int i, bytes;
175    int count;
176    int cbr;
177    unsigned char ch, toc;
178    int framesize;
179    opus_int32 last_size;
180    opus_int32 pad = 0;
181    const unsigned char *data0 = data;
182
183    if (size==NULL)
184       return OPUS_BAD_ARG;
185
186    framesize = opus_packet_get_samples_per_frame(data, 48000);
187
188    cbr = 0;
189    toc = *data++;
190    len--;
191    last_size = len;
192    switch (toc&0x3)
193    {
194    /* One frame */
195    case 0:
196       count=1;
197       break;
198    /* Two CBR frames */
199    case 1:
200       count=2;
201       cbr = 1;
202       if (!self_delimited)
203       {
204          if (len&0x1)
205             return OPUS_INVALID_PACKET;
206          last_size = len/2;
207          /* If last_size doesn't fit in size[0], we'll catch it later */
208          size[0] = (opus_int16)last_size;
209       }
210       break;
211    /* Two VBR frames */
212    case 2:
213       count = 2;
214       bytes = parse_size(data, len, size);
215       len -= bytes;
216       if (size[0]<0 || size[0] > len)
217          return OPUS_INVALID_PACKET;
218       data += bytes;
219       last_size = len-size[0];
220       break;
221    /* Multiple CBR/VBR frames (from 0 to 120 ms) */
222    default: /*case 3:*/
223       if (len<1)
224          return OPUS_INVALID_PACKET;
225       /* Number of frames encoded in bits 0 to 5 */
226       ch = *data++;
227       count = ch&0x3F;
228       if (count <= 0 || framesize*count > 5760)
229          return OPUS_INVALID_PACKET;
230       len--;
231       /* Padding flag is bit 6 */
232       if (ch&0x40)
233       {
234          int p;
235          do {
236             int tmp;
237             if (len<=0)
238                return OPUS_INVALID_PACKET;
239             p = *data++;
240             len--;
241             tmp = p==255 ? 254: p;
242             len -= tmp;
243             pad += tmp;
244          } while (p==255);
245       }
246       if (len<0)
247          return OPUS_INVALID_PACKET;
248       /* VBR flag is bit 7 */
249       cbr = !(ch&0x80);
250       if (!cbr)
251       {
252          /* VBR case */
253          last_size = len;
254          for (i=0;i<count-1;i++)
255          {
256             bytes = parse_size(data, len, size+i);
257             len -= bytes;
258             if (size[i]<0 || size[i] > len)
259                return OPUS_INVALID_PACKET;
260             data += bytes;
261             last_size -= bytes+size[i];
262          }
263          if (last_size<0)
264             return OPUS_INVALID_PACKET;
265       } else if (!self_delimited)
266       {
267          /* CBR case */
268          last_size = len/count;
269          if (last_size*count!=len)
270             return OPUS_INVALID_PACKET;
271          for (i=0;i<count-1;i++)
272             size[i] = (opus_int16)last_size;
273       }
274       break;
275    }
276    /* Self-delimited framing has an extra size for the last frame. */
277    if (self_delimited)
278    {
279       bytes = parse_size(data, len, size+count-1);
280       len -= bytes;
281       if (size[count-1]<0 || size[count-1] > len)
282          return OPUS_INVALID_PACKET;
283       data += bytes;
284       /* For CBR packets, apply the size to all the frames. */
285       if (cbr)
286       {
287          if (size[count-1]*count > len)
288             return OPUS_INVALID_PACKET;
289          for (i=0;i<count-1;i++)
290             size[i] = size[count-1];
291       } else if (bytes+size[count-1] > last_size)
292          return OPUS_INVALID_PACKET;
293    } else
294    {
295       /* Because it's not encoded explicitly, it's possible the size of the
296          last packet (or all the packets, for the CBR case) is larger than
297          1275. Reject them here.*/
298       if (last_size > 1275)
299          return OPUS_INVALID_PACKET;
300       size[count-1] = (opus_int16)last_size;
301    }
302
303    if (payload_offset)
304       *payload_offset = (int)(data-data0);
305
306    for (i=0;i<count;i++)
307    {
308       if (frames)
309          frames[i] = data;
310       data += size[i];
311    }
312
313    if (packet_offset)
314       *packet_offset = pad+(opus_int32)(data-data0);
315
316    if (out_toc)
317       *out_toc = toc;
318
319    return count;
320 }
321
322 int opus_packet_parse(const unsigned char *data, opus_int32 len,
323       unsigned char *out_toc, const unsigned char *frames[48],
324       opus_int16 size[48], int *payload_offset)
325 {
326    return opus_packet_parse_impl(data, len, 0, out_toc,
327                                  frames, size, payload_offset, NULL);
328 }
329