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);
}
void AudioInputAnalogStereo::update(void)
{
audio_block_t *new_left=NULL, *out_left=NULL;
audio_block_t *new_right=NULL, *out_right=NULL;
uint32_t i, dc;
int32_t tmp;
int16_t s, *p, *end;
//Serial.println("update");
// allocate new block (ok if both NULL)
new_left = allocate();
if (new_left == NULL) {
new_right = NULL;
} else {
new_right = allocate();
if (new_right == NULL) {
release(new_left);
new_left = NULL;
}
}
__disable_irq();
if (offset_left < AUDIO_BLOCK_SAMPLES || offset_right < AUDIO_BLOCK_SAMPLES) {
// the DMA hasn't filled up both blocks
if (block_left == NULL) {
block_left = new_left;
offset_left = 0;
new_left = NULL;
}
if (block_right == NULL) {
block_right = new_right;
offset_right = 0;
new_right = NULL;
}
__enable_irq();
if (new_left) release(new_left);
if (new_right) release(new_right);
return;
}
// the DMA filled blocks, so grab them and get the
// new blocks to the DMA, as quickly as possible
out_left = block_left;
out_right = block_right;
block_left = new_left;
block_right = new_right;
offset_left = 0;
offset_right = 0;
__enable_irq();
// Find and subtract DC offset... We use an average of the
// last 16 * AUDIO_BLOCK_SAMPLES samples.
dc = 0;
for (i = 0; i < 16; i++) {
dc += left_dc_average_hist[i];
}
dc /= 16 * AUDIO_BLOCK_SAMPLES;
left_dc_average_hist[current_dc_average_index] = 0;
p = out_left->data;
end = p + AUDIO_BLOCK_SAMPLES;
do {
left_dc_average_hist[current_dc_average_index] += (uint16_t)(*p);
tmp = (uint16_t)(*p) - (int32_t)dc;
s = signed_saturate_rshift(tmp, 16, 0);
*p++ = s;
} while (p < end);
dc = 0;
for (i = 0; i < 16; i++) {
dc += right_dc_average_hist[i];
}
dc /= 16 * AUDIO_BLOCK_SAMPLES;
right_dc_average_hist[current_dc_average_index] = 0;
p = out_right->data;
end = p + AUDIO_BLOCK_SAMPLES;
do {
right_dc_average_hist[current_dc_average_index] += (uint16_t)(*p);
tmp = (uint16_t)(*p) - (int32_t)dc;
s = signed_saturate_rshift(tmp, 16, 0);
*p++ = s;
} while (p < end);
current_dc_average_index = (current_dc_average_index + 1) % 16;
// then transmit the AC data
transmit(out_left, 0);
release(out_left);
transmit(out_right, 1);
release(out_right);
}
void AudioInputAnalogStereo::update(void)
{
audio_block_t *new_left=NULL, *out_left=NULL;
audio_block_t *new_right=NULL, *out_right=NULL;
int32_t tmp;
int16_t s, *p, *end;
//Serial.println("update");
// allocate new block (ok if both NULL)
new_left = allocate();
if (new_left == NULL) {
new_right = NULL;
} else {
new_right = allocate();
if (new_right == NULL) {
release(new_left);
new_left = NULL;
}
}
__disable_irq();
if (offset_left < AUDIO_BLOCK_SAMPLES || offset_right < AUDIO_BLOCK_SAMPLES) {
// the DMA hasn't filled up both blocks
if (block_left == NULL) {
block_left = new_left;
offset_left = 0;
new_left = NULL;
}
if (block_right == NULL) {
block_right = new_right;
offset_right = 0;
new_right = NULL;
}
__enable_irq();
if (new_left) release(new_left);
if (new_right) release(new_right);
return;
}
// the DMA filled blocks, so grab them and get the
// new blocks to the DMA, as quickly as possible
out_left = block_left;
out_right = block_right;
block_left = new_left;
block_right = new_right;
offset_left = 0;
offset_right = 0;
__enable_irq();
//
// DC Offset Removal Filter
// 1-pole digital high-pass filter implementation
// y = a*(x[n] - x[n-1] + y[n-1])
// The coefficient "a" is as follows:
// a = UNITY*e^(-2*pi*fc/fs)
// fc = 2 @ fs = 44100
//
// DC removal, LEFT
p = out_left->data;
end = p + AUDIO_BLOCK_SAMPLES;
do {
tmp = (uint16_t)(*p);
tmp = ( ((int32_t) tmp) << 14);
int32_t acc = hpf_y1[0] - hpf_x1[0];
acc += tmp;
hpf_y1[0] = FRACMUL_SHL(acc, COEF_HPF_DCBLOCK, 1);
hpf_x1[0] = tmp;
s = signed_saturate_rshift(hpf_y1[0], 16, 14);
*p++ = s;
} while (p < end);
// DC removal, RIGHT
p = out_right->data;
end = p + AUDIO_BLOCK_SAMPLES;
do {
tmp = (uint16_t)(*p);
tmp = ( ((int32_t) tmp) << 14);
int32_t acc = hpf_y1[1] - hpf_x1[1];
acc += tmp;
hpf_y1[1]= FRACMUL_SHL(acc, COEF_HPF_DCBLOCK, 1);
hpf_x1[1] = tmp;
s = signed_saturate_rshift(hpf_y1[1], 16, 14);
*p++ = s;
} while (p < end);
// then transmit the AC data
transmit(out_left, 0);
release(out_left);
transmit(out_right, 1);
release(out_right);
}
