e421726f2b427dd2ddd4d20f1dfb99c2afb7008b
[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       opus_int32 packet_offset;
241       int ret;
242
243       dec = (OpusDecoder*)ptr;
244       ptr += (s < st->layout.nb_coupled_streams) ? align(coupled_size) : align(mono_size);
245
246       if (!do_plc && len<=0)
247       {
248          RESTORE_STACK;
249          return OPUS_INTERNAL_ERROR;
250       }
251       packet_offset = 0;
252       ret = opus_decode_native(dec, data, len, buf, frame_size, decode_fec, s!=st->layout.nb_streams-1, &packet_offset, soft_clip);
253       data += packet_offset;
254       len -= packet_offset;
255       if (ret <= 0)
256       {
257          RESTORE_STACK;
258          return ret;
259       }
260       frame_size = ret;
261       if (s < st->layout.nb_coupled_streams)
262       {
263          int chan, prev;
264          prev = -1;
265          /* Copy "left" audio to the channel(s) where it belongs */
266          while ( (chan = get_left_channel(&st->layout, s, prev)) != -1)
267          {
268             (*copy_channel_out)(pcm, st->layout.nb_channels, chan,
269                buf, 2, frame_size);
270             prev = chan;
271          }
272          prev = -1;
273          /* Copy "right" audio to the channel(s) where it belongs */
274          while ( (chan = get_right_channel(&st->layout, s, prev)) != -1)
275          {
276             (*copy_channel_out)(pcm, st->layout.nb_channels, chan,
277                buf+1, 2, frame_size);
278             prev = chan;
279          }
280       } else {
281          int chan, prev;
282          prev = -1;
283          /* Copy audio to the channel(s) where it belongs */
284          while ( (chan = get_mono_channel(&st->layout, s, prev)) != -1)
285          {
286             (*copy_channel_out)(pcm, st->layout.nb_channels, chan,
287                buf, 1, frame_size);
288             prev = chan;
289          }
290       }
291    }
292    /* Handle muted channels */
293    for (c=0;c<st->layout.nb_channels;c++)
294    {
295       if (st->layout.mapping[c] == 255)
296       {
297          (*copy_channel_out)(pcm, st->layout.nb_channels, c,
298             NULL, 0, frame_size);
299       }
300    }
301    RESTORE_STACK;
302    return frame_size;
303 }
304
305 #if !defined(DISABLE_FLOAT_API)
306 static void opus_copy_channel_out_float(
307   void *dst,
308   int dst_stride,
309   int dst_channel,
310   const opus_val16 *src,
311   int src_stride,
312   int frame_size
313 )
314 {
315    float *float_dst;
316    opus_int32 i;
317    float_dst = (float*)dst;
318    if (src != NULL)
319    {
320       for (i=0;i<frame_size;i++)
321 #if defined(FIXED_POINT)
322          float_dst[i*dst_stride+dst_channel] = (1/32768.f)*src[i*src_stride];
323 #else
324          float_dst[i*dst_stride+dst_channel] = src[i*src_stride];
325 #endif
326    }
327    else
328    {
329       for (i=0;i<frame_size;i++)
330          float_dst[i*dst_stride+dst_channel] = 0;
331    }
332 }
333 #endif
334
335 static void opus_copy_channel_out_short(
336   void *dst,
337   int dst_stride,
338   int dst_channel,
339   const opus_val16 *src,
340   int src_stride,
341   int frame_size
342 )
343 {
344    opus_int16 *short_dst;
345    opus_int32 i;
346    short_dst = (opus_int16*)dst;
347    if (src != NULL)
348    {
349       for (i=0;i<frame_size;i++)
350 #if defined(FIXED_POINT)
351          short_dst[i*dst_stride+dst_channel] = src[i*src_stride];
352 #else
353          short_dst[i*dst_stride+dst_channel] = FLOAT2INT16(src[i*src_stride]);
354 #endif
355    }
356    else
357    {
358       for (i=0;i<frame_size;i++)
359          short_dst[i*dst_stride+dst_channel] = 0;
360    }
361 }
362
363
364
365 #ifdef FIXED_POINT
366 int opus_multistream_decode(
367       OpusMSDecoder *st,
368       const unsigned char *data,
369       opus_int32 len,
370       opus_int16 *pcm,
371       int frame_size,
372       int decode_fec
373 )
374 {
375    return opus_multistream_decode_native(st, data, len,
376        pcm, opus_copy_channel_out_short, frame_size, decode_fec, 0);
377 }
378
379 #ifndef DISABLE_FLOAT_API
380 int opus_multistream_decode_float(OpusMSDecoder *st, const unsigned char *data,
381       opus_int32 len, float *pcm, int frame_size, int decode_fec)
382 {
383    return opus_multistream_decode_native(st, data, len,
384        pcm, opus_copy_channel_out_float, frame_size, decode_fec, 0);
385 }
386 #endif
387
388 #else
389
390 int opus_multistream_decode(OpusMSDecoder *st, const unsigned char *data,
391       opus_int32 len, opus_int16 *pcm, int frame_size, int decode_fec)
392 {
393    return opus_multistream_decode_native(st, data, len,
394        pcm, opus_copy_channel_out_short, frame_size, decode_fec, 1);
395 }
396
397 int opus_multistream_decode_float(
398       OpusMSDecoder *st,
399       const unsigned char *data,
400       opus_int32 len,
401       float *pcm,
402       int frame_size,
403       int decode_fec
404 )
405 {
406    return opus_multistream_decode_native(st, data, len,
407        pcm, opus_copy_channel_out_float, frame_size, decode_fec, 0);
408 }
409 #endif
410
411 int opus_multistream_decoder_ctl(OpusMSDecoder *st, int request, ...)
412 {
413    va_list ap;
414    int coupled_size, mono_size;
415    char *ptr;
416    int ret = OPUS_OK;
417
418    va_start(ap, request);
419
420    coupled_size = opus_decoder_get_size(2);
421    mono_size = opus_decoder_get_size(1);
422    ptr = (char*)st + align(sizeof(OpusMSDecoder));
423    switch (request)
424    {
425        case OPUS_GET_BANDWIDTH_REQUEST:
426        case OPUS_GET_SAMPLE_RATE_REQUEST:
427        case OPUS_GET_GAIN_REQUEST:
428        case OPUS_GET_LAST_PACKET_DURATION_REQUEST:
429        case OPUS_GET_PHASE_INVERSION_DISABLED_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        case OPUS_SET_PHASE_INVERSION_DISABLED_REQUEST:
505        {
506           int s;
507           /* This works for int32 params */
508           opus_int32 value = va_arg(ap, opus_int32);
509           for (s=0;s<st->layout.nb_streams;s++)
510           {
511              OpusDecoder *dec;
512
513              dec = (OpusDecoder*)ptr;
514              if (s < st->layout.nb_coupled_streams)
515                 ptr += align(coupled_size);
516              else
517                 ptr += align(mono_size);
518              ret = opus_decoder_ctl(dec, request, value);
519              if (ret != OPUS_OK)
520                 break;
521           }
522        }
523        break;
524        default:
525           ret = OPUS_UNIMPLEMENTED;
526        break;
527    }
528
529    va_end(ap);
530    return ret;
531 bad_arg:
532    va_end(ap);
533    return OPUS_BAD_ARG;
534 }
535
536
537 void opus_multistream_decoder_destroy(OpusMSDecoder *st)
538 {
539     opus_free(st);
540 }