|
0
|
1 ;=============================================================================
|
|
|
2 ;
|
|
634
|
3 ; File math.asm * combined next generation V3.09.4k
|
|
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
|
|
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
|
|
|
349
|
|
634
|
350 ;-----------------------------------------------------------------------------
|
|
|
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
|
|
604
|
356 isr_signed_mult16x16:
|
|
634
|
357 ; do an unsigned multiplication first
|
|
|
358 rcall isr_unsigned_mult16x16
|
|
|
359
|
|
|
360 ; manage sign extension of operand B
|
|
604
|
361 btfss isr_xB+1,7 ; is B negative ?
|
|
|
362 bra isr_signed_mult_checkA ; NO - continue checking operand A
|
|
634
|
363 movf isr_xA+0,W ; YES - add -65536 * A
|
|
|
364 subwf isr_xC+2,F ; - ...
|
|
|
365 movf isr_xA+1,W ; - ...
|
|
|
366 subwfb isr_xC+3,F ; - ...
|
|
604
|
367 isr_signed_mult_checkA ; manage sign extension of operand B
|
|
|
368 btfss isr_xA+1,7 ; is A negative ?
|
|
|
369 return ; NO - done
|
|
634
|
370 movf isr_xB+0,W ; YES - add -65536 * B
|
|
|
371 subwf isr_xC+2,F ; - ...
|
|
|
372 movf isr_xB+1,W ; - ...
|
|
|
373 subwfb isr_xC+3,F ; - ...
|
|
|
374 return ; - done
|
|
|
375
|
|
|
376 ;-----------------------------------------------------------------------------
|
|
0
|
377
|
|
604
|
378 END |
