Subversion Repositories openrisc_me

/*========================================================================

//

//      diag.c

//

//      Infrastructure diagnostic output code

//

//========================================================================

//####ECOSGPLCOPYRIGHTBEGIN####

// -------------------------------------------

// This file is part of eCos, the Embedded Configurable Operating System.

// Copyright (C) 1998, 1999, 2000, 2001, 2002 Red Hat, Inc.

// Copyright (C) 2002 Gary Thomas

//

// eCos 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; either version 2 or (at your option) any later version.

//

// eCos 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 eCos; if not, write to the Free Software Foundation, Inc.,

// 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.

//

// As a special exception, if other files instantiate templates or use macros

// or inline functions from this file, or you compile this file and link it

// with other works to produce a work based on this file, this file does not

// by itself cause the resulting work to be covered by the GNU General Public

// License. However the source code for this file must still be made available

// in accordance with section (3) of the GNU General Public License.

//

// This exception does not invalidate any other reasons why a work based on

// this file might be covered by the GNU General Public License.

//

// Alternative licenses for eCos may be arranged by contacting Red Hat, Inc.

// at http://sources.redhat.com/ecos/ecos-license/

// -------------------------------------------

//####ECOSGPLCOPYRIGHTEND####

//========================================================================

//#####DESCRIPTIONBEGIN####

//

// Author(s):    nickg,gthomas,jlarmour

// Contributors:

// Date:         1999-02-22

// Purpose:      Infrastructure diagnostic output

// Description:  Implementations of infrastructure diagnostic routines.

//

//####DESCRIPTIONEND####

//

//======================================================================*/

#include <pkgconf/system.h>

#include <pkgconf/hal.h>

#include <pkgconf/infra.h>

#include <cyg/infra/cyg_type.h>         // base types

#include <cyg/infra/diag.h>             // HAL polled output

#include <cyg/hal/hal_arch.h>           // architectural stuff for...

#include <cyg/hal/hal_intr.h>           // interrupt control

#include <cyg/hal/hal_diag.h>           // diagnostic output routines

#include <stdarg.h>

#include <limits.h>

#ifdef CYG_HAL_DIAG_LOCK_DATA_DEFN

CYG_HAL_DIAG_LOCK_DATA_DEFN;

#endif

/*----------------------------------------------------------------------*/

externC void diag_write_num(

    cyg_uint32  n,              /* number to write              */

    cyg_ucount8 base,           /* radix to write to            */

    cyg_ucount8 sign,           /* sign, '-' if -ve, '+' if +ve */

    cyg_bool    pfzero,         /* prefix with zero ?           */

    cyg_ucount8 width           /* min width of number          */

    );

class Cyg_dummy_diag_init_class {

public:

    Cyg_dummy_diag_init_class() {

        HAL_DIAG_INIT();

    }

};

// Initialize after HAL.

static Cyg_dummy_diag_init_class cyg_dummy_diag_init_obj

                                      CYGBLD_ATTRIB_INIT_AFTER(CYG_INIT_HAL);

/*----------------------------------------------------------------------*/

/* Write single char to output                                          */

externC void diag_write_char(char c)

{

    /* Translate LF into CRLF */

    if( c == '\n' )

    {

        HAL_DIAG_WRITE_CHAR('\r');

    }

    HAL_DIAG_WRITE_CHAR(c);

}

// Default wrapper function used by diag_printf

static void

_diag_write_char(char c, void **param)

{

    diag_write_char(c);

}

/*----------------------------------------------------------------------*/

/* Initialize. Call to pull in diag initializing constructor            */

externC void diag_init(void)

{

}

//

// This routine is used to send characters during 'printf()' functions.

// It can be replaced by providing a replacement via diag_init_putc().

//

static void (*_putc)(char c, void **param) = _diag_write_char;

void

diag_init_putc(void (*putc)(char c, void **param))

{

    _putc = putc;

}

/*----------------------------------------------------------------------*/

/* Write zero terminated string                                         */

externC void diag_write_string(const char *psz)

{

    while( *psz ) diag_write_char( *psz++ );

}

/*----------------------------------------------------------------------*/

/* Write decimal value                                                  */

externC void diag_write_dec( cyg_int32 n)

{

    cyg_ucount8 sign;

    if( n < 0 ) n = -n, sign = '-';

    else sign = '+';

    diag_write_num( n, 10, sign, false, 0);

}

