| /* | |
| 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. | |
| Changes for the FreeRTOS ports: | |
| - The dot in "%-8.8s" | |
| - The specifiers 'l' (long) and 'L' (long long) | |
| - The specifier 'u' for unsigned | |
| - Dot notation for IP addresses: | |
| sprintf("IP = %xip\n", 0xC0A80164); | |
| will produce "IP = 192.168.1.100\n" | |
| */ | |
| #include <stdarg.h> | |
| #include <stdio.h> | |
| #include <stdlib.h> | |
| #include <string.h> | |
| #include "FreeRTOS.h" | |
| #define PAD_RIGHT 1 | |
| #define PAD_ZERO 2 | |
| /* | |
| * Return 1 for readable, 2 for writeable, 3 for both. | |
| * Function must be provided by the application. | |
| */ | |
| extern BaseType_t xApplicationMemoryPermissions( uint32_t aAddress ); | |
| extern void vOutputChar( const char cChar, const TickType_t xTicksToWait ); | |
| static const TickType_t xTicksToWait = pdMS_TO_TICKS( 20 ); | |
| struct xPrintFlags | |
| { | |
| int base; | |
| int width; | |
| int printLimit; | |
| unsigned | |
| pad : 8, | |
| letBase : 8, | |
| isSigned : 1, | |
| isNumber : 1, | |
| long32 : 1, | |
| long64 : 1; | |
| }; | |
| struct SStringBuf | |
| { | |
| char *str; | |
| const char *orgStr; | |
| const char *nulPos; | |
| int curLen; | |
| struct xPrintFlags flags; | |
| }; | |
| static void strbuf_init( struct SStringBuf *apStr, char *apBuf, const char *apMaxStr ) | |
| { | |
| apStr->str = apBuf; | |
| apStr->orgStr = apBuf; | |
| apStr->nulPos = apMaxStr-1; | |
| apStr->curLen = 0; | |
| memset( &apStr->flags, '\0', sizeof( apStr->flags ) ); | |
| } | |
| /*-----------------------------------------------------------*/ | |
| static BaseType_t strbuf_printchar( struct SStringBuf *apStr, int c ) | |
| { | |
| if( apStr->str == NULL ) | |
| { | |
| vOutputChar( ( char ) c, xTicksToWait ); | |
| apStr->curLen++; | |
| return pdTRUE; | |
| } | |
| if( apStr->str < apStr->nulPos ) | |
| { | |
| *( apStr->str++ ) = c; | |
| apStr->curLen++; | |
| return pdTRUE; | |
| } | |
| if( apStr->str == apStr->nulPos ) | |
| { | |
| *( apStr->str++ ) = '\0'; | |
| } | |
| return pdFALSE; | |
| } | |
| /*-----------------------------------------------------------*/ | |
| static portINLINE BaseType_t strbuf_printchar_inline( struct SStringBuf *apStr, int c ) | |
| { | |
| if( apStr->str == NULL ) | |
| { | |
| vOutputChar( ( char ) c, xTicksToWait ); | |
| if( c == 0 ) | |
| { | |
| return pdFALSE; | |
| } | |
| apStr->curLen++; | |
| return pdTRUE; | |
| } | |
| if( apStr->str < apStr->nulPos ) | |
| { | |
| *(apStr->str++) = c; | |
| if( c == 0 ) | |
| { | |
| return pdFALSE; | |
| } | |
| apStr->curLen++; | |
| return pdTRUE; | |
| } | |
| if( apStr->str == apStr->nulPos ) | |
| { | |
| *( apStr->str++ ) = '\0'; | |
| } | |
| return pdFALSE; | |
| } | |
| /*-----------------------------------------------------------*/ | |
| static portINLINE int i2hex( int aCh ) | |
| { | |
| int iResult; | |
| if( aCh < 10 ) | |
| { | |
| iResult = '0' + aCh; | |
| } | |
| else | |
| { | |
| iResult = 'A' + aCh - 10; | |
| } | |
| return iResult; | |
| } | |
| /*-----------------------------------------------------------*/ | |
| static BaseType_t prints(struct SStringBuf *apBuf, const char *apString ) | |
| { | |
| register int padchar = ' '; | |
| int i,len; | |
| if( apBuf->flags.width > 0 ) | |
| { | |
| register int len = 0; | |
| register const char *ptr; | |
| for( ptr = apString; *ptr; ++ptr ) | |
| { | |
| ++len; | |
| } | |
| if( len >= apBuf->flags.width ) | |
| { | |
| apBuf->flags.width = 0; | |
| } | |
| else | |
| { | |
| apBuf->flags.width -= len; | |
| } | |
| if( apBuf->flags.pad & PAD_ZERO ) | |
| { | |
| padchar = '0'; | |
| } | |
| } | |
| if( ( apBuf->flags.pad & PAD_RIGHT ) == 0 ) | |
| { | |
| for( ; apBuf->flags.width > 0; --apBuf->flags.width ) | |
| { | |
| if( strbuf_printchar( apBuf, padchar ) == 0 ) | |
| { | |
| return pdFALSE; | |
| } | |
| } | |
| } | |
| if( ( apBuf->flags.isNumber == pdTRUE ) && ( apBuf->flags.pad == pdTRUE ) ) | |
| { | |
| /* The string to print represents an integer 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( apString ); | |
| if( len < apBuf->flags.printLimit ) | |
| { | |
| i = apBuf->flags.printLimit - len; | |
| for( ; i; i-- ) | |
| { | |
| if( strbuf_printchar( apBuf, '0' ) == 0 ) | |
| { | |
| return pdFALSE; | |
| } | |
| } | |
| } | |
| } | |
| /* 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( ; apBuf->flags.printLimit && *apString ; ++apString, --apBuf->flags.printLimit ) | |
| { | |
| if( !strbuf_printchar( apBuf, *apString ) ) | |
| { | |
| return pdFALSE; | |
| } | |
| } | |
| for( ; apBuf->flags.width > 0; --apBuf->flags.width ) | |
| { | |
| if( !strbuf_printchar( apBuf, padchar ) ) | |
| { | |
| return pdFALSE; | |
| } | |
| } | |
| return pdTRUE; | |
| } | |
| /*-----------------------------------------------------------*/ | |
| /* the following should be enough for 32 bit int */ | |
| #define PRINT_BUF_LEN 12 /* to print 4294967296 */ | |
| #if SPRINTF_LONG_LONG | |
| #warning 64-bit libraries will be included as well | |
| static BaseType_t printll( struct SStringBuf *apBuf, long long i ) | |
| { | |
| char print_buf[ 2 * PRINT_BUF_LEN ]; | |
| register char *s; | |
| register int t, neg = 0; | |
| register unsigned long long u = i; | |
| lldiv_t lldiv_result; | |
| /* typedef struct | |
| * { | |
| * long long int quot; // quotient | |
| * long long int rem; // remainder | |
| * } lldiv_t; | |
| */ | |
| apBuf->flags.isNumber = pdTRUE; /* Parameter for prints */ | |
| if( i == 0LL ) | |
| { | |
| print_buf[ 0 ] = '0'; | |
| print_buf[ 1 ] = '\0'; | |
| return prints( apBuf, print_buf ); | |
| } | |
| if( ( apBuf->flags.isSigned == pdTRUE ) && ( apBuf->flags.base == 10 ) && ( i < 0LL ) ) | |
| { | |
| neg = 1; | |
| u = -i; | |
| } | |
| s = print_buf + sizeof( print_buf ) - 1; | |
| *s = '\0'; | |
| /* 18446744073709551616 */ | |
| while( u != 0 ) | |
| { | |
| lldiv_result = lldiv( u, ( unsigned long long ) apBuf->flags.base ); | |
| t = lldiv_result.rem; | |
| if( t >= 10 ) | |
| { | |
| t += apBuf->flags.letBase - '0' - 10; | |
| } | |
| *( --s ) = t + '0'; | |
| u = lldiv_result.quot; | |
| } | |
| if( neg != 0 ) | |
| { | |
| if( ( apBuf->flags.width != 0 ) && ( apBuf->flags.pad & PAD_ZERO ) ) | |
| { | |
| if( !strbuf_printchar( apBuf, '-' ) ) | |
| { | |
| return pdFALSE; | |
| } | |
| --apBuf->flags.width; | |
| } | |
| else | |
| { | |
| *( --s ) = '-'; | |
| } | |
| } | |
| return prints( apBuf, s ); | |
| } | |
| #endif /* SPRINTF_LONG_LONG */ | |
| /*-----------------------------------------------------------*/ | |
| static BaseType_t printi( struct SStringBuf *apBuf, int i ) | |
| { | |
| char print_buf[ PRINT_BUF_LEN ]; | |
| register char *s; | |
| register int t, neg = 0; | |
| register unsigned int u = i; | |
| register unsigned base = apBuf->flags.base; | |
| apBuf->flags.isNumber = pdTRUE; /* Parameter for prints */ | |
| if( i == 0 ) | |
| { | |
| print_buf[ 0 ] = '0'; | |
| print_buf[ 1 ] = '\0'; | |
| return prints( apBuf, print_buf ); | |
| } | |
| if( ( apBuf->flags.isSigned == pdTRUE ) && ( base == 10 ) && ( i < 0 ) ) | |
| { | |
| neg = 1; | |
| u = -i; | |
| } | |
| s = print_buf + sizeof( print_buf ) - 1; | |
| *s = '\0'; | |
| switch( base ) | |
| { | |
| case 16: | |
| while( u != 0 ) | |
| { | |
| t = u & 0xF; | |
| if( t >= 10 ) | |
| { | |
| t += apBuf->flags.letBase - '0' - 10; | |
| } | |
| *( --s ) = t + '0'; | |
| u >>= 4; | |
| } | |
| break; | |
| case 8: | |
| case 10: | |
| /* GCC compiles very efficient */ | |
| while( u ) | |
| { | |
| t = u % base; | |
| *( --s ) = t + '0'; | |
| u /= base; | |
| } | |
| break; | |
| /* | |
| // The generic case, not yet in use | |
| default: | |
| while( u ) | |
| { | |
| t = u % base; | |
| if( t >= 10) | |
| { | |
| t += apBuf->flags.letBase - '0' - 10; | |
| } | |
| *( --s ) = t + '0'; | |
| u /= base; | |
| } | |
| break; | |
| */ | |
| } | |
| if( neg != 0 ) | |
| { | |
| if( apBuf->flags.width && (apBuf->flags.pad & PAD_ZERO ) ) | |
| { | |
| if( strbuf_printchar( apBuf, '-' ) == 0 ) | |
| { | |
| return pdFALSE; | |
| } | |
| --apBuf->flags.width; | |
| } | |
| else | |
| { | |
| *( --s ) = '-'; | |
| } | |
| } | |
| return prints( apBuf, s ); | |
| } | |
| /*-----------------------------------------------------------*/ | |
| static BaseType_t printIp(struct SStringBuf *apBuf, unsigned i ) | |
| { | |
| char print_buf[16]; | |
| sprintf( print_buf, "%u.%u.%u.%u", | |
| i >> 24, | |
| ( i >> 16 ) & 0xff, | |
| ( i >> 8 ) & 0xff, | |
| i & 0xff ); | |
| apBuf->flags.isNumber = pdTRUE; /* Parameter for prints */ | |
| prints( apBuf, print_buf ); | |
| return pdTRUE; | |
| } | |
| /*-----------------------------------------------------------*/ | |
| static void tiny_print( struct SStringBuf *apBuf, const char *format, va_list args ) | |
| { | |
| char scr[2]; | |
| for( ; ; ) | |
| { | |
| int ch = *( format++ ); | |
| if( ch != '%' ) | |
| { | |
| do | |
| { | |
| /* Put the most like flow in a small loop */ | |
| if( strbuf_printchar_inline( apBuf, ch ) == 0 ) | |
| { | |
| return; | |
| } | |
| ch = *( format++ ); | |
| } while( ch != '%' ); | |
| } | |
| ch = *( format++ ); | |
| /* Now ch has character after '%', format pointing to next */ | |
| if( ch == '\0' ) | |
| { | |
| break; | |
| } | |
| if( ch == '%' ) | |
| { | |
| if( strbuf_printchar( apBuf, ch ) == 0 ) | |
| { | |
| return; | |
| } | |
| continue; | |
| } | |
| memset( &apBuf->flags, '\0', sizeof( apBuf->flags ) ); | |
| if( ch == '-' ) | |
| { | |
| ch = *( format++ ); | |
| apBuf->flags.pad = PAD_RIGHT; | |
| } | |
| while( ch == '0' ) | |
| { | |
| ch = *( format++ ); | |
| apBuf->flags.pad |= PAD_ZERO; | |
| } | |
| if( ch == '*' ) | |
| { | |
| ch = *( format++ ); | |
| apBuf->flags.width = va_arg( args, int ); | |
| } | |
| else | |
| { | |
| while( ch >= '0' && ch <= '9' ) | |
| { | |
| apBuf->flags.width *= 10; | |
| apBuf->flags.width += ch - '0'; | |
| ch = *( format++ ); | |
| } | |
| } | |
| if( ch == '.' ) | |
| { | |
| ch = *( format++ ); | |
| if( ch == '*' ) | |
| { | |
| apBuf->flags.printLimit = va_arg( args, int ); | |
| ch = *( format++ ); | |
| } | |
| else | |
| { | |
| while( ch >= '0' && ch <= '9' ) | |
| { | |
| apBuf->flags.printLimit *= 10; | |
| apBuf->flags.printLimit += ch - '0'; | |
| ch = *( format++ ); | |
| } | |
| } | |
| } | |
| if( apBuf->flags.printLimit == 0 ) | |
| { | |
| apBuf->flags.printLimit--; /* -1: make it unlimited */ | |
| } | |
| if( ch == 's' ) | |
| { | |
| register char *s = ( char * )va_arg( args, int ); | |
| if( prints( apBuf, s ? s : "(null)" ) == 0 ) | |
| { | |
| break; | |
| } | |
| continue; | |
| } | |
| if( ch == 'c' ) | |
| { | |
| /* char are converted to int then pushed on the stack */ | |
| scr[0] = ( char ) va_arg( args, int ); | |
| if( strbuf_printchar( apBuf, scr[0] ) == 0 ) | |
| { | |
| return; | |
| } | |
| continue; | |
| } | |
| if( ch == 'l' ) | |
| { | |
| ch = *( format++ ); | |
| apBuf->flags.long32 = 1; | |
| /* Makes not difference as u32 == long */ | |
| } | |
| if( ch == 'L' ) | |
| { | |
| ch = *( format++ ); | |
| apBuf->flags.long64 = 1; | |
| /* Does make a difference */ | |
| } | |
| apBuf->flags.base = 10; | |
| apBuf->flags.letBase = 'a'; | |
| if( ch == 'd' || ch == 'u' ) | |
| { | |
| apBuf->flags.isSigned = ( ch == 'd' ); | |
| #if SPRINTF_LONG_LONG | |
| if( apBuf->flags.long64 != pdFALSE ) | |
| { | |
| if( printll( apBuf, va_arg( args, long long ) ) == 0 ) | |
| { | |
| break; | |
| } | |
| } else | |
| #endif /* SPRINTF_LONG_LONG */ | |
| if( printi( apBuf, va_arg( args, int ) ) == 0 ) | |
| { | |
| break; | |
| } | |
| continue; | |
| } | |
| apBuf->flags.base = 16; /* From here all hexadecimal */ | |
| if( ch == 'x' && format[0] == 'i' && format[1] == 'p' ) | |
| { | |
| format += 2; /* eat the "xi" of "xip" */ | |
| /* Will use base 10 again */ | |
| if( printIp( apBuf, va_arg( args, int ) ) == 0 ) | |
| { | |
| break; | |
| } | |
| continue; | |
| } | |
| if( ch == 'x' || ch == 'X' || ch == 'p' || ch == 'o' ) | |
| { | |
| if( ch == 'X' ) | |
| { | |
| apBuf->flags.letBase = 'A'; | |
| } | |
| else if( ch == 'o' ) | |
| { | |
| apBuf->flags.base = 8; | |
| } | |
| #if SPRINTF_LONG_LONG | |
| if( apBuf->flags.long64 != pdFALSE ) | |
| { | |
| if( printll( apBuf, va_arg( args, long long ) ) == 0 ) | |
| { | |
| break; | |
| } | |
| } else | |
| #endif /* SPRINTF_LONG_LONG */ | |
| if( printi( apBuf, va_arg( args, int ) ) == 0 ) | |
| { | |
| break; | |
| } | |
| continue; | |
| } | |
| } | |
| strbuf_printchar( apBuf, '\0' ); | |
| } | |
| /*-----------------------------------------------------------*/ | |
| int tiny_printf( const char *format, ... ) | |
| { | |
| va_list args; | |
| va_start( args, format ); | |
| struct SStringBuf strBuf; | |
| strbuf_init( &strBuf, NULL, ( const char* )NULL ); | |
| tiny_print( &strBuf, format, args ); | |
| va_end( args ); | |
| return strBuf.curLen; | |
| } | |
| /*-----------------------------------------------------------*/ | |
| int vsnprintf( char *apBuf, size_t aMaxLen, const char *apFmt, va_list args ) | |
| { | |
| struct SStringBuf strBuf; | |
| strbuf_init( &strBuf, apBuf, ( const char* )apBuf + aMaxLen ); | |
| tiny_print( &strBuf, apFmt, args ); | |
| return strBuf.curLen; | |
| } | |
| /*-----------------------------------------------------------*/ | |
| int snprintf( char *apBuf, size_t aMaxLen, const char *apFmt, ... ) | |
| { | |
| va_list args; | |
| va_start( args, apFmt ); | |
| struct SStringBuf strBuf; | |
| strbuf_init( &strBuf, apBuf, ( const char* )apBuf + aMaxLen ); | |
| tiny_print( &strBuf, apFmt, args ); | |
| va_end( args ); | |
| return strBuf.curLen; | |
| } | |
| /*-----------------------------------------------------------*/ | |
| int sprintf( char *apBuf, const char *apFmt, ... ) | |
| { | |
| va_list args; | |
| va_start( args, apFmt ); | |
| struct SStringBuf strBuf; | |
| strbuf_init( &strBuf, apBuf, ( const char * )apBuf + 1024 ); | |
| tiny_print( &strBuf, apFmt, args ); | |
| va_end( args ); | |
| return strBuf.curLen; | |
| } | |
| /*-----------------------------------------------------------*/ | |
| int vsprintf( char *apBuf, const char *apFmt, va_list args ) | |
| { | |
| struct SStringBuf strBuf; | |
| strbuf_init( &strBuf, apBuf, ( const char* ) apBuf + 1024 ); | |
| tiny_print( &strBuf, apFmt, args ); | |
| return strBuf.curLen; | |
| } | |
| /*-----------------------------------------------------------*/ | |
| const char *mkSize (unsigned long long aSize, char *apBuf, int aLen) | |
| { | |
| static char retString[33]; | |
| size_t gb, mb, kb, sb; | |
| if (apBuf == NULL) { | |
| apBuf = retString; | |
| aLen = sizeof( retString ); | |
| } | |
| gb = aSize / (1024*1024*1024); | |
| aSize -= gb * (1024*1024*1024); | |
| mb = aSize / (1024*1024); | |
| aSize -= mb * (1024*1024); | |
| kb = aSize / (1024); | |
| aSize -= kb * (1024); | |
| sb = aSize; | |
| if( gb ) | |
| { | |
| snprintf (apBuf, aLen, "%u.%02u GB", ( unsigned ) gb, ( unsigned ) ( ( 100 * mb ) / 1024ul ) ); | |
| } | |
| else if( mb ) | |
| { | |
| snprintf (apBuf, aLen, "%u.%02u MB", ( unsigned ) mb, ( unsigned ) ( ( 100 * kb) / 1024ul ) ); | |
| } | |
| else if( kb != 0ul ) | |
| { | |
| snprintf (apBuf, aLen, "%u.%02u KB", ( unsigned ) kb, ( unsigned ) ( ( 100 * sb) / 1024ul ) ); | |
| } | |
| else | |
| { | |
| snprintf (apBuf, aLen, "%u bytes", ( unsigned ) sb); | |
| } | |
| return apBuf; | |
| } | |