Fixes a trivial multistream fixed-point bug
[opus.git] / src / opus_multistream.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 FOUNDATION OR
19    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 "stdio.h"
37 #include <stdlib.h>
38 #include <string.h>
39 #include <stdarg.h>
40 #include "float_cast.h"
41
42 typedef struct ChannelLayout {
43    int nb_channels;
44    int nb_streams;
45    int nb_coupled_streams;
46    unsigned char mapping[256];
47 } ChannelLayout;
48
49 struct OpusMSEncoder {
50    ChannelLayout layout;
51    int bitrate;
52    /* Encoder states go here */
53 };
54
55 struct OpusMSDecoder {
56    ChannelLayout layout;
57    /* Decoder states go here */
58 };
59
60
61 #ifdef FIXED_POINT
62 #define opus_encode_native opus_encode
63 #else
64 #define opus_encode_native opus_encode_float
65 #endif
66
67 static int validate_layout(const ChannelLayout *layout)
68 {
69    int i, max_channel;
70
71    max_channel = layout->nb_streams+layout->nb_coupled_streams;
72    if (max_channel>255)
73       return 0;
74    for (i=0;i<layout->nb_channels;i++)
75    {
76       if (layout->mapping[i] >= max_channel && layout->mapping[i] != 255)
77          return 0;
78    }
79    return 1;
80 }
81
82
83 static int get_left_channel(const ChannelLayout *layout, int stream_id, int prev)
84 {
85    int i;
86    i = (prev<0) ? 0 : prev+1;
87    for (;i<layout->nb_channels;i++)
88    {
89       if (layout->mapping[i]==stream_id*2)
90          return i;
91    }
92    return -1;
93 }
94
95 static int get_right_channel(const ChannelLayout *layout, int stream_id, int prev)
96 {
97    int i;
98    i = (prev<0) ? 0 : prev+1;
99    for (;i<layout->nb_channels;i++)
100    {
101       if (layout->mapping[i]==stream_id*2+1)
102          return i;
103    }
104    return -1;
105 }
106
107 static int get_mono_channel(const ChannelLayout *layout, int stream_id, int prev)
108 {
109    int i;
110    i = (prev<0) ? 0 : prev+1;
111    for (;i<layout->nb_channels;i++)
112    {
113       if (layout->mapping[i]==2*layout->nb_coupled_streams+stream_id)
114          return i;
115    }
116    return -1;
117 }
118
119 static int validate_encoder_layout(const ChannelLayout *layout)
120 {
121    int s;
122    for (s=0;s<layout->nb_streams;s++)
123    {
124       if (s < layout->nb_coupled_streams)
125       {
126          if (get_left_channel(layout, s, -1)==-1)
127             return 0;
128          if (get_right_channel(layout, s, -1)==-1)
129             return 0;
130       } else {
131          if (get_mono_channel(layout, s, -1)==-1)
132             return 0;
133       }
134    }
135    return 1;
136 }
137
138 int opus_multistream_encoder_get_size(int nb_streams, int nb_coupled_streams)
139 {
140         int coupled_size;
141         int mono_size;
142
143         coupled_size = opus_encoder_get_size(2);
144         mono_size = opus_encoder_get_size(1);
145         return align(sizeof(OpusMSEncoder))
146                         + nb_coupled_streams * align(coupled_size)
147                 + (nb_streams-nb_coupled_streams) * align(mono_size);
148 }
149
150
151
152 int opus_multistream_encoder_init(
153       OpusMSEncoder *st,            /* Encoder state */
154       int Fs,                     /* Sampling rate of input signal (Hz) */
155       int channels,               /* Number of channels (1/2) in input signal */
156       int streams,
157       int coupled_streams,
158       unsigned char *mapping,
159       int application             /* Coding mode (OPUS_APPLICATION_VOIP/OPUS_APPLICATION_AUDIO) */
160 )
161 {
162    int coupled_size;
163    int mono_size;
164    int i;
165    char *ptr;
166
167    st->layout.nb_channels = channels;
168    st->layout.nb_streams = streams;
169    st->layout.nb_coupled_streams = coupled_streams;
170
171    for (i=0;i<st->layout.nb_channels;i++)
172       st->layout.mapping[i] = mapping[i];
173    if (!validate_layout(&st->layout) || !validate_encoder_layout(&st->layout))
174       return OPUS_BAD_ARG;
175    ptr = (char*)st + align(sizeof(OpusMSEncoder));
176    coupled_size = opus_encoder_get_size(2);
177    mono_size = opus_encoder_get_size(1);
178
179    for (i=0;i<st->layout.nb_coupled_streams;i++)
180    {
181       opus_encoder_init((OpusEncoder*)ptr, Fs, 2, application);
182       ptr += align(coupled_size);
183    }
184    for (;i<st->layout.nb_streams;i++)
185    {
186       opus_encoder_init((OpusEncoder*)ptr, Fs, 1, application);
187       ptr += align(mono_size);
188    }
189    return OPUS_OK;
190 }
191
192 OpusMSEncoder *opus_multistream_encoder_create(
193       int Fs,                     /* Sampling rate of input signal (Hz) */
194       int channels,               /* Number of channels (1/2) in input signal */
195       int streams,
196       int coupled_streams,
197       unsigned char *mapping,
198       int application             /* Coding mode (OPUS_APPLICATION_VOIP/OPUS_APPLICATION_AUDIO) */
199 )
200 {
201    OpusMSEncoder *st = malloc(opus_multistream_encoder_get_size(streams, coupled_streams));
202    if (st!=NULL)
203       opus_multistream_encoder_init(st, Fs, channels, streams, coupled_streams, mapping, application);
204    return st;
205 }
206
207
208 #ifdef FIXED_POINT
209 int opus_multistream_encode(
210 #else
211 int opus_multistream_encode_float(
212 #endif
213     OpusMSEncoder *st,            /* Encoder state */
214     const opus_val16 *pcm,      /* Input signal (interleaved if 2 channels). length is frame_size*channels */
215     int frame_size,             /* Number of samples per frame of input signal */
216     unsigned char *data,        /* Output payload (no more than max_data_bytes long) */
217     int max_data_bytes          /* Allocated memory for payload; don't use for controlling bitrate */
218 )
219 {
220    int coupled_size;
221    int mono_size;
222    int s, i;
223    char *ptr;
224    int tot_size;
225    VARDECL(opus_val16, buf);
226    unsigned char tmp_data[1276];
227    ALLOC_STACK;
228
229    ALLOC(buf, 2*frame_size, opus_val16);
230    ptr = (char*)st + align(sizeof(OpusMSEncoder));
231    coupled_size = opus_encoder_get_size(2);
232    mono_size = opus_encoder_get_size(1);
233
234    if (max_data_bytes < 2*st->layout.nb_streams-1)
235    {
236       RESTORE_STACK;
237       return OPUS_BUFFER_TOO_SMALL;
238    }
239    /* Counting ToC */
240    tot_size = 0;
241    for (s=0;s<st->layout.nb_streams;s++)
242    {
243       OpusEncoder *enc;
244       int len;
245       int curr_max;
246
247       enc = (OpusEncoder*)ptr;
248       if (s < st->layout.nb_coupled_streams)
249       {
250          int left, right;
251          left = get_left_channel(&st->layout, s, -1);
252          right = get_right_channel(&st->layout, s, -1);
253          for (i=0;i<frame_size;i++)
254          {
255             buf[2*i] = pcm[st->layout.nb_channels*i+left];
256             buf[2*i+1] = pcm[st->layout.nb_channels*i+right];
257          }
258          ptr += align(coupled_size);
259       } else {
260          int chan = get_mono_channel(&st->layout, s, -1);
261          for (i=0;i<frame_size;i++)
262             buf[i] = pcm[st->layout.nb_channels*i+chan];
263          ptr += align(mono_size);
264       }
265       /* number of bytes left (+Toc) */
266       curr_max = max_data_bytes - tot_size;
267       /* Reserve one byte for the last stream and 2 for the others */
268       curr_max -= 2*(st->layout.nb_streams-s)-1;
269       len = opus_encode_native(enc, buf, frame_size, tmp_data, curr_max);
270       if (len<0)
271       {
272          RESTORE_STACK;
273          return len;
274       }
275       /* ToC first */
276       *data++ = tmp_data[0];
277       if (s != st->layout.nb_streams-1)
278       {
279          int tmp = encode_size(len-1, data);
280          data += tmp;
281          tot_size += tmp;
282       }
283       /* IMPORTANT: Here we assume that the encoder only returned one frame */
284       tot_size += len;
285       memcpy(data, &tmp_data[1], len-1);
286    }
287    RESTORE_STACK;
288    return tot_size;
289
290 }
291
292 #ifdef FIXED_POINT
293
294 #ifndef DISABLE_FLOAT_API
295 int opus_multistream_encode_float(
296     OpusMSEncoder *st,          /* Encoder state */
297     const float *pcm,           /* Input signal (interleaved if 2 channels). length is frame_size*channels */
298     int frame_size,             /* Number of samples per frame of input signal */
299     unsigned char *data,        /* Output payload (no more than max_data_bytes long) */
300     int max_data_bytes          /* Allocated memory for payload; don't use for controlling bitrate */
301 )
302 {
303    int i, ret;
304    VARDECL(opus_int16, in);
305    ALLOC_STACK;
306
307    ALLOC(in, frame_size*st->layout.nb_channels, opus_int16);
308
309    for (i=0;i<frame_size*st->layout.nb_channels;i++)
310       in[i] = FLOAT2INT16(pcm[i]);
311    ret = opus_multistream_encode(st, in, frame_size, data, max_data_bytes);
312    RESTORE_STACK;
313    return ret;
314 }
315 #endif
316
317 #else
318
319 int opus_multistream_encode(
320     OpusMSEncoder *st,          /* Encoder state */
321     const opus_int16 *pcm,      /* Input signal (interleaved if 2 channels). length is frame_size*channels */
322     int frame_size,             /* Number of samples per frame of input signal */
323     unsigned char *data,        /* Output payload (no more than max_data_bytes long) */
324     int max_data_bytes          /* Allocated memory for payload; don't use for controlling bitrate */
325 )
326 {
327    int i, ret;
328    VARDECL(float, in);
329    ALLOC_STACK;
330
331    ALLOC(in, frame_size*st->layout.nb_channels, float);
332
333    for (i=0;i<frame_size*st->layout.nb_channels;i++)
334       in[i] = (1./32768)*pcm[i];
335    ret = opus_multistream_encode_float(st, in, frame_size, data, max_data_bytes);
336    RESTORE_STACK;
337    return ret;
338 }
339
340 #endif
341
342 int opus_multistream_encoder_ctl(OpusMSEncoder *st, int request, ...)
343 {
344    va_list ap;
345    int coupled_size, mono_size;
346    char *ptr;
347    int ret = OPUS_OK;
348
349    va_start(ap, request);
350
351    coupled_size = opus_encoder_get_size(2);
352    mono_size = opus_encoder_get_size(1);
353    ptr = (char*)st + align(sizeof(OpusMSEncoder));
354    switch (request)
355    {
356    case OPUS_SET_BITRATE_REQUEST:
357    {
358       int chan, s;
359       opus_uint32 value = va_arg(ap, opus_uint32);
360       chan = st->layout.nb_streams + st->layout.nb_coupled_streams;
361       value /= chan;
362       for (s=0;s<st->layout.nb_streams;s++)
363       {
364          OpusEncoder *enc;
365          enc = (OpusEncoder*)ptr;
366          opus_encoder_ctl(enc, request, value * (s < st->layout.nb_coupled_streams ? 2 : 1));
367       }
368    }
369    break;
370    /* FIXME: Add missing ones */
371    case OPUS_GET_BITRATE_REQUEST:
372    case OPUS_GET_VBR_REQUEST:
373    case OPUS_GET_APPLICATION_REQUEST:
374    case OPUS_GET_BANDWIDTH_REQUEST:
375    case OPUS_GET_COMPLEXITY_REQUEST:
376    case OPUS_GET_PACKET_LOSS_PERC_REQUEST:
377    case OPUS_GET_DTX_REQUEST:
378    case OPUS_GET_VOICE_RATIO_REQUEST:
379    case OPUS_GET_VBR_CONSTRAINT_REQUEST:
380    case OPUS_GET_SIGNAL_REQUEST:
381    case OPUS_GET_LOOKAHEAD_REQUEST:
382    {
383       int s;
384       /* This works for int32* params */
385       opus_uint32 *value = va_arg(ap, opus_uint32*);
386       for (s=0;s<st->layout.nb_streams;s++)
387       {
388          OpusEncoder *enc;
389
390          enc = (OpusEncoder*)ptr;
391          if (s < st->layout.nb_coupled_streams)
392             ptr += align(coupled_size);
393          else
394             ptr += align(mono_size);
395          ret = opus_encoder_ctl(enc, request, value);
396          if (ret < 0)
397             break;
398       }
399    }
400    break;
401    default:
402    {
403       int s;
404       /* This works for int32 params */
405       opus_uint32 value = va_arg(ap, opus_uint32);
406       for (s=0;s<st->layout.nb_streams;s++)
407       {
408          OpusEncoder *enc;
409
410          enc = (OpusEncoder*)ptr;
411          if (s < st->layout.nb_coupled_streams)
412             ptr += align(coupled_size);
413          else
414             ptr += align(mono_size);
415          ret = opus_encoder_ctl(enc, request, value);
416          if (ret < 0)
417             break;
418       }
419    }
420    break;
421    }
422
423    va_end(ap);
424    return ret;
425 }
426
427 void opus_multistream_encoder_destroy(OpusMSEncoder *st)
428 {
429     free(st);
430 }
431
432
433 /* DECODER */
434
435 int opus_multistream_decoder_get_size(int nb_streams, int nb_coupled_streams)
436 {
437    int coupled_size;
438    int mono_size;
439
440    coupled_size = opus_decoder_get_size(2);
441    mono_size = opus_decoder_get_size(1);
442    return align(sizeof(OpusMSDecoder))
443          + nb_coupled_streams * align(coupled_size)
444          + (nb_streams-nb_coupled_streams) * align(mono_size);
445 }
446
447 int opus_multistream_decoder_init(
448       OpusMSDecoder *st,            /* Encoder state */
449       int Fs,                     /* Sampling rate of input signal (Hz) */
450       int channels,               /* Number of channels (1/2) in input signal */
451       int streams,
452       int coupled_streams,
453       unsigned char *mapping
454 )
455 {
456    int coupled_size;
457    int mono_size;
458    int i;
459    char *ptr;
460
461    st->layout.nb_channels = channels;
462    st->layout.nb_streams = streams;
463    st->layout.nb_coupled_streams = coupled_streams;
464
465    for (i=0;i<st->layout.nb_channels;i++)
466       st->layout.mapping[i] = mapping[i];
467    if (!validate_layout(&st->layout))
468       return OPUS_BAD_ARG;
469
470    ptr = (char*)st + align(sizeof(OpusMSEncoder));
471    coupled_size = opus_decoder_get_size(2);
472    mono_size = opus_decoder_get_size(1);
473
474    for (i=0;i<st->layout.nb_coupled_streams;i++)
475    {
476       opus_decoder_init((OpusDecoder*)ptr, Fs, 2);
477       ptr += align(coupled_size);
478    }
479    for (;i<st->layout.nb_streams;i++)
480    {
481       opus_decoder_init((OpusDecoder*)ptr, Fs, 1);
482       ptr += align(mono_size);
483    }
484    return OPUS_OK;
485 }
486
487
488 OpusMSDecoder *opus_multistream_decoder_create(
489       int Fs,                     /* Sampling rate of input signal (Hz) */
490       int channels,               /* Number of channels (1/2) in input signal */
491       int streams,
492       int coupled_streams,
493       unsigned char *mapping
494 )
495 {
496    OpusMSDecoder *st = malloc(opus_multistream_decoder_get_size(streams, coupled_streams));
497    if (st!=NULL)
498       opus_multistream_decoder_init(st, Fs, channels, streams, coupled_streams, mapping);
499    return st;
500
501
502 }
503
504 static int opus_multistream_decode_native(
505       OpusMSDecoder *st,            /* Encoder state */
506       const unsigned char *data,
507       int len,
508       opus_val16 *pcm,
509       int frame_size,
510       int decode_fec
511 )
512 {
513    int coupled_size;
514    int mono_size;
515    int s, i, c;
516    char *ptr;
517    VARDECL(opus_val16, buf);
518    ALLOC_STACK;
519
520    ALLOC(buf, 2*frame_size, opus_val16);
521    ptr = (char*)st + align(sizeof(OpusMSEncoder));
522    coupled_size = opus_decoder_get_size(2);
523    mono_size = opus_decoder_get_size(1);
524
525    if (len < 2*st->layout.nb_streams-1)
526       return OPUS_BUFFER_TOO_SMALL;
527    for (s=0;s<st->layout.nb_streams;s++)
528    {
529       OpusDecoder *dec;
530       int packet_offset, ret;
531
532       dec = (OpusDecoder*)ptr;
533       ptr += (s < st->layout.nb_coupled_streams) ? align(coupled_size) : align(mono_size);
534
535       if (len<=0)
536       {
537          RESTORE_STACK;
538          return OPUS_CORRUPTED_DATA;
539       }
540       ret = opus_decode_native(dec, data, len, buf, frame_size, decode_fec, 1, &packet_offset);
541       data += packet_offset;
542       len -= packet_offset;
543       if (ret > frame_size)
544       {
545          RESTORE_STACK;
546          return OPUS_BUFFER_TOO_SMALL;
547       }
548       if (s>0 && ret != frame_size)
549       {
550          RESTORE_STACK;
551          return OPUS_CORRUPTED_DATA;
552       }
553       if (ret <= 0)
554       {
555          RESTORE_STACK;
556          return ret;
557       }
558       frame_size = ret;
559       if (s < st->layout.nb_coupled_streams)
560       {
561          int chan, prev;
562          prev = -1;
563          /* Copy "left" audio to the channel(s) where it belongs */
564          while ( (chan = get_left_channel(&st->layout, s, prev)) != -1)
565          {
566             for (i=0;i<frame_size;i++)
567                pcm[st->layout.nb_channels*i+chan] = buf[2*i];
568             prev = chan;
569          }
570          prev = -1;
571          /* Copy "right" audio to the channel(s) where it belongs */
572          while ( (chan = get_right_channel(&st->layout, s, prev)) != -1)
573          {
574             for (i=0;i<frame_size;i++)
575                pcm[st->layout.nb_channels*i+chan] = buf[2*i+1];
576             prev = chan;
577          }
578       } else {
579          int chan, prev;
580          prev = -1;
581          /* Copy audio to the channel(s) where it belongs */
582          while ( (chan = get_mono_channel(&st->layout, s, prev)) != -1)
583          {
584             for (i=0;i<frame_size;i++)
585                pcm[st->layout.nb_channels*i+chan] = buf[i];
586             prev = chan;
587          }
588       }
589    }
590    /* Handle muted channels */
591    for (c=0;c<st->layout.nb_channels;c++)
592    {
593       if (st->layout.mapping[c] == 255)
594       {
595          for (i=0;i<frame_size;i++)
596             pcm[st->layout.nb_channels*i+c] = 0;
597       }
598    }
599    RESTORE_STACK;
600    return frame_size;
601 }
602
603 #ifdef FIXED_POINT
604 int opus_multistream_decode(
605       OpusMSDecoder *st,            /* Encoder state */
606       const unsigned char *data,
607       int len,
608       opus_int16 *pcm,
609       int frame_size,
610       int decode_fec
611 )
612 {
613    return opus_multistream_decode_native(st, data, len, pcm, frame_size, decode_fec);
614 }
615
616 #ifndef DISABLE_FLOAT_API
617 int opus_multistream_decode_float(OpusMSDecoder *st, const unsigned char *data,
618       int len, float *pcm, int frame_size, int decode_fec)
619 {
620    VARDECL(opus_int16, out);
621    int ret, i;
622    ALLOC_STACK;
623
624    ALLOC(out, frame_size*st->layout.nb_channels, opus_int16);
625
626    ret = opus_multistream_decode_native(st, data, len, out, frame_size, decode_fec);
627    if (ret > 0)
628    {
629       for (i=0;i<ret*st->layout.nb_channels;i++)
630          pcm[i] = (1./32768.)*(out[i]);
631    }
632    RESTORE_STACK;
633    return ret;
634 }
635 #endif
636
637 #else
638
639 int opus_multistream_decode(OpusMSDecoder *st, const unsigned char *data,
640       int len, opus_int16 *pcm, int frame_size, int decode_fec)
641 {
642    VARDECL(float, out);
643    int ret, i;
644    ALLOC_STACK;
645
646    ALLOC(out, frame_size*st->layout.nb_channels, float);
647
648    ret = opus_multistream_decode_native(st, data, len, out, frame_size, decode_fec);
649    if (ret > 0)
650    {
651       for (i=0;i<ret*st->layout.nb_channels;i++)
652          pcm[i] = FLOAT2INT16(out[i]);
653    }
654    RESTORE_STACK;
655    return ret;
656 }
657
658 int opus_multistream_decode_float(
659       OpusMSDecoder *st,            /* Encoder state */
660       const unsigned char *data,
661       int len,
662       float *pcm,
663       int frame_size,
664       int decode_fec
665 )
666 {
667    return opus_multistream_decode_native(st, data, len, pcm, frame_size, decode_fec);
668 }
669 #endif
670
671 int opus_multistream_decoder_ctl(OpusMSDecoder *st, int request, ...)
672 {
673    va_list ap;
674    int coupled_size, mono_size;
675    char *ptr;
676    int ret = OPUS_OK;
677
678    va_start(ap, request);
679
680    coupled_size = opus_decoder_get_size(2);
681    mono_size = opus_decoder_get_size(1);
682    ptr = (char*)st + align(sizeof(OpusMSDecoder));
683    switch (request)
684    {
685        default:
686        {
687           int s;
688           /* This only works for int32* params, but that's all we have right now */
689           opus_uint32 *value = va_arg(ap, opus_uint32*);
690           for (s=0;s<st->layout.nb_streams;s++)
691           {
692              OpusDecoder *enc;
693
694              enc = (OpusDecoder*)ptr;
695              if (s < st->layout.nb_coupled_streams)
696                 ptr += align(coupled_size);
697              else
698                 ptr += align(mono_size);
699              ret = opus_decoder_ctl(enc, request, value);
700              if (ret < 0)
701                 break;
702           }
703        }
704        break;
705    }
706
707    va_end(ap);
708    return ret;
709 }
710
711
712 void opus_multistream_decoder_destroy(OpusMSDecoder *st)
713 {
714     free(st);
715 }