MSP430-FORTH/FixPoint.f

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