Fix memory issues in Projection API.
[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   encoder_size =
154       opus_multistream_encoder_get_size(nb_streams, nb_coupled_streams);
155   if (!encoder_size)
156     return 0;
157   return align(sizeof(OpusProjectionEncoder)) + matrix_size + matrix_size + encoder_size;
158 }
159
160 int opus_projection_ambisonics_encoder_init(OpusProjectionEncoder *st, opus_int32 Fs,
161                                             int channels, int mapping_family,
162                                             int *streams, int *coupled_streams,
163                                             int application)
164 {
165   MappingMatrix *mixing_matrix;
166   MappingMatrix *demixing_matrix;
167   OpusMSEncoder *ms_encoder;
168   int nb_streams;
169   int nb_coupled_streams;
170   int i;
171   int ret;
172   unsigned char mapping[255];
173
174   if (get_streams_from_channels(channels, mapping_family,
175                                 &nb_streams, &nb_coupled_streams, NULL)
176       != OPUS_OK)
177     return OPUS_BAD_ARG;
178
179   if (streams == NULL || coupled_streams == NULL) {
180     return OPUS_BAD_ARG;
181   }
182   *streams = nb_streams;
183   *coupled_streams = nb_coupled_streams;
184
185   if (mapping_family == 253)
186   {
187     int order_plus_one;
188     if (get_order_plus_one_from_channels(channels, &order_plus_one) != OPUS_OK)
189       return OPUS_BAD_ARG;
190
191     /* Assign mixing matrix based on available pre-computed matrices. */
192     mixing_matrix = get_mixing_matrix(st);
193     if (order_plus_one == 2)
194     {
195       mapping_matrix_init(mixing_matrix, mapping_matrix_foa_mixing.rows,
196         mapping_matrix_foa_mixing.cols, mapping_matrix_foa_mixing.gain,
197         mapping_matrix_foa_mixing_data, 36 * sizeof(opus_int16));
198     }
199     else if (order_plus_one == 3)
200     {
201       mapping_matrix_init(mixing_matrix, mapping_matrix_soa_mixing.rows,
202         mapping_matrix_soa_mixing.cols, mapping_matrix_soa_mixing.gain,
203         mapping_matrix_soa_mixing_data, 121 * sizeof(opus_int16));
204     }
205     else if (order_plus_one == 4)
206     {
207       mapping_matrix_init(mixing_matrix, mapping_matrix_toa_mixing.rows,
208         mapping_matrix_toa_mixing.cols, mapping_matrix_toa_mixing.gain,
209         mapping_matrix_toa_mixing_data, 324 * sizeof(opus_int16));
210     }
211     st->mixing_matrix_size_in_bytes = mapping_matrix_get_size(
212       mixing_matrix->rows, mixing_matrix->cols);
213
214     /* Assign demixing matrix based on available pre-computed matrices. */
215     demixing_matrix = get_demixing_matrix(st);
216     if (order_plus_one == 2)
217     {
218       mapping_matrix_init(demixing_matrix, mapping_matrix_foa_demixing.rows,
219         mapping_matrix_foa_demixing.cols, mapping_matrix_foa_demixing.gain,
220         mapping_matrix_foa_demixing_data, 36 * sizeof(opus_int16));
221     }
222     else if (order_plus_one == 3)
223     {
224       mapping_matrix_init(demixing_matrix, mapping_matrix_soa_demixing.rows,
225         mapping_matrix_soa_demixing.cols, mapping_matrix_soa_demixing.gain,
226         mapping_matrix_soa_demixing_data, 121 * sizeof(opus_int16));
227     }
228     else if (order_plus_one == 4)
229     {
230       mapping_matrix_init(demixing_matrix, mapping_matrix_toa_demixing.rows,
231         mapping_matrix_toa_demixing.cols, mapping_matrix_toa_demixing.gain,
232         mapping_matrix_toa_demixing_data, 324 * sizeof(opus_int16));
233     }
234     st->demixing_matrix_size_in_bytes = mapping_matrix_get_size(
235       demixing_matrix->rows, demixing_matrix->cols);
236   }
237   else
238     return OPUS_UNIMPLEMENTED;
239
240   /* Ensure matrices are large enough for desired coding scheme. */
241   if (nb_streams + nb_coupled_streams > mixing_matrix->rows ||
242       channels > mixing_matrix->cols ||
243       channels > demixing_matrix->rows ||
244       nb_streams + nb_coupled_streams > demixing_matrix->cols)
245     return OPUS_BAD_ARG;
246
247   /* Set trivial mapping so each input channel pairs with a matrix column. */
248   for (i = 0; i < channels; i++)
249     mapping[i] = i;
250
251   /* Initialize multistream encoder with provided settings. */
252   ms_encoder = get_multistream_encoder(st);
253   ret = opus_multistream_encoder_init(ms_encoder, Fs, channels, nb_streams,
254                                       nb_coupled_streams, mapping, application);
255   return ret;
256 }
257
258 OpusProjectionEncoder *opus_projection_ambisonics_encoder_create(
259     opus_int32 Fs, int channels, int mapping_family, int *streams,
260     int *coupled_streams, int application, int *error)
261 {
262   int size;
263   int ret;
264   OpusProjectionEncoder *st;
265
266   /* Allocate space for the projection encoder. */
267   size = opus_projection_ambisonics_encoder_get_size(channels, mapping_family);
268   if (!size) {
269     if (error)
270       *error = OPUS_ALLOC_FAIL;
271     return NULL;
272   }
273   st = (OpusProjectionEncoder *)opus_alloc(size);
274   if (!st)
275   {
276     if (error)
277       *error = OPUS_ALLOC_FAIL;
278     return NULL;
279   }
280
281   /* Initialize projection encoder with provided settings. */
282   ret = opus_projection_ambisonics_encoder_init(st, Fs, channels,
283      mapping_family, streams, coupled_streams, application);
284   if (ret != OPUS_OK)
285   {
286     opus_free(st);
287     st = NULL;
288   }
289   if (error)
290     *error = ret;
291   return st;
292 }
293
294 int opus_projection_encode(OpusProjectionEncoder *st, const opus_int16 *pcm,
295                            int frame_size, unsigned char *data,
296                            opus_int32 max_data_bytes)
297 {
298   return opus_multistream_encode_native(get_multistream_encoder(st),
299     opus_projection_copy_channel_in_short, pcm, frame_size, data,
300     max_data_bytes, 16, downmix_int, 0, get_mixing_matrix(st));
301 }
302
303 #ifndef DISABLE_FLOAT_API
304 #ifdef FIXED_POINT
305 int opus_projection_encode_float(OpusProjectionEncoder *st, const float *pcm,
306                                  int frame_size, unsigned char *data,
307                                  opus_int32 max_data_bytes)
308 {
309   return opus_multistream_encode_native(get_multistream_encoder(st),
310     opus_projection_copy_channel_in_float, pcm, frame_size, data,
311     max_data_bytes, 16, downmix_float, 1, get_mixing_matrix(st));
312 }
313 #else
314 int opus_projection_encode_float(OpusProjectionEncoder *st, const float *pcm,
315                                  int frame_size, unsigned char *data,
316                                  opus_int32 max_data_bytes)
317 {
318   return opus_multistream_encode_native(get_multistream_encoder(st),
319     opus_projection_copy_channel_in_float, pcm, frame_size, data,
320     max_data_bytes, 24, downmix_float, 1, get_mixing_matrix(st));
321 }
322 #endif
323 #endif
324
325 void opus_projection_encoder_destroy(OpusProjectionEncoder *st)
326 {
327   opus_free(st);
328 }
329
330 int opus_projection_encoder_ctl(OpusProjectionEncoder *st, int request, ...)
331 {
332   MappingMatrix *demixing_matrix;
333   OpusMSEncoder *ms_encoder;
334   int ret = OPUS_OK;
335
336   ms_encoder = get_multistream_encoder(st);
337   demixing_matrix = get_demixing_matrix(st);
338
339   va_list ap;
340   va_start(ap, request);
341   switch(request)
342   {
343   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_SIZE_REQUEST:
344   {
345     opus_int32 *value = va_arg(ap, opus_int32*);
346     if (!value)
347     {
348       goto bad_arg;
349     }
350     *value =
351       ms_encoder->layout.nb_channels * (ms_encoder->layout.nb_streams
352       + ms_encoder->layout.nb_coupled_streams) * sizeof(opus_int16);
353   }
354   break;
355   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_GAIN_REQUEST:
356   {
357     opus_int32 *value = va_arg(ap, opus_int32*);
358     if (!value)
359     {
360       goto bad_arg;
361     }
362     *value = demixing_matrix->gain;
363   }
364   break;
365   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_REQUEST:
366   {
367     int i, j, k, l;
368     int nb_input_streams;
369     int nb_output_streams;
370     unsigned char *external_char;
371     opus_int16 *internal_short;
372     opus_int32 external_size;
373     opus_int32 internal_size;
374
375     /* (I/O is in relation to the decoder's perspective). */
376     nb_input_streams = ms_encoder->layout.nb_streams +
377       ms_encoder->layout.nb_coupled_streams;
378     nb_output_streams = ms_encoder->layout.nb_channels;
379
380     external_char = va_arg(ap, unsigned char *);
381     external_size = va_arg(ap, opus_uint32);
382     if (!external_char)
383     {
384       goto bad_arg;
385     }
386     internal_short = mapping_matrix_get_data(demixing_matrix);
387     internal_size = nb_input_streams * nb_output_streams * sizeof(opus_int16);
388     if (external_size != internal_size)
389     {
390       goto bad_arg;
391     }
392
393     /* Copy demixing matrix subset to output destination. */
394     l = 0;
395     for (i = 0; i < nb_input_streams; i++) {
396       for (j = 0; j < nb_output_streams; j++) {
397         k = demixing_matrix->rows * i + j;
398         external_char[2*l] = (unsigned char)internal_short[k];
399         external_char[2*l+1] = (unsigned char)(internal_short[k] >> 8);
400         l++;
401       }
402     }
403   }
404   break;
405   default:
406   {
407     ret = opus_multistream_encoder_ctl_va_list(ms_encoder, request, ap);
408   }
409   break;
410   }
411   va_end(ap);
412   return ret;
413
414 bad_arg:
415   va_end(ap);
416   return OPUS_BAD_ARG;
417 }
418
419 #endif /* ENABLE_EXPERIMENTAL_AMBISONICS */