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
40struct OpusMSDecoder {
41   ChannelLayout layout;
42   /* Decoder states go here */
43};
44
45
46
47
48/* DECODER */
49
50opus_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
63int 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
110OpusMSDecoder *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
146typedef 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
155static 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
184static 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)
305static 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
334static 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
365int 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
379int 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
389int 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
396int 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
410int 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       {
429          OpusDecoder *dec;
430          /* For int32* GET params, just query the first stream */
431          opus_int32 *value = va_arg(ap, opus_int32*);
432          dec = (OpusDecoder*)ptr;
433          ret = opus_decoder_ctl(dec, request, value);
434       }
435       break;
436       case OPUS_GET_FINAL_RANGE_REQUEST:
437       {
438          int s;
439          opus_uint32 *value = va_arg(ap, opus_uint32*);
440          opus_uint32 tmp;
441          if (!value)
442          {
443             goto bad_arg;
444          }
445          *value = 0;
446          for (s=0;s<st->layout.nb_streams;s++)
447          {
448             OpusDecoder *dec;
449             dec = (OpusDecoder*)ptr;
450             if (s < st->layout.nb_coupled_streams)
451                ptr += align(coupled_size);
452             else
453                ptr += align(mono_size);
454             ret = opus_decoder_ctl(dec, request, &tmp);
455             if (ret != OPUS_OK) break;
456             *value ^= tmp;
457          }
458       }
459       break;
460       case OPUS_RESET_STATE:
461       {
462          int s;
463          for (s=0;s<st->layout.nb_streams;s++)
464          {
465             OpusDecoder *dec;
466
467             dec = (OpusDecoder*)ptr;
468             if (s < st->layout.nb_coupled_streams)
469                ptr += align(coupled_size);
470             else
471                ptr += align(mono_size);
472             ret = opus_decoder_ctl(dec, OPUS_RESET_STATE);
473             if (ret != OPUS_OK)
474                break;
475          }
476       }
477       break;
478       case OPUS_MULTISTREAM_GET_DECODER_STATE_REQUEST:
479       {
480          int s;
481          opus_int32 stream_id;
482          OpusDecoder **value;
483          stream_id = va_arg(ap, opus_int32);
484          if (stream_id<0 || stream_id >= st->layout.nb_streams)
485             ret = OPUS_BAD_ARG;
486          value = va_arg(ap, OpusDecoder**);
487          if (!value)
488          {
489             goto bad_arg;
490          }
491          for (s=0;s<stream_id;s++)
492          {
493             if (s < st->layout.nb_coupled_streams)
494                ptr += align(coupled_size);
495             else
496                ptr += align(mono_size);
497          }
498          *value = (OpusDecoder*)ptr;
499       }
500       break;
501       case OPUS_SET_GAIN_REQUEST:
502       {
503          int s;
504          /* This works for int32 params */
505          opus_int32 value = va_arg(ap, opus_int32);
506          for (s=0;s<st->layout.nb_streams;s++)
507          {
508             OpusDecoder *dec;
509
510             dec = (OpusDecoder*)ptr;
511             if (s < st->layout.nb_coupled_streams)
512                ptr += align(coupled_size);
513             else
514                ptr += align(mono_size);
515             ret = opus_decoder_ctl(dec, request, value);
516             if (ret != OPUS_OK)
517                break;
518          }
519       }
520       break;
521       default:
522          ret = OPUS_UNIMPLEMENTED;
523       break;
524   }
525
526   va_end(ap);
527   return ret;
528bad_arg:
529   va_end(ap);
530   return OPUS_BAD_ARG;
531}
532
533
534void opus_multistream_decoder_destroy(OpusMSDecoder *st)
535{
536    opus_free(st);
537}
538