[Qemu-devel] [PATCH 24/40] xenner: kernel: printk

[Alexander Graf <agraf@suse.de>]

This patch adds a printk implementation for xenner.

Signed-off-by: Alexander Graf <agraf@suse.de>
---
 pc-bios/xenner/printk.c |  682 +++++++++++++++++++++++++++++++++++++++++++++++
 1 files changed, 682 insertions(+), 0 deletions(-)
 create mode 100644 pc-bios/xenner/printk.c

diff --git a/pc-bios/xenner/printk.c b/pc-bios/xenner/printk.c
new file mode 100644
index 0000000..a3ce4c9
--- /dev/null
+++ b/pc-bios/xenner/printk.c
@@ -0,0 +1,682 @@
+/*
+ *  Copyright (C) Red Hat 2007
+ *  Copyright (C) Novell Inc. 2010
+ *
+ *  Author(s): Gerd Hoffmann <kraxel@redhat.com>
+ *             Alexander Graf <agraf@suse.de>
+ *
+ *  printk implementation (mostly from linux)
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; under version 2 of the License.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License along
+ *  with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdarg.h>
+#include <stddef.h>
+#include <inttypes.h>
+
+#include "xenner.h"
+
+/* --- do_div borrowed from linux --- */
+
+#ifdef CONFIG_64BIT
+
+#define do_div(n,base) ({                                   \
+    uint32_t __base = (base);                               \
+    uint32_t __rem;                                         \
+    __rem = ((uint64_t)(n)) % __base;                       \
+    (n) = ((uint64_t)(n)) / __base;                         \
+    __rem;                                                  \
+})
+
+#else
+
+#define do_div(n,base) ({                                   \
+    unsigned long __upper, __low, __high, __mod, __base;    \
+    __base = (base);                                        \
+    asm(""                                                  \
+        : "=a" (__low), "=d" (__high)                       \
+        : "A" (n));                                         \
+    __upper = __high;                                       \
+    if (__high) {                                           \
+            __upper = __high % (__base);                    \
+            __high = __high / (__base);                     \
+    }                                                       \
+    asm("divl %2"                                           \
+        : "=a" (__low), "=d" (__mod)                        \
+        : "rm" (__base), "0" (__low), "1" (__upper));       \
+    asm(""                                                  \
+        : "=A" (n)                                          \
+        : "a" (__low), "d" (__high));                       \
+    __mod;                                                  \
+})
+
+#endif
+
+#define likely(x)   x
+#define unlikely(x) x
+#define WARN_ON(x)  do {} while(0)
+
+/* --- ctype borrowed from linux --- */
+
+#define _U        0x01        /* upper */
+#define _L        0x02        /* lower */
+#define _D        0x04        /* digit */
+#define _C        0x08        /* cntrl */
+#define _P        0x10        /* punct */
+#define _S        0x20        /* white space (space/lf/tab) */
+#define _X        0x40        /* hex digit */
+#define _SP       0x80        /* hard space (0x20) */
+
+unsigned char _ctype[] = {
+    _C,_C,_C,_C,_C,_C,_C,_C,                               /* 0-7 */
+    _C,_C|_S,_C|_S,_C|_S,_C|_S,_C|_S,_C,_C,                /* 8-15 */
+    _C,_C,_C,_C,_C,_C,_C,_C,                               /* 16-23 */
+    _C,_C,_C,_C,_C,_C,_C,_C,                               /* 24-31 */
+    _S|_SP,_P,_P,_P,_P,_P,_P,_P,                           /* 32-39 */
+    _P,_P,_P,_P,_P,_P,_P,_P,                               /* 40-47 */
+    _D,_D,_D,_D,_D,_D,_D,_D,                               /* 48-55 */
+    _D,_D,_P,_P,_P,_P,_P,_P,                               /* 56-63 */
+    _P,_U|_X,_U|_X,_U|_X,_U|_X,_U|_X,_U|_X,_U,             /* 64-71 */
+    _U,_U,_U,_U,_U,_U,_U,_U,                               /* 72-79 */
+    _U,_U,_U,_U,_U,_U,_U,_U,                               /* 80-87 */
+    _U,_U,_U,_P,_P,_P,_P,_P,                               /* 88-95 */
+    _P,_L|_X,_L|_X,_L|_X,_L|_X,_L|_X,_L|_X,_L,             /* 96-103 */
+    _L,_L,_L,_L,_L,_L,_L,_L,                               /* 104-111 */
+    _L,_L,_L,_L,_L,_L,_L,_L,                               /* 112-119 */
+    _L,_L,_L,_P,_P,_P,_P,_C,                               /* 120-127 */
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,                       /* 128-143 */
+    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,                       /* 144-159 */
+    _S|_SP,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,   /* 160-175 */
+    _P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,       /* 176-191 */
+    _U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,       /* 192-207 */
+    _U,_U,_U,_U,_U,_U,_U,_P,_U,_U,_U,_U,_U,_U,_U,_L,       /* 208-223 */
+    _L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,       /* 224-239 */
+    _L,_L,_L,_L,_L,_L,_L,_P,_L,_L,_L,_L,_L,_L,_L,_L        /* 240-255 */
+};
+
+#define __ismask(x) (_ctype[(int)(unsigned char)(x)])
+
+#define isalnum(c)        ((__ismask(c)&(_U|_L|_D)) != 0)
+#define isalpha(c)        ((__ismask(c)&(_U|_L)) != 0)
+#define iscntrl(c)        ((__ismask(c)&(_C)) != 0)
+#define isdigit(c)        ((__ismask(c)&(_D)) != 0)
+#define isgraph(c)        ((__ismask(c)&(_P|_U|_L|_D)) != 0)
+#define islower(c)        ((__ismask(c)&(_L)) != 0)
+#define isprint(c)        ((__ismask(c)&(_P|_U|_L|_D|_SP)) != 0)
+#define ispunct(c)        ((__ismask(c)&(_P)) != 0)
+#define isspace(c)        ((__ismask(c)&(_S)) != 0)
+#define isupper(c)        ((__ismask(c)&(_U)) != 0)
+#define isxdigit(c)       ((__ismask(c)&(_D|_X)) != 0)
+
+#define isascii(c)        (((unsigned char)(c))<=0x7f)
+#define toascii(c)        (((unsigned char)(c))&0x7f)
+
+static inline unsigned char __tolower(unsigned char c)
+{
+    if (isupper(c)) {
+        c -= 'A'-'a';
+    }
+    return c;
+}
+
+static inline unsigned char __toupper(unsigned char c)
+{
+    if (islower(c)) {
+        c -= 'a'-'A';
+    }
+    return c;
+}
+
+#define tolower(c) __tolower(c)
+#define toupper(c) __toupper(c)
+
+/* --- *printf borrowed from linux --- */
+
+static int skip_atoi(const char **s)
+{
+    int i = 0;
+
+    while (isdigit(**s)) {
+            i = i * 10 + *((*s)++) - '0';
+    }
+
+    return i;
+}
+
+static char* put_dec_trunc(char *buf, unsigned q)
+{
+    unsigned d3, d2, d1, d0;
+    d1 = (q >> 4) & 0xf;
+    d2 = (q >> 8) & 0xf;
+    d3 = (q >> 12);
+
+    d0 = 6 * (d3 + d2 + d1) + (q & 0xf);
+    q = (d0 * 0xcd) >> 11;
+    d0 = d0 - 10 * q;
+    *buf++ = d0 + '0'; /* least significant digit */
+    d1 = q + 9 * d3 + 5 * d2 + d1;
+    if (d1 != 0) {
+        q = (d1 * 0xcd) >> 11;
+        d1 = d1 - 10 * q;
+        *buf++ = d1 + '0'; /* next digit */
+
+        d2 = q + 2 * d2;
+        if ((d2 != 0) || (d3 != 0)) {
+            q = (d2 * 0xd) >> 7;
+            d2 = d2 - 10 * q;
+            *buf++ = d2 + '0'; /* next digit */
+
+            d3 = q + 4 * d3;
+            if (d3 != 0) {
+                q = (d3 * 0xcd) >> 11;
+                d3 = d3 - 10 * q;
+                *buf++ = d3 + '0';  /* next digit */
+                if (q != 0) {
+                    *buf++ = q + '0';  /* most sign. digit */
+                }
+            }
+        }
+    }
+    return buf;
+}
+
+static char* put_dec_full(char *buf, unsigned q)
+{
+    /* BTW, if q is in [0,9999], 8-bit ints will be enough, */
+    /* but anyway, gcc produces better code with full-sized ints */
+    unsigned d3, d2, d1, d0;
+
+    d1 = (q >> 4) & 0xf;
+    d2 = (q >> 8) & 0xf;
+    d3 = (q >> 12);
+
+    d0 = 6 * (d3 + d2 + d1) + (q & 0xf);
+    q = (d0 * 0xcd) >> 11;
+    d0 = d0 - 10 * q;
+    *buf++ = d0 + '0';
+    d1 = q + 9 * d3 + 5 * d2 + d1;
+    q = (d1 * 0xcd) >> 11;
+    d1 = d1 - 10 * q;
+    *buf++ = d1 + '0';
+
+    d2 = q + 2 * d2;
+    q = (d2 * 0xd) >> 7;
+    d2 = d2 - 10 * q;
+    *buf++ = d2 + '0';
+
+    d3 = q + 4 * d3;
+    q = (d3 * 0xcd) >> 11;
+    d3 = d3 - 10 * q;
+    *buf++ = d3 + '0';
+    *buf++ = q + '0';
+
+    return buf;
+}
+
+static char* put_dec(char *buf, unsigned long long num)
+{
+    while (1) {
+            unsigned rem;
+            if (num < 100000)
+                    return put_dec_trunc(buf, num);
+            rem = do_div(num, 100000);
+            buf = put_dec_full(buf, rem);
+    }
+}
+
+#define ZEROPAD     1                /* pad with zero */
+#define SIGN        2                /* unsigned/signed long */
+#define PLUS        4                /* show plus */
+#define SPACE       8                /* space if plus */
+#define LEFT        16               /* left justified */
+#define SPECIAL     32               /* 0x */
+#define LARGE       64               /* use 'ABCDEF' instead of 'abcdef' */
+
+static char *number(char *buf, char *end, unsigned long long num, int base,
+                    int size, int precision, int type)
+{
+    char sign,tmp[66];
+    const char *digits;
+    /* we are called with base 8, 10 or 16, only, thus don't need "g..."  */
+    static const char small_digits[] = "0123456789abcdefx";
+    static const char large_digits[] = "0123456789ABCDEFX";
+    int need_pfx = ((type & SPECIAL) && base != 10);
+    int i;
+
+    digits = (type & LARGE) ? large_digits : small_digits;
+    if (type & LEFT) {
+        type &= ~ZEROPAD;
+    }
+    if (base < 2 || base > 36) {
+        return NULL;
+    }
+    sign = 0;
+    if (type & SIGN) {
+        if ((signed long long) num < 0) {
+            sign = '-';
+            num = - (signed long long) num;
+            size--;
+        } else if (type & PLUS) {
+            sign = '+';
+            size--;
+        } else if (type & SPACE) {
+            sign = ' ';
+            size--;
+        }
+    }
+    if (need_pfx) {
+        size--;
+        if (base == 16) {
+            size--;
+        }
+    }
+
+    /* generate full string in tmp[], in reverse order */
+    i = 0;
+    if (num == 0) {
+        tmp[i++] = '0';
+    } else if (base != 10) { /* 8 or 16 */
+        int mask = base - 1;
+        int shift = 3;
+        if (base == 16) {
+            shift = 4;
+        }
+        do {
+            tmp[i++] = digits[((unsigned char)num) & mask];
+            num >>= shift;
+        } while (num);
+    } else { /* base 10 */
+        i = put_dec(tmp, num) - tmp;
+    }
+
+    /* printing 100 using %2d gives "100", not "00" */
+    if (i > precision) {
+            precision = i;
+    }
+    /* leading space padding */
+    size -= precision;
+    if (!(type & (ZEROPAD+LEFT))) {
+        while(--size >= 0) {
+            if (buf < end) {
+                *buf = ' ';
+            }
+            ++buf;
+        }
+    }
+    /* sign */
+    if (sign) {
+        if (buf < end) {
+            *buf = sign;
+        }
+        ++buf;
+    }
+    /* "0x" / "0" prefix */
+    if (need_pfx) {
+        if (buf < end) {
+            *buf = '0';
+        }
+        ++buf;
+        if (base == 16) {
+            if (buf < end) {
+                *buf = digits[16]; /* for arbitrary base: digits[33]; */
+            }
+            ++buf;
+        }
+    }
+    /* zero or space padding */
+    if (!(type & LEFT)) {
+        char c = (type & ZEROPAD) ? '0' : ' ';
+        while (--size >= 0) {
+            if (buf < end) {
+                *buf = c;
+            }
+            ++buf;
+        }
+    }
+    /* hmm even more zero padding? */
+    while (i <= --precision) {
+        if (buf < end) {
+            *buf = '0';
+        }
+        ++buf;
+    }
+    /* actual digits of result */
+    while (--i >= 0) {
+        if (buf < end) {
+            *buf = tmp[i];
+        }
+        ++buf;
+    }
+    /* trailing space padding */
+    while (--size >= 0) {
+        if (buf < end) {
+            *buf = ' ';
+        }
+        ++buf;
+    }
+    return buf;
+}
+
+static size_t strnlen(const char *s, size_t count)
+{
+    const char *sc;
+
+    for (sc = s; count-- && *sc != '\0'; ++sc) {
+            /* nothing */
+    }
+    return sc - s;
+}
+
+static int vsnprintf(char *buf, size_t size, const char *fmt, va_list args)
+{
+    int len;
+    unsigned long long num;
+    int i, base;
+    char *str, *end, c;
+    const char *s;
+
+    int flags;              /* flags to number() */
+
+    int field_width;        /* width of output field */
+    int precision;          /* min. # of digits for integers; max
+                               number of chars for from string */
+    int qualifier;          /* 'h', 'l', or 'L' for integer fields */
+                            /* 'z' support added 23/7/1999 S.H.    */
+                            /* 'z' changed to 'Z' --davidm 1/25/99 */
+                            /* 't' added for ptrdiff_t */
+
+    /* Reject out-of-range values early.  Large positive sizes are
+       used for unknown buffer sizes. */
+    if (unlikely((int) size < 0)) {
+        /* There can be only one.. */
+        static char warn = 1;
+        WARN_ON(warn);
+        warn = 0;
+        return 0;
+    }
+
+    str = buf;
+    end = buf + size;
+
+    /* Make sure end is always >= buf */
+    if (end < buf) {
+        end = ((void *)-1);
+        size = end - buf;
+    }
+
+    for (; *fmt ; ++fmt) {
+        if (*fmt != '%') {
+            if (str < end) {
+                *str = *fmt;
+            }
+            ++str;
+            continue;
+        }
+
+        /* process flags */
+        flags = 0;
+        repeat:
+            ++fmt;                /* this also skips first '%' */
+            switch (*fmt) {
+                case '-': flags |= LEFT; goto repeat;
+                case '+': flags |= PLUS; goto repeat;
+                case ' ': flags |= SPACE; goto repeat;
+                case '#': flags |= SPECIAL; goto repeat;
+                case '0': flags |= ZEROPAD; goto repeat;
+            }
+
+        /* get field width */
+        field_width = -1;
+        if (isdigit(*fmt)) {
+            field_width = skip_atoi(&fmt);
+        } else if (*fmt == '*') {
+            ++fmt;
+            /* it's the next argument */
+            field_width = va_arg(args, int);
+            if (field_width < 0) {
+                field_width = -field_width;
+                flags |= LEFT;
+            }
+        }
+
+        /* get the precision */
+        precision = -1;
+        if (*fmt == '.') {
+            ++fmt;
+            if (isdigit(*fmt)) {
+                precision = skip_atoi(&fmt);
+            } else if (*fmt == '*') {
+                ++fmt;
+                /* it's the next argument */
+                precision = va_arg(args, int);
+            }
+            if (precision < 0) {
+                precision = 0;
+            }
+        }
+
+        /* get the conversion qualifier */
+        qualifier = -1;
+        if (*fmt == 'h' || *fmt == 'l' || *fmt == 'L' ||
+            *fmt =='Z' || *fmt == 'z' || *fmt == 't') {
+            qualifier = *fmt;
+            ++fmt;
+            if (qualifier == 'l' && *fmt == 'l') {
+                qualifier = 'L';
+                ++fmt;
+            }
+        }
+
+        /* default base */
+        base = 10;
+
+        switch (*fmt) {
+            case 'c':
+                if (!(flags & LEFT)) {
+                    while (--field_width > 0) {
+                        if (str < end) {
+                            *str = ' ';
+                        }
+                        ++str;
+                    }
+                }
+                c = (unsigned char) va_arg(args, int);
+                if (str < end) {
+                    *str = c;
+                }
+                ++str;
+                while (--field_width > 0) {
+                    if (str < end) {
+                        *str = ' ';
+                    }
+                    ++str;
+                }
+                continue;
+
+            case 's':
+                s = va_arg(args, char *);
+                if ((unsigned long)s < PAGE_SIZE) {
+                    s = "<NULL>";
+                }
+
+                len = strnlen(s, precision);
+
+                if (!(flags & LEFT)) {
+                    while (len < field_width--) {
+                        if (str < end) {
+                            *str = ' ';
+                        }
+                        ++str;
+                    }
+                }
+                for (i = 0; i < len; ++i) {
+                    if (str < end) {
+                        *str = *s;
+                    }
+                    ++str; ++s;
+                }
+                while (len < field_width--) {
+                    if (str < end) {
+                        *str = ' ';
+                    }
+                    ++str;
+                }
+                continue;
+
+            case 'p':
+                if (field_width == -1) {
+                    field_width = 2*sizeof(void *);
+                    flags |= ZEROPAD;
+                }
+                str = number(str, end,
+                             (unsigned long) va_arg(args, void *),
+                             16, field_width, precision, flags);
+                continue;
+
+
+            case 'n':
+                /* FIXME:
+                * What does C99 say about the overflow case here? */
+                if (qualifier == 'l') {
+                    long * ip = va_arg(args, long *);
+                    *ip = (str - buf);
+                } else if (qualifier == 'Z' || qualifier == 'z') {
+                    size_t * ip = va_arg(args, size_t *);
+                    *ip = (str - buf);
+                } else {
+                    int * ip = va_arg(args, int *);
+                    *ip = (str - buf);
+                }
+                continue;
+
+            case '%':
+                if (str < end) {
+                    *str = '%';
+                }
+                ++str;
+                continue;
+
+            /* integer number formats - set up the flags and "break" */
+            case 'o':
+                base = 8;
+                break;
+
+            case 'X':
+                flags |= LARGE;
+            case 'x':
+                base = 16;
+                break;
+
+            case 'd':
+            case 'i':
+                flags |= SIGN;
+            case 'u':
+                break;
+
+            default:
+                if (str < end) {
+                    *str = '%';
+                }
+                ++str;
+                if (*fmt) {
+                    if (str < end) {
+                        *str = *fmt;
+                    }
+                    ++str;
+                } else {
+                    --fmt;
+                }
+                continue;
+        }
+        if (qualifier == 'L') {
+            num = va_arg(args, long long);
+        } else if (qualifier == 'l') {
+            num = va_arg(args, unsigned long);
+            if (flags & SIGN) {
+                num = (signed long) num;
+            }
+        } else if (qualifier == 'Z' || qualifier == 'z') {
+            num = va_arg(args, size_t);
+        } else if (qualifier == 't') {
+            num = va_arg(args, ptrdiff_t);
+        } else if (qualifier == 'h') {
+            num = (unsigned short) va_arg(args, int);
+            if (flags & SIGN) {
+                num = (signed short) num;
+            }
+        } else {
+            num = va_arg(args, unsigned int);
+            if (flags & SIGN) {
+                num = (signed int) num;
+            }
+        }
+        str = number(str, end, num, base,
+                     field_width, precision, flags);
+    }
+    if (size > 0) {
+        if (str < end) {
+            *str = '\0';
+        } else {
+            end[-1] = '\0';
+        }
+    }
+    /* the trailing null byte doesn't count towards the total */
+    return str-buf;
+}
+
+int vscnprintf(char *buf, size_t size, const char *fmt, va_list args)
+{
+    int i;
+
+    i = vsnprintf(buf,size,fmt,args);
+    return (i >= size) ? (size - 1) : i;
+}
+
+int snprintf(char * buf, size_t size, const char *fmt, ...)
+{
+    va_list args;
+    int i;
+
+    va_start(args, fmt);
+    i = vsnprintf(buf,size,fmt,args);
+    va_end(args);
+    return i;
+}
+
+void write_string(char *msg)
+{
+    int i;
+
+    for (i = 0; msg[i]; i++) {
+        emudev_cmd(EMUDEV_CMD_WRITE_CHAR, msg[i]);
+    }
+}
+
+int printk(int level, const char *fmt, ...)
+{
+    char buf[256];
+    va_list args;
+    int i = 0;
+
+    if (level > vmconf.debug_level) {
+        return 0;
+    }
+
+    i += snprintf(buf+i, sizeof(buf), "<%d>", level);
+    va_start(args, fmt);
+    i += vscnprintf(buf+i, sizeof(buf), fmt, args);
+    va_end(args);
+    write_string(buf);
+    return i;
+}
+
-- 
1.6.0.2