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)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
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)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
357