diff options
Diffstat (limited to 'platforms/avr/xprintf.S')
| -rw-r--r-- | platforms/avr/xprintf.S | 498 | 
1 files changed, 498 insertions, 0 deletions
| diff --git a/platforms/avr/xprintf.S b/platforms/avr/xprintf.S new file mode 100644 index 0000000000..c5a414c35c --- /dev/null +++ b/platforms/avr/xprintf.S @@ -0,0 +1,498 @@ +;---------------------------------------------------------------------------; +; 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 | 
