You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
754 lines
17 KiB
C
754 lines
17 KiB
C
/*
|
|
* 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( xApplicationMemoryPermissions( ( uint32_t ) apString ) == 0 )
|
|
{
|
|
/* The user has probably made a mistake with the parameter
|
|
* for '%s', the memory is not readbale. */
|
|
apString = "INV_MEM";
|
|
}
|
|
|
|
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 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;
|
|
}
|