FreeRTOS-Labs/Demo/FreeRTOS_IoT_Libraries/shadow/shadow_device_operations/printf-stdarg.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;
}