[people/ms/u-boot.git] / lib / tiny-printf.c

/*
 * Tiny printf version for SPL
 *
 * Copied from:
 * http://www.sparetimelabs.com/printfrevisited/printfrevisited.php
 *
 * Copyright (C) 2004,2008  Kustaa Nyholm
 *
 * SPDX-License-Identifier:	LGPL-2.1+
 */

#include <common.h>
#include <stdarg.h>
#include <serial.h>
#include <linux/ctype.h>

struct printf_info {
	char *bf;	/* Digit buffer */
	char zs;	/* non-zero if a digit has been written */
	char *outstr;	/* Next output position for sprintf() */

	/* Output a character */
	void (*putc)(struct printf_info *info, char ch);
};

static void putc_normal(struct printf_info *info, char ch)
{
	putc(ch);
}

static void out(struct printf_info *info, char c)
{
	*info->bf++ = c;
}

static void out_dgt(struct printf_info *info, char dgt)
{
	out(info, dgt + (dgt < 10 ? '0' : 'a' - 10));
	info->zs = 1;
}

static void div_out(struct printf_info *info, unsigned long *num,
		    unsigned long div)
{
	unsigned char dgt = 0;

	while (*num >= div) {
		*num -= div;
		dgt++;
	}

	if (info->zs || dgt > 0)
		out_dgt(info, dgt);
}

#ifdef CONFIG_SPL_NET_SUPPORT
static void string(struct printf_info *info, char *s)
{
	char ch;

	while ((ch = *s++))
		out(info, ch);
}

static const char hex_asc[] = "0123456789abcdef";
#define hex_asc_lo(x)	hex_asc[((x) & 0x0f)]
#define hex_asc_hi(x)	hex_asc[((x) & 0xf0) >> 4]

static inline char *pack_hex_byte(char *buf, u8 byte)
{
	*buf++ = hex_asc_hi(byte);
	*buf++ = hex_asc_lo(byte);
	return buf;
}

static void mac_address_string(struct printf_info *info, u8 *addr,
				bool separator)
{
	/* (6 * 2 hex digits), 5 colons and trailing zero */
	char mac_addr[6 * 3];
	char *p = mac_addr;
	int i;

	for (i = 0; i < 6; i++) {
		p = pack_hex_byte(p, addr[i]);
		if (separator && i != 5)
			*p++ = ':';
	}
	*p = '\0';

	string(info, mac_addr);
}

static char *put_dec_trunc(char *buf, unsigned int q)
{
	unsigned int d3, d2, d1, d0;
	d1 = (q >> 4) & 0xf;
	d2 = (q >> 8) & 0xf;
	d3 = (q >> 12);

	d0 = 6 * (d3 + d2 + d1) + (q & 0xf);
	q = (d0 * 0xcd) >> 11;
	d0 = d0 - 10 * q;
	*buf++ = d0 + '0'; /* least significant digit */
	d1 = q + 9 * d3 + 5 * d2 + d1;
	if (d1 != 0) {
		q = (d1 * 0xcd) >> 11;
		d1 = d1 - 10 * q;
		*buf++ = d1 + '0'; /* next digit */

		d2 = q + 2 * d2;
		if ((d2 != 0) || (d3 != 0)) {
			q = (d2 * 0xd) >> 7;
			d2 = d2 - 10 * q;
			*buf++ = d2 + '0'; /* next digit */

			d3 = q + 4 * d3;
			if (d3 != 0) {
				q = (d3 * 0xcd) >> 11;
				d3 = d3 - 10 * q;
				*buf++ = d3 + '0';  /* next digit */
				if (q != 0)
					*buf++ = q + '0'; /* most sign. digit */
			}
		}
	}
	return buf;
}

static void ip4_addr_string(struct printf_info *info, u8 *addr)
{
	/* (4 * 3 decimal digits), 3 dots and trailing zero */
	char ip4_addr[4 * 4];
	char temp[3];	/* hold each IP quad in reverse order */
	char *p = ip4_addr;
	int i, digits;

	for (i = 0; i < 4; i++) {
		digits = put_dec_trunc(temp, addr[i]) - temp;
		/* reverse the digits in the quad */
		while (digits--)
			*p++ = temp[digits];
		if (i != 3)
			*p++ = '.';
	}
	*p = '\0';

	string(info, ip4_addr);
}
#endif

