;-----------------------------------------------------------------------------;
; Audio Waveform Monitor R0.1                               Nov 11, 2004      ;
; (C)ChaN, 2004; http://elm-chan.org/                                         ;
;-----------------------------------------------------------------------------;
;
.include "m8def.inc"
.include "avr.inc"
.include "akiglcd.inc"

.equ	Sync	= 2		;Port D bit definitions

.equ	FFT_N	= 128	;Number of samples
.equ	LCD_H	= 16	;/

.def	_0	= r15		;Zero
.def	_Flags	= r25	;b0:In captureing
						;b1:In pause
						;b2:MOSI edge detector
						;b3:Low/High Freq
						;b4:ADC Divider (für 5,5kHz Samplerate)
						;b5:WDR Flag 0
						;b6:WDR Flag 1

.equ WDRF0=5
.equ WDRF1=6

;----------------------------------------------------------;
; Data memory area

.dseg
	.org	RAMTOP
CaptBuf:.byte	FFT_N*2		;Sampling buffer
BflyBuf:.byte	FFT_N*4		;Butterfly operation table, Wave form buffer
LvlBuf:	.byte	FFT_N/2		;Spectrum bar length, High Freq
LvlBuf2:.byte	FFT_N/2		;Spectrum bar length, Low Freq
LvlBuf3:.byte	FFT_N/2		;Spectrum bar length, Gesamt

AdPtr:	.byte	1		;Sampling pointer
LcdDiv:	.byte	1		;Divider for LCD drive


;----------------------------------------------------------;
; Program code area

.cseg
	rjmp	reset		; RESET
	rjmp	0		; INT0
	rjmp	0		; INT1
	rjmp	0		; TC2 COMP
	rjmp	0		; TC2 OVF
	rjmp	0		; TC1 CAPT
	rjmp	0		; TC1 COMPA
	rjmp	0		; TC1 COMPB
	rjmp	0		; TC1 OVF
	rjmp	0		; TC0 OVF
	rjmp	0		; SPI
	rjmp	0		; USART RXC
	rjmp	0		; USART UDRE
	rjmp	0		; USART TXC
;	rjmp	isr_adc		; ADC
;	rjmp	0		; EE_RDY
;	rjmp	0		; ANA_COMP
;	rjmp	0		; TWI
;	rjmp	0		; SPM_RDY


;----------------------------------------------------------;
; ADC interrupt (9.6ksps)


isr_adc:
	push	AL
	in	AL, SREG
	pushw	A
	pushw	Y
	sbrs	_Flags, 0	;Skip if in Idle
	rjmp	adc_skip	;/
	sbrs	_Flags, 3
	rjmp nodiv
	sbrs	_Flags, 4
	rjmp setd
	cbr 	_Flags, (1<<4)
	rjmp adc_skip
setd:
	sbr		_Flags, (1<<4)
nodiv:
	lds	YL, AdPtr	;Write pointer
	clr	YH		;
	addiw	Y, CaptBuf	;/
	inw	A, ADC		;Store A/D value
	stw	Y+, A		;/
	lds	AL, AdPtr	;Next pointer
	addi	AL, 2		;
	sts	AdPtr, AL	;/
	brne	PC+2		;Terminate if 128 samples captured.
	cbr	_Flags, bit0	;/
adc_skip:

	popw	Y
	popw	A
	out	SREG, AL
	pop	AL
	reti

;----------------------------------------------------------;
; Initialize peripherals (ATmega8 @ 18,432MHz)

