user_io.c

ucos的gba移植原始版

#include "includes.h"

/*
*************************************************************
*	无浮点支持的内核格式化字符输出函数 printk
*
*	使用方法(printk):
*	. 指定一个输出控制台: console = Com1/Com2/LCD ...
*	. 使用printk()输出
*
*	格式化输出列表于ANSI C基本兼容
*
*
*
*	作者: 李强 ([e-mail] or [e-mail])
*
*	声明:	你仅可以将以下代码使用在你的商用或非商用，获利或非获利软件中
*			你可以任意修改以下代码，
*			唯一要求：请保留本信息及本人名字等信息
*
***********************************************************
*/

#define TMPLEN 31		// 数字字符串化的时候的临时变量空间长度

#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)

static void printk_putc (char c, int *count, cons *info)
{
	info->putc(c);
	*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, cons *info)
{
	int i;

	for (i = curlen; i < field_width; i++)
	{
		info->putc('0');
		count++;
	}
}

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

	for (i = curlen; i < field_width; i++)
	{
		info->putc(' ');
		count++;
	}
}



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

	char vstr[0x31];
	char *vstrp;
	int vlen;

	int done;
	int count = 0;

	int	flags_used;
	int	field_width;

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

	/*
	 * Start parsing apart the format string and display appropriate
	 * formats and data.
	 */
	for (p = (char *)fmt; (c = *p); 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 != '%')
		{
			printk_putc(c, &count, info);
			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;
}