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
opus_pcm_soft_clip(float * _x,int N,int C,float * declip_mem)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-7f;
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
encode_size(int size,unsigned char * data)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
parse_size(const unsigned char * data,opus_int32 len,opus_int16 * size)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
opus_packet_get_samples_per_frame(const unsigned char * data,opus_int32 Fs)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
opus_packet_parse_impl(const unsigned char * data,opus_int32 len,int self_delimited,unsigned char * out_toc,const unsigned char * frames[48],opus_int16 size[48],int * payload_offset,opus_int32 * packet_offset,const unsigned char ** padding,opus_int32 * padding_len)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 const unsigned char **padding, opus_int32 *padding_len)
199 {
200 int i, bytes;
201 int count;
202 int cbr;
203 unsigned char ch, toc;
204 int framesize;
205 opus_int32 last_size;
206 opus_int32 pad = 0;
207 const unsigned char *data0 = data;
208
209 if (size==NULL || len<0)
210 return OPUS_BAD_ARG;
211 if (len==0)
212 return OPUS_INVALID_PACKET;
213
214 framesize = opus_packet_get_samples_per_frame(data, 48000);
215
216 cbr = 0;
217 toc = *data++;
218 len--;
219 last_size = len;
220 switch (toc&0x3)
221 {
222 /* One frame */
223 case 0:
224 count=1;
225 break;
226 /* Two CBR frames */
227 case 1:
228 count=2;
229 cbr = 1;
230 if (!self_delimited)
231 {
232 if (len&0x1)
233 return OPUS_INVALID_PACKET;
234 last_size = len/2;
235 /* If last_size doesn't fit in size[0], we'll catch it later */
236 size[0] = (opus_int16)last_size;
237 }
238 break;
239 /* Two VBR frames */
240 case 2:
241 count = 2;
242 bytes = parse_size(data, len, size);
243 len -= bytes;
244 if (size[0]<0 || size[0] > len)
245 return OPUS_INVALID_PACKET;
246 data += bytes;
247 last_size = len-size[0];
248 break;
249 /* Multiple CBR/VBR frames (from 0 to 120 ms) */
250 default: /*case 3:*/
251 if (len<1)
252 return OPUS_INVALID_PACKET;
253 /* Number of frames encoded in bits 0 to 5 */
254 ch = *data++;
255 count = ch&0x3F;
256 if (count <= 0 || framesize*(opus_int32)count > 5760)
257 return OPUS_INVALID_PACKET;
258 len--;
259 /* Padding flag is bit 6 */
260 if (ch&0x40)
261 {
262 int p;
263 do {
264 int tmp;
265 if (len<=0)
266 return OPUS_INVALID_PACKET;
267 p = *data++;
268 len--;
269 tmp = p==255 ? 254: p;
270 len -= tmp;
271 pad += tmp;
272 } while (p==255);
273 }
274 if (len<0)
275 return OPUS_INVALID_PACKET;
276 /* VBR flag is bit 7 */
277 cbr = !(ch&0x80);
278 if (!cbr)
279 {
280 /* VBR case */
281 last_size = len;
282 for (i=0;i<count-1;i++)
283 {
284 bytes = parse_size(data, len, size+i);
285 len -= bytes;
286 if (size[i]<0 || size[i] > len)
287 return OPUS_INVALID_PACKET;
288 data += bytes;
289 last_size -= bytes+size[i];
290 }
291 if (last_size<0)
292 return OPUS_INVALID_PACKET;
293 } else if (!self_delimited)
294 {
295 /* CBR case */
296 last_size = len/count;
297 if (last_size*count!=len)
298 return OPUS_INVALID_PACKET;
299 for (i=0;i<count-1;i++)
300 size[i] = (opus_int16)last_size;
301 }
302 break;
303 }
304 /* Self-delimited framing has an extra size for the last frame. */
305 if (self_delimited)
306 {
307 bytes = parse_size(data, len, size+count-1);
308 len -= bytes;
309 if (size[count-1]<0 || size[count-1] > len)
310 return OPUS_INVALID_PACKET;
311 data += bytes;
312 /* For CBR packets, apply the size to all the frames. */
313 if (cbr)
314 {
315 if (size[count-1]*count > len)
316 return OPUS_INVALID_PACKET;
317 for (i=0;i<count-1;i++)
318 size[i] = size[count-1];
319 } else if (bytes+size[count-1] > last_size)
320 return OPUS_INVALID_PACKET;
321 } else
322 {
323 /* Because it's not encoded explicitly, it's possible the size of the
324 last packet (or all the packets, for the CBR case) is larger than
325 1275. Reject them here.*/
326 if (last_size > 1275)
327 return OPUS_INVALID_PACKET;
328 size[count-1] = (opus_int16)last_size;
329 }
330
331 if (payload_offset)
332 *payload_offset = (int)(data-data0);
333
334 for (i=0;i<count;i++)
335 {
336 if (frames)
337 frames[i] = data;
338 data += size[i];
339 }
340
341 if (padding != NULL)
342 {
343 *padding = data;
344 *padding_len = pad;
345 }
346 if (packet_offset)
347 *packet_offset = pad+(opus_int32)(data-data0);
348
349 if (out_toc)
350 *out_toc = toc;
351
352 return count;
353 }
354
opus_packet_parse(const unsigned char * data,opus_int32 len,unsigned char * out_toc,const unsigned char * frames[48],opus_int16 size[48],int * payload_offset)355 int opus_packet_parse(const unsigned char *data, opus_int32 len,
356 unsigned char *out_toc, const unsigned char *frames[48],
357 opus_int16 size[48], int *payload_offset)
358 {
359 return opus_packet_parse_impl(data, len, 0, out_toc,
360 frames, size, payload_offset, NULL, NULL, NULL);
361 }
362
363