target_firmware/magpie_fw_dev/build/magpie_1_1/sboot/cmnos/printf/src/cmnos_printf.c

   1 //####ECOSGPLCOPYRIGHTBEGIN####
   2 // -------------------------------------------
   3 // This file is part of eCos, the Embedded Configurable Operating System.
   4 // Copyright (C) 1998, 1999, 2000, 2001, 2002 Red Hat, Inc.
   5 // Copyright (C) 2002 Gary Thomas
   6 //
   7 // eCos is free software; you can redistribute it and/or modify it under
   8 // the terms of the GNU General Public License as published by the Free
   9 // Software Foundation; either version 2 or (at your option) any later version.
  10 //
  11 // eCos is distributed in the hope that it will be useful, but WITHOUT ANY
  12 // WARRANTY; without even the implied warranty of MERCHANTABILITY or
  13 // FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  14 // for more details.
  15 //
  16 // You should have received a copy of the GNU General Public License along
  17 // with eCos; if not, write to the Free Software Foundation, Inc.,
  18 // 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  19 //
  20 // As a special exception, if other files instantiate templates or use macros
  21 // or inline functions from this file, or you compile this file and link it
  22 // with other works to produce a work based on this file, this file does not
  23 // by itself cause the resulting work to be covered by the GNU General Public
  24 // License. However the source code for this file must still be made available
  25 // in accordance with section (3) of the GNU General Public License.
  26 //
  27 // This exception does not invalidate any other reasons why a work based on
  28 // this file might be covered by the GNU General Public License.
  29 //
  30 // Alternative licenses for eCos may be arranged by contacting Red Hat, Inc.
  31 // at http://sources.redhat.com/ecos/ecos-license/
  32 // -------------------------------------------
  33 //####ECOSGPLCOPYRIGHTEND####
  34
  35 #include "dt_defs.h"
  36 #include "sys_cfg.h"
  37
  38 #if SYSTEM_MODULE_PRINT
  39
  40 #include "athos_api.h"
  41
  42 #define is_digit(c) ((c >= '0') && (c <= '9'))
  43
  44 #if defined(__GNUC__) && defined(__mips__)
  45 #define va_list __builtin_va_list
  46 #define va_arg __builtin_va_arg
  47 #define va_start __builtin_va_start
  48 #define va_end __builtin_va_end
  49 #define va_copy __builtin_va_copy
  50 #endif
  51
  52 #if defined(__XCC__)
  53 #include "stdarg.h"
  54 #define va_list __gnuc_va_list
  55 #endif
  56
  57 void
  58 cmnos_write_char(char c)
  59 {
  60     if (c == '\n') {
  61         A_PUTC('\r');
  62         A_PUTC('\n');
  63     } else if (c == '\r') {
  64     } else {
  65       A_PUTC(c);
  66     }
  67 }
  68
  69 void (*_putc)(char c) = cmnos_write_char;
  70
  71 static int _cvt(unsigned long val, char *buf, long radix, char *digits)
  72 {
  73     char temp[80];
  74     char *cp = temp;
  75     int length = 0;
  76
  77     if (val == 0) {
  78         /* Special case */
  79         *cp++ = '0';
  80     } else {
  81         while (val) {
  82             *cp++ = digits[val % radix];
  83             val /= radix;
  84         }
  85     }
  86     while (cp != temp) {
  87         *buf++ = *--cp;
  88         length++;
  89     }
  90     *buf = '\0';
  91     return (length);
  92 }
  93
  94
  95 static int cmnos_vprintf(void (*putc)(char c), const char *fmt, va_list ap)
  96 {
  97     char buf[sizeof(long)*8];
  98     char c, sign, *cp=buf;
  99     int left_prec, right_prec, zero_fill, pad, pad_on_right,
 100         i, islong, islonglong;
 101     long val = 0;
 102     int res = 0, length = 0;
 103
 104     while ((c = *fmt++) != '\0') {
 105         if (c == '%') {
 106             c = *fmt++;
 107             left_prec = right_prec = pad_on_right = islong = islonglong = 0;
 108             if (c == '-') {
 109                 c = *fmt++;
 110                 pad_on_right++;
 111             }
 112             if (c == '0') {
 113                 zero_fill = TRUE;
 114                 c = *fmt++;
 115             } else {
 116                 zero_fill = FALSE;
 117             }
 118             while (is_digit(c)) {
 119                 left_prec = (left_prec * 10) + (c - '0');
 120                 c = *fmt++;
 121             }
 122             if (c == '.') {
 123                 c = *fmt++;
 124                 zero_fill++;
 125                 while (is_digit(c)) {
 126                     right_prec = (right_prec * 10) + (c - '0');
 127                     c = *fmt++;
 128                 }
 129             } else {
 130                 right_prec = left_prec;
 131             }
 132             sign = '\0';
 133             if (c == 'l') {
 134                 // 'long' qualifier
 135                 c = *fmt++;
 136                 islong = 1;
 137                 if (c == 'l') {
 138                     // long long qualifier
 139                     c = *fmt++;
 140                     islonglong = 1;
 141                 }
 142             }
 143             // Fetch value [numeric descriptors only]
 144             switch (c) {
 145             case 'p':
 146                 islong = 1;
 147             case 'd':
 148             case 'D':
 149             case 'x':
 150             case 'X':
 151             case 'u':
 152             case 'U':
 153             case 'b':
 154             case 'B':
 155                 if (islonglong) {
 156                     val = va_arg(ap, long);
 157                 } else if (islong) {
 158                     val = (long)va_arg(ap, long);
 159                 } else{
 160                     val = (long)va_arg(ap, int);
 161                 }
 162                 if ((c == 'd') || (c == 'D')) {
 163                     if (val < 0) {
 164                         sign = '-';
 165                         val = -val;
 166                     }
 167                 } else {
 168                     // Mask to unsigned, sized quantity
 169                     if (islong) {
 170                         val &= ((long)1 << (sizeof(long) * 8)) - 1;
 171                     } else{
 172                         val &= ((long)1 << (sizeof(int) * 8)) - 1;
 173                     }
 174                 }
 175                 break;
 176             default:
 177                 break;
 178             }
 179             // Process output
 180             switch (c) {
 181             case 'p':  // Pointer
 182                 (*putc)('0');
 183                 (*putc)('x');
 184                 zero_fill = TRUE;
 185                 left_prec = sizeof(unsigned long)*2;
 186             case 'd':
 187             case 'D':
 188             case 'u':
 189             case 'U':
 190             case 'x':
 191             case 'X':
 192                 switch (c) {
 193                 case 'd':
 194                 case 'D':
 195                 case 'u':
 196                 case 'U':
 197                     length = _cvt(val, buf, 10, "0123456789");
 198                     break;
 199                 case 'p':
 200                 case 'x':
 201                     length = _cvt(val, buf, 16, "0123456789abcdef");
 202                     break;
 203                 case 'X':
 204                     length = _cvt(val, buf, 16, "0123456789ABCDEF");
 205                     break;
 206                 }
 207                 cp = buf;
 208                 break;
 209             case 's':
 210             case 'S':
 211                 cp = va_arg(ap, char *);
 212                 if (cp == NULL)  {
 213                     cp = "<null>";
 214                 }
 215                 length = 0;
 216                 while (cp[length] != '\0') length++;
 217                 break;
 218             case 'c':
 219             case 'C':
 220                 c = va_arg(ap, int /*char*/);
 221                 (*putc)(c);
 222                 res++;
 223                 continue;
 224             case 'b':
 225             case 'B':
 226                 length = left_prec;
 227                 if (left_prec == 0) {
 228                     if (islonglong)
 229                         length = sizeof(long)*8;
 230                     else if (islong)
 231                         length = sizeof(long)*8;
 232                     else
 233                         length = sizeof(int)*8;
 234                 }
 235                 for (i = 0;  i < length-1;  i++) {
 236                     buf[i] = ((val & ((long)1<<i)) ? '1' : '.');
 237                 }
 238                 cp = buf;
 239                 break;
 240             case '%':
 241                 (*putc)('%');
 242                 break;
 243             default:
 244                 (*putc)('%');
 245                 (*putc)(c);
 246                 res += 2;
 247             }
 248             pad = left_prec - length;
 249             if (sign != '\0') {
 250                 pad--;
 251             }
 252             if (zero_fill) {
 253                 c = '0';
 254                 if (sign != '\0') {
 255                     (*putc)(sign);
 256                     res++;
 257                     sign = '\0';
 258                 }
 259             } else {
 260                 c = ' ';
 261             }
 262             if (!pad_on_right) {
 263                 while (pad-- > 0) {
 264                     (*putc)(c);
 265                     res++;
 266                 }
 267             }
 268             if (sign != '\0') {
 269                 (*putc)(sign);
 270                 res++;
 271             }
 272             while (length-- > 0) {
 273                 c = *cp++;
 274                 (*putc)(c);
 275                 res++;
 276             }
 277             if (pad_on_right) {
 278                 while (pad-- > 0) {
 279                     (*putc)(' ');
 280                     res++;
 281                 }
 282             }
 283         } else {
 284             (*putc)(c);
 285             res++;
 286         }
 287     }
 288     return (res);
 289 }
 290
 291 int cmnos_printf(const char *fmt, ...)
 292 {
 293     va_list ap;
 294     int ret;
 295
 296     va_start(ap, fmt);
 297
 298     //if (A_SERIAL_ENABLED()) {
 299     if (1) {
 300         ret = cmnos_vprintf(_putc, fmt, ap);
 301     } else {
 302         ret = 0;
 303     }
 304
 305     va_end(ap);
 306
 307     return (ret);
 308 }
 309
 310 void
 311 cmnos_printf_init(void)
 312 {
 313 }
 314
 315 void cmnos_printf_module_install(struct printf_api *tbl)
 316 {
 317     tbl->_printf_init = cmnos_printf_init;
 318     tbl->_printf = cmnos_printf;
 319 }
 320
 321 #endif /* SYSTEM_MODULE_PRINT */
 322