/*
 * Show a '%p' thing.  A kernel extension is that the '%p' is followed
 * by an extra set of characters that are extended format
 * specifiers.
 *
 * Right now we handle:
 *
 * - 'M' For a 6-byte MAC address, it prints the address in the
 *       usual colon-separated hex notation.
 * - 'm' Same as above except there is no colon-separator.
 * - 'I4'for IPv4 addresses printed in the usual way (dot-separated
 *       decimal).
 */

static void pointer(struct printf_info *info, const char *fmt, void *ptr)
{
#ifdef DEBUG
	unsigned long num = (uintptr_t)ptr;
	unsigned long div;
#endif

	switch (*fmt) {
#ifdef DEBUG
	case 'a':

		switch (fmt[1]) {
		case 'p':
		default:
			num = *(phys_addr_t *)ptr;
			break;
		}
		break;
#endif
#ifdef CONFIG_SPL_NET_SUPPORT
	case 'm':
		return mac_address_string(info, ptr, false);
	case 'M':
		return mac_address_string(info, ptr, true);
	case 'I':
		if (fmt[1] == '4')
			return ip4_addr_string(info, ptr);
#endif
	default:
		break;
	}
#ifdef DEBUG
	div = 1UL << (sizeof(long) * 8 - 4);
	for (; div; div /= 0x10)
		div_out(info, &num, div);
#endif
}

static int _vprintf(struct printf_info *info, const char *fmt, va_list va)
{
	char ch;
	char *p;
	unsigned long num;
	char buf[12];
	unsigned long div;

	while ((ch = *(fmt++))) {
		if (ch != '%') {
			info->putc(info, ch);
		} else {
			bool lz = false;
			int width = 0;
			bool islong = false;

			ch = *(fmt++);
			if (ch == '-')
				ch = *(fmt++);

			if (ch == '0') {
				ch = *(fmt++);
				lz = 1;
			}

			if (ch >= '0' && ch <= '9') {
				width = 0;
				while (ch >= '0' && ch <= '9') {
					width = (width * 10) + ch - '0';
					ch = *fmt++;
				}
			}
			if (ch == 'l') {
				ch = *(fmt++);
				islong = true;
			}

			info->bf = buf;
			p = info->bf;
			info->zs = 0;

			switch (ch) {
			case '\0':
				goto abort;
			case 'u':
			case 'd':
				div = 1000000000;
				if (islong) {
					num = va_arg(va, unsigned long);
					if (sizeof(long) > 4)
						div *= div * 10;
				} else {
					num = va_arg(va, unsigned int);
				}

				if (ch == 'd') {
					if (islong && (long)num < 0) {
						num = -(long)num;
						out(info, '-');
					} else if (!islong && (int)num < 0) {
						num = -(int)num;
						out(info, '-');
					}
				}
				if (!num) {
					out_dgt(info, 0);
				} else {
					for (; div; div /= 10)
						div_out(info, &num, div);
				}
				break;
			case 'x':
				if (islong) {
					num = va_arg(va, unsigned long);
					div = 1UL << (sizeof(long) * 8 - 4);
				} else {
					num = va_arg(va, unsigned int);
					div = 0x10000000;
				}
				if (!num) {
					out_dgt(info, 0);
				} else {
					for (; div; div /= 0x10)
						div_out(info, &num, div);
				}
				break;
			case 'c':
				out(info, (char)(va_arg(va, int)));
				break;
			case 's':
				p = va_arg(va, char*);
				break;
			case 'p':
				pointer(info, fmt, va_arg(va, void *));
				while (isalnum(fmt[0]))
					fmt++;
				break;
			case '%':
				out(info, '%');
			default:
				break;
			}

			*info->bf = 0;
			info->bf = p;
			while (*info->bf++ && width > 0)
				width--;
			while (width-- > 0)
				info->putc(info, lz ? '0' : ' ');
			if (p) {
				while ((ch = *p++))
					info->putc(info, ch);
			}
		}
	}

abort:
	return 0;
}

int vprintf(const char *fmt, va_list va)
{
	struct printf_info info;

	info.putc = putc_normal;
	return _vprintf(&info, fmt, va);
}

int printf(const char *fmt, ...)
{
	struct printf_info info;

	va_list va;
	int ret;

	info.putc = putc_normal;
	va_start(va, fmt);
	ret = _vprintf(&info, fmt, va);
	va_end(va);

	return ret;
}

