cc65/libsrc/common/_printf.s

;
; _printf: Basic layer for all printf type functions.
;
; Ullrich von Bassewitz, 21.10.2000
;

  	.export	  	__printf

	.import	  	popax, pushax, pusheax, decsp6, push1, axlong, axulong
	.import		__ctype
	.import		_ltoa, _ultoa
	.import		_strlower, _strlen
	.importzp	sp, ptr1, ptr2, tmp1, regbank, sreg

	.macpack	generic

; ----------------------------------------------------------------------------
; We will store variables into the register bank in the zeropage. Define
; equates for these variables.

ArgList	       	= regbank+0		; Argument list pointer
Format 	       	= regbank+2 		; Format string
OutData	     	= regbank+4 		; Function parameters

; ----------------------------------------------------------------------------
; Other zero page cells

Base	     	= ptr1
FSave	     	= ptr1
FCount		= ptr2

.code

; ----------------------------------------------------------------------------
; Get one character from the format string and increment the pointer. Will
; return zero in Y.

GetFormatChar:
  	ldy    	#0
  	lda	(Format),y
IncFormatPtr:
	inc	Format
	bne	@L1
	inc	Format+1
@L1:	rts

; ----------------------------------------------------------------------------
; Output a pad character: outfunc (d, &padchar, 1)

OutputPadChar:
	lda	PadChar

; ----------------------------------------------------------------------------
; Call the output function with one character in A

Output1:
	sta	CharArg
	jsr	PushOutData
	lda	#<CharArg
	ldx	#>CharArg
	jsr	pushax
    	jsr  	push1
	jmp	CallOutFunc	; fout (OutData, &CharArg, 1)

; ----------------------------------------------------------------------------
; Decrement the argument list pointer by 2

DecArgList2:
	lda	ArgList
	sub	#2
	sta	ArgList
	bcs	@L1
	dec	ArgList+1
@L1:	rts

; ----------------------------------------------------------------------------
; Get an unsigned int or long argument depending on the IsLong flag.

GetUnsignedArg:
	lda	IsLong	       		; Check flag
	bne	GetLongArg     		; Long sets all
	jsr	GetIntArg      		; Get an integer argument
	jmp    	axulong	       		; Convert to unsigned long

; ----------------------------------------------------------------------------
; Get an signed int or long argument depending on the IsLong flag.

GetSignedArg:
	lda	IsLong	       		; Check flag
	bne	GetLongArg     		; Long sets all
	jsr	GetIntArg      		; Get an integer argument
	jmp	axlong	       		; Convert to long

; ----------------------------------------------------------------------------
; Get a long argument from the argument list. Returns 0 in Y.

GetLongArg:
	jsr	GetIntArg      		; Get high word
	sta	sreg
	stx	sreg+1

; Run into GetIntArg fetching the low word

; ----------------------------------------------------------------------------
; Get an integer argument from the argument list. Returns 0 in Y.

GetIntArg:
	jsr	DecArgList2
	ldy	#1
	lda	(ArgList),y
	tax
	dey
	lda	(ArgList),y
	rts

; ----------------------------------------------------------------------------
; Read an integer from the format string. Will return zero in Y.

ReadInt:
    	ldy	#0
    	sty	ptr1
    	sty	ptr1+1	       		; Start with zero
@Loop:	lda	(Format),y    		; Get format string character
	sub	#'0'			; Make number from ascii digit
	bcc	@L9			; Jump if done
	cmp	#9+1
	bcs	@L9			; Jump if done

; Skip the digit character

	jsr	IncFormatPtr

; Add the digit to the value we have in ptr1

     	pha				; Save digit value
     	lda	ptr1
     	ldx	ptr1+1
     	asl	ptr1
     	rol	ptr1+1	      		; * 2
     	asl	ptr1
     	rol	ptr1+1	      		; * 4, assume carry clear
       	adc	ptr1
     	sta	ptr1
     	txa
     	adc	ptr1+1
     	sta	ptr1+1	      		; * 5
     	asl	ptr1
     	rol  	ptr1+1	      		; * 10, assume carry clear
     	pla
       	adc	ptr1 	      		; Add digit value
     	sta	ptr1
     	bcc	@Loop
     	inc	ptr1+1
	bcs	@Loop			; Branch always

