Mercurial > public > hwos_code

comparison src/math.asm @ 604:ca4556fb60b9

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
bump to 2.99beta, work on 3.00 stable
author heinrichsweikamp
date Thu, 22 Nov 2018 19:47:26 +0100
parents 653a3ab08062
children c40025d8e750
comparison
equal deleted inserted replaced
603:00b24fb4324d 604:ca4556fb60b9
1 ;============================================================================= 1 ;=============================================================================
2 ; 2 ;
3 ; File math.asm 3 ; File math.asm REFACTORED VERSION V2.99d
4 ; 4 ;
5 ; Math subroutines 5 ; Math subroutines
6 ; 6 ;
7 ; Copyright (c) 2011, JD Gascuel, HeinrichsWeikamp, all right reserved. 7 ; Copyright (c) 2011, JD Gascuel, HeinrichsWeikamp, all right reserved.
8 ;============================================================================= 8 ;=============================================================================
9 ; HISTORY 9 ; HISTORY
10 ; 2011-08-03 : [mH] moving from OSTC code 10 ; 2011-08-03 : [mH] moving from OSTC code
11 11
12 12
13 #include "hwos.inc" ; Mandatory header 13 #include "hwos.inc" ; mandatory header
14 14
15 basic CODE 15 math CODE
16 ;============================================================================= 16
17 global convert_time 17 ;=============================================================================
18 convert_time: ; converts hi:lo in minutes to hours (hi) and minutes (lo) 18
19 movff lo,xA+0 ; divide by 60... 19 global convert_time ; converts hi:lo in minutes to hours (up:hi) and minutes (lo)
20 movff hi,xA+1 ; 20 ; also usable for conversion of seconds to minutes and seconds
21 movlw d'60' ; 21 ; trashes xA, xB, xC
22 movwf xB+0 ; 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
23 clrf xB+1 ; 27 clrf xB+1 ;
24 rcall div16x16 ; xA/xB=xC with xA as remainder 28 rcall div16x16 ; xC = xA / xB with xA as remainder
25 movff xC+0,hi ; Hours 29 movff xC+1,up ; result is hours / minutes, copy to up (high byte)
26 movff xA+0,lo ; =remaining minutes (0.....59) 30 movff xC+0,hi ; result is hours / minutes, copy to hi (low byte)
27 return 31 movff xA+0,lo ; remainder is minutes / seconds, copy to lo
28 32 return
29 global div16 33
34
35 global div16 ; divA:2 = divA:2 / 2^WREG
36 ; trashes WREG
30 div16: 37 div16:
31 ; divA=divA/2^divB (divB: 8Bit only!) 38 bcf STATUS,C ; clear carry
32 bcf STATUS,C 39 rrcf divA+1 ; rotate right high byte, carry into MSB, LSB into carry
33 rrcf divA+1 40 rrcf divA+0 ; rotate right low byte, carry into MSB, LSB into carry
34 rrcf divA+0 41 decfsz WREG ; decrement counter, done?
35 decfsz divB 42 bra div16 ; NO - loop
36 bra div16 43 return ; YES - done
37 return 44
38 45
39 global sub16 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
55
56
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
40 sub16: 60 sub16:
41 ; sub_c = sub_a - sub_b (with signed values) 61 bcf neg_flag ; clear flag which will indicate if result is negative
42 bcf neg_flag 62 movf sub_b+0,W ; get value to be subtracted, low byte
43 movf sub_b+0, W ; Get Value to be subtracted 63 subwf sub_a+0,W ; execute subtraction on low byte
44 subwf sub_a+0, W ; Do the High Byte 64 movwf sub_c+0 ; copy result to output variable, low byte
45 movwf sub_c+0 65 movf sub_b+1,W ; get value to be subtracted, high byte
46 movf sub_b+1, W ; Get the Value to be Subbed 66 subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag
47 subwfb sub_a+1, W 67 movwf sub_c+1 ; copy result to output variable, high byte
48 movwf sub_c+1 68 btfss STATUS,N ; result negative ?
49 69 return ; NO - result positive, done
50 btfss STATUS,N ; Negativ result ? 70 bsf neg_flag ; YES - set flag
51 return ; NO: result positive done. 71 comf sub_c+1 ; - do a 16 bit sign change
52 72 negf sub_c+0 ;
53 bsf neg_flag ; MARK result negative 73 btfsc STATUS,C ; - carry to be propagated ?
54 74 incf sub_c+1,F ; YES - do it
55 comf sub_c+1 ; 16bit sign change. 75 return ; - done
56 negf sub_c+0 76
57 btfsc STATUS,C ; Carry to propagate ? 77
58 incf sub_c+1,F ; YES: do it. 78 global subU16 ; sub_c:2 = sub_a:2 - sub_b:2 with UNSIGNED values
59 79 ; sets neg_flag if result is < 0
60 return 80 ; trashes WREG
61
62 global subU16
63 subU16: 81 subU16:
64 ; sub_c = sub_a - sub_b (with UNSIGNED values) 82 bcf neg_flag ; clear flag which will indicate if result is negative
65 bcf neg_flag 83 movf sub_b+0,W ; get Value to be subtracted, low byte
66 movf sub_b+0, W ; Get Value to be subtracted 84 subwf sub_a+0,W ; execute subtraction on low byte
67 subwf sub_a+0, W ; Do the High Byte 85 movwf sub_c+0 ; copy result to output variable, low byte
68 movwf sub_c+0 86 movf sub_b+1,W ; get value to be subtracted, high byte
69 movf sub_b+1, W ; Get the Value to be Subbed 87 subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag
70 subwfb sub_a+1, W 88 movwf sub_c+1 ; copy result to output variable, high byte
71 movwf sub_c+1 89 btfsc STATUS,C ; borrow to propagate (B == /CARRY) ?
72 btfsc STATUS,C ; Borrow to propagate ? (B == /CARRY) 90 return ; NO - result positive, done
73 return ; NO: result positive done. 91 bsf neg_flag ; YES - set flag
74 bsf neg_flag ; MARK result negative 92 comf sub_c+1 ; - do a 16 bit sign change
75 comf sub_c+1 ; 16bit sign change. 93 negf sub_c+0 ;
76 negf sub_c+0 94 btfsc STATUS,C ; - carry to be propagated ?
77 btfsc STATUS,C ; Carry to propagate ? 95 incf sub_c+1,F ; YES - do it
78 incf sub_c+1,F ; YES: do it. 96 return ; - done
79 return 97
80 98
81 global mult16x16 99 global mult16x16 ; xC:4 = xA:2 * xB:2 with UNSIGNED values
82 mult16x16: ;xA*xB=xC 100 ; trashes PRODL, PRODH, WREG
83 clrf xC+2 ; Clear the High-Order Bits 101 mult16x16:
84 clrf xC+3 102 clrf xC+2 ; clear the high-order bits
85 movf xA, w ; Do the "L" Multiplication first 103 clrf xC+3
86 mulwf xB 104 ;
87 movf PRODL, w ; Save result 105 movf xA+0,W ; do the "L" multiplication
88 movwf xC 106 mulwf xB+0
89 movf PRODH, w 107 movff PRODL,xC+0 ; copy result to xC
90 movwf xC+1 108 movff PRODH,xC+1
91 movf xA, w ; Do the "I" Multiplication 109 ;
92 mulwf xB+1 110 movf xA+0,W ; do the "I" multiplication
93 movf PRODL, w ; Save the Most Significant Byte First 111 mulwf xB+1
94 addwf xC+1, f 112 movf PRODL,W ; get the product's low byte...
95 movf PRODH, w 113 addwf xC+1,F ; ... and add it to xC+1
96 addwfc xC+2, f ; Add to the Last Result 114 movf PRODH,W ; get the product's high byte...
97 movf xA+1, w ; Do the "O" Multiplication 115 addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1
98 mulwf xB 116 ;
99 movf PRODL, w ; Add the Lower Byte Next 117 movf xA+1,W ; do the "O" multiplication
100 addwf xC+1, f 118 mulwf xB+0
101 movf PRODH, w ; Add the High Byte First 119 movf PRODL,W ; get the product's low byte...
102 addwfc xC+2, f 120 addwf xC+1,F ; ... and add it to xC+1
103 btfsc STATUS, C ; Add the Carry 121 movf PRODH,W ; get the product's high byte...
104 incf xC+3, f 122 addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1
105 movf xA+1, w ; Do the "F" Multiplication 123 clrf WREG ; clear WREG...
106 mulwf xB+1 124 addwfc xC+3,F ; ... add add it to xC+3 obeying carry bit from xC+2 (can only happen in "O" multiplication)
107 movf PRODL, w 125 ;
108 addwf xC+2, f 126 movf xA+1,W ; do the "F" multiplication
109 movf PRODH, w 127 mulwf xB+1
110 addwfc xC+3, f 128 movf PRODL,W ; get the product's low byte...
111 return 129 addwf xC+2,F ; ... and add it to xC+2
112 130 movf PRODH,W ; get the product's high byte...
113 global div16x16 131 addwfc xC+3,F ; ... and add it to xC+3 obeying carry bit from xC+2
114 div16x16: ;xA/xB=xC with xA+0 as remainder 132 return
115 ;uses divB as temp variable 133
116 clrf xC+0 134
117 clrf xC+1 135 global div16x16 ; xC:2 = xA:2 / xB:2 with xA as remainder
118 MOVF xB+0,W ; Check for zero 136 ; trashes WREG
119 IORWF xB+1,W ; 137 div16x16:
120 BTFSC STATUS,Z ; Check for zero 138 movlw .16 ; process 16 bits ...
121 RETLW H'FF' ; return 0xFF if illegal 139 movwf math_loop ; ... initialize loop counter
122 MOVLW 1 ; Start count at 1 140 movff xA+0,xC+0 ; copy xA to xC
123 MOVWF divB ; Clear Count 141 movff xA+1,xC+1 ; ...
142 clrf xA+0 ; clear xA, will now be used to hold the remainder
143 clrf xA+1 ; ...
124 div16x16_1: 144 div16x16_1:
125 BTFSC xB+1,7 ; High bit set ? 145 bcf STATUS,C ; clear carry flag to shift in a zero bit
126 bra div16x16_2 ; Yes then continue 146 rlcf xC+0,F ; shift left xC
127 INCF divB,F ; Increment count 147 rlcf xC+1,F ; ... shifting MSB out of xC...
128 148 rlcf xA+0,F ; ... and into LSB of xA
129 bcf STATUS,C 149 rlcf xA+1,F ; ...
130 rlcf xB+0,F 150 btfsc STATUS,C ; did the remainder overflow (carry set)?
131 rlcf xB+1,F 151 bra div16x16_2 ; YES - directly generate a result bit = 1
132 bra div16x16_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
133 div16x16_2: 158 div16x16_2:
134 ; Shift result left 159 bsf xC+0,0 ; NO - result bit = 1, set LSB of xC+0 to 1
135 bcf STATUS,C 160 movf xB+0,W ; - subtract divisor from remainder "for real": xA = xA - xB
136 rlcf xC+0,F 161 subwf xA+0,F ; - ...
137 rlcf xC+1,F 162 movf xB+1,W ; - ...
138 163 subwfb xA+1,F ; - ...
139 ; Reduce Divisor
140
141 MOVF xB,W ; Get low byte of subtrahend
142 SUBWF xA,F ; Subtract DST(low) - SRC(low)
143 MOVF xB+1,W ; Now get high byte of subtrahend
144 BTFSS STATUS,C ; If there was a borrow, rather than
145 INCF xB+1,W ; decrement high byte of dst we inc src
146 SUBWF xA+1,F ; Subtract the high byte and we're done
147
148
149 BTFSC STATUS, C ; Did it reduce?
150 bra div16x16_3 ; No, so it was less than
151
152 movf xB+0,W ; Reverse subtraction
153 addwf xA+0,F
154 movf xB+1,W
155 addwfc xA+1,F
156
157 bra div16x16_4 ; Continue the process
158 div16x16_3: 164 div16x16_3:
159 BSF xC+0,0 ; Yes it did, this gets a 1 bit 165 decfsz math_loop,F ; decrement loop counter, all bits done?
160 div16x16_4: 166 bra div16x16_1 ; NO - loop
161 DECF divB,F ; Decrement N_COUNT 167 return ; YES - done
162 BTFSC STATUS,Z ; If its not zero then continue 168
163 return 169
164 170 global div32x16 ; xC:4 = xC:4 / xB:2 with xA as remainder
165 bcf STATUS,C 171 ; trashes WREG
166 rrcf xB+1,F 172 div32x16:
167 rrcf xB+0,F 173 movlw .32 ; process 32 bits ...
168 174 movwf math_loop ; ... initialize loop counter
169 bra div16x16_2 ; Next bit. 175 clrf xA+0 ; clear xA, will be used to hold the remainder
170 176 clrf xA+1 ; ...
171 global div32x16 177 div32x16_1:
172 div32x16: ; xC:4 / xB:2 = xC+3:xC+2 with xC+1:xC+0 as remainder 178 bcf STATUS,C ; clear carry flag to shift in a zero bit
173 ; Setup 179 rlcf xC+0,F ; shift left xC
174 movlw .32 ; setup shift counter 180 rlcf xC+1,F ; ...
175 movwf divB 181 rlcf xC+2,F ; ...
176 movff xC+3,xA+1 ; move ACCb to ACCf 182 rlcf xC+3,F ; ... shifting MSB out of xC...
177 movff xC+2,xA+0 183 rlcf xA+0,F ; ... and into LSB of xA
178 movff xC+1,sub_a+1 ; move ACCc to ACCe 184 rlcf xA+1,F ; ...
179 movff xC+0,sub_a+0 185 btfsc STATUS,C ; did the remainder overflow (carry set)?
180 clrf xC+3 186 bra div32x16_2 ; YES - directly generate a result bit = 1
181 clrf xC+2 187 movf xB+0,W ; NO - compute remainder - divisor = xA - xB, trash result to WREG
182 clrf xC+1 188 subwf xA+0,W ; - ...
183 clrf xC+0 189 movf xB+1,W ; - ...
184 clrf sub_b+1 190 subwfb xA+1,W ; - ...
185 clrf sub_b+0 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
186 div32x16_2: 193 div32x16_2:
187 bcf STATUS,C 194 bsf xC+0,0 ; NO - result bit = 1, set LSB of xC+0 to 1
188 rlcf sub_a+0,F 195 movf xB+0,W ; - subtract divisor from remainder "for real": xA = xA - xB
189 rlcf sub_a+1,F 196 subwf xA+0,F ; - ...
190 rlcf xA+0,F 197 movf xB+1,W ; - ...
191 rlcf xA+1,F 198 subwfb xA+1,F ; - ...
192 rlcf sub_b+0,F
193 rlcf sub_b+1,F
194 movf xB+1,W
195 subwf sub_b+1,W ; check if a>d
196 btfss STATUS,Z
197 bra div32x16_3
198 movf xB+0,W
199 subwf sub_b+0,W ; if msb equal then check lsb
200 div32x16_3: 199 div32x16_3:
201 btfss STATUS,C ; carry set if d>a 200 decfsz math_loop,F ; decrement loop counter, all bits done?
202 bra div32x16_4 201 bra div32x16_1 ; NO - loop
203 movf xB+0,W ; d-a into d 202 return ; YES - done
204 subwf sub_b+0,F 203
205 btfss STATUS,C 204
206 decf sub_b+1,F 205 ;=============================================================================
207 movf xB+1,W 206
208 subwf sub_b+1,F 207 ; ISR math functions
209 bsf STATUS,C ; shift a 1 into b (result) 208
210 div32x16_4: 209 global isr_shift_C31 ; 24 bit shift, repeated WREG times, dedicated to a specific usage:
211 rlcf xC+0,F 210 ; Because less than 8 bits are shifted and only C[2:1] is needed,
212 rlcf xC+1,F 211 ; we don't care what bit is inserted.
213 rlcf xC+2,F
214 rlcf xC+3,F
215 decfsz divB,F ; loop until all bits checked
216 bra div32x16_2
217 return
218
219 ;;=============================================================================
220 ;; u16 * u16 --> 32bit multiply (xA * xB --> xC)
221 ;; Used in interupt service routines, to compute temperature and pressure.
222 ;;
223 ; global isr_mult16x16
224 ;isr_mult16x16:
225 ; clrf isr_xC+2 ; Clear the High-Order Bits
226 ; clrf isr_xC+3
227 ; movf isr_xA, w ; Do the "L" Multiplication first
228 ; mulwf isr_xB
229 ; movf PRODL, w ; Save result
230 ; movwf isr_xC+0
231 ; movf PRODH, w
232 ; movwf isr_xC+1
233 ; movf isr_xA+0, w ; Do the "I" Multiplication
234 ; mulwf isr_xB+1
235 ; movf PRODL, w ; Save the Most Significant Byte First
236 ; addwf isr_xC+1, f
237 ; movf PRODH, w
238 ; addwfc isr_xC+2, f ; Add to the Last Result
239 ; movf isr_xA+1, w ; Do the "O" Multiplication
240 ; mulwf isr_xB
241 ; movf PRODL, w ; Add the Lower Byte Next
242 ; addwf isr_xC+1, f
243 ; movf PRODH, w ; Add the High Byte First
244 ; addwfc isr_xC+2, f
245 ; btfsc STATUS, C ; Add the Carry
246 ; incf isr_xC+3, f
247 ; movf isr_xA+1, w ; Do the "F" Multiplication
248 ; mulwf isr_xB+1
249 ; movf PRODL, w
250 ; addwf isr_xC+2, f
251 ; movf PRODH, w
252 ; addwfc isr_xC+3, f
253 ; return
254
255 ;=============================================================================
256 ; 24bit shift, repeted WREG times.
257 ; Because we shift less than 8bits, and keep only C[2:1], we don't care what
258 ; bit is inserted...
259 ;
260 global isr_shift_C31
261 isr_shift_C31: 212 isr_shift_C31:
262 rrcf isr_xC+3,F ; Shift the three bytes... 213 rrcf isr_xC+3,F ; shift three bytes
263 rrcf isr_xC+2,F 214 rrcf isr_xC+2,F
264 rrcf isr_xC+1,F 215 rrcf isr_xC+1,F
265 decfsz WREG 216 decfsz WREG ; decrement loop counter, done?
266 bra isr_shift_C31 217 bra isr_shift_C31 ; NO - loop
267 return 218 return ; YES - done
268 219
269 ;============================================================================= 220
270 ; s16 * s16 --> 32bit multiply (xA * xB --> xC) 221 global isr_unsigned_mult16x16 ; isr_xC = isr_xA * _isr_xB with UNSIGNED values
271 ; Signed multiplication. 222 ; trashes PRODL, PRODH, WREG
272 ; Code from... the Pic18F documentation ;-)
273 global isr_unsigned_mult16x16
274 isr_unsigned_mult16x16: 223 isr_unsigned_mult16x16:
275 MOVF isr_xA+0, W ; Lowest is simply a[0] * b[0] 224 movf isr_xA+0,W ; multiply a[0] * b[0]
276 MULWF isr_xB+0 225 mulwf isr_xB+0
277 MOVFF PRODL, isr_xC+0 226 movff PRODL,isr_xC+0 ; store product to c[1]:c[0]
278 MOVFF PRODH, isr_xC+1 227 movff PRODH,isr_xC+1
279 ; 228 ;
280 MOVF isr_xA+1, W ; And highest a[1] * b[1] 229 movf isr_xA+1,W ; multiply a[1] * b[1]
281 MULWF isr_xB+1 230 mulwf isr_xB+1
282 MOVFF PRODL, isr_xC+2 231 movff PRODL, isr_xC+2 ; store product to c[3]:c[2]
283 MOVFF PRODH, isr_xC+3 232 movff PRODH, isr_xC+3
284 ; 233 ;
285 MOVF isr_xA+0, W ; Intermediates do propagate: 234 movf isr_xA+0,W ; multiply a[0] * b[1]
286 MULWF isr_xB+1 235 mulwf isr_xB+1
287 MOVF PRODL, W 236 movf PRODL,W ; add cross product to c[3]:c[2]:c[1]
288 ADDWF isr_xC+1, F ; Add cross products 237 addwf isr_xC+1,F
289 MOVF PRODH, W 238 movf PRODH,W
290 ADDWFC isr_xC+2, F ; with propagated carry 239 addwfc isr_xC+2,F ; propagate carry
291 CLRF WREG 240 clrf WREG
292 ADDWFC isr_xC+3, F ; on the three bytes. 241 addwfc isr_xC+3,F ; propagate carry
293 ; 242 ;
294 MOVF isr_xA+1, W ; And the second one, similarly. 243 movf isr_xA+1,W ; multiply a[1] * b[0]
295 MULWF isr_xB+0 244 mulwf isr_xB+0
296 MOVF PRODL, W 245 movf PRODL,W ; add cross product
297 ADDWF isr_xC+1, F ; Add cross products 246 addwf isr_xC+1,F
298 MOVF PRODH, W 247 movf PRODH,W
299 ADDWFC isr_xC+2, F 248 addwfc isr_xC+2,F ; propagate carry
300 CLRF WREG 249 clrf WREG
301 ADDWFC isr_xC+3, F 250 addwfc isr_xC+3,F ; propagate carry
302 return 251 return
303 252
304 global isr_signed_mult16x16 253
254 global isr_signed_mult16x16 ; isr_xC = isr_xA * _isr_xB with SIGNED values
255 ; trashes PRODL, PRODH, WREG
305 isr_signed_mult16x16: 256 isr_signed_mult16x16:
306 rcall isr_unsigned_mult16x16 257 rcall isr_unsigned_mult16x16 ; do an unsigned multiplication first
307 258 ; ; manage sign extension of operand B
308 ; Manage sign extension of operand B 259 btfss isr_xB+1,7 ; is B negative ?
309 BTFSS isr_xB+1,7 ; Is B negatif ? 260 bra isr_signed_mult_checkA ; NO - continue checking operand A
310 BRA isr_signed_mult_checkA ; No: check ARG1 261 movf isr_xA+0,W ; Yes - add -65536 * A
311 MOVF isr_xA+0, W ; Yes: add -65536 * A 262 subwf isr_xC+2,F
312 SUBWF isr_xC+2, F 263 movf isr_xA+1,W
313 MOVF isr_xA+1, W 264 subwfb isr_xC+3,F
314 SUBWFB isr_xC+3, F 265 isr_signed_mult_checkA ; manage sign extension of operand B
315 ; And of operand A 266 btfss isr_xA+1,7 ; is A negative ?
316 isr_signed_mult_checkA 267 return ; NO - done
317 BTFSS isr_xA+1, 7 ; Is A negatif ? 268 movf isr_xB+0,W ; Yes - add -65536 * B
318 RETURN ; No: done 269 subwf isr_xC+2,F
319 MOVF isr_xB+0, W 270 movf isr_xB+1,W
320 SUBWF isr_xC+2, F 271 subwfb isr_xC+3,F
321 MOVF isr_xB+1, W 272 return
322 SUBWFB isr_xC+3, F 273
323 RETURN 274 END
324
325 END