Commit | Line | Data |
---|---|---|
a5b1af49 FS |
1 | ;---------------------------------------------------------------------------; |
2 | ; Extended itoa, puts, printf and atoi (C)ChaN, 2011 | |
3 | ;---------------------------------------------------------------------------; | |
4 | ||
5 | // Base size is 152 bytes | |
6 | #define CR_CRLF 0 // Convert \n to \r\n (+10 bytes) | |
7 | #define USE_XPRINTF 1 // Enable xprintf function (+194 bytes) | |
8 | #define USE_XSPRINTF 0 // Add xsprintf function (+78 bytes) | |
9 | #define USE_XFPRINTF 0 // Add xfprintf function (+54 bytes) | |
10 | #define USE_XATOI 0 // Enable xatoi function (+182 bytes) | |
11 | ||
12 | ||
13 | #if FLASHEND > 0x1FFFF | |
14 | #error xitoa module does not support 256K devices | |
15 | #endif | |
16 | ||
17 | .nolist | |
18 | #include <avr/io.h> // Include device specific definitions. | |
19 | .list | |
20 | ||
21 | #ifdef SPM_PAGESIZE // Recent devices have "lpm Rd,Z+" and "movw". | |
22 | .macro _LPMI reg | |
23 | lpm \reg, Z+ | |
24 | .endm | |
25 | .macro _MOVW dh,dl, sh,sl | |
26 | movw \dl, \sl | |
27 | .endm | |
28 | #else // Earlier devices do not have "lpm Rd,Z+" nor "movw". | |
29 | .macro _LPMI reg | |
30 | lpm | |
31 | mov \reg, r0 | |
32 | adiw ZL, 1 | |
33 | .endm | |
34 | .macro _MOVW dh,dl, sh,sl | |
35 | mov \dl, \sl | |
36 | mov \dh, \sh | |
37 | .endm | |
38 | #endif | |
39 | ||
40 | ||
41 | ||
42 | ;--------------------------------------------------------------------------- | |
43 | ; Stub function to forward to user output function | |
44 | ; | |
45 | ;Prototype: void xputc (char chr // a character to be output | |
46 | ; ); | |
47 | ;Size: 12/12 words | |
48 | ||
49 | .section .bss | |
50 | .global xfunc_out ; xfunc_out must be initialized before using this module. | |
51 | xfunc_out: .ds.w 1 | |
52 | .section .text | |
53 | ||
54 | ||
55 | .func xputc | |
56 | .global xputc | |
57 | xputc: | |
58 | #if CR_CRLF | |
59 | cpi r24, 10 ;LF --> CRLF | |
60 | brne 1f ; | |
61 | ldi r24, 13 ; | |
62 | rcall 1f ; | |
63 | ldi r24, 10 ;/ | |
64 | 1: | |
65 | #endif | |
66 | push ZH | |
67 | push ZL | |
68 | lds ZL, xfunc_out+0 ;Pointer to the registered output function. | |
69 | lds ZH, xfunc_out+1 ;/ | |
70 | sbiw ZL, 0 ;Skip if null | |
71 | breq 2f ;/ | |
72 | icall | |
73 | 2: pop ZL | |
74 | pop ZH | |
75 | ret | |
76 | .endfunc | |
77 | ||
78 | ||
79 | ||
80 | ;--------------------------------------------------------------------------- | |
81 | ; Direct ROM string output | |
82 | ; | |
83 | ;Prototype: void xputs (const char *str_p // rom string to be output | |
84 | ; ); | |
85 | ||
86 | .func xputs | |
87 | .global xputs | |
88 | xputs: | |
89 | _MOVW ZH,ZL, r25,r24 ; Z = pointer to rom string | |
90 | 1: _LPMI r24 | |
91 | cpi r24, 0 | |
92 | breq 2f | |
93 | rcall xputc | |
94 | rjmp 1b | |
95 | 2: ret | |
96 | .endfunc | |
97 | ||
98 | ||
99 | ;--------------------------------------------------------------------------- | |
100 | ; Extended direct numeral string output (32bit version) | |
101 | ; | |
102 | ;Prototype: void xitoa (long value, // value to be output | |
103 | ; char radix, // radix | |
104 | ; char width); // minimum width | |
105 | ; | |
106 | ||
107 | .func xitoa | |
108 | .global xitoa | |
109 | xitoa: | |
110 | ;r25:r22 = value, r20 = base, r18 = digits | |
111 | clr r31 ;r31 = stack level | |
112 | ldi r30, ' ' ;r30 = sign | |
113 | ldi r19, ' ' ;r19 = filler | |
114 | sbrs r20, 7 ;When base indicates signd format and the value | |
115 | rjmp 0f ;is minus, add a '-'. | |
116 | neg r20 ; | |
117 | sbrs r25, 7 ; | |
118 | rjmp 0f ; | |
119 | ldi r30, '-' ; | |
120 | com r22 ; | |
121 | com r23 ; | |
122 | com r24 ; | |
123 | com r25 ; | |
124 | adc r22, r1 ; | |
125 | adc r23, r1 ; | |
126 | adc r24, r1 ; | |
127 | adc r25, r1 ;/ | |
128 | 0: sbrs r18, 7 ;When digits indicates zero filled, | |
129 | rjmp 1f ;filler is '0'. | |
130 | neg r18 ; | |
131 | ldi r19, '0' ;/ | |
132 | ;----- string conversion loop | |
133 | 1: ldi r21, 32 ;r26 = r25:r22 % r20 | |
134 | clr r26 ;r25:r22 /= r20 | |
135 | 2: lsl r22 ; | |
136 | rol r23 ; | |
137 | rol r24 ; | |
138 | rol r25 ; | |
139 | rol r26 ; | |
140 | cp r26, r20 ; | |
141 | brcs 3f ; | |
142 | sub r26, r20 ; | |
143 | inc r22 ; | |
144 | 3: dec r21 ; | |
145 | brne 2b ;/ | |
146 | cpi r26, 10 ;r26 is a numeral digit '0'-'F' | |
147 | brcs 4f ; | |
148 | subi r26, -7 ; | |
149 | 4: subi r26, -'0' ;/ | |
150 | push r26 ;Stack it | |
151 | inc r31 ;/ | |
152 | cp r22, r1 ;Repeat until r25:r22 gets zero | |
153 | cpc r23, r1 ; | |
154 | cpc r24, r1 ; | |
155 | cpc r25, r1 ; | |
156 | brne 1b ;/ | |
157 | ||
158 | cpi r30, '-' ;Minus sign if needed | |
159 | brne 5f ; | |
160 | push r30 ; | |
161 | inc r31 ;/ | |
162 | 5: cp r31, r18 ;Filler | |
163 | brcc 6f ; | |
164 | push r19 ; | |
165 | inc r31 ; | |
166 | rjmp 5b ;/ | |
167 | ||
168 | 6: pop r24 ;Flush stacked digits and exit | |
169 | rcall xputc ; | |
170 | dec r31 ; | |
171 | brne 6b ;/ | |
172 | ||
173 | ret | |
174 | .endfunc | |
175 | ||
176 | ||
177 | ||
178 | ;---------------------------------------------------------------------------; | |
179 | ; Formatted string output (16/32bit version) | |
180 | ; | |
181 | ;Prototype: | |
182 | ; void __xprintf (const char *format_p, ...); | |
183 | ; void __xsprintf(char*, const char *format_p, ...); | |
184 | ; void __xfprintf(void(*func)(char), const char *format_p, ...); | |
185 | ; | |
186 | ||
187 | #if USE_XPRINTF | |
188 | ||
189 | .func xvprintf | |
190 | xvprintf: | |
191 | ld ZL, Y+ ;Z = pointer to format string | |
192 | ld ZH, Y+ ;/ | |
193 | ||
194 | 0: _LPMI r24 ;Get a format char | |
195 | cpi r24, 0 ;End of format string? | |
196 | breq 90f ;/ | |
197 | cpi r24, '%' ;Is format? | |
198 | breq 20f ;/ | |
199 | 1: rcall xputc ;Put a normal character | |
200 | rjmp 0b ;/ | |
201 | 90: ret | |
202 | ||
203 | 20: ldi r18, 0 ;r18: digits | |
204 | clt ;T: filler | |
205 | _LPMI r21 ;Get flags | |
206 | cpi r21, '%' ;Is a %? | |
207 | breq 1b ;/ | |
208 | cpi r21, '0' ;Zero filled? | |
209 | brne 23f ; | |
210 | set ;/ | |
211 | 22: _LPMI r21 ;Get width | |
212 | 23: cpi r21, '9'+1 ; | |
213 | brcc 24f ; | |
214 | subi r21, '0' ; | |
215 | brcs 90b ; | |
216 | lsl r18 ; | |
217 | mov r0, r18 ; | |
218 | lsl r18 ; | |
219 | lsl r18 ; | |
220 | add r18, r0 ; | |
221 | add r18, r21 ; | |
222 | rjmp 22b ;/ | |
223 | ||
224 | 24: brtc 25f ;get value (low word) | |
225 | neg r18 ; | |
226 | 25: ld r24, Y+ ; | |
227 | ld r25, Y+ ;/ | |
228 | cpi r21, 'c' ;Is type character? | |
229 | breq 1b ;/ | |
230 | cpi r21, 's' ;Is type RAM string? | |
231 | breq 50f ;/ | |
232 | cpi r21, 'S' ;Is type ROM string? | |
233 | breq 60f ;/ | |
234 | _MOVW r23,r22,r25,r24 ;r25:r22 = value | |
235 | clr r24 ; | |
236 | clr r25 ; | |
237 | clt ;/ | |
238 | cpi r21, 'l' ;Is long int? | |
239 | brne 26f ; | |
240 | ld r24, Y+ ;get value (high word) | |
241 | ld r25, Y+ ; | |
242 | set ; | |
243 | _LPMI r21 ;/ | |
244 | 26: cpi r21, 'd' ;Is type signed decimal? | |
245 | brne 27f ;/ | |
246 | ldi r20, -10 ; | |
247 | brts 40f ; | |
248 | sbrs r23, 7 ; | |
249 | rjmp 40f ; | |
250 | ldi r24, -1 ; | |
251 | ldi r25, -1 ; | |
252 | rjmp 40f ;/ | |
253 | 27: cpi r21, 'u' ;Is type unsigned decimal? | |
254 | ldi r20, 10 ; | |
255 | breq 40f ;/ | |
256 | cpi r21, 'X' ;Is type hexdecimal? | |
257 | ldi r20, 16 ; | |
258 | breq 40f ;/ | |
259 | cpi r21, 'b' ;Is type binary? | |
260 | ldi r20, 2 ; | |
261 | breq 40f ;/ | |
262 | ret ;abort | |
263 | 40: push ZH ;Output the value | |
264 | push ZL ; | |
265 | rcall xitoa ; | |
266 | 42: pop ZL ; | |
267 | pop ZH ; | |
268 | rjmp 0b ;/ | |
269 | ||
270 | 50: push ZH ;Put a string on the RAM | |
271 | push ZL | |
272 | _MOVW ZH,ZL, r25,r24 | |
273 | 51: ld r24, Z+ | |
274 | cpi r24, 0 | |
275 | breq 42b | |
276 | rcall xputc | |
277 | rjmp 51b | |
278 | ||
279 | 60: push ZH ;Put a string on the ROM | |
280 | push ZL | |
281 | rcall xputs | |
282 | rjmp 42b | |
283 | .endfunc | |
284 | ||
285 | ||
286 | .func __xprintf | |
287 | .global __xprintf | |
288 | __xprintf: | |
289 | push YH | |
290 | push YL | |
291 | in YL, _SFR_IO_ADDR(SPL) | |
292 | #ifdef SPH | |
293 | in YH, _SFR_IO_ADDR(SPH) | |
294 | #else | |
295 | clr YH | |
296 | #endif | |
297 | adiw YL, 5 ;Y = pointer to arguments | |
298 | rcall xvprintf | |
299 | pop YL | |
300 | pop YH | |
301 | ret | |
302 | .endfunc | |
303 | ||
304 | ||
305 | #if USE_XSPRINTF | |
306 | ||
307 | .func __xsprintf | |
308 | putram: | |
309 | _MOVW ZH,ZL, r15,r14 | |
310 | st Z+, r24 | |
311 | _MOVW r15,r14, ZH,ZL | |
312 | ret | |
313 | .global __xsprintf | |
314 | __xsprintf: | |
315 | push YH | |
316 | push YL | |
317 | in YL, _SFR_IO_ADDR(SPL) | |
318 | #ifdef SPH | |
319 | in YH, _SFR_IO_ADDR(SPH) | |
320 | #else | |
321 | clr YH | |
322 | #endif | |
323 | adiw YL, 5 ;Y = pointer to arguments | |
324 | lds ZL, xfunc_out+0 ;Save registered output function | |
325 | lds ZH, xfunc_out+1 ; | |
326 | push ZL ; | |
327 | push ZH ;/ | |
328 | ldi ZL, lo8(pm(putram));Set local output function | |
329 | ldi ZH, hi8(pm(putram)); | |
330 | sts xfunc_out+0, ZL ; | |
331 | sts xfunc_out+1, ZH ;/ | |
332 | push r15 ;Initialize pointer to string buffer | |
333 | push r14 ; | |
334 | ld r14, Y+ ; | |
335 | ld r15, Y+ ;/ | |
336 | rcall xvprintf | |
337 | _MOVW ZH,ZL, r15,r14 ;Terminate string | |
338 | st Z, r1 ; | |
339 | pop r14 ; | |
340 | pop r15 ;/ | |
341 | pop ZH ;Restore registered output function | |
342 | pop ZL ; | |
343 | sts xfunc_out+0, ZL ; | |
344 | sts xfunc_out+1, ZH ;/ | |
345 | pop YL | |
346 | pop YH | |
347 | ret | |
348 | .endfunc | |
349 | #endif | |
350 | ||
351 | ||
352 | #if USE_XFPRINTF | |
353 | .func __xfprintf | |
354 | .global __xfprintf | |
355 | __xfprintf: | |
356 | push YH | |
357 | push YL | |
358 | in YL, _SFR_IO_ADDR(SPL) | |
359 | #ifdef SPH | |
360 | in YH, _SFR_IO_ADDR(SPH) | |
361 | #else | |
362 | clr YH | |
363 | #endif | |
364 | adiw YL, 5 ;Y = pointer to arguments | |
365 | lds ZL, xfunc_out+0 ;Save registered output function | |
366 | lds ZH, xfunc_out+1 ; | |
367 | push ZL ; | |
368 | push ZH ;/ | |
369 | ld ZL, Y+ ;Set output function | |
370 | ld ZH, Y+ ; | |
371 | sts xfunc_out+0, ZL ; | |
372 | sts xfunc_out+1, ZH ;/ | |
373 | rcall xvprintf | |
374 | pop ZH ;Restore registered output function | |
375 | pop ZL ; | |
376 | sts xfunc_out+0, ZL ; | |
377 | sts xfunc_out+1, ZH ;/ | |
378 | pop YL | |
379 | pop YH | |
380 | ret | |
381 | .endfunc | |
382 | #endif | |
383 | ||
384 | #endif | |
385 | ||
386 | ||
387 | ||
388 | ;--------------------------------------------------------------------------- | |
389 | ; Extended numeral string input | |
390 | ; | |
391 | ;Prototype: | |
392 | ; char xatoi ( /* 1: Successful, 0: Failed */ | |
393 | ; const char **str, /* pointer to pointer to source string */ | |
394 | ; long *res /* result */ | |
395 | ; ); | |
396 | ; | |
397 | ||
398 | ||
399 | #if USE_XATOI | |
400 | .func xatoi | |
401 | .global xatoi | |
402 | xatoi: | |
403 | _MOVW r1, r0, r23, r22 | |
404 | _MOVW XH, XL, r25, r24 | |
405 | ld ZL, X+ | |
406 | ld ZH, X+ | |
407 | clr r18 ;r21:r18 = 0; | |
408 | clr r19 ; | |
409 | clr r20 ; | |
410 | clr r21 ;/ | |
411 | clt ;T = 0; | |
412 | ||
413 | ldi r25, 10 ;r25 = 10; | |
414 | rjmp 41f ;/ | |
415 | 40: adiw ZL, 1 ;Z++; | |
416 | 41: ld r22, Z ;r22 = *Z; | |
417 | cpi r22, ' ' ;if(r22 == ' ') continue | |
418 | breq 40b ;/ | |
419 | brcs 70f ;if(r22 < ' ') error; | |
420 | cpi r22, '-' ;if(r22 == '-') { | |
421 | brne 42f ; T = 1; | |
422 | set ; continue; | |
423 | rjmp 40b ;} | |
424 | 42: cpi r22, '9'+1 ;if(r22 > '9') error; | |
425 | brcc 70f ;/ | |
426 | cpi r22, '0' ;if(r22 < '0') error; | |
427 | brcs 70f ;/ | |
428 | brne 51f ;if(r22 > '0') cv_start; | |
429 | ldi r25, 8 ;r25 = 8; | |
430 | adiw ZL, 1 ;r22 = *(++Z); | |
431 | ld r22, Z ;/ | |
432 | cpi r22, ' '+1 ;if(r22 <= ' ') exit; | |
433 | brcs 80f ;/ | |
434 | cpi r22, 'b' ;if(r22 == 'b') { | |
435 | brne 43f ; r25 = 2; | |
436 | ldi r25, 2 ; cv_start; | |
437 | rjmp 50f ;} | |
438 | 43: cpi r22, 'x' ;if(r22 != 'x') error; | |
439 | brne 51f ;/ | |
440 | ldi r25, 16 ;r25 = 16; | |
441 | ||
442 | 50: adiw ZL, 1 ;Z++; | |
443 | ld r22, Z ;r22 = *Z; | |
444 | 51: cpi r22, ' '+1 ;if(r22 <= ' ') break; | |
445 | brcs 80f ;/ | |
446 | cpi r22, 'a' ;if(r22 >= 'a') r22 =- 0x20; | |
447 | brcs 52f ; | |
448 | subi r22, 0x20 ;/ | |
449 | 52: subi r22, '0' ;if((r22 -= '0') < 0) error; | |
450 | brcs 70f ;/ | |
451 | cpi r22, 10 ;if(r22 >= 10) { | |
452 | brcs 53f ; r22 -= 7; | |
453 | subi r22, 7 ; if(r22 < 10) | |
454 | cpi r22, 10 ; | |
455 | brcs 70f ;} | |
456 | 53: cp r22, r25 ;if(r22 >= r25) error; | |
457 | brcc 70f ;/ | |
458 | 60: ldi r24, 33 ;r21:r18 *= r25; | |
459 | sub r23, r23 ; | |
460 | 61: brcc 62f ; | |
461 | add r23, r25 ; | |
462 | 62: lsr r23 ; | |
463 | ror r21 ; | |
464 | ror r20 ; | |
465 | ror r19 ; | |
466 | ror r18 ; | |
467 | dec r24 ; | |
468 | brne 61b ;/ | |
469 | add r18, r22 ;r21:r18 += r22; | |
470 | adc r19, r24 ; | |
471 | adc r20, r24 ; | |
472 | adc r21, r24 ;/ | |
473 | rjmp 50b ;repeat | |
474 | ||
475 | 70: ldi r24, 0 | |
476 | rjmp 81f | |
477 | 80: ldi r24, 1 | |
478 | 81: brtc 82f | |
479 | clr r22 | |
480 | com r18 | |
481 | com r19 | |
482 | com r20 | |
483 | com r21 | |
484 | adc r18, r22 | |
485 | adc r19, r22 | |
486 | adc r20, r22 | |
487 | adc r21, r22 | |
488 | 82: st -X, ZH | |
489 | st -X, ZL | |
490 | _MOVW XH, XL, r1, r0 | |
491 | st X+, r18 | |
492 | st X+, r19 | |
493 | st X+, r20 | |
494 | st X+, r21 | |
495 | clr r1 | |
496 | ret | |
497 | .endfunc | |
498 | #endif | |
499 | ||
500 |