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