MSP430_FORTH/FixPoint.f

   1 \ TARGET SELECTION
   2 \ MSP_EXP430FR5739  MSP_EXP430FR5969    MSP_EXP430FR5994    MSP_EXP430FR6989
   3 \ MSP_EXP430FR4133  CHIPSTICK_FR2433
   4 \ MY_MSP430FR5738_1 MY_MSP430FR5738     MY_MSP430FR5948     MY_MSP430FR5948_1
   5 \ JMJ_BOX
   6
   7
   8
   9 PWR_STATE
  10     \
  11 [DEFINED] {FIXPOINT} [IF] {FIXPOINT} [THEN]     \ remove {FIXPOINT} if outside core
  12     \
  13 [DEFINED] ASM [UNDEFINED] {FIXPOINT} AND [IF]   \ assembler required, don't replicate {FIXPOINT} inside core
  14     \
  15 MARKER {FIXPOINT}
  16     \
  17 \ https://forth-standard.org/standard/core/HOLDS
  18 \ Adds the string represented by addr u to the pictured numeric output string
  19 \ compilation use: <# S" string" HOLDS #>
  20 \ free chars area in the 32+2 bytes HOLD buffer = {26,23,2} chars with a 32 bits sized {hexa,decimal,binary} number.
  21 \ (2 supplementary bytes are room for sign - and decimal point)
  22 \ C HOLDS    addr u --
  23 CODE HOLDS
  24             MOV @PSP+,X     \ 2
  25             ADD TOS,X       \ 1 src
  26             MOV &HP,Y       \ 3 dst
  27 BEGIN       SUB #1,X        \ 1 src-1
  28             SUB #1,TOS      \ 1 cnt-1
  29 U>= WHILE   SUB #1,Y        \ 1 dst-1
  30             MOV.B @X,0(Y)   \ 4
  31 REPEAT      MOV Y,&HP       \ 3
  32             MOV @PSP+,TOS   \ 2
  33             MOV @IP+,PC     \ 4  15 words
  34 ENDCODE
  35     \
  36 CODE F+                 \ add s15q16 numbers
  37     ADD @PSP+,2(PSP)    \ -- sumlo  d1hi d2hi
  38     ADDC @PSP+,TOS      \ -- sumlo sumhi
  39     MOV @IP+,PC
  40 ENDCODE
  41     \
  42 CODE F-                 \ substract s15q16 numbers
  43     SUB @PSP+,2(PSP)    \ -- diflo d1hi d2hi
  44     SUBC TOS,0(PSP)     \ -- diflo difhi d2hi
  45     MOV @PSP+,TOS
  46     MOV @IP+,PC
  47 ENDCODE
  48     \
  49 CODE F/                     \ s15q16 / s15q16 --> s15q16 result
  50         MOV 2(PSP),S        \
  51         XOR TOS,S           \ MDhi XOR MRhi --> S keep sign of result
  52         MOV #0,T            \ DVDlo = 0
  53         MOV 4(PSP),Y        \ DVDlo --> DVDhi
  54         MOV 2(PSP),X        \ DVDhi --> REMlo
  55         BIT #8000,X         \ MD < 0 ?
  56 0<> IF  XOR #-1,Y           \ lo
  57         XOR #-1,X           \ hi
  58         ADD #1,Y            \ lo
  59         ADDC #0,X           \ hi
  60 THEN    BIT #8000,TOS
  61 0<> IF  XOR #-1,0(PSP)
  62         XOR #-1,TOS
  63         ADD #1,0(PSP)
  64         ADDC #0,TOS
  65 THEN
  66 \ don't uncomment lines below !
  67 \ ------------------------------------------------------------------------
  68 \           UD/MOD    DVDlo DVDhi DVRlo DVRhi -- REMlo REMhi QUOTlo QUOThi
  69 \ ------------------------------------------------------------------------
  70 \           MOV 4(PSP),T    \ DVDlo
  71 \           MOV 2(PSP),Y    \ DVDhi
  72 \           MOV #0,X        \ REMlo = 0
  73             PUSHM R7,R4
  74             MOV #0,W        \  REMhi = 0
  75             MOV @PSP,R6     \  DIVlo
  76             MOV #32,R5      \  init loop count
  77 BW1         CMP TOS,W       \ 1 REMhi = DIVhi ?
  78     0= IF   CMP R6,X        \ 1 REMlo U< DIVlo ?
  79     THEN
  80     U>= IF  SUB R6,X        \ 1 no:  REMlo - DIVlo  (carry is set)
  81             SUBC TOS,W      \ 1      REMhi - DIVhi
  82     THEN
  83 BW2         ADDC R7,R7      \ 1 RLC quotLO
  84             ADDC R4,R4      \ 1 RLC quotHI
  85             SUB #1,R5       \ 1 Decrement loop counter
  86             0< ?GOTO FW1    \ 2 out of loop if count<0
  87             ADD T,T         \ 1 RLA DVDlo
  88             ADDC Y,Y        \ 1 RLC DVDhi
  89             ADDC X,X        \ 1 RLC REMlo
  90             ADDC W,W        \ 1 RLC REMhi
  91             U< ?GOTO BW1    \ 2 15~ loop
  92             SUB R6,X        \ 1 REMlo - DIVlo
  93             SUBC TOS,W      \ 1 REMhi - DIVhi
  94             BIS #1,SR       \ 1
  95             GOTO BW2        \ 2 16~ loop
  96 FW1
  97 \           MOV X,4(PSP)    \ REMlo
  98 \           MOV W,2(PSP)    \ REMhi
  99             ADD #4,PSP      \ skip REMlo REMhi
 100
 101             MOV R7,0(PSP)   \ QUOTlo
 102             MOV R4,TOS      \ QUOThi
 103             POPM R4,R7      \ restore R7 to R4
 104 \           MOV @IP+,PC     \ end of UD/MOD
 105 \ ------------------------------------------------------------------------
 106 BW1     AND #-1,S           \ clear V, set N
 107 S< IF   XOR #-1,0(PSP)
 108         XOR #-1,TOS
 109         ADD #1,0(PSP)
 110         ADDC #0,TOS
 111 THEN    MOV @IP+,PC
 112 ENDCODE
 113     \
 114 $1A04 C@ $EF > [IF] ; test tag value MSP430FR413x subfamily without hardware_MPY
 115     \
 116 \ F#S    Shi Flo -- Shi 0   convert fractional part Flo of S15Q16 fixed point number
 117 CODE F#S
 118             SUB #2,PSP              \ -- Shi x Flo
 119             MOV TOS,0(PSP)          \ -- Shi Flo x
 120             MOV #4,TOS              \ -- Shi Flo x      TOS = limit for base 16
 121             CMP #10,&BASE
 122 0= IF       ADD #1,TOS              \                   TOS = limit for base 10
 123 THEN        PUSHM TOS,IP            \
 124             MOV #0,S                \ -- Shi Flo x
 125 BEGIN       PUSH S                  \                   R-- limit IP count
 126             MOV &BASE,TOS           \ -- Shi Flo base
 127             LO2HI
 128             UM*                     \                   u1 u2 -- RESlo REShi
 129             HI2LO                   \ -- Shi RESlo digit
 130             SUB #2,IP
 131             CMP #10,TOS             \                   digit to char
 132     U>= IF  ADD #7,TOS
 133     THEN    ADD #$30,TOS
 134             MOV @RSP+,S             \                       R-- limit IP
 135             MOV.B TOS,HOLDS_ORG(S)  \ -- Shi RESlo char     char to string
 136             ADD #1,S                \                       count+1
 137             CMP 2(RSP),S            \                       count=limit ?
 138 U>= UNTIL   POPM IP,TOS             \
 139             MOV #0,0(PSP)           \ -- Shi 0 len
 140             SUB #2,PSP              \ -- Shi 0 x len
 141             MOV #HOLDS_ORG,0(PSP)   \ -- Shi 0 addr len
 142             JMP HOLDS
 143 ENDCODE
 144     \
 145 \ unsigned multiply 32*32 = 64
 146 \ don't use S reg (keep sign)
 147 CODE UDM*
 148             PUSH IP         \ 3
 149             PUSHM R7,R4     \ 6 save R7 ~ R4 regs
 150             MOV 4(PSP),IP   \ 3 MDlo
 151             MOV 2(PSP),T    \ 3 MDhi
 152             MOV @PSP,W      \ 2 MRlo
 153             MOV #0,R4       \ 1 MDLO=0
 154             MOV #0,R5       \ 1 MDHI=0
 155             MOV #0,4(PSP)   \ 3 RESlo=0
 156             MOV #0,2(PSP)   \ 3 REShi=0
 157             MOV #0,R6       \ 1 RESLO=0
 158             MOV #0,R7       \ 1 RESHI=0
 159             MOV #1,X        \ 1 BIT TEST REGlo
 160             MOV #0,Y        \ 1 BIT TEST2 REGhi
 161 BEGIN       CMP #0,X
 162     0<> IF  BIT X,W         \ 1 TEST ACTUAL BIT MRlo
 163     ELSE    BIT Y,TOS       \ 1 TEST ACTUAL BIT MRhi
 164     THEN
 165     0<> IF  ADD IP,4(PSP)   \ 3 IF 1: ADD MDlo TO RESlo
 166             ADDC T,2(PSP)   \ 3      ADDC MDhi TO REShi
 167             ADDC R4,R6      \ 1      ADDC MDLO TO RESLO
 168             ADDC R5,R7      \ 1      ADDC MDHI TO RESHI
 169     THEN    ADD IP,IP       \ 1 (RLA LSBs) MDlo *2
 170             ADDC T,T        \ 1 (RLC MSBs) MDhi *2
 171             ADDC R4,R4      \ 1 (RLA LSBs) MDLO *2
 172             ADDC R5,R5      \ 1 (RLC MSBs) MDHI *2
 173             ADD X,X         \ 1 (RLA) NEXT BIT TO TEST
 174             ADDC Y,Y        \ 1 (RLA) NEXT BIT TO TEST
 175 U>= UNTIL   MOV R6,0(PSP)   \ 2 IF BIT IN CARRY: FINISHED    32 * 16~ (average loop)
 176             MOV R7,TOS      \ 1 high result in TOS
 177             POPM R4,R7      \ 6 restore R4 ~ R7 regs
 178             MOV @RSP+,IP    \ 2
 179             MOV @IP+,PC
 180 ENDCODE
 181     \
 182 CODE F*                 \ s15.16 * s15.16 --> s15.16 result
 183     MOV 2(PSP),S        \
 184     XOR TOS,S           \ 1s15 XOR 2s15 --> S keep sign of result
 185     BIT #8000,2(PSP)    \ MD < 0 ?
 186 0<> IF  XOR #-1,2(PSP)
 187         XOR #-1,4(PSP)
 188         ADD #1,4(PSP)
 189         ADDC #0,2(PSP)
 190 THEN
 191     COLON
 192     DABS UDM*           \ -- RES0 RES1 RES2 RES3
 193     HI2LO
 194     MOV @RSP+,IP
 195     MOV @PSP+,TOS       \ -- RES0 RES1 RES2
 196     MOV @PSP+,0(PSP)    \ -- RES1 RES2
 197     GOTO BW1            \ goto end of F/ to process sign of result
 198 ENDCODE
 199     \
 200 [ELSE]                  \ hardware multiplier
 201     \
 202 \ F#S    Shi Flo -- Shi 0   convert fractionnal part of S15Q16 fixed point number (direct order)
 203 CODE F#S
 204             SUB #2,PSP              \ -- Shi x Flo
 205             MOV TOS,0(PSP)          \ -- Shi Flo x
 206             MOV #4,T                \ -- Shi Flo x      T = limit for base 16
 207             CMP #10,&BASE
 208 0= IF       ADD #1,T                \                   T = limit for base 10
 209 THEN        MOV #0,S                \                   S = count
 210 BEGIN       MOV @PSP,&MPY           \                   Load 1st operand
 211             MOV &BASE,&OP2          \                   Load 2nd operand
 212             MOV &RES0,0(PSP)        \ -- Shi RESlo x        low result on stack
 213             MOV &RES1,TOS           \ -- Shi RESlo REShi    high result in TOS
 214             CMP #10,TOS             \                   digit to char
 215     U>= IF  ADD #7,TOS
 216     THEN    ADD #$30,TOS
 217             MOV.B TOS,HOLDS_ORG(S)  \ -- Shi RESlo char     char to string
 218             ADD #1,S                \                   count+1
 219             CMP T,S                 \                   count=limit ?
 220 U>= UNTIL   MOV T,TOS               \ -- Shi RESlo limit
 221             MOV #0,0(PSP)           \ -- Shi 0 limit
 222             SUB #2,PSP              \ -- Shi 0 x len
 223             MOV #HOLDS_ORG,0(PSP)   \ -- Shi 0 addr len
 224             JMP HOLDS
 225 ENDCODE
 226     \
 227 CODE F*                 \ signed s15.16 multiplication --> s15.16 result
 228     MOV 4(PSP),&MPYS32L \ 5 Load 1st operand
 229     MOV 2(PSP),&MPYS32H \ 5
 230     MOV @PSP,&OP2L      \ 4 load 2nd operand
 231     MOV TOS,&OP2H       \ 3
 232     ADD #4,PSP          \ 1 remove 2 cells
 233     NOP2                \ 2
 234     NOP2                \ 2 wait 8 cycles after write OP2L before reading RES1
 235     MOV &RES1,0(PSP)    \ 5
 236     MOV &RES2,TOS       \ 5
 237     MOV @IP+,PC
 238 ENDCODE
 239     \
 240 [THEN]  \ hardware multiplier
 241     \
 242 : F.                \ display a s15q16 number
 243     <# DUP >R DABS  \ -- udlo udhi          R-- sign
 244     SWAP            \ -- sign udhi udlo
 245     F#S             \ -- sign udhi 0
 246     $2C HOLD #S     \ -- sign 0 0
 247     R> SIGN #>      \ -- addr len           R--
 248     TYPE SPACE      \ --
 249     EXIT
 250 ;
 251     \
 252 CODE S>F         \ convert a signed number to a s15q16 (signed) number
 253     SUB #2,PSP
 254     MOV #0,0(PSP)
 255     MOV @IP+,PC
 256 ENDCODE
 257     \
 258 CODE D>F         \ convert a signed double number (-.32768|.32767) to a s15q16 (signed) number
 259     MOV @PSP,TOS
 260     MOV #0,0(PSP)
 261     MOV @IP+,PC
 262 ENDCODE
 263     \
 264
 265 \ https://forth-standard.org/standard/double/TwoCONSTANT
 266 : 2CONSTANT \  udlo/dlo/Flo udhi/dhi/Shi --         to create double or s15q16 CONSTANT
 267 CREATE
 268 SWAP , ,            \ compile Flo then Shi
 269 DOES>
 270 HI2LO
 271 MOV @RSP+,IP
 272 SUB #2,PSP          \ -- x PFA
 273 MOV @TOS+,0(PSP)    \ -- lo PFA+2
 274 MOV @TOS,TOS        \ -- lo hi
 275 MOV @IP+,PC
 276 ENDCODE
 277     \
 278 [THEN]
 279     \
 280 ECHO
 281 PWR_HERE
 282     \
 283 ; -----------------------
 284 ; (volatile) tests
 285 ; -----------------------
 286 3,14159 2CONSTANT PI
 287 PI -1,0 F* 2CONSTANT -PI
 288     \
 289 PI 2,0 F/ F.
 290 PI 2,0 F* F.
 291 PI -2,0 F/ F.
 292 PI -2,0 F* F.
 293 -PI 2,0 F/ F.
 294 -PI 2,0 F* F.
 295 -PI -2,0 F/ F.
 296 -PI -2,0 F* F.