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