win32: Add ambisonics sources to VS project
[opus.git] / src / opus_projection_decoder.c
1 /* Copyright (c) 2017 Google Inc.
2    Written by Andrew Allen */
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 "mathops.h"
33 #include "os_support.h"
34 #include "opus_private.h"
35 #include "opus_defines.h"
36 #include "opus_projection.h"
37 #include "opus_multistream.h"
38 #include "mapping_matrix.h"
39 #include "stack_alloc.h"
40
41 struct OpusProjectionDecoder
42 {
43   opus_int32 demixing_matrix_size_in_bytes;
44   /* Encoder states go here */
45 };
46
47 #if !defined(DISABLE_FLOAT_API)
48 static void opus_projection_copy_channel_out_float(
49   void *dst,
50   int dst_stride,
51   int dst_channel,
52   const opus_val16 *src,
53   int src_stride,
54   int frame_size,
55   void *user_data)
56 {
57   float *float_dst;
58   const MappingMatrix *matrix;
59   float_dst = (float *)dst;
60   matrix = (const MappingMatrix *)user_data;
61
62   if (dst_channel == 0)
63     OPUS_CLEAR(float_dst, frame_size * dst_stride);
64
65   if (src != NULL)
66     mapping_matrix_multiply_channel_out_float(matrix, src, dst_channel,
67       src_stride, float_dst, dst_stride, frame_size);
68 }
69 #endif
70
71 static void opus_projection_copy_channel_out_short(
72   void *dst,
73   int dst_stride,
74   int dst_channel,
75   const opus_val16 *src,
76   int src_stride,
77   int frame_size,
78   void *user_data)
79 {
80   opus_int16 *short_dst;
81   const MappingMatrix *matrix;
82   short_dst = (opus_int16 *)dst;
83   matrix = (const MappingMatrix *)user_data;
84   if (dst_channel == 0)
85     OPUS_CLEAR(short_dst, frame_size * dst_stride);
86
87   if (src != NULL)
88     mapping_matrix_multiply_channel_out_short(matrix, src, dst_channel,
89       src_stride, short_dst, dst_stride, frame_size);
90 }
91
92 static MappingMatrix *get_demixing_matrix(OpusProjectionDecoder *st)
93 {
94   /* void* cast avoids clang -Wcast-align warning */
95   return (MappingMatrix*)(void*)((char*)st +
96     align(sizeof(OpusProjectionDecoder)));
97 }
98
99 static OpusMSDecoder *get_multistream_decoder(OpusProjectionDecoder *st)
100 {
101   /* void* cast avoids clang -Wcast-align warning */
102   return (OpusMSDecoder*)(void*)((char*)st +
103     align(sizeof(OpusProjectionDecoder) +
104     st->demixing_matrix_size_in_bytes));
105 }
106
107 opus_int32 opus_projection_decoder_get_size(int channels, int streams,
108                                             int coupled_streams)
109 {
110   opus_int32 matrix_size;
111   opus_int32 decoder_size;
112
113   matrix_size =
114     mapping_matrix_get_size(streams + coupled_streams, channels);
115   if (!matrix_size)
116     return 0;
117
118   decoder_size = opus_multistream_decoder_get_size(streams, coupled_streams);
119   if (!decoder_size)
120     return 0;
121
122   return align(sizeof(OpusProjectionDecoder)) + matrix_size + decoder_size;
123 }
124
125 int opus_projection_decoder_init(OpusProjectionDecoder *st, opus_int32 Fs,
126   int channels, int streams, int coupled_streams,
127   unsigned char *demixing_matrix, opus_int32 demixing_matrix_size)
128 {
129   int nb_input_streams;
130   opus_int32 expected_matrix_size;
131   int i, ret;
132   unsigned char mapping[255];
133   VARDECL(opus_int16, buf);
134   ALLOC_STACK;
135
136   /* Verify supplied matrix size. */
137   nb_input_streams = streams + coupled_streams;
138   expected_matrix_size = nb_input_streams * channels * sizeof(opus_int16);
139   if (expected_matrix_size != demixing_matrix_size)
140   {
141     RESTORE_STACK;
142     return OPUS_BAD_ARG;
143   }
144
145   /* Convert demixing matrix input into internal format. */
146   ALLOC(buf, nb_input_streams * channels, opus_int16);
147   for (i = 0; i < nb_input_streams * channels; i++)
148   {
149     int s = demixing_matrix[2*i + 1] << 8 | demixing_matrix[2*i];
150     s = ((s & 0xFFFF) ^ 0x8000) - 0x8000;
151     buf[i] = (opus_int16)s;
152   }
153
154   /* Assign demixing matrix. */
155   st->demixing_matrix_size_in_bytes =
156     mapping_matrix_get_size(channels, nb_input_streams);
157   if (!st->demixing_matrix_size_in_bytes)
158   {
159     RESTORE_STACK;
160     return OPUS_BAD_ARG;
161   }
162
163   mapping_matrix_init(get_demixing_matrix(st), channels, nb_input_streams, 0,
164     buf, demixing_matrix_size);
165
166   /* Set trivial mapping so each input channel pairs with a matrix column. */
167   for (i = 0; i < channels; i++)
168     mapping[i] = i;
169
170   ret = opus_multistream_decoder_init(
171     get_multistream_decoder(st), Fs, channels, streams, coupled_streams, mapping);
172   RESTORE_STACK;
173   return ret;
174 }
175
176 OpusProjectionDecoder *opus_projection_decoder_create(
177   opus_int32 Fs, int channels, int streams, int coupled_streams,
178   unsigned char *demixing_matrix, opus_int32 demixing_matrix_size, int *error)
179 {
180   int size;
181   int ret;
182   OpusProjectionDecoder *st;
183
184   /* Allocate space for the projection decoder. */
185   size = opus_projection_decoder_get_size(channels, streams, coupled_streams);
186   if (!size) {
187     if (error)
188       *error = OPUS_ALLOC_FAIL;
189     return NULL;
190   }
191   st = (OpusProjectionDecoder *)opus_alloc(size);
192   if (!st)
193   {
194     if (error)
195       *error = OPUS_ALLOC_FAIL;
196     return NULL;
197   }
198
199   /* Initialize projection decoder with provided settings. */
200   ret = opus_projection_decoder_init(st, Fs, channels, streams, coupled_streams,
201                                      demixing_matrix, demixing_matrix_size);
202   if (ret != OPUS_OK)
203   {
204     opus_free(st);
205     st = NULL;
206   }
207   if (error)
208     *error = ret;
209   return st;
210 }
211
212 #ifdef FIXED_POINT
213 int opus_projection_decode(OpusProjectionDecoder *st, const unsigned char *data,
214                            opus_int32 len, opus_int16 *pcm, int frame_size,
215                            int decode_fec)
216 {
217   return opus_multistream_decode_native(get_multistream_decoder(st), data, len,
218     pcm, opus_projection_copy_channel_out_short, frame_size, decode_fec, 0,
219     get_demixing_matrix(st));
220 }
221 #else
222 int opus_projection_decode(OpusProjectionDecoder *st, const unsigned char *data,
223                            opus_int32 len, opus_int16 *pcm, int frame_size,
224                            int decode_fec)
225 {
226   return opus_multistream_decode_native(get_multistream_decoder(st), data, len,
227     pcm, opus_projection_copy_channel_out_short, frame_size, decode_fec, 1,
228     get_demixing_matrix(st));
229 }
230 #endif
231
232 #ifndef DISABLE_FLOAT_API
233 int opus_projection_decode_float(OpusProjectionDecoder *st, const unsigned char *data,
234                                  opus_int32 len, float *pcm, int frame_size, int decode_fec)
235 {
236   return opus_multistream_decode_native(get_multistream_decoder(st), data, len,
237     pcm, opus_projection_copy_channel_out_float, frame_size, decode_fec, 0,
238     get_demixing_matrix(st));
239 }
240 #endif
241
242 int opus_projection_decoder_ctl(OpusProjectionDecoder *st, int request, ...)
243 {
244   va_list ap;
245   int ret = OPUS_OK;
246
247   va_start(ap, request);
248   ret = opus_multistream_decoder_ctl_va_list(get_multistream_decoder(st),
249     request, ap);
250   va_end(ap);
251   return ret;
252 }
253
254 void opus_projection_decoder_destroy(OpusProjectionDecoder *st)
255 {
256   opus_free(st);
257 }
258