N equ 120                         ;filter order
delta equ 0.005                    ;delta
pctl  equ $fffd                   ; PLL control register
bcr equ $fffe                     ; BCR Register
ogdbr equ $fffc                   ; OnCE GDB Register
ecsr equ $ffeb                    ; EMI Control/Status Register
edrr0 equ $ffea                   ; EMI Data Read Register
ebar0 equ $ffe8                   ; EMI Base Address Register 0
eor0 equ $ffe9                    ; EMI Offset Register
edrf equ 13                       ; EMI EDRR Full flag
hrne equ 17                       ; SHI FIFO Not Empty flag
hrx equ $fff3                     ; SHI HRX FIFO
hcsr equ $fff1                    ; SHI Control/Status Register
hi2c equ 1                        ; SHI IIC Enable Control Bit
ma equ 0                          ; OMR Mode A
mb equ 1                          ; OMR Mode B
mc equ 4                          ; OMR Mode C
gpior equ $fff7                   ; GPIO Control/Data Register
gdd3 equ 11                       ; direction bit for GPIO3
gdd2 equ 10                       ; direction bit for GPIO2
gdd1 equ 9                        ; direction bit for GPIO1
gdd0 equ 8                        ; direction bit for GPIO0
gd3 equ 1                         ; data bit for GPIO3
rcs equ $ffe1                     ;SAI receiver control / status register
tcs equ $ffe4                     ;SAI transmitter conntrol /status register
brc equ $ffe0                     ;baud rate control register
rx0 equ $ffe2                     ;SAI receive data register
rx1 equ $ffe3                     ;SAI receive data register
tx0 equ $ffe5                     ;SAI transmit data register
tx1 equ $ffe6                     ;SAI transmit data register
tx2 equ $ffe7                     ;SAI transmit data register
ipr equ $ffff                     ;interrupt priority register
rldf equ 14                       ; left receive register full
rrdf equ 15                       ; right ......          full
tlde equ 14                       ; left transmit reg. empty
trde equ 15                       ; right ......


buf_delta_i equ  N+2              ;integer part of delta
buf_delta_f equ  N+3              ;fractional part of delta
buf_sin_x_i equ  N+4              ;int. sine coordinates
buf_sin_x_f equ  N+5              ;fract. sine coordinates
buf_cos_x_i equ  N+6              ;int. cosine coor.
buf_cos_x_f equ  N+7              ;fract.cosine coor.
sin_x       equ  N+8              ;currnet sine val.
cos_x       equ  N+9              ;current cosine val.

sinep equ $100                   ;sine table starting address
cosinep equ $14A                 ;cosine start address
mask equ $0ff                    ;base address plus N minus one



  org p:$0
  jmp start1                     ;jump main

;  org p:$2    ;stack error
;  nop
;  nop

;   org p:$4   ;trace
;   nop
;   nop

;   org p:$6   ;SWI
;   nop
;   nop

;   org p:$8       ;IRQa
;    nop
;    nop

;   org p:$0a      ;IRQb
;    nop
;    nop

;   org p:$10      ;SAI L TX
;   jmp send_l
  ; nop
  ; nop

;   org p:$12      ;SAI R TX
;   jmp send_r
  ; nop
  ; nop

;  org p:$14       ;SAI TX Exception
;  jmp exc_tx
 ;  nop
 ;  nop

;  org p:$16         ;SAI L RX
 ; jmp receive_l
;   nop
;   nop

;  org p:$18          ;SAI R RX
;  jmp receive_r
 ;  nop
 ;  nop

;  org p:$1a         ;SAI RX Exception
;  jmp exc_rx
;   nop
;   nop

;  org p:$4a
;  nop
;  nop

