sprintf.c

基于ARM7的直流电机的驱动,还有FLASH驱动,LCD驱动等

#include <stdio.h>
#include <stdarg.h>
#include <limits.h>
#include <string.h>
#include "defs.h"

#define ULONG		unsigned long

#define MAXBUF	64
#define	PADSZ	16

#define FLG_LEFT	0x00000001
#define FLG_PLUS	0x00000002
#define FLG_SPACE	0x00000004
#define FLG_SHAP	0x00000008
#define FLG_ZERO	0x00000010
#define FLG_SIGNED	0x00000040
#define FLG_NEG		0x00000080

#define EMIT_C(c)	*out++ = c
#define EMIT_S(s,l)	do { Memcpy(out,s,l); out += l; }while(0)

typedef struct
{
  int		width;
  int		prec;
  const char	*pad_with;
  ULONG		flags;
  int		radix;
  ULONG		value;
  const char	*digits;
  char		number[MAXBUF];
}FMT_INFO;

static const char __bank[] = "                ";
static const char __zero[] = "0000000000000000";
char	__DIGITS[] = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
static const char __digits[] = "0123456789abcdefghijklmnopqrstuvwxyz";

static __inline int
isdigit(int c)
{
  return c >= '0' && c <= '9';	
}

static __inline const char*
_atoi(register const char *str,int *v)
{
  register int value = 0;
	
  while(isdigit(*str))
	value = value * 10 + *str++ - '0';
  *v = value;
  return str;
}

static int
_itoa(FMT_INFO *info)
{
  register ULONG v = info->value;
  char		number[MAXBUF];
  register char	*p =& number[MAXBUF];
  register const char *digs;
  int		nlen;
	
  digs = info->digits;
  switch(info->radix) {
	case 2://2进制
		do {
			*--p = digs[v & 1];
			v >>= 1;
		}while(v);
		break;
	case 8://8进制
		do {
			*--p = digs[v & 7];
			v >>= 3;
		}while(v);
		break;
	case 16://16进制
		do {
			*--p = digs[v & 15];
			v >>= 4;
		}while(v);
		break;
	case 10://10进制
		do {
			*--p = digs[v % 10];
			v /= 10;
		}while(v);
		break;
	default:
		do {
			*--p = digs[v % info->radix];
			v /= info->radix;
		}while(v);
  }
  nlen = &number[MAXBUF] - p;//实际长度
  if(info->width && info->width < nlen)
  	nlen = info->width;
  Memcpy(info->number,p,nlen);

  return nlen;
}

int
myvsprintf(char *out_ptr, const char *Afmt, va_list ap) {
  register char		*out = out_ptr;
  register const char	*fmt = Afmt;
  FMT_INFO		info;
  register int 		str_len;
  register const char	*str;
  
  while(*fmt)
  {
	while(*fmt && *fmt != '%')
		*out++ = *fmt++;

	if(!*fmt)
		goto ret;

	info.flags = 0;
	info.width = 0;
	info.prec = -1;
	info.digits = __digits;
	info.pad_with=__bank;

do_flags:
	fmt++;
	switch(*fmt)
	{
	case '-': info.flags |= FLG_LEFT; goto do_flags;
	case '+': info.flags |= FLG_PLUS; goto do_flags;
	case ' ': info.flags |= FLG_SPACE;goto do_flags;
	case '#': info.flags |= FLG_SHAP; goto do_flags;
	case '0': info.flags |= FLG_ZERO; goto do_flags;
	default: break;
	}

 	if(*fmt == '*')
	{
		info.width = va_arg(ap,int);
		if(info.width < 0)
		{
			info.width = -info.width;
			info.flags |= FLG_LEFT;
		}
		fmt++;
	}
	else
		fmt = _atoi(fmt,&info.width);

	if(*fmt == '.')
	{
		info.flags &= ~FLG_ZERO;

		if(*++fmt == '*')
		{
			info.prec = va_arg(ap,int);
			fmt++;
		}
		else
			fmt = _atoi(fmt,&info.prec);
	}

	if(*fmt == 'h' || *fmt == 'l')
		fmt++;

	switch(*fmt++)
	{
	case 's':
		{
		int pad;
		
		str = va_arg(ap,const char*);
		if(!str)
		{
			str_len = 6;
			str = "(null)";
		}
		else
			str_len = strlen(str);
out_string:
		pad = info.width - str_len;
		if(!(info.flags & FLG_LEFT) && pad > 0)/*PAD left*/
		{
			while(pad >= PADSZ)
			{
				EMIT_S(info.pad_with,PADSZ);
				pad -= PADSZ;
			}
			if(pad)
				EMIT_S(info.pad_with,pad);
		}

		EMIT_S(str,str_len);
		
		if((info.flags & FLG_LEFT) && pad > 0)
		{
			while(pad >= PADSZ)	/*PAD right*/
			{
				EMIT_S(__bank,PADSZ);
				pad -= PADSZ;
			}
			if(pad)
				EMIT_S(__bank,pad);
		}
		}
		break;

	case 'c':
		EMIT_C(va_arg(ap, int));
		break;
	case '%':
		EMIT_C('%');
		break;

	case 'd':
	case 'i':
		info.radix = 10;
		info.flags |= FLG_SIGNED;
out_number:
		{
		if(info.flags & FLG_SIGNED)
		{
			long t = va_arg(ap,long);
			if(t < 0)
			{
				info.flags |= FLG_NEG;
				info.value = (ULONG)-t;
			}
			else
				info.value = (ULONG)t;
		}
		else
			info.value = va_arg(ap,ULONG);

		info.pad_with = info.flags & FLG_ZERO ? __zero : __bank;
		str_len = _itoa(&info);
						
		if(info.flags & FLG_NEG)
		{
			info.width--;
			EMIT_C('-');
		}
		else if(info.flags & FLG_PLUS)
		{
			info.width--;
			EMIT_C('+');
		}
		else if(info.flags & FLG_SPACE)
		{
			info.width--;
			EMIT_C(' ');
		}
		else if((info.flags & FLG_SHAP))
		{
			if(info.radix == 8)
			{
				info.width--;
				EMIT_C('0');
			}
			else if(info.radix == 16)
			{
				info.width-=2;
				EMIT_C('0');
				EMIT_C('x');
			}
		}
	
		str=info.number;
		goto out_string;
		}
		break;

	case 'u':
		info.radix = 10;
		goto out_number;
	case 'o':
		info.radix = 8;
		goto out_number;
	case 'p':
		info.flags |= FLG_SHAP;
	case 'X':
		info.digits = __DIGITS;
	case 'x':
		info.radix = 16;
		goto out_number;
		
	case 'e':
	case 'E':
	case 'f':
	case 'g':
	case 'G':
		(void) va_arg(ap, double); // take off arg anyway
		break;
	}
  }/*while(*fmt)*/
ret:
  *out = '\0';
  return (int)(out - out_ptr);
}

int
mysprintf(char *out_ptr, const char *Afmt,...)
{
  va_list ap;
  int	n;
  
  va_start(ap,Afmt);
  n = myvsprintf(out_ptr, Afmt, ap);
  va_end(ap);
  return n;
}