sys_printf.c

某个ARM9板子的实际bootloader 对裁剪

/****************************************************************************
* Copyright  Storlink Corp 2005.  All rights reserved.                
*--------------------------------------------------------------------------
* Name			: sys_printf.c
* Description	: 
*		Handle printf
*
* History
*
*	Date		Writer		Description
*	-----------	-----------	-------------------------------------------------
*	04/19/2005	Gary Chen	Create
*
****************************************************************************/
#include <define.h>
#include <stdarg.h>
#include <sl2312.h>

static void sys_write_num(
    UINT32  n,              /* number to write              */
    UINT8   base,           /* radix to write to            */
    UINT8   sign,           /* sign, '-' if -ve, '+' if +ve */
    UINT32  pfzero,         /* prefix with zero ?           */
    UINT8 	width           /* min width of number          */
    );

int sys_vprintf(const char *fmt, va_list ap);

static char *sprint_bufp = NULL;

/*--------------------------------------------------------------
* sys_printf
---------------------------------------------------------------*/
void sys_printf(char *fmt, ... )
{
    va_list ap;
    int ret;
    unsigned long old_ints;
    
	HAL_DISABLE_INTERRUPTS(old_ints);
	sprint_bufp = NULL;
    va_start(ap, fmt);
    ret = sys_vprintf(fmt, ap);
    va_end(ap);
    HAL_RESTORE_INTERRUPTS(old_ints);
    HAL_ENABLE_INTERRUPTS();
 }

/*--------------------------------------------------------------
* sys_sprintf
---------------------------------------------------------------*/
int sys_sprintf(char *buf, char *fmt, ... )
{
    va_list ap;
    int ret;

	sprint_bufp = buf;
    va_start(ap, fmt);
    ret = sys_vprintf(fmt, ap);
    va_end(ap);
    
    return (int)(sys_strlen(buf));
    
 }

/*--------------------------------------------------------------
* printf_putc
---------------------------------------------------------------*/
static void printf_putc(UINT8 data)
{
	if (sprint_bufp)
	{
		if (data == '\b')
			sprint_bufp--;
		else
			*sprint_bufp++ = data;
		*sprint_bufp = 0x00;
	}
	else
	{
		uart_putc(data);
	}
}

#if 0
/*--------------------------------------------------------------
* printf_puts
---------------------------------------------------------------*/
static void printf_puts(UINT8 *datap)
{
	if (sprint_bufp)
	{
		strcpy(sprint_bufp, datap);
		sprint_bufp += strlen(datap);
	}
	else
	{
		uart_puts(datap);
	}
}
#endif

/*--------------------------------------------------------------
* sys_write_dec
---------------------------------------------------------------*/
static void sys_write_dec(INT32 n)
{
    UINT8 sign;

    if( n < 0 ) n = -n, sign = '-';
    else sign = '+';
    
    sys_write_num( n, 10, sign, FALSE, 0);
}

/*--------------------------------------------------------------
* sys_write_hex - Write hexadecimal value
---------------------------------------------------------------*/
static void sys_write_hex( UINT32 n)
{
    sys_write_num( n, 16, '+', FALSE, 0);
}    

/*--------------------------------------------------------------
* sys_write_hex - Write hexadecimal value
* 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.                                                            
---------------------------------------------------------------*/
static void sys_write_long_num(
    UINT64  n,              /* number to write              */
    UINT8   base,           /* radix to write to            */
    UINT8   sign,           /* sign, '-' if -ve, '+' if +ve */
    UINT32  pfzero,         /* prefix with zero ?           */
    UINT8 	width           /* min width of number          */
    )
{
    char buf[32];
    int bpos;
    char bufinit = pfzero?'0':' ';
    char *digits = "0123456789ABCDEF";

    /* init buffer to padding char: space or zero */
    for( bpos = 0; bpos < (int)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 )
    {
        UINT8 d = n % base;
        buf[bpos++] = digits[d];
        n /= base;
    }

    /* set pos to width if less. */
    if( 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 )
        printf_putc(buf[bpos--]);
}

/*--------------------------------------------------------------
* sys_write_num 
---------------------------------------------------------------*/
static void sys_write_num
(
    UINT32  n,              /* number to write              */
    UINT8   base,           /* radix to write to            */
    UINT8   sign,           /* sign, '-' if -ve, '+' if +ve */
    UINT32  pfzero,         /* prefix with zero ?           */
    UINT8 	width           /* min width of number          */
    )
{
    sys_write_long_num((long long)n, base, sign, pfzero, width);
}

/*--------------------------------------------------------------
* sys_check_string 
---------------------------------------------------------------*/
static UINT32 sys_check_string( const char *str )
{
    UINT32 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 > 256 ) result = FALSE;

    }

    return result;
}

/*--------------------------------------------------------------
* _cvt 
---------------------------------------------------------------*/
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) {
        	if(radix == 0)
        	{
        		*buf = '\0';
    			return (length);
        	}
        		
            *cp++ = digits[(val % radix)&0xf];
            val /= radix;
        }
    }
    while (cp != temp) {
        *buf++ = *--cp;
        length++;
    }
    *buf = '\0';
    
    return (length);
}

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

/*--------------------------------------------------------------
* sys_vprintf 
---------------------------------------------------------------*/
int sys_vprintf(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 (!sys_check_string(fmt)) {
        uart_puts("<Bad format 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
                printf_putc('0');
                printf_putc('x');
                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 (!sys_check_string(cp)) {
                    uart_puts("<Not a string: 0x");
                    sys_write_hex((UINT32)cp);
                    cp = ">";
                }
                length = 0;
                while (cp[length] != '\0') length++;
                break;
            case 'c':
            case 'C':
                c = va_arg(ap, int /*char*/);
                printf_putc(c);
                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 '%':
                printf_putc('%');
                break;
            default:
                printf_putc('%');
                printf_putc(c);
                res += 2;
            }
            pad = left_prec - length;
            if (sign != '\0') {
                pad--;
            }
            if (zero_fill) {
                c = '0';
                if (sign != '\0') {
                    printf_putc(sign);
                    res++;
                    sign = '\0';
                }
            } else {
                c = ' ';
            }
            if (!pad_on_right) {
                while (pad-- > 0) {
                    printf_putc(c);
                    res++;
                }
            }
            if (sign != '\0') {
                printf_putc(sign);
                res++;
            }
            while (length-- > 0) {
                c = *cp++;
                if (c == '\n') {
                    printf_putc('\r');
                    res++;
                }
                printf_putc(c);
                res++;
            }
            if (pad_on_right) {
                while (pad-- > 0) {
                    printf_putc(' ');
                    res++;
                }
            }
        } else {
            if (c == '\n') {
                printf_putc('\r');
                res++;
            }
            printf_putc(c);
            res++;
        }
    }
    return (res);
}