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