|
0
|
1 ;=============================================================================
|
|
|
2 ;
|
|
604
|
3 ; File math.asm REFACTORED VERSION V2.99d
|
|
0
|
4 ;
|
|
|
5 ; Math subroutines
|
|
|
6 ;
|
|
|
7 ; Copyright (c) 2011, JD Gascuel, HeinrichsWeikamp, all right reserved.
|
|
|
8 ;=============================================================================
|
|
|
9 ; HISTORY
|
|
|
10 ; 2011-08-03 : [mH] moving from OSTC code
|
|
|
11
|
|
|
12
|
|
604
|
13 #include "hwos.inc" ; mandatory header
|
|
0
|
14
|
|
604
|
15 math CODE
|
|
|
16
|
|
0
|
17 ;=============================================================================
|
|
604
|
18
|
|
|
19 global convert_time ; converts hi:lo in minutes to hours (up:hi) and minutes (lo)
|
|
|
20 ; also usable for conversion of seconds to minutes and seconds
|
|
|
21 ; trashes xA, xB, xC
|
|
|
22 convert_time:
|
|
|
23 movff lo,xA+0 ; copy hi:lo to xA
|
|
|
24 movff hi,xA+1 ;
|
|
|
25 movlw d'60' ; set divisor to 60
|
|
|
26 movwf xB+0 ; write 60 to xB
|
|
0
|
27 clrf xB+1 ;
|
|
604
|
28 rcall div16x16 ; xC = xA / xB with xA as remainder
|
|
|
29 movff xC+1,up ; result is hours / minutes, copy to up (high byte)
|
|
|
30 movff xC+0,hi ; result is hours / minutes, copy to hi (low byte)
|
|
|
31 movff xA+0,lo ; remainder is minutes / seconds, copy to lo
|
|
0
|
32 return
|
|
|
33
|
|
604
|
34
|
|
|
35 global div16 ; divA:2 = divA:2 / 2^WREG
|
|
|
36 ; trashes WREG
|
|
|
37 div16:
|
|
|
38 bcf STATUS,C ; clear carry
|
|
|
39 rrcf divA+1 ; rotate right high byte, carry into MSB, LSB into carry
|
|
|
40 rrcf divA+0 ; rotate right low byte, carry into MSB, LSB into carry
|
|
|
41 decfsz WREG ; decrement counter, done?
|
|
|
42 bra div16 ; NO - loop
|
|
|
43 return ; YES - done
|
|
0
|
44
|
|
|
45
|
|
604
|
46 global mult16 ; xA:2 = xA:2 * 2^WREG
|
|
|
47 ; trashes WREG
|
|
|
48 mult16:
|
|
|
49 bcf STATUS,C ; clear carry
|
|
|
50 rlcf divA+0,F ; rotate left low byte, carry into LSB, MSB into carry
|
|
|
51 rlcf divA+1,F ; rotate left high byte, carry into LSB, MSB into carry
|
|
|
52 decfsz WREG ; decrement counter, done?
|
|
|
53 bra mult16 ; NO - loop
|
|
|
54 return ; YES - done
|
|
0
|
55
|
|
|
56
|
|
604
|
57 global sub16 ; sub_c:2 = sub_a:2 - sub_b:2 with SIGNED values
|
|
|
58 ; sets neg_flag if result is < 0
|
|
|
59 ; trashes WREG
|
|
|
60 sub16:
|
|
|
61 bcf neg_flag ; clear flag which will indicate if result is negative
|
|
|
62 movf sub_b+0,W ; get value to be subtracted, low byte
|
|
|
63 subwf sub_a+0,W ; execute subtraction on low byte
|
|
|
64 movwf sub_c+0 ; copy result to output variable, low byte
|
|
|
65 movf sub_b+1,W ; get value to be subtracted, high byte
|
|
|
66 subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag
|
|
|
67 movwf sub_c+1 ; copy result to output variable, high byte
|
|
|
68 btfss STATUS,N ; result negative ?
|
|
|
69 return ; NO - result positive, done
|
|
|
70 bsf neg_flag ; YES - set flag
|
|
|
71 comf sub_c+1 ; - do a 16 bit sign change
|
|
|
72 negf sub_c+0 ;
|
|
|
73 btfsc STATUS,C ; - carry to be propagated ?
|
|
|
74 incf sub_c+1,F ; YES - do it
|
|
|
75 return ; - done
|
|
|
76
|
|
0
|
77
|
|
604
|
78 global subU16 ; sub_c:2 = sub_a:2 - sub_b:2 with UNSIGNED values
|
|
|
79 ; sets neg_flag if result is < 0
|
|
|
80 ; trashes WREG
|
|
|
81 subU16:
|
|
|
82 bcf neg_flag ; clear flag which will indicate if result is negative
|
|
|
83 movf sub_b+0,W ; get Value to be subtracted, low byte
|
|
|
84 subwf sub_a+0,W ; execute subtraction on low byte
|
|
|
85 movwf sub_c+0 ; copy result to output variable, low byte
|
|
|
86 movf sub_b+1,W ; get value to be subtracted, high byte
|
|
|
87 subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag
|
|
|
88 movwf sub_c+1 ; copy result to output variable, high byte
|
|
|
89 btfsc STATUS,C ; borrow to propagate (B == /CARRY) ?
|
|
|
90 return ; NO - result positive, done
|
|
|
91 bsf neg_flag ; YES - set flag
|
|
|
92 comf sub_c+1 ; - do a 16 bit sign change
|
|
|
93 negf sub_c+0 ;
|
|
|
94 btfsc STATUS,C ; - carry to be propagated ?
|
|
|
95 incf sub_c+1,F ; YES - do it
|
|
|
96 return ; - done
|
|
|
97
|
|
0
|
98
|
|
604
|
99 global mult16x16 ; xC:4 = xA:2 * xB:2 with UNSIGNED values
|
|
|
100 ; trashes PRODL, PRODH, WREG
|
|
|
101 mult16x16:
|
|
|
102 clrf xC+2 ; clear the high-order bits
|
|
|
103 clrf xC+3
|
|
|
104 ;
|
|
|
105 movf xA+0,W ; do the "L" multiplication
|
|
|
106 mulwf xB+0
|
|
|
107 movff PRODL,xC+0 ; copy result to xC
|
|
|
108 movff PRODH,xC+1
|
|
|
109 ;
|
|
|
110 movf xA+0,W ; do the "I" multiplication
|
|
|
111 mulwf xB+1
|
|
|
112 movf PRODL,W ; get the product's low byte...
|
|
|
113 addwf xC+1,F ; ... and add it to xC+1
|
|
|
114 movf PRODH,W ; get the product's high byte...
|
|
|
115 addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1
|
|
|
116 ;
|
|
|
117 movf xA+1,W ; do the "O" multiplication
|
|
|
118 mulwf xB+0
|
|
|
119 movf PRODL,W ; get the product's low byte...
|
|
|
120 addwf xC+1,F ; ... and add it to xC+1
|
|
|
121 movf PRODH,W ; get the product's high byte...
|
|
|
122 addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1
|
|
|
123 clrf WREG ; clear WREG...
|
|
|
124 addwfc xC+3,F ; ... add add it to xC+3 obeying carry bit from xC+2 (can only happen in "O" multiplication)
|
|
|
125 ;
|
|
|
126 movf xA+1,W ; do the "F" multiplication
|
|
|
127 mulwf xB+1
|
|
|
128 movf PRODL,W ; get the product's low byte...
|
|
|
129 addwf xC+2,F ; ... and add it to xC+2
|
|
|
130 movf PRODH,W ; get the product's high byte...
|
|
|
131 addwfc xC+3,F ; ... and add it to xC+3 obeying carry bit from xC+2
|
|
|
132 return
|
|
0
|
133
|
|
|
134
|
|
604
|
135 global div16x16 ; xC:2 = xA:2 / xB:2 with xA as remainder
|
|
|
136 ; trashes WREG
|
|
|
137 div16x16:
|
|
|
138 movlw .16 ; process 16 bits ...
|
|
|
139 movwf math_loop ; ... initialize loop counter
|
|
|
140 movff xA+0,xC+0 ; copy xA to xC
|
|
|
141 movff xA+1,xC+1 ; ...
|
|
|
142 clrf xA+0 ; clear xA, will now be used to hold the remainder
|
|
|
143 clrf xA+1 ; ...
|
|
|
144 div16x16_1:
|
|
|
145 bcf STATUS,C ; clear carry flag to shift in a zero bit
|
|
|
146 rlcf xC+0,F ; shift left xC
|
|
|
147 rlcf xC+1,F ; ... shifting MSB out of xC...
|
|
|
148 rlcf xA+0,F ; ... and into LSB of xA
|
|
|
149 rlcf xA+1,F ; ...
|
|
|
150 btfsc STATUS,C ; did the remainder overflow (carry set)?
|
|
|
151 bra div16x16_2 ; YES - directly generate a result bit = 1
|
|
|
152 movf xB+0,W ; NO - compute remainder - divisor = xA - xB, trash result to WREG
|
|
|
153 subwf xA+0,W ; - ...
|
|
|
154 movf xB+1,W ; - ...
|
|
|
155 subwfb xA+1,W ; - ...
|
|
|
156 btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ?
|
|
|
157 bra div16x16_3 ; YES - result bit = 0, keep LSB of xC+0 being 0
|
|
|
158 div16x16_2:
|
|
|
159 bsf xC+0,0 ; NO - result bit = 1, set LSB of xC+0 to 1
|
|
|
160 movf xB+0,W ; - subtract divisor from remainder "for real": xA = xA - xB
|
|
|
161 subwf xA+0,F ; - ...
|
|
|
162 movf xB+1,W ; - ...
|
|
|
163 subwfb xA+1,F ; - ...
|
|
0
|
164 div16x16_3:
|
|
604
|
165 decfsz math_loop,F ; decrement loop counter, all bits done?
|
|
|
166 bra div16x16_1 ; NO - loop
|
|
|
167 return ; YES - done
|
|
0
|
168
|
|
|
169
|
|
604
|
170 global div32x16 ; xC:4 = xC:4 / xB:2 with xA as remainder
|
|
|
171 ; trashes WREG
|
|
|
172 div32x16:
|
|
|
173 movlw .32 ; process 32 bits ...
|
|
|
174 movwf math_loop ; ... initialize loop counter
|
|
|
175 clrf xA+0 ; clear xA, will be used to hold the remainder
|
|
|
176 clrf xA+1 ; ...
|
|
|
177 div32x16_1:
|
|
|
178 bcf STATUS,C ; clear carry flag to shift in a zero bit
|
|
|
179 rlcf xC+0,F ; shift left xC
|
|
|
180 rlcf xC+1,F ; ...
|
|
|
181 rlcf xC+2,F ; ...
|
|
|
182 rlcf xC+3,F ; ... shifting MSB out of xC...
|
|
|
183 rlcf xA+0,F ; ... and into LSB of xA
|
|
|
184 rlcf xA+1,F ; ...
|
|
|
185 btfsc STATUS,C ; did the remainder overflow (carry set)?
|
|
|
186 bra div32x16_2 ; YES - directly generate a result bit = 1
|
|
|
187 movf xB+0,W ; NO - compute remainder - divisor = xA - xB, trash result to WREG
|
|
|
188 subwf xA+0,W ; - ...
|
|
|
189 movf xB+1,W ; - ...
|
|
|
190 subwfb xA+1,W ; - ...
|
|
|
191 btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ?
|
|
|
192 bra div32x16_3 ; YES - result bit = 0, keep LSB of xC+0 being 0
|
|
0
|
193 div32x16_2:
|
|
604
|
194 bsf xC+0,0 ; NO - result bit = 1, set LSB of xC+0 to 1
|
|
|
195 movf xB+0,W ; - subtract divisor from remainder "for real": xA = xA - xB
|
|
|
196 subwf xA+0,F ; - ...
|
|
|
197 movf xB+1,W ; - ...
|
|
|
198 subwfb xA+1,F ; - ...
|
|
0
|
199 div32x16_3:
|
|
604
|
200 decfsz math_loop,F ; decrement loop counter, all bits done?
|
|
|
201 bra div32x16_1 ; NO - loop
|
|
|
202 return ; YES - done
|
|
|
203
|
|
|
204
|
|
|
205 ;=============================================================================
|
|
|
206
|
|
|
207 ; ISR math functions
|
|
|
208
|
|
|
209 global isr_shift_C31 ; 24 bit shift, repeated WREG times, dedicated to a specific usage:
|
|
|
210 ; Because less than 8 bits are shifted and only C[2:1] is needed,
|
|
|
211 ; we don't care what bit is inserted.
|
|
|
212 isr_shift_C31:
|
|
|
213 rrcf isr_xC+3,F ; shift three bytes
|
|
|
214 rrcf isr_xC+2,F
|
|
|
215 rrcf isr_xC+1,F
|
|
|
216 decfsz WREG ; decrement loop counter, done?
|
|
|
217 bra isr_shift_C31 ; NO - loop
|
|
|
218 return ; YES - done
|
|
|
219
|
|
|
220
|
|
|
221 global isr_unsigned_mult16x16 ; isr_xC = isr_xA * _isr_xB with UNSIGNED values
|
|
|
222 ; trashes PRODL, PRODH, WREG
|
|
|
223 isr_unsigned_mult16x16:
|
|
|
224 movf isr_xA+0,W ; multiply a[0] * b[0]
|
|
|
225 mulwf isr_xB+0
|
|
|
226 movff PRODL,isr_xC+0 ; store product to c[1]:c[0]
|
|
|
227 movff PRODH,isr_xC+1
|
|
|
228 ;
|
|
|
229 movf isr_xA+1,W ; multiply a[1] * b[1]
|
|
|
230 mulwf isr_xB+1
|
|
|
231 movff PRODL, isr_xC+2 ; store product to c[3]:c[2]
|
|
|
232 movff PRODH, isr_xC+3
|
|
|
233 ;
|
|
|
234 movf isr_xA+0,W ; multiply a[0] * b[1]
|
|
|
235 mulwf isr_xB+1
|
|
|
236 movf PRODL,W ; add cross product to c[3]:c[2]:c[1]
|
|
|
237 addwf isr_xC+1,F
|
|
|
238 movf PRODH,W
|
|
|
239 addwfc isr_xC+2,F ; propagate carry
|
|
|
240 clrf WREG
|
|
|
241 addwfc isr_xC+3,F ; propagate carry
|
|
|
242 ;
|
|
|
243 movf isr_xA+1,W ; multiply a[1] * b[0]
|
|
|
244 mulwf isr_xB+0
|
|
|
245 movf PRODL,W ; add cross product
|
|
|
246 addwf isr_xC+1,F
|
|
|
247 movf PRODH,W
|
|
|
248 addwfc isr_xC+2,F ; propagate carry
|
|
|
249 clrf WREG
|
|
|
250 addwfc isr_xC+3,F ; propagate carry
|
|
0
|
251 return
|
|
|
252
|
|
|
253
|
|
604
|
254 global isr_signed_mult16x16 ; isr_xC = isr_xA * _isr_xB with SIGNED values
|
|
|
255 ; trashes PRODL, PRODH, WREG
|
|
|
256 isr_signed_mult16x16:
|
|
|
257 rcall isr_unsigned_mult16x16 ; do an unsigned multiplication first
|
|
|
258 ; ; manage sign extension of operand B
|
|
|
259 btfss isr_xB+1,7 ; is B negative ?
|
|
|
260 bra isr_signed_mult_checkA ; NO - continue checking operand A
|
|
|
261 movf isr_xA+0,W ; Yes - add -65536 * A
|
|
|
262 subwf isr_xC+2,F
|
|
|
263 movf isr_xA+1,W
|
|
|
264 subwfb isr_xC+3,F
|
|
|
265 isr_signed_mult_checkA ; manage sign extension of operand B
|
|
|
266 btfss isr_xA+1,7 ; is A negative ?
|
|
|
267 return ; NO - done
|
|
|
268 movf isr_xB+0,W ; Yes - add -65536 * B
|
|
|
269 subwf isr_xC+2,F
|
|
|
270 movf isr_xB+1,W
|
|
|
271 subwfb isr_xC+3,F
|
|
0
|
272 return
|
|
|
273
|
|
604
|
274 END |
