Scenix Fft FFT2SINE.SRC

	device	pins28,pages4,banks8,turbo,oschs,optionx,carryx,stackx
	reset	start		


;		A Cooley-Tukey Radix-2 DIF FFT
;
;	Radix-2 implementation
;	Decimation In Frequency
;	Single Butterfly
;	Table Lookup of Twiddle Factors 
;	Complex Input & Complex Output
;
;	All data is assumed to be 16 bits and the intermediate
;	results are stored in 32 bits
;
;	Length Of FFT must be a Power Of 2
;
;
;

;
FftLen	=	16	; FFT Length
Power	=	4	; (2**Power = FftLen)

SCALE_BUTTERFLY	=	1		; intermediate scaling performed

RamAddr	= $90	
Data	= RamAddr


;
;*************************************************************************
;
BB0	=	0
B0	=	BB0
BB1	=	1
B1	=	BB1
BB2	=	2
B2	=	BB2
BB3	=	3	; RAM offset constants
B3	=	BB3
	
	; for 16 bit x 16 bit multiplication
	org	$50
arith	=	$

md16		ds	2
mr16		ds	2
upper_prdt	ds	2
count		ds	1
sign		ds	1


AARG	=	mr16
AARG1	=	mr16+1	; 16 bit multiplier A

BARG	=	md16
BARG1	=	md16+1	; 16 bit multiplicand B

DPX	=	mr16		
DPX1	=	mr16+1
DPX2	=	upper_prdt
DPX3    =	upper_prdt+1	; 32 bit multiplier result = A*B
	
	
ACC		ds	1
ACC1		ds	1
ACC2		ds	1
ACC3		ds	1	; 32 bit accumulator for computations

	org	$8			;=16
primary		=	$
Cos		ds	1
write_ptr	=	Cos		; reuse
Cos1		ds	1
read_ptr	=	Cos1		; reuse		
			;=24
Sin		ds	1
rw_cnt		=	Sin

Sin1		ds	1
rw_temp		=	Sin1	

testCount	ds	1
temp1		=	testCount

ptr		ds	1
pulseCount	=	ptr
temp2		=	ptr

temp3		ds	1
temp4		ds	1


	org	$10
Xi		ds	1
Xi1		ds	1

Yi		ds	1
Yi1		ds	1

Xl		ds	1
Xl1		ds	1

Yl		ds	1
Yl1		ds	1
				;=41
Xt		ds	1
Xt1		ds	1

Yt		ds	1
Yt1		ds	1

	org	$30				;=45
var		=	$
VarIloop	ds	1

VarJloop	ds	1
				;=30
VarKloop	ds	1

VarL		ds	1

temp		ds	1
TF_Addr		ds	1
TF_Addr1	ds	1
TF_Offset	ds	1		;twiddle factor offset


count1		ds	1	; N1
count2		ds	1	; N2
top		ds	1
bottom		ds	1

				;=54 bytes 
	
;
;

MOVK16  MACRO   2

	mov	\2+BB0,#(\1) & $ff
	mov     \2+BB1,#((\1) >> 8)
	
	ENDM

MOV16 	MACRO     2			; 16 bit move

	mov	\2+B0,\1+B0
	mov	\2+B1,\1+B1
		
	ENDM

MOV32	MACRO   2

	mov	\2+B0,\1+B0               ; move A(B0) to B(B0)
	mov	\2+B1,\1+B1               ; move A(B1) to B(B1)
	mov	\2+B2,\1+B2               ; move A(B2) to B(B2)
	mov	\2+B3,\1+B3               ; move A(B3) to B(B3)

	ENDM

ADD16 	MACRO     2
	clc
	add	\2+B0,\1+B0	
	add	\2+B1,\1+B1        

	ENDM

ADD32 	MACRO     2

	clc
	add	\2+B0,\1+B0
	add	\2+B1,\1+B1
	add	\2+B2,\1+B2
	add	\2+B3,\1+B3
	           
	ENDM