reset:
	clr	_0			;Zero reg.
	ldiw	Z, RAMTOP		;Clear all variables
	ldiw	X, RAMEND-RAMTOP+1	;
	st	Z+, _0			;
	sbiw	XL, 1			;
	brne	PC-2			;/
	sbiw	ZL, 1			;SP �‰Šú‰»
	outw	SP, Z			;/

	outi	PORTB, 0b00101000	;Port B �‰Šú‰»
	outi	DDRB,  0b00010110

	outi	DDRC,  0

	outi	PORTD, 0b00000100	;Port D
	outi	DDRD,  0b00000100	;Port D

	; Start TC1 with 444kHz on OC1B pin (for MAX293 clock)
	ldiw	A, 36-1			;TOP value
	outw	ICR1, A			;/
	ldiw	A, 36/2			;half value
	outw	OCR1B, A		;/
	outi	TCCR1A, 0b00100010	;COM1B[1:0]=10, WGM1[1:0]=10
	outi	TCCR1B, 0b00011001	;WGM1[3:2]=11, CS1[2:0]=001

	; Start A/D with ch0, free running, 44ksps
	outi	ADMUX, 0b00000000	;MUX[3:0]=0000
	outi	ADCSR, 0b11101111 	;ADEN=1,ADSC=1,ADFR=1,ADIE=1,ADPS[2:0]=110

	outi 	ucsrb, 8
	outi 	ubrrh, 0
	outi 	ubrrl, 9

	in r16, WDTCR
  	ori r16, (1<<WDCE) | (1<<WDE)
  	out WDTCR, r16
  	ldi r16, (1<<WDE) | (1<<WDP2)
  	out WDTCR, r16

	clr	_Flags
	wdr
	sei

	sbr	_Flags, bit0	;Start wave form captureing


;----------------------------------------------------------;
; Main


main:
	sbrc	_Flags, 0	;Wait for end of wave captureing
	rjmp	main		;/
	
	sbrc _Flags, 3
	rjmp LoF
	outi	ADCSR, 0b11101000 + 7	;ADEN=1,ADSC=1,ADFR=1,ADIE=1,ADPS[2:0]=111, 11kHz Samplerate
	outi	ADMUX, 0b00000001	;MUX[3:0]=0001
	sbr _Flags, (1<<3)
	rjmp Hif
Lof:
	outi	ADCSR, 0b11101000 + 5	;ADEN=1,ADSC=1,ADFR=1,ADIE=1,ADPS[2:0]=101, 44khz Samplerate
	outi	ADMUX, 0b00000000	;MUX[3:0]=0000
	cbr _Flags, (1<<3)
Hif:
	 rcall	do_window			;Fill butterfly table			[340us]
   	 sbr	_Flags, bit0		;Restart captureing for next
	 rcall	do_fft				;Butterfly operations			[4.4ms]
	
	sbrc _Flags, 3				;Gesetzt: jetzt High Freq verarbeiten
	rjmp Lof2
	rcall	make_barsl			;Get scalar values, update spectrum	[3.3ms]
	rjmp Hif2
Lof2:
	rcall	make_barsh			;Get scalar values, update spectrum	[3.3ms]
	rcall	rfsh_bars			;Display spectrum bars into LCD		[660us]
Hif2:

hier:
	rjmp	main				; (approx. 30 cycles/sec)


;----------------------------------------------------------;
; Spectrum analyzer
;----------------------------------------------------------;
; 16bit fixed point FFT performance with megaAVR @16MHz
;
;  Points:  Input,    FFT, Output,   Total: Throughput
;   64pts:  .17ms,  1.9ms,  1.4ms,   3.5ms:   18.3kpps   (expected)
;  128pts:  .34ms,  4.4ms,  2.6ms,   7.3ms:   17.5kpps   (measured)
;  256pts:  .68ms, 10.1ms,  5.2ms,  16.0ms:   16.0kpps   (expected)
;  512pts:  1.4ms, 22.6ms, 10.4ms,  34.4ms:   14.8kpps   (expected)
;
; Input: Input waveform into butterfly table with applying window
; FFT: Execute butterfly operations
; Output: Descramble and output the spectrum as scalar values


do_window: ; Fill butterfly table with applying window function
	ldiw	X, CaptBuf	;Source
	ldiw	Y, BflyBuf	;Destination
	ldiw	Z, t_hamming128*2	;Window table
	ldi	CL, FFT_N
ms_l1:	lpmw	A, Z+		;Get a window attenuation ratio
	ldw	B, X+		;Get an input value
	subiw	B, 512		;unsigned -> signed
	FMULS16	A, B, T4, T2	;Apply window
	stw	Y+, T4		;Store real
	st	Y+, _0		;Clear image
	st	Y+, _0		;/
	dec	CL
	brne	ms_l1
	ret


