target_firmware/magpie_fw_dev/target/cmnos/k2_fw_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 // 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, 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
  36 #include "sys_cfg.h"
  37
  38 #include "dt_defs.h"
  39
  40 #if SYSTEM_MODULE_PRINT
  41
  42 #if MOVE_PRINT_TO_RAM
  43
  44 #include "athos_api.h"
  45
  46 #define is_digit(c) ((c >= '0') && (c <= '9'))
  47
  48 #if defined(__GNUC__) && defined(__mips__)
  49 #define va_list __builtin_va_list
  50 #define va_arg __builtin_va_arg
  51 #define va_start __builtin_va_start
  52 #define va_end __builtin_va_end
  53 #define va_copy __builtin_va_copy
  54 #endif
  55
  56 #include <stdarg.h>
  57
  58 LOCAL void
  59 cmnos_write_char(char c)
  60 {
  61     if (c == '\n') {
  62         A_PUTC('\r');
  63         A_PUTC('\n');
  64     } else if (c == '\r') {
  65     } else {
  66       A_PUTC(c);
  67     }
  68 }
  69
  70 LOCAL void
  71 (*_putc)(char c) = cmnos_write_char;
  72
  73 LOCAL int
  74 _cvt(unsigned long val, char *buf, long radix, char *digits)
  75 {
  76     char temp[80];
  77     char *cp = temp;
  78     int length = 0;
  79
  80     if (val == 0) {
  81         /* Special case */
  82         *cp++ = '0';
  83     } else {
  84         while (val) {
  85             *cp++ = digits[val % radix];
  86             val /= radix;
  87         }
  88     }
  89     while (cp != temp) {
  90         *buf++ = *--cp;
  91         length++;
  92     }
  93     *buf = '\0';
  94     return (length);
  95 }
  96
  97
  98 LOCAL
  99 int cmnos_vprintf(void (*putc)(char c), const char *fmt, va_list ap)
 100 {
 101     char buf[sizeof(long)*8];
 102     char c, sign, *cp=buf;
 103     int left_prec, right_prec, zero_fill, pad, pad_on_right,
 104         i, islong, islonglong;
 105     long val = 0;
 106     int res = 0, length = 0;
 107
 108     while ((c = *fmt++) != '\0') {
 109         if (c == '%') {
 110             c = *fmt++;
 111             left_prec = right_prec = pad_on_right = islong = islonglong = 0;
 112             if (c == '-') {
 113                 c = *fmt++;
 114                 pad_on_right++;
 115             }
 116             if (c == '0') {
 117                 zero_fill = TRUE;
 118                 c = *fmt++;
 119             } else {
 120                 zero_fill = FALSE;
 121             }
 122             while (is_digit(c)) {
 123                 left_prec = (left_prec * 10) + (c - '0');
 124                 c = *fmt++;
 125             }
 126             if (c == '.') {
 127                 c = *fmt++;
 128                 zero_fill++;
 129                 while (is_digit(c)) {
 130                     right_prec = (right_prec * 10) + (c - '0');
 131                     c = *fmt++;
 132                 }
 133             } else {
 134                 right_prec = left_prec;
 135             }
 136             sign = '\0';
 137             if (c == 'l') {
 138                 // 'long' qualifier
 139                 c = *fmt++;
 140                 islong = 1;
 141                 if (c == 'l') {
 142                     // long long qualifier
 143                     c = *fmt++;
 144                     islonglong = 1;
 145                 }
 146             }
 147             // Fetch value [numeric descriptors only]
 148             switch (c) {
 149             case 'p':
 150                 islong = 1;
 151             case 'd':
 152             case 'D':
 153             case 'x':
 154             case 'X':
 155             case 'u':
 156             case 'U':
 157             case 'b':
 158             case 'B':
 159                 if (islonglong) {
 160                     val = va_arg(ap, long);
 161                 } else if (islong) {
 162                     val = (long)va_arg(ap, long);
 163                 } else{
 164                     val = (long)va_arg(ap, int);
 165                 }
 166                 if ((c == 'd') || (c == 'D')) {
 167                     if (val < 0) {
 168                         sign = '-';
 169                         val = -val;
 170                     }
 171                 } else {
 172                     // Mask to unsigned, sized quantity
 173                     if (islong) {
 174                         val &= (1ULL << (sizeof(long) * 8)) - 1;
 175                     } else{
 176                         val &= (1ULL << (sizeof(int) * 8)) - 1;
 177                     }
 178                 }
 179                 break;
 180             default:
 181                 break;
 182             }
 183             // Process output
 184             switch (c) {
 185             case 'p':  // Pointer
 186                 (*putc)('0');
 187                 (*putc)('x');
 188                 zero_fill = TRUE;
 189                 left_prec = sizeof(unsigned long)*2;
 190             case 'd':
 191             case 'D':
 192             case 'u':
 193             case 'U':
 194             case 'x':
 195             case 'X':
 196                 switch (c) {
 197                 case 'd':
 198                 case 'D':
 199                 case 'u':
 200                 case 'U':
 201                     length = _cvt(val, buf, 10, "0123456789");
 202                     break;
 203                 case 'p':
 204                 case 'x':
 205                     length = _cvt(val, buf, 16, "0123456789abcdef");
 206                     break;
 207                 case 'X':
 208                     length = _cvt(val, buf, 16, "0123456789ABCDEF");
 209                     break;
 210                 }
 211                 cp = buf;
 212                 break;
 213             case 's':
 214             case 'S':
 215                 cp = va_arg(ap, char *);
 216                 if (cp == NULL)  {
 217                     cp = "<null>";
 218                 }
 219                 length = 0;
 220                 while (cp[length] != '\0') length++;
 221                 break;
 222             case 'c':
 223             case 'C':
 224                 c = va_arg(ap, int /*char*/);
 225                 (*putc)(c);
 226                 res++;
 227                 continue;
 228             case 'b':
 229             case 'B':
 230                 length = left_prec;
 231                 if (left_prec == 0) {
 232                     if (islonglong)
 233                         length = sizeof(long)*8;
 234                     else if (islong)
 235                         length = sizeof(long)*8;
 236                     else
 237                         length = sizeof(int)*8;
 238                 }
 239                 for (i = 0;  i < length-1;  i++) {
 240                     buf[i] = ((val & ((long)1<<i)) ? '1' : '.');
 241                 }
 242                 cp = buf;
 243                 break;
 244             case '%':
 245                 (*putc)('%');
 246                 break;
 247             default:
 248                 (*putc)('%');
 249                 (*putc)(c);
 250                 res += 2;
 251             }
 252             pad = left_prec - length;
 253             if (sign != '\0') {
 254                 pad--;
 255             }
 256             if (zero_fill) {
 257                 c = '0';
 258                 if (sign != '\0') {
 259                     (*putc)(sign);
 260                     res++;
 261                     sign = '\0';
 262                 }
 263             } else {
 264                 c = ' ';
 265             }
 266             if (!pad_on_right) {
 267                 while (pad-- > 0) {
 268                     (*putc)(c);
 269                     res++;
 270                 }
 271             }
 272             if (sign != '\0') {
 273                 (*putc)(sign);
 274                 res++;
 275             }
 276             while (length-- > 0) {
 277                 c = *cp++;
 278                 (*putc)(c);
 279                 res++;
 280             }
 281             if (pad_on_right) {
 282                 while (pad-- > 0) {
 283                     (*putc)(' ');
 284                     res++;
 285                 }
 286             }
 287         } else {
 288             (*putc)(c);
 289             res++;
 290         }
 291     }
 292     return (res);
 293 }
 294
 295 int
 296 fw_cmnos_printf(const char *fmt, ...)
 297 {
 298     va_list ap;
 299     int ret;
 300
 301     va_start(ap, fmt);
 302
 303     //if (A_SERIAL_ENABLED()) {
 304     if (1) {
 305         ret = cmnos_vprintf(_putc, fmt, ap);
 306     } else {
 307         ret = 0;
 308     }
 309
 310     va_end(ap);
 311     return (ret);
 312 }
 313
 314 #endif /* MOVE_PRINT_TO_RAM */
 315
 316 #endif /* SYSTEM_MODULE_PRINT */