src/main/common/printf.c

   1 /*
   2  * Copyright (c) 2004,2012 Kustaa Nyholm / SpareTimeLabs
   3  *
   4  * All rights reserved.
   5  *
   6  * Redistribution and use in source and binary forms, with or without modification,
   7  * are permitted provided that the following conditions are met:
   8  *
   9  * Redistributions of source code must retain the above copyright notice, this list
  10  * of conditions and the following disclaimer.
  11  *
  12  * Redistributions in binary form must reproduce the above copyright notice, this
  13  * list of conditions and the following disclaimer in the documentation and/or other
  14  * materials provided with the distribution.
  15  *
  16  * Neither the name of the Kustaa Nyholm or SpareTimeLabs nor the names of its
  17  * contributors may be used to endorse or promote products derived from this software
  18  * without specific prior written permission.
  19  *
  20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  22  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  23  * IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
  24  * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
  25  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
  26  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
  27  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  28  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
  29  * OF SUCH DAMAGE.
  30  */
  31
  32 #include <stdbool.h>
  33 #include <stdint.h>
  34 #include <stdarg.h>
  35 #include <stdlib.h>
  36
  37 #include "platform.h"
  38
  39 #include "build/build_config.h"
  40
  41 #include "common/utils.h"
  42
  43 #include "drivers/serial.h"
  44 #include "printf.h"
  45
  46 #include "io/serial.h"
  47
  48
  49 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
  50 #include "typeconversion.h"
  51 #define MAX UINT64_MAX
  52 #else
  53 #define MAX UINT32_MAX
  54 #endif
  55
  56 static serialPort_t *printfSerialPort;
  57
  58 #ifdef REQUIRE_CC_ARM_PRINTF_SUPPORT
  59
  60 typedef void (*putcf) (void *, char);
  61 static putcf stdout_putf;
  62 static void *stdout_putp;
  63
  64 // print bf, padded from left to at least n characters.
  65 // padding is zero ('0') if z!=0, space (' ') otherwise
  66 static int putchw(void *putp, const void *end, putcf putf, int n, char z, char *bf)
  67 {
  68     int written = 0;
  69     char fc = z ? '0' : ' ';
  70     char pr = 0;
  71     if (n < 0) {
  72         pr = 1;
  73         n = -n;
  74     }
  75     char ch;
  76     char *p = bf;
  77     while (*p++ && n > 0)
  78         n--;
  79     if (pr == 0) {
  80         while (n-- > 0) {
  81             if (putp < end) {
  82                 putf(putp, fc);
  83             }
  84             written++;
  85         }
  86     }
  87     while ((ch = *bf++)) {
  88         if (putp < end) {
  89             putf(putp, ch);
  90         }
  91         written++;
  92     }
  93     if (pr == 1) {
  94         while (n-- > 0) {
  95             if (putp < end) {
  96                 putf(putp, fc);
  97             }
  98             written++;
  99         }
 100     }
 101     return written;
 102 }
 103
 104 int tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
 105 {
 106     return tfp_nformat(putp, -1, putf, fmt, va);
 107 }
 108
 109 // return number of bytes written
 110 int tfp_nformat(void *putp, int size, void (*putf) (void *, char), const char *fmt, va_list va)
 111 {
 112     char bf[12];
 113     int written = 0;
 114     char ch;
 115
 116     const void *end = size < 0 ? (void*)MAX : ((char *)putp + size - 1);
 117
 118     while ((ch = *(fmt++))) {
 119         if (ch != '%') {
 120             if (putp < end) {
 121                 putf(putp, ch);
 122             }
 123             written++;
 124         } else {
 125             char lz = 0;
 126             char pr = 0; // padding at the right?
 127 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
 128             char lng = 0;
 129 #endif
 130             int w = 0;
 131             ch = *(fmt++);
 132             if (ch == '-') {
 133                 ch = *(fmt++);
 134                 pr = 1;
 135             }
 136             if (ch == '0') {
 137                 ch = *(fmt++);
 138                 lz = 1;
 139             }
 140             if (ch >= '0' && ch <= '9') {
 141                 ch = a2i(ch, &fmt, 10, &w);
 142                 if (pr) {
 143                     w = -w;
 144                 }
 145             }
 146 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
 147             if (ch == 'l') {
 148                 ch = *(fmt++);
 149                 lng = 1;
 150             }
 151 #endif
 152             switch (ch) {
 153             case 0:
 154                 goto abort;
 155             case 'u':{
 156 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
 157                     if (lng)
 158                         uli2a(va_arg(va, unsigned long int), 10, 0, bf);
 159                     else
 160 #endif
 161                         ui2a(va_arg(va, unsigned int), 10, 0, bf);
 162                     written += putchw(putp, end, putf, w, lz, bf);
 163                     break;
 164                 }
 165             case 'i':
 166             case 'd':{
 167 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
 168                     if (lng)
 169                         li2a(va_arg(va, unsigned long int), bf);
 170                     else
 171 #endif
 172                         i2a(va_arg(va, int), bf);
 173                     written += putchw(putp, end, putf, w, lz, bf);
 174                     break;
 175                 }
 176             case 'x':
 177             case 'X':
 178 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
 179                 if (lng)
 180                     uli2a(va_arg(va, unsigned long int), 16, (ch == 'X'), bf);
 181                 else
 182 #endif
 183                     ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf);
 184                 written += putchw(putp, end, putf, w, lz, bf);
 185                 break;
 186             case 'c':
 187                 if (putp < end) {
 188                     putf(putp, (char) (va_arg(va, int)));
 189                 }
 190                 written++;
 191                 break;
 192             case 's':
 193                 written += putchw(putp, end, putf, w, 0, va_arg(va, char *));
 194                 break;
 195             case '%':
 196                 if (putp < end) {
 197                     putf(putp, ch);
 198                 }
 199                 written++;
 200                 break;
 201             case 'n':
 202                 *va_arg(va, int*) = written;
 203                 break;
 204             case 'f':
 205                 ftoa(va_arg(va, double), bf);
 206                 written += putchw(putp, end, putf, w, lz, bf);
 207                 break;
 208             default:
 209                 break;
 210             }
 211         }
 212     }
 213 abort:
 214     return written;
 215 }
 216
 217 void init_printf(void *putp, void (*putf) (void *, char))
 218 {
 219     stdout_putf = putf;
 220     stdout_putp = putp;
 221 }
 222
 223 int tfp_printf(const char *fmt, ...)
 224 {
 225     va_list va;
 226     va_start(va, fmt);
 227     int written = tfp_format(stdout_putp, stdout_putf, fmt, va);
 228     va_end(va);
 229     while (!isSerialTransmitBufferEmpty(printfSerialPort));
 230     return written;
 231 }
 232
 233 static void putcp(void *p, char c)
 234 {
 235     *(*((char **) p))++ = c;
 236 }
 237
 238 int tfp_sprintf(char *s, const char *fmt, ...)
 239 {
 240     va_list va;
 241
 242     va_start(va, fmt);
 243     int written = tfp_vsprintf(s, fmt, va);
 244     va_end(va);
 245     return written;
 246 }
 247
 248 int tfp_snprintf(char *s, int size, const char *fmt, ...)
 249 {
 250     va_list va;
 251
 252     va_start(va, fmt);
 253     int written = tfp_vsnprintf(s, size, fmt, va);
 254     va_end(va);
 255     return written;
 256 }
 257
 258 int tfp_vsprintf(char *s, const char *fmt, va_list va)
 259 {
 260     return tfp_vsnprintf(s, -1, fmt, va);
 261 }
 262
 263 int tfp_vsnprintf(char *s, int size, const char *fmt, va_list va)
 264 {
 265     int written = tfp_nformat(&s, size, putcp, fmt, va);
 266     putcp(&s, 0);
 267     return written;
 268 }
 269
 270
 271 static void _putc(void *p, char c)
 272 {
 273     UNUSED(p);
 274     serialWrite(printfSerialPort, c);
 275 }
 276
 277 void printfSupportInit(void)
 278 {
 279     init_printf(NULL, _putc);
 280 }
 281
 282 #else
 283
 284 // keil/armcc version
 285 int fputc(int c, FILE *f)
 286 {
 287     // let DMA catch up a bit when using set or dump, we're too fast.
 288     while (!isSerialTransmitBufferEmpty(printfSerialPort));
 289     serialWrite(printfSerialPort, c);
 290     return c;
 291 }
 292
 293 void printfSupportInit(void)
 294 {
 295     // Nothing to do
 296 }
 297 #endif
 298
 299 void setPrintfSerialPort(serialPort_t *serialPort)
 300 {
 301     printfSerialPort = serialPort;
 302 }