lecua.s54

The line echo canceller (LEC) is designed to provide the maximum attainable transparent voice qualit

	.mmregs
	.include "leci.h54"
	.text

;-------------------------------------------------------------------------
	.global				_lec_dummy1
	.sym _lec_dummy1, _lec_dummy1, 32, 2, 0
;-------------------------------------------------------------------------
_lec_dummy1
	ret
	.sect	".lectxt1"
;-------------------------------------------------------------------------
	.global				_lec_avrg
	.sym _lec_avrg, _lec_avrg, 32, 2, 0
;-------------------------------------------------------------------------
_lec_avrg
;	S16 *psVal	= a
;	S16	sNew	= sp(1)
;	S16 sCoeff	= sp(2)
;
	stlm	a, ar4
	ssbx	sxm
	ssbx	frct
	ld		*sp(1), 16, a
	sub		*ar4, 16, a
	ld		*ar4, 16, b
	macar	*sp(2)
	sth		b, *ar4
	rsbx	frct
	ret
;-------------------------------------------------------------------------
	.global				_lec_pkts_cpy
	.sym _lec_pkts_cpy, _lec_pkts_cpy, 32, 2, 0
;-------------------------------------------------------------------------
_lec_pkts_cpy
;	S16 *psTo		= a
;	S16 *psFrom		= sp(1)
;	S16 uPktsToCopy	= sp(2)
;
	stlm	a, ar2
	addm	#(-1), *sp(2)
	mvdk	*sp(2), brc
	mvdk	*sp(1), ar3
	rptb	L02-1
		rpt		#(ILEC_FR_SZ-1)
			mvdd	*ar3+, *ar2+
L02
	ret
;-------------------------------------------------------------------------
	.global				_lec_pkt_rand
	.sym _lec_pkt_rand, _lec_pkt_rand, 32, 2, 0
;-------------------------------------------------------------------------
_lec_pkt_rand
;	S16 *psTo	= a
;	S16 *psSeed = sp(1)
;
MULT    .set   31821
INC     .set   13849
;
    stlm    a, ar2              ; pTo
    ssbx    sxm
    mvdk    *sp(1), ar3         ; pSeed
    stm     #MULT, t
    ld      #INC, b
    stm     #(ILEC_FR_SZ-1), brc
    rptb    L03-1
        mpy     *ar3, a         ; [1]
        add     b, a            ; [1]
        stl     a, *ar3         ; [1]
        stl     a, *ar2+        ; [1]
L03
    ret
;-------------------------------------------------------------------------
	.global				_lec_pkt_energy
	.sym _lec_pkt_energy, _lec_pkt_energy, 32, 2, 0
;-------------------------------------------------------------------------
_lec_pkt_energy
;	S16 *psData
;
;	return S32 En
;
    stlm    a, ar2
    ssbx    sxm
    rptz    b, #(ILEC_FR_SZ/2-1)
        squra    *ar2+, b
    rptz    a, #(ILEC_FR_SZ/2-1)
        squra    *ar2+, a
    retd
    ld      a, -2, a
    add     b, -2, a
;-------------------------------------------------------------------------
	.global				_lec_en2log
	.sym _lec_en2log, _lec_en2log, 32, 2, 0
;-------------------------------------------------------------------------
_lec_en2log
;	S32 slEn 	= a
;
	ssbx	sxm
	add		#1, a
	exp		a
	ld		#0x780-1024, b
	norm	a
	ld		a, -16, a
	add		a, -5, b
	mac		#-512, b, a
;	sub		#1024, a
	ret
;-------------------------------------------------------------------------
	.global				_lec_exp
	.sym _lec_exp, _lec_exp, 32, 2, 0
;-------------------------------------------------------------------------
_lec_exp
;	S16 slog
;	returns S16 sExp in a
;
	frame	#-1

	ssbx	sxm
	ld		a, -10, b
	ld		a, 2, a
	and		#4095, a
	stl		a, *sp(0)
	squr	*sp(0), a
	ld		a, -13, a
	add		*sp(0), a
	stl		a, *sp(0)
	mpy		*sp(0), #21845, a
	ld		a, -15, a
	stlm	b, t
	add		#4096, a
	norm	a
	frame	#1
	ret
