--- /dev/null
+/*\r
+ Copyright 2001, 2002 Georges Menie (www.menie.org)\r
+ stdarg version contributed by Christian Ettinger\r
+\r
+ This program is free software; you can redistribute it and/or modify\r
+ it under the terms of the GNU Lesser General Public License as published by\r
+ the Free Software Foundation; either version 2 of the License, or\r
+ (at your option) any later version.\r
+\r
+ Changes for the FreeRTOS ports:\r
+\r
+ - The dot in "%-8.8s"\r
+ - The specifiers 'l' (long) and 'L' (long long)\r
+ - The specifier 'u' for unsigned\r
+ - Dot notation for IP addresses:\r
+ sprintf("IP = %xip\n", 0xC0A80164);\r
+ will produce "IP = 192.168.1.100\n"\r
+*/\r
+\r
+#include <stdarg.h>\r
+#include <stdio.h>\r
+#include <stdlib.h>\r
+#include <string.h>\r
+\r
+#include "FreeRTOS.h"\r
+\r
+#define PAD_RIGHT 1\r
+#define PAD_ZERO 2\r
+\r
+/*\r
+ * Return 1 for readable, 2 for writeable, 3 for both.\r
+ * Function must be provided by the application.\r
+ */\r
+extern BaseType_t xApplicationMemoryPermissions( uint32_t aAddress );\r
+\r
+extern void vOutputChar( const char cChar, const TickType_t xTicksToWait );\r
+static const TickType_t xTicksToWait = pdMS_TO_TICKS( 20 );\r
+\r
+struct xPrintFlags\r
+{\r
+ int base;\r
+ int width;\r
+ int printLimit;\r
+ unsigned\r
+ pad : 8,\r
+ letBase : 8,\r
+ isSigned : 1,\r
+ isNumber : 1,\r
+ long32 : 1,\r
+ long64 : 1;\r
+};\r
+\r
+struct SStringBuf\r
+{\r
+ char *str;\r
+ const char *orgStr;\r
+ const char *nulPos;\r
+ int curLen;\r
+ struct xPrintFlags flags;\r
+};\r
+\r
+static void strbuf_init( struct SStringBuf *apStr, char *apBuf, const char *apMaxStr )\r
+{\r
+ apStr->str = apBuf;\r
+ apStr->orgStr = apBuf;\r
+ apStr->nulPos = apMaxStr-1;\r
+ apStr->curLen = 0;\r
+\r
+ memset( &apStr->flags, '\0', sizeof( apStr->flags ) );\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+static BaseType_t strbuf_printchar( struct SStringBuf *apStr, int c )\r
+{\r
+ if( apStr->str == NULL )\r
+ {\r
+ vOutputChar( ( char ) c, xTicksToWait );\r
+ apStr->curLen++;\r
+ return pdTRUE;\r
+ }\r
+ if( apStr->str < apStr->nulPos )\r
+ {\r
+ *( apStr->str++ ) = c;\r
+ apStr->curLen++;\r
+ return pdTRUE;\r
+ }\r
+ if( apStr->str == apStr->nulPos )\r
+ {\r
+ *( apStr->str++ ) = '\0';\r
+ }\r
+ return pdFALSE;\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+static portINLINE BaseType_t strbuf_printchar_inline( struct SStringBuf *apStr, int c )\r
+{\r
+ if( apStr->str == NULL )\r
+ {\r
+ vOutputChar( ( char ) c, xTicksToWait );\r
+ if( c == 0 )\r
+ {\r
+ return pdFALSE;\r
+ }\r
+ apStr->curLen++;\r
+ return pdTRUE;\r
+ }\r
+ if( apStr->str < apStr->nulPos )\r
+ {\r
+ *(apStr->str++) = c;\r
+ if( c == 0 )\r
+ {\r
+ return pdFALSE;\r
+ }\r
+ apStr->curLen++;\r
+ return pdTRUE;\r
+ }\r
+ if( apStr->str == apStr->nulPos )\r
+ {\r
+ *( apStr->str++ ) = '\0';\r
+ }\r
+ return pdFALSE;\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+static portINLINE int i2hex( int aCh )\r
+{\r
+int iResult;\r
+\r
+ if( aCh < 10 )\r
+ {\r
+ iResult = '0' + aCh;\r
+ }\r
+ else\r
+ {\r
+ iResult = 'A' + aCh - 10;\r
+ }\r
+\r
+ return iResult;\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+static BaseType_t prints(struct SStringBuf *apBuf, const char *apString )\r
+{\r
+ register int padchar = ' ';\r
+ int i,len;\r
+\r
+ if( xApplicationMemoryPermissions( ( uint32_t )apString ) == 0 )\r
+ {\r
+ /* The user has probably made a mistake with the parameter\r
+ for '%s', the memory is not readbale. */\r
+ apString = "INV_MEM";\r
+ }\r
+\r
+ if( apBuf->flags.width > 0 )\r
+ {\r
+ register int len = 0;\r
+ register const char *ptr;\r
+ for( ptr = apString; *ptr; ++ptr )\r
+ {\r
+ ++len;\r
+ }\r
+\r
+ if( len >= apBuf->flags.width )\r
+ {\r
+ apBuf->flags.width = 0;\r
+ }\r
+ else\r
+ {\r
+ apBuf->flags.width -= len;\r
+ }\r
+\r
+ if( apBuf->flags.pad & PAD_ZERO )\r
+ {\r
+ padchar = '0';\r
+ }\r
+ }\r
+ if( ( apBuf->flags.pad & PAD_RIGHT ) == 0 )\r
+ {\r
+ for( ; apBuf->flags.width > 0; --apBuf->flags.width )\r
+ {\r
+ if( strbuf_printchar( apBuf, padchar ) == 0 )\r
+ {\r
+ return pdFALSE;\r
+ }\r
+ }\r
+ }\r
+ if( ( apBuf->flags.isNumber == pdTRUE ) && ( apBuf->flags.pad == pdTRUE ) )\r
+ {\r
+ /* The string to print represents an integer number.\r
+ * In this case, printLimit is the min number of digits to print\r
+ * If the length of the number to print is less than the min nb of i\r
+ * digits to display, we add 0 before printing the number\r
+ */\r
+ len = strlen( apString );\r
+\r
+ if( len < apBuf->flags.printLimit )\r
+ {\r
+ i = apBuf->flags.printLimit - len;\r
+ for( ; i; i-- )\r
+ {\r
+ if( strbuf_printchar( apBuf, '0' ) == 0 )\r
+ {\r
+ return pdFALSE;\r
+ }\r
+ }\r
+ }\r
+ }\r
+ /* The string to print is not the result of a number conversion to ascii.\r
+ * For a string, printLimit is the max number of characters to display\r
+ */\r
+ for( ; apBuf->flags.printLimit && *apString ; ++apString, --apBuf->flags.printLimit )\r
+ {\r
+ if( !strbuf_printchar( apBuf, *apString ) )\r
+ {\r
+ return pdFALSE;\r
+ }\r
+ }\r
+\r
+ for( ; apBuf->flags.width > 0; --apBuf->flags.width )\r
+ {\r
+ if( !strbuf_printchar( apBuf, padchar ) )\r
+ {\r
+ return pdFALSE;\r
+ }\r
+ }\r
+\r
+ return pdTRUE;\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+/* the following should be enough for 32 bit int */\r
+#define PRINT_BUF_LEN 12 /* to print 4294967296 */\r
+\r
+#if SPRINTF_LONG_LONG\r
+#warning 64-bit libraries will be included as well\r
+static BaseType_t printll( struct SStringBuf *apBuf, long long i )\r
+{\r
+ char print_buf[ 2 * PRINT_BUF_LEN ];\r
+ register char *s;\r
+ register int t, neg = 0;\r
+ register unsigned long long u = i;\r
+ lldiv_t lldiv_result;\r
+\r
+/* typedef struct\r
+ * {\r
+ * long long int quot; // quotient\r
+ * long long int rem; // remainder\r
+ * } lldiv_t;\r
+ */\r
+\r
+ apBuf->flags.isNumber = pdTRUE; /* Parameter for prints */\r
+ if( i == 0LL )\r
+ {\r
+ print_buf[ 0 ] = '0';\r
+ print_buf[ 1 ] = '\0';\r
+ return prints( apBuf, print_buf );\r
+ }\r
+\r
+ if( ( apBuf->flags.isSigned == pdTRUE ) && ( apBuf->flags.base == 10 ) && ( i < 0LL ) )\r
+ {\r
+ neg = 1;\r
+ u = -i;\r
+ }\r
+\r
+ s = print_buf + sizeof( print_buf ) - 1;\r
+\r
+ *s = '\0';\r
+ /* 18446744073709551616 */\r
+ while( u != 0 )\r
+ {\r
+ lldiv_result = lldiv( u, ( unsigned long long ) apBuf->flags.base );\r
+ t = lldiv_result.rem;\r
+ if( t >= 10 )\r
+ {\r
+ t += apBuf->flags.letBase - '0' - 10;\r
+ }\r
+ *( --s ) = t + '0';\r
+ u = lldiv_result.quot;\r
+ }\r
+\r
+ if( neg != 0 )\r
+ {\r
+ if( ( apBuf->flags.width != 0 ) && ( apBuf->flags.pad & PAD_ZERO ) )\r
+ {\r
+ if( !strbuf_printchar( apBuf, '-' ) )\r
+ {\r
+ return pdFALSE;\r
+ }\r
+ --apBuf->flags.width;\r
+ }\r
+ else\r
+ {\r
+ *( --s ) = '-';\r
+ }\r
+ }\r
+\r
+ return prints( apBuf, s );\r
+}\r
+#endif /* SPRINTF_LONG_LONG */\r
+/*-----------------------------------------------------------*/\r
+\r
+static BaseType_t printi( struct SStringBuf *apBuf, int i )\r
+{\r
+ char print_buf[ PRINT_BUF_LEN ];\r
+ register char *s;\r
+ register int t, neg = 0;\r
+ register unsigned int u = i;\r
+ register unsigned base = apBuf->flags.base;\r
+\r
+ apBuf->flags.isNumber = pdTRUE; /* Parameter for prints */\r
+\r
+ if( i == 0 )\r
+ {\r
+ print_buf[ 0 ] = '0';\r
+ print_buf[ 1 ] = '\0';\r
+ return prints( apBuf, print_buf );\r
+ }\r
+\r
+ if( ( apBuf->flags.isSigned == pdTRUE ) && ( base == 10 ) && ( i < 0 ) )\r
+ {\r
+ neg = 1;\r
+ u = -i;\r
+ }\r
+\r
+ s = print_buf + sizeof( print_buf ) - 1;\r
+\r
+ *s = '\0';\r
+ switch( base )\r
+ {\r
+ case 16:\r
+ while( u != 0 )\r
+ {\r
+ t = u & 0xF;\r
+ if( t >= 10 )\r
+ {\r
+ t += apBuf->flags.letBase - '0' - 10;\r
+ }\r
+ *( --s ) = t + '0';\r
+ u >>= 4;\r
+ }\r
+ break;\r
+\r
+ case 8:\r
+ case 10:\r
+ /* GCC compiles very efficient */\r
+ while( u )\r
+ {\r
+ t = u % base;\r
+ *( --s ) = t + '0';\r
+ u /= base;\r
+ }\r
+ break;\r
+/*\r
+ // The generic case, not yet in use\r
+ default:\r
+ while( u )\r
+ {\r
+ t = u % base;\r
+ if( t >= 10)\r
+ {\r
+ t += apBuf->flags.letBase - '0' - 10;\r
+ }\r
+ *( --s ) = t + '0';\r
+ u /= base;\r
+ }\r
+ break;\r
+*/\r
+ }\r
+\r
+ if( neg != 0 )\r
+ {\r
+ if( apBuf->flags.width && (apBuf->flags.pad & PAD_ZERO ) )\r
+ {\r
+ if( strbuf_printchar( apBuf, '-' ) == 0 )\r
+ {\r
+ return pdFALSE;\r
+ }\r
+ --apBuf->flags.width;\r
+ }\r
+ else\r
+ {\r
+ *( --s ) = '-';\r
+ }\r
+ }\r
+\r
+ return prints( apBuf, s );\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+static BaseType_t printIp(struct SStringBuf *apBuf, unsigned i )\r
+{\r
+ char print_buf[16];\r
+\r
+ sprintf( print_buf, "%u.%u.%u.%u",\r
+ i >> 24,\r
+ ( i >> 16 ) & 0xff,\r
+ ( i >> 8 ) & 0xff,\r
+ i & 0xff );\r
+ apBuf->flags.isNumber = pdTRUE; /* Parameter for prints */\r
+ prints( apBuf, print_buf );\r
+\r
+ return pdTRUE;\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+static void tiny_print( struct SStringBuf *apBuf, const char *format, va_list args )\r
+{\r
+ char scr[2];\r
+\r
+ for( ; ; )\r
+ {\r
+ int ch = *( format++ );\r
+\r
+ if( ch != '%' )\r
+ {\r
+ do\r
+ {\r
+ /* Put the most like flow in a small loop */\r
+ if( strbuf_printchar_inline( apBuf, ch ) == 0 )\r
+ {\r
+ return;\r
+ }\r
+ ch = *( format++ );\r
+ } while( ch != '%' );\r
+ }\r
+ ch = *( format++ );\r
+ /* Now ch has character after '%', format pointing to next */\r
+\r
+ if( ch == '\0' )\r
+ {\r
+ break;\r
+ }\r
+ if( ch == '%' )\r
+ {\r
+ if( strbuf_printchar( apBuf, ch ) == 0 )\r
+ {\r
+ return;\r
+ }\r
+ continue;\r
+ }\r
+ memset( &apBuf->flags, '\0', sizeof( apBuf->flags ) );\r
+\r
+ if( ch == '-' )\r
+ {\r
+ ch = *( format++ );\r
+ apBuf->flags.pad = PAD_RIGHT;\r
+ }\r
+ while( ch == '0' )\r
+ {\r
+ ch = *( format++ );\r
+ apBuf->flags.pad |= PAD_ZERO;\r
+ }\r
+ if( ch == '*' )\r
+ {\r
+ ch = *( format++ );\r
+ apBuf->flags.width = va_arg( args, int );\r
+ }\r
+ else\r
+ {\r
+ while( ch >= '0' && ch <= '9' )\r
+ {\r
+ apBuf->flags.width *= 10;\r
+ apBuf->flags.width += ch - '0';\r
+ ch = *( format++ );\r
+ }\r
+ }\r
+ if( ch == '.' )\r
+ {\r
+ ch = *( format++ );\r
+ if( ch == '*' )\r
+ {\r
+ apBuf->flags.printLimit = va_arg( args, int );\r
+ ch = *( format++ );\r
+ }\r
+ else\r
+ {\r
+ while( ch >= '0' && ch <= '9' )\r
+ {\r
+ apBuf->flags.printLimit *= 10;\r
+ apBuf->flags.printLimit += ch - '0';\r
+ ch = *( format++ );\r
+ }\r
+ }\r
+ }\r
+ if( apBuf->flags.printLimit == 0 )\r
+ {\r
+ apBuf->flags.printLimit--; /* -1: make it unlimited */\r
+ }\r
+ if( ch == 's' )\r
+ {\r
+ register char *s = ( char * )va_arg( args, int );\r
+ if( prints( apBuf, s ? s : "(null)" ) == 0 )\r
+ {\r
+ break;\r
+ }\r
+ continue;\r
+ }\r
+ if( ch == 'c' )\r
+ {\r
+ /* char are converted to int then pushed on the stack */\r
+ scr[0] = ( char ) va_arg( args, int );\r
+\r
+ if( strbuf_printchar( apBuf, scr[0] ) == 0 )\r
+ {\r
+ return;\r
+ }\r
+\r
+ continue;\r
+ }\r
+ if( ch == 'l' )\r
+ {\r
+ ch = *( format++ );\r
+ apBuf->flags.long32 = 1;\r
+ /* Makes not difference as u32 == long */\r
+ }\r
+ if( ch == 'L' )\r
+ {\r
+ ch = *( format++ );\r
+ apBuf->flags.long64 = 1;\r
+ /* Does make a difference */\r
+ }\r
+ apBuf->flags.base = 10;\r
+ apBuf->flags.letBase = 'a';\r
+\r
+ if( ch == 'd' || ch == 'u' )\r
+ {\r
+ apBuf->flags.isSigned = ( ch == 'd' );\r
+#if SPRINTF_LONG_LONG\r
+ if( apBuf->flags.long64 != pdFALSE )\r
+ {\r
+ if( printll( apBuf, va_arg( args, long long ) ) == 0 )\r
+ {\r
+ break;\r
+ }\r
+ } else\r
+#endif /* SPRINTF_LONG_LONG */\r
+ if( printi( apBuf, va_arg( args, int ) ) == 0 )\r
+ {\r
+ break;\r
+ }\r
+ continue;\r
+ }\r
+\r
+ apBuf->flags.base = 16; /* From here all hexadecimal */\r
+\r
+ if( ch == 'x' && format[0] == 'i' && format[1] == 'p' )\r
+ {\r
+ format += 2; /* eat the "xi" of "xip" */\r
+ /* Will use base 10 again */\r
+ if( printIp( apBuf, va_arg( args, int ) ) == 0 )\r
+ {\r
+ break;\r
+ }\r
+ continue;\r
+ }\r
+ if( ch == 'x' || ch == 'X' || ch == 'p' || ch == 'o' )\r
+ {\r
+ if( ch == 'X' )\r
+ {\r
+ apBuf->flags.letBase = 'A';\r
+ }\r
+ else if( ch == 'o' )\r
+ {\r
+ apBuf->flags.base = 8;\r
+ }\r
+#if SPRINTF_LONG_LONG\r
+ if( apBuf->flags.long64 != pdFALSE )\r
+ {\r
+ if( printll( apBuf, va_arg( args, long long ) ) == 0 )\r
+ {\r
+ break;\r
+ }\r
+ } else\r
+#endif /* SPRINTF_LONG_LONG */\r
+ if( printi( apBuf, va_arg( args, int ) ) == 0 )\r
+ {\r
+ break;\r
+ }\r
+ continue;\r
+ }\r
+ }\r
+ strbuf_printchar( apBuf, '\0' );\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+int vsnprintf( char *apBuf, size_t aMaxLen, const char *apFmt, va_list args )\r
+{\r
+ struct SStringBuf strBuf;\r
+ strbuf_init( &strBuf, apBuf, ( const char* )apBuf + aMaxLen );\r
+ tiny_print( &strBuf, apFmt, args );\r
+\r
+ return strBuf.curLen;\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+int snprintf( char *apBuf, size_t aMaxLen, const char *apFmt, ... )\r
+{\r
+ va_list args;\r
+\r
+ va_start( args, apFmt );\r
+ struct SStringBuf strBuf;\r
+ strbuf_init( &strBuf, apBuf, ( const char* )apBuf + aMaxLen );\r
+ tiny_print( &strBuf, apFmt, args );\r
+ va_end( args );\r
+\r
+ return strBuf.curLen;\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+int sprintf( char *apBuf, const char *apFmt, ... )\r
+{\r
+ va_list args;\r
+\r
+ va_start( args, apFmt );\r
+ struct SStringBuf strBuf;\r
+ strbuf_init( &strBuf, apBuf, ( const char * )apBuf + 1024 );\r
+ tiny_print( &strBuf, apFmt, args );\r
+ va_end( args );\r
+\r
+ return strBuf.curLen;\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+int vsprintf( char *apBuf, const char *apFmt, va_list args )\r
+{\r
+ struct SStringBuf strBuf;\r
+ strbuf_init( &strBuf, apBuf, ( const char* ) apBuf + 1024 );\r
+ tiny_print( &strBuf, apFmt, args );\r
+\r
+ return strBuf.curLen;\r
+}\r
+/*-----------------------------------------------------------*/\r
+\r
+const char *mkSize (unsigned long long aSize, char *apBuf, int aLen)\r
+{\r
+static char retString[33];\r
+size_t gb, mb, kb, sb;\r
+\r
+ if (apBuf == NULL) {\r
+ apBuf = retString;\r
+ aLen = sizeof( retString );\r
+ }\r
+ gb = aSize / (1024*1024*1024);\r
+ aSize -= gb * (1024*1024*1024);\r
+ mb = aSize / (1024*1024);\r
+ aSize -= mb * (1024*1024);\r
+ kb = aSize / (1024);\r
+ aSize -= kb * (1024);\r
+ sb = aSize;\r
+ if( gb )\r
+ {\r
+ snprintf (apBuf, aLen, "%u.%02u GB", ( unsigned ) gb, ( unsigned ) ( ( 100 * mb ) / 1024ul ) );\r
+ }\r
+ else if( mb )\r
+ {\r
+ snprintf (apBuf, aLen, "%u.%02u MB", ( unsigned ) mb, ( unsigned ) ( ( 100 * kb) / 1024ul ) );\r
+ }\r
+ else if( kb != 0ul )\r
+ {\r
+ snprintf (apBuf, aLen, "%u.%02u KB", ( unsigned ) kb, ( unsigned ) ( ( 100 * sb) / 1024ul ) );\r
+ }\r
+ else\r
+ {\r
+ snprintf (apBuf, aLen, "%u bytes", ( unsigned ) sb);\r
+ }\r
+ return apBuf;\r
+}\r