TipsAndTriks/avr/float.inc

; ************* Floating point library for AVR microprocessors *************
;
; Assembler     AVRASM v. 1.21
; Processor     AT90S2313, AT90S8515
; Code Size     256 words (512 bytes)
; **************************************************************************
; Author        Y.Andrienko
; Phone         (057-2) 44-85-40, 98-07-58
; FIDO          2:461/44.14
; E-mail        yurik@aqua.kharkov.ua, aya@apexmail.com
;
; Last modification     - 28.09.1999
; **************************************************************************
; Floating point number format: 3-bytes
; 1st - order - bit 7 - number sign (1 - negative number)
;                   6 - order sign (1 - positive order)
;                   0..5 - order value
; 2nd - mantissa low byte
; 3d  - mantissa high byte

; !!! This variables must be defined in main programm !!!

;.def  Float1O =r16
;.def  Float1L =r17
;.def  Float1H =r18
;
;.def  Float2O =r19
;.def  Float2L =r20
;.def  Float2H =r21

;.def  FTempO  =r22
;.def  FTempL  =r23
;.def  FTempH  =r24
;.def  FCnt    =r25


; ************************************************************************
; Fix2Float - translate fixed format number to floating point format
; Float2Fix - translate floating point number to fixed format
; FNeg      - negate floating point number
; SwapFl    - exchange Float1 and Float2
; FAdd      - floating point addition Float2=Float2+Float1
; FSub      - floating point substraction Float2=Float2-Float1
; FMul      - floating point multiplication Float2=Float2*Float1
; FDiv      - floating point division Float2=Float1/Float2
;*************************************************************************

; translate number from fix to floating point format
; Input - 2-byte number in Float2L,Float2H
; Output - 3-byte floating point number in Float2
;
; high registers used - Float2O,Float2L,Float2H

Fix2Float:
        clc                     ; clear carry
        tst     Float2L         ; check number for 0
        brne    Fx2Fl_A
        tst     Float2H
        brne    Fx2Fl_A
        ldi     Float2O,$40     ; if Float2L=Float2H=0 set number to 0
        ret
Fx2Fl_A:
        ldi     Float2O,$50     ; if fixed num # 0 set order to 50H
Fx2Fl_B:
        sbrc    Float2H,7       ; skip if bit 7#0
        ret                     ; else return
        rol     FLoat2L         ; shift number left
        rol     Float2H
        dec     Float2O         ; and decrement order
        rjmp    Fx2Fl_B         ; continue while bit 7#1



; translate floating point number to fixed format
; Input - Float2
; Output - Float2L, Float2H
; carry=1 if overflow
;
; registers used - Float2O, Float2L, Float2H

Float2Fix:
        sbrc    Float2O,7       ; check number sign
        rjmp    SetOverflow     ; for negative number set overflow and return
        sbrs    Float2O,6       ; check order sign
        rjmp    Float2eq0       ; for neg.order set number to 0
        andi    Float2O,$3f     ; mask sign bits
        cpi     Float2O,$11     ; check maximal order
        brlo    Fl2Fx_A         ; jump if order<11H
        rjmp    SetOverflow     ; else set overflow and return
Fl2Fx_A:
        subi    Float2O,$10
        neg     Float2O         ; Float2O=10H-Float2O
Fl2Fx_C:
        clc                     ; clear carry
        tst     Float2O         ; check order
        brne    Fl2Fx_B
        ret                     ; return if order 0
Fl2Fx_B:
        ror     Float2H         ; shift right
        ror     Float2L
        dec     Float2O         ; decrement order
        rjmp    Fl2Fx_C         ; repeat while order # 0

Float2eq0:
        clr     Float2L
        clr     Float2H
        ret

; Change sign of floating point number Float2=-Float2
; Input - Float2
; Output - Float2
; registers used - Float2O,Float2L,Float2H

FNeg:
        neg     Float2O
        subi    Float2O,$80
        neg     Float2O
        clc
        ret

; Swap Float1 and Float2 numbers
; (Thanks for A.Redchhuk)
SwapFl:
;       it is                        it was
        eor Float1O,Float2O        ; push    Float1O
        eor Float2O,Float1O        ; mov     Float1O,Float2O
        eor Float1O,Float2O        ; pop     Float2O

        eor Float1L,Float2L        ; push    Float1L
        eor Float2L,Float1L        ; mov     Float1L,Float2L
        eor Float1L,Float2L        ; pop     Float2L

        eor Float1H,Float2H        ; push    Float1H
        eor Float2H,Float1H        ; mov     Float1H,Float2H
        eor Float1H,Float2H        ; pop     Float2H
        ret

; Floating point addition Float2=Float1+Float2
; Input - Float1, Float2
; Output - Float2
; registers used - Float1O, Float1L, Float1H, Float2O, Float2L, Float2H

FAdd:
        sbrc    Float1O,7       ; skip if Float1>0
        rjmp    FAdd_A          ; jump if Float1<0
        sbrs    Float2O,7       ; skip if Float2<0
        rjmp    FAdd_B          ; jump if FLoat1 and Float2 < 0

