Subversion Repositories c0or1k

/*********************************************************************

 *

 * Copyright (C) 2002-2004  Karlsruhe University

 *

 * File path:     generic/printk.cc

 * Description:   Implementation of printf

 *

 * Redistribution and use in source and binary forms, with or without

 * modification, are permitted provided that the following conditions

 * are met:

 * 1. Redistributions of source code must retain the above copyright

 *    notice, this list of conditions and the following disclaimer.

 * 2. Redistributions in binary form must reproduce the above copyright

 *    notice, this list of conditions and the following disclaimer in the

 *    documentation and/or other materials provided with the distribution.

 *

 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND

 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE

 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL

 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS

 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)

 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY

 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF

 * SUCH DAMAGE.

 *

 ********************************************************************/

#include <stdarg.h>     /* for va_list, ... comes with gcc */

#include <l4/lib/printk.h>

#include <l4/lib/mutex.h>

/* FIXME: LICENSE LICENCE */

typedef unsigned int word_t;

extern void putc(const char c);

extern int print_tid (word_t val, word_t width, word_t precision, int adjleft);

/* convert nibble to lowercase hex char */

#define hexchars(x) (((x) < 10) ? ('0' + (x)) : ('a' + ((x) - 10)))

/**

 *      Print hexadecimal value

 *

 *      @param val              value to print

 *      @param width            width in caracters

 *      @param precision        minimum number of digits to apprear

 *      @param adjleft          left adjust the value

 *      @param nullpad          pad with leading zeros (when right padding)

 *

 *      Prints a hexadecimal value with leading zeroes of given width

 *      using putc(), or if adjleft argument is given, print

 *      hexadecimal value with space padding to the right.

 *

 *      @returns the number of charaters printed (should be same as width).

 */

int print_hex64(u64 val, int width, int precision, int adjleft, int nullpad)

{

    int i, n = 0;

    int nwidth = 0;

    u32 high, low;

    high = val >> 32;

    low = (u32)val;

    // Find width of hexnumber

    if (high) {

        while ((high >> (4 * nwidth)) && ((unsigned) nwidth <  2 * sizeof (u32)))

            nwidth++;

        nwidth += 32;

    } else {

        while ((low >> (4 * nwidth)) && ((unsigned) nwidth <  2 * sizeof (u32)))

            nwidth++;

    }

    if (nwidth == 0)

        nwidth = 1;

    // May need to increase number of printed digits

    if (precision > nwidth)

        nwidth = precision;

    // May need to increase number of printed characters

    if (width == 0 && width < nwidth)

        width = nwidth;

    // Print number with padding

    if (high)

    {

        if (!adjleft)

            for (i = width - nwidth; i > 0; i--, n++)

                putc (nullpad ? '0' : ' ');

        for (i = 4 * (nwidth - 33); i >= 0; i -= 4, n++)

            putc (hexchars ((high >> i) & 0xF));

        if (adjleft)

            for (i = width - nwidth; i > 0; i--, n++)

                putc (' ');

        width -= 32;

        nwidth -= 32;

        nullpad = 1;

    }

    if (! adjleft)

        for (i = width - nwidth; i > 0; i--, n++)

            putc (nullpad ? '0' : ' ');

    for (i = 4 * (nwidth - 1); i >= 0; i -= 4, n++)

        putc (hexchars ((low >> i) & 0xF));

    if (adjleft)

        for (i = width - nwidth; i > 0; i--, n++)

            putc (' ');

    return n;

}

int print_hex_3arg(const word_t val, int width, int precision)

{

    long i, n = 0;

    long nwidth = 0;

    int adjleft = 0;

    int nullpad = 0;

    // Find width of hexnumber

    while ((val >> (4 * nwidth)) && (word_t) nwidth <  2 * sizeof (word_t))

        nwidth++;

    if (nwidth == 0)

        nwidth = 1;

    // May need to increase number of printed digits

    if (precision > nwidth)

        nwidth = precision;

    // May need to increase number of printed characters

    if (width == 0 && width < nwidth)

        width = nwidth;

    // Print number with padding

    if (! adjleft)

        for (i = width - nwidth; i > 0; i--, n++)

            putc (nullpad ? '0' : ' ');

    for (i = 4 * (nwidth - 1); i >= 0; i -= 4, n++)

        putc (hexchars ((val >> i) & 0xF));

    if (adjleft)

        for (i = width - nwidth; i > 0; i--, n++)

            putc (' ');

    return n;

}

int print_hex_5arg(const word_t val, int width,

                   int precision, int adjleft, int nullpad)

{

    long i, n = 0;

    long nwidth = 0;

    // Find width of hexnumber

    while ((val >> (4 * nwidth)) && (word_t) nwidth <  2 * sizeof (word_t))

        nwidth++;

    if (nwidth == 0)

        nwidth = 1;

    // May need to increase number of printed digits

    if (precision > nwidth)

        nwidth = precision;

    // May need to increase number of printed characters

    if (width == 0 && width < nwidth)

        width = nwidth;

    // Print number with padding

    if (! adjleft)

        for (i = width - nwidth; i > 0; i--, n++)

            putc (nullpad ? '0' : ' ');

    for (i = 4 * (nwidth - 1); i >= 0; i -= 4, n++)

        putc (hexchars ((val >> i) & 0xF));

    if (adjleft)

        for (i = width - nwidth; i > 0; i--, n++)

            putc (' ');

    return n;

}