; We're done converting

@L9:	lda	ptr1
	ldx	ptr1+1 	  		; Load result
	rts


; ----------------------------------------------------------------------------
; Put a character into the argument buffer and increment the buffer index

PutBuf:	ldy	BufIdx
	inc	BufIdx
	sta	Buf,y
	rts

; ----------------------------------------------------------------------------
; Get a pointer to the current buffer end and push it onto the stack

PushBufPtr:
    	lda	#<Buf
    	ldx	#>Buf
    	add	BufIdx
    	bcc	@L1
    	inx
@L1:	jmp	pushax

; ----------------------------------------------------------------------------
; Push OutData onto the software stack

PushOutData:
	lda	OutData
	ldx	OutData+1
	jmp	pushax

; ----------------------------------------------------------------------------
; Output Width pad characters
;

PadLoop:
       	jsr 	OutputPadChar
OutputPadding:
	inc	Width
    	bne	PadLoop
    	inc	Width+1
    	bne    	PadLoop
	rts

; ----------------------------------------------------------------------------
; Output the argument itself: outfunc (d, str, arglen);
;

OutputArg:
	jsr	PushOutData
	lda	Str
	ldx	Str+1
	jsr	pushax
	lda	ArgLen
	ldx	ArgLen+1
	jsr	pushax
	jmp	CallOutFunc

; ----------------------------------------------------------------------------
; ltoa: Wrapper for _ltoa that pushes all arguments

ltoa:	sty	Base			; Save base
	jsr	pusheax			; Push value
    	jsr	PushBufPtr		; Push the buffer pointer...
       	lda	Base			; Restore base
    	jmp	_ltoa 			; ultoa (l, s, base);


; ----------------------------------------------------------------------------
; ultoa: Wrapper for _ultoa that pushes all arguments

ultoa: 	sty    	Base			; Save base
	jsr	pusheax	   		; Push value
    	jsr	PushBufPtr 		; Push the buffer pointer...
       	lda	Base			; Restore base
    	jmp  	_ultoa 	   		; ultoa (l, s, base);


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

__printf:

; Save the register bank variables into the save area

 	pha				; Save low byte of ap
       	ldy	#5
Save:	lda	regbank,y
     	sta	RegSave,y
     	dey
       	bpl	Save

; Get the parameters from the stack

	pla				; Restore low byte of ap
	sta	ArgList			; Argument list pointer
	stx	ArgList+1

	jsr	popax	       		; Format string
	sta	Format
	stx	Format+1

  	jsr	popax 	 		; Output descriptor
      	sta	OutData
	stx	OutData+1

; Initialize the output counter in the output descriptor to zero

  	lda  	#0
	tay
    	sta  	(OutData),y
  	iny
  	sta  	(OutData),y

; Get the output function from the output descriptor and remember it

	iny
	lda	(OutData),y
	sta  	CallOutFunc+1
	iny
	lda  	(OutData),y
	sta  	CallOutFunc+2

; Start parsing the format string

MainLoop:
	lda	Format			; Remember current format pointer
	sta	FSave
	lda	Format+1
	sta	FSave+1

	ldy	#0  			; Index
@L1:	lda	(Format),y		; Get next char
       	beq 	@L2			; Jump on end of string
	cmp	#'%'			; Format spec?
	beq	@L2
   	iny	    			; Bump pointer
       	bne    	@L1
	inc	Format+1		; Bump high byte of pointer
	bne	@L1 			; Branch always

; Found a '%' character or end of string. Update the Format pointer so it is
; current (points to this character).

@L2:	tya	    		     	; Low byte of offset
	add	Format
	sta	Format
	bcc	@L3
    	inc	Format+1

; Calculate, how many characters must be output. Beware: This number may
; be zero. A still contains the low byte of the pointer.

