; ----------
-; CORDIC.4th
+; CORDIC.4th for MSP_EXP430FR5994
; ----------
+; -----------------------------------------------------------
+; requires FIXPOINT_INPUT kernel addon, see forthMSP430FR.asm
+; -----------------------------------------------------------
-PWR_STATE
+[DEFINED] {CORDIC} [IF] {CORDIC} [THEN]
-: DEFINED! ECHO 1 ABORT" already loaded!" ;
+MARKER {CORDIC}
-[DEFINED] {CORDIC} [IF] DEFINED!
-[ELSE]
-MARKER {CORDIC}
+CREATE T_ARCTAN
+11520 ,
+6801 ,
+3593 ,
+1824 ,
+916 ,
+458 ,
+229 ,
+115 ,
+57 ,
+29 ,
+14 ,
+7 ,
+4 ,
+2 ,
+1 ,
-[UNDEFINED] {FIXPOINT} [IF]
+CREATE T_SCALE
+46340 ,
+41448 ,
+40211 ,
+39900 ,
+39822 ,
+39803 ,
+39798 ,
+39797 ,
+39797 ,
+39797 ,
+39797 ,
+39797 ,
+39797 ,
+39797 ,
+39797 ,
+[UNDEFINED] @ [IF]
+CODE @
+MOV @R14,R14
+MOV @R13+,R0
+ENDCODE
+[THEN]
+
+[UNDEFINED] R> [IF]
+CODE R>
+SUB #2,R15
+MOV R14,0(R15)
+MOV @R1+,R14
+MOV @R13+,R0
+ENDCODE
+[THEN]
+
+[UNDEFINED] = [IF]
+CODE =
+SUB @R15+,R14
+0<> IF
+ AND #0,R14
+ MOV @R13+,R0
+THEN
+XOR #-1,R14
+MOV @R13+,R0
+ENDCODE
+[THEN]
+
+[UNDEFINED] U< [IF]
+CODE U<
+SUB @R15+,R14
+0<> IF
+ MOV #-1,R14
+ U< IF
+ AND #0,R14
+ THEN
+THEN
+MOV @R13+,R0
+ENDCODE
+[THEN]
+
+[UNDEFINED] DABS [IF]
+CODE DABS
+AND #-1,R14
+S< IF
+ XOR #-1,0(R15)
+ XOR #-1,R14
+ ADD #1,0(R15)
+ ADDC #0,R14
+THEN
+MOV @R13+,R0
+ENDCODE
+[THEN]
+
+[UNDEFINED] HOLDS [IF]
CODE HOLDS
MOV @R15+,R9
- ADD R14,R9
+BW3 ADD R14,R9
MOV &$1DB2,R8
BEGIN SUB #1,R9
SUB #1,R14
MOV @R15+,R14
MOV @R13+,R0
ENDCODE
+[THEN]
+
+$81EF $1A04 @ U<
+$1A04 @ $81F3 U<
+= [IF] ; MSP430FR413x subfamily without hardware_MPY
+
+[UNDEFINED] F#S [IF]
+CODE F#S
+ MOV @R15,R12
+ MOV #0,R11
+ PUSHM #3,R13
+ MOV 2(R15),0(R15)
+ MOV R14,2(R15)
+BEGIN MOV &$1DDC,R14
+ LO2HI
+ UM*
+ HI2LO
+ CMP #10,R14
+ U>= IF ADD #7,R14
+ THEN ADD #$30,R14
+ MOV @R1,R11
+ MOV.B R14,$1D90(R11)
+ ADD #1,R11
+ MOV R11,0(R1)
+ CMP 2(R15),R11
+U>= UNTIL POPM #3,R13
+ MOV R11,R14
+ MOV R12,2(R15)
+ MOV #0,0(R15)
+ MOV #$1D90,R9
+ GOTO BW3
+ENDCODE
+[THEN]
+
+ASM XSCALE
+SUB #4,R15
+MOV R14,2(R15)
+MOV R9,0(R15)
+MOV T_SCALE(R10),R14
+MOV @R15,R12
+CMP #0,R12
+S< IF
+ XOR #-1,0(R15)
+ ADD #1,0(R15)
+THEN
+PUSHM #6,R13
+LO2HI
+UM*
+HI2LO
+POPM #6,R13
+CMP #0,R12
+S< IF
+ XOR #-1,0(R15)
+ XOR #-1,R14
+ ADD #1,0(R15)
+ ADDC #0,R14
+THEN
+MOV R14,R9
+ADD #2,R15
+MOV @R15+,R14
+MOV @R1+,R0
+ENDASM
+
+
+[ELSE] ; hardware multiplier
+
+[UNDEFINED] F#S [IF]
CODE F#S
MOV 2(R15),R9
MOV @R15,2(R15)
MOV R14,R11
MOV #0,R12
BEGIN MOV @R15,&$4C0
- MOV &BASE,&$4C8
+ MOV &$1DDC,&$4C8
MOV &$4E4,0(R15)
MOV &$4E6,R14
CMP #10,R14
MOV.B R14,$1D90(R12)
ADD #1,R12
CMP R11,R12
-0= UNTIL MOV #0,0(R15)
- MOV R11,R14
- SUB #2,R15
- MOV #$1D90,0(R15)
- JMP HOLDS
+0= UNTIL MOV R11,R14
+ MOV #0,0(R15)
+ MOV #$1D90,R9
+ GOTO BW3
ENDCODE
+[THEN]
+ASM XSCALE
+MOV R9,&$4C0
+MOV T_SCALE(R10),&$4C8
+MOV &$4E6,R9
+MOV @R1+,R0
+ENDASM
+
+[THEN] ; endcase of hardware multiplier
+
+
+[UNDEFINED] F. [IF]
CODE F.
MOV R14,R12
MOV #4,R11
-MOV &BASE,R10
+MOV &$1DDC,R10
CMP ##10,R10
0= IF
ADD #1,R11
$2C HOLD
#S
R> SIGN #>
- TYPE SPACE
+ TYPE $20 EMIT
;
[THEN]
-
-CREATE T_ARCTAN
-11520 ,
-6801 ,
-3593 ,
-1824 ,
-916 ,
-458 ,
-229 ,
-115 ,
-57 ,
-29 ,
-14 ,
-7 ,
-4 ,
-2 ,
-1 ,
-
-CREATE T_SCALE
-46340 ,
-41448 ,
-40211 ,
-39900 ,
-39822 ,
-39803 ,
-39798 ,
-39797 ,
-39797 ,
-39797 ,
-39797 ,
-39797 ,
-39797 ,
-39797 ,
-39797 ,
-
-
CODE POL2REC
PUSH R13
MOV @R15+,R8
RRA R12
RRA R11
ADD #1,R10
- FW1 CMP R13,R10
+FW1 CMP R13,R10
0= UNTIL
ADD R10,R10
CMP #0,R14
CMP #14,R13
0= UNTIL
THEN
-MOV R9,&$4C0
-MOV T_SCALE(R10),&$4C8
-MOV &$4E6,0(R15)
-MOV R8,&$4C0
-MOV T_SCALE(R10),&$4C8
-MOV &$4E6,R14
+CALL #XSCALE
+MOV R9,0(R15)
+MOV R8,R9
+CALL #XSCALE
+MOV R9,R14
MOV @R1+,R13
MOV @R13+,R0
ENDCODE
RRA R12
RRA R11
ADD #1,R10
- FW1 CMP R13,R10
+FW1 CMP R13,R10
0= UNTIL
ADD R10,R10
CMP #0,R8
CMP #14,R13
0= UNTIL
THEN
-MOV R9,&$4C0
-MOV T_SCALE(R10),&$4C8
-MOV &$4E6,R9
+CALL #XSCALE
POPM #2,R13
GOTO FW1
BEGIN
MOV @R13+,R0
ENDCODE
-RST_HERE
+PWR_HERE
-[THEN]
+[UNDEFINED] SWAP [IF]
+CODE SWAP
+MOV @R15,R10
+MOV R14,0(R15)
+MOV R10,R14
+MOV @R13+,R0
+ENDCODE
+[THEN]
+
+[UNDEFINED] ROT [IF]
+CODE ROT
+MOV @R15,R10
+MOV R14,0(R15)
+MOV 2(R15),R14
+MOV R10,2(R15)
+MOV @R13+,R0
+ENDCODE
+[THEN]
+
+[UNDEFINED] DO [IF]
+CODE DO
+SUB #2,R15
+MOV R14,0(R15)
+ADD #2,&$1DC6
+MOV &$1DC6,R14
+MOV #$404C,-2(R14)
+ADD #2,&$1C00
+MOV &$1C00,R10
+MOV #0,0(R10)
+MOV @R13+,R0
+ENDCODE IMMEDIATE
-: 2000CORDIC
-1000 0 DO
+CODE LOOP
+ MOV #$406E,R9
+BW1 ADD #4,&$1DC6
+ MOV &$1DC6,R10
+ MOV R9,-4(R10)
+ MOV R14,-2(R10)
+BEGIN
+ MOV &$1C00,R14
+ SUB #2,&$1C00
+ MOV @R14,R14
+ CMP #0,R14
+0<> WHILE
+ MOV R10,0(R14)
+REPEAT
+ MOV @R15+,R14
+ MOV @R13+,R0
+ENDCODE IMMEDIATE
+
+CODE +LOOP
+MOV #$405C,R9
+GOTO BW1
+ENDCODE IMMEDIATE
+[THEN]
+
+: 1000CORDIC
+500 0 DO
POL2REC REC2POL
LOOP
;
ECHO
+; -----------------------------------------------------------
+; requires FIXPOINT_INPUT kernel addon, see forthMSP430FR.asm
+; -----------------------------------------------------------
+
10000 89,0 POL2REC . . ; sin, cos -->
10000 75,0 POL2REC . . ; sin, cos -->
10000 60,0 POL2REC . . ; sin, cos -->
1000 -1000 REC2POL F. . ; phase module -->
16000 8000 REC2POL F. . ; phase module -->
16000 -8000 REC2POL F. . ; phase module -->
-16000 0 REC2POL F. . ; phase module -->
-0 16000 REC2POL F. . ; phase module -->
+16000 0 REC2POL F. . ; phase module -->
+0 16000 REC2POL F. . ; phase module -->
10000 89,0 POL2REC REC2POL ROT . F.
10000 7,125 POL2REC REC2POL ROT . F.
10000 1,0 POL2REC REC2POL ROT . F.
-10000 89,0 2000CORDIC ROT . F.
-10000 75,0 2000CORDIC ROT . F.
-10000 60,0 2000CORDIC ROT . F.
-10000 45,0 2000CORDIC ROT . F.
-10000 30,0 2000CORDIC ROT . F.
-10000 26,565 2000CORDIC ROT . F.
-10000 15,0 2000CORDIC ROT . F.
-10000 14,036 2000CORDIC ROT . F.
-10000 7,125 2000CORDIC ROT . F.
-10000 1,0 2000CORDIC ROT . F.
-
+10000 89,0 1000CORDIC ROT . F.
+10000 75,0 1000CORDIC ROT . F.
+10000 60,0 1000CORDIC ROT . F.
+10000 45,0 1000CORDIC ROT . F.
+10000 30,0 1000CORDIC ROT . F.
+10000 26,565 1000CORDIC ROT . F.
+10000 15,0 1000CORDIC ROT . F.
+10000 14,036 1000CORDIC ROT . F.
+10000 7,125 1000CORDIC ROT . F.
+10000 1,0 1000CORDIC ROT . F.
+PWR_STATE