1 /* Copyright 2019 The Chromium OS Authors. All rights reserved.
2 * Use of this source code is governed by a BSD-style license that can be
3 * found in the LICENSE file.
4 */
5 #include <stdint.h>
6 #include <limits.h>
7 #include <string.h>
8
9 #include "cras_fmt_conv_ops.h"
10
11 #define MAX(a, b) \
12 ({ \
13 __typeof__(a) _a = (a); \
14 __typeof__(b) _b = (b); \
15 _a > _b ? _a : _b; \
16 })
17 #define MIN(a, b) \
18 ({ \
19 __typeof__(a) _a = (a); \
20 __typeof__(b) _b = (b); \
21 _a < _b ? _a : _b; \
22 })
23
24 /*
25 * Add and clip.
26 */
s16_add_and_clip(int16_t a,int16_t b)27 static int16_t s16_add_and_clip(int16_t a, int16_t b)
28 {
29 int32_t sum;
30
31 a = htole16(a);
32 b = htole16(b);
33 sum = (int32_t)a + (int32_t)b;
34 sum = MAX(sum, SHRT_MIN);
35 sum = MIN(sum, SHRT_MAX);
36 return (int16_t)le16toh(sum);
37 }
38
39 /*
40 * Format converter.
41 */
convert_u8_to_s16le(const uint8_t * in,size_t in_samples,uint8_t * out)42 void convert_u8_to_s16le(const uint8_t *in, size_t in_samples, uint8_t *out)
43 {
44 size_t i;
45 uint16_t *_out = (uint16_t *)out;
46
47 for (i = 0; i < in_samples; i++, in++, _out++)
48 *_out = (uint16_t)((int16_t)*in - 0x80) << 8;
49 }
50
convert_s243le_to_s16le(const uint8_t * in,size_t in_samples,uint8_t * out)51 void convert_s243le_to_s16le(const uint8_t *in, size_t in_samples, uint8_t *out)
52 {
53 /* find how to calculate in and out size, implement the conversion
54 * between S24_3LE and S16 */
55
56 size_t i;
57 int8_t *_in = (int8_t *)in;
58 uint16_t *_out = (uint16_t *)out;
59
60 for (i = 0; i < in_samples; i++, _in += 3, _out++)
61 memcpy(_out, _in + 1, 2);
62 }
63
convert_s24le_to_s16le(const uint8_t * in,size_t in_samples,uint8_t * out)64 void convert_s24le_to_s16le(const uint8_t *in, size_t in_samples, uint8_t *out)
65 {
66 size_t i;
67 int32_t *_in = (int32_t *)in;
68 uint16_t *_out = (uint16_t *)out;
69
70 for (i = 0; i < in_samples; i++, _in++, _out++)
71 *_out = (int16_t)((*_in & 0x00ffffff) >> 8);
72 }
73
convert_s32le_to_s16le(const uint8_t * in,size_t in_samples,uint8_t * out)74 void convert_s32le_to_s16le(const uint8_t *in, size_t in_samples, uint8_t *out)
75 {
76 size_t i;
77 int32_t *_in = (int32_t *)in;
78 uint16_t *_out = (uint16_t *)out;
79
80 for (i = 0; i < in_samples; i++, _in++, _out++)
81 *_out = (int16_t)(*_in >> 16);
82 }
83
convert_s16le_to_u8(const uint8_t * in,size_t in_samples,uint8_t * out)84 void convert_s16le_to_u8(const uint8_t *in, size_t in_samples, uint8_t *out)
85 {
86 size_t i;
87 int16_t *_in = (int16_t *)in;
88
89 for (i = 0; i < in_samples; i++, _in++, out++)
90 *out = (uint8_t)(*_in >> 8) + 128;
91 }
92
convert_s16le_to_s243le(const uint8_t * in,size_t in_samples,uint8_t * out)93 void convert_s16le_to_s243le(const uint8_t *in, size_t in_samples, uint8_t *out)
94 {
95 size_t i;
96 int16_t *_in = (int16_t *)in;
97 uint8_t *_out = (uint8_t *)out;
98
99 for (i = 0; i < in_samples; i++, _in++, _out += 3) {
100 *_out = 0;
101 memcpy(_out + 1, _in, 2);
102 }
103 }
104
convert_s16le_to_s24le(const uint8_t * in,size_t in_samples,uint8_t * out)105 void convert_s16le_to_s24le(const uint8_t *in, size_t in_samples, uint8_t *out)
106 {
107 size_t i;
108 int16_t *_in = (int16_t *)in;
109 uint32_t *_out = (uint32_t *)out;
110
111 for (i = 0; i < in_samples; i++, _in++, _out++)
112 *_out = ((uint32_t)(int32_t)*_in << 8);
113 }
114
convert_s16le_to_s32le(const uint8_t * in,size_t in_samples,uint8_t * out)115 void convert_s16le_to_s32le(const uint8_t *in, size_t in_samples, uint8_t *out)
116 {
117 size_t i;
118 int16_t *_in = (int16_t *)in;
119 uint32_t *_out = (uint32_t *)out;
120
121 for (i = 0; i < in_samples; i++, _in++, _out++)
122 *_out = ((uint32_t)(int32_t)*_in << 16);
123 }
124
125 /*
126 * Channel converter: mono to stereo.
127 */
s16_mono_to_stereo(const uint8_t * _in,size_t in_frames,uint8_t * _out)128 size_t s16_mono_to_stereo(const uint8_t *_in, size_t in_frames, uint8_t *_out)
129 {
130 size_t i;
131 const int16_t *in = (const int16_t *)_in;
132 int16_t *out = (int16_t *)_out;
133
134 for (i = 0; i < in_frames; i++) {
135 out[2 * i] = in[i];
136 out[2 * i + 1] = in[i];
137 }
138 return in_frames;
139 }
140
141 /*
142 * Channel converter: stereo to mono.
143 */
s16_stereo_to_mono(const uint8_t * _in,size_t in_frames,uint8_t * _out)144 size_t s16_stereo_to_mono(const uint8_t *_in, size_t in_frames, uint8_t *_out)
145 {
146 size_t i;
147 const int16_t *in = (const int16_t *)_in;
148 int16_t *out = (int16_t *)_out;
149
150 for (i = 0; i < in_frames; i++)
151 out[i] = s16_add_and_clip(in[2 * i], in[2 * i + 1]);
152 return in_frames;
153 }
154
155 /*
156 * Channel converter: mono to 5.1 surround.
157 *
158 * Fit mono to front center of the output, or split to front left/right
159 * if front center is missing from the output channel layout.
160 */
s16_mono_to_51(size_t left,size_t right,size_t center,const uint8_t * _in,size_t in_frames,uint8_t * _out)161 size_t s16_mono_to_51(size_t left, size_t right, size_t center,
162 const uint8_t *_in, size_t in_frames, uint8_t *_out)
163 {
164 size_t i;
165 const int16_t *in = (const int16_t *)_in;
166 int16_t *out = (int16_t *)_out;
167
168 memset(out, 0, sizeof(*out) * 6 * in_frames);
169
170 if (center != -1)
171 for (i = 0; i < in_frames; i++)
172 out[6 * i + center] = in[i];
173 else if (left != -1 && right != -1)
174 for (i = 0; i < in_frames; i++) {
175 out[6 * i + right] = in[i] / 2;
176 out[6 * i + left] = in[i] / 2;
177 }
178 else
179 /* Select the first channel to convert to as the
180 * default behavior.
181 */
182 for (i = 0; i < in_frames; i++)
183 out[6 * i] = in[i];
184
185 return in_frames;
186 }
187
188 /*
189 * Channel converter: stereo to 5.1 surround.
190 *
191 * Fit the left/right of input to the front left/right of output respectively
192 * and fill others with zero. If any of the front left/right is missed from
193 * the output channel layout, mix to front center.
194 */
s16_stereo_to_51(size_t left,size_t right,size_t center,const uint8_t * _in,size_t in_frames,uint8_t * _out)195 size_t s16_stereo_to_51(size_t left, size_t right, size_t center,
196 const uint8_t *_in, size_t in_frames, uint8_t *_out)
197 {
198 size_t i;
199 const int16_t *in = (const int16_t *)_in;
200 int16_t *out = (int16_t *)_out;
201
202 memset(out, 0, sizeof(*out) * 6 * in_frames);
203
204 if (left != -1 && right != -1)
205 for (i = 0; i < in_frames; i++) {
206 out[6 * i + left] = in[2 * i];
207 out[6 * i + right] = in[2 * i + 1];
208 }
209 else if (center != -1)
210 for (i = 0; i < in_frames; i++)
211 out[6 * i + center] =
212 s16_add_and_clip(in[2 * i], in[2 * i + 1]);
213 else
214 /* Select the first two channels to convert to as the
215 * default behavior.
216 */
217 for (i = 0; i < in_frames; i++) {
218 out[6 * i] = in[2 * i];
219 out[6 * i + 1] = in[2 * i + 1];
220 }
221
222 return in_frames;
223 }
224
225 /*
226 * Channel converter: 5.1 surround to stereo.
227 *
228 * The out buffer can have room for just stereo samples. This convert function
229 * is used as the default behavior when channel layout is not set from the
230 * client side.
231 */
s16_51_to_stereo(const uint8_t * _in,size_t in_frames,uint8_t * _out)232 size_t s16_51_to_stereo(const uint8_t *_in, size_t in_frames, uint8_t *_out)
233 {
234 const int16_t *in = (const int16_t *)_in;
235 int16_t *out = (int16_t *)_out;
236 static const unsigned int left_idx = 0;
237 static const unsigned int right_idx = 1;
238 /* static const unsigned int left_surround_idx = 2; */
239 /* static const unsigned int right_surround_idx = 3; */
240 static const unsigned int center_idx = 4;
241 /* static const unsigned int lfe_idx = 5; */
242 size_t i;
243
244 for (i = 0; i < in_frames; i++) {
245 unsigned int half_center;
246
247 half_center = in[6 * i + center_idx] / 2;
248 out[2 * i + left_idx] =
249 s16_add_and_clip(in[6 * i + left_idx], half_center);
250 out[2 * i + right_idx] =
251 s16_add_and_clip(in[6 * i + right_idx], half_center);
252 }
253 return in_frames;
254 }
255
256 /*
257 * Channel converter: stereo to quad (front L/R, rear L/R).
258 *
259 * Fit left/right of input to the front left/right of output respectively
260 * and fill others with zero.
261 */
s16_stereo_to_quad(size_t front_left,size_t front_right,size_t rear_left,size_t rear_right,const uint8_t * _in,size_t in_frames,uint8_t * _out)262 size_t s16_stereo_to_quad(size_t front_left, size_t front_right,
263 size_t rear_left, size_t rear_right,
264 const uint8_t *_in, size_t in_frames, uint8_t *_out)
265 {
266 size_t i;
267 const int16_t *in = (const int16_t *)_in;
268 int16_t *out = (int16_t *)_out;
269
270 if (front_left != -1 && front_right != -1 && rear_left != -1 &&
271 rear_right != -1)
272 for (i = 0; i < in_frames; i++) {
273 out[4 * i + front_left] = in[2 * i];
274 out[4 * i + front_right] = in[2 * i + 1];
275 out[4 * i + rear_left] = in[2 * i];
276 out[4 * i + rear_right] = in[2 * i + 1];
277 }
278 else
279 /* Select the first four channels to convert to as the
280 * default behavior.
281 */
282 for (i = 0; i < in_frames; i++) {
283 out[4 * i] = in[2 * i];
284 out[4 * i + 1] = in[2 * i + 1];
285 out[4 * i + 2] = in[2 * i];
286 out[4 * i + 3] = in[2 * i + 1];
287 }
288
289 return in_frames;
290 }
291
292 /*
293 * Channel converter: quad (front L/R, rear L/R) to stereo.
294 */
s16_quad_to_stereo(size_t front_left,size_t front_right,size_t rear_left,size_t rear_right,const uint8_t * _in,size_t in_frames,uint8_t * _out)295 size_t s16_quad_to_stereo(size_t front_left, size_t front_right,
296 size_t rear_left, size_t rear_right,
297 const uint8_t *_in, size_t in_frames, uint8_t *_out)
298 {
299 size_t i;
300 const int16_t *in = (const int16_t *)_in;
301 int16_t *out = (int16_t *)_out;
302
303 if (front_left == -1 || front_right == -1 || rear_left == -1 ||
304 rear_right == -1) {
305 front_left = 0;
306 front_right = 1;
307 rear_left = 2;
308 rear_right = 3;
309 }
310
311 for (i = 0; i < in_frames; i++) {
312 out[2 * i] = s16_add_and_clip(in[4 * i + front_left],
313 in[4 * i + rear_left] / 4);
314 out[2 * i + 1] = s16_add_and_clip(in[4 * i + front_right],
315 in[4 * i + rear_right] / 4);
316 }
317 return in_frames;
318 }
319
320 /*
321 * Channel converter: N channels to M channels.
322 *
323 * The out buffer must have room for M channel. This convert function is used
324 * as the default behavior when channel layout is not set from the client side.
325 */
s16_default_all_to_all(struct cras_audio_format * out_fmt,size_t num_in_ch,size_t num_out_ch,const uint8_t * _in,size_t in_frames,uint8_t * _out)326 size_t s16_default_all_to_all(struct cras_audio_format *out_fmt,
327 size_t num_in_ch, size_t num_out_ch,
328 const uint8_t *_in, size_t in_frames,
329 uint8_t *_out)
330 {
331 unsigned int in_ch, out_ch, i;
332 const int16_t *in = (const int16_t *)_in;
333 int16_t *out = (int16_t *)_out;
334
335 memset(out, 0, in_frames * cras_get_format_bytes(out_fmt));
336 for (out_ch = 0; out_ch < num_out_ch; out_ch++) {
337 for (in_ch = 0; in_ch < num_in_ch; in_ch++) {
338 for (i = 0; i < in_frames; i++) {
339 out[out_ch + i * num_out_ch] +=
340 in[in_ch + i * num_in_ch] / num_in_ch;
341 }
342 }
343 }
344 return in_frames;
345 }
346
347 /*
348 * Multiplies buffer vector with coefficient vector.
349 */
s16_multiply_buf_with_coef(float * coef,const int16_t * buf,size_t size)350 int16_t s16_multiply_buf_with_coef(float *coef, const int16_t *buf, size_t size)
351 {
352 int32_t sum = 0;
353 int i;
354
355 for (i = 0; i < size; i++)
356 sum += coef[i] * buf[i];
357 sum = MAX(sum, -0x8000);
358 sum = MIN(sum, 0x7fff);
359 return (int16_t)sum;
360 }
361
362 /*
363 * Channel layout converter.
364 *
365 * Converts channels based on the channel conversion coefficient matrix.
366 */
s16_convert_channels(float ** ch_conv_mtx,size_t num_in_ch,size_t num_out_ch,const uint8_t * _in,size_t in_frames,uint8_t * _out)367 size_t s16_convert_channels(float **ch_conv_mtx, size_t num_in_ch,
368 size_t num_out_ch, const uint8_t *_in,
369 size_t in_frames, uint8_t *_out)
370 {
371 unsigned i, fr;
372 unsigned in_idx = 0;
373 unsigned out_idx = 0;
374 const int16_t *in = (const int16_t *)_in;
375 int16_t *out = (int16_t *)_out;
376
377 for (fr = 0; fr < in_frames; fr++) {
378 for (i = 0; i < num_out_ch; i++)
379 out[out_idx + i] = s16_multiply_buf_with_coef(
380 ch_conv_mtx[i], &in[in_idx], num_in_ch);
381 in_idx += num_in_ch;
382 out_idx += num_out_ch;
383 }
384
385 return in_frames;
386 }
387