do_fft:	; Execute butterfly operations (not optimized)
	ldi	XH, 1		;u
	ldi	EL, FFT_N/2	;l
df_l1:	ldiw	Z, BflyBuf	;Z = BflyBuf
	ldiw	Y, BflyBuf	;Y = BflyBuf + l * 4
	muli	EL, 4		;
	addw	Y, T0		;/
	mul	AL, XH		;T10L = u * 4
	mov	T10L, T0L	;/
	mov	XL, XH		;w = u
df_l2:	clr	T14L		;p=0
df_l3:	lddw	A, Z+0		;A = [Z+0] - [Y+0], [Z+0] += [Y+0]
	movw	CL, AL		;B = [Z+2] - [Y+2], [Z+2] += [Y+2]
	lddw	D, Y+0		;
	subw	A, D		;
	addw	C, D		;
	stw	Z+, C		;
	lddw	B, Z+0		;
	movw	CL, BL		;
	lddw	D, Y+2		;
	subw	B, D		;
	addw	C, D		;
	stw	Z+, C		;/
	movw	T0L, ZL
	movw	ZL, T14L	;C=cos(p), D=sin(p)
	addiw	Z, t_cos_sin128*2	;
	lpmw	C, Z+		;
	lpmw	D, Z+		;/
	movw	ZL, T0L
	FMULS16	A, C, T4, T2	;[Y+0] = A * C + B * D
	FMULS16	B, D, T8, T6	;[Y+2] = B * C - A * D
	addw	T2, T6		;
	adcw	T4, T8		;
	stw	Y+, T4		;
	FMULS16	B, C, T4, T2	;
	FMULS16	A, D, T8, T6	;
	subw	T2, T6		;
	sbcw	T4, T8		;
	stw	Y+, T4		;/
	add	T14L, T10L	;p += u
	rjne	df_l3
	muli	EL, 4		;Y += l * 4, Z += l * 4; (skip split segment)
	addw	Y, T0		;
	addw	Z, T0		;/
	dec	XL		;--w
	rjne	df_l2
	lsl	XH		;u *= 2
	lsr	EL		;l /= 2
	rjne	df_l1
	ret

make_barsl: ;Get scalar spectrum and update spectrum bar length
	ldiw	Z, t_desc128*2	;Descramble table
	ldiw	Y, LvlBuf	;Bar length buffer
	ldi	CH, FFT_N/2	;Number of elements
ub_l1:	lpmw	A, Z+		;Get a vector value (A = r, B = i)
	ldiw	X, BflyBuf	;
	addw	X, A		;
	ldw	A, X+		;
	ldw	B, X+		;/
	FMULS16	A, A, T4, T2	;A = sqrt(A * A + B * B); 0..32767 (scalar)
	FMULS16	B, B, T8, T6	;
	addw	T2, T6		;
	adcw	T4, T8		;
	SQRT32			;/
	movw	T2L, AL		; AL = sqrt(A); 0..181 (root compression)
	SQRT16			; / * remove this for linear scale
	ldi	AH, 100		;T0H = AL/3; (scaling)	(Orginal: 256/3)
	mul	AL, AH		;/
	ld	AH, Y		;Update bar length with peak holding
	subi	AH, 2		; <-peak decay rate
	brcs	PC+3		;
	cp	AH, T0H		;
	brcc	PC+2		;
	mov	AH, T0H		;
	subi AH, 1
	brcc notzero
	clr AH
notzero:
	st	Y+, AH		;/
	dec	CH
	rjne	ub_l1
	sbr _Flags, (1<<WDRF0)
	ret

make_barsh: ;Get scalar spectrum and update spectrum bar length
	ldiw	Z, t_desc128*2	;Descramble table
	ldiw	Y, LvlBuf2	;Bar length buffer
	ldi	CH, FFT_N/2	;Number of elements
