flight/libraries/printf-stdarg.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup PIOS PIOS Core hardware abstraction layer
   4  * @{
   5  *
   6  * @file       printf-stdarg.c
   7  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   8  *             Parts by Thorsten Klose (tk@midibox.org)
   9  * @brief      Formatted print functions
  10  * @see        The GNU Public License (GPL) Version 3
  11  *
  12  *****************************************************************************/
  13 /*
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 3 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful, but
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  21  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  22  * for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License along
  25  * with this program; if not, write to the Free Software Foundation, Inc.,
  26  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  27  */
  28
  29 /*
  30         putchar is the only external dependency for this file,
  31         if you have a working putchar, leave it commented out.
  32         If not, uncomment the define below and
  33         replace outbyte(c) by your own function call.
  34
  35    #define putchar(c) outbyte(c)
  36  */
  37 // #define putchar(c) COMSendChar(c)
  38
  39 #include <pios.h>
  40
  41 static void printchar(char * *str, int c)
  42 {
  43     if (str) {
  44         **str = c;
  45         ++(*str);
  46     }
  47 #ifdef PIOS_COM_DEBUG
  48     else {
  49         PIOS_COM_SendChar(PIOS_COM_DEBUG, c);
  50     }
  51 #endif
  52 }
  53
  54 #define PAD_RIGHT 1
  55 #define PAD_ZERO  2
  56
  57 static int prints(char * *out, const char *string, int width, int pad, int limit)
  58 {
  59     register int pc = limit, padchar = ' ';
  60
  61     if (width > 0) {
  62         register int len = 0;
  63         register const char *ptr;
  64         for (ptr = string; *ptr; ++ptr) {
  65             ++len;
  66         }
  67         if (len >= width) {
  68             width = 0;
  69         } else {
  70             width -= len;
  71         }
  72         if (pad & PAD_ZERO) {
  73             padchar = '0';
  74         }
  75     }
  76     if (!(pad & PAD_RIGHT)) {
  77         for (; width > 0 && pc; --width) {
  78             printchar(out, padchar);
  79             --pc;
  80         }
  81     }
  82     for (; *string && pc; ++string) {
  83         printchar(out, *string);
  84         --pc;
  85     }
  86     for (; width > 0 && pc; --width) {
  87         printchar(out, padchar);
  88         --pc;
  89     }
  90
  91     return limit - pc;
  92 }
  93
  94 /* the following should be enough for 32 bit int */
  95 #define PRINT_BUF_LEN 12
  96
  97 static int printi(char * *out, int i, int b, int sg, int width, int pad, int letbase, int limit)
  98 {
  99     char print_buf[PRINT_BUF_LEN];
 100     register char *s;
 101     register int t, neg = 0, pc = limit;
 102     register unsigned int u = i;
 103
 104     if (i == 0) {
 105         print_buf[0] = '0';
 106         print_buf[1] = '\0';
 107         return prints(out, print_buf, width, pad, limit);
 108     }
 109
 110     if (sg && b == 10 && i < 0) {
 111         neg = 1;
 112         u   = -i;
 113     }
 114
 115     s  = print_buf + PRINT_BUF_LEN - 1;
 116     *s = '\0';
 117
 118     while (u) {
 119         t = u % b;
 120         if (t >= 10) {
 121             t += letbase - '0' - 10;
 122         }
 123         *--s = t + '0';
 124         u   /= b;
 125     }
 126
 127     if (neg && pc) {
 128         if (width && (pad & PAD_ZERO)) {
 129             printchar(out, '-');
 130             --pc;
 131             --width;
 132         } else {
 133             *--s = '-';
 134         }
 135     }
 136
 137     return (limit - pc) + prints(out, s, width, pad, pc);
 138 }
 139
 140 static int print(int limit, char * *out, const char *format, va_list args)
 141 {
 142     register int width, pad;
 143     register int pc = limit;
 144     char scr[2];
 145
 146     for (; *format != 0 && pc; ++format) {
 147         if (*format == '%') {
 148             ++format;
 149             width = pad = 0;
 150             if (*format == '\0') {
 151                 break;
 152             }
 153             if (*format == '%') {
 154                 goto out;
 155             }
 156             if (*format == '-') {
 157                 ++format;
 158                 pad = PAD_RIGHT;
 159             }
 160             while (*format == '0') {
 161                 ++format;
 162                 pad |= PAD_ZERO;
 163             }
 164             for (; *format >= '0' && *format <= '9'; ++format) {
 165                 width *= 10;
 166                 width += *format - '0';
 167             }
 168             if (*format == 's') {
 169                 register char *s = (char *)va_arg(args, int);
 170                 pc -= prints(out, s ? s : "(null)", width, pad, pc);
 171                 continue;
 172             }
 173             if (*format == 'd') {
 174                 pc -= printi(out, va_arg(args, int), 10, 1, width, pad, 'a', pc);
 175                 continue;
 176             }
 177             if (*format == 'x') {
 178                 pc -= printi(out, va_arg(args, int), 16, 0, width, pad, 'a', pc);
 179                 continue;
 180             }
 181             if (*format == 'X') {
 182                 pc -= printi(out, va_arg(args, int), 16, 0, width, pad, 'A', pc);
 183                 continue;
 184             }
 185             if (*format == 'u') {
 186                 pc -= printi(out, va_arg(args, int), 10, 0, width, pad, 'a', pc);
 187                 continue;
 188             }
 189             if (*format == 'c') {
 190                 /* char are converted to int then pushed on the stack */
 191                 scr[0] = (char)va_arg(args, int);
 192                 scr[1] = '\0';
 193                 pc    -= prints(out, scr, width, pad, pc);
 194                 continue;
 195             }
 196         } else {
 197 out:
 198             printchar(out, *format);
 199             --pc;
 200         }
 201     }
 202     if (out) {
 203         **out = '\0';
 204     }
 205     va_end(args);
 206     return limit - pc;
 207 }
 208
 209 int printf(const char *format, ...)
 210 {
 211     va_list args;
 212
 213     va_start(args, format);
 214     return print(-1, 0, format, args);
 215 }
 216
 217 // TK: added for alternative parameter passing
 218 int vprintf(const char *format, va_list args)
 219 {
 220     return print(-1, 0, format, args);
 221 }
 222
 223 int sprintf(char *out, const char *format, ...)
 224 {
 225     va_list args;
 226
 227     va_start(args, format);
 228     return print(-1, &out, format, args);
 229 }
 230
 231 // TK: added for alternative parameter passing
 232 int vsprintf(char *out, const char *format, va_list args)
 233 {
 234     char *_out;
 235
 236     _out = out;
 237     return print(-1, &_out, format, args);
 238 }
 239
 240 int snprintf(char *buf, size_t count, const char *format, ...)
 241 {
 242     va_list args;
 243
 244
 245     va_start(args, format);
 246     return print(count, &buf, format, args);
 247 }
 248
 249 // TK: added for alternative parameter passing
 250 int vsnprintf(char *buf, size_t count, const char *format, va_list args)
 251 {
 252     return print(count, &buf, format, args);
 253 }
 254
 255 /**
 256  * @}
 257  */