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

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

#include <common.h>
#include <log.h>
#include <serial.h>
#include <stdarg.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 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
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 __maybe_unused 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
	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':
			case 'i':
				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 != 'u') {
					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 'p':
				if (CONFIG_IS_ENABLED(NET) || _DEBUG) {
					pointer(info, fmt, va_arg(va, void *));
					/*
					 * Skip this because it pulls in _ctype which is
					 * 256 bytes, and we don't generally implement
					 * pointer anyway
					 */
					while (isalnum(fmt[0]))
						fmt++;
					break;
				}
				islong = true;
				/* no 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 '%':
				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;
}

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

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;
}
#endif

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;
}

#if CONFIG_IS_ENABLED(LOG)
/* Note that size is ignored */
int vsnprintf(char *buf, size_t size, const char *fmt, va_list va)
{
	struct printf_info info;
	int ret;

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

	return ret;
}
#endif

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