| /* Copyright 2019 The Chromium OS Authors. All rights reserved. |
| * Use of this source code is governed by a BSD-style license that can be |
| * found in the LICENSE file. |
| */ |
| #include <stdint.h> |
| #include <limits.h> |
| #include <string.h> |
| |
| #include "cras_fmt_conv_ops.h" |
| |
| #define MAX(a, b) \ |
| ({ \ |
| __typeof__(a) _a = (a); \ |
| __typeof__(b) _b = (b); \ |
| _a > _b ? _a : _b; \ |
| }) |
| #define MIN(a, b) \ |
| ({ \ |
| __typeof__(a) _a = (a); \ |
| __typeof__(b) _b = (b); \ |
| _a < _b ? _a : _b; \ |
| }) |
| |
| /* |
| * Add and clip. |
| */ |
| static int16_t s16_add_and_clip(int16_t a, int16_t b) |
| { |
| int32_t sum; |
| |
| a = htole16(a); |
| b = htole16(b); |
| sum = (int32_t)a + (int32_t)b; |
| sum = MAX(sum, SHRT_MIN); |
| sum = MIN(sum, SHRT_MAX); |
| return (int16_t)le16toh(sum); |
| } |
| |
| /* |
| * Format converter. |
| */ |
| void convert_u8_to_s16le(const uint8_t *in, size_t in_samples, uint8_t *out) |
| { |
| size_t i; |
| uint16_t *_out = (uint16_t *)out; |
| |
| for (i = 0; i < in_samples; i++, in++, _out++) |
| *_out = (uint16_t)((int16_t)*in - 0x80) << 8; |
| } |
| |
| void convert_s243le_to_s16le(const uint8_t *in, size_t in_samples, uint8_t *out) |
| { |
| /* find how to calculate in and out size, implement the conversion |
| * between S24_3LE and S16 */ |
| |
| size_t i; |
| int8_t *_in = (int8_t *)in; |
| uint16_t *_out = (uint16_t *)out; |
| |
| for (i = 0; i < in_samples; i++, _in += 3, _out++) |
| memcpy(_out, _in + 1, 2); |
| } |
| |
| void convert_s24le_to_s16le(const uint8_t *in, size_t in_samples, uint8_t *out) |
| { |
| size_t i; |
| int32_t *_in = (int32_t *)in; |
| uint16_t *_out = (uint16_t *)out; |
| |
| for (i = 0; i < in_samples; i++, _in++, _out++) |
| *_out = (int16_t)((*_in & 0x00ffffff) >> 8); |
| } |
| |
| void convert_s32le_to_s16le(const uint8_t *in, size_t in_samples, uint8_t *out) |
| { |
| size_t i; |
| int32_t *_in = (int32_t *)in; |
| uint16_t *_out = (uint16_t *)out; |
| |
| for (i = 0; i < in_samples; i++, _in++, _out++) |
| *_out = (int16_t)(*_in >> 16); |
| } |
| |
| void convert_s16le_to_u8(const uint8_t *in, size_t in_samples, uint8_t *out) |
| { |
| size_t i; |
| int16_t *_in = (int16_t *)in; |
| |
| for (i = 0; i < in_samples; i++, _in++, out++) |
| *out = (uint8_t)(*_in >> 8) + 128; |
| } |
| |
| void convert_s16le_to_s243le(const uint8_t *in, size_t in_samples, uint8_t *out) |
| { |
| size_t i; |
| int16_t *_in = (int16_t *)in; |
| uint8_t *_out = (uint8_t *)out; |
| |
| for (i = 0; i < in_samples; i++, _in++, _out += 3) { |
| *_out = 0; |
| memcpy(_out + 1, _in, 2); |
| } |
| } |
| |
| void convert_s16le_to_s24le(const uint8_t *in, size_t in_samples, uint8_t *out) |
| { |
| size_t i; |
| int16_t *_in = (int16_t *)in; |
| uint32_t *_out = (uint32_t *)out; |
| |
| for (i = 0; i < in_samples; i++, _in++, _out++) |
| *_out = ((uint32_t)(int32_t)*_in << 8); |
| } |
| |
| void convert_s16le_to_s32le(const uint8_t *in, size_t in_samples, uint8_t *out) |
| { |
| size_t i; |
| int16_t *_in = (int16_t *)in; |
| uint32_t *_out = (uint32_t *)out; |
| |
| for (i = 0; i < in_samples; i++, _in++, _out++) |
| *_out = ((uint32_t)(int32_t)*_in << 16); |
| } |
| |
| /* |
| * Channel converter: mono to stereo. |
| */ |
| size_t s16_mono_to_stereo(const uint8_t *_in, size_t in_frames, uint8_t *_out) |
| { |
| size_t i; |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| |
| for (i = 0; i < in_frames; i++) { |
| out[2 * i] = in[i]; |
| out[2 * i + 1] = in[i]; |
| } |
| return in_frames; |
| } |
| |
| /* |
| * Channel converter: stereo to mono. |
| */ |
| size_t s16_stereo_to_mono(const uint8_t *_in, size_t in_frames, uint8_t *_out) |
| { |
| size_t i; |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| |
| for (i = 0; i < in_frames; i++) |
| out[i] = s16_add_and_clip(in[2 * i], in[2 * i + 1]); |
| return in_frames; |
| } |
| |
| /* |
| * Channel converter: mono to 5.1 surround. |
| * |
| * Fit mono to front center of the output, or split to front left/right |
| * if front center is missing from the output channel layout. |
| */ |
| size_t s16_mono_to_51(size_t left, size_t right, size_t center, |
| const uint8_t *_in, size_t in_frames, uint8_t *_out) |
| { |
| size_t i; |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| |
| memset(out, 0, sizeof(*out) * 6 * in_frames); |
| |
| if (center != -1) |
| for (i = 0; i < in_frames; i++) |
| out[6 * i + center] = in[i]; |
| else if (left != -1 && right != -1) |
| for (i = 0; i < in_frames; i++) { |
| out[6 * i + right] = in[i] / 2; |
| out[6 * i + left] = in[i] / 2; |
| } |
| else |
| /* Select the first channel to convert to as the |
| * default behavior. |
| */ |
| for (i = 0; i < in_frames; i++) |
| out[6 * i] = in[i]; |
| |
| return in_frames; |
| } |
| |
| /* |
| * Channel converter: stereo to 5.1 surround. |
| * |
| * Fit the left/right of input to the front left/right of output respectively |
| * and fill others with zero. If any of the front left/right is missed from |
| * the output channel layout, mix to front center. |
| */ |
| size_t s16_stereo_to_51(size_t left, size_t right, size_t center, |
| const uint8_t *_in, size_t in_frames, uint8_t *_out) |
| { |
| size_t i; |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| |
| memset(out, 0, sizeof(*out) * 6 * in_frames); |
| |
| if (left != -1 && right != -1) |
| for (i = 0; i < in_frames; i++) { |
| out[6 * i + left] = in[2 * i]; |
| out[6 * i + right] = in[2 * i + 1]; |
| } |
| else if (center != -1) |
| for (i = 0; i < in_frames; i++) |
| out[6 * i + center] = |
| s16_add_and_clip(in[2 * i], in[2 * i + 1]); |
| else |
| /* Select the first two channels to convert to as the |
| * default behavior. |
| */ |
| for (i = 0; i < in_frames; i++) { |
| out[6 * i] = in[2 * i]; |
| out[6 * i + 1] = in[2 * i + 1]; |
| } |
| |
| return in_frames; |
| } |
| |
| /* |
| * Channel converter: quad to 5.1 surround. |
| * |
| * Fit the front left/right of input to the front left/right of output |
| * and rear left/right of input to the rear left/right of output |
| * respectively and fill others with zero. |
| */ |
| size_t s16_quad_to_51(size_t font_left, size_t front_right, size_t rear_left, |
| size_t rear_right, const uint8_t *_in, size_t in_frames, |
| uint8_t *_out) |
| { |
| size_t i; |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| |
| memset(out, 0, sizeof(*out) * 6 * in_frames); |
| |
| if (font_left != -1 && front_right != -1 && rear_left != -1 && |
| rear_right != -1) |
| for (i = 0; i < in_frames; i++) { |
| out[6 * i + font_left] = in[4 * i]; |
| out[6 * i + front_right] = in[4 * i + 1]; |
| out[6 * i + rear_left] = in[4 * i + 2]; |
| out[6 * i + rear_right] = in[4 * i + 3]; |
| } |
| else |
| /* Use default 5.1 channel mapping for the conversion. |
| */ |
| for (i = 0; i < in_frames; i++) { |
| out[6 * i] = in[4 * i]; |
| out[6 * i + 1] = in[4 * i + 1]; |
| out[6 * i + 4] = in[4 * i + 2]; |
| out[6 * i + 5] = in[4 * i + 3]; |
| } |
| |
| return in_frames; |
| } |
| |
| /* |
| * Channel converter: 5.1 surround to stereo. |
| * |
| * The out buffer can have room for just stereo samples. This convert function |
| * is used as the default behavior when channel layout is not set from the |
| * client side. |
| */ |
| size_t s16_51_to_stereo(const uint8_t *_in, size_t in_frames, uint8_t *_out) |
| { |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| static const unsigned int left_idx = 0; |
| static const unsigned int right_idx = 1; |
| static const unsigned int center_idx = 2; |
| /* static const unsigned int lfe_idx = 3; */ |
| /* static const unsigned int left_surround_idx = 4; */ |
| /* static const unsigned int right_surround_idx = 5; */ |
| |
| size_t i; |
| int16_t half_center; |
| /* Use the normalized_factor from the left channel = 1 / (|1| + |0.707|) |
| * to prevent mixing overflow. |
| */ |
| const float normalized_factor = 0.585; |
| for (i = 0; i < in_frames; i++) { |
| half_center = |
| in[6 * i + center_idx] * 0.707 * normalized_factor; |
| out[2 * i + left_idx] = |
| in[6 * i + left_idx] * normalized_factor + half_center; |
| out[2 * i + right_idx] = |
| in[6 * i + right_idx] * normalized_factor + half_center; |
| } |
| return in_frames; |
| } |
| |
| /* |
| * Channel converter: 5.1 surround to quad (front L/R, rear L/R). |
| * |
| * The out buffer can have room for just quad samples. This convert function |
| * is used as the default behavior when channel layout is not set from the |
| * client side. |
| */ |
| size_t s16_51_to_quad(const uint8_t *_in, size_t in_frames, uint8_t *_out) |
| { |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| static const unsigned int l_quad = 0; |
| static const unsigned int r_quad = 1; |
| static const unsigned int rl_quad = 2; |
| static const unsigned int rr_quad = 3; |
| |
| static const unsigned int l_51 = 0; |
| static const unsigned int r_51 = 1; |
| static const unsigned int center_51 = 2; |
| static const unsigned int lfe_51 = 3; |
| static const unsigned int rl_51 = 4; |
| static const unsigned int rr_51 = 5; |
| |
| /* Use normalized_factor from the left channel = 1 / (|1| + |0.707| + |0.5|) |
| * to prevent overflow. */ |
| const float normalized_factor = 0.453; |
| size_t i; |
| for (i = 0; i < in_frames; i++) { |
| int16_t half_center; |
| int16_t lfe; |
| |
| half_center = in[6 * i + center_51] * 0.707 * normalized_factor; |
| lfe = in[6 * i + lfe_51] * 0.5 * normalized_factor; |
| out[4 * i + l_quad] = normalized_factor * in[6 * i + l_51] + |
| half_center + lfe; |
| out[4 * i + r_quad] = normalized_factor * in[6 * i + r_51] + |
| half_center + lfe; |
| out[4 * i + rl_quad] = |
| normalized_factor * in[6 * i + rl_51] + lfe; |
| out[4 * i + rr_quad] = |
| normalized_factor * in[6 * i + rr_51] + lfe; |
| } |
| return in_frames; |
| } |
| |
| /* |
| * Channel converter: stereo to quad (front L/R, rear L/R). |
| * |
| * Fit left/right of input to the front left/right of output respectively |
| * and fill others with zero. |
| */ |
| size_t 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) |
| { |
| size_t i; |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| |
| if (front_left != -1 && front_right != -1 && rear_left != -1 && |
| rear_right != -1) |
| for (i = 0; i < in_frames; i++) { |
| out[4 * i + front_left] = in[2 * i]; |
| out[4 * i + front_right] = in[2 * i + 1]; |
| out[4 * i + rear_left] = in[2 * i]; |
| out[4 * i + rear_right] = in[2 * i + 1]; |
| } |
| else |
| /* Select the first four channels to convert to as the |
| * default behavior. |
| */ |
| for (i = 0; i < in_frames; i++) { |
| out[4 * i] = in[2 * i]; |
| out[4 * i + 1] = in[2 * i + 1]; |
| out[4 * i + 2] = in[2 * i]; |
| out[4 * i + 3] = in[2 * i + 1]; |
| } |
| |
| return in_frames; |
| } |
| |
| /* |
| * Channel converter: quad (front L/R, rear L/R) to stereo. |
| */ |
| size_t 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) |
| { |
| size_t i; |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| |
| if (front_left == -1 || front_right == -1 || rear_left == -1 || |
| rear_right == -1) { |
| front_left = 0; |
| front_right = 1; |
| rear_left = 2; |
| rear_right = 3; |
| } |
| |
| for (i = 0; i < in_frames; i++) { |
| out[2 * i] = s16_add_and_clip(in[4 * i + front_left], |
| in[4 * i + rear_left] / 4); |
| out[2 * i + 1] = s16_add_and_clip(in[4 * i + front_right], |
| in[4 * i + rear_right] / 4); |
| } |
| return in_frames; |
| } |
| |
| /* |
| * Channel converter: N channels to M channels. |
| * |
| * The out buffer must have room for M channel. This convert function is used |
| * as the default behavior when channel layout is not set from the client side. |
| */ |
| size_t 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) |
| { |
| unsigned int in_ch, out_ch, i; |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| int32_t sum; |
| |
| for (i = 0; i < in_frames; i++) { |
| sum = 0; |
| for (in_ch = 0; in_ch < num_in_ch; in_ch++) { |
| sum += (int32_t)in[in_ch + i * num_in_ch]; |
| } |
| /* |
| * 1. Divide `int32_t` by `size_t` without an explicit |
| * conversion will generate corrupted results. |
| * 2. After the division, `sum` should be in the range of |
| * int16_t. No clipping is needed. |
| */ |
| sum /= (int32_t)num_in_ch; |
| for (out_ch = 0; out_ch < num_out_ch; out_ch++) { |
| out[out_ch + i * num_out_ch] = (int16_t)sum; |
| } |
| } |
| return in_frames; |
| } |
| |
| /* |
| * Copies the input channels across output channels. Drops input channels that |
| * don't fit. Ignores output channels greater than the number of input channels. |
| */ |
| size_t s16_some_to_some(const struct cras_audio_format *out_fmt, |
| const size_t num_in_ch, const size_t num_out_ch, |
| const uint8_t *_in, const size_t frame_count, |
| uint8_t *_out) |
| { |
| unsigned int i; |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| const size_t num_copy_ch = MIN(num_in_ch, num_out_ch); |
| |
| memset(out, 0, frame_count * cras_get_format_bytes(out_fmt)); |
| for (i = 0; i < frame_count; i++, out += num_out_ch, in += num_in_ch) { |
| memcpy(out, in, num_copy_ch * sizeof(int16_t)); |
| } |
| |
| return frame_count; |
| } |
| |
| /* |
| * Multiplies buffer vector with coefficient vector. |
| */ |
| int16_t s16_multiply_buf_with_coef(float *coef, const int16_t *buf, size_t size) |
| { |
| int32_t sum = 0; |
| int i; |
| |
| for (i = 0; i < size; i++) |
| sum += coef[i] * buf[i]; |
| sum = MAX(sum, -0x8000); |
| sum = MIN(sum, 0x7fff); |
| return (int16_t)sum; |
| } |
| |
| /* |
| * Channel layout converter. |
| * |
| * Converts channels based on the channel conversion coefficient matrix. |
| */ |
| size_t 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) |
| { |
| unsigned i, fr; |
| unsigned in_idx = 0; |
| unsigned out_idx = 0; |
| const int16_t *in = (const int16_t *)_in; |
| int16_t *out = (int16_t *)_out; |
| |
| for (fr = 0; fr < in_frames; fr++) { |
| for (i = 0; i < num_out_ch; i++) |
| out[out_idx + i] = s16_multiply_buf_with_coef( |
| ch_conv_mtx[i], &in[in_idx], num_in_ch); |
| in_idx += num_in_ch; |
| out_idx += num_out_ch; |
| } |
| |
| return in_frames; |
| } |