;-------------------------------------------------------------------------
	.global				_lec_pkt_zc
	.sym _lec_pkt_zc, _lec_pkt_zc, 32, 2, 0
;-------------------------------------------------------------------------
_lec_pkt_zc
;	S16 *psIn = a
;
    stlm    a, ar2
    rsbx    sxm
    ld      #0, a
;
; bit 15 is the sign bit.
    stm     #ILEC_FR_SZ-2, brc
    rptb	L04-1
        ldu     *ar2+, b
        xor     *ar2, b
        add     b, -15, a
L04
    stlm    a, t
    retd
    mpy     #170, a
;-------------------------------------------------------------------------
	.global				_lec_pkt_peak
	.sym _lec_pkt_peak, _lec_pkt_peak, 32, 2, 0
;-------------------------------------------------------------------------
_lec_pkt_peak
;	S16 *psIn = a
;
    stlm    a, ar2
    ssbx   sxm
    stm    #ILEC_FR_SZ-1, brc
    ld     #0, a
;
    rptb    L05-1
        ld    *ar2+, b
        abs   b
        max   a             ; a= max(a, b)
L05
	ret
;-------------------------------------------------------------------------
	.global				_lec_pkt_excess
	.sym _lec_pkt_excess, _lec_pkt_excess, 32, 2, 0
;-------------------------------------------------------------------------
_lec_pkt_excess
;	S16 *psIn	= a
;	S16 ClipThr	= sp(1)
;	
	ssbx  	sxm
    stlm    a, ar4      ; ar4 = pError
	ld      *sp(1), b   ; b = ClipThr
;
; set context
    stm     #ILEC_FR_SZ-1, brc
	stm     #0, ar3
	rptb	L06-1
		ld	    *ar4+, a; [1]
		abs     a       ; [1]
        sub     b, a
		nop
		nop             ; [1]
		xc		1, agt   ; [3]
			mar		*ar3+
L06
	nop
    nop
	ldm	    ar3, a			; acc = excess
	ret
	
;-------------------------------------------------------------------------
	.global				_lec_make_noise
	.sym _lec_make_noise, _lec_make_noise, 32, 2, 0
;-------------------------------------------------------------------------
	.sect	".lecd"
_asNoiseCoeff
 	.word 12000,   9000,    6000,    3000,    5000
	.text
	
_lec_make_noise
;	S16 *psTo		= a
;	S16 *psFrom		= sp(1)
;	S16 sNoiseLevel	= sp(2)
;
	stlm	a, ar5
	ssbx	sxm
	ssbx	frct
	stm		#_asNoiseCoeff, ar3
	mvdk	*sp(1), ar2
	stm		#ILEC_FR_SZ-1, brc
	stm		#1-5, ar0
	rptb	L07-1
		rptz	a, #(5-2)
			mac		*ar2+, *ar3+, a
		mac		*ar2+0%, *ar3+0%, a
		mar		*ar2+				; next ...
		mpya	*sp(2)
		add		#1, 15, b
		sth		b, *ar5+
L07
	rsbx	frct
	ret	
	.sect	".lectxt0"
;-------------------------------------------------------------------------
	.global				_lec_central_clipping
	.sym _lec_central_clipping, _lec_central_clipping, 32, 2, 0
;-------------------------------------------------------------------------
_lec_central_clipping
;	S16 *pTo		= a,
;	S16 *pFrom		= sp(1)
;	S16 Threshold	= sp(2)
;
	stlm	a, ar5
	ssbx	sxm
	ld		#0, b
	mvdk	*sp(1), ar4
	stm	    #ILEC_FR_SZ-1, brc
;
	rptb	L08-1
		bit		*ar4, 15-15 ; tc = IsNegative
		ld      *ar4+, a    ; [1]
		abs     a           ; [1]
		sub		*sp(2), a	; [1] acc = abs(*pFrom) - ClipThr
		max		a			; a = 0, if < 0
		xc		1, tc
			neg     a
		add		*ar5, a
		stl	    a, *ar5+
L08
	ret