@L3:	sub	FSave
	sta	FCount
	lda	Format+1
	sbc	FSave+1
	sta	FCount+1
	ora	FCount 		     	; Is the result zero?
       	beq	@L4		     	; Jump if yes

; Output the characters that we have until now. To make the call to out
; faster, build the stack frame by hand (don't use pushax)

	jsr	decsp6		     	; 3 args
	ldy	#5
	lda	OutData+1
	sta	(sp),y
	dey
	lda	OutData
	sta	(sp),y
	dey
	lda	FSave+1
	sta	(sp),y
	dey
	lda	FSave
	sta	(sp),y
	dey
	lda	FCount+1
	sta	(sp),y
	dey
	lda	FCount
	sta	(sp),y
	jsr	CallOutFunc	     	; Call the output function

; We're back from out(), or we didn't call it. Check for end of string.

@L4:  	jsr  	GetFormatChar	     	; Get one char, zero in Y
    	tax  	      		     	; End of format string reached?
       	bne    	NotDone		     	; End not reached

; End of format string reached. Restore the zeropage registers and return.

  	ldx    	#5
Rest:	lda	RegSave,x
  	sta	regbank,x
  	dex
	bpl	Rest
	rts

; Still a valid format character. Check for '%' and a '%%' sequence. Output
; anything that is not a format specifier. On intro, Y is zero.

NotDone:
	cmp	#'%'
    	bne	@L1
      	lda	(Format),y	     	; Check for "%%"
	cmp	#'%'
	bne	FormatSpec	     	; Jump if really a format specifier
	jsr	IncFormatPtr	     	; Skip the second '%'
@L1:	jsr	Output1		     	; Output the character...
	jmp	MainLoop	     	; ...and continue

; We have a real format specifier
; Format is: %[flags][width][.precision][mod]type
; Y is zero on entry.

FormatSpec:

; Initialize the flags

	lda	#0
       	ldx	#FormatVarSize-1
@L1:	sta	FormatVars,x
    	dex
	bpl	@L1

; Start with reading the flags if there are any. X is $FF which is used
; for "true"

ReadFlags:
  	lda	(Format),y	        ; Get next char...
  	cmp	#'-'
  	bne	@L1
  	stx	LeftJust
  	beq	@L4

@L1:	cmp	#'+'
  	bne	@L2
  	stx	AddSign
  	beq	@L4

@L2:	cmp	#' '
  	bne	@L3
  	stx	AddBlank
    	beq	@L4

@L3:	cmp	#'#'
    	bne	ReadPadding
    	stx	AltForm

@L4:	jsr	IncFormatPtr
      	jmp	ReadFlags		; ...and start over

; Done with flags, read the pad char. Y is still zero if we come here.

ReadPadding:
	ldx	#' '  	  		; PadChar
	cmp	#'0'
	bne	@L1
	tax	      			; PadChar is '0'
  	jsr	IncFormatPtr
  	lda	(Format),y     		; Read current for later
@L1:	stx	PadChar

; Read the width. Even here, Y is still zero. A contains the current character
; from the format string

ReadWidth:
    	cmp	#'*'
    	bne	@L1
    	jsr	IncFormatPtr
    	jsr	GetIntArg      		; Width is an additional argument
    	jmp	@L2

@L1:	jsr	ReadInt	  		; Read integer from format string...
@L2:	sta	Width
    	stx	Width+1	  		; ...and remember in Width

; Read the precision. Even here, Y is still zero.

	sty	Prec	       		; Assume Precision is zero
	sty	Prec+1
    	lda	(Format),y		; Load next format string char
    	cmp	#'.'	  		; Precision given?
	bne	ReadMod	  		; Branch if no precision given

ReadPrec:
	jsr	IncFormatPtr		; Skip the '.'
	lda	(Format),y
	cmp	#'*'  	  		; Variable precision?
	bne	@L1
	jsr	IncFormatPtr		; Skip the '*'
	jsr	GetIntArg		; Get integer argument
      	jmp	@L2

@L1:  	jsr	ReadInt			; Read integer from format string
@L2:  	sta	Prec
      	stx	Prec+1

; Read the modifiers. Y is still zero.

ReadMod:
	lda 	(Format),y
      	cmp 	#'F'
      	beq 	@L1			; Read and ignore this one
      	cmp 	#'N'
      	beq 	@L1			; Read and ignore this one
      	cmp 	#'h'
      	beq 	@L1			; Read and ignore this one
      	cmp 	#'L'
      	beq 	@L1	 		; Read and ignore this one
      	cmp 	#'l'
      	bne 	DoFormat
      	lda 	#$FF
      	sta 	IsLong
@L1:  	jsr 	IncFormatPtr
      	jmp	ReadMod

; Initialize the argument buffer pointers. We use a static buffer (ArgBuf) to
; assemble strings. A zero page index (BufIdx) is used to keep the current
; write position. A pointer to the buffer (Str) is used to point to the the
; argument in case we will not use the buffer but a user supplied string.
; Y is zero when we come here.

DoFormat:
	sty	BufIdx 			; Clear BufIdx
	ldx    	#<Buf
       	stx	Str
	ldx	#>Buf
	stx	Str+1

; Skip the current format character, then check it (current char in A)

	jsr	IncFormatPtr

; Is it a character?

	cmp	#'c'
	bne	CheckInt

; It is a character

	jsr	GetIntArg		; Get the argument (promoted to int)
	sta	Buf			; Place it as zero terminated string...
	lda	#0
	sta	Buf+1			; ...into the buffer
	jmp	HaveArg			; Done

; Is it an integer?

CheckInt:
	cmp	#'d'
	beq	@L1
	cmp	#'i'
	bne	CheckCount

; It is an integer

@L1:	ldx	#0
	lda	AddBlank		; Add a blank for positives?
    	beq	@L2			; Jump if no
   	ldx	#' '
@L2:	lda	AddSign			; Add a plus for positives (precedence)?
   	beq	@L3
   	ldx	#'+'
@L3:	stx    	Leader

; Integer argument

   	jsr	GetSignedArg           	; Get argument as a long
   	ldy	sreg+1	 	    	; Check sign
   	bmi	@Int1
   	ldy	Leader
   	beq	@Int1
   	sty	Buf
   	inc	BufIdx

@Int1:	ldy    	#10			; Base
	jsr	ltoa			; Push arguments, call _ltoa
    	jmp	HaveArg

; Is it a count pseudo format?

CheckCount:
	cmp	#'n'
	bne	CheckOctal

; It is a count pseudo argument

	jsr	GetIntArg
	sta	ptr1
	stx	ptr1+1			; Get user supplied pointer
	ldy	#0
	lda	(OutData),y		; Low byte of OutData->ccount
	sta	(ptr1),y
	iny
	lda	(OutData),y		; High byte of OutData->ccount
	sta	(ptr1),y
	jmp	MainLoop		; Done

; Check for an octal digit

CheckOctal:
	cmp	#'o'
	bne	CheckPointer

; Integer in octal representation

	jsr	GetSignedArg	   	; Get argument as a long
       	ldy	AltForm		   	; Alternative form?
	beq	@Oct1  		   	; Jump if no
	pha    	       		   	; Save low byte of value
	stx	tmp1
	ora	tmp1
	ora	sreg
	ora	sreg+1
	ora	Prec
	ora	Prec+1 		   	; Check if value or Prec != 0
	beq	@Oct1
	lda	#'0'
	jsr	PutBuf
	pla	       		   	; Restore low byte

@Oct1:	ldy    	#8     		   	; Load base
	jsr	ltoa		   	; Push arguments, call _ltoa
    	jmp	HaveArg

; Check for a pointer specifier (%p)

CheckPointer:
	cmp	#'p'
	bne	CheckString

; It's a pointer. Use %#x conversion

	ldx	#0
	stx	IsLong			; IsLong = 0;
	inx
       	stx	AltForm			; AltForm = 1;
	lda	#'x'
	bne	IsHex			; Branch always

; Check for a string specifier (%s)

CheckString:
	cmp	#'s'
	bne	CheckUnsigned

; It's a string

	jsr	GetIntArg		; Get 16bit argument
	sta	Str
	stx	Str+1
	jmp	HaveArg

; Check for an unsigned integer (%u)

CheckUnsigned:
	cmp	#'u'
	bne	CheckHex

; It's an unsigned integer

	jsr	GetUnsignedArg		; Get argument as unsigned long
       	ldy    	#10    			; Load base
       	jsr    	ultoa			; Push arguments, call _ultoa
    	jmp	HaveArg

; Check for a hexadecimal integer (%x)

CheckHex:
	cmp	#'x'
	beq	IsHex
	cmp	#'X'
	bne	UnknownFormat

; Hexadecimal integer

IsHex:	pha	       			; Save the format spec
	lda	AltForm
	beq	@L1
	lda	#'0'
	jsr	PutBuf
	lda	#'X'
	jsr	PutBuf

@L1: 	jsr	GetUnsignedArg		; Get argument as an unsigned long
       	ldy    	#16    			; Load base
       	jsr    	ultoa			; Push arguments, call _ultoa

	pla	       			; Get the format spec
	cmp	#'x'   			; Lower case?
	bne	@L2
	lda	Str
	ldx	Str+1
	jsr	_strlower		; Make characters lower case
@L2:	jmp	HaveArg

; Unknown format character, skip it

UnknownFormat:
	jmp	MainLoop

; We have the argument, do argument string formatting

HaveArg:

; ArgLen = strlen (Str);

	lda	Str
	ldx	Str+1
	jsr	_strlen			; Get length of argument
	sta	ArgLen
	stx	ArgLen+1

; if (Prec && Prec < ArgLen) ArgLen = Prec;

	lda	Prec
	ora	Prec+1
	beq	@L1
	ldx    	Prec
	cpx	ArgLen
	lda	Prec+1
	tay
	sbc	ArgLen+1
        bcs	@L1
	stx	ArgLen
	sty	ArgLen+1

;  if (Width > ArgLen) {
;      Width -= ArgLen;  		/* padcount */
;  } else {
;      Width = 0;
;  }
; Since width is used as a counter below, calculate -(width+1)

@L1:	sec
	lda	Width
	sbc	ArgLen
	tax
	lda	Width+1
	sbc	ArgLen+1
	bcs	@L2
	lda	#0
	tax
@L2:	eor	#$FF
	sta	Width+1
	txa
	eor	#$FF
	sta	Width

;  /* Do padding on the left side if needed */
;  if (!leftjust) {
;      /* argument right justified */
;      while (width) {
; 	  fout (d, &padchar, 1);
; 	  --width;
;      }
;  }

    	lda	LeftJust
    	bne	@L3
	jsr	OutputPadding

; Output the argument itself

@L3:	jsr	OutputArg

;  /* Output right padding bytes if needed */
;  if (leftjust) {
;      /* argument left justified */
;      while (width) {
; 	  fout (d, &padchar, 1);
; 	  --width;
;      }
;  }

	lda	LeftJust
	beq	@L4
	jsr	OutputPadding

; Done, parse next chars from format string

@L4:	jmp	MainLoop


; ----------------------------------------------------------------------------
; Local data (all static)

.bss

; Save area for the zero page registers
RegSave:	.res  	6

; One character argument for OutFunc
CharArg:	.byte 	0

; Format variables
FormatVars:
LeftJust:  	.byte 	0
AddSign:   	.byte 	0
AddBlank:  	.byte 	0
AltForm:   	.byte 	0
PadChar:   	.byte 	0
Width:	   	.word 	0
Prec:	   	.word 	0
IsLong:	   	.byte 	0
Leader:	   	.byte 	0
BufIdx:		.byte	0	; Argument string pointer
FormatVarSize	= * - FormatVars

; Argument buffer and pointer
Buf:		.res  	20
Str: 		.word 	0
ArgLen:		.res	2

.data

; Stuff from OutData. Is used as a vector and must be aligned
CallOutFunc:	jmp	$0000