Merge branch 'for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/drzeus/mmc
[linux-2.6] / drivers / staging / echo / echo.c
1 /*
2  * SpanDSP - a series of DSP components for telephony
3  *
4  * echo.c - A line echo canceller.  This code is being developed
5  *          against and partially complies with G168.
6  *
7  * Written by Steve Underwood <steveu@coppice.org>
8  *         and David Rowe <david_at_rowetel_dot_com>
9  *
10  * Copyright (C) 2001, 2003 Steve Underwood, 2007 David Rowe
11  *
12  * Based on a bit from here, a bit from there, eye of toad, ear of
13  * bat, 15 years of failed attempts by David and a few fried brain
14  * cells.
15  *
16  * All rights reserved.
17  *
18  * This program is free software; you can redistribute it and/or modify
19  * it under the terms of the GNU General Public License version 2, as
20  * published by the Free Software Foundation.
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
30  *
31  * $Id: echo.c,v 1.20 2006/12/01 18:00:48 steveu Exp $
32  */
33
34 /*! \file */
35
36 /* Implementation Notes
37    David Rowe
38    April 2007
39
40    This code started life as Steve's NLMS algorithm with a tap
41    rotation algorithm to handle divergence during double talk.  I
42    added a Geigel Double Talk Detector (DTD) [2] and performed some
43    G168 tests.  However I had trouble meeting the G168 requirements,
44    especially for double talk - there were always cases where my DTD
45    failed, for example where near end speech was under the 6dB
46    threshold required for declaring double talk.
47
48    So I tried a two path algorithm [1], which has so far given better
49    results.  The original tap rotation/Geigel algorithm is available
50    in SVN http://svn.rowetel.com/software/oslec/tags/before_16bit.
51    It's probably possible to make it work if some one wants to put some
52    serious work into it.
53
54    At present no special treatment is provided for tones, which
55    generally cause NLMS algorithms to diverge.  Initial runs of a
56    subset of the G168 tests for tones (e.g ./echo_test 6) show the
57    current algorithm is passing OK, which is kind of surprising.  The
58    full set of tests needs to be performed to confirm this result.
59
60    One other interesting change is that I have managed to get the NLMS
61    code to work with 16 bit coefficients, rather than the original 32
62    bit coefficents.  This reduces the MIPs and storage required.
63    I evaulated the 16 bit port using g168_tests.sh and listening tests
64    on 4 real-world samples.
65
66    I also attempted the implementation of a block based NLMS update
67    [2] but although this passes g168_tests.sh it didn't converge well
68    on the real-world samples.  I have no idea why, perhaps a scaling
69    problem.  The block based code is also available in SVN
70    http://svn.rowetel.com/software/oslec/tags/before_16bit.  If this
71    code can be debugged, it will lead to further reduction in MIPS, as
72    the block update code maps nicely onto DSP instruction sets (it's a
73    dot product) compared to the current sample-by-sample update.
74
75    Steve also has some nice notes on echo cancellers in echo.h
76
77
78    References:
79
80    [1] Ochiai, Areseki, and Ogihara, "Echo Canceller with Two Echo
81        Path Models", IEEE Transactions on communications, COM-25,
82        No. 6, June
83        1977.
84        http://www.rowetel.com/images/echo/dual_path_paper.pdf
85
86    [2] The classic, very useful paper that tells you how to
87        actually build a real world echo canceller:
88          Messerschmitt, Hedberg, Cole, Haoui, Winship, "Digital Voice
89          Echo Canceller with a TMS320020,
90          http://www.rowetel.com/images/echo/spra129.pdf
91
92    [3] I have written a series of blog posts on this work, here is
93        Part 1: http://www.rowetel.com/blog/?p=18
94
95    [4] The source code http://svn.rowetel.com/software/oslec/
96
97    [5] A nice reference on LMS filters:
98          http://en.wikipedia.org/wiki/Least_mean_squares_filter
99
100    Credits:
101
102    Thanks to Steve Underwood, Jean-Marc Valin, and Ramakrishnan
103    Muthukrishnan for their suggestions and email discussions.  Thanks
104    also to those people who collected echo samples for me such as
105    Mark, Pawel, and Pavel.
106 */
107
108 #include <linux/kernel.h>       /* We're doing kernel work */
109 #include <linux/module.h>
110 #include <linux/kernel.h>
111 #include <linux/slab.h>
112 #define malloc(a) kmalloc((a), GFP_KERNEL)
113 #define free(a) kfree(a)
114
115 #include "bit_operations.h"
116 #include "echo.h"
117
118 #define MIN_TX_POWER_FOR_ADAPTION   64
119 #define MIN_RX_POWER_FOR_ADAPTION   64
120 #define DTD_HANGOVER               600     /* 600 samples, or 75ms     */
121 #define DC_LOG2BETA                  3     /* log2() of DC filter Beta */
122
123 /*-----------------------------------------------------------------------*\
124                                FUNCTIONS
125 \*-----------------------------------------------------------------------*/
126
127 /* adapting coeffs using the traditional stochastic descent (N)LMS algorithm */
128
129
130 #ifdef __BLACKFIN_ASM__
131 static void __inline__ lms_adapt_bg(echo_can_state_t *ec, int clean, int shift)
132 {
133     int i, j;
134     int offset1;
135     int offset2;
136     int factor;
137     int exp;
138     int16_t *phist;
139     int n;
140
141     if (shift > 0)
142         factor = clean << shift;
143     else
144         factor = clean >> -shift;
145
146     /* Update the FIR taps */
147
148     offset2 = ec->curr_pos;
149     offset1 = ec->taps - offset2;
150     phist = &ec->fir_state_bg.history[offset2];
151
152     /* st: and en: help us locate the assembler in echo.s */
153
154     //asm("st:");
155     n = ec->taps;
156     for (i = 0, j = offset2;  i < n;  i++, j++)
157     {
158        exp = *phist++ * factor;
159        ec->fir_taps16[1][i] += (int16_t) ((exp+(1<<14)) >> 15);
160     }
161     //asm("en:");
162
163     /* Note the asm for the inner loop above generated by Blackfin gcc
164        4.1.1 is pretty good (note even parallel instructions used):
165
166         R0 = W [P0++] (X);
167         R0 *= R2;
168         R0 = R0 + R3 (NS) ||
169         R1 = W [P1] (X) ||
170         nop;
171         R0 >>>= 15;
172         R0 = R0 + R1;
173         W [P1++] = R0;
174
175         A block based update algorithm would be much faster but the
176         above can't be improved on much.  Every instruction saved in
177         the loop above is 2 MIPs/ch!  The for loop above is where the
178         Blackfin spends most of it's time - about 17 MIPs/ch measured
179         with speedtest.c with 256 taps (32ms).  Write-back and
180         Write-through cache gave about the same performance.
181     */
182 }
183
184 /*
185    IDEAS for further optimisation of lms_adapt_bg():
186
187    1/ The rounding is quite costly.  Could we keep as 32 bit coeffs
188    then make filter pluck the MS 16-bits of the coeffs when filtering?
189    However this would lower potential optimisation of filter, as I
190    think the dual-MAC architecture requires packed 16 bit coeffs.
191
192    2/ Block based update would be more efficient, as per comments above,
193    could use dual MAC architecture.
194
195    3/ Look for same sample Blackfin LMS code, see if we can get dual-MAC
196    packing.
197
198    4/ Execute the whole e/c in a block of say 20ms rather than sample
199    by sample.  Processing a few samples every ms is inefficient.
200 */
201
202 #else
203 static __inline__ void lms_adapt_bg(echo_can_state_t *ec, int clean, int shift)
204 {
205     int i;
206
207     int offset1;
208     int offset2;
209     int factor;
210     int exp;
211
212     if (shift > 0)
213         factor = clean << shift;
214     else
215         factor = clean >> -shift;
216
217     /* Update the FIR taps */
218
219     offset2 = ec->curr_pos;
220     offset1 = ec->taps - offset2;
221
222     for (i = ec->taps - 1;  i >= offset1;  i--)
223     {
224        exp = (ec->fir_state_bg.history[i - offset1]*factor);
225        ec->fir_taps16[1][i] += (int16_t) ((exp+(1<<14)) >> 15);
226     }
227     for (  ;  i >= 0;  i--)
228     {
229        exp = (ec->fir_state_bg.history[i + offset2]*factor);
230        ec->fir_taps16[1][i] += (int16_t) ((exp+(1<<14)) >> 15);
231     }
232 }
233 #endif
234
235 /*- End of function --------------------------------------------------------*/
236
237 echo_can_state_t *echo_can_create(int len, int adaption_mode)
238 {
239     echo_can_state_t *ec;
240     int i;
241     int j;
242
243     ec = kmalloc(sizeof(*ec), GFP_KERNEL);
244     if (ec == NULL)
245         return  NULL;
246     memset(ec, 0, sizeof(*ec));
247
248     ec->taps = len;
249     ec->log2taps = top_bit(len);
250     ec->curr_pos = ec->taps - 1;
251
252     for (i = 0;  i < 2;  i++)
253     {
254         if ((ec->fir_taps16[i] = (int16_t *) malloc((ec->taps)*sizeof(int16_t))) == NULL)
255         {
256             for (j = 0;  j < i;  j++)
257                 kfree(ec->fir_taps16[j]);
258             kfree(ec);
259             return  NULL;
260         }
261         memset(ec->fir_taps16[i], 0, (ec->taps)*sizeof(int16_t));
262     }
263
264     fir16_create(&ec->fir_state,
265                  ec->fir_taps16[0],
266                  ec->taps);
267     fir16_create(&ec->fir_state_bg,
268                  ec->fir_taps16[1],
269                  ec->taps);
270
271     for(i=0; i<5; i++) {
272       ec->xvtx[i] = ec->yvtx[i] = ec->xvrx[i] = ec->yvrx[i] = 0;
273     }
274
275     ec->cng_level = 1000;
276     echo_can_adaption_mode(ec, adaption_mode);
277
278     ec->snapshot = (int16_t*)malloc(ec->taps*sizeof(int16_t));
279     memset(ec->snapshot, 0, sizeof(int16_t)*ec->taps);
280
281     ec->cond_met = 0;
282     ec->Pstates = 0;
283     ec->Ltxacc = ec->Lrxacc = ec->Lcleanacc = ec->Lclean_bgacc = 0;
284     ec->Ltx = ec->Lrx = ec->Lclean = ec->Lclean_bg = 0;
285     ec->tx_1 = ec->tx_2 = ec->rx_1 = ec->rx_2 = 0;
286     ec->Lbgn = ec->Lbgn_acc = 0;
287     ec->Lbgn_upper = 200;
288     ec->Lbgn_upper_acc = ec->Lbgn_upper << 13;
289
290     return  ec;
291 }
292 /*- End of function --------------------------------------------------------*/
293
294 void echo_can_free(echo_can_state_t *ec)
295 {
296         int i;
297
298         fir16_free(&ec->fir_state);
299         fir16_free(&ec->fir_state_bg);
300         for (i = 0;  i < 2;  i++)
301                 kfree(ec->fir_taps16[i]);
302         kfree(ec->snapshot);
303         kfree(ec);
304 }
305 /*- End of function --------------------------------------------------------*/
306
307 void echo_can_adaption_mode(echo_can_state_t *ec, int adaption_mode)
308 {
309     ec->adaption_mode = adaption_mode;
310 }
311 /*- End of function --------------------------------------------------------*/
312
313 void echo_can_flush(echo_can_state_t *ec)
314 {
315     int i;
316
317     ec->Ltxacc = ec->Lrxacc = ec->Lcleanacc = ec->Lclean_bgacc = 0;
318     ec->Ltx = ec->Lrx = ec->Lclean = ec->Lclean_bg = 0;
319     ec->tx_1 = ec->tx_2 = ec->rx_1 = ec->rx_2 = 0;
320
321     ec->Lbgn = ec->Lbgn_acc = 0;
322     ec->Lbgn_upper = 200;
323     ec->Lbgn_upper_acc = ec->Lbgn_upper << 13;
324
325     ec->nonupdate_dwell = 0;
326
327     fir16_flush(&ec->fir_state);
328     fir16_flush(&ec->fir_state_bg);
329     ec->fir_state.curr_pos = ec->taps - 1;
330     ec->fir_state_bg.curr_pos = ec->taps - 1;
331     for (i = 0;  i < 2;  i++)
332         memset(ec->fir_taps16[i], 0, ec->taps*sizeof(int16_t));
333
334     ec->curr_pos = ec->taps - 1;
335     ec->Pstates = 0;
336 }
337 /*- End of function --------------------------------------------------------*/
338
339 void echo_can_snapshot(echo_can_state_t *ec) {
340     memcpy(ec->snapshot, ec->fir_taps16[0], ec->taps*sizeof(int16_t));
341 }
342 /*- End of function --------------------------------------------------------*/
343
344 /* Dual Path Echo Canceller ------------------------------------------------*/
345
346 int16_t echo_can_update(echo_can_state_t *ec, int16_t tx, int16_t rx)
347 {
348     int32_t echo_value;
349     int clean_bg;
350     int tmp, tmp1;
351
352     /* Input scaling was found be required to prevent problems when tx
353        starts clipping.  Another possible way to handle this would be the
354        filter coefficent scaling. */
355
356     ec->tx = tx; ec->rx = rx;
357     tx >>=1;
358     rx >>=1;
359
360     /*
361        Filter DC, 3dB point is 160Hz (I think), note 32 bit precision required
362        otherwise values do not track down to 0. Zero at DC, Pole at (1-Beta)
363        only real axis.  Some chip sets (like Si labs) don't need
364        this, but something like a $10 X100P card does.  Any DC really slows
365        down convergence.
366
367        Note: removes some low frequency from the signal, this reduces
368        the speech quality when listening to samples through headphones
369        but may not be obvious through a telephone handset.
370
371        Note that the 3dB frequency in radians is approx Beta, e.g. for
372        Beta = 2^(-3) = 0.125, 3dB freq is 0.125 rads = 159Hz.
373     */
374
375     if (ec->adaption_mode & ECHO_CAN_USE_RX_HPF) {
376       tmp = rx << 15;
377 #if 1
378         /* Make sure the gain of the HPF is 1.0. This can still saturate a little under
379            impulse conditions, and it might roll to 32768 and need clipping on sustained peak
380            level signals. However, the scale of such clipping is small, and the error due to
381            any saturation should not markedly affect the downstream processing. */
382         tmp -= (tmp >> 4);
383 #endif
384       ec->rx_1 += -(ec->rx_1>>DC_LOG2BETA) + tmp - ec->rx_2;
385
386       /* hard limit filter to prevent clipping.  Note that at this stage
387          rx should be limited to +/- 16383 due to right shift above */
388       tmp1 = ec->rx_1 >> 15;
389       if (tmp1 > 16383) tmp1 = 16383;
390       if (tmp1 < -16383) tmp1 = -16383;
391       rx = tmp1;
392       ec->rx_2 = tmp;
393     }
394
395     /* Block average of power in the filter states.  Used for
396        adaption power calculation. */
397
398     {
399         int new, old;
400
401         /* efficient "out with the old and in with the new" algorithm so
402            we don't have to recalculate over the whole block of
403            samples. */
404         new = (int)tx * (int)tx;
405         old = (int)ec->fir_state.history[ec->fir_state.curr_pos] *
406               (int)ec->fir_state.history[ec->fir_state.curr_pos];
407         ec->Pstates += ((new - old) + (1<<ec->log2taps)) >> ec->log2taps;
408         if (ec->Pstates < 0) ec->Pstates = 0;
409     }
410
411     /* Calculate short term average levels using simple single pole IIRs */
412
413     ec->Ltxacc += abs(tx) - ec->Ltx;
414     ec->Ltx = (ec->Ltxacc + (1<<4)) >> 5;
415     ec->Lrxacc += abs(rx) - ec->Lrx;
416     ec->Lrx = (ec->Lrxacc + (1<<4)) >> 5;
417
418     /* Foreground filter ---------------------------------------------------*/
419
420     ec->fir_state.coeffs = ec->fir_taps16[0];
421     echo_value = fir16(&ec->fir_state, tx);
422     ec->clean = rx - echo_value;
423     ec->Lcleanacc += abs(ec->clean) - ec->Lclean;
424     ec->Lclean = (ec->Lcleanacc + (1<<4)) >> 5;
425
426     /* Background filter ---------------------------------------------------*/
427
428     echo_value = fir16(&ec->fir_state_bg, tx);
429     clean_bg = rx - echo_value;
430     ec->Lclean_bgacc += abs(clean_bg) - ec->Lclean_bg;
431     ec->Lclean_bg = (ec->Lclean_bgacc + (1<<4)) >> 5;
432
433     /* Background Filter adaption -----------------------------------------*/
434
435     /* Almost always adap bg filter, just simple DT and energy
436        detection to minimise adaption in cases of strong double talk.
437        However this is not critical for the dual path algorithm.
438     */
439     ec->factor = 0;
440     ec->shift = 0;
441     if ((ec->nonupdate_dwell == 0)) {
442         int   P, logP, shift;
443
444         /* Determine:
445
446            f = Beta * clean_bg_rx/P ------ (1)
447
448            where P is the total power in the filter states.
449
450            The Boffins have shown that if we obey (1) we converge
451            quickly and avoid instability.
452
453            The correct factor f must be in Q30, as this is the fixed
454            point format required by the lms_adapt_bg() function,
455            therefore the scaled version of (1) is:
456
457            (2^30) * f  = (2^30) * Beta * clean_bg_rx/P
458                factor  = (2^30) * Beta * clean_bg_rx/P         ----- (2)
459
460            We have chosen Beta = 0.25 by experiment, so:
461
462                factor  = (2^30) * (2^-2) * clean_bg_rx/P
463
464                                        (30 - 2 - log2(P))
465                factor  = clean_bg_rx 2                         ----- (3)
466
467            To avoid a divide we approximate log2(P) as top_bit(P),
468            which returns the position of the highest non-zero bit in
469            P.  This approximation introduces an error as large as a
470            factor of 2, but the algorithm seems to handle it OK.
471
472            Come to think of it a divide may not be a big deal on a
473            modern DSP, so its probably worth checking out the cycles
474            for a divide versus a top_bit() implementation.
475         */
476
477         P = MIN_TX_POWER_FOR_ADAPTION + ec->Pstates;
478         logP = top_bit(P) + ec->log2taps;
479         shift = 30 - 2 - logP;
480         ec->shift = shift;
481
482         lms_adapt_bg(ec, clean_bg, shift);
483     }
484
485     /* very simple DTD to make sure we dont try and adapt with strong
486        near end speech */
487
488     ec->adapt = 0;
489     if ((ec->Lrx > MIN_RX_POWER_FOR_ADAPTION) && (ec->Lrx > ec->Ltx))
490         ec->nonupdate_dwell = DTD_HANGOVER;
491     if (ec->nonupdate_dwell)
492         ec->nonupdate_dwell--;
493
494     /* Transfer logic ------------------------------------------------------*/
495
496     /* These conditions are from the dual path paper [1], I messed with
497        them a bit to improve performance. */
498
499     if ((ec->adaption_mode & ECHO_CAN_USE_ADAPTION) &&
500         (ec->nonupdate_dwell == 0) &&
501         (8*ec->Lclean_bg < 7*ec->Lclean) /* (ec->Lclean_bg < 0.875*ec->Lclean) */ &&
502         (8*ec->Lclean_bg < ec->Ltx)      /* (ec->Lclean_bg < 0.125*ec->Ltx)    */ )
503     {
504         if (ec->cond_met == 6) {
505             /* BG filter has had better results for 6 consecutive samples */
506             ec->adapt = 1;
507             memcpy(ec->fir_taps16[0], ec->fir_taps16[1], ec->taps*sizeof(int16_t));
508         }
509         else
510             ec->cond_met++;
511     }
512     else
513         ec->cond_met = 0;
514
515     /* Non-Linear Processing ---------------------------------------------------*/
516
517     ec->clean_nlp = ec->clean;
518     if (ec->adaption_mode & ECHO_CAN_USE_NLP)
519     {
520         /* Non-linear processor - a fancy way to say "zap small signals, to avoid
521            residual echo due to (uLaw/ALaw) non-linearity in the channel.". */
522
523       if ((16*ec->Lclean < ec->Ltx))
524       {
525         /* Our e/c has improved echo by at least 24 dB (each factor of 2 is 6dB,
526            so 2*2*2*2=16 is the same as 6+6+6+6=24dB) */
527         if (ec->adaption_mode & ECHO_CAN_USE_CNG)
528         {
529             ec->cng_level = ec->Lbgn;
530
531             /* Very elementary comfort noise generation.  Just random
532                numbers rolled off very vaguely Hoth-like.  DR: This
533                noise doesn't sound quite right to me - I suspect there
534                are some overlfow issues in the filtering as it's too
535                "crackly".  TODO: debug this, maybe just play noise at
536                high level or look at spectrum.
537             */
538
539             ec->cng_rndnum = 1664525U*ec->cng_rndnum + 1013904223U;
540             ec->cng_filter = ((ec->cng_rndnum & 0xFFFF) - 32768 + 5*ec->cng_filter) >> 3;
541             ec->clean_nlp = (ec->cng_filter*ec->cng_level*8) >> 14;
542
543         }
544         else if (ec->adaption_mode & ECHO_CAN_USE_CLIP)
545         {
546             /* This sounds much better than CNG */
547             if (ec->clean_nlp > ec->Lbgn)
548               ec->clean_nlp = ec->Lbgn;
549             if (ec->clean_nlp < -ec->Lbgn)
550               ec->clean_nlp = -ec->Lbgn;
551         }
552         else
553         {
554           /* just mute the residual, doesn't sound very good, used mainly
555              in G168 tests */
556           ec->clean_nlp = 0;
557         }
558       }
559       else {
560           /* Background noise estimator.  I tried a few algorithms
561              here without much luck.  This very simple one seems to
562              work best, we just average the level using a slow (1 sec
563              time const) filter if the current level is less than a
564              (experimentally derived) constant.  This means we dont
565              include high level signals like near end speech.  When
566              combined with CNG or especially CLIP seems to work OK.
567           */
568           if (ec->Lclean < 40) {
569               ec->Lbgn_acc += abs(ec->clean) - ec->Lbgn;
570               ec->Lbgn = (ec->Lbgn_acc + (1<<11)) >> 12;
571           }
572        }
573     }
574
575     /* Roll around the taps buffer */
576     if (ec->curr_pos <= 0)
577         ec->curr_pos = ec->taps;
578     ec->curr_pos--;
579
580     if (ec->adaption_mode & ECHO_CAN_DISABLE)
581       ec->clean_nlp = rx;
582
583     /* Output scaled back up again to match input scaling */
584
585     return (int16_t) ec->clean_nlp << 1;
586 }
587
588 /*- End of function --------------------------------------------------------*/
589
590 /* This function is seperated from the echo canceller is it is usually called
591    as part of the tx process.  See rx HP (DC blocking) filter above, it's
592    the same design.
593
594    Some soft phones send speech signals with a lot of low frequency
595    energy, e.g. down to 20Hz.  This can make the hybrid non-linear
596    which causes the echo canceller to fall over.  This filter can help
597    by removing any low frequency before it gets to the tx port of the
598    hybrid.
599
600    It can also help by removing and DC in the tx signal.  DC is bad
601    for LMS algorithms.
602
603    This is one of the classic DC removal filters, adjusted to provide sufficient
604    bass rolloff to meet the above requirement to protect hybrids from things that
605    upset them. The difference between successive samples produces a lousy HPF, and
606    then a suitably placed pole flattens things out. The final result is a nicely
607    rolled off bass end. The filtering is implemented with extended fractional
608    precision, which noise shapes things, giving very clean DC removal.
609 */
610
611 int16_t echo_can_hpf_tx(echo_can_state_t *ec, int16_t tx) {
612     int tmp, tmp1;
613
614     if (ec->adaption_mode & ECHO_CAN_USE_TX_HPF) {
615         tmp = tx << 15;
616 #if 1
617         /* Make sure the gain of the HPF is 1.0. The first can still saturate a little under
618            impulse conditions, and it might roll to 32768 and need clipping on sustained peak
619            level signals. However, the scale of such clipping is small, and the error due to
620            any saturation should not markedly affect the downstream processing. */
621         tmp -= (tmp >> 4);
622 #endif
623         ec->tx_1 += -(ec->tx_1>>DC_LOG2BETA) + tmp - ec->tx_2;
624         tmp1 = ec->tx_1 >> 15;
625         if (tmp1 > 32767) tmp1 = 32767;
626         if (tmp1 < -32767) tmp1 = -32767;
627         tx = tmp1;
628         ec->tx_2 = tmp;
629     }
630
631     return tx;
632 }