Using a first-order filter for DC rejection
[opus.git] / src / opus_projection_decoder.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 "mapping_matrix.h"
39 #include "stack_alloc.h"
40
41 #ifdef ENABLE_EXPERIMENTAL_AMBISONICS
42
43 struct OpusProjectionDecoder
44 {
45   opus_int32 demixing_matrix_size_in_bytes;
46   /* Encoder states go here */
47 };
48
49 #if !defined(DISABLE_FLOAT_API)
50 static void opus_projection_copy_channel_out_float(
51   void *dst,
52   int dst_stride,
53   int dst_channel,
54   const opus_val16 *src,
55   int src_stride,
56   int frame_size,
57   void *user_data)
58 {
59   float *float_dst;
60   const MappingMatrix *matrix;
61   float_dst = (float *)dst;
62   matrix = (const MappingMatrix *)user_data;
63
64   if (dst_channel == 0)
65     OPUS_CLEAR(float_dst, frame_size * dst_stride);
66
67   if (src != NULL)
68     mapping_matrix_multiply_channel_out_float(matrix, src, dst_channel,
69       src_stride, float_dst, dst_stride, frame_size);
70 }
71 #endif
72
73 static void opus_projection_copy_channel_out_short(
74   void *dst,
75   int dst_stride,
76   int dst_channel,
77   const opus_val16 *src,
78   int src_stride,
79   int frame_size,
80   void *user_data)
81 {
82   opus_int16 *short_dst;
83   const MappingMatrix *matrix;
84   short_dst = (opus_int16 *)dst;
85   matrix = (const MappingMatrix *)user_data;
86   if (dst_channel == 0)
87     OPUS_CLEAR(short_dst, frame_size * dst_stride);
88
89   if (src != NULL)
90     mapping_matrix_multiply_channel_out_short(matrix, src, dst_channel,
91       src_stride, short_dst, dst_stride, frame_size);
92 }
93
94 static MappingMatrix *get_demixing_matrix(OpusProjectionDecoder *st)
95 {
96   return (MappingMatrix*)((char*)st + align(sizeof(OpusProjectionDecoder)));
97 }
98
99 static OpusMSDecoder *get_multistream_decoder(OpusProjectionDecoder *st)
100 {
101   return (OpusMSDecoder*)((char*)st + align(sizeof(OpusProjectionDecoder) +
102     st->demixing_matrix_size_in_bytes));
103 }
104
105 opus_int32 opus_projection_decoder_get_size(int channels, int streams,
106                                             int coupled_streams)
107 {
108   opus_int32 matrix_size;
109   opus_int32 decoder_size;
110
111   matrix_size =
112     mapping_matrix_get_size(streams + coupled_streams, channels);
113   if (!matrix_size)
114     return 0;
115
116   decoder_size = opus_multistream_decoder_get_size(streams, coupled_streams);
117   if (!decoder_size)
118     return 0;
119
120   return align(sizeof(OpusProjectionDecoder)) + matrix_size + decoder_size;
121 }
122
123 int opus_projection_decoder_init(OpusProjectionDecoder *st, opus_int32 Fs,
124   int channels, int streams, int coupled_streams,
125   unsigned char *demixing_matrix, opus_int32 demixing_matrix_size)
126 {
127   int nb_input_streams;
128   opus_int32 expected_matrix_size;
129   int i, ret;
130   unsigned char mapping[255];
131   VARDECL(opus_int16, buf);
132   ALLOC_STACK;
133
134   /* Verify supplied matrix size. */
135   nb_input_streams = streams + coupled_streams;
136   expected_matrix_size = nb_input_streams * channels * sizeof(opus_int16);
137   if (expected_matrix_size != demixing_matrix_size)
138   {
139     RESTORE_STACK;
140     return OPUS_BAD_ARG;
141   }
142
143   /* Convert demixing matrix input into internal format. */
144   ALLOC(buf, nb_input_streams * channels, opus_int16);
145   for (i = 0; i < nb_input_streams * channels; i++)
146   {
147     int s = demixing_matrix[2*i + 1] << 8 | demixing_matrix[2*i];
148     s = ((s & 0xFFFF) ^ 0x8000) - 0x8000;
149     buf[i] = (opus_int16)s;
150   }
151
152   /* Assign demixing matrix. */
153   st->demixing_matrix_size_in_bytes =
154     mapping_matrix_get_size(channels, nb_input_streams);
155   if (!st->demixing_matrix_size_in_bytes)
156   {
157     RESTORE_STACK;
158     return OPUS_BAD_ARG;
159   }
160
161   mapping_matrix_init(get_demixing_matrix(st), channels, nb_input_streams, 0,
162     buf, demixing_matrix_size);
163
164   /* Set trivial mapping so each input channel pairs with a matrix column. */
165   for (i = 0; i < channels; i++)
166     mapping[i] = i;
167
168   ret = opus_multistream_decoder_init(
169     get_multistream_decoder(st), Fs, channels, streams, coupled_streams, mapping);
170   RESTORE_STACK;
171   return ret;
172 }
173
174 OpusProjectionDecoder *opus_projection_decoder_create(
175   opus_int32 Fs, int channels, int streams, int coupled_streams,
176   unsigned char *demixing_matrix, opus_int32 demixing_matrix_size, int *error)
177 {
178   int size;
179   int ret;
180   OpusProjectionDecoder *st;
181
182   /* Allocate space for the projection decoder. */
183   size = opus_projection_decoder_get_size(channels, streams, coupled_streams);
184   if (!size) {
185     if (error)
186       *error = OPUS_ALLOC_FAIL;
187     return NULL;
188   }
189   st = (OpusProjectionDecoder *)opus_alloc(size);
190   if (!st)
191   {
192     if (error)
193       *error = OPUS_ALLOC_FAIL;
194     return NULL;
195   }
196
197   /* Initialize projection decoder with provided settings. */
198   ret = opus_projection_decoder_init(st, Fs, channels, streams, coupled_streams,
199                                      demixing_matrix, demixing_matrix_size);
200   if (ret != OPUS_OK)
201   {
202     opus_free(st);
203     st = NULL;
204   }
205   if (error)
206     *error = ret;
207   return st;
208 }
209
210 #ifdef FIXED_POINT
211 int opus_projection_decode(OpusProjectionDecoder *st, const unsigned char *data,
212                            opus_int32 len, opus_int16 *pcm, int frame_size,
213                            int decode_fec)
214 {
215   return opus_multistream_decode_native(get_multistream_decoder(st), data, len,
216     pcm, opus_projection_copy_channel_out_short, frame_size, decode_fec, 0,
217     get_demixing_matrix(st));
218 }
219 #else
220 int opus_projection_decode(OpusProjectionDecoder *st, const unsigned char *data,
221                            opus_int32 len, opus_int16 *pcm, int frame_size,
222                            int decode_fec)
223 {
224   return opus_multistream_decode_native(get_multistream_decoder(st), data, len,
225     pcm, opus_projection_copy_channel_out_short, frame_size, decode_fec, 1,
226     get_demixing_matrix(st));
227 }
228 #endif
229
230 #ifndef DISABLE_FLOAT_API
231 int opus_projection_decode_float(OpusProjectionDecoder *st, const unsigned char *data,
232                                  opus_int32 len, float *pcm, int frame_size, int decode_fec)
233 {
234   return opus_multistream_decode_native(get_multistream_decoder(st), data, len,
235     pcm, opus_projection_copy_channel_out_float, frame_size, decode_fec, 0,
236     get_demixing_matrix(st));
237 }
238 #endif
239
240 int opus_projection_decoder_ctl(OpusProjectionDecoder *st, int request, ...)
241 {
242   va_list ap;
243   int ret = OPUS_OK;
244
245   va_start(ap, request);
246   ret = opus_multistream_decoder_ctl_va_list(get_multistream_decoder(st),
247     request, ap);
248   va_end(ap);
249   return ret;
250 }
251
252 void opus_projection_decoder_destroy(OpusProjectionDecoder *st)
253 {
254   opus_free(st);
255 }
256
257 #endif /* ENABLE_EXPERIMENTAL_AMBISONICS */