blob: a306d216925aabf7143dd1c6144a94ecba038ac0 [file] [log] [blame]
/* 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: 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;
}