/*----------------------------------------------------------------------*/

/* Write hexadecimal value                                              */

externC void diag_write_hex( cyg_uint32 n)

{

    diag_write_num( n, 16, '+', false, 0);

}

/*----------------------------------------------------------------------*/

/* Generic number writing function                                      */

/* The parameters determine what radix is used, the signed-ness of the  */

/* number, its minimum width and whether it is zero or space filled on  */

/* the left.                                                            */

externC void diag_write_long_num(

    cyg_uint64  n,              /* number to write              */

    cyg_ucount8 base,           /* radix to write to            */

    cyg_ucount8 sign,           /* sign, '-' if -ve, '+' if +ve */

    cyg_bool    pfzero,         /* prefix with zero ?           */

    cyg_ucount8 width           /* min width of number          */

    )

{

    char buf[32];

    cyg_count8 bpos;

    char bufinit = pfzero?'0':' ';

    char *digits = "0123456789ABCDEF";

    /* init buffer to padding char: space or zero */

    for( bpos = 0; bpos < (cyg_count8)sizeof(buf); bpos++ ) buf[bpos] = bufinit;

    /* Set pos to start */

    bpos = 0;

    /* construct digits into buffer in reverse order */

    if( n == 0 ) buf[bpos++] = '0';

    else while( n != 0 )

    {

        cyg_ucount8 d = n % base;

        buf[bpos++] = digits[d];

        n /= base;

    }

    /* set pos to width if less. */

    if( (cyg_count8)width > bpos ) bpos = width;

    /* set sign if negative. */

    if( sign == '-' )

    {

        if( buf[bpos-1] == bufinit ) bpos--;

        buf[bpos] = sign;

    }

    else bpos--;

    /* Now write it out in correct order. */

    while( bpos >= 0 )

        diag_write_char(buf[bpos--]);

}

externC void diag_write_num(

    cyg_uint32  n,              /* number to write              */

    cyg_ucount8 base,           /* radix to write to            */

    cyg_ucount8 sign,           /* sign, '-' if -ve, '+' if +ve */

    cyg_bool    pfzero,         /* prefix with zero ?           */

    cyg_ucount8 width           /* min width of number          */

    )

{

    diag_write_long_num((long long)n, base, sign, pfzero, width);

}

/*----------------------------------------------------------------------*/

/* perform some simple sanity checks on a string to ensure that it      */

/* consists of printable characters and is of reasonable length.        */

static cyg_bool diag_check_string( const char *str )

{

    cyg_bool result = true;

    const char *s;

    if( str == NULL ) return false;

    for( s = str ; result && *s ; s++ )

    {

        char c = *s;

        /* Check for a reasonable length string. */

        if( s-str > 2048 ) result = false;

        /* We only really support CR, NL, and backspace at present. If

         * we want to use tabs or other special chars, this test will

         * have to be expanded.  */

        if( c == '\n' || c == '\r' || c == '\b' )

            continue;

        /* Check for printable chars. This assumes ASCII */

        if( c < ' ' || c > '~' )

            result = false;

    }

    return result;

}

/*----------------------------------------------------------------------*/

static int

_cvt(unsigned long long val, char *buf, long radix, char *digits)

{

    char temp[80];

    char *cp = temp;

    int length = 0;

    if (val == 0) {

        /* Special case */

        *cp++ = '0';

    } else {

        while (val) {

            *cp++ = digits[val % radix];

            val /= radix;

        }

    }

    while (cp != temp) {

        *buf++ = *--cp;

        length++;

    }

    *buf = '\0';

    return (length);

}

#define is_digit(c) ((c >= '0') && (c <= '9'))

static int

_vprintf(void (*putc)(char c, void **param), void **param, const char *fmt, va_list ap)

