void applyGainThenAdd(int16_t *data, const int16_t *in, int32_t mult)
{
uint32_t *dst = (uint32_t *)data;
const uint32_t *src = (uint32_t *)in;
const uint32_t *end = (uint32_t *)(data + AUDIO_BLOCK_SAMPLES);
if (mult == 65536) {
do {
uint32_t tmp32 = *dst;
*dst++ = signed_add_16_and_16(tmp32, *src++);
tmp32 = *dst;
*dst++ = signed_add_16_and_16(tmp32, *src++);
} while (dst < end);
} else {
do {
uint32_t tmp32 = *src++; // read 2 samples from *data
int32_t val1 = signed_multiply_32x16b(mult, tmp32);
int32_t val2 = signed_multiply_32x16t(mult, tmp32);
val1 = signed_saturate_rshift(val1, 16, 0);
val2 = signed_saturate_rshift(val2, 16, 0);
tmp32 = pack_16x16(val2, val1);
uint32_t tmp32b = *dst;
*dst++ = signed_add_16_and_16(tmp32, tmp32b);
} while (dst < end);
}
}
void AudioFilterLBCF::update(void)
{
audio_block_t *block;
int32_t b0, b1, b2, a1, a2, sum;
uint32_t in2, out2, bprev, aprev, cprev, flag;
uint32_t *data, *end;
int32_t *state;
block = receiveWritable();
if (!block) return;
end = (uint32_t *)(block->data) + AUDIO_BLOCK_SAMPLES/2;
state = (int32_t *)definition;
do {
b0 = *state++; // 1
b1 = *state++; // -d
b2 = *state++; // stage
a1 = *state++; // d
a2 = *state++; // f*(1-d)
bprev = *state++;
aprev = *state++;
sum = *state & 0x3FFF;
data = end - AUDIO_BLOCK_SAMPLES/2;
do {
in2 = *data;
circ_idx[b2]++;
if (circ_idx[b2] >= delay_length[b2])
circ_idx[b2] = 0;
cprev = outDelayline[b2][circ_idx[b2]];
sum = signed_multiply_accumulate_32x16b(sum, b0, in2);
sum = signed_multiply_accumulate_32x16t(sum, b1, bprev);
sum = signed_multiply_accumulate_32x16t(sum, a1, aprev);
sum = signed_multiply_accumulate_32x16b(sum, a2, cprev);
out2 = signed_saturate_rshift(sum, 16, 14);
sum &= 0x3FFF;
sum = signed_multiply_accumulate_32x16t(sum, b0, in2);
sum = signed_multiply_accumulate_32x16b(sum, b1, in2);
sum = signed_multiply_accumulate_32x16b(sum, a1, out2);
sum = signed_multiply_accumulate_32x16t(sum, a2, cprev);
bprev = in2;
aprev = pack_16x16(
signed_saturate_rshift(sum, 16, 14), out2);
sum &= 0x3FFF;
bprev = in2;
*data++ = aprev;
outDelayline[b2][circ_idx[b2]] = aprev;
} while (data < end);
flag = *state & 0x80000000;
*state++ = sum | flag;
*(state-2) = aprev;
*(state-3) = bprev;
} while (flag);
transmit(block);
release(block);
}
void applyGain(int16_t *data, int32_t mult)
{
uint32_t *p = (uint32_t *)data;
const uint32_t *end = (uint32_t *)(data + AUDIO_BLOCK_SAMPLES);
do {
uint32_t tmp32 = *p; // read 2 samples from *data
int32_t val1 = signed_multiply_32x16b(mult, tmp32);
int32_t val2 = signed_multiply_32x16t(mult, tmp32);
val1 = signed_saturate_rshift(val1, 16, 0);
val2 = signed_saturate_rshift(val2, 16, 0);
*p++ = pack_16x16(val2, val1);
} while (p < end);
}
