ADDON/FixPoint.asm

   1
   2
   3     FORTHWORD "{FIXPOINT}"
   4     mNEXT
   5
   6 ; https://forth-standard.org/standard/core/HOLDS
   7 ; Adds the string represented by addr u to the pictured numeric output string
   8 ; compilation use: <# S" string" HOLDS #>
   9 ; free chars area in the 32+2 bytes HOLD buffer = {26,23,2} chars with a 32 bits sized {hexa,decimal,binary} number.
  10 ; (2 supplementary bytes are room for sign - and decimal point)
  11 ; perfect to display all a line on LCD 2x20 chars...
  12 ; C HOLDS    addr u --
  13     FORTHWORD "HOLDS"
  14 HOLDS       MOV @PSP+,X     ; 2
  15 HOLDS1      ADD TOS,X       ; 1 src
  16             MOV &HP,Y       ; 3 dst
  17 HOLDSLOOP   SUB #1,X        ; 1 src-1
  18             SUB #1,TOS      ; 1 cnt-1
  19             JLO HOLDSNEXT   ; 2
  20             SUB #1,Y        ; 1 dst-1
  21             MOV.B @X,0(Y)   ; 4
  22             JMP HOLDSLOOP   ; 2
  23 HOLDSNEXT   MOV Y,&HP       ; 3
  24             MOV @PSP+,TOS   ; 2
  25             mNEXT            ; 4  15 words
  26
  27         FORTHWORD "F+"      ; -- d1lo d1hi d2lo d2hi
  28         ADD @PSP+,2(PSP)    ; -- sumlo  d1hi d2hi
  29         ADDC @PSP+,TOS      ; -- sumlo sumhi
  30         MOV @IP+,PC
  31
  32         FORTHWORD "F-"      ; -- d1lo d1hi d2lo d2hi
  33         SUB @PSP+,2(PSP)    ; -- diflo d1hi d2hi
  34         SUBC TOS,0(PSP)     ; -- diflo difhi d2hi
  35         MOV @PSP+,TOS
  36         MOV @IP+,PC
  37
  38
  39        FORTHWORD "F/"      ; s15.16 / s15.16 --> s15.16 result
  40 FDIV    MOV 2(PSP),S        ;
  41         XOR TOS,S           ; MDhi XOR MRhi --> S keep sign of result
  42         MOV #0,T            ; DVDlo = 0
  43         MOV 4(PSP),Y        ; DVDlo --> DVDhi
  44         MOV 2(PSP),X        ; DVDhi --> REMlo
  45         BIT #8000,X         ; MD < 0 ?
  46         JZ FDIV1            ; no
  47         XOR #-1,Y           ; lo
  48         XOR #-1,X           ; hi
  49         ADD #1,Y            ; lo
  50         ADDC #0,X           ; hi
  51 FDIV1   BIT #8000,TOS
  52         JZ FDIV2
  53         XOR #-1,0(PSP)
  54         XOR #-1,TOS
  55         ADD #1,0(PSP)
  56         ADDC #0,TOS
  57 FDIV2
  58 ; unsigned 32-BIT DIVIDEND : 32-BIT DIVISOR --> 32-BIT QUOTIENT, 32-BIT REMAINDER
  59 ; DVDhi|DVDlo : DVRhi|DVRlo --> QUOThi|QUOTlo, REMAINDER
  60 ;            FORTHWORD "UD/MOD"
  61 ;            MOV 4(PSP),T   ; DVDlo
  62 ;            MOV 2(PSP),Y   ; DVDhi
  63 ;            MOV #0,X       ; REMlo = 0
  64 Q6432       .word 1537h     ; PUSHM R7,R4
  65             MOV #0,W        ; REMhi = 0
  66             MOV @PSP,R6     ; DIVlo
  67             MOV #32,R5      ; init loop count
  68 Q321        CMP TOS,W       ;1 REMhi <> DIVhi ?
  69             JNZ Q322        ;2 yes
  70             CMP R6,X        ;1 REMlo U< DIVlo ?
  71 Q322        JLO Q323        ;2 yes: REM U< DIV
  72             SUB R6,X        ;1 no:  REMlo - DIVlo  (carry is set)
  73             SUBC TOS,W      ;1      REMhi - DIVhi
  74 Q323        ADDC R7,R7      ;1 RLC quotLO
  75             ADDC R4,R4      ;1 RLC quotHI
  76             SUB #1,R5       ;1 Decrement loop counter
  77             JN Q6432END     ;2 loop back if count>=0
  78             ADD T,T         ;1 RLA DVDlo
  79             ADDC Y,Y        ;1 RLC DVDhi
  80             ADDC X,X        ;1 RLC REMlo
  81             ADDC W,W        ;1 RLC REMhi
  82             JNC Q321        ;
  83             SUB R6,X        ;1 REMlo - DIVlo
  84             SUBC TOS,W      ;1 REMhi - DIVhi
  85             BIS #1,SR
  86             JMP Q323
  87 Q6432END
  88 ;            MOV X,4(PSP)   ; REMlo
  89 ;            MOV W,2(PSP)   ; REMhi
  90             ADD #4,PSP      ; skip REMlo REMhi
  91
  92             MOV R7,0(PSP)   ; QUOTlo
  93             MOV R4,TOS      ; QUOThi
  94             .word 1734h     ; POPM R4,R7
  95 ;            MOV @IP+,PC    ; 33 words
  96
  97 FDIVSGN AND #-1,S       ; clear V, set N
  98         JGE FDIVEND     ; if positive
  99         XOR #-1,0(PSP)
 100         XOR #-1,TOS
 101         ADD #1,0(PSP)
 102         ADDC #0,TOS
 103 FDIVEND MOV @IP+,PC
 104
 105     .IFDEF MPY ; hardware multiplier
 106
 107 ; F#S    Qlo Qhi -- Qhi 0   convert fractionnal part of Q15.16 fixed point number
 108     FORTHWORD "F#S"
 109 FNUMS       MOV @PSP,X              ; -- Qlo Qhi    X = Qlo
 110             MOV TOS,0(PSP)          ; -- Qhi Qhi
 111             SUB #2,PSP              ; -- Qhi x Qhi
 112             MOV X,0(PSP)            ; -- Qhi Qlo Qhi
 113             MOV #4,T                ; -- Qhi Qlo x      T = limit for base 16
 114             CMP #10,&BASE
 115             JNZ FNUMS2
 116             ADD #1,T                ;                   T = limit for base 10
 117 FNUMS2      MOV #0,S                ;                   S = count
 118 FNUMSLOOP   MOV @PSP,&MPY           ;                   Load 1st operand
 119             MOV &BASE,&OP2          ;                   Load 2nd operand
 120             MOV &RES0,0(PSP)        ; -- Qhi Qlo' x     low result on stack
 121             MOV &RES1,TOS           ; -- Qhi Qlo' digit high result in TOS
 122             CMP #10,TOS             ;                   digit to char
 123             JLO FNUMS2CHAR
 124             ADD #7,TOS
 125 FNUMS2CHAR  ADD #30h,TOS
 126             MOV.B TOS,HOLDS_ORG(S)  ; -- Qhi Qlo' char  char to string
 127             ADD #1,S                ;                   count+1
 128             CMP T,S                 ;2                  count=limit ?
 129             JLO FNUMSLOOP           ;                   loop back if U<
 130             MOV T,TOS               ; -- Qhi Qlo' limit
 131             MOV #0,0(PSP)           ; -- Qhi 0 limit
 132             MOV #HOLDS_ORG,X        ; -- Qhi 0 len      X= org
 133             JMP HOLDS1
 134
 135             FORTHWORD "F*"      ; signed s15.16 multiplication --> s15.16 result
 136             MOV 4(PSP),&MPYS32L ; 5 Load 1st operand
 137             MOV 2(PSP),&MPYS32H ; 5
 138             MOV @PSP,&OP2L      ; 4 load 2nd operand
 139             MOV TOS,&OP2H       ; 3
 140             ADD #4,PSP          ; 1 remove 2 cells
 141             NOP2                ; 2
 142             NOP2                ; 2 wait 8 cycles after write OP2L before reading RES1
 143             MOV &RES1,0(PSP)    ; 5
 144             MOV &RES2,TOS       ; 5
 145             MOV @IP+,PC
 146
 147     .ELSE ; no hardware multiplier
 148
 149 ; F#S    Qlo Qhi -- Qhi 0   convert fractionnal part of Q15.16 fixed point number
 150     FORTHWORD "F#S"
 151 ; create a counted string at PAD+CPL+2
 152 ; with digit high result of Qdlo * base
 153 ; UMstar use S,T,W,X,Y
 154 ; mov &BASE,S , jmp UMSTAR1 without hardware MPY
 155 ; result: digit in tos (high) to convert in digit
 156 ;
 157 FNUMS       MOV @PSP,X              ; -- Qlo Qhi    X = Qlo
 158             MOV TOS,0(PSP)          ; -- Qhi Qhi
 159             SUB #2,PSP              ; -- Qhi x Qhi
 160             MOV X,0(PSP)            ; -- Qhi Qlo Qhi
 161             MOV #4,TOS              ; -- Qhi Qlo limit      TOS = count for base 16
 162             CMP #10,&BASE
 163             JNZ FNUMS2
 164             ADD #1,TOS              ;                   TOS = limit for base 10
 165 FNUMS2      .word 151Eh             ;                   PUSHM TOS,IP  TOS=limit IP count
 166             MOV #FNUMSNEXT,IP       ; -- Qhi Qlo limit
 167             MOV #0,S
 168 FNUMSLOOP   PUSH S                  ;                   R-- limit IP count
 169             MOV &BASE,TOS           ; -- Qhi Qlo base
 170             MOV #UMSTAR,PC
 171 FNUMSNEXT   FORTHtoASM              ; -- Qhi QloRem digit
 172             SUB #2,IP
 173             CMP #10,TOS             ;                   digit to char
 174             JLO FNUMS2CHAR
 175             ADD #7,TOS
 176 FNUMS2CHAR  ADD #30h,TOS
 177             MOV @RSP+,S             ;                       R-- limit IP
 178             MOV.B TOS,HOLDS_ORG(S)  ; -- Qhi Qlorem char    char to stringto string
 179             ADD #1,S                ;                       count+1
 180             CMP 2(RSP),S            ;3                      count=limit ?
 181             JLO FNUMSLOOP           ;                       no
 182             .word 171Dh             ; -- Qhi Qlorem limit   POPM IP,TOS ;
 183             MOV #0,0(PSP)           ; -- Qhi 0 limit
 184             MOV #HOLDS_ORG,X        ; -- Qhi 0 len          X= org
 185             JMP HOLDS1
 186
 187 ; unsigned multiply 32*32 = 64
 188 ; don't use S reg (keep sign)
 189         FORTHWORD "UDM*"
 190 UDMT    PUSH IP         ; 3
 191         .word 1537h     ; 6 PUSHM R7,R4     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 UDMT1   CMP #0,X
 204         JNZ UDMT2       ; 2
 205         BIT Y,TOS       ; 1 TEST ACTUAL BIT MRhi
 206         JMP UDMT3
 207 UDMT2   BIT X,W         ; 1 TEST ACTUAL BIT MRlo
 208 UDMT3   JZ UDMT4        ;
 209         ADD IP,4(PSP)   ; 3 IF 1: ADD MDlo TO RESlo
 210         ADDC T,2(PSP)   ; 3      ADDC MDhi TO REShi
 211         ADDC R4,R6      ; 1      ADDC MDLO TO RESLO
 212         ADDC R5,R7      ; 1      ADDC MDHI TO RESHI
 213 UDMT4   ADD IP,IP       ; 1 (RLA LSBs) MDlo *2
 214         ADDC T,T        ; 1 (RLC MSBs) MDhi *2
 215         ADDC R4,R4      ; 1 (RLA LSBs) MDLO *2
 216         ADDC R5,R5      ; 1 (RLC MSBs) MDHI *2
 217         ADD X,X         ; 1 (RLA) NEXT BIT TO TEST
 218         ADDC Y,Y        ; 1 (RLA) NEXT BIT TO TEST
 219         JLO UDMT1       ; 2 IF BIT IN CARRY: FINISHED    32 * 16~ (average loop)
 220         MOV R6,0(PSP)   ; 3
 221         MOV R7,TOS      ; 1 high result in TOS
 222         .word 1734h     ; 6  POPM R4,R7  restore R4 ~ R7 regs
 223         MOV @RSP+,IP    ; 2
 224         MOV @IP+,PC
 225
 226
 227         FORTHWORD "F*"      ; s15.16 * s15.16 --> s15.16 result
 228         MOV 2(PSP),S        ;
 229         XOR TOS,S           ; MDhi XOR MRhi --> S keep sign of result
 230         BIT #8000,2(PSP)    ; MD < 0 ?
 231         JZ FSTAR1           ; no
 232         XOR #-1,2(PSP)
 233         XOR #-1,4(PSP)
 234         ADD #1,4(PSP)
 235         ADDC #0,2(PSP)
 236 FSTAR1   mDOCOL
 237         .word DABBS,UDMT
 238         FORTHtoASM          ; -- RES0 RES1 RES2 RES3
 239         MOV @RSP+,IP
 240         MOV @PSP+,TOS       ; -- RES0 RES1 RES2
 241         MOV @PSP+,0(PSP)    ; -- RES1 RES2
 242         JMP FDIVSGN         ; goto end of F/ to process sign of result
 243
 244
 245     .ENDIF
 246
 247         FORTHWORD "F."      ; display a Q15.16 number
 248         mDOCOL
 249         .word   LESSNUM,DUP,TOR,DABBS
 250         .word   FNUMS,lit,',',HOLD,NUMS
 251         .word   RFROM,SIGN,NUMGREATER,TYPE,SPACE,EXIT
 252
 253         FORTHWORD "S>F"     ; convert a signed number to a Q15.16 (signed) number
 254         SUB #2,PSP
 255         MOV #0,0(PSP)
 256         MOV @IP+,PC
 257
 258 ; https://forth-standard.org/standard/core/TwoFetch
 259 ; 2@    a-addr -- x1 x2    fetch 2 cells ; the lower address will appear on top of stack
 260             FORTHWORD "2@"
 261 TWOFETCH
 262         SUB #2,PSP
 263         MOV 2(TOS),0(PSP)
 264         MOV @TOS,TOS
 265         MOV @IP+,PC
 266
 267
 268 ; https://forth-standard.org/standard/double/TwoCONSTANT
 269 ; udlo/dlo/Flo udhi/dhi/Qhi --         create a double or a Q15.16 CONSTANT
 270         FORTHWORD "2CONSTANT"
 271         mDOCOL
 272         .word CREATE
 273         .word COMMA,COMMA       ; compile udhi/dhi/Qhi then udlo/dlo/Qlo
 274         .word DOES
 275         .word TWOFETCH
 276         .word EXIT
 277