qemu/tests/tcg/mips/mips64-dsp/printf.c

   1
   2 typedef unsigned long va_list;
   3
   4 #define ACC    4
   5 #define __read(source)                    \
   6 ({ va_list __res;                    \
   7     __asm__ __volatile__(                \
   8         "move\t%0, " #source "\n\t"        \
   9         : "=r" (__res));            \
  10     __res;                        \
  11 })
  12
  13 enum format_type {
  14     FORMAT_TYPE_NONE,
  15     FORMAT_TYPE_HEX,
  16     FORMAT_TYPE_ULONG,
  17     FORMAT_TYPE_FLOAT
  18 };
  19
  20 struct printf_spec {
  21     char    type;
  22 };
  23
  24 static int format_decode(char *fmt, struct printf_spec *spec)
  25 {
  26     char *start = fmt;
  27
  28     for (; *fmt ; ++fmt) {
  29         if (*fmt == '%') {
  30             break;
  31         }
  32     }
  33
  34     switch (*++fmt) {
  35     case 'x':
  36         spec->type = FORMAT_TYPE_HEX;
  37         break;
  38
  39     case 'd':
  40         spec->type = FORMAT_TYPE_ULONG;
  41         break;
  42
  43     case 'f':
  44         spec->type = FORMAT_TYPE_FLOAT;
  45         break;
  46
  47     default:
  48         spec->type = FORMAT_TYPE_NONE;
  49     }
  50
  51     return ++fmt - start;
  52 }
  53
  54 void *memcpy(void *dest, void *src, int n)
  55 {
  56     int i;
  57     char *s = src;
  58     char *d = dest;
  59
  60     for (i = 0; i < n; i++) {
  61         d[i] = s[i];
  62     }
  63     return dest;
  64 }
  65
  66 char *number(char *buf, va_list num)
  67 {
  68     int i;
  69     char *str = buf;
  70     static char digits[16] = "0123456789abcdef";
  71     str = str + sizeof(num) * 2;
  72
  73     for (i = 0; i < sizeof(num) * 2; i++) {
  74         *--str = digits[num & 15];
  75         num >>= 4;
  76     }
  77
  78     return buf + sizeof(num) * 2;
  79 }
  80
  81 char *__number(char *buf, va_list num)
  82 {
  83     int i;
  84     va_list mm = num;
  85     char *str = buf;
  86
  87     if (!num) {
  88         *str++ = '0';
  89         return str;
  90     }
  91
  92     for (i = 0; mm; mm = mm/10, i++) {
  93         /* Do nothing. */
  94     }
  95
  96     str = str + i;
  97
  98     while (num) {
  99         *--str = num % 10 + 48;
 100         num = num / 10;
 101     }
 102
 103     return str + i;
 104 }
 105
 106 va_list modf(va_list args, va_list *integer, va_list *num)
 107 {
 108     int i;
 109     double dot_v = 0;
 110     va_list E, DOT, DOT_V;
 111
 112     if (!args) {
 113         return 0;
 114     }
 115
 116     for (i = 0, args = args << 1 >> 1; i < 52; i++) {
 117         if ((args >> i) & 0x1) {
 118             break;
 119         }
 120     }
 121
 122     *integer = 0;
 123
 124     if ((args >> 56 != 0x3f) || (args >> 52 == 0x3ff)) {
 125         E = (args >> 52) - 1023;
 126         DOT = 52 - E - i;
 127         DOT_V = args << (12 + E) >> (12 + E) >> i;
 128         *integer = ((args << 12 >> 12) >> (i + DOT)) | (1 << E);
 129     } else {
 130         E = ~((args >> 52) - 1023) + 1;
 131         DOT_V = args << 12 >> 12;
 132
 133         dot_v += 1.0 / (1 << E);
 134
 135         for (i = 1; i <= 16; i++) {
 136             if ((DOT_V >> (52 - i)) & 0x1) {
 137                 dot_v += 1.0 / (1 << E + i);
 138             }
 139         }
 140
 141         for (i = 1, E = 0; i <= ACC; i++) {
 142             dot_v *= 10;
 143             if (!(va_list)dot_v) {
 144                 E++;
 145             }
 146     }
 147
 148     *num = E;
 149
 150     return dot_v;
 151     }
 152
 153     if (args & 0xf) {
 154         for (i = 1; i <= 16; i++) {
 155             if ((DOT_V >> (DOT - i)) & 0x1) {
 156                 dot_v += 1.0 / (1 << i);
 157             }
 158         }
 159
 160         for (i = 1, E = 0; i <= ACC; i++) {
 161             dot_v *= 10;
 162             if (!(va_list)dot_v) {
 163                 E++;
 164             }
 165         }
 166
 167         *num = E;
 168
 169         return dot_v;
 170     } else if (DOT) {
 171         for (i = 1; i <= DOT; i++) {
 172             if ((DOT_V >> (DOT - i)) & 0x1) {
 173                 dot_v += 1.0 / (1 << i);
 174             }
 175         }
 176
 177         for (i = 1; i <= ACC; i++) {
 178             dot_v = dot_v * 10;
 179         }
 180
 181     return dot_v;
 182     }
 183
 184     return 0;
 185 }
 186
 187 int vsnprintf(char *buf, int size, char *fmt, va_list args)
 188 {
 189     char *str, *mm;
 190     struct printf_spec spec = {0};
 191
 192     str = mm = buf;
 193
 194     while (*fmt) {
 195         char *old_fmt = fmt;
 196         int read = format_decode(fmt, &spec);
 197
 198         fmt += read;
 199
 200         switch (spec.type) {
 201         case FORMAT_TYPE_NONE: {
 202             memcpy(str, old_fmt, read);
 203             str += read;
 204             break;
 205         }
 206         case FORMAT_TYPE_HEX: {
 207             memcpy(str, old_fmt, read);
 208             str = number(str + read, args);
 209             for (; *mm ; ++mm) {
 210                 if (*mm == '%') {
 211                     *mm = '0';
 212                 break;
 213                 }
 214             }
 215         break;
 216         }
 217         case FORMAT_TYPE_ULONG: {
 218             memcpy(str, old_fmt, read - 2);
 219             str = __number(str + read - 2, args);
 220             break;
 221         }
 222         case FORMAT_TYPE_FLOAT: {
 223             va_list integer, dot_v, num;
 224             dot_v = modf(args, &integer, &num);
 225             memcpy(str, old_fmt, read - 2);
 226             str += read - 2;
 227             if ((args >> 63 & 0x1)) {
 228                 *str++ = '-';
 229             }
 230             str = __number(str, integer);
 231             if (dot_v) {
 232                 *str++ = '.';
 233                 while (num--) {
 234                     *str++ = '0';
 235                 }
 236                 str = __number(str, dot_v);
 237             }
 238             break;
 239         }
 240         }
 241     }
 242     *str = '\0';
 243
 244     return str - buf;
 245 }
 246
 247 static void serial_out(char *str)
 248 {
 249     while (*str) {
 250         *(char *)0xffffffffb80003f8 = *str++;
 251     }
 252 }
 253
 254 int vprintf(char *fmt, va_list args)
 255 {
 256     int printed_len = 0;
 257     static char printf_buf[512];
 258     printed_len = vsnprintf(printf_buf, sizeof(printf_buf), fmt, args);
 259     serial_out(printf_buf);
 260     return printed_len;
 261 }
 262
 263 int printf(char *fmt, ...)
 264 {
 265     return vprintf(fmt, __read($5));
 266 }