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); } }