ssp_vsprintf.c

abstract rtos

#include "syscfg.h"
#if( CONFIG_OS_VXWORKS == TRUE )
#include <vxWorks.h>
#endif
#include "stdarg.h"
#include "glb_def.h"

#define AOS_ASSERT_FUNC(f,func)\
 \
do{\
    if( !(f) )\
   	{\
        ssp_assert( #f, __FILE__, __LINE__ );\
        func;\
   	}\
}while(0)

#define VA_LIST va_list
#define VA_ARG(list, type)\
        
#define VA_END(list)             va_end(list)
#define VA_START(list, last_arg) \
        

extern VOID ssp_assert( S8*cond, S8 *file, U32 line );

extern U32 aos_ntohl(U32 n);

#define _U	0x01	
#define _L	0x02	
#define _D	0x04	
#define _C	0x08	
#define _P	0x10	
#define _S	0x20	
#define _X	0x40	
#define _SP	0x80	

static unsigned char _ctype[] = {
_C,_C,_C,_C,_C,_C,_C,_C,			
_C,_C|_S,_C|_S,_C|_S,_C|_S,_C|_S,_C,_C,		
_C,_C,_C,_C,_C,_C,_C,_C,			
_C,_C,_C,_C,_C,_C,_C,_C,			
_S|_SP,_P,_P,_P,_P,_P,_P,_P,			
_P,_P,_P,_P,_P,_P,_P,_P,			
_D,_D,_D,_D,_D,_D,_D,_D,			
_D,_D,_P,_P,_P,_P,_P,_P,			
_P,_U|_X,_U|_X,_U|_X,_U|_X,_U|_X,_U|_X,_U,	
_U,_U,_U,_U,_U,_U,_U,_U,			
_U,_U,_U,_U,_U,_U,_U,_U,			
_U,_U,_U,_P,_P,_P,_P,_P,			
_P,_L|_X,_L|_X,_L|_X,_L|_X,_L|_X,_L|_X,_L,	
_L,_L,_L,_L,_L,_L,_L,_L,			
_L,_L,_L,_L,_L,_L,_L,_L,			
_L,_L,_L,_P,_P,_P,_P,_C,			
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,		
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,		
_S|_SP,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,   
_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,_P,       
_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,_U,       
_U,_U,_U,_U,_U,_U,_U,_P,_U,_U,_U,_U,_U,_U,_U,_L,       
_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,_L,       
_L,_L,_L,_L,_L,_L,_L,_P,_L,_L,_L,_L,_L,_L,_L,_L};      

#define __ismask(x) (_ctype[(int)(unsigned char)(x)])
#define isalnum(c)	((__ismask(c)&(_U|_L|_D)) != 0)
#define isalpha(c)	((__ismask(c)&(_U|_L)) != 0)
#define iscntrl(c)	((__ismask(c)&(_C)) != 0)
#define isdigit(c)	((__ismask(c)&(_D)) != 0)
#define isgraph(c)	((__ismask(c)&(_P|_U|_L|_D)) != 0)
#define islower(c)	((__ismask(c)&(_L)) != 0)
#define isprint(c)	((__ismask(c)&(_P|_U|_L|_D|_SP)) != 0)
#define ispunct(c)	((__ismask(c)&(_P)) != 0)
#define isspace(c)	((__ismask(c)&(_S)) != 0)
#define isupper(c)	((__ismask(c)&(_U)) != 0)
#define isxdigit(c)	((__ismask(c)&(_D|_X)) != 0)

#ifndef isascii
#define isascii(c) (((unsigned char)(c))<=0x7f)
#endif

#ifndef toascii
#define toascii(c) (((unsigned char)(c))&0x7f)
#endif

static unsigned char __tolower(unsigned char c)
{
	if (isupper(c))
		c -= 'A'-'a';
	return c;
}

static unsigned char __toupper(unsigned char c)
{
	if (islower(c))
		c -= 'a'-'A';
	return c;
}

#define tolower(c) __tolower(c)
#define toupper(c) __toupper(c)

#define INT_MAX		((int)(~0U>>1))

#define do_div(n,base) ({ \
int __res; \
__res = ((unsigned long) n) % (unsigned) base; \
n = ((unsigned long) n) / (unsigned) base; \
__res; })


