-- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- All math routines in this file are 3-byte / 24 bit straight binary math routines -- -- The math routines acts on fixed variables X and Y, definied in this library -- -- overview of the available routines -- byte3_XCHG Y = X , X = Y -- byte3_SHL ( N ) Y = Y shl N -- byte3_SRL ( N , rot) Y = Y shl/rol N -- byte3_SRR ( N , rot) Y = Y shr/ror N -- byte3_CLIP_NEG Y = 0 if msb set, else Y is left unchanged -- byteN_ADD Y = Y + X , X unchanged -- byteN_INC Y = Y + 1 ,if Y==0 then X1=0 else X1=1 (X3,X2 unchanged) -- byteN_SUB Y = Y - X , X unchanged -- byte3_DEC ... -- byte3_LOG Y = log ( X ) , X CHANGED !! -- Accuracy of the LOG function must be set by the user, through the constant -- const LOG_ACCURACY = 3 -- -- Author : Stef Mientki -- uses : automatic translated INC files in SM-notation -- -- Version: 1.3 15-08-2002 -- - INC 2,3,4,N bytes added -- - DEC added ?? -- - CMP 2,3,4,N bytes compare -- - ADD 2,3,4,N bytes compare -- - SUB 2,3,4,N bytes compare -- -- Version: 1.2 05-03-2002 -- - SHL replaced by SRL -- - SRR added -- - CLIP_NEG added -- -- Version: 1.1 27-02-2002 -- - LOG_ACCURACY must be definied outside this library -- -- Version: 1.0 26-02-2002 orginal release -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- this contents determines the length of the lookuptable for log calculation -- and thereby the accuracy, length = 2 ^ ( LUT_Nbits - 1 ) -- LUT_Nbits error argument which gives the largest error -- 3 0.3 % 0x09 -- 4 0.06 % 0x11 -- 5 0.013 % 0x21 -- 6 0.003 % 0x41 const LUT_Nbits = LOG_ACCURACY -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- Definitions of the global vars -- X = 24 bit = X3 .. X2 .. X1 -- Y = 24 bit var byte x4,x3,x2,x1 var byte y4,y3,y2,y1 var byte z4,z3,z2,z1 -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- 24 bit Exchange routine -- Y = X -- X = Y -- ----------------------------------------------------------------------------- procedure byte3_XCHG is var byte x x = X1 X1 = Y1 Y1 = x x = X2 X2 = Y2 Y2 = x x = X3 X3 = Y3 Y3 = x end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- 24 bit negative clipping -- if Y negative (msb is set) Y = 0 else Y is left unchanged -- X is left unchanged -- -- in fact this is not a straight binary routine, -- it'll clip negative (offset binary) values to zero -- ----------------------------------------------------------------------------- procedure byte3_CLIP_NEG is assembler local finish btfss y3,7 ; goto finish ; movlw 0 ; movwf y3 ; movwf y2 ; movwf y1 ; finish: ; end assembler end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- 24 bit SHL routine -- Y = Y shl N -- X is left unchanged -- ----------------------------------------------------------------------------- procedure byte3_SHL ( byte in N ) is assembler local next next: ; bcf status,C ;clear Carry rlf y1,f ; rlf y2,f ; rlf y3,f ; decfsz N,f ; goto next ; end assembler end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- 24 bit shift/rotate LEFT routine -- if shift (default) then Y = Y shl N -- if rotate then Y = Y rol N -- X is left unchanged -- ----------------------------------------------------------------------------- procedure byte3_SRL ( byte in N = 1 , bit in rotate = false ) is assembler local next, no_rotate next: ; bcf status,C ;clear Carry btfss rotate ;test if rotate goto no_rotate ;if shift rather than rotate btfsc y3,7 ;if rotate, test lsb bsf status,C ;set Carry according to lsb no_rotate: ; rrf y1,f ; rrf y2,f ; rrf y3,f ; decfsz N,f ; goto next ; end assembler end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- 24 bit shift/rotate Right routine -- if shift (default) then Y = Y shr N -- if rotate then Y = Y ror N -- X is left unchanged -- ----------------------------------------------------------------------------- procedure byte3_SRR ( byte in N = 1 , bit in rotate = false ) is assembler local next, no_rotate next: ; bcf status,C ;clear Carry btfss rotate ;test if rotate goto no_rotate ;if shift rather than rotate btfsc y1,0 ;if rotate, test lsb bsf status,C ;set Carry according to lsb no_rotate: ; rrf y3,f ; rrf y2,f ; rrf y1,f ; decfsz N,f ; goto next ; end assembler end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- N,2,3,4 byte ADD routine -- Y = Y + X -- X is left unchanged -- ----------------------------------------------------------------------------- procedure byteN_ADD is assembler local carry_y2, carry_y3, carry_y4, start goto start ; -- this method can easily be expanded to more bytes carry_y2: ;aggregate the Carry through all the bytes incfsz y2,f ; return ; carry_y3: incfsz y3,f ; return ; carry_y4: incf y4,f ; return ; start: ; movf x1,w ; W = X1 addwf y1,f ; Y1 = Y1 + X1 btfsc status,C ; skip if no Carry call carry_y2 ; add Carry movf x2,w ; W = X2 addwf y2,f ; Y2 = Y2 + X2 btfsc status,C ; skip if no Carry call carry_y3 ; add Carry movf x3,w ; W = X3 addwf y3,f ; Y3 = Y3 + X3 btfsc status,C ; skip if no Carry call carry_y4 ; add Carry movf x4,w ; W = X4 addwf y4,f ; Y4 = Y4 + X4 end assembler end procedure -- ----------------------------------------------------------------------------- procedure byte2_ADD is byteN_add end procedure -- ----------------------------------------------------------------------------- procedure byte3_ADD is byteN_add end procedure -- ----------------------------------------------------------------------------- procedure byte4_ADD is byteN_add end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- N,2,3,4 bytes CMP routine -- if X=Y then Z1=0 else Z1=1 -- X, Y, Z4,Z3,X2 are left unchanged -- ----------------------------------------------------------------------------- procedure byteN_cmp (byte in N) is assembler local byte4, byte3, byte2, byte1, equal, not_equal, finish decf N,f ; btfsc Z ; goto byte1 ;if ZERO (N=1) decf N,f ; btfsc Z ; goto byte2 ;if ZERO (N=2) decf N,f ; btfsc Z ; goto byte3 ;if ZERO (N=3) byte4: ; movf x4,w ; subwf y4,w ; btfss Z ; goto not_equal ; byte3: ; movf x3,w ; subwf y3,w ; btfss Z ; goto not_equal ; byte2: movf x2,w ; subwf y2,w ; btfss Z ; goto not_equal ; byte1: movf x1,w ; subwf y1,w ; btfss Z ; goto not_equal ; equal: ; movlw 0 ; goto finish ; not_equal: ; movlw 1 ; finish: ; movwf z1 ; end assembler end procedure -- ----------------------------------------------------------------------------- procedure byte2_CMP is byteN_CMP (2) end procedure -- ----------------------------------------------------------------------------- procedure byte3_CMP is byteN_CMP (3) end procedure -- ----------------------------------------------------------------------------- procedure byte4_CMP is byteN_CMP (4) end procedure -- ----------------------------------------------------------------------------- function byteN_fcmp (byte in N) return bit is byteN_cmp (N) return Z1 == 0 end function -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- N,2,3,4 bytes INC routine -- Y = Y + 1 -- if Y=0 then Z1=0 else Z1=1 -- X, Z4,Z3,X2 are left unchanged -- ----------------------------------------------------------------------------- procedure byte2_INC is assembler local non_zero, finish incfsz y1,f ; goto non_zero ; incfsz y2,f ; goto non_zero ; movlw 0 ; goto finish ; non_zero: ; movlw 1 ; finish: ; movwf z1 ; end assembler end procedure -- ----------------------------------------------------------------------------- procedure byte3_INC is assembler local non_zero, finish incfsz y1,f ; goto non_zero ; incfsz y2,f ; goto non_zero ; incfsz y3,f ; goto non_zero ; movlw 0 ; goto finish ; non_zero: ; movlw 1 ; finish: ; movwf z1 ; end assembler end procedure -- ----------------------------------------------------------------------------- procedure byte4_INC is assembler local non_zero, finish incfsz y1,f ; goto non_zero ; incfsz y2,f ; goto non_zero ; incfsz y3,f ; goto non_zero ; incfsz y4,f ; goto non_zero ; movlw 0 ; goto finish ; non_zero: ; movlw 1 ; finish: ; movwf z1 ; end assembler end procedure -- ----------------------------------------------------------------------------- procedure byteN_INC (byte in N) is if N == 2 then byte2_inc elsif N == 3 then byte3_inc elsif N == 4 then byte4_inc else pragma error -- invalid number of bytes end if end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- 24 bit multiply routine -- Y = Y * X -- X is CHANGED !! -- ----------------------------------------------------------------------------- procedure byte3_MUL is var byte s3, s2, s1, N assembler local next, no_add movfw y3 ; S = Y movwf s3 ; movfw y2 ; movwf s2 ; movfw y1 ; movwf s1 ; ; movlw 0 ; Y = 0 movwf y3 ; movwf y2 ; movwf y1 ; movlw 24 ; movwf N ; next: ; btfss S1,0 ; goto no_add ; call byte3_add ; Y = Y + X ;clip at overflow no_add: ; bcf C ; X = X shl 1 (shift in Carry = 0) rlf x1,f ; rlf x2,f ; rlf x3,f ; rrf s3,f ; S = S shr 1 (carry is don't care) rrf s2,f ; rrf s1,f ; decfsz N,f ; goto next ; ; end assembler end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- N,2,3,4 byte SUB routine -- Y = Y - X -- X is left unchanged -- ----------------------------------------------------------------------------- procedure byteN_SUB is assembler local borrow_y2, borrow_y3, borrow_y4, start page start goto start ; -- this method can easily be expanded to more bytes borrow_y2: ;aggregate the Borrow through all the bytes decf y2,f ; comf y2,w ; btfss status,Z ; return ; borrow_y3: ;aggregate the Borrow through all the bytes decf y3,f ; comf y3,w ; btfss status,Z ; return ; borrow_y4: decf y4,f ; return ; start: ; movf x1,w ; W = X1 subwf y1,f ; Y1 = Y1 - X1 btfss status,C ; skip if no Borrow call borrow_y2 ; subtract Borrow movf x2,w ; W = X2 subwf y2,f ; Y2 = Y2 - X2 btfss status,C ; skip if no Borrow call borrow_y3 ; subtract Borrow movf x3,w ; W = X3 subwf y3,f ; Y3 = Y3 - X3 btfss status,C ; skip if no Borrow call borrow_y4 ; subtract Borrow movf x4,w ; W = X4 subwf y4,f ; Y4 = Y4 - X4 end assembler end procedure -- ----------------------------------------------------------------------------- procedure byte2_SUB is byteN_SUB end procedure -- ----------------------------------------------------------------------------- procedure byte3_SUB is byteN_SUB end procedure -- ----------------------------------------------------------------------------- procedure byte4_SUB is byteN_SUB end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- 24 bit DEC routine -- Y = Y - 1 -- X = 1 -- ----------------------------------------------------------------------------- procedure byte3_DEC is X1 = 1 X2 = 0 X3 = 0 byte3_SUB end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- procedure _table_log_msb (byte in indx, byte out x) is pragma jump_table if LUT_Nbits <= 2 then elsif LUT_Nbits == 3 then assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; PIC 24 bits log tables ; for 3 bits lookup ; gainfactor = $080000 ; largest value = $00BFFFF9 ; MSB table (3bit) retlw 0x10 retlw 0x12 retlw 0x14 retlw 0x16 retlw 0x18 exit: ; end assembler elsif LUT_Nbits == 4 then assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; PIC 24 bits log tables ; for 4 bits lookup ; gainfactor = $080000 ; largest value = $00BFFFF7 ; MSB table (4bit) retlw 0x18 retlw 0x19 retlw 0x1A retlw 0x1B retlw 0x1C retlw 0x1D retlw 0x1E retlw 0x1F retlw 0x20 exit: ; end assembler elsif LUT_Nbits == 5 then assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; PIC 24 bits log tables ; for 5 bits lookup ; gainfactor = $080000 ; largest value = $00BFFFF6 ; MSB table (5bit) retlw 0x20 retlw 0x20 retlw 0x21 retlw 0x21 retlw 0x22 retlw 0x23 retlw 0x23 retlw 0x24 retlw 0x24 retlw 0x25 retlw 0x25 retlw 0x26 retlw 0x26 retlw 0x26 retlw 0x27 retlw 0x27 retlw 0x28 exit: ; end assembler else assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; PIC 24 bits log tables ; for 6 bits lookup ; gainfactor = $080000 ; largest value = $00BFFFFA ; MSB table (6bit) retlw 0x28 retlw 0x28 retlw 0x28 retlw 0x29 retlw 0x29 retlw 0x29 retlw 0x29 retlw 0x2A retlw 0x2A retlw 0x2A retlw 0x2B retlw 0x2B retlw 0x2B retlw 0x2B retlw 0x2C retlw 0x2C retlw 0x2C retlw 0x2C retlw 0x2D retlw 0x2D retlw 0x2D retlw 0x2D retlw 0x2E retlw 0x2E retlw 0x2E retlw 0x2E retlw 0x2E retlw 0x2F retlw 0x2F retlw 0x2F retlw 0x2F retlw 0x2F retlw 0x30 exit: ; end assembler end if end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- procedure _table_log_middle (byte in indx, byte out x) is pragma jump_table if LUT_Nbits <= 2 then elsif LUT_Nbits == 3 then assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; middle table (3bit) retlw 0x00 retlw 0x93 retlw 0xAE retlw 0x75 retlw 0x00 exit: ; end assembler elsif LUT_Nbits == 4 then assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; middle table (4bit) retlw 0x00 retlw 0x5C retlw 0x93 retlw 0xAC retlw 0xAE retlw 0x9A retlw 0x75 retlw 0x41 retlw 0x00 exit: ; end assembler elsif LUT_Nbits == 5 then assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; middle table (5bit) retlw 0x00 retlw 0xB3 retlw 0x5C retlw 0xFB retlw 0x93 retlw 0x23 retlw 0xAC retlw 0x30 retlw 0xAE retlw 0x26 retlw 0x9A retlw 0x0A retlw 0x75 retlw 0xDD retlw 0x41 retlw 0xA2 retlw 0x00 exit: ; end assembler else assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; middle table (6bit) retlw 0x00 retlw 0x5A retlw 0xB3 retlw 0x08 retlw 0x5C retlw 0xAC retlw 0xFB retlw 0x48 retlw 0x93 retlw 0xDC retlw 0x23 retlw 0x68 retlw 0xAC retlw 0xEF retlw 0x30 retlw 0x6F retlw 0xAE retlw 0xEA retlw 0x26 retlw 0x61 retlw 0x9A retlw 0xD2 retlw 0x0A retlw 0x40 retlw 0x75 retlw 0xA9 retlw 0xDD retlw 0x0F retlw 0x41 retlw 0x72 retlw 0xA2 retlw 0xD1 retlw 0x00 exit: ; end assembler end if end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- procedure _table_log_lsb (byte in indx, byte out x) is pragma jump_table if LUT_Nbits <= 2 then elsif LUT_Nbits == 3 then assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; LSB table (3bit) retlw 0x00 retlw 0x4F retlw 0x01 retlw 0x76 retlw 0x00 exit: ; end assembler elsif LUT_Nbits == 4 then assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; LSB table (4bit) retlw 0x00 retlw 0x02 retlw 0x4F retlw 0xEA retlw 0x01 retlw 0x80 retlw 0x76 retlw 0x50 retlw 0x00 exit: ; end assembler elsif LUT_Nbits == 5 then assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; LSB table (5bit) retlw 0x00 retlw 0x20 retlw 0x02 retlw 0xC1 retlw 0x4F retlw 0x77 retlw 0xEA retlw 0x41 retlw 0x01 retlw 0x9E retlw 0x80 retlw 0x02 retlw 0x76 retlw 0x25 retlw 0x50 retlw 0x32 retlw 0x00 exit: ; end assembler else -- LUT_Nbits == 6 assembler local table,exit page exit movfw indx ; call table ; movwf x ; goto exit ; table: ; addwf PCL,F ; ; LSB table (6bit) retlw 0x00 retlw 0xEB retlw 0x20 retlw 0xC6 retlw 0x02 retlw 0xF6 retlw 0xC1 retlw 0x81 retlw 0x4F retlw 0x44 retlw 0x77 retlw 0xFD retlw 0xEA retlw 0x51 retlw 0x41 retlw 0xCC retlw 0x01 retlw 0xED retlw 0x9E retlw 0x21 retlw 0x80 retlw 0xC8 retlw 0x02 retlw 0x3A retlw 0x76 retlw 0xC2 retlw 0x25 retlw 0xA7 retlw 0x50 retlw 0x26 retlw 0x32 retlw 0x78 retlw 0x00 exit: ; end assembler end if end procedure -- ----------------------------------------------------------------------------- -- ----------------------------------------------------------------------------- -- 24 bit LOG routine -- Y = log ( X ) -- X is CHANGED !! -- ----------------------------------------------------------------------------- procedure byte3_LOG is const TOT_Nbits = 24 const frac_bit = 7 - LUT_Nbits -- 4 var byte lut_index var byte lut_mask var byte nshift var byte x var byte x_shl var byte z3,z2,z1 -- limit lower argument value at 1 if (x3 == 0) & (x2 == 0) & (x1 == 0) then x1 = 1 end if -- determine X_SHL x_shl = 0 assembler local next_rot, test_msb, finish page finish bcf status,C ;clear Carry goto test_msb ; next_rot: ;shift X until msb is "1" rlf x1,f ; rlf x2,f ; rlf x3,f ; test_msb: ; btfsc x3,7 ;test msb goto finish ; incf x_shl,f ;remember number of shifts goto next_rot ; finish: ; end assembler -- determine lookupindex LUT_mask = 0xFF >> LUT_Nbits x = ( x3 & ! LUT_mask ) >> ( 8 - LUT_Nbits ) -- determine fraction z1 = x1 z2 = x2 z3 = x3 & LUT_mask -- calculate the difference between the 2 consecutive table values x = x - (1 << (LUT_Nbits - 1)) _table_log_msb ( x, X3 ) _table_log_middle ( x, X2 ) _table_log_lsb ( x, X1 ) x = x + 1 _table_log_msb ( x, Y3 ) _table_log_middle ( x, Y2 ) _table_log_lsb ( x, Y1 ) byte3_sub -- y = LUT[i+1] - LUT[i] -- x = LUT[i] byte3_xchg -- exchange x an y -- so now y = LUT[i] and here the result of the fraction will be added -- x = delta-LUT = LUT[i+1] - LUT[i] nshift = TOT_Nbits - LUT_Nbits assembler local next_frac, no_add page no_add ;shift delta_LUT one bit to the right next_frac: ; bcf status,C ;divide delta-LUT by 2 rrf X3,f ; rrf X2,f ; rrf X1,f ; ;test the relevant bit of the fraction btfss Z3,frac_bit; goto no_add ; ;add another part of the fraction call byte3_add ; Y = Y + X = sum + delta-LUT ;shift fraction one bit to the left (Carry doesn't mind) no_add: ; rlf Z1,f ; rlf Z2,f ; rlf Z3,f ; ;test if ready decfsz nshift,f ; goto next_frac ; end assembler -- now Y is the result, except for the X_SHL correction -- exchange X and Y, so X = result byte3_xchg -- fill Y Y1 = ( TOT_Nbits - LUT_Nbits ) - x_shl Y2 = 0 Y3 = 0 byte3_shl ( TOT_Nbits - 5 ) -- now get the final result byte3_add end procedure -- -----------------------------------------------------------------------------