flight/libraries/printf2.c

   1 /*******************************************************************************
   2    Copyright 2001, 2002 Georges Menie (<URL snipped>)
   3    This program is free software; you can redistribute it and/or modify
   4    it under the terms of the GNU Lesser General Public License as published by
   5    the Free Software Foundation; either version 2 of the License, or
   6    (at your option) any later version.
   7    This program is distributed in the hope that it will be useful,
   8    but WITHOUT ANY WARRANTY; without even the implied warranty of
   9    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  10    GNU Lesser General Public License for more details.
  11    You should have received a copy of the GNU Lesser General Public License
  12    along with this program; if not, write to the Free Software
  13    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  14  */
  15
  16 /*******************************************************************************
  17    putchar is the only external dependency for this file,
  18    if you have a working putchar, just remove the following
  19    define. If the function should be called something else,
  20    replace outbyte(c) by your own function call.
  21  */
  22 // *******************************************************************************
  23 // Updated by Daniel D Miller.  Changes to the original Menie code are
  24 // Copyright 2009-2012 Daniel D Miller
  25 // All such changes are distributed under the same license as the original,
  26 // as described above.
  27 // 11/06/09 - adding floating-point support
  28 // 03/19/10 - pad fractional portion of floating-point number with 0s
  29 // 03/30/10 - document the \% bug
  30 // 07/20/10 - Fix a round-off bug in floating-point conversions
  31 // ( 0.99 with %.1f did not round to 1.0 )
  32 // 10/25/11 - Add support for %+ format (always show + on positive numbers)
  33 // 01/19/12 - fix handling of %f with no decimal; it was defaulting to 0
  34 // decimal places, rather than printf's 6.
  35 // *******************************************************************************
  36 // BUGS
  37 // If '%' is included in a format string, in the form \% with the intent
  38 // of including the percent sign in the output string, this function will
  39 // mis-handle the data entirely!!
  40 // Essentially, it will just discard the character following the percent sign.
  41 // This bug is not easy to fix in the existing code;
  42 // for now, I'll just try to remember to use %% instead of \% ...
  43 // *******************************************************************************
  44
  45 // lint -esym(752, debug_output)
  46 // lint -esym(766, stdio.h)
  47
  48 // #define  TEST_PRINTF    1
  49
  50 #include <pios.h>
  51
  52 static uint use_leading_plus = 0;
  53
  54 /* based on a example-code from Keil for CS G++ */
  55
  56 /* for caddr_t (typedef char * caddr_t;) */
  57 #include <sys/types.h>
  58
  59 // * NEWLIB STUBS *//
  60 #include <stdlib.h>
  61 #include <sys/unistd.h>
  62 #include <sys/stat.h>
  63 #include <sys/times.h>
  64 #include <errno.h>
  65
  66 /*==============================================================================
  67  * Environment variables.
  68  * A pointer to a list of environment variables and their values. For a minimal
  69  * environment, this empty list is adequate:
  70  */
  71 char *__env[1] = { 0 };
  72 char * *environ = __env;
  73
  74 /*==============================================================================
  75  * Close a file.
  76  */
  77 int _close(__attribute__((unused)) int file)
  78 {
  79     return -1;
  80 }
  81
  82 /*==============================================================================
  83  * Transfer control to a new process.
  84  */
  85 int _execve(__attribute__((unused)) char *name, __attribute__((unused)) char * *argv, __attribute__((unused)) char * *env)
  86 {
  87     errno = ENOMEM;
  88     return -1;
  89 }
  90
  91 /*==============================================================================
  92  * Exit a program without cleaning up files.
  93  */
  94 void _exit(__attribute__((unused)) int code)
  95 {
  96     /* Should we force a system reset? */
  97     while (1) {
  98         ;
  99     }
 100 }
 101
 102 /*==============================================================================
 103  * Create a new process.
 104  */
 105 int _fork(void)
 106 {
 107     errno = EAGAIN;
 108     return -1;
 109 }
 110
 111 /*==============================================================================
 112  * Status of an open file.
 113  */
 114 int _fstat(__attribute__((unused)) int file, struct stat *st)
 115 {
 116     st->st_mode = S_IFCHR;
 117     return 0;
 118 }
 119
 120 /*==============================================================================
 121  * Process-ID
 122  */
 123 int _getpid(void)
 124 {
 125     return 1;
 126 }
 127
 128 /*==============================================================================
 129  * Query whether output stream is a terminal.
 130  */
 131 int _isatty(__attribute__((unused)) int file)
 132 {
 133     return 1;
 134 }
 135
 136 /*==============================================================================
 137  * Send a signal.
 138  */
 139 int _kill(__attribute__((unused)) int pid, __attribute__((unused)) int sig)
 140 {
 141     errno = EINVAL;
 142     return -1;
 143 }
 144
 145 /*==============================================================================
 146  * Establish a new name for an existing file.
 147  */
 148 int _link(__attribute__((unused)) char *old, __attribute__((unused)) char *new)
 149 {
 150     errno = EMLINK;
 151     return -1;
 152 }
 153
 154 /*==============================================================================
 155  * Set position in a file.
 156  */
 157 int _lseek(__attribute__((unused)) int file, __attribute__((unused)) int ptr, __attribute__((unused)) int dir)
 158 {
 159     return 0;
 160 }
 161
 162 /*==============================================================================
 163  * Open a file.
 164  */
 165 int _open(__attribute__((unused)) const char *name, __attribute__((unused)) int flags, __attribute__((unused)) int mode)
 166 {
 167     return -1;
 168 }
 169
 170 /*==============================================================================
 171  * Read from a file.
 172  */
 173 int _read(__attribute__((unused)) int file, __attribute__((unused)) char *ptr, __attribute__((unused)) int len)
 174 {
 175     return 0;
 176 }
 177
 178 /*==============================================================================
 179  * Write to a file. libc subroutines will use this system routine for output to
 180  * all files, including stdout�so if you need to generate any output, for
 181  * example to a serial port for debugging, you should make your minimal write
 182  * capable of doing this.
 183  */
 184 int _write_r(__attribute__((unused)) void *reent, __attribute__((unused)) int file, __attribute__((unused)) char *ptr, __attribute__((unused)) int len)
 185 {
 186     return 0;
 187 }
 188
 189 /*==============================================================================
 190  * Increase program data space. As malloc and related functions depend on this,
 191  * it is useful to have a working implementation. The following suffices for a
 192  * standalone system; it exploits the symbol _end automatically defined by the
 193  * GNU linker.
 194  */
 195 caddr_t _sbrk(int incr)
 196 {
 197     extern char _end; /* Defined by the linker */
 198     static char *heap_end;
 199     char *prev_heap_end;
 200     char *stack_ptr;
 201
 202     if (heap_end == 0) {
 203         heap_end = &_end;
 204     }
 205
 206     prev_heap_end = heap_end;
 207     asm volatile ("MRS %0, msp" : "=r" (stack_ptr));
 208     if (heap_end + incr > stack_ptr) {
 209         _write_r((void *)0, 1, "Heap and stack collision\n", 25);
 210         _exit(1);
 211     }
 212
 213     heap_end += incr;
 214     return (caddr_t)prev_heap_end;
 215 }
 216
 217 /*==============================================================================
 218  * Status of a file (by name).
 219  */
 220 int _stat(__attribute__((unused)) char *file, struct stat *st)
 221 {
 222     st->st_mode = S_IFCHR;
 223     return 0;
 224 }
 225
 226 /*==============================================================================
 227  * Timing information for current process.
 228  */
 229 int _times(__attribute__((unused)) struct tms *buf)
 230 {
 231     return -1;
 232 }
 233
 234 /*==============================================================================
 235  * Remove a file's directory entry.
 236  */
 237 int _unlink(__attribute__((unused)) char *name)
 238 {
 239     errno = ENOENT;
 240     return -1;
 241 }
 242
 243 /*==============================================================================
 244  * Wait for a child process.
 245  */
 246 int _wait(__attribute__((unused)) int *status)
 247 {
 248     errno = ECHILD;
 249     return -1;
 250 }
 251 // * NEWLIB STUBS *//
 252
 253
 254 // ****************************************************************************
 255 static void printchar(char * *str, int c)
 256 {
 257     if (str) {
 258         **str = c;
 259         ++(*str);
 260     }
 261 #ifdef TEST_PRINTF
 262     else {
 263         extern int putchar(int c);
 264         (void)putchar(c);
 265     }
 266 #endif
 267 }
 268
 269 // ****************************************************************************
 270 static uint my_strlen(char *str)
 271 {
 272     if (str == 0) {
 273         return 0;
 274     }
 275     uint slen = 0;
 276     while (*str != 0) {
 277         slen++;
 278         str++;
 279     }
 280     return slen;
 281 }
 282
 283 // ****************************************************************************
 284 // This version returns the length of the output string.
 285 // It is more useful when implementing a walking-string function.
 286 // ****************************************************************************
 287 static const double round_nums[8] = {
 288     0.5L,
 289     0.05L,
 290     0.005L,
 291     0.0005L,
 292     0.00005L,
 293     0.000005L,
 294     0.0000005L,
 295     0.00000005L
 296 };
 297
 298 static unsigned dbl2stri(char *outbfr, double dbl, unsigned dec_digits)
 299 {
 300     static char local_bfr[128];
 301     char *output = (outbfr == 0) ? local_bfr : outbfr;
 302
 303     // *******************************************
 304     // extract negative info
 305     // *******************************************
 306     if (dbl < 0.0L) {
 307         *output++ = '-';
 308         dbl *= -1.0L;
 309     } else {
 310         if (use_leading_plus) {
 311             *output++ = '+';
 312         }
 313     }
 314
 315     // handling rounding by adding .5LSB to the floating-point data
 316     if (dec_digits < 8) {
 317         dbl += round_nums[dec_digits];
 318     }
 319
 320     // **************************************************************************
 321     // construct fractional multiplier for specified number of digits.
 322     // **************************************************************************
 323     uint mult = 1;
 324     uint idx;
 325     for (idx = 0; idx < dec_digits; idx++) {
 326         mult *= 10;
 327     }
 328
 329     // printf("mult=%u\n", mult) ;
 330     uint wholeNum   = (uint)dbl;
 331     uint decimalNum = (uint)((dbl - wholeNum) * mult);
 332
 333     // *******************************************
 334     // convert integer portion
 335     // *******************************************
 336     char tbfr[40];
 337     idx = 0;
 338     while (wholeNum != 0) {
 339         tbfr[idx++] = '0' + (wholeNum % 10);
 340         wholeNum   /= 10;
 341     }
 342     // printf("%.3f: whole=%s, dec=%d\n", dbl, tbfr, decimalNum) ;
 343     if (idx == 0) {
 344         *output++ = '0';
 345     } else {
 346         while (idx > 0) {
 347             *output++ = tbfr[idx - 1]; // lint !e771
 348             idx--;
 349         }
 350     }
 351     if (dec_digits > 0) {
 352         *output++ = '.';
 353
 354         // *******************************************
 355         // convert fractional portion
 356         // *******************************************
 357         idx = 0;
 358         while (decimalNum != 0) {
 359             tbfr[idx++] = '0' + (decimalNum % 10);
 360             decimalNum /= 10;
 361         }
 362         // pad the decimal portion with 0s as necessary;
 363         // We wouldn't want to report 3.093 as 3.93, would we??
 364         while (idx < dec_digits) {
 365             tbfr[idx++] = '0';
 366         }
 367         // printf("decimal=%s\n", tbfr) ;
 368         if (idx == 0) {
 369             *output++ = '0';
 370         } else {
 371             while (idx > 0) {
 372                 *output++ = tbfr[idx - 1];
 373                 idx--;
 374             }
 375         }
 376     }
 377     *output = 0;
 378
 379     // prepare output
 380     output  = (outbfr == 0) ? local_bfr : outbfr;
 381     return my_strlen(output);
 382 }
 383
 384 // ****************************************************************************
 385 #define  PAD_RIGHT 1
 386 #define  PAD_ZERO  2
 387
 388 static int prints(char * *out, const char *string, int width, int pad)
 389 {
 390     register int pc = 0, padchar = ' ';
 391
 392     if (width > 0) {
 393         int len = 0;
 394         const char *ptr;
 395         for (ptr = string; *ptr; ++ptr) {
 396             ++len;
 397         }
 398         if (len >= width) {
 399             width = 0;
 400         } else {
 401             width -= len;
 402         }
 403         if (pad & PAD_ZERO) {
 404             padchar = '0';
 405         }
 406     }
 407     if (!(pad & PAD_RIGHT)) {
 408         for (; width > 0; --width) {
 409             printchar(out, padchar);
 410             ++pc;
 411         }
 412     }
 413     for (; *string; ++string) {
 414         printchar(out, *string);
 415         ++pc;
 416     }
 417     for (; width > 0; --width) {
 418         printchar(out, padchar);
 419         ++pc;
 420     }
 421     return pc;
 422 }
 423
 424 // ****************************************************************************
 425 /* the following should be enough for 32 bit int */
 426 #define PRINT_BUF_LEN 12
 427 static int printi(char * *out, int i, int b, int sg, int width, int pad, int letbase)
 428 {
 429     char print_buf[PRINT_BUF_LEN];
 430     char *s;
 431     int t, neg = 0, pc = 0;
 432     unsigned u = (unsigned)i;
 433
 434     if (i == 0) {
 435         print_buf[0] = '0';
 436         print_buf[1] = '\0';
 437         return prints(out, print_buf, width, pad);
 438     }
 439     if (sg && b == 10 && i < 0) {
 440         neg = 1;
 441         u   = (unsigned)-i;
 442     }
 443     // make sure print_buf is NULL-term
 444     s  = print_buf + PRINT_BUF_LEN - 1;
 445     *s = '\0';
 446
 447
 448     while (u) {
 449         t = u % b; // lint !e573  Warning 573: Signed-unsigned mix with divide
 450         if (t >= 10) {
 451             t += letbase - '0' - 10;
 452         }
 453         *--s = t + '0';
 454         u   /= b;  // lint !e573  Warning 573: Signed-unsigned mix with divide
 455     }
 456     if (neg) {
 457         if (width && (pad & PAD_ZERO)) {
 458             printchar(out, '-');
 459             ++pc;
 460             --width;
 461         } else {
 462             *--s = '-';
 463         }
 464     } else {
 465         if (use_leading_plus) {
 466             *--s = '+';
 467         }
 468     }
 469     return pc + prints(out, s, width, pad);
 470 }
 471
 472 // ****************************************************************************
 473 static int print(char * *out, int *varg)
 474 {
 475     int post_decimal;
 476     int width, pad;
 477     unsigned dec_width = 6;
 478     int pc = 0;
 479     char *format = (char *)(*varg++);
 480     char scr[2];
 481
 482     use_leading_plus = 0; // start out with this clear
 483     for (; *format != 0; ++format) {
 484         if (*format == '%') {
 485             dec_width = 6;
 486             ++format;
 487             width     = pad = 0;
 488             if (*format == '\0') {
 489                 break;
 490             }
 491             if (*format == '%') {
 492                 goto out_lbl;
 493             }
 494             if (*format == '-') {
 495                 ++format;
 496                 pad = PAD_RIGHT;
 497             }
 498             if (*format == '+') {
 499                 ++format;
 500                 use_leading_plus = 1;
 501             }
 502             while (*format == '0') {
 503                 ++format;
 504                 pad |= PAD_ZERO;
 505             }
 506             post_decimal = 0;
 507             if (*format == '.' ||
 508                 (*format >= '0' && *format <= '9')) {
 509                 while (1) {
 510                     if (*format == '.') {
 511                         post_decimal = 1;
 512                         dec_width    = 0;
 513                         format++;
 514                     } else if ((*format >= '0' && *format <= '9')) {
 515                         if (post_decimal) {
 516                             dec_width *= 10;
 517                             dec_width += *format - '0';
 518                         } else {
 519                             width *= 10;
 520                             width += *format - '0';
 521                         }
 522                         format++;
 523                     } else {
 524                         break;
 525                     }
 526                 }
 527             }
 528             if (*format == 'l') {
 529                 ++format;
 530             }
 531             switch (*format) {
 532             case 's':
 533             {
 534                 // char *s = *((char **) varg++);   //lint !e740
 535                 char *s = (char *)*varg++; // lint !e740 !e826  convert to double pointer
 536                 pc += prints(out, s ? s : "(null)", width, pad);
 537                 use_leading_plus = 0; // reset this flag after printing one value
 538             }
 539             break;
 540             case 'd':
 541                 pc += printi(out, *varg++, 10, 1, width, pad, 'a');
 542                 use_leading_plus = 0; // reset this flag after printing one value
 543                 break;
 544             case 'x':
 545                 pc += printi(out, *varg++, 16, 0, width, pad, 'a');
 546                 use_leading_plus = 0; // reset this flag after printing one value
 547                 break;
 548             case 'X':
 549                 pc += printi(out, *varg++, 16, 0, width, pad, 'A');
 550                 use_leading_plus = 0; // reset this flag after printing one value
 551                 break;
 552             case 'u':
 553                 pc += printi(out, *varg++, 10, 0, width, pad, 'a');
 554                 use_leading_plus = 0; // reset this flag after printing one value
 555                 break;
 556             case 'c':
 557                 /* char are converted to int then pushed on the stack */
 558                 scr[0] = *varg++;
 559                 scr[1] = '\0';
 560                 pc    += prints(out, scr, width, pad);
 561                 use_leading_plus = 0; // reset this flag after printing one value
 562                 break;
 563
 564             case 'f':
 565             {
 566                 // http://wiki.debian.org/ArmEabiPort#Structpackingandalignment
 567                 // Stack alignment
 568                 //
 569                 // The ARM EABI requires 8-byte stack alignment at public function entry points,
 570                 // compared to the previous 4-byte alignment.
 571 #ifdef USE_NEWLIB
 572                 char *cptr = (char *)varg; // lint !e740 !e826  convert to double pointer
 573                 uint caddr = (uint)cptr;
 574                 if ((caddr & 0xF) != 0) {
 575                     cptr += 4;
 576                 }
 577                 double *dblptr = (double *)cptr; // lint !e740 !e826  convert to double pointer
 578 #else
 579                 double *dblptr = (double *)varg; // lint !e740 !e826  convert to double pointer
 580 #endif
 581                 double dbl     = *dblptr++;   // increment double pointer
 582                 varg = (int *)dblptr; // lint !e740  copy updated pointer back to base pointer
 583                 char bfr[81];
 584                 // unsigned slen =
 585                 dbl2stri(bfr, dbl, dec_width);
 586                 // stuff_talkf("[%s], width=%u, dec_width=%u\n", bfr, width, dec_width) ;
 587                 pc += prints(out, bfr, width, pad);
 588                 use_leading_plus = 0; // reset this flag after printing one value
 589             }
 590             break;
 591
 592             default:
 593                 printchar(out, '%');
 594                 printchar(out, *format);
 595                 use_leading_plus = 0; // reset this flag after printing one value
 596                 break;
 597             }
 598         } else {
 599             // if (*format == '\\') {
 600             //
 601             // } else
 602 out_lbl:
 603             printchar(out, *format);
 604             ++pc;
 605         }
 606     } // for each char in format string
 607     if (out) { // lint !e850
 608         **out = '\0';
 609     }
 610     return pc;
 611 }
 612
 613 // ****************************************************************************
 614 int stringf(char *out, const char *format, ...)
 615 {
 616     // create a pointer into the stack.
 617     // Thematically this should be a void*, since the actual usage of the
 618     // pointer will vary.  However, int* works fine too.
 619     // Either way, the called function will need to re-cast the pointer
 620     // for any type which isn't sizeof(int)
 621     int *varg = (int *)(char *)(&format);
 622
 623     return print(&out, varg);
 624 }
 625
 626 int printf(const char *format, ...)
 627 {
 628     int *varg = (int *)(char *)(&format);
 629
 630     return print(0, varg);
 631 }
 632
 633 int sprintf(char *out, const char *format, ...)
 634 {
 635     int *varg = (int *)(char *)(&format);
 636
 637     return print(&out, varg);
 638 }
 639
 640 /**
 641  * @}
 642  */