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