; if numbers signs not equal (Float1>0,Float2<0 or Float1<0, Float2>0)
; if Float1>0 Float2<0  result = Float1-Float2
FAdd_C:
        rcall   FNeg            ; change Float2 sign
        rcall   SwapFl
        rjmp    FSub            ; and continue in substraction programm
; If Float1<0,Float2>0 result = Float2-Float1
FAdd_X:
        rcall   SwapFl          ; Change Float1 sign
        rcall   FNeg
        rcall   SwapFl
        rjmp    FSub
FAdd_A:
        sbrs    Float2O,7       ; skip if Float1<0 and Float2<0
        rjmp    FAdd_X
; if numbers signs are equal
FAdd_B:
        tst     Float1H         ; check Float1 for 0
        brne    FAdd_D
        tst     Float1L
        brne    FAdd_D
        ret                     ; if Float1=0 result =Float2
FAdd_D:
        tst     Float2H         ; check Float2 for 0
        brne    FAdd_E
        tst     Float2L
        brne    FAdd_E
        rjmp    Fl1toFl2        ; if Float2=0 copy Float1 as result and return
FAdd_E:
        cp      Float2O,Float1O ; compare orders
        breq    FAdd_G          ; if orders are equal
        brlo    FAdd_F          ; if Float2O<Float1O
        rcall   SwapFl          ; else swap numbers
FAdd_F:
        sub     Float2O,Float1O
        neg     Float2O         ; Float2O=Float1O-Float2O
        cpi     Float2O,$10
        brlo    FAdd_H          ; if Float1O-Float2O<16
        rjmp    Fl1toFl2        ; else copy Float1 as result and return
FAdd_H:
        clc                     ; clear carry
        ror     Float2H         ; shift Float2 right
        ror     Float2L
        dec     Float2O         ; decrement order
        brne    FAdd_H          ; repeat while orders not equal
; if orders are equal
FAdd_G:
        add     Float2L,Float1L ; add numbers
        adc     Float2H,Float1H
        brcs    FAdd_I          ; jump if carry set
FAdd_J:
        mov     Float2O,Float1O ; copy order value
        ret                     ; and return
FAdd_I:
        ror     FLoat2H         ; shift number
        ror     Float2L
        inc     Float1O         ; increment order
        rjmp    FAdd_J

; Floating point numbers substraction Float2=Float1-Float2
; Input - Float1, Float2
; Output - Float2
; registers used - Float1O, Float1L, Float1H, Float2O, Float2L, Float2H

FSub:
        sbrc    Float1O,7       ; skip if Float1>0
        rjmp    FSub_A          ; jump if Float1<0
        sbrs    Float2O,7       ; skip if Float2<0
        rjmp    FSub_B          ; jump if Float1>0 and Float2>0

; if numbers signs not equal
FSub_C:
        rcall   SwapFl
        rcall   FNeg            ; change sign of 2nd number
        rcall   SwapFl
        rjmp    FAdd            ; and continue in substraction programm
FSub_A:
        sbrs    Float2O,7       ; skip if Float2<0 and Float1<0
        rjmp    FSub_C          ; jump if Float2>0 (Float1<0)

; if signs of numbers are equal
FSub_B:
        tst     Float1H         ; check Float1 for 0
        brne    FSub_D
        tst     Float1L
        brne    FSub_D
        ret                     ; if Float1=0  result = Float2 - return
FSub_D:
        tst     Float2H         ; check Float2 for 0
        brne    FSub_E
        tst     Float2L
        brne    FSub_E
        rcall   Fl1toFl2        ; if FLoat2=0, result = -Float1, copy an return
        rjmp    FNeg            ; change sign and return
FSub_E:
        clt
        cp      Float1O,Float2O ; compare orders
        breq    FSub_G          ; if orders are equal
        brlo    FSub_F          ; if Float1O<Float2O
        set                     ; set flag
        rcall   SwapFl          ; swap numbers if Float1O>Float2O
FSub_F:
        sub     Float1O,Float2O
        neg     Float1O         ; Float1O=Float2O-Float1O
        cpi     Float1O,$10
        brlo    FSub_H          ; if Float2O-Float1O<16
        ret                     ; else result=Float2

; equalize orders
FSub_H:
        clc
        ror     Float1H         ; shift Float right
        ror     Float1L
        dec     Float1O
        brne    FSub_H          ; repeat before orders not equal
        brtc    FSub_G          ;
        rcall   SwapFl          ; swap again
        clt
FSub_G:
        cp      Float2L,Float1L
        cpc     Float2H,Float1H
        brlo    FSub_I
FSub_J:
        sub     Float2L,Float1L ; add numbers
        sbc     Float2H,Float1H
        sbrc    Float2H,7       ; skip if MSB=0
        ret                     ; else return
        rjmp    Fx2Fl_B         ; normalise result
FSub_I:
        rcall   SwapFl          ; swap numbers
        rcall   FNeg            ; change result sign
        rjmp    FSub_J

        ror     FLoat2H         ; shift number
        ror     Float2L
        inc     Float2O         ; increment order
        rcall   FNeg            ; and change sign
        ret                     ; else return


