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