|
0
|
1 ;=============================================================================
|
|
|
2 ;
|
|
634
|
3 ; File math.asm * combined next generation V3.09.4k
|
|
0
|
4 ;
|
|
|
5 ; Math subroutines
|
|
|
6 ;
|
|
654
|
7 ; Copyright (c) 2011, JD Gascuel, heinrichs weikamp gmbh, all right reserved.
|
|
0
|
8 ;=============================================================================
|
|
|
9 ; HISTORY
|
|
|
10 ; 2011-08-03 : [mH] moving from OSTC code
|
|
|
11
|
|
|
12
|
|
623
|
13 #include "hwos.inc" ; mandatory header
|
|
0
|
14
|
|
604
|
15
|
|
0
|
16 ;=============================================================================
|
|
634
|
17 math1 CODE
|
|
|
18 ;=============================================================================
|
|
604
|
19
|
|
634
|
20 ;-----------------------------------------------------------------------------
|
|
|
21 ; Divide a 16 Bit Integer by a Power of 2: divA:2 = divA:2 / 2^WREG
|
|
|
22 ;
|
|
|
23 ; trashes WREG
|
|
|
24 ;
|
|
|
25 global div16
|
|
604
|
26 div16:
|
|
|
27 bcf STATUS,C ; clear carry
|
|
|
28 rrcf divA+1 ; rotate right high byte, carry into MSB, LSB into carry
|
|
|
29 rrcf divA+0 ; rotate right low byte, carry into MSB, LSB into carry
|
|
|
30 decfsz WREG ; decrement counter, done?
|
|
|
31 bra div16 ; NO - loop
|
|
|
32 return ; YES - done
|
|
0
|
33
|
|
|
34
|
|
634
|
35 ;=============================================================================
|
|
|
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
|
|
604
|
45 mult16:
|
|
|
46 bcf STATUS,C ; clear carry
|
|
|
47 rlcf divA+0,F ; rotate left low byte, carry into LSB, MSB into carry
|
|
|
48 rlcf divA+1,F ; rotate left high byte, carry into LSB, MSB into carry
|
|
|
49 decfsz WREG ; decrement counter, done?
|
|
|
50 bra mult16 ; NO - loop
|
|
|
51 return ; YES - done
|
|
0
|
52
|
|
|
53
|
|
634
|
54 ;=============================================================================
|
|
|
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
|
|
628
|
64 addU16:
|
|
|
65 movf sub_a+0,W ; get 1st summand (low byte) to WREG
|
|
|
66 addwf sub_b+0,W ; add 2nd summand (low byte) and store result in WREG
|
|
|
67 movwf sub_c+0 ; copy result (low byte) to sub_c
|
|
|
68 movf sub_a+1,W ; get 1st summand (high byte) to WREG
|
|
|
69 addwfc sub_b+1,W ; add 2nd summand (high byte) and store result in WREG
|
|
|
70 movwf sub_c+1 ; copy result (high byte) to sub_c
|
|
|
71 return ; done
|
|
|
72
|
|
|
73
|
|
634
|
74 ;=============================================================================
|
|
|
75 math4 CODE
|
|
|
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
|
|
604
|
84 sub16:
|
|
|
85 bcf neg_flag ; clear flag which will indicate if result is negative
|
|
|
86 movf sub_b+0,W ; get value to be subtracted, low byte
|
|
|
87 subwf sub_a+0,W ; execute subtraction on low byte
|
|
|
88 movwf sub_c+0 ; copy result to output variable, low byte
|
|
|
89 movf sub_b+1,W ; get value to be subtracted, high byte
|
|
|
90 subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag
|
|
|
91 movwf sub_c+1 ; copy result to output variable, high byte
|
|
|
92 btfss STATUS,N ; result negative ?
|
|
|
93 return ; NO - result positive, done
|
|
|
94 bsf neg_flag ; YES - set flag
|
|
|
95 comf sub_c+1 ; - do a 16 bit sign change
|
|
|
96 negf sub_c+0 ;
|
|
|
97 btfsc STATUS,C ; - carry to be propagated ?
|
|
|
98 incf sub_c+1,F ; YES - do it
|
|
|
99 return ; - done
|
|
|
100
|
|
0
|
101
|
|
634
|
102 ;=============================================================================
|
|
|
103 math5 CODE
|
|
|
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
|
|
604
|
112 subU16:
|
|
|
113 bcf neg_flag ; clear flag which will indicate if result is negative
|
|
623
|
114 movf sub_b+0,W ; get value to be subtracted, low byte
|
|
604
|
115 subwf sub_a+0,W ; execute subtraction on low byte
|
|
|
116 movwf sub_c+0 ; copy result to output variable, low byte
|
|
|
117 movf sub_b+1,W ; get value to be subtracted, high byte
|
|
|
118 subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag
|
|
|
119 movwf sub_c+1 ; copy result to output variable, high byte
|
|
|
120 btfsc STATUS,C ; borrow to propagate (B == /CARRY) ?
|
|
|
121 return ; NO - result positive, done
|
|
|
122 bsf neg_flag ; YES - set flag
|
|
|
123 comf sub_c+1 ; - do a 16 bit sign change
|
|
623
|
124 negf sub_c+0 ; - ...
|
|
604
|
125 btfsc STATUS,C ; - carry to be propagated ?
|
|
|
126 incf sub_c+1,F ; YES - do it
|
|
|
127 return ; - done
|
|
|
128
|
|
0
|
129
|
|
634
|
130 ;=============================================================================
|
|
|
131 math6 CODE
|
|
|
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
|
|
623
|
141 cmpU16:
|
|
|
142 bcf neg_flag ; clear flag which will indicate if result is negative
|
|
|
143 movf sub_b+0,W ; get value to be subtracted, low byte
|
|
|
144 subwf sub_a+0,W ; execute subtraction on low byte
|
|
|
145 movf sub_b+1,W ; get value to be subtracted, high byte
|
|
|
146 subwfb sub_a+1,W ; execute subtraction on high byte, considering borrow flag
|
|
|
147 btfss STATUS,C ; borrow to propagate (B == /CARRY) ?
|
|
|
148 bsf neg_flag ; YES - result is negative, set flag
|
|
|
149 return ; done
|
|
|
150
|
|
|
151
|
|
634
|
152 ;=============================================================================
|
|
|
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
|
|
604
|
162 mult16x16:
|
|
|
163 clrf xC+2 ; clear the high-order bits
|
|
634
|
164 clrf xC+3 ; ...
|
|
|
165
|
|
604
|
166 movf xA+0,W ; do the "L" multiplication
|
|
|
167 mulwf xB+0
|
|
|
168 movff PRODL,xC+0 ; copy result to xC
|
|
|
169 movff PRODH,xC+1
|
|
634
|
170
|
|
604
|
171 movf xA+0,W ; do the "I" multiplication
|
|
|
172 mulwf xB+1
|
|
|
173 movf PRODL,W ; get the product's low byte...
|
|
|
174 addwf xC+1,F ; ... and add it to xC+1
|
|
|
175 movf PRODH,W ; get the product's high byte...
|
|
|
176 addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1
|
|
634
|
177
|
|
604
|
178 movf xA+1,W ; do the "O" multiplication
|
|
|
179 mulwf xB+0
|
|
634
|
180 movf PRODL,W ; get the product's low byte...
|
|
604
|
181 addwf xC+1,F ; ... and add it to xC+1
|
|
|
182 movf PRODH,W ; get the product's high byte...
|
|
|
183 addwfc xC+2,F ; ... and add it to xC+2 obeying carry bit from xC+1
|
|
|
184 clrf WREG ; clear WREG...
|
|
|
185 addwfc xC+3,F ; ... add add it to xC+3 obeying carry bit from xC+2 (can only happen in "O" multiplication)
|
|
634
|
186
|
|
604
|
187 movf xA+1,W ; do the "F" multiplication
|
|
|
188 mulwf xB+1
|
|
634
|
189 movf PRODL,W ; get the product's low byte...
|
|
604
|
190 addwf xC+2,F ; ... and add it to xC+2
|
|
|
191 movf PRODH,W ; get the product's high byte...
|
|
|
192 addwfc xC+3,F ; ... and add it to xC+3 obeying carry bit from xC+2
|
|
634
|
193
|
|
|
194 return ; done
|
|
0
|
195
|
|
|
196
|
|
634
|
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
|
|
604
|
207 div16x16:
|
|
|
208 movlw .16 ; process 16 bits ...
|
|
|
209 movwf math_loop ; ... initialize loop counter
|
|
|
210 movff xA+0,xC+0 ; copy xA to xC
|
|
|
211 movff xA+1,xC+1 ; ...
|
|
|
212 clrf xA+0 ; clear xA, will now be used to hold the remainder
|
|
|
213 clrf xA+1 ; ...
|
|
|
214 div16x16_1:
|
|
|
215 bcf STATUS,C ; clear carry flag to shift in a zero bit
|
|
|
216 rlcf xC+0,F ; shift left xC
|
|
|
217 rlcf xC+1,F ; ... shifting MSB out of xC...
|
|
|
218 rlcf xA+0,F ; ... and into LSB of xA
|
|
|
219 rlcf xA+1,F ; ...
|
|
|
220 btfsc STATUS,C ; did the remainder overflow (carry set)?
|
|
|
221 bra div16x16_2 ; YES - directly generate a result bit = 1
|
|
|
222 movf xB+0,W ; NO - compute remainder - divisor = xA - xB, trash result to WREG
|
|
|
223 subwf xA+0,W ; - ...
|
|
|
224 movf xB+1,W ; - ...
|
|
|
225 subwfb xA+1,W ; - ...
|
|
|
226 btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ?
|
|
|
227 bra div16x16_3 ; YES - result bit = 0, keep LSB of xC+0 being 0
|
|
|
228 div16x16_2:
|
|
|
229 bsf xC+0,0 ; NO - result bit = 1, set LSB of xC+0 to 1
|
|
|
230 movf xB+0,W ; - subtract divisor from remainder "for real": xA = xA - xB
|
|
|
231 subwf xA+0,F ; - ...
|
|
|
232 movf xB+1,W ; - ...
|
|
|
233 subwfb xA+1,F ; - ...
|
|
0
|
234 div16x16_3:
|
|
604
|
235 decfsz math_loop,F ; decrement loop counter, all bits done?
|
|
|
236 bra div16x16_1 ; NO - loop
|
|
|
237 return ; YES - done
|
|
0
|
238
|
|
|
239
|
|
634
|
240 ;=============================================================================
|
|
|
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
|
|
604
|
250 div32x16:
|
|
|
251 movlw .32 ; process 32 bits ...
|
|
|
252 movwf math_loop ; ... initialize loop counter
|
|
|
253 clrf xA+0 ; clear xA, will be used to hold the remainder
|
|
|
254 clrf xA+1 ; ...
|
|
|
255 div32x16_1:
|
|
|
256 bcf STATUS,C ; clear carry flag to shift in a zero bit
|
|
|
257 rlcf xC+0,F ; shift left xC
|
|
|
258 rlcf xC+1,F ; ...
|
|
|
259 rlcf xC+2,F ; ...
|
|
|
260 rlcf xC+3,F ; ... shifting MSB out of xC...
|
|
|
261 rlcf xA+0,F ; ... and into LSB of xA
|
|
|
262 rlcf xA+1,F ; ...
|
|
|
263 btfsc STATUS,C ; did the remainder overflow (carry set)?
|
|
|
264 bra div32x16_2 ; YES - directly generate a result bit = 1
|
|
|
265 movf xB+0,W ; NO - compute remainder - divisor = xA - xB, trash result to WREG
|
|
|
266 subwf xA+0,W ; - ...
|
|
|
267 movf xB+1,W ; - ...
|
|
|
268 subwfb xA+1,W ; - ...
|
|
|
269 btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ?
|
|
|
270 bra div32x16_3 ; YES - result bit = 0, keep LSB of xC+0 being 0
|
|
0
|
271 div32x16_2:
|
|
604
|
272 bsf xC+0,0 ; NO - result bit = 1, set LSB of xC+0 to 1
|
|
|
273 movf xB+0,W ; - subtract divisor from remainder "for real": xA = xA - xB
|
|
|
274 subwf xA+0,F ; - ...
|
|
|
275 movf xB+1,W ; - ...
|
|
|
276 subwfb xA+1,F ; - ...
|
|
0
|
277 div32x16_3:
|
|
604
|
278 decfsz math_loop,F ; decrement loop counter, all bits done?
|
|
|
279 bra div32x16_1 ; NO - loop
|
|
|
280 return ; YES - done
|
|
|
281
|
|
|
282
|
|
634
|
283 ;-----------------------------------------------------------------------------
|
|
|
284 ;
|
|
|
285 ; ISR math functions
|
|
|
286 ;
|
|
|
287 ;-----------------------------------------------------------------------------
|
|
|
288
|
|
|
289 ;=============================================================================
|
|
|
290 math10 CODE
|
|
604
|
291 ;=============================================================================
|
|
|
292
|
|
634
|
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
|
|
604
|
300 isr_shift_C31:
|
|
|
301 rrcf isr_xC+3,F ; shift three bytes
|
|
634
|
302 rrcf isr_xC+2,F ; ...
|
|
|
303 rrcf isr_xC+1,F ; ...
|
|
604
|
304 decfsz WREG ; decrement loop counter, done?
|
|
|
305 bra isr_shift_C31 ; NO - loop
|
|
|
306 return ; YES - done
|
|
|
307
|
|
|
308
|
|
634
|
309 ;=============================================================================
|
|
|
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
|
|
604
|
319 isr_unsigned_mult16x16:
|
|
|
320 movf isr_xA+0,W ; multiply a[0] * b[0]
|
|
634
|
321 mulwf isr_xB+0 ; ...
|
|
604
|
322 movff PRODL,isr_xC+0 ; store product to c[1]:c[0]
|
|
634
|
323 movff PRODH,isr_xC+1 ; ...
|
|
|
324
|
|
604
|
325 movf isr_xA+1,W ; multiply a[1] * b[1]
|
|
634
|
326 mulwf isr_xB+1 ; ...
|
|
604
|
327 movff PRODL, isr_xC+2 ; store product to c[3]:c[2]
|
|
634
|
328 movff PRODH, isr_xC+3 ; ...
|
|
|
329
|
|
604
|
330 movf isr_xA+0,W ; multiply a[0] * b[1]
|
|
634
|
331 mulwf isr_xB+1 ; ...
|
|
604
|
332 movf PRODL,W ; add cross product to c[3]:c[2]:c[1]
|
|
634
|
333 addwf isr_xC+1,F ; ...
|
|
|
334 movf PRODH,W ; ...
|
|
604
|
335 addwfc isr_xC+2,F ; propagate carry
|
|
634
|
336 clrf WREG ; ...
|
|
|
337 addwfc isr_xC+3,F ; ...
|
|
|
338
|
|
604
|
339 movf isr_xA+1,W ; multiply a[1] * b[0]
|
|
634
|
340 mulwf isr_xB+0 ; ...
|
|
604
|
341 movf PRODL,W ; add cross product
|
|
634
|
342 addwf isr_xC+1,F ; ...
|
|
|
343 movf PRODH,W ; ...
|
|
604
|
344 addwfc isr_xC+2,F ; propagate carry
|
|
634
|
345 clrf WREG ; ...
|
|
|
346 addwfc isr_xC+3,F ; ...
|
|
|
347 return ; done
|
|
0
|
348
|
|
643
|
349 ;=============================================================================
|
|
|
350 math12 CODE
|
|
|
351 ;=============================================================================
|
|
0
|
352
|
|
634
|
353 ;-----------------------------------------------------------------------------
|
|
|
354 ; Multiply two SIGNED 16 Bit Integers: isr_xC = isr_xA * isr_xB
|
|
|
355 ;
|
|
|
356 ; trashes PRODL, PRODH, WREG
|
|
|
357 ;
|
|
|
358 global isr_signed_mult16x16
|
|
604
|
359 isr_signed_mult16x16:
|
|
634
|
360 ; do an unsigned multiplication first
|
|
643
|
361 call isr_unsigned_mult16x16
|
|
634
|
362
|
|
|
363 ; manage sign extension of operand B
|
|
604
|
364 btfss isr_xB+1,7 ; is B negative ?
|
|
|
365 bra isr_signed_mult_checkA ; NO - continue checking operand A
|
|
634
|
366 movf isr_xA+0,W ; YES - add -65536 * A
|
|
|
367 subwf isr_xC+2,F ; - ...
|
|
|
368 movf isr_xA+1,W ; - ...
|
|
|
369 subwfb isr_xC+3,F ; - ...
|
|
643
|
370 isr_signed_mult_checkA: ; manage sign extension of operand B
|
|
604
|
371 btfss isr_xA+1,7 ; is A negative ?
|
|
|
372 return ; NO - done
|
|
634
|
373 movf isr_xB+0,W ; YES - add -65536 * B
|
|
|
374 subwf isr_xC+2,F ; - ...
|
|
|
375 movf isr_xB+1,W ; - ...
|
|
|
376 subwfb isr_xC+3,F ; - ...
|
|
|
377 return ; - done
|
|
|
378
|
|
643
|
379 ;=============================================================================
|
|
|
380 math13 CODE
|
|
|
381 ;=============================================================================
|
|
|
382
|
|
|
383 ;-----------------------------------------------------------------------------
|
|
|
384 ; Multiply two UNSIGNED 24 Bit Integers: isr_xC = isr_xA * isr_xB
|
|
|
385 ;
|
|
|
386 ; trashes PRODL, PRODH, WREG
|
|
|
387 ;
|
|
|
388 global isr_unsigned_mult24x24
|
|
|
389 isr_unsigned_mult24x24:
|
|
|
390 call isr_unsigned_mult16x16 ; Do the 16x16 first
|
|
|
391 clrf isr_xC+4
|
|
|
392 clrf isr_xC+5 ; clear upper 16 bit
|
|
|
393 ; do the remaining three multiplications
|
|
|
394 movf isr_xA+0,W ; multiply a[0] * b[2]
|
|
|
395 mulwf isr_xB+2 ; ...
|
|
|
396 movf PRODL,W ; add cross product to c[4]:c[3]:c[2]
|
|
|
397 addwf isr_xC+2,F ; ...
|
|
|
398 movf PRODH,W ; ...
|
|
|
399 addwfc isr_xC+3,F ; propagate carry
|
|
|
400 clrf WREG ; ...
|
|
|
401 addwfc isr_xC+4,F ; ...
|
|
|
402 addwfc isr_xC+5,F ; ...
|
|
|
403
|
|
|
404 movf isr_xA+2,W ; multiply a[2] * b[0]
|
|
|
405 mulwf isr_xB+0 ; ...
|
|
|
406 movf PRODL,W ; add cross product to c[4]:c[3]:c[2]
|
|
|
407 addwf isr_xC+2,F ; ...
|
|
|
408 movf PRODH,W ; ...
|
|
|
409 addwfc isr_xC+3,F ; propagate carry
|
|
|
410 clrf WREG ; ...
|
|
|
411 addwfc isr_xC+4,F ; ...
|
|
|
412 addwfc isr_xC+5,F ; ...
|
|
|
413 ; isr_xC[2] done.
|
|
|
414
|
|
|
415 movf isr_xA+2,W ; multiply a[2] * b[1]
|
|
|
416 mulwf isr_xB+1 ; ...
|
|
|
417 movf PRODL,W ; add cross product to c[5]:c[4]:c[3]
|
|
|
418 addwf isr_xC+3,F ; ...
|
|
|
419 movf PRODH,W ; ...
|
|
|
420 addwfc isr_xC+4,F ; propagate carry
|
|
|
421 clrf WREG ; ...
|
|
|
422 addwfc isr_xC+5,F ; ...
|
|
|
423
|
|
|
424 movf isr_xA+1,W ; multiply a[1] * b[2]
|
|
|
425 mulwf isr_xB+2 ; ...
|
|
|
426 movf PRODL,W ; add cross product to c[5]:c[4]:c[3]
|
|
|
427 addwf isr_xC+3,F ; ...
|
|
|
428 movf PRODH,W ; ...
|
|
|
429 addwfc isr_xC+4,F ; propagate carry
|
|
|
430 clrf WREG ; ...
|
|
|
431 addwfc isr_xC+5,F ; ...
|
|
|
432 ; isr_xC[3] done.
|
|
|
433
|
|
|
434 movf isr_xA+2,W ; multiply a[2] * b[2]
|
|
|
435 mulwf isr_xB+2 ; ...
|
|
|
436 movf PRODL,W ; add cross product to c[5]:c[4]
|
|
|
437 addwf isr_xC+4,F ; ...
|
|
|
438 movf PRODH,W ; ...
|
|
|
439 addwfc isr_xC+5,F ; propagate carry
|
|
|
440 ; isr_xC[4] done.
|
|
|
441 ; isr_xC[5] done.
|
|
|
442
|
|
|
443 return ; done
|
|
|
444
|
|
|
445 ;=============================================================================
|
|
|
446 math14 CODE
|
|
|
447 ;=============================================================================
|
|
|
448
|
|
|
449 ;-----------------------------------------------------------------------------
|
|
|
450 ; Multiply two UNSIGNED Integers (32 and 24 Bit): isr_xC = isr_xA * isr_xB
|
|
|
451 ;
|
|
|
452 ; trashes PRODL, PRODH, WREG
|
|
|
453 ;
|
|
|
454 global isr_unsigned_mult32x24
|
|
|
455 isr_unsigned_mult32x24:
|
|
|
456 call isr_unsigned_mult24x24 ; Do the 24x24 first
|
|
|
457 clrf isr_xC+6 ; clear upper 8 bit
|
|
|
458 ; do the remaining three multiplications
|
|
|
459 movf isr_xA+3,W ; multiply a[3] * b[0]
|
|
|
460 mulwf isr_xB+0 ; ...
|
|
|
461 movf PRODL,W ; add cross product to c[5]:c[4]:c[3]
|
|
|
462 addwf isr_xC+3,F ; ...
|
|
|
463 movf PRODH,W ; ...
|
|
|
464 addwfc isr_xC+4,F ; propagate carry
|
|
|
465 clrf WREG ; ...
|
|
|
466 addwfc isr_xC+5,F ; ...
|
|
|
467 addwfc isr_xC+6,F ; ...
|
|
|
468 ; isr_xC[3] done.
|
|
|
469
|
|
|
470 movf isr_xA+3,W ; multiply a[3] * b[1]
|
|
|
471 mulwf isr_xB+1 ; ...
|
|
|
472 movf PRODL,W ; add cross product to c[5]:c[4]
|
|
|
473 addwf isr_xC+4,F ; ...
|
|
|
474 movf PRODH,W ; ...
|
|
|
475 addwfc isr_xC+5,F ; propagate carry
|
|
|
476 clrf WREG ; ...
|
|
|
477 addwfc isr_xC+6,F ; ...
|
|
|
478 ; isr_xC[4] done.
|
|
|
479
|
|
|
480 movf isr_xA+3,W ; multiply a[3] * b[2]
|
|
|
481 mulwf isr_xB+2 ; ...
|
|
|
482 movf PRODL,W ; add cross product to c[5]:c[4]
|
|
|
483 addwf isr_xC+5,F ; ...
|
|
|
484 movf PRODH,W ; ...
|
|
|
485 addwfc isr_xC+6,F ; propagate carry
|
|
|
486 ; isr_xC[5] done.
|
|
|
487 ; isr_xC[6] done.
|
|
|
488 return ; done
|
|
|
489
|
|
|
490
|
|
|
491 ;=============================================================================
|
|
|
492 math15 CODE
|
|
|
493 ;=============================================================================
|
|
|
494
|
|
|
495 ;-----------------------------------------------------------------------------
|
|
|
496 ; Multiply two SIGNED 24 Bit Integers: isr_xC = isr_xA * isr_xB
|
|
|
497 ;
|
|
|
498 ; trashes PRODL, PRODH, WREG
|
|
|
499 ;
|
|
|
500 global isr_signed_mult24x24
|
|
|
501 isr_signed_mult24x24:
|
|
|
502 ; do an unsigned multiplication first
|
|
|
503 call isr_unsigned_mult24x24
|
|
|
504
|
|
|
505 ; manage sign extension of operand B
|
|
|
506 btfss isr_xB+2,7 ; is B negative ?
|
|
|
507 bra isr_signed24_mult_checkA ; NO - continue checking operand A
|
|
|
508 movf isr_xA+0,W ; YES - add -16777216 * A
|
|
|
509 subwf isr_xC+3,F ; - ...
|
|
|
510 movf isr_xA+1,W ; - ...
|
|
|
511 subwfb isr_xC+4,F ; - ...
|
|
|
512 movf isr_xA+2,W ; - ...
|
|
|
513 subwfb isr_xC+5,F ; - ...
|
|
|
514 isr_signed24_mult_checkA: ; manage sign extension of operand B
|
|
|
515 btfss isr_xA+2,7 ; is A negative ?
|
|
|
516 return ; NO - done
|
|
|
517 movf isr_xB+0,W ; YES - add -16777216 * B
|
|
|
518 subwf isr_xC+3,F ; - ...
|
|
|
519 movf isr_xB+1,W ; - ...
|
|
|
520 subwfb isr_xC+4,F ; - ...
|
|
|
521 movf isr_xB+2,W ; - ...
|
|
|
522 subwfb isr_xC+5,F ; - ...
|
|
|
523 return
|
|
|
524
|
|
|
525 ;=============================================================================
|
|
|
526 math16 CODE
|
|
|
527 ;=============================================================================
|
|
|
528
|
|
|
529 ;-----------------------------------------------------------------------------
|
|
|
530 ; Divide two UNSIGNED 16 Bit Integers: isr_xC:2 = isr_xA:2 / isr_xB:2 with xA as remainder
|
|
|
531 ;
|
|
|
532 ; trashes WREG
|
|
|
533 ;
|
|
|
534 global isr_div16x16
|
|
|
535 isr_div16x16:
|
|
|
536 movlw .16 ; process 16 bits ...
|
|
|
537 movwf isr_math_loop ; ... initialize loop counter
|
|
|
538 movff isr_xA+0,isr_xC+0 ; copy isr_xA to isr_xC
|
|
|
539 movff isr_xA+1,isr_xC+1 ; ...
|
|
|
540 clrf isr_xA+0 ; clear isr_xA, will now be used to hold the remainder
|
|
|
541 clrf isr_xA+1 ; ...
|
|
|
542 isr_div16x16_1:
|
|
|
543 bcf STATUS,C ; clear carry flag to shift in a zero bit
|
|
|
544 rlcf isr_xC+0,F ; shift left isr_xC
|
|
|
545 rlcf isr_xC+1,F ; ... shifting MSB out of isr_xC...
|
|
|
546 rlcf isr_xA+0,F ; ... and into LSB of isr_xA
|
|
|
547 rlcf isr_xA+1,F ; ...
|
|
|
548 btfsc STATUS,C ; did the remainder overflow (carry set)?
|
|
|
549 bra isr_div16x16_2 ; YES - directly generate a result bit = 1
|
|
|
550 movf isr_xB+0,W ; NO - compute remainder - divisor = isr_xA - isr_xB, trash result to WREG
|
|
|
551 subwf isr_xA+0,W ; - ...
|
|
|
552 movf isr_xB+1,W ; - ...
|
|
|
553 subwfb isr_xA+1,W ; - ...
|
|
|
554 btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ?
|
|
|
555 bra isr_div16x16_3 ; YES - result bit = 0, keep LSB of isr_xC+0 being 0
|
|
|
556 isr_div16x16_2:
|
|
|
557 bsf isr_xC+0,0 ; NO - result bit = 1, set LSB of isr_xC+0 to 1
|
|
|
558 movf isr_xB+0,W ; - subtract divisor from remainder "for real": isr_xA = isr_xA - isr_xB
|
|
|
559 subwf isr_xA+0,F ; - ...
|
|
|
560 movf isr_xB+1,W ; - ...
|
|
|
561 subwfb isr_xA+1,F ; - ...
|
|
|
562 isr_div16x16_3:
|
|
|
563 decfsz isr_math_loop,F ; decrement loop counter, all bits done?
|
|
|
564 bra isr_div16x16_1 ; NO - loop
|
|
|
565 return ; YES - done
|
|
|
566
|
|
|
567 ;=============================================================================
|
|
|
568 math17 CODE
|
|
|
569 ;=============================================================================
|
|
|
570
|
|
|
571 ;-----------------------------------------------------------------------------
|
|
|
572 ; Divide a 32 Bit Integer by a 16 Bit Integer: isr_xC:4 = isr_xC:4 / isr_xB:2 with isr_xA as remainder
|
|
|
573 ;
|
|
|
574 ; trashes WREG
|
|
|
575 ;
|
|
|
576 global isr_div32x16
|
|
|
577 isr_div32x16:
|
|
|
578 movlw .32 ; process 32 bits ...
|
|
|
579 movwf isr_math_loop ; ... initialize loop counter
|
|
|
580 clrf isr_xA+0 ; clear isr_xA, will be used to hold the remainder
|
|
|
581 clrf isr_xA+1 ; ...
|
|
|
582 isr_div32x16_1:
|
|
|
583 bcf STATUS,C ; clear carry flag to shift in a zero bit
|
|
|
584 rlcf isr_xC+0,F ; shift left isr_xC
|
|
|
585 rlcf isr_xC+1,F ; ...
|
|
|
586 rlcf isr_xC+2,F ; ...
|
|
|
587 rlcf isr_xC+3,F ; ... shifting MSB out of isr_xC...
|
|
|
588 rlcf isr_xA+0,F ; ... and into LSB of isr_xA
|
|
|
589 rlcf isr_xA+1,F ; ...
|
|
|
590 btfsc STATUS,C ; did the remainder overflow (carry set)?
|
|
|
591 bra isr_div32x16_2 ; YES - directly generate a result bit = 1
|
|
|
592 movf isr_xB+0,W ; NO - compute remainder - divisor = isr_xA - isr_xB, trash result to WREG
|
|
|
593 subwf isr_xA+0,W ; - ...
|
|
|
594 movf isr_xB+1,W ; - ...
|
|
|
595 subwfb isr_xA+1,W ; - ...
|
|
|
596 btfss STATUS,C ; - remainder < divisor (-> borrow flag set, equals carry flag cleared) ?
|
|
|
597 bra isr_div32x16_3 ; YES - result bit = 0, keep LSB of isr_xC+0 being 0
|
|
|
598 isr_div32x16_2:
|
|
|
599 bsf isr_xC+0,0 ; NO - result bit = 1, set LSB of isr_xC+0 to 1
|
|
|
600 movf isr_xB+0,W ; - subtract divisor from remainder "for real": isr_xA = isr_xA - isr_xB
|
|
|
601 subwf isr_xA+0,F ; - ...
|
|
|
602 movf isr_xB+1,W ; - ...
|
|
|
603 subwfb isr_xA+1,F ; - ...
|
|
|
604 isr_div32x16_3:
|
|
|
605 decfsz isr_math_loop,F ; decrement loop counter, all bits done?
|
|
|
606 bra isr_div32x16_1 ; NO - loop
|
|
|
607 return ; YES - done
|
|
|
608
|
|
|
609
|
|
634
|
610 ;-----------------------------------------------------------------------------
|
|
0
|
611
|
|
604
|
612 END |
