cpda.s55

CP detector (CPD) only reports the onset and removal of a tone. The analysis of timing (if required)

*-------------------------------------------------------------------------*
*                                                                         *
*   THIS IS AN UNPUBLISHED WORK CONTAINING CONFIDENTIAL AND PROPRIETARY   *
*   INFORMATION.  IF PUBLICATION OCCURS, THE FOLLOWING NOTICE APPLIES:    *
*      "COPYRIGHT 2001 MIKET DSP SOLUTIONS, ALL RIGHTS RESERVED"          *
*                                                                         *
*-------------------------------------------------------------------------*/
	.mmregs
    .arms_on
    .cpl_on
	.model call=c55_std
	.model mem=large
	
	.include "cpdi.h55"

	.global _CPD_aBpI
	.global _CPD_aBpQ;
	.global _CPD_aPg;
	.global _CPD_aPhase;
	
	.text
	
macro_en2log		.macro acc, treg, added
    acc = acc + #1;
    acc = mant (acc), treg = -exp(acc) || nop
    acc = acc << #-21;
    acc = acc + #(0x780 + added);
    acc = acc + (treg*#512);
	.endm
	
;-------------------------------------------------------------------------
	.global				_cpd_en2log
;	.sym _cpd_en2log, _cpd_en2log, 32, 2, 0
;-------------------------------------------------------------------------
_cpd_en2log
;	S32 slEn 	= ac0
;	S16 sCorr	= t0
;
; 	returns t0
;
    ac0 = ac0 + #1;
    ac0 = mant (ac0), t1 = -exp(ac0) || nop;
    ac0 = ac0 << #-21;
    ac0 = ac0 + #0x780;
    ac0 = ac0 + t0;
    ac0 = ac0 + (t1*#512);
	t0 = ac0
	return
;-------------------------------------------------------------------------
	.global _cpd_move_data_in
;	.sym _cpd_move_data_in, _cpd_move_data_in, 32, 2, 0
_cpd_move_data_in
;-------------------------------------------------------------------------*/
;	CPD_tDb *pDb 	= ar0
;	CPD_tSc *pSc	= ar1
; 	S16 *psIn		= ar2
;
;	
	xar3 = mar(*ar0(#(CPD_tDb.asBpDataSav)))
	xar4 = mar(*ar1(#(CPD_tSc.asBpData)))
	repeat	(#(CPD_BP_SSZ-1))
		*ar4+ = *ar3+;
		
	xar3 = dbl(*ar0(#(CPD_tDb.pCfg)))
	t1 = *ar3(#(ICPD_tCfg.sNormShift))
	brc0 = #(ICPD_FR_SZ-1)
	
	localrepeat {
		ac0 = *ar2+ << t1; 
		*ar4+ = ac0;
	}

	xar3 = mar(*ar0(#(CPD_tDb.asPgIDataSav)))
	xar4 = mar(*ar1(#(CPD_tSc.asPgIData)))
	repeat (#(CPD_PG_SSZ-1))
		*ar4+ = *ar3+
			
	xar3 = mar(*ar0(#(CPD_tDb.asPgQDataSav)))
	xar4 = mar(*ar1(#(CPD_tSc.asPgQData)))
	repeat (#(CPD_PG_SSZ-1))
		*ar4+ = *ar3+
	return
	
;-------------------------------------------------------------------------*/
	.global _cpd_filter_bp
;	.sym _cpd_filter_bp, _cpd_filter_bp, 32, 2, 0
_cpd_filter_bp
;-------------------------------------------------------------------------*/
;	CPD_tSc *pSc 	= ar0
;	
	.arms_off
	bit(ST2, #ST2_ARMS) = #0;

	brc0 = #(ICPD_FR_SZ/CPD_BP_DR-1);
	ac0 = #0;
	xcdp = mar(*ar0(#(CPD_tSc.asBpData)))
	xar1 = #_CPD_aBpI;
	xar2 = #_CPD_aBpQ;
	xar3 = mar(*ar0(#(CPD_tSc.asPgIData + CPD_PG_SSZ)))
	xar4 = mar(*ar0(#(CPD_tSc.asPgQData + CPD_PG_SSZ)))
	t0 = #(1-(CPD_BP_FSZ - CPD_BP_DR));
	t1 = #(1-CPD_BP_FSZ);
	localrepeat {
		ac1 = *ar1+ * coef(*cdp+),
		ac2 = *ar2+ * coef(*cdp+)
		|| repeat (#(CPD_BP_FSZ-3))
			ac1 = ac1 + (*ar1+ * coef(*cdp+)),
			ac2 = ac2 + (*ar2+ * coef(*cdp+))
		ac1 = rnd(ac1 + (*(ar1+t1) * coef(*(cdp+t0)))),
		ac2 = rnd(ac2 + (*(ar2+t1) * coef(*(cdp+t0))))
		*ar3+ = HI(ac1);
		|| ac0 = ac0 + (ac1 * ac1);
		*ar4+ = HI(ac2);
		|| ac0 = ac0 + (ac2 * ac2);
	}
	dbl(*ar0(#(CPD_tSc.slBpEn))) = ac0;
	macro_en2log ac0, t0, CPD_EN_BP
	*ar0(#(CPD_tSc.sBpEn)) = ac0;
	.arms_on
	bit(ST2, #ST2_ARMS) = #1;
	return;
		
;-------------------------------------------------------------------------*/
	.global	_cpd_sum_sub
;	.sym _cpd_sum_sub, _cpd_sum_sub, 32, 2, 0
_cpd_sum_sub
;-------------------------------------------------------------------------*/
;	CPD_tSc *pSc	= ar0
;
; I
	xar1 = mar(*ar0(#(CPD_tSc.asPgIData)))
	xar2 = mar(*ar0(#(CPD_tSc.asPgIData+(CPD_PG_FSZ-1))))
	xar3 = mar(*ar0(#(CPD_tSc.asPgISum)))
	xar4 = mar(*ar0(#(CPD_tSc.asPgIDif)))
	brc0 = #(CPD_PG_HSZ-1)
	localrepeat {
		t0 = *ar2-;
		HI(ac0) = *ar1+ + t0, LO(ac0) = *ar1+ - t0;
		*ar4+ = LO(ac0), *ar3+ = HI(ac0)
	}
;
; Q	
	xar1 = mar(*ar0(#(CPD_tSc.asPgQData)))
	xar2 = mar(*ar0(#(CPD_tSc.asPgQData+(CPD_PG_FSZ-1))))
	xar3 = mar(*ar0(#(CPD_tSc.asPgQSum)))
	xar4 = mar(*ar0(#(CPD_tSc.asPgQDif)))
	brc0 = #(CPD_PG_HSZ-1)
	localrepeat {
		t0 = *ar2-;
		HI(ac0) = *ar1+ + t0, LO(ac0) = *ar1+ - t0;
		*ar4+ = LO(ac0), *ar3+ = HI(ac0)
	}
	return

;-------------------------------------------------------------------------*/
	.global _cpd_filter_pg
;	.sym _cpd_filter_pg, _cpd_filter_pg, 32, 2, 0
_cpd_filter_pg
;-------------------------------------------------------------------------*/
;	CPD_tSc *pSc	= ar0
;
	xar1 = mar(*ar0(#(CPD_tSc.asPgISum)))
	xar2 = mar(*ar0(#(CPD_tSc.asPgQSum)))
	xar3 = mar(*ar0(#(CPD_tSc.aOut)))
	xcdp = #_CPD_aPg
	t0 = #(1-2*CPD_PG_HSZ);
	
	brc0 = #(CPD_FREQS-1)
	localrepeat {
		ac0 = *ar1+ * coef(*cdp+),
		ac1 = *ar2+ * coef(*cdp+)
		|| repeat (#(CPD_PG_HSZ-3))
			ac0 = ac0 + (*ar1+ * coef(*cdp+)),
			ac1 = ac1 + (*ar2+ * coef(*cdp+))
		
		ac0 = ac0 + (*ar1+ * coef(*cdp+)),
		ac1 = ac1 + (*ar2+ * coef(*cdp+))
		|| repeat (#(CPD_PG_HSZ-2))
			ac0 = ac0 - (*ar2+ * coef(*cdp+)),
			ac1 = ac1 + (*ar1+ * coef(*cdp+))
		ac0 = rnd(ac0 - (*(ar2+t0) * coef(*cdp+))),
		ac1 = rnd(ac1 + (*(ar1+t0) * coef(*cdp+)))
		*ar3+ = HI(ac0);
		|| ac0 = ac0 * ac0;
		*ar3+ = HI(ac1);
		|| ac0 = ac0 + (ac1 * ac1);
		dbl(*ar3+) = ac0;
	}
	return
	
;-------------------------------------------------------------------------*/
	.global _cpd_sort_freqs
;	.sym _cpd_sort_freqs, _cpd_sort_freqs, 32, 2, 0
_cpd_sort_freqs
;-------------------------------------------------------------------------*/
;	CPD_tSc *pSc 	= a
;
	xar1 = mar(*ar0(#(CPD_tSc.aOut+CPD_tOut.slEn)))
	xar2 = mar(*ar0(#(CPD_tSc.aslEnSort)))
	t0 = #(CPD_tOut_Sz)
	repeat (#(CPD_FREQS-1))
		dbl(*ar2+) = dbl(*(ar1+t0));
		
;
; find max
	xar2 = mar(*ar0(#(CPD_tSc.aslEnSort)))
	ac0	 = dbl(*ar2+)
    t0   = #0
    ac3  = #0;
	brc0 = #(CPD_FREQS-2)
	localrepeat {
        ac1 = dbl(*ar2+) || ac3 = ac3 + #1;
        ac0 = max (ac1, ac0)                ; carry = 0 if ac1>ac0
        if (!CARRY) execute(D_unit)         ; 
            || t0 = ac3
	}
	*ar0(#(CPD_tSc.sMaxIdx)) = t0;
	xar2 = mar(*ar0(#(CPD_tSc.aslEnSort)))
	xar1 = mar(*ar2(t0))
	ac3	 = #0;
	dbl(*ar1(t0)) = ac3;
	macro_en2log	ac0, t0, CPD_EN_PG
	*ar0(#(CPD_tSc.sMaxEn)) = ac0;
;
; find next
	ac0	 = dbl(*ar2+)
    t0   = #0
	brc0 = #(CPD_FREQS-2)
	localrepeat {
        ac1 = dbl(*ar2+) || ac3 = ac3 + #1;
        ac0 = max (ac1, ac0)                ; carry = 0 if ac1>ac0
        if (!CARRY) execute(D_unit)         ; 
            || t0 = ac3
	}
	*ar0(#(CPD_tSc.sNextIdx)) = t0;
	xar2 = mar(*ar0(#(CPD_tSc.aslEnSort)))
	xar1 = mar(*ar2(t0))
	ac3	 = #0;
	dbl(*ar1(t0)) = ac3;
	macro_en2log	ac0, t0, CPD_EN_PG
	*ar0(#(CPD_tSc.sNextEn)) = ac0;
;
; find rest
	ac0	 = dbl(*ar2+)
    t0   = #0
	brc0 = #(CPD_FREQS-2)
	localrepeat {
        ac1 = dbl(*ar2+) || ac3 = ac3 + #1;
        ac0 = max (ac1, ac0)                ; carry = 0 if ac1>ac0
        if (!CARRY) execute(D_unit)         ; 
            || t0 = ac3
	}
	*ar0(#(CPD_tSc.sRestIdx)) = t0;
	macro_en2log	ac0, t0, CPD_EN_PG
	*ar0(#(CPD_tSc.sRestEn)) = ac0;
	.if 0
	.endif
	return;

;-------------------------------------------------------------------------*/
	.global _cpd_get_err
;	.sym _cpd_get_err, _cpd_get_err, 32, 2, 0
_cpd_get_err
;-------------------------------------------------------------------------*/
;	S32 slEn = ac0
;	S32 slErr = ac1
;
	sp = sp - #2;
	
	ac2 = |ac1|
	ac3 = ac2;
	ac3 = ac3 - ac0;
	
	ac0 = ac0 + #1;
	ac0 = mant(ac0), t0 = -exp(ac0) || nop
	t0 = -t0;
	ac2 = ac2 << t0;
	ac2 = ac2 << #-16 
	ac2 = ac2 << #15 
	*sp = HI(ac0);
	repeat (#(16-1))
		subc (*sp, ac2, ac2);
	*sp = ac2;
	ac2 = *sp << #16;
	ac2 = ac2 * #(-5450);
	ac2 = ac2 << #-15;
	
	if (ac3 >= #0) execute (D_unit)
		|| ac2 = #-5450;
		
	if (ac1 < #0) execute (D_unit)
		|| ac2 = -ac2;
	
	t0 = ac2;
	
	sp = sp + #2;
	return;
	
;-------------------------------------------------------------------------*/
	.global _cpd_get_elem_data
;	.sym _cpd_get_elem_data, _cpd_get_elem_data, 32, 2, 0
_cpd_get_elem_data
;-------------------------------------------------------------------------*/
;	CPD_tDb *pDb	= ar0
;	CPD_tSc *pSc	= ar1
; 	CPD_tElem *pElem = ar2
; 	S16 sIdx		= t0
;
	t0 = t0 << #1;
	.arms_off
	bit(ST2, #ST2_ARMS) = #0;
	
	xcdp = #_CPD_aPhase;

	mar(*ar1), mar(*ar2), mar ( coef(*(cdp + t0)))

	t1 = t0 + #(CPD_tDb.aOutSav);
	xar3 = mar(*ar0(t1));
	
	t0 = t0 << #1;
	t1 = t0 + #(CPD_tSc.aOut);
	xar4 = mar(*ar1(t1));

	xar0 = mar(*ar3(#1))
	xar1 = mar(*ar2(#(CPD_tElem.sCpred)));
;    
; make prediction    
    ac2 = *ar3 * coef(*cdp+), 
	ac3 = *ar0 * coef(*cdp+);
	ac3 = ac3 - (*ar3 * coef(*cdp+)),
    ac2 = ac2 + (*ar0 * coef(*cdp+)); 
; save prediction    
    *ar1+ = HI(rnd(ac2 << #1));
    *ar1- = HI(rnd(ac3 << #1));
;	
; make distance
	ac2 = (*ar1+ << #16) - (*ar4+ << #16);
	ac3 = ac2 * ac2;
	ac2 = (*ar1- << #16) - (*ar4  << #16);
	ac3 = ac3 + (ac2 * ac2);
; make distance	energy
	macro_en2log	ac3, t1, CPD_EN_PG
; save distance	
	*ar2(#(CPD_tElem.sDist)) = ac3;
;	
; make error	
	ac1 = *ar4- * *ar1+;
	ac1 = ac1 - (*ar4+ * *ar1);
	mar (*ar4+)
	ac0 = dbl(*ar4)
	call _cpd_get_err
	*ar2(#(CPD_tElem.sErr)) = t0;
;
; exit	
	.arms_on
	bit(ST2, #ST2_ARMS) = #1;
    .if 0
	.endif
	return;

;-------------------------------------------------------------------------*/
	.global _cpd_move_data_out
;	.sym _cpd_move_data_out, _cpd_move_data_out, 32, 2, 0
_cpd_move_data_out
;-------------------------------------------------------------------------*/
;	CPD_tDb *pDb 	= ar0
; 	CPD_tSc *pSc	= ar1
;
; save Bp data	
	xar2 = mar(*ar0(#(CPD_tDb.asBpDataSav)))
	xar3 = mar(*ar1(#(CPD_tSc.asBpData+(CPD_BP_ASZ-CPD_BP_SSZ))))
	repeat (#(CPD_BP_SSZ-1))
		*ar2+ = *ar3+
; save PgI data		
	xar2 = mar(*ar0(#(CPD_tDb.asPgIDataSav)))
	xar3 = mar(*ar1(#(CPD_tSc.asPgIData+(CPD_PG_ASZ-CPD_PG_SSZ))))
	repeat (#(CPD_PG_SSZ-1))
		*ar2+ = *ar3+
; save PgQ data		
	xar2 = mar(*ar0(#(CPD_tDb.asPgQDataSav)))
	xar3 = mar(*ar1(#(CPD_tSc.asPgQData+(CPD_PG_ASZ-CPD_PG_SSZ))))
	repeat (#(CPD_PG_SSZ-1))
		*ar2+ = *ar3+
; save Out: sC, sS		
	xar2 = mar(*ar0(#(CPD_tDb.aOutSav)))
	xar3 = mar(*ar1(#(CPD_tSc.aOut)))
	brc0 = #(CPD_FREQS-1)
	t0 = #(CPD_tOut_Sz-1)
	localrepeat {
		*ar2+ = *ar3+
		*ar2+ = *(ar3+t0)
	}
; delay asSum and update asSum[0]
	xar2 = mar(*ar0(#(CPD_tDb.v.asSumEn+CPD_EN_SZ-2)))
	xar3 = mar(*ar1(#(CPD_tSc.sSumEn)))
	
	repeat (#(CPD_EN_SZ-2))
		delay 	(*ar2-)
	mar (*ar2+)
	*ar2 = *ar3
	return

;-------------------------------------------------------------------------
	.global				_cpd_avrg
;	.sym _cpd_avrg, _cpd_avrg, 32, 2, 0
_cpd_avrg
;-------------------------------------------------------------------------
;	S16 *psVal	= ar0
;	S16	sNew	= t0
;	S16 sCoeff	= t1
;
	ac0 = t0;
	ac0 = ac0 - *ar0;
	ac0 = ac0 << 16;
	ac0 = ac0 * t1;
	ac0 = ac0 + (*ar0 << #15);
	*ar0 = HI(rnd(ac0 << #1));
	return;