;---------------------------------------------------------------------------
;
;	File:		FFT_float_asm.a
;
;	Author: 	Stéphane TAVENARD
;	Revision:	0.1
;	Date:		14/08/1995
;
;	$VER:		FFT_float_asm.a  0.1  (14/08/95)
;
;	(C) Copyright 1994-1994 Stéphane TAVENARD
;	    All Rights Reserved
;
;	----------------------------------------------------------------
;
;       #0 (23/04/1995) Initial revision
;       #1 (14/08/1995) Changed fmul by fsglmul & fdiv by fsgldiv
;       #1 (14/08/1995) Changed double COS,SIN by float COS,SIN
;
;	Routines for FFT calculation with floating points values
;
;---------------------------------------------------------------------------

                  section  text,code

		  XDEF	   @ASM_FFT_main_loop
		  XDEF	   _ASM_FFT_main_loop	; for SAS-C 6.5
		  XDEF	   @ASM_FFT_energy_phi
		  XDEF	   _ASM_FFT_energy_phi	; for SAS-C 6.5
;
;		  even

;		  a0: float *x_real
;		  a1: float *x_imag
;		  a2: float *cos
;		  a3: float *sin
;		  d0: power
;
_ASM_FFT_main_loop
@ASM_FFT_main_loop
		  movem.l  d2-d7/a2-a6,-(sp)
		  move.l   a2,a4	  ; a4 = cos
		  move.l   a3,a5	  ; a5 = sin
		  moveq.l  #1,d1	  ; d1 = d = 1
		  clr.l    d2
		  bset	   d0,d2
		  asr.l    #1,d2	  ; d2 = li = 1<<( power-1 )

		  subq.w   #1,d0	  ; d0 = i
FFT_main_loop_1
		  movem.l  a0-a1,-(sp)
		  lea.l    (a0,d1.w*4),a2 ; a2 = &x_real[ d ]
		  lea.l    (a1,d1.w*4),a3 ; a3 = &x_imag[ d ]

		  move.w   d2,d3	  ;
		  subq.w   #1,d3	  ; d3 = j
FFT_main_loop_2   move.w   d1,d4
		  subq.w   #1,d4	  ; d4 = l
FFT_main_loop_3   fmove.s  (a0),fp0	  ; fp0 = *real_ptr
		  fmove.s  (a1),fp1	  ; fp1 = *imag_ptr
		  fmove.s  (a2),fp2	  ; fp2 = *t_real_ptr
		  fmove.s  (a3),fp3	  ; fp3 = *t_imag_ptr
		  fmove.x  fp2,fp4
		  fmove.x  fp3,fp5
		  fadd.x   fp0,fp2	  ; fp2 = *real_ptr + *t_real_ptr
		  fadd.x   fp1,fp3	  ; fp3 = *imag_ptr + *t_imag_ptr
		  fsub.x   fp4,fp0	  ; fp0 = *real_ptr - *t_real_ptr
		  fsub.x   fp5,fp1	  ; fp1 = *imag_ptr - *t_imag_ptr
		  fmove.s  fp2,(a0)+	  ; fp2 -> *real_ptr++
		  fmove.s  fp3,(a1)+	  ; fp3 -> *imag_ptr++
		  fmove.s  fp0,(a2)+	  ; fp4 -> *t_real_ptr++
		  fmove.s  fp1,(a3)+	  ; fp5 -> *t_imag_ptr++
		  dbra	   d4,FFT_main_loop_3
		  lea.l    (a0,d1.w*4),a0 ; real_ptr += d;
		  lea.l    (a1,d1.w*4),a1 ; imag_ptr += d;
		  lea.l    (a2,d1.w*4),a2 ; t_real_ptr += d;
		  lea.l    (a3,d1.w*4),a3 ; t_imag_ptr += d;
		  dbra	   d3,FFT_main_loop_2

		  movem.l  (sp)+,a0-a1
		  movem.l  a0-a1,-(sp)

		  asl.l    #1,d1	  ; d = d<<1;
		  asr.l    #1,d2	  ; li = li>>1;
		  beq	   FFT_main_loop_9 ; li = 0 -> end of FFT
		  lea.l    (a0,d1.w*4),a0 ; a0 = real_ptr = &x_real[ d ]
		  lea.l    (a1,d1.w*4),a1 ; a1 = imag_ptr = &x_imag[ d ]
		  move.w   d2,d3
		  subq.w   #1,d3	  ; d3 = j