U32 simple_strtoul(const S8 *cp,S8 **endp,U32 base)
{
	U32 result = 0,value;
    
	if(16==base) {
		if (*cp == '0') {
			cp++;
			if ((*cp == 'x') || (*cp == 'X')) {
				cp++;
			}
		}
	}
	if (!base) {
		base = 10;
		if (*cp == '0') {
			base = 8;
			cp++;
			if ((*cp == 'x') && isxdigit(cp[1])) {
				cp++;
				base = 16;
			}
		}
	}
	while (isxdigit(*cp) &&
	       (value = isdigit(*cp) ? *cp-'0' : toupper(*cp)-'A'+10) < base) {
		result = result*base + value;
		cp++;
	}
	if (endp)
		*endp = (S8 *)cp;
	return result;
}

S32 simple_strtol(const S8 *cp,S8 **endp,U32 base)
{
	if(*cp=='-')
		return 0-simple_strtoul(cp+1,endp,base);
	return simple_strtoul(cp,endp,base);
}
#if 0
unsigned long long simple_strtoull(const S8 *cp,S8 **endp,U32 base)
{
	unsigned long long result = 0,value;

	if (!base) {
		base = 10;
		if (*cp == '0') {
			base = 8;
			cp++;
			if ((*cp == 'x') && isxdigit(cp[1])) {
				cp++;
				base = 16;
			}
		}
	}
	while (isxdigit(*cp) && (value = isdigit(*cp) ? *cp-'0' : (islower(*cp)
	    ? toupper(*cp) : *cp)-'A'+10) < base) {
		result = result*base + value;
		cp++;
	}
	if (endp)
		*endp = (S8 *)cp;
	return result;
}

long long simple_strtoll(const S8 *cp,S8 **endp,U32 base)
{
	if(*cp=='-')
		return -simple_strtoull(cp+1,endp,base);
	return simple_strtoull(cp,endp,base);
}
#endif
static S32 skip_atoi(const S8 **s)
{
	S32 i=0;

	while (isdigit(**s))
		i = i*10 + *((*s)++) - '0';
	return i;
}

#define ZEROPAD	1		
#define SIGN	2		
#define PLUS	4		
#define SPACE	8		
#define LEFT	16		
#define SPECIAL	32		
#define LARGE	64		

static S8 * number(S8 * buf, S8 * end, S32 num, S32 base, S32 size, S32 precision, S32 type)
{
	S8 c,sign,tmp[66];
	const S8 *digits;
	const S8 small_digits[] = "0123456789abcdefghijklmnopqrstuvwxyz";
	const S8 large_digits[] = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
	S32 i;

	digits = (type & LARGE) ? large_digits : small_digits;
	if (type & LEFT)
		type &= ~ZEROPAD;
	if (base < 2 || base > 36)
		return 0;
	c = (type & ZEROPAD) ? '0' : ' ';
	sign = 0;
	if (type & SIGN) {
		if (num < 0) {
			sign = '-';
			num = -num;
			size--;
		} else if (type & PLUS) {
			sign = '+';
			size--;
		} else if (type & SPACE) {
			sign = ' ';
			size--;
		}
	}
	if (type & SPECIAL) {
		if (base == 16)
			size -= 2;
		else if (base == 8)
			size--;
	}
	i = 0;
	if (num == 0)
		tmp[i++]='0';
	else while (num != 0)
	{
		int __res;
		__res = ((unsigned long) num) % (unsigned) base;
		num = ((unsigned long) num) / (unsigned) base;
		tmp[i++] = digits[__res];
	}

	if (i > precision)
		precision = i;
	size -= precision;
	if (!(type&(ZEROPAD+LEFT))) {
		while(size-->0) {
			if (buf <= end)
				*buf = ' ';
			++buf;
		}
	}
	if (sign) {
		if (buf <= end)
			*buf = sign;
		++buf;
	}
	if (type & SPECIAL) {
		if (base==8) {
			if (buf <= end)
				*buf = '0';
			++buf;
		} else if (base==16) {
			if (buf <= end)
				*buf = '0';
			++buf;
			if (buf <= end)
				*buf = digits[33];
			++buf;
		}
	}
	if (!(type & LEFT)) {
		while (size-- > 0) {
			if (buf <= end)
				*buf = c;
			++buf;
		}
	}
	while (i < precision--) {
		if (buf <= end)
			*buf = '0';
		++buf;
	}
	while (i-- > 0) {
		if (buf <= end)
			*buf = tmp[i];
		++buf;
	}
	while (size-- > 0) {
		if (buf <= end)
			*buf = ' ';
		++buf;
	}
	return buf;
}

