Mercurial > public > hwos_code
view src/math.asm @ 650:bc214815deb2
3.19/10.75 release
| author | heinrichsweikamp |
|---|---|
| date | Sun, 28 Aug 2022 13:13:38 +0200 |
| parents | 7d8a4c60ec1a |
| children | 75e90cd0c2c3 |
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 ;============================================================================= math12 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; 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 call 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 ;============================================================================= math13 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Multiply two UNSIGNED 24 Bit Integers: isr_xC = isr_xA * isr_xB ; ; trashes PRODL, PRODH, WREG ; global isr_unsigned_mult24x24 isr_unsigned_mult24x24: call isr_unsigned_mult16x16 ; Do the 16x16 first clrf isr_xC+4 clrf isr_xC+5 ; clear upper 16 bit ; do the remaining three multiplications movf isr_xA+0,W ; multiply a[0] * b[2] mulwf isr_xB+2 ; ... movf PRODL,W ; add cross product to c[4]:c[3]:c[2] addwf isr_xC+2,F ; ... movf PRODH,W ; ... addwfc isr_xC+3,F ; propagate carry clrf WREG ; ... addwfc isr_xC+4,F ; ... addwfc isr_xC+5,F ; ... movf isr_xA+2,W ; multiply a[2] * b[0] mulwf isr_xB+0 ; ... movf PRODL,W ; add cross product to c[4]:c[3]:c[2] addwf isr_xC+2,F ; ... movf PRODH,W ; ... addwfc isr_xC+3,F ; propagate carry clrf WREG ; ... addwfc isr_xC+4,F ; ... addwfc isr_xC+5,F ; ... ; isr_xC[2] done. movf isr_xA+2,W ; multiply a[2] * b[1] mulwf isr_xB+1 ; ... movf PRODL,W ; add cross product to c[5]:c[4]:c[3] addwf isr_xC+3,F ; ... movf PRODH,W ; ... addwfc isr_xC+4,F ; propagate carry clrf WREG ; ... addwfc isr_xC+5,F ; ... movf isr_xA+1,W ; multiply a[1] * b[2] mulwf isr_xB+2 ; ... movf PRODL,W ; add cross product to c[5]:c[4]:c[3] addwf isr_xC+3,F ; ... movf PRODH,W ; ... addwfc isr_xC+4,F ; propagate carry clrf WREG ; ... addwfc isr_xC+5,F ; ... ; isr_xC[3] done. movf isr_xA+2,W ; multiply a[2] * b[2] mulwf isr_xB+2 ; ... movf PRODL,W ; add cross product to c[5]:c[4] addwf isr_xC+4,F ; ... movf PRODH,W ; ... addwfc isr_xC+5,F ; propagate carry ; isr_xC[4] done. ; isr_xC[5] done. return ; done ;============================================================================= math14 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Multiply two UNSIGNED Integers (32 and 24 Bit): isr_xC = isr_xA * isr_xB ; ; trashes PRODL, PRODH, WREG ; global isr_unsigned_mult32x24 isr_unsigned_mult32x24: call isr_unsigned_mult24x24 ; Do the 24x24 first clrf isr_xC+6 ; clear upper 8 bit ; do the remaining three multiplications movf isr_xA+3,W ; multiply a[3] * b[0] mulwf isr_xB+0 ; ... movf PRODL,W ; add cross product to c[5]:c[4]:c[3] addwf isr_xC+3,F ; ... movf PRODH,W ; ... addwfc isr_xC+4,F ; propagate carry clrf WREG ; ... addwfc isr_xC+5,F ; ... addwfc isr_xC+6,F ; ... ; isr_xC[3] done. movf isr_xA+3,W ; multiply a[3] * b[1] mulwf isr_xB+1 ; ... movf PRODL,W ; add cross product to c[5]:c[4] addwf isr_xC+4,F ; ... movf PRODH,W ; ... addwfc isr_xC+5,F ; propagate carry clrf WREG ; ... addwfc isr_xC+6,F ; ... ; isr_xC[4] done. movf isr_xA+3,W ; multiply a[3] * b[2] mulwf isr_xB+2 ; ... movf PRODL,W ; add cross product to c[5]:c[4] addwf isr_xC+5,F ; ... movf PRODH,W ; ... addwfc isr_xC+6,F ; propagate carry ; isr_xC[5] done. ; isr_xC[6] done. return ; done ;============================================================================= math15 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Multiply two SIGNED 24 Bit Integers: isr_xC = isr_xA * isr_xB ; ; trashes PRODL, PRODH, WREG ; global isr_signed_mult24x24 isr_signed_mult24x24: ; do an unsigned multiplication first call isr_unsigned_mult24x24 ; manage sign extension of operand B btfss isr_xB+2,7 ; is B negative ? bra isr_signed24_mult_checkA ; NO - continue checking operand A movf isr_xA+0,W ; YES - add -16777216 * A subwf isr_xC+3,F ; - ... movf isr_xA+1,W ; - ... subwfb isr_xC+4,F ; - ... movf isr_xA+2,W ; - ... subwfb isr_xC+5,F ; - ... isr_signed24_mult_checkA: ; manage sign extension of operand B btfss isr_xA+2,7 ; is A negative ? return ; NO - done movf isr_xB+0,W ; YES - add -16777216 * B subwf isr_xC+3,F ; - ... movf isr_xB+1,W ; - ... subwfb isr_xC+4,F ; - ... movf isr_xB+2,W ; - ... subwfb isr_xC+5,F ; - ... return ;============================================================================= math16 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Divide two UNSIGNED 16 Bit Integers: isr_xC:2 = isr_xA:2 / isr_xB:2 with xA as remainder ; ; trashes WREG ; global isr_div16x16 isr_div16x16: movlw .16 ; process 16 bits ... movwf isr_math_loop ; ... initialize loop counter movff isr_xA+0,isr_xC+0 ; copy isr_xA to isr_xC movff isr_xA+1,isr_xC+1 ; ... clrf isr_xA+0 ; clear isr_xA, will now be used to hold the remainder clrf isr_xA+1 ; ... isr_div16x16_1: bcf STATUS,C ; clear carry flag to shift in a zero bit rlcf isr_xC+0,F ; shift left isr_xC rlcf isr_xC+1,F ; ... shifting MSB out of isr_xC... rlcf isr_xA+0,F ; ... and into LSB of isr_xA rlcf isr_xA+1,F ; ... btfsc STATUS,C ; did the remainder overflow (carry set)? bra isr_div16x16_2 ; YES - directly generate a result bit = 1 movf isr_xB+0,W ; NO - compute remainder - divisor = isr_xA - isr_xB, trash result to WREG subwf isr_xA+0,W ; - ... movf isr_xB+1,W ; - ... subwfb isr_xA+1,W ; - ... btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ? bra isr_div16x16_3 ; YES - result bit = 0, keep LSB of isr_xC+0 being 0 isr_div16x16_2: bsf isr_xC+0,0 ; NO - result bit = 1, set LSB of isr_xC+0 to 1 movf isr_xB+0,W ; - subtract divisor from remainder "for real": isr_xA = isr_xA - isr_xB subwf isr_xA+0,F ; - ... movf isr_xB+1,W ; - ... subwfb isr_xA+1,F ; - ... isr_div16x16_3: decfsz isr_math_loop,F ; decrement loop counter, all bits done? bra isr_div16x16_1 ; NO - loop return ; YES - done ;============================================================================= math17 CODE ;============================================================================= ;----------------------------------------------------------------------------- ; Divide a 32 Bit Integer by a 16 Bit Integer: isr_xC:4 = isr_xC:4 / isr_xB:2 with isr_xA as remainder ; ; trashes WREG ; global isr_div32x16 isr_div32x16: movlw .32 ; process 32 bits ... movwf isr_math_loop ; ... initialize loop counter clrf isr_xA+0 ; clear isr_xA, will be used to hold the remainder clrf isr_xA+1 ; ... isr_div32x16_1: bcf STATUS,C ; clear carry flag to shift in a zero bit rlcf isr_xC+0,F ; shift left isr_xC rlcf isr_xC+1,F ; ... rlcf isr_xC+2,F ; ... rlcf isr_xC+3,F ; ... shifting MSB out of isr_xC... rlcf isr_xA+0,F ; ... and into LSB of isr_xA rlcf isr_xA+1,F ; ... btfsc STATUS,C ; did the remainder overflow (carry set)? bra isr_div32x16_2 ; YES - directly generate a result bit = 1 movf isr_xB+0,W ; NO - compute remainder - divisor = isr_xA - isr_xB, trash result to WREG subwf isr_xA+0,W ; - ... movf isr_xB+1,W ; - ... subwfb isr_xA+1,W ; - ... btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ? bra isr_div32x16_3 ; YES - result bit = 0, keep LSB of isr_xC+0 being 0 isr_div32x16_2: bsf isr_xC+0,0 ; NO - result bit = 1, set LSB of isr_xC+0 to 1 movf isr_xB+0,W ; - subtract divisor from remainder "for real": isr_xA = isr_xA - isr_xB subwf isr_xA+0,F ; - ... movf isr_xB+1,W ; - ... subwfb isr_xA+1,F ; - ... isr_div32x16_3: decfsz isr_math_loop,F ; decrement loop counter, all bits done? bra isr_div32x16_1 ; NO - loop return ; YES - done ;----------------------------------------------------------------------------- END