********************************************************************* * DPLL.asm: TMS320F2407 implementation of digital phase-locked loops * for C2000 CCS simulator *-------------------------------------------------------------------- * DSP Algorithms: * * e(n) ----------------- e'(n) ------- c(n) * ---->| Median filter |------->| LPF |-----> * ----------------- ------- * * where * Median filter: Order 5, the output e'(n) is the median of 5 * samples in the buffer * LPF is the moving-average filter of length L * implemented by the first-order IIR filter. * * NOTE: data buffers for median filters and LPFs are * initialized to the value of 2047 * e(n) is the data from data file "en.int" ******************************************************************** .mmregs ; include symbolic names of MMRs ******************************************************************** * Interrupt vector table for TMS320F2407 * See TMS320F2407 data sheet, pp. 24-25 ******************************************************************** .sect ".vectors" RSVECT B START ; PM 0, Reset ******************************************************************** * Following interrupts may be defined later based on target hardware * and add interrupt service routines. For example, * * INT1 B ISR_1 ; branch to ISR_1 if INT1 occurs * * Each vector occupy two words, B ISR_routinename takes two words * however, RET only uses 1 word, thus use NOP to fill the gap here ******************************************************************** INT1 RET ; PM 2, INT1 NOP ; dummy for 2-word table INT2 RET ; PM 4, INT2 NOP INT3 RET ; PM 6, INT3 NOP INT4 RET ; PM 8, INT4 NOP INT5 RET ; PM Ah, INT5 NOP INT6 RET ; PM Ch, INT6 NOP RESERVED RET ; PM Eh NOP SW_INT8 RET ; PM 10h, User S/W int NOP SW_INT9 RET ; PM 12h, User S/W int NOP SW_INT10 RET ; PM 14h, User S/W int NOP SW_INT11 RET ; PM 16h, User S/W int NOP SW_INT12 RET ; PM 18h, User S/W int NOP SW_INT13 RET ; PM 1Ah, User S/W int NOP SW_INT14 RET ; PM 1Ch, User S/W int NOP SW_INT15 RET ; PM 1Eh, User S/W int NOP SW_INT16 RET ; PM 20h, User S/W int NOP TRAP RET ; PM 22h, Trap vector NOP NMI RET ; PM 24h, Non maskable Int NOP EMU_TRAP RET ; PM 26h, Emulator Trap NOP SW_INT20 RET ; PM 28h, User S/W int NOP SW_INT21 RET ; PM 2Ah, User S/W int NOP SW_INT22 RET ; PM 2Ch, User S/W int NOP SW_INT23 RET ; PM 2Eh, User S/W int NOP ********************************************************************** * include e(n) from data files en.int into data * memory locations for program development ********************************************************************** .sect ".indata" en_in .copy "en.int" ; copy en.dat to memory label en_in .bss cn_out,1024 ; reserve 1024 words for saving c(n) .text ; program segment * LPF (or mean estimator) is the 1st-order IIR filter approximation * of moving-average filter of order L * * e'(n) ------- c(n) * ------->| LPF |------> * ------- * * c(n) = (1-a)*c(n-1) + a*e'(n) * = c(n-1) - a*c(n-1) + a*e'(n) * * where a = 1/L. Assume L = 2^m, a = 2^-m, which can be implemented * by shifting right m bits of data * * Let c(n) be represented by double-precision 32-bit word, * shift right m-bit can be implemented by * loading ACC with shift left (16-m) bits * * Relationship between L, m, and 16-m: * * L | 2 4 8 16 32 64 128 256 512 1024 2048 4096 8192 16384 32768 * m | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 * 16-m |15 14 13 12 11 10 9 8 7 6 5 4 3 2 1 * * Assuming L = 256 is used LPF, m = 8, 16-m = 8 * L = 16 is used for mean estimator, m = 4, 16-m = 12 m8 .set 8 ; order 256 m4 .set 12 ; order 16 file_length .set 1024 ; length of input data file ********************************************************************** * Memory (on-chip DARAM) map: * * B2 (page 0): 060h - 07Fh, used for switching and automatic phase * compensation, data I/O buffer, MISC * B0 (pages 4, 5): 0200h - 02FFh, used for REF 1 * B1 (pages 6, 7): 0300h - 03FFh, used for REF 2 * ********************************************************************** * Page 0, B2 DARAM, 060h - 07Fh * en .set 060h ; address of e(n) cn .set 061h ; address of c(n) loop_cnt .set 065h ; address of loop counter ONE .set 066h ; address of ONE, contain constant 1 ********************************************************************** * Page 4, B0, 0200h - 027Fh, use for REF 1 processing * * buffer for median filter med1_buf0 .set 0200h ; address of med_buf[0] med1_buf1 .set 0201h ; address of med_buf[1] med1_buf2 .set 0202h ; address of med_buf[2] med1_buf3 .set 0203h ; address of med_buf[3] med1_buf4 .set 0204h ; address of med_buf[4] * buffer for sorting of median filter sort1_buf0 .set 0205h ; address of sort_buf[0] sort1_buf1 .set 0206h ; address of sort_buf[1] sort1_buf2 .set 0207h ; address of sort_buf[2] sort1_buf3 .set 0208h ; address of sort_buf[3] sort1_buf4 .set 0209h ; address of sort_buf[4] temp1 .set 020ah ; address of temp1 cn_high .set 020bh ; address of c(n), high word cn_low .set 020ch ; address of c(n), low word *--------------------------------------------------------------------- * Page 5, B0, 0280h - 02FFh, not been used yet! ********************************************************************** * Page 6, B1, 0300h - 037Fh, use for REF 2 processing *--------------------------------------------------------------------- * Page 7, B1, 0380h - 03FFh, not been used yet! *===================================================================== * Processor initialization - starts here from RESET *===================================================================== START: DINT ; disable interrupt LDP #0 ; DP = 0, page 0 SETC SXM ; SXM = 1, sign-extension mode * * Initialize memory on page 0 * MAR *,AR7 ; ARP = 7, AR7 used as pointer LAR AR7,#060h ; AR7 -> lowest address of B2 ZAC ; ACC = 0 RPT #31 ; repeat next instruction 32 times SACL *+ ; clear 32 words memory on B2 ; REF_ID = 0, REF1 is used ; OFFSET = loop_cnt = 0 LACL #1 ; ACC = 1 SACL ONE ; ONE = 1 * * Clear pages 4, 5, 6, and 7 * LAR AR7,#0200h ; AR7 -> lowest address of page 4 ZAC ; ACC = 0 RPT #255 ; repeat next instruction 256 times SACL *+ ; clear memory on pages 4 & 5 RPT #255 ; repeat next instruction 256 times SACL *+ ; clear memory on pages 6 & 7 * * Initialize data memory locations * LACC #2047 ; ACC = 2047 LAR AR7,#med1_buf0 ; AR7 -> median buffer of REF1 RPT #4 ; repeat next instruction 5 times SACL *+ ; med1_buf[i] = 2047, i=0,1,2,3,4 LAR AR7,#cn_high ; AR7 -> c(n), high word SACL * ; c(n) = 2047 * * Initialize ARs as address pointers * LAR AR1,#en_in ; AR1 -> e(n) input data buffer LAR AR3,#cn_out ; AR3 -> c(n) output dtat buffer ; AR7 used for MISC EINT ; enable interrupts * *************************************************************************** * Start of main program *************************************************************************** * Processing of REF 1 signal * MAIN: LDP #4 ; DP = 4, page 4 MAR *,AR7 ; ARP = 7 * * e(n) ----------------- e'(n) * ----->| Median filter |------> * ----------------- * * Update median filter buffer LAR AR7,#med1_buf3 ; AR7 -> med_buf[3] RPT #3 ; repeat next instruction 4 times DMOV *- ; med_buf[i+1] <= med_buf[i],i=3,2,1,0 MAR *,AR1 ; ARP = 1 LACL *+,AR7 ; ACCL <= e(n), ARP = 7, AR1+1 thus ; AR1 points to next e(n) LAR AR7,#med1_buf0 ; AR7 -> med_buf[0] SACL * ; med_buf[0] <= e(n), new input data * Move data from median filter buffer to sorting buffer BLDD *+,#sort1_buf0 ; sort_buf[0] <= med_buf[0] BLDD *+,#sort1_buf1 ; sort_buf[1] <= med_buf[1] BLDD *+,#sort1_buf2 ; sort_buf[2] <= med_buf[2] BLDD *+,#sort1_buf3 ; sort_buf[3] <= med_buf[3] BLDD *+,#sort1_buf4 ; sort_buf[4] <= med_buf[4] * * Sorting data in sort_buf of size 5 * A. sort_buf[0] <- largest sample * B. sort_buf[1] <- second largest sample * C. sort_buf[2] <- median sample, output of median filter * * A. Find largest sample, put into sort_buf[0] ; comparing sort_buf[0] with sort_buf[1] LACL sort1_buf0 ; ACC <= sort_buf[0] SUB sort1_buf1 ; compare with sort_buf[1] BCND NEXT1_A2,GEQ ; branch to NEXT1_A2 if sort_buf[0] is larger * Swap sort_buf[0] with sort_buf[1] if sort_buf[1] is larger LACL sort1_buf0 ; ACC <= sort_buf[0] SACL temp1 ; temp1 <= sort_buf[0] LACL sort1_buf1 ; ACC <= sort_buf[1] SACL sort1_buf0 ; sort_buf[0] <= sort_buf[1] LACL temp1 ; ACC <= old sort_buf[0] SACL sort1_buf1 ; sort_buf[1] <= old sort_buf[0] NEXT1_A2: ; comparing sort_buf[0] with sort_buf[2] LACL sort1_buf0 ; ACC <= sort_buf[0] SUB sort1_buf2 ; compare with sort_buf[2] BCND NEXT1_A3,GEQ ; branch to NEXT1_A3 if sort_buf[0] is larger * Swap sort_buf[0] with sort_buf[2] if sort_buf[2] is larger LACL sort1_buf0 ; ACC <= sort_buf[0] SACL temp1 ; temp1 <= sort_buf[0] LACL sort1_buf2 ; ACC <= sort_buf[2] SACL sort1_buf0 ; sort_buf[0] <= sort_buf[2] LACL temp1 ; ACC <= old sort_buf[0] SACL sort1_buf2 ; sort_buf[2] <= old sort_buf[0] NEXT1_A3: ; comparing sort_buf[0] with sort_buf[3] LACL sort1_buf0 ; ACC <= sort_buf[0] SUB sort1_buf3 ; compare with sort_buf[3] BCND NEXT1_A4,GEQ ; branch to NEXT1_A4 if sort_buf[0] is larger * Swap sort_buf[0] with sort_buf[3] if sort_buf[3] is larger LACL sort1_buf0 ; ACC <= sort_buf[0] SACL temp1 ; temp1 <= sort_buf[0] LACL sort1_buf3 ; ACC <= sort_buf[3] SACL sort1_buf0 ; sort_buf[0] <= sort_buf[3] LACL temp1 ; ACC <= old sort_buf[0] SACL sort1_buf3 ; sort_buf[3] <= old sort_buf[0] NEXT1_A4: ; comparing sort_buf[0] with sort_buf[4] LACL sort1_buf0 ; ACC <= sort_buf[0] SUB sort1_buf4 ; compare with sort_buf[4] BCND NEXT1_B2,GEQ ; branch to NEXT1_B2 if sort_buf[0] is larger * Swap sort_buf[0] with sort_buf[4] if sort_buf[4] is larger LACL sort1_buf0 ; ACC <= sort_buf[0] SACL temp1 ; temp1 <= sort_buf[0] LACL sort1_buf4 ; ACC <= sort_buf[4] SACL sort1_buf0 ; sort_buf[0] <= sort_buf[4] LACL temp1 ; ACC <= old sort_buf[0] SACL sort1_buf4 ; sort_buf[4] <= old sort_buf[0] * B. Find the second largest sample, put into sort_buf[1] NEXT1_B2: ; comparing sort_buf[1] with sort_buf[2] LACL sort1_buf1 ; ACC <= sort_buf[1] SUB sort1_buf2 ; compare with sort_buf[2] BCND NEXT1_B3,GEQ ; branch to NEXT1_B3 if sort_buf[1] is larger * Swap sort_buf[1] with sort_buf[2] if sort_buf[2] is larger LACL sort1_buf1 ; ACC <= sort_buf[1] SACL temp1 ; temp1 <= sort_buf[1] LACL sort1_buf2 ; ACC <= sort_buf[2] SACL sort1_buf1 ; sort_buf[1] <= sort_buf[2] LACL temp1 ; ACC <= old sort_buf[1] SACL sort1_buf2 ; sort_buf[2] <= old sort_buf[1] NEXT1_B3: ; comparing sort_buf[1] with sort_buf[3] LACL sort1_buf1 ; ACC <= sort_buf[1] SUB sort1_buf3 ; compare with sort_buf[3] BCND NEXT1_B4,GEQ ; branch to NEXT1_B4 if sort_buf[1] is larger * Swap sort_buf[1] with sort_buf[3] if sort_buf[3] is larger LACL sort1_buf1 ; ACC <= sort_buf[1] SACL temp1 ; temp1 <= sort_buf[1] LACL sort1_buf3 ; ACC <= sort_buf[3] SACL sort1_buf1 ; sort_buf[1] <= sort_buf[3] LACL temp1 ; ACC <= old sort_buf[1] SACL sort1_buf3 ; sort_buf[3] <= old sort_buf[1] NEXT1_B4: ; comparing sort_buf[1] with sort_buf[4] LACL sort1_buf1 ; ACC <= sort_buf[1] SUB sort1_buf4 ; compare with sort_buf[4] BCND NEXT1_C3,GEQ ; branch to NEXT1_C3 if sort_buf[1] is larger * Swap sort_buf[1] with sort_buf[4] if sort_buf[4] is larger LACL sort1_buf1 ; ACC <= sort_buf[1] SACL temp1 ; temp1 <= sort_buf[1] LACL sort1_buf4 ; ACC <= sort_buf[4] SACL sort1_buf1 ; sort_buf[1] <= sort_buf[4] LACL temp1 ; ACC <= old sort_buf[1] SACL sort1_buf4 ; sort_buf[4] <= old sort_buf[1] * C. Find the median sample, put into sort_buf[2] NEXT1_C3: ; comparing sort_buf[2] with sort_buf[3] LACL sort1_buf2 ; ACC <= sort_buf[2] SUB sort1_buf3 ; compare with sort_buf[3] BCND NEXT1_C4,GEQ ; branch to NEXT1_C4 if sort_buf[2] is larger * Swap sort_buf[2] with sort_buf[3] if sort_buf[3] is larger LACL sort1_buf2 ; ACC <= sort_buf[2] SACL temp1 ; temp1 <= sort_buf[2] LACL sort1_buf3 ; ACC <= sort_buf[3] SACL sort1_buf2 ; sort_buf[1] <= sort_buf[3] LACL temp1 ; ACC <= old sort_buf[2] SACL sort1_buf3 ; sort_buf[3] <= old sort_buf[2] NEXT1_C4: ; comparing sort_buf[2] with sort_buf[4] LACL sort1_buf2 ; ACC <= sort_buf[2] SUB sort1_buf4 ; compare with sort_buf[4] BCND LPF,GEQ ; branch to LPF if sort_buf[2] is larger * sort_buf[2] <= sort_buf[4] if sort_buf[4] is larger LACL sort1_buf4 ; ACC <= sort_buf[4] SACL sort1_buf2 ; sort_buf[2] <= sort_buf[4] * * Sorting is completed, sort_buf[2] contains median filter output e'(n) * * LPF is implemented as 1st-order IIR filter approximation of * moving-average filter of order L * * e'(n) ------- c(n) * ------>| LPF |-----> * ------- * * c(n) = (1-a)*c(n-1) + a*e'(n) * = c(n-1) - a*c(n-1) + a*e'(n) * where * a = 1/L. Assume L = 2^m, * a = 2^-m, which can be implemented by shifting right m bits * Assume L = 256 is used for LPF here LPF: LACC cn_high,16 ; ACCH <= c(n-1), high word ADDS cn_low ; ACC = c(n-1) SUB cn_high,m8 ; - a*c(n-1) ADD sort1_buf2,m8 ; + a*e'(n) SACH cn_high ; save c(n), high word SACL cn_low ; save c(n), low word LAR AR7,#cn ; AR7 -> address of c(n) at page 0 ADD #1,15 ; rounding SACH * ; move to page 0, AR7 -> address of c(n) MAR *,AR3 ; ARP = 3 SACH *+ ; save c(n) to memory locations ; pointed by AR3, cn_out * Following code is used for CCS simulation only LACL loop_cnt ; ACC <= loop counter ADD ONE ; ACC + 1 SACL loop_cnt ; loop counter incremented by 1 OUTPUT: LACL loop_cnt ; ACC <= loop counter SUB #file_length ; end of loop? BCND FOREVER,EQ ; end of loop, branch to FOREVER B MAIN ; branch to MAIN for next iteration FOREVER: B FOREVER ; end of loop, loop forever here .end