S32 aos_vsnprintf(S8 *buf, SIZE_T size, const S8 *fmt, va_list args)
{
	S32 len;
	U32 num;
	S32 i, base;
	S8 *str, *end, c;
	const S8 *s;

	S32 flags;		

	S32 field_width;	
	S32 precision;		/* min. # of digits for integers; max
				   number of chars for from string */
	S32 qualifier;		
				
				

	str = buf;
	end = buf + size - 1;

	if (end < buf - 1) {
		end = ((void *) -1);
		size = end - buf + 1;
	}

	for (; *fmt ; ++fmt) {
		if (*fmt != '%') {
			if (str <= end)
				*str = *fmt;
			++str;
			continue;
		}

		
		flags = 0;
		repeat:
			++fmt;		
			switch (*fmt) {
				case '-': flags |= LEFT; goto repeat;
				case '+': flags |= PLUS; goto repeat;
				case ' ': flags |= SPACE; goto repeat;
				case '#': flags |= SPECIAL; goto repeat;
				case '0': flags |= ZEROPAD; goto repeat;
			}

		
		field_width = -1;
		if (isdigit(*fmt))
			field_width = skip_atoi(&fmt);
		else if (*fmt == '*') {
			++fmt;
			
			field_width = VA_ARG(args, S32);
			if (field_width < 0) {
				field_width = -field_width;
				flags |= LEFT;
			}
		}

		
		precision = -1;
		if (*fmt == '.') {
			++fmt;	
			if (isdigit(*fmt))
				precision = skip_atoi(&fmt);
			else if (*fmt == '*') {
				++fmt;
				
				precision = VA_ARG(args, S32);
			}
			if (precision < 0)
				precision = 0;
		}

		
		qualifier = -1;
		if (*fmt == 'h' || *fmt == 'l'|| *fmt =='Z') {
			qualifier = *fmt;
			++fmt;
#if 0			
			if (qualifier == 'l' && *fmt == 'l') {
				qualifier = 'L';
				++fmt;
			}
#endif			
		}

		
		base = 10;

		switch (*fmt) {
			case 'c':
				if (!(flags & LEFT)) {
					while (--field_width > 0) {
						if (str <= end)
							*str = ' ';
						++str;
					}
				}
				c = (U8) VA_ARG(args, S32);
				if (str <= end)
					*str = c;
				++str;
				while (--field_width > 0) {
					if (str <= end)
						*str = ' ';
					++str;
				}
				continue;

			case 's':
				s = VA_ARG(args, S8 *);
				if (!s)
					s = "<NULL>";

				len = aos_strnlen(s, precision);

				if (!(flags & LEFT)) {
					while (len < field_width--) {
						if (str <= end)
						{
							if(flags&ZEROPAD)
							*str = '0';
							else
							*str = ' ';
						}

						++str;
					}
				}
				for (i = 0; i < len; ++i) {
					if (str <= end)
						*str = *s;
					++str; ++s;
				}
				while (len < field_width--) {
					if (str <= end)
					{
						if(flags&ZEROPAD)
						*str = '0';
						else
						*str = ' ';
					}

					++str;
				}
				continue;

			case 'p':
				if (field_width == -1) {
					field_width = 2*sizeof(void *);
					flags |= ZEROPAD;
				}
				str = number(str, end,
						(U32) VA_ARG(args, void *),
						16, field_width, precision, flags);
				continue;


			case 'n':
				if (qualifier == 'l') {
					S32 * ip = VA_ARG(args, S32 *);
					*ip = (str - buf);
				} else if (qualifier == 'Z') {
					SIZE_T * ip = VA_ARG(args, SIZE_T *);
					*ip = (str - buf);
				} else {
					S32 * ip = VA_ARG(args, S32 *);
					*ip = (str - buf);
				}
				continue;

			case '%':
				if (str <= end)
					*str = '%';
				++str;
				continue;

				
			case 'o':
				base = 8;
				break;

			case 'X':
				flags |= LARGE;
			case 'x':
				base = 16;
				break;

			case 'd':
			case 'i':
				flags |= SIGN;
			case 'u':
				break;

			case 'A':

				num = VA_ARG(args, U32);

                num = aos_ntohl(num);

				str = number(str, end, num >> 24, base,
        				field_width, precision, flags);
				if (str <= end)
    				*str++ = '.';
				str = number(str, end, (num >> 16)&0xff, base,
        				field_width, precision, flags);
				if (str <= end)
    				*str++ = '.';
				str = number(str, end, (num >> 8)&0xff, base,
        				field_width, precision, flags);
				if (str <= end)
    				*str++ = '.';
				str = number(str, end, num&0xff, base,
        				field_width, precision, flags);

				continue;

			default:
				if (str <= end)
					*str = '%';
				++str;
				if (*fmt) {
					if (str <= end)
						*str = *fmt;
					++str;
				} else {
					--fmt;
				}
				continue;
		}
#if 0		
		if (qualifier == 'L')
			num = VA_ARG(args, long long);
		else
#endif
        if (qualifier == 'l') {
			num = VA_ARG(args, U32);
			if (flags & SIGN)
				num = (S32) num;
		} else if (qualifier == 'Z') {
			num = VA_ARG(args, SIZE_T);
		} else if (qualifier == 'h') {
			num = (U16) VA_ARG(args, S32);
			if (flags & SIGN)
				num = (S16) num;
		} else {
			num = VA_ARG(args, U32);
			if (flags & SIGN)
				num = (S32) num;
		}
		str = number(str, end, num, base,
				field_width, precision, flags);
	}
	if (str <= end)
		*str = '\0';
	else if (size > 0)
		
		*end = '\0';
	if(*fmt)
	{
        AOS_ASSERT_FUNC(0, aos_printf( 0, "vsprintf,buf=0x%x room too small",buf ));
        aos_task_show_call_stack(0);
	}

	if (str <= end)
	{
    	return str-buf;
	}
    else
    {
        
        return ( (size>0) ? size-1:0 );
    }
}

