mcu_base/src/printf.c

#include "global.h"

//#define WITH_FLOAT  // this is busted still

#include <string.h>

uint32_t parse_int(const char *src, int b, int width)
{
	int s = 0;
	uint32_t val = 0;

	if (!width) width = 12;

	while (*src && width) {
		if (*src == '-') {
			s = 1;
			goto skip;
		}
		if (*src == ' ')
			break;

		val *= b;

		if (*src >= 'a' && *src <= 'z')
			val += *src - 'a' + 10;
		else if (*src >= 'A' && *src <= 'Z')
			val += *src - 'A' + 10;
		else if (*src >= '0' && *src <= '9')
			val += *src - '0';
		else
			break;
	skip:
		src++;
		width--;
	}

	if (s)
		val = (uint32_t) -((int32_t)val);

	return val;
}

#ifdef WITH_FLOAT
static int __print_float(char *buf, int buf_len, double i, int width, int width2)
{
	char *s, *p = buf;
	uint8_t use_width = width;
	double t;
	double u = i;

	if (i == 0.0f) {
		s = "0.0\0";
		width--;
		goto almost;
	}

	if (i < 0.0f) {
		*buf++ = '-';
		i = -i;
		width--;
	}
	s = buf + buf_len - 1;
	*s = '\0';

	while (i != 0.0f && (!use_width || (width > 0))) {
		t = u % 10.0f;
		*--s = (char)(t + '0');
		u /= 10.0f;
		width--;
	}


almost:
	while (width > 0) {
		*buf++ = '0';
		width--;
	}
	strcpy(buf, s);

	return strlen(p);
}
#endif

static int __print_int(char *buf, int buf_len, int i, int base, int sign, int width)
{
	char *s, *p = buf;
	unsigned int u = (unsigned int) i;
	uint8_t use_width;
	int t;

	use_width = width;

	if (i == 0) {
		s = "0\0";
		width--;
		goto almost;
	}

	if (sign && base == 10 && i < 0) {
		*buf++ = '-';
		u = (unsigned int)-i;
		width--;
	}
	s = buf + buf_len - 1;
	*s = '\0';

	while (u && (!use_width || (width > 0))) {
		t = (unsigned int) u % base;
		if (t >= 10)
			t += 'A' - '0' - 10;
		*--s = (char)(t + '0');
		u /= base;
		width--;
	}

almost:
	while (width > 0) {
		*buf++ = '0';
		width--;
	}
	strcpy(buf, s);

	return strlen(p);
}

/* Very crude vsnprintf */
int mcu_vsnprintf(char *str, size_t size, const char *format, va_list ap)
{
	size_t len = 0;

	*str = 0;
	size--; /* Ensure we null-terminate within our buffer */

	while (*format && len < size) {
		uint8_t width = 0;
		if (*format != '%') {  /* Non-special */
			*str++ = *format++;
			len++;
			continue;
		}
		if (*(format + 1) == '%') { /* Escaped % */
			*str++ = '%';
			len ++;
			format += 2;
			continue;
		}
		format++;
		if (*format == '0') {  /* Width */
			width = parse_int(format, 10, 2);
			format+=2;
		}
		if (*format == 'l') {
			format++;
			// XXX handle LONG?
		}
		switch(*format) {
		case 'c': { /* Character */
			char tmp = va_arg(ap, int);
			*(str++) = tmp;
			len++;
			break;
		}
		case 'u': { /* Unsigned integer */
			uint8_t tmp = __print_int(str, size-len,
					     va_arg(ap, unsigned int),
					     10, 0, width);
			str += tmp;
			len += tmp;
			break;
		}
		case 'd': { /* Signed integer */
			uint8_t tmp = __print_int(str, size-len,
					     va_arg(ap, int),
					     10, 1, width);
			str += tmp;
			len += tmp;
			break;
		}
		case 'p': { /* Pointer */
			width = 8;
			uint8_t tmp = __print_int(str, size-len,
					     va_arg(ap, unsigned long),
					     16, 1, width);
			str += tmp;
			len += tmp;
			break;
		}
		case 'x':
		case 'X': { /* Unsigned hex number */
			uint8_t tmp = __print_int(str, size-len,
					     va_arg(ap, unsigned int),
					     16, 0, width);
			str += tmp;
			len += tmp;
			break;
		} case 'm': { /* MAC Address */
			uint8_t *addr = va_arg(ap, uint8_t *);
			int i;
			width = 2;
			for(i = 0 ; i < MAC_ADDR_LEN ; i++) {
				uint8_t tmp = *addr++;
				tmp = __print_int(str, size-len,
						  tmp,
						  16, 0, width);
				str += tmp;
				len += tmp;

				if (i+1 < MAC_ADDR_LEN) {
					*(str++) = ':';
					len++;
				}
			}
			break;
		}
		case 'i': {  /* IP addr */
			uint32_t addr = va_arg(ap, uint32_t);
			int i;
			addr = htonl(addr);
			for (i = 24 ; i >= 0 ; i -= 8) {
				uint8_t tmp = __print_int(str, size-len,
						     (addr >> i) & 0xff,
						     10, 0, width);
				str += tmp;
				len += tmp;

				if (i > 0) {
					*(str++) = '.';
					len++;
				}
			}
			break;
		}
		case 's': { /* String */
			char *ptr = va_arg(ap, char *);
			while (ptr && *ptr && len < size) {
				*str++ = *ptr++;
				len++;
			}
			break;
		}
#ifdef WITH_FLOAT
		case 'f': { /* Floating point */
			uint8_t tmp = __print_float(str, size-len,
					     va_arg(ap, double),
						    width, width);
			str += tmp;
			len += tmp;
			break;

		}
#endif
		default: {
			// char *ptr = va_arg(ap, char *);
			// XXX insert error into output?
			break;
		}
		}
		format++;
	}

	/* Null-terminate */
	*str = 0;

	if (len > size)
		return size + 2; /* Compensate for -1 earlier */

	return len;
}

int mcu_snprintf(char *str, size_t size, const char *format, ...)
{
	va_list ap;
	int len;

	va_start(ap, format);
	len = mcu_vsnprintf(str, size, format, ap);
	va_end(ap);

	return len;
}