ADDON/FixPoint.asm

   1 ; -*- coding: utf-8 -*-
   2 ; http://patorjk.com/software/taag/#p=display&f=Banner&t=Fast Forth
   3
   4 ; Fast Forth For Texas Instrument MSP430FRxxxx FRAM devices
   5 ; Copyright (C) <2015>  <J.M. THOORENS>
   6 ;
   7 ; This program is free software: you can redistribute it and/or modify
   8 ; it under the terms of the GNU General Public License as published by
   9 ; the Free Software Foundation, either version 3 of the License, or
  10 ; (at your option) any later version.
  11 ;
  12 ; This program is distributed in the hope that it will be useful,
  13 ; but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15 ; GNU General Public License for more details.
  16 ;
  17 ; You should have received a copy of the GNU General Public License
  18 ; along with this program.  If not, see <http://www.gnu.org/licenses/>.
  19
  20
  21
  22     FORTHWORD "{FIXPOINT}"
  23     mNEXT
  24
  25 ; https://forth-standard.org/standard/core/HOLDS
  26 ; Adds the string represented by addr u to the pictured numeric output string
  27 ; compilation use: <# S" string" HOLDS #>
  28 ; free chars area in the 32+2 bytes HOLD buffer = {26,23,2} chars with a 32 bits sized {hexa,decimal,binary} number.
  29 ; (2 supplementary bytes are room for sign - and decimal point)
  30 ; perfect to display all a line on LCD 2x20 chars...
  31 ; C HOLDS    addr u --
  32     FORTHWORD "HOLDS"
  33 HOLDS       MOV @PSP+,X     ; 2
  34 HOLDS1      ADD TOS,X       ; 1 src
  35             MOV &HP,Y       ; 3 dst
  36 HOLDSLOOP   SUB #1,X        ; 1 src-1
  37             SUB #1,TOS      ; 1 cnt-1
  38             JLO HOLDSNEXT   ; 2
  39             SUB #1,Y        ; 1 dst-1
  40             MOV.B @X,0(Y)   ; 4
  41             JMP HOLDSLOOP   ; 2
  42 HOLDSNEXT   MOV Y,&HP       ; 3
  43             MOV @PSP+,TOS   ; 2
  44             mNEXT            ; 4  15 words
  45
  46         FORTHWORD "F+"      ; -- d1lo d1hi d2lo d2hi
  47         ADD @PSP+,2(PSP)    ; -- sumlo  d1hi d2hi
  48         ADDC @PSP+,TOS      ; -- sumlo sumhi
  49         MOV @IP+,PC
  50
  51         FORTHWORD "F-"      ; -- d1lo d1hi d2lo d2hi
  52         SUB @PSP+,2(PSP)    ; -- diflo d1hi d2hi
  53         SUBC TOS,0(PSP)     ; -- diflo difhi d2hi
  54         MOV @PSP+,TOS
  55         MOV @IP+,PC
  56
  57     .IFDEF MPY ; hardware multiplier
  58
  59        FORTHWORD "F/"      ; s15.16 / s15.16 --> s15.16 result
  60 FDIV
  61         PUSHM #4,R7         ; PUSHM R7,R4
  62         MOV @PSP+,R6        ; DIVlo
  63         MOV @PSP+,X         ; DVDhi --> REMlo
  64         MOV #0,W            ; REMhi = 0
  65         MOV @PSP,Y          ; DVDlo --> DVDhi
  66         MOV #0,T            ; DVDlo = 0
  67         MOV X,S             ;
  68         XOR TOS,S           ; MDhi XOR MRhi --> S keep sign of result
  69         AND #-1,X           ; MD < 0 ?
  70         JGE FDIV1           ; no
  71         XOR #-1,Y           ; lo
  72         XOR #-1,X           ; hi
  73         ADD #1,Y            ; lo
  74         ADDC #0,X           ; hi
  75 FDIV1   AND #-1,TOS
  76         JGE FDIV2
  77         XOR #-1,R6
  78         XOR #-1,TOS
  79         ADD #1,R6
  80         ADDC #0,TOS
  81 FDIV2
  82 ; unsigned 32-BIT DIVIDEND : 32-BIT DIVISOR --> 32-BIT QUOTIENT, 32-BIT REMAINDER
  83 ; DVDhi|DVDlo : DVRhi|DVRlo --> QUOThi|QUOTlo, REMAINDER
  84 ;            FORTHWORD "UD/MOD"
  85 ;            MOV 4(PSP),T   ; DVDlo
  86 ;            MOV 2(PSP),Y   ; DVDhi
  87 ;            MOV #0,X       ; REMlo = 0
  88 Q6432       MOV #32,R5      ; init loop count
  89 Q321        CMP TOS,W       ;1 REMhi <> DIVhi ?
  90             JNZ Q322        ;2 yes
  91             CMP R6,X        ;1 REMlo U< DIVlo ?
  92 Q322        JLO Q323        ;2 yes: REM U< DIV
  93             SUB R6,X        ;1 no:  REMlo - DIVlo  (carry is set)
  94             SUBC TOS,W      ;1      REMhi - DIVhi
  95 Q323        ADDC R7,R7      ;1 RLC quotLO
  96             ADDC R4,R4      ;1 RLC quotHI
  97             SUB #1,R5       ;1 Decrement loop counter
  98             JN Q6432END     ;2 loop back if count>=0
  99             ADD T,T         ;1 RLA DVDlo
 100             ADDC Y,Y        ;1 RLC DVDhi
 101             ADDC X,X        ;1 RLC REMlo
 102             ADDC W,W        ;1 RLC REMhi
 103             JNC Q321        ;
 104             SUB R6,X        ;1 REMlo - DIVlo
 105             SUBC TOS,W      ;1 REMhi - DIVhi
 106             BIS #1,SR
 107             JMP Q323
 108 Q6432END
 109 ;            MOV X,4(PSP)   ; REMlo
 110 ;            MOV W,2(PSP)   ; REMhi
 111 ;            MOV @IP+,PC    ; 33 words
 112         AND #-1,S           ; clear V, set N
 113         JGE FDIVEND         ; if positive
 114         XOR #-1,R7
 115         XOR #-1,R4
 116         ADD #1,R7
 117         ADDC #0,R4
 118 FDIVEND MOV R7,0(PSP)       ; QUOTlo
 119         MOV R4,TOS          ; QUOThi
 120         POPM  #4,R7         ; POPM R4 R5 R6 R7
 121         MOV @IP+,PC
 122
 123 ; F#S    Qlo Qhi u -- Qhi 0   convert fractionnal part of Q15.16 fixed point number
 124 ;                             with u digits
 125     FORTHWORD "F#S"
 126 FNUMS
 127             MOV 2(PSP),X            ; -- Qlo Qhi u      X = Qlo
 128             MOV @PSP,2(PSP)         ; -- Qhi Qhi u
 129             MOV X,0(PSP)            ; -- Qhi Qlo u
 130             MOV TOS,T               ;                   T = limit
 131             MOV #0,S                ;                   S = count
 132 FNUMSLOOP   MOV @PSP,&MPY           ;                   Load 1st operand
 133             MOV &BASE,&OP2          ;                   Load 2nd operand
 134             MOV &RES0,0(PSP)        ; -- Qhi Qlo' x     low result on stack
 135             MOV &RES1,TOS           ; -- Qhi Qlo' digit high result in TOS
 136             CMP #10,TOS             ;                   digit to char
 137             JLO FNUMS2CHAR
 138             ADD #7,TOS
 139 FNUMS2CHAR  ADD #30h,TOS
 140             MOV.B TOS,HOLDS_ORG(S)  ; -- Qhi Qlo' char  char to string
 141             ADD #1,S                ;                   count+1
 142             CMP T,S                 ;2                  count=limit ?
 143             JLO FNUMSLOOP           ;                   loop back if U<
 144             MOV T,TOS               ; -- Qhi Qlo' limit
 145             MOV #0,0(PSP)           ; -- Qhi 0 limit
 146             MOV #HOLDS_ORG,X        ; -- Qhi 0 len      X= org
 147             JMP HOLDS1
 148
 149             FORTHWORD "F*"      ; signed s15.16 multiplication --> s15.16 result
 150             MOV 4(PSP),&MPYS32L ; 5 Load 1st operand
 151             MOV 2(PSP),&MPYS32H ; 5
 152             MOV @PSP,&OP2L      ; 4 load 2nd operand
 153             MOV TOS,&OP2H       ; 3
 154             ADD #4,PSP          ; 1 remove 2 cells
 155             NOP2                ; 2
 156             NOP2                ; 2 wait 8 cycles after write OP2L before reading RES1
 157             MOV &RES1,0(PSP)    ; 5
 158             MOV &RES2,TOS       ; 5
 159             MOV @IP+,PC
 160
 161     .ELSE ; no hardware multiplier
 162
 163        FORTHWORD "F/"      ; s15.16 / s15.16 --> s15.16 result
 164 FDIV
 165         PUSHM  #4,R7        ; PUSHM R7,R4
 166         MOV @PSP+,R6        ; DIVlo
 167         MOV @PSP+,X         ; DVDhi --> REMlo
 168         MOV #0,W            ; REMhi = 0
 169         MOV @PSP,Y          ; DVDlo --> DVDhi
 170         MOV #0,T            ; DVDlo = 0
 171         MOV X,S             ;
 172         XOR TOS,S           ; MDhi XOR MRhi --> S keep sign of result
 173         AND #-1,X           ; MD < 0 ?
 174         JGE FDIV1           ; no
 175         XOR #-1,Y           ; lo
 176         XOR #-1,X           ; hi
 177         ADD #1,Y            ; lo
 178         ADDC #0,X           ; hi
 179 FDIV1   AND #-1,TOS
 180         JGE FDIV2
 181         XOR #-1,R6
 182         XOR #-1,TOS
 183         ADD #1,R6
 184         ADDC #0,TOS
 185 FDIV2
 186 ; unsigned 32-BIT DIVIDEND : 32-BIT DIVISOR --> 32-BIT QUOTIENT, 32-BIT REMAINDER
 187 ; DVDhi|DVDlo : DVRhi|DVRlo --> QUOThi|QUOTlo, REMAINDER
 188 ;            FORTHWORD "UD/MOD"
 189 ;            MOV 4(PSP),T   ; DVDlo
 190 ;            MOV 2(PSP),Y   ; DVDhi
 191 ;            MOV #0,X       ; REMlo = 0
 192 Q6432       MOV #32,R5      ; init loop count
 193 Q321        CMP TOS,W       ;1 REMhi <> DIVhi ?
 194             JNZ Q322        ;2 yes
 195             CMP R6,X        ;1 REMlo U< DIVlo ?
 196 Q322        JLO Q323        ;2 yes: REM U< DIV
 197             SUB R6,X        ;1 no:  REMlo - DIVlo  (carry is set)
 198             SUBC TOS,W      ;1      REMhi - DIVhi
 199 Q323        ADDC R7,R7      ;1 RLC quotLO
 200             ADDC R4,R4      ;1 RLC quotHI
 201             SUB #1,R5       ;1 Decrement loop counter
 202             JN Q6432END     ;2 loop back if count>=0
 203             ADD T,T         ;1 RLA DVDlo
 204             ADDC Y,Y        ;1 RLC DVDhi
 205             ADDC X,X        ;1 RLC REMlo
 206             ADDC W,W        ;1 RLC REMhi
 207             JNC Q321        ;
 208             SUB R6,X        ;1 REMlo - DIVlo
 209             SUBC TOS,W      ;1 REMhi - DIVhi
 210             BIS #1,SR
 211             JMP Q323
 212 Q6432END
 213 ;            MOV X,4(PSP)   ; REMlo
 214 ;            MOV W,2(PSP)   ; REMhi
 215 ;            ADD #4,PSP     ; skip REMlo REMhi
 216
 217             MOV R7,0(PSP)   ; QUOTlo
 218             MOV R4,TOS      ; QUOThi
 219             POPM  #4,R7     ; POPM R4 R5 R6 R7
 220 ;            MOV @IP+,PC    ; 33 words
 221
 222 FDIVSGN AND #-1,S       ; clear V, set N
 223         JGE FDIVEND     ; if positive
 224         XOR #-1,0(PSP)
 225         XOR #-1,TOS
 226         ADD #1,0(PSP)
 227         ADDC #0,TOS
 228 FDIVEND MOV @IP+,PC
 229
 230 ; F#S    Qlo Qhi u -- Qhi 0   convert fractionnal part of Q15.16 fixed point number
 231 ;                             with u digits
 232     FORTHWORD "F#S"
 233 ; create a counted string at PAD+CPL+2
 234 ; with digit high result of Qdlo * base
 235 ; UMstar use S,T,W,X,Y
 236 ; mov &BASE,S , jmp UMSTAR1 without hardware MPY
 237 ; result: digit in tos (high) to convert in digit
 238 ;
 239 FNUMS
 240             MOV 2(PSP),X            ; -- Qlo Qhi u      X = Qlo
 241             MOV @PSP,2(PSP)         ; -- Qhi Qhi u
 242             MOV X,0(PSP)            ; -- Qhi Qlo u
 243             PUSHM #2,TOS            ;                   PUSHM TOS,IP  TOS=limit IP
 244             MOV #0,S                ;                   S=count
 245             MOV #FNUMSNEXT,IP       ; -- Qhi Qlo limit
 246 FNUMSLOOP   PUSH S                  ;                   R-- limit IP count
 247             MOV &BASE,TOS           ; -- Qhi Qlo base
 248             MOV #UMSTAR,PC
 249 FNUMSNEXT   FORTHtoASM              ; -- Qhi QloRem digit
 250             SUB #2,IP
 251             CMP #10,TOS             ;                   digit to char
 252             JLO FNUMS2CHAR
 253             ADD #7,TOS
 254 FNUMS2CHAR  ADD #30h,TOS
 255             MOV @RSP+,S             ;                       R-- limit IP
 256             MOV.B TOS,HOLDS_ORG(S)  ; -- Qhi Qlorem char    char to stringto string
 257             ADD #1,S                ;                       count+1
 258             CMP 2(RSP),S            ;3                      count=limit ?
 259             JLO FNUMSLOOP           ;                       no
 260             POPM #2,TOS             ; -- Qhi Qlorem limit   POPM IP,TOS
 261             MOV #0,0(PSP)           ; -- Qhi 0 limit
 262             MOV #HOLDS_ORG,X        ; -- Qhi 0 len          X= org
 263             JMP HOLDS1
 264
 265 ; unsigned multiply 32*32 = 64
 266 ; don't use S reg (keep sign)
 267         FORTHWORD "UDM*"
 268 UDMT    PUSH IP         ; 3
 269         PUSHM  #4,R7     ; 6 PUSHM R7,R4     save R7 ~ R4 regs
 270         MOV 4(PSP),IP   ; 3 MDlo
 271         MOV 2(PSP),T    ; 3 MDhi
 272         MOV @PSP,W      ; 2 MRlo
 273         MOV #0,R4       ; 1 MDLO=0
 274         MOV #0,R5       ; 1 MDHI=0
 275         MOV #0,4(PSP)   ; 3 RESlo=0
 276         MOV #0,2(PSP)   ; 3 REShi=0
 277         MOV #0,R6       ; 1 RESLO=0
 278         MOV #0,R7       ; 1 RESHI=0
 279         MOV #1,X        ; 1 BIT TEST REGlo
 280         MOV #0,Y        ; 1 BIT TEST2 REGhi
 281 UDMT1   CMP #0,X
 282         JNZ UDMT2       ; 2
 283         BIT Y,TOS       ; 1 TEST ACTUAL BIT MRhi
 284         JMP UDMT3
 285 UDMT2   BIT X,W         ; 1 TEST ACTUAL BIT MRlo
 286 UDMT3   JZ UDMT4        ;
 287         ADD IP,4(PSP)   ; 3 IF 1: ADD MDlo TO RESlo
 288         ADDC T,2(PSP)   ; 3      ADDC MDhi TO REShi
 289         ADDC R4,R6      ; 1      ADDC MDLO TO RESLO
 290         ADDC R5,R7      ; 1      ADDC MDHI TO RESHI
 291 UDMT4   ADD IP,IP       ; 1 (RLA LSBs) MDlo *2
 292         ADDC T,T        ; 1 (RLC MSBs) MDhi *2
 293         ADDC R4,R4      ; 1 (RLA LSBs) MDLO *2
 294         ADDC R5,R5      ; 1 (RLC MSBs) MDHI *2
 295         ADD X,X         ; 1 (RLA) NEXT BIT TO TEST
 296         ADDC Y,Y        ; 1 (RLA) NEXT BIT TO TEST
 297         JLO UDMT1       ; 2 IF BIT IN CARRY: FINISHED    32 * 16~ (average loop)
 298         MOV R6,0(PSP)   ; 3
 299         MOV R7,TOS      ; 1 high result in TOS
 300         POPM  #4,R7     ; 6  POPM R4 R5 R6 R7
 301         MOV @RSP+,IP    ; 2
 302         MOV @IP+,PC
 303
 304
 305         FORTHWORD "F*"      ; s15.16 * s15.16 --> s15.16 result
 306         MOV 2(PSP),S        ;
 307         XOR TOS,S           ; MDhi XOR MRhi --> S keep sign of result
 308         BIT #8000,2(PSP)    ; MD < 0 ?
 309         JZ FSTAR1           ; no
 310         XOR #-1,2(PSP)
 311         XOR #-1,4(PSP)
 312         ADD #1,4(PSP)
 313         ADDC #0,2(PSP)
 314 FSTAR1   mDOCOL
 315         .word DABBS,UDMT
 316         FORTHtoASM          ; -- RES0 RES1 RES2 RES3
 317         MOV @RSP+,IP
 318         MOV @PSP+,TOS       ; -- RES0 RES1 RES2
 319         MOV @PSP+,0(PSP)    ; -- RES1 RES2
 320         JMP FDIVSGN         ; goto end of F/ to process sign of result
 321
 322
 323     .ENDIF
 324
 325         FORTHWORD "F."      ; display a Q15.16 number with 4 digits after comma
 326         mDOCOL
 327         .word   LESSNUM,DUP,TOR,DABBS
 328         .word   lit,4,FNUMS,lit,',',HOLD,NUMS
 329         .word   RFROM,SIGN,NUMGREATER,TYPE,SPACE,EXIT
 330
 331         FORTHWORD "S>F"     ; convert a signed number to a Q15.16 (signed) number
 332         SUB #2,PSP
 333         MOV #0,0(PSP)
 334         MOV @IP+,PC
 335
 336 ; https://forth-standard.org/standard/core/TwoFetch
 337 ; 2@    a-addr -- x1 x2    fetch 2 cells ; the lower address will appear on top of stack
 338         FORTHWORD "2@"
 339 TWOFETCH
 340         SUB #2,PSP
 341         MOV 2(TOS),0(PSP)
 342         MOV @TOS,TOS
 343         MOV @IP+,PC
 344
 345     .IFNDEF ASM_EXT
 346 ; https://forth-standard.org/standard/double/TwoCONSTANT
 347 ; udlo/dlo/Flo udhi/dhi/Qhi --         create a double or a Q15.16 CONSTANT
 348         FORTHWORD "2CONSTANT"
 349         mDOCOL
 350         .word CREATE
 351         .word COMMA,COMMA       ; compile udhi/dhi/Qhi then udlo/dlo/Qlo
 352         .word DOES
 353         .word TWOFETCH
 354         .word EXIT
 355     .ENDIF