N equ 120
delta equ 0.03                    ;delta
pctl  equ $fffd
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 GPIO3
gdd1 equ 9 ; direction bit for GPIO3
gdd0 equ 8 ; direction bit for GPIO3

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
rrdf equ 15
tlde equ 14
trde equ 15


buf_delta_i equ  N+2
buf_delta_f equ  N+3
buf_sin_x_i equ  N+4
buf_sin_x_f equ  N+5
buf_cos_x_i equ  N+6
buf_cos_x_f equ  N+7
sin_x       equ  N+8
cos_x       equ  N+9

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



  org p:$0 ; bootstrap code starts at $0
  jmp start1

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

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

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

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

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

;   org p:$10      ;SAI L TX
;   jmp wyslij_l
 ;   jmp wyjatek
  ; nop
  ; nop

;   org p:$12      ;SAI R TX
;   jmp wyslij_r
   ; jmp wyjatek
  ; nop
  ; nop

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

;  org p:$16         ;SAI L RX
 ; jmp odbierz_lewy
;  jmp wyjatek
;   nop
;   nop
;  org p:$18          ;SAI R RX
;  jmp odbierz_prawy
;  jmp wyjatek
 ;  nop
 ;  nop

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

;  org p:$4a
  ;jmp wyjatek
;  nop
;  nop

wspolczynniki:
  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:
 ;filtr fir n

  move       #wspolczynniki,r0
  move       #0,r1
  do  #N+1,wsploop
  move       p:(r0)+,x0
  move       x0,y:(r1)+
wsploop:



    movep #$260035,x:pctl    ; uaktywnij PLL i ustaw 50MHz
    movep #$000201,x:rcs     ;uaktywnij SAI

    movep #$000203,x:tcs     ;uaktywnij SAI
  ;  movep #$000101,x:brc     ;uaktywnij preskaler

    movep #$000c00,x:ipr
;    andi   #$0fc,mr   ;wlacz przerwania


      move #0,r0      ;poczatek bufora probek
      move #0,r4      ;pocz. buf. wsp.
      move #N-1,m0    ;arytm. mod N dla rej. adres
      move m0,m4      ;dotycz. obu buforow


      MOVE #(N/2)+1,N0
      MOVE #(N/2)+1,N1

      move #128,r1      ;poczatek bufora probek
      move #0,r5      ;pocz. buf. wsp.
      move #N-1,m1    ;arytm. mod N dla rej. adres
      move m1,m5      ;dotycz. obu buforow


 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
 move m7,m6                    ;set up mask value
 move #0,n7                     ;initialize n2
 move #sinep+1,r7              ;initialize r2 with the value r1+ 1

 clr a  #0,n2                    ;initialize a and n1
 move #mask,m3
 move m3,m2 ;set up mask value                                     petla:
 move #0,n3                     ;initialize n2                         nop
 move #cosinep+1,r3              ;initialize r2 with the value r1+ 1     jset #rldf,x:rcs,odbierz_lewy
 move #cosinep,r2

 move a1,y:buf_sin_x_i
 move a0,y:buf_sin_x_f
 move a1,y:buf_cos_x_i
 move a0,y:buf_cos_x_f


petla:
    jset #rldf,x:rcs,odbierz_lewy
    jset #rrdf,x:rcs,odbierz_prawy
    jmp petla

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

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





odbierz_lewy:
 ; ORI   #$03,mr   ;wYlacz przerwania
  bCLR #gdd1,x:gpior
 move y:buf_sin_x_i,a1
 move y:buf_sin_x_f,a0
     clr b
 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
 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
 move y:buf_delta_f,y0
 add y,a                    ;
 move b,y:sin_x               ;update calculation, output sample (1)
 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
 move a0,y:buf_sin_x_f





    move x:rx0,a
;    MOVE A,X:tx0
    move a,x:(r0)               ;zapisz probke do pamieci
    clr a    x:(r0)+,x0      y:(r4)+,y0
    do  #N-1,splot
    mac x0,y0,a    x:(r0)+,x0   y:(r4)+,y0
splot:
    macr x0,y0,a   (r0)-
    move a,x0            ;xh
 ;   MOVE A,X:tx1
    move y:cos_x,y0    ; cosinus
    MOVE y0,X:tx1
    clr b
    mac  x0,y0,b        ;B=Xh * cos
    move x:(r0+N),x0    ;xd
;    move x0,x:tx1
    move y:sin_x,y0   ;sinus
 ;   MOVE y0,X:tx1
    move b,a
;    neg a
    macr x0,y0,a
    move a,x:tx0
    bSET #gdd1,x:gpior
  ;      andi   #$0fc,mr   ;wlacz przerwania
    jmp petla
   rti




odbierz_prawy:
  bclr #gdd1,x:gpior
     clr a
 move y:buf_cos_x_i,a1
 move y:buf_cos_x_f,a0
     clr b
 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
 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
 move y:buf_delta_f,y0
 add y,a                    ;
 move b,y:cos_x               ;update calculation, output 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
 move a0,y:buf_cos_x_f


    move x:rx0,a
;    MOVE A,X:tx1
    move a,x:(r1)               ;zapisz probke do pamieci
    clr a    x:(r1)+,x0      y:(r5)+,y0
    do  #N-1,splot1
    mac x0,y0,a    x:(r1)+,x0   y:(r5)+,y0
splot1:
    macr x0,y0,a   (r1)-
    move a,x0            ;xh
 ;   MOVE A,X:tx1
    move y:cos_x,y0    ; cosinus
;    MOVE Y0,X:tx1
    clr b
    mac  x0,y0,b
    move x:(r1+N),x0    ;xd
 ;   MOVE x0,X:tx1
    move y:sin_x,y0   ;sinus
    MOVE y0,X:tx1
    move b,a
    macr x0,y0,a
    move a,x:tx0
    bSET #gdd1,x:gpior
  ;      andi   #$0fc,mr   ;wlacz przerwania
   jmp petla
   rti