ub_l1h:	lpmw	A, Z+		;Get a vector value (A = r, B = i)
	ldiw	X, BflyBuf	;
	addw	X, A		;
	ldw	A, X+		;
	ldw	B, X+		;/
	FMULS16	A, A, T4, T2	;A = sqrt(A * A + B * B); 0..32767 (scalar)
	FMULS16	B, B, T8, T6	;
	addw	T2, T6		;
	adcw	T4, T8		;
	SQRT32			;/
	movw	T2L, AL		; AL = sqrt(A); 0..181 (root compression)
	SQRT16			; / * remove this for linear scale
	ldi	AH, 100		;T0H = AL/3; (scaling)	(Orginal: 256/3)
	mul	AL, AH		;/
	ld	AH, Y		;Update bar length with peak holding
	subi	AH, 2		; <-peak decay rate
	brcs	PC+3		;
	cp	AH, T0H		;
	brcc	PC+2		;
	mov	AH, T0H		;
	subi AH, 2		;Eins mehr abziehen um die Störungen wegzubekommen
	brcc notzeroh
	clr AH
notzeroh:
	st	Y+, AH		;/
	dec	CH
	rjne	ub_l1h
	sbrc _Flags, WDRF0					;Nur wenn WRDF0 gesetzt ist, WDRF1 setzen
	sbr _Flags, (1<<WDRF1)
	cbr _Flags, (1<<WDRF0)
	ret


rfsh_bars: ;Draw spectrum bars
	cbi portd, Sync
	ldi	DL, LCD_H-7	; (This routine refreshes LCD with bar length
	nop
	nop
	nop
	nop
	nop		;Sync verlängern
	nop
	nop
	sbi portd, Sync			;component, 2nd value is fundamental freq
sendl:	
	 rcall	ds_hl			;and last value is highest freq)
	subi	DL, 8
	brcc	sendl
	sbrc _Flags, WDRF1		;Nur wenn alles durchlaufen wurde: WDR
 	wdr
	cbr _Flags, (1<<WDRF0)|(1<<WDRF1)
	ret

make2p:
	ld	AH, Y+
	cp AL, AH
	brlo tausche1
rjmp weiter
tausche1:
	mov AL, AH
rjmp weiter

make3p:
	ld	AH, Y+
	cp AL, AH
	brlo tausche3
	rjmp weitert1
tausche3:
	mov AL, AH
weitert1:			;AL=Max(1,2)
	ld	AH, Y+
	cp AL, AH
	brlo tausche4b
rjmp weiter
tausche4b:
	mov AL, AH