SUB16 	MACRO     2		; 16 bit subtraction, result in second parameter

	stc
	sub	\2+B0,\1+B0
	sub	\2+B1,\1+B1

	ENDM



SUB16ACC MACRO  3		; 16 bit subtraction, result in the third parameter
				; \3=\2-\1
	stc
	mov	w,\1+B0	        ; get lowest byte of a into w
	mov	w,\2+B0-w	
	mov     \3+B0,w

	mov	w,\1+B1		; get 2nd byte of a into w
	mov	w,\2+B1-w
	mov	\3+B1,w
	
	ENDM

SUB32 	MACRO     2		; 32 bit subtraction

	stc
	sub	\2+B0,\1+B0
	sub	\2+B1,\1+B1
	sub	\2+B2,\1+B2
	sub	\2+B3,\1+B3

	ENDM

CLR16 	MACRO  1		;  Clear 2 consecutive bytes of data memory
	clr	\1+B0
	clr	\1+B1
	
	ENDM

INC16 	MACRO     1
	
	inc	\1+B0
	snz
	inc	\1+B1
	

	ENDM

DEC16 	MACRO     1		; decrement 16 bit register
	stc
	sub     \1+B0,#1
	clr	w
	sub 	\1+B1,w

	ENDM

RLC16 	MACRO     1		; 16 bit rotate left

	clc
	rl	\1+B0
	rl	\1+B1
	
	ENDM

RLC16AB	MACRO	2		;a,b	16 bit rotate left A into B (=2*a)

	clc
	
	mov	w,<<\1+BB0
	mov	\2+BB0,w
	mov	w,<<\1+BB1	
	mov	\2+BB1,w

	ENDM

RRC16 	MACRO     1		; 16 bit signed rotate right (=/2)

	mov	w,<<\1+B1	; move sign into carry bit
	rr	\1+B1		; rotate MSByte first
	rr	\1+B0		; then LSByte to propagate carry
	
	ENDM

TFSZ16 MACRO    1		; 16 bit test, skip if zero

	mov    	w,\1+B0
	or	w,\1+B1
	sz
	
	ENDM



; get test data
gen_test	MACRO
;*******************************************************************
;		Test Routine For FFT
; read table of test data
;*******************************************************************
testFft
	mov	temp1,#testdata//256	; low
	mov	temp2,#testdata/256	; high
	mov	fsr,#RamAddr		; load start address	
	mov	rw_cnt,#Fftlen		; how many times to write
loaddata
	mov	m,temp2
	mov	w,temp1
	iread				; read data
	mov	ind,w			; store lower byte of real data
	inc	temp1			; next
	mov	m,temp2
	mov	w,temp1
	iread				; read data
	inc	fsr			
	mov	ind,w			; store upper byte of real data
	clr	w			; imaginary data is always 0
	inc	fsr
	mov	ind,w
	inc	fsr
	mov	ind,w

	inc	temp1
	inc	fsr
	setb	fsr.4
	decsz	rw_cnt
	jmp	loaddata

	ENDM

	
;******************************************************************
;		Test Program For FFT Subroutine
;******************************************************************

	ORG	0
start
	clr	fsr		; reset all ram banks
:loop	setb	fsr.4		; only second half is addressable
	clr	ind		; clear
			
	ijnz	fsr,:loop	; all
	
	; fsr=0 on entry

	gen_test		; Generate Test Vector Data

	call	R2FFT		; Compute Fourier Transform
	page	Unscramble
	call	Unscramble	; bit reverse the scrambled data

; Fourier Transform Completed
;
	page	start
self	jmp	self
;----------------------------------------------------
	
	; 16 bit x 16 bit signed multiplication 
	; entry: multiplicand in $09,08, multiplier at $0b,$0a
	; exit : 32 bit product at $0d,$0c,$b,$a
						; cycles=19+258+16=293