coeff:      ; Hilbert N-ordre coefficients
  dc     0.0000
  dc    -0.0009
  dc     0.0000
  dc    -0.0007
  dc     0.0000
  dc    -0.0009
  dc     0.0000
  dc    -0.0012
  dc     0.0000
  dc    -0.0015
  dc     0.0000
  dc    -0.0019
  dc     0.0000
  dc    -0.0024
  dc     0.0000
  dc    -0.0030
  dc     0.0000
  dc    -0.0037
  dc     0.0000
  dc    -0.0045
  dc     0.0000
  dc    -0.0053
  dc     0.0000
  dc    -0.0064
  dc     0.0000
  dc    -0.0075
  dc     0.0000
  dc    -0.0089
  dc     0.0000
  dc    -0.0104
  dc     0.0000
   dc   -0.0122
   dc    0.0000
   dc   -0.0142
   dc    0.0000
   dc   -0.0165
   dc    0.0000
   dc   -0.0193
   dc    0.0000
   dc   -0.0224
   dc    0.0000
   dc   -0.0262
   dc    0.0000
   dc   -0.0308
   dc    0.0000
   dc   -0.0365
   dc    0.0000
   dc   -0.0437
   dc    0.0000
   dc   -0.0534
   dc    0.0000
   dc   -0.0670
   dc    0.0000
   dc   -0.0880
   dc    0.0000
   dc   -0.1252
   dc    0.0000
   dc   -0.2109
   dc    0.0000
   dc   -0.6362
   dc         0
   dc    0.6362
   dc    0.0000
   dc    0.2109
   dc    0.0000
   dc    0.1252
   dc    0.0000
   dc    0.0880
   dc    0.0000
   dc    0.0670
   dc    0.0000
   dc    0.0534
   dc    0.0000
   dc    0.0437
   dc    0.0000
   dc    0.0365
   dc    0.0000
   dc    0.0308
   dc    0.0000
   dc    0.0262
   dc    0.0000
  dc     0.0224
  dc     0.0000
  dc     0.0193
  dc     0.0000
  dc     0.0165
  dc     0.0000
  dc     0.0142
  dc     0.0000
  dc     0.0122
  dc     0.0000
  dc     0.0104
  dc     0.0000
  dc     0.0089
  dc     0.0000
  dc     0.0075
  dc     0.0000
  dc     0.0064
  dc     0.0000
  dc     0.0053
  dc     0.0000
  dc     0.0045
  dc     0.0000
  dc     0.0037
  dc     0.0000
  dc     0.0030
  dc     0.0000
  dc     0.0024
  dc     0.0000
  dc     0.0019
  dc     0.0000
  dc     0.0015
   dc    0.0000
   dc    0.0012
   dc    0.0000
   dc    0.0009
   dc    0.0000
   dc    0.0007
   dc    0.0000
   dc    0.0009
   dc    0.0000

 start1:
  move       #coeff,r0         ;start ROM addr. of coeff. table
  move       #0,r1             ;start RAM addr. of coeff. table
  do        #N+1,coefloop       ;transfer coeff. to Y:RAM
       move       p:(r0)+,x0   ;ROM->x0
       move       x0,y:(r1)+   ;x0->y:RAM
