diff --git a/lib/printf.c b/lib/printf.c index 21431355f..84eedb396 100644 --- a/lib/printf.c +++ b/lib/printf.c @@ -33,399 +33,399 @@ * $Id$ */ -/** - * \file printf-stdarg.c - * - * \brief sprintf functions to replace newlib for AVR32 UC3. - * - * \author $Author: umanzoli $ - * - * Created on : 17-mar-2009 - * - * $Id$ - */ - -/* - * Copyright 2001, 2002 Georges Menie (www.menie.org) - * stdarg version contributed by Christian Ettinger - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU Lesser General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * 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 Lesser General Public License for more details. - * - * You should have received a copy of the GNU Lesser General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - - -#include -#include -#include - -#include -#include - -#define __putc(x) uart1_putc(x) - -/** - * Structure to hold data to be passed to print function with format. - * Aka print context. - */ -struct __print_ctx_t -{ - //! pointer to next char to be filled. - char* _ptr; - //! maximum length of the buffer. - size_t _max_len; -}; -typedef struct __print_ctx_t _print_ctx_t; - -/** - * Pad string to right - */ -#define _PRINTFMT_PAD_RIGHT 1 - -/** - * Pad the number with zeroes - */ -#define _PRINTFMT_PAD_ZERO 2 - -/** - * The following should be enough for 32 bit int - */ -#define _PRINTFMT_INT_BUF_LEN 12 - -/** - * Print a character to stdout (if string is null) - * otherwise, put the character at the end of the provided string. - */ -static void __print_char( _print_ctx_t* ctx, char c ) -{ - if( ctx ) { - if( c == '\r' || c == '\n' ) { - if( ctx->_max_len > 1 ) { - *(ctx->_ptr)='\r'; - ctx->_max_len--; - ctx->_ptr++; - *(ctx->_ptr)='\n'; - ctx->_max_len--; - ctx->_ptr++; - } else { - *(ctx->_ptr)='\n'; - ctx->_max_len--; - ctx->_ptr++; - } - } else { - if( ctx->_max_len ) { - *(ctx->_ptr)=c; - ctx->_max_len--; - ctx->_ptr++; - } - } - } else { - __putc( (uint8_t)c ); - } -} - -/** - * Print a string to a given string. - */ -static int __print_str( _print_ctx_t* ctx, - const char *string, - int width, - int pad, - int print_limit, - bool is_number ) -{ - int pc = 0; - int padchar = ' '; - int i, len; - - if( width > 0 ) { - register int len = 0; - register const char *ptr; - for( ptr = string; *ptr; ++ptr ) - ++len; - if( len >= width ) - width = 0; - else - width -= len; - if( pad & _PRINTFMT_PAD_ZERO ) - padchar = '0'; - } - if( !( pad & _PRINTFMT_PAD_RIGHT ) ) { - for( ; width > 0; --width ) { - __print_char( ctx, padchar ); - ++pc; - } - } - - // The string to print is not the result of a number conversion to ascii. - if( false == is_number ) { - // For a string, printlimit is the max number of characters to display. - for( ; print_limit && *string; ++string, --print_limit ) { - __print_char( ctx, *string ); - ++pc; - } - } - - // The string to print represents an integer number. - if( true == is_number ) { - // In this case, printlimit is the min number of digits to print. - - // If the length of the number to print is less than the min nb of i - // digits to display, we add 0 before printing the number. - len = strlen( string ); - if( len < print_limit ) { - i = print_limit - len; - for( ; i; i-- ) { - __print_char( ctx, '0' ); - ++pc; - } - } - } - - /* - * Else: The string to print is not the result of a number conversion to ascii. - * For a string, printlimit is the max number of characters to display. - */ - for( ; print_limit && *string; ++string, --print_limit ) { - __print_char( ctx, *string ); - ++pc; - } - - for( ; width > 0; --width ) { - __print_char( ctx, padchar ); - ++pc; - } - - return pc; -} - -/** - * Print a number to the given string, with the given base. - */ -static int __print_int( _print_ctx_t* ctx, - int i, - int b, - int sg, - int width, - int pad, - int letbase, - int print_limit ) -{ - char print_buf[_PRINTFMT_INT_BUF_LEN]; - register char *s; - register int t, neg = 0, pc = 0; - register unsigned int u = i; - - if( i == 0 ) { - print_buf[0] = '0'; - print_buf[1] = '\0'; - return __print_str( ctx, print_buf, width, pad, print_limit, true ); - } - - if( sg && b == 10 && i < 0 ) { - neg = 1; - u = -i; - } - - s = print_buf + _PRINTFMT_INT_BUF_LEN - 1; - *s = '\0'; - - while( u ) { - t = u % b; - if( t >= 10 ) - t += letbase - '0' - 10; - *--s = t + '0'; - u /= b; - } - - if( neg ) { - if( width && ( pad & _PRINTFMT_PAD_ZERO ) ) { - __print_char( ctx, '-' ); - ++pc; - --width; - } else { - *--s = '-'; - } - } - - return pc + __print_str( ctx, s, width, pad, print_limit, true ); -} -/* -#if __GNUC__ -int fprintf( __FILE *stream, const char *format, ... ) -{ - return 0; -} -#endif -*/ - -/** - * Print the given arguments, with given format onto string out. - */ -static int __print_fmt( _print_ctx_t* ctx, const char *format, va_list args ) -{ - int width; - int pad; - int print_limit; - int pc = 0; - char scr[2]; - - for( ; *format != 0; ++format ) { - if( *format == '%' ) { - ++format; - width = pad = print_limit = 0; - - if( *format == '\0' ) { - break; - } - - if( *format == '%' ) { - goto out; - } - - if( *format == '-' ) { - ++format; - pad = _PRINTFMT_PAD_RIGHT; - } - - while( *format == '0' ) { - ++format; - pad |= _PRINTFMT_PAD_ZERO; - } - - for( ; *format >= '0' && *format <= '9'; ++format ) { - width *= 10; - width += *format - '0'; - } - - if( *format == '.' ) { - ++format; - for( ; *format >= '0' && *format <= '9'; ++format ) { - print_limit *= 10; - print_limit += *format - '0'; - } - } - - if( 0 == print_limit ) { - print_limit--; - } - - if( *format == 'l' ) { - ++format; - } - - if( *format == 's' ) { - register char *s = (char *) va_arg( args, int ); - pc += __print_str( ctx, - s ? s : "(null)", - width, - pad, - print_limit, - false ); - continue; - } - - if( *format == 'd' ) { - pc += __print_int( ctx, va_arg( args, int ), 10, 1, width, pad, 'a', print_limit ); - continue; - } - - if( ( *format == 'x' ) || ( *format == 'p' ) ) { - pc += __print_int( ctx, va_arg( args, int ), 16, 0, width, pad, 'a', print_limit ); - continue; - } - - if( *format == 'X' ) { - pc += __print_int( ctx, va_arg( args, int ), 16, 0, width, pad, 'A', print_limit ); - continue; - } - - if( *format == 'u' ) { - pc += __print_int( ctx, va_arg( args, int ), 10, 0, width, pad, 'a', print_limit ); - continue; - } - - if( *format == 'c' ) { - // char are converted to int then pushed on the stack - scr[0] = (char) va_arg( args, int ); - scr[1] = '\0'; - pc += __print_str( ctx, scr, width, pad, print_limit, false ); - continue; - } - } else { -out: - __print_char( ctx, *format ); - ++pc; - } - } - - if( ctx && ctx->_max_len ) { - *(ctx->_ptr) = '\0'; - } - - return pc; -} - -/* -int sprintf( char *out, const char *format, ... ) -{ - int retval = 0; - _print_ctx_t ctx; - va_list args; - - ctx._ptr = out; - ctx._max_len = BLOCK_MEM_SIZE; - - va_start( args, format ); - retval = __print_fmt( &ctx, format, args ); - va_end( args ); - - return retval; -} -*/ - -int printf( const char *format, ... ) -{ - int retval = 0; -// memory_t* buf; - va_list args; - - /* - buf = memory_alloc( 10 ); - - if( buf ) { - _print_ctx_t ctx; - ctx._ptr = (char*)buf->_data; - ctx._max_len = BLOCK_MEM_SIZE; - - va_start( args, format ); - retval = __print_fmt( &ctx, format, args ); - va_end( args ); - - buf->_len = strlen( (const char*)buf->_data ); - -// LCD_WriteString( ll, buf ); - ll++; - ll &= 0x03; - if( uart_task_send( buf ) == false ) { - memory_free( buf ); - } - } - */ - - va_start( args, format ); - retval = __print_fmt( NULL, format, args ); - va_end( args ); - - return retval; -} +/** + * \file printf-stdarg.c + * + * \brief sprintf functions to replace newlib for AVR32 UC3. + * + * \author $Author: umanzoli $ + * + * Created on : 17-mar-2009 + * + * $Id$ + */ + +/* + * Copyright 2001, 2002 Georges Menie (www.menie.org) + * stdarg version contributed by Christian Ettinger + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * 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 Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + + +#include +#include +#include + +#include +#include + +#define __putc(x) uart1_putc(x) + +/** + * Structure to hold data to be passed to print function with format. + * Aka print context. + */ +struct __print_ctx_t +{ + //! pointer to next char to be filled. + char* _ptr; + //! maximum length of the buffer. + size_t _max_len; +}; +typedef struct __print_ctx_t _print_ctx_t; + +/** + * Pad string to right + */ +#define _PRINTFMT_PAD_RIGHT 1 + +/** + * Pad the number with zeroes + */ +#define _PRINTFMT_PAD_ZERO 2 + +/** + * The following should be enough for 32 bit int + */ +#define _PRINTFMT_INT_BUF_LEN 12 + +/** + * Print a character to stdout (if string is null) + * otherwise, put the character at the end of the provided string. + */ +static void __print_char( _print_ctx_t* ctx, char c ) +{ + if( ctx ) { + if( c == '\r' || c == '\n' ) { + if( ctx->_max_len > 1 ) { + *(ctx->_ptr)='\r'; + ctx->_max_len--; + ctx->_ptr++; + *(ctx->_ptr)='\n'; + ctx->_max_len--; + ctx->_ptr++; + } else { + *(ctx->_ptr)='\n'; + ctx->_max_len--; + ctx->_ptr++; + } + } else { + if( ctx->_max_len ) { + *(ctx->_ptr)=c; + ctx->_max_len--; + ctx->_ptr++; + } + } + } else { + __putc( (uint8_t)c ); + } +} + +/** + * Print a string to a given string. + */ +static int __print_str( _print_ctx_t* ctx, + const char *string, + int width, + int pad, + int print_limit, + bool is_number ) +{ + int pc = 0; + int padchar = ' '; + int i, len; + + if( width > 0 ) { + register int len = 0; + register const char *ptr; + for( ptr = string; *ptr; ++ptr ) + ++len; + if( len >= width ) + width = 0; + else + width -= len; + if( pad & _PRINTFMT_PAD_ZERO ) + padchar = '0'; + } + if( !( pad & _PRINTFMT_PAD_RIGHT ) ) { + for( ; width > 0; --width ) { + __print_char( ctx, padchar ); + ++pc; + } + } + + // The string to print is not the result of a number conversion to ascii. + if( false == is_number ) { + // For a string, printlimit is the max number of characters to display. + for( ; print_limit && *string; ++string, --print_limit ) { + __print_char( ctx, *string ); + ++pc; + } + } + + // The string to print represents an integer number. + if( true == is_number ) { + // In this case, printlimit is the min number of digits to print. + + // If the length of the number to print is less than the min nb of i + // digits to display, we add 0 before printing the number. + len = strlen( string ); + if( len < print_limit ) { + i = print_limit - len; + for( ; i; i-- ) { + __print_char( ctx, '0' ); + ++pc; + } + } + } + + /* + * Else: The string to print is not the result of a number conversion to ascii. + * For a string, printlimit is the max number of characters to display. + */ + for( ; print_limit && *string; ++string, --print_limit ) { + __print_char( ctx, *string ); + ++pc; + } + + for( ; width > 0; --width ) { + __print_char( ctx, padchar ); + ++pc; + } + + return pc; +} + +/** + * Print a number to the given string, with the given base. + */ +static int __print_int( _print_ctx_t* ctx, + int i, + int b, + int sg, + int width, + int pad, + int letbase, + int print_limit ) +{ + char print_buf[_PRINTFMT_INT_BUF_LEN]; + register char *s; + register int t, neg = 0, pc = 0; + register unsigned int u = i; + + if( i == 0 ) { + print_buf[0] = '0'; + print_buf[1] = '\0'; + return __print_str( ctx, print_buf, width, pad, print_limit, true ); + } + + if( sg && b == 10 && i < 0 ) { + neg = 1; + u = -i; + } + + s = print_buf + _PRINTFMT_INT_BUF_LEN - 1; + *s = '\0'; + + while( u ) { + t = u % b; + if( t >= 10 ) + t += letbase - '0' - 10; + *--s = t + '0'; + u /= b; + } + + if( neg ) { + if( width && ( pad & _PRINTFMT_PAD_ZERO ) ) { + __print_char( ctx, '-' ); + ++pc; + --width; + } else { + *--s = '-'; + } + } + + return pc + __print_str( ctx, s, width, pad, print_limit, true ); +} +/* +#if __GNUC__ +int fprintf( __FILE *stream, const char *format, ... ) +{ + return 0; +} +#endif +*/ + +/** + * Print the given arguments, with given format onto string out. + */ +static int __print_fmt( _print_ctx_t* ctx, const char *format, va_list args ) +{ + int width; + int pad; + int print_limit; + int pc = 0; + char scr[2]; + + for( ; *format != 0; ++format ) { + if( *format == '%' ) { + ++format; + width = pad = print_limit = 0; + + if( *format == '\0' ) { + break; + } + + if( *format == '%' ) { + goto out; + } + + if( *format == '-' ) { + ++format; + pad = _PRINTFMT_PAD_RIGHT; + } + + while( *format == '0' ) { + ++format; + pad |= _PRINTFMT_PAD_ZERO; + } + + for( ; *format >= '0' && *format <= '9'; ++format ) { + width *= 10; + width += *format - '0'; + } + + if( *format == '.' ) { + ++format; + for( ; *format >= '0' && *format <= '9'; ++format ) { + print_limit *= 10; + print_limit += *format - '0'; + } + } + + if( 0 == print_limit ) { + print_limit--; + } + + if( *format == 'l' ) { + ++format; + } + + if( *format == 's' ) { + register char *s = (char *) va_arg( args, int ); + pc += __print_str( ctx, + s ? s : "(null)", + width, + pad, + print_limit, + false ); + continue; + } + + if( *format == 'd' ) { + pc += __print_int( ctx, va_arg( args, int ), 10, 1, width, pad, 'a', print_limit ); + continue; + } + + if( ( *format == 'x' ) || ( *format == 'p' ) ) { + pc += __print_int( ctx, va_arg( args, int ), 16, 0, width, pad, 'a', print_limit ); + continue; + } + + if( *format == 'X' ) { + pc += __print_int( ctx, va_arg( args, int ), 16, 0, width, pad, 'A', print_limit ); + continue; + } + + if( *format == 'u' ) { + pc += __print_int( ctx, va_arg( args, int ), 10, 0, width, pad, 'a', print_limit ); + continue; + } + + if( *format == 'c' ) { + // char are converted to int then pushed on the stack + scr[0] = (char) va_arg( args, int ); + scr[1] = '\0'; + pc += __print_str( ctx, scr, width, pad, print_limit, false ); + continue; + } + } else { +out: + __print_char( ctx, *format ); + ++pc; + } + } + + if( ctx && ctx->_max_len ) { + *(ctx->_ptr) = '\0'; + } + + return pc; +} + +/* +int sprintf( char *out, const char *format, ... ) +{ + int retval = 0; + _print_ctx_t ctx; + va_list args; + + ctx._ptr = out; + ctx._max_len = BLOCK_MEM_SIZE; + + va_start( args, format ); + retval = __print_fmt( &ctx, format, args ); + va_end( args ); + + return retval; +} +*/ + +int printf( const char *format, ... ) +{ + int retval = 0; +// memory_t* buf; + va_list args; + + /* + buf = memory_alloc( 10 ); + + if( buf ) { + _print_ctx_t ctx; + ctx._ptr = (char*)buf->_data; + ctx._max_len = BLOCK_MEM_SIZE; + + va_start( args, format ); + retval = __print_fmt( &ctx, format, args ); + va_end( args ); + + buf->_len = strlen( (const char*)buf->_data ); + +// LCD_WriteString( ll, buf ); + ll++; + ll &= 0x03; + if( uart_task_send( buf ) == false ) { + memory_free( buf ); + } + } + */ + + va_start( args, format ); + retval = __print_fmt( NULL, format, args ); + va_end( args ); + + return retval; +}