Cleaning up multistream packet validation
[opus.git] / src / opus_multistream_decoder.c
1 /* Copyright (c) 2011 Xiph.Org Foundation
2    Written by Jean-Marc Valin */
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_multistream.h"
33 #include "opus.h"
34 #include "opus_private.h"
35 #include "stack_alloc.h"
36 #include <stdarg.h>
37 #include "float_cast.h"
38 #include "os_support.h"
39
40 struct OpusMSDecoder {
41    ChannelLayout layout;
42    /* Decoder states go here */
43 };
44
45
46
47
48 /* DECODER */
49
50 opus_int32 opus_multistream_decoder_get_size(int nb_streams, int nb_coupled_streams)
51 {
52    int coupled_size;
53    int mono_size;
54
55    if(nb_streams<1||nb_coupled_streams>nb_streams||nb_coupled_streams<0)return 0;
56    coupled_size = opus_decoder_get_size(2);
57    mono_size = opus_decoder_get_size(1);
58    return align(sizeof(OpusMSDecoder))
59          + nb_coupled_streams * align(coupled_size)
60          + (nb_streams-nb_coupled_streams) * align(mono_size);
61 }
62
63 int opus_multistream_decoder_init(
64       OpusMSDecoder *st,
65       opus_int32 Fs,
66       int channels,
67       int streams,
68       int coupled_streams,
69       const unsigned char *mapping
70 )
71 {
72    int coupled_size;
73    int mono_size;
74    int i, ret;
75    char *ptr;
76
77    if ((channels>255) || (channels<1) || (coupled_streams>streams) ||
78        (coupled_streams+streams>255) || (streams<1) || (coupled_streams<0))
79       return OPUS_BAD_ARG;
80
81    st->layout.nb_channels = channels;
82    st->layout.nb_streams = streams;
83    st->layout.nb_coupled_streams = coupled_streams;
84
85    for (i=0;i<st->layout.nb_channels;i++)
86       st->layout.mapping[i] = mapping[i];
87    if (!validate_layout(&st->layout))
88       return OPUS_BAD_ARG;
89
90    ptr = (char*)st + align(sizeof(OpusMSDecoder));
91    coupled_size = opus_decoder_get_size(2);
92    mono_size = opus_decoder_get_size(1);
93
94    for (i=0;i<st->layout.nb_coupled_streams;i++)
95    {
96       ret=opus_decoder_init((OpusDecoder*)ptr, Fs, 2);
97       if(ret!=OPUS_OK)return ret;
98       ptr += align(coupled_size);
99    }
100    for (;i<st->layout.nb_streams;i++)
101    {
102       ret=opus_decoder_init((OpusDecoder*)ptr, Fs, 1);
103       if(ret!=OPUS_OK)return ret;
104       ptr += align(mono_size);
105    }
106    return OPUS_OK;
107 }
108
109
110 OpusMSDecoder *opus_multistream_decoder_create(
111       opus_int32 Fs,
112       int channels,
113       int streams,
114       int coupled_streams,
115       const unsigned char *mapping,
116       int *error
117 )
118 {
119    int ret;
120    OpusMSDecoder *st;
121    if ((channels>255) || (channels<1) || (coupled_streams>streams) ||
122        (coupled_streams+streams>255) || (streams<1) || (coupled_streams<0))
123    {
124       if (error)
125          *error = OPUS_BAD_ARG;
126       return NULL;
127    }
128    st = (OpusMSDecoder *)opus_alloc(opus_multistream_decoder_get_size(streams, coupled_streams));
129    if (st==NULL)
130    {
131       if (error)
132          *error = OPUS_ALLOC_FAIL;
133       return NULL;
134    }
135    ret = opus_multistream_decoder_init(st, Fs, channels, streams, coupled_streams, mapping);
136    if (error)
137       *error = ret;
138    if (ret != OPUS_OK)
139    {
140       opus_free(st);
141       st = NULL;
142    }
143    return st;
144 }
145
146 typedef void (*opus_copy_channel_out_func)(
147   void *dst,
148   int dst_stride,
149   int dst_channel,
150   const opus_val16 *src,
151   int src_stride,
152   int frame_size
153 );
154
155 static int opus_multistream_packet_validate(const unsigned char *data,
156       opus_int32 len, int nb_streams, opus_int32 Fs)
157 {
158    int s;
159    int i;
160    int count;
161    unsigned char toc;
162    opus_int16 size[48];
163    int offset;
164    int samples=0;
165
166    for (s=0;s<nb_streams;s++)
167    {
168       int tmp_samples;
169       if (len<=0)
170          return OPUS_INVALID_PACKET;
171       count = opus_packet_parse_impl(data, len, s!=nb_streams-1, &toc, NULL, size, &offset);
172       if (count<0)
173          return count;
174       for (i=0;i<count;i++)
175          offset += size[i];
176       tmp_samples = opus_packet_get_nb_samples(data, offset, Fs);
177       if (s!=0 && samples != tmp_samples)
178          return OPUS_INVALID_PACKET;
179       samples = tmp_samples;
180       data += offset;
181       len -= offset;
182    }
183    return samples;
184 }
185
186 static int opus_multistream_decode_native(
187       OpusMSDecoder *st,
188       const unsigned char *data,
189       opus_int32 len,
190       void *pcm,
191       opus_copy_channel_out_func copy_channel_out,
192       int frame_size,
193       int decode_fec,
194       int soft_clip
195 )
196 {
197    opus_int32 Fs;
198    int coupled_size;
199    int mono_size;
200    int s, c;
201    char *ptr;
202    int do_plc=0;
203    VARDECL(opus_val16, buf);
204    ALLOC_STACK;
205
206    /* Limit frame_size to avoid excessive stack allocations. */
207    opus_multistream_decoder_ctl(st, OPUS_GET_SAMPLE_RATE(&Fs));
208    frame_size = IMIN(frame_size, Fs/25*3);
209    ALLOC(buf, 2*frame_size, opus_val16);
210    ptr = (char*)st + align(sizeof(OpusMSDecoder));
211    coupled_size = opus_decoder_get_size(2);
212    mono_size = opus_decoder_get_size(1);
213
214    if (len==0)
215       do_plc = 1;
216    if (len < 0)
217    {
218       RESTORE_STACK;
219       return OPUS_BAD_ARG;
220    }
221    if (!do_plc && len < 2*st->layout.nb_streams-1)
222    {
223       RESTORE_STACK;
224       return OPUS_INVALID_PACKET;
225    }
226    if (!do_plc)
227    {
228       int ret = opus_multistream_packet_validate(data, len, st->layout.nb_coupled_streams, Fs);
229       if (ret < 0)
230       {
231          RESTORE_STACK;
232          return ret;
233       } else if (ret > frame_size)
234       {
235          RESTORE_STACK;
236          return OPUS_BUFFER_TOO_SMALL;
237       }
238    }
239    for (s=0;s<st->layout.nb_streams;s++)
240    {
241       OpusDecoder *dec;
242       int packet_offset, ret;
243
244       dec = (OpusDecoder*)ptr;
245       ptr += (s < st->layout.nb_coupled_streams) ? align(coupled_size) : align(mono_size);
246
247       if (!do_plc && len<=0)
248       {
249          RESTORE_STACK;
250          return OPUS_INTERNAL_ERROR;
251       }
252       packet_offset = 0;
253       ret = opus_decode_native(dec, data, len, buf, frame_size, decode_fec, s!=st->layout.nb_streams-1, &packet_offset, soft_clip);
254       data += packet_offset;
255       len -= packet_offset;
256       if (ret <= 0)
257       {
258          RESTORE_STACK;
259          return ret;
260       }
261       frame_size = ret;
262       if (s < st->layout.nb_coupled_streams)
263       {
264          int chan, prev;
265          prev = -1;
266          /* Copy "left" audio to the channel(s) where it belongs */
267          while ( (chan = get_left_channel(&st->layout, s, prev)) != -1)
268          {
269             (*copy_channel_out)(pcm, st->layout.nb_channels, chan,
270                buf, 2, frame_size);
271             prev = chan;
272          }
273          prev = -1;
274          /* Copy "right" audio to the channel(s) where it belongs */
275          while ( (chan = get_right_channel(&st->layout, s, prev)) != -1)
276          {
277             (*copy_channel_out)(pcm, st->layout.nb_channels, chan,
278                buf+1, 2, frame_size);
279             prev = chan;
280          }
281       } else {
282          int chan, prev;
283          prev = -1;
284          /* Copy audio to the channel(s) where it belongs */
285          while ( (chan = get_mono_channel(&st->layout, s, prev)) != -1)
286          {
287             (*copy_channel_out)(pcm, st->layout.nb_channels, chan,
288                buf, 1, frame_size);
289             prev = chan;
290          }
291       }
292    }
293    /* Handle muted channels */
294    for (c=0;c<st->layout.nb_channels;c++)
295    {
296       if (st->layout.mapping[c] == 255)
297       {
298          (*copy_channel_out)(pcm, st->layout.nb_channels, c,
299             NULL, 0, frame_size);
300       }
301    }
302    RESTORE_STACK;
303    return frame_size;
304 }
305
306 #if !defined(DISABLE_FLOAT_API)
307 static void opus_copy_channel_out_float(
308   void *dst,
309   int dst_stride,
310   int dst_channel,
311   const opus_val16 *src,
312   int src_stride,
313   int frame_size
314 )
315 {
316    float *float_dst;
317    opus_int32 i;
318    float_dst = (float*)dst;
319    if (src != NULL)
320    {
321       for (i=0;i<frame_size;i++)
322 #if defined(FIXED_POINT)
323          float_dst[i*dst_stride+dst_channel] = (1/32768.f)*src[i*src_stride];
324 #else
325          float_dst[i*dst_stride+dst_channel] = src[i*src_stride];
326 #endif
327    }
328    else
329    {
330       for (i=0;i<frame_size;i++)
331          float_dst[i*dst_stride+dst_channel] = 0;
332    }
333 }
334 #endif
335
336 static void opus_copy_channel_out_short(
337   void *dst,
338   int dst_stride,
339   int dst_channel,
340   const opus_val16 *src,
341   int src_stride,
342   int frame_size
343 )
344 {
345    opus_int16 *short_dst;
346    opus_int32 i;
347    short_dst = (opus_int16*)dst;
348    if (src != NULL)
349    {
350       for (i=0;i<frame_size;i++)
351 #if defined(FIXED_POINT)
352          short_dst[i*dst_stride+dst_channel] = src[i*src_stride];
353 #else
354          short_dst[i*dst_stride+dst_channel] = FLOAT2INT16(src[i*src_stride]);
355 #endif
356    }
357    else
358    {
359       for (i=0;i<frame_size;i++)
360          short_dst[i*dst_stride+dst_channel] = 0;
361    }
362 }
363
364
365
366 #ifdef FIXED_POINT
367 int opus_multistream_decode(
368       OpusMSDecoder *st,
369       const unsigned char *data,
370       opus_int32 len,
371       opus_int16 *pcm,
372       int frame_size,
373       int decode_fec
374 )
375 {
376    return opus_multistream_decode_native(st, data, len,
377        pcm, opus_copy_channel_out_short, frame_size, decode_fec, 0);
378 }
379
380 #ifndef DISABLE_FLOAT_API
381 int opus_multistream_decode_float(OpusMSDecoder *st, const unsigned char *data,
382       opus_int32 len, float *pcm, int frame_size, int decode_fec)
383 {
384    return opus_multistream_decode_native(st, data, len,
385        pcm, opus_copy_channel_out_float, frame_size, decode_fec, 0);
386 }
387 #endif
388
389 #else
390
391 int opus_multistream_decode(OpusMSDecoder *st, const unsigned char *data,
392       opus_int32 len, opus_int16 *pcm, int frame_size, int decode_fec)
393 {
394    return opus_multistream_decode_native(st, data, len,
395        pcm, opus_copy_channel_out_short, frame_size, decode_fec, 1);
396 }
397
398 int opus_multistream_decode_float(
399       OpusMSDecoder *st,
400       const unsigned char *data,
401       opus_int32 len,
402       float *pcm,
403       int frame_size,
404       int decode_fec
405 )
406 {
407    return opus_multistream_decode_native(st, data, len,
408        pcm, opus_copy_channel_out_float, frame_size, decode_fec, 0);
409 }
410 #endif
411
412 int opus_multistream_decoder_ctl(OpusMSDecoder *st, int request, ...)
413 {
414    va_list ap;
415    int coupled_size, mono_size;
416    char *ptr;
417    int ret = OPUS_OK;
418
419    va_start(ap, request);
420
421    coupled_size = opus_decoder_get_size(2);
422    mono_size = opus_decoder_get_size(1);
423    ptr = (char*)st + align(sizeof(OpusMSDecoder));
424    switch (request)
425    {
426        case OPUS_GET_BANDWIDTH_REQUEST:
427        case OPUS_GET_SAMPLE_RATE_REQUEST:
428        case OPUS_GET_GAIN_REQUEST:
429        case OPUS_GET_LAST_PACKET_DURATION_REQUEST:
430        {
431           OpusDecoder *dec;
432           /* For int32* GET params, just query the first stream */
433           opus_int32 *value = va_arg(ap, opus_int32*);
434           dec = (OpusDecoder*)ptr;
435           ret = opus_decoder_ctl(dec, request, value);
436        }
437        break;
438        case OPUS_GET_FINAL_RANGE_REQUEST:
439        {
440           int s;
441           opus_uint32 *value = va_arg(ap, opus_uint32*);
442           opus_uint32 tmp;
443           if (!value)
444           {
445              goto bad_arg;
446           }
447           *value = 0;
448           for (s=0;s<st->layout.nb_streams;s++)
449           {
450              OpusDecoder *dec;
451              dec = (OpusDecoder*)ptr;
452              if (s < st->layout.nb_coupled_streams)
453                 ptr += align(coupled_size);
454              else
455                 ptr += align(mono_size);
456              ret = opus_decoder_ctl(dec, request, &tmp);
457              if (ret != OPUS_OK) break;
458              *value ^= tmp;
459           }
460        }
461        break;
462        case OPUS_RESET_STATE:
463        {
464           int s;
465           for (s=0;s<st->layout.nb_streams;s++)
466           {
467              OpusDecoder *dec;
468
469              dec = (OpusDecoder*)ptr;
470              if (s < st->layout.nb_coupled_streams)
471                 ptr += align(coupled_size);
472              else
473                 ptr += align(mono_size);
474              ret = opus_decoder_ctl(dec, OPUS_RESET_STATE);
475              if (ret != OPUS_OK)
476                 break;
477           }
478        }
479        break;
480        case OPUS_MULTISTREAM_GET_DECODER_STATE_REQUEST:
481        {
482           int s;
483           opus_int32 stream_id;
484           OpusDecoder **value;
485           stream_id = va_arg(ap, opus_int32);
486           if (stream_id<0 || stream_id >= st->layout.nb_streams)
487              ret = OPUS_BAD_ARG;
488           value = va_arg(ap, OpusDecoder**);
489           if (!value)
490           {
491              goto bad_arg;
492           }
493           for (s=0;s<stream_id;s++)
494           {
495              if (s < st->layout.nb_coupled_streams)
496                 ptr += align(coupled_size);
497              else
498                 ptr += align(mono_size);
499           }
500           *value = (OpusDecoder*)ptr;
501        }
502        break;
503        case OPUS_SET_GAIN_REQUEST:
504        {
505           int s;
506           /* This works for int32 params */
507           opus_int32 value = va_arg(ap, opus_int32);
508           for (s=0;s<st->layout.nb_streams;s++)
509           {
510              OpusDecoder *dec;
511
512              dec = (OpusDecoder*)ptr;
513              if (s < st->layout.nb_coupled_streams)
514                 ptr += align(coupled_size);
515              else
516                 ptr += align(mono_size);
517              ret = opus_decoder_ctl(dec, request, value);
518              if (ret != OPUS_OK)
519                 break;
520           }
521        }
522        break;
523        default:
524           ret = OPUS_UNIMPLEMENTED;
525        break;
526    }
527
528    va_end(ap);
529    return ret;
530 bad_arg:
531    va_end(ap);
532    return OPUS_BAD_ARG;
533 }
534
535
536 void opus_multistream_decoder_destroy(OpusMSDecoder *st)
537 {
538     opus_free(st);
539 }