{

    char buf[sizeof(long long)*8];

    char c, sign, *cp=buf;

    int left_prec, right_prec, zero_fill, pad, pad_on_right,

        i, islong, islonglong;

    long long val = 0;

    int res = 0, length = 0;

    if (!diag_check_string(fmt)) {

        diag_write_string("<Bad format string: ");

        diag_write_hex((cyg_uint32)fmt);

        diag_write_string(" :");

        for( i = 0; i < 8; i++ ) {

            diag_write_char(' ');

            val = va_arg(ap, unsigned long);

            diag_write_hex(val);

        }

        diag_write_string(">\n");

        return 0;

    }

    while ((c = *fmt++) != '\0') {

        if (c == '%') {

            c = *fmt++;

            left_prec = right_prec = pad_on_right = islong = islonglong = 0;

            if (c == '-') {

                c = *fmt++;

                pad_on_right++;

            }

            if (c == '0') {

                zero_fill = true;

                c = *fmt++;

            } else {

                zero_fill = false;

            }

            while (is_digit(c)) {

                left_prec = (left_prec * 10) + (c - '0');

                c = *fmt++;

            }

            if (c == '.') {

                c = *fmt++;

                zero_fill++;

                while (is_digit(c)) {

                    right_prec = (right_prec * 10) + (c - '0');

                    c = *fmt++;

                }

            } else {

                right_prec = left_prec;

            }

            sign = '\0';

            if (c == 'l') {

                // 'long' qualifier

                c = *fmt++;

                islong = 1;

                if (c == 'l') {

                    // long long qualifier

                    c = *fmt++;

                    islonglong = 1;

                }

            }

            // Fetch value [numeric descriptors only]

            switch (c) {

            case 'p':

                islong = 1;

            case 'd':

            case 'D':

            case 'x':

            case 'X':

            case 'u':

            case 'U':

            case 'b':

            case 'B':

                if (islonglong) {

                    val = va_arg(ap, long long);

                } else if (islong) {

                    val = (long long)va_arg(ap, long);

                } else{

                    val = (long long)va_arg(ap, int);

                }

                if ((c == 'd') || (c == 'D')) {

                    if (val < 0) {

                        sign = '-';

                        val = -val;

                    }

                } else {

                    // Mask to unsigned, sized quantity

                    if (islong) {

                        val &= ((long long)1 << (sizeof(long) * 8)) - 1;

                    } else{

                        val &= ((long long)1 << (sizeof(int) * 8)) - 1;

                    }

                }

                break;

            default:

                break;

            }

            // Process output

            switch (c) {

            case 'p':  // Pointer

                (*putc)('0', param);

                (*putc)('x', param);

                zero_fill = true;

                left_prec = sizeof(unsigned long)*2;

            case 'd':

            case 'D':

            case 'u':

            case 'U':

            case 'x':

            case 'X':

                switch (c) {

                case 'd':

                case 'D':

                case 'u':

                case 'U':

                    length = _cvt(val, buf, 10, "0123456789");

                    break;

                case 'p':

                case 'x':

                    length = _cvt(val, buf, 16, "0123456789abcdef");

                    break;

                case 'X':

                    length = _cvt(val, buf, 16, "0123456789ABCDEF");

                    break;

                }

                cp = buf;

                break;

            case 's':

            case 'S':

                cp = va_arg(ap, char *);

                if (cp == NULL)

                    cp = "<null>";

                else if (!diag_check_string(cp)) {

                    diag_write_string("<Not a string: 0x");

                    diag_write_hex((cyg_uint32)cp);

                    cp = ">";

                }

                length = 0;

                while (cp[length] != '\0') length++;

                break;

            case 'c':

            case 'C':

                c = va_arg(ap, int /*char*/);

                (*putc)(c, param);

                res++;

                continue;

            case 'b':

            case 'B':

                length = left_prec;

                if (left_prec == 0) {

                    if (islonglong)

                        length = sizeof(long long)*8;

                    else if (islong)

                        length = sizeof(long)*8;

                    else

                        length = sizeof(int)*8;

                }

                for (i = 0;  i < length-1;  i++) {

                    buf[i] = ((val & ((long long)1<<i)) ? '1' : '.');

                }

                cp = buf;

                break;

            case '%':

                (*putc)('%', param);

                break;

            default:

                (*putc)('%', param);

                (*putc)(c, param);

                res += 2;

            }

            pad = left_prec - length;

            if (sign != '\0') {

                pad--;

            }

            if (zero_fill) {

                c = '0';

                if (sign != '\0') {

                    (*putc)(sign, param);

                    res++;

                    sign = '\0';

                }

            } else {

                c = ' ';

            }

            if (!pad_on_right) {

                while (pad-- > 0) {

                    (*putc)(c, param);

                    res++;

                }

            }

            if (sign != '\0') {

                (*putc)(sign, param);

                res++;

            }

            while (length-- > 0) {

                c = *cp++;

                (*putc)(c, param);

                res++;

            }

            if (pad_on_right) {

                while (pad-- > 0) {

                    (*putc)(' ', param);

                    res++;

                }

            }

        } else {

            (*putc)(c, param);

            res++;

        }

    }

    return (res);

}

