Implement basic string formatting

srv/debug.c

@@ -9,11 +9,12 @@
 
 #include "debug.h"
 #include "dma_mbuf.h"
+#include "format.h"
 
 
 //--local definitions-----------------------------------------------------------
 
-static void print_debug(char* str);
+static uint32_t write_debug(uint8_t c, void* arg);
 
 #define BUFFER_SIZE 162  //(80 char line + \n) * 2
 
@@ -50,8 +51,13 @@ void _debug_init(enum UsartPeriph usart, enum GpioPort tx_port,
 
 void _debug_trace(char* format, ...)
 {
-	print_debug("T:");
-	print_debug(format);
+	write_debug('T', nullptr);
+	write_debug(':', nullptr);
+	va_list va;
+	va_start(va, format);
+	format_vfctprintf(write_debug, nullptr, format, va);
+	va_end(va);
+	write_debug('\n', nullptr);
 
 	//ensure everything is written when done
 	while (dma_mbuf_switch(&mbuf)) {}
@@ -89,16 +95,15 @@ void _debug_error(char* format, ...)
 
 #if DEBUG_TRACE || DEBUG_WARN || DEBUG_ERROR
 
-static void print_debug(char* str)
+static uint32_t write_debug(uint8_t c, void* arg)
 {
-	while (*str != '\0') {
-		if (dma_mbuf_write_byte(&mbuf, *str))
-		{
-			//buffer is full, wait until transfer started
-			while (dma_mbuf_switch(&mbuf)) {}
-		}
-		++str;
+	while (dma_mbuf_write_byte(&mbuf, c))
+	{
+		//buffer is full, wait until transfer started
+		while (dma_mbuf_switch(&mbuf)) {}
 	}
+
+	return 0;
 }
 
 #endif

srv/format.c

@@ -0,0 +1,162 @@
+/** @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;
+};
+
+uint32_t buffer_write(uint8_t byte, void* arg);
+uint32_t render_format(FormatCallback callback, void* arg,
+		const char* restrict format, va_list va);
+uint32_t render_signed(FormatCallback callback, void* arg,
+		va_list va, uint8_t size, uint8_t base);
+uint32_t render_unsigned(FormatCallback callback, void* arg,
+		va_list* va, uint8_t size, uint8_t base);
+
+
+//--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-------------------------------------------------------------
+
+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;
+}
+
+uint32_t render_format(FormatCallback callback, void* arg,
+		const char* restrict format, va_list va)
+{
+	uint8_t size;
+	bool in_format = false;
+
+	for (const char* c = format; *c != '\0'; ++c) {
+
+		if (in_format) {
+			switch (*c) {
+				case '%':
+					callback('%', arg);
+					break;
+				case 'h':
+					size = size / 2;
+					continue;
+				case 'l':
+					size = size * 2;
+					continue;
+				case 'c':
+					callback((uint8_t)va_arg(va, uint32_t), arg);
+					break;
+				case 's':
+					{
+						const char* str = va_arg(va, const char*);
+						for (const char* c2 = str; *c2 != '\0'; ++c2) {
+							callback(*c2, arg);
+						}
+					}
+					break;
+				case 'i':
+					//render_signed(callback, arg, va, size, 10);
+					break;
+				case 'u':
+					render_unsigned(callback, arg, &va, size, 10);
+					break;
+				case 'x':
+					render_unsigned(callback, arg, &va, size, 16);
+					break;
+				default:
+					//TODO manage error
+					break;
+			}
+		} else {
+			switch (*c) {
+				case '%':
+					in_format = true;
+					size = 4;
+					continue;
+				default:
+					callback(*c, arg);
+					break;
+			}
+		}
+
+		in_format = false;
+	}
+
+	return 0;
+}
+
+uint32_t render_unsigned(FormatCallback callback, void* arg,
+		va_list* va, uint8_t size, uint8_t base)
+{
+	uint32_t number = va_arg(*va, uint32_t);
+	uint32_t base_power = base;
+
+	while (base_power <= number) {
+		base_power *= base;
+	}
+
+	while (base_power > 1) {
+		base_power /= base;
+
+		uint8_t digit = 0;
+		while (number >= base_power) {
+			number -= base_power;
+			++digit;
+		}
+
+		if (digit < 10) {
+			callback('0' + digit, arg);
+		} else {
+			callback('A' + digit - 10, arg);
+		}
+	}
+
+	return 0;
+}

srv/format.h

@@ -0,0 +1,33 @@
+/** @file format.h
+ * Module handling various formating functions
+ *
+ * The module provides a lightweight printf function and the attached tools
+ */
+
+#ifndef _FORMAT_H_
+#define _FORMAT_H_
+
+//--includes--------------------------------------------------------------------
+
+#include <stdint.h>
+#include <stdarg.h>
+
+
+//--type definitions------------------------------------------------------------
+
+typedef uint32_t (*FormatCallback)(uint8_t byte, void* arg);
+
+
+//--functions-------------------------------------------------------------------
+
+uint32_t format_snprintf(char* restrict buffer, uint32_t buffer_size,
+		const char* restrict format, ...);
+
+uint32_t format_fctprintf(FormatCallback callback, void* arg,
+		const char* restrict format, ...);
+
+uint32_t format_vfctprintf(FormatCallback callback, void* arg,
+		const char* restrict format, va_list va);
+
+#endif //_FORMAT_H_
+