DblMult
	; process the sign first
						
	mov	w,md16+1		; 1	test the sign
	xor	w,mr16+1		; 1	same sign?		
	mov	sign,w			; 1	save, so that we can bit test it at the end
	sb	md16+1.7		; 1/2	msb=1?
	jmp	check_mr		; 3	no, check multiplier
						; =7
	; msb of multiplicand=1, negate
	not	md16			; 1	1's complement
	inc	md16			; 1	2's complement=1's complement+1
	snz				; 1/2	special case if =0
	dec	md16+1			; 1	dec then complement = increment
	not	md16+1			; 1	this will take care of the carry from lsb to msb
					;=5

check_mr
	sb	mr16+1.7		; 1/2	msb=1?
	jmp	normal_mult		; 3	sign check done
		
	; msb of multiplier=1, negate
	not	mr16			; 1	1's complement
	inc	mr16			; 1	2's complement=1's complement+1
	snz				; 1/2	special case if =0
	dec	mr16+1			; 1	dec then complement = increment
	not	mr16+1			; 1	this will take care of the carry from lsb to msb
					;=7
					; worst case = 19
	; following routine has a worst case of 261-3 (no ret)= 258 cycles
normal_mult	
	mov	count,#17		; 2	set number of times to shift
	clr	upper_prdt		; 1	clear upper product
	clr	upper_prdt+1		; 1	higher byte of the 16 bit upeper product
	clc				; 1	clear carry
						; the following are executed [count] times
m1616loop
	rr	upper_prdt+1		; 1	rotate right the whole product
	rr	upper_prdt		; 1 	lower byte of the 16 bit upper product
	rr	mr16+1			; 1	high byte of the multiplier
	rr	mr16			; 1	check lsb
	sc				; 1	skip addition if no carry
	jmp	no_add			; 3     no addition since lsb=0
	clc				; 1	clear carry
	add	upper_prdt,md16		; 1	add multiplicand to upper product
	add	upper_prdt+1,md16+1	; 1	add the next 16 bit of multiplicand
no_add
	decsz	count			; 1/2	loop [count] times to get proper product
	jmp	m1616loop		; 3	jmp to rotate the next half of product

					; following instructions have a total of 16 cycles
	sb	sign.7			; 1/2	check sign
	ret				; 3	positive, do nothing, just return
	; lower product
	not	mr16			; 1 form 1's complement
	not	mr16+1			; 1
	not	upper_prdt		; 1
	not	upper_prdt+1		; 1

	mov	w,#1			; 1	add 1
	clc				; 1	to form 2's complement
	add	mr16,w			; 1	
	mov	w,#0			; 1 	add with 0 to propagate
	add	mr16+1,w		; 1	carry to higher bytes
	add	upper_prdt,w		; 1
	add	upper_prdt+1,w		; 1
	ret				; 3



;			RADIX-2 FFT
;
;	Decimation In Frequency 
;
; Input Data should be unscrambled
; Output Data at the end is in scrambled form
;

R2FFT
	bank	var
	mov	TF_Offset,#1		; Init TF_Offset = 1
	
	mov	count2,#FftLen		; count2 = N=16
	mov 	VarKloop,#Power		; Kloop
Kloop					; for K = 1 to Power-1
	mov	count1,count2		; count1 = count2

	clc
	rr	count2			; count2 = count2/2
	clr	VarJloop		; J = 0
	
	clr	TF_Addr
	clr	TF_Addr1

	

Jloop
;
; Read Twiddle factors from Sine/Cosine Table from Prog Mem
;
	mov	temp1,#SineTable//256
	mov	temp2,#SineTable/256	; load sine table address to table pointers
	mov	w,TF_Addr
	clc
	add	temp1,w
	mov	w,TF_Addr1
	add	temp2,w
	
	mov	m,temp2			; load m first, since w will be used in doing it
	mov	w,temp1

	iread	
	mov	Sin,w			; get the sine value (low byte)

	inc	temp1			; no need to propagate carry since sine table is aligned to
					; X00
	
	; both m and w are altered by iread, thus the reload

	mov	m,temp2		; m should be loaded first
	mov	w,temp1		

	iread
	mov	Sin+BB1,w			; Read MSB of Sine Value from lookup table

	clc
	add	temp1,#(Fftlen/4)*2-1		; prepare to read cosine table, *2 because each
						; entry occupies two bytes, -1 because we have 
						; incremented once to read the high byte of sine
	
	mov	m,temp2
	mov	w,temp1
	
	iread
	
	mov	Cos,w

	inc	temp1
	; both m and w are altered by iread, thus the reload

	mov	m,temp2
	mov	w,temp1

	iread
	mov	Cos+BB1,w			; Read MSB of cosine Value from lookup table

	clc
	mov	w,<<TF_Offset
	add	TF_Addr,w
	add	TF_Addr1,#0		; propagate carry
	
	
	clc
	mov	w,<<VarJloop
	mov	VarIloop,w		; I = J*2 since Real followed by Imag Data