coefloop:



    movep #$260035,x:pctl    ; activate PLL and set clk=50MHz
    movep #$000201,x:rcs     ; activate SAI:RX - master mode
    movep #$000203,x:tcs     ; activate SAI:TX - master mode

    movep #$000c00,x:ipr     ;set interrupt priority register
   ; andi   #$0fc,mr          ;turn interrupts on


      move #0,r0             ;start of samples buffer chanel l
      move #0,r4             ;start of coeff. buffer
      move #N-1,m0           ;arythm. mod N for adr. register
      move m0,m4             ;consider both buffers


      MOVE #(N/2)+1,N0       ;middle sample of left buffer-delay of filter
      MOVE #(N/2)+1,N1       ;middle ..........right .............

      move #128,r1           ;start of samples buffer chanel r
      move #0,r5             ;start of coeff. buffer
      move #N-1,m1           ;arythm. mod N for adr. register
      move m1,m5             ;consider both buffers


 move #(delta-@cvi(delta)),b0 ;store fraction part in b0
 asl b #sinep,r6              ;eliminate sign bit, initialize r1
 move b0,y:buf_delta_f        ;unsigned fractional Delta in y0
 move #>@cvi(delta),b0
 move b0,y:buf_delta_i        ;integer part of Delta in y0
 clr a  #0,n6                 ;initialize a and n1
 move #mask,m7                ;set up mask value for sine pointer
 move m7,m6                   ;set up mask value .....
 move #0,n7                   ;initialize n2 - get 0 val. from sine table
 move #sinep+1,r7             ;initialize r2 with the value r1+ 1 - next val.

 clr a  #0,n2                 ;initialize a and n1
 move #mask,m3                ;set up mask value  for cosine generator
 move m3,m2                   ;set up mask value
 move #0,n3                   ;initialize n3 - get 0 val. from cosine table
 move #cosinep+1,r3           ;initialize r3 with the value r1+ 1 - next val.
 move #cosinep,r2
                             ;a=0
 move a1,y:buf_sin_x_i       ;initialize buffers
 move a0,y:buf_sin_x_f       ;...
 move a1,y:buf_cos_x_i       ;...
 move a0,y:buf_cos_x_f       ;...


wait_loop:                      ;idle loop
    jset #rldf,x:rcs,receive_l  ;if rx left buff. full jump to process
    jset #rrdf,x:rcs,receive_r  ;if rx right buff. full jump to process
    jmp wait_loop

 ;  exc_rx:
 ;   bset #gdd1,x:gpior
 ; nop
 ; nop
 ; bclr #gdd1,x:gpior
 ;
 ;  move x:rcs,a2
 ;  move x:rx0,a2
 ; nop
 ; nop
 ; nop
 ; nop
 ;  rti

 ;  exc_tx:
 ; bclr #gdd1,x:gpior
 ; bset #gdd1,x:gpior
 ;  move x:tcs,a2
 ; move a2,x:tx0
 ; nop
 ; nop
 ; nop
 ; nop
 ; rti



receive_l:
 ; ORI   #$03,mr              ;turn off int.
  bCLR #gdd1,x:gpior          ;set io  for cpu usage measure
    clr a                     ;clr a2
 move y:buf_sin_x_i,a1        ;restore sine coordinates
 move y:buf_sin_x_f,a0        ;....
     clr b                    ; clr b2
 move a0,b1                   ;fraction part of ptr in b1, b0= 0 (1)
 lsr b   y:(r6+n6),x0         ;b1= signed fr. Delta, x0= sine (2)
; move b1,x:tx1               ;output sample for control purposes
 move b,x1 y:(r7+n7),b        ;sign. fract. in x1, sine in b1 (2)
 sub x0,b                     ;subtr sines to obtain slope (1)
 move b,x0   y:(r6+n6),b      ;slope value to x0, sine in b1 (2)
 macr x0,x1,b                 ;sample approximation = m* x+ b (1)
 move y:buf_delta_i,y1        ;load int. part of delta
 move y:buf_delta_f,y0        ;load fractoin. part of delta
 add y,a                      ;update calculation of  sine coordin.
 move b,y:sin_x               ;store sine sample
 move m7,x1                   ;set up mask value in x1 (1)
 and x1,a                     ;mask a1 = modulo arithmetic (1)
 move a1,n6                   ;update offset register n1 (1)
 move a1,n7                   ;update offset register n2 (1)                                 ;return from the subroutine (1)
 move a1,y:buf_sin_x_i        ;store sine coordinates
 move a0,y:buf_sin_x_f        ;....





    move x:rx0,a              ;get input sample
;    MOVE A,X:tx0             ;output sample for control purposes
    move a,x:(r0)             ;write sample to RAM
    clr a    x:(r0)+,x0      y:(r4)+,y0   ;update registers
    do  #N-1,conv                         ;calculate convoltion
       mac x0,y0,a    x:(r0)+,x0   y:(r4)+,y0