S32 aos_snprintf(S8 * buf, SIZE_T size, const S8 *fmt, ...)
{
	va_list args;
	S32 i;

	VA_START(args, fmt);
	i=aos_vsnprintf(buf,size,fmt,args);
	VA_END(args);
	return i;
}

S32 aos_vsprintf(S8 *buf, const S8 *fmt, va_list args)
{
    U32 room;
    room = mem_obj_room(buf);
    if( !room )
    { 
        AOS_ASSERT_FUNC(0, aos_printf( 0, "vsprintf,buf=0x%x no room",buf ));
        aos_task_show_call_stack(0);
        return 0;
    }
	return aos_vsnprintf(buf, room, fmt, args);
}


S32 aos_sprintf(S8 * buf, const S8 *fmt, ...)
{
	va_list args;
	S32 i;

	VA_START(args, fmt);
	i=aos_vsprintf(buf,fmt,args);
	VA_END(args);
	return i;
}

S32 aos_vsscanf(const S8 * buf, const S8 * fmt, va_list args)
{
	const S8 *str = buf;
	S8 *next;
	S32 num = 0;
	S32 qualifier;
	S32 base;
	S32 field_width = -1;
	S32 is_sign = 0;

	while(*fmt && *str) {
		
		if (isspace(*fmt)) {
			while (isspace(*fmt))
				++fmt;
			while (isspace(*str))
				++str;
		}

		
		if (*fmt != '%' && *fmt) {
			if (*fmt++ != *str++)
				break;
			continue;
		}

		if (!*fmt)
			break;
		++fmt;
		
		if (*fmt == '*') {
			while (!isspace(*fmt) && *fmt)
				fmt++;
			while (!isspace(*str) && *str)
				str++;
			continue;
		}

		
		if (isdigit(*fmt))
			field_width = skip_atoi(&fmt);

		
		qualifier = -1;
		if (*fmt == 'h' || *fmt == 'l'|| *fmt == 'Z') {
			qualifier = *fmt;
			fmt++;
		}
		base = 10;
		is_sign = 0;

		if (!*fmt || !*str)
			break;

		switch(*fmt++) {
		case 'c':
		{
			S8 *s = (S8 *) VA_ARG(args,S8*);
			if (field_width == -1)
				field_width = 1;
			do {
				*s++ = *str++;
			} while(field_width-- > 0 && *str);
			num++;
		}
		continue;
		case 's':
		{
			S8 *s = (S8 *) VA_ARG(args, S8 *);
			if(field_width == -1)
				field_width = INT_MAX;
			
			while (isspace(*str))
				str++;

			
			while (*str && !isspace(*str) && field_width--) {
				*s++ = *str++;
			}
			*s = '\0';
			num++;
		}
		continue;
		case 'n':
			
		{
			S32 *i = (S32 *)VA_ARG(args,S32*);
			*i = str - buf;
		}
		continue;
		case 'o':
			base = 8;
			break;
		case 'x':
		case 'X':
			base = 16;
			break;
		case 'd':
		case 'i':
			is_sign = 1;
		case 'u':
			break;
		case '%':
			
			if (*str++ != '%') 
				return num;
			continue;
		default:
			
			return num;
		}

		while (isspace(*str))
			str++;
          
		if(!*str || !(isxdigit(*str) || (is_sign && ('-'==*str))))
			break;

		switch(qualifier) {
		case 'h':
			if (is_sign) {
				S16 *s = (S16 *) VA_ARG(args,S16 *);
				*s = (S16) simple_strtol(str,&next,base);
			} else {
				U16 *s = (U16 *) VA_ARG(args, U16 *);
				*s = (U16) simple_strtoul(str, &next, base);
			}
			break;
		case 'l':
			if (is_sign) {
				S32 *l = (S32 *) VA_ARG(args,S32 *);
				*l = simple_strtol(str,&next,base);
			} else {
				U32 *l = (U32*) VA_ARG(args,U32*);
				*l = simple_strtoul(str,&next,base);
			}
			break;
#if 0			
		case 'L':
			if (is_sign) {
				long long *l = (long long*) VA_ARG(args,long long *);
				*l = simple_strtoll(str,&next,base);
			} else {
				unsigned long long *l = (unsigned long long*) VA_ARG(args,unsigned long long*);
				*l = simple_strtoull(str,&next,base);
			}
			break;
#endif			
		case 'Z':
		{
			SIZE_T *s = (SIZE_T*) VA_ARG(args,SIZE_T*);
			*s = (SIZE_T) simple_strtoul(str,&next,base);
		}
		break;
		default:
			if (is_sign) {
				S32 *i = (S32 *) VA_ARG(args, S32*);
				*i = (S32) simple_strtol(str,&next,base);
			} else {
				U32 *i = (U32*) VA_ARG(args, U32*);
				*i = (U32) simple_strtoul(str,&next,base);
			}
			break;
		}
		num++;

		if (!next)
			break;
		str = next;
	}
	return num;
}

S32 aos_sscanf(const S8 * buf, const S8 * fmt, ...)
{
	va_list args;
	S32 i;

	VA_START(args,fmt);
	i = aos_vsscanf(buf,fmt,args);
	VA_END(args);
	return i;
}


U32 aos_strtoul(const S8 *nptr, S8 **endptr, S32 base)
{
	return simple_strtoul(nptr,endptr,base);
}