Iloop
	;compute for pointer address that can be used throughout the loop
	clc
	mov	w,<<VarIloop	
	mov	temp1,w
	and	w,#%11110000		; mask off lsb
	; no overflow can happen, else we will be out of range of memory
	add	w,temp1			; adjust for gap in register banks
	mov	top,w
	add	top,#RamAddr

	mov	w,<<count2
	mov	VarL,w			; VarL = count2*2
	add	VarL,VarIloop		; VarL = I+count2*2

	
	mov	w,<<VarL	
	mov	temp1,w
	and	w,#%11110000		; mask off lsb
	add	w,temp1			; adjust for gap in register banks
	mov	bottom,w
	add	bottom,#RamAddr
	
	mov	fsr,bottom		; read bottom data on the butterfly
	mov	temp1,ind
	inc	fsr
	mov	temp2,ind
	inc	fsr
	mov	temp3,ind
	inc	fsr
	mov	temp4,ind

	bank	primary

	mov	Xl,temp1
	mov	Xl+1,temp2
	mov	Yl,temp3
	mov	Yl+1,temp4

	; read top data on the butterfly
	bank	var
	mov	fsr,top

	mov	temp1,ind
	inc	fsr
	mov	temp2,ind
	inc	fsr
	mov	temp3,ind
	inc	fsr
	mov	temp4,ind

	bank	primary

	mov	Xi,temp1
	mov	Xi+1,temp2
	mov	Yi,temp3
	mov	Yi+1,temp4


; Real & Imag Data is fetched
; Compute Butterfly 

	SUB16ACC	Xl,Xi,Xt	; Xt = Xi - Xl
	ADD16		Xl,Xi		; Xi = Xi + Xl
	SUB16ACC	Yl,Yi,Yt	; Yt = Yi - Yl
	ADD16		Yl,Yi		; Yi = Yi + Yl

IF SCALE_BUTTERFLY
		RRC16	Xi
		RRC16	Yi
		RRC16	Xt
		RRC16	Yt
ENDIF
	
	bank	arith
	mov	mr16,Sin
	mov	mr16+1,Sin1		; multiplier=Cos value

	bank 	primary
	mov	w,Xt
	bank	arith
	mov	md16,w

	bank	primary
	mov	w,Xt1
	bank	arith
	mov	md16+1,w


	Call	DblMult		; SIN*Xt

	MOV32	DPX,ACC

	mov	md16,Cos
	mov	md16+1,Cos1	; put COS in multiplicand since we want to preserve it

	bank	primary
	mov	w,Yt
	bank	arith
	mov	mr16,w

	bank	primary
	mov	w,Yt1
	bank	arith
	mov	mr16+1,w

	Call	DblMult		; COS*Yt, Scale if necessary

	SUB32	ACC,DPX			
	
	clc
	mov	w,<<DPX+2		; scale decimal point
	bank	primary
	mov	Yl,w
	bank	arith
	mov	w,<<DPX+3
	bank	primary
	mov	Yl+1,w		; Yl = COS*Yt - SIN*Xt


	mov	w,Xt
	bank	arith
	mov	mr16,w
	bank	primary
	mov	w,Xt+1
	bank	arith
	mov	mr16+1,w

	mov	md16,Cos
	mov	md16+1,Cos1	; put COS in multiplicand since we want to preserve it

	Call	DblMult		; COS*Xt
	MOV32	DPX,ACC

	bank	arith
	mov	mr16,Sin
	mov	mr16+1,Sin1		; multiplier=Sin value

	bank 	primary
	mov	w,Yt
	bank	arith
	mov	md16,w

	bank	primary
	mov	w,Yt1
	bank	arith
	mov	md16+1,w

	Call	DblMult		; Sin*Yt, Scale if necessary

	ADD32	ACC,DPX		; DPX = COS*Xt + SIN*Yt

	clc
	mov	w,<<DPX+2		;scale decimal point
	bank	primary
	mov	Xl,w
	bank	arith
	mov	w,<<DPX+3
	bank	primary
	mov	Xl+1,w		; Xl = COS*Xt + SIN*Yt

	mov	temp1,Xi
	mov	temp2,Xi+1
	mov	temp3,Yi
	mov	temp4,Yi+1	; prepare to store