struct _sputc_info {

    char *ptr;

    int max, len;

};

static void

_sputc(char c, void **param)

{

    struct _sputc_info *info = (struct _sputc_info *)param;

    if (info->len < info->max) {

        *(info->ptr)++ = c;

        *(info->ptr) = '\0';

        info->len++;

    }

}

int

diag_sprintf(char *buf, const char *fmt, ...)

{

    int ret;

    va_list ap;

    struct _sputc_info info;

    va_start(ap, fmt);

    info.ptr = buf;

    info.max = 1024;  // Unlimited

    info.len = 0;

    ret = _vprintf(_sputc, (void **)&info, fmt, ap);

    va_end(ap);

    return (info.len);

}

int

diag_snprintf(char *buf, size_t len, const char *fmt, ...)

{

    int ret;

    va_list ap;

    struct _sputc_info info;

    va_start(ap, fmt);

    info.ptr = buf;

    info.len = 0;

    info.max = len;

    ret = _vprintf(_sputc, (void **)&info, fmt, ap);

    va_end(ap);

    return (info.len);

}

int

diag_vsprintf(char *buf, const char *fmt, va_list ap)

{

    int ret;

    struct _sputc_info info;

    info.ptr = buf;

    info.max = 1024;  // Unlimited

    info.len = 0;

    ret = _vprintf(_sputc, (void **)&info, fmt, ap);

    return (info.len);

}

int

diag_printf(const char *fmt, ...)

{

    va_list ap;

    int ret;

    va_start(ap, fmt);

    ret = _vprintf(_putc, (void **)0, fmt, ap);

    va_end(ap);

    return (ret);

}

int

diag_vprintf(const char *fmt, va_list ap)

{

    int ret;

    ret = _vprintf(_putc, (void **)0, fmt, ap);

    return (ret);

}

void

diag_vdump_buf_with_offset(__printf_fun *pf,

                           cyg_uint8     *p,

                           CYG_ADDRWORD   s,

                           cyg_uint8     *base)

{

    int i, c;

    if ((CYG_ADDRWORD)s > (CYG_ADDRWORD)p) {

        s = (CYG_ADDRWORD)s - (CYG_ADDRWORD)p;

    }

    while ((int)s > 0) {

        if (base) {

            (*pf)("%08X: ", (CYG_ADDRWORD)p - (CYG_ADDRWORD)base);

        } else {

            (*pf)("%08X: ", p);

        }

        for (i = 0;  i < 16;  i++) {

            if (i < (int)s) {

                (*pf)("%02X ", p[i] & 0xFF);

            } else {

                (*pf)("   ");

            }

            if (i == 7) (*pf)(" ");

        }

        (*pf)(" |");

        for (i = 0;  i < 16;  i++) {

            if (i < (int)s) {

                c = p[i] & 0xFF;

                if ((c < 0x20) || (c >= 0x7F)) c = '.';

            } else {

                c = ' ';

            }

            (*pf)("%c", c);

        }

        (*pf)("|\n");

        s -= 16;

        p += 16;

    }

}

void

diag_dump_buf_with_offset(cyg_uint8     *p,

                          CYG_ADDRWORD   s,

                          cyg_uint8     *base)

{

    diag_vdump_buf_with_offset(diag_printf, p, s, base);

}

void

diag_dump_buf(void *p, CYG_ADDRWORD s)

{

   diag_dump_buf_with_offset((cyg_uint8 *)p, s, 0);

}

void

diag_dump_buf_with_offset_32bit(cyg_uint32   *p,

                                CYG_ADDRWORD  s,

                                cyg_uint32   *base)

{

    int i;

    if ((CYG_ADDRWORD)s > (CYG_ADDRWORD)p) {

        s = (CYG_ADDRWORD)s - (CYG_ADDRWORD)p;

    }

    while ((int)s > 0) {

        if (base) {

            diag_printf("%08X: ", (CYG_ADDRWORD)p - (CYG_ADDRWORD)base);

        } else {

            diag_printf("%08X: ", p);

        }

        for (i = 0;  i < 4;  i++) {

            if (i < (int)s/4) {

                diag_printf("%08X ", p[i] );

            } else {

                diag_printf("         ");

            }

        }

        diag_printf("\n");