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 "io/serial.h"
  45
  46 #include "printf.h"
  47
  48 #ifdef REQUIRE_PRINTF_LONG_SUPPORT
  49 #include "typeconversion.h"
  50 #endif
  51
  52 static serialPort_t *printfSerialPort;
  53
  54 #ifdef REQUIRE_CC_ARM_PRINTF_SUPPORT
  55
  56 typedef void (*putcf) (void *, char);
  57 static putcf stdout_putf;
  58 static void *stdout_putp;
  59
  60 // print bf, padded from left to at least n characters.
  61 // padding is zero ('0') if z!=0, space (' ') otherwise
  62 static int putchw(void *putp, putcf putf, int n, char z, char *bf)
  63 {
  64     int written = 0;
  65     char fc = z ? '0' : ' ';
  66     char ch;
  67     char *p = bf;
  68     while (*p++ && n > 0)
  69         n--;
  70     while (n-- > 0) {
  71         putf(putp, fc); written++;
  72     }
  73     while ((ch = *bf++)) {
  74         putf(putp, ch); written++;
  75     }
  76     return written;
  77 }
  78
  79 // retrun number of bytes written
  80 int tfp_format(void *putp, putcf putf, const char *fmt, va_list va)
  81 {
  82     char bf[12];
  83     int written = 0;
  84     char ch;
  85
  86     while ((ch = *(fmt++))) {
  87         if (ch != '%') {
  88             putf(putp, ch); written++;
  89         } else {
  90             char lz = 0;
  91 #ifdef  REQUIRE_PRINTF_LONG_SUPPORT
  92             char lng = 0;
  93 #endif
  94             int w = 0;
  95             ch = *(fmt++);
  96             if (ch == '0') {
  97                 ch = *(fmt++);
  98                 lz = 1;
  99             }
 100             if (ch >= '0' && ch <= '9') {
 101                 ch = a2i(ch, &fmt, 10, &w);
 102             }
 103 #ifdef  REQUIRE_PRINTF_LONG_SUPPORT
 104             if (ch == 'l') {
 105                 ch = *(fmt++);
 106                 lng = 1;
 107             }
 108 #endif
 109             switch (ch) {
 110             case 0:
 111                 goto abort;
 112             case 'u':{
 113 #ifdef  REQUIRE_PRINTF_LONG_SUPPORT
 114                     if (lng)
 115                         uli2a(va_arg(va, unsigned long int), 10, 0, bf);
 116                     else
 117 #endif
 118                         ui2a(va_arg(va, unsigned int), 10, 0, bf);
 119                     written += putchw(putp, putf, w, lz, bf);
 120                     break;
 121                 }
 122             case 'd':{
 123 #ifdef  REQUIRE_PRINTF_LONG_SUPPORT
 124                     if (lng)
 125                         li2a(va_arg(va, unsigned long int), bf);
 126                     else
 127 #endif
 128                         i2a(va_arg(va, int), bf);
 129                     written += putchw(putp, putf, w, lz, bf);
 130                     break;
 131                 }
 132             case 'x':
 133             case 'X':
 134 #ifdef  REQUIRE_PRINTF_LONG_SUPPORT
 135                 if (lng)
 136                     uli2a(va_arg(va, unsigned long int), 16, (ch == 'X'), bf);
 137                 else
 138 #endif
 139                     ui2a(va_arg(va, unsigned int), 16, (ch == 'X'), bf);
 140                 written += putchw(putp, putf, w, lz, bf);
 141                 break;
 142             case 'c':
 143                 putf(putp, (char) (va_arg(va, int))); written++;
 144                 break;
 145             case 's':
 146                 written += putchw(putp, putf, w, 0, va_arg(va, char *));
 147                 break;
 148             case '%':
 149                 putf(putp, ch); written++;
 150                 break;
 151             case 'n':
 152                 *va_arg(va, int*) = written;
 153                 break;
 154             default:
 155                 break;
 156             }
 157         }
 158     }
 159 abort:
 160     return written;
 161 }
 162
 163 void init_printf(void *putp, void (*putf) (void *, char))
 164 {
 165     stdout_putf = putf;
 166     stdout_putp = putp;
 167 }
 168
 169 int tfp_printf(const char *fmt, ...)
 170 {
 171     va_list va;
 172     va_start(va, fmt);
 173     int written = tfp_format(stdout_putp, stdout_putf, fmt, va);
 174     va_end(va);
 175     while (!isSerialTransmitBufferEmpty(printfSerialPort));
 176     return written;
 177 }
 178
 179 static void putcp(void *p, char c)
 180 {
 181     *(*((char **) p))++ = c;
 182 }
 183
 184 int tfp_sprintf(char *s, const char *fmt, ...)
 185 {
 186     va_list va;
 187
 188     va_start(va, fmt);
 189     int written = tfp_format(&s, putcp, fmt, va);
 190     putcp(&s, 0);
 191     va_end(va);
 192     return written;
 193 }
 194
 195
 196 static void _putc(void *p, char c)
 197 {
 198     UNUSED(p);
 199     serialWrite(printfSerialPort, c);
 200 }
 201
 202 void printfSupportInit(void)
 203 {
 204     init_printf(NULL, _putc);
 205 }
 206
 207 #else
 208
 209 // keil/armcc version
 210 int fputc(int c, FILE *f)
 211 {
 212     // let DMA catch up a bit when using set or dump, we're too fast.
 213     while (!isSerialTransmitBufferEmpty(printfSerialPort));
 214     serialWrite(printfSerialPort, c);
 215     return c;
 216 }
 217
 218 void printfSupportInit(void)
 219 {
 220     // Nothing to do
 221 }
 222 #endif
 223
 224 void setPrintfSerialPort(serialPort_t *serialPort)
 225 {
 226     printfSerialPort = serialPort;
 227 }