printf.c

zigbee 飞思卡尔 音频传输 基于ucos的所有源码

/*
 * File:		printk.c
 * Purpose:		The standard C library routine printf(), but without
 *				all the baggage.
 */

#include "common.h"
#include <stdarg.h>

/********************************************************************/

typedef struct
{
	int	dest;
	void (*func)(char);
	char *loc;
} PRINTK_INFO;

int 
printk (PRINTK_INFO *, const char *, va_list);

/********************************************************************/

#define DEST_CONSOLE	(1)
#define DEST_STRING		(2)

#define FLAGS_MINUS		(0x01)
#define FLAGS_PLUS		(0x02)
#define FLAGS_SPACE		(0x04)
#define FLAGS_ZERO		(0x08)
#define FLAGS_POUND		(0x10)

#define IS_FLAG_MINUS(a)	(a & FLAGS_MINUS)
#define IS_FLAG_PLUS(a)		(a & FLAGS_PLUS)
#define IS_FLAG_SPACE(a)	(a & FLAGS_SPACE)
#define IS_FLAG_ZERO(a)		(a & FLAGS_ZERO)
#define IS_FLAG_POUND(a)	(a & FLAGS_POUND)

#define LENMOD_h		(0x01)
#define LENMOD_l		(0x02)
#define LENMOD_L		(0x04)

#define IS_LENMOD_h(a)	(a & LENMOD_h)
#define IS_LENMOD_l(a)	(a & LENMOD_l)
#define IS_LENMOD_L(a)	(a & LENMOD_L)

#define FMT_d	(0x0001)
#define FMT_o	(0x0002)
#define FMT_x	(0x0004)
#define FMT_X	(0x0008)
#define FMT_u	(0x0010)
#define FMT_c	(0x0020)
#define FMT_s	(0x0040)
#define FMT_p	(0x0080)
#define FMT_n	(0x0100)

#define IS_FMT_d(a)		(a & FMT_d)
#define IS_FMT_o(a)		(a & FMT_o)
#define IS_FMT_x(a)		(a & FMT_x)
#define IS_FMT_X(a)		(a & FMT_X)
#define IS_FMT_u(a)		(a & FMT_u)
#define IS_FMT_c(a)		(a & FMT_c)
#define IS_FMT_s(a)		(a & FMT_s)
#define IS_FMT_p(a)		(a & FMT_p)
#define IS_FMT_n(a)		(a & FMT_n)

/********************************************************************/
static void
printk_putc (int c, int *count, PRINTK_INFO *info)
{
	switch (info->dest)
	{
		case DEST_CONSOLE:
			info->func((char)c);
			break;
		case DEST_STRING:
			*(info->loc) = (unsigned char)c;
			++(info->loc);
			break;
		default:
			break;
	}
	*count += 1;
}

/********************************************************************/
static int
printk_mknumstr (char *numstr, void *nump, int neg, int radix)
{
	int a,b,c;
	unsigned int ua,ub,uc;

	int nlen;
	char *nstrp;

	nlen = 0;
	nstrp = numstr;
	*nstrp++ = '\0';

	if (neg)
	{
		a = *(int *)nump;
		if (a == 0)
		{
			*nstrp = '0';
			++nlen;
			goto done;
		}
		while (a != 0)
		{
			b = (int)a / (int)radix;
			c = (int)a - ((int)b * (int)radix);
			if (c < 0)
			{
				c = ~c + 1 + '0';
			}
			else
			{
				c = c + '0';
			}
			a = b;
			*nstrp++ = (char)c;
			++nlen;
		}
	}
	else
	{
		ua = *(unsigned int *)nump;
		if (ua == 0)
		{
			*nstrp = '0';
			++nlen;
			goto done;
		}
		while (ua != 0)
		{
			ub = (unsigned int)ua / (unsigned int)radix;
			uc = (unsigned int)ua - ((unsigned int)ub * (unsigned int)radix);
			if (uc < 10)
			{
				uc = uc + '0';
			}
			else
			{
				uc = uc - 10 + 'A';
			}
			ua = ub;
			*nstrp++ = (char)uc;
			++nlen;
		}
	}
	done:
	return nlen;
}

/********************************************************************/
static void
printk_pad_zero (int curlen, int field_width, int *count, PRINTK_INFO *info)
{
	int i;

	for (i = curlen; i < field_width; i++)
	{
		printk_putc('0',count, info);
	}
}

/********************************************************************/
static void
printk_pad_space (int curlen, int field_width, int *count, PRINTK_INFO *info)
{
	int i;

	for (i = curlen; i < field_width; i++)
	{
		printk_putc(' ',count, info);
	}
}

