target/linux/ar71xx/image/lzma-loader/src/printf.c

   1 /*
   2  * Copyright (C) 2001 MontaVista Software Inc.
   3  * Author: Jun Sun, jsun@mvista.com or jsun@junsun.net
   4  *
   5  * This program is free software; you can redistribute  it and/or modify it
   6  * under  the terms of  the GNU General  Public License as published by the
   7  * Free Software Foundation;  either version 2 of the  License, or (at your
   8  * option) any later version.
   9  *
  10  */
  11
  12 #include        "printf.h"
  13
  14 extern void board_putc(int ch);
  15
  16 /* this is the maximum width for a variable */
  17 #define         LP_MAX_BUF      256
  18
  19 /* macros */
  20 #define         IsDigit(x)      ( ((x) >= '0') && ((x) <= '9') )
  21 #define         Ctod(x)         ( (x) - '0')
  22
  23 /* forward declaration */
  24 static int PrintChar(char *, char, int, int);
  25 static int PrintString(char *, char *, int, int);
  26 static int PrintNum(char *, unsigned long, int, int, int, int, char, int);
  27
  28 /* private variable */
  29 static const char theFatalMsg[] = "fatal error in lp_Print!";
  30
  31 /* -*-
  32  * A low level printf() function.
  33  */
  34 static void
  35 lp_Print(void (*output)(void *, char *, int),
  36          void * arg,
  37          char *fmt,
  38          va_list ap)
  39 {
  40
  41 #define         OUTPUT(arg, s, l)  \
  42   { if (((l) < 0) || ((l) > LP_MAX_BUF)) { \
  43        (*output)(arg, (char*)theFatalMsg, sizeof(theFatalMsg)-1); for(;;); \
  44     } else { \
  45       (*output)(arg, s, l); \
  46     } \
  47   }
  48
  49     char buf[LP_MAX_BUF];
  50
  51     char c;
  52     char *s;
  53     long int num;
  54
  55     int longFlag;
  56     int negFlag;
  57     int width;
  58     int prec;
  59     int ladjust;
  60     char padc;
  61
  62     int length;
  63
  64     for(;;) {
  65         {
  66             /* scan for the next '%' */
  67             char *fmtStart = fmt;
  68             while ( (*fmt != '\0') && (*fmt != '%')) {
  69                 fmt ++;
  70             }
  71
  72             /* flush the string found so far */
  73             OUTPUT(arg, fmtStart, fmt-fmtStart);
  74
  75             /* are we hitting the end? */
  76             if (*fmt == '\0') break;
  77         }
  78
  79         /* we found a '%' */
  80         fmt ++;
  81
  82         /* check for long */
  83         if (*fmt == 'l') {
  84             longFlag = 1;
  85             fmt ++;
  86         } else {
  87             longFlag = 0;
  88         }
  89
  90         /* check for other prefixes */
  91         width = 0;
  92         prec = -1;
  93         ladjust = 0;
  94         padc = ' ';
  95
  96         if (*fmt == '-') {
  97             ladjust = 1;
  98             fmt ++;
  99         }
 100
 101         if (*fmt == '0') {
 102             padc = '0';
 103             fmt++;
 104         }
 105
 106         if (IsDigit(*fmt)) {
 107             while (IsDigit(*fmt)) {
 108                 width = 10 * width + Ctod(*fmt++);
 109             }
 110         }
 111
 112         if (*fmt == '.') {
 113             fmt ++;
 114             if (IsDigit(*fmt)) {
 115                 prec = 0;
 116                 while (IsDigit(*fmt)) {
 117                     prec = prec*10 + Ctod(*fmt++);
 118                 }
 119             }
 120         }
 121
 122
 123         /* check format flag */
 124         negFlag = 0;
 125         switch (*fmt) {
 126          case 'b':
 127             if (longFlag) {
 128                 num = va_arg(ap, long int);
 129             } else {
 130                 num = va_arg(ap, int);
 131             }
 132             length = PrintNum(buf, num, 2, 0, width, ladjust, padc, 0);
 133             OUTPUT(arg, buf, length);
 134             break;
 135
 136          case 'd':
 137          case 'D':
 138             if (longFlag) {
 139                 num = va_arg(ap, long int);
 140             } else {
 141                 num = va_arg(ap, int);
 142             }
 143             if (num < 0) {
 144                 num = - num;
 145                 negFlag = 1;
 146             }
 147             length = PrintNum(buf, num, 10, negFlag, width, ladjust, padc, 0);
 148             OUTPUT(arg, buf, length);
 149             break;
 150
 151          case 'o':
 152          case 'O':
 153             if (longFlag) {
 154                 num = va_arg(ap, long int);
 155             } else {
 156                 num = va_arg(ap, int);
 157             }
 158             length = PrintNum(buf, num, 8, 0, width, ladjust, padc, 0);
 159             OUTPUT(arg, buf, length);
 160             break;
 161
 162          case 'u':
 163          case 'U':
 164             if (longFlag) {
 165                 num = va_arg(ap, long int);
 166             } else {
 167                 num = va_arg(ap, int);
 168             }
 169             length = PrintNum(buf, num, 10, 0, width, ladjust, padc, 0);
 170             OUTPUT(arg, buf, length);
 171             break;
 172
 173          case 'x':
 174             if (longFlag) {
 175                 num = va_arg(ap, long int);
 176             } else {
 177                 num = va_arg(ap, int);
 178             }
 179             length = PrintNum(buf, num, 16, 0, width, ladjust, padc, 0);
 180             OUTPUT(arg, buf, length);
 181             break;
 182
 183          case 'X':
 184             if (longFlag) {
 185                 num = va_arg(ap, long int);
 186             } else {
 187                 num = va_arg(ap, int);
 188             }
 189             length = PrintNum(buf, num, 16, 0, width, ladjust, padc, 1);
 190             OUTPUT(arg, buf, length);
 191             break;
 192
 193          case 'c':
 194             c = (char)va_arg(ap, int);
 195             length = PrintChar(buf, c, width, ladjust);
 196             OUTPUT(arg, buf, length);
 197             break;
 198
 199          case 's':
 200             s = (char*)va_arg(ap, char *);
 201             length = PrintString(buf, s, width, ladjust);
 202             OUTPUT(arg, buf, length);
 203             break;
 204
 205          case '\0':
 206             fmt --;
 207             break;
 208
 209          default:
 210             /* output this char as it is */
 211             OUTPUT(arg, fmt, 1);
 212         }       /* switch (*fmt) */
 213
 214         fmt ++;
 215     }           /* for(;;) */
 216
 217     /* special termination call */
 218     OUTPUT(arg, "\0", 1);
 219 }
 220
 221
 222 /* --------------- local help functions --------------------- */
 223 static int
 224 PrintChar(char * buf, char c, int length, int ladjust)
 225 {
 226     int i;
 227
 228     if (length < 1) length = 1;
 229     if (ladjust) {
 230         *buf = c;
 231         for (i=1; i< length; i++) buf[i] = ' ';
 232     } else {
 233         for (i=0; i< length-1; i++) buf[i] = ' ';
 234         buf[length - 1] = c;
 235     }
 236     return length;
 237 }
 238
 239 static int
 240 PrintString(char * buf, char* s, int length, int ladjust)
 241 {
 242     int i;
 243     int len=0;
 244     char* s1 = s;
 245     while (*s1++) len++;
 246     if (length < len) length = len;
 247
 248     if (ladjust) {
 249         for (i=0; i< len; i++) buf[i] = s[i];
 250         for (i=len; i< length; i++) buf[i] = ' ';
 251     } else {
 252         for (i=0; i< length-len; i++) buf[i] = ' ';
 253         for (i=length-len; i < length; i++) buf[i] = s[i-length+len];
 254     }
 255     return length;
 256 }
 257
 258 static int
 259 PrintNum(char * buf, unsigned long u, int base, int negFlag,
 260          int length, int ladjust, char padc, int upcase)
 261 {
 262     /* algorithm :
 263      *  1. prints the number from left to right in reverse form.
 264      *  2. fill the remaining spaces with padc if length is longer than
 265      *     the actual length
 266      *     TRICKY : if left adjusted, no "0" padding.
 267      *              if negtive, insert  "0" padding between "0" and number.
 268      *  3. if (!ladjust) we reverse the whole string including paddings
 269      *  4. otherwise we only reverse the actual string representing the num.
 270      */
 271
 272     int actualLength =0;
 273     char *p = buf;
 274     int i;
 275
 276     do {
 277         int tmp = u %base;
 278         if (tmp <= 9) {
 279             *p++ = '0' + tmp;
 280         } else if (upcase) {
 281             *p++ = 'A' + tmp - 10;
 282         } else {
 283             *p++ = 'a' + tmp - 10;
 284         }
 285         u /= base;
 286     } while (u != 0);
 287
 288     if (negFlag) {
 289         *p++ = '-';
 290     }
 291
 292     /* figure out actual length and adjust the maximum length */
 293     actualLength = p - buf;
 294     if (length < actualLength) length = actualLength;
 295
 296     /* add padding */
 297     if (ladjust) {
 298         padc = ' ';
 299     }
 300     if (negFlag && !ladjust && (padc == '0')) {
 301         for (i = actualLength-1; i< length-1; i++) buf[i] = padc;
 302         buf[length -1] = '-';
 303     } else {
 304         for (i = actualLength; i< length; i++) buf[i] = padc;
 305     }
 306
 307
 308     /* prepare to reverse the string */
 309     {
 310         int begin = 0;
 311         int end;
 312         if (ladjust) {
 313             end = actualLength - 1;
 314         } else {
 315             end = length -1;
 316         }
 317
 318         while (end > begin) {
 319             char tmp = buf[begin];
 320             buf[begin] = buf[end];
 321             buf[end] = tmp;
 322             begin ++;
 323             end --;
 324         }
 325     }
 326
 327     /* adjust the string pointer */
 328     return length;
 329 }
 330
 331 static void printf_output(void *arg, char *s, int l)
 332 {
 333     int i;
 334
 335     // special termination call
 336     if ((l==1) && (s[0] == '\0')) return;
 337
 338     for (i=0; i< l; i++) {
 339         board_putc(s[i]);
 340         if (s[i] == '\n') board_putc('\r');
 341     }
 342 }
 343
 344 void printf(char *fmt, ...)
 345 {
 346     va_list ap;
 347     va_start(ap, fmt);
 348     lp_Print(printf_output, 0, fmt, ap);
 349     va_end(ap);
 350 }