.title "fir.asm"
     .mmregs
     .def _fir
     .global _fir,_FFT_p,_size
;-----------------------------
;         rfft_task
;-----------------------------    
a0               .set 3100h
xn               .set 3200h
length           .set 126
     .bss ave,1
     .bss max_v,1
     .bss temp_a,1
     .bss y,1
     .sect "fir_prg"
_fir:
       pshm ar1
       pshm ar6
       pshm ar7
       pshm ST0
       pshm ST1
       
       nop
       frame #-4
       nop
       call average
       call AGC
       call fir_low
       call AGC
       nop
       frame #4
       nop
       
       popm ST1
       popm ST0
       popm ar7
       popm ar6
       popm ar1
       ret
;-----------------------------
;       average x(n) 
;-----------------------------
     .sect "fir_prg"
average:
;square x(n)
       ssbx frct
       ld *(_size),a
       sub #1,a
       stlm a,brc
       
       ldu *(_FFT_p),a
       stlm a,ar2
       stlm a,ar3
       ld #0,b
       rptb square_end-1
       squr *ar2,a
       sth a,*ar2+
       add *ar3+,b       
square_end:
;sub aeverage
       ld b,-13,a
       stl a,*(ave)
       ld *(_size),b
       sub #1,b
       stlm b,brc
       ldu *(_FFT_p),b
       stlm b,ar2
       rptb sub_end-1
       ld *ar2,a
       sub *(ave),a
       stl a,*ar2+
sub_end:
       nop
       nop
       nop
       nop
       ret
;-----------------------------
;      auto gain control
;-----------------------------  
      .sect "fir_prg" 
AGC:   
       ld *(_size),a
       sub #1,a
       stlm a,brc
       ssbx frct
       ldu *(_FFT_p),a
       stlm a,ar2
       stlm a,ar3
       ld #0,b
       rptb max_end-1
       ld *ar2+,a
       abs a
       max b     
max_end:
;compare
       stl b,*(max_v)
       add #32768,b
       nop
       nop
       nop
       nop
       bc AGC_end,BEQ
        
       nop
       nop
       ld #32767,a
       ld #-1,b
       rsbx frct
compare:
       sub *(max_v),a
       add #1,b
       nop
       nop
       nop
       bc compare,AGT
;AGC       
       ld *(_size),a
       sub #1,a
       stlm a,brc
       stlm b,t
       add #-1,b
       bc multiply,BNEQ
       ssbx frct
       ret
multiply:
       rptb AGC_end-1
       mpy *ar3,a
       stl a,*ar3+
AGC_end: 
       ssbx frct
       nop
       nop
       ret
;-----------------------------
;     low pass fir filer
;-----------------------------
     .sect "fir_prg"  
fir_low:
       ssbx frct
       stm #xn+length-1,ar3
       stm #xn+length-1,ar2
       stm #a0+length-1,ar4
       stm #length-2,brc
       stm #length,bk
       stm #-1,ar0
       mvdk *(_FFT_p),ar5
;initial       
       rptb initial_end-1
       st #0,*ar2-
initial_end:  
;fir
       ld *(_size),a
       sub #1,a
       stlm a,brc
       mvdd *ar5,*ar2
       
       rptb fir_end-1
       rptz a,#length-1
       mac *ar3+0%,*ar4+0%,a 
       sth a,*ar5+
       mvdd *ar5,*ar3+0%
fir_end
       nop
       nop
       nop
       nop 
       ret
     .end