Only call isqrt32() with a positive argument
[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   if (channels < 1 || channels > 227)
90     return OPUS_BAD_ARG;
91
92   order_plus_one_ = isqrt32(channels);
93   acn_channels = order_plus_one_ * order_plus_one_;
94   nondiegetic_channels = channels - acn_channels;
95   if (nondiegetic_channels != 0 && nondiegetic_channels != 2)
96     return OPUS_BAD_ARG;
97
98   if (order_plus_one)
99     *order_plus_one = order_plus_one_;
100   return OPUS_OK;
101 }
102
103 static int get_streams_from_channels(int channels, int mapping_family,
104                                      int *streams, int *coupled_streams,
105                                      int *order_plus_one)
106 {
107   if (mapping_family == 253)
108   {
109     if (get_order_plus_one_from_channels(channels, order_plus_one) != OPUS_OK)
110       return OPUS_BAD_ARG;
111     if (streams)
112       *streams = (channels + 1) / 2;
113     if (coupled_streams)
114       *coupled_streams = channels / 2;
115     return OPUS_OK;
116   }
117   return OPUS_BAD_ARG;
118 }
119
120 static MappingMatrix *get_mixing_matrix(OpusProjectionEncoder *st)
121 {
122   return (MappingMatrix *)((char*)st + align(sizeof(OpusProjectionEncoder)));
123 }
124
125 static MappingMatrix *get_demixing_matrix(OpusProjectionEncoder *st)
126 {
127   return (MappingMatrix *)((char*)st + align(sizeof(OpusProjectionEncoder) +
128     st->mixing_matrix_size_in_bytes));
129 }
130
131 static OpusMSEncoder *get_multistream_encoder(OpusProjectionEncoder *st)
132 {
133   return (OpusMSEncoder *)((char*)st + align(sizeof(OpusProjectionEncoder) +
134     st->mixing_matrix_size_in_bytes + st->demixing_matrix_size_in_bytes));
135 }
136
137 opus_int32 opus_projection_ambisonics_encoder_get_size(int channels,
138                                                        int mapping_family)
139 {
140   int nb_streams;
141   int nb_coupled_streams;
142   int order_plus_one;
143   int mixing_matrix_rows, mixing_matrix_cols;
144   int demixing_matrix_rows, demixing_matrix_cols;
145   opus_int32 mixing_matrix_size, demixing_matrix_size;
146   opus_int32 encoder_size;
147   int ret;
148
149   ret = get_streams_from_channels(channels, mapping_family, &nb_streams,
150                                   &nb_coupled_streams, &order_plus_one);
151   if (ret != OPUS_OK)
152     return 0;
153
154   if (order_plus_one == 2)
155   {
156     mixing_matrix_rows = mapping_matrix_foa_mixing.rows;
157     mixing_matrix_cols = mapping_matrix_foa_mixing.cols;
158     demixing_matrix_rows = mapping_matrix_foa_demixing.rows;
159     demixing_matrix_cols = mapping_matrix_foa_demixing.cols;
160   }
161   else if (order_plus_one == 3)
162   {
163     mixing_matrix_rows = mapping_matrix_soa_mixing.rows;
164     mixing_matrix_cols = mapping_matrix_soa_mixing.cols;
165     demixing_matrix_rows = mapping_matrix_soa_demixing.rows;
166     demixing_matrix_cols = mapping_matrix_soa_demixing.cols;
167   }
168   else if (order_plus_one == 4)
169   {
170     mixing_matrix_rows = mapping_matrix_toa_mixing.rows;
171     mixing_matrix_cols = mapping_matrix_toa_mixing.cols;
172     demixing_matrix_rows = mapping_matrix_toa_demixing.rows;
173     demixing_matrix_cols = mapping_matrix_toa_demixing.cols;
174   }
175   else
176     return 0;
177
178   mixing_matrix_size =
179     mapping_matrix_get_size(mixing_matrix_rows, mixing_matrix_cols);
180   if (!mixing_matrix_size)
181     return 0;
182
183   demixing_matrix_size =
184     mapping_matrix_get_size(demixing_matrix_rows, demixing_matrix_cols);
185   if (!demixing_matrix_size)
186     return 0;
187
188   encoder_size =
189       opus_multistream_encoder_get_size(nb_streams, nb_coupled_streams);
190   if (!encoder_size)
191     return 0;
192
193   return align(sizeof(OpusProjectionEncoder)) +
194     mixing_matrix_size + demixing_matrix_size + encoder_size;
195 }
196
197 int opus_projection_ambisonics_encoder_init(OpusProjectionEncoder *st, opus_int32 Fs,
198                                             int channels, int mapping_family,
199                                             int *streams, int *coupled_streams,
200                                             int application)
201 {
202   MappingMatrix *mixing_matrix;
203   MappingMatrix *demixing_matrix;
204   OpusMSEncoder *ms_encoder;
205   int i;
206   int ret;
207   int order_plus_one;
208   unsigned char mapping[255];
209
210   if (streams == NULL || coupled_streams == NULL) {
211     return OPUS_BAD_ARG;
212   }
213
214   if (get_streams_from_channels(channels, mapping_family, streams,
215     coupled_streams, &order_plus_one) != OPUS_OK)
216     return OPUS_BAD_ARG;
217
218   if (mapping_family == 253)
219   {
220     /* Assign mixing matrix based on available pre-computed matrices. */
221     mixing_matrix = get_mixing_matrix(st);
222     if (order_plus_one == 2)
223     {
224       mapping_matrix_init(mixing_matrix, mapping_matrix_foa_mixing.rows,
225         mapping_matrix_foa_mixing.cols, mapping_matrix_foa_mixing.gain,
226         mapping_matrix_foa_mixing_data,
227         sizeof(mapping_matrix_foa_mixing_data));
228     }
229     else if (order_plus_one == 3)
230     {
231       mapping_matrix_init(mixing_matrix, mapping_matrix_soa_mixing.rows,
232         mapping_matrix_soa_mixing.cols, mapping_matrix_soa_mixing.gain,
233         mapping_matrix_soa_mixing_data,
234         sizeof(mapping_matrix_soa_mixing_data));
235     }
236     else if (order_plus_one == 4)
237     {
238       mapping_matrix_init(mixing_matrix, mapping_matrix_toa_mixing.rows,
239         mapping_matrix_toa_mixing.cols, mapping_matrix_toa_mixing.gain,
240         mapping_matrix_toa_mixing_data,
241         sizeof(mapping_matrix_toa_mixing_data));
242     }
243     else
244       return OPUS_BAD_ARG;
245
246     st->mixing_matrix_size_in_bytes = mapping_matrix_get_size(
247       mixing_matrix->rows, mixing_matrix->cols);
248     if (!st->mixing_matrix_size_in_bytes)
249       return OPUS_BAD_ARG;
250
251     /* Assign demixing matrix based on available pre-computed matrices. */
252     demixing_matrix = get_demixing_matrix(st);
253     if (order_plus_one == 2)
254     {
255       mapping_matrix_init(demixing_matrix, mapping_matrix_foa_demixing.rows,
256         mapping_matrix_foa_demixing.cols, mapping_matrix_foa_demixing.gain,
257         mapping_matrix_foa_demixing_data,
258         sizeof(mapping_matrix_foa_demixing_data));
259     }
260     else if (order_plus_one == 3)
261     {
262       mapping_matrix_init(demixing_matrix, mapping_matrix_soa_demixing.rows,
263         mapping_matrix_soa_demixing.cols, mapping_matrix_soa_demixing.gain,
264         mapping_matrix_soa_demixing_data,
265         sizeof(mapping_matrix_soa_demixing_data));
266     }
267     else if (order_plus_one == 4)
268     {
269       mapping_matrix_init(demixing_matrix, mapping_matrix_toa_demixing.rows,
270         mapping_matrix_toa_demixing.cols, mapping_matrix_toa_demixing.gain,
271         mapping_matrix_toa_demixing_data,
272         sizeof(mapping_matrix_toa_demixing_data));
273     }
274     else
275       return OPUS_BAD_ARG;
276
277     st->demixing_matrix_size_in_bytes = mapping_matrix_get_size(
278       demixing_matrix->rows, demixing_matrix->cols);
279     if (!st->demixing_matrix_size_in_bytes)
280       return OPUS_BAD_ARG;
281   }
282   else
283     return OPUS_UNIMPLEMENTED;
284
285   /* Ensure matrices are large enough for desired coding scheme. */
286   if (*streams + *coupled_streams > mixing_matrix->rows ||
287       channels > mixing_matrix->cols ||
288       channels > demixing_matrix->rows ||
289       *streams + *coupled_streams > demixing_matrix->cols)
290     return OPUS_BAD_ARG;
291
292   /* Set trivial mapping so each input channel pairs with a matrix column. */
293   for (i = 0; i < channels; i++)
294     mapping[i] = i;
295
296   /* Initialize multistream encoder with provided settings. */
297   ms_encoder = get_multistream_encoder(st);
298   ret = opus_multistream_encoder_init(ms_encoder, Fs, channels, *streams,
299                                       *coupled_streams, mapping, application);
300   return ret;
301 }
302
303 OpusProjectionEncoder *opus_projection_ambisonics_encoder_create(
304     opus_int32 Fs, int channels, int mapping_family, int *streams,
305     int *coupled_streams, int application, int *error)
306 {
307   int size;
308   int ret;
309   OpusProjectionEncoder *st;
310
311   /* Allocate space for the projection encoder. */
312   size = opus_projection_ambisonics_encoder_get_size(channels, mapping_family);
313   if (!size) {
314     if (error)
315       *error = OPUS_ALLOC_FAIL;
316     return NULL;
317   }
318   st = (OpusProjectionEncoder *)opus_alloc(size);
319   if (!st)
320   {
321     if (error)
322       *error = OPUS_ALLOC_FAIL;
323     return NULL;
324   }
325
326   /* Initialize projection encoder with provided settings. */
327   ret = opus_projection_ambisonics_encoder_init(st, Fs, channels,
328      mapping_family, streams, coupled_streams, application);
329   if (ret != OPUS_OK)
330   {
331     opus_free(st);
332     st = NULL;
333   }
334   if (error)
335     *error = ret;
336   return st;
337 }
338
339 int opus_projection_encode(OpusProjectionEncoder *st, const opus_int16 *pcm,
340                            int frame_size, unsigned char *data,
341                            opus_int32 max_data_bytes)
342 {
343   return opus_multistream_encode_native(get_multistream_encoder(st),
344     opus_projection_copy_channel_in_short, pcm, frame_size, data,
345     max_data_bytes, 16, downmix_int, 0, get_mixing_matrix(st));
346 }
347
348 #ifndef DISABLE_FLOAT_API
349 #ifdef FIXED_POINT
350 int opus_projection_encode_float(OpusProjectionEncoder *st, const float *pcm,
351                                  int frame_size, unsigned char *data,
352                                  opus_int32 max_data_bytes)
353 {
354   return opus_multistream_encode_native(get_multistream_encoder(st),
355     opus_projection_copy_channel_in_float, pcm, frame_size, data,
356     max_data_bytes, 16, downmix_float, 1, get_mixing_matrix(st));
357 }
358 #else
359 int opus_projection_encode_float(OpusProjectionEncoder *st, const float *pcm,
360                                  int frame_size, unsigned char *data,
361                                  opus_int32 max_data_bytes)
362 {
363   return opus_multistream_encode_native(get_multistream_encoder(st),
364     opus_projection_copy_channel_in_float, pcm, frame_size, data,
365     max_data_bytes, 24, downmix_float, 1, get_mixing_matrix(st));
366 }
367 #endif
368 #endif
369
370 void opus_projection_encoder_destroy(OpusProjectionEncoder *st)
371 {
372   opus_free(st);
373 }
374
375 int opus_projection_encoder_ctl(OpusProjectionEncoder *st, int request, ...)
376 {
377   MappingMatrix *demixing_matrix;
378   OpusMSEncoder *ms_encoder;
379   int ret = OPUS_OK;
380
381   ms_encoder = get_multistream_encoder(st);
382   demixing_matrix = get_demixing_matrix(st);
383
384   va_list ap;
385   va_start(ap, request);
386   switch(request)
387   {
388   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_SIZE_REQUEST:
389   {
390     opus_int32 *value = va_arg(ap, opus_int32*);
391     if (!value)
392     {
393       goto bad_arg;
394     }
395     *value =
396       ms_encoder->layout.nb_channels * (ms_encoder->layout.nb_streams
397       + ms_encoder->layout.nb_coupled_streams) * sizeof(opus_int16);
398   }
399   break;
400   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_GAIN_REQUEST:
401   {
402     opus_int32 *value = va_arg(ap, opus_int32*);
403     if (!value)
404     {
405       goto bad_arg;
406     }
407     *value = demixing_matrix->gain;
408   }
409   break;
410   case OPUS_PROJECTION_GET_DEMIXING_MATRIX_REQUEST:
411   {
412     int i, j, k, l;
413     int nb_input_streams;
414     int nb_output_streams;
415     unsigned char *external_char;
416     opus_int16 *internal_short;
417     opus_int32 external_size;
418     opus_int32 internal_size;
419
420     /* (I/O is in relation to the decoder's perspective). */
421     nb_input_streams = ms_encoder->layout.nb_streams +
422       ms_encoder->layout.nb_coupled_streams;
423     nb_output_streams = ms_encoder->layout.nb_channels;
424
425     external_char = va_arg(ap, unsigned char *);
426     external_size = va_arg(ap, opus_int32);
427     if (!external_char)
428     {
429       goto bad_arg;
430     }
431     internal_short = mapping_matrix_get_data(demixing_matrix);
432     internal_size = nb_input_streams * nb_output_streams * sizeof(opus_int16);
433     if (external_size != internal_size)
434     {
435       goto bad_arg;
436     }
437
438     /* Copy demixing matrix subset to output destination. */
439     l = 0;
440     for (i = 0; i < nb_input_streams; i++) {
441       for (j = 0; j < nb_output_streams; j++) {
442         k = demixing_matrix->rows * i + j;
443         external_char[2*l] = (unsigned char)internal_short[k];
444         external_char[2*l+1] = (unsigned char)(internal_short[k] >> 8);
445         l++;
446       }
447     }
448   }
449   break;
450   default:
451   {
452     ret = opus_multistream_encoder_ctl_va_list(ms_encoder, request, ap);
453   }
454   break;
455   }
456   va_end(ap);
457   return ret;
458
459 bad_arg:
460   va_end(ap);
461   return OPUS_BAD_ARG;
462 }
463
464 #else /* ENABLE_EXPERIMENTAL_AMBISONICS */
465
466 opus_int32 opus_projection_ambisonics_encoder_get_size(
467     int channels, int mapping_family)
468 {
469   (void)channels;
470   (void)mapping_family;
471   return OPUS_UNIMPLEMENTED;
472 }
473
474 OpusProjectionEncoder *opus_projection_ambisonics_encoder_create(
475     opus_int32 Fs, int channels, int mapping_family, int *streams,
476     int *coupled_streams, int application, int *error)
477 {
478   (void)Fs;
479   (void)channels;
480   (void)mapping_family;
481   (void)streams;
482   (void)coupled_streams;
483   (void)application;
484   if (error) *error = OPUS_UNIMPLEMENTED;
485   return NULL;
486 }
487
488 int opus_projection_ambisonics_encoder_init(
489     OpusProjectionEncoder *st,
490     opus_int32 Fs,
491     int channels,
492     int mapping_family,
493     int *streams,
494     int *coupled_streams,
495     int application)
496 {
497   (void)st;
498   (void)Fs;
499   (void)channels;
500   (void)mapping_family;
501   (void)streams;
502   (void)coupled_streams;
503   (void)application;
504   return OPUS_UNIMPLEMENTED;
505 }
506
507 int opus_projection_encode(
508     OpusProjectionEncoder *st,
509     const opus_int16 *pcm,
510     int frame_size,
511     unsigned char *data,
512     opus_int32 max_data_bytes)
513 {
514   (void)st;
515   (void)pcm;
516   (void)frame_size;
517   (void)data;
518   (void)max_data_bytes;
519   return OPUS_UNIMPLEMENTED;
520 }
521
522 int opus_projection_encode_float(
523     OpusProjectionEncoder *st,
524     const float *pcm,
525     int frame_size,
526     unsigned char *data,
527     opus_int32 max_data_bytes)
528 {
529   (void)st;
530   (void)pcm;
531   (void)frame_size;
532   (void)data;
533   (void)max_data_bytes;
534   return OPUS_UNIMPLEMENTED;
535 }
536
537 void opus_projection_encoder_destroy(
538     OpusProjectionEncoder *st)
539 {
540   (void)st;
541 }
542
543 int opus_projection_encoder_ctl(
544     OpusProjectionEncoder *st,
545     int request,
546     ...)
547 {
548   (void)st;
549   (void)request;
550   return OPUS_UNIMPLEMENTED;
551 }
552
553 #endif /* ENABLE_EXPERIMENTAL_AMBISONICS */