Subversion Repositories svnkaklik

;---------------------------------------------------------------------------;

; Extended itoa, puts, printf and atoi                     (C)ChaN, 2006

;

; Module size: 277/261 words (max)

;

#define USE_XPUTS

#define USE_XITOA

#define USE_XPRINTF

#define USE_XATOI

.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: 15/15 words

.section .bss

.global xfunc_out	; xfunc_out must be initialized before using this module.

xfunc_out:	.ds.w	1

.section .text

.global xputc

.func xputc

xputc:

	cpi	r24, 10		;LF --> CRLF

	brne	1f		;

	ldi	r24, 13		;

	rcall	1f		;

	ldi	r24, 10		;/

1:	push	ZH

	push	ZL

	lds	ZL, xfunc_out+0	;Pointer to the registered output function.

	lds	ZH, xfunc_out+1	;/

	icall

	pop	ZL

	pop	ZH

	ret

.endfunc

;---------------------------------------------------------------------------

; Direct ROM string output

;

;Prototype: void xputs (const prog_char *str // rom string to be output

;			);

;Size: 10/7 words

#ifdef USE_XPUTS

.global xputs

.func 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

#endif

;---------------------------------------------------------------------------

; Extended direct numeral string output (32bit version)

;

;Prototype: void xitoa (long value,	// value to be output

;                       char radix,	// radix

;                       char width);	// minimum width

;Size: 59/59 words

;

#ifdef USE_XITOA

.global xitoa

.func 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

#endif

;---------------------------------------------------------------------------;

; Formatted string output (16/32bit version)

;

;Prototype:

; void xprintf (const prog_char *format, ...);

;Size: 104/94 words

;

#ifdef USE_XPRINTF

.global xprintf

.func xprintf

xprintf:

	push	YH

	push	YL

	in	YL, _SFR_IO_ADDR(SPL)

#ifdef SPH

	in	YH, _SFR_IO_ADDR(SPH)

#else

	clr	YH

#endif

#if FLASHEND > 0x1FFFF

	adiw	YL, 6		;Y = pointer to arguments

#else

	adiw	YL, 5		;Y = pointer to arguments

#endif

	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:	pop	YL

	pop	YH

	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		;/

	rjmp	90b		;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

#endif

;---------------------------------------------------------------------------

; Extended numeral string input

;

;Prototype:

; char xatoi (           /* 1: Successful, 0: Failed */

;      const char **str, /* pointer to pointer to source string */

;      long *res         /* result */

; );

;Size: 94/91 words

;

#ifdef USE_XATOI

.global xatoi

.func 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