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