common/xprintf.S

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