stm32f1xx_HBL/srv/format.c

/** @file format.c
 * Module handling various formating functions
 *
 * The module provides a lightweight printf function and the attached tools
 */

//--includes--------------------------------------------------------------------

#include "format.h"


//--local definitions-----------------------------------------------------------

struct BufferArg {
	char* restrict buffer;
	uint32_t buffer_size;
	uint32_t byte_index;
};

static uint32_t buffer_write(uint8_t byte, void* arg);
static uint32_t render_format(FormatCallback callback, void* arg,
		const char* restrict format, va_list va);
static uint32_t render_signed(FormatCallback callback, void* arg,
		int32_t number, uint8_t base, uint8_t width, bool zero_padding);
static uint32_t render_unsigned(FormatCallback callback, void* arg,
		uint32_t number, uint8_t base, uint8_t width, bool zero_padding);


//--local variables-------------------------------------------------------------

//--public functions------------------------------------------------------------

uint32_t format_snprintf(char* restrict buffer, uint32_t buffer_size,
		const char* restrict format, ...)
{
	va_list va;
	va_start(va, format);
	struct BufferArg arg = { buffer, buffer_size, 0 };
	uint32_t ret = render_format(buffer_write, &arg, format, va);
	va_end(va);
	return ret;
}

uint32_t format_fctprintf(FormatCallback callback, void* arg,
		const char* restrict format, ...)
{
	va_list va;
	va_start(va, format);
	uint32_t ret = render_format(callback, arg, format, va);
	va_end(va);
	return ret;
}

uint32_t format_vfctprintf(FormatCallback callback, void* arg,
		const char* restrict format, va_list va)
{
	return render_format(callback, arg, format, va);
}

//--local functions-------------------------------------------------------------

static uint32_t buffer_write(uint8_t byte, void* arg)
{
	struct BufferArg* buffer_arg = (struct BufferArg*)arg;
	if (buffer_arg->byte_index >= buffer_arg->buffer_size) {
		return 1;
	}

	buffer_arg->buffer[buffer_arg->byte_index] = byte;
	++buffer_arg->byte_index;
	return 0;
}

static uint32_t render_format(FormatCallback callback, void* arg,
		const char* restrict format, va_list va)
{
	uint8_t width = 0;
	bool in_format = false;
	bool zero_padding = false;

	for (const char* c = format; *c != '\0'; ++c) {

		if (in_format) {
			switch (*c) {
				case '%':
					if (callback('%', arg)) return 1;
					break;
				case 'c':
					if (callback((uint8_t)va_arg(va, uint32_t), arg)) {
						return 1;
					}
					break;
				case 's':
					{
						const char* str = va_arg(va, const char*);
						for (const char* c2 = str; *c2 != '\0'; ++c2) {
							if (callback(*c2, arg)) return 1;
						}
					}
					break;
				case 'i':
					if (render_signed(callback, arg, va_arg(va, int32_t),
								10, width, zero_padding)) {
						return 1;
					}
					break;
				case 'u':
					if (render_unsigned(callback, arg, va_arg(va, uint32_t),
								10, width, zero_padding)) {
						return 1;
					}
					break;
				case 'x':
					if (render_unsigned(callback, arg, va_arg(va, uint32_t),
								16, width, zero_padding)) {
						return 1;
					}
					break;
				case '0':
					if (width == 0) {
						zero_padding = true;
						continue;
					}
					// fall through
				case '1':
				case '2':
				case '3':
				case '4':
				case '5':
				case '6':
				case '7':
				case '8':
				case '9':
					width *= 10;
					width += (*c - '0');
					continue;
				default:
					//TODO manage error
					break;
			}
		} else {
			switch (*c) {
				case '%':
					in_format = true;
					zero_padding = false;
					width = 0;
					continue;
				default:
					if (callback(*c, arg)) return 1;
					break;
			}
		}

		in_format = false;
	}

	return 0;
}

static uint32_t render_signed(FormatCallback callback, void* arg,
		int32_t number, uint8_t base, uint8_t width, bool zero_padding)
{
	if (width > 0 && number < 0) {
		--width;
	}

	if (width > 0) {
		uint32_t abs_number;
		if (number < 0) {
			abs_number = -number;
		} else {
			abs_number = number;
		}

		uint32_t base_power = base;
		while (width > 0 && base_power <= abs_number) {
			base_power *= base;
			--width;
		}
		if (width > 0) {
			--width;
		}

		while (width > 0) {
			--width;
			if (zero_padding) {
				if (callback('0', arg)) return 1;
			} else {
				if (callback(' ', arg)) return 1;
			}
		}
	}

	if (number < 0) {
		callback('-', arg);
		number = -number;
	}

	return render_unsigned(callback, arg, (uint32_t)number, base, 0,
			zero_padding);
}

static uint32_t render_unsigned(FormatCallback callback, void* arg,
		 uint32_t number, uint8_t base, uint8_t width, bool zero_padding)
{
	uint32_t base_power = base;

	while (base_power <= number) {
		base_power *= base;
		if (width > 0) {
			--width;
		}
	}
	if (width > 0) {
		--width;
	}

	while (width > 0) {
		--width;
		if (zero_padding) {
			if (callback('0', arg)) return 1;
		} else {
			if (callback(' ', arg)) return 1;
		}
	}

	while (base_power > 1) {
		base_power /= base;

		uint8_t digit = 0;
		while (number >= base_power) {
			number -= base_power;
			++digit;
		}

		if (digit < 10) {
			if (callback('0' + digit, arg)) return 1;
		} else {
			if (callback('A' + digit - 10, arg)) return 1;
		}
	}

	return 0;
}