tmk_core/common/avr/xprintf.S

   1 ;---------------------------------------------------------------------------;
   2 ; Extended itoa, puts, printf and atoi                     (C)ChaN, 2011
   3 ;---------------------------------------------------------------------------;
   4
   5                                 // Base size is 152 bytes
   6 #define CR_CRLF         0       // Convert \n to \r\n (+10 bytes)
   7 #define USE_XPRINTF     1       // Enable xprintf function (+194 bytes)
   8 #define USE_XSPRINTF    0       // Add xsprintf function (+78 bytes)
   9 #define USE_XFPRINTF    0       // Add xfprintf function (+54 bytes)
  10 #define USE_XATOI       0       // Enable xatoi function (+182 bytes)
  11
  12
  13 #if FLASHEND > 0x1FFFF
  14 #error xitoa module does not support 256K devices
  15 #endif
  16
  17 .nolist
  18 #include <avr/io.h>     // Include device specific definitions.
  19 .list
  20
  21 #ifdef SPM_PAGESIZE     // Recent devices have "lpm Rd,Z+" and "movw".
  22 .macro  _LPMI   reg
  23         lpm     \reg, Z+
  24 .endm
  25 .macro  _MOVW   dh,dl, sh,sl
  26         movw    \dl, \sl
  27 .endm
  28 #else                   // Earlier devices do not have "lpm Rd,Z+" nor "movw".
  29 .macro  _LPMI   reg
  30         lpm
  31         mov     \reg, r0
  32         adiw    ZL, 1
  33 .endm
  34 .macro  _MOVW   dh,dl, sh,sl
  35         mov     \dl, \sl
  36         mov     \dh, \sh
  37 .endm
  38 #endif
  39
  40
  41
  42 ;---------------------------------------------------------------------------
  43 ; Stub function to forward to user output function
  44 ;
  45 ;Prototype: void xputc (char chr        // a character to be output
  46 ;                       );
  47 ;Size: 12/12 words
  48
  49 .section .bss
  50 .global xfunc_out       ; xfunc_out must be initialized before using this module.
  51 xfunc_out:      .ds.w   1
  52 .section .text
  53
  54
  55 .func xputc
  56 .global xputc
  57 xputc:
  58 #if CR_CRLF
  59         cpi     r24, 10         ;LF --> CRLF
  60         brne    1f              ;
  61         ldi     r24, 13         ;
  62         rcall   1f              ;
  63         ldi     r24, 10         ;/
  64 1:
  65 #endif
  66         push    ZH
  67         push    ZL
  68         lds     ZL, xfunc_out+0 ;Pointer to the registered output function.
  69         lds     ZH, xfunc_out+1 ;/
  70         sbiw    ZL, 0           ;Skip if null
  71         breq    2f              ;/
  72         icall
  73 2:      pop     ZL
  74         pop     ZH
  75         ret
  76 .endfunc
  77
  78
  79
  80 ;---------------------------------------------------------------------------
  81 ; Direct ROM string output
  82 ;
  83 ;Prototype: void xputs (const char *str_p // rom string to be output
  84 ;                       );
  85
  86 .func xputs
  87 .global xputs
  88 xputs:
  89         _MOVW   ZH,ZL, r25,r24  ; Z = pointer to rom string
  90 1:      _LPMI   r24
  91         cpi     r24, 0
  92         breq    2f
  93         rcall   xputc
  94         rjmp    1b
  95 2:      ret
  96 .endfunc
  97
  98
  99 ;---------------------------------------------------------------------------
 100 ; Extended direct numeral string output (32bit version)
 101 ;
 102 ;Prototype: void xitoa (long value,     // value to be output
 103 ;                       char radix,     // radix
 104 ;                       char width);    // minimum width
 105 ;
 106
 107 .func xitoa
 108 .global xitoa
 109 xitoa:
 110                                 ;r25:r22 = value, r20 = base, r18 = digits
 111         clr     r31             ;r31 = stack level
 112         ldi     r30, ' '        ;r30 = sign
 113         ldi     r19, ' '        ;r19 = filler
 114         sbrs    r20, 7          ;When base indicates signd format and the value
 115         rjmp    0f              ;is minus, add a '-'.
 116         neg     r20             ;
 117         sbrs    r25, 7          ;
 118         rjmp    0f              ;
 119         ldi     r30, '-'        ;
 120         com     r22             ;
 121         com     r23             ;
 122         com     r24             ;
 123         com     r25             ;
 124         adc     r22, r1         ;
 125         adc     r23, r1         ;
 126         adc     r24, r1         ;
 127         adc     r25, r1         ;/
 128 0:      sbrs    r18, 7          ;When digits indicates zero filled,
 129         rjmp    1f              ;filler is '0'.
 130         neg     r18             ;
 131         ldi     r19, '0'        ;/
 132                                 ;----- string conversion loop
 133 1:      ldi     r21, 32         ;r26 = r25:r22 % r20
 134         clr     r26             ;r25:r22 /= r20
 135 2:      lsl     r22             ;
 136         rol     r23             ;
 137         rol     r24             ;
 138         rol     r25             ;
 139         rol     r26             ;
 140         cp      r26, r20        ;
 141         brcs    3f              ;
 142         sub     r26, r20        ;
 143         inc     r22             ;
 144 3:      dec     r21             ;
 145         brne    2b              ;/
 146         cpi     r26, 10         ;r26 is a numeral digit '0'-'F'
 147         brcs    4f              ;
 148         subi    r26, -7         ;
 149 4:      subi    r26, -'0'       ;/
 150         push    r26             ;Stack it
 151         inc     r31             ;/
 152         cp      r22, r1         ;Repeat until r25:r22 gets zero
 153         cpc     r23, r1         ;
 154         cpc     r24, r1         ;
 155         cpc     r25, r1         ;
 156         brne    1b              ;/
 157
 158         cpi     r30, '-'        ;Minus sign if needed
 159         brne    5f              ;
 160         push    r30             ;
 161         inc     r31             ;/
 162 5:      cp      r31, r18        ;Filler
 163         brcc    6f              ;
 164         push    r19             ;
 165         inc     r31             ;
 166         rjmp    5b              ;/
 167
 168 6:      pop     r24             ;Flush stacked digits and exit
 169         rcall   xputc           ;
 170         dec     r31             ;
 171         brne    6b              ;/
 172
 173         ret
 174 .endfunc
 175
 176
 177
 178 ;---------------------------------------------------------------------------;
 179 ; Formatted string output (16/32bit version)
 180 ;
 181 ;Prototype:
 182 ; void __xprintf (const char *format_p, ...);
 183 ; void __xsprintf(char*, const char *format_p, ...);
 184 ; void __xfprintf(void(*func)(char), const char *format_p, ...);
 185 ;
 186
 187 #if USE_XPRINTF
 188
 189 .func xvprintf
 190 xvprintf:
 191         ld      ZL, Y+          ;Z = pointer to format string
 192         ld      ZH, Y+          ;/
 193
 194 0:      _LPMI   r24             ;Get a format char
 195         cpi     r24, 0          ;End of format string?
 196         breq    90f             ;/
 197         cpi     r24, '%'        ;Is format?
 198         breq    20f             ;/
 199 1:      rcall   xputc           ;Put a normal character
 200         rjmp    0b              ;/
 201 90:     ret
 202
 203 20:     ldi     r18, 0          ;r18: digits
 204         clt                     ;T: filler
 205         _LPMI   r21             ;Get flags
 206         cpi     r21, '%'        ;Is a %?
 207         breq    1b              ;/
 208         cpi     r21, '0'        ;Zero filled?
 209         brne    23f             ;
 210         set                     ;/
 211 22:     _LPMI   r21             ;Get width
 212 23:     cpi     r21, '9'+1      ;
 213         brcc    24f             ;
 214         subi    r21, '0'        ;
 215         brcs    90b             ;
 216         lsl     r18             ;
 217         mov     r0, r18         ;
 218         lsl     r18             ;
 219         lsl     r18             ;
 220         add     r18, r0         ;
 221         add     r18, r21        ;
 222         rjmp    22b             ;/
 223
 224 24:     brtc    25f             ;get value (low word)
 225         neg     r18             ;
 226 25:     ld      r24, Y+         ;
 227         ld      r25, Y+         ;/
 228         cpi     r21, 'c'        ;Is type character?
 229         breq    1b              ;/
 230         cpi     r21, 's'        ;Is type RAM string?
 231         breq    50f             ;/
 232         cpi     r21, 'S'        ;Is type ROM string?
 233         breq    60f             ;/
 234         _MOVW   r23,r22,r25,r24 ;r25:r22 = value
 235         clr     r24             ;
 236         clr     r25             ;
 237         clt                     ;/
 238         cpi     r21, 'l'        ;Is long int?
 239         brne    26f             ;
 240         ld      r24, Y+         ;get value (high word)
 241         ld      r25, Y+         ;
 242         set                     ;
 243         _LPMI   r21             ;/
 244 26:     cpi     r21, 'd'        ;Is type signed decimal?
 245         brne    27f             ;/
 246         ldi     r20, -10        ;
 247         brts    40f             ;
 248         sbrs    r23, 7          ;
 249         rjmp    40f             ;
 250         ldi     r24, -1         ;
 251         ldi     r25, -1         ;
 252         rjmp    40f             ;/
 253 27:     cpi     r21, 'u'        ;Is type unsigned decimal?
 254         ldi     r20, 10         ;
 255         breq    40f             ;/
 256         cpi     r21, 'X'        ;Is type hexdecimal?
 257         ldi     r20, 16         ;
 258         breq    40f             ;/
 259         cpi     r21, 'b'        ;Is type binary?
 260         ldi     r20, 2          ;
 261         breq    40f             ;/
 262         ret                     ;abort
 263 40:     push    ZH              ;Output the value
 264         push    ZL              ;
 265         rcall   xitoa           ;
 266 42:     pop     ZL              ;
 267         pop     ZH              ;
 268         rjmp    0b              ;/
 269
 270 50:     push    ZH              ;Put a string on the RAM
 271         push    ZL
 272         _MOVW   ZH,ZL, r25,r24
 273 51:     ld      r24, Z+
 274         cpi     r24, 0
 275         breq    42b
 276         rcall   xputc
 277         rjmp    51b
 278
 279 60:     push    ZH              ;Put a string on the ROM
 280         push    ZL
 281         rcall   xputs
 282         rjmp    42b
 283 .endfunc
 284
 285
 286 .func __xprintf
 287 .global __xprintf
 288 __xprintf:
 289         push    YH
 290         push    YL
 291         in      YL, _SFR_IO_ADDR(SPL)
 292 #ifdef SPH
 293         in      YH, _SFR_IO_ADDR(SPH)
 294 #else
 295         clr     YH
 296 #endif
 297         adiw    YL, 5           ;Y = pointer to arguments
 298         rcall   xvprintf
 299         pop     YL
 300         pop     YH
 301         ret
 302 .endfunc
 303
 304
 305 #if USE_XSPRINTF
 306
 307 .func __xsprintf
 308 putram:
 309         _MOVW   ZH,ZL, r15,r14
 310         st      Z+, r24
 311         _MOVW   r15,r14, ZH,ZL
 312         ret
 313 .global __xsprintf
 314 __xsprintf:
 315         push    YH
 316         push    YL
 317         in      YL, _SFR_IO_ADDR(SPL)
 318 #ifdef SPH
 319         in      YH, _SFR_IO_ADDR(SPH)
 320 #else
 321         clr     YH
 322 #endif
 323         adiw    YL, 5           ;Y = pointer to arguments
 324         lds     ZL, xfunc_out+0 ;Save registered output function
 325         lds     ZH, xfunc_out+1 ;
 326         push    ZL              ;
 327         push    ZH              ;/
 328         ldi     ZL, lo8(pm(putram));Set local output function
 329         ldi     ZH, hi8(pm(putram));
 330         sts     xfunc_out+0, ZL ;
 331         sts     xfunc_out+1, ZH ;/
 332         push    r15             ;Initialize pointer to string buffer
 333         push    r14             ;
 334         ld      r14, Y+         ;
 335         ld      r15, Y+         ;/
 336         rcall   xvprintf
 337         _MOVW   ZH,ZL, r15,r14  ;Terminate string
 338         st      Z, r1           ;
 339         pop     r14             ;
 340         pop     r15             ;/
 341         pop     ZH              ;Restore registered output function
 342         pop     ZL              ;
 343         sts     xfunc_out+0, ZL ;
 344         sts     xfunc_out+1, ZH ;/
 345         pop     YL
 346         pop     YH
 347         ret
 348 .endfunc
 349 #endif
 350
 351
 352 #if USE_XFPRINTF
 353 .func __xfprintf
 354 .global __xfprintf
 355 __xfprintf:
 356         push    YH
 357         push    YL
 358         in      YL, _SFR_IO_ADDR(SPL)
 359 #ifdef SPH
 360         in      YH, _SFR_IO_ADDR(SPH)
 361 #else
 362         clr     YH
 363 #endif
 364         adiw    YL, 5           ;Y = pointer to arguments
 365         lds     ZL, xfunc_out+0 ;Save registered output function
 366         lds     ZH, xfunc_out+1 ;
 367         push    ZL              ;
 368         push    ZH              ;/
 369         ld      ZL, Y+          ;Set output function
 370         ld      ZH, Y+          ;
 371         sts     xfunc_out+0, ZL ;
 372         sts     xfunc_out+1, ZH ;/
 373         rcall   xvprintf
 374         pop     ZH              ;Restore registered output function
 375         pop     ZL              ;
 376         sts     xfunc_out+0, ZL ;
 377         sts     xfunc_out+1, ZH ;/
 378         pop     YL
 379         pop     YH
 380         ret
 381 .endfunc
 382 #endif
 383
 384 #endif
 385
 386
 387
 388 ;---------------------------------------------------------------------------
 389 ; Extended numeral string input
 390 ;
 391 ;Prototype:
 392 ; char xatoi (           /* 1: Successful, 0: Failed */
 393 ;      const char **str, /* pointer to pointer to source string */
 394 ;      long *res         /* result */
 395 ; );
 396 ;
 397
 398
 399 #if USE_XATOI
 400 .func xatoi
 401 .global xatoi
 402 xatoi:
 403         _MOVW   r1, r0, r23, r22
 404         _MOVW   XH, XL, r25, r24
 405         ld      ZL, X+
 406         ld      ZH, X+
 407         clr     r18             ;r21:r18 = 0;
 408         clr     r19             ;
 409         clr     r20             ;
 410         clr     r21             ;/
 411         clt                     ;T = 0;
 412
 413         ldi     r25, 10         ;r25 = 10;
 414         rjmp    41f             ;/
 415 40:     adiw    ZL, 1           ;Z++;
 416 41:     ld      r22, Z          ;r22 = *Z;
 417         cpi     r22, ' '        ;if(r22 == ' ') continue
 418         breq    40b             ;/
 419         brcs    70f             ;if(r22 < ' ') error;
 420         cpi     r22, '-'        ;if(r22 == '-') {
 421         brne    42f             ; T = 1;
 422         set                     ; continue;
 423         rjmp    40b             ;}
 424 42:     cpi     r22, '9'+1      ;if(r22 > '9') error;
 425         brcc    70f             ;/
 426         cpi     r22, '0'        ;if(r22 < '0') error;
 427         brcs    70f             ;/
 428         brne    51f             ;if(r22 > '0') cv_start;
 429         ldi     r25, 8          ;r25 = 8;
 430         adiw    ZL, 1           ;r22 = *(++Z);
 431         ld      r22, Z          ;/
 432         cpi     r22, ' '+1      ;if(r22 <= ' ') exit;
 433         brcs    80f             ;/
 434         cpi     r22, 'b'        ;if(r22 == 'b') {
 435         brne    43f             ; r25 = 2;
 436         ldi     r25, 2          ; cv_start;
 437         rjmp    50f             ;}
 438 43:     cpi     r22, 'x'        ;if(r22 != 'x') error;
 439         brne    51f             ;/
 440         ldi     r25, 16         ;r25 = 16;
 441
 442 50:     adiw    ZL, 1           ;Z++;
 443         ld      r22, Z          ;r22 = *Z;
 444 51:     cpi     r22, ' '+1      ;if(r22 <= ' ') break;
 445         brcs    80f             ;/
 446         cpi     r22, 'a'        ;if(r22 >= 'a') r22 =- 0x20;
 447         brcs    52f             ;
 448         subi    r22, 0x20       ;/
 449 52:     subi    r22, '0'        ;if((r22 -= '0') < 0) error;
 450         brcs    70f             ;/
 451         cpi     r22, 10         ;if(r22 >= 10) {
 452         brcs    53f             ; r22 -= 7;
 453         subi    r22, 7          ; if(r22 < 10)
 454         cpi     r22, 10         ;
 455         brcs    70f             ;}
 456 53:     cp      r22, r25        ;if(r22 >= r25) error;
 457         brcc    70f             ;/
 458 60:     ldi     r24, 33         ;r21:r18 *= r25;
 459         sub     r23, r23        ;
 460 61:     brcc    62f             ;
 461         add     r23, r25        ;
 462 62:     lsr     r23             ;
 463         ror     r21             ;
 464         ror     r20             ;
 465         ror     r19             ;
 466         ror     r18             ;
 467         dec     r24             ;
 468         brne    61b             ;/
 469         add     r18, r22        ;r21:r18 += r22;
 470         adc     r19, r24        ;
 471         adc     r20, r24        ;
 472         adc     r21, r24        ;/
 473         rjmp    50b             ;repeat
 474
 475 70:     ldi     r24, 0
 476         rjmp    81f
 477 80:     ldi     r24, 1
 478 81:     brtc    82f
 479         clr     r22
 480         com     r18
 481         com     r19
 482         com     r20
 483         com     r21
 484         adc     r18, r22
 485         adc     r19, r22
 486         adc     r20, r22
 487         adc     r21, r22
 488 82:     st      -X, ZH
 489         st      -X, ZL
 490         _MOVW   XH, XL, r1, r0
 491         st      X+, r18
 492         st      X+, r19
 493         st      X+, r20
 494         st      X+, r21
 495         clr     r1
 496         ret
 497 .endfunc
 498 #endif
 499
 500