-;---------------------------------------------------------------------------;\r
-; Extended itoa, puts, printf and atoi (C)ChaN, 2011\r
-;---------------------------------------------------------------------------;\r
-\r
- // Base size is 152 bytes\r
-#define CR_CRLF 0 // Convert \n to \r\n (+10 bytes)\r
-#define USE_XPRINTF 1 // Enable xprintf function (+194 bytes)\r
-#define USE_XSPRINTF 0 // Add xsprintf function (+78 bytes)\r
-#define USE_XFPRINTF 0 // Add xfprintf function (+54 bytes)\r
-#define USE_XATOI 0 // Enable xatoi function (+182 bytes)\r
-\r
-\r
-#if FLASHEND > 0x1FFFF\r
-#error xitoa module does not support 256K devices\r
-#endif\r
-\r
-.nolist\r
-#include <avr/io.h> // Include device specific definitions.\r
-.list\r
-\r
-#ifdef SPM_PAGESIZE // Recent devices have "lpm Rd,Z+" and "movw".\r
-.macro _LPMI reg\r
- lpm \reg, Z+\r
-.endm\r
-.macro _MOVW dh,dl, sh,sl\r
- movw \dl, \sl\r
-.endm\r
-#else // Earlier devices do not have "lpm Rd,Z+" nor "movw".\r
-.macro _LPMI reg\r
- lpm\r
- mov \reg, r0\r
- adiw ZL, 1\r
-.endm\r
-.macro _MOVW dh,dl, sh,sl\r
- mov \dl, \sl\r
- mov \dh, \sh\r
-.endm\r
-#endif\r
-\r
-\r
-\r
-;---------------------------------------------------------------------------\r
-; Stub function to forward to user output function\r
-;\r
-;Prototype: void xputc (char chr // a character to be output\r
-; );\r
-;Size: 12/12 words\r
-\r
-.section .bss\r
-.global xfunc_out ; xfunc_out must be initialized before using this module.\r
-xfunc_out: .ds.w 1\r
-.section .text\r
-\r
-\r
-.func xputc\r
-.global xputc\r
-xputc:\r
-#if CR_CRLF\r
- cpi r24, 10 ;LF --> CRLF\r
- brne 1f ;\r
- ldi r24, 13 ;\r
- rcall 1f ;\r
- ldi r24, 10 ;/\r
-1:\r
-#endif\r
- push ZH\r
- push ZL\r
- lds ZL, xfunc_out+0 ;Pointer to the registered output function.\r
- lds ZH, xfunc_out+1 ;/\r
- sbiw ZL, 0 ;Skip if null\r
- breq 2f ;/\r
- icall\r
-2: pop ZL\r
- pop ZH\r
- ret\r
-.endfunc\r
-\r
-\r
-\r
-;---------------------------------------------------------------------------\r
-; Direct ROM string output\r
-;\r
-;Prototype: void xputs (const char *str_p // rom string to be output\r
-; );\r
-\r
-.func xputs\r
-.global xputs\r
-xputs:\r
- _MOVW ZH,ZL, r25,r24 ; Z = pointer to rom string\r
-1: _LPMI r24\r
- cpi r24, 0\r
- breq 2f\r
- rcall xputc\r
- rjmp 1b\r
-2: ret\r
-.endfunc\r
-\r
-\r
-;---------------------------------------------------------------------------\r
-; Extended direct numeral string output (32bit version)\r
-;\r
-;Prototype: void xitoa (long value, // value to be output\r
-; char radix, // radix\r
-; char width); // minimum width\r
-;\r
-\r
-.func xitoa\r
-.global xitoa\r
-xitoa:\r
- ;r25:r22 = value, r20 = base, r18 = digits\r
- clr r31 ;r31 = stack level\r
- ldi r30, ' ' ;r30 = sign\r
- ldi r19, ' ' ;r19 = filler\r
- sbrs r20, 7 ;When base indicates signd format and the value\r
- rjmp 0f ;is minus, add a '-'.\r
- neg r20 ;\r
- sbrs r25, 7 ;\r
- rjmp 0f ;\r
- ldi r30, '-' ;\r
- com r22 ;\r
- com r23 ;\r
- com r24 ;\r
- com r25 ;\r
- adc r22, r1 ;\r
- adc r23, r1 ;\r
- adc r24, r1 ;\r
- adc r25, r1 ;/\r
-0: sbrs r18, 7 ;When digits indicates zero filled,\r
- rjmp 1f ;filler is '0'.\r
- neg r18 ;\r
- ldi r19, '0' ;/\r
- ;----- string conversion loop\r
-1: ldi r21, 32 ;r26 = r25:r22 % r20\r
- clr r26 ;r25:r22 /= r20\r
-2: lsl r22 ;\r
- rol r23 ;\r
- rol r24 ;\r
- rol r25 ;\r
- rol r26 ;\r
- cp r26, r20 ;\r
- brcs 3f ;\r
- sub r26, r20 ;\r
- inc r22 ;\r
-3: dec r21 ;\r
- brne 2b ;/\r
- cpi r26, 10 ;r26 is a numeral digit '0'-'F'\r
- brcs 4f ;\r
- subi r26, -7 ;\r
-4: subi r26, -'0' ;/\r
- push r26 ;Stack it\r
- inc r31 ;/\r
- cp r22, r1 ;Repeat until r25:r22 gets zero\r
- cpc r23, r1 ;\r
- cpc r24, r1 ;\r
- cpc r25, r1 ;\r
- brne 1b ;/\r
-\r
- cpi r30, '-' ;Minus sign if needed\r
- brne 5f ;\r
- push r30 ;\r
- inc r31 ;/\r
-5: cp r31, r18 ;Filler\r
- brcc 6f ;\r
- push r19 ;\r
- inc r31 ;\r
- rjmp 5b ;/\r
-\r
-6: pop r24 ;Flush stacked digits and exit\r
- rcall xputc ;\r
- dec r31 ;\r
- brne 6b ;/\r
-\r
- ret\r
-.endfunc\r
-\r
-\r
-\r
-;---------------------------------------------------------------------------;\r
-; Formatted string output (16/32bit version)\r
-;\r
-;Prototype:\r
-; void __xprintf (const char *format_p, ...);\r
-; void __xsprintf(char*, const char *format_p, ...);\r
-; void __xfprintf(void(*func)(char), const char *format_p, ...);\r
-;\r
-\r
-#if USE_XPRINTF\r
-\r
-.func xvprintf\r
-xvprintf:\r
- ld ZL, Y+ ;Z = pointer to format string\r
- ld ZH, Y+ ;/\r
-\r
-0: _LPMI r24 ;Get a format char\r
- cpi r24, 0 ;End of format string?\r
- breq 90f ;/\r
- cpi r24, '%' ;Is format?\r
- breq 20f ;/\r
-1: rcall xputc ;Put a normal character\r
- rjmp 0b ;/\r
-90: ret\r
-\r
-20: ldi r18, 0 ;r18: digits\r
- clt ;T: filler\r
- _LPMI r21 ;Get flags\r
- cpi r21, '%' ;Is a %?\r
- breq 1b ;/\r
- cpi r21, '0' ;Zero filled?\r
- brne 23f ;\r
- set ;/\r
-22: _LPMI r21 ;Get width\r
-23: cpi r21, '9'+1 ;\r
- brcc 24f ;\r
- subi r21, '0' ;\r
- brcs 90b ;\r
- lsl r18 ;\r
- mov r0, r18 ;\r
- lsl r18 ;\r
- lsl r18 ;\r
- add r18, r0 ;\r
- add r18, r21 ;\r
- rjmp 22b ;/\r
-\r
-24: brtc 25f ;get value (low word)\r
- neg r18 ;\r
-25: ld r24, Y+ ;\r
- ld r25, Y+ ;/\r
- cpi r21, 'c' ;Is type character?\r
- breq 1b ;/\r
- cpi r21, 's' ;Is type RAM string?\r
- breq 50f ;/\r
- cpi r21, 'S' ;Is type ROM string?\r
- breq 60f ;/\r
- _MOVW r23,r22,r25,r24 ;r25:r22 = value\r
- clr r24 ;\r
- clr r25 ;\r
- clt ;/\r
- cpi r21, 'l' ;Is long int?\r
- brne 26f ;\r
- ld r24, Y+ ;get value (high word)\r
- ld r25, Y+ ;\r
- set ;\r
- _LPMI r21 ;/\r
-26: cpi r21, 'd' ;Is type signed decimal?\r
- brne 27f ;/\r
- ldi r20, -10 ;\r
- brts 40f ;\r
- sbrs r23, 7 ;\r
- rjmp 40f ;\r
- ldi r24, -1 ;\r
- ldi r25, -1 ;\r
- rjmp 40f ;/\r
-27: cpi r21, 'u' ;Is type unsigned decimal?\r
- ldi r20, 10 ;\r
- breq 40f ;/\r
- cpi r21, 'X' ;Is type hexdecimal?\r
- ldi r20, 16 ;\r
- breq 40f ;/\r
- cpi r21, 'b' ;Is type binary?\r
- ldi r20, 2 ;\r
- breq 40f ;/\r
- ret ;abort\r
-40: push ZH ;Output the value\r
- push ZL ;\r
- rcall xitoa ;\r
-42: pop ZL ;\r
- pop ZH ;\r
- rjmp 0b ;/\r
-\r
-50: push ZH ;Put a string on the RAM\r
- push ZL\r
- _MOVW ZH,ZL, r25,r24\r
-51: ld r24, Z+\r
- cpi r24, 0\r
- breq 42b\r
- rcall xputc\r
- rjmp 51b\r
-\r
-60: push ZH ;Put a string on the ROM\r
- push ZL\r
- rcall xputs\r
- rjmp 42b\r
-.endfunc\r
-\r
-\r
-.func __xprintf\r
-.global __xprintf\r
-__xprintf:\r
- push YH\r
- push YL\r
- in YL, _SFR_IO_ADDR(SPL)\r
-#ifdef SPH\r
- in YH, _SFR_IO_ADDR(SPH)\r
-#else\r
- clr YH\r
-#endif\r
- adiw YL, 5 ;Y = pointer to arguments\r
- rcall xvprintf\r
- pop YL\r
- pop YH\r
- ret\r
-.endfunc\r
-\r
-\r
-#if USE_XSPRINTF\r
-\r
-.func __xsprintf\r
-putram:\r
- _MOVW ZH,ZL, r15,r14\r
- st Z+, r24\r
- _MOVW r15,r14, ZH,ZL\r
- ret\r
-.global __xsprintf\r
-__xsprintf:\r
- push YH\r
- push YL\r
- in YL, _SFR_IO_ADDR(SPL)\r
-#ifdef SPH\r
- in YH, _SFR_IO_ADDR(SPH)\r
-#else\r
- clr YH\r
-#endif\r
- adiw YL, 5 ;Y = pointer to arguments\r
- lds ZL, xfunc_out+0 ;Save registered output function\r
- lds ZH, xfunc_out+1 ;\r
- push ZL ;\r
- push ZH ;/\r
- ldi ZL, lo8(pm(putram));Set local output function\r
- ldi ZH, hi8(pm(putram));\r
- sts xfunc_out+0, ZL ;\r
- sts xfunc_out+1, ZH ;/\r
- push r15 ;Initialize pointer to string buffer\r
- push r14 ;\r
- ld r14, Y+ ;\r
- ld r15, Y+ ;/\r
- rcall xvprintf\r
- _MOVW ZH,ZL, r15,r14 ;Terminate string\r
- st Z, r1 ;\r
- pop r14 ;\r
- pop r15 ;/\r
- pop ZH ;Restore registered output function\r
- pop ZL ;\r
- sts xfunc_out+0, ZL ;\r
- sts xfunc_out+1, ZH ;/\r
- pop YL\r
- pop YH\r
- ret\r
-.endfunc\r
-#endif\r
-\r
-\r
-#if USE_XFPRINTF\r
-.func __xfprintf\r
-.global __xfprintf\r
-__xfprintf:\r
- push YH\r
- push YL\r
- in YL, _SFR_IO_ADDR(SPL)\r
-#ifdef SPH\r
- in YH, _SFR_IO_ADDR(SPH)\r
-#else\r
- clr YH\r
-#endif\r
- adiw YL, 5 ;Y = pointer to arguments\r
- lds ZL, xfunc_out+0 ;Save registered output function\r
- lds ZH, xfunc_out+1 ;\r
- push ZL ;\r
- push ZH ;/\r
- ld ZL, Y+ ;Set output function\r
- ld ZH, Y+ ;\r
- sts xfunc_out+0, ZL ;\r
- sts xfunc_out+1, ZH ;/\r
- rcall xvprintf\r
- pop ZH ;Restore registered output function\r
- pop ZL ;\r
- sts xfunc_out+0, ZL ;\r
- sts xfunc_out+1, ZH ;/\r
- pop YL\r
- pop YH\r
- ret\r
-.endfunc\r
-#endif\r
-\r
-#endif\r
-\r
-\r
-\r
-;---------------------------------------------------------------------------\r
-; Extended numeral string input\r
-;\r
-;Prototype:\r
-; char xatoi ( /* 1: Successful, 0: Failed */\r
-; const char **str, /* pointer to pointer to source string */\r
-; long *res /* result */\r
-; );\r
-;\r
-\r
-\r
-#if USE_XATOI\r
-.func xatoi\r
-.global xatoi\r
-xatoi:\r
- _MOVW r1, r0, r23, r22\r
- _MOVW XH, XL, r25, r24\r
- ld ZL, X+\r
- ld ZH, X+\r
- clr r18 ;r21:r18 = 0;\r
- clr r19 ;\r
- clr r20 ;\r
- clr r21 ;/\r
- clt ;T = 0;\r
-\r
- ldi r25, 10 ;r25 = 10;\r
- rjmp 41f ;/\r
-40: adiw ZL, 1 ;Z++;\r
-41: ld r22, Z ;r22 = *Z;\r
- cpi r22, ' ' ;if(r22 == ' ') continue\r
- breq 40b ;/\r
- brcs 70f ;if(r22 < ' ') error;\r
- cpi r22, '-' ;if(r22 == '-') {\r
- brne 42f ; T = 1;\r
- set ; continue;\r
- rjmp 40b ;}\r
-42: cpi r22, '9'+1 ;if(r22 > '9') error;\r
- brcc 70f ;/\r
- cpi r22, '0' ;if(r22 < '0') error;\r
- brcs 70f ;/\r
- brne 51f ;if(r22 > '0') cv_start;\r
- ldi r25, 8 ;r25 = 8;\r
- adiw ZL, 1 ;r22 = *(++Z);\r
- ld r22, Z ;/\r
- cpi r22, ' '+1 ;if(r22 <= ' ') exit;\r
- brcs 80f ;/\r
- cpi r22, 'b' ;if(r22 == 'b') {\r
- brne 43f ; r25 = 2;\r
- ldi r25, 2 ; cv_start;\r
- rjmp 50f ;}\r
-43: cpi r22, 'x' ;if(r22 != 'x') error;\r
- brne 51f ;/\r
- ldi r25, 16 ;r25 = 16;\r
-\r
-50: adiw ZL, 1 ;Z++;\r
- ld r22, Z ;r22 = *Z;\r
-51: cpi r22, ' '+1 ;if(r22 <= ' ') break;\r
- brcs 80f ;/\r
- cpi r22, 'a' ;if(r22 >= 'a') r22 =- 0x20;\r
- brcs 52f ;\r
- subi r22, 0x20 ;/\r
-52: subi r22, '0' ;if((r22 -= '0') < 0) error;\r
- brcs 70f ;/\r
- cpi r22, 10 ;if(r22 >= 10) {\r
- brcs 53f ; r22 -= 7;\r
- subi r22, 7 ; if(r22 < 10) \r
- cpi r22, 10 ;\r
- brcs 70f ;}\r
-53: cp r22, r25 ;if(r22 >= r25) error;\r
- brcc 70f ;/\r
-60: ldi r24, 33 ;r21:r18 *= r25;\r
- sub r23, r23 ;\r
-61: brcc 62f ;\r
- add r23, r25 ;\r
-62: lsr r23 ;\r
- ror r21 ;\r
- ror r20 ;\r
- ror r19 ;\r
- ror r18 ;\r
- dec r24 ;\r
- brne 61b ;/\r
- add r18, r22 ;r21:r18 += r22;\r
- adc r19, r24 ;\r
- adc r20, r24 ;\r
- adc r21, r24 ;/\r
- rjmp 50b ;repeat\r
-\r
-70: ldi r24, 0\r
- rjmp 81f\r
-80: ldi r24, 1\r
-81: brtc 82f\r
- clr r22\r
- com r18\r
- com r19\r
- com r20\r
- com r21\r
- adc r18, r22\r
- adc r19, r22\r
- adc r20, r22\r
- adc r21, r22\r
-82: st -X, ZH\r
- st -X, ZL\r
- _MOVW XH, XL, r1, r0\r
- st X+, r18\r
- st X+, r19\r
- st X+, r20\r
- st X+, r21\r
- clr r1\r
- ret\r
-.endfunc\r
-#endif\r
-\r
-\r
+;---------------------------------------------------------------------------;
+; Extended itoa, puts, printf and atoi (C)ChaN, 2011
+;---------------------------------------------------------------------------;
+
+ // Base size is 152 bytes
+#define CR_CRLF 0 // Convert \n to \r\n (+10 bytes)
+#define USE_XPRINTF 1 // Enable xprintf function (+194 bytes)
+#define USE_XSPRINTF 0 // Add xsprintf function (+78 bytes)
+#define USE_XFPRINTF 0 // Add xfprintf function (+54 bytes)
+#define USE_XATOI 0 // Enable xatoi function (+182 bytes)
+
+
+#if FLASHEND > 0x1FFFF
+#error xitoa module does not support 256K devices
+#endif
+
+.nolist
+#include <avr/io.h> // Include device specific definitions.
+.list
+
+#ifdef SPM_PAGESIZE // Recent devices have "lpm Rd,Z+" and "movw".
+.macro _LPMI reg
+ lpm \reg, Z+
+.endm
+.macro _MOVW dh,dl, sh,sl
+ movw \dl, \sl
+.endm
+#else // Earlier devices do not have "lpm Rd,Z+" nor "movw".
+.macro _LPMI reg
+ lpm
+ mov \reg, r0
+ adiw ZL, 1
+.endm
+.macro _MOVW dh,dl, sh,sl
+ mov \dl, \sl
+ mov \dh, \sh
+.endm
+#endif
+
+
+
+;---------------------------------------------------------------------------
+; Stub function to forward to user output function
+;
+;Prototype: void xputc (char chr // a character to be output
+; );
+;Size: 12/12 words
+
+.section .bss
+.global xfunc_out ; xfunc_out must be initialized before using this module.
+xfunc_out: .ds.w 1
+.section .text
+
+
+.func xputc
+.global xputc
+xputc:
+#if CR_CRLF
+ cpi r24, 10 ;LF --> CRLF
+ brne 1f ;
+ ldi r24, 13 ;
+ rcall 1f ;
+ ldi r24, 10 ;/
+1:
+#endif
+ push ZH
+ push ZL
+ lds ZL, xfunc_out+0 ;Pointer to the registered output function.
+ lds ZH, xfunc_out+1 ;/
+ sbiw ZL, 0 ;Skip if null
+ breq 2f ;/
+ icall
+2: pop ZL
+ pop ZH
+ ret
+.endfunc
+
+
+
+;---------------------------------------------------------------------------
+; Direct ROM string output
+;
+;Prototype: void xputs (const char *str_p // rom string to be output
+; );
+
+.func xputs
+.global xputs
+xputs:
+ _MOVW ZH,ZL, r25,r24 ; Z = pointer to rom string
+1: _LPMI r24
+ cpi r24, 0
+ breq 2f
+ rcall xputc
+ rjmp 1b
+2: ret
+.endfunc
+
+
+;---------------------------------------------------------------------------
+; Extended direct numeral string output (32bit version)
+;
+;Prototype: void xitoa (long value, // value to be output
+; char radix, // radix
+; char width); // minimum width
+;
+
+.func xitoa
+.global xitoa
+xitoa:
+ ;r25:r22 = value, r20 = base, r18 = digits
+ clr r31 ;r31 = stack level
+ ldi r30, ' ' ;r30 = sign
+ ldi r19, ' ' ;r19 = filler
+ sbrs r20, 7 ;When base indicates signd format and the value
+ rjmp 0f ;is minus, add a '-'.
+ neg r20 ;
+ sbrs r25, 7 ;
+ rjmp 0f ;
+ ldi r30, '-' ;
+ com r22 ;
+ com r23 ;
+ com r24 ;
+ com r25 ;
+ adc r22, r1 ;
+ adc r23, r1 ;
+ adc r24, r1 ;
+ adc r25, r1 ;/
+0: sbrs r18, 7 ;When digits indicates zero filled,
+ rjmp 1f ;filler is '0'.
+ neg r18 ;
+ ldi r19, '0' ;/
+ ;----- string conversion loop
+1: ldi r21, 32 ;r26 = r25:r22 % r20
+ clr r26 ;r25:r22 /= r20
+2: lsl r22 ;
+ rol r23 ;
+ rol r24 ;
+ rol r25 ;
+ rol r26 ;
+ cp r26, r20 ;
+ brcs 3f ;
+ sub r26, r20 ;
+ inc r22 ;
+3: dec r21 ;
+ brne 2b ;/
+ cpi r26, 10 ;r26 is a numeral digit '0'-'F'
+ brcs 4f ;
+ subi r26, -7 ;
+4: subi r26, -'0' ;/
+ push r26 ;Stack it
+ inc r31 ;/
+ cp r22, r1 ;Repeat until r25:r22 gets zero
+ cpc r23, r1 ;
+ cpc r24, r1 ;
+ cpc r25, r1 ;
+ brne 1b ;/
+
+ cpi r30, '-' ;Minus sign if needed
+ brne 5f ;
+ push r30 ;
+ inc r31 ;/
+5: cp r31, r18 ;Filler
+ brcc 6f ;
+ push r19 ;
+ inc r31 ;
+ rjmp 5b ;/
+
+6: pop r24 ;Flush stacked digits and exit
+ rcall xputc ;
+ dec r31 ;
+ brne 6b ;/
+
+ ret
+.endfunc
+
+
+
+;---------------------------------------------------------------------------;
+; Formatted string output (16/32bit version)
+;
+;Prototype:
+; void __xprintf (const char *format_p, ...);
+; void __xsprintf(char*, const char *format_p, ...);
+; void __xfprintf(void(*func)(char), const char *format_p, ...);
+;
+
+#if USE_XPRINTF
+
+.func xvprintf
+xvprintf:
+ ld ZL, Y+ ;Z = pointer to format string
+ ld ZH, Y+ ;/
+
+0: _LPMI r24 ;Get a format char
+ cpi r24, 0 ;End of format string?
+ breq 90f ;/
+ cpi r24, '%' ;Is format?
+ breq 20f ;/
+1: rcall xputc ;Put a normal character
+ rjmp 0b ;/
+90: ret
+
+20: ldi r18, 0 ;r18: digits
+ clt ;T: filler
+ _LPMI r21 ;Get flags
+ cpi r21, '%' ;Is a %?
+ breq 1b ;/
+ cpi r21, '0' ;Zero filled?
+ brne 23f ;
+ set ;/
+22: _LPMI r21 ;Get width
+23: cpi r21, '9'+1 ;
+ brcc 24f ;
+ subi r21, '0' ;
+ brcs 90b ;
+ lsl r18 ;
+ mov r0, r18 ;
+ lsl r18 ;
+ lsl r18 ;
+ add r18, r0 ;
+ add r18, r21 ;
+ rjmp 22b ;/
+
+24: brtc 25f ;get value (low word)
+ neg r18 ;
+25: ld r24, Y+ ;
+ ld r25, Y+ ;/
+ cpi r21, 'c' ;Is type character?
+ breq 1b ;/
+ cpi r21, 's' ;Is type RAM string?
+ breq 50f ;/
+ cpi r21, 'S' ;Is type ROM string?
+ breq 60f ;/
+ _MOVW r23,r22,r25,r24 ;r25:r22 = value
+ clr r24 ;
+ clr r25 ;
+ clt ;/
+ cpi r21, 'l' ;Is long int?
+ brne 26f ;
+ ld r24, Y+ ;get value (high word)
+ ld r25, Y+ ;
+ set ;
+ _LPMI r21 ;/
+26: cpi r21, 'd' ;Is type signed decimal?
+ brne 27f ;/
+ ldi r20, -10 ;
+ brts 40f ;
+ sbrs r23, 7 ;
+ rjmp 40f ;
+ ldi r24, -1 ;
+ ldi r25, -1 ;
+ rjmp 40f ;/
+27: cpi r21, 'u' ;Is type unsigned decimal?
+ ldi r20, 10 ;
+ breq 40f ;/
+ cpi r21, 'X' ;Is type hexdecimal?
+ ldi r20, 16 ;
+ breq 40f ;/
+ cpi r21, 'b' ;Is type binary?
+ ldi r20, 2 ;
+ breq 40f ;/
+ ret ;abort
+40: push ZH ;Output the value
+ push ZL ;
+ rcall xitoa ;
+42: pop ZL ;
+ pop ZH ;
+ rjmp 0b ;/
+
+50: push ZH ;Put a string on the RAM
+ push ZL
+ _MOVW ZH,ZL, r25,r24
+51: ld r24, Z+
+ cpi r24, 0
+ breq 42b
+ rcall xputc
+ rjmp 51b
+
+60: push ZH ;Put a string on the ROM
+ push ZL
+ rcall xputs
+ rjmp 42b
+.endfunc
+
+
+.func __xprintf
+.global __xprintf
+__xprintf:
+ push YH
+ push YL
+ in YL, _SFR_IO_ADDR(SPL)
+#ifdef SPH
+ in YH, _SFR_IO_ADDR(SPH)
+#else
+ clr YH
+#endif
+ adiw YL, 5 ;Y = pointer to arguments
+ rcall xvprintf
+ pop YL
+ pop YH
+ ret
+.endfunc
+
+
+#if USE_XSPRINTF
+
+.func __xsprintf
+putram:
+ _MOVW ZH,ZL, r15,r14
+ st Z+, r24
+ _MOVW r15,r14, ZH,ZL
+ ret
+.global __xsprintf
+__xsprintf:
+ push YH
+ push YL
+ in YL, _SFR_IO_ADDR(SPL)
+#ifdef SPH
+ in YH, _SFR_IO_ADDR(SPH)
+#else
+ clr YH
+#endif
+ adiw YL, 5 ;Y = pointer to arguments
+ lds ZL, xfunc_out+0 ;Save registered output function
+ lds ZH, xfunc_out+1 ;
+ push ZL ;
+ push ZH ;/
+ ldi ZL, lo8(pm(putram));Set local output function
+ ldi ZH, hi8(pm(putram));
+ sts xfunc_out+0, ZL ;
+ sts xfunc_out+1, ZH ;/
+ push r15 ;Initialize pointer to string buffer
+ push r14 ;
+ ld r14, Y+ ;
+ ld r15, Y+ ;/
+ rcall xvprintf
+ _MOVW ZH,ZL, r15,r14 ;Terminate string
+ st Z, r1 ;
+ pop r14 ;
+ pop r15 ;/
+ pop ZH ;Restore registered output function
+ pop ZL ;
+ sts xfunc_out+0, ZL ;
+ sts xfunc_out+1, ZH ;/
+ pop YL
+ pop YH
+ ret
+.endfunc
+#endif
+
+
+#if USE_XFPRINTF
+.func __xfprintf
+.global __xfprintf
+__xfprintf:
+ push YH
+ push YL
+ in YL, _SFR_IO_ADDR(SPL)
+#ifdef SPH
+ in YH, _SFR_IO_ADDR(SPH)
+#else
+ clr YH
+#endif
+ adiw YL, 5 ;Y = pointer to arguments
+ lds ZL, xfunc_out+0 ;Save registered output function
+ lds ZH, xfunc_out+1 ;
+ push ZL ;
+ push ZH ;/
+ ld ZL, Y+ ;Set output function
+ ld ZH, Y+ ;
+ sts xfunc_out+0, ZL ;
+ sts xfunc_out+1, ZH ;/
+ rcall xvprintf
+ pop ZH ;Restore registered output function
+ pop ZL ;
+ sts xfunc_out+0, ZL ;
+ sts xfunc_out+1, ZH ;/
+ pop YL
+ pop YH
+ ret
+.endfunc
+#endif
+
+#endif
+
+
+
+;---------------------------------------------------------------------------
+; Extended numeral string input
+;
+;Prototype:
+; char xatoi ( /* 1: Successful, 0: Failed */
+; const char **str, /* pointer to pointer to source string */
+; long *res /* result */
+; );
+;
+
+
+#if USE_XATOI
+.func xatoi
+.global xatoi
+xatoi:
+ _MOVW r1, r0, r23, r22
+ _MOVW XH, XL, r25, r24
+ ld ZL, X+
+ ld ZH, X+
+ clr r18 ;r21:r18 = 0;
+ clr r19 ;
+ clr r20 ;
+ clr r21 ;/
+ clt ;T = 0;
+
+ ldi r25, 10 ;r25 = 10;
+ rjmp 41f ;/
+40: adiw ZL, 1 ;Z++;
+41: ld r22, Z ;r22 = *Z;
+ cpi r22, ' ' ;if(r22 == ' ') continue
+ breq 40b ;/
+ brcs 70f ;if(r22 < ' ') error;
+ cpi r22, '-' ;if(r22 == '-') {
+ brne 42f ; T = 1;
+ set ; continue;
+ rjmp 40b ;}
+42: cpi r22, '9'+1 ;if(r22 > '9') error;
+ brcc 70f ;/
+ cpi r22, '0' ;if(r22 < '0') error;
+ brcs 70f ;/
+ brne 51f ;if(r22 > '0') cv_start;
+ ldi r25, 8 ;r25 = 8;
+ adiw ZL, 1 ;r22 = *(++Z);
+ ld r22, Z ;/
+ cpi r22, ' '+1 ;if(r22 <= ' ') exit;
+ brcs 80f ;/
+ cpi r22, 'b' ;if(r22 == 'b') {
+ brne 43f ; r25 = 2;
+ ldi r25, 2 ; cv_start;
+ rjmp 50f ;}
+43: cpi r22, 'x' ;if(r22 != 'x') error;
+ brne 51f ;/
+ ldi r25, 16 ;r25 = 16;
+
+50: adiw ZL, 1 ;Z++;
+ ld r22, Z ;r22 = *Z;
+51: cpi r22, ' '+1 ;if(r22 <= ' ') break;
+ brcs 80f ;/
+ cpi r22, 'a' ;if(r22 >= 'a') r22 =- 0x20;
+ brcs 52f ;
+ subi r22, 0x20 ;/
+52: subi r22, '0' ;if((r22 -= '0') < 0) error;
+ brcs 70f ;/
+ cpi r22, 10 ;if(r22 >= 10) {
+ brcs 53f ; r22 -= 7;
+ subi r22, 7 ; if(r22 < 10)
+ cpi r22, 10 ;
+ brcs 70f ;}
+53: cp r22, r25 ;if(r22 >= r25) error;
+ brcc 70f ;/
+60: ldi r24, 33 ;r21:r18 *= r25;
+ sub r23, r23 ;
+61: brcc 62f ;
+ add r23, r25 ;
+62: lsr r23 ;
+ ror r21 ;
+ ror r20 ;
+ ror r19 ;
+ ror r18 ;
+ dec r24 ;
+ brne 61b ;/
+ add r18, r22 ;r21:r18 += r22;
+ adc r19, r24 ;
+ adc r20, r24 ;
+ adc r21, r24 ;/
+ rjmp 50b ;repeat
+
+70: ldi r24, 0
+ rjmp 81f
+80: ldi r24, 1
+81: brtc 82f
+ clr r22
+ com r18
+ com r19
+ com r20
+ com r21
+ adc r18, r22
+ adc r19, r22
+ adc r20, r22
+ adc r21, r22
+82: st -X, ZH
+ st -X, ZL
+ _MOVW XH, XL, r1, r0
+ st X+, r18
+ st X+, r19
+ st X+, r20
+ st X+, r21
+ clr r1
+ ret
+.endfunc
+#endif
+
+
projects / qmk_firmware.git / blobdiff