void pa_sample_clamp(pa_sample_format_t format, void *dst, size_t dstr, const void *src, size_t sstr, unsigned n) {
const float *s;
float *d;
s = src; d = dst;
if (format == PA_SAMPLE_FLOAT32NE) {
for (; n > 0; n--) {
float f;
f = *s;
*d = PA_CLAMP_UNLIKELY(f, -1.0f, 1.0f);
s = (const float*) ((const uint8_t*) s + sstr);
d = (float*) ((uint8_t*) d + dstr);
}
} else {
pa_assert(format == PA_SAMPLE_FLOAT32RE);
for (; n > 0; n--) {
float f;
f = PA_FLOAT32_SWAP(*s);
f = PA_CLAMP_UNLIKELY(f, -1.0f, 1.0f);
*d = PA_FLOAT32_SWAP(f);
s = (const float*) ((const uint8_t*) s + sstr);
d = (float*) ((uint8_t*) d + dstr);
}
}
}
static void pa_mix_s32ne_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, int32_t *data, unsigned length) {
unsigned channel = 0;
length /= sizeof(int32_t);
for (; length > 0; length--, data++) {
int64_t sum = 0;
unsigned i;
for (i = 0; i < nstreams; i++) {
pa_mix_info *m = streams + i;
int32_t cv = m->linear[channel].i;
int64_t v;
if (PA_LIKELY(cv > 0)) {
v = *((int32_t*) m->ptr);
v = (v * cv) >> 16;
sum += v;
}
m->ptr = (uint8_t*) m->ptr + sizeof(int32_t);
}
sum = PA_CLAMP_UNLIKELY(sum, -0x80000000LL, 0x7FFFFFFFLL);
*data = (int32_t) sum;
if (PA_UNLIKELY(++channel >= channels))
channel = 0;
}
static void pa_mix_s16re_c(pa_mix_info streams[], unsigned nstreams, unsigned channels, int16_t *data, unsigned length) {
unsigned channel = 0;
length /= sizeof(int16_t);
for (; length > 0; length--, data++) {
int32_t sum = 0;
unsigned i;
for (i = 0; i < nstreams; i++) {
pa_mix_info *m = streams + i;
int32_t cv = m->linear[channel].i;
if (PA_LIKELY(cv > 0))
sum += pa_mult_s16_volume(PA_INT16_SWAP(*((int16_t*) m->ptr)), cv);
m->ptr = (uint8_t*) m->ptr + sizeof(int16_t);
}
sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
*data = PA_INT16_SWAP((int16_t) sum);
if (PA_UNLIKELY(++channel >= channels))
channel = 0;
}
}
/* special case: mix 2 s16ne streams, 2 channels each */
static void pa_mix2_ch2_s16ne(pa_mix_info streams[], int16_t *data, unsigned length) {
const int16_t *ptr0 = streams[0].ptr;
const int16_t *ptr1 = streams[1].ptr;
length /= sizeof(int16_t) * 2;
for (; length > 0; length--) {
int32_t sum;
sum = pa_mult_s16_volume(*ptr0++, streams[0].linear[0].i);
sum += pa_mult_s16_volume(*ptr1++, streams[1].linear[0].i);
sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
*data++ = sum;
sum = pa_mult_s16_volume(*ptr0++, streams[0].linear[1].i);
sum += pa_mult_s16_volume(*ptr1++, streams[1].linear[1].i);
sum = PA_CLAMP_UNLIKELY(sum, -0x8000, 0x7FFF);
*data++ = sum;
}
}
/* special case: mix s16ne streams, 2 channels each */
static void pa_mix_ch2_s16ne(pa_mix_info streams[], unsigned nstreams, int16_t *data, unsigned length) {
length /= sizeof(int16_t) * 2;
for (; length > 0; length--) {
int32_t sum0 = 0, sum1 = 0;
unsigned i;
for (i = 0; i < nstreams; i++) {
pa_mix_info *m = streams + i;
int32_t cv0 = m->linear[0].i;
int32_t cv1 = m->linear[1].i;
sum0 += pa_mult_s16_volume(*((int16_t*) m->ptr), cv0);
m->ptr = (uint8_t*) m->ptr + sizeof(int16_t);
sum1 += pa_mult_s16_volume(*((int16_t*) m->ptr), cv1);
m->ptr = (uint8_t*) m->ptr + sizeof(int16_t);
}
*data++ = PA_CLAMP_UNLIKELY(sum0, -0x8000, 0x7FFF);
*data++ = PA_CLAMP_UNLIKELY(sum1, -0x8000, 0x7FFF);
}
}
/* Called from I/O thread context */
static int sink_input_pop_cb(pa_sink_input *i, size_t nbytes, pa_memchunk *chunk) {
struct userdata *u;
float *src, *dst;
unsigned n;
pa_memchunk tchunk;
unsigned j, k, l;
float sum_right, sum_left;
float current_sample;
pa_sink_input_assert_ref(i);
pa_assert(chunk);
pa_assert_se(u = i->userdata);
/* Hmm, process any rewind request that might be queued up */
pa_sink_process_rewind(u->sink, 0);
while (pa_memblockq_peek(u->memblockq, &tchunk) < 0) {
pa_memchunk nchunk;
pa_sink_render(u->sink, nbytes * u->sink_fs / u->fs, &nchunk);
pa_memblockq_push(u->memblockq, &nchunk);
pa_memblock_unref(nchunk.memblock);
}
tchunk.length = PA_MIN(nbytes * u->sink_fs / u->fs, tchunk.length);
pa_assert(tchunk.length > 0);
n = (unsigned) (tchunk.length / u->sink_fs);
pa_assert(n > 0);
chunk->index = 0;
chunk->length = n * u->fs;
chunk->memblock = pa_memblock_new(i->sink->core->mempool, chunk->length);
pa_memblockq_drop(u->memblockq, n * u->sink_fs);
src = pa_memblock_acquire_chunk(&tchunk);
dst = pa_memblock_acquire(chunk->memblock);
for (l = 0; l < n; l++) {
memcpy(((char*) u->input_buffer) + u->input_buffer_offset * u->sink_fs, ((char *) src) + l * u->sink_fs, u->sink_fs);
sum_right = 0;
sum_left = 0;
/* fold the input buffer with the impulse response */
for (j = 0; j < u->hrir_samples; j++) {
for (k = 0; k < u->channels; k++) {
current_sample = u->input_buffer[((u->input_buffer_offset + j) % u->hrir_samples) * u->channels + k];
sum_left += current_sample * u->hrir_data[j * u->hrir_channels + u->mapping_left[k]];
sum_right += current_sample * u->hrir_data[j * u->hrir_channels + u->mapping_right[k]];
}
}
dst[2 * l] = PA_CLAMP_UNLIKELY(sum_left, -1.0f, 1.0f);
dst[2 * l + 1] = PA_CLAMP_UNLIKELY(sum_right, -1.0f, 1.0f);
u->input_buffer_offset--;
if (u->input_buffer_offset < 0)
u->input_buffer_offset += u->hrir_samples;
}
pa_memblock_release(tchunk.memblock);
pa_memblock_release(chunk->memblock);
pa_memblock_unref(tchunk.memblock);
return 0;
}
