misc/echo/echo.c

106 static inline void lms_adapt_bg(struct oslec_state *ec, int clean, int shift)
122 	offset2 = ec->curr_pos;
123 	offset1 = ec->taps - offset2;
125 	for (i = ec->taps - 1; i >= offset1; i--) {
126 		exp = (ec->fir_state_bg.history[i - offset1] * factor);
127 		ec->fir_taps16[1][i] += (int16_t) ((exp + (1 << 14)) >> 15);
130 		exp = (ec->fir_state_bg.history[i + offset2] * factor);
131 		ec->fir_taps16[1][i] += (int16_t) ((exp + (1 << 14)) >> 15);
145 	struct oslec_state *ec;
149 	ec = kzalloc(sizeof(*ec), GFP_KERNEL);
150 	if (!ec)
153 	ec->taps = len;
154 	ec->log2taps = top_bit(len);
155 	ec->curr_pos = ec->taps - 1;
157 	ec->fir_taps16[0] =
158 	    kcalloc(ec->taps, sizeof(int16_t), GFP_KERNEL);
159 	if (!ec->fir_taps16[0])
162 	ec->fir_taps16[1] =
163 	    kcalloc(ec->taps, sizeof(int16_t), GFP_KERNEL);
164 	if (!ec->fir_taps16[1])
167 	history = fir16_create(&ec->fir_state, ec->fir_taps16[0], ec->taps);
170 	history = fir16_create(&ec->fir_state_bg, ec->fir_taps16[1], ec->taps);
175 		ec->xvtx[i] = ec->yvtx[i] = ec->xvrx[i] = ec->yvrx[i] = 0;
177 	ec->cng_level = 1000;
178 	oslec_adaption_mode(ec, adaption_mode);
180 	ec->snapshot = kcalloc(ec->taps, sizeof(int16_t), GFP_KERNEL);
181 	if (!ec->snapshot)
184 	ec->cond_met = 0;
185 	ec->pstates = 0;
186 	ec->ltxacc = ec->lrxacc = ec->lcleanacc = ec->lclean_bgacc = 0;
187 	ec->ltx = ec->lrx = ec->lclean = ec->lclean_bg = 0;
188 	ec->tx_1 = ec->tx_2 = ec->rx_1 = ec->rx_2 = 0;
189 	ec->lbgn = ec->lbgn_acc = 0;
190 	ec->lbgn_upper = 200;
191 	ec->lbgn_upper_acc = ec->lbgn_upper << 13;
193 	return ec;
196 	fir16_free(&ec->fir_state_bg);
198 	fir16_free(&ec->fir_state);
200 	kfree(ec->fir_taps16[1]);
202 	kfree(ec->fir_taps16[0]);
204 	kfree(ec);
209 void oslec_free(struct oslec_state *ec)
213 	fir16_free(&ec->fir_state);
214 	fir16_free(&ec->fir_state_bg);
216 		kfree(ec->fir_taps16[i]);
217 	kfree(ec->snapshot);
218 	kfree(ec);
222 void oslec_adaption_mode(struct oslec_state *ec, int adaption_mode)
224 	ec->adaption_mode = adaption_mode;
228 void oslec_flush(struct oslec_state *ec)
232 	ec->ltxacc = ec->lrxacc = ec->lcleanacc = ec->lclean_bgacc = 0;
233 	ec->ltx = ec->lrx = ec->lclean = ec->lclean_bg = 0;
234 	ec->tx_1 = ec->tx_2 = ec->rx_1 = ec->rx_2 = 0;
236 	ec->lbgn = ec->lbgn_acc = 0;
237 	ec->lbgn_upper = 200;
238 	ec->lbgn_upper_acc = ec->lbgn_upper << 13;
240 	ec->nonupdate_dwell = 0;
242 	fir16_flush(&ec->fir_state);
243 	fir16_flush(&ec->fir_state_bg);
244 	ec->fir_state.curr_pos = ec->taps - 1;
245 	ec->fir_state_bg.curr_pos = ec->taps - 1;
247 		memset(ec->fir_taps16[i], 0, ec->taps * sizeof(int16_t));
249 	ec->curr_pos = ec->taps - 1;
250 	ec->pstates = 0;
254 void oslec_snapshot(struct oslec_state *ec)
256 	memcpy(ec->snapshot, ec->fir_taps16[0], ec->taps * sizeof(int16_t));
262 int16_t oslec_update(struct oslec_state *ec, int16_t tx, int16_t rx)
275 	ec->tx = tx;
276 	ec->rx = rx;
295 	if (ec->adaption_mode & ECHO_CAN_USE_RX_HPF) {
308 		ec->rx_1 += -(ec->rx_1 >> DC_LOG2BETA) + tmp - ec->rx_2;
315 		tmp1 = ec->rx_1 >> 15;
321 		ec->rx_2 = tmp;
334 		old = (int)ec->fir_state.history[ec->fir_state.curr_pos] *
335 		    (int)ec->fir_state.history[ec->fir_state.curr_pos];
336 		ec->pstates +=
337 		    ((new - old) + (1 << (ec->log2taps - 1))) >> ec->log2taps;
338 		if (ec->pstates < 0)
339 			ec->pstates = 0;
344 	ec->ltxacc += abs(tx) - ec->ltx;
345 	ec->ltx = (ec->ltxacc + (1 << 4)) >> 5;
346 	ec->lrxacc += abs(rx) - ec->lrx;
347 	ec->lrx = (ec->lrxacc + (1 << 4)) >> 5;
351 	ec->fir_state.coeffs = ec->fir_taps16[0];
352 	echo_value = fir16(&ec->fir_state, tx);
353 	ec->clean = rx - echo_value;
354 	ec->lcleanacc += abs(ec->clean) - ec->lclean;
355 	ec->lclean = (ec->lcleanacc + (1 << 4)) >> 5;
359 	echo_value = fir16(&ec->fir_state_bg, tx);
361 	ec->lclean_bgacc += abs(clean_bg) - ec->lclean_bg;
362 	ec->lclean_bg = (ec->lclean_bgacc + (1 << 4)) >> 5;
370 	ec->factor = 0;
371 	ec->shift = 0;
372 	if (!ec->nonupdate_dwell) {
408 		p = MIN_TX_POWER_FOR_ADAPTION + ec->pstates;
409 		logp = top_bit(p) + ec->log2taps;
411 		ec->shift = shift;
413 		lms_adapt_bg(ec, clean_bg, shift);
419 	ec->adapt = 0;
420 	if ((ec->lrx > MIN_RX_POWER_FOR_ADAPTION) && (ec->lrx > ec->ltx))
421 		ec->nonupdate_dwell = DTD_HANGOVER;
422 	if (ec->nonupdate_dwell)
423 		ec->nonupdate_dwell--;
430 	if ((ec->adaption_mode & ECHO_CAN_USE_ADAPTION) &&
431 	    (ec->nonupdate_dwell == 0) &&
432 	    /* (ec->Lclean_bg < 0.875*ec->Lclean) */
433 	    (8 * ec->lclean_bg < 7 * ec->lclean) &&
434 	    /* (ec->Lclean_bg < 0.125*ec->Ltx) */
435 	    (8 * ec->lclean_bg < ec->ltx)) {
436 		if (ec->cond_met == 6) {
441 			ec->adapt = 1;
442 			memcpy(ec->fir_taps16[0], ec->fir_taps16[1],
443 			       ec->taps * sizeof(int16_t));
445 			ec->cond_met++;
447 		ec->cond_met = 0;
451 	ec->clean_nlp = ec->clean;
452 	if (ec->adaption_mode & ECHO_CAN_USE_NLP) {
459 		if ((16 * ec->lclean < ec->ltx)) {
465 			if (ec->adaption_mode & ECHO_CAN_USE_CNG) {
466 				ec->cng_level = ec->lbgn;
479 				ec->cng_rndnum =
480 				    1664525U * ec->cng_rndnum + 1013904223U;
481 				ec->cng_filter =
482 				    ((ec->cng_rndnum & 0xFFFF) - 32768 +
483 				     5 * ec->cng_filter) >> 3;
484 				ec->clean_nlp =
485 				    (ec->cng_filter * ec->cng_level * 8) >> 14;
487 			} else if (ec->adaption_mode & ECHO_CAN_USE_CLIP) {
489 				if (ec->clean_nlp > ec->lbgn)
490 					ec->clean_nlp = ec->lbgn;
491 				if (ec->clean_nlp < -ec->lbgn)
492 					ec->clean_nlp = -ec->lbgn;
498 				ec->clean_nlp = 0;
511 			if (ec->lclean < 40) {
512 				ec->lbgn_acc += abs(ec->clean) - ec->lbgn;
513 				ec->lbgn = (ec->lbgn_acc + (1 << 11)) >> 12;
519 	if (ec->curr_pos <= 0)
520 		ec->curr_pos = ec->taps;
521 	ec->curr_pos--;
523 	if (ec->adaption_mode & ECHO_CAN_DISABLE)
524 		ec->clean_nlp = rx;
528 	return (int16_t) ec->clean_nlp << 1;
554 int16_t oslec_hpf_tx(struct oslec_state *ec, int16_t tx)
559 	if (ec->adaption_mode & ECHO_CAN_USE_TX_HPF) {
572 		ec->tx_1 += -(ec->tx_1 >> DC_LOG2BETA) + tmp - ec->tx_2;
573 		tmp1 = ec->tx_1 >> 15;
579 		ec->tx_2 = tmp;