;
;
; Store results of butterfly
; store top result
	bank	var
	mov	fsr,top

	mov	ind,temp1
	inc	fsr
	mov	ind,temp2
	inc	fsr
	mov	ind,temp3
	inc	fsr
	mov	ind,temp4
	
	bank	primary
	; prepare to store data (L)
	mov	temp1,Xl
	mov	temp2,Xl+1
	mov	temp3,Yl
	mov	temp4,Yl+1	; prepare to store

	
	; store bottom result
	bank	var
	mov	fsr,bottom

	mov	ind,temp1
	inc	fsr
	mov	ind,temp2
	inc	fsr
	mov	ind,temp3
	inc	fsr
	mov	ind,temp4		; X(L) & Y(L) stored
;
; Increment for next Iloop
;
	bank	var
	clc
	mov	w,<<count1
	add	VarIloop,w		; I=I+count1*2

	mov	temp,#FftLen*2
	clc
	sub	temp,VarIloop		; temp = 2*FftLen - I-1
	
	sb	temp.7		
	jmp	Iloop			; while I < 2*FftLen
;
; I Loop end
;
; increment for next J Loop
;

	inc	VarJloop		; J = J + 1

	mov	temp,count2
	clc
	sub	temp,VarJloop		; temp = count2 - J-1
	
	sb	temp.7		
	jmp	Jloop			; while J < count2
;
; J Loop end
;
; increment for next K Loop
;
	clc
	rl	TF_Offset		; TF_Offset = 2 * TF_Offset
	decsz	VarKloop
	jmp	Kloop			; while K < Power
;
	ret				; FFT complete
;
; K Loop End
; FFT Computation Over with data scrambled
; Descramble the data using "Unscramble" Routine
		org	$200

;
;******************************************************************
;	Unscramble Data Order Sequence
;	bit reversal
;******************************************************************
;******************************************************************
;	
;	Unscramble Data Order Sequence Of Radix-2 FFT
;	Length (must be a power of 2) 
;
;******************************************************************

Unscramble
	clr	VarIloop		; i=0..15
reverse
	clr	VarL
	snb	VarIloop.3
	setb	VarL.0
	snb	VarIloop.2
	setb	VarL.1
	snb	VarIloop.1
	setb	VarL.2
	snb	VarIloop.0
	setb	VarL.3
	; L contains the bit reversed version of I
	
	stc
	cjb	VarIloop,VarL,swapdata	; I-L
					; swap only if I<L
					; carry=1 if no borrow, meaning I is >=L
rev_cont
	
	inc	VarIloop
	stc
	cjb	VarIloop,#12,reverse	; if I <=11, continue, else done
	ret
swapdata
	clc
	mov	temp,VarIloop
	rl	temp
	rl	temp
	mov	w,temp
	and	w,#%11110000		; mask off lsb
	; no overflow can happen, else we will be out of range of memory
	add	w,temp			; adjust for gap in register banks
	mov	top,w
	add	top,#RamAddr

	clc
	mov	temp,VarL
	rl	temp
	rl	temp
	mov	w,temp
	and	w,#%11110000		; mask off lsb
	; no overflow can happen, else we will be out of range of memory
	add	w,temp			; adjust for gap in register banks
	mov	bottom,w
	add	bottom,#RamAddr

