Allow disabling phase inversion for multistream
[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        (streams<1) || (coupled_streams<0) || (streams>255-coupled_streams))
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        (streams<1) || (coupled_streams<0) || (streams>255-coupled_streams))
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 count;
160    unsigned char toc;
161    opus_int16 size[48];
162    int samples=0;
163    opus_int32 packet_offset;
164
165    for (s=0;s<nb_streams;s++)
166    {
167       int tmp_samples;
168       if (len<=0)
169          return OPUS_INVALID_PACKET;
170       count = opus_packet_parse_impl(data, len, s!=nb_streams-1, &toc, NULL,
171                                      size, NULL, &packet_offset);
172       if (count<0)
173          return count;
174       tmp_samples = opus_packet_get_nb_samples(data, packet_offset, Fs);
175       if (s!=0 && samples != tmp_samples)
176          return OPUS_INVALID_PACKET;
177       samples = tmp_samples;
178       data += packet_offset;
179       len -= packet_offset;
180    }
181    return samples;
182 }
183
184 static int opus_multistream_decode_native(
185       OpusMSDecoder *st,
186       const unsigned char *data,
187       opus_int32 len,
188       void *pcm,
189       opus_copy_channel_out_func copy_channel_out,
190       int frame_size,
191       int decode_fec,
192       int soft_clip
193 )
194 {
195    opus_int32 Fs;
196    int coupled_size;
197    int mono_size;
198    int s, c;
199    char *ptr;
200    int do_plc=0;
201    VARDECL(opus_val16, buf);
202    ALLOC_STACK;
203
204    /* Limit frame_size to avoid excessive stack allocations. */
205    opus_multistream_decoder_ctl(st, OPUS_GET_SAMPLE_RATE(&Fs));
206    frame_size = IMIN(frame_size, Fs/25*3);
207    ALLOC(buf, 2*frame_size, opus_val16);
208    ptr = (char*)st + align(sizeof(OpusMSDecoder));
209    coupled_size = opus_decoder_get_size(2);
210    mono_size = opus_decoder_get_size(1);
211
212    if (len==0)
213       do_plc = 1;
214    if (len < 0)
215    {
216       RESTORE_STACK;
217       return OPUS_BAD_ARG;
218    }
219    if (!do_plc && len < 2*st->layout.nb_streams-1)
220    {
221       RESTORE_STACK;
222       return OPUS_INVALID_PACKET;
223    }
224    if (!do_plc)
225    {
226       int ret = opus_multistream_packet_validate(data, len, st->layout.nb_streams, Fs);
227       if (ret < 0)
228       {
229          RESTORE_STACK;
230          return ret;
231       } else if (ret > frame_size)
232       {
233          RESTORE_STACK;
234          return OPUS_BUFFER_TOO_SMALL;
235       }
236    }
237    for (s=0;s<st->layout.nb_streams;s++)
238    {
239       OpusDecoder *dec;
240       int packet_offset, ret;
241
242       dec = (OpusDecoder*)ptr;
243       ptr += (s < st->layout.nb_coupled_streams) ? align(coupled_size) : align(mono_size);
244
245       if (!do_plc && len<=0)
246       {
247          RESTORE_STACK;
248          return OPUS_INTERNAL_ERROR;
249       }
250       packet_offset = 0;
251       ret = opus_decode_native(dec, data, len, buf, frame_size, decode_fec, s!=st->layout.nb_streams-1, &packet_offset, soft_clip);
252       data += packet_offset;
253       len -= packet_offset;
254       if (ret <= 0)
255       {
256          RESTORE_STACK;
257          return ret;
258       }
259       frame_size = ret;
260       if (s < st->layout.nb_coupled_streams)
261       {
262          int chan, prev;
263          prev = -1;
264          /* Copy "left" audio to the channel(s) where it belongs */
265          while ( (chan = get_left_channel(&st->layout, s, prev)) != -1)
266          {
267             (*copy_channel_out)(pcm, st->layout.nb_channels, chan,
268                buf, 2, frame_size);
269             prev = chan;
270          }
271          prev = -1;
272          /* Copy "right" audio to the channel(s) where it belongs */
273          while ( (chan = get_right_channel(&st->layout, s, prev)) != -1)
274          {
275             (*copy_channel_out)(pcm, st->layout.nb_channels, chan,
276                buf+1, 2, frame_size);
277             prev = chan;
278          }
279       } else {
280          int chan, prev;
281          prev = -1;
282          /* Copy audio to the channel(s) where it belongs */
283          while ( (chan = get_mono_channel(&st->layout, s, prev)) != -1)
284          {
285             (*copy_channel_out)(pcm, st->layout.nb_channels, chan,
286                buf, 1, frame_size);
287             prev = chan;
288          }
289       }
290    }
291    /* Handle muted channels */
292    for (c=0;c<st->layout.nb_channels;c++)
293    {
294       if (st->layout.mapping[c] == 255)
295       {
296          (*copy_channel_out)(pcm, st->layout.nb_channels, c,
297             NULL, 0, frame_size);
298       }
299    }
300    RESTORE_STACK;
301    return frame_size;
302 }
303
304 #if !defined(DISABLE_FLOAT_API)
305 static void opus_copy_channel_out_float(
306   void *dst,
307   int dst_stride,
308   int dst_channel,
309   const opus_val16 *src,
310   int src_stride,
311   int frame_size
312 )
313 {
314    float *float_dst;
315    opus_int32 i;
316    float_dst = (float*)dst;
317    if (src != NULL)
318    {
319       for (i=0;i<frame_size;i++)
320 #if defined(FIXED_POINT)
321          float_dst[i*dst_stride+dst_channel] = (1/32768.f)*src[i*src_stride];
322 #else
323          float_dst[i*dst_stride+dst_channel] = src[i*src_stride];
324 #endif
325    }
326    else
327    {
328       for (i=0;i<frame_size;i++)
329          float_dst[i*dst_stride+dst_channel] = 0;
330    }
331 }
332 #endif
333
334 static void opus_copy_channel_out_short(
335   void *dst,
336   int dst_stride,
337   int dst_channel,
338   const opus_val16 *src,
339   int src_stride,
340   int frame_size
341 )
342 {
343    opus_int16 *short_dst;
344    opus_int32 i;
345    short_dst = (opus_int16*)dst;
346    if (src != NULL)
347    {
348       for (i=0;i<frame_size;i++)
349 #if defined(FIXED_POINT)
350          short_dst[i*dst_stride+dst_channel] = src[i*src_stride];
351 #else
352          short_dst[i*dst_stride+dst_channel] = FLOAT2INT16(src[i*src_stride]);
353 #endif
354    }
355    else
356    {
357       for (i=0;i<frame_size;i++)
358          short_dst[i*dst_stride+dst_channel] = 0;
359    }
360 }
361
362
363
364 #ifdef FIXED_POINT
365 int opus_multistream_decode(
366       OpusMSDecoder *st,
367       const unsigned char *data,
368       opus_int32 len,
369       opus_int16 *pcm,
370       int frame_size,
371       int decode_fec
372 )
373 {
374    return opus_multistream_decode_native(st, data, len,
375        pcm, opus_copy_channel_out_short, frame_size, decode_fec, 0);
376 }
377
378 #ifndef DISABLE_FLOAT_API
379 int opus_multistream_decode_float(OpusMSDecoder *st, const unsigned char *data,
380       opus_int32 len, float *pcm, int frame_size, int decode_fec)
381 {
382    return opus_multistream_decode_native(st, data, len,
383        pcm, opus_copy_channel_out_float, frame_size, decode_fec, 0);
384 }
385 #endif
386
387 #else
388
389 int opus_multistream_decode(OpusMSDecoder *st, const unsigned char *data,
390       opus_int32 len, opus_int16 *pcm, int frame_size, int decode_fec)
391 {
392    return opus_multistream_decode_native(st, data, len,
393        pcm, opus_copy_channel_out_short, frame_size, decode_fec, 1);
394 }
395
396 int opus_multistream_decode_float(
397       OpusMSDecoder *st,
398       const unsigned char *data,
399       opus_int32 len,
400       float *pcm,
401       int frame_size,
402       int decode_fec
403 )
404 {
405    return opus_multistream_decode_native(st, data, len,
406        pcm, opus_copy_channel_out_float, frame_size, decode_fec, 0);
407 }
408 #endif
409
410 int opus_multistream_decoder_ctl(OpusMSDecoder *st, int request, ...)
411 {
412    va_list ap;
413    int coupled_size, mono_size;
414    char *ptr;
415    int ret = OPUS_OK;
416
417    va_start(ap, request);
418
419    coupled_size = opus_decoder_get_size(2);
420    mono_size = opus_decoder_get_size(1);
421    ptr = (char*)st + align(sizeof(OpusMSDecoder));
422    switch (request)
423    {
424        case OPUS_GET_BANDWIDTH_REQUEST:
425        case OPUS_GET_SAMPLE_RATE_REQUEST:
426        case OPUS_GET_GAIN_REQUEST:
427        case OPUS_GET_LAST_PACKET_DURATION_REQUEST:
428        case OPUS_GET_PHASE_INVERSION_DISABLED_REQUEST:
429        {
430           OpusDecoder *dec;
431           /* For int32* GET params, just query the first stream */
432           opus_int32 *value = va_arg(ap, opus_int32*);
433           dec = (OpusDecoder*)ptr;
434           ret = opus_decoder_ctl(dec, request, value);
435        }
436        break;
437        case OPUS_GET_FINAL_RANGE_REQUEST:
438        {
439           int s;
440           opus_uint32 *value = va_arg(ap, opus_uint32*);
441           opus_uint32 tmp;
442           if (!value)
443           {
444              goto bad_arg;
445           }
446           *value = 0;
447           for (s=0;s<st->layout.nb_streams;s++)
448           {
449              OpusDecoder *dec;
450              dec = (OpusDecoder*)ptr;
451              if (s < st->layout.nb_coupled_streams)
452                 ptr += align(coupled_size);
453              else
454                 ptr += align(mono_size);
455              ret = opus_decoder_ctl(dec, request, &tmp);
456              if (ret != OPUS_OK) break;
457              *value ^= tmp;
458           }
459        }
460        break;
461        case OPUS_RESET_STATE:
462        {
463           int s;
464           for (s=0;s<st->layout.nb_streams;s++)
465           {
466              OpusDecoder *dec;
467
468              dec = (OpusDecoder*)ptr;
469              if (s < st->layout.nb_coupled_streams)
470                 ptr += align(coupled_size);
471              else
472                 ptr += align(mono_size);
473              ret = opus_decoder_ctl(dec, OPUS_RESET_STATE);
474              if (ret != OPUS_OK)
475                 break;
476           }
477        }
478        break;
479        case OPUS_MULTISTREAM_GET_DECODER_STATE_REQUEST:
480        {
481           int s;
482           opus_int32 stream_id;
483           OpusDecoder **value;
484           stream_id = va_arg(ap, opus_int32);
485           if (stream_id<0 || stream_id >= st->layout.nb_streams)
486              ret = OPUS_BAD_ARG;
487           value = va_arg(ap, OpusDecoder**);
488           if (!value)
489           {
490              goto bad_arg;
491           }
492           for (s=0;s<stream_id;s++)
493           {
494              if (s < st->layout.nb_coupled_streams)
495                 ptr += align(coupled_size);
496              else
497                 ptr += align(mono_size);
498           }
499           *value = (OpusDecoder*)ptr;
500        }
501        break;
502        case OPUS_SET_GAIN_REQUEST:
503        case OPUS_SET_PHASE_INVERSION_DISABLED_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 }