00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include "wvtelephony.h"
00012 #include <stdio.h>
00013
00014 #define SQ(x) ((x)*(x))
00015 #define ROUND_DIV(a,b) (((a) + ((b)/2))/(b))
00016
00106 void echo_cancel(const short *out_buf,
00107 const short *in,
00108 size_t block_size,
00109 int *filter_q16,
00110 size_t num_taps,
00111 short *echo_cancelled_in)
00112 {
00113 const short *cur_out = &out_buf[block_size-1];
00114 const short *cur_in = &in[block_size-1];
00115 short *cur_in_ec = &echo_cancelled_in[block_size-1];
00116
00117 const short *out_p;
00118 int out_norm_sq_p16;
00119
00120
00121
00122 out_norm_sq_p16 = 0;
00123 for (out_p = &out_buf[block_size+num_taps-1];
00124 out_p != &out_buf[block_size-1];
00125 --out_p)
00126 {
00127 out_norm_sq_p16 += SQ(ROUND_DIV((int) *out_p, 1<<8));
00128 }
00129
00130 do
00131 {
00132 int *filter_q16_p;
00133 int echo_est_q16;
00134 short echo_est;
00135
00136
00137
00138 echo_est_q16 = 0;
00139 for (out_p = &cur_out[num_taps-1],
00140 filter_q16_p = &filter_q16[num_taps-1];
00141 out_p != cur_out;
00142 --out_p, --filter_q16_p)
00143 {
00144 echo_est_q16 += (int) *out_p * *filter_q16_p;
00145 }
00146 echo_est = ROUND_DIV(echo_est_q16, 1<<16);
00147
00148
00149
00150
00151
00152
00153 *cur_in_ec = *cur_in - echo_est;
00154
00155
00156
00157 out_norm_sq_p16 += SQ(ROUND_DIV((int) cur_out[0], 1<<8))
00158 - SQ(ROUND_DIV((int) cur_out[num_taps], 1<<8));
00159
00160
00161
00162 if (out_norm_sq_p16 != 0)
00163 {
00164 for (out_p = &cur_out[num_taps-1],
00165 filter_q16_p = &filter_q16[num_taps-1];
00166 out_p != cur_out;
00167 --out_p, --filter_q16_p)
00168 {
00169 *filter_q16_p +=
00170 ROUND_DIV((int) *cur_in_ec * (int) *out_p, out_norm_sq_p16);
00171 }
00172 }
00173 } while (cur_out--, cur_in_ec--, cur_in-- != in);
00174 }
00175