Mercurial > public > hwos_code

comparison src/math.asm @ 634:4050675965ea

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
3.10 stable release
author heinrichsweikamp
date Tue, 28 Apr 2020 17:34:31 +0200
parents cd58f7fc86db
children 7d8a4c60ec1a
comparison
equal deleted inserted replaced
633:690c48db7b5b 634:4050675965ea
1 ;============================================================================= 1 ;=============================================================================
2 ; 2 ;
3 ; File math.asm combined next generation V3.04.3 3 ; File math.asm * combined next generation V3.09.4k
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 ;=============================================================================
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 math CODE 15
16 16 ;=============================================================================
17 ;============================================================================= 17 math1 CODE
18 18 ;=============================================================================
19 global convert_time ; converts hi:lo in minutes to hours (up:hi) and minutes (lo) 19
20 ; also usable for conversion of seconds to minutes and seconds 20 ;-----------------------------------------------------------------------------
21 ; trashes xA, xB, xC 21 ; Divide a 16 Bit Integer by a Power of 2: divA:2 = divA:2 / 2^WREG
22 convert_time: 22 ;
23 movff lo,xA+0 ; copy hi:lo to xA 23 ; trashes WREG
24 movff hi,xA+1 ; 24 ;
25 movlw d'60' ; set divisor to 60 25 global div16
26 movwf xB+0 ; write 60 to xB
27 clrf xB+1 ;
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
32 return
33
34
35 global div16 ; divA:2 = divA:2 / 2^WREG
36 ; trashes WREG
37 div16: 26 div16:
38 bcf STATUS,C ; clear carry 27 bcf STATUS,C ; clear carry
39 rrcf divA+1 ; rotate right high byte, carry into MSB, LSB into carry 28 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 29 rrcf divA+0 ; rotate right low byte, carry into MSB, LSB into carry
41 decfsz WREG ; decrement counter, done? 30 decfsz WREG ; decrement counter, done?
42 bra div16 ; NO - loop 31 bra div16 ; NO - loop
43 return ; YES - done 32 return ; YES - done
44 33
45 34
46 global mult16 ; xA:2 = xA:2 * 2^WREG 35 ;=============================================================================
47 ; trashes WREG 36 math2 CODE
37 ;=============================================================================
38
39 ;-----------------------------------------------------------------------------
40 ; Multiply a 16 bit Integer by a Power of 2: xA:2 = xA:2 * 2^WREG
41 ;
42 ; trashes WREG
43 ;
44 global mult16
48 mult16: 45 mult16:
49 bcf STATUS,C ; clear carry 46 bcf STATUS,C ; clear carry
50 rlcf divA+0,F ; rotate left low byte, carry into LSB, MSB into carry 47 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 48 rlcf divA+1,F ; rotate left high byte, carry into LSB, MSB into carry
52 decfsz WREG ; decrement counter, done? 49 decfsz WREG ; decrement counter, done?
53 bra mult16 ; NO - loop 50 bra mult16 ; NO - loop
54 return ; YES - done 51 return ; YES - done
55 52
56 53
57 global addU16 ; ; sub_c:2 = sub_a:2 + sub_b:2 with USIGNED values 54 ;=============================================================================
58 ; trashes WREG 55 math3 CODE
56 ;=============================================================================
57
58 ;-----------------------------------------------------------------------------
59 ; Add two UNSIGNED 16 bit Integers: sub_c:2 = sub_a:2 + sub_b:2
60 ;
61 ; trashes WREG
62 ;
63 global addU16
59 addU16: 64 addU16:
60 movf sub_a+0,W ; get 1st summand (low byte) to WREG 65 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 66 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 67 movwf sub_c+0 ; copy result (low byte) to sub_c
63 movf sub_a+1,W ; get 1st summand (high byte) to WREG 68 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 69 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 70 movwf sub_c+1 ; copy result (high byte) to sub_c
66 return ; done 71 return ; done
67 72
68 73
69 global sub16 ; sub_c:2 = sub_a:2 - sub_b:2 with SIGNED values 74 ;=============================================================================
70 ; sets neg_flag if result is < 0 75 math4 CODE
71 ; trashes WREG 76 ;=============================================================================
77
78 ;-----------------------------------------------------------------------------
79 ; Subtract two SIGNED 16 Bit Integers: sub_c:2 = sub_a:2 - sub_b:2
80 ;
81 ; sets neg_flag if result is < 0, trashes WREG
82 ;
83 global sub16
72 sub16: 84 sub16:
73 bcf neg_flag ; clear flag which will indicate if result is negative 85 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 86 movf sub_b+0,W ; get value to be subtracted, low byte
75 subwf sub_a+0,W ; execute subtraction on low byte 87 subwf sub_a+0,W ; execute subtraction on low byte
76 movwf sub_c+0 ; copy result to output variable, low byte 88 movwf sub_c+0 ; copy result to output variable, low byte
85 btfsc STATUS,C ; - carry to be propagated ? 97 btfsc STATUS,C ; - carry to be propagated ?
86 incf sub_c+1,F ; YES - do it 98 incf sub_c+1,F ; YES - do it
87 return ; - done 99 return ; - done
88 100
89 101
90 global subU16 ; sub_c:2 = sub_a:2 - sub_b:2 with UNSIGNED values 102 ;=============================================================================
91 ; sets neg_flag if result is < 0 103 math5 CODE
92 ; trashes WREG 104 ;=============================================================================
105
106 ;-----------------------------------------------------------------------------
107 ; Subtract two UNSIGNED 16 Bit Integers: sub_c:2 = sub_a:2 - sub_b:2
108 ;
109 ; sets neg_flag if result is < 0, trashes WREG
110 ;
111 global subU16
93 subU16: 112 subU16:
94 bcf neg_flag ; clear flag which will indicate if result is negative 113 bcf neg_flag ; clear flag which will indicate if result is negative
95 movf sub_b+0,W ; get value to be subtracted, low byte 114 movf sub_b+0,W ; get value to be subtracted, low byte
96 subwf sub_a+0,W ; execute subtraction on low byte 115 subwf sub_a+0,W ; execute subtraction on low byte
97 movwf sub_c+0 ; copy result to output variable, low byte 116 movwf sub_c+0 ; copy result to output variable, low byte
106 btfsc STATUS,C ; - carry to be propagated ? 125 btfsc STATUS,C ; - carry to be propagated ?
107 incf sub_c+1,F ; YES - do it 126 incf sub_c+1,F ; YES - do it
108 return ; - done 127 return ; - done
109 128
110 129
111 global cmpU16 ; sub_a:2 - sub_b:2 with UNSIGNED values 130 ;=============================================================================
112 ; sets neg_flag if result is < 0, but does not store result itself 131 math6 CODE
113 ; trashes WREG 132 ;=============================================================================
133
134 ;-----------------------------------------------------------------------------
135 ; Compare two UNSIGNED 16 Bit Integers: sub_a:2 - sub_b:2
136 ;
137 ; sets neg_flag if result is < 0, trashes WREG
138 ; the subtraction result is not stored
139 ;
140 global cmpU16
114 cmpU16: 141 cmpU16:
115 bcf neg_flag ; clear flag which will indicate if result is negative 142 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 143 movf sub_b+0,W ; get value to be subtracted, low byte
117 subwf sub_a+0,W ; execute subtraction on low byte 144 subwf sub_a+0,W ; execute subtraction on low byte
118 movf sub_b+1,W ; get value to be subtracted, high byte 145 movf sub_b+1,W ; get value to be subtracted, high byte
120 btfss STATUS,C ; borrow to propagate (B == /CARRY) ? 147 btfss STATUS,C ; borrow to propagate (B == /CARRY) ?
121 bsf neg_flag ; YES - result is negative, set flag 148 bsf neg_flag ; YES - result is negative, set flag
122 return ; done 149 return ; done
123 150
124 151
125 global mult16x16 ; xC:4 = xA:2 * xB:2 with UNSIGNED values 152 ;=============================================================================
126 ; trashes PRODL, PRODH, WREG 153 math7 CODE
154 ;=============================================================================
155
156 ;-----------------------------------------------------------------------------
157 ; Multiply two UNSIGNED 16 bit Integers: xC:4 = xA:2 * xB:2
158 ;
159 ; trashes PRODL, PRODH, WREG
160 ;
161 global mult16x16
127 mult16x16: 162 mult16x16:
128 clrf xC+2 ; clear the high-order bits 163 clrf xC+2 ; clear the high-order bits
129 clrf xC+3 164 clrf xC+3 ; ...
130 ; 165
131 movf xA+0,W ; do the "L" multiplication 166 movf xA+0,W ; do the "L" multiplication
132 mulwf xB+0 167 mulwf xB+0
133 movff PRODL,xC+0 ; copy result to xC 168 movff PRODL,xC+0 ; copy result to xC
134 movff PRODH,xC+1 169 movff PRODH,xC+1
135 ; 170
136 movf xA+0,W ; do the "I" multiplication 171 movf xA+0,W ; do the "I" multiplication
137 mulwf xB+1 172 mulwf xB+1
138 movf PRODL,W ; get the product's low byte... 173 movf PRODL,W ; get the product's low byte...
139 addwf xC+1,F ; ... and add it to xC+1 174 addwf xC+1,F ; ... and add it to xC+1
140 movf PRODH,W ; get the product's high byte... 175 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 176 addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1
142 ; 177
143 movf xA+1,W ; do the "O" multiplication 178 movf xA+1,W ; do the "O" multiplication
144 mulwf xB+0 179 mulwf xB+0
145 movf PRODL,W ; get the product's low byte... 180 movf PRODL,W ; get the product's low byte...
146 addwf xC+1,F ; ... and add it to xC+1 181 addwf xC+1,F ; ... and add it to xC+1
147 movf PRODH,W ; get the product's high byte... 182 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 183 addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1
149 clrf WREG ; clear WREG... 184 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) 185 addwfc xC+3,F ; ... add add it to xC+3 obeying carry bit from xC+2 (can only happen in "O" multiplication)
151 ; 186
152 movf xA+1,W ; do the "F" multiplication 187 movf xA+1,W ; do the "F" multiplication
153 mulwf xB+1 188 mulwf xB+1
154 movf PRODL,W ; get the product's low byte... 189 movf PRODL,W ; get the product's low byte...
155 addwf xC+2,F ; ... and add it to xC+2 190 addwf xC+2,F ; ... and add it to xC+2
156 movf PRODH,W ; get the product's high byte... 191 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 192 addwfc xC+3,F ; ... and add it to xC+3 obeying carry bit from xC+2
158 return 193
159 194 return ; done
160 195
161 global div16x16 ; xC:2 = xA:2 / xB:2 with xA as remainder 196
162 ; trashes WREG 197 ;=============================================================================
198 math8 CODE
199 ;=============================================================================
200
201 ;-----------------------------------------------------------------------------
202 ; Divide two UNSIGNED 16 Bit Integers: xC:2 = xA:2 / xB:2 with xA as remainder
203 ;
204 ; trashes WREG
205 ;
206 global div16x16
163 div16x16: 207 div16x16:
164 movlw .16 ; process 16 bits ... 208 movlw .16 ; process 16 bits ...
165 movwf math_loop ; ... initialize loop counter 209 movwf math_loop ; ... initialize loop counter
166 movff xA+0,xC+0 ; copy xA to xC 210 movff xA+0,xC+0 ; copy xA to xC
167 movff xA+1,xC+1 ; ... 211 movff xA+1,xC+1 ; ...
191 decfsz math_loop,F ; decrement loop counter, all bits done? 235 decfsz math_loop,F ; decrement loop counter, all bits done?
192 bra div16x16_1 ; NO - loop 236 bra div16x16_1 ; NO - loop
193 return ; YES - done 237 return ; YES - done
194 238
195 239
196 global div32x16 ; xC:4 = xC:4 / xB:2 with xA as remainder 240 ;=============================================================================
197 ; trashes WREG 241 math9 CODE
242 ;=============================================================================
243
244 ;-----------------------------------------------------------------------------
245 ; Divide a 32 Bit Integer by a 16 Bit Integer: xC:4 = xC:4 / xB:2 with xA as remainder
246 ;
247 ; trashes WREG
248 ;
249 global div32x16
198 div32x16: 250 div32x16:
199 movlw .32 ; process 32 bits ... 251 movlw .32 ; process 32 bits ...
200 movwf math_loop ; ... initialize loop counter 252 movwf math_loop ; ... initialize loop counter
201 clrf xA+0 ; clear xA, will be used to hold the remainder 253 clrf xA+0 ; clear xA, will be used to hold the remainder
202 clrf xA+1 ; ... 254 clrf xA+1 ; ...
226 decfsz math_loop,F ; decrement loop counter, all bits done? 278 decfsz math_loop,F ; decrement loop counter, all bits done?
227 bra div32x16_1 ; NO - loop 279 bra div32x16_1 ; NO - loop
228 return ; YES - done 280 return ; YES - done
229 281
230 282
231 ;============================================================================= 283 ;-----------------------------------------------------------------------------
232 284 ;
233 ; ISR math functions 285 ; ISR math functions
234 286 ;
235 global isr_shift_C31 ; 24 bit shift, repeated WREG times, dedicated to a specific usage: 287 ;-----------------------------------------------------------------------------
236 ; Because less than 8 bits are shifted and only C[2:1] is needed, 288
237 ; we don't care what bit is inserted. 289 ;=============================================================================
290 math10 CODE
291 ;=============================================================================
292
293 ;-----------------------------------------------------------------------------
294 ; 24 Bit Shift, repeated WREG Times, dedicated to a specific Usage
295 ;
296 ; Because less than 8 bits are shifted and only C[2:1] is needed,
297 ; we don't care what bit is inserted.
298 ;
299 global isr_shift_C31
238 isr_shift_C31: 300 isr_shift_C31:
239 rrcf isr_xC+3,F ; shift three bytes 301 rrcf isr_xC+3,F ; shift three bytes
240 rrcf isr_xC+2,F 302 rrcf isr_xC+2,F ; ...
241 rrcf isr_xC+1,F 303 rrcf isr_xC+1,F ; ...
242 decfsz WREG ; decrement loop counter, done? 304 decfsz WREG ; decrement loop counter, done?
243 bra isr_shift_C31 ; NO - loop 305 bra isr_shift_C31 ; NO - loop
244 return ; YES - done 306 return ; YES - done
245 307
246 308
247 global isr_unsigned_mult16x16 ; isr_xC = isr_xA * _isr_xB with UNSIGNED values 309 ;=============================================================================
248 ; trashes PRODL, PRODH, WREG 310 math11 CODE
311 ;=============================================================================
312
313 ;-----------------------------------------------------------------------------
314 ; Multiply two UNSIGNED 16 Bit Integers: isr_xC = isr_xA * isr_xB
315 ;
316 ; trashes PRODL, PRODH, WREG
317 ;
318 global isr_unsigned_mult16x16
249 isr_unsigned_mult16x16: 319 isr_unsigned_mult16x16:
250 movf isr_xA+0,W ; multiply a[0] * b[0] 320 movf isr_xA+0,W ; multiply a[0] * b[0]
251 mulwf isr_xB+0 321 mulwf isr_xB+0 ; ...
252 movff PRODL,isr_xC+0 ; store product to c[1]:c[0] 322 movff PRODL,isr_xC+0 ; store product to c[1]:c[0]
253 movff PRODH,isr_xC+1 323 movff PRODH,isr_xC+1 ; ...
254 ; 324
255 movf isr_xA+1,W ; multiply a[1] * b[1] 325 movf isr_xA+1,W ; multiply a[1] * b[1]
256 mulwf isr_xB+1 326 mulwf isr_xB+1 ; ...
257 movff PRODL, isr_xC+2 ; store product to c[3]:c[2] 327 movff PRODL, isr_xC+2 ; store product to c[3]:c[2]
258 movff PRODH, isr_xC+3 328 movff PRODH, isr_xC+3 ; ...
259 ; 329
260 movf isr_xA+0,W ; multiply a[0] * b[1] 330 movf isr_xA+0,W ; multiply a[0] * b[1]
261 mulwf isr_xB+1 331 mulwf isr_xB+1 ; ...
262 movf PRODL,W ; add cross product to c[3]:c[2]:c[1] 332 movf PRODL,W ; add cross product to c[3]:c[2]:c[1]
263 addwf isr_xC+1,F 333 addwf isr_xC+1,F ; ...
264 movf PRODH,W 334 movf PRODH,W ; ...
265 addwfc isr_xC+2,F ; propagate carry 335 addwfc isr_xC+2,F ; propagate carry
266 clrf WREG 336 clrf WREG ; ...
267 addwfc isr_xC+3,F ; propagate carry 337 addwfc isr_xC+3,F ; ...
268 ; 338
269 movf isr_xA+1,W ; multiply a[1] * b[0] 339 movf isr_xA+1,W ; multiply a[1] * b[0]
270 mulwf isr_xB+0 340 mulwf isr_xB+0 ; ...
271 movf PRODL,W ; add cross product 341 movf PRODL,W ; add cross product
272 addwf isr_xC+1,F 342 addwf isr_xC+1,F ; ...
273 movf PRODH,W 343 movf PRODH,W ; ...
274 addwfc isr_xC+2,F ; propagate carry 344 addwfc isr_xC+2,F ; propagate carry
275 clrf WREG 345 clrf WREG ; ...
276 addwfc isr_xC+3,F ; propagate carry 346 addwfc isr_xC+3,F ; ...
277 return 347 return ; done
278 348
279 349
280 global isr_signed_mult16x16 ; isr_xC = isr_xA * isr_xB with SIGNED values 350 ;-----------------------------------------------------------------------------
281 ; trashes PRODL, PRODH, WREG 351 ; Multiply two SIGNED 16 Bit Integers: isr_xC = isr_xA * isr_xB
352 ;
353 ; trashes PRODL, PRODH, WREG
354 ;
355 global isr_signed_mult16x16
282 isr_signed_mult16x16: 356 isr_signed_mult16x16:
283 rcall isr_unsigned_mult16x16 ; do an unsigned multiplication first 357 ; do an unsigned multiplication first
284 ; ; manage sign extension of operand B 358 rcall isr_unsigned_mult16x16
359
360 ; manage sign extension of operand B
285 btfss isr_xB+1,7 ; is B negative ? 361 btfss isr_xB+1,7 ; is B negative ?
286 bra isr_signed_mult_checkA ; NO - continue checking operand A 362 bra isr_signed_mult_checkA ; NO - continue checking operand A
287 movf isr_xA+0,W ; Yes - add -65536 * A 363 movf isr_xA+0,W ; YES - add -65536 * A
288 subwf isr_xC+2,F 364 subwf isr_xC+2,F ; - ...
289 movf isr_xA+1,W 365 movf isr_xA+1,W ; - ...
290 subwfb isr_xC+3,F 366 subwfb isr_xC+3,F ; - ...
291 isr_signed_mult_checkA ; manage sign extension of operand B 367 isr_signed_mult_checkA ; manage sign extension of operand B
292 btfss isr_xA+1,7 ; is A negative ? 368 btfss isr_xA+1,7 ; is A negative ?
293 return ; NO - done 369 return ; NO - done
294 movf isr_xB+0,W ; Yes - add -65536 * B 370 movf isr_xB+0,W ; YES - add -65536 * B
295 subwf isr_xC+2,F 371 subwf isr_xC+2,F ; - ...
296 movf isr_xB+1,W 372 movf isr_xB+1,W ; - ...
297 subwfb isr_xC+3,F 373 subwfb isr_xC+3,F ; - ...
298 return 374 return ; - done
375
376 ;-----------------------------------------------------------------------------
299 377
300 END 378 END