diff options
author | Stephen Christie <undersampled@live.com> | 2016-09-19 18:05:58 -0400 |
---|---|---|
committer | Stephen Christie <undersampled@live.com> | 2016-09-19 18:05:58 -0400 |
commit | 57a6828a7a73d7b7c3dd9693e04d6873e2e3d1c2 (patch) | |
tree | bdabbdbdd9fd6de500728721d1865058468871e3 /tmk_core/common/avr/xprintf.S | |
parent | bf2b260429dbc015d49d005065a37441c8f15753 (diff) | |
parent | 5f9c2f63ffb2c58db73e67442134be58bfe2c129 (diff) |
Merge branch 'master' of http://github.com/jackhumbert/qmk_firmware
Diffstat (limited to 'tmk_core/common/avr/xprintf.S')
-rw-r--r-- | tmk_core/common/avr/xprintf.S | 1000 |
1 files changed, 500 insertions, 500 deletions
diff --git a/tmk_core/common/avr/xprintf.S b/tmk_core/common/avr/xprintf.S index 0cec70ce22..06434b98d9 100644 --- a/tmk_core/common/avr/xprintf.S +++ b/tmk_core/common/avr/xprintf.S @@ -1,500 +1,500 @@ -;---------------------------------------------------------------------------;
-; 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
-
-
+;---------------------------------------------------------------------------; +; 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 + + |