2013-05-14 07:16:57 +00:00
;---------------------------------------------------------------------------;
; Extended itoa, puts, printf and atoi (C)ChaN, 2011
;---------------------------------------------------------------------------;
/ / Base s i z e i s 1 5 2 b y t e s
# define C R _ C R L F 0 / / C o n v e r t \ n t o \ r \ n ( + 1 0 b y t e s )
# define U S E _ X P R I N T F 1 / / E n a b l e x p r i n t f f u n c t i o n ( + 1 9 4 b y t e s )
# define U S E _ X S P R I N T F 0 / / A d d x s p r i n t f f u n c t i o n ( + 7 8 b y t e s )
# define U S E _ X F P R I N T F 0 / / A d d x f p r i n t f f u n c t i o n ( + 5 4 b y t e s )
# define U S E _ X A T O I 0 / / E n a b l e x a t o i f u n c t i o n ( + 1 8 2 b y t e s )
# if F L A S H E N D > 0 x1 F F F F
# error x i t o a m o d u l e d o e s n o t s u p p o r t 2 5 6 K d e v i c e s
# endif
.nolist
# include < a v r / i o . h > / / I n c l u d e d e v i c e s p e c i f i c d e f i n i t i o n s .
.list
# ifdef S P M _ P A G E S I Z E / / R e c e n t d e v i c e s h a v e " l p m R d ,Z + " a n d " m o v w " .
.macro _LPMI reg
lpm \ r e g , Z +
.endm
.macro _MOVW dh,d l , s h ,s l
movw \ d l , \ s l
.endm
# else / / E a r l i e r d e v i c e s d o n o t h a v e " l p m R d ,Z + " n o r " m o v w " .
.macro _LPMI reg
lpm
mov \ r e g , r0
adiw Z L , 1
.endm
.macro _MOVW dh,d l , s h ,s l
mov \ d l , \ s l
mov \ d h , \ s h
.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 C R _ C R L F
cpi r24 , 1 0 ;LF --> CRLF
brne 1 f ;
ldi r24 , 1 3 ;
rcall 1 f ;
ldi r24 , 1 0 ;/
1 :
# endif
push Z H
push Z L
lds Z L , x f u n c _ o u t + 0 ;Pointer to the registered output function.
lds Z H , x f u n c _ o u t + 1 ;/
sbiw Z L , 0 ;Skip if null
breq 2 f ;/
icall
2 : pop Z L
pop Z H
ret
.endfunc
;---------------------------------------------------------------------------
; Direct ROM string output
;
2013-05-20 13:42:29 +00:00
;Prototype: void xputs (const char *str_p // rom string to be output
2013-05-14 07:16:57 +00:00
; );
.func xputs
.global xputs
xputs :
_ MOVW Z H ,Z L , r25 ,r24 ; Z = pointer to rom string
1 : _ LPMI r24
cpi r24 , 0
breq 2 f
rcall x p u t c
rjmp 1 b
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 0 f ;is minus, add a '-'.
neg r20 ;
sbrs r25 , 7 ;
rjmp 0 f ;
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 1 f ;filler is '0'.
neg r18 ;
ldi r19 , ' 0 ' ;/
;----- string conversion loop
1 : ldi r21 , 3 2 ;r26 = r25:r22 % r20
clr r26 ;r25:r22 /= r20
2 : lsl r22 ;
rol r23 ;
rol r24 ;
rol r25 ;
rol r26 ;
cp r26 , r20 ;
brcs 3 f ;
sub r26 , r20 ;
inc r22 ;
3 : dec r21 ;
brne 2 b ;/
cpi r26 , 1 0 ;r26 is a numeral digit '0'-'F'
brcs 4 f ;
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 1 b ;/
cpi r30 , ' - ' ;Minus sign if needed
brne 5 f ;
push r30 ;
inc r31 ;/
5 : cp r31 , r18 ;Filler
brcc 6 f ;
push r19 ;
inc r31 ;
rjmp 5 b ;/
6 : pop r24 ;Flush stacked digits and exit
rcall x p u t c ;
dec r31 ;
brne 6 b ;/
ret
.endfunc
;---------------------------------------------------------------------------;
; Formatted string output (16/32bit version)
;
;Prototype:
2013-05-20 13:42:29 +00:00
; void __xprintf (const char *format_p, ...);
; void __xsprintf(char*, const char *format_p, ...);
; void __xfprintf(void(*func)(char), const char *format_p, ...);
2013-05-14 07:16:57 +00:00
;
# if U S E _ X P R I N T F
.func xvprintf
xvprintf :
ld Z L , Y + ;Z = pointer to format string
ld Z H , Y + ;/
0 : _ LPMI r24 ;Get a format char
cpi r24 , 0 ;End of format string?
breq 9 0 f ;/
cpi r24 , ' % ' ;Is format?
breq 2 0 f ;/
1 : rcall x p u t c ;Put a normal character
rjmp 0 b ;/
90 : ret
20 : ldi r18 , 0 ;r18: digits
clt ;T: filler
_ LPMI r21 ;Get flags
cpi r21 , ' % ' ;Is a %?
breq 1 b ;/
cpi r21 , ' 0 ' ;Zero filled?
brne 2 3 f ;
set ;/
22 : _ LPMI r21 ;Get width
23 : cpi r21 , ' 9 ' + 1 ;
brcc 2 4 f ;
subi r21 , ' 0 ' ;
brcs 9 0 b ;
lsl r18 ;
mov r0 , r18 ;
lsl r18 ;
lsl r18 ;
add r18 , r0 ;
add r18 , r21 ;
rjmp 2 2 b ;/
24 : brtc 2 5 f ;get value (low word)
neg r18 ;
25 : ld r24 , Y + ;
ld r25 , Y + ;/
cpi r21 , ' c ' ;Is type character?
breq 1 b ;/
cpi r21 , ' s ' ;Is type RAM string?
breq 5 0 f ;/
cpi r21 , ' S ' ;Is type ROM string?
breq 6 0 f ;/
_ MOVW r23 ,r22 ,r25 ,r24 ;r25:r22 = value
clr r24 ;
clr r25 ;
clt ;/
cpi r21 , ' l ' ;Is long int?
brne 2 6 f ;
ld r24 , Y + ;get value (high word)
ld r25 , Y + ;
set ;
_ LPMI r21 ;/
26 : cpi r21 , ' d ' ;Is type signed decimal?
brne 2 7 f ;/
ldi r20 , - 1 0 ;
brts 4 0 f ;
sbrs r23 , 7 ;
rjmp 4 0 f ;
ldi r24 , - 1 ;
ldi r25 , - 1 ;
rjmp 4 0 f ;/
27 : cpi r21 , ' u ' ;Is type unsigned decimal?
ldi r20 , 1 0 ;
breq 4 0 f ;/
cpi r21 , ' X ' ;Is type hexdecimal?
ldi r20 , 1 6 ;
breq 4 0 f ;/
cpi r21 , ' b ' ;Is type binary?
ldi r20 , 2 ;
breq 4 0 f ;/
ret ;abort
40 : push Z H ;Output the value
push Z L ;
rcall x i t o a ;
42 : pop Z L ;
pop Z H ;
rjmp 0 b ;/
50 : push Z H ;Put a string on the RAM
push Z L
_ MOVW Z H ,Z L , r25 ,r24
51 : ld r24 , Z +
cpi r24 , 0
breq 4 2 b
rcall x p u t c
rjmp 5 1 b
60 : push Z H ;Put a string on the ROM
push Z L
rcall x p u t s
rjmp 4 2 b
.endfunc
.func __xprintf
.global __xprintf
__xprintf :
push Y H
push Y L
in Y L , _ S F R _ I O _ A D D R ( S P L )
# ifdef S P H
in Y H , _ S F R _ I O _ A D D R ( S P H )
# else
clr Y H
# endif
adiw Y L , 5 ;Y = pointer to arguments
rcall x v p r i n t f
pop Y L
pop Y H
ret
.endfunc
# if U S E _ X S P R I N T F
.func __xsprintf
putram :
_ MOVW Z H ,Z L , r15 ,r14
st Z + , r24
_ MOVW r15 ,r14 , Z H ,Z L
ret
.global __xsprintf
__xsprintf :
push Y H
push Y L
in Y L , _ S F R _ I O _ A D D R ( S P L )
# ifdef S P H
in Y H , _ S F R _ I O _ A D D R ( S P H )
# else
clr Y H
# endif
adiw Y L , 5 ;Y = pointer to arguments
lds Z L , x f u n c _ o u t + 0 ;Save registered output function
lds Z H , x f u n c _ o u t + 1 ;
push Z L ;
push Z H ;/
ldi Z L , l o 8 ( p m ( p u t r a m ) ) ;Set local output function
ldi Z H , h i 8 ( p m ( p u t r a m ) ) ;
sts x f u n c _ o u t + 0 , Z L ;
sts x f u n c _ o u t + 1 , Z H ;/
push r15 ;Initialize pointer to string buffer
push r14 ;
ld r14 , Y + ;
ld r15 , Y + ;/
rcall x v p r i n t f
_ MOVW Z H ,Z L , r15 ,r14 ;Terminate string
st Z , r1 ;
pop r14 ;
pop r15 ;/
pop Z H ;Restore registered output function
pop Z L ;
sts x f u n c _ o u t + 0 , Z L ;
sts x f u n c _ o u t + 1 , Z H ;/
pop Y L
pop Y H
ret
.endfunc
# endif
# if U S E _ X F P R I N T F
.func __xfprintf
.global __xfprintf
__xfprintf :
push Y H
push Y L
in Y L , _ S F R _ I O _ A D D R ( S P L )
# ifdef S P H
in Y H , _ S F R _ I O _ A D D R ( S P H )
# else
clr Y H
# endif
adiw Y L , 5 ;Y = pointer to arguments
lds Z L , x f u n c _ o u t + 0 ;Save registered output function
lds Z H , x f u n c _ o u t + 1 ;
push Z L ;
push Z H ;/
ld Z L , Y + ;Set output function
ld Z H , Y + ;
sts x f u n c _ o u t + 0 , Z L ;
sts x f u n c _ o u t + 1 , Z H ;/
rcall x v p r i n t f
pop Z H ;Restore registered output function
pop Z L ;
sts x f u n c _ o u t + 0 , Z L ;
sts x f u n c _ o u t + 1 , Z H ;/
pop Y L
pop Y H
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 U S E _ X A T O I
.func xatoi
.global xatoi
xatoi :
_ MOVW r1 , r0 , r23 , r22
_ MOVW X H , X L , r25 , r24
ld Z L , X +
ld Z H , X +
clr r18 ;r21:r18 = 0;
clr r19 ;
clr r20 ;
clr r21 ;/
clt ;T = 0;
ldi r25 , 1 0 ;r25 = 10;
rjmp 4 1 f ;/
40 : adiw Z L , 1 ;Z++;
41 : ld r22 , Z ;r22 = *Z;
cpi r22 , ' ' ;if(r22 == ' ') continue
breq 4 0 b ;/
brcs 7 0 f ;if(r22 < ' ') error;
cpi r22 , ' - ' ;if(r22 == '-') {
brne 4 2 f ; T = 1;
set ; continue;
rjmp 4 0 b ;}
42 : cpi r22 , ' 9 ' + 1 ;if(r22 > '9') error;
brcc 7 0 f ;/
cpi r22 , ' 0 ' ;if(r22 < '0') error;
brcs 7 0 f ;/
brne 5 1 f ;if(r22 > '0') cv_start;
ldi r25 , 8 ;r25 = 8;
adiw Z L , 1 ;r22 = *(++Z);
ld r22 , Z ;/
cpi r22 , ' ' + 1 ;if(r22 <= ' ') exit;
brcs 8 0 f ;/
cpi r22 , ' b ' ;if(r22 == 'b') {
brne 4 3 f ; r25 = 2;
ldi r25 , 2 ; cv_start;
rjmp 5 0 f ;}
43 : cpi r22 , ' x ' ;if(r22 != 'x') error;
brne 5 1 f ;/
ldi r25 , 1 6 ;r25 = 16;
50 : adiw Z L , 1 ;Z++;
ld r22 , Z ;r22 = *Z;
51 : cpi r22 , ' ' + 1 ;if(r22 <= ' ') break;
brcs 8 0 f ;/
cpi r22 , ' a ' ;if(r22 >= 'a') r22 =- 0x20;
brcs 5 2 f ;
subi r22 , 0 x20 ;/
52 : subi r22 , ' 0 ' ;if((r22 -= '0') < 0) error;
brcs 7 0 f ;/
cpi r22 , 1 0 ;if(r22 >= 10) {
brcs 5 3 f ; r22 -= 7;
subi r22 , 7 ; if(r22 < 10)
cpi r22 , 1 0 ;
brcs 7 0 f ;}
53 : cp r22 , r25 ;if(r22 >= r25) error;
brcc 7 0 f ;/
60 : ldi r24 , 3 3 ;r21:r18 *= r25;
sub r23 , r23 ;
61 : brcc 6 2 f ;
add r23 , r25 ;
62 : lsr r23 ;
ror r21 ;
ror r20 ;
ror r19 ;
ror r18 ;
dec r24 ;
brne 6 1 b ;/
add r18 , r22 ;r21:r18 += r22;
adc r19 , r24 ;
adc r20 , r24 ;
adc r21 , r24 ;/
rjmp 5 0 b ;repeat
70 : ldi r24 , 0
rjmp 8 1 f
80 : ldi r24 , 1
81 : brtc 8 2 f
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 , Z H
st - X , Z L
_ MOVW X H , X L , r1 , r0
st X + , r18
st X + , r19
st X + , r20
st X + , r21
clr r1
ret
.endfunc
# endif