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