Ensure mapping matrix size is always valid.
[opus.git] / src / opus_projection_encoder.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 "stack_alloc.h"
39 #include "mapping_matrix.h"
40
41 #ifdef ENABLE_EXPERIMENTAL_AMBISONICS
42
43 struct OpusProjectionEncoder
44 {
45   opus_int32 mixing_matrix_size_in_bytes;
46   opus_int32 demixing_matrix_size_in_bytes;
47   /* Encoder states go here */
48 };
49
50 #if !defined(DISABLE_FLOAT_API)
51 static void opus_projection_copy_channel_in_float(
52   opus_val16 *dst,
53   int dst_stride,
54   const void *src,
55   int src_stride,
56   int src_channel,
57   int frame_size,
58   void *user_data
59 )
60 {
61   mapping_matrix_multiply_channel_in_float((const MappingMatrix*)user_data,
62     (const float*)src, src_stride, dst, src_channel, dst_stride, frame_size);
63 }
64 #endif
65
66 static void opus_projection_copy_channel_in_short(
67   opus_val16 *dst,
68   int dst_stride,
69   const void *src,
70   int src_stride,
71   int src_channel,
72   int frame_size,
73   void *user_data
74 )
75 {
76   mapping_matrix_multiply_channel_in_short((const MappingMatrix*)user_data,
77     (const opus_int16*)src, src_stride, dst, src_channel, dst_stride, frame_size);
78 }
79
80 static int get_order_plus_one_from_channels(int channels, int *order_plus_one)
81 {
82   int order_plus_one_;
83   int acn_channels;
84   int nondiegetic_channels;
85
86   /* Allowed numbers of channels:
87    * (1 + n)^2 + 2j, for n = 0...14 and j = 0 or 1.
88    */
89   order_plus_one_ = isqrt32(channels);
90   acn_channels = order_plus_one_ * order_plus_one_;
91   nondiegetic_channels = channels - acn_channels;
92   if (order_plus_one)
93     *order_plus_one = order_plus_one_;
94
95   if (order_plus_one_ < 1 || order_plus_one_ > 15 ||
96       (nondiegetic_channels != 0 && nondiegetic_channels != 2))
97     return OPUS_BAD_ARG;
98   return OPUS_OK;
99 }
100
101 static int get_streams_from_channels(int channels, int mapping_family,
102                                      int *streams, int *coupled_streams,
103                                      int *order_plus_one)
104 {
105   if (mapping_family == 253)
106   {
107     if (get_order_plus_one_from_channels(channels, order_plus_one) != OPUS_OK)
108       return OPUS_BAD_ARG;
109     if (streams)
110       *streams = (channels + 1) / 2;
111     if (coupled_streams)
112       *coupled_streams = channels / 2;
113     return OPUS_OK;
114   }
115   return OPUS_BAD_ARG;
116 }
117
118 static MappingMatrix *get_mixing_matrix(OpusProjectionEncoder *st)
119 {
120   return (MappingMatrix *)((char*)st + align(sizeof(OpusProjectionEncoder)));
121 }
122
123 static MappingMatrix *get_demixing_matrix(OpusProjectionEncoder *st)
124 {
125   return (MappingMatrix *)((char*)st + align(sizeof(OpusProjectionEncoder) +
126     st->mixing_matrix_size_in_bytes));
127 }
128
129 static OpusMSEncoder *get_multistream_encoder(OpusProjectionEncoder *st)
130 {
131   return (OpusMSEncoder *)((char*)st + align(sizeof(OpusProjectionEncoder) +
132     st->mixing_matrix_size_in_bytes + st->demixing_matrix_size_in_bytes));
133 }
134
135 opus_int32 opus_projection_ambisonics_encoder_get_size(int channels,
136                                                        int mapping_family)
137 {
138   int nb_streams;
139   int nb_coupled_streams;
140   int order_plus_one;
141   int matrix_rows;
142   opus_int32 matrix_size;
143   opus_int32 encoder_size;
144   int ret;
145
146   ret = get_streams_from_channels(channels, mapping_family, &nb_streams,
147                                   &nb_coupled_streams, &order_plus_one);
148   if (ret != OPUS_OK || order_plus_one < 2 || order_plus_one > 4)
149     return 0;
150
151   matrix_rows = order_plus_one * order_plus_one + 2;
152   matrix_size = mapping_matrix_get_size(matrix_rows, matrix_rows);
153   if (!matrix_size)
154     return 0;
155
156   encoder_size =
157       opus_multistream_encoder_get_size(nb_streams, nb_coupled_streams);
158   if (!encoder_size)
159     return 0;
160   return align(sizeof(OpusProjectionEncoder)) + matrix_size + matrix_size + encoder_size;
161 }
162
163 int opus_projection_ambisonics_encoder_init(OpusProjectionEncoder *st, opus_int32 Fs,
164                                             int channels, int mapping_family,
165                                             int *streams, int *coupled_streams,
166                                             int application)
167 {
168   MappingMatrix *mixing_matrix;
169   MappingMatrix *demixing_matrix;
170   OpusMSEncoder *ms_encoder;
171   int nb_streams;
172   int nb_coupled_streams;
173   int i;
174   int ret;
175   unsigned char mapping[255];
176
177   if (get_streams_from_channels(channels, mapping_family,
178                                 &nb_streams, &nb_coupled_streams, NULL)
179       != OPUS_OK)
180     return OPUS_BAD_ARG;
181
182   if (streams == NULL || coupled_streams == NULL) {
183     return OPUS_BAD_ARG;
184   }
185   *streams = nb_streams;
186   *coupled_streams = nb_coupled_streams;
187
188   if (mapping_family == 253)
189   {
190     int order_plus_one;
191     if (get_order_plus_one_from_channels(channels, &order_plus_one) != OPUS_OK)
192       return OPUS_BAD_ARG;
193
194     /* Assign mixing matrix based on available pre-computed matrices. */
195     mixing_matrix = get_mixing_matrix(st);
196     if (order_plus_one == 2)
197     {
198       mapping_matrix_init(mixing_matrix, mapping_matrix_foa_mixing.rows,
199         mapping_matrix_foa_mixing.cols, mapping_matrix_foa_mixing.gain,
200         mapping_matrix_foa_mixing_data, 36 * sizeof(opus_int16));
201     }
202     else if (order_plus_one == 3)
203     {
204       mapping_matrix_init(mixing_matrix, mapping_matrix_soa_mixing.rows,
205         mapping_matrix_soa_mixing.cols, mapping_matrix_soa_mixing.gain,
206         mapping_matrix_soa_mixing_data, 121 * sizeof(opus_int16));
207     }
208     else if (order_plus_one == 4)
209     {
210       mapping_matrix_init(mixing_matrix, mapping_matrix_toa_mixing.rows,
211         mapping_matrix_toa_mixing.cols, mapping_matrix_toa_mixing.gain,
212         mapping_matrix_toa_mixing_data, 324 * sizeof(opus_int16));
213     }
214     st->mixing_matrix_size_in_bytes = mapping_matrix_get_size(
215       mixing_matrix->rows, mixing_matrix->cols);
216     if (!st->mixing_matrix_size_in_bytes)
217       return OPUS_BAD_ARG;
218
219     /* Assign demixing matrix based on available pre-computed matrices. */
220     demixing_matrix = get_demixing_matrix(st);
221     if (order_plus_one == 2)
222     {
223       mapping_matrix_init(demixing_matrix, mapping_matrix_foa_demixing.rows,
224         mapping_matrix_foa_demixing.cols, mapping_matrix_foa_demixing.gain,
225         mapping_matrix_foa_demixing_data, 36 * sizeof(opus_int16));
226     }
227     else if (order_plus_one == 3)
228     {
229       mapping_matrix_init(demixing_matrix, mapping_matrix_soa_demixing.rows,
230         mapping_matrix_soa_demixing.cols, mapping_matrix_soa_demixing.gain,
231         mapping_matrix_soa_demixing_data, 121 * sizeof(opus_int16));
232     }
233     else if (order_plus_one == 4)
234     {
235       mapping_matrix_init(demixing_matrix, mapping_matrix_toa_demixing.rows,
236         mapping_matrix_toa_demixing.cols, mapping_matrix_toa_demixing.gain,
237         mapping_matrix_toa_demixing_data, 324 * sizeof(opus_int16));
238     }
239     st->demixing_matrix_size_in_bytes = mapping_matrix_get_size(
240       demixing_matrix->rows, demixing_matrix->cols);
241     if (!st->demixing_matrix_size_in_bytes)
242       return OPUS_BAD_ARG;
243   }
244   else
245     return OPUS_UNIMPLEMENTED;
246
247   /* Ensure matrices are large enough for desired coding scheme. */
248   if (nb_streams + nb_coupled_streams > mixing_matrix->rows ||
249       channels > mixing_matrix->cols ||
250       channels > demixing_matrix->rows ||
251       nb_streams + nb_coupled_streams > demixing_matrix->cols)
252     return OPUS_BAD_ARG;
253
254   /* Set trivial mapping so each input channel pairs with a matrix column. */
255   for (i = 0; i < channels; i++)
256     mapping[i] = i;
257
258   /* Initialize multistream encoder with provided settings. */
259   ms_encoder = get_multistream_encoder(st);
260   ret = opus_multistream_encoder_init(ms_encoder, Fs, channels, nb_streams,
261                                       nb_coupled_streams, mapping, application);
262   return ret;
263 }
264
265 OpusProjectionEncoder *opus_projection_ambisonics_encoder_create(
266     opus_int32 Fs, int channels, int mapping_family, int *streams,
267     int *coupled_streams, int application, int *error)
268 {
269   int size;
270   int ret;
271   OpusProjectionEncoder *st;
272
273   /* Allocate space for the projection encoder. */
274   size = opus_projection_ambisonics_encoder_get_size(channels, mapping_family);
275   if (!size) {
276     if (error)
277       *error = OPUS_ALLOC_FAIL;
278     return NULL;
279   }
280   st = (OpusProjectionEncoder *)opus_alloc(size);
281   if (!st)
282   {
283     if (error)
284       *error = OPUS_ALLOC_FAIL;
285     return NULL;
286   }
287
288   /* Initialize projection encoder with provided settings. */
289   ret = opus_projection_ambisonics_encoder_init(st, Fs, channels,
290      mapping_family, streams, coupled_streams, application);
291   if (ret != OPUS_OK)
292   {
293     opus_free(st);
294     st = NULL;
295   }
296   if (error)
297     *error = ret;
298   return st;
299 }
300
301 int opus_projection_encode(OpusProjectionEncoder *st, const opus_int16 *pcm,
302                            int frame_size, unsigned char *data,
303                            opus_int32 max_data_bytes)
304 {
305   return opus_multistream_encode_native(get_multistream_encoder(st),
306     opus_projection_copy_channel_in_short, pcm, frame_size, data,
307     max_data_bytes, 16, downmix_int, 0, get_mixing_matrix(st));
308 }
309
310 #ifndef DISABLE_FLOAT_API
311 #ifdef FIXED_POINT
312 int opus_projection_encode_float(OpusProjectionEncoder *st, const float *pcm,
313                                  int frame_size, unsigned char *data,
314                                  opus_int32 max_data_bytes)
315 {
316   return opus_multistream_encode_native(get_multistream_encoder(st),
317     opus_projection_copy_channel_in_float, pcm, frame_size, data,
318     max_data_bytes, 16, downmix_float, 1, get_mixing_matrix(st));
319 }
320 #else
321 int opus_projection_encode_float(OpusProjectionEncoder *st, const float *pcm,
322                                  int frame_size, unsigned char *data,
323                                  opus_int32 max_data_bytes)
324 {
325   return opus_multistream_encode_native(get_multistream_encoder(st),
326     opus_projection_copy_channel_in_float, pcm, frame_size, data,
327     max_data_bytes, 24, downmix_float, 1, get_mixing_matrix(st));
328 }
329 #endif
330 #endif
331
332 void opus_projection_encoder_destroy(OpusProjectionEncoder *st)
333 {
334   opus_free(st);
335 }
336
337 int opus_projection_encoder_ctl(OpusProjectionEncoder *st, int request, ...)
338 {
339   MappingMatrix *demixing_matrix;
340   OpusMSEncoder *ms_encoder;
341   int ret = OPUS_OK;
342
343   ms_encoder = get_multistream_encoder(st);
344   demixing_matrix = get_demixing_matrix(st);
345
346   va_list ap;
347   va_start(ap, request);
348   switch(request)
349   {
350   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_SIZE_REQUEST:
351   {
352     opus_int32 *value = va_arg(ap, opus_int32*);
353     if (!value)
354     {
355       goto bad_arg;
356     }
357     *value =
358       ms_encoder->layout.nb_channels * (ms_encoder->layout.nb_streams
359       + ms_encoder->layout.nb_coupled_streams) * sizeof(opus_int16);
360   }
361   break;
362   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_GAIN_REQUEST:
363   {
364     opus_int32 *value = va_arg(ap, opus_int32*);
365     if (!value)
366     {
367       goto bad_arg;
368     }
369     *value = demixing_matrix->gain;
370   }
371   break;
372   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_REQUEST:
373   {
374     int i, j, k, l;
375     int nb_input_streams;
376     int nb_output_streams;
377     unsigned char *external_char;
378     opus_int16 *internal_short;
379     opus_int32 external_size;
380     opus_int32 internal_size;
381
382     /* (I/O is in relation to the decoder's perspective). */
383     nb_input_streams = ms_encoder->layout.nb_streams +
384       ms_encoder->layout.nb_coupled_streams;
385     nb_output_streams = ms_encoder->layout.nb_channels;
386
387     external_char = va_arg(ap, unsigned char *);
388     external_size = va_arg(ap, opus_uint32);
389     if (!external_char)
390     {
391       goto bad_arg;
392     }
393     internal_short = mapping_matrix_get_data(demixing_matrix);
394     internal_size = nb_input_streams * nb_output_streams * sizeof(opus_int16);
395     if (external_size != internal_size)
396     {
397       goto bad_arg;
398     }
399
400     /* Copy demixing matrix subset to output destination. */
401     l = 0;
402     for (i = 0; i < nb_input_streams; i++) {
403       for (j = 0; j < nb_output_streams; j++) {
404         k = demixing_matrix->rows * i + j;
405         external_char[2*l] = (unsigned char)internal_short[k];
406         external_char[2*l+1] = (unsigned char)(internal_short[k] >> 8);
407         l++;
408       }
409     }
410   }
411   break;
412   default:
413   {
414     ret = opus_multistream_encoder_ctl_va_list(ms_encoder, request, ap);
415   }
416   break;
417   }
418   va_end(ap);
419   return ret;
420
421 bad_arg:
422   va_end(ap);
423   return OPUS_BAD_ARG;
424 }
425
426 #endif /* ENABLE_EXPERIMENTAL_AMBISONICS */