NLSF VQ now uses absolute error, and prediction
[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        {
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;
528 bad_arg:
529    va_end(ap);
530    return OPUS_BAD_ARG;
531 }
532
533
534 void opus_multistream_decoder_destroy(OpusMSDecoder *st)
535 {
536     opus_free(st);
537 }