.title "sfft.asm"
     .mmregs
     .global _sfft,_size
;-----------------------------
;         rfft_task
;-----------------------------    
K_DATA_IDX_1     .set 2
K_DATA_IDX_2     .set 4
K_DATA_IDX_3     .set 8
K_FLY_COUNT_3    .set 4
K_TWID_TBL_SIZE  .set 4096
K_TWID_IDX_3     .set 1024
DATA_BUFFER      .set 4000h
DATA_BUFFER2     .set 5000h
cosine           .set 2000h
sine             .set 6000h
     .bss d_twid_idx,1
     .bss d_grps_cnt,1
     .bss d_data_idx,1
     .sect "rsfft_prg"
_sfft:
       pshm ar1
       pshm ar6
       pshm ar7
       pshm ST0
       pshm ST1
       
       nop
       frame #-4
       nop
       call bit_rev
       call fft
       call unpack
       call spectrum
       nop
       frame #4
       nop
       
       popm ST1
       popm ST0
       popm ar7
       popm ar6
       popm ar1
       ret
;-----------------------------
;        bit_rev
;-----------------------------
     .asg ar2,REORDERED_DATA
     .asg ar3,ORIGINAL_INPUT
     .asg ar7,DATA_PROC_BUF
     .sect "rsfft_prg"
bit_rev:
       ssbx frct
       ld *(_size),-1,a
       stm #DATA_BUFFER,ORIGINAL_INPUT
       stm #DATA_BUFFER2,DATA_PROC_BUF
       mvmm DATA_PROC_BUF,REORDERED_DATA
       stlm a,ar0
       sub #1,a
       stlm a,brc 
       nop   
       rptb bit_rev_end-1
       mvdd *ORIGINAL_INPUT+,*REORDERED_DATA+
       mvdd *ORIGINAL_INPUT-,*REORDERED_DATA+
       mar *ORIGINAL_INPUT+0B
bit_rev_end:     
       ret
;-----------------------------
;          fft
;-----------------------------
     .asg ar1,GROUP_COUNTER
     .asg ar2,PX
     .asg ar3,QX
     .asg ar4,WR
     .asg ar5,WI
     .asg ar6,BUTTERFLY_COUNTER
     .asg ar7,DATA_PROC_BUF 
     .asg ar7,STAGE_COUNTER 
     .sect "rsfft_prg"  
fft:
       nop
       nop
       nop
       nop 
;stage 1
       stm #0,bk   
       ld #-1,asm
       mvmm  DATA_PROC_BUF,PX  
       ld *PX,16,a
       ld *(_size),-2,b
       sub #1,b
       stlm b,brc
       stm #DATA_BUFFER2+K_DATA_IDX_1,QX
       rptbd stage1end-1
       stm #K_DATA_IDX_1+1,ar0
       sub *QX,16,a,b
       add *QX,16,a
       sth a,asm,*PX+
       st b,*QX+
       ||ld *PX,a
       sub *QX,16,a,b
       add *QX,16,a
       sth a,asm,*PX+0
       st b,*QX+0%
       ||ld *PX,a  
stage1end:
;stage 2
       mvmm DATA_PROC_BUF,PX
       stm #DATA_BUFFER2+K_DATA_IDX_2,QX
       ld *(_size),-3,b
       sub #1,b
       stlm b,brc
       ld *PX,16,a
       rptbd stage2end-1
       stm #K_DATA_IDX_2+1,ar0
;the first butterfly
       sub *QX,16,a,b
       add *QX,16,a
       sth a,asm,*PX+
       st b,*QX+
       ||ld *PX,a
       sub *QX,16,a,b
       add *QX,16,a
       sth a,asm,*PX+
       sth b,asm,*QX+
;the second butterfly
       mar *QX+
       add *PX,*QX,a
       sub *PX,*QX-,b
       sth a,asm,*PX+
       sub *PX,*QX,a
       st b,*QX
       ||ld *QX+,b
       st a,*PX
       ||add *PX+0%,a
       st a,*QX+0%
       ||ld *PX,a
stage2end: 
;stage 3 to stage logN-1
       stm #K_TWID_TBL_SIZE,bk
       st #K_TWID_IDX_3,*(d_twid_idx)
       stm #K_TWID_IDX_3,ar0
       stm #cosine,WR
       stm #sine,WI
       ld *(_size),a
       exp a
       nop
       ldm t,a
       sub #1ah,a,b
       neg b
       stlm b,STAGE_COUNTER
       ld *(_size),-4,a
       sub #1,a,b
       stl b,*(d_grps_cnt)
       stm #K_FLY_COUNT_3-1,BUTTERFLY_COUNTER
       st #K_DATA_IDX_3,*(d_data_idx)
stage: 
       stm #DATA_BUFFER2,PX
       ld *(d_data_idx),a
       add *(PX),a
       stlm a,QX
       mvdk *(d_grps_cnt),GROUP_COUNTER 