static void putc_outstr(struct printf_info *info, char ch)
{
	*info->outstr++ = ch;
}

int sprintf(char *buf, const char *fmt, ...)
{
	struct printf_info info;
	va_list va;
	int ret;

	va_start(va, fmt);
	info.outstr = buf;
	info.putc = putc_outstr;
	ret = _vprintf(&info, fmt, va);
	va_end(va);
	*info.outstr = '\0';

	return ret;
}

/* Note that size is ignored */
int snprintf(char *buf, size_t size, const char *fmt, ...)
{
	struct printf_info info;
	va_list va;
	int ret;

	va_start(va, fmt);
	info.outstr = buf;
	info.putc = putc_outstr;
	ret = _vprintf(&info, fmt, va);
	va_end(va);
	*info.outstr = '\0';

	return ret;
}

void __assert_fail(const char *assertion, const char *file, unsigned line,
		   const char *function)
{
	/* This will not return */
	printf("%s:%u: %s: Assertion `%s' failed.", file, line, function,
	       assertion);
	hang();
}
Commit	Line	Data
	1	/*
	2	* Tiny printf version for SPL
	3	*
	4	* Copied from:
	5	* http://www.sparetimelabs.com/printfrevisited/printfrevisited.php
	6	*
	7	* Copyright (C) 2004,2008 Kustaa Nyholm
	8	*
	9	* SPDX-License-Identifier: LGPL-2.1+
	10	*/
	11
	12	#include <common.h>
	13	#include <stdarg.h>
	14	#include <serial.h>
	15	#include <linux/ctype.h>
	16
	17	struct printf_info {
	18	char bf; / Digit buffer */
	19	char zs; /* non-zero if a digit has been written */
	20	char outstr; / Next output position for sprintf() */
	21
	22	/* Output a character */
	23	void (putc)(struct printf_info info, char ch);
	24	};
	25
	26	static void putc_normal(struct printf_info *info, char ch)
	27	{
	28	putc(ch);
	29	}
	30
	31	static void out(struct printf_info *info, char c)
	32	{
	33	*info->bf++ = c;
	34	}
	35
	36	static void out_dgt(struct printf_info *info, char dgt)
	37	{
	38	out(info, dgt + (dgt < 10 ? '0' : 'a' - 10));
	39	info->zs = 1;
	40	}
	41
	42	static void div_out(struct printf_info info, unsigned long num,
	43	unsigned long div)
	44	{
	45	unsigned char dgt = 0;
	46
	47	while (*num >= div) {
	48	*num -= div;
	49	dgt++;
	50	}
	51
	52	if (info->zs \|\| dgt > 0)
	53	out_dgt(info, dgt);
	54	}
	55
	56	#ifdef CONFIG_SPL_NET_SUPPORT
	57	static void string(struct printf_info info, char s)
	58	{
	59	char ch;
	60
	61	while ((ch = *s++))
	62	out(info, ch);
	63	}
	64
	65	static const char hex_asc[] = "0123456789abcdef";
	66	#define hex_asc_lo(x) hex_asc[((x) & 0x0f)]
	67	#define hex_asc_hi(x) hex_asc[((x) & 0xf0) >> 4]
	68
	69	static inline char pack_hex_byte(char buf, u8 byte)
	70	{
	71	*buf++ = hex_asc_hi(byte);
	72	*buf++ = hex_asc_lo(byte);
	73	return buf;
	74	}
	75
	76	static void mac_address_string(struct printf_info info, u8 addr,
	77	bool separator)
	78	{
	79	/* (6 * 2 hex digits), 5 colons and trailing zero */
	80	char mac_addr[6 * 3];
	81	char *p = mac_addr;
	82	int i;
	83
	84	for (i = 0; i < 6; i++) {
	85	p = pack_hex_byte(p, addr[i]);
	86	if (separator && i != 5)
	87	*p++ = ':';
	88	}
	89	*p = '\0';
	90
	91	string(info, mac_addr);
	92	}
	93
	94	static char put_dec_trunc(char buf, unsigned int q)
	95	{
	96	unsigned int d3, d2, d1, d0;
	97	d1 = (q >> 4) & 0xf;
	98	d2 = (q >> 8) & 0xf;
	99	d3 = (q >> 12);
	100
	101	d0 = 6 * (d3 + d2 + d1) + (q & 0xf);
	102	q = (d0 * 0xcd) >> 11;
	103	d0 = d0 - 10 * q;
	104	buf++ = d0 + '0'; / least significant digit */
	105	d1 = q + 9 * d3 + 5 * d2 + d1;
	106	if (d1 != 0) {
	107	q = (d1 * 0xcd) >> 11;
	108	d1 = d1 - 10 * q;
	109	buf++ = d1 + '0'; / next digit */
	110
	111	d2 = q + 2 * d2;
	112	if ((d2 != 0) \|\| (d3 != 0)) {
	113	q = (d2 * 0xd) >> 7;
	114	d2 = d2 - 10 * q;
	115	buf++ = d2 + '0'; / next digit */
	116
	117	d3 = q + 4 * d3;
	118	if (d3 != 0) {
	119	q = (d3 * 0xcd) >> 11;
	120	d3 = d3 - 10 * q;
	121	buf++ = d3 + '0'; / next digit */
	122	if (q != 0)
	123	buf++ = q + '0'; / most sign. digit */
	124	}
	125	}
	126	}
	127	return buf;
	128	}
	129
	130	static void ip4_addr_string(struct printf_info info, u8 addr)
	131	{
	132	/* (4 * 3 decimal digits), 3 dots and trailing zero */
	133	char ip4_addr[4 * 4];
	134	char temp[3]; /* hold each IP quad in reverse order */
	135	char *p = ip4_addr;
	136	int i, digits;
	137
	138	for (i = 0; i < 4; i++) {
	139	digits = put_dec_trunc(temp, addr[i]) - temp;
	140	/* reverse the digits in the quad */
	141	while (digits--)
	142	*p++ = temp[digits];
	143	if (i != 3)
	144	*p++ = '.';
	145	}
	146	*p = '\0';
	147
	148	string(info, ip4_addr);
	149	}
	150	#endif
	151
	152	/*
	153	* Show a '%p' thing. A kernel extension is that the '%p' is followed
	154	* by an extra set of characters that are extended format
	155	* specifiers.
	156	*
	157	* Right now we handle:
	158	*
	159	* - 'M' For a 6-byte MAC address, it prints the address in the
	160	* usual colon-separated hex notation.
	161	* - 'm' Same as above except there is no colon-separator.
	162	* - 'I4'for IPv4 addresses printed in the usual way (dot-separated
	163	* decimal).
	164	*/
	165
	166	static void pointer(struct printf_info info, const char fmt, void *ptr)
	167	{
	168	#ifdef DEBUG
	169	unsigned long num = (uintptr_t)ptr;
	170	unsigned long div;
	171	#endif
	172
	173	switch (*fmt) {
	174	#ifdef DEBUG
	175	case 'a':
	176
	177	switch (fmt[1]) {
	178	case 'p':
	179	default:
	180	num = (phys_addr_t )ptr;
	181	break;
	182	}
	183	break;
	184	#endif
	185	#ifdef CONFIG_SPL_NET_SUPPORT
	186	case 'm':
	187	return mac_address_string(info, ptr, false);
	188	case 'M':
	189	return mac_address_string(info, ptr, true);
	190	case 'I':
	191	if (fmt[1] == '4')
	192	return ip4_addr_string(info, ptr);
	193	#endif
	194	default:
	195	break;
	196	}
	197	#ifdef DEBUG
	198	div = 1UL << (sizeof(long) * 8 - 4);
	199	for (; div; div /= 0x10)
	200	div_out(info, &num, div);
	201	#endif
	202	}
	203
	204	static int _vprintf(struct printf_info info, const char fmt, va_list va)
	205	{
	206	char ch;
	207	char *p;
	208	unsigned long num;
	209	char buf[12];
	210	unsigned long div;
	211
	212	while ((ch = *(fmt++))) {
	213	if (ch != '%') {
	214	info->putc(info, ch);
	215	} else {
	216	bool lz = false;
	217	int width = 0;
	218	bool islong = false;
	219
	220	ch = *(fmt++);
	221	if (ch == '-')
	222	ch = *(fmt++);
	223
	224	if (ch == '0') {
	225	ch = *(fmt++);
	226	lz = 1;
	227	}
	228
	229	if (ch >= '0' && ch <= '9') {
	230	width = 0;
	231	while (ch >= '0' && ch <= '9') {
	232	width = (width * 10) + ch - '0';
	233	ch = *fmt++;
	234	}
	235	}
	236	if (ch == 'l') {
	237	ch = *(fmt++);
	238	islong = true;
	239	}
	240
	241	info->bf = buf;
	242	p = info->bf;
	243	info->zs = 0;
	244
	245	switch (ch) {
	246	case '\0':
	247	goto abort;
	248	case 'u':
	249	case 'd':
	250	div = 1000000000;
	251	if (islong) {
	252	num = va_arg(va, unsigned long);
	253	if (sizeof(long) > 4)
	254	div = div 10;
	255	} else {
	256	num = va_arg(va, unsigned int);
	257	}
	258
	259	if (ch == 'd') {
	260	if (islong && (long)num < 0) {
	261	num = -(long)num;
	262	out(info, '-');
	263	} else if (!islong && (int)num < 0) {
	264	num = -(int)num;
	265	out(info, '-');
	266	}
	267	}
	268	if (!num) {
	269	out_dgt(info, 0);
	270	} else {
	271	for (; div; div /= 10)
	272	div_out(info, &num, div);
	273	}
	274	break;
	275	case 'x':
	276	if (islong) {
	277	num = va_arg(va, unsigned long);
	278	div = 1UL << (sizeof(long) * 8 - 4);
	279	} else {
	280	num = va_arg(va, unsigned int);
	281	div = 0x10000000;
	282	}
	283	if (!num) {
	284	out_dgt(info, 0);
	285	} else {
	286	for (; div; div /= 0x10)
	287	div_out(info, &num, div);
	288	}
	289	break;
	290	case 'c':
	291	out(info, (char)(va_arg(va, int)));
	292	break;
	293	case 's':
	294	p = va_arg(va, char*);
	295	break;
	296	case 'p':
	297	pointer(info, fmt, va_arg(va, void *));
	298	while (isalnum(fmt[0]))
	299	fmt++;
	300	break;
	301	case '%':
	302	out(info, '%');
	303	default:
	304	break;
	305	}
	306
	307	*info->bf = 0;
	308	info->bf = p;
	309	while (*info->bf++ && width > 0)
	310	width--;
	311	while (width-- > 0)
	312	info->putc(info, lz ? '0' : ' ');
	313	if (p) {
	314	while ((ch = *p++))
	315	info->putc(info, ch);
	316	}
	317	}
	318	}
	319
	320	abort:
	321	return 0;
	322	}
	323
	324	int vprintf(const char *fmt, va_list va)
	325	{
	326	struct printf_info info;
	327
	328	info.putc = putc_normal;
	329	return _vprintf(&info, fmt, va);
	330	}
	331
	332	int printf(const char *fmt, ...)
	333	{
	334	struct printf_info info;
	335
	336	va_list va;
	337	int ret;
	338
	339	info.putc = putc_normal;
	340	va_start(va, fmt);
	341	ret = _vprintf(&info, fmt, va);
	342	va_end(va);
	343
	344	return ret;
	345	}
	346
	347	static void putc_outstr(struct printf_info *info, char ch)
	348	{
	349	*info->outstr++ = ch;
	350	}
	351
	352	int sprintf(char buf, const char fmt, ...)
	353	{
	354	struct printf_info info;
	355	va_list va;
	356	int ret;
	357
	358	va_start(va, fmt);
	359	info.outstr = buf;
	360	info.putc = putc_outstr;
	361	ret = _vprintf(&info, fmt, va);
	362	va_end(va);
	363	*info.outstr = '\0';
	364
	365	return ret;
	366	}
	367
	368	/* Note that size is ignored */
	369	int snprintf(char buf, size_t size, const char fmt, ...)
	370	{
	371	struct printf_info info;
	372	va_list va;
	373	int ret;
	374
	375	va_start(va, fmt);
	376	info.outstr = buf;
	377	info.putc = putc_outstr;
	378	ret = _vprintf(&info, fmt, va);
	379	va_end(va);
	380	*info.outstr = '\0';
	381
	382	return ret;
	383	}
	384
	385	void __assert_fail(const char assertion, const char file, unsigned line,
	386	const char *function)
	387	{
	388	/* This will not return */
	389	printf("%s:%u: %s: Assertion `%s' failed.", file, line, function,
	390	assertion);
	391	hang();
	392	}