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