MSP430-FORTH/FixPoint.f

   1
   2 ; -----------------------------------------------------
   3 ; FIXPOINT.f
   4 ; -----------------------------------------------------
   5 \
   6 \ to see kernel options, download FastForthSpecs.f
   7 \ FastForth kernel options: MSP430ASSEMBLER, CONDCOMP, FIXPOINT_INPUT
   8 \
   9 \ TARGET SELECTION
  10 \ MSP_EXP430FR5739  MSP_EXP430FR5969    MSP_EXP430FR5994    MSP_EXP430FR6989
  11 \ MSP_EXP430FR2433  MSP_EXP430FR4133    MSP_EXP430FR2355    CHIPSTICK_FR2433
  12 \
  13 \ REGISTERS USAGE
  14 \ rDODOES to rEXIT must be saved before use and restored after
  15 \ scratch registers Y to S are free for use
  16 \ under interrupt, IP is free for use
  17 \
  18 \ PUSHM order : PSP,TOS, IP,  S,  T,  W,  X,  Y, rEXIT,rDOVAR,rDOCON, rDODOES, R3, SR,RSP, PC
  19 \ PUSHM order : R15,R14,R13,R12,R11,R10, R9, R8,  R7  ,  R6  ,  R5  ,   R4   , R3, R2, R1, R0
  20 \
  21 \ example : PUSHM #6,IP pushes IP,S,T,W,X,Y registers to return stack
  22 \
  23 \ POPM  order :  PC,RSP, SR, R3, rDODOES,rDOCON,rDOVAR,rEXIT,  Y,  X,  W,  T,  S, IP,TOS,PSP
  24 \ POPM  order :  R0, R1, R2, R3,   R4   ,  R5  ,  R6  ,  R7 , R8, R9,R10,R11,R12,R13,R14,R15
  25 \
  26 \ example : POPM #6,IP   pop Y,X,W,T,S,IP registers from return stack
  27 \
  28 \ FORTH conditionnals:  unary{ 0= 0< 0> }, binary{ = < > U< }
  29 \
  30 \ ASSEMBLER conditionnal usage with IF UNTIL WHILE  S<  S>=  U<   U>=  0=  0<>  0>=
  31 \ ASSEMBLER conditionnal usage with ?JMP ?GOTO      S<  S>=  U<   U>=  0=  0<>  0<
  32 \
  33
  34 PWR_STATE
  35
  36 [DEFINED] {FIXPOINT} [IF] {FIXPOINT} [THEN]     \ remove {FIXPOINT} if outside core
  37
  38 [UNDEFINED] {FIXPOINT} [IF]   \ don't replicate {FIXPOINT} inside core
  39
  40 MARKER {FIXPOINT}
  41
  42 \ https://forth-standard.org/standard/core/HOLDS
  43 \ Adds the string represented by addr u to the pictured numeric output string
  44 \ compilation use: <# S" string" HOLDS #>
  45 \ free chars area in the 32+2 bytes HOLD buffer = {26,23,2} chars with a 32 bits sized {hexa,decimal,binary} number.
  46 \ (2 supplementary bytes are room for sign - and decimal point)
  47 \ C HOLDS    addr u --
  48 CODE HOLDS
  49             MOV @PSP+,X     \ 2
  50             ADD TOS,X       \ 1 src
  51             MOV &HP,Y       \ 3 dst
  52 BEGIN       SUB #1,X        \ 1 src-1
  53             SUB #1,TOS      \ 1 cnt-1
  54 U>= WHILE   SUB #1,Y        \ 1 dst-1
  55             MOV.B @X,0(Y)   \ 4
  56 REPEAT      MOV Y,&HP       \ 3
  57             MOV @PSP+,TOS   \ 2
  58             MOV @IP+,PC     \ 4  15 words
  59 ENDCODE
  60
  61 CODE F+                     \ add Q15.16 numbers
  62     ADD @PSP+,2(PSP)        \ -- sumlo  d1hi d2hi
  63     ADDC @PSP+,TOS          \ -- sumlo sumhi
  64     MOV @IP+,PC
  65 ENDCODE
  66
  67 CODE F-                     \ substract Q15.16 numbers
  68     SUB @PSP+,2(PSP)        \ -- diflo d1hi d2hi
  69     SUBC TOS,0(PSP)         \ -- diflo difhi d2hi
  70     MOV @PSP+,TOS
  71     MOV @IP+,PC
  72 ENDCODE
  73
  74 $1A04 C@ $EF > [IF] ; test tag value MSP430FR413x subfamily without hardware_MPY
  75
  76 CODE F/                     \ Q15.16 / Q15.16 --> Q15.16 result
  77         PUSHM #4,R7
  78         MOV @PSP+,R6        \ DVRlo
  79         MOV @PSP+,X         \ DVDhi --> REMlo
  80         MOV #0,W            \ REMhi = 0
  81         MOV @PSP,Y          \ DVDlo --> DVDhi
  82         MOV #0,T            \ DVDlo = 0
  83         MOV X,S             \
  84         XOR TOS,S           \ DVDhi XOR DVRhi --> S keep sign of result
  85         AND #-1,X           \ DVD < 0 ?
  86 S< IF   XOR #-1,Y           \ INV(DVDlo)
  87         XOR #-1,X           \ INV(DVDhi)
  88         ADD #1,Y            \ INV(DVDlo)+1
  89         ADDC #0,X           \ INV(DVDhi)+C
  90 THEN    AND #-1,TOS         \ DVR < 0 ?
  91 S< IF   XOR #-1,R6          \ INV(DVRlo)
  92         XOR #-1,TOS         \ INV(DVRhi)
  93         ADD #1,R6           \ INV(DVRlo)+1
  94         ADDC #0,TOS         \ INV(DVRhi)+C
  95 THEN
  96 \ don't uncomment lines below !
  97 \ ------------------------------------------------------------------------
  98 \           UD/MOD    DVDlo DVDhi DVRlo DVRhi -- REMlo REMhi QUOTlo QUOThi
  99 \ ------------------------------------------------------------------------
 100 \           MOV 4(PSP),T    \ DVDlo
 101 \           MOV 2(PSP),Y    \ DVDhi
 102 \           MOV #0,X        \ REMlo = 0
 103 \           MOV #0,W        \ REMhi = 0
 104             MOV #32,R5      \  init loop count
 105 BW1         CMP TOS,W       \ 1 REMhi = DVRhi ?
 106     0= IF   CMP R6,X        \ 1 REMlo U< DVRlo ?
 107     THEN
 108     U>= IF  SUB R6,X        \ 1 no:  REMlo - DVRlo  (carry is set)
 109             SUBC TOS,W      \ 1      REMhi - DVRhi
 110     THEN
 111 BW2         ADDC R7,R7      \ 1 RLC quotLO
 112             ADDC R4,R4      \ 1 RLC quotHI
 113             SUB #1,R5       \ 1 Decrement loop counter
 114             0< ?GOTO FW1    \ 2 out of loop if count<0
 115             ADD T,T         \ 1 RLA DVDlo
 116             ADDC Y,Y        \ 1 RLC DVDhi
 117             ADDC X,X        \ 1 RLC REMlo
 118             ADDC W,W        \ 1 RLC REMhi
 119             U< ?GOTO BW1    \ 2 15~ loop
 120             SUB R6,X        \ 1 REMlo - DVRlo
 121             SUBC TOS,W      \ 1 REMhi - DVRhi
 122             BIS #1,SR       \ 1
 123             GOTO BW2        \ 2 16~ loop
 124 FW1
 125 \           MOV X,4(PSP)    \ REMlo
 126 \           MOV W,2(PSP)    \ REMhi
 127 \           ADD #4,PSP      \ skip REMlo REMhi
 128             MOV R7,0(PSP)   \ QUOTlo
 129             MOV R4,TOS      \ QUOThi
 130             POPM #4,R7      \ restore R4 to R7
 131 \           MOV @IP+,PC     \ end of UD/MOD
 132 \ ------------------------------------------------------------------------
 133 BW1     AND #-1,S           \ clear V, set N; QUOT < 0 ?
 134 S< IF   XOR #-1,0(PSP)      \ INV(QUOTlo)
 135         XOR #-1,TOS         \ INV(QUOThi)
 136         ADD #1,0(PSP)       \ INV(QUOTlo)+1
 137         ADDC #0,TOS         \ INV(QUOThi)+C
 138 THEN    MOV @IP+,PC
 139 ENDCODE
 140
 141 \ F#S    Qlo Qhi u -- Qhi 0   convert fractional part Qlo of Q15.16 fixed point number
 142 \                             with u digits
 143 CODE F#S
 144             MOV 2(PSP),X            \ -- Qlo Qhi u      X = Qlo
 145             MOV @PSP,2(PSP)         \ -- Qhi Qhi u
 146             MOV X,0(PSP)            \ -- Qhi Qlo u
 147             PUSHM #2,TOS            \                   save TOS,IP
 148             MOV #0,S                \ -- Qhi Qlo x
 149 BEGIN       PUSH S                  \                   R-- limit IP count
 150             MOV &BASE,TOS           \ -- Qhi Qlo base
 151             LO2HI
 152             UM*                     \                   u1 u2 -- RESlo REShi
 153             HI2LO                   \ -- Qhi RESlo digit
 154             SUB #2,IP
 155             CMP #10,TOS             \                   digit to char
 156     U>= IF  ADD #7,TOS
 157     THEN    ADD #$30,TOS
 158             MOV @RSP+,S             \                       R-- limit IP
 159             MOV.B TOS,HOLDS_ORG(S)  \ -- Qhi RESlo char     char to string
 160             ADD #1,S                \                       count+1
 161             CMP 2(RSP),S            \                       count=limit ?
 162 U>= UNTIL
 163             POPM #2,TOS             \                       restore IP,TOS
 164             MOV #0,0(PSP)           \ -- Qhi 0 len
 165             SUB #2,PSP              \ -- Qhi 0 x len
 166             MOV #HOLDS_ORG,0(PSP)   \ -- Qhi 0 addr len
 167             JMP HOLDS
 168 ENDCODE
 169
 170 \ unsigned multiply 32*32 = 64
 171 \ don't use S reg (keep sign)
 172 CODE UDM*
 173             PUSH IP         \ 3
 174             PUSHM #4,R7     \ 6 save R7 ~ R4 regs
 175             MOV 4(PSP),IP   \ 3 MDlo
 176             MOV 2(PSP),T    \ 3 MDhi
 177             MOV @PSP,W      \ 2 MRlo
 178             MOV #0,R4       \ 1 MDLO=0
 179             MOV #0,R5       \ 1 MDHI=0
 180             MOV #0,4(PSP)   \ 3 RESlo=0
 181             MOV #0,2(PSP)   \ 3 REShi=0
 182             MOV #0,R6       \ 1 RESLO=0
 183             MOV #0,R7       \ 1 RESHI=0
 184             MOV #1,X        \ 1 BIT TEST REGlo
 185             MOV #0,Y        \ 1 BIT TEST2 REGhi
 186 BEGIN       CMP #0,X
 187     0<> IF  BIT X,W         \ 2+1 TEST ACTUAL BIT MRlo
 188     ELSE    BIT Y,TOS       \ 2+1 TEST ACTUAL BIT MRhi
 189     THEN
 190     0<> IF  ADD IP,4(PSP)   \ 2+3 IF 1: ADD MDlo TO RESlo
 191             ADDC T,2(PSP)   \ 3      ADDC MDhi TO REShi
 192             ADDC R4,R6      \ 1      ADDC MDLO TO RESLO
 193             ADDC R5,R7      \ 1      ADDC MDHI TO RESHI
 194     THEN    ADD IP,IP       \ 1 (RLA LSBs) MDlo *2
 195             ADDC T,T        \ 1 (RLC MSBs) MDhi *2
 196             ADDC R4,R4      \ 1 (RLA LSBs) MDLO *2
 197             ADDC R5,R5      \ 1 (RLC MSBs) MDHI *2
 198             ADD X,X         \ 1 (RLA) NEXT BIT TO TEST
 199             ADDC Y,Y        \ 1 (RLA) NEXT BIT TO TEST
 200 U>= UNTIL   MOV R6,0(PSP)   \ 2+2 IF BIT IN CARRY: FINISHED    32 * 16~ (average loop)
 201             MOV R7,TOS      \ 1 high result in TOS
 202             POPM #4,R7      \ 6 restore R4 to R7
 203             MOV @RSP+,IP    \ 2
 204             MOV @IP+,PC
 205 ENDCODE
 206
 207 CODE F*                 \ s15.16 * s15.16 --> s15.16 result
 208     MOV 2(PSP),S        \
 209     XOR TOS,S           \ 1s15 XOR 2s15 --> S keep sign of result
 210     BIT #$8000,2(PSP)   \ MD < 0 ?
 211 0<> IF  XOR #-1,2(PSP)
 212         XOR #-1,4(PSP)
 213         ADD #1,4(PSP)
 214         ADDC #0,2(PSP)
 215 THEN
 216     COLON
 217     DABS UDM*           \ -- RES0 RES1 RES2 RES3
 218     HI2LO
 219     MOV @RSP+,IP
 220     MOV @PSP+,TOS       \ -- RES0 RES1 RES2
 221     MOV @PSP+,0(PSP)    \ -- RES1 RES2
 222     GOTO BW1            \ goto end of F/ to process sign of result
 223 ENDCODE
 224
 225 [ELSE] \ hardware multiplier
 226
 227 CODE F/                     \ Q15.16 / Q15.16 --> Q15.16 result
 228 \ TOS = DVRhi
 229 \ 0(PSP) = DVRlo
 230 \ 2(PSP) = DVDhi
 231 \ 4(PSP) = DVDlo
 232         PUSHM #4,R7         \ 6 PUSHM R7 to R4
 233         MOV @PSP+,R6        \ 2 DVRlo
 234         MOV @PSP+,X         \ 2 DVDhi --> REMlo
 235         MOV #0,W            \ 1 REMhi = 0
 236         MOV @PSP,Y          \ 2 DVDlo --> DVDhi
 237         MOV #0,T            \ 1 DVDlo = 0
 238         MOV X,S             \ 1
 239         XOR TOS,S           \ 1 DVDhi XOR DVRhi --> S keep sign of result
 240         AND #-1,X           \ 1 DVD < 0 ?
 241 S< IF   XOR #-1,Y           \ 1 INV(DVDlo)
 242         XOR #-1,X           \ 1 INV(DVDhi)
 243         ADD #1,Y            \ 1 INV(DVDlo)+1
 244         ADDC #0,X           \ 1 INV(DVDhi)+C
 245 THEN    AND #-1,TOS         \ 1 DVR < 0 ?
 246 S< IF   XOR #-1,R6          \ 1 INV(DVRlo)
 247         XOR #-1,TOS         \ 1 INV(DVRhi)
 248         ADD #1,R6           \ 1 INV(DVRlo)+1
 249         ADDC #0,TOS         \ 1 INV(DVRhi)+C
 250 THEN    MOV #32,R5          \ 2 init loop count
 251 BW1     CMP TOS,W           \ 1 REMhi = DVRhi ?
 252     0= IF                   \ 2
 253         CMP R6,X            \ 1 REMlo U< DVRlo ?
 254     THEN
 255     U>= IF                  \ 2
 256         SUB R6,X            \ 1 no:  REMlo - DVRlo  (carry is set)
 257         SUBC TOS,W          \ 1      REMhi - DVRhi
 258     THEN
 259 BW2     ADDC R7,R7          \ 1 RLC quotLO
 260         ADDC R4,R4          \ 1 RLC quotHI
 261         SUB #1,R5           \ 1 Decrement loop counter
 262         0< ?GOTO FW1        \ 2 out of loop if count<0
 263         ADD T,T             \ 1 RLA DVDlo
 264         ADDC Y,Y            \ 1 RLC DVDhi
 265         ADDC X,X            \ 1 RLC REMlo
 266         ADDC W,W            \ 1 RLC REMhi
 267         U< ?GOTO BW1        \ 2 19~ loop
 268         SUB R6,X            \ 1 REMlo - DVRlo
 269         SUBC TOS,W          \ 1 REMhi - DVRhi
 270         BIS #1,SR           \ 1
 271         GOTO BW2            \ 2 16~ loop
 272 FW1     AND #-1,S           \ 1 clear V, set N; QUOT < 0 ?
 273 S< IF   XOR #-1,R7          \ 1 INV(QUOTlo)
 274         XOR #-1,R4          \ 1 INV(QUOThi)
 275         ADD #1,R7           \ 1 INV(QUOTlo)+1
 276         ADDC #0,R4          \ 1 INV(QUOThi)+C
 277 THEN    MOV R7,0(PSP)       \ 3 QUOTlo
 278         MOV R4,TOS          \ 1 QUOThi
 279         POPM #4,R7          \ 6 restore R4 to R7
 280         MOV @IP+,PC         \ 4
 281 ENDCODE
 282
 283 \ F#S    Qlo Qhi u -- Qhi 0   convert fractionnal part of Q15.16 fixed point number
 284 \                             with u digits
 285 CODE F#S
 286             MOV 2(PSP),X            \ -- Qlo Qhi u      X = Qlo
 287             MOV @PSP,2(PSP)         \ -- Qhi Qhi u
 288             MOV X,0(PSP)            \ -- Qhi Qlo u
 289             MOV TOS,T               \                   T = limit
 290             MOV #0,S                \                   S = count
 291 BEGIN       MOV @PSP,&MPY           \                   Load 1st operand
 292             MOV &BASE,&OP2          \                   Load 2nd operand
 293             MOV &RES0,0(PSP)        \ -- Qhi RESlo x        low result on stack
 294             MOV &RES1,TOS           \ -- Qhi RESlo REShi    high result in TOS
 295             CMP #10,TOS             \                   digit to char
 296     U>= IF  ADD #7,TOS
 297     THEN    ADD #$30,TOS
 298             MOV.B TOS,HOLDS_ORG(S)  \ -- Qhi RESlo char     char to string
 299             ADD #1,S                \                   count+1
 300             CMP T,S                 \                   count=limit ?
 301 0= UNTIL    MOV #0,0(PSP)           \ -- Qhi 0 REShi
 302             MOV T,TOS               \ -- Qhi 0 limit
 303             SUB #2,PSP              \ -- Qhi 0 x len
 304             MOV #HOLDS_ORG,0(PSP)   \ -- Qhi 0 addr len
 305             JMP HOLDS
 306 ENDCODE
 307
 308 CODE F*                 \ signed s15.16 multiplication --> s15.16 result
 309     MOV 4(PSP),&MPYS32L \ 5 Load 1st operand
 310     MOV 2(PSP),&MPYS32H \ 5
 311     MOV @PSP,&OP2L      \ 4 load 2nd operand
 312     MOV TOS,&OP2H       \ 3
 313     ADD #4,PSP          \ 1 remove 2 cells
 314 \    NOP2                \ 2
 315 \    NOP2                \ 2 wait 8 cycles after write OP2L before reading RES1
 316     MOV &RES1,0(PSP)    \ 5
 317     MOV &RES2,TOS       \ 5
 318     MOV @IP+,PC
 319 ENDCODE
 320
 321 [THEN]  \ hardware multiplier
 322
 323 CODE F.             \ display a Q15.16 number with 4/5/16 digits after comma
 324 MOV TOS,S           \ S = sign
 325 MOV #4,T            \ T = 4     preset 4 digits for base 16 and by default
 326 MOV &BASE,W
 327 CMP ##10,W
 328 0= IF               \           if base 10
 329     ADD #1,T        \ T = 5     set 5 digits
 330 ELSE
 331     CMP #%10,W
 332     0= IF           \           if base 2
 333         MOV #16,T   \ T = 16    set 16 digits
 334     THEN
 335 THEN
 336 PUSHM #3,IP         \                   R-- IP sign #digit
 337 LO2HI
 338     <# DABS         \ -- uQlo uQhi      R-- IP sign #digit
 339     R> F#S          \ -- uQhi 0         R-- IP sign
 340     $2C HOLD        \                   $2C = char ','
 341     #S              \ -- 0 0
 342     R> SIGN #>      \ -- addr len       R-- IP
 343     TYPE SPACE      \ --
 344 ;
 345
 346 CODE S>F         \ convert a signed number to a Q15.16 (signed) number
 347     SUB #2,PSP
 348     MOV #0,0(PSP)
 349     MOV @IP+,PC
 350 ENDCODE
 351
 352 [UNDEFINED] 2@ [IF]
 353
 354 \ https://forth-standard.org/standard/core/TwoFetch
 355 \ 2@    a-addr -- x1 x2    fetch 2 cells ; the lower address will appear on top of stack
 356 CODE 2@
 357 SUB #2,PSP
 358 MOV 2(TOS),0(PSP)
 359 MOV @TOS,TOS
 360 NEXT
 361 ENDCODE
 362 [THEN] \ of [UNDEFINED] 2@
 363
 364 [UNDEFINED] 2CONSTANT [IF]
 365
 366 \ https://forth-standard.org/standard/double/TwoCONSTANT
 367 : 2CONSTANT \  udlo/dlo/Qlo udhi/dhi/Qhi --         to create double or Q15.16 CONSTANT
 368 CREATE , ,  \ compile Qhi then Qlo
 369 DOES> 2@    \ execution part    addr -- Qhi Qlo
 370 ;
 371
 372 [THEN] \ of [UNDEFINED] 2CONSTANT
 373
 374 RST_HERE
 375 [THEN] \ of [UNDEFINED] {FIXPOINT}
 376
 377 ECHO
 378
 379 ; -----------------------
 380 ; (volatile) tests
 381 ; -----------------------
 382
 383
 384 3,14159 2CONSTANT PI
 385 PI -1,0 F* 2CONSTANT -PI
 386
 387 $10 BASE !  PI F.
 388            -PI F.
 389 %10 BASE !  PI F.
 390            -PI F.
 391 #10 BASE !  PI F.
 392            -PI F.
 393
 394 PI 2,0 F* F.
 395 PI -2,0 F* F.
 396 -PI 2,0 F* F.
 397 -PI -2,0 F* F.
 398
 399 PI 2,0 F/ F.
 400 PI -2,0 F/ F.
 401 -PI 2,0 F/ F.
 402 -PI -2,0 F/ F.
 403
 404 32767,99999 1,0 f* F.
 405 32767,99999 1,0 f/ F.
 406 32767,99999 2,0 f/ F.
 407 32767,99999 4,0 f/ F.
 408 32767,99999 8,0 f/ F.
 409 32767,99999 16,0 f/ F.
 410
 411 -32767,0 -1,0 f* F.
 412 -32767,0 -1,0 f/ F.
 413 -32767,0 -2,0 f/ F.
 414 -32767,0 -4,0 f/ F.
 415 -32767,0 -8,0 f/ F.
 416 -32767,0 -16,0 f/ F.
 417 -32767,0 -32,0 f/ F.
 418 -32767,0 -64,0 f/ F.
 419
 420 ; sqrt(32768)^2 = 32768
 421 181,01933598375 181,01933598375 f* f.
 422 181,01933598375 -181,01933598375 f* f.
 423 -181,01933598375 181,01933598375 f* f.
 424 -181,01933598375 -181,01933598375 f* f.