cc65/libsrc/common/vsnprintf.s

;
; int vsnprintf (char* Buf, size_t size, const char* Format, va_list ap);
;
; Ullrich von Bassewitz, 2009-09-26
;

   	.export	      	_vsnprintf, vsnprintf
 	.import	      	ldaxysp, popax, incsp2, incsp6
	.import	      	_memcpy, __printf
	.importzp     	sp, ptr1

	.macpack      	generic

.data

; ----------------------------------------------------------------------------
;
; Static data for the _vsnprintf routine
;

outdesc:	 	 	; Static outdesc structure
ccount: .word   0               ; ccount
func:   .word   out	 	; Output function pointer
bufptr: .word   0	 	; ptr
bufsize:.word   0               ; Buffer size

.code

; ----------------------------------------------------------------------------
; vsprintf - formatted output into a buffer
;
; int __fastcall__ vsnprintf (char* buf, size_t size, const char* format, va_list ap);
;

_vsnprintf:
    	pha	   		; Save ap
        txa
        pha

; Setup the outdesc structure. This is also an additional entry point for
; vsprintf with ap on stack

vsnprintf:
    	lda	#0
    	sta	ccount+0
    	sta	ccount+1        ; Clear ccount

; Get the size parameter and replace it by a pointer to outdesc. This is to
; build a stack frame for the call to _printf.
; If size is zero, there's nothing to do.

        ldy     #2
        lda     (sp),y
        sta     ptr1

        lda     #<outdesc
        sta     (sp),y

        iny
        lda     (sp),y
        sta     ptr1+1

        ora     ptr1
        beq     L9

        lda     #>outdesc
        sta     (sp),y

; Write size-1 to outdesc.uns

        ldy     ptr1+1
        ldx     ptr1
        bne     L1
        dey
L1:     dex
        stx     bufsize+0
        sty     bufsize+1

; Copy buf to the outdesc.ptr

        ldy     #5
        jsr     ldaxysp
        sta     bufptr+0
        stx     bufptr+1

; Restore ap and call _printf

	pla
        tax
        pla
	jsr	__printf

; Terminate the string. The last char is either at bufptr+ccount or
; bufptr+bufsize, whichever is smaller.

        lda     ccount+0
        ldx     ccount+1
        cpx     bufsize+1
        bne     L2
        cmp     bufsize+0
L2:     bcc     L3
        lda     bufsize+0
        ldx     bufsize+1
        clc
L3:     adc     bufptr+0
        sta     ptr1
        txa
        adc     bufptr+1
        sta     ptr1+1

        lda     #0
        tay
        sta     (ptr1),y

; Return the number of bytes written and drop buf

        lda     ccount+0
        ldx     ccount+1
	jmp     incsp2

; Bail out if size is zero.

L9:     pla
        pla                     ; Discard ap
        lda     #0
        tax
        jmp     incsp6          ; Drop parameters


; ----------------------------------------------------------------------------
; Callback routine used for the actual output.
;
; static void out (struct outdesc* d, const char* buf, unsigned count)
; /* Routine used for writing */
;
; Since we know, we're called with a pointer to our static outdesc structure,
; we don't need the pointer passed on the stack.

out:

; Calculate the space left in the buffer. If no space is left, don't copy
; any characters

        lda     bufsize+0               ; Low byte of buffer size
        sec
        sbc     ccount+0                ; Low byte of bytes already written
        sta     ptr1
        lda     bufsize+1
        sbc     ccount+1
        sta     ptr1+1
        bcs     @L0                     ; Branch if space left
        lda     #$00
        sta     ptr1
        sta     ptr1+1                  ; No space left

; Replace the pointer to d by a pointer to the write position in the buffer
; for the call to memcpy that follows.

@L0:    lda     bufptr+0
        clc
        adc     ccount+0
        ldy     #4
        sta     (sp),y

        lda     bufptr+1
        adc     ccount+1
        iny
        sta     (sp),y

; Get Count from stack

        jsr     popax

; outdesc.ccount += Count;

        pha
        clc
        adc     ccount+0
        sta     ccount+0
        txa
        adc     ccount+1
        sta     ccount+1
        pla

; if (Count > Left) Count = Left;

        cpx     ptr1+1
        bne     @L1
        cmp     ptr1
@L1:    bcc     @L2
        lda     ptr1
        ldx     ptr1+1

; Jump to memcpy, which will cleanup the stack and return to the caller

@L2:    jmp     _memcpy