;
; swap data
	mov	fsr,top
	mov	temp1,ind
	inc	fsr
	mov	temp2,ind
	inc	fsr
	mov	temp3,ind
	inc	fsr
	mov	temp4,ind

	bank	var
	mov	fsr,bottom
	mov	Cos,ind
	inc	fsr
	mov	Cos1,ind
	inc	fsr
	mov	Sin,ind
	inc	fsr
	mov	Sin1,ind
	
	bank	var
	mov	fsr,top
	mov	ind,Cos
	inc	fsr
	mov	ind,Cos1
	inc	fsr
	mov	ind,Sin
	inc	fsr
	mov	ind,Sin1

	bank	var
	mov	fsr,bottom
	mov	ind,temp1
	inc	fsr
	mov	ind,temp2
	inc	fsr
	mov	ind,temp3
	inc	fsr
	mov	ind,temp4

	bank	var
	jmp	rev_cont


	org	$300
;
;*****************************************************************
;		Sine-Cosine Tables
;*****************************************************************
;
;
;*****************************************************************
;	FFT Input/Output Data Stored  with 2 bytes of
; Real Data followed by 2 bytes of Imaginary Data.
;*****************************************************************
;
;		16 Point FFT Sine Table
; coefficient table (size of table is 3n/4).
SineTable
				;	radians		0.392699082		
				;		degrees	sin	16 bit form	pt.
dw	0	//256		;	0	0	0	0	0
dw	0	/256		;	0	0	0	0	0
dw	12539	//256		;	0.392699082	22.5	0.382683432	12539	1
dw	12539	/256		;	0.392699082	22.5	0.382683432	12539	1
dw	23170	//256		;	0.785398163	45	0.707106781	23170	2
dw	23170	/256		;	0.785398163	45	0.707106781	23170	2
dw	30273	//256		;	1.178097245	67.5	0.923879533	30273	3
dw	30273	/256		;	1.178097245	67.5	0.923879533	30273	3

CosTable

dw	32767	//256		;	1.570796327	90	1	32767	4
dw	32767	/256		;	1.570796327	90	1	32767	4
dw	30273	//256		;	1.963495408	112.5	0.923879533	30273	5
dw	30273	/256		;	1.963495408	112.5	0.923879533	30273	5
dw	23170	//256		;	2.35619449	135	0.707106781	23170	6
dw	23170	/256		;	2.35619449	135	0.707106781	23170	6
dw	12539	//256		;	2.748893572	157.5	0.382683432	12539	7
dw	12539	/256		;	2.748893572	157.5	0.382683432	12539	7
dw	0	//256		;	3.141592654	180	1.22515E-16	0	8
dw	0	/256		;	3.141592654	180	1.22515E-16	0	8
dw	-12539	//256		;	3.534291735	202.5	-0.382683432	-12539	9
dw	-12539	/256	-1	;	3.534291735	202.5	-0.382683432	-12539	9
dw	-23170	//256		;	3.926990817	225	-0.707106781	-23170	10
dw	-23170	/256	-1	;	3.926990817	225	-0.707106781	-23170	10
dw	-30273	//256		;	4.319689899	247.5	-0.923879533	-30273	11
dw	-30273	/256	-1	;	4.319689899	247.5	-0.923879533	-30273	11
dw	-32767	//256		;	4.71238898	270	-1	-32767	12
dw	-32767	/256	-1	;	4.71238898	270	-1	-32767	12
									
testdata
dw	0
dw	0
dw	$40
dw	$2d
dw	$ff
dw	$3f
dw	$40
dw	$2d
dw	0
dw	0
dw	$c0
dw	$d2
dw	1
dw	$c0
dw	$c0
dw	$d2
dw	0
dw	0
dw	$40
dw	$2d
dw	$ff
dw	$3f
dw	$40
dw	$2d
dw	0
dw	0
dw	$c0
dw	$d2
dw	1
dw	$c0
dw	$c0
dw	$d2