conv:
    macr x0,y0,a   (r0)-      ;calculate last sample,shift sample buff.pointer
    move a,x0                 ;x_h
;    MOVE A,X:tx0             ;output sample for control purposes
    move y:cos_x,y0           ;get cosine value
    MOVE y0,X:tx1             ;output cosine value for control
    mpy  x0,y0,b              ;b=Xh*cos
    move x:(r0+N),x0          ;X_d
;    move x0,x:tx0            ;output delayed inp.sample for control purposes
    move y:sin_x,y0           ;get sine  value
;    MOVE y0,X:tx1            ;output sine value for control
    move b,a                  ;prepare to final calculation
;    neg a                    ;negate a for down-convertion
    macr x0,y0,a              ;a=X_d*sin+X_h*cos
    move a,x:tx0              ;outut sample
    bSET #gdd1,x:gpior        ;set io  for cpu usage measure
;   andi   #$0fc,mr           ;turn int. on
    jmp wait_loop             ;jump to idle loop
;   rti




receive_r:
  bclr #gdd1,x:gpior           ;set io  for cpu usage measure
     clr a                     ;clr a2
 move y:buf_cos_x_i,a1         ;restore sine coordinates
 move y:buf_cos_x_f,a0         ;....
     clr b                     ; clr b2
 move a0,b1                    ;fraction part of ptr in b1, b0= 0 (1)
  lsr b   y:(r2+n2),x0         ;b1= signed fr. Delta, x0= sine (2)
; move b1,x:tx1                ;output sample for control purposes
 move b,x1 y:(r3+n3),b         ;sign. fract. in x1, sine in b1 (2)
 sub x0,b                      ;subtr sines to obtain slope (1)
 move b,x0   y:(r2+n2),b       ;slope value to x0, sine in b1 (2)
 macr x0,x1,b                  ;sample approximation = m* x+ b (1)
 move y:buf_delta_i,y1         ;load int. part of delta
 move y:buf_delta_f,y0         ;load fractoin. part of delta
 add y,a                       ;update calculation
 move b,y:cos_x                ;store cosine sample (1)
 move m3,x1                    ;set up mask value in x1 (1)
 and x1,a                      ;mask a1 = modulo arithmetic (1)
 move a1,n2                    ;update offset register n1 (1)
 move a1,n3                    ;update offset register n2 (1)                                 ;return from the subroutine (1)
 move a1,y:buf_cos_x_i         ;store sine coordinates
 move a0,y:buf_cos_x_f         ;....


    move x:rx0,a              ;get input sample
;    MOVE A,X:tx1             ;output sample for control purposes
    move a,x:(r1)             ;write sample to RAM
    clr a    x:(r1)+,x0      y:(r5)+,y0      ;update registers
    do  #N-1,conv1                          ;calculate convoltion
       mac x0,y0,a    x:(r1)+,x0   y:(r5)+,y0
conv1:
    macr x0,y0,a   (r1)-      ;calculate last sample,shift sample buff.pointe
    move a,x0                 ;x_h
 ;   MOVE A,X:tx1             ;output sample for control purposes
    move y:cos_x,y0           ;get cosine value
;    MOVE Y0,X:tx1            ;output cosine value for control
    mpy  x0,y0,b              ;b=Xh*cos
    move x:(r1+N),x0          ;X_d
 ;   MOVE x0,X:tx1            ;output delayed inp.sample for control purposes
    move y:sin_x,y0           ;get sine  value
    MOVE y0,X:tx1             ;output sine value for control
    move b,a                  ;prepare to final calculation
 ;  neg a                     ;negate a for down-convertion
    macr x0,y0,a              ;a=X_d*sin+X_h*cos
    move a,x:tx0              ;outut sample
    bSET #gdd1,x:gpior        ;set io  for cpu usage measure
  ;      andi   #$0fc,mr      ;turn int. on
   jmp wait_loop              ;jump to idle loop
;   rti





