kvm/user/test/x86/printf.c

   1 #include "printf.h"
   2 #include "smp.h"
   3 #include <stdarg.h>
   4
   5 static struct spinlock lock;
   6
   7 void print(const char *s);
   8
   9 typedef struct pstream {
  10     char *buffer;
  11     int remain;
  12     int added;
  13 } pstream_t;
  14
  15 static void addchar(pstream_t *p, char c)
  16 {
  17     if (p->remain) {
  18         *p->buffer++ = c;
  19         --p->remain;
  20     }
  21     ++p->added;
  22 }
  23
  24 void print_str(pstream_t *p, const char *s)
  25 {
  26     while (*s)
  27         addchar(p, *s++);
  28 }
  29
  30 static char digits[16] = "0123456789abcdef";
  31
  32 void print_int(pstream_t *ps, long n, int base)
  33 {
  34     char buf[sizeof(long) * 3 + 2], *p = buf;
  35     int s = 0, i;
  36
  37     if (n < 0) {
  38         n = -n;
  39         s = 1;
  40     }
  41
  42     while (n) {
  43         *p++ = digits[n % base];
  44         n /= base;
  45     }
  46
  47     if (s)
  48         *p++ = '-';
  49
  50     if (p == buf)
  51         *p++ = '0';
  52
  53     for (i = 0; i < (p - buf) / 2; ++i) {
  54         char tmp;
  55
  56         tmp = buf[i];
  57         buf[i] = p[-1-i];
  58         p[-1-i] = tmp;
  59     }
  60
  61     *p = 0;
  62
  63     print_str(ps, buf);
  64 }
  65
  66 void print_unsigned(pstream_t *ps, unsigned long n, int base)
  67 {
  68     char buf[sizeof(long) * 3 + 1], *p = buf;
  69     int i;
  70
  71     while (n) {
  72         *p++ = digits[n % base];
  73         n /= base;
  74     }
  75
  76     if (p == buf)
  77         *p++ = '0';
  78
  79     for (i = 0; i < (p - buf) / 2; ++i) {
  80         char tmp;
  81
  82         tmp = buf[i];
  83         buf[i] = p[-1-i];
  84         p[-1-i] = tmp;
  85     }
  86
  87     *p = 0;
  88
  89     print_str(ps, buf);
  90 }
  91
  92 int vsnprintf(char *buf, int size, const char *fmt, va_list va)
  93 {
  94     int n;
  95     pstream_t s;
  96
  97     s.buffer = buf;
  98     s.remain = size - 1;
  99     s.added = 0;
 100     while (*fmt) {
 101         char f = *fmt++;
 102
 103         if (f != '%') {
 104             addchar(&s, f);
 105             continue;
 106         }
 107         f = *fmt++;
 108         switch (f) {
 109         case '%':
 110             addchar(&s, '%');
 111             break;
 112         case '\0':
 113             --fmt;
 114             break;
 115         case 'd':
 116             print_int(&s, va_arg(va, int), 10);
 117             break;
 118         case 'x':
 119             print_unsigned(&s, va_arg(va, int), 16);
 120             break;
 121         case 'p':
 122             print_str(&s, "0x");
 123             print_unsigned(&s, (unsigned long)va_arg(va, void *), 16);
 124             break;
 125         case 's':
 126             print_str(&s, va_arg(va, const char *));
 127             break;
 128         default:
 129             addchar(&s, f);
 130             break;
 131         }
 132     }
 133     *s.buffer = 0;
 134     ++s.added;
 135     return s.added;
 136 }
 137
 138
 139 int snprintf(char *buf, int size, const char *fmt, ...)
 140 {
 141     va_list va;
 142     int r;
 143
 144     va_start(va, fmt);
 145     r = vsnprintf(buf, size, fmt, va);
 146     va_end(va);
 147     return r;
 148 }
 149
 150 void print_serial(const char *buf)
 151 {
 152     while (*buf)
 153         asm volatile ("out %%al, $0xf1" : : "a"(*buf++));
 154 }
 155
 156 int printf(const char *fmt, ...)
 157 {
 158     va_list va;
 159     char buf[2000];
 160     int r;
 161
 162     va_start(va, fmt);
 163     r = vsnprintf(buf, sizeof buf, fmt, va);
 164     va_end(va);
 165     spin_lock(&lock);
 166     print_serial(buf);
 167     spin_unlock(&lock);
 168     return r;
 169 }