git.sur5r.net Git - cc65/blob - libsrc/common/vsnprintf.s

   1 ;
   2 ; int __fastcall__ vsnprintf (char* Buf, size_t size, const char* Format, va_list ap);
   3 ;
   4 ; 2009-09-26, Ullrich von Bassewitz
   5 ; 2015-07-17, Greg King
   6 ;
   7
   8         .export         _vsnprintf, vsnprintf
   9         .import         ldaxysp, popax, incsp2, incsp6
  10         .import         _memcpy, __printf
  11         .importzp       sp, ptr1
  12
  13         .include        "errno.inc"
  14
  15         .macpack        generic
  16
  17 .data
  18
  19 ; ----------------------------------------------------------------------------
  20 ;
  21 ; Static data for the _vsnprintf routine
  22 ;
  23
  24 outdesc:                        ; Static outdesc structure
  25 ccount: .word   0               ; ccount
  26 func:   .word   out             ; Output function pointer
  27 bufptr: .word   0               ; ptr
  28 bufsize:.word   0               ; Buffer size
  29
  30 .code
  31
  32 ; ----------------------------------------------------------------------------
  33 ; vsprintf - formatted output into a buffer
  34 ;
  35 ; int __fastcall__ vsnprintf (char* buf, size_t size, const char* format, va_list ap);
  36 ;
  37
  38 _vsnprintf:
  39         pha                     ; Save ap
  40         txa
  41         pha
  42
  43 ; Setup the outdesc structure. This is also an additional entry point for
  44 ; vsprintf with ap on stack
  45
  46 vsnprintf:
  47         lda     #0
  48         sta     ccount+0
  49         sta     ccount+1        ; Clear ccount
  50
  51 ; Get the size parameter and replace it by a pointer to outdesc. This is to
  52 ; build a stack frame for the call to _printf. The size must not be greater
  53 ; than INT_MAX because the return type is int. If the size is zero,
  54 ; then nothing will be written into the buffer; but, the arguments still will
  55 ; be formatted and counted.
  56
  57         ldy     #2
  58         lda     (sp),y
  59         sta     ptr1
  60
  61         lda     #<outdesc
  62         sta     (sp),y
  63
  64         iny
  65         lda     (sp),y
  66         bmi     L9              ; More than $7FFF
  67         sta     ptr1+1
  68
  69         lda     #>outdesc
  70         sta     (sp),y
  71
  72 ; Write size-1 to outdesc.uns.  It will be -1 if there is no buffer.
  73
  74         ldy     ptr1+1
  75         ldx     ptr1
  76         bne     L1
  77         dey
  78 L1:     dex
  79         stx     bufsize+0
  80         sty     bufsize+1
  81
  82 ; Copy buf to the outdesc.ptr
  83
  84         ldy     #5
  85         jsr     ldaxysp
  86         sta     bufptr+0
  87         stx     bufptr+1
  88
  89 ; There must be a buffer if its size is non-zero.
  90
  91         bit     bufsize+1
  92         bmi     L5
  93         ora     bufptr+1
  94         bze     L0              ; The pointer shouldn't be NULL
  95
  96 ; Restore ap and call _printf
  97
  98 L5:     pla
  99         tax
 100         pla
 101         jsr     __printf
 102
 103 ; Terminate the string if there is a buffer.  The last char. is at either
 104 ; bufptr+bufsize or bufptr+ccount, whichever is smaller.
 105
 106         ldx     bufsize+1
 107         bmi     L4              ; -1 -- No buffer
 108         lda     bufsize+0
 109         cpx     ccount+1
 110         bne     L2
 111         cmp     ccount+0
 112 L2:     bcc     L3
 113         lda     ccount+0
 114         ldx     ccount+1
 115         clc
 116 L3:     adc     bufptr+0
 117         sta     ptr1
 118         txa
 119         adc     bufptr+1
 120         sta     ptr1+1
 121
 122         lda     #0
 123         tay
 124         sta     (ptr1),y
 125
 126 ; Return the number of bytes written and drop buf
 127
 128 L4:     lda     ccount+0
 129         ldx     ccount+1
 130         jmp     incsp2
 131
 132 ; Bail out if size is too high.
 133
 134 L9:     ldy     #ERANGE
 135         .byte   $2C             ;(bit $xxxx)
 136
 137 ; NULL buffer pointers usually are invalid.
 138
 139 L0:     ldy     #EINVAL
 140         pla                     ; Drop ap
 141         pla
 142         tya
 143         jsr     __directerrno   ; Return -1
 144         jmp     incsp6          ; Drop parameters
 145
 146
 147 ; ----------------------------------------------------------------------------
 148 ; Callback routine used for the actual output.
 149 ;
 150 ; static void __cdecl__ out (struct outdesc* d, const char* buf, unsigned count)
 151 ; /* Routine used for writing */
 152 ;
 153 ; Since we know, we're called with a pointer to our static outdesc structure,
 154 ; we don't need the pointer passed on the stack.
 155
 156 out:
 157
 158 ; Calculate the space left in the buffer. If no space is left, don't copy
 159 ; any characters
 160
 161         lda     bufsize+0               ; Low byte of buffer size
 162         sec
 163         sbc     ccount+0                ; Low byte of bytes already written
 164         sta     ptr1
 165         lda     bufsize+1
 166         bmi     @L9                     ; -1 -- No buffer
 167         sbc     ccount+1
 168         sta     ptr1+1
 169         bcs     @L0                     ; Branch if space left
 170 @L9:    lda     #$0000
 171         sta     ptr1
 172         sta     ptr1+1                  ; No space left
 173
 174 ; Replace the pointer to d by a pointer to the write position in the buffer
 175 ; for the call to memcpy that follows.
 176
 177 @L0:    lda     bufptr+0
 178         clc
 179         adc     ccount+0
 180         ldy     #4
 181         sta     (sp),y
 182
 183         lda     bufptr+1
 184         adc     ccount+1
 185         iny
 186         sta     (sp),y
 187
 188 ; Get Count from stack
 189
 190         jsr     popax
 191
 192 ; outdesc.ccount += Count;
 193
 194         pha
 195         clc
 196         adc     ccount+0
 197         sta     ccount+0
 198         txa
 199         adc     ccount+1
 200         sta     ccount+1
 201         pla
 202
 203 ; if (Count > Left) Count = Left;
 204
 205         cpx     ptr1+1
 206         bne     @L1
 207         cmp     ptr1
 208 @L1:    bcc     @L2
 209         lda     ptr1
 210         ldx     ptr1+1
 211
 212 ; Jump to memcpy, which will cleanup the stack and return to the caller
 213
 214 @L2:    jmp     _memcpy
 215
 216
 217