group:
       mvmd BUTTERFLY_COUNTER,brc
       ld *WR,t
       mpy *QX+,a
       rptb butterflyend-1
       macr *WI+0%,*QX-,a
       add *PX,16,a,b 
       st b,*PX
       ||sub *PX+,b
       st b,*QX
       ||mpy *QX+,a
       masr *QX,*WR+0%,a
       add *PX,16,a,b
       st b,*QX+
       ||sub *PX,b
       ld *WR,t
       st b,*PX+
       ||mpy *QX+,a 
butterflyend:
;next group
       pshm ar0
       mvdk *(d_data_idx),ar0
       mar *PX+0
       mar *QX+0
       banzd group,*GROUP_COUNTER-
       popm ar0
       mar *QX-
;next stage
       ld *(d_data_idx),a
       sub #1,a,b
       stlm b,BUTTERFLY_COUNTER
       stl a,1,*(d_data_idx)
       ld *(d_grps_cnt),a
       stl a,asm,*(d_grps_cnt)
       ld *(d_twid_idx),a
       stl a,asm,*(d_twid_idx)
       mvdk *(d_twid_idx),ar0
       banz stage,*STAGE_COUNTER-     
fft_end:
       ret
;-----------------------------
;        unpack
;-----------------------------
unpack:
     .sect "rsfft_prg" 
;compute RP,RM,IP,IM
     .asg ar2,XP_K
     .asg ar3,XP_Nminusk
     .asg ar6,XM_K
     .asg ar7,XM_Nminusk 
       nop
       nop
       nop
       nop
       stm #DATA_BUFFER2+2,XP_K
       ld #DATA_BUFFER2-2,a
       add *(_size),a
       stlm a,XP_Nminusk 
       stm #DATA_BUFFER+3,XM_Nminusk
       ldu *(_size),a
       add  #DATA_BUFFER,a
       sub #1,a
       stlm a,XM_K 
       ld *(_size),-2,a
       sub #2,a
       stlm a,brc
       stm #3,ar0
       rptb phase3end-1
       add *XP_K,*XP_Nminusk,a
       sub *XP_K,*XP_Nminusk,b
       sth a,asm,*XP_K+
       sth a,asm,*XP_Nminusk+
       sth b,asm,*XM_K-
       neg b
       sth b,asm,*XM_Nminusk-
       add *XP_K,*XP_Nminusk,a
       sub *XP_K,*XP_Nminusk,b
       sth a,asm,*XP_K+
       sth a,asm,*XP_Nminusk-0
       sth b,asm,*XM_K-
       neg b
       sth b,asm,*XM_Nminusk+0      
phase3end:
       st #0,*XM_K-
       st #0,*XM_K
       nop
       nop 
       nop
       nop
       nop
       nop 
;compute AR[0],AI[0],AR[N],AI[N]
     .asg ar2,AX_K
     .asg ar4,IP_0
     .asg ar5,AX_N
       stm #DATA_BUFFER2,AX_K
       stm #DATA_BUFFER2+1,IP_0
       stm #DATA_BUFFER+1,AX_N
       add *AX_K,*IP_0,a
       sub *AX_K,*IP_0,b
       sth a,asm,*AX_K+
       st #0,*AX_K
       mvdd *AX_K+,*AX_N-
       sth b,asm,*AX_N
;compute AR[K],AI[K]
     .asg ar3,AX_2Nminusk
     .asg ar4,COS
     .asg ar5,SIN
       ldu *(_size),a
       add  #DATA_BUFFER,a
       sub #1,a
       stlm a,AX_2Nminusk 
       ldu *(d_twid_idx),a
       add #cosine,a
       stlm a,COS
       ldu *(d_twid_idx),a
       add #sine,a
       stlm a,SIN
       ld *(_size),-1,a
       sub #2,a
       stlm a,brc
       mvdk *(d_twid_idx),ar0
       rptb phase4end-1
       ld *AX_K+,16,a
       macr *COS,*AX_K,a
       masr *SIN,*AX_2Nminusk-,a
       ld *AX_2Nminusk+,16,b
       masr *SIN+0%,*AX_K-,b
       masr *COS+0%,*AX_2Nminusk,b
       sth a,asm,*AX_K+
       sth b,asm,*AX_K+
       neg b
       sth b,asm,*AX_2Nminusk-
       sth a,asm,*AX_2Nminusk-
phase4end:       
       ret
;compute power spectrum
spectrum:
       ld *(_size),-1,a
       sub #1,a
       stlm a,brc
       stm #DATA_BUFFER2,ar2
       stm #DATA_BUFFER2,ar3
       rptb power_end1-1
       squr *ar2+,a
       squra *ar2+,a
       sth a,*ar3+       
power_end1:
       stm #DATA_BUFFER,ar2
       ld *(_size),-1,a
       sub #1,a
       stlm a,brc
       nop 
       rptb power_end2-1
       squr *ar2+,a
       squra *ar2+,a
       sth a,*ar3+       
power_end2:
       nop
       nop 
       nop
       nop
       nop
       nop 
       ret
     .end