Mercurial > public > hwos_code
view src/math.asm @ 642:a9a0188091e4
fix rare upgrade issue with OSTC sport 2019 hardware
| author | heinrichsweikamp |
|---|---|
| date | Thu, 14 Jan 2021 16:24:07 +0100 |
| parents | 4050675965ea |
| children | 7d8a4c60ec1a |
line wrap: on
line source
;============================================================================= ; ; File math.asm * combined next generation V3.09.4k ; ; Math subroutines ; ; Copyright (c) 2011, JD Gascuel, HeinrichsWeikamp, all right reserved. ;============================================================================= ; HISTORY ; 2011-08-03 : [mH] moving from OSTC code #include "hwos.inc" ; mandatory header ;============================================================================= math1 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Divide a 16 Bit Integer by a Power of 2: divA:2 = divA:2 / 2^WREG ; ; trashes WREG ; global div16 div16: bcf STATUS,C ; clear carry rrcf divA+1 ; rotate right high byte, carry into MSB, LSB into carry rrcf divA+0 ; rotate right low byte, carry into MSB, LSB into carry decfsz WREG ; decrement counter, done? bra div16 ; NO - loop return ; YES - done ;============================================================================= math2 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Multiply a 16 bit Integer by a Power of 2: xA:2 = xA:2 * 2^WREG ; ; trashes WREG ; global mult16 mult16: bcf STATUS,C ; clear carry rlcf divA+0,F ; rotate left low byte, carry into LSB, MSB into carry rlcf divA+1,F ; rotate left high byte, carry into LSB, MSB into carry decfsz WREG ; decrement counter, done? bra mult16 ; NO - loop return ; YES - done ;============================================================================= math3 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Add two UNSIGNED 16 bit Integers: sub_c:2 = sub_a:2 + sub_b:2 ; ; trashes WREG ; global addU16 addU16: movf sub_a+0,W ; get 1st summand (low byte) to WREG addwf sub_b+0,W ; add 2nd summand (low byte) and store result in WREG movwf sub_c+0 ; copy result (low byte) to sub_c movf sub_a+1,W ; get 1st summand (high byte) to WREG addwfc sub_b+1,W ; add 2nd summand (high byte) and store result in WREG movwf sub_c+1 ; copy result (high byte) to sub_c return ; done ;============================================================================= math4 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Subtract two SIGNED 16 Bit Integers: sub_c:2 = sub_a:2 - sub_b:2 ; ; sets neg_flag if result is < 0, trashes WREG ; global sub16 sub16: bcf neg_flag ; clear flag which will indicate if result is negative movf sub_b+0,W ; get value to be subtracted, low byte subwf sub_a+0,W ; execute subtraction on low byte movwf sub_c+0 ; copy result to output variable, low byte movf sub_b+1,W ; get value to be subtracted, high byte subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag movwf sub_c+1 ; copy result to output variable, high byte btfss STATUS,N ; result negative ? return ; NO - result positive, done bsf neg_flag ; YES - set flag comf sub_c+1 ; - do a 16 bit sign change negf sub_c+0 ; btfsc STATUS,C ; - carry to be propagated ? incf sub_c+1,F ; YES - do it return ; - done ;============================================================================= math5 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Subtract two UNSIGNED 16 Bit Integers: sub_c:2 = sub_a:2 - sub_b:2 ; ; sets neg_flag if result is < 0, trashes WREG ; global subU16 subU16: bcf neg_flag ; clear flag which will indicate if result is negative movf sub_b+0,W ; get value to be subtracted, low byte subwf sub_a+0,W ; execute subtraction on low byte movwf sub_c+0 ; copy result to output variable, low byte movf sub_b+1,W ; get value to be subtracted, high byte subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag movwf sub_c+1 ; copy result to output variable, high byte btfsc STATUS,C ; borrow to propagate (B == /CARRY) ? return ; NO - result positive, done bsf neg_flag ; YES - set flag comf sub_c+1 ; - do a 16 bit sign change negf sub_c+0 ; - ... btfsc STATUS,C ; - carry to be propagated ? incf sub_c+1,F ; YES - do it return ; - done ;============================================================================= math6 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Compare two UNSIGNED 16 Bit Integers: sub_a:2 - sub_b:2 ; ; sets neg_flag if result is < 0, trashes WREG ; the subtraction result is not stored ; global cmpU16 cmpU16: bcf neg_flag ; clear flag which will indicate if result is negative movf sub_b+0,W ; get value to be subtracted, low byte subwf sub_a+0,W ; execute subtraction on low byte movf sub_b+1,W ; get value to be subtracted, high byte subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag btfss STATUS,C ; borrow to propagate (B == /CARRY) ? bsf neg_flag ; YES - result is negative, set flag return ; done ;============================================================================= math7 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Multiply two UNSIGNED 16 bit Integers: xC:4 = xA:2 * xB:2 ; ; trashes PRODL, PRODH, WREG ; global mult16x16 mult16x16: clrf xC+2 ; clear the high-order bits clrf xC+3 ; ... movf xA+0,W ; do the "L" multiplication mulwf xB+0 movff PRODL,xC+0 ; copy result to xC movff PRODH,xC+1 movf xA+0,W ; do the "I" multiplication mulwf xB+1 movf PRODL,W ; get the product's low byte... addwf xC+1,F ; ... and add it to xC+1 movf PRODH,W ; get the product's high byte... addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1 movf xA+1,W ; do the "O" multiplication mulwf xB+0 movf PRODL,W ; get the product's low byte... addwf xC+1,F ; ... and add it to xC+1 movf PRODH,W ; get the product's high byte... addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1 clrf WREG ; clear WREG... addwfc xC+3,F ; ... add add it to xC+3 obeying carry bit from xC+2 (can only happen in "O" multiplication) movf xA+1,W ; do the "F" multiplication mulwf xB+1 movf PRODL,W ; get the product's low byte... addwf xC+2,F ; ... and add it to xC+2 movf PRODH,W ; get the product's high byte... addwfc xC+3,F ; ... and add it to xC+3 obeying carry bit from xC+2 return ; done ;============================================================================= math8 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Divide two UNSIGNED 16 Bit Integers: xC:2 = xA:2 / xB:2 with xA as remainder ; ; trashes WREG ; global div16x16 div16x16: movlw .16 ; process 16 bits ... movwf math_loop ; ... initialize loop counter movff xA+0,xC+0 ; copy xA to xC movff xA+1,xC+1 ; ... clrf xA+0 ; clear xA, will now be used to hold the remainder clrf xA+1 ; ... div16x16_1: bcf STATUS,C ; clear carry flag to shift in a zero bit rlcf xC+0,F ; shift left xC rlcf xC+1,F ; ... shifting MSB out of xC... rlcf xA+0,F ; ... and into LSB of xA rlcf xA+1,F ; ... btfsc STATUS,C ; did the remainder overflow (carry set)? bra div16x16_2 ; YES - directly generate a result bit = 1 movf xB+0,W ; NO - compute remainder - divisor = xA - xB, trash result to WREG subwf xA+0,W ; - ... movf xB+1,W ; - ... subwfb xA+1,W ; - ... btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ? bra div16x16_3 ; YES - result bit = 0, keep LSB of xC+0 being 0 div16x16_2: bsf xC+0,0 ; NO - result bit = 1, set LSB of xC+0 to 1 movf xB+0,W ; - subtract divisor from remainder "for real": xA = xA - xB subwf xA+0,F ; - ... movf xB+1,W ; - ... subwfb xA+1,F ; - ... div16x16_3: decfsz math_loop,F ; decrement loop counter, all bits done? bra div16x16_1 ; NO - loop return ; YES - done ;============================================================================= math9 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Divide a 32 Bit Integer by a 16 Bit Integer: xC:4 = xC:4 / xB:2 with xA as remainder ; ; trashes WREG ; global div32x16 div32x16: movlw .32 ; process 32 bits ... movwf math_loop ; ... initialize loop counter clrf xA+0 ; clear xA, will be used to hold the remainder clrf xA+1 ; ... div32x16_1: bcf STATUS,C ; clear carry flag to shift in a zero bit rlcf xC+0,F ; shift left xC rlcf xC+1,F ; ... rlcf xC+2,F ; ... rlcf xC+3,F ; ... shifting MSB out of xC... rlcf xA+0,F ; ... and into LSB of xA rlcf xA+1,F ; ... btfsc STATUS,C ; did the remainder overflow (carry set)? bra div32x16_2 ; YES - directly generate a result bit = 1 movf xB+0,W ; NO - compute remainder - divisor = xA - xB, trash result to WREG subwf xA+0,W ; - ... movf xB+1,W ; - ... subwfb xA+1,W ; - ... btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ? bra div32x16_3 ; YES - result bit = 0, keep LSB of xC+0 being 0 div32x16_2: bsf xC+0,0 ; NO - result bit = 1, set LSB of xC+0 to 1 movf xB+0,W ; - subtract divisor from remainder "for real": xA = xA - xB subwf xA+0,F ; - ... movf xB+1,W ; - ... subwfb xA+1,F ; - ... div32x16_3: decfsz math_loop,F ; decrement loop counter, all bits done? bra div32x16_1 ; NO - loop return ; YES - done ;----------------------------------------------------------------------------- ; ; ISR math functions ; ;----------------------------------------------------------------------------- ;============================================================================= math10 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; 24 Bit Shift, repeated WREG Times, dedicated to a specific Usage ; ; Because less than 8 bits are shifted and only C[2:1] is needed, ; we don't care what bit is inserted. ; global isr_shift_C31 isr_shift_C31: rrcf isr_xC+3,F ; shift three bytes rrcf isr_xC+2,F ; ... rrcf isr_xC+1,F ; ... decfsz WREG ; decrement loop counter, done? bra isr_shift_C31 ; NO - loop return ; YES - done ;============================================================================= math11 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Multiply two UNSIGNED 16 Bit Integers: isr_xC = isr_xA * isr_xB ; ; trashes PRODL, PRODH, WREG ; global isr_unsigned_mult16x16 isr_unsigned_mult16x16: movf isr_xA+0,W ; multiply a[0] * b[0] mulwf isr_xB+0 ; ... movff PRODL,isr_xC+0 ; store product to c[1]:c[0] movff PRODH,isr_xC+1 ; ... movf isr_xA+1,W ; multiply a[1] * b[1] mulwf isr_xB+1 ; ... movff PRODL, isr_xC+2 ; store product to c[3]:c[2] movff PRODH, isr_xC+3 ; ... movf isr_xA+0,W ; multiply a[0] * b[1] mulwf isr_xB+1 ; ... movf PRODL,W ; add cross product to c[3]:c[2]:c[1] addwf isr_xC+1,F ; ... movf PRODH,W ; ... addwfc isr_xC+2,F ; propagate carry clrf WREG ; ... addwfc isr_xC+3,F ; ... movf isr_xA+1,W ; multiply a[1] * b[0] mulwf isr_xB+0 ; ... movf PRODL,W ; add cross product addwf isr_xC+1,F ; ... movf PRODH,W ; ... addwfc isr_xC+2,F ; propagate carry clrf WREG ; ... addwfc isr_xC+3,F ; ... return ; done ;----------------------------------------------------------------------------- ; Multiply two SIGNED 16 Bit Integers: isr_xC = isr_xA * isr_xB ; ; trashes PRODL, PRODH, WREG ; global isr_signed_mult16x16 isr_signed_mult16x16: ; do an unsigned multiplication first rcall isr_unsigned_mult16x16 ; manage sign extension of operand B btfss isr_xB+1,7 ; is B negative ? bra isr_signed_mult_checkA ; NO - continue checking operand A movf isr_xA+0,W ; YES - add -65536 * A subwf isr_xC+2,F ; - ... movf isr_xA+1,W ; - ... subwfb isr_xC+3,F ; - ... isr_signed_mult_checkA ; manage sign extension of operand B btfss isr_xA+1,7 ; is A negative ? return ; NO - done movf isr_xB+0,W ; YES - add -65536 * B subwf isr_xC+2,F ; - ... movf isr_xB+1,W ; - ... subwfb isr_xC+3,F ; - ... return ; - done ;----------------------------------------------------------------------------- END