FFT_main_loop_5   move.l   a4,a2	  ; a2 = cos_ptr = cos
		  move.l   a5,a3	  ; a3 = sin_ptr = sin
		  move.w   d1,d4
		  subq.w   #1,d4	  ; d4 = l
FFT_main_loop_6   fmove.s  (a0),fp0	  ; fp0 = *real_ptr
		  fmove.s  (a1),fp1	  ; fp1 = *imag_ptr
		  fmove.s  (a2),fp2	  ; fp2 = *cos_ptr
		  fmove.s  (a3),fp3	  ; fp3 = *sin_ptr
		  fmove.x  fp2,fp4
		  fmove.x  fp3,fp5
		  fsglmul.x   fp0,fp2	  ; fp2 = (*real_ptr)*(*cos_ptr)
		  fsglmul.x   fp1,fp3	  ; fp3 = (*imag_ptr)*(*sin_ptr)
		  fadd.x   fp2,fp3	  ; fp3 = real
		  fsglmul.x   fp1,fp4	  ; fp4 = (*imag_ptr)*(*cos_ptr)
		  fsglmul.x   fp0,fp5	  ; fp5 = (*real_ptr)*(*sin_ptr)
		  fsub.x   fp5,fp4	  ; fp4 = imag
		  fmove.s  fp3,(a0)+	  ; *real_ptr++ = real
		  fmove.s  fp4,(a1)+	  ; *imag_ptr++ = imag
		  lea.l    (a2,d2.w*4),a2 ; cos_ptr += li
		  lea.l    (a3,d2.w*4),a3 ; sin_ptr += li
		  dbra	   d4,FFT_main_loop_6
		  lea.l    (a0,d1.w*4),a0 ; real_ptr += d
		  lea.l    (a1,d1.w*4),a1 ; imag_ptr += d
		  dbra	   d3,FFT_main_loop_5
FFT_main_loop_9
		  movem.l  (sp)+,a0-a1

		  dbra	   d0,FFT_main_loop_1

		  movem.l  (sp)+,d2-d7/a2-a6
		  rts

;		  even

;		  a0: float *x_real
;		  a1: float *x_imag
;		  a2: float *energy
;		  a3: float *phi
;		  d0: n
;
_ASM_FFT_energy_phi
@ASM_FFT_energy_phi
		  subq.w   #1,d0
FFT_energy_phi_0
		  fmove.s  (a0)+,fp0
		  fmove.s  (a1)+,fp1
		  fmove.x  fp0,fp4
		  fmove.x  fp1,fp5
		  fsglmul.x   fp4,fp4
		  fsglmul.x   fp5,fp5
		  fadd.x   fp4,fp5	     ; fp5 = en = real^2 + imag^2
		  fmove.s  fp5,(a2)+	     ; *energy++ = en
		  ftst.x   fp5
		  fbne.w   FFT_energy_phi_1  ; en <> 0
		  fmove.s  #0,fp1	     ; phi = 0
		  bra	   FFT_energy_phi_5
FFT_energy_phi_1  ftst.x   fp0
		  fbeq.w   FFT_energy_phi_3  ; real == 0
FFT_energy_phi_2  fsgldiv.x   fp0,fp1	     ; fp1 = imag / real
		  fatan.x  fp1,fp1	     ; fp1 = atan( imag / real )
		  bra	   FFT_energy_phi_5
FFT_energy_phi_3  fmove.s  #1E-20,fp0
		  bra	   FFT_energy_phi_2
FFT_energy_phi_5  fmove.s  fp1,(a3)+	     ; *phi++ = phi
		  dbra	   d0,FFT_energy_phi_0
		  rts

;		  even

		  end