|
0
|
1 ;=============================================================================
|
|
|
2 ;
|
|
628
|
3 ; File math.asm combined next generation V3.04.3
|
|
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
|
|
623
|
13 #include "hwos.inc" ; mandatory header
|
|
0
|
14
|
|
623
|
15 math CODE
|
|
604
|
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
|
|
628
|
57 global addU16 ; ; sub_c:2 = sub_a:2 + sub_b:2 with USIGNED values
|
|
|
58 ; trashes WREG
|
|
|
59 addU16:
|
|
|
60 movf sub_a+0,W ; get 1st summand (low byte) to WREG
|
|
|
61 addwf sub_b+0,W ; add 2nd summand (low byte) and store result in WREG
|
|
|
62 movwf sub_c+0 ; copy result (low byte) to sub_c
|
|
|
63 movf sub_a+1,W ; get 1st summand (high byte) to WREG
|
|
|
64 addwfc sub_b+1,W ; add 2nd summand (high byte) and store result in WREG
|
|
|
65 movwf sub_c+1 ; copy result (high byte) to sub_c
|
|
|
66 return ; done
|
|
|
67
|
|
|
68
|
|
604
|
69 global sub16 ; sub_c:2 = sub_a:2 - sub_b:2 with SIGNED values
|
|
|
70 ; sets neg_flag if result is < 0
|
|
|
71 ; trashes WREG
|
|
|
72 sub16:
|
|
|
73 bcf neg_flag ; clear flag which will indicate if result is negative
|
|
|
74 movf sub_b+0,W ; get value to be subtracted, low byte
|
|
|
75 subwf sub_a+0,W ; execute subtraction on low byte
|
|
|
76 movwf sub_c+0 ; copy result to output variable, low byte
|
|
|
77 movf sub_b+1,W ; get value to be subtracted, high byte
|
|
|
78 subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag
|
|
|
79 movwf sub_c+1 ; copy result to output variable, high byte
|
|
|
80 btfss STATUS,N ; result negative ?
|
|
|
81 return ; NO - result positive, done
|
|
|
82 bsf neg_flag ; YES - set flag
|
|
|
83 comf sub_c+1 ; - do a 16 bit sign change
|
|
|
84 negf sub_c+0 ;
|
|
|
85 btfsc STATUS,C ; - carry to be propagated ?
|
|
|
86 incf sub_c+1,F ; YES - do it
|
|
|
87 return ; - done
|
|
|
88
|
|
0
|
89
|
|
604
|
90 global subU16 ; sub_c:2 = sub_a:2 - sub_b:2 with UNSIGNED values
|
|
|
91 ; sets neg_flag if result is < 0
|
|
|
92 ; trashes WREG
|
|
|
93 subU16:
|
|
|
94 bcf neg_flag ; clear flag which will indicate if result is negative
|
|
623
|
95 movf sub_b+0,W ; get value to be subtracted, low byte
|
|
604
|
96 subwf sub_a+0,W ; execute subtraction on low byte
|
|
|
97 movwf sub_c+0 ; copy result to output variable, low byte
|
|
|
98 movf sub_b+1,W ; get value to be subtracted, high byte
|
|
|
99 subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag
|
|
|
100 movwf sub_c+1 ; copy result to output variable, high byte
|
|
|
101 btfsc STATUS,C ; borrow to propagate (B == /CARRY) ?
|
|
|
102 return ; NO - result positive, done
|
|
|
103 bsf neg_flag ; YES - set flag
|
|
|
104 comf sub_c+1 ; - do a 16 bit sign change
|
|
623
|
105 negf sub_c+0 ; - ...
|
|
604
|
106 btfsc STATUS,C ; - carry to be propagated ?
|
|
|
107 incf sub_c+1,F ; YES - do it
|
|
|
108 return ; - done
|
|
|
109
|
|
0
|
110
|
|
623
|
111 global cmpU16 ; sub_a:2 - sub_b:2 with UNSIGNED values
|
|
|
112 ; sets neg_flag if result is < 0, but does not store result itself
|
|
|
113 ; trashes WREG
|
|
|
114 cmpU16:
|
|
|
115 bcf neg_flag ; clear flag which will indicate if result is negative
|
|
|
116 movf sub_b+0,W ; get value to be subtracted, low byte
|
|
|
117 subwf sub_a+0,W ; execute subtraction on low byte
|
|
|
118 movf sub_b+1,W ; get value to be subtracted, high byte
|
|
|
119 subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag
|
|
|
120 btfss STATUS,C ; borrow to propagate (B == /CARRY) ?
|
|
|
121 bsf neg_flag ; YES - result is negative, set flag
|
|
|
122 return ; done
|
|
|
123
|
|
|
124
|
|
604
|
125 global mult16x16 ; xC:4 = xA:2 * xB:2 with UNSIGNED values
|
|
|
126 ; trashes PRODL, PRODH, WREG
|
|
|
127 mult16x16:
|
|
|
128 clrf xC+2 ; clear the high-order bits
|
|
|
129 clrf xC+3
|
|
|
130 ;
|
|
|
131 movf xA+0,W ; do the "L" multiplication
|
|
|
132 mulwf xB+0
|
|
|
133 movff PRODL,xC+0 ; copy result to xC
|
|
|
134 movff PRODH,xC+1
|
|
|
135 ;
|
|
|
136 movf xA+0,W ; do the "I" multiplication
|
|
|
137 mulwf xB+1
|
|
|
138 movf PRODL,W ; get the product's low byte...
|
|
|
139 addwf xC+1,F ; ... and add it to xC+1
|
|
|
140 movf PRODH,W ; get the product's high byte...
|
|
|
141 addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1
|
|
|
142 ;
|
|
|
143 movf xA+1,W ; do the "O" multiplication
|
|
|
144 mulwf xB+0
|
|
|
145 movf PRODL,W ; get the product's low byte...
|
|
|
146 addwf xC+1,F ; ... and add it to xC+1
|
|
|
147 movf PRODH,W ; get the product's high byte...
|
|
|
148 addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1
|
|
|
149 clrf WREG ; clear WREG...
|
|
|
150 addwfc xC+3,F ; ... add add it to xC+3 obeying carry bit from xC+2 (can only happen in "O" multiplication)
|
|
|
151 ;
|
|
|
152 movf xA+1,W ; do the "F" multiplication
|
|
|
153 mulwf xB+1
|
|
|
154 movf PRODL,W ; get the product's low byte...
|
|
|
155 addwf xC+2,F ; ... and add it to xC+2
|
|
|
156 movf PRODH,W ; get the product's high byte...
|
|
|
157 addwfc xC+3,F ; ... and add it to xC+3 obeying carry bit from xC+2
|
|
|
158 return
|
|
0
|
159
|
|
|
160
|
|
604
|
161 global div16x16 ; xC:2 = xA:2 / xB:2 with xA as remainder
|
|
|
162 ; trashes WREG
|
|
|
163 div16x16:
|
|
|
164 movlw .16 ; process 16 bits ...
|
|
|
165 movwf math_loop ; ... initialize loop counter
|
|
|
166 movff xA+0,xC+0 ; copy xA to xC
|
|
|
167 movff xA+1,xC+1 ; ...
|
|
|
168 clrf xA+0 ; clear xA, will now be used to hold the remainder
|
|
|
169 clrf xA+1 ; ...
|
|
|
170 div16x16_1:
|
|
|
171 bcf STATUS,C ; clear carry flag to shift in a zero bit
|
|
|
172 rlcf xC+0,F ; shift left xC
|
|
|
173 rlcf xC+1,F ; ... shifting MSB out of xC...
|
|
|
174 rlcf xA+0,F ; ... and into LSB of xA
|
|
|
175 rlcf xA+1,F ; ...
|
|
|
176 btfsc STATUS,C ; did the remainder overflow (carry set)?
|
|
|
177 bra div16x16_2 ; YES - directly generate a result bit = 1
|
|
|
178 movf xB+0,W ; NO - compute remainder - divisor = xA - xB, trash result to WREG
|
|
|
179 subwf xA+0,W ; - ...
|
|
|
180 movf xB+1,W ; - ...
|
|
|
181 subwfb xA+1,W ; - ...
|
|
|
182 btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ?
|
|
|
183 bra div16x16_3 ; YES - result bit = 0, keep LSB of xC+0 being 0
|
|
|
184 div16x16_2:
|
|
|
185 bsf xC+0,0 ; NO - result bit = 1, set LSB of xC+0 to 1
|
|
|
186 movf xB+0,W ; - subtract divisor from remainder "for real": xA = xA - xB
|
|
|
187 subwf xA+0,F ; - ...
|
|
|
188 movf xB+1,W ; - ...
|
|
|
189 subwfb xA+1,F ; - ...
|
|
0
|
190 div16x16_3:
|
|
604
|
191 decfsz math_loop,F ; decrement loop counter, all bits done?
|
|
|
192 bra div16x16_1 ; NO - loop
|
|
|
193 return ; YES - done
|
|
0
|
194
|
|
|
195
|
|
604
|
196 global div32x16 ; xC:4 = xC:4 / xB:2 with xA as remainder
|
|
|
197 ; trashes WREG
|
|
|
198 div32x16:
|
|
|
199 movlw .32 ; process 32 bits ...
|
|
|
200 movwf math_loop ; ... initialize loop counter
|
|
|
201 clrf xA+0 ; clear xA, will be used to hold the remainder
|
|
|
202 clrf xA+1 ; ...
|
|
|
203 div32x16_1:
|
|
|
204 bcf STATUS,C ; clear carry flag to shift in a zero bit
|
|
|
205 rlcf xC+0,F ; shift left xC
|
|
|
206 rlcf xC+1,F ; ...
|
|
|
207 rlcf xC+2,F ; ...
|
|
|
208 rlcf xC+3,F ; ... shifting MSB out of xC...
|
|
|
209 rlcf xA+0,F ; ... and into LSB of xA
|
|
|
210 rlcf xA+1,F ; ...
|
|
|
211 btfsc STATUS,C ; did the remainder overflow (carry set)?
|
|
|
212 bra div32x16_2 ; YES - directly generate a result bit = 1
|
|
|
213 movf xB+0,W ; NO - compute remainder - divisor = xA - xB, trash result to WREG
|
|
|
214 subwf xA+0,W ; - ...
|
|
|
215 movf xB+1,W ; - ...
|
|
|
216 subwfb xA+1,W ; - ...
|
|
|
217 btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ?
|
|
|
218 bra div32x16_3 ; YES - result bit = 0, keep LSB of xC+0 being 0
|
|
0
|
219 div32x16_2:
|
|
604
|
220 bsf xC+0,0 ; NO - result bit = 1, set LSB of xC+0 to 1
|
|
|
221 movf xB+0,W ; - subtract divisor from remainder "for real": xA = xA - xB
|
|
|
222 subwf xA+0,F ; - ...
|
|
|
223 movf xB+1,W ; - ...
|
|
|
224 subwfb xA+1,F ; - ...
|
|
0
|
225 div32x16_3:
|
|
604
|
226 decfsz math_loop,F ; decrement loop counter, all bits done?
|
|
|
227 bra div32x16_1 ; NO - loop
|
|
|
228 return ; YES - done
|
|
|
229
|
|
|
230
|
|
|
231 ;=============================================================================
|
|
|
232
|
|
|
233 ; ISR math functions
|
|
|
234
|
|
|
235 global isr_shift_C31 ; 24 bit shift, repeated WREG times, dedicated to a specific usage:
|
|
|
236 ; Because less than 8 bits are shifted and only C[2:1] is needed,
|
|
|
237 ; we don't care what bit is inserted.
|
|
|
238 isr_shift_C31:
|
|
|
239 rrcf isr_xC+3,F ; shift three bytes
|
|
|
240 rrcf isr_xC+2,F
|
|
|
241 rrcf isr_xC+1,F
|
|
|
242 decfsz WREG ; decrement loop counter, done?
|
|
|
243 bra isr_shift_C31 ; NO - loop
|
|
|
244 return ; YES - done
|
|
|
245
|
|
|
246
|
|
|
247 global isr_unsigned_mult16x16 ; isr_xC = isr_xA * _isr_xB with UNSIGNED values
|
|
|
248 ; trashes PRODL, PRODH, WREG
|
|
|
249 isr_unsigned_mult16x16:
|
|
|
250 movf isr_xA+0,W ; multiply a[0] * b[0]
|
|
|
251 mulwf isr_xB+0
|
|
|
252 movff PRODL,isr_xC+0 ; store product to c[1]:c[0]
|
|
|
253 movff PRODH,isr_xC+1
|
|
|
254 ;
|
|
|
255 movf isr_xA+1,W ; multiply a[1] * b[1]
|
|
|
256 mulwf isr_xB+1
|
|
|
257 movff PRODL, isr_xC+2 ; store product to c[3]:c[2]
|
|
|
258 movff PRODH, isr_xC+3
|
|
|
259 ;
|
|
|
260 movf isr_xA+0,W ; multiply a[0] * b[1]
|
|
|
261 mulwf isr_xB+1
|
|
|
262 movf PRODL,W ; add cross product to c[3]:c[2]:c[1]
|
|
|
263 addwf isr_xC+1,F
|
|
|
264 movf PRODH,W
|
|
|
265 addwfc isr_xC+2,F ; propagate carry
|
|
|
266 clrf WREG
|
|
|
267 addwfc isr_xC+3,F ; propagate carry
|
|
|
268 ;
|
|
|
269 movf isr_xA+1,W ; multiply a[1] * b[0]
|
|
|
270 mulwf isr_xB+0
|
|
|
271 movf PRODL,W ; add cross product
|
|
|
272 addwf isr_xC+1,F
|
|
|
273 movf PRODH,W
|
|
|
274 addwfc isr_xC+2,F ; propagate carry
|
|
|
275 clrf WREG
|
|
|
276 addwfc isr_xC+3,F ; propagate carry
|
|
0
|
277 return
|
|
|
278
|
|
|
279
|
|
623
|
280 global isr_signed_mult16x16 ; isr_xC = isr_xA * isr_xB with SIGNED values
|
|
604
|
281 ; trashes PRODL, PRODH, WREG
|
|
|
282 isr_signed_mult16x16:
|
|
|
283 rcall isr_unsigned_mult16x16 ; do an unsigned multiplication first
|
|
|
284 ; ; manage sign extension of operand B
|
|
|
285 btfss isr_xB+1,7 ; is B negative ?
|
|
|
286 bra isr_signed_mult_checkA ; NO - continue checking operand A
|
|
|
287 movf isr_xA+0,W ; Yes - add -65536 * A
|
|
|
288 subwf isr_xC+2,F
|
|
|
289 movf isr_xA+1,W
|
|
|
290 subwfb isr_xC+3,F
|
|
|
291 isr_signed_mult_checkA ; manage sign extension of operand B
|
|
|
292 btfss isr_xA+1,7 ; is A negative ?
|
|
|
293 return ; NO - done
|
|
|
294 movf isr_xB+0,W ; Yes - add -65536 * B
|
|
|
295 subwf isr_xC+2,F
|
|
|
296 movf isr_xB+1,W
|
|
|
297 subwfb isr_xC+3,F
|
|
0
|
298 return
|
|
|
299
|
|
604
|
300 END |
