int16_t awgn(awgn_state_t *s)
{
double fac;
double r;
double v1;
double v2;
double amp;
if (s->iset == 0)
{
do
{
v1 = 2.0*ran1(s) - 1.0;
v2 = 2.0*ran1(s) - 1.0;
r = v1*v1 + v2*v2;
}
while (r >= 1.0);
fac = sqrt(-2.0*log(r)/r);
s->gset = v1*fac;
s->iset = 1;
amp = v2*fac*s->rms;
}
else
{
s->iset = 0;
amp = s->gset*s->rms;
}
return fsaturate(amp);
}
int plc_rx(plc_state_t *s, int16_t amp[], int len)
{
int i;
int pitch_overlap;
float old_step;
float new_step;
float old_weight;
float new_weight;
float gain;
if (s->missing_samples)
{
/* Although we have a real signal, we need to smooth it to fit well
with the synthetic signal we used for the previous block */
/* The start of the real data is overlapped with the next 1/4 cycle
of the synthetic data. */
pitch_overlap = s->pitch >> 2;
if (pitch_overlap > len)
pitch_overlap = len;
gain = 1.0 - s->missing_samples*ATTENUATION_INCREMENT;
if (gain < 0.0)
gain = 0.0;
new_step = 1.0/pitch_overlap;
old_step = new_step*gain;
new_weight = new_step;
old_weight = (1.0 - new_step)*gain;
for (i = 0; i < pitch_overlap; i++)
{
amp[i] = fsaturate(old_weight*s->pitchbuf[s->pitch_offset] + new_weight*amp[i]);
if (++s->pitch_offset >= s->pitch)
s->pitch_offset = 0;
new_weight += new_step;
old_weight -= old_step;
if (old_weight < 0.0)
old_weight = 0.0;
}
s->missing_samples = 0;
}
int main(int argc, char *argv[])
{
printf("Testing 16 bit saturation\n");
if (saturate16(10000) != 10000
||
saturate16(-10000) != -10000
||
saturate16(32767) != 32767
||
saturate16(-32768) != -32768
||
saturate16(32768) != 32767
||
saturate16(-32769) != -32768)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 15 bit saturation\n");
if (saturate15(10000) != 10000
||
saturate15(-10000) != -10000
||
saturate15(16383) != 16383
||
saturate15(-16384) != -16384
||
saturate15(16384) != 16383
||
saturate15(-16385) != -16384)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 bit unsigned saturation\n");
if (saturateu16(10000) != 10000
||
saturateu16(32767) != 32767
||
saturateu16(65535) != 65535
||
saturateu16(65536) != 65535)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 8 bit unsigned saturation\n");
if (saturateu8(100) != 100
||
saturateu8(127) != 127
||
saturateu8(255) != 255
||
saturateu8(256) != 255)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 bit saturation from float\n");
if (fsaturatef(10000.0f) != 10000
||
fsaturatef(-10000.0f) != -10000
||
fsaturatef(32767.0f) != 32767
||
fsaturatef(-32768.0f) != -32768
||
fsaturatef(32768.0f) != 32767
||
fsaturatef(-32769.0f) != -32768)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 bit saturation from double\n");
if (fsaturate(10000.0) != 10000
||
fsaturate(-10000.0) != -10000
||
fsaturate(32767.0) != 32767
||
fsaturate(-32768.0) != -32768
||
fsaturate(32768.0) != 32767
||
fsaturate(-32769.0) != -32768)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 bit fast saturation from float\n");
if (ffastsaturatef(10000.0f) != 10000
||
ffastsaturatef(-10000.0f) != -10000
||
ffastsaturatef(32767.0f) != 32767
||
ffastsaturatef(-32768.0f) != -32768
||
ffastsaturatef(32768.0f) != 32767
||
ffastsaturatef(-32769.0f) != -32768)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 bit fast saturation from double\n");
if (ffastsaturate(10000.0) != 10000
||
ffastsaturate(-10000.0) != -10000
||
ffastsaturate(32767.0) != 32767
||
ffastsaturate(-32768.0) != -32768
||
ffastsaturate(32768.0) != 32767
||
ffastsaturate(-32769.0) != -32768)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 bit float saturation from float\n");
if (ffsaturatef(10000.0f) != 10000.0f
||
ffsaturatef(-10000.0f) != -10000.0f
||
ffsaturatef(32767.0f) != 32767.0f
||
ffsaturatef(-32768.0f) != -32768.0f
||
ffsaturatef(32768.0f) != 32767.0f
||
ffsaturatef(-32769.0f) != -32768.0f)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 bit double saturation from double\n");
if (ffsaturate(10000.0) != 10000.0
||
ffsaturate(-10000.0) != -10000.0
||
ffsaturate(32767.0) != 32767.0
||
ffsaturate(-32768.0) != -32768.0
||
ffsaturate(32768.0) != 32767.0
||
ffsaturate(-32769.0) != -32768.0)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 bit add\n");
if (saturated_add16(10000, 10000) != 20000
||
saturated_add16(10000, -10000) != 0
||
saturated_add16(-10000, 10000) != 0
||
saturated_add16(-10000, -10000) != -20000
||
saturated_add16(-30000, -30000) != INT16_MIN
||
saturated_add16(30000, 30000) != INT16_MAX)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 32 bit add\n");
if (saturated_add32(10000, 10000) != 20000
||
saturated_add32(10000, -10000) != 0
||
saturated_add32(-10000, 10000) != 0
||
saturated_add32(-10000, -10000) != -20000
||
saturated_add32(-2000000000, -2000000000) != INT32_MIN
||
saturated_add32(2000000000, 2000000000) != INT32_MAX)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 bit subtract\n");
if (saturated_sub16(10000, 10000) != 0
||
saturated_sub16(10000, -10000) != 20000
||
saturated_sub16(-10000, 10000) != -20000
||
saturated_sub16(-10000, -10000) != 0
||
saturated_sub16(-30000, 30000) != INT16_MIN
||
saturated_sub16(30000, -30000) != INT16_MAX)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 32 bit subtract\n");
if (saturated_sub32(10000, 10000) != 0
||
saturated_sub32(10000, -10000) != 20000
||
saturated_sub32(-10000, 10000) != -20000
||
saturated_sub32(-10000, -10000) != 0
||
saturated_sub32(-2000000000, 2000000000) != INT32_MIN
||
saturated_sub32(2000000000, -2000000000) != INT32_MAX)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 x 16 => 16 bit multiply\n");
if (saturated_mul16(100, 100) != 0
||
saturated_mul16(255, 255) != 1
||
saturated_mul16(32767, -32768) != -32767
||
saturated_mul16(-32768, 32767) != -32767
||
saturated_mul16(32767, 32767) != 32766
||
saturated_mul16(-32768, -32768) != 32767)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 x 16 => 32 bit multiply\n");
if (saturated_mul16_32(100, 100) != 20000
||
saturated_mul16_32(-100, 100) != -20000
||
saturated_mul16_32(32767, -32768) != -2147418112
||
saturated_mul16_32(-32768, 32767) != -2147418112
||
saturated_mul16_32(32767, 32767) != 2147352578
||
saturated_mul16_32(-32768, -32768) != -2147483648)
{
printf("Test failed.\n");
exit(2);
}
printf("Testing 16 bit absolute\n");
if (saturated_abs16(10000) != 10000
||
saturated_abs16(-10000) != 10000
||
saturated_abs16(32767) != 32767
||
saturated_abs16(-32768) != 32767)
{
printf("Test failed.\n");
exit(2);
}
printf("Tests passed.\n");
return 0;
}
void both_ways_line_model(both_ways_line_model_state_t *s,
int16_t output1[],
const int16_t input1[],
int16_t output2[],
const int16_t input2[],
int samples)
{
int i;
float in1;
float in2;
float out1;
float out2;
float tmp1;
float tmp2;
int16_t amp[1];
/* The path being modelled is:
terminal
| < hybrid echo
|
| < noise and filtering
|
| < hybrid echo
CO
|
| < A-law distortion + bulk delay
|
CO
| < hybrid echo
|
| < noise and filtering
|
| < hybrid echo
terminal
*/
for (i = 0; i < samples; i++)
{
in1 = input1[i];
in2 = input2[i];
/* Near end analogue sections */
/* Echo from each terminal's CO hybrid */
tmp1 = in1 + s->fout2*s->line1.near_co_hybrid_echo;
tmp2 = in2 + s->fout1*s->line2.near_co_hybrid_echo;
/* Line model filters & noise */
s->fout1 = calc_near_line_filter(&s->line1, tmp1);
s->fout2 = calc_near_line_filter(&s->line2, tmp2);
/* Long distance digital section */
/* Introduce distortion due to A-law or u-law munging. */
amp[0] = s->fout1;
codec_munge(s->line1.munge, amp, 1);
s->fout1 = amp[0];
amp[0] = s->fout2;
codec_munge(s->line2.munge, amp, 1);
s->fout2 = amp[0];
/* Introduce the bulk delay of the long distance digital link. */
out1 = s->line1.bulk_delay_buf[s->line1.bulk_delay_ptr];
s->line1.bulk_delay_buf[s->line1.bulk_delay_ptr] = s->fout1;
s->fout1 = out1;
if (++s->line1.bulk_delay_ptr >= s->line1.bulk_delay)
s->line1.bulk_delay_ptr = 0;
out2 = s->line2.bulk_delay_buf[s->line2.bulk_delay_ptr];
s->line2.bulk_delay_buf[s->line2.bulk_delay_ptr] = s->fout2;
s->fout2 = out2;
if (++s->line2.bulk_delay_ptr >= s->line2.bulk_delay)
s->line2.bulk_delay_ptr = 0;
/* Far end analogue sections */
/* Echo from each terminal's own hybrid */
out1 += in2*s->line1.far_cpe_hybrid_echo;
out2 += in1*s->line2.far_cpe_hybrid_echo;
/* Line model filters & noise */
out1 = calc_far_line_filter(&s->line1, out1);
out2 = calc_far_line_filter(&s->line2, out2);
output1[i] = fsaturate(out1 + s->line1.dc_offset);
output2[i] = fsaturate(out2 + s->line2.dc_offset);
}
}