/**

 *      Print a string

 *

 *      @param s        zero-terminated string to print

 *      @param width    minimum width of printed string

 *

 *      Prints the zero-terminated string using putc().  The printed

 *      string will be right padded with space to so that it will be

 *      at least WIDTH characters wide.

 *

 *      @returns the number of charaters printed.

 */

int print_string_3arg(const char * s, const int width, const int precision)

{

    int n = 0;

    for (;;)

    {

        if (*s == 0)

            break;

        putc(*s++);

        n++;

        if (precision && n >= precision)

            break;

    }

    while (n < width) { putc(' '); n++; }

    return n;

}

int print_string_1arg(const char * s)

{

        int n = 0;

        int width = 0;

        int precision = 0;

        for (;;) {

                if (*s == 0)

                        break;

                putc(*s++);

                n++;

                if (precision && n >= precision)

                        break;

        }

        while (n < width) {

                putc(' ');

                n++;

        }

        return n;

}

/**

 *      Print hexadecimal value with a separator

 *

 *      @param val      value to print

 *      @param bits     number of lower-most bits before which to

 *                      place the separator

 *      @param sep      the separator to print

 *

 *      @returns the number of charaters printed.

 */

int print_hex_sep(const word_t val, const int bits, const char *sep)

{

    int n = 0;

    n = print_hex_3arg(val >> bits, 0, 0);

    n += print_string_1arg(sep);

    n += print_hex_3arg(val & ((1 << bits) - 1), 0, 0);

    return n;

}

/**

 *      Print decimal value

 *

 *      @param val      value to print

 *      @param width    width of field

 *      @param pad      character used for padding value up to width

 *

 *      Prints a value as a decimal in the given WIDTH with leading

 *      whitespaces.

 *

 *      @returns the number of characters printed (may be more than WIDTH)

 */

int print_dec(const word_t val, int width)

{

    word_t divisor;

    int digits;

    /* estimate number of spaces and digits */

    for (divisor = 1, digits = 1; val/divisor >= 10; divisor *= 10, digits++);

    /* print spaces */

    for ( ; digits < width; digits++ )

        putc(' ');

    /* print digits */

    do {

        putc(((val/divisor) % 10) + '0');

    } while (divisor /= 10);

    /* report number of digits printed */

    return digits;

}

/**

 *      Does the real printk work

 *

 *      @param format_p         pointer to format string

 *      @param args             list of arguments, variable length

 *

 *      Prints the given arguments as specified by the format string.

 *      Implements a subset of the well-known printf plus some L4-specifics.

 *

 *      @returns the number of characters printed

 */

int do_printk(char* format_p, va_list args)

{

    const char* format = format_p;

    int n = 0;

    int i = 0;

    int width = 8;

    int precision = 0;

    int adjleft = 0, nullpad = 0;

#define arg(x) va_arg(args, x)

    /* sanity check */

    if (format == '\0')

    {

        return 0;

    }

    while (*format)

    {

        switch (*(format))

        {

        case '%':

            width = precision = 0;

            adjleft = nullpad = 0;

        reentry:

            switch (*(++format))

            {

                /* modifiers */

            case '.':

                for (format++; *format >= '0' && *format <= '9'; format++)

                    precision = precision * 10 + (*format) - '0';

                if (*format == 'w')

                {

                    // Set precision to printsize of a hex word

                    precision = sizeof (word_t) * 2;

                    format++;

                }

                format--;

                goto reentry;

            case '0':

                nullpad = (width == 0);

            case '1'...'9':

                width = width*10 + (*format)-'0';

                goto reentry;

            case 'w':

                // Set width to printsize of a hex word

                width = sizeof (word_t) * 2;

                goto reentry;

            case '-':

                adjleft = 0;

                goto reentry;

            case 'l':

                goto reentry;

                break;

            case 'c':

                putc(arg(int));

                n++;

                break;

            case 'm':   /* microseconds */

            {

                n += print_hex64(arg(u64), width, precision,

                               adjleft, nullpad);

                break;

            }

            case 'd':

            {

                long val = arg(long);

                if (val < 0)

                {

                    putc('-');

                    val = -val;

                }

                n += print_dec(val, width);

                break;

            }

            case 'u':

                n += print_dec(arg(long), width);

                break;

            case 'p':

                precision = sizeof (word_t) * 2;

            case 'x':

                n += print_hex_5arg(arg(long), width, precision, adjleft, nullpad);

                break;

            case 's':

            {

                char* s = arg(char*);

                if (s)

                    n += print_string_3arg(s, width, precision);

                else

                    n += print_string_3arg("(null)", width, precision);

            }

            break;

            case 't':

            case 'T':

                // Do nothing for now.

                //n += print_tid (arg (word_t), width, precision, adjleft);

                break;

            case '%':

                putc('%');

                n++;

                format++;

                continue;

            default:

                n += print_string_1arg("?");

                break;

            };

            i++;

            break;

        default:

            putc(*format);

            n++;

            break;

        }

        format++;

    }

    return n;

}

DECLARE_SPINLOCK(printk_lock);

/**

 *      Flexible print function

 *

 *      @param format   string containing formatting and parameter type

 *                      information

 *      @param ...      variable list of parameters

 *

 *      @returns the number of characters printed

 */

int printk(char *format, ...)

{

    va_list args;

    int i;

    unsigned long irqstate;

    va_start(args, format);

    spin_lock_irq(&printk_lock, &irqstate);

    i = do_printk(format, args);

    spin_unlock_irq(&printk_lock, irqstate);

    va_end(args);

    return i;

}