MSP430-FORTH/FixPoint.f

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