tmk_core/common/chibios/printf.c

   1 /*
   2  * found at: http://www.sparetimelabs.com/tinyprintf/tinyprintf.php
   3  * and:      http://www.sparetimelabs.com/printfrevisited/printfrevisited.php
   4  */
   5
   6 /*
   7 File: printf.c
   8
   9 Copyright (C) 2004  Kustaa Nyholm
  10
  11 This library is free software; you can redistribute it and/or
  12 modify it under the terms of the GNU Lesser General Public
  13 License as published by the Free Software Foundation; either
  14 version 2.1 of the License, or (at your option) any later version.
  15
  16 This library is distributed in the hope that it will be useful,
  17 but WITHOUT ANY WARRANTY; without even the implied warranty of
  18 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  19 Lesser General Public License for more details.
  20
  21 You should have received a copy of the GNU Lesser General Public
  22 License along with this library; if not, write to the Free Software
  23 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  24
  25 */
  26
  27 #include "printf.h"
  28
  29 typedef void (*putcf) (void*,char);
  30 static putcf stdout_putf;
  31 static void* stdout_putp;
  32
  33 // this adds cca 400 bytes
  34 #define PRINTF_LONG_SUPPORT
  35
  36 #ifdef PRINTF_LONG_SUPPORT
  37
  38 static void uli2a(unsigned long int num, unsigned int base, int uc,char * bf)
  39     {
  40     int n=0;
  41     unsigned int d=1;
  42     while (num/d >= base)
  43         d*=base;
  44     while (d!=0) {
  45         int dgt = num / d;
  46         num%=d;
  47         d/=base;
  48         if (n || dgt>0|| d==0) {
  49             *bf++ = dgt+(dgt<10 ? '0' : (uc ? 'A' : 'a')-10);
  50             ++n;
  51             }
  52         }
  53     *bf=0;
  54     }
  55
  56 static void li2a (long num, char * bf)
  57     {
  58     if (num<0) {
  59         num=-num;
  60         *bf++ = '-';
  61         }
  62     uli2a(num,10,0,bf);
  63     }
  64
  65 #endif
  66
  67 static void ui2a(unsigned int num, unsigned int base, int uc,char * bf)
  68     {
  69     int n=0;
  70     unsigned int d=1;
  71     while (num/d >= base)
  72         d*=base;
  73     while (d!=0) {
  74         int dgt = num / d;
  75         num%= d;
  76         d/=base;
  77         if (n || dgt>0 || d==0) {
  78             *bf++ = dgt+(dgt<10 ? '0' : (uc ? 'A' : 'a')-10);
  79             ++n;
  80             }
  81         }
  82     *bf=0;
  83     }
  84
  85 static void i2a (int num, char * bf)
  86     {
  87     if (num<0) {
  88         num=-num;
  89         *bf++ = '-';
  90         }
  91     ui2a(num,10,0,bf);
  92     }
  93
  94 static int a2d(char ch)
  95     {
  96     if (ch>='0' && ch<='9')
  97         return ch-'0';
  98     else if (ch>='a' && ch<='f')
  99         return ch-'a'+10;
 100     else if (ch>='A' && ch<='F')
 101         return ch-'A'+10;
 102     else return -1;
 103     }
 104
 105 static char a2i(char ch, char** src,int base,int* nump)
 106     {
 107     char* p= *src;
 108     int num=0;
 109     int digit;
 110     while ((digit=a2d(ch))>=0) {
 111         if (digit>base) break;
 112         num=num*base+digit;
 113         ch=*p++;
 114         }
 115     *src=p;
 116     *nump=num;
 117     return ch;
 118     }
 119
 120 static void putchw(void* putp,putcf putf,int n, char z, char* bf)
 121     {
 122     char fc=z? '0' : ' ';
 123     char ch;
 124     char* p=bf;
 125     while (*p++ && n > 0)
 126         n--;
 127     while (n-- > 0)
 128         putf(putp,fc);
 129     while ((ch= *bf++))
 130         putf(putp,ch);
 131     }
 132
 133 void tfp_format(void* putp,putcf putf,char *fmt, va_list va)
 134     {
 135     char bf[12];
 136
 137     char ch;
 138
 139
 140     while ((ch=*(fmt++))) {
 141         if (ch!='%')
 142             putf(putp,ch);
 143         else {
 144             char lz=0;
 145 #ifdef  PRINTF_LONG_SUPPORT
 146             char lng=0;
 147 #endif
 148             int w=0;
 149             ch=*(fmt++);
 150             if (ch=='0') {
 151                 ch=*(fmt++);
 152                 lz=1;
 153                 }
 154             if (ch>='0' && ch<='9') {
 155                 ch=a2i(ch,&fmt,10,&w);
 156                 }
 157 #ifdef  PRINTF_LONG_SUPPORT
 158             if (ch=='l') {
 159                 ch=*(fmt++);
 160                 lng=1;
 161             }
 162 #endif
 163             switch (ch) {
 164                 case 0:
 165                     goto abort;
 166                 case 'u' : {
 167 #ifdef  PRINTF_LONG_SUPPORT
 168                     if (lng)
 169                         uli2a(va_arg(va, unsigned long int),10,0,bf);
 170                     else
 171 #endif
 172                     ui2a(va_arg(va, unsigned int),10,0,bf);
 173                     putchw(putp,putf,w,lz,bf);
 174                     break;
 175                     }
 176                 case 'd' :  {
 177 #ifdef  PRINTF_LONG_SUPPORT
 178                     if (lng)
 179                         li2a(va_arg(va, unsigned long int),bf);
 180                     else
 181 #endif
 182                     i2a(va_arg(va, int),bf);
 183                     putchw(putp,putf,w,lz,bf);
 184                     break;
 185                     }
 186                 case 'x': case 'X' :
 187 #ifdef  PRINTF_LONG_SUPPORT
 188                     if (lng)
 189                         uli2a(va_arg(va, unsigned long int),16,(ch=='X'),bf);
 190                     else
 191 #endif
 192                     ui2a(va_arg(va, unsigned int),16,(ch=='X'),bf);
 193                     putchw(putp,putf,w,lz,bf);
 194                     break;
 195                 case 'c' :
 196                     putf(putp,(char)(va_arg(va, int)));
 197                     break;
 198                 case 's' :
 199                     putchw(putp,putf,w,0,va_arg(va, char*));
 200                     break;
 201                 case '%' :
 202                     putf(putp,ch);
 203                 default:
 204                     break;
 205                 }
 206             }
 207         }
 208     abort:;
 209     }
 210
 211
 212 void init_printf(void* putp,void (*putf) (void*,char))
 213     {
 214     stdout_putf=putf;
 215     stdout_putp=putp;
 216     }
 217
 218 void tfp_printf(char *fmt, ...)
 219     {
 220     va_list va;
 221     va_start(va,fmt);
 222     tfp_format(stdout_putp,stdout_putf,fmt,va);
 223     va_end(va);
 224     }
 225
 226 static void putcp(void* p,char c)
 227     {
 228     *(*((char**)p))++ = c;
 229     }
 230
 231
 232
 233 void tfp_sprintf(char* s,char *fmt, ...)
 234     {
 235     va_list va;
 236     va_start(va,fmt);
 237     tfp_format(&s,putcp,fmt,va);
 238     putcp(&s,0);
 239     va_end(va);
 240     }