rjmp weiter			;AL=Max(AL,AH)=Max(1,2,3




ds_hl:	ldiw	Y, LvlBuf+1
	ldi	CL, 16		;24 Werte werden zu 16 zusammengefasst: 43-1333Hz
senl:	
	ld	AL, Y+		;1-8		8

	cpi CL, 5		;17-24		4
	brlo make3p

	cpi CL, 9		;9-16		4
	brlo make2p
weiter:	
	sub	AL, DL
	brcc	gnull	
	ldi	AH, 0	
	rjmp	inull
gnull:
	ldi	AH, 0xFF
	cpi	AL, 8
	brcc	inull
	ldi	AH, bit7
loope:	
	subi	AL, 1
	brcs	inull
	asr	AH
	rjmp	loope
inull:
	mov	AL, AH
	 rcall	lcd_put
	dec	CL
	brne	senl
	rjmp ds_hl2

make2p2:
	ld	AH, Y+
	cp AL, AH
	brlo tausche12
rjmp weiter2
tausche12:
	mov AL, AH
rjmp weiter2

make3p2:
	ld	AH, Y+
	cp AL, AH
	brlo tausche32
	rjmp weitert12
tausche32:
	mov AL, AH
weitert12:			;AL=Max(1,2)
	ld	AH, Y+
	cp AL, AH
	brlo tausche4b2
rjmp weiter2
tausche4b2:
	mov AL, AH
rjmp weiter2			;AL=Max(AL,AH)=Max(1,2,3)

make4p2:
	ld	AH, Y+
	cp AL, AH
	brlo tausche42
rjmp weitert22
tausche42:
	mov AL, AH
weitert22:			;AL=Max(1,2)
	ld	BL, Y+
	ld	BH, Y+
	cp BL, BH
	brlo tausche52
rjmp weitert32
tausche52:
	mov BL, BH
weitert32:			;BL=Max(3,4)
	cp AL, BL
	brlo tausche62
rjmp weiter2			;AL=Max(AL,BL)=Max(1,2,3,4)
tausche62:
	mov AL, BL
rjmp weiter2			;AL=Max(AL,BL)=Max(1,2,3,4)


ds_hl2:	ldiw	Y, LvlBuf2+5
	ldi	CL, 16		;59 Werte werden zu 16 zusammengefasst: 1731-20021Hz
senl2:	
	ld	AL, Y+		;1-5		5

	cpi CL, 2		;17-31		2
	brlo make8p2			

	cpi CL, 4		;17-31		2
	brlo make6p2

	cpi CL, 6		;23-22		2
	brlo make4p2

	cpi CL, 9		;17-22		3
	brlo make3p2

	cpi CL, 12		;5-10		3
	brlo make2p2
weiter2:	
	sub	AL, DL
	brcc	gnull2	
	ldi	AH, 0	
	rjmp	inull2
gnull2:
	ldi	AH, 0xFF
	cpi	AL, 8
	brcc	inull2
	ldi	AH, bit7
loope2:	
	subi	AL, 1
	brcs	inull2
	asr	AH
	rjmp	loope2
inull2:
	mov	AL, AH
	 rcall	lcd_put
	dec	CL
	brne	senl2
	ret

make6p2:
	ld	AH, Y+
	cp AL, AH
	brlo tausche72
rjmp weitert42
tausche72:
	mov AL, AH
weitert42:				;AL=Max(1,2)
	ld	BL, Y+
	ld	BH, Y+
	cp BL, BH
	brlo tausche82
rjmp weitert52
tausche82:
	mov BL, BH
weitert52:				;BL=Max(3,4)
	cp AL, BL
	brlo tausche92
rjmp weitert62			;AL=Max(AL,BL)=Max(1,2,3,4)
tausche92:
	mov AL, BL
weitert62:				;AL=Max(AL,BL)=Max(1,2,3,4)
	ld	BL, Y+
	ld	BH, Y+
	cp BL, BH
	brlo tausche102
rjmp weitert72
tausche102:
	mov BL, BH
weitert72:				;BL=Max(5,6)
	cp AL, BL
	brlo tausche112
rjmp weiter2			;AL=Max(AL,BL)=Max(1,2,3,4,5,6)
tausche112:
	mov AL, BL
rjmp weiter2			;AL=Max(AL,BL)=Max(1,2,3,4,5,6)

make8p2:
	ld	AH, Y+
	cp AL, AH
	brlo tausche122
rjmp weitert82
tausche122:
	mov AL, AH
weitert82:				;AL=Max(1,2)
	ld	BL, Y+
	ld	BH, Y+
	cp BL, BH
	brlo tausche132
rjmp weitert92
tausche132:
	mov BL, BH
weitert92:				;BL=Max(3,4)
	cp AL, BL
	brlo tausche142
rjmp weitert102			;AL=Max(AL,BL)=Max(1,2,3,4)
tausche142:
	mov AL, BL
weitert102:				;AL=Max(AL,BL)=Max(1,2,3,4)
	ld	BL, Y+
	ld	BH, Y+
	cp BL, BH
	brlo tausche152
rjmp weitert112
tausche152:
	mov BL, BH
weitert112:				;BL=Max(5,6)
	ld	AH, Y+
	ld	BH, Y+
	cp AH, BH
	brlo tausche162
rjmp weitert122			;AH=Max(AH,BH)=Max(7,8)
tausche162:
	mov AH, BH
weitert122:				;AH=Max(AH,BH)=Max(7,8)
	cp AH, BL
	brlo tausche172
rjmp weitert132			;AH=Max(AH,BL)=Max(5,6,7,8)
tausche172:
	mov AH, BL
weitert132:				;AH=Max(AH,BH)=Max(5,6,7,8)
	cp AL, AH
	brlo tausche182
rjmp weiter2			;AL=Max(AL,BL)=Max(1,2,3,4,5,6)
tausche182:
	mov AL, BH
rjmp weiter2			;AL=Max(AL,BL)=Max(1,2,3,4,5,6)



lcd_put:
	sbis ucsra, udre
	rjmp lcd_put
	out udr, AL
	inc	BL
	cpi	BL, FFT_N
	brne	exitm
	inc	BH
	clr	BL
exitm:
	ret


; These tables must be rebuilt when change FFT_N

t_cos_sin128:	; {cos(x),sin(x)} table (0 <= x < pi, in 64 steps)
	.dw	32767, 0, 32727, 1607, 32609, 3211, 32412, 4807
	.dw	32137, 6392, 31785, 7961, 31356, 9511, 30851, 11038
	.dw	30272, 12539, 29621, 14009, 28897, 15446, 28105, 16845
	.dw	27244, 18204, 26318, 19519, 25329, 20787, 24278, 22004
	.dw	23169, 23169, 22004, 24278, 20787, 25329, 19519, 26318
	.dw	18204, 27244, 16845, 28105, 15446, 28897, 14009, 29621
	.dw	12539, 30272, 11038, 30851, 9511, 31356, 7961, 31785
	.dw	6392, 32137, 4807, 32412, 3211, 32609, 1607, 32727
	.dw	0, 32767, -1607, 32727, -3211, 32609, -4807, 32412
	.dw	-6392, 32137, -7961, 31785, -9511, 31356, -11038, 30851
	.dw	-12539, 30272, -14009, 29621, -15446, 28897, -16845, 28105
	.dw	-18204, 27244, -19519, 26318, -20787, 25329, -22004, 24278
	.dw	-23169, 23169, -24278, 22005, -25329, 20787, -26318, 19519
	.dw	-27244, 18204, -28105, 16845, -28897, 15446, -29620, 14009
	.dw	-30272, 12539, -30851, 11038, -31356, 9511, -31784, 7961
	.dw	-32137, 6392, -32412, 4807, -32609, 3211, -32727, 1607

t_hamming128: ; Hamming window (for 128 samples)
	.dw	2621, 2639, 2693, 2784, 2910, 3073, 3270, 3502
	.dw	3768, 4068, 4401, 4765, 5161, 5587, 6042, 6525
	.dw	7036, 7571, 8132, 8715, 9320, 9945, 10588, 11249
	.dw	11926, 12616, 13318, 14031, 14753, 15482, 16216, 16954
	.dw	17694, 18433, 19171, 19905, 20634, 21356, 22069, 22772
	.dw	23462, 24138, 24799, 25443, 26068, 26673, 27256, 27816
	.dw	28352, 28862, 29345, 29800, 30226, 30622, 30987, 31319
	.dw	31619, 31885, 32117, 32315, 32477, 32603, 32694, 32748
	.dw	32767, 32748, 32694, 32603, 32477, 32315, 32117, 31885
	.dw	31619, 31319, 30987, 30622, 30226, 29800, 29345, 28862
	.dw	28352, 27816, 27256, 26673, 26068, 25443, 24799, 24138
	.dw	23462, 22772, 22069, 21356, 20634, 19905, 19171, 18433
	.dw	17694, 16954, 16216, 15482, 14753, 14031, 13318, 12616
	.dw	11926, 11249, 10588, 9945, 9320, 8715, 8132, 7571
	.dw	7036, 6526, 6042, 5587, 5161, 4765, 4401, 4068
	.dw	3768, 3502, 3270, 3073, 2910, 2784, 2693, 2639

t_desc128:	; Descramble table (for 128 point FFT)
	.dw	0*4, 64*4, 32*4, 96*4, 16*4, 80*4, 48*4, 112*4
	.dw	8*4, 72*4, 40*4, 104*4, 24*4, 88*4, 56*4, 120*4
	.dw	4*4, 68*4, 36*4, 100*4, 20*4, 84*4, 52*4, 116*4
	.dw	12*4, 76*4, 44*4, 108*4, 28*4, 92*4, 60*4, 124*4
	.dw	2*4, 66*4, 34*4, 98*4, 18*4, 82*4, 50*4, 114*4
	.dw	10*4, 74*4, 42*4, 106*4, 26*4, 90*4, 58*4, 122*4
	.dw	6*4, 70*4, 38*4, 102*4, 22*4, 86*4, 54*4, 118*4
	.dw	14*4, 78*4, 46*4, 110*4, 30*4, 94*4, 62*4, 126*4