;============================================================================ ; PROJECT : PIC'SPECTRUM ; ; FILE : FIXED.INC ; ; VERSION : 1.0 ; ; DESCRIPTION : ; ; Fixed point math routines, using a s-2-13 format (sign bit in 2-complement, ; 2 bits for the integer part, 13 bits for the decimal part). ; ; So values ranging +/-3.99999, resolution 1/2 exp 13 = 0.000122 ; ; All functions and macros operate on registers ra,rb and rdiv. ; Storage could be in a generic "arithmetic" register (2 bytes) or in the ; FFT data storage table ("data") ; ; The FFT data table ("data") is a 4x64 word (2 bytes) array, each of the ; four blocks being at at fixed adress ($80 to $ff) in the 4 banks. ; ; WARNING : ra/rb/rdiv are supposed to be in unbanked memory. Bank selector ; must be zone_main on input (access to arithmetic tempory registers) and is ; garanteed to be zone_main on exit ; ; In general, WREG is scrambled ; ;============================================================================ ; Developped & Copyrighted by Robert LACOSTE ;============================================================================ ;---------------------------------------------------------------------------- ; m_lda : copy a given general register (2 bytes) in register A ;---------------------------------------------------------------------------- m_lda macro inreg movfp inreg,WREG movwf ra movfp inreg+1,WREG movwf ra+1 endm ;---------------------------------------------------------------------------- ; m_ldai : get the data value pointed by reg and store it in A ;---------------------------------------------------------------------------- m_ldai macro indexreg movfp indexreg, WREG movlr 0 ; start with bank 0 btfsc WREG,7 ; select the good bank bsf BSR,5 btfsc WREG,6 ; based on 2 first bits of WREG bsf BSR,4 bcf ALUSTA,C ; multiply by 2 rlcf WREG,1 bsf WREG,7 ; and set high bit (data start at $80) movwf FSR0 ; and use it as an index movpf INDF0,ra ; to get msb value incf FSR0,1 movpf INDF0,ra+1 ; and lsb value BANKSEL zone_main ; reset bank register endm ;---------------------------------------------------------------------------- ; m_ldb : copy a given general register (2 bytes) in register B ;---------------------------------------------------------------------------- m_ldb macro inreg movfp inreg,WREG movwf rb movfp inreg+1,WREG movwf rb+1 endm ;---------------------------------------------------------------------------- ; m_ldbi : get the data value pointed by reg and store it in B ;---------------------------------------------------------------------------- m_ldbi macro indexreg movfp indexreg,WREG movlr 0 ; start with bank 0 btfsc WREG,7 ; select the good bank bsf BSR,5 btfsc WREG,6 ; based on 2 first bits of WREG bsf BSR,4 bcf ALUSTA,C ; multiply by 2 rlcf WREG,1 bsf WREG,7 ; and set high bit (data start at $80) movwf FSR0 ; and use it as an index movpf INDF0,rb ; to get msb value incf FSR0,1 movpf INDF0,rb+1 ; and lsb value BANKSEL zone_main ; reset bank register endm ;---------------------------------------------------------------------------- ; m_lddiv : copy a given register (int, 1 byte) in register DIV ;---------------------------------------------------------------------------- m_lddiv macro inreg movfp inreg,WREG movwf rdiv endm ;---------------------------------------------------------------------------- ; m_sta : store the value of A in a given general register (2 bytes) ;---------------------------------------------------------------------------- m_sta macro outreg movfp ra,WREG movwf outreg movfp ra+1,WREG movwf outreg+1 endm ;---------------------------------------------------------------------------- ; m_stai : store the value of A in the data value pointed by reg ;---------------------------------------------------------------------------- m_stai macro indexreg movfp indexreg, WREG movlr 0 ; start with bank 0 btfsc WREG,7 ; select the good bank bsf BSR,5 btfsc WREG,6 ; based on 2 first bits of WREG bsf BSR,4 bcf ALUSTA,C ; multiply by 2 rlcf WREG,1 bsf WREG,7 ; and set high bit (data start at $80) movwf FSR0 ; and use it as an index movfp ra,INDF0 ; to store msb value incf FSR0,1 movfp ra+1,INDF0 ; and lsb value BANKSEL zone_main ; reset bank register endm ;---------------------------------------------------------------------------- ; m_ldaconst : load an immediate value in A ;---------------------------------------------------------------------------- m_ldaconst macro immvalue movlw high immvalue movwf ra movlw low immvalue movwf ra+1 endm ;---------------------------------------------------------------------------- ; m_mvba : copy B to A ;---------------------------------------------------------------------------- m_mvba macro movfp rb,WREG movwf ra movfp rb+1,WREG movwf ra+1 endm ;---------------------------------------------------------------------------- ; m_mvab : copy A to B ;---------------------------------------------------------------------------- m_mvab macro movfp ra,WREG movwf rb movfp ra+1,WREG movwf rb+1 endm ;---------------------------------------------------------------------------- ; m_add : add B to A (A=A+B) ;---------------------------------------------------------------------------- m_add macro movfp rb+1,WREG ; low byte first addwf ra+1,1 movfp rb,WREG ; and high byte with carry addwfc ra,1 endm ;---------------------------------------------------------------------------- ; m_sub : substract B to A (A=A-B) ;---------------------------------------------------------------------------- m_sub macro movfp rb+1,WREG ; low byte first subwf ra+1,1 movfp rb,WREG ; and high byte with borrow subwfb ra,1 endm ;---------------------------------------------------------------------------- ; m_mult : multiply A to B (A=A*B) ; Warning : overflow not detected if result>4 ;---------------------------------------------------------------------------- m_mult movfp ra+1,WREG ; low byte x low byte mulwf rb+1 movpf PRODH,mres1 movpf PRODL,mres0 movfp ra,WREG ; high byte x high byte mulwf rb movpf PRODH,mres3 movpf PRODL,mres2 movfp ra+1,WREG ; low byte x high byte mulwf rb movfp PRODL,WREG addwf mres1,F movfp PRODH,WREG addwfc mres2,F clrf WREG,F addwfc mres3,F movfp ra,WREG ; high byte x low byte mulwf rb+1 movfp PRODL,WREG addwf mres1,F movfp PRODH,WREG addwfc mres2,F clrf WREG,F addwfc mres3,F btfss rb,7 ; test if B<0 goto sign_ra movfp ra+1,WREG ; if so, adjust value subwf mres2,F movfp ra,WREG subwfb mres3,F sign_ra btfss ra,7 ; test if A<0 goto sign_end movfp rb+1,WREG ; if so, adjust value subwf mres2,F movfp rb,WREG subwfb mres3,F sign_end rlcf mres1,F ; shift left one time rlcf mres2,F rlcf mres3,F rlcf mres1,F ; shift left one time rlcf mres2,F rlcf mres3,F rlcf mres1,F ; shift left one time rlcf mres2,F rlcf mres3,F movfp mres3, ra ; forget mres1 and store result movfp mres2, ra+1 ; in register A end_mult return ;---------------------------------------------------------------------------- ; m_divi : divide A with rdiv (A=A/DIV), rdiv being a non signed integer ; value and not a fixed decimal value ; ; call routines provided in Microchip's application notes (see div.inc) ;---------------------------------------------------------------------------- ; Correspondances between AN617 variables and pic'spectrum's... #define AARGB0 (ra) #define AARGB1 (ra+1) #define ACCB0 (ra) #define ACCB1 (ra+1) #define BARGB0 (rdiv) #define REMB0 (mres0) #define TEMP (mres1) #include "div.inc" ; include 16/8 unsigned routine ; from microchip's application ; note AN617 m_divi bcf msign,0 btfss ra,7 ; save sign of A goto m_divdiv bsf msign,0 comf ra,F ; if A<0, A=-A comf ra+1,F movlw 1 addwf ra+1,F movlw 0 addwfc ra,F m_divdiv call FXD1608U ; do the division btfss msign,0 ; if A was <0 goto end_mdivi comf ra,F ; A=-A comf ra+1,F movlw 1 addwf ra+1,F movlw 0 addwfc ra,F end_mdivi return ;---------------------------------------------------------------------------- ; m_div2 : divide A with 2 (A=A/2) ;---------------------------------------------------------------------------- m_div2 macro bcf ALUSTA,C rrcf ra,1 ; divide high byte rrcf ra+1,1 ; and low byte btfsc ra,6 ; test old sign bit bsf ra,7 ; if set, report it to msb endm ;---------------------------------------------------------------------------- ; m_sin : replace A with sin(A) ; ; Sinus calculated by interpolation of tables values got with the MSB of A ; (v_msb and v_msb+1 are the table values corresponding for msb A and the next ; one in the table, with rollover on 0) ; ; ra=(((v_msb+1)*lsb ra+(v_msb)*(1-lsb ra)); ; ;---------------------------------------------------------------------------- radix dec sintable dw 0 dw 255, 511, 766, 1021, 1274, 1527, 1777, 2026 dw 2273, 2518, 2760, 3000, 3237, 3470, 3700, 3927 dw 4150, 4368, 4583, 4793, 4998, 5198, 5393, 5583 dw 5768, 5947, 6120, 6287, 6448, 6603, 6751, 6893 dw 7028, 7156, 7277, 7391, 7498, 7597, 7689, 7774 dw 7850, 7920, 7981, 8035, 8081, 8119, 8149, 8171 dw 8185, 8191, 8189, 8179, 8162, 8136, 8102, 8060 dw 8011, 7953, 7888, 7815, 7735, 7647, 7551, 7448 dw 7338, 7221, 7097, 6965, 6827, 6682, 6531, 6373 dw 6210, 6040, 5864, 5682, 5495, 5303, 5105, 4902 dw 4695, 4483, 4266, 4046, 3821, 3593, 3361, 3126 dw 2888, 2647, 2404, 2158, 1910, 1660, 1408, 1156 dw 902, 647, 391, 135, -120, -375, -631, -886 dw -1140, -1393, -1644, -1894, -2142, -2388, -2632, -2873 dw -3111, -3347, -3579, -3807, -4032, -4253, -4469, -4682 dw -4889, -5092, -5290, -5483, -5671, -5853, -6029, 6199 dw 6029, 5853, 5671, 5483, 5290, 5092, 4889, 4682 dw 4469, 4253, 4032, 3807, 3579, 3347, 3111, 2873 dw 2632, 2388, 2142, 1894, 1644, 1393, 1140, 886 dw 631, 375, 120, -135, -391, -647, -902, -1156 dw -1408, -1660, -1910, -2158, -2404, -2647, -2888, -3126 dw -3361, -3593, -3821, -4046, -4266, -4483, -4695, -4902 dw -5105, -5303, -5495, -5682, -5864, -6040, -6210, -6373 dw -6531, -6682, -6827, -6965, -7097, -7221, -7338, -7448 dw -7551, -7647, -7735, -7815, -7888, -7953, -8011, -8060 dw -8102, -8136, -8162, -8179, -8189, -8191, -8185, -8171 dw -8149, -8119, -8081, -8035, -7981, -7920, -7850, -7774 dw -7689, -7597, -7498, -7391, -7277, -7156, -7028, -6893 dw -6751, -6603, -6448, -6287, -6120, -5947, -5768, -5583 dw -5393, -5198, -4998, -4793, -4583, -4368, -4150, -3927 dw -3700, -3470, -3237, -3000, -2760, -2518, -2273, -2026 dw -1777, -1527, -1274, -1021, -766, -511, -255 radix hex m_sin movpf rb,sin_saveb ; save B register movpf rb+1,sin_saveb+1 clrf x0+1,F movlw low (sintable) ; get table value corresponding addwf ra,W ; to the MSB of A -> x0 movwf TBLPTRL movlw high (sintable) addwfc x0+1,W movwf TBLPTRH tablrd 0,0,x0+1 ; dummy read, update TABLATH tlrd 1,x0 ; read high byte tablrd 0,1,x0+1 ; and low byte. clrf x1+1,F movfp ra,WREG ; get table value corresponding incf WREG,W ; to the MSB of A+1, with rollover at 0 addlw low (sintable) ; -> x1 movwf TBLPTRL movlw high (sintable) addwfc x1+1,W movwf TBLPTRH tablrd 0,0,x1+1 ; dummy read, update TABLATH tlrd 1,x1 ; read high byte tablrd 0,1,x1+1 ; and low byte. movfp ra+1,WREG ; get LSB of A in B movwf rb ; and convert it to a weight factor clrf rb+1,F bcf ALUSTA,C rrcf rb,F rrcf rb+1,F bcf ALUSTA,C rrcf rb,F rrcf rb+1,F bcf ALUSTA,C rrcf rb,F rrcf rb+1,F m_lda x1 ; multiply "next" table value by B call m_mult m_sta x2 ; and save it m_ldaconst H'2000' ; calculate 1-weight factor m_sub m_ldb x0 ; and multiply it by "previous" value call m_mult m_ldb x2 ; add the 2 values m_add end_sin m_ldb sin_saveb ; restore B return ;---------------------------------------------------------------------------- ; m_scalelin : linear scaling of A, result in WREG (0 to MAXPIXVAL) ; and in rdiv ;---------------------------------------------------------------------------- m_scalelin movpf ra,scaletmp ; copy A to scaletmp reg movpf ra+1,scaletmp+1 bcf ALUSTA,C ; divide by 16 rrcf scaletmp,F ; 2 rrcf scaletmp+1,F bcf ALUSTA,C rrcf scaletmp,F ; 4 rrcf scaletmp+1,F bcf ALUSTA,C rrcf scaletmp,F ; 8 rrcf scaletmp+1,F bcf ALUSTA,C rrcf scaletmp,F ; 16 rrcf scaletmp+1,F tstfsz scaletmp ; if tmp>255, return MAXPIVXVAL goto m_scaleover movlw MAXPIXVAL ; if tmp>MAXPIXVAL, return MAXPIXVAL cpfslt scaletmp+1 goto m_scaleover movfp scaletmp+1,WREG goto end_scalelin m_scaleover movlw MAXPIXVAL ; return MAXPIXVAL end_scalelin movwf rdiv return ;---------------------------------------------------------------------------- ; m_scalelog : logarithmic scaling of A, result in WREG (0 to 127) ; and in rdiv ;---------------------------------------------------------------------------- radix dec logtable dw -4652, -4599, -4548, -4496, -4445, -4394, -4344, -4294 dw -4245, -4196, -4147, -4098, -4050, -4002, -3955, -3908 dw -3861, -3815, -3769, -3723, -3677, -3632, -3587, -3543 dw -3498, -3454, -3410, -3367, -3324, -3281, -3238, -3196 dw -3154, -3112, -3071, -3029, -2988, -2947, -2907, -2867 dw -2826, -2787, -2747, -2708, -2669, -2630, -2591, -2553 dw -2514, -2476, -2438, -2401, -2363, -2326, -2289, -2253 dw -2216, -2180, -2143, -2107, -2072, -2036, -2001, -1965 dw -1930, -1895, -1861, -1826, -1792, -1758, -1724, -1690 dw -1656, -1623, -1590, -1556, -1523, -1491, -1458, -1425 dw -1393, -1361, -1329, -1297, -1265, -1234, -1202, -1171 dw -1140, -1109, -1078, -1047, -1017, -986, -956, -926 dw -896, -866, -836, -806, -777, -748, -718, -689 dw -660, -631, -603, -574, -545, -517, -489, -461 dw -433, -405, -377, -349, -322, -294, -267, -240 dw -213, -186, -159, -132, -105, -79, -52, -26 radix hex m_scalelog tstfsz ra ; if A<32, return 0 goto log_testover movlw D'32' cpfslt ra+1 goto log_testover movlw 0 goto end_scalelog log_testover movlw high (D'032'*MAXPIXVAL) ; if A>32xMAXPIXVAL, cpfsgt ra ; return MAXPIXVAL goto log_1 goto log_scaleover log_1 cpfseq ra goto log_ok movlw low (D'032'*MAXPIXVAL) cpfslt ra+1 goto log_scaleover log_ok clrf logresult,F ; clear result register clrf logresult+1,F movpf ra,scaletmp ; copy A to scaletmp reg movpf ra+1,scaletmp+1 ; (A is between 32 and 32xMAXPIXVAL) bcf ALUSTA,C ; multiply tmp by 8 rlcf scaletmp+1,F ; 2 rlcf scaletmp,F bcf ALUSTA,C rlcf scaletmp+1,F ; 4 rlcf scaletmp,F bcf ALUSTA,C rlcf scaletmp+1,F ; 8 rlcf scaletmp,F ; (tmp is between 256 and 256xMAXPIXVAL) ; normalization phase, until tmp < 256 log_normalize tstfsz scaletmp goto log_2 goto log_endnorm log_2 movlw low D'4652' ; add 256*ln(2)*MAXPIXVAL/ln(MAXPIXVAL) addwf logresult+1,F ; (=4652) to current evaluation movlw high D'4652' addwfc logresult,F bcf ALUSTA,C rrcf scaletmp,F ; divide tmp by 2 rrcf scaletmp+1,F goto log_normalize ; and loop log_endnorm movlw H'7F' ; tmp is now between 128 and 255 andwf scaletmp+1,W ; substract 128 addlw low (logtable) ; and use it as index in logtable movwf TBLPTRL movlw high (logtable) addwfc scaletmp,W movwf TBLPTRH tablrd 0,0,scaletmp+1 ; dummy read, update TABLATH tlrd 1,scaletmp ; read high byte tablrd 0,1,scaletmp+1 ; and low byte. movfp scaletmp+1,WREG addwf logresult+1,F ; add this value to result movfp scaletmp,WREG addwf logresult,F movfp logresult,WREG ; returned value is high byte goto end_scalelog log_scaleover movlw MAXPIXVAL end_scalelog movwf rdiv return ;---------------------------------------------------------------------------- ; m_mathtest : Test utility for mathlib functions ; ; to be used for debug only ;---------------------------------------------------------------------------- m_mathtest m_ldaconst H'23EF' ; (1.123) m_mvab m_ldaconst H'B561' ; (-2.332) m_lddiv 0 nop ; expected : ; RA = -19103 $B561 -2.331909 RB = 9199 $23EF 1.122925 RDIV = 0 $00 call m_sin nop ; expected : ; RA = -5931 $E8D5 -0.723999 RB = 9199 $23EF 1.122925 RDIV = 0 $00 call m_mult nop ; expected : ; RA = -6660 $E5FC -0.812988 RB = 9199 $23EF 1.122925 RDIV = 0 $00 m_mvab m_ldaconst H'F88B' ; (-0.233) m_add nop ; expected : ; RA = -8568 $DE88 -1.045898 RB = -6660 $E5FC -0.812988 RDIV = 0 $00 movlw D'44' movwf h1i m_stai h1i m_ldaconst 0 movlw D'44' movwf h1i m_ldbi h1i m_mvba nop ; expected : ; RA = -8568 $DE88 -1.045898 RB = -8568 $DE88 -1.045898 RDIV = 0 $00 m_ldaconst H'F8DB' ; (-0.2233); m_sub nop ; expected : ; RA = 6739 $1A53 0.822632 RB = -8568 $DE88 -1.045898 RDIV = 0 $00 m_mvab m_ldaconst H'470A' ; (2.22) m_add nop ; expected : ; RA = 24925 $615D 3.042603 RB = 6739 $1A53 0.822632 RDIV = 0 $00 call m_sin m_mvab m_ldaconst H'F560' ; (-0.332) call m_mult nop ; expected : ; RA = -268 $FEF4 -0.032715 RB = 809 $0329 0.098755 RDIV = 0 $00 m_sta h1r movlw D'31' movwf h2r m_lddiv h2r call m_divi nop ; expected : ; RA = -8 $FFF8 -0.000977 RB = 809 $0329 0.098755 RDIV = 31 $1F m_ldb h1r m_sub call m_mult nop ; expected : ; RA = -8 $FFF8 -0.000977 RB = -268 $FEF4 -0.032715 RDIV = 31 $1F m_mvab m_lda h1r call m_sin call m_mult nop ; expected : ; RA = 0 $0000 0.000000 RB = -8 $FFF8 -0.000977 RDIV = 31 $1F m_ldaconst H'18E2' ; (0.7776) call m_scalelin nop ; expected : ; RA = 6370 $18E2 0.777588 RB = -8 $FFF8 -0.000977 RDIV = 127 $7F m_ldaconst H'C15' ; (0.3776); call m_scalelin nop ; expected : ; RA = 3093 $0C15 0.377563 RB = -8 $FFF8 -0.000977 RDIV = 96 $60 m_ldaconst H'C15' ; (0.3776) m_div2 call m_scalelin nop ; expected : ; RA = 1546 $060A 0.188721 RB = -8 $FFF8 -0.000977 RDIV = 48 $30 m_ldaconst H'18E2' ; (0.7776); call m_scalelog nop ; expected : ; RA = 6370 $18E2 0.777588 RB = -8 $FFF8 -0.000977 RDIV = 127 $7F m_ldaconst H'C15' ; (0.3776) call m_scalelog nop ; expected : ; RA = 3093 $0C15 0.377563 RB = -8 $FFF8 -0.000977 RDIV = 119 $77 m_ldaconst H'C15' ; (0.3776) m_div2 call m_scalelog nop ; expected : ; RA = 1546 $060A 0.188721 RB = -8 $FFF8 -0.000977 RDIV = 101 $65 m_lda h1r m_div2 call m_sin m_mvab m_ldaconst H'438D' ; (2.111) call m_mult nop ; expected : ; RA = -280 $FEE8 -0.034180 RB = -133 $FF7B -0.016235 RDIV = 101 $65 call m_scalelog nop ; expected : ; RA = -280 $FEE8 -0.034180 RB = -133 $FF7B -0.016235 RDIV = 0 $00 return ;============================================================================ ; - END OF FILE - ;============================================================================