/********************************************************************/
int
printk (PRINTK_INFO *info, const char *fmt, va_list ap)
{
	/* va_list ap; */
	char *p;
	int c;

	char vstr[33];
	char *vstrp;
	int vlen;

	int done;
	int count = 0;

	int	flags_used;
	int	field_width;
#if 0
	int	precision_used;
	int	precision_width;
	int	length_modifier;
#endif

	int	ival;
	int schar, dschar;
	int *ivalp;
	char *sval;
	int cval;
	unsigned int uval;

	/*
	 * Start parsing apart the format string and display appropriate
	 * formats and data.
	 */
	for (p = (char *)fmt; (c = *p) != 0; p++)
	{
		/*
		 * All formats begin with a '%' marker.  Special chars like
		 * '\n' or '\t' are normally converted to the appropriate
		 * character by the __compiler__.  Thus, no need for this
		 * routine to account for the '\' character.
		 */
		if (c != '%')
		{
			/*
			 * This needs to be replaced with something like
			 * 'out_char()' or call an OS routine.
			 */
#ifndef UNIX_DEBUG
			if (c != '\n')
			{
				printk_putc(c, &count, info);
			}
			else
			{
				printk_putc(0x0D /* CR */, &count, info);
				printk_putc(0x0A /* LF */, &count, info);
			}
#else
			printk_putc(c, &count, info);
#endif

			/*
			 * By using 'continue', the next iteration of the loop
			 * is used, skipping the code that follows.
			 */
			continue;
		}

		/*
		 * First check for specification modifier flags.
		 */
		flags_used = 0;
		done = FALSE;
		while (!done)
		{
			switch (/* c = */ *++p)
			{
				case '-':
					flags_used |= FLAGS_MINUS;
					break;
				case '+':
					flags_used |= FLAGS_PLUS;
					break;
				case ' ':
					flags_used |= FLAGS_SPACE;
					break;
				case '0':
					flags_used |= FLAGS_ZERO;
					break;
				case '#':
					flags_used |= FLAGS_POUND;
					break;
				default:
					/* we've gone one char too far */
					--p;
					done = TRUE;
					break;
			}
		}

		/*
		 * Next check for minimum field width.
		 */
		field_width = 0;
		done = FALSE;
		while (!done)
		{
			switch (c = *++p)
			{
				case '0':
				case '1':
				case '2':
				case '3':
				case '4':
				case '5':
				case '6':
				case '7':
				case '8':
				case '9':
					field_width = (field_width * 10) + (c - '0');
					break;
				default:
					/* we've gone one char too far */
					--p;
					done = TRUE;
					break;
			}
		}

		/*
		 * Next check for the width and precision field separator.
		 */
		if (/* (c = *++p) */ *++p == '.')
		{
			/* precision_used = TRUE; */

			/*
			 * Must get precision field width, if present.
			 */
			/* precision_width = 0; */
			done = FALSE;
			while (!done)
			{
				switch (/* c = uncomment if used below */ *++p)
				{
					case '0':
					case '1':
					case '2':
					case '3':
					case '4':
					case '5':
					case '6':
					case '7':
					case '8':
					case '9':
#if 0
						precision_width = (precision_width * 10) +
							(c - '0');
#endif
						break;
					default:
						/* we've gone one char too far */
						--p;
						done = TRUE;
						break;
				}
			}
		}
		else
		{
			/* we've gone one char too far */
			--p;
#if 0
			precision_used = FALSE;
			precision_width = 0;
#endif
		}

		/*
		 * Check for the length modifier.
		 */
		/* length_modifier = 0; */
		switch (/* c = */ *++p)
		{
			case 'h':
				/* length_modifier |= LENMOD_h; */
				break;
			case 'l':
				/* length_modifier |= LENMOD_l; */
				break;
			case 'L':
				/* length_modifier |= LENMOD_L; */
				break;
			default:
				/* we've gone one char too far */
				--p;
				break;
		}

		/*
		 * Now we're ready to examine the format.
		 */
		switch (c = *++p)
		{
			case 'd':
			case 'i':
				ival = (int)va_arg(ap, int);
				vlen = printk_mknumstr(vstr,&ival,TRUE,10);
				vstrp = &vstr[vlen];

				if (ival < 0)
				{
					schar = '-';
					++vlen;
				}
				else
				{
					if (IS_FLAG_PLUS(flags_used))
					{
						schar = '+';
						++vlen;
					}
					else
					{
						if (IS_FLAG_SPACE(flags_used))
						{
							schar = ' ';
							++vlen;
						}
						else
						{
							schar = 0;
						}
					}
				}
				dschar = FALSE;
			
				/*
				 * do the ZERO pad.
				 */
				if (IS_FLAG_ZERO(flags_used))
				{
					if (schar)
						printk_putc(schar, &count, info);
					dschar = TRUE;
			
					printk_pad_zero (vlen, field_width, &count, info);
					vlen = field_width;
				}
				else
				{
					if (!IS_FLAG_MINUS(flags_used))
					{
						printk_pad_space (vlen, field_width, &count, info);
			
						if (schar)
							printk_putc(schar, &count, info);
						dschar = TRUE;
					}
				}
			
				/* the string was built in reverse order, now display in */
				/* correct order */
				if (!dschar && schar)
				{
					printk_putc(schar, &count, info);
				}
				goto cont_xd;

			case 'x':
			case 'X':
				uval = (unsigned int)va_arg(ap, unsigned int);
				vlen = printk_mknumstr(vstr,&uval,FALSE,16);
				vstrp = &vstr[vlen];

				dschar = FALSE;
				if (IS_FLAG_ZERO(flags_used))
				{
					if (IS_FLAG_POUND(flags_used))
					{
						printk_putc('0', &count, info);
						printk_putc('x', &count, info);
						/*vlen += 2;*/
						dschar = TRUE;
					}
					printk_pad_zero (vlen, field_width, &count, info);
					vlen = field_width;
				}
				else
				{
					if (!IS_FLAG_MINUS(flags_used))
					{
						if (IS_FLAG_POUND(flags_used))
						{
							vlen += 2;
						}
						printk_pad_space (vlen, field_width, &count, info);
						if (IS_FLAG_POUND(flags_used))
						{
							printk_putc('0', &count, info);
							printk_putc('x', &count, info);
							dschar = TRUE;
						}
					}
				}

				if ((IS_FLAG_POUND(flags_used)) && !dschar)
				{
					printk_putc('0', &count, info);
					printk_putc('x', &count, info);
					vlen += 2;
				}
				goto cont_xd;

			case 'o':
				uval = (unsigned int)va_arg(ap, unsigned int);
				vlen = printk_mknumstr(vstr,&uval,FALSE,8);
				goto cont_u;
			case 'b':
				uval = (unsigned int)va_arg(ap, unsigned int);
				vlen = printk_mknumstr(vstr,&uval,FALSE,2);
				goto cont_u;
			case 'p':
				uval = (unsigned int)va_arg(ap, void *);
				vlen = printk_mknumstr(vstr,&uval,FALSE,16);
				goto cont_u;
			case 'u':
				uval = (unsigned int)va_arg(ap, unsigned int);
				vlen = printk_mknumstr(vstr,&uval,FALSE,10);

				cont_u:
					vstrp = &vstr[vlen];

					if (IS_FLAG_ZERO(flags_used))
					{
						printk_pad_zero (vlen, field_width, &count, info);
						vlen = field_width;
					}
					else
					{
						if (!IS_FLAG_MINUS(flags_used))
						{
							printk_pad_space (vlen, field_width, &count, info);
						}
					}

				cont_xd:
					while (*vstrp)
						printk_putc(*vstrp--, &count, info);

					if (IS_FLAG_MINUS(flags_used))
					{
						printk_pad_space (vlen, field_width, &count, info);
					}
				break;

			case 'c':
				cval = (char)va_arg(ap, unsigned int);
				printk_putc(cval,&count, info);
				break;
			case 's':
				sval = (char *)va_arg(ap, char *);
				if (sval)
				{
					vlen = strlen(sval);
					if (!IS_FLAG_MINUS(flags_used))
					{
						printk_pad_space (vlen, field_width, &count, info);
					}
					while (*sval)
						printk_putc(*sval++,&count, info);
					if (IS_FLAG_MINUS(flags_used))
					{
						printk_pad_space (vlen, field_width, &count, info);
					}
				}
				break;
			case 'n':
				ivalp = (int *)va_arg(ap, int *);
				*ivalp = count;
				break;
			default:
				printk_putc(c,&count, info);
				break;
		}
	}
	return count;
}

/********************************************************************/
int
printf (const char *fmt, ...)
{
	va_list ap;
	int rvalue;
	PRINTK_INFO info;


	info.dest = DEST_CONSOLE;
	info.func = &out_char;
	/*
	 * Initialize the pointer to the variable length argument list.
	 */
	va_start(ap, fmt);
	rvalue = printk(&info, fmt, ap);
	/*
	 * Cleanup the variable length argument list.
	 */
	va_end(ap);
	return rvalue;
}

/********************************************************************/
int
sprintf (char *s, const char *fmt, ...)
{
	va_list ap;
	int rvalue = 0;
	PRINTK_INFO info;

	/*
	 * Initialize the pointer to the variable length argument list.
	 */
	if (s != 0)
	{
		info.dest = DEST_STRING;
		info.loc = s;
		va_start(ap, fmt);
		rvalue = printk(&info, fmt, ap);
		*info.loc = '\0';
		va_end(ap);
	}
	return rvalue;
}

/********************************************************************/