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 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
encode_size(int size,unsigned char * data)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
parse_size(const unsigned char * data,opus_int32 len,opus_int16 * size)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
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)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
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)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
330