; Floating point multiplication Float2 = Float1 * Float2
; Input - Float1, Float2
; Output - Float2
; registers used - Float1O, Float1L, Float1H, Float2O, Float2L, Float2H
;                  FTempO, FTempL, FCnt
;
FMul:
        tst     Float1H         ; check Float1 for 0
        brne    FMul_A
        tst     Float1L
        brne    FMul_A
        rjmp    SetRes0         ; if Float1=0, set Float2=0 and return
FMul_A:
        tst     Float2H         ; check Float2 for 0
        brne    FMul_B
        tst     Float2L
        brne    FMul_B
        ret                     ; if Float2=0 - return

FMul_B:
        mov     FTempO,Float1O  ; Find result sign
        eor     FTempO,Float2O  ; FTempO bit 7 equal result sign
        cbr     Float1O,$80     ; reset numbers signs
        cbr     Float2O,$80
        bst     FTempO,7        ; save result sign in T
        add     Float2O,Float1O ; add orders
        ldi     Float1O,$40     ;
        add     Float2O,Float1O
        bld     Float2O,7

; 16x16 bits unsigned multiplication from ****AVR200**** application note
FMul_D:
        clr     FTempO          ;clear 2 highest bytes of result
        clr     FTempL
        ldi     FCnt,16         ;init loop counter
        lsr     Float2H
        ror     Float2L
FMul_E:
        brcc    FMul_F          ;if bit 0 of multiplier set
        add     FTempO,Float1L  ;add multiplicand Low to byte 2 of res
        adc     FTempL,Float1H  ;add multiplicand high to byte 3 of res
FMul_F:
        ror     FTempL          ;shift right result byte 3
        ror     FTempO          ;rotate right result byte 2
        ror     Float2H         ;rotate result byte 1 and multiplier High
        ror     Float2L         ;rotate result byte 0 and multiplier Low
        dec     FCnt            ;decrement loop counter
        brne    FMul_E          ;if not done, loop more
; after multiplication - normalise result
FMul_H:
        sbrc    FTempL,7
        rjmp    FMul_G          ; jump if result is normalised
        rol     Float2L         ; shift number left
        rol     Float2H
        rol     FTempO
        rol     FTempL
        dec     Float2O         ; decrement result order
        rjmp    FMul_H
FMul_G:
        bld     Float2O,7       ; restore result sign from T
        mov     Float2L,FTempO  ; copy mantissa to result
        mov     Float2H,FTempL
        ret

; Floating point division  Float2 = Float1/Float2
; Input - Float1, Float2
; Output - Float2
; registers used - Float1O, Float1L, Float1H, Float2O, Float2L, Float2H
;                  FTempO, FTempL, FTempH, FCnt

FDiv:
        mov     FTempO,Float1L  ; Check Float1 for 0
        or      FTempO,Float1H
        tst     FTempO
        brne    FDiv_A
        rjmp    SetRes0         ; if Float1=0 - result = 0
FDiv_A:
        sbrs    Float2H,7
        rjmp    SetOverflow     ; if Float2 not normalized or 0 - overflow
        ldi     FCnt,$10        ; bits counter
        mov     FTempO,Float1O
        andi    FTempO,$80      ; select sign bit
        add     FTempO,Float2O  ;
        bst     FTempO,7        ; store result sign in T
        mov     FTempO,Float1O  ; read 1st number order
        sub     FTempO,Float2O  ; substract orders
        subi    FTempO,-$41     ; add 41H
        bld     FTempO,7        ; restore result sign
        clr     Float1O         ; clear orders
        clr     Float2O
        clr     FTempH
        ldi     FTempL,1
; main division cykle
FDiv_C:
        sbrs    FTempL,0
        rjmp    FDiv_D
        sub     Float1L,Float2L
        sbc     Float1H,Float2H
        sbc     Float1O,Float2O ; set carry
        rjmp    FDiv_E
FDiv_D:
        add     Float1L,Float2L
        adc     Float1H,Float2H
        adc     Float1O,Float2O
FDiv_E:
        andi    Float1O,$01
        push    Float1O         ; save order
        clc
        eor     Float1O,Float2O
        brne    FDiv_F
        sec
FDiv_F:
        pop     Float1O         ; restore order
        rol     FTempL
        rol     FTempH
        clc                     ; clear carry
        rol     Float1L
        rol     Float1H
        mov     Float1O,FTempH
        rol     Float1O
        dec     FCnt            ; decremet counter
        brne    FDiv_C          ; repeate while FCnt#0
        mov     Float2L,FTempL  ; copy result
        mov     Float2H,FTempH
        mov     Float2O,FTempO
        rjmp    Fx2Fl_B         ; normalise result

; Common subroutines

; Set overflow flag and return
SetOverflow:
        sec
        ret

; Copy Float1 to Float2
Fl1toFl2:
        mov     Float2O,Float1O
        mov     Float2H,Float1H
        mov     Float2L,Float1L
        ret

; Set result to 0 (Float2=0)
SetRes0:
        clr     Float2L         ; set result=0
        clr     Float2H
        ldi     Float2O,$40
        ret